Simbody  3.6
Geodesic.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMMATH_GEODESIC_H_
2 #define SimTK_SIMMATH_GEODESIC_H_
3 
4 /* -------------------------------------------------------------------------- *
5  * Simbody(tm): SimTKmath *
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) 2012 Stanford University and the Authors. *
13  * Authors: Ian Stavness, 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 
30 //==============================================================================
31 // GEODESIC CLASS
32 //==============================================================================
33 
34 
35 #include "SimTKcommon.h"
37 
38 namespace SimTK {
39 
52 public:
54  Geodesic() {clear();}
55 
56  int getNumPoints() const {return (int)frenetFrames.size(); }
57 
66  const Array_<Transform>& getFrenetFrames() const {return frenetFrames;}
67  Array_<Transform>& updFrenetFrames() {return frenetFrames;}
68  void addFrenetFrame(const Transform& Kf) {frenetFrames.push_back(Kf);}
69 
70  Array_<Real>& updArcLengths() {return arcLengths;}
71  const Array_<Real>& getArcLengths() const {return arcLengths;}
72  void addArcLength(Real s) {arcLengths.push_back(s);}
73 
74  Array_<Real>& updCurvatures() {return curvature;}
75  const Array_<Real>& getCurvatures() const {return curvature;}
76  void addCurvature(Real kappa) {curvature.push_back(kappa);}
77 
79  { return directionalSensitivityPtoQ; }
81  { return directionalSensitivityPtoQ; }
83  directionalSensitivityPtoQ.push_back(jP);
84  }
85 
87  { return directionalSensitivityQtoP; }
89  { return directionalSensitivityQtoP; }
91  directionalSensitivityQtoP.push_back(jQ);
92  }
93 
95  { return positionalSensitivityPtoQ; }
97  { return positionalSensitivityPtoQ; }
99  positionalSensitivityPtoQ.push_back(jtP);
100  }
101 
103  { return positionalSensitivityQtoP; }
105  { return positionalSensitivityQtoP; }
107  positionalSensitivityQtoP.push_back(jtQ);
108  }
109 
110  void setTorsionAtP(Real tauP) {torsionAtP = tauP;}
111  void setTorsionAtQ(Real tauQ) {torsionAtQ = tauQ;}
112  void setBinormalCurvatureAtP(Real muP) {binormalCurvatureAtP = muP;}
113  void setBinormalCurvatureAtQ(Real muQ) {binormalCurvatureAtQ = muQ;}
114 
117  Real getLength() const {return arcLengths.empty() ? 0 : arcLengths.back();}
118 
124  Real calcLengthDot(const Vec3& xdotP, const Vec3& xdotQ) const
125  { return ~xdotQ*getTangentQ() - ~xdotP*getTangentP(); }
126 
129  const Vec3& getPointP() const {return frenetFrames.front().p();}
132  const Vec3& getPointQ() const {return frenetFrames.back().p();}
133 
137  const UnitVec3& getNormalP() const {return frenetFrames.front().z();}
141  const UnitVec3& getNormalQ() const {return frenetFrames.back().z();}
142 
145  const UnitVec3& getTangentP() const {return frenetFrames.front().y();}
148  const UnitVec3& getTangentQ() const {return frenetFrames.back().y();}
149 
152  const UnitVec3& getBinormalP() const {return frenetFrames.front().x();}
155  const UnitVec3& getBinormalQ() const {return frenetFrames.back().x();}
156 
162  Real getCurvatureP() const {return curvature.front();}
170  Real getCurvatureQ() const {return curvature.back();}
171 
176  Real getTorsionP() const {return torsionAtP;}
181  Real getTorsionQ() const {return torsionAtQ;}
182 
186  Real getBinormalCurvatureP() const {return binormalCurvatureAtP;}
190  Real getBinormalCurvatureQ() const {return binormalCurvatureAtQ;}
191 
197  Real getJacobiP() const {return directionalSensitivityQtoP.front()[0];}
203  Real getJacobiQ() const {return directionalSensitivityPtoQ.back()[0];}
204 
207  // Note sign change here -- we calculated this by integrating backwards
208  // so the arc length we used had the opposite sign from the true arc
209  // length parameter.
210  Real getJacobiPDot() const {return -directionalSensitivityQtoP.front()[1];}
213  Real getJacobiQDot() const {return directionalSensitivityPtoQ.back()[1];}
214 
215  // XXX testing
216  Real getJacobiTransP() const {return positionalSensitivityQtoP.front()[0];}
217  Real getJacobiTransQ() const {return positionalSensitivityPtoQ.back()[0];}
218  Real getJacobiTransPDot() const {return -positionalSensitivityQtoP.front()[1];}
219  Real getJacobiTransQDot() const {return positionalSensitivityPtoQ.back()[1];}
220 
223  void clear() {
224  arcLengths.clear();
225  frenetFrames.clear();
226  directionalSensitivityPtoQ.clear();
227  directionalSensitivityQtoP.clear();
228  positionalSensitivityPtoQ.clear();
229  positionalSensitivityQtoP.clear();
230  curvature.clear();
231  torsionAtP = torsionAtQ = NaN;
232  binormalCurvatureAtP = binormalCurvatureAtQ = NaN;
233  convexFlag = shortestFlag = false;
234  initialStepSizeHint = achievedAccuracy = NaN;
235  }
236 
237  void setIsConvex(bool isConvex) {convexFlag = isConvex;}
238  void setIsShortest(bool isShortest) {shortestFlag = isShortest;}
239  void setInitialStepSizeHint(Real sz) {initialStepSizeHint=sz;}
240  void setAchievedAccuracy(Real acc) {achievedAccuracy=acc;}
241 
242  bool isConvex() const {return convexFlag;}
243  bool isShortest() const {return shortestFlag;}
244  Real getInitialStepSizeHint() const {return initialStepSizeHint;}
245  Real getAchievedAccuracy() const {return achievedAccuracy;}
246 
247  void dump(std::ostream& o) const;
248 
249 private:
250  // All these arrays are the same length when the geodesic is complete.
251  Array_<Real> arcLengths; // arc length coord corresponding to point
252  Array_<Transform> frenetFrames; // see above for more info
253  Array_<Vec2> directionalSensitivityPtoQ; // jQ and jQdot
254  Array_<Vec2> directionalSensitivityQtoP; // jP and -jPdot
255  Array_<Vec2> positionalSensitivityPtoQ; // jtQ and jtQdot
256  Array_<Vec2> positionalSensitivityQtoP; // jtP and -jtPdot
257  Array_<Real> curvature; // normal curvature kappa in tangent direction
258  // These are only calculated at the end points.
259  Real torsionAtP, torsionAtQ; // torsion tau (only at ends)
260  Real binormalCurvatureAtP, binormalCurvatureAtQ;
261 
262  // This flag is set true if curvature[i]>=0 for all i.
263  bool convexFlag; // is this geodesic over a convex surface?
264 
265  bool shortestFlag; // XXX is this geodesic the shortest one of the surface?
266  Real initialStepSizeHint; // the initial step size to be tried when integrating this geodesic
267  Real achievedAccuracy; // the accuracy to which this geodesic curve has been calculated
268 };
269 
270 
275 public:
276  GeodesicDecorator(const Geodesic& geod, const Vec3& color) :
277  m_geod(geod), m_color(color) { }
278 
279  virtual void generateDecorations(const State& state,
280  Array_<DecorativeGeometry>& geometry) override {
281 // m_system.realize(state, Stage::Position);
282 
283  // draw connected line segments from pts
284  const Array_<Transform>& Kfs = m_geod.getFrenetFrames();
285  Vec3 prevPt;
286  for (int i = 0; i < (int) Kfs.size(); ++i) {
287  Vec3 cur = Kfs[i].p();
288  if (i > 0) {
289  geometry.push_back(
290  DecorativeLine(prevPt, cur)
291  .setColor(m_color)
292  .setLineThickness(3));
293  }
294  prevPt = cur;
295 
296  geometry.push_back(DecorativeFrame(Real(.2)).setTransform(Kfs[i]));
297  }
298  }
299 
300 private:
301  const Geodesic& m_geod;
302  const Vec3& m_color;
303 };
304 
305 
306 
307 
312  // XXX stub
313 };
314 
315 
316 } // namespace SimTK
317 
318 #endif // SimTK_SIMMATH_GEODESIC_H_
void setInitialStepSizeHint(Real sz)
Definition: Geodesic.h:239
bool isConvex() const
Definition: Geodesic.h:242
int getNumPoints() const
Definition: Geodesic.h:56
Real getJacobiPDot() const
Return the derivative of jP with respect to s, the arc length of the geodesic (which always runs from...
Definition: Geodesic.h:210
void addArcLength(Real s)
Definition: Geodesic.h:72
void addFrenetFrame(const Transform &Kf)
Definition: Geodesic.h:68
size_type size() const
Return the current number of elements stored in this array.
Definition: Array.h:2075
GeodesicDecorator(const Geodesic &geod, const Vec3 &color)
Definition: Geodesic.h:276
const Array_< Real > & getCurvatures() const
Definition: Geodesic.h:75
Real getJacobiTransQDot() const
Definition: Geodesic.h:219
const Array_< Vec2 > & getPositionalSensitivityQtoP() const
Definition: Geodesic.h:104
This defines geometry to represent a coordinate frame.
Definition: DecorativeGeometry.h:486
Array_< Vec2 > & updPositionalSensitivityQtoP()
Definition: Geodesic.h:102
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
const Array_< Vec2 > & getDirectionalSensitivityQtoP() const
Definition: Geodesic.h:88
void addPositionalSensitivityPtoQ(const Vec2 &jtP)
Definition: Geodesic.h:98
This class stores a geodesic curve after it has been determined.
Definition: Geodesic.h:51
Real getCurvatureQ() const
Return the geodesic normal curvature at Q, defined to be positive when the surface is convex in the c...
Definition: Geodesic.h:170
SimTK_Real Real
This is the default compiled-in floating point type for SimTK, either float or double.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:606
Real getJacobiTransPDot() const
Definition: Geodesic.h:218
Real getJacobiQDot() const
Return the derivative of jQ with respect to s, the arc length of the geodesic.
Definition: Geodesic.h:213
Array_< Vec2 > & updDirectionalSensitivityQtoP()
Definition: Geodesic.h:86
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:280
void setTorsionAtQ(Real tauQ)
Definition: Geodesic.h:111
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
void addDirectionalSensitivityQtoP(const Vec2 &jQ)
Definition: Geodesic.h:90
Array_< Transform > & updFrenetFrames()
Definition: Geodesic.h:67
Real getJacobiQ() const
Return jQ, the Jacobi field term giving the sensitivity of the Q end of the geodesic to changes in ta...
Definition: Geodesic.h:203
void addDirectionalSensitivityPtoQ(const Vec2 &jP)
Definition: Geodesic.h:82
const UnitVec3 & getTangentP() const
Return the unit tangent to the geodesic at P, pointing in the direction of increasing arc length para...
Definition: Geodesic.h:145
Real getJacobiTransQ() const
Definition: Geodesic.h:217
void setBinormalCurvatureAtP(Real muP)
Definition: Geodesic.h:112
const Array_< Transform > & getFrenetFrames() const
Frenet frame of geodesic at arc length s:
Definition: Geodesic.h:66
const UnitVec3 & getTangentQ() const
Return the unit tangent to the geodesic at Q, pointing in the direction of increasing arc length para...
Definition: Geodesic.h:148
void setIsShortest(bool isShortest)
Definition: Geodesic.h:238
Includes internal headers providing declarations for the basic SimTK Core classes, including Simmatrix.
bool isShortest() const
Definition: Geodesic.h:243
Array_< Vec2 > & updPositionalSensitivityPtoQ()
Definition: Geodesic.h:94
A DecorationGenerator is used to define geometry that may change over the course of a simulation...
Definition: DecorationGenerator.h:45
Real getCurvatureP() const
Return the geodesic normal curvature at P, defined to be positive when the surface is convex in the c...
Definition: Geodesic.h:162
This class stores options for calculating geodesics.
Definition: Geodesic.h:311
Real getJacobiTransP() const
Definition: Geodesic.h:216
The Array_<T> container class is a plug-compatible replacement for the C++ standard template library ...
Definition: Array.h:53
Real getAchievedAccuracy() const
Definition: Geodesic.h:245
const Vec3 & getPointP() const
Return the location on the surface of the geodesic&#39;s starting point P, measured and expressed in the ...
Definition: Geodesic.h:129
const UnitVec3 & getNormalQ() const
Return the surface outward unit normal at Q, which is aligned with the curve normal there but will ha...
Definition: Geodesic.h:141
void addPositionalSensitivityQtoP(const Vec2 &jtQ)
Definition: Geodesic.h:106
const UnitVec3 & getNormalP() const
Return the surface outward unit normal at P, which is aligned with the curve normal there but will ha...
Definition: Geodesic.h:137
void setTorsionAtP(Real tauP)
Definition: Geodesic.h:110
Real getInitialStepSizeHint() const
Definition: Geodesic.h:244
const Array_< Vec2 > & getPositionalSensitivityPtoQ() const
Definition: Geodesic.h:96
Real getJacobiP() const
Return jP, the Jacobi field term giving the sensitivity of the P end of the geodesic to changes in ta...
Definition: Geodesic.h:197
Real getTorsionQ() const
Return the geodesic torsion at Q, that is, the twisting of the Frenet frame as you move along the tan...
Definition: Geodesic.h:181
const Array_< Real > & getArcLengths() const
Definition: Geodesic.h:71
Real getLength() const
Return the total arc length of this geodesic curve.
Definition: Geodesic.h:117
void setAchievedAccuracy(Real acc)
Definition: Geodesic.h:240
Real getBinormalCurvatureP() const
Return the surface curvature in the binormal direction at P; don&#39;t confuse this with the geodesic tor...
Definition: Geodesic.h:186
This is the header file that every Simmath compilation unit should include first. ...
Real calcLengthDot(const Vec3 &xdotP, const Vec3 &xdotQ) const
Given the time derivatives of the surface coordinates of P and Q, calculate the rate of change of len...
Definition: Geodesic.h:124
const Array_< Vec2 > & getDirectionalSensitivityPtoQ() const
Definition: Geodesic.h:80
void setBinormalCurvatureAtQ(Real muQ)
Definition: Geodesic.h:113
Array_< Real > & updCurvatures()
Definition: Geodesic.h:74
const Vec3 & getPointQ() const
Return the location on the surface of the geodesic&#39;s ending point Q, measured and expressed in the su...
Definition: Geodesic.h:132
void setIsConvex(bool isConvex)
Definition: Geodesic.h:237
Array_< Real > & updArcLengths()
Definition: Geodesic.h:70
Array_< Vec2 > & updDirectionalSensitivityPtoQ()
Definition: Geodesic.h:78
void addCurvature(Real kappa)
Definition: Geodesic.h:76
const UnitVec3 & getBinormalQ() const
Return the unit binormal vector to the curve at Q, defined as bQ = tQ X nQ.
Definition: Geodesic.h:155
This class generates decoration (line segments) for a geodesic curve.
Definition: Geodesic.h:274
A line between two points.
Definition: DecorativeGeometry.h:306
Real getBinormalCurvatureQ() const
Return the surface curvature in the binormal direction at Q; don&#39;t confuse this with the geodesic tor...
Definition: Geodesic.h:190
void push_back(const T &value)
This method increases the size of the Array by one element at the end and initializes that element by...
Definition: Array.h:2399
const UnitVec3 & getBinormalP() const
Return the unit binormal vector to the curve at P, defined as bP = tP X nP.
Definition: Geodesic.h:152
void clear()
Clear the data in this geodesic, returning it to its default-constructed state, although memory remai...
Definition: Geodesic.h:223
Geodesic()
Construct an empty geodesic.
Definition: Geodesic.h:54
Real getTorsionP() const
Return the geodesic torsion at P, that is, the twisting of the Frenet frame as you move along the tan...
Definition: Geodesic.h:176
#define SimTK_SIMMATH_EXPORT
Definition: SimTKmath/include/simmath/internal/common.h:64
virtual void generateDecorations(const State &state, Array_< DecorativeGeometry > &geometry) override
This will be called every time a new State is about to be visualized.
Definition: Geodesic.h:279