Simbody 3.7
Geo_Point.h
Go to the documentation of this file.
1#ifndef SimTK_SIMMATH_GEO_POINT_H_
2#define SimTK_SIMMATH_GEO_POINT_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"
33
34#include <cassert>
35#include <cmath>
36#include <algorithm>
37
38namespace SimTK {
39
40//==============================================================================
41// GEO POINT
42//==============================================================================
45template <class P>
47typedef P RealP;
48typedef Vec<3,P> Vec3P;
49typedef Mat<3,3,P> Mat33P;
54
55public:
60Point_(const Vec3P& location) : p(location) {}
61
63Point_& setLocation(const Vec3P& location) {p=location; return *this;}
64
66const Vec3P& getLocation() const {return p;}
67
70RealP calcDistance(const Vec3P& p2) const
71{ return calcDistance(p, p2); }
72
75RealP findDistanceSqr(const Vec3P& p2) const
76{ return findDistanceSqr(p, p2); }
77
87static RealP calcDistance(const Vec3P& p1, const Vec3P& p2)
88{ return std::sqrt(findDistanceSqr(p1,p2)); }
89
92static RealP findDistanceSqr(const Vec3P& p1, const Vec3P& p2)
93{ return (p2-p1).normSqr(); }
94
96static Vec3P findMidpoint(const Vec3P& p1, const Vec3P& p2)
97{ return (p1+p2)/2; }
98
106static bool pointsAreNumericallyCoincident(const Vec3P& p1, const Vec3P& p2)
107{
108 return pointsAreNumericallyCoincident(p1,p2,Geo::getDefaultTol<RealP>());
109}
112 (const Vec3P& p1, const Vec3P& p2, RealP tol)
113{
114 const RealP maxcoord = std::max(max(p1.abs()),max(p2.abs())); // ~7 flops
115 const RealP scale = std::max(tol, maxcoord*tol); // 2 flops
116 return findDistanceSqr(p1,p2) < square(scale); // 10 flops
117}
118
119
123SimTK_SIMMATH_EXPORT static void
124findSupportPoint(const Array_<Vec3P>& points, const UnitVec3P& direction,
125 int& most, RealP& mostCoord);
126
129SimTK_SIMMATH_EXPORT static void
131 const UnitVec3P& direction,
132 int& most, RealP& mostCoord);
133
138SimTK_SIMMATH_EXPORT static void
139findExtremePoints(const Array_<Vec3P>& points, const UnitVec3P& direction,
140 int& least, int& most,
141 RealP& leastCoord, RealP& mostCoord);
142
145SimTK_SIMMATH_EXPORT static void
147 const UnitVec3P& direction,
148 int& least, int& most,
149 RealP& leastCoord, RealP& mostCoord);
150
155
160
163SimTK_SIMMATH_EXPORT static void
165 Vec3P& centroid, SymMat33P& covariance);
166
169SimTK_SIMMATH_EXPORT static void
171 Vec3P& centroid, SymMat33P& covariance);
172
178SimTK_SIMMATH_EXPORT static void
180 TransformP& X_FP);
181
184SimTK_SIMMATH_EXPORT static void
186 TransformP& X_FP);
187
200SimTK_SIMMATH_EXPORT static void
202 int least[3], int most[3],
203 Vec3P& low, Vec3P& high);
204
207SimTK_SIMMATH_EXPORT static void
209 int least[3], int most[3],
210 Vec3P& low, Vec3P& high);
211
216 Array_<int>& support);
217
221{ Array_<int> support;
222 return calcAxisAlignedBoundingBox(points,support); }
223
228 Array_<int>& support);
229
233{ Array_<int> support;
234 return calcAxisAlignedBoundingBoxIndirect(points,support); }
235
255SimTK_SIMMATH_EXPORT static void
257 const RotationP& R_FB,
258 int least[3], int most[3],
259 Vec3P& low_B, Vec3P& high_B);
260
263SimTK_SIMMATH_EXPORT static void
265 const RotationP& R_FB,
266 int least[3], int most[3],
267 Vec3P& low_B, Vec3P& high_B);
268
274 Array_<int>& support,
275 bool optimize=true);
276
280{ Array_<int> support;
281 return calcOrientedBoundingBox(points,support); }
282
287 Array_<int>& support,
288 bool optimize=true);
289
293 bool optimize=true)
294{ Array_<int> support;
295 return calcOrientedBoundingBoxIndirect(points,support,optimize); }
334{ return Sphere_<P>(p, 0).stretchBoundary(); }
335
336
340static Sphere_<P> calcBoundingSphere(const Vec3P& p0, const Vec3P& p1) {
341 Array_<int> which;
342 return calcBoundingSphere(p0,p1,which);
343}
344
345
348 (const Vec3P& p0, const Vec3P& p1, const Vec3P& p2) {
349 Array_<int> which;
350 return calcBoundingSphere(p0,p1,p2,false,which);
351}
352
355 (const Vec3P& p0, const Vec3P& p1, const Vec3P& p2, const Vec3P& p3) {
356 Array_<int> which;
357 return calcBoundingSphere(p0,p1,p2,p3,false,which);
358}
359
364 Array_<int> which;
365 return calcBoundingSphere(points, which);
366}
367
370static Sphere_<P>
371calcBoundingSphere(const std::vector<Vec3P>& points) {
372 return calcBoundingSphere // no copy done here
373 (ArrayViewConst_<Vec3P>(points));
374}
375
380 Array_<int> which;
381 return calcBoundingSphereIndirect(points, which);
382}
383
386static Sphere_<P>
387calcBoundingSphere(const std::vector<const Vec3P*>& points) {
388 return calcBoundingSphereIndirect // no copy done here
390}
391
395{ which.clear(); which.push_back(0);
396 return Sphere_<P>(p0,0).stretchBoundary(); }
397
407calcBoundingSphere(const Vec3P& p0, const Vec3P& p1, Array_<int>& which);
408
416calcBoundingSphere(const Vec3P& p0, const Vec3P& p1, const Vec3P& p2,
417 bool forceCircumsphere, Array_<int>& which);
418
426calcBoundingSphere(const Vec3P& p0, const Vec3P& p1, const Vec3P& p2,
427 const Vec3P& p3, bool forceCircumsphere, Array_<int>& which);
428
435
439 Array_<int>& which);
440
445
448static Sphere_<P>
449calcApproxBoundingSphere(const std::vector<Vec3P>& points) {
450 return calcApproxBoundingSphere // no copy done here
451 (ArrayViewConst_<Vec3P>(points));
452}
453
457
460static Sphere_<P>
461calcApproxBoundingSphereIndirect(const std::vector<const Vec3P*>& points) {
462 return calcApproxBoundingSphereIndirect // no copy done here
464}
468private:
469Vec3P p;
470};
471
472} // namespace SimTK
473
474#endif // SimTK_SIMMATH_GEO_POINT_H_
Defines geometric primitive shapes and algorthms.
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
This Array_ helper class is the base class for ArrayView_ which is the base class for Array_; here we...
Definition: Array.h:324
The Array_<T> container class is a plug-compatible replacement for the C++ standard template library ...
Definition: Array.h:1520
void push_back(const T &value)
This method increases the size of the Array by one element at the end and initializes that element by...
Definition: Array.h:2399
void clear()
Erase all the elements currently in this array without changing the capacity; equivalent to erase(beg...
Definition: Array.h:2598
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
TODO: A 3d box oriented and positioned with respect to an unspecified frame F.
Definition: Geo_Box.h:528
A 3d point primitive represented by a Vec3 from the origin of an unspecified frame,...
Definition: Geo_Point.h:46
static Sphere_< P > calcBoundingSphere(const Vec3P &p0, const Vec3P &p1, const Vec3P &p2, bool forceCircumsphere, Array_< int > &which)
Create a minimum sphere around three points.
static Sphere_< P > calcBoundingSphere(const Array_< Vec3P > &points)
Create a minimal bounding sphere around a collection of n points.
Definition: Geo_Point.h:363
static Geo::AlignedBox_< P > calcAxisAlignedBoundingBox(const Array_< Vec3P > &points, Array_< int > &support)
Calculate the smallest axis-aligned bounding box including all n given points.
Point_(const Vec3P &location)
Construct a Point with the given location. Also serves as implicit conversion from Vec3 to Geo::Point...
Definition: Geo_Point.h:60
static Sphere_< P > calcBoundingSphere(const Vec3P &p0, const Vec3P &p1, Array_< int > &which)
Create a minimum sphere around two points.
static void calcPrincipalComponentsIndirect(const Array_< const Vec3P * > &points_F, TransformP &X_FP)
Alternate signature taking an array of pointers to points rather than the points themselves.
static Sphere_< P > calcBoundingSphere(const Vec3P &p0, Array_< int > &which)
Create one-point bounding sphere and return the (trivial) support point, of which there is always one...
Definition: Geo_Point.h:394
static void findExtremePoints(const Array_< Vec3P > &points, const UnitVec3P &direction, int &least, int &most, RealP &leastCoord, RealP &mostCoord)
Given a set of points, find the two points that are the most extreme along a given direction (not nec...
static Geo::OrientedBox_< P > calcOrientedBoundingBoxIndirect(const Array_< const Vec3P * > &points, bool optimize=true)
Alternate signature doesn't return support points.
Definition: Geo_Point.h:292
static void calcCovarianceIndirect(const Array_< const Vec3P * > &points_F, Vec3P &centroid, SymMat33P &covariance)
Alternate signature taking an array of pointers to points rather than the points themselves.
static Geo::AlignedBox_< P > calcAxisAlignedBoundingBox(const Array_< Vec3P > &points)
Alternate signature doesn't return support points.
Definition: Geo_Point.h:220
Point_()
Construct an uninitialized Point object; the location will be garbage.
Definition: Geo_Point.h:57
static Vec3P calcCentroid(const Array_< Vec3P > &points_F)
Given a set of points, calculate the centroid (average location) of those points.
static void findAxisAlignedExtremePointsIndirect(const Array_< const Vec3P * > &points, int least[3], int most[3], Vec3P &low, Vec3P &high)
Alternate signature taking an array of pointers to points rather than the points themselves.
static Geo::AlignedBox_< P > calcAxisAlignedBoundingBoxIndirect(const Array_< const Vec3P * > &points)
Alternate signature doesn't return support points.
Definition: Geo_Point.h:232
static Sphere_< P > calcBoundingSphere(const Vec3P &p0, const Vec3P &p1, const Vec3P &p2)
Create a minimal bounding sphere around three points.
Definition: Geo_Point.h:348
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 RealP calcDistance(const Vec3P &p1, const Vec3P &p2)
Calculate the distance between two points (expensive).
Definition: Geo_Point.h:87
RealP calcDistance(const Vec3P &p2) const
Calculate the distance between this point and another one whose location is expressed in the same fra...
Definition: Geo_Point.h:70
static bool pointsAreNumericallyCoincident(const Vec3P &p1, const Vec3P &p2, RealP tol)
Alternate signature with explicitly-supplied tolerance.
Definition: Geo_Point.h:112
static Sphere_< P > calcBoundingSphere(const Vec3P &p0, const Vec3P &p1, const Vec3P &p2, const Vec3P &p3)
Create a minimal bounding sphere around four points.
Definition: Geo_Point.h:355
static Sphere_< P > calcBoundingSphere(const std::vector< Vec3P > &points)
This signature takes an std::vector rather than a SimTK::Array_; no extra copying is required.
Definition: Geo_Point.h:371
static Sphere_< P > calcApproxBoundingSphereIndirect(const std::vector< const Vec3P * > &points)
This signature takes an std::vector rather than a SimTK::Array_; no extra copying is required.
Definition: Geo_Point.h:461
static void findOrientedExtremePointsIndirect(const Array_< const Vec3P * > &points_F, const RotationP &R_FB, int least[3], int most[3], Vec3P &low_B, Vec3P &high_B)
Alternate signature taking an array of pointers to points rather than the points themselves.
static Geo::AlignedBox_< P > calcAxisAlignedBoundingBoxIndirect(const Array_< const Vec3P * > &points, Array_< int > &support)
Alternate signature taking an array of pointers to points rather than the points themselves.
static void findExtremePointsIndirect(const Array_< const Vec3P * > &points, const UnitVec3P &direction, int &least, int &most, RealP &leastCoord, RealP &mostCoord)
Alternate signature taking an array of pointers to points rather than the points themselves.
static RealP findDistanceSqr(const Vec3P &p1, const Vec3P &p2)
Find the square of the distance between two points (cheap).
Definition: Geo_Point.h:92
static void findSupportPointIndirect(const Array_< const Vec3P * > &points, const UnitVec3P &direction, int &most, RealP &mostCoord)
Alternate signature taking an array of pointers to points rather than the points themselves.
static void calcPrincipalComponents(const Array_< Vec3P > &points_F, TransformP &X_FP)
Given a set of points in an unspecified frame F, find the principal component directions describing t...
static Vec3P calcCentroidIndirect(const Array_< const Vec3P * > &points_F)
Alternate signature taking an array of pointers to points rather than the points themselves.
static void findSupportPoint(const Array_< Vec3P > &points, const UnitVec3P &direction, int &most, RealP &mostCoord)
Given a set of points, find the one that is the furthest in a given direction, and return its index a...
RealP findDistanceSqr(const Vec3P &p2) const
Find the square of the distance between this point and another one whose location is expressed in the...
Definition: Geo_Point.h:75
static Sphere_< P > calcBoundingSphere(const Vec3P &p)
Create a tiny bounding sphere around a single point.
Definition: Geo_Point.h:333
static Sphere_< P > calcBoundingSphereIndirect(const Array_< const Vec3P * > &points)
Create a minimal bounding sphere around a collection of n points, given indirectly as an array of poi...
Definition: Geo_Point.h:379
static void findOrientedExtremePoints(const Array_< Vec3P > &points_F, const RotationP &R_FB, int least[3], int most[3], Vec3P &low_B, Vec3P &high_B)
Given a set of points, find the six points that are the most extreme along specified orientation dire...
static void calcCovariance(const Array_< Vec3P > &points_F, Vec3P &centroid, SymMat33P &covariance)
Given a set of points, calculate the centroid (average location) and covariance matrix of those point...
const Vec3P & getLocation() const
Get the location of this Point.
Definition: Geo_Point.h:66
static Geo::OrientedBox_< P > calcOrientedBoundingBoxIndirect(const Array_< const Vec3P * > &points, Array_< int > &support, bool optimize=true)
Alternate signature taking an array of pointers to points rather than the points themselves.
static Sphere_< P > calcBoundingSphere(const Vec3P &p0, const Vec3P &p1, const Vec3P &p2, const Vec3P &p3, bool forceCircumsphere, Array_< int > &which)
Create a minimum sphere around four points.
static Sphere_< P > calcBoundingSphereIndirect(const Array_< const Vec3P * > &points, Array_< int > &which)
Alternate signature works with an array of pointers to points.
static Sphere_< P > calcBoundingSphere(const Vec3P &p0, const Vec3P &p1)
Create a minimal bounding sphere around two points.
Definition: Geo_Point.h:340
static Sphere_< P > calcBoundingSphere(const Array_< Vec3P > &points, Array_< int > &which)
Create an optimal minimum sphere around a collection of n points.
static bool pointsAreNumericallyCoincident(const Vec3P &p1, const Vec3P &p2)
Determine whether two points whose locations are known to an accuracy tol are numerically indistingui...
Definition: Geo_Point.h:106
static void findAxisAlignedExtremePoints(const Array_< Vec3P > &points, int least[3], int most[3], Vec3P &low, Vec3P &high)
Given a set of points, find the six points that are the most extreme along the axial directions (not ...
static Sphere_< P > calcApproxBoundingSphere(const Array_< Vec3P > &points)
Calculate an approximate bounding sphere. You should normally use calcBoundingSphere() which will giv...
static Sphere_< P > calcApproxBoundingSphere(const std::vector< Vec3P > &points)
This signature takes an std::vector rather than a SimTK::Array_; no extra copying is required.
Definition: Geo_Point.h:449
static Sphere_< P > calcApproxBoundingSphereIndirect(const Array_< const Vec3P * > &points)
Alternate signature works with an array of pointers to points.
static Geo::OrientedBox_< P > calcOrientedBoundingBox(const Array_< Vec3P > &points)
Alternate signature doesn't return support points.
Definition: Geo_Point.h:279
static Vec3P findMidpoint(const Vec3P &p1, const Vec3P &p2)
Find the point midway between two points.
Definition: Geo_Point.h:96
static Sphere_< P > calcBoundingSphere(const std::vector< const Vec3P * > &points)
This signature takes an std::vector rather than a SimTK::Array_; no extra copying is required.
Definition: Geo_Point.h:387
Point_ & setLocation(const Vec3P &location)
Change the location of this point.
Definition: Geo_Point.h:63
A geometric primitive representing a sphere by its radius and center point, and a collection of spher...
Definition: Geo_Sphere.h:47
Sphere_ & stretchBoundary()
Stretch this sphere in place by a small amount to ensure that there will be no roundoff problems if t...
Definition: Geo_Sphere.h:78
The Rotation class is a Mat33 that guarantees that the matrix can be interpreted as a legitimate 3x3 ...
Definition: Rotation.h:111
This class represents the rotate-and-shift transform which gives the location and orientation of a ne...
Definition: Transform.h:108
This class is a Vec3 plus an ironclad guarantee either that:
Definition: UnitVec.h:56
TAbs abs() const
Elementwise absolute value; that is, the return value has the same dimension as this Vec but with eac...
Definition: Vec.h:347
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
ELEM max(const VectorBase< ELEM > &v)
Definition: VectorMath.h:251
unsigned char square(unsigned char u)
Definition: Scalar.h:349