Simbody  3.5
Geo.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMMATH_GEO_H_
2 #define SimTK_SIMMATH_GEO_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 
30 #include "SimTKcommon.h"
32 
33 #include <cassert>
34 #include <cmath>
35 #include <algorithm>
36 
37 namespace SimTK {
38 
39 //==============================================================================
40 // GEO
41 //==============================================================================
54 public:
55 template <class P> class Point_;
56 template <class P> class Sphere_;
57 template <class P> class LineSeg_;
58 template <class P> class Line_;
59 template <class P> class Plane_;
60 template <class P> class Circle_;
61 template <class P> class Box_;
62 template <class P> class AlignedBox_;
63 template <class P> class OrientedBox_;
64 template <class P> class Triangle_;
65 template <class P> class CubicHermiteCurve_;
66 template <class P> class BicubicHermitePatch_;
67 template <class P> class CubicBezierCurve_;
68 template <class P> class BicubicBezierPatch_;
69 
70 typedef Point_<Real> Point;
73 typedef Line_<Real> Line;
76 typedef Box_<Real> Box;
84 
115 template <class RealP, int S> static bool
117 { return Pu.normSqr() < getDefaultTolSqr<RealP>(); }
118 
132 template <class RealP, int S> static bool
134 { return (Pu % Puu).normSqr() < getDefaultTolSqr<RealP>(); }
135 
139 template <class RealP, int S> static UnitVec<RealP,1>
141  const RealP dsdu = Pu.norm(); // ~25 flops
142  SimTK_ERRCHK(dsdu >= getDefaultTol<RealP>(),
143  "Geo::calcUnitTangent()", "Unit tangent undefined at a cusp.");
144 
145  return UnitVec<RealP,1>(Pu/dsdu, true); // ~13 flops
146 }
147 
170 template <class RealP, int S> static Vec<3,RealP>
172  const RealP Pu2 = Pu.normSqr(); // (ds/du)^2, 5 flops
173  SimTK_ERRCHK(Pu2 >= getDefaultTolSqr<RealP>(),
174  "Geo::calcCurvatureVector()", "Curvature undefined at a cusp.");
175  const RealP PuPuu = dot(Pu,Puu);
176  const RealP uPrimeSqr = 1/Pu2; // ~10 flops
177  const RealP u2Prime = -PuPuu * square(uPrimeSqr); // 8 flops
178  return uPrimeSqr*Puu + u2Prime*Pu; // 9 flops
179 }
180 
191 template <class RealP, int S> static UnitVec<RealP,1>
193  typedef UnitVec<RealP,1> UnitVec3P;
194 
195  const RealP Pu2 = Pu.normSqr(); // (ds/du)^2, 5 flops
196  SimTK_ERRCHK(Pu2 >= getDefaultTolSqr<RealP>(),
197  "Geo::calcUnitNormal()", "The normal is undefined at a cusp.");
198 
199  // Now check if we're at an inflection point, meaning |Pu X Puu|==0.
200  // Use this handy identity from Struik eq. 3-9 to avoid calculating
201  // the cross product: (Pu X Puu)^2 = Pu^2Puu^2 - (~Pu*Puu)^2
202  const RealP Puu2 = Puu.normSqr(); // 5 flops
203  const RealP PuPuu = dot(Pu,Puu); // 5 flops
204  const RealP PuXPuu2 = Pu2*Puu2 - square(PuPuu); // 3 flops
205  if (PuXPuu2 < getDefaultTolSqr<RealP>()) // 1 flop
206  return UnitVec3P(Pu).perp();
207  // Calculate the curvature vector, negate, and normalize.
208  const RealP uPrimeSqr = 1/Pu2; // ~10 flops
209  const RealP u2Prime = -PuPuu * square(uPrimeSqr); // 3 flops
210  const Vec<3,RealP> c = uPrimeSqr*Puu + u2Prime*Pu; // 9 flops
211  return UnitVec3P(-c); // ~40 flops
212 }
213 
223 template <class RealP, int S> static RealP
225  const Vec<3,RealP,S>& Pu,
226  const Vec<3,RealP,S>& Puu,
227  Transform_<RealP>& X_FP) {
228  typedef UnitVec<RealP,1> UnitVec3P;
229 
230  const RealP Pu2 = Pu.normSqr(); // (ds/du)^2, 5 flops
231  SimTK_ERRCHK(Pu2 >= getDefaultTolSqr<RealP>(),
232  "Geo::calcCurveFrame()", "Curve frame is undefined at a cusp.");
233 
234  // Set the point P(u) as the frame origin.
235  X_FP.updP() = P;
236 
237  // Calculate the unit tangent t, our y axis.
238  const RealP uPrimeSqr = 1/Pu2; // ~10 flops
239  const RealP uPrime = std::sqrt(uPrimeSqr); // ~20 flops
240  const UnitVec3P t(uPrime*Pu, true); // 3 flops
241 
242  // Next calculate unit normal n, our x axis. See calcUnitNormal() above
243  // for theory.
244  const RealP Puu2 = Puu.normSqr(); // 5 flops
245  const RealP PuPuu = dot(Pu,Puu); // 5 flops
246  const RealP PuXPuu2 = Pu2*Puu2 - square(PuPuu); // 3 flops
247  UnitVec3P n; // unit normal
248  RealP k; // curvature magnitude
249  if (PuXPuu2 < getDefaultTolSqr<RealP>()) { // 1 flop
250  k = 0;
251  n = t.perp(); // arbitrary
252  } else {
253  // Calculate the curvature vector, negate, and normalize.
254  const RealP u2Prime = -PuPuu * square(uPrimeSqr); // 8 flops
255  const Vec<3,RealP> c = uPrimeSqr*Puu + u2Prime*Pu; // 9 flops
256  k = c.norm(); // curvature >= 0, ~25 flops
257  n = UnitVec3P((-1/k)*c, true); // ~13 flops
258  }
259 
260  // Finally calculate the binormal, our z axis. No need to normalize
261  // here because n and t are perpendicular unit vectors.
262  const UnitVec3P b(n % t, true); // 9 flops
263 
264  // Construct the coordinate frame without normalizing.
265  X_FP.updR().setRotationFromUnitVecsTrustMe(n,t,b);
266 
267  return k;
268 }
269 
282 template <class RealP, int S> static RealP
284  const RealP Pu2 = Pu.normSqr(); // (ds/du)^2, 5 flops
285  SimTK_ERRCHK(Pu2 >= getDefaultTolSqr<RealP>(),
286  "Geo::calcCurvatureSqr()", "Curvature is undefined at a cusp.");
287  const RealP num = cross(Pu,Puu).normSqr(); // 14 flops
288  const RealP den = cube(Pu2); // 2 flops
289  return num/den; // ~10 flops
290 }
291 
305 template <class RealP, int S> static RealP
307  const Vec<3,RealP,S>& Puuu) {
308  const Vec<3,RealP> PuXPuu = cross(Pu,Puu); // 9 flops
309  const RealP PuXPuu2 = PuXPuu.normSqr(); // 5 flops
310  SimTK_ERRCHK(PuXPuu2 >= getDefaultTolSqr<RealP>(), "Geo::calcTorsion()",
311  "Torsion is undefined at a cusp or inflection point.");
312  const RealP num = dot(PuXPuu, Puuu); // 5 flops
313  return num/PuXPuu2; // ~10 flops
314 }
338 template <class RealP> static void
339 findClosestPointsOfTwoLines
340  (const Vec<3,RealP>& p0, const UnitVec<RealP,1>& d0,
341  const Vec<3,RealP>& p1, const UnitVec<RealP,1>& d1,
342  Vec<3,RealP>& x0, Vec<3,RealP>& x1, bool& linesAreParallel)
343 {
344  const Vec<3,RealP> w = p1-p0; // vector from p0 to p1; 3 flops
345  const RealP s2Theta = (d0 % d1).normSqr(); // sin^2(angle); 14 flops
346  const RealP d = dot(w,d0); // 5 flops
347  const RealP e = -dot(w,d1); // 6 flops
348 
349  RealP t0, t1; // line parameters of closest points
350  if (s2Theta < square(NTraits<RealP>::getSignificant())) { // 3 flops
351  // Lines are parallel. Return a point on each line midway between
352  // the origin points.
353  linesAreParallel = true; // parallel
354  t0 = d/2; t1 = e/2; // 2 flops
355  } else {
356  linesAreParallel = false;
357  const RealP cTheta = dot(d0,d1); // cos(angle between lines); 5 flops
358  const RealP oos2Theta = 1/s2Theta; // about 10 flops
359  t0 = (e*cTheta + d) * oos2Theta; // 3 flops
360  t1 = (d*cTheta + e) * oos2Theta; // 3 flops
361  }
362 
363  x0 = p0 + t0*d0; // 6 flops
364  x1 = p1 + t1*d1; // 6 flops
365 }
366 
376 template <class RealP> static RealP getDefaultTol()
377 { return NTraits<RealP>::getSignificant(); }
380 template <class RealP> static RealP getDefaultTolSqr()
381 { return square(getDefaultTol<RealP>()); }
382 
385 template <class RealP> static RealP getEps()
386 { return NTraits<RealP>::getEps(); }
388 template <class RealP> static RealP getNaN()
389 { return NTraits<RealP>::getNaN(); }
391 template <class RealP> static RealP getInfinity()
392 { return NTraits<RealP>::getInfinity(); }
393 
398 template <class RealP> static RealP stretchBy(RealP length, RealP tol) {
399  SimTK_ERRCHK2(tol >= getEps<RealP>(), "Geo::stretchBy()",
400  "The supplied tolerance %g is too small; must be at least %g"
401  " for significance at this precision.",
402  (double)tol, (double)getEps<RealP>());
403 
404  return length + std::max(length*tol, tol);
405 }
406 
409 template <class RealP> static RealP stretch(RealP length)
410 { return stretchBy(length, getDefaultTol<RealP>()); }
411 
414 };
415 
416 
417 } // namespace SimTK
418 
419 #endif // SimTK_SIMMATH_GEO_H_
A geometric primitive representing a triangle by its vertices as points in some unspecified frame...
Definition: Geo.h:64
Vec< 3, typename CNT< E1 >::template Result< E2 >::Mul > cross(const Vec< 3, E1, S1 > &a, const Vec< 3, E2, S2 > &b)
Definition: SmallMatrixMixed.h:413
Triangle_< Real > Triangle
Definition: Geo.h:79
This class is a Vec3 plus an ironclad guarantee either that:
Definition: UnitVec.h:40
AlignedBox_< Real > AlignedBox
Definition: Geo.h:77
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
A primitive useful for computations involving a single cubic Hermite curve segment in algebraic or ge...
Definition: Geo.h:65
Definition: Geo.h:58
static bool isCusp(const Vec< 3, RealP, S > &Pu)
Given the parametric derivative Pu(u)=dP/du, determine whether the point P(u) is at a cusp...
Definition: Geo.h:116
LineSeg_< Real > LineSeg
Definition: Geo.h:72
A 3d rectangular box aligned with an unspecified frame F and centered at that frame&#39;s origin...
Definition: Geo.h:61
Point_< Real > Point
Definition: Geo.h:68
CubicHermiteCurve_< Real > CubicHermiteCurve
Definition: Geo.h:80
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
#define SimTK_ERRCHK2(cond, whereChecked, fmt, a1, a2)
Definition: ExceptionMacros.h:328
CubicBezierCurve_< Real > CubicBezierCurve
Definition: Geo.h:82
static RealP getEps()
Return machine precision for floating point calculations at precision RealP.
Definition: Geo.h:385
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
unsigned char square(unsigned char u)
Definition: Scalar.h:351
This is a fixed-length column vector designed for no-overhead inline computation. ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:605
Includes internal headers providing declarations for the basic SimTK Core classes, including Simmatrix.
BicubicHermitePatch_< Real > BicubicHermitePatch
Definition: Geo.h:81
static bool isInflectionPoint(const Vec< 3, RealP, S > &Pu, const Vec< 3, RealP, S > &Puu)
Given the parametric derivatives Pu(u)=dP/du, and Puu(u)=d2P/du2 determine whether point P(u) is at a...
Definition: Geo.h:133
The Geo class collects geometric primitives intended to deal with raw, fixed-size geometric shapes oc...
Definition: Geo.h:53
BicubicBezierPatch_< Real > BicubicBezierPatch
Definition: Geo.h:83
This class represents the rotate-and-shift transform which gives the location and orientation of a ne...
Definition: Transform.h:43
static RealP getDefaultTolSqr()
Returns the square of the default tolerance.
Definition: Geo.h:380
ELEM max(const VectorBase< ELEM > &v)
Definition: VectorMath.h:251
static RealP getNaN()
Return a NaN (not a number) at precision RealP.
Definition: Geo.h:388
A 3d line segment primitive represented by its end points in an unspecified frame, and a collection of line segment-related utility methods.
Definition: Geo.h:57
#define SimTK_ERRCHK(cond, whereChecked, msg)
Definition: ExceptionMacros.h:324
Plane_< Real > Plane
Definition: Geo.h:74
OrientedBox_< Real > OrientedBox
Definition: Geo.h:78
Rotation_< P > & updR()
Return a writable (lvalue) reference to the contained rotation R_BF.
Definition: Transform.h:218
Line_< Real > Line
Definition: Geo.h:73
static RealP getInfinity()
Return Infinity at precision RealP. You can negate this for -Infinity.
Definition: Geo.h:391
static RealP stretch(RealP length)
Stretch a dimension using the default tolerance for this precision as the tolerance in stretchBy()...
Definition: Geo.h:409
Vec< 3, P > & updP()
Return a writable (lvalue) reference to our translation vector p_BF.
Definition: Transform.h:243
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
Box_< Real > Box
Definition: Geo.h:76
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
CNT< typename CNT< E1 >::THerm >::template Result< E2 >::Mul dot(const Vec< M, E1, S1 > &r, const Vec< M, E2, S2 > &v)
Definition: SmallMatrixMixed.h:86
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 RealP getDefaultTol()
Return the default tolerance to use for degeneracy tests and other tests for "too small" or "near eno...
Definition: Geo.h:376
Sphere_< Real > Sphere
Definition: Geo.h:71
ScalarNormSq normSqr() const
Definition: Vec.h:606
A primitive useful for computations involving a single bicubic Hermite patch.
Definition: Geo.h:66
static RealP stretchBy(RealP length, RealP tol)
Stretch a dimension by a given tolerance amount.
Definition: Geo.h:398
A geometric primitive representing a sphere by its radius and center point, and a collection of spher...
Definition: Geo.h:56
CNT< ScalarNormSq >::TSqrt norm() const
Definition: Vec.h:608
Circle_< Real > Circle
Definition: Geo.h:75
This is a primitive useful for computations involving a single cubic Bezier curve segment...
Definition: Geo.h:67
A primitive useful for computations involving a single bicubic Bezier patch.
Definition: Geo.h:68
A 3d point primitive represented by a Vec3 from the origin of an unspecified frame, and a collection of point-related utility methods.
Definition: Geo.h:55
Definition: Geo.h:60
#define SimTK_SIMMATH_EXPORT
Definition: SimTKmath/include/simmath/internal/common.h:64
Definition: negator.h:64
Definition: Geo.h:59
unsigned char cube(unsigned char u)
Definition: Scalar.h:424