ContactGeometry.h

Go to the documentation of this file.

#ifndef SimTK_SIMMATH_CONTACT_GEOMETRY_H_
#define SimTK_SIMMATH_CONTACT_GEOMETRY_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) 2008-12 Stanford University and the Authors.        *
 * Authors: Peter Eastman, Michael Sherman                                    *
 * Contributors: Ian Stavness, Andreas Scholz                                                              *
 *                                                                            *
 * 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/OrientedBoundingBox.h"
#include "simmath/internal/Geodesic.h"
#include "simmath/internal/BicubicSurface.h"

#include <cassert>

namespace SimTK {

SimTK_DEFINE_UNIQUE_INDEX_TYPE(ContactGeometryTypeId);

class ContactGeometryImpl;
class OBBTreeNodeImpl;
class OBBTree;
class Plane;



//==============================================================================
//                             CONTACT GEOMETRY
//==============================================================================
class SimTK_SIMMATH_EXPORT ContactGeometry {
public:
class HalfSpace;
class Sphere;
class Ellipsoid;
class Torus;
class SmoothHeightMap;
class Cylinder;
class Brick;
class TriangleMesh;

// TODO
class Cone;

ContactGeometry() : impl(0) {}
ContactGeometry(const ContactGeometry& src);
ContactGeometry& operator=(const ContactGeometry& src);
~ContactGeometry();

DecorativeGeometry createDecorativeGeometry() const;

Vec3 findNearestPoint(const Vec3& position, bool& inside, UnitVec3& normal) const;

Vec3 projectDownhillToNearestPoint(const Vec3& pointQ) const;

bool trackSeparationFromLine(const Vec3& pointOnLine,
                             const UnitVec3& directionOfLine,
                             const Vec3& startingGuessForClosestPoint,
                             Vec3& newClosestPointOnSurface,
                             Vec3& closestPointOnLine,
                             Real& height) const;



bool intersectsRay(const Vec3& origin, const UnitVec3& direction, 
                   Real& distance, UnitVec3& normal) const;

void getBoundingSphere(Vec3& center, Real& radius) const;

bool isSmooth() const;

void calcCurvature(const Vec3& point, Vec2& curvature, 
                   Rotation& orientation) const;

const Function& getImplicitFunction() const;

Real calcSurfaceValue(const Vec3& point) const;

UnitVec3 calcSurfaceUnitNormal(const Vec3& point) const;

Vec3 calcSurfaceGradient(const Vec3& point) const;

Mat33 calcSurfaceHessian(const Vec3& point) const;

Real calcGaussianCurvature(const Vec3&  gradient,
                           const Mat33& Hessian) const;

Real calcGaussianCurvature(const Vec3& point) const {
    return calcGaussianCurvature(calcSurfaceGradient(point),
                                 calcSurfaceHessian(point)); 
}

Real calcSurfaceCurvatureInDirection(const Vec3& point, const UnitVec3& direction) const;

void calcSurfacePrincipalCurvatures(const Vec3& point, Vec2& curvature, 
                                    Rotation& R_SP) const;

bool isConvex() const;

Vec3 calcSupportPoint(UnitVec3 direction) const;

ContactGeometryTypeId getTypeId() const;

static Vec2 evalParametricCurvature(const Vec3& P, const UnitVec3& nn,
                                    const Vec3& dPdu, const Vec3& dPdv,
                                    const Vec3& d2Pdu2, const Vec3& d2Pdv2, 
                                    const Vec3& d2Pdudv,
                                    Transform& X_EP);

static void combineParaboloids(const Rotation& R_SP1, const Vec2& k1,
                               const UnitVec3& x2, const Vec2& k2,
                               Rotation& R_SP, Vec2& k);

static void combineParaboloids(const Rotation& R_SP1, const Vec2& k1,
                               const UnitVec3& x2, const Vec2& k2,
                               Vec2& k);


void initGeodesic(const Vec3& xP, const Vec3& xQ, const Vec3& xSP,
        const GeodesicOptions& options, Geodesic& geod) const;


// XXX if xP and xQ are the exact end-points of prevGeod; then geod = prevGeod;
void continueGeodesic(const Vec3& xP, const Vec3& xQ, const Geodesic& prevGeod,
        const GeodesicOptions& options, Geodesic& geod) const;

void makeStraightLineGeodesic(const Vec3& xP, const Vec3& xQ,
        const UnitVec3& defaultDirectionIfNeeded,
        const GeodesicOptions& options, Geodesic& geod) const;


// XXX what to do if tP is not in the tangent plane at P -- project it?
void shootGeodesicInDirectionUntilLengthReached
   (const Vec3& xP, const UnitVec3& tP, const Real& terminatingLength, 
    const GeodesicOptions& options, Geodesic& geod) const;

void calcGeodesicReverseSensitivity
   (Geodesic& geodesic,
    const Vec2& initSensitivity = Vec2(0,1)) const; // j, jdot at end point


// XXX what to do if tP is not in the tangent plane at P -- project it?
// XXX what to do if we don't hit the plane
void shootGeodesicInDirectionUntilPlaneHit(const Vec3& xP, const UnitVec3& tP,
        const Plane& terminatingPlane, const GeodesicOptions& options,
        Geodesic& geod) const;


void calcGeodesic(const Vec3& xP, const Vec3& xQ,
        const Vec3& tPhint, const Vec3& tQhint, Geodesic& geod) const;

void calcGeodesicUsingOrthogonalMethod(const Vec3& xP, const Vec3& xQ,
        const Vec3& tPhint, Real lengthHint, Geodesic& geod) const;

void calcGeodesicUsingOrthogonalMethod(const Vec3& xP, const Vec3& xQ,
        Geodesic& geod) const
{
    const Vec3 r_PQ = xQ - xP;
    const Real lengthHint = r_PQ.norm();
    const UnitVec3 n = calcSurfaceUnitNormal(xP);
    // Project r_PQ into the tangent plane.
    const Vec3 t_PQ = r_PQ - (~r_PQ*n)*n;
    const Real tLength = t_PQ.norm();
    const UnitVec3 tPhint =
        tLength != 0 ? UnitVec3(t_PQ/tLength, true)
                     : n.perp(); // some arbitrary perpendicular to n
    calcGeodesicUsingOrthogonalMethod(xP, xQ, Vec3(tPhint), lengthHint, geod);           
}


Vec2 calcSplitGeodError(const Vec3& P, const Vec3& Q,
                   const UnitVec3& tP, const UnitVec3& tQ,
                   Geodesic* geod=0) const;



// XXX what to do if tP is not in the tangent plane at P -- project it?
void shootGeodesicInDirectionUntilLengthReachedAnalytical
   (const Vec3& xP, const UnitVec3& tP, const Real& terminatingLength,
    const GeodesicOptions& options, Geodesic& geod) const;


// XXX what to do if tP is not in the tangent plane at P -- project it?
// XXX what to do if we don't hit the plane
void shootGeodesicInDirectionUntilPlaneHitAnalytical(const Vec3& xP, const UnitVec3& tP,
        const Plane& terminatingPlane, const GeodesicOptions& options,
        Geodesic& geod) const;


void calcGeodesicAnalytical(const Vec3& xP, const Vec3& xQ,
        const Vec3& tPhint, const Vec3& tQhint, Geodesic& geod) const;

Vec2 calcSplitGeodErrorAnalytical(const Vec3& P, const Vec3& Q,
                   const UnitVec3& tP, const UnitVec3& tQ,
                   Geodesic* geod=0) const;

const Plane& getPlane() const;
void setPlane(const Plane& plane) const;
const Geodesic& getGeodP() const;
const Geodesic& getGeodQ() const;
const int getNumGeodesicsShot() const;
void addVizReporter(ScheduledEventReporter* reporter) const;
explicit ContactGeometry(ContactGeometryImpl* impl); 
bool isOwnerHandle() const;                          
bool isEmptyHandle() const;                          
bool hasImpl() const {return impl != 0;}             
const ContactGeometryImpl& getImpl() const {assert(impl); return *impl;}
ContactGeometryImpl& updImpl() {assert(impl); return *impl; }

protected:
ContactGeometryImpl* impl; 
};



//==============================================================================
//                                 HALF SPACE
//==============================================================================
class SimTK_SIMMATH_EXPORT ContactGeometry::HalfSpace : public ContactGeometry {
public:
HalfSpace();

UnitVec3 getNormal() const;

static bool isInstance(const ContactGeometry& geo)
{   return geo.getTypeId()==classTypeId(); }
static const HalfSpace& getAs(const ContactGeometry& geo)
{   assert(isInstance(geo)); return static_cast<const HalfSpace&>(geo); }
static HalfSpace& updAs(ContactGeometry& geo)
{   assert(isInstance(geo)); return static_cast<HalfSpace&>(geo); }

static ContactGeometryTypeId classTypeId();

class Impl; 
const Impl& getImpl() const; 
Impl& updImpl(); 
};



//==============================================================================
//                                CYLINDER
//==============================================================================
class SimTK_SIMMATH_EXPORT ContactGeometry::Cylinder : public ContactGeometry {
public:
explicit Cylinder(Real radius);
Real getRadius() const;
void setRadius(Real radius);

static bool isInstance(const ContactGeometry& geo)
{   return geo.getTypeId()==classTypeId(); }
static const Cylinder& getAs(const ContactGeometry& geo)
{   assert(isInstance(geo)); return static_cast<const Cylinder&>(geo); }
static Cylinder& updAs(ContactGeometry& geo)
{   assert(isInstance(geo)); return static_cast<Cylinder&>(geo); }

static ContactGeometryTypeId classTypeId();

class Impl; 
const Impl& getImpl() const; 
Impl& updImpl(); 
};



//==============================================================================
//                                  SPHERE
//==============================================================================
class SimTK_SIMMATH_EXPORT ContactGeometry::Sphere : public ContactGeometry {
public:
explicit Sphere(Real radius);
Real getRadius() const;
void setRadius(Real radius);

static bool isInstance(const ContactGeometry& geo)
{   return geo.getTypeId()==classTypeId(); }
static const Sphere& getAs(const ContactGeometry& geo)
{   assert(isInstance(geo)); return static_cast<const Sphere&>(geo); }
static Sphere& updAs(ContactGeometry& geo)
{   assert(isInstance(geo)); return static_cast<Sphere&>(geo); }

static ContactGeometryTypeId classTypeId();

class Impl; 
const Impl& getImpl() const; 
Impl& updImpl(); 
};



//==============================================================================
//                                  ELLIPSOID
//==============================================================================
class SimTK_SIMMATH_EXPORT ContactGeometry::Ellipsoid : public ContactGeometry {
public:
explicit Ellipsoid(const Vec3& radii);
const Vec3& getRadii() const;
void setRadii(const Vec3& radii);

const Vec3& getCurvatures() const;

UnitVec3 findUnitNormalAtPoint(const Vec3& P) const;

Vec3 findPointWithThisUnitNormal(const UnitVec3& n) const;

Vec3 findPointInSameDirection(const Vec3& Q) const;

void findParaboloidAtPoint(const Vec3& Q, Transform& X_EP, Vec2& k) const;

void findParaboloidAtPointWithNormal(const Vec3& Q, const UnitVec3& n,
    Transform& X_EP, Vec2& k) const;

static bool isInstance(const ContactGeometry& geo)
{   return geo.getTypeId()==classTypeId(); }
static const Ellipsoid& getAs(const ContactGeometry& geo)
{   assert(isInstance(geo)); return static_cast<const Ellipsoid&>(geo); }
static Ellipsoid& updAs(ContactGeometry& geo)
{   assert(isInstance(geo)); return static_cast<Ellipsoid&>(geo); }

static ContactGeometryTypeId classTypeId();

class Impl; 
const Impl& getImpl() const; 
Impl& updImpl(); 
};



//==============================================================================
//                            SMOOTH HEIGHT MAP
//==============================================================================
class SimTK_SIMMATH_EXPORT 
ContactGeometry::SmoothHeightMap : public ContactGeometry {
public:
explicit SmoothHeightMap(const BicubicSurface& surface);

const BicubicSurface& getBicubicSurface() const;

const OBBTree& getOBBTree() const;

static bool isInstance(const ContactGeometry& geo)
{   return geo.getTypeId()==classTypeId(); }
static const SmoothHeightMap& getAs(const ContactGeometry& geo)
{   assert(isInstance(geo)); return static_cast<const SmoothHeightMap&>(geo); }
static SmoothHeightMap& updAs(ContactGeometry& geo)
{   assert(isInstance(geo)); return static_cast<SmoothHeightMap&>(geo); }

static ContactGeometryTypeId classTypeId();

class Impl; 
const Impl& getImpl() const; 
Impl& updImpl(); 
};


//==============================================================================
//                                  BRICK
//==============================================================================
class SimTK_SIMMATH_EXPORT ContactGeometry::Brick : public ContactGeometry {
public:
explicit Brick(const Vec3& halfLengths);
const Vec3& getHalfLengths() const;
void setHalfLengths(const Vec3& halfLengths);

const Geo::Box& getGeoBox() const;

static bool isInstance(const ContactGeometry& geo)
{   return geo.getTypeId()==classTypeId(); }
static const Brick& getAs(const ContactGeometry& geo)
{   assert(isInstance(geo)); return static_cast<const Brick&>(geo); }
static Brick& updAs(ContactGeometry& geo)
{   assert(isInstance(geo)); return static_cast<Brick&>(geo); }

static ContactGeometryTypeId classTypeId();

class Impl; 
const Impl& getImpl() const; 
Impl& updImpl(); 
};


//==============================================================================
//                              TRIANGLE MESH
//==============================================================================
class SimTK_SIMMATH_EXPORT ContactGeometry::TriangleMesh 
:   public ContactGeometry {
public:
class OBBTreeNode;
TriangleMesh(const ArrayViewConst_<Vec3>& vertices, const ArrayViewConst_<int>& faceIndices, bool smooth=false);
explicit TriangleMesh(const PolygonalMesh& mesh, bool smooth=false);
int getNumEdges() const;
int getNumFaces() const;
int getNumVertices() const;
const Vec3& getVertexPosition(int index) const;
int getFaceEdge(int face, int edge) const;
int getFaceVertex(int face, int vertex) const;
int getEdgeFace(int edge, int face) const;
int getEdgeVertex(int edge, int vertex) const;
void findVertexEdges(int vertex, Array_<int>& edges) const;
const UnitVec3& getFaceNormal(int face) const;
Real getFaceArea(int face) const;
Vec3 findPoint(int face, const Vec2& uv) const;
Vec3 findCentroid(int face) const;
UnitVec3 findNormalAtPoint(int face, const Vec2& uv) const;
Vec3 findNearestPoint(const Vec3& position, bool& inside, UnitVec3& normal) const;
Vec3 findNearestPoint(const Vec3& position, bool& inside, int& face, Vec2& uv) const;

Vec3 findNearestPointToFace(const Vec3& position, int face, Vec2& uv) const;


bool intersectsRay(const Vec3& origin, const UnitVec3& direction, Real& distance, UnitVec3& normal) const;
bool intersectsRay(const Vec3& origin, const UnitVec3& direction, Real& distance, int& face, Vec2& uv) const;
OBBTreeNode getOBBTreeNode() const;

PolygonalMesh createPolygonalMesh() const;

static bool isInstance(const ContactGeometry& geo)
{   return geo.getTypeId()==classTypeId(); }
static const TriangleMesh& getAs(const ContactGeometry& geo)
{   assert(isInstance(geo)); return static_cast<const TriangleMesh&>(geo); }
static TriangleMesh& updAs(ContactGeometry& geo)
{   assert(isInstance(geo)); return static_cast<TriangleMesh&>(geo); }

static ContactGeometryTypeId classTypeId();

class Impl; 
const Impl& getImpl() const; 
Impl& updImpl(); 
};



//==============================================================================
//                       TRIANGLE MESH :: OBB TREE NODE
//==============================================================================
class SimTK_SIMMATH_EXPORT ContactGeometry::TriangleMesh::OBBTreeNode {
public:
OBBTreeNode(const OBBTreeNodeImpl& impl);
const OrientedBoundingBox& getBounds() const;
bool isLeafNode() const;
const OBBTreeNode getFirstChildNode() const;
const OBBTreeNode getSecondChildNode() const;
const Array_<int>& getTriangles() const;
int getNumTriangles() const;

private:
const OBBTreeNodeImpl* impl;
};

//==============================================================================
//                                TORUS
//==============================================================================
class SimTK_SIMMATH_EXPORT ContactGeometry::Torus : public ContactGeometry {
public:
Torus(Real torusRadius, Real tubeRadius);
Real getTorusRadius() const;
void setTorusRadius(Real radius);
Real getTubeRadius() const;
void setTubeRadius(Real radius);

static bool isInstance(const ContactGeometry& geo)
{   return geo.getTypeId()==classTypeId(); }
static const Torus& getAs(const ContactGeometry& geo)
{   assert(isInstance(geo)); return static_cast<const Torus&>(geo); }
static Torus& updAs(ContactGeometry& geo)
{   assert(isInstance(geo)); return static_cast<Torus&>(geo); }

static ContactGeometryTypeId classTypeId();

class Impl; 
const Impl& getImpl() const; 
Impl& updImpl(); 
};




//==============================================================================
//                     GEODESIC EVALUATOR helper classes
//==============================================================================


class Plane {
public:
    Plane() : m_normal(1,0,0), m_offset(0) { }
    Plane(const Vec3& normal, const Real& offset)
    :   m_normal(normal), m_offset(offset) { }

    Real getDistance(const Vec3& pt) const {
        return ~m_normal*pt - m_offset;
    }

    Vec3 getNormal() const {
        return m_normal;
    }

    Real getOffset() const {
        return m_offset;
    }

private:
    Vec3 m_normal;
    Real m_offset;
}; // class Plane


class GeodHitPlaneEvent : public TriggeredEventHandler {
public:
    GeodHitPlaneEvent()
    :   TriggeredEventHandler(Stage::Position) { }

    explicit GeodHitPlaneEvent(const Plane& aplane)
    :   TriggeredEventHandler(Stage::Position) {
        plane = aplane;
    }

    // event is triggered if distance of geodesic endpoint to plane is zero
    Real getValue(const State& state) const {
        if (!enabled) {
            return 1;
        }
        Vec3 endpt(&state.getQ()[0]);
        Real dist =  plane.getDistance(endpt);
//        std::cout << "dist = " << dist << std::endl;
        return dist;
    }

    // This method is called whenever this event occurs.
    void handleEvent(State& state, Real accuracy, bool& shouldTerminate) const {
        if (!enabled) {
            return;
        }

        // This should be triggered when geodesic endpoint to plane is zero.
        Vec3 endpt;
        const Vector& q = state.getQ();
        endpt[0] = q[0]; endpt[1] = q[1]; endpt[2] = q[2];
        Real dist = plane.getDistance(endpt);

//        ASSERT(std::abs(dist) < 0.01 );
        shouldTerminate = true;
//        std::cout << "hit plane!" << std::endl;
    }

    void setPlane(const Plane& aplane) const {
        plane = aplane;
    }

    const Plane& getPlane() const {
        return plane;
    }

    const void setEnabled(bool enabledFlag) {
        enabled = enabledFlag;
    }

    const bool isEnabled() {
        return enabled;
    }

private:
    mutable Plane plane;
    bool enabled;

}; // class GeodHitPlaneEvent

class PathDecorator : public DecorationGenerator {
public:
    PathDecorator(const Vector& x, const Vec3& O, const Vec3& I, const Vec3& color) :
            m_x(x), m_O(O), m_I(I), m_color(color) { }

    virtual void generateDecorations(const State& state,
            Array_<DecorativeGeometry>& geometry) {
//        m_system.realize(state, Stage::Position);

        Vec3 P, Q;
        P[0] = m_x[0]; P[1] = m_x[1]; P[2] = m_x[2];
        Q[0] = m_x[3]; Q[1] = m_x[4]; Q[2] = m_x[5];

        geometry.push_back(DecorativeSphere(Real(.05)).setColor(Black).setTransform(m_O));
        geometry.push_back(DecorativeSphere(Real(.05)).setColor(Black).setTransform(P));
        geometry.push_back(DecorativeSphere(Real(.05)).setColor(Black).setTransform(Q));
        geometry.push_back(DecorativeSphere(Real(.05)).setColor(Black).setTransform(m_I));

        geometry.push_back(DecorativeLine(m_O,P)
                .setColor(m_color)
                .setLineThickness(2));
        geometry.push_back(DecorativeLine(Q,m_I)
                .setColor(m_color)
                .setLineThickness(2));

    }

private:
    const Vector& m_x; // x = ~[P Q]
    const Vec3& m_O;
    const Vec3& m_I;
    const Vec3& m_color;
    Rotation R_plane;
    Vec3 offset;
}; // class DecorationGenerator


class PlaneDecorator : public DecorationGenerator {
public:
    PlaneDecorator(const Plane& plane, const Vec3& color) :
            m_plane(plane), m_color(color) { }

    virtual void generateDecorations(const State& state,
            Array_<DecorativeGeometry>& geometry) {
//        m_system.realize(state, Stage::Position);

        // draw plane
        R_plane.setRotationFromOneAxis(UnitVec3(m_plane.getNormal()),
                CoordinateAxis::XCoordinateAxis());
        offset = 0;
        offset[0] = m_plane.getOffset();
        geometry.push_back(
                DecorativeBrick(Vec3(Real(.01),1,1))
                .setTransform(Transform(R_plane, R_plane*offset))
                .setColor(m_color)
                .setOpacity(Real(.2)));
    }

private:
    const Plane& m_plane;
    const Vec3& m_color;
    Rotation R_plane;
    Vec3 offset;
}; // class DecorationGenerator


} // namespace SimTK

#endif // SimTK_SIMMATH_CONTACT_GEOMETRY_H_