1 #ifndef SimTK_SIMMATH_GEO_H_ 2 #define SimTK_SIMMATH_GEO_H_ 61 template <
class P>
class Box_;
115 template <
class RealP,
int S>
static bool 117 {
return Pu.
normSqr() < getDefaultTolSqr<RealP>(); }
132 template <
class RealP,
int S>
static bool 134 {
return (Pu % Puu).normSqr() < getDefaultTolSqr<RealP>(); }
141 const RealP dsdu = Pu.
norm();
143 "Geo::calcUnitTangent()",
"Unit tangent undefined at a cusp.");
172 const RealP Pu2 = Pu.
normSqr();
174 "Geo::calcCurvatureVector()",
"Curvature undefined at a cusp.");
175 const RealP PuPuu =
dot(Pu,Puu);
176 const RealP uPrimeSqr = 1/Pu2;
177 const RealP u2Prime = -PuPuu *
square(uPrimeSqr);
178 return uPrimeSqr*Puu + u2Prime*Pu;
195 const RealP Pu2 = Pu.
normSqr();
197 "Geo::calcUnitNormal()",
"The normal is undefined at a cusp.");
202 const RealP Puu2 = Puu.
normSqr();
203 const RealP PuPuu =
dot(Pu,Puu);
204 const RealP PuXPuu2 = Pu2*Puu2 -
square(PuPuu);
205 if (PuXPuu2 < getDefaultTolSqr<RealP>())
206 return UnitVec3P(Pu).perp();
208 const RealP uPrimeSqr = 1/Pu2;
209 const RealP u2Prime = -PuPuu *
square(uPrimeSqr);
211 return UnitVec3P(-c);
223 template <
class RealP,
int S>
static RealP
230 const RealP Pu2 = Pu.
normSqr();
232 "Geo::calcCurveFrame()",
"Curve frame is undefined at a cusp.");
238 const RealP uPrimeSqr = 1/Pu2;
239 const RealP uPrime = std::sqrt(uPrimeSqr);
240 const UnitVec3P t(uPrime*Pu,
true);
244 const RealP Puu2 = Puu.
normSqr();
245 const RealP PuPuu =
dot(Pu,Puu);
246 const RealP PuXPuu2 = Pu2*Puu2 -
square(PuPuu);
249 if (PuXPuu2 < getDefaultTolSqr<RealP>()) {
254 const RealP u2Prime = -PuPuu *
square(uPrimeSqr);
257 n = UnitVec3P((-1/k)*c,
true);
262 const UnitVec3P b(n % t,
true);
265 X_FP.
updR().setRotationFromUnitVecsTrustMe(n,t,b);
282 template <
class RealP,
int S>
static RealP
284 const RealP Pu2 = Pu.
normSqr();
286 "Geo::calcCurvatureSqr()",
"Curvature is undefined at a cusp.");
287 const RealP num =
cross(Pu,Puu).normSqr();
288 const RealP den =
cube(Pu2);
305 template <
class RealP,
int S>
static RealP
309 const RealP PuXPuu2 = PuXPuu.
normSqr();
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);
338 template <
class RealP>
static void 339 findClosestPointsOfTwoLines
345 const RealP s2Theta = (d0 % d1).normSqr();
346 const RealP d =
dot(w,d0);
347 const RealP e = -
dot(w,d1);
353 linesAreParallel =
true;
356 linesAreParallel =
false;
357 const RealP cTheta =
dot(d0,d1);
358 const RealP oos2Theta = 1/s2Theta;
359 t0 = (e*cTheta + d) * oos2Theta;
360 t1 = (d*cTheta + e) * oos2Theta;
381 {
return square(getDefaultTol<RealP>()); }
385 template <
class RealP>
static RealP
getEps()
388 template <
class RealP>
static RealP
getNaN()
398 template <
class RealP>
static RealP
stretchBy(RealP length, RealP tol) {
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>());
404 return length +
std::max(length*tol, tol);
409 template <
class RealP>
static RealP
stretch(RealP length)
410 {
return stretchBy(length, getDefaultTol<RealP>()); }
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
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'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
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's...
Definition: Geo.h:62
CNT< ScalarNormSq >::TSqrt norm() const
Definition: Vec.h:610
unsigned char square(unsigned char u)
Definition: Scalar.h:349
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.
ScalarNormSq normSqr() const
Definition: Vec.h:608
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
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
Plane_< Real > Plane
Definition: Geo.h:74
OrientedBox_< Real > OrientedBox
Definition: Geo.h:78
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
#define SimTK_ERRCHK(cond, whereChecked, msg)
Definition: ExceptionMacros.h:324
#define SimTK_ERRCHK2(cond, whereChecked, fmt, a1, a2)
Definition: ExceptionMacros.h:328
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
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
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
#define SimTK_SIMMATH_EXPORT
Definition: SimTKmath/include/simmath/internal/common.h:64
unsigned char cube(unsigned char u)
Definition: Scalar.h:420