simbody/Geo__Point_8h_source.html

00001 #ifndef SimTK_SIMMATH_GEO_POINT_H_
00002 #define SimTK_SIMMATH_GEO_POINT_H_
00003
00004 /* -------------------------------------------------------------------------- *
00005  *                        Simbody(tm): SimTKmath                              *
00006  * -------------------------------------------------------------------------- *
00007  * This is part of the SimTK biosimulation toolkit originating from           *
00008  * Simbios, the NIH National Center for Physics-Based Simulation of           *
00009  * Biological Structures at Stanford, funded under the NIH Roadmap for        *
00010  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
00011  *                                                                            *
00012  * Portions copyright (c) 2011-12 Stanford University and the Authors.        *
00013  * Authors: Michael Sherman                                                   *
00014  * Contributors:                                                              *
00015  *                                                                            *
00016  * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
00017  * not use this file except in compliance with the License. You may obtain a  *
00018  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
00019  *                                                                            *
00020  * Unless required by applicable law or agreed to in writing, software        *
00021  * distributed under the License is distributed on an "AS IS" BASIS,          *
00022  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
00023  * See the License for the specific language governing permissions and        *
00024  * limitations under the License.                                             *
00025  * -------------------------------------------------------------------------- */
00026
00030 #include "SimTKcommon.h"
00031 #include "simmath/internal/common.h"
00032 #include "simmath/internal/Geo.h"
00033
00034 #include <cassert>
00035 #include <cmath>
00036 #include <algorithm>
00037
00038 namespace SimTK {
00039
00040 //==============================================================================
00041 //                               GEO POINT
00042 //==============================================================================
00045 template <class P>
00046 class Geo::Point_ {
00047 typedef P               RealP;
00048 typedef Vec<3,P>        Vec3P;
00049 typedef Mat<3,3,P>      Mat33P;
00050 typedef SymMat<3,P>     SymMat33P;
00051 typedef UnitVec<P,1>    UnitVec3P;
00052 typedef Rotation_<P>    RotationP;
00053 typedef Transform_<P>   TransformP;
00054
00055 public:
00057 Point_() {}
00060 Point_(const Vec3P& location) : p(location) {}
00061
00063 Point_& setLocation(const Vec3P& location) {p=location; return *this;}
00064
00066 const Vec3P& getLocation() const {return p;}
00067
00070 RealP calcDistance(const Vec3P& p2) const
00071 {   return calcDistance(p, p2); }
00072
00075 RealP findDistanceSqr(const Vec3P& p2) const
00076 {   return findDistanceSqr(p, p2); }
00077
00087 static RealP calcDistance(const Vec3P& p1, const Vec3P& p2)
00088 {   return std::sqrt(findDistanceSqr(p1,p2)); }
00089
00092 static RealP findDistanceSqr(const Vec3P& p1, const Vec3P& p2)
00093 {   return (p2-p1).normSqr(); }
00094
00096 static Vec3P findMidpoint(const Vec3P& p1, const Vec3P& p2)
00097 {   return (p1+p2)/2; }
00098
00106 static bool pointsAreNumericallyCoincident(const Vec3P& p1, const Vec3P& p2)
00107 {
00108     return pointsAreNumericallyCoincident(p1,p2,Geo::getDefaultTol<RealP>());
00109 }
00111 static bool pointsAreNumericallyCoincident
00112    (const Vec3P& p1, const Vec3P& p2, RealP tol)
00113 {
00114     const RealP maxcoord = std::max(max(p1.abs()),max(p2.abs())); // ~7 flops
00115     const RealP scale = std::max(tol, maxcoord*tol); // 2 flops
00116     return findDistanceSqr(p1,p2) < square(scale); // 10 flops
00117 }
00118
00119
00123 SimTK_SIMMATH_EXPORT static void
00124 findSupportPoint(const Array_<Vec3P>& points, const UnitVec3P& direction,
00125                  int& most, RealP& mostCoord);
00126
00129 SimTK_SIMMATH_EXPORT static void
00130 findSupportPointIndirect(const Array_<const Vec3P*>& points,
00131                          const UnitVec3P& direction,
00132                          int& most, RealP& mostCoord);
00133
00138 SimTK_SIMMATH_EXPORT static void
00139 findExtremePoints(const Array_<Vec3P>& points, const UnitVec3P& direction,
00140                   int& least, int& most,
00141                   RealP& leastCoord, RealP& mostCoord);
00142
00145 SimTK_SIMMATH_EXPORT static void
00146 findExtremePointsIndirect(const Array_<const Vec3P*>& points,
00147                           const UnitVec3P& direction,
00148                           int& least, int& most,
00149                           RealP& leastCoord, RealP& mostCoord);
00150
00153 SimTK_SIMMATH_EXPORT static Vec3P
00154 calcCentroid(const Array_<Vec3P>& points_F);
00155
00158 SimTK_SIMMATH_EXPORT static Vec3P
00159 calcCentroidIndirect(const Array_<const Vec3P*>& points_F);
00160
00163 SimTK_SIMMATH_EXPORT static void
00164 calcCovariance(const Array_<Vec3P>& points_F,
00165                Vec3P& centroid, SymMat33P& covariance);
00166
00169 SimTK_SIMMATH_EXPORT static void
00170 calcCovarianceIndirect(const Array_<const Vec3P*>& points_F,
00171                        Vec3P& centroid, SymMat33P& covariance);
00172
00178 SimTK_SIMMATH_EXPORT static void
00179 calcPrincipalComponents(const Array_<Vec3P>& points_F,
00180                         TransformP&          X_FP);
00181
00184 SimTK_SIMMATH_EXPORT static void
00185 calcPrincipalComponentsIndirect(const Array_<const Vec3P*>& points_F,
00186                                 TransformP&                 X_FP);
00187
00200 SimTK_SIMMATH_EXPORT static void
00201 findAxisAlignedExtremePoints(const Array_<Vec3P>& points,
00202                              int least[3], int most[3],
00203                              Vec3P& low, Vec3P& high);
00204
00207 SimTK_SIMMATH_EXPORT static void
00208 findAxisAlignedExtremePointsIndirect(const Array_<const Vec3P*>& points,
00209                                      int least[3], int most[3],
00210                                      Vec3P& low, Vec3P& high);
00211
00214 SimTK_SIMMATH_EXPORT static Geo::AlignedBox_<P>
00215 calcAxisAlignedBoundingBox(const Array_<Vec3P>& points,
00216                            Array_<int>&         support);
00217
00219 static Geo::AlignedBox_<P>
00220 calcAxisAlignedBoundingBox(const Array_<Vec3P>& points)
00221 {   Array_<int> support;
00222     return calcAxisAlignedBoundingBox(points,support); }
00223
00226 SimTK_SIMMATH_EXPORT static Geo::AlignedBox_<P>
00227 calcAxisAlignedBoundingBoxIndirect(const Array_<const Vec3P*>& points,
00228                                    Array_<int>&                support);
00229
00231 static Geo::AlignedBox_<P>
00232 calcAxisAlignedBoundingBoxIndirect(const Array_<const Vec3P*>& points)
00233 {   Array_<int> support;
00234     return calcAxisAlignedBoundingBoxIndirect(points,support); }
00235
00255 SimTK_SIMMATH_EXPORT static void
00256 findOrientedExtremePoints(const Array_<Vec3P>&  points_F,
00257                           const RotationP&      R_FB,
00258                           int least[3], int most[3],
00259                           Vec3P& low_B, Vec3P& high_B);
00260
00263 SimTK_SIMMATH_EXPORT static void
00264 findOrientedExtremePointsIndirect(const Array_<const Vec3P*>&  points_F,
00265                                   const RotationP&             R_FB,
00266                                   int least[3], int most[3],
00267                                   Vec3P& low_B, Vec3P& high_B);
00268
00272 SimTK_SIMMATH_EXPORT static Geo::OrientedBox_<P>
00273 calcOrientedBoundingBox(const Array_<Vec3P>& points,
00274                         Array_<int>&         support,
00275                         bool                 optimize=true);
00276
00278 static Geo::OrientedBox_<P>
00279 calcOrientedBoundingBox(const Array_<Vec3P>& points)
00280 {   Array_<int> support;
00281     return calcOrientedBoundingBox(points,support); }
00282
00285 SimTK_SIMMATH_EXPORT static Geo::OrientedBox_<P>
00286 calcOrientedBoundingBoxIndirect(const Array_<const Vec3P*>& points,
00287                                 Array_<int>&                support,
00288                                 bool                        optimize=true);
00289
00291 static Geo::OrientedBox_<P>
00292 calcOrientedBoundingBoxIndirect(const Array_<const Vec3P*>& points,
00293                                 bool                        optimize=true)
00294 {   Array_<int> support;
00295     return calcOrientedBoundingBoxIndirect(points,support,optimize); }
00333 static Sphere_<P> calcBoundingSphere(const Vec3P& p)
00334 {   return Sphere_<P>(p, 0).stretchBoundary(); }
00335
00336
00340 static Sphere_<P> calcBoundingSphere(const Vec3P& p0, const Vec3P& p1) {
00341     Array_<int> which;
00342     return calcBoundingSphere(p0,p1,which);
00343 }
00344
00345
00347 static Sphere_<P> calcBoundingSphere
00348    (const Vec3P& p0, const Vec3P& p1, const Vec3P& p2) {
00349     Array_<int> which;
00350     return calcBoundingSphere(p0,p1,p2,false,which);
00351 }
00352
00354 static Sphere_<P> calcBoundingSphere
00355    (const Vec3P& p0, const Vec3P& p1, const Vec3P& p2, const Vec3P& p3) {
00356     Array_<int> which;
00357     return calcBoundingSphere(p0,p1,p2,p3,false,which);
00358 }
00359
00363 static Sphere_<P> calcBoundingSphere(const Array_<Vec3P>& points) {
00364     Array_<int> which;
00365     return calcBoundingSphere(points, which);
00366 }
00367
00370 static Sphere_<P>
00371 calcBoundingSphere(const std::vector<Vec3P>& points) {
00372     return calcBoundingSphere // no copy done here
00373                 (ArrayViewConst_<Vec3P>(points));
00374 }
00375
00379 static Sphere_<P> calcBoundingSphereIndirect(const Array_<const Vec3P*>& points) {
00380     Array_<int> which;
00381     return calcBoundingSphereIndirect(points, which);
00382 }
00383
00386 static Sphere_<P>
00387 calcBoundingSphere(const std::vector<const Vec3P*>& points) {
00388     return calcBoundingSphereIndirect // no copy done here
00389                 (ArrayViewConst_<const Vec3P*>(points));
00390 }
00391
00394 static Sphere_<P> calcBoundingSphere(const Vec3P& p0, Array_<int>& which)
00395 {   which.clear(); which.push_back(0);
00396     return Sphere_<P>(p0,0).stretchBoundary(); }
00397
00406 SimTK_SIMMATH_EXPORT static Sphere_<P>
00407 calcBoundingSphere(const Vec3P& p0, const Vec3P& p1, Array_<int>& which);
00408
00415 SimTK_SIMMATH_EXPORT static Sphere_<P>
00416 calcBoundingSphere(const Vec3P& p0, const Vec3P& p1, const Vec3P& p2,
00417                    bool forceCircumsphere, Array_<int>& which);
00418
00425 SimTK_SIMMATH_EXPORT static Sphere_<P>
00426 calcBoundingSphere(const Vec3P& p0, const Vec3P& p1, const Vec3P& p2,
00427                    const Vec3P& p3, bool forceCircumsphere, Array_<int>& which);
00428
00433 SimTK_SIMMATH_EXPORT static Sphere_<P>
00434 calcBoundingSphere(const Array_<Vec3P>& points, Array_<int>& which);
00435
00437 SimTK_SIMMATH_EXPORT static Sphere_<P>
00438 calcBoundingSphereIndirect(const Array_<const Vec3P*>& points,
00439                            Array_<int>& which);
00440
00443 SimTK_SIMMATH_EXPORT static Sphere_<P>
00444 calcApproxBoundingSphere(const Array_<Vec3P>& points);
00445
00448 static Sphere_<P>
00449 calcApproxBoundingSphere(const std::vector<Vec3P>& points) {
00450     return calcApproxBoundingSphere // no copy done here
00451                 (ArrayViewConst_<Vec3P>(points));
00452 }
00453
00455 SimTK_SIMMATH_EXPORT static Sphere_<P>
00456 calcApproxBoundingSphereIndirect(const Array_<const Vec3P*>& points);
00457
00460 static Sphere_<P>
00461 calcApproxBoundingSphereIndirect(const std::vector<const Vec3P*>& points) {
00462     return calcApproxBoundingSphereIndirect // no copy done here
00463                 (ArrayViewConst_<const Vec3P*>(points));
00464 }
00468 private:
00469 Vec3P   p;
00470 };
00471
00472 } // namespace SimTK
00473
00474 #endif // SimTK_SIMMATH_GEO_POINT_H_