Simbody 3.7
Geo_CubicBezierCurve.h
Go to the documentation of this file.
1#ifndef SimTK_SIMMATH_GEO_CUBIC_BEZIER_CURVE_H_
2#define SimTK_SIMMATH_GEO_CUBIC_BEZIER_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"
37
38#include <cassert>
39#include <cmath>
40#include <algorithm>
41
42namespace SimTK {
43
44
45//==============================================================================
46// GEO CUBIC BEZIER CURVE
47//==============================================================================
136template <class P>
138typedef P RealP;
139typedef Vec<3,RealP> Vec3P;
140typedef UnitVec<P,1> UnitVec3P;
141typedef Rotation_<P> RotationP;
143
144public:
147
150template <int S>
151explicit CubicBezierCurve_(const Vec<4,Vec3P,S>& controlPoints)
152: B(controlPoints) {}
153
156template <int S>
157explicit CubicBezierCurve_(const Row<4,Vec3P,S>& controlPoints)
158: B(controlPoints.positionalTranspose()) {}
159
162const Vec<4,Vec3P>& getControlPoints() const {return B;}
172Vec3P evalP(RealP u) const {return evalPUsingB(B,u);}
176Vec3P evalPu(RealP u) const {return evalPuUsingB(B,u);}
180Vec3P evalPuu(RealP u) const {return evalPuuUsingB(B,u);}
184Vec3P evalPuuu(RealP u) const {return evalPuuuUsingB(B,u);}
185
189RealP calcDsdu(RealP u) const {return evalPu(u).norm();}
190
195 const Vec3P Pu=evalPu(u); // 15 flops
196 return Geo::calcUnitTangent(Pu); // ~40 flops
197}
198
203 const Vec3P Pu=evalPu(u), Puu=evalPuu(u); // 25 flops
204 return Geo::calcCurvatureVector(Pu,Puu); // ~30 flops
205}
206
210RealP calcCurvatureSqr(RealP u) {
211 const Vec3P Pu=evalPu(u), Puu=evalPuu(u); // 25 flops
212 return Geo::calcCurvatureSqr(Pu,Puu); // ~30 flops
213}
214
221RealP calcTorsion(RealP u) {
222 const Vec3P Pu=evalPu(u), Puu=evalPuu(u), Puuu=evalPuuu(u); // 28 flops
223 return Geo::calcTorsion(Pu,Puu,Puuu);
224}
225
226
234UnitVec3P calcUnitNormal(RealP u) const {
235 const Vec3P Pu=evalPu(u), Puu=evalPuu(u); // 25 flops
236 return Geo::calcUnitNormal(Pu,Puu); // ~80 flops
237}
238
248RealP calcCurveFrame(RealP u, TransformP& X_FP) const {
249 const Vec3P Pval=evalP(u), Pu=evalPu(u), Puu=evalPuu(u); // 45 flops
250 return Geo::calcCurveFrame(Pval,Pu,Puu,X_FP);
251}
252
259void split(RealP u, CubicBezierCurve_<P>& left,
260 CubicBezierCurve_<P>& right) const {
261 const RealP tol = getDefaultTol<RealP>();
262 SimTK_ERRCHK1(tol <= u && u <= 1-tol, "Geo::CubicBezierCurve::split()",
263 "Can't split curve at parameter %g; it is either out of range or"
264 " too close to an end point.", (double)u);
265
266 const RealP u1 = 1-u;
267 const Vec3P p01 = u1*B[0] + u*B[1]; // 3x9 flops
268 const Vec3P p12 = u1*B[1] + u*B[2];
269 const Vec3P p23 = u1*B[2] + u*B[3];
270 left.B[0] = B[0];
271 left.B[1] = p01;
272 left.B[2] = u1*p01 + u*p12; // 3x3 flops
273
274 right.B[3] = B[3];
275 right.B[2] = p23;
276 right.B[1] = u1*p12 + u*p23;
277 left.B[3] = right.B[0] = u1*left.B[2] + u*right.B[1]; // 3x3 flops
278}
279
284 CubicBezierCurve_<P>& right) const {
285 const Vec3P p01 = (B[0] + B[1])/2; // 3x6 flops
286 const Vec3P p12 = (B[1] + B[2])/2;
287 const Vec3P p23 = (B[2] + B[3])/2;
288 left.B[0] = B[0];
289 left.B[1] = p01;
290 left.B[2] = (p01 + p12)/2; // 3x2 flops
291
292 right.B[3] = B[3];
293 right.B[2] = p23;
294 right.B[1] = (p12 + p23)/2;
295 left.B[3] = right.B[0] = (left.B[2] + right.B[1])/2; // 3x2 flops
296}
297
298
304{ return Geo::Point_<P>::calcBoundingSphere(B[0],B[1],B[2],B[3]); }
305
311{ const ArrayViewConst_<Vec3P> points(&B[0], &B[0]+4); // no copy or heap use
313
319{ const ArrayViewConst_<Vec3P> points(&B[0], &B[0]+4); // no copy or heap use
321
328static Row<4,P> calcFb(RealP u) {
329 const RealP u2 = u*u, u3 = u*u2; // powers of u
330 const RealP u1 = 1-u, u12=u1*u1, u13=u1*u12; // powers of 1-u
331 return Row<4,P>(u13, 3*u*u12, 3*u2*u1, u3);
332}
333
336static Row<4,P> calcDFb(RealP u) {
337 const RealP u6=6*u, u2 = u*u, u23 = 3*u2, u29 = 9*u2;
338 return Row<4,P>(u6-u23-3, u29-12*u+3, u6-u29, u23);
339}
340
343static Row<4,P> calcD2Fb(RealP u) {
344 const RealP u6 = 6*u, u18 = 18*u;
345 return Row<4,P>(6-u6, u18-12, 6-u18, u6);
346}
347
351static Row<4,P> calcD3Fb(RealP u) {
352 return Row<4,P>(-6, 18, -18, 6);
353}
354
358template <int S>
360 const Vec3P& b0=B[0]; const Vec3P& b1=B[1]; // aliases for beauty
361 const Vec3P& b2=B[2]; const Vec3P& b3=B[3];
362 return Vec<4,Vec3P>(b3-b0+3*(b1-b2), 3*(b0+b2)-6*b1, 3*(b1-b0), b0);
363}
364
368template <int S>
370 const Vec3P& a3=A[0]; const Vec3P& a2=A[1]; // aliases for beauty
371 const Vec3P& a1=A[2]; const Vec3P& a0=A[3];
372 return Vec<4,Vec3P>(a0, a1/3 + a0, (a2+2*a1)/3 + a0, a3+a2+a1+a0);
373}
374
378template <int S>
380 const Vec3P& b0=B[0]; const Vec3P& b1=B[1]; // aliases for beauty
381 const Vec3P& b2=B[2]; const Vec3P& b3=B[3];
382 return Vec<4,Vec3P>(b0, b3, 3*(b1-b0), 3*(b3-b2));
383}
384
388template <int S>
390 const Vec3P& h0= H[0]; const Vec3P& h1= H[1]; // aliases for beauty
391 const Vec3P& hu0=H[2]; const Vec3P& hu1=H[3];
392 return Vec<4,Vec3P>(h0, h0 + hu0/3, h1 - hu1/3, h1);
393}
394
400template <int S>
401static Vec3P evalPUsingB(const Vec<4,Vec3P,S>& B, RealP u) {
402 return calcFb(u)*B; // 9 + 3*7 = 30 flops
403}
409template <int S>
410static Vec3P evalPuUsingB(const Vec<4,Vec3P,S>& B, RealP u) {
411 return calcDFb(u)*B; // 10 + 3*7 = 31 flops
412}
418template <int S>
419static Vec3P evalPuuUsingB(const Vec<4,Vec3P,S>& B, RealP u) {
420 return calcD2Fb(u)*B; // 5 + 3*7 = 26 flops
421}
427template <int S>
428static Vec3P evalPuuuUsingB(const Vec<4,Vec3P,S>& B, RealP u) {
429 return calcD3Fb(u)*B; // 0 + 3*7 = 21 flops
430}
436 return Mat<4,4,P>( -1, 3, -3, 1,
437 3, -6, 3, 0,
438 -3, 3, 0, 0,
439 1, 0, 0, 0);
440}
441
446template <int S>
448 const RealP b0=b[0], b1=b[1], b2=b[2], b3=b[3];
449 return Vec<4,P>(3*(b1-b2)+b3-b0, 3*(b0+b2)-6*b1, 3*(b1-b0), b0);
450}
451
458 return Mat<4,4,P>( 0, 0, 0, 1,
459 0, 0, P(1)/3, 1,
460 0, P(1)/3, P(2)/3, 1,
461 1, 1, 1, 1 );
462}
463
468template <int S>
470 const RealP b0=b[0], b1=b[1], b2=b[2], b3=b[3];
471 return Vec<4,P>(b3, b2/3+b3, (b1+2*b2)/3+b3, b0+b1+b2+b3);
472}
473
482 return Mat<4,4,P>( 1, 0, 0, 0,
483 0, 0, 0, 1,
484 -3, 3, 0, 0,
485 0, 0, -3, 3 );
486}
489template <int S>
491 const RealP v0=v[0], v1=v[1], v2=v[2], v3=v[3];
492 return Vec<4,P>(v0, v3, 3*(v1-v0), 3*(v3-v2));
493}
494
504 return Mat<4,4,P>( 1, 0, 0, 0,
505 1, 0, P(1)/3, 0,
506 0, 1, 0, P(-1)/3,
507 0, 1, 0, 0 );
508}
511template <int S>
513 const RealP v0=v[0], v1=v[1], v2=v[2], v3=v[3];
514 return Vec<4,P>(v0, v0+v2/3, v1-v3/3, v1);
515}
518private:
520};
521
522
523
524} // namespace SimTK
525
526#endif // SimTK_SIMMATH_GEO_CUBIC_BEZIER_CURVE_H_
#define SimTK_ERRCHK1(cond, whereChecked, fmt, a1)
Definition: ExceptionMacros.h:326
Defines geometric primitive shapes and algorthms.
Defines primitive operations involving 3d rectangular boxes.
Defines primitive computations involving points.
Defines primitive operations on spheres.
Includes internal headers providing declarations for the basic SimTK Core classes,...
This is the header file that every Simmath compilation unit should include first.
This Array_ helper class is the base class for ArrayView_ which is the base class for Array_; here we...
Definition: Array.h:324
A 3d box aligned with an unspecified frame F and centered at a given point measured from that frame's...
Definition: Geo_Box.h:457
This is a primitive useful for computations involving a single cubic Bezier curve segment.
Definition: Geo_CubicBezierCurve.h:137
const Vec< 4, Vec3P > & getControlPoints() const
Return a reference to the Bezier control points B=[b0 b1 b2 b3] that are stored in this object.
Definition: Geo_CubicBezierCurve.h:162
CubicBezierCurve_(const Vec< 4, Vec3P, S > &controlPoints)
Construct a cubic Bezier curve using the given control points B=[b0 b1 b2 b3].
Definition: Geo_CubicBezierCurve.h:151
static Row< 4, P > calcD3Fb(RealP u)
Calculate third derivatives d3Fb=[B0uuu..B3uuu] of the Bernstein basis functions for a given value of...
Definition: Geo_CubicBezierCurve.h:351
Vec3P calcCurvatureVector(RealP u) const
The curvature vector c=dt/ds where t is the unit tangent vector (t=dP/ds) and s is arclength.
Definition: Geo_CubicBezierCurve.h:202
UnitVec3P calcUnitNormal(RealP u) const
In our definition, the unit normal vector n points in the "outward" direction, that is,...
Definition: Geo_CubicBezierCurve.h:234
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_CubicBezierCurve.h:176
Vec< 4, Vec3P > calcAlgebraicCoefficients() const
Calculate the algebraic coefficients A=[a3 a2 a1 a0] from the stored Bezier control points.
Definition: Geo_CubicBezierCurve.h:165
RealP calcDsdu(RealP u) const
Return ds/du, the change in arc length per change in curve parameter.
Definition: Geo_CubicBezierCurve.h:189
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_CubicBezierCurve.h:180
static Row< 4, P > calcDFb(RealP u)
Calculate first derivatives dFb=[B0u..B3u] of the Bernstein basis functions for a given value of the ...
Definition: Geo_CubicBezierCurve.h:336
static Mat< 4, 4, P > getMbInv()
Obtain the inverse inv(Mb) of the Bezier basis matrix explicitly.
Definition: Geo_CubicBezierCurve.h:457
Geo::AlignedBox_< P > calcAxisAlignedBoundingBox() const
Return an axis-aligned bounding box (AABB) that surrounds the entire curve segment in the u=[0....
Definition: Geo_CubicBezierCurve.h:310
static Mat< 4, 4, P > getMbInvMh()
Obtain the product Mb^-1*Mh explicitly; this is the matrix used for conversion from Hermite to Bezier...
Definition: Geo_CubicBezierCurve.h:503
static Vec3P evalPUsingB(const Vec< 4, Vec3P, S > &B, RealP u)
Given Bezier control points B and a value for the curve parameter u, return the point P(u) at that lo...
Definition: Geo_CubicBezierCurve.h:401
CubicBezierCurve_()
Construct an uninitialized curve; control points will be garbage.
Definition: Geo_CubicBezierCurve.h:146
Geo::OrientedBox_< P > calcOrientedBoundingBox() const
Return an oriented bounding box (OBB) that surrounds the entire curve segment in the u=[0....
Definition: Geo_CubicBezierCurve.h:318
static Vec3P evalPuUsingB(const Vec< 4, Vec3P, S > &B, RealP u)
Given Bezier control points B and a value for the curve parameter u, return the first derivative Pu(u...
Definition: Geo_CubicBezierCurve.h:410
void bisect(CubicBezierCurve_< P > &left, CubicBezierCurve_< P > &right) const
Split this curve into two at the point u=1/2 (halfway in parameter space, not necessarily in arclengt...
Definition: Geo_CubicBezierCurve.h:283
Vec3P evalP(RealP u) const
Evaluate a point on this curve given a value for parameter u in [0,1].
Definition: Geo_CubicBezierCurve.h:172
static Vec< 4, P > multiplyByMbInv(const Vec< 4, P, S > &b)
Form the product of the inverse inv(Mb) of the Bezier basis matrix Mb and a 4-vector,...
Definition: Geo_CubicBezierCurve.h:469
void split(RealP u, CubicBezierCurve_< P > &left, CubicBezierCurve_< P > &right) const
Split this curve into two at a point u=t such that 0 < t < 1, such that the first curve coincides wit...
Definition: Geo_CubicBezierCurve.h:259
static Vec< 4, Vec3P > calcBFromA(const Vec< 4, Vec3P, S > &A)
Given the algebraic coefficients A=~[a3 a2 a1 a0], return the Bezier control points B=~[b0 b1 b2 b3].
Definition: Geo_CubicBezierCurve.h:369
static Vec< 4, P > multiplyByMb(const Vec< 4, P, S > &b)
Form the product of the Bezier basis matrix Mb and a 4-vector, exploiting the structure of Mb.
Definition: Geo_CubicBezierCurve.h:447
Vec3P evalPuuu(RealP u) const
Evaluate the third derivative Puuu=d3P/du3 on this curve.
Definition: Geo_CubicBezierCurve.h:184
static Vec3P evalPuuuUsingB(const Vec< 4, Vec3P, S > &B, RealP u)
Given Bezier control points B and a value for the curve parameter u, return the third derivative Puuu...
Definition: Geo_CubicBezierCurve.h:428
static Vec< 4, P > multiplyByMhInvMb(const Vec< 4, P, S > &v)
Given a vector v, form the product inv(Mh)*Mb*v, exploiting the structure of the constant matrix inv(...
Definition: Geo_CubicBezierCurve.h:490
Geo::Sphere_< P > calcBoundingSphere() const
Return a sphere that surrounds the entire curve segment in the u=[0..1] range.
Definition: Geo_CubicBezierCurve.h:303
static Vec< 4, Vec3P > calcBFromH(const Vec< 4, Vec3P, S > &H)
Given the Hermite coefficients H=~[h0 h1 hu0 hu1], return the Bezier control points B=~[b0 b1 b2 b3].
Definition: Geo_CubicBezierCurve.h:389
static Vec< 4, P > multiplyByMbInvMh(const Vec< 4, P, S > &v)
Given a vector v, form the product inv(Mb)*Mh*v, exploiting the structure of the constant matrix inv(...
Definition: Geo_CubicBezierCurve.h:512
static Mat< 4, 4, P > getMb()
Obtain the Bezier basis matrix Mb explicitly.
Definition: Geo_CubicBezierCurve.h:435
static Vec3P evalPuuUsingB(const Vec< 4, Vec3P, S > &B, RealP u)
Given Bezier control points B and a value for the curve parameter u, return the second derivative Puu...
Definition: Geo_CubicBezierCurve.h:419
static Vec< 4, Vec3P > calcHFromB(const Vec< 4, Vec3P, S > &B)
Given the Bezier control points B=~[b0 b1 b2 b3], return the Hermite coefficients H=~[h0 h1 hu0 hu1].
Definition: Geo_CubicBezierCurve.h:379
Vec< 4, Vec3P > calcHermiteCoefficients() const
Calculate the Hermite (geometric) coefficients H=[h0 h1 hu0 hu1] from the stored Bezier control point...
Definition: Geo_CubicBezierCurve.h:168
static Vec< 4, Vec3P > calcAFromB(const Vec< 4, Vec3P, S > &B)
Given the Bezier control points B=~[b0 b1 b2 b3], return the algebraic coefficients A=~[a3 a2 a1 a0].
Definition: Geo_CubicBezierCurve.h:359
static Row< 4, P > calcFb(RealP u)
Calculate the Bernstein basis functions Fb=[B0..B3] for a given value of the parameter u.
Definition: Geo_CubicBezierCurve.h:328
RealP calcCurvatureSqr(RealP u)
Return k^2, the square of the scalar curvature k at the point P(u) on the curve.
Definition: Geo_CubicBezierCurve.h:210
static Row< 4, P > calcD2Fb(RealP u)
Calculate second derivatives d2Fb=[B0uu..B3uu] of the Bernstein basis functions for a given value of ...
Definition: Geo_CubicBezierCurve.h:343
UnitVec3P calcUnitTangent(RealP u) const
The unit tangent vector t=dP/ds where s is the arc length.
Definition: Geo_CubicBezierCurve.h:194
static Mat< 4, 4, P > getMhInvMb()
Obtain the product Mh^-1*Mb explicitly; this is the matrix used for conversion from Bezier to Hermite...
Definition: Geo_CubicBezierCurve.h:481
RealP calcCurveFrame(RealP u, TransformP &X_FP) const
Return the magnitude of the curvature (always positive), and a frame whose origin is a point along th...
Definition: Geo_CubicBezierCurve.h:248
CubicBezierCurve_(const Row< 4, Vec3P, S > &controlPoints)
Alternate signature accepts a Row of control points, although they are stored internally as a Vec.
Definition: Geo_CubicBezierCurve.h:157
RealP calcTorsion(RealP u)
Return tau, the torsion or "second curvature".
Definition: Geo_CubicBezierCurve.h:221
TODO: A 3d box oriented and positioned with respect to an unspecified frame F.
Definition: Geo_Box.h:528
static Geo::AlignedBox_< P > calcAxisAlignedBoundingBox(const Array_< Vec3P > &points, Array_< int > &support)
Calculate the smallest axis-aligned bounding box including all n given points.
static Geo::OrientedBox_< P > calcOrientedBoundingBox(const Array_< Vec3P > &points, Array_< int > &support, bool optimize=true)
Calculate a tight-fitting oriented bounding box (OBB) that includes all n given points.
static Sphere_< P > calcBoundingSphere(const Vec3P &p)
Create a tiny bounding sphere around a single point.
Definition: Geo_Point.h:333
A geometric primitive representing a sphere by its radius and center point, and a collection of spher...
Definition: Geo_Sphere.h:47
static Vec< 3, RealP > calcCurvatureVector(const Vec< 3, RealP, S > &Pu, const Vec< 3, RealP, S > &Puu)
Return the curvature vector c=dt/ds=d2P/ds2, given Pu=dP/du and Puu=d2P/du2.
Definition: Geo.h:171
static RealP calcCurvatureSqr(const Vec< 3, RealP, S > &Pu, const Vec< 3, RealP, S > &Puu)
Return k^2, the square of the scalar curvature k, given Pu=dP/du and Puu=d2P/du2.
Definition: Geo.h:283
static UnitVec< RealP, 1 > calcUnitTangent(const Vec< 3, RealP, S > &Pu)
Calculate the unit tangent vector t=dP/ds, given Pu=dP/du.
Definition: Geo.h:140
static UnitVec< RealP, 1 > calcUnitNormal(const Vec< 3, RealP, S > &Pu, const Vec< 3, RealP, S > &Puu)
In our definition, the unit normal vector n points in the "outward" direction, that is,...
Definition: Geo.h:192
static RealP calcCurveFrame(const Vec< 3, RealP, S > &P, const Vec< 3, RealP, S > &Pu, const Vec< 3, RealP, S > &Puu, Transform_< RealP > &X_FP)
Return the the curvature k (always positive), and a frame whose origin is a point along the curve,...
Definition: Geo.h:224
static RealP calcTorsion(const Vec< 3, RealP, S > &Pu, const Vec< 3, RealP, S > &Puu, const Vec< 3, RealP, S > &Puuu)
Return tau, the torsion or "second curvature" given Pu=dP/du, Puu=d2P/du2, Puuu=d3P/du3.
Definition: Geo.h:306
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: Mat.h:97
The Rotation class is a Mat33 that guarantees that the matrix can be interpreted as a legitimate 3x3 ...
Definition: Rotation.h:111
This is a fixed-length row vector designed for no-overhead inline computation.
Definition: Row.h:132
This class represents the rotate-and-shift transform which gives the location and orientation of a ne...
Definition: Transform.h:108
This class is a Vec3 plus an ironclad guarantee either that:
Definition: UnitVec.h:56
CNT< ScalarNormSq >::TSqrt norm() const
Definition: Vec.h:610
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37