Program Listing for File symmetric3.hpp
↰ Return to documentation for file (include/pinocchio/spatial/symmetric3.hpp
)
//
// Copyright (c) 2014-2021 CNRS INRIA
//
#ifndef __pinocchio_spatial_symmetric3__
#define __pinocchio_spatial_symmetric3__
#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/math/matrix.hpp"
namespace pinocchio
{
template<typename _Scalar, int _Options>
struct traits<Symmetric3Tpl<_Scalar, _Options>>
{
typedef _Scalar Scalar;
};
template<typename _Scalar, int _Options>
class Symmetric3Tpl : public NumericalBase<Symmetric3Tpl<_Scalar, _Options>>
{
public:
typedef _Scalar Scalar;
enum
{
Options = _Options
};
typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6;
typedef Eigen::Matrix<Scalar, 3, 3, Options> Matrix3;
typedef Eigen::Matrix<Scalar, 2, 2, Options> Matrix2;
typedef Eigen::Matrix<Scalar, 3, 2, Options> Matrix32;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
public:
Symmetric3Tpl()
{
}
template<typename Sc, int Opt>
explicit Symmetric3Tpl(const Eigen::Matrix<Sc, 3, 3, Opt> & I)
{
assert(check_expression_if_real<Scalar>(pinocchio::isZero((I - I.transpose()))));
m_data(0) = I(0, 0);
m_data(1) = I(1, 0);
m_data(2) = I(1, 1);
m_data(3) = I(2, 0);
m_data(4) = I(2, 1);
m_data(5) = I(2, 2);
}
explicit Symmetric3Tpl(const Vector6 & I)
: m_data(I)
{
}
Symmetric3Tpl(const Symmetric3Tpl & other)
{
*this = other;
}
template<typename S2, int O2>
explicit Symmetric3Tpl(const Symmetric3Tpl<S2, O2> & other)
{
*this = other.template cast<Scalar>();
}
Symmetric3Tpl & operator=(const Symmetric3Tpl & clone) // Copy assignment operator
{
m_data = clone.m_data;
return *this;
}
Symmetric3Tpl(
const Scalar & a0,
const Scalar & a1,
const Scalar & a2,
const Scalar & a3,
const Scalar & a4,
const Scalar & a5)
{
m_data << a0, a1, a2, a3, a4, a5;
}
static Symmetric3Tpl Zero()
{
return Symmetric3Tpl(Vector6::Zero());
}
void setZero()
{
m_data.setZero();
}
static Symmetric3Tpl Random()
{
return RandomPositive();
}
void setRandom()
{
Scalar a = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
b = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
c = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
d = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
e = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
f = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0;
m_data << a, b, c, d, e, f;
}
static Symmetric3Tpl Identity()
{
return Symmetric3Tpl(Scalar(1), Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(1));
}
void setIdentity()
{
m_data << Scalar(1), Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(1);
}
template<typename Vector3Like>
void setDiagonal(const Eigen::MatrixBase<Vector3Like> & diag)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
m_data[0] = diag[0];
m_data[2] = diag[1];
m_data[5] = diag[2];
}
/* Required by Inertia::operator== */
bool operator==(const Symmetric3Tpl & other) const
{
return m_data == other.m_data;
}
bool operator!=(const Symmetric3Tpl & other) const
{
return !(*this == other);
}
bool isApprox(
const Symmetric3Tpl & other,
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
{
return m_data.isApprox(other.m_data, prec);
}
bool isZero(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
{
return m_data.isZero(prec);
}
void fill(const Scalar value)
{
m_data.fill(value);
}
template<typename Matrix3Like>
void inverse(const Eigen::MatrixBase<Matrix3Like> & res_) const
{
Matrix3Like & res = res_.const_cast_derived();
const Scalar &a11 = m_data[0], a21 = m_data[1], a22 = m_data[2], a31 = m_data[3],
a32 = m_data[4], a33 = m_data[5];
res(0, 0) = a33 * a22 - a32 * a32;
res(1, 0) = res(0, 1) = -(a33 * a21 - a32 * a31);
res(2, 0) = res(0, 2) = a32 * a21 - a22 * a31;
res(1, 1) = a33 * a11 - a31 * a31;
res(2, 1) = res(1, 2) = -(a32 * a11 - a21 * a31);
res(2, 2) = a22 * a11 - a21 * a21;
const Scalar det = a11 * res(0, 0) + a21 * res(0, 1) + a31 * res(0, 2);
res /= det;
}
Matrix3 inverse() const
{
Matrix3 res;
inverse(res);
return res;
}
struct SkewSquare
{
const Vector3 & v;
SkewSquare(const Vector3 & v)
: v(v)
{
}
operator Symmetric3Tpl() const
{
const Scalar &x = v[0], &y = v[1], &z = v[2];
return Symmetric3Tpl(-y * y - z * z, x * y, -x * x - z * z, x * z, y * z, -x * x - y * y);
}
}; // struct SkewSquare
Symmetric3Tpl operator-(const SkewSquare & v) const
{
const Scalar &x = v.v[0], &y = v.v[1], &z = v.v[2];
return Symmetric3Tpl(
m_data[0] + y * y + z * z, m_data[1] - x * y, m_data[2] + x * x + z * z, m_data[3] - x * z,
m_data[4] - y * z, m_data[5] + x * x + y * y);
}
Symmetric3Tpl & operator-=(const SkewSquare & v)
{
const Scalar &x = v.v[0], &y = v.v[1], &z = v.v[2];
m_data[0] += y * y + z * z;
m_data[1] -= x * y;
m_data[2] += x * x + z * z;
m_data[3] -= x * z;
m_data[4] -= y * z;
m_data[5] += x * x + y * y;
return *this;
}
struct AlphaSkewSquare
{
const Scalar & m;
const Vector3 & v;
AlphaSkewSquare(const Scalar & m, const SkewSquare & v)
: m(m)
, v(v.v)
{
}
AlphaSkewSquare(const Scalar & m, const Vector3 & v)
: m(m)
, v(v)
{
}
operator Symmetric3Tpl() const
{
const Scalar &x = v[0], &y = v[1], &z = v[2];
return Symmetric3Tpl(
-m * (y * y + z * z), m * x * y, -m * (x * x + z * z), m * x * z, m * y * z,
-m * (x * x + y * y));
}
};
friend AlphaSkewSquare operator*(const Scalar & m, const SkewSquare & sk)
{
return AlphaSkewSquare(m, sk);
}
Symmetric3Tpl operator-(const AlphaSkewSquare & v) const
{
const Scalar &x = v.v[0], &y = v.v[1], &z = v.v[2];
return Symmetric3Tpl(
m_data[0] + v.m * (y * y + z * z), m_data[1] - v.m * x * y,
m_data[2] + v.m * (x * x + z * z), m_data[3] - v.m * x * z, m_data[4] - v.m * y * z,
m_data[5] + v.m * (x * x + y * y));
}
Symmetric3Tpl & operator-=(const AlphaSkewSquare & v)
{
const Scalar &x = v.v[0], &y = v.v[1], &z = v.v[2];
m_data[0] += v.m * (y * y + z * z);
m_data[1] -= v.m * x * y;
m_data[2] += v.m * (x * x + z * z);
m_data[3] -= v.m * x * z;
m_data[4] -= v.m * y * z;
m_data[5] += v.m * (x * x + y * y);
return *this;
}
const Vector6 & data() const
{
return m_data;
}
Vector6 & data()
{
return m_data;
}
// static Symmetric3Tpl SkewSq( const Vector3 & v )
// {
// const Scalar & x = v[0], & y = v[1], & z = v[2];
// return Symmetric3Tpl(-y*y-z*z,
// x*y, -x*x-z*z,
// x*z, y*z, -x*x-y*y );
// }
/* Shoot a positive definite matrix. */
static Symmetric3Tpl RandomPositive()
{
Scalar a = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
b = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
c = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
d = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
e = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
f = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0;
return Symmetric3Tpl(
a * a + b * b + d * d, a * b + b * c + d * e, b * b + c * c + e * e, a * d + b * e + d * f,
b * d + c * e + e * f, d * d + e * e + f * f);
}
Matrix3 matrix() const
{
Matrix3 res;
res(0, 0) = m_data(0);
res(0, 1) = m_data(1);
res(0, 2) = m_data(3);
res(1, 0) = m_data(1);
res(1, 1) = m_data(2);
res(1, 2) = m_data(4);
res(2, 0) = m_data(3);
res(2, 1) = m_data(4);
res(2, 2) = m_data(5);
return res;
}
operator Matrix3() const
{
return matrix();
}
Scalar vtiv(const Vector3 & v) const
{
const Scalar & x = v[0];
const Scalar & y = v[1];
const Scalar & z = v[2];
const Scalar xx = x * x;
const Scalar xy = x * y;
const Scalar xz = x * z;
const Scalar yy = y * y;
const Scalar yz = y * z;
const Scalar zz = z * z;
return m_data(0) * xx + m_data(2) * yy + m_data(5) * zz
+ 2. * (m_data(1) * xy + m_data(3) * xz + m_data(4) * yz);
}
template<typename Vector3, typename Matrix3>
static void vxs(
const Eigen::MatrixBase<Vector3> & v,
const Symmetric3Tpl & S3,
const Eigen::MatrixBase<Matrix3> & M)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3, 3);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3, 3, 3);
const Scalar & a = S3.data()[0];
const Scalar & b = S3.data()[1];
const Scalar & c = S3.data()[2];
const Scalar & d = S3.data()[3];
const Scalar & e = S3.data()[4];
const Scalar & f = S3.data()[5];
const typename Vector3::RealScalar & v0 = v[0];
const typename Vector3::RealScalar & v1 = v[1];
const typename Vector3::RealScalar & v2 = v[2];
Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3, M);
M_(0, 0) = d * v1 - b * v2;
M_(1, 0) = a * v2 - d * v0;
M_(2, 0) = b * v0 - a * v1;
M_(0, 1) = e * v1 - c * v2;
M_(1, 1) = b * v2 - e * v0;
M_(2, 1) = c * v0 - b * v1;
M_(0, 2) = f * v1 - e * v2;
M_(1, 2) = d * v2 - f * v0;
M_(2, 2) = e * v0 - d * v1;
}
template<typename Vector3>
Matrix3 vxs(const Eigen::MatrixBase<Vector3> & v) const
{
Matrix3 M;
vxs(v, *this, M);
return M;
}
template<typename Vector3, typename Matrix3>
static void svx(
const Eigen::MatrixBase<Vector3> & v,
const Symmetric3Tpl & S3,
const Eigen::MatrixBase<Matrix3> & M)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3, 3);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3, 3, 3);
const Scalar & a = S3.data()[0];
const Scalar & b = S3.data()[1];
const Scalar & c = S3.data()[2];
const Scalar & d = S3.data()[3];
const Scalar & e = S3.data()[4];
const Scalar & f = S3.data()[5];
const typename Vector3::RealScalar & v0 = v[0];
const typename Vector3::RealScalar & v1 = v[1];
const typename Vector3::RealScalar & v2 = v[2];
Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3, M);
M_(0, 0) = b * v2 - d * v1;
M_(1, 0) = c * v2 - e * v1;
M_(2, 0) = e * v2 - f * v1;
M_(0, 1) = d * v0 - a * v2;
M_(1, 1) = e * v0 - b * v2;
M_(2, 1) = f * v0 - d * v2;
M_(0, 2) = a * v1 - b * v0;
M_(1, 2) = b * v1 - c * v0;
M_(2, 2) = d * v1 - e * v0;
}
template<typename Vector3>
Matrix3 svx(const Eigen::MatrixBase<Vector3> & v) const
{
Matrix3 M;
svx(v, *this, M);
return M;
}
Symmetric3Tpl operator+(const Symmetric3Tpl & s2) const
{
return Symmetric3Tpl(m_data + s2.m_data);
}
Symmetric3Tpl operator-(const Symmetric3Tpl & s2) const
{
return Symmetric3Tpl(m_data - s2.m_data);
}
Symmetric3Tpl & operator+=(const Symmetric3Tpl & s2)
{
m_data += s2.m_data;
return *this;
}
Symmetric3Tpl & operator-=(const Symmetric3Tpl & s2)
{
m_data -= s2.m_data;
return *this;
}
Symmetric3Tpl & operator*=(const Scalar s)
{
m_data *= s;
return *this;
}
template<typename V3in, typename V3out>
static void rhsMult(
const Symmetric3Tpl & S3,
const Eigen::MatrixBase<V3in> & vin,
const Eigen::MatrixBase<V3out> & vout)
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(V3in, Vector3);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(V3out, Vector3);
V3out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3out, vout);
vout_[0] = S3.m_data(0) * vin[0] + S3.m_data(1) * vin[1] + S3.m_data(3) * vin[2];
vout_[1] = S3.m_data(1) * vin[0] + S3.m_data(2) * vin[1] + S3.m_data(4) * vin[2];
vout_[2] = S3.m_data(3) * vin[0] + S3.m_data(4) * vin[1] + S3.m_data(5) * vin[2];
}
template<typename V3>
Vector3 operator*(const Eigen::MatrixBase<V3> & v) const
{
Vector3 res;
rhsMult(*this, v, res);
return res;
}
// Matrix3 operator*(const Matrix3 &a) const
// {
// Matrix3 r;
// for(unsigned int i=0; i<3; ++i)
// {
// r(0,i) = m_data(0) * a(0,i) + m_data(1) * a(1,i) + m_data(3) * a(2,i);
// r(1,i) = m_data(1) * a(0,i) + m_data(2) * a(1,i) + m_data(4) * a(2,i);
// r(2,i) = m_data(3) * a(0,i) + m_data(4) * a(1,i) + m_data(5) * a(2,i);
// }
// return r;
// }
const Scalar & operator()(const int i, const int j) const
{
return ((i != 2) && (j != 2)) ? m_data[i + j] : m_data[i + j + 1];
}
template<typename Matrix3Like>
Symmetric3Tpl operator-(const Eigen::MatrixBase<Matrix3Like> & S) const
{
assert(check_expression_if_real<Scalar>(pinocchio::isZero(S - S.transpose())));
return Symmetric3Tpl(
m_data(0) - S(0, 0), m_data(1) - S(1, 0), m_data(2) - S(1, 1), m_data(3) - S(2, 0),
m_data(4) - S(2, 1), m_data(5) - S(2, 2));
}
template<typename Matrix3Like>
Symmetric3Tpl operator+(const Eigen::MatrixBase<Matrix3Like> & S) const
{
assert(check_expression_if_real<Scalar>(pinocchio::isZero(S - S.transpose())));
return Symmetric3Tpl(
m_data(0) + S(0, 0), m_data(1) + S(1, 0), m_data(2) + S(1, 1), m_data(3) + S(2, 0),
m_data(4) + S(2, 1), m_data(5) + S(2, 2));
}
/* --- Symmetric R*S*R' and R'*S*R products --- */
public: // private:
Matrix32 decomposeltI() const
{
Matrix32 L;
L << m_data(0) - m_data(5), m_data(1), m_data(1), m_data(2) - m_data(5), 2 * m_data(3),
m_data(4) + m_data(4);
return L;
}
/* R*S*R' */
template<typename D>
Symmetric3Tpl rotate(const Eigen::MatrixBase<D> & R) const
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(D, 3, 3);
assert(
check_expression_if_real<Scalar>(isUnitary(R.transpose() * R))
&& "R is not a Unitary matrix");
Symmetric3Tpl Sres;
// 4 a
const Matrix32 L(decomposeltI());
// Y = R' L ===> (12 m + 8 a)
const Matrix2 Y(R.template block<2, 3>(1, 0) * L);
// Sres= Y R ===> (16 m + 8a)
Sres.m_data(1) = Y(0, 0) * R(0, 0) + Y(0, 1) * R(0, 1);
Sres.m_data(2) = Y(0, 0) * R(1, 0) + Y(0, 1) * R(1, 1);
Sres.m_data(3) = Y(1, 0) * R(0, 0) + Y(1, 1) * R(0, 1);
Sres.m_data(4) = Y(1, 0) * R(1, 0) + Y(1, 1) * R(1, 1);
Sres.m_data(5) = Y(1, 0) * R(2, 0) + Y(1, 1) * R(2, 1);
// r=R' v ( 6m + 3a)
const Vector3 r(
-R(0, 0) * m_data(4) + R(0, 1) * m_data(3), -R(1, 0) * m_data(4) + R(1, 1) * m_data(3),
-R(2, 0) * m_data(4) + R(2, 1) * m_data(3));
// Sres_11 (3a)
Sres.m_data(0) = L(0, 0) + L(1, 1) - Sres.m_data(2) - Sres.m_data(5);
// Sres + D + (Ev)x ( 9a)
Sres.m_data(0) += m_data(5);
Sres.m_data(1) += r(2);
Sres.m_data(2) += m_data(5);
Sres.m_data(3) -= r(1);
Sres.m_data(4) += r(0);
Sres.m_data(5) += m_data(5);
return Sres;
}
template<typename NewScalar>
Symmetric3Tpl<NewScalar, Options> cast() const
{
return Symmetric3Tpl<NewScalar, Options>(m_data.template cast<NewScalar>());
}
friend std::ostream & operator<<(std::ostream & os, const Symmetric3Tpl<Scalar, Options> & S3)
{
os << "m_data: " << S3.m_data.transpose() << "\n";
return os;
}
// TODO: adjust code
// bool isValid() const
// {
// return
// m_data(0) >= Scalar(0)
// && m_data(2) >= Scalar(0)
// && m_data(5) >= Scalar(0);
// }
protected:
Vector6 m_data;
}; // class Symmetric3Tpl
} // namespace pinocchio
#endif // ifndef __pinocchio_spatial_symmetric3__