Geo_Box.h

Go to the documentation of this file.

#ifndef SimTK_SIMMATH_GEO_BOX_H_
#define SimTK_SIMMATH_GEO_BOX_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 BOX
//==============================================================================
template <class P>
class Geo::Box_ {
typedef P               RealP;
typedef Vec<3,P>        Vec3P;
typedef Mat<3,3,P>      Mat33P;
typedef Rotation_<P>    RotationP;
typedef Transform_<P>    TransformP;

public:
Box_() {}
Box_(const Vec3P& halfLengths) {setHalfLengths(halfLengths);} 

Box_& setHalfLengths(const Vec3P& halfLengths) {
    SimTK_ERRCHK3(halfLengths >= 0, "Geo::Box_::setHalfLengths()",
        "Half lengths must be nonnegative; got %g,%g,%g.",
        (double)halfLengths[0],(double)halfLengths[1],(double)halfLengths[2]);
    h = halfLengths; 
    sortEdges();
    return *this; 
}

Box_& addToHalfLengths(const Vec3P& incr) {
    h += incr; 
    SimTK_ERRCHK3(h >= 0, "Geo::Box_::addToHalfLengths()",
        "Half lengths must be nonnegative but were %g,%g,%g after change.",
        (double)h[0],(double)h[1],(double)h[2]);
    sortEdges();
    return *this; 
}

const Vec3P& getHalfLengths() const {return h;}

RealP getOrderedHalfLength(int i) const {
    SimTK_INDEXCHECK(i, 3, "Geo::Box_::getOrderedHalfLength()");
    return h[order[i]];
}

CoordinateAxis getOrderedAxis(int i) const {
    SimTK_INDEXCHECK(i, 3, "Geo::Box_::getOrderedAxis()");
    return CoordinateAxis(order[i]);
}

RealP findVolume() const {return 8*h[0]*h[1]*h[2];}
RealP findArea() const {return 8*(h[0]*h[1] + h[0]*h[2] + h[1]*h[2]);}

bool containsPoint(const Vec3P& pt) const {
    const Vec3P absPt = pt.abs(); // reflect to first quadrant
    return absPt <= h;
}

RealP findDistanceSqrToPoint(const Vec3P& pt) const {
    const Vec3P absPt = pt.abs(); // reflect to first quadrant
    RealP d2 = 0;
    if (absPt[0] > h[0]) d2 += square(absPt[0]-h[0]);
    if (absPt[1] > h[1]) d2 += square(absPt[1]-h[1]);
    if (absPt[2] > h[2]) d2 += square(absPt[2]-h[2]);
    return d2;
}

RealP findDistanceSqrToSphere(const Geo::Sphere_<P>& sphere) const {
    const Vec3P absCtr = sphere.getCenter().abs(); // reflect to first quadrant
    const Vec3P grow = h + sphere.getRadius(); // 3 flops
    RealP d2 = 0;
    if (absCtr[0] > grow[0]) d2 += square(absCtr[0]-grow[0]);
    if (absCtr[1] > grow[1]) d2 += square(absCtr[1]-grow[1]);
    if (absCtr[2] > grow[2]) d2 += square(absCtr[2]-grow[2]);
    return d2;
}

RealP findDistanceSqrToAlignedBox(const Geo::AlignedBox_<P>& aab) const {
    const Vec3P absCtr = aab.getCenter().abs(); // reflect to first quadrant
    const Vec3P grow = h + aab.getHalfLengths();
    RealP d2 = 0;
    if (absCtr[0] > grow[0]) d2 += square(absCtr[0]-grow[0]);
    if (absCtr[1] > grow[1]) d2 += square(absCtr[1]-grow[1]);
    if (absCtr[2] > grow[2]) d2 += square(absCtr[2]-grow[2]);
    return d2;
}

bool intersectsSphere(const Geo::Sphere_<P>& sphere) const {
    const Vec3P absCtr = sphere.getCenter().abs(); // reflect to first quadrant
    const RealP r = sphere.getRadius();
    if (absCtr[0] > h[0]+r) return false;
    if (absCtr[1] > h[1]+r) return false;
    if (absCtr[2] > h[2]+r) return false;
    return true;
}

bool intersectsAlignedBox(const Geo::AlignedBox_<P>& aab) const {
    const Vec3P absCtr = aab.getCenter().abs(); // reflect to first quadrant
    const Vec3P& aabh = aab.getHalfLengths();
    if (absCtr[0] > h[0]+aabh[0]) return false;
    if (absCtr[1] > h[1]+aabh[1]) return false;
    if (absCtr[2] > h[2]+aabh[2]) return false;
    return true;
}

SimTK_SIMMATH_EXPORT bool 
intersectsOrientedBox(const Geo::OrientedBox_<P>& ob) const;

SimTK_SIMMATH_EXPORT bool 
mayIntersectOrientedBox(const Geo::OrientedBox_<P>& ob) const;


private:
// Call this whenever an edge length changes. Each axis will appear once.
void sortEdges() {
    CoordinateAxis shortest = XAxis, longest = ZAxis; 
    if (h[YAxis] < h[shortest]) shortest=YAxis;
    if (h[ZAxis] < h[shortest]) shortest=ZAxis;
    if (h[XAxis] > h[longest])  longest=XAxis;
    if (h[YAxis] > h[longest])  longest=YAxis;
    order[0] = shortest; order[2] = longest; 
    order[1] = shortest.getThirdAxis(longest); // not shortest or longest
}

int intersectsOrientedBoxHelper(const OrientedBox_<P>& O,
                                Mat33P&  absR_BO, 
                                Vec3P&   absP_BO) const;

Vec3P           h;         // half-dimensions of the box
unsigned char   order[3];  // 0,1,2 reordered short to long
};



//==============================================================================
//                              GEO ALIGNED BOX
//==============================================================================
template <class P>
class SimTK_SIMMATH_EXPORT Geo::AlignedBox_ {
typedef P               RealP;
typedef Vec<3,RealP>    Vec3P;

public:
AlignedBox_() {}
AlignedBox_(const Vec3P& center, const Geo::Box_<P>& box) 
:   center(center), box(box) {} 
AlignedBox_(const Vec3P& center, const Vec3P& halfLengths) 
:   center(center), box(halfLengths) {} 

AlignedBox_& setCenter(const Vec3P& center) 
{   this->center=center; return *this; }

AlignedBox_& setHalfLengths(const Vec3P& halfLengths) 
{   box.setHalfLengths(halfLengths); return *this; }

const Vec3P& getCenter() const {return center;}
Vec3P& updCenter() {return center;}
const Vec3P& getHalfLengths() const {return box.getHalfLengths();}
// no updHalfLengths()
const Box_<P>& getBox() const {return box;}
Box_<P>& updBox() {return box;}

bool containsPoint(const Vec3P& pt_F) const
{   return box.containsPoint(pt_F - center); } // shift to box frame B

AlignedBox_& stretchBoundary() {
    const RealP tol    = Geo::getDefaultTol<P>();
    const RealP maxdim = max(getCenter().abs());
    const RealP maxrad = max(getHalfLengths());
    const RealP scale  = std::max(maxdim, maxrad);
    const RealP incr   = std::max(scale*Geo::getEps<P>(), tol);
    box.addToHalfLengths(Vec3P(incr));
    return *this; 
}

private:
Vec3P           center;
Geo::Box_<P>    box;
};


//==============================================================================
//                              GEO ORIENTED BOX
//==============================================================================
template <class P>
class SimTK_SIMMATH_EXPORT Geo::OrientedBox_ {
typedef P               RealP;
typedef Vec<3,P>        Vec3P;
typedef Rotation_<P>    RotationP;
typedef Transform_<P>   TransformP;

public:
OrientedBox_() {}
OrientedBox_(const TransformP& X_FB, const Geo::Box_<P>& box) 
:   X_FB(X_FB), box(box) {} 
OrientedBox_(const TransformP& X_FB, const Vec3P& halfLengths) 
:   X_FB(X_FB), box(halfLengths) {} 


OrientedBox_& setTransform(const TransformP& newX_FB) 
{   X_FB=newX_FB; return *this; }

OrientedBox_& setHalfLengths(const Vec3P& halfLengths) 
{   box.setHalfLengths(halfLengths); return *this; }

const Vec3P& getCenter() const {return X_FB.p();}
Vec3P& updCenter() {return X_FB.updP();}
const RotationP& getOrientation() const {return X_FB.R();}
RotationP& updOrientation() {return X_FB.updR();}
const TransformP& getTransform() const {return X_FB;}
TransformP& updTransform() {return X_FB;}
const Vec3P& getHalfLengths() const {return box.getHalfLengths();}
// no updHalfLengths()
const Box_<P>& getBox() const {return box;}
Box_<P>& updBox() {return box;}


bool containsPoint(const Vec3P& pt_F) const
{   return box.containsPoint(~X_FB*pt_F); } // shift to box frame B

OrientedBox_& stretchBoundary() {
    const RealP tol    = Geo::getDefaultTol<P>();
    const RealP maxdim = max(getCenter().abs());
    const RealP maxrad = max(getHalfLengths());
    const RealP scale  = std::max(maxdim, maxrad);
    const RealP incr   = std::max(scale*Geo::getEps<P>(), tol);
    box.addToHalfLengths(Vec3P(incr));
    return *this; 
}

private:
TransformP      X_FB;
Geo::Box_<P>    box;
};


} // namespace SimTK

#endif // SimTK_SIMMATH_GEO_BOX_H_