ContactGeometry.h

Go to the documentation of this file.

#ifndef SimTK_SIMBODY_CONTACT_GEOMETRY_H_
#define SimTK_SIMBODY_CONTACT_GEOMETRY_H_

/* -------------------------------------------------------------------------- *
 *                      SimTK Core: SimTK Simbody(tm)                         *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK Core 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.               *
 *                                                                            *
 * Portions copyright (c) 2008-11 Stanford University and the Authors.        *
 * Authors: Peter Eastman                                                     *
 * Contributors: Michael Sherman                                              *
 *                                                                            *
 * Permission is hereby granted, free of charge, to any person obtaining a    *
 * copy of this software and associated documentation files (the "Software"), *
 * to deal in the Software without restriction, including without limitation  *
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,   *
 * and/or sell copies of the Software, and to permit persons to whom the      *
 * Software is furnished to do so, subject to the following conditions:       *
 *                                                                            *
 * The above copyright notice and this permission notice shall be included in *
 * all copies or substantial portions of the Software.                        *
 *                                                                            *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,   *
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL    *
 * THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,    *
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR      *
 * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE  *
 * USE OR OTHER DEALINGS IN THE SOFTWARE.                                     *
 * -------------------------------------------------------------------------- */

#include "SimTKcommon.h"
#include "simbody/internal/common.h"
#include "simbody/internal/OrientedBoundingBox.h"

#include <cassert>

namespace SimTK {

SimTK_DEFINE_UNIQUE_INDEX_TYPE(ContactGeometryTypeId);

class ContactGeometryImpl;
class OBBTreeNodeImpl;



//==============================================================================
//                             CONTACT GEOMETRY
//==============================================================================
class SimTK_SIMBODY_EXPORT ContactGeometry {
public:
class HalfSpace;
class Sphere;
class Ellipsoid;
class TriangleMesh;
class HalfSpaceImpl;
class SphereImpl;
class EllipsoidImpl;
class TriangleMeshImpl;

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

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

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

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

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

const std::string& getType() const;
int getTypeIndex() const;

ContactGeometryTypeId getTypeId() 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_SIMBODY_EXPORT ContactGeometry::HalfSpace : public ContactGeometry {
public:
HalfSpace();

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();
};



//==============================================================================
//                                  SPHERE
//==============================================================================
class SimTK_SIMBODY_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();

const SphereImpl& getImpl() const;
SphereImpl& updImpl();
};



//==============================================================================
//                                  ELLIPSOID
//==============================================================================
class SimTK_SIMBODY_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();

const EllipsoidImpl& getImpl() const;
EllipsoidImpl& updImpl();
};



//==============================================================================
//                              TRIANGLE MESH
//==============================================================================
class SimTK_SIMBODY_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;
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();

const TriangleMeshImpl& getImpl() const;
TriangleMeshImpl& updImpl();
};



//==============================================================================
//                       TRIANGLE MESH :: OBB TREE NODE
//==============================================================================
class SimTK_SIMBODY_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;
};

} // namespace SimTK

#endif // SimTK_SIMBODY_CONTACT_GEOMETRY_H_