Simbody  3.4
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
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"
32 #include "simmath/internal/common.h"
33 #include "simmath/internal/Geo.h"
34 #include "simmath/internal/Geo_Point.h"
35 #include "simmath/internal/Geo_Sphere.h"
36 #include "simmath/internal/Geo_Box.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:
146 CubicBezierCurve_() {}
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;}
165 Vec<4,Vec3P> calcAlgebraicCoefficients() const {return calcAFromB(B);}
168 Vec<4,Vec3P> calcHermiteCoefficients() const {return calcHFromB(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 
283 void bisect(CubicBezierCurve_<P>& left,
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 
303 Geo::Sphere_<P> calcBoundingSphere() const
304 { return Geo::Point_<P>::calcBoundingSphere(B[0],B[1],B[2],B[3]); }
305 
310 Geo::AlignedBox_<P> calcAxisAlignedBoundingBox() const
311 { const ArrayViewConst_<Vec3P> points(&B[0], &B[0]+4); // no copy or heap use
312  return Geo::Point_<P>::calcAxisAlignedBoundingBox(points); }
313 
318 Geo::OrientedBox_<P> calcOrientedBoundingBox() const
319 { const ArrayViewConst_<Vec3P> points(&B[0], &B[0]+4); // no copy or heap use
320  return Geo::Point_<P>::calcOrientedBoundingBox(points); }
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>
359 static Vec<4,Vec3P> calcAFromB(const Vec<4,Vec3P,S>& B) {
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>
369 static Vec<4,Vec3P> calcBFromA(const Vec<4,Vec3P,S>& A) {
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>
379 static Vec<4,Vec3P> calcHFromB(const Vec<4,Vec3P,S>& B) {
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>
389 static Vec<4,Vec3P> calcBFromH(const Vec<4,Vec3P,S>& H) {
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 
457 static Mat<4,4,P> getMbInv() {
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>
469 static Vec<4,P> multiplyByMbInv(const Vec<4,P,S>& b) {
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 
481 static Mat<4,4,P> getMhInvMb() {
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>
490 static Vec<4,P> multiplyByMhInvMb(const Vec<4,P,S>& v) {
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 
503 static Mat<4,4,P> getMbInvMh() {
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>
512 static Vec<4,P> multiplyByMbInvMh(const Vec<4,P,S>& v) {
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_