1 #ifndef SimTK_SIMMATRIX_SPATIAL_ALGEBRA_H_ 2 #define SimTK_SIMMATRIX_SPATIAL_ALGEBRA_H_ 78 const SpatialVec& V_FA,
80 const SpatialVec& V_FB);
82 const SpatialVec& V_FA,
83 const SpatialVec& V_FB);
86 const SpatialVec& V_FA,
87 const SpatialVec& A_FA,
89 const SpatialVec& V_FB,
90 const SpatialVec& A_FB);
92 const SpatialVec& V_FA,
93 const SpatialVec& A_FA,
94 const SpatialVec& V_FB,
95 const SpatialVec& A_FB);
98 const SpatialVec& V_AB);
100 const SpatialVec& V_AB);
149 const SpatialVec& V_FA,
151 const SpatialVec& V_FB)
153 const Vec3 p_AB_F = X_FB.
p() - X_FA.
p();
190 const SpatialVec& V_FA,
191 const SpatialVec& V_FB)
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;
246 const SpatialVec& V_FA,
247 const SpatialVec& A_FA,
249 const SpatialVec& V_FB,
250 const SpatialVec& A_FB)
252 const Vec3 p_AB_F = X_FB.
p() - X_FA.
p();
296 const SpatialVec& V_FA,
297 const SpatialVec& A_FA,
298 const SpatialVec& V_FB,
299 const SpatialVec& A_FB)
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;
365 const SpatialVec& V_AB)
371 return ~X_AB.
R()*V_BA_A;
409 const SpatialVec& V_AB)
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]); }
700 const Vec3&
l()
const {
return l_; }
714 const Vec3&
l()
const {
return phi.l();}
742 return SpatialMat( m(0,0) + x*m(1,0), m(0,1) + x*m(1,1),
751 v[1] + v[0] % phiT.
l());
760 return SpatialMat( m(0,0) - m(0,1) * x, m(0,1),
761 m(1,0) - m(1,1) * x, m(1,1) );
769 return SpatialMat( m(0,0) - m(0,1) * x, m(0,1),
770 m(1,0) - m(1,1) * x, m(1,1) );
776 return p1.
l() == p2.
l();
782 return p1.
l() == p2.
l();
786 #endif // SimTK_SIMMATRIX_SPATIAL_ALGEBRA_H_ const Vec3 & l() const
Definition: SpatialAlgebra.h:700
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
const Vec3 & l() const
Definition: SpatialAlgebra.h:714
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:774
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
SpatialMat toSpatialMat() const
Definition: SpatialAlgebra.h:709
Definition: SpatialAlgebra.h:685
PhiMatrixTranspose(const PhiMatrix &phi)
Definition: SpatialAlgebra.h:707
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:810
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
SpatialMat toSpatialMat() const
Definition: SpatialAlgebra.h:695
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:607
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
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