Geo_Point.h

Go to the documentation of this file.

#ifndef SimTK_SIMMATH_GEO_POINT_H_
#define SimTK_SIMMATH_GEO_POINT_H_

/* -------------------------------------------------------------------------- *
 *                        Simbody(tm): SimTKmath                              *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK biosimulation toolkit originating from           *
 * Simbios, the NIH National Center for Physics-Based Simulation of           *
 * Biological Structures at Stanford, funded under the NIH Roadmap for        *
 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
 *                                                                            *
 * Portions copyright (c) 2011-12 Stanford University and the Authors.        *
 * Authors: Michael Sherman                                                   *
 * Contributors:                                                              *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

#include "SimTKcommon.h"
#include "simmath/internal/common.h"
#include "simmath/internal/Geo.h"

#include <cassert>
#include <cmath>
#include <algorithm>

namespace SimTK {

//==============================================================================
//                               GEO POINT
//==============================================================================
template <class P>
class Geo::Point_ {
typedef P               RealP;
typedef Vec<3,P>        Vec3P;
typedef Mat<3,3,P>      Mat33P;
typedef SymMat<3,P>     SymMat33P;
typedef UnitVec<P,1>    UnitVec3P;
typedef Rotation_<P>    RotationP;
typedef Transform_<P>   TransformP;

public:
Point_() {}
Point_(const Vec3P& location) : p(location) {} 

Point_& setLocation(const Vec3P& location) {p=location; return *this;}

const Vec3P& getLocation() const {return p;}

RealP calcDistance(const Vec3P& p2) const 
{   return calcDistance(p, p2); }

RealP findDistanceSqr(const Vec3P& p2) const 
{   return findDistanceSqr(p, p2); }

static RealP calcDistance(const Vec3P& p1, const Vec3P& p2)
{   return std::sqrt(findDistanceSqr(p1,p2)); }

static RealP findDistanceSqr(const Vec3P& p1, const Vec3P& p2)
{   return (p2-p1).normSqr(); }

static Vec3P findMidpoint(const Vec3P& p1, const Vec3P& p2)
{   return (p1+p2)/2; }

static bool pointsAreNumericallyCoincident(const Vec3P& p1, const Vec3P& p2)
{
    return pointsAreNumericallyCoincident(p1,p2,Geo::getDefaultTol<RealP>());
}
static bool pointsAreNumericallyCoincident
   (const Vec3P& p1, const Vec3P& p2, RealP tol)
{
    const RealP maxcoord = std::max(max(p1.abs()),max(p2.abs())); // ~7 flops
    const RealP scale = std::max(tol, maxcoord*tol); // 2 flops
    return findDistanceSqr(p1,p2) < square(scale); // 10 flops
}


SimTK_SIMMATH_EXPORT static void
findSupportPoint(const Array_<Vec3P>& points, const UnitVec3P& direction,
                 int& most, RealP& mostCoord);

SimTK_SIMMATH_EXPORT static void
findSupportPointIndirect(const Array_<const Vec3P*>& points, 
                         const UnitVec3P& direction,
                         int& most, RealP& mostCoord);

SimTK_SIMMATH_EXPORT static void
findExtremePoints(const Array_<Vec3P>& points, const UnitVec3P& direction,
                  int& least, int& most,
                  RealP& leastCoord, RealP& mostCoord);

SimTK_SIMMATH_EXPORT static void
findExtremePointsIndirect(const Array_<const Vec3P*>& points, 
                          const UnitVec3P& direction,
                          int& least, int& most,
                          RealP& leastCoord, RealP& mostCoord);

SimTK_SIMMATH_EXPORT static Vec3P
calcCentroid(const Array_<Vec3P>& points_F);

SimTK_SIMMATH_EXPORT static Vec3P
calcCentroidIndirect(const Array_<const Vec3P*>& points_F);

SimTK_SIMMATH_EXPORT static void
calcCovariance(const Array_<Vec3P>& points_F,
               Vec3P& centroid, SymMat33P& covariance);

SimTK_SIMMATH_EXPORT static void
calcCovarianceIndirect(const Array_<const Vec3P*>& points_F,
                       Vec3P& centroid, SymMat33P& covariance);

SimTK_SIMMATH_EXPORT static void
calcPrincipalComponents(const Array_<Vec3P>& points_F,
                        TransformP&          X_FP);

SimTK_SIMMATH_EXPORT static void
calcPrincipalComponentsIndirect(const Array_<const Vec3P*>& points_F,
                                TransformP&                 X_FP);

SimTK_SIMMATH_EXPORT static void
findAxisAlignedExtremePoints(const Array_<Vec3P>& points,
                             int least[3], int most[3],
                             Vec3P& low, Vec3P& high);

SimTK_SIMMATH_EXPORT static void
findAxisAlignedExtremePointsIndirect(const Array_<const Vec3P*>& points,
                                     int least[3], int most[3],
                                     Vec3P& low, Vec3P& high);

SimTK_SIMMATH_EXPORT static Geo::AlignedBox_<P> 
calcAxisAlignedBoundingBox(const Array_<Vec3P>& points,
                           Array_<int>&         support);

static Geo::AlignedBox_<P> 
calcAxisAlignedBoundingBox(const Array_<Vec3P>& points)
{   Array_<int> support; 
    return calcAxisAlignedBoundingBox(points,support); }

SimTK_SIMMATH_EXPORT static Geo::AlignedBox_<P> 
calcAxisAlignedBoundingBoxIndirect(const Array_<const Vec3P*>& points,
                                   Array_<int>&                support);

static Geo::AlignedBox_<P> 
calcAxisAlignedBoundingBoxIndirect(const Array_<const Vec3P*>& points)
{   Array_<int> support; 
    return calcAxisAlignedBoundingBoxIndirect(points,support); }

SimTK_SIMMATH_EXPORT static void
findOrientedExtremePoints(const Array_<Vec3P>&  points_F, 
                          const RotationP&      R_FB,
                          int least[3], int most[3],
                          Vec3P& low_B, Vec3P& high_B);

SimTK_SIMMATH_EXPORT 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);

SimTK_SIMMATH_EXPORT static Geo::OrientedBox_<P> 
calcOrientedBoundingBox(const Array_<Vec3P>& points,
                        Array_<int>&         support,
                        bool                 optimize=true);

static Geo::OrientedBox_<P> 
calcOrientedBoundingBox(const Array_<Vec3P>& points)
{   Array_<int> support; 
    return calcOrientedBoundingBox(points,support); }

SimTK_SIMMATH_EXPORT static Geo::OrientedBox_<P> 
calcOrientedBoundingBoxIndirect(const Array_<const Vec3P*>& points,
                                Array_<int>&                support,
                                bool                        optimize=true);

static Geo::OrientedBox_<P> 
calcOrientedBoundingBoxIndirect(const Array_<const Vec3P*>& points,
                                bool                        optimize=true)
{   Array_<int> support; 
    return calcOrientedBoundingBoxIndirect(points,support,optimize); }
static Sphere_<P> calcBoundingSphere(const Vec3P& p)
{   return Sphere_<P>(p, 0).stretchBoundary(); }


static Sphere_<P> calcBoundingSphere(const Vec3P& p0, const Vec3P& p1) {
    Array_<int> which;
    return calcBoundingSphere(p0,p1,which);
}


static Sphere_<P> calcBoundingSphere
   (const Vec3P& p0, const Vec3P& p1, const Vec3P& p2) {
    Array_<int> which;
    return calcBoundingSphere(p0,p1,p2,false,which);
}

static Sphere_<P> calcBoundingSphere
   (const Vec3P& p0, const Vec3P& p1, const Vec3P& p2, const Vec3P& p3) {
    Array_<int> which;
    return calcBoundingSphere(p0,p1,p2,p3,false,which);
}

static Sphere_<P> calcBoundingSphere(const Array_<Vec3P>& points) {
    Array_<int> which; 
    return calcBoundingSphere(points, which);
}

static Sphere_<P> 
calcBoundingSphere(const std::vector<Vec3P>& points) {
    return calcBoundingSphere // no copy done here
                (ArrayViewConst_<Vec3P>(points));
}

static Sphere_<P> calcBoundingSphereIndirect(const Array_<const Vec3P*>& points) {
    Array_<int> which; 
    return calcBoundingSphereIndirect(points, which);
}

static Sphere_<P> 
calcBoundingSphere(const std::vector<const Vec3P*>& points) {
    return calcBoundingSphereIndirect // no copy done here
                (ArrayViewConst_<const Vec3P*>(points));
}

static Sphere_<P> calcBoundingSphere(const Vec3P& p0, Array_<int>& which) 
{   which.clear(); which.push_back(0); 
    return Sphere_<P>(p0,0).stretchBoundary(); }

SimTK_SIMMATH_EXPORT static Sphere_<P> 
calcBoundingSphere(const Vec3P& p0, const Vec3P& p1, Array_<int>& which);

SimTK_SIMMATH_EXPORT static Sphere_<P> 
calcBoundingSphere(const Vec3P& p0, const Vec3P& p1, const Vec3P& p2, 
                   bool forceCircumsphere, Array_<int>& which);

SimTK_SIMMATH_EXPORT static Sphere_<P> 
calcBoundingSphere(const Vec3P& p0, const Vec3P& p1, const Vec3P& p2, 
                   const Vec3P& p3, bool forceCircumsphere, Array_<int>& which);

SimTK_SIMMATH_EXPORT static Sphere_<P> 
calcBoundingSphere(const Array_<Vec3P>& points, Array_<int>& which);

SimTK_SIMMATH_EXPORT static Sphere_<P> 
calcBoundingSphereIndirect(const Array_<const Vec3P*>& points, 
                           Array_<int>& which);

SimTK_SIMMATH_EXPORT static Sphere_<P> 
calcApproxBoundingSphere(const Array_<Vec3P>& points);

static Sphere_<P> 
calcApproxBoundingSphere(const std::vector<Vec3P>& points) {
    return calcApproxBoundingSphere // no copy done here
                (ArrayViewConst_<Vec3P>(points));
}

SimTK_SIMMATH_EXPORT static Sphere_<P> 
calcApproxBoundingSphereIndirect(const Array_<const Vec3P*>& points);

static Sphere_<P> 
calcApproxBoundingSphereIndirect(const std::vector<const Vec3P*>& points) {
    return calcApproxBoundingSphereIndirect // no copy done here
                (ArrayViewConst_<const Vec3P*>(points));
}
private:
Vec3P   p;
};

} // namespace SimTK

#endif // SimTK_SIMMATH_GEO_POINT_H_