1 #ifndef SimTK_SIMMATRIX_SPATIAL_ALGEBRA_H_ 2 #define SimTK_SIMMATRIX_SPATIAL_ALGEBRA_H_ 153 const Vec3 p_AB_F = X_FB.
p() - X_FA.
p();
194 const Vec3 w_AB_F = V_FB[0] - V_FA[0];
196 const Vec3 p_AB_F_dot = V_FB[1] - V_FA[1];
199 const Vec3 v_AB_F = p_AB_F_dot - V_FA[0] % p_AB_F;
252 const Vec3 p_AB_F = X_FB.
p() - X_FA.
p();
301 const Vec3& w_FA = V_FA[0];
302 const Vec3& w_FB = V_FB[0];
303 const Vec3& b_FA = A_FA[0];
304 const Vec3& b_FB = A_FB[0];
306 const Vec3 p_AB_F_dot = V_FB[1] - V_FA[1];
307 const Vec3 p_AB_F_dotdot = A_FB[1] - A_FA[1];
312 p_AB_F_dot - w_FA % p_AB_F;
314 const Vec3 w_AB_F_dot = b_FB - b_FA;
315 const Vec3 v_AB_F_dot =
316 p_AB_F_dotdot - (b_FA % p_AB_F + w_FA % p_AB_F_dot);
322 w_AB_F_dot - w_FA % w_AB_F;
324 v_AB_F_dot - w_FA % v_AB_F;
371 return ~X_AB.
R()*V_BA_A;
454 {
return SpatialVec( V_AB[0], V_AB[1] + V_AB[0] % r_A ); }
540 A_AB[1] + A_AB[0] % r_A + w_AB % (w_AB % r_A) ); }
626 {
return SpatialVec(F_AP[0] - r_A % F_AP[1], F_AP[1]); }
683 class PhiMatrixTranspose;
700 const Vec3&
l()
const {
return l_; }
719 inline PhiMatrixTranspose
726 inline PhiMatrixTranspose
742 return SpatialMat( m(0,0) + x*m(1,0), m(0,1) + x*m(1,1),
752 m(1,0), m(1,0)*x + m(1,1) );
760 v[1] + v[0] % phiT.
l());
769 m(1,0)-x*m(0,0), m(1,1)-x*m(0,1));
777 return SpatialMat( m(0,0) - m(0,1) * x, m(0,1),
778 m(1,0) - m(1,1) * x, m(1,1) );
786 return SpatialMat( m(0,0) - m(0,1) * x, m(0,1),
787 m(1,0) - m(1,1) * x, m(1,1) );
793 return p1.
l() == p2.
l();
799 return p1.
l() == p2.
l();
803 #endif // SimTK_SIMMATRIX_SPATIAL_ALGEBRA_H_ Vec< 2, Vec3 > SpatialVec
Spatial vectors are used for (rotation,translation) quantities and consist of a pair of Vec3 objects...
Definition: MassProperties.h:50
PhiMatrixTranspose transpose(const PhiMatrix &phi)
Definition: SpatialAlgebra.h:720
SpatialVec reverseRelativeVelocityInA(const Transform &X_AB, const SpatialVec &V_AB)
Given the relative velocity of frame B in frame A, reverse that to give the relative velocity of fram...
Definition: SpatialAlgebra.h:408
SpatialVec shiftVelocityBy(const SpatialVec &V_AB, const Vec3 &r_A)
Shift a relative spatial velocity measured at some point to that same relative spatial quantity but m...
Definition: SpatialAlgebra.h:453
PhiMatrixTranspose operator~(const PhiMatrix &phi)
Definition: SpatialAlgebra.h:727
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
SpatialVec findRelativeVelocity(const Transform &X_FA, const SpatialVec &V_FA, const Transform &X_FB, const SpatialVec &V_FB)
Find the relative spatial velocity between two frames A and B whose individual spatial velocities are...
Definition: SpatialAlgebra.h:148
void setToNaN()
Definition: SpatialAlgebra.h:693
SpatialMat toSpatialMat() const
Definition: SpatialAlgebra.h:709
Definition: SpatialAlgebra.h:705
Row< 2, Row3 > SpatialRow
This is the type of a transposed SpatialVec; it does not usually appear explicitly in user programs...
Definition: MassProperties.h:60
bool operator==(const PhiMatrix &p1, const PhiMatrix &p2)
Definition: SpatialAlgebra.h:791
SpatialVec findRelativeVelocityInF(const Vec3 &p_AB_F, const SpatialVec &V_FA, const SpatialVec &V_FB)
Find the relative spatial velocity between two frames A and B whose individual spatial velocities are...
Definition: SpatialAlgebra.h:189
Definition: SpatialAlgebra.h:685
PhiMatrixTranspose(const PhiMatrix &phi)
Definition: SpatialAlgebra.h:707
const Vec3 & l() const
Definition: SpatialAlgebra.h:714
const Vec3 & l() const
Definition: SpatialAlgebra.h:700
void setToNaN()
Set every scalar in this Vec to NaN; this is the default initial value in Debug builds, but not in Release.
Definition: Vec.h:812
Matrix_< E > operator*(const MatrixBase< E > &l, const typename CNT< E >::StdNumber &r)
Definition: BigMatrix.h:605
SpatialVec shiftAccelerationFromTo(const SpatialVec &A_A_BP, const Vec3 &w_AB, const Vec3 &fromP_A, const Vec3 &toQ_A)
Shift a relative spatial acceleration measured at some point P to that same relative spatial quantity...
Definition: SpatialAlgebra.h:585
Mat< 3, 3 > Mat33
This is the most common 3x3 matrix type: three packed columns of 3 Real values each.
Definition: SmallMatrix.h:142
SpatialVec shiftAccelerationBy(const SpatialVec &A_AB, const Vec3 &w_AB, const Vec3 &r_A)
Shift a relative spatial acceleration measured at some point to that same relative spatial quantity b...
Definition: SpatialAlgebra.h:536
void setToZero()
Definition: SpatialAlgebra.h:692
This file is the user-includeable header to be included in user programs to provide fixed-length Vec ...
Mat< 2, 2, Mat33 > SpatialMat
Spatial matrices are used to hold 6x6 matrices that are best viewed as 2x2 matrices of 3x3 matrices; ...
Definition: MassProperties.h:72
Mat< 3, 3, E > crossMat(const Vec< 3, E, S > &v)
Calculate matrix M(v) such that M(v)*w = v % w.
Definition: SmallMatrixMixed.h:649
SpatialVec shiftVelocityFromTo(const SpatialVec &V_A_BP, const Vec3 &fromP_A, const Vec3 &toQ_A)
Shift a relative spatial velocity measured at some point P to that same relative spatial quantity but...
Definition: SpatialAlgebra.h:493
PhiMatrix(const Vec3 &l)
Definition: SpatialAlgebra.h:690
PhiMatrix()
Definition: SpatialAlgebra.h:689
Vec< 3 > Vec3
This is the most common 3D vector type: a column of 3 Real values stored consecutively in memory (pac...
Definition: SmallMatrix.h:129
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:620
SpatialVec shiftForceFromTo(const SpatialVec &F_AP, const Vec3 &fromP_A, const Vec3 &toQ_A)
Shift a spatial force applied at some point P of a body to that same spatial force applied at a new p...
Definition: SpatialAlgebra.h:668
SpatialVec findRelativeAcceleration(const Transform &X_FA, const SpatialVec &V_FA, const SpatialVec &A_FA, const Transform &X_FB, const SpatialVec &V_FB, const SpatialVec &A_FB)
Find the relative spatial acceleration between two frames A and B whose individual spatial accelerati...
Definition: SpatialAlgebra.h:245
SpatialVec findRelativeAccelerationInF(const Vec3 &p_AB_F, const SpatialVec &V_FA, const SpatialVec &A_FA, const SpatialVec &V_FB, const SpatialVec &A_FB)
Find the relative spatial acceleration between two frames A and B whose individual spatial accelerati...
Definition: SpatialAlgebra.h:295
SpatialMat toSpatialMat() const
Definition: SpatialAlgebra.h:695
PhiMatrixTranspose TransposeType
Definition: SpatialAlgebra.h:687
SpatialVec shiftForceBy(const SpatialVec &F_AP, const Vec3 &r_A)
Shift a spatial force applied at some point of a body to that same spatial force applied at a new poi...
Definition: SpatialAlgebra.h:625
Transform_< Real > Transform
Definition: Transform.h:44
SpatialVec reverseRelativeVelocity(const Transform &X_AB, const SpatialVec &V_AB)
Given the relative velocity of frame B in frame A, reverse that to give the relative velocity of fram...
Definition: SpatialAlgebra.h:364