Simbody  3.8
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"
33 #include "simmath/internal/Geo.h"
37 
38 #include <cassert>
39 #include <cmath>
40 #include <algorithm>
41 
42 namespace SimTK {
43 
44 
45 //==============================================================================
46 // GEO CUBIC BEZIER CURVE
47 //==============================================================================
136 template <class P>
138 typedef P RealP;
139 typedef Vec<3,RealP> Vec3P;
140 typedef UnitVec<P,1> UnitVec3P;
141 typedef Rotation_<P> RotationP;
142 typedef Transform_<P> TransformP;
143 
144 public:
147 
150 template <int S>
151 explicit CubicBezierCurve_(const Vec<4,Vec3P,S>& controlPoints)
152 : B(controlPoints) {}
153 
156 template <int S>
157 explicit CubicBezierCurve_(const Row<4,Vec3P,S>& controlPoints)
158 : B(controlPoints.positionalTranspose()) {}
159 
162 const Vec<4,Vec3P>& getControlPoints() const {return B;}
172 Vec3P evalP(RealP u) const {return evalPUsingB(B,u);}
176 Vec3P evalPu(RealP u) const {return evalPuUsingB(B,u);}
180 Vec3P evalPuu(RealP u) const {return evalPuuUsingB(B,u);}
184 Vec3P evalPuuu(RealP u) const {return evalPuuuUsingB(B,u);}
185 
189 RealP calcDsdu(RealP u) const {return evalPu(u).norm();}
190 
194 UnitVec3P calcUnitTangent(RealP u) const {
195  const Vec3P Pu=evalPu(u); // 15 flops
196  return Geo::calcUnitTangent(Pu); // ~40 flops
197 }
198 
202 Vec3P calcCurvatureVector(RealP u) const {
203  const Vec3P Pu=evalPu(u), Puu=evalPuu(u); // 25 flops
204  return Geo::calcCurvatureVector(Pu,Puu); // ~30 flops
205 }
206 
210 RealP calcCurvatureSqr(RealP u) {
211  const Vec3P Pu=evalPu(u), Puu=evalPuu(u); // 25 flops
212  return Geo::calcCurvatureSqr(Pu,Puu); // ~30 flops
213 }
214 
221 RealP 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 
234 UnitVec3P 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 
248 RealP 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 
259 void 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 
328 static 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 
336 static 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 
343 static 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 
351 static Row<4,P> calcD3Fb(RealP u) {
352  return Row<4,P>(-6, 18, -18, 6);
353 }
354 
358 template <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 
368 template <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 
378 template <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 
388 template <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 
400 template <int S>
401 static Vec3P evalPUsingB(const Vec<4,Vec3P,S>& B, RealP u) {
402  return calcFb(u)*B; // 9 + 3*7 = 30 flops
403 }
409 template <int S>
410 static Vec3P evalPuUsingB(const Vec<4,Vec3P,S>& B, RealP u) {
411  return calcDFb(u)*B; // 10 + 3*7 = 31 flops
412 }
418 template <int S>
419 static Vec3P evalPuuUsingB(const Vec<4,Vec3P,S>& B, RealP u) {
420  return calcD2Fb(u)*B; // 5 + 3*7 = 26 flops
421 }
427 template <int S>
428 static Vec3P evalPuuuUsingB(const Vec<4,Vec3P,S>& B, RealP u) {
429  return calcD3Fb(u)*B; // 0 + 3*7 = 21 flops
430 }
435 static Mat<4,4,P> getMb() {
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 
446 template <int S>
447 static Vec<4,P> multiplyByMb(const Vec<4,P,S>& b) {
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 
468 template <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 }
489 template <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 }
511 template <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 }
518 private:
519 Vec<4,Vec3P> B;
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
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
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
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
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
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 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
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 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
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
CubicBezierCurve_()
Construct an uninitialized curve; control points will be garbage.
Definition: Geo_CubicBezierCurve.h:146
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 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
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 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
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, 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 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
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
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 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
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
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
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
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 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 Mat< 4, 4, P > getMb()
Obtain the Bezier basis matrix Mb explicitly.
Definition: Geo_CubicBezierCurve.h:435
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
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
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
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
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 > getMbInv()
Obtain the inverse inv(Mb) of the Bezier basis matrix explicitly.
Definition: Geo_CubicBezierCurve.h:457
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
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
TODO: A 3d box oriented and positioned with respect to an unspecified frame F.
Definition: Geo_Box.h:528
static Sphere_< P > calcBoundingSphere(const Vec3P &p)
Create a tiny bounding sphere around a single point.
Definition: Geo_Point.h:333
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 Geo::AlignedBox_< P > calcAxisAlignedBoundingBox(const Array_< Vec3P > &points, Array_< int > &support)
Calculate the smallest axis-aligned bounding box including all n given points.
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 > 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
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
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