Simbody 3.7
Loading...
Searching...
No Matches
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
37namespace SimTK {
38
39//==============================================================================
40// GEO
41//==============================================================================
54public:
55template <class P> class Point_;
56template <class P> class Sphere_;
57template <class P> class LineSeg_;
58template <class P> class Line_;
59template <class P> class Plane_;
60template <class P> class Circle_;
61template <class P> class Box_;
62template <class P> class AlignedBox_;
63template <class P> class OrientedBox_;
64template <class P> class Triangle_;
65template <class P> class CubicHermiteCurve_;
66template <class P> class BicubicHermitePatch_;
67template <class P> class CubicBezierCurve_;
68template <class P> class BicubicBezierPatch_;
69
84
115template <class RealP, int S> static bool
117{ return Pu.normSqr() < getDefaultTolSqr<RealP>(); }
118
132template <class RealP, int S> static bool
134{ return (Pu % Puu).normSqr() < getDefaultTolSqr<RealP>(); }
135
139template <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
170template <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
191template <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
223template <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
282template <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
305template <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}
338template <class RealP> static void
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
376template <class RealP> static RealP getDefaultTol()
380template <class RealP> static RealP getDefaultTolSqr()
381{ return square(getDefaultTol<RealP>()); }
382
385template <class RealP> static RealP getEps()
386{ return NTraits<RealP>::getEps(); }
388template <class RealP> static RealP getNaN()
389{ return NTraits<RealP>::getNaN(); }
391template <class RealP> static RealP getInfinity()
392{ return NTraits<RealP>::getInfinity(); }
393
398template <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
409template <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_
#define SimTK_ERRCHK2(cond, whereChecked, fmt, a1, a2)
Definition ExceptionMacros.h:328
#define SimTK_ERRCHK(cond, whereChecked, msg)
Definition ExceptionMacros.h:324
Includes internal headers providing declarations for the basic SimTK Core classes,...
This is the header file that every Simmath compilation unit should include first.
#define SimTK_SIMMATH_EXPORT
Definition SimTKmath/include/simmath/internal/common.h:64
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
A primitive useful for computations involving a single bicubic Bezier patch.
Definition Geo_BicubicBezierPatch.h:101
A primitive useful for computations involving a single bicubic Hermite patch.
Definition Geo_BicubicHermitePatch.h:97
A 3d rectangular box aligned with an unspecified frame F and centered at that frame's origin.
Definition Geo_Box.h:48
Definition Geo.h:60
This is a primitive useful for computations involving a single cubic Bezier curve segment.
Definition Geo_CubicBezierCurve.h:137
A primitive useful for computations involving a single cubic Hermite curve segment in algebraic or ge...
Definition Geo_CubicHermiteCurve.h:131
A 3d line segment primitive represented by its end points in an unspecified frame,...
Definition Geo_LineSeg.h:50
Definition Geo.h:58
TODO: A 3d box oriented and positioned with respect to an unspecified frame F.
Definition Geo_Box.h:528
Definition Geo.h:59
A 3d point primitive represented by a Vec3 from the origin of an unspecified frame,...
Definition Geo_Point.h:46
A geometric primitive representing a sphere by its radius and center point, and a collection of spher...
Definition Geo_Sphere.h:47
A geometric primitive representing a triangle by its vertices as points in some unspecified frame,...
Definition Geo_Triangle.h:49
The Geo class collects geometric primitives intended to deal with raw, fixed-size geometric shapes oc...
Definition Geo.h:53
static RealP stretch(RealP length)
Stretch a dimension using the default tolerance for this precision as the tolerance in stretchBy().
Definition Geo.h:409
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
Box_< Real > Box
Definition Geo.h:76
LineSeg_< Real > LineSeg
Definition Geo.h:72
AlignedBox_< Real > AlignedBox
Definition Geo.h:77
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
CubicHermiteCurve_< Real > CubicHermiteCurve
Definition Geo.h:80
Plane_< Real > Plane
Definition Geo.h:74
OrientedBox_< Real > OrientedBox
Definition Geo.h:78
static RealP getDefaultTolSqr()
Returns the square of the default tolerance.
Definition Geo.h:380
CubicBezierCurve_< Real > CubicBezierCurve
Definition Geo.h:82
Line_< Real > Line
Definition Geo.h:73
static RealP getNaN()
Return a NaN (not a number) at precision RealP.
Definition Geo.h:388
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 > calcUnitTangent(const Vec< 3, RealP, S > &Pu)
Calculate the unit tangent vector t=dP/ds, given Pu=dP/du.
Definition Geo.h:140
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
BicubicBezierPatch_< Real > BicubicBezierPatch
Definition Geo.h:83
static RealP getEps()
Return machine precision for floating point calculations at precision RealP.
Definition Geo.h:385
static RealP getInfinity()
Return Infinity at precision RealP. You can negate this for -Infinity.
Definition Geo.h:391
Point_< Real > Point
Definition Geo.h:70
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 stretchBy(RealP length, RealP tol)
Stretch a dimension by a given tolerance amount.
Definition Geo.h:398
static void findClosestPointsOfTwoLines(const Vec< 3, RealP > &p0, const UnitVec< RealP, 1 > &d0, const Vec< 3, RealP > &p1, const UnitVec< RealP, 1 > &d1, Vec< 3, RealP > &x0, Vec< 3, RealP > &x1, bool &linesAreParallel)
Find the points of closest approach on two lines L0 and L1, each represented by an origin point and a...
Definition Geo.h:340
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
Triangle_< Real > Triangle
Definition Geo.h:79
BicubicHermitePatch_< Real > BicubicHermitePatch
Definition Geo.h:81
Circle_< Real > Circle
Definition Geo.h:75
Definition NTraits.h:436
This class represents the rotate-and-shift transform which gives the location and orientation of a ne...
Definition Transform.h:108
Vec< 3, P > & updP()
Return a writable (lvalue) reference to our translation vector p_BF.
Definition Transform.h:243
Rotation_< P > & updR()
Return a writable (lvalue) reference to the contained rotation R_BF.
Definition Transform.h:218
This class is a Vec3 plus an ironclad guarantee either that:
Definition UnitVec.h:56
This is a fixed-length column vector designed for no-overhead inline computation.
Definition Vec.h:184
CNT< ScalarNormSq >::TSqrt norm() const
Definition Vec.h:610
ScalarNormSq normSqr() const
Definition Vec.h:608
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition Assembler.h:37
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
CNT< typenameCNT< E1 >::THerm >::template Result< E2 >::Mul dot(const Vec< M, E1, S1 > &r, const Vec< M, E2, S2 > &v)
Definition SmallMatrixMixed.h:86
unsigned char square(unsigned char u)
Definition Scalar.h:349
unsigned char cube(unsigned char u)
Definition Scalar.h:420