Simbody 3.7
Loading...
Searching...
No Matches
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
35namespace SimTK {
36
37
68typedef Vec<2, Vec3> SpatialVec;
70typedef Row<2, Row3> SpatialRow;
72typedef 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.
77inline SpatialVec findRelativeVelocity( const Transform& X_FA,
78 const SpatialVec& V_FA,
79 const Transform& X_FB,
80 const SpatialVec& V_FB);
81inline SpatialVec findRelativeVelocityInF( const Vec3& p_AB_F,
82 const SpatialVec& V_FA,
83 const SpatialVec& V_FB);
84
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);
91inline 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
98 const SpatialVec& V_AB);
100 const SpatialVec& V_AB);
101
102inline SpatialVec shiftVelocityBy(const SpatialVec& V_AB, const Vec3& r_A);
103inline SpatialVec shiftVelocityFromTo(const SpatialVec& V_A_BP,
104 const Vec3& fromP_A,
105 const Vec3& toQ_A);
106
107inline SpatialVec shiftForceBy(const SpatialVec& F_AP, const Vec3& r_A);
108inline 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//==============================================================================
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//==============================================================================
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//==============================================================================
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//==============================================================================
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//==============================================================================
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//==============================================================================
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//==============================================================================
453inline 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//==============================================================================
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//==============================================================================
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//==============================================================================
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//==============================================================================
625inline 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//==============================================================================
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
683class PhiMatrixTranspose;
684
686public:
688
690 explicit PhiMatrix(const Vec3& l) : l_(l) {}
691
692 void setToZero() { l_ = 0; }
693 void setToNaN() { l_.setToNaN(); }
694
696 return SpatialMat(Mat33(1), crossMat(l_),
697 Mat33(0), Mat33(1));
698 }
699
700 const Vec3& l() const { return l_; }
701private:
702 Vec3 l_;
703};
704
706public:
707 explicit PhiMatrixTranspose(const PhiMatrix& phi) : phi(phi) {}
708
710 return SpatialMat( Mat33(1) , Mat33(0),
711 crossMat(-l()) , Mat33(1));
712 }
713
714 const Vec3& l() const {return phi.l();}
715private:
716 const PhiMatrix& phi;
717};
718
719inline PhiMatrixTranspose
721{
722 PhiMatrixTranspose ret(phi);
723 return ret;
724}
725
726inline PhiMatrixTranspose
727operator~(const PhiMatrix& phi) {return transpose(phi);}
728
729inline SpatialVec
731 const SpatialVec& v)
732{
733 return SpatialVec(v[0] + phi.l() % v[1], // 12 flops
734 v[1]);
735}
736
737inline SpatialMat
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
746inline SpatialMat
748 const PhiMatrix& phi)
749{
750 const Mat33 x = crossMat(phi.l()); // 3 flops
751 return SpatialMat( m(0,0), m(0,0)*x + m(0,1), // 54 flops
752 m(1,0), m(1,0)*x + m(1,1) ); // 54 flops
753}
754
755inline SpatialVec
757 const SpatialVec& v)
758{
759 return SpatialVec(v[0],
760 v[1] + v[0] % phiT.l()); // 12 flops
761}
762
763inline SpatialMat
765 const SpatialMat& m)
766{
767 const Mat33 x = crossMat(phiT.l()); // 3 flops
768 return SpatialMat( m(0,0) , m(0,1),
769 m(1,0)-x*m(0,0), m(1,1)-x*m(0,1)); // 108 flops
770}
771
772inline SpatialMat
774 const PhiMatrixTranspose& phiT)
775{
776 const Mat33 x = crossMat(phiT.l()); // 3 flops
777 return SpatialMat( m(0,0) - m(0,1) * x, m(0,1), // 54 flops
778 m(1,0) - m(1,1) * x, m(1,1) ); // 54 flops
779}
780
781inline SpatialMat
783 const PhiMatrixTranspose& phiT)
784{
785 const Mat33 x = crossMat(phiT.l()); // 3 flops
786 return SpatialMat( m(0,0) - m(0,1) * x, m(0,1), // 54 flops
787 m(1,0) - m(1,1) * x, m(1,1) ); // 54 flops
788}
789
790inline bool
791operator==(const PhiMatrix& p1, const PhiMatrix& p2)
792{
793 return p1.l() == p2.l();
794}
795
796inline bool
798{
799 return p1.l() == p2.l();
800}
801} // namespace SimTK
802
803#endif // SimTK_SIMMATRIX_SPATIAL_ALGEBRA_H_
This file is the user-includeable header to be included in user programs to provide fixed-length Vec ...
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition Mat.h:97
Definition SpatialAlgebra.h:705
PhiMatrixTranspose(const PhiMatrix &phi)
Definition SpatialAlgebra.h:707
const Vec3 & l() const
Definition SpatialAlgebra.h:714
SpatialMat toSpatialMat() const
Definition SpatialAlgebra.h:709
Definition SpatialAlgebra.h:685
const Vec3 & l() const
Definition SpatialAlgebra.h:700
PhiMatrixTranspose TransposeType
Definition SpatialAlgebra.h:687
void setToZero()
Definition SpatialAlgebra.h:692
void setToNaN()
Definition SpatialAlgebra.h:693
PhiMatrix()
Definition SpatialAlgebra.h:689
SpatialMat toSpatialMat() const
Definition SpatialAlgebra.h:695
PhiMatrix(const Vec3 &l)
Definition SpatialAlgebra.h:690
const Vec< 3, P > & p() const
Return a read-only reference to our translation vector p_BF.
Definition Transform.h:239
void setToNaN()
Set every scalar in this Vec to NaN; this is the default initial value in Debug builds,...
Definition Vec.h:812
Mat< 3, 3 > Mat33
This is the most common 3x3 matrix type: three packed columns of 3 Real values each.
Definition SmallMatrix.h:142
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
Vec< 2, Vec3 > SpatialVec
Spatial vectors are used for (rotation,translation) quantities and consist of a pair of Vec3 objects,...
Definition MassProperties.h:50
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
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
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
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
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
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
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
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
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
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
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 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
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
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
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition Assembler.h:37
Matrix_< E > operator*(const MatrixBase< E > &l, const typename CNT< E >::StdNumber &r)
Definition BigMatrix.h:605
PhiMatrixTranspose transpose(const PhiMatrix &phi)
Definition SpatialAlgebra.h:720
PhiMatrixTranspose operator~(const PhiMatrix &phi)
Definition SpatialAlgebra.h:727
bool operator==(const PhiMatrix &p1, const PhiMatrix &p2)
Definition SpatialAlgebra.h:791
Transform_< Real > Transform
Definition Transform.h:46
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