Simbody 3.7
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"
34
35#include <cassert>
36#include <cmath>
37#include <algorithm>
38
39namespace SimTK {
40
41
42//==============================================================================
43// GEO CUBIC HERMITE CURVE
44//==============================================================================
130template <class P>
132typedef P RealP;
133typedef Vec<3,RealP> Vec3P;
134
135public:
142explicit CubicHermiteCurve_(const Vec<4,Vec3P>& A) : A(A) {}
145const Vec<4,Vec3P>& getAlgebraicCoefficients() const {return A;}
153Vec3P evalP(RealP u) const {return evalPUsingA(A,u);}
157Vec3P evalPu(RealP u) const {return evalPuUsingA(A,u);}
161Vec3P evalPuu(RealP u) const {return evalPuuUsingA(A,u);}
165Vec3P evalPuuu(RealP u) const {return evalPuuuUsingA(A,u);}
166
172static Row<4,P> calcU(RealP u) {
173 const RealP u2 = u*u;
174 return Row<4,P>(u*u2, u2, u, 1);
175}
176
179static 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
187static 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
195static 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
204static 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
232static 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}
238static Vec3P evalPuUsingA(const Vec<4,Vec3P>& A, RealP u) {
239 return (3*u*u)*A[0] + (2*u)*A[1] + A[2];
240}
243static Vec3P evalPuuUsingA(const Vec<4,Vec3P>& A, RealP u) {
244 return (6*u)*A[0] + 2*A[1];
245}
250static Vec3P evalPuuuUsingA(const Vec<4,Vec3P>& A, RealP u) {
251 return 6*A[0];
252}
253
259static Vec3P evalPUsingH(const Vec<4,Vec3P>& H, RealP u) {
260 return calcFh(u)*H; // 10 + 3*7 = 31 flops
261}
267static Vec3P evalPuUsingH(const Vec<4,Vec3P>& H, RealP u) {
268 return calcFhu(u)*H; // 10 + 3*7 = 31 flops
269}
275static Vec3P evalPuuUsingH(const Vec<4,Vec3P>& H, RealP u) {
276 return calcFhuu(u)*H; // 6 + 3*7 = 27 flops
277}
283static Vec3P evalPuuuUsingH(const Vec<4,Vec3P>& H, RealP u) {
284 return calcFhuuu(u)*H; // 0 + 3*7 = 21 flops
285}
286
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
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}
326private:
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
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 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
static Mat< 4, 4, P > getMh()
Obtain the Hermite basis matrix Mh explicitly.
Definition: Geo_CubicHermiteCurve.h:291
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 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 Mat< 4, 4, P > getMhInv()
Obtain the inverse inv(Mh) of the Hermite basis matrix explicitly.
Definition: Geo_CubicHermiteCurve.h:310
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 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 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
CubicHermiteCurve_()
Construct an uninitialized curve; coefficients will be garbage.
Definition: Geo_CubicHermiteCurve.h:137
static Row< 4, P > calcU(RealP u)
Return the row vector U=[u^3 u^2 u 1].
Definition: Geo_CubicHermiteCurve.h:172
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 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 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
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 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 > calcFhuuu(RealP u)
Calculate third derivatives Fhuuu=[F1uuu..F4uuu] of the Hermite basis functions for a given value of ...
Definition: Geo_CubicHermiteCurve.h:204
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
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