Simbody  3.5
Geo_CubicHermiteCurve.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMMATH_GEO_CUBIC_HERMITE_CURVE_H_
2 #define SimTK_SIMMATH_GEO_CUBIC_HERMITE_CURVE_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) 2011-12 Stanford University and the Authors. *
13  * Authors: 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 
31 #include "SimTKcommon.h"
33 #include "simmath/internal/Geo.h"
34 
35 #include <cassert>
36 #include <cmath>
37 #include <algorithm>
38 
39 namespace SimTK {
40 
41 
42 //==============================================================================
43 // GEO CUBIC HERMITE CURVE
44 //==============================================================================
130 template <class P>
131 class Geo::CubicHermiteCurve_ {
132 typedef P RealP;
133 typedef Vec<3,RealP> Vec3P;
134 
135 public:
142 explicit CubicHermiteCurve_(const Vec<4,Vec3P>& A) : A(A) {}
145 const Vec<4,Vec3P>& getAlgebraicCoefficients() const {return A;}
153 Vec3P evalP(RealP u) const {return evalPUsingA(A,u);}
157 Vec3P evalPu(RealP u) const {return evalPuUsingA(A,u);}
161 Vec3P evalPuu(RealP u) const {return evalPuuUsingA(A,u);}
165 Vec3P evalPuuu(RealP u) const {return evalPuuuUsingA(A,u);}
166 
172 static Row<4,P> calcU(RealP u) {
173  const RealP u2 = u*u;
174  return Row<4,P>(u*u2, u2, u, 1);
175 }
176 
179 static Row<4,P> calcFh(RealP u) {
180  const RealP u2 = u*u, u3 = u*u2;
181  const RealP t = 3*u2 - 2*u3;
182  return Row<4,P>(1-t, t, u3-2*u2+u, u3-u2);
183 }
184 
187 static Row<4,P> calcFhu(RealP u) {
188  const RealP u2 = u*u, u23=3*u2;
189  const RealP dt = 6*(u-u2);
190  return Row<4,P>(-dt, dt, u23-4*u+1, u23-2*u);
191 }
192 
195 static Row<4,P> calcFhuu(RealP u) {
196  const RealP u6 = 6*u;
197  const RealP ddt = 6 - 12*u;
198  return Row<4,P>(-ddt, ddt, u6-4, u6-2);
199 }
200 
204 static Row<4,P> calcFhuuu(RealP u) {
205  return Row<4,P>(12, -12, 6, 6);
206 }
207 
212  const Vec3P& h0= H[0]; const Vec3P& h1= H[1]; // aliases for beauty
213  const Vec3P& hu0=H[2]; const Vec3P& hu1=H[3];
214  const Vec3P h01 = h0 - h1;
215  return Vec<4,Vec3P>( 2*h01 + hu0 + hu1,
216  -3*h01 - 2*hu0 - hu1,
217  hu0,
218  h0 );
219 }
220 
225  const Vec3P& a3=A[0]; const Vec3P& a2=A[1]; // aliases for beauty
226  const Vec3P& a1=A[2]; const Vec3P& a0=A[3];
227  return Vec<4,Vec3P>(a0, a3+a2+a1+a0, a1, 3*a3+2*a2+a1);
228 }
229 
232 static Vec3P evalPUsingA(const Vec<4,Vec3P>& A, RealP u) {
233  const RealP u2 = u*u, u3 = u*u2;
234  return u3*A[0] + u2*A[1] + u*A[2] + A[3]; // (vectors)
235 }
238 static Vec3P evalPuUsingA(const Vec<4,Vec3P>& A, RealP u) {
239  return (3*u*u)*A[0] + (2*u)*A[1] + A[2];
240 }
243 static Vec3P evalPuuUsingA(const Vec<4,Vec3P>& A, RealP u) {
244  return (6*u)*A[0] + 2*A[1];
245 }
250 static Vec3P evalPuuuUsingA(const Vec<4,Vec3P>& A, RealP u) {
251  return 6*A[0];
252 }
253 
259 static Vec3P evalPUsingH(const Vec<4,Vec3P>& H, RealP u) {
260  return calcFh(u)*H; // 10 + 3*7 = 31 flops
261 }
267 static Vec3P evalPuUsingH(const Vec<4,Vec3P>& H, RealP u) {
268  return calcFhu(u)*H; // 10 + 3*7 = 31 flops
269 }
275 static Vec3P evalPuuUsingH(const Vec<4,Vec3P>& H, RealP u) {
276  return calcFhuu(u)*H; // 6 + 3*7 = 27 flops
277 }
283 static Vec3P evalPuuuUsingH(const Vec<4,Vec3P>& H, RealP u) {
284  return calcFhuuu(u)*H; // 0 + 3*7 = 21 flops
285 }
286 
291 static Mat<4,4,P> getMh() {
292  return Mat<4,4,P>( 2, -2, 1, 1,
293  -3, 3, -2, -1,
294  0, 0, 1, 0,
295  1, 0, 0, 0 );
296 }
297 
300 static Vec<4,P> multiplyByMh(const Vec<4,P>& v) {
301  const RealP v0=v[0], v1=v[1], v2=v[2], v3=v[3];
302  const RealP v01 = v0-v1;
303  return Vec<4,P>(2*v01+v2+v3, -3*v01-2*v2-v3, v2, v0);
304 }
305 
311  return Mat<4,4,P>( 0, 0, 0, 1,
312  1, 1, 1, 1,
313  0, 0, 1, 0,
314  3, 2, 1, 0 );
315 }
316 
321  const RealP v0=v[0], v1=v[1], v2=v[2], v3=v[3];
322  return Vec<4,P>(v3, v0+v1+v2+v3, v2, 3*v0+2*v1+v2);
323 }
326 private:
327 Vec<4,Vec3P> A;
328 };
329 
330 
331 
332 } // namespace SimTK
333 
334 #endif // SimTK_SIMMATH_GEO_CUBIC_HERMITE_CURVE_H_
static Mat< 4, 4, P > getMhInv()
Obtain the inverse inv(Mh) of the Hermite basis matrix explicitly.
Definition: Geo_CubicHermiteCurve.h:310
Vec3P evalP(RealP u) const
Evaluate a point P(u) on this curve given a value for parameter u in [0,1].
Definition: Geo_CubicHermiteCurve.h:153
static Vec3P evalPUsingH(const Vec< 4, Vec3P > &H, RealP u)
Given Hermite coefficients H and a value for the curve parameter u, return the point P(u) at that loc...
Definition: Geo_CubicHermiteCurve.h:259
static Row< 4, P > calcFhuuu(RealP u)
Calculate third derivatives Fhuuu=[F1uuu..F4uuu] of the Hermite basis functions for a given value of ...
Definition: Geo_CubicHermiteCurve.h:204
const Vec< 4, Vec3P > & getAlgebraicCoefficients() const
Return a reference to the algebraic coefficients A=[a3 a2 a1 a0] that are stored in this object...
Definition: Geo_CubicHermiteCurve.h:145
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
static Vec3P evalPuUsingH(const Vec< 4, Vec3P > &H, RealP u)
Given Hermite coefficients H and a value for the curve parameter u, return the first derivative Pu(u)...
Definition: Geo_CubicHermiteCurve.h:267
static Vec3P evalPUsingA(const Vec< 4, Vec3P > &A, RealP u)
Given algebraic coefficients A and a value for the curve parameter u, return the point P(u) at that l...
Definition: Geo_CubicHermiteCurve.h:232
static Mat< 4, 4, P > getMh()
Obtain the Hermite basis matrix Mh explicitly.
Definition: Geo_CubicHermiteCurve.h:291
static Vec3P evalPuUsingA(const Vec< 4, Vec3P > &A, RealP u)
Given algebraic coefficients A and a value for the curve parameter u, return the first derivative Pu(...
Definition: Geo_CubicHermiteCurve.h:238
static Row< 4, P > calcFh(RealP u)
Calculate the Hermite basis functions Fh=[F1..F4] for a given value of the parameter u...
Definition: Geo_CubicHermiteCurve.h:179
Vec3P evalPuuu(RealP u) const
Evaluate the third derivative Puuu=d3P/du3 on this curve.
Definition: Geo_CubicHermiteCurve.h:165
static Vec3P evalPuuUsingA(const Vec< 4, Vec3P > &A, RealP u)
Given algebraic coefficients A and a value for the curve parameter u, return the second derivative Pu...
Definition: Geo_CubicHermiteCurve.h:243
This is a fixed-length column vector designed for no-overhead inline computation. ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:605
CubicHermiteCurve_(const Vec< 4, Vec3P > &A)
Construct a cubic Hermite curve using the given algebraic coefficients A=[a3 a2 a1 a0]...
Definition: Geo_CubicHermiteCurve.h:142
Includes internal headers providing declarations for the basic SimTK Core classes, including Simmatrix.
static Vec< 4, P > multiplyByMh(const Vec< 4, P > &v)
Form the product of the Hermite basis matrix Mh and a 4-vector, exploiting the structure of Mh (which...
Definition: Geo_CubicHermiteCurve.h:300
static Vec< 4, P > multiplyByMhInv(const Vec< 4, P > &v)
Form the product of the inverse Hermite basis matrix inv(Mh) and a 4-vector, exploiting the structure...
Definition: Geo_CubicHermiteCurve.h:320
static Row< 4, P > calcFhuu(RealP u)
Calculate second derivatives Fhuu=[F1uu..F4uu] of the Hermite basis functions for a given value of th...
Definition: Geo_CubicHermiteCurve.h:195
CubicHermiteCurve_()
Construct an uninitialized curve; coefficients will be garbage.
Definition: Geo_CubicHermiteCurve.h:137
static Vec3P evalPuuuUsingA(const Vec< 4, Vec3P > &A, RealP u)
Given algebraic coefficients A and a value for the curve parameter u, return the third derivative Puu...
Definition: Geo_CubicHermiteCurve.h:250
Defines geometric primitive shapes and algorthms.
static Row< 4, P > calcU(RealP u)
Return the row vector U=[u^3 u^2 u 1].
Definition: Geo_CubicHermiteCurve.h:172
static Vec3P evalPuuUsingH(const Vec< 4, Vec3P > &H, RealP u)
Given Hermite coefficients H and a value for the curve parameter u, return the second derivative Puu(...
Definition: Geo_CubicHermiteCurve.h:275
static Row< 4, P > calcFhu(RealP u)
Calculate first derivatives Fhu=[F1u..F4u] of the Hermite basis functions for a given value of the pa...
Definition: Geo_CubicHermiteCurve.h:187
This is the header file that every Simmath compilation unit should include first. ...
This is a fixed-length row vector designed for no-overhead inline computation.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:606
static Vec< 4, Vec3P > calcAFromH(const Vec< 4, Vec3P > &H)
Given the Hermite coefficients H=~[h0 h1 hu0 hu1], return the algebraic coefficients A=~[a3 a2 a1 a0]...
Definition: Geo_CubicHermiteCurve.h:211
Vec< 4, Vec3P > calcGeometricCoefficients() const
Calculate the Hermite coefficients H=[h0 h1 hu0 hu1] from the stored algebraic coefficients.
Definition: Geo_CubicHermiteCurve.h:149
Vec3P evalPu(RealP u) const
Evaluate the tangent Pu=dP/du on this curve given a value for parameter u in [0,1].
Definition: Geo_CubicHermiteCurve.h:157
Vec3P evalPuu(RealP u) const
Evaluate the second derivative Puu=d2P/du2 on this curve given a value for parameter u in [0...
Definition: Geo_CubicHermiteCurve.h:161
static Vec< 4, Vec3P > calcHFromA(const Vec< 4, Vec3P > &A)
Given the algebraic coefficients A=~[a3 a2 a1 a0], return the Hermite coefficients H=~[h0 h1 hu0 hu1]...
Definition: Geo_CubicHermiteCurve.h:224
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:607
static Vec3P evalPuuuUsingH(const Vec< 4, Vec3P > &H, RealP u)
Given Hermite coefficients H and a value for the curve parameter u, return the third derivative Puuu(...
Definition: Geo_CubicHermiteCurve.h:283