Simbody  3.5
SpatialAlgebra.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMMATRIX_SPATIAL_ALGEBRA_H_
2 #define SimTK_SIMMATRIX_SPATIAL_ALGEBRA_H_
3 
4 /* -------------------------------------------------------------------------- *
5  * Simbody(tm): SimTKcommon *
6  * -------------------------------------------------------------------------- *
7  * This is part of the SimTK biosimulation toolkit originating from *
8  * Simbios, the NIH National Center for Physics-Based Simulation of *
9  * Biological Structures at Stanford, funded under the NIH Roadmap for *
10  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11  * *
12  * Portions copyright (c) 2005-12 Stanford University and the Authors. *
13  * Authors: Michael Sherman *
14  * Contributors: *
15  * *
16  * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17  * not use this file except in compliance with the License. You may obtain a *
18  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19  * *
20  * Unless required by applicable law or agreed to in writing, software *
21  * distributed under the License is distributed on an "AS IS" BASIS, *
22  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23  * See the License for the specific language governing permissions and *
24  * limitations under the License. *
25  * -------------------------------------------------------------------------- */
26 
32 
33 #include <iostream>
34 
35 namespace SimTK {
36 
37 
68 typedef Vec<2, Vec3> SpatialVec;
70 typedef Row<2, Row3> SpatialRow;
72 typedef Mat<2,2, Mat33> SpatialMat;
73 
74 
75 // Pre-declare methods here so that we can list them in whatever order we'd
76 // like them to appear in Doxygen.
77 inline SpatialVec findRelativeVelocity( const Transform& X_FA,
78  const SpatialVec& V_FA,
79  const Transform& X_FB,
80  const SpatialVec& V_FB);
81 inline SpatialVec findRelativeVelocityInF( const Vec3& p_AB_F,
82  const SpatialVec& V_FA,
83  const SpatialVec& V_FB);
84 
85 inline SpatialVec findRelativeAcceleration( const Transform& X_FA,
86  const SpatialVec& V_FA,
87  const SpatialVec& A_FA,
88  const Transform& X_FB,
89  const SpatialVec& V_FB,
90  const SpatialVec& A_FB);
91 inline SpatialVec findRelativeAccelerationInF( const Vec3& p_AB_F,
92  const SpatialVec& V_FA,
93  const SpatialVec& A_FA,
94  const SpatialVec& V_FB,
95  const SpatialVec& A_FB);
96 
97 inline SpatialVec reverseRelativeVelocity(const Transform& X_AB,
98  const SpatialVec& V_AB);
99 inline SpatialVec reverseRelativeVelocityInA(const Transform& X_AB,
100  const SpatialVec& V_AB);
101 
102 inline SpatialVec shiftVelocityBy(const SpatialVec& V_AB, const Vec3& r_A);
103 inline SpatialVec shiftVelocityFromTo(const SpatialVec& V_A_BP,
104  const Vec3& fromP_A,
105  const Vec3& toQ_A);
106 
107 inline SpatialVec shiftForceBy(const SpatialVec& F_AP, const Vec3& r_A);
108 inline SpatialVec shiftForceFromTo(const SpatialVec& F_AP,
109  const Vec3& fromP_A,
110  const Vec3& toQ_A);
111 
112 
113 
114 //==============================================================================
115 // FIND RELATIVE VELOCITY
116 //==============================================================================
148 inline SpatialVec findRelativeVelocity(const Transform& X_FA,
149  const SpatialVec& V_FA,
150  const Transform& X_FB,
151  const SpatialVec& V_FB)
152 {
153  const Vec3 p_AB_F = X_FB.p() - X_FA.p(); // 3 flops
154  return ~X_FA.R()*findRelativeVelocityInF(p_AB_F,V_FA,V_FB); // 48 flops
155 }
156 
157 
158 
159 //==============================================================================
160 // FIND RELATIVE VELOCITY IN F
161 //==============================================================================
189 inline SpatialVec findRelativeVelocityInF(const Vec3& p_AB_F,
190  const SpatialVec& V_FA,
191  const SpatialVec& V_FB)
192 {
193  // Relative angular velocity of B in A, expressed in F.
194  const Vec3 w_AB_F = V_FB[0] - V_FA[0]; // 3 flops
195  // Relative linear velocity of B in A, taken and expressed in F.
196  const Vec3 p_AB_F_dot = V_FB[1] - V_FA[1]; // 3 flops
197  // Get linear velocity taken in A by removing the component due
198  // to A's rotation in F (still expressed in F).
199  const Vec3 v_AB_F = p_AB_F_dot - V_FA[0] % p_AB_F; // 12 flops
200 
201  return SpatialVec(w_AB_F, v_AB_F);
202 }
203 
204 
205 
206 //==============================================================================
207 // FIND RELATIVE ACCELERATION
208 //==============================================================================
245 inline SpatialVec findRelativeAcceleration( const Transform& X_FA,
246  const SpatialVec& V_FA,
247  const SpatialVec& A_FA,
248  const Transform& X_FB,
249  const SpatialVec& V_FB,
250  const SpatialVec& A_FB)
251 {
252  const Vec3 p_AB_F = X_FB.p() - X_FA.p(); // 3 flops
253  return ~X_FA.R() * // 30 flops
254  findRelativeAccelerationInF(p_AB_F,V_FA,A_FA,V_FB,A_FB); // 72 flops
255 }
256 
257 
258 
259 //==============================================================================
260 // FIND RELATIVE ACCELERATION IN F
261 //==============================================================================
295 inline SpatialVec findRelativeAccelerationInF( const Vec3& p_AB_F,
296  const SpatialVec& V_FA,
297  const SpatialVec& A_FA,
298  const SpatialVec& V_FB,
299  const SpatialVec& A_FB)
300 {
301  const Vec3& w_FA = V_FA[0]; // aliases for convenience
302  const Vec3& w_FB = V_FB[0];
303  const Vec3& b_FA = A_FA[0];
304  const Vec3& b_FB = A_FB[0];
305 
306  const Vec3 p_AB_F_dot = V_FB[1] - V_FA[1]; // d/dt p taken in F (3 flops)
307  const Vec3 p_AB_F_dotdot = A_FB[1] - A_FA[1]; // d^2/dt^2 taken in F (3 flops)
308 
309  const Vec3 w_AB_F = // relative angvel of B in A, exp. in F
310  w_FB - w_FA; // (3 flops)
311  const Vec3 v_AB_F = // d/dt p taken in A, exp in F
312  p_AB_F_dot - w_FA % p_AB_F; // (12 flops)
313 
314  const Vec3 w_AB_F_dot = b_FB - b_FA; // d/dt of w_AB_F taken in F (3 flops)
315  const Vec3 v_AB_F_dot = // d/dt v_AB_F taken in F
316  p_AB_F_dotdot - (b_FA % p_AB_F + w_FA % p_AB_F_dot); // (24 flops)
317 
318  // We have the derivative in F; change it to derivative in A by adding in
319  // contribution caused by motion of F in A, that is w_AF X w_AB_F. (Note
320  // that w_AF=-w_FA.)
321  const Vec3 b_AB_F = // ang. accel. of B in A, exp. in F
322  w_AB_F_dot - w_FA % w_AB_F; // (12 flops)
323  const Vec3 a_AB_F = // taken in A, exp. in F
324  v_AB_F_dot - w_FA % v_AB_F; // (12 flops)
325 
326  return SpatialVec(b_AB_F, a_AB_F); // taken in A, expressed in F
327 }
328 
329 
330 
331 //==============================================================================
332 // REVERSE RELATIVE VELOCITY
333 //==============================================================================
364 inline SpatialVec reverseRelativeVelocity(const Transform& X_AB,
365  const SpatialVec& V_AB)
366 {
367  // Reverse the velocity but with the result still expressed in A.
368  const SpatialVec V_BA_A = reverseRelativeVelocityInA(X_AB,V_AB);
369  // 21 flops
370  // Then reexpress in B.
371  return ~X_AB.R()*V_BA_A; // 30 flops
372 }
373 
374 
375 
376 //==============================================================================
377 // REVERSE RELATIVE VELOCITY IN A
378 //==============================================================================
408 inline SpatialVec reverseRelativeVelocityInA(const Transform& X_AB,
409  const SpatialVec& V_AB)
410 {
411  // Change the measurement point from a point coincident with OB
412  // to a point coincident with OA, and negate since we want A's velocity
413  // in B rather than the other way around.
414  const SpatialVec V_BA_A = -shiftVelocityBy(V_AB, -X_AB.p()); // 21 flops
415  return V_BA_A;
416 }
417 
418 
419 
420 //==============================================================================
421 // SHIFT VELOCITY BY
422 //==============================================================================
453 inline SpatialVec shiftVelocityBy(const SpatialVec& V_AB, const Vec3& r_A)
454 { return SpatialVec( V_AB[0], V_AB[1] + V_AB[0] % r_A ); } // vp=v + wXr
455 
456 
457 //==============================================================================
458 // SHIFT VELOCITY FROM TO
459 //==============================================================================
493 inline SpatialVec shiftVelocityFromTo(const SpatialVec& V_A_BP,
494  const Vec3& fromP_A,
495  const Vec3& toQ_A)
496 { return shiftVelocityBy(V_A_BP, toQ_A - fromP_A); }
497 
498 
499 
500 //==============================================================================
501 // SHIFT ACCELERATION BY
502 //==============================================================================
536 inline SpatialVec shiftAccelerationBy(const SpatialVec& A_AB,
537  const Vec3& w_AB,
538  const Vec3& r_A)
539 { return SpatialVec( A_AB[0],
540  A_AB[1] + A_AB[0] % r_A + w_AB % (w_AB % r_A) ); }
541 
542 
543 
544 //==============================================================================
545 // SHIFT ACCELERATION FROM TO
546 //==============================================================================
585 inline SpatialVec shiftAccelerationFromTo(const SpatialVec& A_A_BP,
586  const Vec3& w_AB,
587  const Vec3& fromP_A,
588  const Vec3& toQ_A)
589 { return shiftAccelerationBy(A_A_BP, w_AB, toQ_A - fromP_A); }
590 
591 
592 
593 //==============================================================================
594 // SHIFT FORCE BY
595 //==============================================================================
625 inline SpatialVec shiftForceBy(const SpatialVec& F_AP, const Vec3& r_A)
626 { return SpatialVec(F_AP[0] - r_A % F_AP[1], F_AP[1]); } // mq = mp - r X f
627 
628 
629 
630 //==============================================================================
631 // SHIFT FORCE FROM TO
632 //==============================================================================
668 inline SpatialVec shiftForceFromTo(const SpatialVec& F_AP,
669  const Vec3& fromP_A,
670  const Vec3& toQ_A)
671 { return shiftForceBy(F_AP, toQ_A - fromP_A); }
672 
677 //==============================================================================
678 // PHI MATRIX
679 //==============================================================================
680 // support for efficient matrix multiplication involving the special phi
681 // matrix
682 
683 class PhiMatrixTranspose;
684 
685 class PhiMatrix {
686 public:
688 
690  PhiMatrix(const Vec3& l) : l_(l) {}
691 
692  void setToZero() { l_ = 0; }
693  void setToNaN() { l_.setToNaN(); }
694 
695  SpatialMat toSpatialMat() const {
696  return SpatialMat(Mat33(1), crossMat(l_),
697  Mat33(0), Mat33(1));
698  }
699 
700  const Vec3& l() const { return l_; }
701 private:
702  Vec3 l_;
703 };
704 
706 public:
707  PhiMatrixTranspose(const PhiMatrix& phi) : phi(phi) {}
708 
709  SpatialMat toSpatialMat() const {
710  return SpatialMat( Mat33(1) , Mat33(0),
711  crossMat(-l()) , Mat33(1));
712  }
713 
714  const Vec3& l() const {return phi.l();}
715 private:
716  const PhiMatrix& phi;
717 };
718 
719 inline PhiMatrixTranspose
720 transpose(const PhiMatrix& phi)
721 {
722  PhiMatrixTranspose ret(phi);
723  return ret;
724 }
725 
726 inline PhiMatrixTranspose
727 operator~(const PhiMatrix& phi) {return transpose(phi);}
728 
729 inline SpatialVec
730 operator*(const PhiMatrix& phi,
731  const SpatialVec& v)
732 {
733  return SpatialVec(v[0] + phi.l() % v[1], // 12 flops
734  v[1]);
735 }
736 
737 inline SpatialMat
738 operator*(const PhiMatrix& phi,
739  const SpatialMat& m)
740 {
741  const Mat33 x = crossMat(phi.l()); // 3 flops
742  return SpatialMat( m(0,0) + x*m(1,0), m(0,1) + x*m(1,1), // 108 flops
743  m(1,0) , m(1,1));
744 }
745 
746 inline SpatialVec
748  const SpatialVec& v)
749 {
750  return SpatialVec(v[0],
751  v[1] + v[0] % phiT.l()); // 12 flops
752 }
753 
754 
755 inline SpatialMat
757  const PhiMatrixTranspose& phiT)
758 {
759  const Mat33 x = crossMat(phiT.l()); // 3 flops
760  return SpatialMat( m(0,0) - m(0,1) * x, m(0,1), // 54 flops
761  m(1,0) - m(1,1) * x, m(1,1) ); // 54 flops
762 }
763 
764 inline SpatialMat
765 operator*(const SpatialMat& m,
766  const PhiMatrixTranspose& phiT)
767 {
768  const Mat33 x = crossMat(phiT.l()); // 3 flops
769  return SpatialMat( m(0,0) - m(0,1) * x, m(0,1), // 54 flops
770  m(1,0) - m(1,1) * x, m(1,1) ); // 54 flops
771 }
772 
773 inline bool
774 operator==(const PhiMatrix& p1, const PhiMatrix& p2)
775 {
776  return p1.l() == p2.l();
777 }
778 
779 inline bool
781 {
782  return p1.l() == p2.l();
783 }
784 } // namespace SimTK
785 
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
This class represents the rotate-and-shift transform which gives the location and orientation of a ne...
Definition: Transform.h:43
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 ...
const Vec< 3, P > & p() const
Return a read-only reference to our translation vector p_BF.
Definition: Transform.h:239
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
const Rotation_< P > & R() const
Return a read-only reference to the contained rotation R_BF.
Definition: Transform.h:215
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