Simbody  3.6
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>
137 class Geo::CubicBezierCurve_ {
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_
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
Defines primitive operations involving 3d rectangular boxes.
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
Vec3P evalPuuu(RealP u) const
Evaluate the third derivative Puuu=d3P/du3 on this curve.
Definition: Geo_CubicBezierCurve.h:184
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
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
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
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
UnitVec3P calcUnitTangent(RealP u) const
The unit tangent vector t=dP/ds where s is the arc length.
Definition: Geo_CubicBezierCurve.h:194
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
#define SimTK_ERRCHK1(cond, whereChecked, fmt, a1)
Definition: ExceptionMacros.h:326
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< 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 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
TODO: A 3d box oriented and positioned with respect to an unspecified frame F.
Definition: Geo.h:63
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
A 3d box aligned with an unspecified frame F and centered at a given point measured from that frame&#39;s...
Definition: Geo.h:62
CNT< ScalarNormSq >::TSqrt norm() const
Definition: Vec.h:610
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
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
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
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
This is a fixed-length column vector designed for no-overhead inline computation. ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:618
Includes internal headers providing declarations for the basic SimTK Core classes, including Simmatrix.
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 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 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 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
Defines geometric primitive shapes and algorthms.
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
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
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
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 Mat< 4, 4, P > getMbInv()
Obtain the inverse inv(Mb) of the Bezier basis matrix explicitly.
Definition: Geo_CubicBezierCurve.h:457
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, exploiting the structure of inv(Mb).
Definition: Geo_CubicBezierCurve.h:469
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
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
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
CubicBezierCurve_()
Construct an uninitialized curve; control points will be garbage.
Definition: Geo_CubicBezierCurve.h:146
This is the header file that every Simmath compilation unit should include first. ...
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
This Array_ helper class is the base class for ArrayView_ which is the base class for Array_; here we...
Definition: Array.h:51
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
This is a fixed-length row vector designed for no-overhead inline computation.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:619
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 calcDsdu(RealP u) const
Return ds/du, the change in arc length per change in curve parameter.
Definition: Geo_CubicBezierCurve.h:189
Defines primitive computations involving points.
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 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
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
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 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
A geometric primitive representing a sphere by its radius and center point, and a collection of spher...
Definition: Geo.h:56
RealP calcTorsion(RealP u)
Return tau, the torsion or "second curvature".
Definition: Geo_CubicBezierCurve.h:221
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
This is a primitive useful for computations involving a single cubic Bezier curve segment...
Definition: Geo.h:67
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:620
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
Defines primitive operations on spheres.
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
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
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 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 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
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 Sphere_< P > calcBoundingSphere(const Vec3P &p)
Create a tiny bounding sphere around a single point.
Definition: Geo_Point.h:333