00001 #ifndef SimTK_SIMBODY_CONTACT_GEOMETRY_IMPL_H_
00002 #define SimTK_SIMBODY_CONTACT_GEOMETRY_IMPL_H_
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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 }
00199
00200 #endif // SimTK_SIMBODY_CONTACT_GEOMETRY_IMPL_H_