Simbody  3.8
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"
32 #include "simmath/internal/Geo.h"
33 
34 #include <cassert>
35 #include <cmath>
36 #include <algorithm>
37 
38 namespace SimTK {
39 
40 //==============================================================================
41 // GEO POINT
42 //==============================================================================
45 template <class P>
46 class Geo::Point_ {
47 typedef P RealP;
48 typedef Vec<3,P> Vec3P;
49 typedef Mat<3,3,P> Mat33P;
50 typedef SymMat<3,P> SymMat33P;
51 typedef UnitVec<P,1> UnitVec3P;
52 typedef Rotation_<P> RotationP;
54 
55 public:
57 Point_() {}
60 Point_(const Vec3P& location) : p(location) {}
61 
63 Point_& setLocation(const Vec3P& location) {p=location; return *this;}
64 
66 const Vec3P& getLocation() const {return p;}
67 
70 RealP calcDistance(const Vec3P& p2) const
71 { return calcDistance(p, p2); }
72 
75 RealP findDistanceSqr(const Vec3P& p2) const
76 { return findDistanceSqr(p, p2); }
77 
87 static RealP calcDistance(const Vec3P& p1, const Vec3P& p2)
88 { return std::sqrt(findDistanceSqr(p1,p2)); }
89 
92 static RealP findDistanceSqr(const Vec3P& p1, const Vec3P& p2)
93 { return (p2-p1).normSqr(); }
94 
96 static Vec3P findMidpoint(const Vec3P& p1, const Vec3P& p2)
97 { return (p1+p2)/2; }
98 
106 static 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 
123 SimTK_SIMMATH_EXPORT static void
124 findSupportPoint(const Array_<Vec3P>& points, const UnitVec3P& direction,
125  int& most, RealP& mostCoord);
126 
129 SimTK_SIMMATH_EXPORT static void
131  const UnitVec3P& direction,
132  int& most, RealP& mostCoord);
133 
138 SimTK_SIMMATH_EXPORT static void
139 findExtremePoints(const Array_<Vec3P>& points, const UnitVec3P& direction,
140  int& least, int& most,
141  RealP& leastCoord, RealP& mostCoord);
142 
145 SimTK_SIMMATH_EXPORT static void
147  const UnitVec3P& direction,
148  int& least, int& most,
149  RealP& leastCoord, RealP& mostCoord);
150 
154 calcCentroid(const Array_<Vec3P>& points_F);
155 
160 
163 SimTK_SIMMATH_EXPORT static void
165  Vec3P& centroid, SymMat33P& covariance);
166 
169 SimTK_SIMMATH_EXPORT static void
171  Vec3P& centroid, SymMat33P& covariance);
172 
178 SimTK_SIMMATH_EXPORT static void
180  TransformP& X_FP);
181 
184 SimTK_SIMMATH_EXPORT static void
186  TransformP& X_FP);
187 
200 SimTK_SIMMATH_EXPORT static void
202  int least[3], int most[3],
203  Vec3P& low, Vec3P& high);
204 
207 SimTK_SIMMATH_EXPORT static void
209  int least[3], int most[3],
210  Vec3P& low, Vec3P& high);
211 
216  Array_<int>& support);
217 
219 static Geo::AlignedBox_<P>
221 { Array_<int> support;
222  return calcAxisAlignedBoundingBox(points,support); }
223 
228  Array_<int>& support);
229 
231 static Geo::AlignedBox_<P>
233 { Array_<int> support;
234  return calcAxisAlignedBoundingBoxIndirect(points,support); }
235 
255 SimTK_SIMMATH_EXPORT static void
257  const RotationP& R_FB,
258  int least[3], int most[3],
259  Vec3P& low_B, Vec3P& high_B);
260 
263 SimTK_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 
278 static Geo::OrientedBox_<P>
280 { Array_<int> support;
281  return calcOrientedBoundingBox(points,support); }
282 
287  Array_<int>& support,
288  bool optimize=true);
289 
291 static Geo::OrientedBox_<P>
293  bool optimize=true)
294 { Array_<int> support;
295  return calcOrientedBoundingBoxIndirect(points,support,optimize); }
334 { return Sphere_<P>(p, 0).stretchBoundary(); }
335 
336 
340 static 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 
370 static Sphere_<P>
371 calcBoundingSphere(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 
386 static Sphere_<P>
387 calcBoundingSphere(const std::vector<const Vec3P*>& points) {
388  return calcBoundingSphereIndirect // no copy done here
390 }
391 
394 static Sphere_<P> calcBoundingSphere(const Vec3P& p0, Array_<int>& which)
395 { which.clear(); which.push_back(0);
396  return Sphere_<P>(p0,0).stretchBoundary(); }
397 
407 calcBoundingSphere(const Vec3P& p0, const Vec3P& p1, Array_<int>& which);
408 
416 calcBoundingSphere(const Vec3P& p0, const Vec3P& p1, const Vec3P& p2,
417  bool forceCircumsphere, Array_<int>& which);
418 
426 calcBoundingSphere(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 
448 static Sphere_<P>
449 calcApproxBoundingSphere(const std::vector<Vec3P>& points) {
450  return calcApproxBoundingSphere // no copy done here
451  (ArrayViewConst_<Vec3P>(points));
452 }
453 
457 
460 static Sphere_<P>
461 calcApproxBoundingSphereIndirect(const std::vector<const Vec3P*>& points) {
462  return calcApproxBoundingSphereIndirect // no copy done here
464 }
468 private:
469 Vec3P 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)
Create a minimal bounding sphere around two points.
Definition: Geo_Point.h:340
static Sphere_< P > calcApproxBoundingSphereIndirect(const Array_< const Vec3P * > &points)
Alternate signature works with an array of pointers to 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
const Vec3P & getLocation() const
Get the location of this Point.
Definition: Geo_Point.h:66
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 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 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.
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 > calcAxisAlignedBoundingBox(const Array_< Vec3P > &points)
Alternate signature doesn't return support points.
Definition: Geo_Point.h:220
Point_ & setLocation(const Vec3P &location)
Change the location of this point.
Definition: Geo_Point.h:63
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 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, Array_< int > &which)
Alternate signature works with an array of pointers to points.
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 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 > 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 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 Sphere_< P > calcApproxBoundingSphere(const Array_< Vec3P > &points)
Calculate an approximate bounding sphere. You should normally use calcBoundingSphere() which will giv...
static Sphere_< P > calcBoundingSphere(const Array_< Vec3P > &points, Array_< int > &which)
Create an optimal minimum sphere around a collection of n points.
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 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
static Geo::AlignedBox_< P > calcAxisAlignedBoundingBoxIndirect(const Array_< const Vec3P * > &points)
Alternate signature doesn't return support points.
Definition: Geo_Point.h:232
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 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 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 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 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...
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 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 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 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 Geo::AlignedBox_< P > calcAxisAlignedBoundingBox(const Array_< Vec3P > &points, Array_< int > &support)
Calculate the smallest axis-aligned bounding box including all n given points.
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 > calcBoundingSphere(const Vec3P &p0, const Vec3P &p1, const Vec3P &p2)
Create a minimal bounding sphere around three points.
Definition: Geo_Point.h:348
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 > calcBoundingSphere(const Array_< Vec3P > &points)
Create a minimal bounding sphere around a collection of n points.
Definition: Geo_Point.h:363
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 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 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 Vec3P &p0, const Vec3P &p1, Array_< int > &which)
Create a minimum sphere around two points.
static Geo::OrientedBox_< P > calcOrientedBoundingBox(const Array_< Vec3P > &points)
Alternate signature doesn't return support points.
Definition: Geo_Point.h:279
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