ContactGeometryImpl.h

Go to the documentation of this file.
00001 #ifndef SimTK_SIMBODY_CONTACT_GEOMETRY_IMPL_H_
00002 #define SimTK_SIMBODY_CONTACT_GEOMETRY_IMPL_H_
00003 
00004 /* -------------------------------------------------------------------------- *
00005  *                      SimTK Core: SimTK Simbody(tm)                         *
00006  * -------------------------------------------------------------------------- *
00007  * This is part of the SimTK Core 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.               *
00011  *                                                                            *
00012  * Portions copyright (c) 2008 Stanford University and the Authors.           *
00013  * Authors: Peter Eastman                                                     *
00014  * Contributors:                                                              *
00015  *                                                                            *
00016  * Permission is hereby granted, free of charge, to any person obtaining a    *
00017  * copy of this software and associated documentation files (the "Software"), *
00018  * to deal in the Software without restriction, including without limitation  *
00019  * the rights to use, copy, modify, merge, publish, distribute, sublicense,   *
00020  * and/or sell copies of the Software, and to permit persons to whom the      *
00021  * Software is furnished to do so, subject to the following conditions:       *
00022  *                                                                            *
00023  * The above copyright notice and this permission notice shall be included in *
00024  * all copies or substantial portions of the Software.                        *
00025  *                                                                            *
00026  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
00027  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,   *
00028  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL    *
00029  * THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,    *
00030  * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR      *
00031  * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE  *
00032  * USE OR OTHER DEALINGS IN THE SOFTWARE.                                     *
00033  * -------------------------------------------------------------------------- */
00034 
00035 
00036 #include "simbody/internal/ContactGeometry.h"
00037 
00038 namespace SimTK {
00039 
00040 class SimTK_SIMBODY_EXPORT ContactGeometryImpl {
00041 public:
00042     ContactGeometryImpl(const std::string& type);
00043     virtual ~ContactGeometryImpl() {
00044         clearMyHandle();
00045     }
00046     const std::string& getType() const {
00047         return type;
00048     }
00049     int getTypeIndex() const {
00050         return typeIndex;
00051     }
00052     static int getIndexForType(std::string type);
00053     virtual ContactGeometryImpl* clone() const = 0;
00054     virtual Vec3 findNearestPoint(const Vec3& position, bool& inside, UnitVec3& normal) const = 0;
00055     virtual bool intersectsRay(const Vec3& origin, const UnitVec3& direction, Real& distance, UnitVec3& normal) const = 0;
00056     virtual void getBoundingSphere(Vec3& center, Real& radius) const = 0;
00057     ContactGeometry* getMyHandle() {
00058         return myHandle;
00059     }
00060     void setMyHandle(ContactGeometry& h) {
00061         myHandle = &h;
00062     }
00063     void clearMyHandle() {
00064         myHandle = 0;
00065     }
00066 protected:
00067     ContactGeometry* myHandle;
00068     const std::string& type;
00069     int typeIndex;
00070 };
00071 
00072 class ContactGeometry::HalfSpaceImpl : public ContactGeometryImpl {
00073 public:
00074     HalfSpaceImpl() : ContactGeometryImpl(Type()) {
00075     }
00076     ContactGeometryImpl* clone() const {
00077         return new HalfSpaceImpl();
00078     }
00079     static const std::string& Type() {
00080         static std::string type = "halfspace";
00081         return type;
00082     }
00083     Vec3 findNearestPoint(const Vec3& position, bool& inside, UnitVec3& normal) const;
00084     bool intersectsRay(const Vec3& origin, const UnitVec3& direction, Real& distance, UnitVec3& normal) const;
00085     void getBoundingSphere(Vec3& center, Real& radius) const;
00086 };
00087 
00088 class ContactGeometry::SphereImpl : public ContactGeometryImpl {
00089 public:
00090     SphereImpl(Real radius) : ContactGeometryImpl(Type()), radius(radius) {
00091     }
00092     ContactGeometryImpl* clone() const {
00093         return new SphereImpl(radius);
00094     }
00095     Real getRadius() const {
00096         return radius;
00097     }
00098     void setRadius(Real r) {
00099         radius = r;
00100     }
00101     static const std::string& Type() {
00102         static std::string type = "sphere";
00103         return type;
00104     }
00105     Vec3 findNearestPoint(const Vec3& position, bool& inside, UnitVec3& normal) const;
00106     bool intersectsRay(const Vec3& origin, const UnitVec3& direction, Real& distance, UnitVec3& normal) const;
00107     void getBoundingSphere(Vec3& center, Real& radius) const;
00108 private:
00109     Real radius;
00110 };
00111 
00112 class OBBTreeNodeImpl {
00113 public:
00114     OBBTreeNodeImpl() : child1(NULL), child2(NULL) {
00115     }
00116     OBBTreeNodeImpl(const OBBTreeNodeImpl& copy);
00117     ~OBBTreeNodeImpl();
00118     OrientedBoundingBox bounds;
00119     OBBTreeNodeImpl* child1;
00120     OBBTreeNodeImpl* child2;
00121     std::vector<int> triangles;
00122     int numTriangles;
00123     Vec3 findNearestPoint(const ContactGeometry::TriangleMeshImpl& mesh, const Vec3& position, Real cutoff2, Real& distance2, int& face, Vec2& uv) const;
00124     bool intersectsRay(const ContactGeometry::TriangleMeshImpl& mesh, const Vec3& origin, const UnitVec3& direction, Real& distance, int& face, Vec2& uv) const;
00125 };
00126 
00127 class ContactGeometry::TriangleMeshImpl : public ContactGeometryImpl {
00128 public:
00129     class Edge;
00130     class Face;
00131     class Vertex;
00132     TriangleMeshImpl(const std::vector<Vec3>& vertexPositions, const std::vector<int>& faceIndices, bool smooth);
00133     TriangleMeshImpl(const PolygonalMesh& mesh, bool smooth);
00134     ContactGeometryImpl* clone() const {
00135         return new TriangleMeshImpl(*this);
00136     }
00137     static const std::string& Type() {
00138         static std::string type = "triangle mesh";
00139         return type;
00140     }
00141     UnitVec3 findNormalAtPoint(int face, const Vec2& uv) const;
00142     Vec3 findNearestPoint(const Vec3& position, bool& inside, UnitVec3& normal) const;
00143     Vec3 findNearestPoint(const Vec3& position, bool& inside, int& face, Vec2& uv) const;
00144     Vec3 findNearestPointToFace(const Vec3& position, int face, Vec2& uv) const;
00145     bool intersectsRay(const Vec3& origin, const UnitVec3& direction, Real& distance, UnitVec3& normal) const;
00146     bool intersectsRay(const Vec3& origin, const UnitVec3& direction, Real& distance, int& face, Vec2& uv) const;
00147     void getBoundingSphere(Vec3& center, Real& radius) const;
00148 private:
00149     void init(const std::vector<Vec3>& vertexPositions, const std::vector<int>& faceIndices);
00150     void createObbTree(OBBTreeNodeImpl& node, const std::vector<int>& faceIndices);
00151     void splitObbAxis(const std::vector<int>& parentIndices, std::vector<int>& child1Indices, std::vector<int>& child2Indices, int axis);
00152     void findBoundingSphere(Vec3* point[], int p, int b, Vec3& center, Real& radius);
00153     friend class ContactGeometry::TriangleMesh;
00154     friend class OBBTreeNodeImpl;
00155     std::vector<Edge> edges;
00156     std::vector<Face> faces;
00157     std::vector<Vertex> vertices;
00158     Vec3 boundingSphereCenter;
00159     Real boundingSphereRadius;
00160     OBBTreeNodeImpl obb;
00161     bool smooth;
00162 };
00163 
00164 class ContactGeometry::TriangleMeshImpl::Edge {
00165 public:
00166     Edge(int vert1, int vert2, int face1, int face2) {
00167         vertices[0] = vert1;
00168         vertices[1] = vert2;
00169         faces[0] = face1;
00170         faces[1] = face2;
00171     }
00172     int vertices[2];
00173     int faces[2];
00174 };
00175 
00176 class ContactGeometry::TriangleMeshImpl::Face {
00177 public:
00178     Face(int vert1, int vert2, int vert3, const Vec3& normal, Real area) : normal(normal), area(area) {
00179         vertices[0] = vert1;
00180         vertices[1] = vert2;
00181         vertices[2] = vert3;
00182     }
00183     int vertices[3];
00184     int edges[3];
00185     UnitVec3 normal;
00186     Real area;
00187 };
00188 
00189 class ContactGeometry::TriangleMeshImpl::Vertex {
00190 public:
00191     Vertex(Vec3 pos) : pos(pos), firstEdge(-1) {
00192     }
00193     Vec3 pos;
00194     UnitVec3 normal;
00195     int firstEdge;
00196 };
00197 
00198 } // namespace SimTK
00199 
00200 #endif // SimTK_SIMBODY_CONTACT_GEOMETRY_IMPL_H_

Generated by  doxygen 1.6.2