1 /* -------------------------------------------------------------------------- *
2 * Simbody(tm) *
3 * -------------------------------------------------------------------------- *
4 * This is part of the SimTK biosimulation toolkit originating from *
5 * Simbios, the NIH National Center for Physics-Based Simulation of *
6 * Biological Structures at Stanford, funded under the NIH Roadmap for *
7 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
8 * *
9 * Portions copyright (c) 2010-13 Stanford University and the Authors. *
10 * Authors: Michael Sherman *
11 * Contributors: *
12 * *
13 * Licensed under the Apache License, Version 2.0 (the "License"); you may *
14 * not use this file except in compliance with the License. You may obtain a *
15 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
16 * *
17 * Unless required by applicable law or agreed to in writing, software *
18 * distributed under the License is distributed on an "AS IS" BASIS, *
19 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
20 * See the License for the specific language governing permissions and *
21 * limitations under the License. *
22 * -------------------------------------------------------------------------- */
23
24 #include "SimTKcommon.h"
25
26 #include "simbody/internal/common.h"
27 #include "simbody/internal/MobilizedBody.h"
28 #include "simbody/internal/SimbodyMatterSubsystem.h"
29 #include "simbody/internal/Force_Gravity.h"
30
31 #include "ForceImpl.h"
32
33 namespace SimTK {
34
35 //==============================================================================
36 // FORCE :: GRAVITY IMPL
37 //==============================================================================
38 // This is the hidden implementation class for Force::Gravity.
39 class Force::GravityImpl : public ForceImpl {
40 friend class Force::Gravity;
41
42 // These are settable parameters including gravity vector, zero height,
43 // and which if any mobilized bodies are immune to gravity. Modifying this
44 // variable invalidates Dynamics stage automatically, but every modification
45 // must explicitly invalidate the force cache because that can be filled in
46 // as early as Position stage.
47 struct Parameters {
ParametersSimTK::Force::GravityImpl::Parameters48 Parameters(const UnitVec3& defDirection,
49 Real defMagnitude, Real defZeroHeight,
50 const Array_<bool,MobilizedBodyIndex>& defMobodIsImmune)
51 : d(defDirection), g(defMagnitude), z(defZeroHeight),
52 mobodIsImmune(defMobodIsImmune) {}
53
54 Parameters() = default;
55
56 UnitVec3 d;
57 Real g{NaN}, z{NaN};
58 Array_<bool,MobilizedBodyIndex> mobodIsImmune; // [nb]
59 };
60
61 // The cache has a SpatialVec for each mobilized body, a Vec3 for each
62 // particle [not used], and a scalar for potential energy. The SpatialVec
63 // corresponding to Ground is initialized to zero and stays that way.
64 // This is a lazy-evaluated cache entry that can be calculated any time
65 // after Position stage. It is automatically invalidated if a Position
66 // stage variable changes. But, it is also dependent on the parameter
67 // values in the discrete Parameters variable and must be invalidated
68 // explicitly if any of those change.
69 struct ForceCache {
ForceCacheSimTK::Force::GravityImpl::ForceCache70 ForceCache() {}
allocateSimTK::Force::GravityImpl::ForceCache71 void allocate(int nb, int np, bool initToZero)
72 { F_GB.resize(nb); f_GP.resize(np);
73 if (initToZero) setToZero(); else setToNaN(); }
setToZeroSimTK::Force::GravityImpl::ForceCache74 void setToZero() {F_GB.setToZero(); f_GP.setToZero(); pe=0;}
setToNaNSimTK::Force::GravityImpl::ForceCache75 void setToNaN()
76 { F_GB.setToNaN(); F_GB[0]=SpatialVec(Vec3(0)); // Ground
77 f_GP.setToNaN(); pe=NaN;}
78 Vector_<SpatialVec> F_GB; // rigid body forces
79 Vector_<Vec3> f_GP; // particle forces
80 Real pe; // total potential energy
81 };
82
83 // Constructor from a direction and magnitude.
GravityImpl(const SimbodyMatterSubsystem & matter,const UnitVec3 & direction,Real magnitude,Real zeroHeight)84 GravityImpl(const SimbodyMatterSubsystem& matter,
85 const UnitVec3& direction,
86 Real magnitude,
87 Real zeroHeight)
88 : matter(matter), defDirection(direction), defMagnitude(magnitude),
89 defZeroHeight(zeroHeight),
90 defMobodIsImmune(matter.getNumBodies(), false),
91 numEvaluations(0)
92 { defMobodIsImmune.front() = true; } // Ground is always immune
93
94 // Constructor from a gravity vector, which might have zero magnitude.
95 // In that case we'll negate the default up direction in the System as the
96 // default direction. That would only be noticeable if the magnitude were
97 // later increased without giving a new direction.
GravityImpl(const SimbodyMatterSubsystem & matter,const Vec3 & gravityVec,Real zeroHeight)98 GravityImpl(const SimbodyMatterSubsystem& matter,
99 const Vec3& gravityVec,
100 Real zeroHeight)
101 : matter(matter)
102 { // Invoke the general constructor using system up direction.
103 new (this) GravityImpl(matter, -matter.getSystem().getUpDirection(),
104 gravityVec.norm(), zeroHeight);
105 if (defMagnitude > 0)
106 defDirection=UnitVec3(gravityVec/defMagnitude,true);
107 }
108
109 // Constructor from just gravity magnitude; use negative of System "up"
110 // direction as the down direction here.
GravityImpl(const SimbodyMatterSubsystem & matter,Real magnitude,Real zeroHeight)111 GravityImpl(const SimbodyMatterSubsystem& matter,
112 Real magnitude,
113 Real zeroHeight)
114 : matter(matter)
115 { // Invoke the general constructor using system up direction.
116 new (this) GravityImpl(matter, -matter.getSystem().getUpDirection(),
117 magnitude, zeroHeight);
118 }
119
setMobodIsImmuneByDefault(MobilizedBodyIndex mbx,bool isImmune)120 void setMobodIsImmuneByDefault(MobilizedBodyIndex mbx, bool isImmune) {
121 if (mbx == 0) return; // can't change Ground's innate immunity
122 invalidateTopologyCache();
123 if (defMobodIsImmune.size() < mbx+1)
124 defMobodIsImmune.resize(mbx+1, false);
125 defMobodIsImmune[mbx] = isImmune;
126 }
127
getMobodIsImmuneByDefault(MobilizedBodyIndex mbx) const128 bool getMobodIsImmuneByDefault(MobilizedBodyIndex mbx) const {
129 if (defMobodIsImmune.size() < mbx+1)
130 return false;
131 return defMobodIsImmune[mbx];
132 }
133
setMobodIsImmune(State & state,MobilizedBodyIndex mbx,bool isImmune) const134 void setMobodIsImmune(State& state, MobilizedBodyIndex mbx,
135 bool isImmune) const {
136 if (mbx == 0) return; // no messing with Ground
137 invalidateForceCache(state);
138 Parameters& p = updParameters(state);
139 p.mobodIsImmune[mbx] = isImmune;
140 }
141
getMobodIsImmune(const State & state,MobilizedBodyIndex mbx) const142 bool getMobodIsImmune(const State& state, MobilizedBodyIndex mbx) const {
143 const Parameters& p = getParameters(state);
144 return p.mobodIsImmune[mbx];
145 }
146
clone() const147 GravityImpl* clone() const override {
148 return new GravityImpl(*this);
149 }
150
151 // We are doing our own caching here, so don't override the
152 // dependsOnlyOnPositions() method which would cause the base class also
153 // to cache the results.
154
155 void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
156 Vector_<Vec3>& particleForces, Vector& mobilityForces) const
157 override;
158 Real calcPotentialEnergy(const State& state) const override;
159
160 // Allocate the state variables and cache entries.
161 void realizeTopology(State& s) const override;
162
getParameters(const State & s) const163 const Parameters& getParameters(const State& s) const
164 { return Value<Parameters>::downcast
165 (getForceSubsystem().getDiscreteVariable(s,parametersIx)); }
updParameters(State & s) const166 Parameters& updParameters(State& s) const
167 { return Value<Parameters>::updDowncast
168 (getForceSubsystem().updDiscreteVariable(s,parametersIx)); }
169
getForceCache(const State & s) const170 const ForceCache& getForceCache(const State& s) const
171 { return Value<ForceCache>::downcast
172 (getForceSubsystem().getCacheEntry(s,forceCacheIx)); }
updForceCache(const State & s) const173 ForceCache& updForceCache(const State& s) const
174 { return Value<ForceCache>::updDowncast
175 (getForceSubsystem().updCacheEntry(s,forceCacheIx)); }
176
isForceCacheValid(const State & s) const177 bool isForceCacheValid(const State& s) const
178 { return getForceSubsystem().isCacheValueRealized(s,forceCacheIx); }
markForceCacheValid(const State & s) const179 void markForceCacheValid(const State& s) const
180 { getForceSubsystem().markCacheValueRealized(s,forceCacheIx); }
invalidateForceCache(const State & s) const181 void invalidateForceCache(const State& s) const
182 { getForceSubsystem().markCacheValueNotRealized(s,forceCacheIx); }
183
184 // This method calculates gravity forces if needed, and bumps the
185 // numEvaluations counter if it has to do any work.
186 void ensureForceCacheValid(const State&) const;
187
188 // TOPOLOGY STATE
189 const SimbodyMatterSubsystem& matter;
190 UnitVec3 defDirection;
191 Real defMagnitude;
192 Real defZeroHeight;
193 Array_<bool,MobilizedBodyIndex> defMobodIsImmune;
194
195 // TOPOLOGY CACHE
196 DiscreteVariableIndex parametersIx;
197 CacheEntryIndex forceCacheIx;
198
199 mutable long long numEvaluations;
200 };
201
202
203 //==============================================================================
204 // FORCE :: GRAVITY
205 //==============================================================================
206
207 SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Force::Gravity,
208 Force::GravityImpl, Force);
209
Gravity(GeneralForceSubsystem & forces,const SimbodyMatterSubsystem & matter,const UnitVec3 & defDirection,Real defMagnitude,Real defZeroHeight)210 Force::Gravity::Gravity
211 (GeneralForceSubsystem& forces,
212 const SimbodyMatterSubsystem& matter,
213 const UnitVec3& defDirection,
214 Real defMagnitude,
215 Real defZeroHeight)
216 : Force(new GravityImpl(matter,defDirection,defMagnitude,defZeroHeight))
217 {
218 SimTK_ERRCHK1_ALWAYS(defMagnitude >= 0,
219 "Force::Gravity::Gravity(downDirection,magnitude)",
220 "The gravity magnitude g must be nonnegative but was specified as %g.",
221 defMagnitude);
222 SimTK_ERRCHK_ALWAYS(defDirection.isFinite(),
223 "Force::Gravity::Gravity(downDirection,magnitude)",
224 "A non-finite 'down' direction was received; did you specify a zero-"
225 "length Vec3? The direction must be non-zero.");
226 updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
227 }
228
Gravity(GeneralForceSubsystem & forces,const SimbodyMatterSubsystem & matter,const Vec3 & defGravity)229 Force::Gravity::Gravity
230 (GeneralForceSubsystem& forces,
231 const SimbodyMatterSubsystem& matter,
232 const Vec3& defGravity)
233 : Force(new GravityImpl(matter,defGravity,0))
234 {
235 updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
236 }
237
Gravity(GeneralForceSubsystem & forces,const SimbodyMatterSubsystem & matter,Real defMagnitude)238 Force::Gravity::Gravity
239 (GeneralForceSubsystem& forces,
240 const SimbodyMatterSubsystem& matter,
241 Real defMagnitude)
242 : Force(new GravityImpl(matter,defMagnitude,0))
243 {
244 SimTK_ERRCHK1_ALWAYS(defMagnitude >= 0,
245 "Force::Gravity::Gravity(magnitude)",
246 "The gravity magnitude g must be nonnegative but was specified as %g.",
247 defMagnitude);
248 updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
249 }
250
251 // Each of the setDefault methods must invalidate the topology cache.
252
253 Force::Gravity& Force::Gravity::
setDefaultBodyIsExcluded(MobilizedBodyIndex mobod,bool isExcluded)254 setDefaultBodyIsExcluded(MobilizedBodyIndex mobod, bool isExcluded) {
255 // Invalidates topology cache.
256 updImpl().setMobodIsImmuneByDefault(mobod, isExcluded);
257 return *this;
258 }
259
260 Force::Gravity& Force::Gravity::
setDefaultGravityVector(const Vec3 & gravity)261 setDefaultGravityVector(const Vec3& gravity) {
262 const Real g = gravity.norm();
263 getImpl().invalidateTopologyCache();
264 updImpl().defMagnitude = g;
265 // Don't change the direction if the magnitude is zero.
266 if (g > 0)
267 updImpl().defDirection = UnitVec3(gravity/g, true);
268 return *this;
269 }
270
271 Force::Gravity& Force::Gravity::
setDefaultDownDirection(const UnitVec3 & down)272 setDefaultDownDirection(const UnitVec3& down) {
273 SimTK_ERRCHK_ALWAYS(down.isFinite(),
274 "Force::Gravity::setDefaultDownDirection()",
275 "A non-finite 'down' direction was received; did you specify a zero-"
276 "length Vec3? The direction must be non-zero.");
277
278 getImpl().invalidateTopologyCache();
279 updImpl().defDirection = down;
280 return *this;
281 }
282
283 Force::Gravity& Force::Gravity::
setDefaultMagnitude(Real g)284 setDefaultMagnitude(Real g) {
285 SimTK_ERRCHK1_ALWAYS(g >= 0,
286 "Force::Gravity::setDefaultMagnitude()",
287 "The gravity magnitude g must be nonnegative but was specified as %g.",
288 g);
289
290 getImpl().invalidateTopologyCache();
291 updImpl().defMagnitude = g;
292 return *this;
293 }
294
295 Force::Gravity& Force::Gravity::
setDefaultZeroHeight(Real zeroHeight)296 setDefaultZeroHeight(Real zeroHeight) {
297 getImpl().invalidateTopologyCache();
298 updImpl().defZeroHeight = zeroHeight;
299 return *this;
300 }
301
302 bool Force::Gravity::
getDefaultBodyIsExcluded(MobilizedBodyIndex mobod) const303 getDefaultBodyIsExcluded(MobilizedBodyIndex mobod) const
304 { return getImpl().getMobodIsImmuneByDefault(mobod); }
305 Vec3 Force::Gravity::
getDefaultGravityVector() const306 getDefaultGravityVector() const
307 { return getImpl().defDirection * getImpl().defMagnitude;}
308 const UnitVec3& Force::Gravity::
getDefaultDownDirection() const309 getDefaultDownDirection() const {return getImpl().defDirection;}
310 Real Force::Gravity::
getDefaultMagnitude() const311 getDefaultMagnitude() const {return getImpl().defMagnitude;}
312 Real Force::Gravity::
getDefaultZeroHeight() const313 getDefaultZeroHeight() const {return getImpl().defZeroHeight;}
314
315
316 // These set routines must explicitly invalidate the force cache since only
317 // Dynamics stage is invalidated automatically by this state variable. Try
318 // not to do anything if the new value is the same as the old one.
319
320 const Force::Gravity& Force::Gravity::
setBodyIsExcluded(State & state,MobilizedBodyIndex mobod,bool isExcluded) const321 setBodyIsExcluded(State& state, MobilizedBodyIndex mobod,
322 bool isExcluded) const
323 {
324 const GravityImpl& impl = getImpl();
325 SimTK_ERRCHK2_ALWAYS(mobod < impl.matter.getNumBodies(),
326 "Force::Gravity::setBodyIsExcluded()",
327 "Attempted to exclude mobilized body with index %d but only mobilized"
328 " bodies with indices between 0 and %d exist in this System.",
329 (int)mobod, impl.matter.getNumBodies()-1);
330
331 if (getBodyIsExcluded(state, mobod) != isExcluded) {
332 // Invalidates force cache.
333 impl.setMobodIsImmune(state, mobod, isExcluded);
334 // The zero must be precalculated if the body is immune to gravity.
335 SpatialVec& F = impl.updForceCache(state).F_GB[mobod];
336 if (isExcluded || getMagnitude(state) == 0)
337 F.setToZero();
338 else
339 F.setToNaN();
340 }
341 return *this;
342 }
343
344 const Force::Gravity& Force::Gravity::
setGravityVector(State & state,const Vec3 & gravity) const345 setGravityVector(State& state, const Vec3& gravity) const {
346 const Real newg = gravity.norm();
347 const UnitVec3 newd = newg > 0 ? UnitVec3(gravity/newg, true)
348 : getDownDirection(state);
349 if (getMagnitude(state) != newg || getDownDirection(state) != newd) {
350 getImpl().invalidateForceCache(state);
351 getImpl().updParameters(state).g = newg;
352 getImpl().updParameters(state).d = newd;
353
354 if (newg == 0)
355 getImpl().updForceCache(state).setToZero(); // must precalculate
356 }
357 return *this;
358 }
359
360 const Force::Gravity& Force::Gravity::
setDownDirection(State & state,const UnitVec3 & down) const361 setDownDirection(State& state, const UnitVec3& down) const {
362 SimTK_ERRCHK_ALWAYS(down.isFinite(),
363 "Force::Gravity::setDownDirection()",
364 "A non-finite 'down' direction was received; did you specify a zero-"
365 "length Vec3? The direction must be non-zero.");
366
367 if (getDownDirection(state) != down) {
368 getImpl().invalidateForceCache(state);
369 getImpl().updParameters(state).d = down;
370 }
371 return *this;
372 }
373
374 const Force::Gravity& Force::Gravity::
setMagnitude(State & state,Real g) const375 setMagnitude(State& state, Real g) const {
376 SimTK_ERRCHK1_ALWAYS(g >= 0,
377 "Force::Gravity::setMagnitude()",
378 "The gravity magnitude g must be nonnegative but was specified as %g.",
379 g);
380
381 if (getMagnitude(state) != g) {
382 getImpl().invalidateForceCache(state);
383 getImpl().updParameters(state).g = g;
384
385 if (g == 0)
386 getImpl().updForceCache(state).setToZero(); // must precalculate
387 }
388 return *this;
389 }
390
391 const Force::Gravity& Force::Gravity::
setZeroHeight(State & state,Real zeroHeight) const392 setZeroHeight(State& state, Real zeroHeight) const {
393 if (getZeroHeight(state) != zeroHeight) {
394 getImpl().invalidateForceCache(state);
395 getImpl().updParameters(state).z = zeroHeight;
396 }
397 return *this;
398 }
399
400 bool Force::Gravity::
getBodyIsExcluded(const State & state,MobilizedBodyIndex mobod) const401 getBodyIsExcluded(const State& state, MobilizedBodyIndex mobod) const
402 { return getImpl().getMobodIsImmune(state, mobod); }
403
404
405 Vec3 Force::Gravity::
getGravityVector(const State & state) const406 getGravityVector(const State& state) const
407 { const GravityImpl::Parameters& p = getImpl().getParameters(state);
408 return p.g*p.d; }
409
410 const UnitVec3& Force::Gravity::
getDownDirection(const State & state) const411 getDownDirection(const State& state) const
412 { return getImpl().getParameters(state).d; }
413
414
getMagnitude(const State & state) const415 Real Force::Gravity::getMagnitude(const State& state) const
416 { return getImpl().getParameters(state).g; }
417
getZeroHeight(const State & state) const418 Real Force::Gravity::getZeroHeight(const State& state) const
419 { return getImpl().getParameters(state).z; }
420
421 Real Force::Gravity::
getPotentialEnergy(const State & s) const422 getPotentialEnergy(const State& s) const
423 { getImpl().ensureForceCacheValid(s);
424 return getImpl().getForceCache(s).pe; }
425
426 const Vector_<SpatialVec>& Force::Gravity::
getBodyForces(const State & s) const427 getBodyForces(const State& s) const
428 { getImpl().ensureForceCacheValid(s);
429 const GravityImpl::ForceCache& fc = getImpl().getForceCache(s);
430 return fc.F_GB; }
431
432 const Vector_<Vec3>& Force::Gravity::
getParticleForces(const State & s) const433 getParticleForces(const State& s) const
434 { getImpl().ensureForceCacheValid(s);
435 const GravityImpl::ForceCache& fc = getImpl().getForceCache(s);
436 return fc.f_GP; }
437
438 long long Force::Gravity::
getNumEvaluations() const439 getNumEvaluations() const
440 { return getImpl().numEvaluations; }
441
442 bool Force::Gravity::
isForceCacheValid(const State & state) const443 isForceCacheValid(const State& state) const
444 { return getImpl().isForceCacheValid(state); }
445
446 void Force::Gravity::
invalidateForceCache(const State & state) const447 invalidateForceCache(const State& state) const
448 { getImpl().invalidateForceCache(state); }
449
450
451 //==============================================================================
452 // FORCE :: GRAVITY IMPL
453 //==============================================================================
454
455 //----------------------------- REALIZE TOPOLOGY -------------------------------
456 // Allocate the state variables and cache entries. The force cache is a lazy-
457 // evaluation entry - although it can be calculated any time after
458 // Stage::Position, it won't be unless someone asks for it. And if it is
459 // evaluated early, it should not be re-evaluated when used as a force during
460 // Stage::Dynamics (via the calcForce() call).
461 //
462 // In addition, the force cache has a dependency on the parameter values that
463 // are stored in a discrete state variable. Changes to that variable
464 // automatically invalidate Dynamics stage, but must be carefully managed also
465 // to invalidate the force cache here since it is only Position-dependent.
466 void Force::GravityImpl::
realizeTopology(State & s) const467 realizeTopology(State& s) const {
468 GravityImpl* mThis = const_cast<GravityImpl*>(this);
469 const int nb=matter.getNumBodies(), np=matter.getNumParticles();
470
471 // In case more mobilized bodies were added after this Gravity element
472 // was constructed, make room for the rest now. Earlier default immunity
473 // settings are preserved.
474 if (defMobodIsImmune.size() != nb)
475 mThis->defMobodIsImmune.resize(nb, false);
476
477 // Allocate a discrete state variable to hold parameters; see above comment.
478 const Parameters p(defDirection,defMagnitude,defZeroHeight,
479 defMobodIsImmune); // initial value
480 mThis->parametersIx = getForceSubsystem()
481 .allocateDiscreteVariable(s, Stage::Dynamics, new Value<Parameters>(p));
482
483 // Don't allocate force cache space yet since we would have to copy it.
484 // Caution -- dependence on Parameters requires manual invalidation.
485 mThis->forceCacheIx = getForceSubsystem().allocateLazyCacheEntry(s,
486 Stage::Position, new Value<ForceCache>());
487
488 // Now allocate the appropriate amount of space, and set to zero now
489 // any forces that we know will end up zero so we don't have to calculate
490 // them at run time. Precalculated zeroes must be provided for any
491 // immune elements, or for all elements if g==0, and this must be kept
492 // up to date if there are runtime changes to the parameters.
493
494 ForceCache& fc = updForceCache(s);
495 if (defMagnitude == 0) {
496 fc.allocate(nb, np, true); // initially zero since no gravity
497 } else {
498 fc.allocate(nb, np, false); // initially NaN except for Ground
499 for (MobilizedBodyIndex mbx(1); mbx < nb; ++mbx)
500 if (defMobodIsImmune[mbx])
501 fc.F_GB[mbx] = SpatialVec(Vec3(0),Vec3(0));
502 // This doesn't mean the ForceCache is valid yet.
503 }
504 }
505
506
507 //------------------------- ENSURE FORCE CACHE VALID ---------------------------
508 // This will also calculate potential energy since we can do it on the cheap
509 // simultaneously with the force. Note that if the strength of gravity was set
510 // to zero then we already zeroed out the forces and pe during realizeInstance()
511 // so all we have to do in that case is mark the cache valid now. Also, any
512 // immune bodies have their force set to zero in realizeInstance() so we don't
513 // have to do it again here.
514 void Force::GravityImpl::
ensureForceCacheValid(const State & state) const515 ensureForceCacheValid(const State& state) const {
516 if (isForceCacheValid(state)) return;
517
518 SimTK_STAGECHECK_GE_ALWAYS(state.getSystemStage(), Stage::Position,
519 "Force::GravityImpl::ensureForceCacheValid()");
520
521 const Parameters& p = getParameters(state);
522 if (p.g == 0) { // no gravity
523 markForceCacheValid(state); // zeroes must have been precalculated
524 return;
525 }
526
527 // Gravity is non-zero and gravity forces are not up to date, so this counts
528 // as an evaluation.
529 ++numEvaluations;
530
531 const Vec3 gravity = p.g * p.d;
532 const Real zeroPEOffset = p.g * p.z;
533 ForceCache& fc = updForceCache(state);
534 fc.pe = 0;
535
536 const int nb = matter.getNumBodies();
537 // Skip Ground since we know it is immune.
538 for (MobilizedBodyIndex mbx(1); mbx < nb; ++mbx) {
539 if (p.mobodIsImmune[mbx])
540 continue; // don't apply gravity to this body; F already zero
541
542 const MobilizedBody& mobod = matter.getMobilizedBody(mbx);
543 const MassProperties& mprops = mobod.getBodyMassProperties(state);
544 const Transform& X_GB = mobod.getBodyTransform(state);
545
546 Real m = mprops.getMass();
547 const Vec3& p_CB = mprops.getMassCenter(); // in B
548 const Vec3 p_CB_G = X_GB.R()*p_CB; // exp. in G; 15 flops
549 const Vec3 p_G_CB = X_GB.p() + p_CB_G; // meas. in G; 3 flops
550
551 const Vec3 F_CB_G = m*gravity; // force at mass center; 3 flops
552 fc.F_GB[mbx] = SpatialVec(p_CB_G % F_CB_G, F_CB_G); // body frc; 9 flops
553
554 // odd signs here because height is in -gravity direction.
555 fc.pe -= m*(~gravity*p_G_CB + zeroPEOffset); // 8 flops
556 }
557
558 const int np = matter.getNumParticles();
559 if (np) {
560 const Vector& m = matter.getAllParticleMasses(state);
561 const Vector_<Vec3>& p_GP = matter.getAllParticleLocations(state);
562 for (ParticleIndex px(0); px < np; ++px) {
563 fc.f_GP[px] = m[px] * gravity; // 3 flops
564 fc.pe -= m[px]*(~gravity*p_GP[px] + zeroPEOffset); // 8 flops
565 }
566 }
567
568 markForceCacheValid(state);
569 }
570
571 //------------------------------- CALC FORCE -----------------------------------
572 void Force::GravityImpl::
calcForce(const State & state,Vector_<SpatialVec> & bodyForces,Vector_<Vec3> & particleForces,Vector & mobilityForces) const573 calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
574 Vector_<Vec3>& particleForces, Vector& mobilityForces) const
575 { ensureForceCacheValid(state);
576 const ForceCache& fc = getForceCache(state);
577 bodyForces += fc.F_GB;
578 particleForces += fc.f_GP; }
579
580
581 //-------------------------- CALC POTENTIAL ENERGY -----------------------------
582 // If the force was calculated, then the potential energy will already
583 // be valid. Otherwise we'll have to calculate it.
584 Real Force::GravityImpl::
calcPotentialEnergy(const State & state) const585 calcPotentialEnergy(const State& state) const
586 { ensureForceCacheValid(state);
587 const ForceCache& fc = getForceCache(state);
588 return fc.pe; }
589
590
591 } // namespace SimTK
592
593