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