Simbody  3.5
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;
53 typedef Transform_<P> TransformP;
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 
153 SimTK_SIMMATH_EXPORT static Vec3P
154 calcCentroid(const Array_<Vec3P>& points_F);
155 
158 SimTK_SIMMATH_EXPORT static Vec3P
160 
163 SimTK_SIMMATH_EXPORT static void
164 calcCovariance(const Array_<Vec3P>& points_F,
165  Vec3P& centroid, SymMat33P& covariance);
166 
169 SimTK_SIMMATH_EXPORT static void
171  Vec3P& centroid, SymMat33P& covariance);
172 
178 SimTK_SIMMATH_EXPORT static void
179 calcPrincipalComponents(const Array_<Vec3P>& points_F,
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); }
333 static Sphere_<P> calcBoundingSphere(const Vec3P& p)
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 
434 calcBoundingSphere(const Array_<Vec3P>& points, Array_<int>& which);
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_
static Sphere_< P > calcApproxBoundingSphereIndirect(const Array_< const Vec3P * > &points)
Alternate signature works with an array of pointers to points.
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
TAbs abs() const
Elementwise absolute value; that is, the return value has the same dimension as this Vec but with eac...
Definition: Vec.h:345
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::AlignedBox_< P > calcAxisAlignedBoundingBox(const Array_< Vec3P > &points)
Alternate signature doesn&#39;t return support points.
Definition: Geo_Point.h:220
static Sphere_< P > calcApproxBoundingSphere(const Array_< Vec3P > &points)
Calculate an approximate bounding sphere. You should normally use calcBoundingSphere() which will giv...
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 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 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 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.
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
static Geo::OrientedBox_< P > calcOrientedBoundingBoxIndirect(const Array_< const Vec3P * > &points, bool optimize=true)
Alternate signature doesn&#39;t return support points.
Definition: Geo_Point.h:292
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 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 ...
Point_ & setLocation(const Vec3P &location)
Change the location of this point.
Definition: Geo_Point.h:63
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
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 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 > 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
TODO: A 3d box oriented and positioned with respect to an unspecified frame F.
Definition: Geo.h:63
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
static Geo::AlignedBox_< P > calcAxisAlignedBoundingBoxIndirect(const Array_< const Vec3P * > &points)
Alternate signature doesn&#39;t return support points.
Definition: Geo_Point.h:232
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...
static Vec3P calcCentroidIndirect(const Array_< const Vec3P * > &points_F)
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
unsigned char square(unsigned char u)
Definition: Scalar.h:351
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 Vec3P calcCentroid(const Array_< Vec3P > &points_F)
Given a set of points, calculate the centroid (average location) of those points. ...
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.
Includes internal headers providing declarations for the basic SimTK Core classes, including Simmatrix.
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 Sphere_< P > calcBoundingSphere(const Vec3P &p0, const Vec3P &p1)
Create a minimal bounding sphere around two points.
Definition: Geo_Point.h:340
The SimTK::Array_<T> container class is a plug-compatible replacement for the C++ standard template l...
Definition: Array.h:50
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.
ELEM max(const VectorBase< ELEM > &v)
Definition: VectorMath.h:251
Defines geometric primitive shapes and algorthms.
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 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 Geo::OrientedBox_< P > calcOrientedBoundingBox(const Array_< Vec3P > &points)
Alternate signature doesn&#39;t return support points.
Definition: Geo_Point.h:279
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.
This is the header file that every Simmath compilation unit should include first. ...
This Array_ helper class is the base class for ArrayView_ which is the base class for Array_; here we...
Definition: Array.h:48
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
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
void clear()
Erase all the elements currently in this array without changing the capacity; equivalent to erase(beg...
Definition: Array.h:2522
static RealP calcDistance(const Vec3P &p1, const Vec3P &p2)
Calculate the distance between two points (expensive).
Definition: Geo_Point.h:87
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...
A geometric primitive representing a sphere by its radius and center point, and a collection of spher...
Definition: Geo.h:56
const Vec3P & getLocation() const
Get the location of this Point.
Definition: Geo_Point.h:66
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
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:2359
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
Point_()
Construct an uninitialized Point object; the location will be garbage.
Definition: Geo_Point.h:57
static Sphere_< P > calcBoundingSphere(const Array_< Vec3P > &points)
Create a minimal bounding sphere around a collection of n points.
Definition: Geo_Point.h:363
#define SimTK_SIMMATH_EXPORT
Definition: SimTKmath/include/simmath/internal/common.h:64
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 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 Sphere_< P > calcBoundingSphere(const Vec3P &p)
Create a tiny bounding sphere around a single point.
Definition: Geo_Point.h:333