1 /* -------------------------------------------------------------------------- *
2 * OpenSim: Point.cpp *
3 * -------------------------------------------------------------------------- *
4 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. *
5 * See http://opensim.stanford.edu and the NOTICE file for more information. *
6 * OpenSim is developed at Stanford University and supported by the US *
7 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA *
8 * through the Warrior Web program. *
9 * *
10 * Copyright (c) 2005-2017 Stanford University and the Authors *
11 * Author(s): Ajay Seth *
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 //=============================================================================
25 // INCLUDES
26 //=============================================================================
27 #include "Point.h"
28 #include "Frame.h"
29
30 //=============================================================================
31 // STATICS
32 //=============================================================================
33 using namespace std;
34 using namespace OpenSim;
35 using SimTK::Mat33;
36 using SimTK::Vec3;
37
38 //=============================================================================
39 // CONSTRUCTOR(S)
40 //=============================================================================
41 //_____________________________________________________________________________
42 /**
43 * Default constructor.
44 */
Point()45 Point::Point() : ModelComponent()
46 {
47 setAuthors("Ajay Seth");
48 }
49
50
extendAddToSystem(SimTK::MultibodySystem & system) const51 void Point::extendAddToSystem(SimTK::MultibodySystem& system) const
52 {
53 Super::extendAddToSystem(system);
54 SimTK::Vec3 v(SimTK::NaN);
55 // If the properties, topology or coordinate values change,
56 // Stage::Position will be invalid.
57 addCacheVariable("location", v, SimTK::Stage::Position);
58 addCacheVariable("velocity", v, SimTK::Stage::Velocity);
59 addCacheVariable("acceleration", v, SimTK::Stage::Acceleration);
60 }
61
getLocationInGround(const SimTK::State & s) const62 const SimTK::Vec3& Point::getLocationInGround(const SimTK::State& s) const
63 {
64 if (!getSystem().getDefaultSubsystem().
65 isCacheValueRealized(s, _locationIndex)){
66 //cache is not valid so calculate the transform
67 SimTK::Value<SimTK::Vec3>::downcast(
68 getSystem().getDefaultSubsystem().
69 updCacheEntry(s, _locationIndex)).upd()
70 = calcLocationInGround(s);
71 // mark cache as up-to-date
72 getSystem().getDefaultSubsystem().
73 markCacheValueRealized(s, _locationIndex);
74 }
75 return SimTK::Value<SimTK::Vec3>::downcast(
76 getSystem().getDefaultSubsystem().
77 getCacheEntry(s, _locationIndex)).get();
78 }
79
getVelocityInGround(const SimTK::State & s) const80 const SimTK::Vec3& Point::getVelocityInGround(const SimTK::State& s) const
81 {
82 if (!getSystem().getDefaultSubsystem().
83 isCacheValueRealized(s, _velocityIndex)) {
84 //cache is not valid so calculate the transform
85 SimTK::Value<SimTK::Vec3>::downcast(
86 getSystem().getDefaultSubsystem().
87 updCacheEntry(s, _velocityIndex)).upd()
88 = calcVelocityInGround(s);
89 // mark cache as up-to-date
90 getSystem().getDefaultSubsystem().
91 markCacheValueRealized(s, _velocityIndex);
92 }
93 return SimTK::Value<SimTK::Vec3>::downcast(
94 getSystem().getDefaultSubsystem().
95 getCacheEntry(s, _velocityIndex)).get();
96 }
97
getAccelerationInGround(const SimTK::State & s) const98 const SimTK::Vec3& Point::getAccelerationInGround(const SimTK::State& s) const
99 {
100 if (!getSystem().getDefaultSubsystem().
101 isCacheValueRealized(s, _accelerationIndex)) {
102 //cache is not valid so calculate the transform
103 SimTK::Value<SimTK::Vec3>::downcast(
104 getSystem().getDefaultSubsystem().
105 updCacheEntry(s, _accelerationIndex)).upd()
106 = calcAccelerationInGround(s);
107 // mark cache as up-to-date
108 getSystem().getDefaultSubsystem().
109 markCacheValueRealized(s, _accelerationIndex);
110 }
111 return SimTK::Value<SimTK::Vec3>::downcast(
112 getSystem().getDefaultSubsystem().
113 getCacheEntry(s, _accelerationIndex)).get();
114 }
115
116 //=============================================================================
117 // Helpful Point Calculations
118 //=============================================================================
calcDistanceBetween(const SimTK::State & s,const Point & o) const119 double Point::calcDistanceBetween(const SimTK::State& s, const Point& o) const
120 {
121 return (getLocationInGround(s) - o.getLocationInGround(s)).norm();
122 }
123
calcDistanceBetween(const SimTK::State & s,const Frame & f,const SimTK::Vec3 & p) const124 double Point::calcDistanceBetween(const SimTK::State& s,
125 const Frame& f, const SimTK::Vec3& p) const
126 {
127 return (getLocationInGround(s) - f.getTransformInGround(s)*p).norm();
128 }
129
calcSpeedBetween(const SimTK::State & s,const Point & o) const130 double Point::calcSpeedBetween(const SimTK::State& s, const Point& o) const
131 {
132 const auto r = getLocationInGround(s) - o.getLocationInGround(s);
133 const double d = r.norm();
134 const auto v = getVelocityInGround(s) - o.getVelocityInGround(s);
135 if (d < SimTK::Eps) // avoid divide by zero
136 return v.norm();
137 else // speed is the projection of relative velocity, v, onto the
138 // displacement unit vector, r_hat = r/d;
139 return dot(v, r/d);
140 }
141
142 //=============================================================================
143 // Component level realizations
144 //=============================================================================
extendRealizeTopology(SimTK::State & s) const145 void Point::extendRealizeTopology(SimTK::State& s) const
146 {
147 Super::extendRealizeTopology(s);
148 _locationIndex = getCacheVariableIndex("location");
149 _velocityIndex = getCacheVariableIndex("velocity");
150 _accelerationIndex = getCacheVariableIndex("acceleration");
151 }
152