Simbody 3.7
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
38namespace SimTK {
39
52public:
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
249private:
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
275public:
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
300private:
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_
Includes internal headers providing declarations for the basic SimTK Core classes,...
This is the header file that every Simmath compilation unit should include first.
#define SimTK_SIMMATH_EXPORT
Definition: SimTKmath/include/simmath/internal/common.h:64
The Array_<T> container class is a plug-compatible replacement for the C++ standard template library ...
Definition: Array.h:1520
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
size_type size() const
Return the current number of elements stored in this array.
Definition: Array.h:2075
A DecorationGenerator is used to define geometry that may change over the course of a simulation.
Definition: DecorationGenerator.h:45
This defines geometry to represent a coordinate frame.
Definition: DecorativeGeometry.h:486
DecorativeFrame & setTransform(const Transform &X_BD)
Definition: DecorativeGeometry.h:497
A line between two points.
Definition: DecorativeGeometry.h:306
DecorativeLine & setLineThickness(Real t)
Definition: DecorativeGeometry.h:324
DecorativeLine & setColor(const Vec3 &rgb)
Definition: DecorativeGeometry.h:322
This class generates decoration (line segments) for a geodesic curve.
Definition: Geodesic.h:274
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
GeodesicDecorator(const Geodesic &geod, const Vec3 &color)
Definition: Geodesic.h:276
This class stores options for calculating geodesics.
Definition: Geodesic.h:311
This class stores a geodesic curve after it has been determined.
Definition: Geodesic.h:51
Real getInitialStepSizeHint() const
Definition: Geodesic.h:244
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
const Array_< Real > & getCurvatures() const
Definition: Geodesic.h:75
Real getBinormalCurvatureP() const
Return the surface curvature in the binormal direction at P; don't confuse this with the geodesic tor...
Definition: Geodesic.h:186
void setTorsionAtQ(Real tauQ)
Definition: Geodesic.h:111
Geodesic()
Construct an empty geodesic.
Definition: Geodesic.h:54
Real getJacobiTransPDot() const
Definition: Geodesic.h:218
void setIsConvex(bool isConvex)
Definition: Geodesic.h:237
Real getJacobiTransQDot() const
Definition: Geodesic.h:219
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
Array_< Real > & updCurvatures()
Definition: Geodesic.h:74
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
Real getBinormalCurvatureQ() const
Return the surface curvature in the binormal direction at Q; don't confuse this with the geodesic tor...
Definition: Geodesic.h:190
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
const Array_< Vec2 > & getPositionalSensitivityQtoP() const
Definition: Geodesic.h:104
void addArcLength(Real s)
Definition: Geodesic.h:72
bool isConvex() const
Definition: Geodesic.h:242
void addCurvature(Real kappa)
Definition: Geodesic.h:76
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
Array_< Real > & updArcLengths()
Definition: Geodesic.h:70
const Array_< Vec2 > & getDirectionalSensitivityPtoQ() const
Definition: Geodesic.h:80
Real getLength() const
Return the total arc length of this geodesic curve.
Definition: Geodesic.h:117
const Array_< Real > & getArcLengths() const
Definition: Geodesic.h:71
const Array_< Vec2 > & getDirectionalSensitivityQtoP() const
Definition: Geodesic.h:88
const Vec3 & getPointP() const
Return the location on the surface of the geodesic's starting point P, measured and expressed in the ...
Definition: Geodesic.h:129
void addFrenetFrame(const Transform &Kf)
Definition: Geodesic.h:68
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_< Transform > & getFrenetFrames() const
Frenet frame of geodesic at arc length s:
Definition: Geodesic.h:66
const UnitVec3 & getBinormalQ() const
Return the unit binormal vector to the curve at Q, defined as bQ = tQ X nQ.
Definition: Geodesic.h:155
void setAchievedAccuracy(Real acc)
Definition: Geodesic.h:240
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 dump(std::ostream &o) const
void setInitialStepSizeHint(Real sz)
Definition: Geodesic.h:239
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
Array_< Vec2 > & updDirectionalSensitivityPtoQ()
Definition: Geodesic.h:78
Array_< Vec2 > & updPositionalSensitivityPtoQ()
Definition: Geodesic.h:94
Array_< Vec2 > & updPositionalSensitivityQtoP()
Definition: Geodesic.h:102
void clear()
Clear the data in this geodesic, returning it to its default-constructed state, although memory remai...
Definition: Geodesic.h:223
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 setTorsionAtP(Real tauP)
Definition: Geodesic.h:110
void addDirectionalSensitivityPtoQ(const Vec2 &jP)
Definition: Geodesic.h:82
void setBinormalCurvatureAtP(Real muP)
Definition: Geodesic.h:112
Real getJacobiTransQ() const
Definition: Geodesic.h:217
void addDirectionalSensitivityQtoP(const Vec2 &jQ)
Definition: Geodesic.h:90
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
const Array_< Vec2 > & getPositionalSensitivityPtoQ() const
Definition: Geodesic.h:96
void addPositionalSensitivityPtoQ(const Vec2 &jtP)
Definition: Geodesic.h:98
void setBinormalCurvatureAtQ(Real muQ)
Definition: Geodesic.h:113
int getNumPoints() const
Definition: Geodesic.h:56
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
const UnitVec3 & getBinormalP() const
Return the unit binormal vector to the curve at P, defined as bP = tP X nP.
Definition: Geodesic.h:152
const Vec3 & getPointQ() const
Return the location on the surface of the geodesic's ending point Q, measured and expressed in the su...
Definition: Geodesic.h:132
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
bool isShortest() const
Definition: Geodesic.h:243
Real getJacobiQDot() const
Return the derivative of jQ with respect to s, the arc length of the geodesic.
Definition: Geodesic.h:213
Real getJacobiTransP() const
Definition: Geodesic.h:216
void setIsShortest(bool isShortest)
Definition: Geodesic.h:238
Real getAchievedAccuracy() const
Definition: Geodesic.h:245
Array_< Vec2 > & updDirectionalSensitivityQtoP()
Definition: Geodesic.h:86
Array_< Transform > & updFrenetFrames()
Definition: Geodesic.h:67
void addPositionalSensitivityQtoP(const Vec2 &jtQ)
Definition: Geodesic.h:106
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:280
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
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