diff --git a/src/collision/PolyhedronMesh.cpp b/src/collision/PolyhedronMesh.cpp index df43a47a..50ef5bab 100644 --- a/src/collision/PolyhedronMesh.cpp +++ b/src/collision/PolyhedronMesh.cpp @@ -36,6 +36,17 @@ PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray) { // Create the half-edge structure of the mesh createHalfEdgeStructure(); + + // Create the face normals array + mFacesNormals = new Vector3[mHalfEdgeStructure.getNbFaces()]; + + // Compute the faces normals + computeFacesNormals(); +} + +// Destructor +PolyhedronMesh::~PolyhedronMesh() { + delete[] mFacesNormals; } // Create the half-edge structure of the mesh @@ -59,6 +70,8 @@ void PolyhedronMesh::createHalfEdgeStructure() { faceVertices.push_back(mPolygonVertexArray->getVertexIndexInFace(f, v)); } + assert(faceVertices.size() >= 3); + // Addd the face into the half-edge structure mHalfEdgeStructure.addFace(faceVertices); } @@ -97,3 +110,17 @@ Vector3 PolyhedronMesh::getVertex(uint index) const { return vertex; } + +void PolyhedronMesh::computeFacesNormals() { + + // For each face + for (uint f=0; f < mHalfEdgeStructure.getNbFaces(); f++) { + HalfEdgeStructure::Face face = mHalfEdgeStructure.getFace(f); + + assert(face.faceVertices.size() >= 3); + + const Vector3 vec1 = getVertex(face.faceVertices[1]) - getVertex(face.faceVertices[0]); + const Vector3 vec2 = getVertex(face.faceVertices[2]) - getVertex(face.faceVertices[0]); + mFacesNormals[f] = vec1.cross(vec2); + } +} diff --git a/src/collision/PolyhedronMesh.h b/src/collision/PolyhedronMesh.h index f842cae1..940b2db0 100644 --- a/src/collision/PolyhedronMesh.h +++ b/src/collision/PolyhedronMesh.h @@ -52,11 +52,17 @@ class PolyhedronMesh { /// Half-edge structure of the mesh HalfEdgeStructure mHalfEdgeStructure; + /// Array with the face normals + Vector3* mFacesNormals; + // -------------------- Methods -------------------- // /// Create the half-edge structure of the mesh void createHalfEdgeStructure(); + /// Compute the faces normals + void computeFacesNormals(); + public: // -------------------- Methods -------------------- // @@ -65,7 +71,7 @@ class PolyhedronMesh { PolyhedronMesh(PolygonVertexArray* polygonVertexArray); /// Destructor - ~PolyhedronMesh() = default; + ~PolyhedronMesh(); /// Return the number of vertices uint getNbVertices() const; @@ -73,6 +79,9 @@ class PolyhedronMesh { /// Return a vertex Vector3 getVertex(uint index) const; + /// Return a face normal + Vector3 getFaceNormal(uint faceIndex) const; + /// Return the half-edge structure of the mesh const HalfEdgeStructure& getHalfEdgeStructure() const; }; @@ -82,6 +91,12 @@ inline uint PolyhedronMesh::getNbVertices() const { return mHalfEdgeStructure.getNbVertices(); } +// Return a face normal +inline Vector3 PolyhedronMesh::getFaceNormal(uint faceIndex) const { + assert(faceIndex < mHalfEdgeStructure.getNbFaces()); + return mFacesNormals[faceIndex]; +} + // Return the half-edge structure of the mesh inline const HalfEdgeStructure& PolyhedronMesh::getHalfEdgeStructure() const { return mHalfEdgeStructure; diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index ba5dfa1a..f00210c2 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/src/collision/shapes/BoxShape.cpp @@ -42,6 +42,37 @@ BoxShape::BoxShape(const Vector3& extent, decimal margin) assert(extent.x > decimal(0.0) && extent.x > margin); assert(extent.y > decimal(0.0) && extent.y > margin); assert(extent.z > decimal(0.0) && extent.z > margin); + + // Vertices + mHalfEdgeStructure.addVertex(0); + mHalfEdgeStructure.addVertex(1); + mHalfEdgeStructure.addVertex(2); + mHalfEdgeStructure.addVertex(3); + mHalfEdgeStructure.addVertex(4); + mHalfEdgeStructure.addVertex(5); + mHalfEdgeStructure.addVertex(6); + mHalfEdgeStructure.addVertex(7); + + // Faces + std::vector face0; + face0.push_back(0); face0.push_back(1); face0.push_back(2); face0.push_back(3); + std::vector face1; + face0.push_back(1); face0.push_back(5); face0.push_back(6); face0.push_back(2); + std::vector face2; + face0.push_back(4); face0.push_back(7); face0.push_back(6); face0.push_back(5); + std::vector face3; + face0.push_back(4); face0.push_back(0); face0.push_back(3); face0.push_back(7); + std::vector face4; + face0.push_back(4); face0.push_back(5); face0.push_back(1); face0.push_back(0); + std::vector face5; + face0.push_back(2); face0.push_back(6); face0.push_back(7); face0.push_back(3); + + mHalfEdgeStructure.addFace(face0); + mHalfEdgeStructure.addFace(face1); + mHalfEdgeStructure.addFace(face2); + mHalfEdgeStructure.addFace(face3); + mHalfEdgeStructure.addFace(face4); + mHalfEdgeStructure.addFace(face5); } // Return the local inertia tensor of the collision shape diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h index c3d5caf2..d78bda25 100644 --- a/src/collision/shapes/BoxShape.h +++ b/src/collision/shapes/BoxShape.h @@ -59,6 +59,9 @@ class BoxShape : public ConvexPolyhedron { /// Extent sizes of the box in the x, y and z direction Vector3 mExtent; + /// Half-edge structure of the polyhedron + HalfEdgeStructure mHalfEdgeStructure; + // -------------------- Methods -------------------- // /// Return a local support point in a given direction without the object margin @@ -99,11 +102,32 @@ class BoxShape : public ConvexPolyhedron { /// Return the local bounds of the shape in x, y and z directions virtual void getLocalBounds(Vector3& min, Vector3& max) const override; - /// Return true if the collision shape is a polyhedron - virtual bool isPolyhedron() const override; - /// Return the local inertia tensor of the collision shape virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override; + + /// Return the number of faces of the polyhedron + virtual uint getNbFaces() const override; + + /// Return a given face of the polyhedron + virtual HalfEdgeStructure::Face getFace(uint faceIndex) const override; + + /// Return the number of vertices of the polyhedron + virtual uint getNbVertices() const override; + + /// Return a given vertex of the polyhedron + virtual HalfEdgeStructure::Vertex getVertex(uint vertexIndex) const override; + + /// Return the number of half-edges of the polyhedron + virtual uint getNbHalfEdges() const override; + + /// Return a given half-edge of the polyhedron + virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const override; + + /// Return the position of a given vertex + virtual Vector3 getVertexPosition(uint vertexIndex) const override; + + /// Return the normal vector of a given face of the polyhedron + virtual Vector3 getFaceNormal(uint faceIndex) const override; }; // Return the extents of the box @@ -137,11 +161,6 @@ inline void BoxShape::getLocalBounds(Vector3& min, Vector3& max) const { min = -max; } -// Return true if the collision shape is a polyhedron -inline bool BoxShape::isPolyhedron() const { - return true; -} - // Return the number of bytes used by the collision shape inline size_t BoxShape::getSizeInBytes() const { return sizeof(BoxShape); @@ -163,6 +182,71 @@ inline bool BoxShape::testPointInside(const Vector3& localPoint, ProxyShape* pro localPoint.z < mExtent[2] && localPoint.z > -mExtent[2]); } +// Return the number of faces of the polyhedron +inline uint BoxShape::getNbFaces() const { + return 6; +} + +// Return a given face of the polyhedron +inline HalfEdgeStructure::Face BoxShape::getFace(uint faceIndex) const { + assert(faceIndex < mHalfEdgeStructure.getNbFaces()); + return mHalfEdgeStructure.getFace(faceIndex); +} + +// Return the number of vertices of the polyhedron +inline uint BoxShape::getNbVertices() const { + return 8; +} + +// Return a given vertex of the polyhedron +inline HalfEdgeStructure::Vertex BoxShape::getVertex(uint vertexIndex) const { + assert(vertexIndex < getNbVertices()); + return mHalfEdgeStructure.getVertex(vertexIndex); +} + +// Return the position of a given vertex +inline Vector3 BoxShape::getVertexPosition(uint vertexIndex) const { + assert(vertexIndex < getNbVertices()); + + Vector3 extent = getExtent(); + + switch(vertexIndex) { + case 0: return Vector3(-extent.x, -extent.y, extent.z); + case 1: return Vector3(extent.x, -extent.y, extent.z); + case 2: return Vector3(extent.x, extent.y, extent.z); + case 3: return Vector3(-extent.x, extent.y, extent.z); + case 4: return Vector3(-extent.x, -extent.y, -extent.z); + case 5: return Vector3(extent.x, -extent.y, -extent.z); + case 6: return Vector3(extent.x, extent.y, -extent.z); + case 7: return Vector3(-extent.x, extent.y, -extent.z); + } +} + +// Return the normal vector of a given face of the polyhedron +inline Vector3 BoxShape::getFaceNormal(uint faceIndex) const { + assert(faceIndex < getNbFaces()); + + switch(faceIndex) { + case 0: return Vector3(0, 0, 1); + case 1: return Vector3(1, 0, 0); + case 2: return Vector3(0, 0, -1); + case 3: return Vector3(-1, 0, 0); + case 4: return Vector3(0, -1, 0); + case 5: return Vector3(0, 1, 0); + } +} + +// Return the number of half-edges of the polyhedron +inline uint BoxShape::getNbHalfEdges() const { + return 24; +} + +// Return a given half-edge of the polyhedron +inline HalfEdgeStructure::Edge BoxShape::getHalfEdge(uint edgeIndex) const { + assert(edgeIndex < getNbHalfEdges()); + return mHalfEdgeStructure.getHalfEdge(edgeIndex); +} + } #endif diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index c33fd5bd..cc88d38d 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/src/collision/shapes/ConvexMeshShape.h @@ -119,8 +119,29 @@ class ConvexMeshShape : public ConvexPolyhedron { /// Return the local inertia tensor of the collision shape. virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override; - /// Return true if the collision shape is a polyhedron - virtual bool isPolyhedron() const override; + /// Return the number of faces of the polyhedron + virtual uint getNbFaces() const override; + + /// Return a given face of the polyhedron + virtual HalfEdgeStructure::Face getFace(uint faceIndex) const override; + + /// Return the number of vertices of the polyhedron + virtual uint getNbVertices() const override; + + /// Return a given vertex of the polyhedron + virtual HalfEdgeStructure::Vertex getVertex(uint vertexIndex) const override; + + /// Return the number of half-edges of the polyhedron + virtual uint getNbHalfEdges() const override; + + /// Return a given half-edge of the polyhedron + virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const override; + + /// Return the position of a given vertex + virtual Vector3 getVertexPosition(uint vertexIndex) const override; + + /// Return the normal vector of a given face of the polyhedron + virtual Vector3 getFaceNormal(uint faceIndex) const override; }; /// Set the scaling vector of the collision shape @@ -134,11 +155,6 @@ inline size_t ConvexMeshShape::getSizeInBytes() const { return sizeof(ConvexMeshShape); } -// Return true if the collision shape is a polyhedron -inline bool ConvexMeshShape::isPolyhedron() const { - return true; -} - // Return the local bounds of the shape in x, y and z directions /** * @param min The minimum bounds of the shape in local-space coordinates @@ -178,6 +194,51 @@ inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint, mNarrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape); } +// Return the number of faces of the polyhedron +inline uint ConvexMeshShape::getNbFaces() const { + return mPolyhedronMesh->getHalfEdgeStructure().getNbFaces(); +} + +// Return a given face of the polyhedron +inline HalfEdgeStructure::Face ConvexMeshShape::getFace(uint faceIndex) const { + assert(faceIndex < getNbFaces()); + return mPolyhedronMesh->getHalfEdgeStructure().getFace(faceIndex); +} + +// Return the number of vertices of the polyhedron +inline uint ConvexMeshShape::getNbVertices() const { + return mPolyhedronMesh->getHalfEdgeStructure().getNbVertices(); +} + +// Return a given vertex of the polyhedron +inline HalfEdgeStructure::Vertex ConvexMeshShape::getVertex(uint vertexIndex) const { + assert(vertexIndex < getNbVertices()); + return mPolyhedronMesh->getHalfEdgeStructure().getVertex(vertexIndex); +} + +// Return the number of half-edges of the polyhedron +inline uint ConvexMeshShape::getNbHalfEdges() const { + return mPolyhedronMesh->getHalfEdgeStructure().getNbHalfEdges(); +} + +// Return a given half-edge of the polyhedron +inline HalfEdgeStructure::Edge ConvexMeshShape::getHalfEdge(uint edgeIndex) const { + assert(edgeIndex < getNbHalfEdges()); + return mPolyhedronMesh->getHalfEdgeStructure().getHalfEdge(edgeIndex); +} + +// Return the position of a given vertex +inline Vector3 ConvexMeshShape::getVertexPosition(uint vertexIndex) const { + assert(vertexIndex < getNbVertices()); + return mPolyhedronMesh->getVertex(vertexIndex); +} + +// Return the normal vector of a given face of the polyhedron +inline Vector3 ConvexMeshShape::getFaceNormal(uint faceIndex) const { + assert(faceIndex < getNbFaces()); + return mPolyhedronMesh->getFaceNormal(faceIndex); +} + } #endif diff --git a/src/collision/shapes/ConvexPolyhedron.h b/src/collision/shapes/ConvexPolyhedron.h index 27bbc0b5..2a544077 100644 --- a/src/collision/shapes/ConvexPolyhedron.h +++ b/src/collision/shapes/ConvexPolyhedron.h @@ -28,6 +28,7 @@ // Libraries #include "ConvexShape.h" +#include "collision/HalfEdgeStructure.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -60,8 +61,40 @@ class ConvexPolyhedron : public ConvexShape { /// Deleted assignment operator ConvexPolyhedron& operator=(const ConvexPolyhedron& shape) = delete; + + /// Return the number of faces of the polyhedron + virtual uint getNbFaces() const=0; + + /// Return a given face of the polyhedron + virtual HalfEdgeStructure::Face getFace(uint faceIndex) const=0; + + /// Return the number of vertices of the polyhedron + virtual uint getNbVertices() const=0; + + /// Return a given vertex of the polyhedron + virtual HalfEdgeStructure::Vertex getVertex(uint vertexIndex) const=0; + + /// Return the position of a given vertex + virtual Vector3 getVertexPosition(uint vertexIndex) const=0; + + /// Return the normal vector of a given face of the polyhedron + virtual Vector3 getFaceNormal(uint faceIndex) const=0; + + /// Return the number of half-edges of the polyhedron + virtual uint getNbHalfEdges() const=0; + + /// Return a given half-edge of the polyhedron + virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const=0; + + /// Return true if the collision shape is a polyhedron + virtual bool isPolyhedron() const override; }; +// Return true if the collision shape is a polyhedron +inline bool ConvexPolyhedron::isPolyhedron() const { + return true; +} + } #endif diff --git a/src/collision/shapes/ConvexShape.h b/src/collision/shapes/ConvexShape.h index 4b12cae4..9180fefe 100644 --- a/src/collision/shapes/ConvexShape.h +++ b/src/collision/shapes/ConvexShape.h @@ -84,7 +84,7 @@ class ConvexShape : public CollisionShape { friend class EPAAlgorithm; }; -/// Return true if the collision shape is convex, false if it is concave +// Return true if the collision shape is convex, false if it is concave inline bool ConvexShape::isConvex() const { return true; }