Simbody  3.4
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
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"
31 #include "simmath/internal/common.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 }
111 static bool pointsAreNumericallyCoincident
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
130 findSupportPointIndirect(const Array_<const Vec3P*>& points,
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
146 findExtremePointsIndirect(const Array_<const Vec3P*>& points,
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
159 calcCentroidIndirect(const Array_<const Vec3P*>& points_F);
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
170 calcCovarianceIndirect(const Array_<const Vec3P*>& points_F,
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
185 calcPrincipalComponentsIndirect(const Array_<const Vec3P*>& points_F,
186  TransformP& X_FP);
187 
200 SimTK_SIMMATH_EXPORT static void
201 findAxisAlignedExtremePoints(const Array_<Vec3P>& points,
202  int least[3], int most[3],
203  Vec3P& low, Vec3P& high);
204 
207 SimTK_SIMMATH_EXPORT static void
208 findAxisAlignedExtremePointsIndirect(const Array_<const Vec3P*>& points,
209  int least[3], int most[3],
210  Vec3P& low, Vec3P& high);
211 
214 SimTK_SIMMATH_EXPORT static Geo::AlignedBox_<P>
215 calcAxisAlignedBoundingBox(const Array_<Vec3P>& points,
216  Array_<int>& support);
217 
219 static Geo::AlignedBox_<P>
220 calcAxisAlignedBoundingBox(const Array_<Vec3P>& points)
221 { Array_<int> support;
222  return calcAxisAlignedBoundingBox(points,support); }
223 
226 SimTK_SIMMATH_EXPORT static Geo::AlignedBox_<P>
227 calcAxisAlignedBoundingBoxIndirect(const Array_<const Vec3P*>& points,
228  Array_<int>& support);
229 
231 static Geo::AlignedBox_<P>
232 calcAxisAlignedBoundingBoxIndirect(const Array_<const Vec3P*>& points)
233 { Array_<int> support;
234  return calcAxisAlignedBoundingBoxIndirect(points,support); }
235 
255 SimTK_SIMMATH_EXPORT static void
256 findOrientedExtremePoints(const Array_<Vec3P>& points_F,
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
264 findOrientedExtremePointsIndirect(const Array_<const Vec3P*>& points_F,
265  const RotationP& R_FB,
266  int least[3], int most[3],
267  Vec3P& low_B, Vec3P& high_B);
268 
272 SimTK_SIMMATH_EXPORT static Geo::OrientedBox_<P>
273 calcOrientedBoundingBox(const Array_<Vec3P>& points,
274  Array_<int>& support,
275  bool optimize=true);
276 
278 static Geo::OrientedBox_<P>
279 calcOrientedBoundingBox(const Array_<Vec3P>& points)
280 { Array_<int> support;
281  return calcOrientedBoundingBox(points,support); }
282 
285 SimTK_SIMMATH_EXPORT static Geo::OrientedBox_<P>
286 calcOrientedBoundingBoxIndirect(const Array_<const Vec3P*>& points,
287  Array_<int>& support,
288  bool optimize=true);
289 
291 static Geo::OrientedBox_<P>
292 calcOrientedBoundingBoxIndirect(const Array_<const Vec3P*>& points,
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 
347 static Sphere_<P> calcBoundingSphere
348  (const Vec3P& p0, const Vec3P& p1, const Vec3P& p2) {
349  Array_<int> which;
350  return calcBoundingSphere(p0,p1,p2,false,which);
351 }
352 
354 static Sphere_<P> calcBoundingSphere
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 
363 static Sphere_<P> calcBoundingSphere(const Array_<Vec3P>& points) {
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 
379 static Sphere_<P> calcBoundingSphereIndirect(const Array_<const Vec3P*>& points) {
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
389  (ArrayViewConst_<const Vec3P*>(points));
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 
406 SimTK_SIMMATH_EXPORT static Sphere_<P>
407 calcBoundingSphere(const Vec3P& p0, const Vec3P& p1, Array_<int>& which);
408 
415 SimTK_SIMMATH_EXPORT static Sphere_<P>
416 calcBoundingSphere(const Vec3P& p0, const Vec3P& p1, const Vec3P& p2,
417  bool forceCircumsphere, Array_<int>& which);
418 
425 SimTK_SIMMATH_EXPORT static Sphere_<P>
426 calcBoundingSphere(const Vec3P& p0, const Vec3P& p1, const Vec3P& p2,
427  const Vec3P& p3, bool forceCircumsphere, Array_<int>& which);
428 
433 SimTK_SIMMATH_EXPORT static Sphere_<P>
434 calcBoundingSphere(const Array_<Vec3P>& points, Array_<int>& which);
435 
437 SimTK_SIMMATH_EXPORT static Sphere_<P>
438 calcBoundingSphereIndirect(const Array_<const Vec3P*>& points,
439  Array_<int>& which);
440 
443 SimTK_SIMMATH_EXPORT static Sphere_<P>
444 calcApproxBoundingSphere(const Array_<Vec3P>& points);
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 
455 SimTK_SIMMATH_EXPORT static Sphere_<P>
456 calcApproxBoundingSphereIndirect(const Array_<const Vec3P*>& points);
457 
460 static Sphere_<P>
461 calcApproxBoundingSphereIndirect(const std::vector<const Vec3P*>& points) {
462  return calcApproxBoundingSphereIndirect // no copy done here
463  (ArrayViewConst_<const Vec3P*>(points));
464 }
468 private:
469 Vec3P p;
470 };
471 
472 } // namespace SimTK
473 
474 #endif // SimTK_SIMMATH_GEO_POINT_H_