1 #ifndef SimTK_SIMMATH_GEO_CUBIC_BEZIER_CURVE_H_ 2 #define SimTK_SIMMATH_GEO_CUBIC_BEZIER_CURVE_H_ 137 class Geo::CubicBezierCurve_ {
139 typedef Vec<3,RealP> Vec3P;
140 typedef UnitVec<P,1> UnitVec3P;
141 typedef Rotation_<P> RotationP;
142 typedef Transform_<P> TransformP;
152 : B(controlPoints) {}
158 : B(controlPoints.positionalTranspose()) {}
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);
266 const RealP u1 = 1-u;
267 const Vec3P p01 = u1*B[0] + u*B[1];
268 const Vec3P p12 = u1*B[1] + u*B[2];
269 const Vec3P p23 = u1*B[2] + u*B[3];
272 left.B[2] = u1*p01 + u*p12;
276 right.B[1] = u1*p12 + u*p23;
277 left.B[3] = right.B[0] = u1*left.B[2] + u*right.B[1];
285 const Vec3P p01 = (B[0] + B[1])/2;
286 const Vec3P p12 = (B[1] + B[2])/2;
287 const Vec3P p23 = (B[2] + B[3])/2;
290 left.B[2] = (p01 + p12)/2;
294 right.B[1] = (p12 + p23)/2;
295 left.B[3] = right.B[0] = (left.B[2] + right.B[1])/2;
329 const RealP u2 = u*u, u3 = u*u2;
330 const RealP u1 = 1-u, u12=u1*u1, u13=u1*u12;
331 return Row<4,P>(u13, 3*u*u12, 3*u2*u1, u3);
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);
344 const RealP u6 = 6*u, u18 = 18*u;
345 return Row<4,P>(6-u6, u18-12, 6-u18, u6);
362 return Vec<4,Vec3P>(b3-b0+3*(b1-b2), 3*(b0+b2)-6*b1, 3*(b1-b0), b0);
372 return Vec<4,Vec3P>(a0, a1/3 + a0, (a2+2*a1)/3 + a0, a3+a2+a1+a0);
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);
460 0, P(1)/3, P(2)/3, 1,
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);
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));
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);
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 class is a Vec3 plus an ironclad guarantee either that:
Definition: UnitVec.h:40
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'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