forked from pz4kybsvg/Conception
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
61 lines
2.1 KiB
61 lines
2.1 KiB
#pragma once
|
|
|
|
#include <Eigen/Dense>
|
|
|
|
#include "drake/math/gradient.h"
|
|
#include "drake/math/gradient_util.h"
|
|
|
|
namespace drake {
|
|
namespace math {
|
|
/** Computes the normalized vector, optionally with its gradient and second
|
|
derivative.
|
|
@param[in] x An N x 1 vector to be normalized. Must not be zero.
|
|
@param[out] x_norm The normalized vector (N x 1).
|
|
@param[out] dx_norm
|
|
If non-null, returned as an N x N matrix,
|
|
where dx_norm(i,j) = D x_norm(i)/D x(j).
|
|
@param[out] ddx_norm
|
|
If non-null, and dx_norm is non-null, returned as an N^2 x N matrix,
|
|
where ddx_norm.col(j) = D dx_norm/D x(j), with dx_norm stacked
|
|
columnwise.
|
|
|
|
(D x / D y above means partial derivative of x with respect to y.) */
|
|
template <typename Derived>
|
|
void NormalizeVector(
|
|
const Eigen::MatrixBase<Derived>& x,
|
|
// TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
|
|
typename Derived::PlainObject& x_norm,
|
|
typename drake::math::Gradient<Derived, Derived::RowsAtCompileTime,
|
|
1>::type* dx_norm = nullptr,
|
|
typename drake::math::Gradient<Derived, Derived::RowsAtCompileTime,
|
|
2>::type* ddx_norm = nullptr) {
|
|
typename Derived::Scalar xdotx = x.squaredNorm();
|
|
typename Derived::Scalar norm_x = sqrt(xdotx);
|
|
x_norm = x / norm_x;
|
|
|
|
if (dx_norm) {
|
|
dx_norm->setIdentity(x.rows(), x.rows());
|
|
(*dx_norm) -= x * x.transpose() / xdotx;
|
|
(*dx_norm) /= norm_x;
|
|
|
|
if (ddx_norm) {
|
|
auto dx_norm_transpose = transposeGrad(*dx_norm, x.rows());
|
|
auto minus_ddx_norm_times_norm = matGradMultMat(
|
|
x_norm, x_norm.transpose(), (*dx_norm), dx_norm_transpose);
|
|
auto dnorm_inv = -x.transpose() / (xdotx * norm_x);
|
|
(*ddx_norm) = -minus_ddx_norm_times_norm / norm_x;
|
|
auto temp = (*dx_norm) * norm_x;
|
|
typename Derived::Index n = x.rows();
|
|
for (int col = 0; col < n; col++) {
|
|
auto column_as_matrix = (dnorm_inv(0, col) * temp);
|
|
for (int row_block = 0; row_block < n; row_block++) {
|
|
ddx_norm->block(row_block * n, col, n, 1) +=
|
|
column_as_matrix.col(row_block);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
} // namespace math
|
|
} // namespace drake
|