1 #ifndef SimTK_SIMBODY_FORCE_IMPL_H_ 2 #define SimTK_SIMBODY_FORCE_IMPL_H_ 3 4 /* -------------------------------------------------------------------------- * 5 * Simbody(tm) * 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) 2008-12 Stanford University and the Authors. * 13 * Authors: Peter Eastman, 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 27 #include "SimTKcommon.h" 28 #include "simbody/internal/common.h" 29 #include "simbody/internal/Force.h" 30 #include "simbody/internal/Force_BuiltIns.h" 31 32 namespace SimTK { 33 34 // This is what a Force handle points to. 35 class ForceImpl : public PIMPLImplementation<Force, ForceImpl> { 36 public: ForceImpl()37 ForceImpl() : forces(0), defaultDisabled(false) {} ForceImpl(const ForceImpl & clone)38 ForceImpl(const ForceImpl& clone) {*this = clone;} 39 setDisabledByDefault(bool shouldBeDisabled)40 void setDisabledByDefault(bool shouldBeDisabled) 41 { invalidateTopologyCache(); 42 defaultDisabled = shouldBeDisabled; } 43 isDisabledByDefault()44 bool isDisabledByDefault() const 45 { return defaultDisabled; } 46 ~ForceImpl()47 virtual ~ForceImpl() {} 48 virtual ForceImpl* clone() const = 0; dependsOnlyOnPositions()49 virtual bool dependsOnlyOnPositions() const { 50 return false; 51 } shouldBeParallelIfPossible()52 virtual bool shouldBeParallelIfPossible() const{ 53 return false; 54 } getForceIndex()55 ForceIndex getForceIndex() const {return index;} getForceSubsystem()56 const GeneralForceSubsystem& getForceSubsystem() const 57 { assert(forces); return *forces; } setForceSubsystem(GeneralForceSubsystem & frcsub,ForceIndex ix)58 void setForceSubsystem(GeneralForceSubsystem& frcsub, ForceIndex ix) { 59 forces = &frcsub; 60 index = ix; 61 } invalidateTopologyCache()62 void invalidateTopologyCache() const { 63 if (forces) forces->invalidateSubsystemTopologyCache(); 64 } 65 66 // Every force element must provide the next two methods. Note that 67 // calcForce() must *add in* (+=) its forces to the given arrays. 68 virtual void calcForce(const State& state, 69 Vector_<SpatialVec>& bodyForces, 70 Vector_<Vec3>& particleForces, 71 Vector& mobilityForces) const = 0; 72 virtual Real calcPotentialEnergy(const State& state) const = 0; 73 realizeTopology(State & state)74 virtual void realizeTopology (State& state) const {} realizeModel(State & state)75 virtual void realizeModel (State& state) const {} realizeInstance(const State & state)76 virtual void realizeInstance (const State& state) const {} realizeTime(const State & state)77 virtual void realizeTime (const State& state) const {} realizePosition(const State & state)78 virtual void realizePosition (const State& state) const {} realizeVelocity(const State & state)79 virtual void realizeVelocity (const State& state) const {} realizeDynamics(const State & state)80 virtual void realizeDynamics (const State& state) const {} realizeAcceleration(const State & state)81 virtual void realizeAcceleration(const State& state) const {} realizeReport(const State & state)82 virtual void realizeReport (const State& state) const {} 83 calcDecorativeGeometryAndAppend(const State & s,Stage stage,Array_<DecorativeGeometry> & geom)84 virtual void calcDecorativeGeometryAndAppend 85 (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const {} 86 87 private: 88 // CONSTRUCTION 89 GeneralForceSubsystem* forces; // just a reference; no delete on destruction 90 ForceIndex index; 91 92 // TOPOLOGY "STATE" 93 // Changing anything here invalidates the topology of the containing 94 // force Subsystem and thus of the whole System. 95 96 // This says whether the Instance-stage "disabled" flag for this force 97 // element should be initially on or off. Most force elements are enabled 98 // by default. 99 bool defaultDisabled; 100 101 // TOPOLOGY "CACHE" 102 // Nothing in the base Impl class. 103 }; 104 105 106 107 //------------------------------------------------------------------------------ 108 // TWO POINT LINEAR SPRING IMPL 109 //------------------------------------------------------------------------------ 110 class Force::TwoPointLinearSpringImpl : public ForceImpl { 111 public: 112 TwoPointLinearSpringImpl(const MobilizedBody& body1, const Vec3& station1, 113 const MobilizedBody& body2, const Vec3& station2, 114 Real k, Real x0); 115 clone()116 TwoPointLinearSpringImpl* clone() const override { 117 return new TwoPointLinearSpringImpl(*this); 118 } dependsOnlyOnPositions()119 bool dependsOnlyOnPositions() const override { 120 return true; 121 } 122 void calcForce(const State& state, 123 Vector_<SpatialVec>& bodyForces, 124 Vector_<Vec3>& particleForces, 125 Vector& mobilityForces) const override; 126 Real calcPotentialEnergy(const State& state) const override; 127 128 void calcDecorativeGeometryAndAppend(const State& s, Stage stage, 129 Array_<DecorativeGeometry>& geom) 130 const override; 131 132 private: 133 const SimbodyMatterSubsystem& matter; 134 const MobilizedBodyIndex body1, body2; 135 Vec3 station1, station2; 136 Real k, x0; 137 }; 138 139 140 141 //------------------------------------------------------------------------------ 142 // TWO POINT LINEAR DAMPER IMPL 143 //------------------------------------------------------------------------------ 144 class Force::TwoPointLinearDamperImpl : public ForceImpl { 145 public: 146 TwoPointLinearDamperImpl(const MobilizedBody& body1, const Vec3& station1, const MobilizedBody& body2, const Vec3& station2, Real damping); clone()147 TwoPointLinearDamperImpl* clone() const override { 148 return new TwoPointLinearDamperImpl(*this); 149 } 150 void calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const override; 151 Real calcPotentialEnergy(const State& state) const override; 152 private: 153 const SimbodyMatterSubsystem& matter; 154 const MobilizedBodyIndex body1, body2; 155 Vec3 station1, station2; 156 Real damping; 157 }; 158 159 160 161 //------------------------------------------------------------------------------ 162 // TWO POINT CONSTANT FORCE IMPL 163 //------------------------------------------------------------------------------ 164 class Force::TwoPointConstantForceImpl : public ForceImpl { 165 public: 166 TwoPointConstantForceImpl(const MobilizedBody& body1, const Vec3& station1, const MobilizedBody& body2, const Vec3& station2, Real force); clone()167 TwoPointConstantForceImpl* clone() const override { 168 return new TwoPointConstantForceImpl(*this); 169 } dependsOnlyOnPositions()170 bool dependsOnlyOnPositions() const override { 171 return true; 172 } 173 void calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const override; 174 Real calcPotentialEnergy(const State& state) const override; 175 private: 176 const SimbodyMatterSubsystem& matter; 177 const MobilizedBodyIndex body1, body2; 178 Vec3 station1, station2; 179 Real force; 180 }; 181 182 183 184 //------------------------------------------------------------------------------ 185 // MOBILITY LINEAR SPRING IMPL 186 //------------------------------------------------------------------------------ 187 class Force::MobilityLinearSpringImpl : public ForceImpl { 188 friend class MobilityLinearSpring; 189 190 MobilityLinearSpringImpl(const MobilizedBody& mobod, 191 MobilizerQIndex whichQ, 192 Real defaultStiffness, 193 Real defaultQZero); 194 getParams(const State & state)195 const std::pair<Real,Real>& getParams(const State& state) const { 196 return Value< std::pair<Real,Real> >::downcast 197 (getForceSubsystem().getDiscreteVariable(state, m_paramsIx)); 198 } updParams(State & state)199 std::pair<Real,Real>& updParams(State& state) const { 200 return Value< std::pair<Real,Real> >::updDowncast 201 (getForceSubsystem().updDiscreteVariable(state, m_paramsIx)); 202 } 203 clone()204 MobilityLinearSpringImpl* clone() const override 205 { return new MobilityLinearSpringImpl(*this); } dependsOnlyOnPositions()206 bool dependsOnlyOnPositions() const override {return true;} 207 void calcForce(const State& state, Vector_<SpatialVec>& bodyForces, 208 Vector_<Vec3>& particleForces, Vector& mobilityForces) const 209 override; 210 Real calcPotentialEnergy(const State& state) const override; 211 212 // Allocate the discrete state variable for the parameters. realizeTopology(State & s)213 void realizeTopology(State& s) const override { 214 m_paramsIx = getForceSubsystem() 215 .allocateDiscreteVariable(s, Stage::Dynamics, 216 new Value< std::pair<Real,Real> > 217 (std::make_pair(m_defaultStiffness, m_defaultQZero))); 218 } 219 220 //------------------------------------------------------------------------------ 221 // TOPOLOGY STATE 222 const SimbodyMatterSubsystem& m_matter; 223 const MobilizedBodyIndex m_mobodIx; 224 const MobilizerQIndex m_whichQ; 225 Real m_defaultStiffness; 226 Real m_defaultQZero; 227 228 // TOPOLOGY CACHE (Set only once in realizeTopology(); const thereafter.) 229 // The value is a pair<Real,Real> containing k,q0 230 mutable DiscreteVariableIndex m_paramsIx; 231 }; 232 233 234 235 //------------------------------------------------------------------------------ 236 // MOBILITY LINEAR DAMPER IMPL 237 //------------------------------------------------------------------------------ 238 class Force::MobilityLinearDamperImpl : public ForceImpl { 239 friend class MobilityLinearDamper; 240 241 MobilityLinearDamperImpl(const MobilizedBody& mobod, 242 MobilizerUIndex whichU, 243 Real defaultDamping); 244 getDamping(const State & state)245 const Real& getDamping(const State& state) const { 246 return Value<Real>::downcast 247 (getForceSubsystem().getDiscreteVariable(state, m_dampingIx)); 248 } updDamping(State & state)249 Real& updDamping(State& state) const { 250 return Value<Real>::updDowncast 251 (getForceSubsystem().updDiscreteVariable(state, m_dampingIx)); 252 } 253 clone()254 MobilityLinearDamperImpl* clone() const override 255 { return new MobilityLinearDamperImpl(*this); } 256 257 void calcForce(const State& state, Vector_<SpatialVec>& bodyForces, 258 Vector_<Vec3>& particleForces, Vector& mobilityForces) const 259 override; 260 Real calcPotentialEnergy(const State& state) const override; 261 262 // Allocate the discrete state variable for the parameters. realizeTopology(State & s)263 void realizeTopology(State& s) const override { 264 m_dampingIx = getForceSubsystem() 265 .allocateDiscreteVariable(s, Stage::Dynamics, 266 new Value<Real>(m_defaultDamping)); 267 } 268 269 //------------------------------------------------------------------------------ 270 // TOPOLOGY STATE 271 const SimbodyMatterSubsystem& m_matter; 272 const MobilizedBodyIndex m_mobodIx; 273 MobilizerUIndex m_whichU; 274 Real m_defaultDamping; 275 276 // TOPOLOGY CACHE (Set only once in realizeTopology(); const thereafter.) 277 // The value is a Real containing c. 278 mutable DiscreteVariableIndex m_dampingIx; 279 280 }; 281 282 283 284 //------------------------------------------------------------------------------ 285 // MOBILITY CONSTANT FORCE IMPL 286 //------------------------------------------------------------------------------ 287 // Hidden implementation class for a MobilityConstantForce force element. 288 class Force::MobilityConstantForceImpl : public ForceImpl { 289 friend class MobilityConstantForce; 290 291 MobilityConstantForceImpl(const MobilizedBody& mobod, 292 MobilizerUIndex whichU, 293 Real defaultForce); 294 getForce(const State & state)295 Real getForce(const State& state) const { 296 return Value<Real>::downcast 297 (getForceSubsystem().getDiscreteVariable(state, m_forceIx)); 298 } updForce(State & state)299 Real& updForce(State& state) const { 300 return Value<Real>::updDowncast 301 (getForceSubsystem().updDiscreteVariable(state, m_forceIx)); 302 } 303 304 // Implementation of virtual methods from ForceImpl: 305 clone()306 MobilityConstantForceImpl* clone() const override 307 { return new MobilityConstantForceImpl(*this); } 308 309 // Has to wait for Dynamics stage because that's all that gets invalidated 310 // if the constant force is changed. dependsOnlyOnPositions()311 bool dependsOnlyOnPositions() const override {return false;} 312 313 void calcForce(const State& state, Vector_<SpatialVec>& bodyForces, 314 Vector_<Vec3>& particleForces, Vector& mobilityForces) const 315 override; 316 calcPotentialEnergy(const State & state)317 Real calcPotentialEnergy(const State& state) const override {return 0;} 318 319 // Allocate the discrete state variable for the force. realizeTopology(State & s)320 void realizeTopology(State& s) const override { 321 m_forceIx = getForceSubsystem() 322 .allocateDiscreteVariable(s, Stage::Dynamics, 323 new Value<Real>(m_defaultForce)); 324 } 325 326 //------------------------------------------------------------------------------ 327 // TOPOLOGY STATE 328 const SimbodyMatterSubsystem& m_matter; 329 const MobilizedBodyIndex m_mobodIx; 330 const MobilizerUIndex m_whichU; 331 Real m_defaultForce; 332 333 // TOPOLOGY CACHE (Set only once in realizeTopology(); const thereafter.) 334 mutable DiscreteVariableIndex m_forceIx; 335 }; 336 337 //------------------------------------------------------------------------------ 338 // MOBILITY LINEAR STOP IMPL 339 //------------------------------------------------------------------------------ 340 // Hidden implementation class for a MobilityLinearStop force element. 341 class Force::MobilityLinearStopImpl : public ForceImpl { 342 friend class MobilityLinearStop; 343 344 // Type of the discrete state variable that holds values for this 345 // stop's changeable parameters in a State. Since these affect only forces 346 // the variable invalidates Dynamics stage and later when it changes. 347 struct Parameters { ParametersParameters348 Parameters(Real defStiffness, Real defDissipation, 349 Real defQLow, Real defQHigh) 350 : k(defStiffness), d(defDissipation), 351 qLow(defQLow), qHigh(defQHigh) {} 352 353 Parameters() = default; 354 355 Real k{NaN}, d{NaN}, qLow{NaN}, qHigh{NaN}; 356 }; 357 358 MobilityLinearStopImpl(const MobilizedBody& mobod, 359 MobilizerQIndex whichQ, 360 Real defaultStiffness, 361 Real defaultDissipation, 362 Real defaultQLow, 363 Real defaultQHigh); 364 365 // Implementation of virtual methods from ForceImpl: clone()366 MobilityLinearStopImpl* clone() const override 367 { return new MobilityLinearStopImpl(*this); } dependsOnlyOnPositions()368 bool dependsOnlyOnPositions() const override {return false;} 369 370 void calcForce(const State& state, Vector_<SpatialVec>& bodyForces, 371 Vector_<Vec3>& particleForces, Vector& mobilityForces) const 372 override; 373 374 // We're not bothering to cache P.E. -- just recalculate it when asked. 375 Real calcPotentialEnergy(const State& state) const override; 376 377 // Allocate the state variables and cache entry. realizeTopology(State & s)378 void realizeTopology(State& s) const override { 379 // Allocate the discrete variable for dynamics parameters. 380 const Parameters dv(m_defStiffness, m_defDissipation, 381 m_defQLow, m_defQHigh); 382 m_parametersIx = getForceSubsystem() 383 .allocateDiscreteVariable(s, Stage::Dynamics, 384 new Value<Parameters>(dv)); 385 } 386 getParameters(const State & s)387 const Parameters& getParameters(const State& s) const 388 { return Value<Parameters>::downcast 389 (getForceSubsystem().getDiscreteVariable(s,m_parametersIx)); } updParameters(State & s)390 Parameters& updParameters(State& s) const 391 { return Value<Parameters>::updDowncast 392 (getForceSubsystem().updDiscreteVariable(s,m_parametersIx)); } 393 394 //------------------------------------------------------------------------------ 395 // TOPOLOGY STATE 396 const SimbodyMatterSubsystem& m_matter; 397 const MobilizedBodyIndex m_mobodIx; 398 const MobilizerQIndex m_whichQ; 399 400 Real m_defStiffness, m_defDissipation, m_defQLow, m_defQHigh; 401 402 // TOPOLOGY CACHE (Set only once in realizeTopology(); const thereafter.) 403 mutable DiscreteVariableIndex m_parametersIx; // k, d, qLow, qHigh 404 }; 405 406 407 408 //------------------------------------------------------------------------------ 409 // MOBILITY DISCRETE FORCE IMPL 410 //------------------------------------------------------------------------------ 411 // This Force element allocates a scalar discrete variable and applies its 412 // value as a generalized force at a particular mobility. The value can be 413 // set externally in an event handler or between steps. 414 class Force::MobilityDiscreteForceImpl : public ForceImpl { 415 public: 416 MobilityDiscreteForceImpl(const MobilizedBody& mobod, 417 MobilizerUIndex whichU, 418 Real defaultForce); 419 420 // Change the force value to be applied. The force will remain at this 421 // value until changed again. 422 void setMobilityForce(State& state, Real f) const; 423 424 // Get the value of the generalized force to be applied. 425 Real getMobilityForce(const State& state) const; 426 427 // Override five virtuals from base class: 428 429 // This is called at Simbody's realize(Dynamics) stage. 430 void calcForce( const State& state, 431 Vector_<SpatialVec>& /*bodyForces*/, 432 Vector_<Vec3>& /*particleForces*/, 433 Vector& mobilityForces) const override; 434 435 // This force element does not store potential energy. calcPotentialEnergy(const State & state)436 Real calcPotentialEnergy(const State& state) const override {return 0;} 437 438 // Allocate the needed state variable and record its index. 439 void realizeTopology(State& state) const override; 440 clone()441 MobilityDiscreteForceImpl* clone() const override 442 { return new MobilityDiscreteForceImpl(*this); } 443 444 // Force this to wait for Dynamics stage before calculating, because that's 445 // all that gets invalidated when a new forces is applied. dependsOnlyOnPositions()446 bool dependsOnlyOnPositions() const override {return false;} 447 448 private: 449 friend class Force::MobilityDiscreteForce; 450 451 const SimbodyMatterSubsystem& m_matter; 452 const MobilizedBodyIndex m_mobodIx; 453 const MobilizerUIndex m_whichU; 454 Real m_defaultVal; 455 456 mutable DiscreteVariableIndex m_forceIx; 457 }; 458 459 460 461 //------------------------------------------------------------------------------ 462 // DISCRETE FORCES IMPL 463 //------------------------------------------------------------------------------ 464 // This Force element allocates a Vector discrete variables and applies their 465 // values as generalized and body spatial forces. The values can be 466 // set externally in an event handler or between steps. 467 class Force::DiscreteForcesImpl : public ForceImpl { 468 public: 469 DiscreteForcesImpl(const SimbodyMatterSubsystem& matter); 470 471 const Vector& getAllMobilityForces(const State& state) const; 472 Vector& updAllMobilityForces(State& state) const; 473 474 const Vector_<SpatialVec>& getAllBodyForces(const State& state) const; 475 Vector_<SpatialVec>& updAllBodyForces(State& state) const; 476 477 // Override five virtuals from base class: 478 479 // This is called at Simbody's realize(Dynamics) stage. 480 void calcForce( const State& state, 481 Vector_<SpatialVec>& bodyForces, 482 Vector_<Vec3>& particleForces, 483 Vector& mobilityForces) const override; 484 485 // This force element does not store potential energy. calcPotentialEnergy(const State & state)486 Real calcPotentialEnergy(const State& state) const override {return 0;} 487 488 // Allocate the needed state variable and record its index. 489 void realizeTopology(State& state) const override; 490 clone()491 DiscreteForcesImpl* clone() const override 492 { return new DiscreteForcesImpl(*this); } 493 494 // Force this to wait for Dynamics stage before calculating, because that's 495 // all that gets invalidated when a new forces is applied. dependsOnlyOnPositions()496 bool dependsOnlyOnPositions() const override {return false;} 497 498 private: 499 friend class Force::DiscreteForces; 500 501 const SimbodyMatterSubsystem& m_matter; 502 503 mutable DiscreteVariableIndex m_mobForcesIx; // Vector(n) 504 mutable DiscreteVariableIndex m_bodyForcesIx; // Vector_<SpatialVec>(nb) 505 }; 506 507 508 509 //------------------------------------------------------------------------------ 510 // CONSTANT FORCE IMPL 511 //------------------------------------------------------------------------------ 512 class Force::ConstantForceImpl : public ForceImpl { 513 public: 514 ConstantForceImpl(const MobilizedBody& body, const Vec3& station, const Vec3& force); clone()515 ConstantForceImpl* clone() const override { 516 return new ConstantForceImpl(*this); 517 } dependsOnlyOnPositions()518 bool dependsOnlyOnPositions() const override { 519 return true; 520 } 521 void calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const override; 522 Real calcPotentialEnergy(const State& state) const override; 523 private: 524 const SimbodyMatterSubsystem& matter; 525 const MobilizedBodyIndex body; 526 Vec3 station, force; 527 }; 528 529 530 531 //------------------------------------------------------------------------------ 532 // CONSTANT TORQUE IMPL 533 //------------------------------------------------------------------------------ 534 class Force::ConstantTorqueImpl : public ForceImpl { 535 public: 536 ConstantTorqueImpl(const MobilizedBody& body, const Vec3& torque); clone()537 ConstantTorqueImpl* clone() const override { 538 return new ConstantTorqueImpl(*this); 539 } dependsOnlyOnPositions()540 bool dependsOnlyOnPositions() const override { 541 return true; 542 } 543 void calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const override; 544 Real calcPotentialEnergy(const State& state) const override; 545 private: 546 const SimbodyMatterSubsystem& matter; 547 const MobilizedBodyIndex body; 548 Vec3 torque; 549 }; 550 551 552 553 //------------------------------------------------------------------------------ 554 // GLOBAL DAMPER IMPL 555 //------------------------------------------------------------------------------ 556 class Force::GlobalDamperImpl : public ForceImpl { 557 public: 558 GlobalDamperImpl(const SimbodyMatterSubsystem& matter, Real damping); clone()559 GlobalDamperImpl* clone() const override { 560 return new GlobalDamperImpl(*this); 561 } 562 void calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const override; 563 Real calcPotentialEnergy(const State& state) const override; 564 private: 565 const SimbodyMatterSubsystem& matter; 566 Real damping; 567 }; 568 569 570 571 //------------------------------------------------------------------------------ 572 // UNIFORM GRAVITY IMPL 573 //------------------------------------------------------------------------------ 574 class Force::UniformGravityImpl : public ForceImpl { 575 public: 576 UniformGravityImpl(const SimbodyMatterSubsystem& matter, const Vec3& g, Real zeroHeight); clone()577 UniformGravityImpl* clone() const override { 578 return new UniformGravityImpl(*this); 579 } 580 void calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const override; 581 Real calcPotentialEnergy(const State& state) const override; getGravity()582 Vec3 getGravity() const { 583 return g; 584 } setGravity(const Vec3 & gravity)585 void setGravity(const Vec3& gravity) { 586 g = gravity; 587 invalidateTopologyCache(); 588 } getZeroHeight()589 Real getZeroHeight() const { 590 return zeroHeight; 591 } setZeroHeight(Real height)592 void setZeroHeight(Real height) { 593 zeroHeight = height; 594 invalidateTopologyCache(); 595 } 596 private: 597 const SimbodyMatterSubsystem& matter; 598 Vec3 g; 599 Real zeroHeight; 600 }; 601 602 603 604 //------------------------------------------------------------------------------ 605 // CUSTOM IMPL 606 //------------------------------------------------------------------------------ 607 class Force::CustomImpl : public ForceImpl { 608 public: 609 CustomImpl(Force::Custom::Implementation* implementation); clone()610 CustomImpl* clone() const override { 611 return new CustomImpl(*this); 612 } dependsOnlyOnPositions()613 bool dependsOnlyOnPositions() const override { 614 return implementation->dependsOnlyOnPositions(); 615 } 616 void calcForce(const State& state, Vector_<SpatialVec>& bodyForces, 617 Vector_<Vec3>& particleForces, Vector& mobilityForces) 618 const override; 619 Real calcPotentialEnergy(const State& state) const override; shouldBeParallelIfPossible()620 bool shouldBeParallelIfPossible() const override { 621 return implementation->shouldBeParallelIfPossible(); 622 } ~CustomImpl()623 ~CustomImpl() { 624 delete implementation; 625 } getImplementation()626 const Force::Custom::Implementation& getImplementation() const { 627 return *implementation; 628 } updImplementation()629 Force::Custom::Implementation& updImplementation() { 630 return *implementation; 631 } 632 protected: realizeTopology(State & state)633 void realizeTopology(State& state) const override { 634 implementation->realizeTopology(state); 635 } realizeModel(State & state)636 void realizeModel(State& state) const override { 637 implementation->realizeModel(state); 638 } realizeInstance(const State & state)639 void realizeInstance(const State& state) const override { 640 implementation->realizeInstance(state); 641 } realizeTime(const State & state)642 void realizeTime(const State& state) const override { 643 implementation->realizeTime(state); 644 } realizePosition(const State & state)645 void realizePosition(const State& state) const override { 646 implementation->realizePosition(state); 647 } realizeVelocity(const State & state)648 void realizeVelocity(const State& state) const override { 649 implementation->realizeVelocity(state); 650 } realizeDynamics(const State & state)651 void realizeDynamics(const State& state) const override { 652 implementation->realizeDynamics(state); 653 } realizeAcceleration(const State & state)654 void realizeAcceleration(const State& state) const override { 655 implementation->realizeAcceleration(state); 656 } realizeReport(const State & state)657 void realizeReport(const State& state) const override { 658 implementation->realizeReport(state); 659 } calcDecorativeGeometryAndAppend(const State & state,Stage stage,Array_<DecorativeGeometry> & geom)660 void calcDecorativeGeometryAndAppend(const State& state, Stage stage, 661 Array_<DecorativeGeometry>& geom) const override 662 { 663 implementation->calcDecorativeGeometryAndAppend(state,stage,geom); 664 } 665 private: 666 Force::Custom::Implementation* implementation; 667 }; 668 669 } // namespace SimTK 670 671 #endif // SimTK_SIMBODY_FORCE_IMPL_H_ 672