Simbody  3.8
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>
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_
Defines geometric primitive shapes and algorthms.
Includes internal headers providing declarations for the basic SimTK Core classes,...
This is the header file that every Simmath compilation unit should include first.
A primitive useful for computations involving a single cubic Hermite curve segment in algebraic or ge...
Definition: Geo_CubicHermiteCurve.h:131
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
static Mat< 4, 4, P > getMh()
Obtain the Hermite basis matrix Mh explicitly.
Definition: Geo_CubicHermiteCurve.h:291
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
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
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
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
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
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 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
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
Vec3P evalPuuu(RealP u) const
Evaluate the third derivative Puuu=d3P/du3 on this curve.
Definition: Geo_CubicHermiteCurve.h:165
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
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 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 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 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
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 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
Vec< 4, Vec3P > calcGeometricCoefficients() const
Calculate the Hermite coefficients H=[h0 h1 hu0 hu1] from the stored algebraic coefficients.
Definition: Geo_CubicHermiteCurve.h:149
static Mat< 4, 4, P > getMhInv()
Obtain the inverse inv(Mh) of the Hermite basis matrix explicitly.
Definition: Geo_CubicHermiteCurve.h:310
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
CubicHermiteCurve_()
Construct an uninitialized curve; coefficients will be garbage.
Definition: Geo_CubicHermiteCurve.h:137
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 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
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
static Row< 4, P > calcU(RealP u)
Return the row vector U=[u^3 u^2 u 1].
Definition: Geo_CubicHermiteCurve.h:172
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: Mat.h:97
This is a fixed-length row vector designed for no-overhead inline computation.
Definition: Row.h:132
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37