26 using namespace Eigen;
30 const static bool mhi=
true;
40 Vector3d c_eig=Map<const Vector3d>(c_.
data);
41 Map<Matrix3d>(
I.
data)=Map<const Matrix3d>(Ic.
data)-m_*(c_eig*c_eig.transpose()-c_eig.dot(c_eig)*Matrix3d::Identity());
62 Vector3d r_eig = Map<Vector3d>(X.
p.
data);
63 Vector3d h_eig = Map<const Vector3d>(I.
h.
data);
64 Vector3d hmr_eig = Map<Vector3d>(hmr.
data);
65 Matrix3d rcrosshcross = h_eig *r_eig.transpose()-r_eig.
dot(h_eig)*Matrix3d::Identity();
66 Matrix3d hmrcrossrcross = r_eig*hmr_eig.transpose()-hmr_eig.dot(r_eig)*Matrix3d::Identity();
67 Matrix3d R = Map<Matrix3d>(X.
M.
data);
69 Map<Matrix3d>(Ib.data) = R*((Map<const Matrix3d>(I.
I.
data)+rcrosshcross+hmrcrossrcross)*R.transpose());
78 Matrix3d R = Map<const Matrix3d>(M.
data);
80 Map<Matrix3d>(Ib.data) = R.transpose()*(Map<const Matrix3d>(I.
I.
data)*R);
90 Vector3d r_eig = Map<const Vector3d>(p.
data);
91 Vector3d h_eig = Map<Vector3d>(this->
h.
data);
92 Vector3d hmr_eig = Map<Vector3d>(hmr.
data);
93 Matrix3d rcrosshcross = h_eig * r_eig.transpose()-r_eig.
dot(h_eig)*Matrix3d::Identity();
94 Matrix3d hmrcrossrcross = r_eig*hmr_eig.transpose()-hmr_eig.dot(r_eig)*Matrix3d::Identity();
96 Map<Matrix3d>(Ib.
data) = Map<Matrix3d>(this->
I.
data)+rcrosshcross+hmrcrossrcross;
represents rotations in 3 dimensional space.
Vector vel
The velocity of that point.
friend double dot(const Vector &lhs, const Vector &rhs)
6D Inertia of a rigid body
RigidBodyInertia RefPoint(const Vector &p)
Rotation M
Orientation of the Frame.
represents both translational and rotational velocities.
A concrete implementation of a 3 dimensional vector class.
Vector rot
The rotational velocity of that point.
RigidBodyInertia(double m=0, const Vector &oc=Vector::Zero(), const RotationalInertia &Ic=RotationalInertia::Zero())
Vector p
origine of the Frame
Frame Inverse() const
Gives back inverse transformation of a Frame.
represents a frame transformation in 3D space (rotation + translation)
represents both translational and rotational acceleration.
friend RigidBodyInertia operator+(const RigidBodyInertia &Ia, const RigidBodyInertia &Ib)
friend RigidBodyInertia operator*(double a, const RigidBodyInertia &I)