diff --git a/src/collision/HalfEdgeStructure.h b/src/collision/HalfEdgeStructure.h index 01e305c5..6b4e3aa1 100644 --- a/src/collision/HalfEdgeStructure.h +++ b/src/collision/HalfEdgeStructure.h @@ -53,6 +53,9 @@ class HalfEdgeStructure { uint edgeIndex; // Index of an half-edge of the face std::vector faceVertices; // Index of the vertices of the face + /// Constructor + Face() {} + /// Constructor Face(std::vector vertices) : faceVertices(vertices) {} }; @@ -155,7 +158,7 @@ inline HalfEdgeStructure::Edge HalfEdgeStructure::getHalfEdge(uint index) const return mEdges[index]; } -// Retunr a given vertex +// Return a given vertex inline HalfEdgeStructure::Vertex HalfEdgeStructure::getVertex(uint index) const { assert(index < mVertices.size()); return mVertices[index]; diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 767aff8a..3bd3e796 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -46,6 +46,8 @@ const decimal SATAlgorithm::SAME_SEPARATING_AXIS_BIAS = decimal(0.001); // Test collision between a sphere and a convex mesh bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { + PROFILE("SATAlgorithm::testCollisionSphereVsConvexPolyhedron()"); + bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE; assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || @@ -77,32 +79,37 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo(); - // If the last frame collision info is valid and was also using SAT algorithm - if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) { + // If the shapes are not triangles (no temporal coherence for triangle collision because we do not store previous + // frame collision data per triangle) + if (polyhedron->getType() != CollisionShapeType::TRIANGLE) { - // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating - // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If - // the shapes are still separated along this axis, we directly exit with no collision. + // If the last frame collision info is valid and was also using SAT algorithm + if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) { - // Compute the penetration depth of the shapes along the face normal direction - decimal penetrationDepth = computePolyhedronFaceVsSpherePenetrationDepth(lastFrameInfo.satMinAxisFaceIndex, polyhedron, - sphere, sphereCenter); + // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating + // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If + // the shapes are still separated along this axis, we directly exit with no collision. - // If the previous axis is a separating axis - if (penetrationDepth <= decimal(0.0)) { + // Compute the penetration depth of the shapes along the face normal direction + decimal penetrationDepth = computePolyhedronFaceVsSpherePenetrationDepth(lastFrameInfo.satMinAxisFaceIndex, polyhedron, + sphere, sphereCenter); - // Return no collision - return false; - } + // If the previous axis is a separating axis + if (penetrationDepth <= decimal(0.0)) { - // The two shapes are overlapping as in the previous frame and on the same axis, therefore - // we will skip the entire SAT algorithm because the minimum separating axis did not change - isTemporalCoherenceValid = lastFrameInfo.wasColliding; + // Return no collision + return false; + } - if (isTemporalCoherenceValid) { + // The two shapes are overlapping as in the previous frame and on the same axis, therefore + // we will skip the entire SAT algorithm because the minimum separating axis did not change + isTemporalCoherenceValid = lastFrameInfo.wasColliding; - minPenetrationDepth = penetrationDepth; - minFaceIndex = lastFrameInfo.satMinAxisFaceIndex; + if (isTemporalCoherenceValid) { + + minPenetrationDepth = penetrationDepth; + minFaceIndex = lastFrameInfo.satMinAxisFaceIndex; + } } } @@ -170,6 +177,8 @@ decimal SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth(uint faceInd // Test collision between a capsule and a convex mesh bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { + PROFILE("SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron()"); + bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE; assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || @@ -206,78 +215,83 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo(); - // If the last frame collision info is valid and was also using SAT algorithm - if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) { + // If the shapes are not triangles (no temporal coherence for triangle collision because we do not store previous + // frame collision data per triangle) + if (polyhedron->getType() != CollisionShapeType::TRIANGLE) { - // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating - // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If - // the shapes are still separated along this axis, we directly exit with no collision. + // If the last frame collision info is valid and was also using SAT algorithm + if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) { - // If the previous minimum separation axis was a face normal of the polyhedron - if (lastFrameInfo.satIsAxisFacePolyhedron1) { + // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating + // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If + // the shapes are still separated along this axis, we directly exit with no collision. - Vector3 outFaceNormalCapsuleSpace; + // If the previous minimum separation axis was a face normal of the polyhedron + if (lastFrameInfo.satIsAxisFacePolyhedron1) { - // Compute the penetration depth along the polyhedron face normal direction - const decimal penetrationDepth = computePolyhedronFaceVsCapsulePenetrationDepth(lastFrameInfo.satMinAxisFaceIndex, polyhedron, - capsuleShape, polyhedronToCapsuleTransform, - outFaceNormalCapsuleSpace); + Vector3 outFaceNormalCapsuleSpace; - // If the previous axis is a separating axis - if (penetrationDepth <= decimal(0.0)) { + // Compute the penetration depth along the polyhedron face normal direction + const decimal penetrationDepth = computePolyhedronFaceVsCapsulePenetrationDepth(lastFrameInfo.satMinAxisFaceIndex, polyhedron, + capsuleShape, polyhedronToCapsuleTransform, + outFaceNormalCapsuleSpace); - // Return no collision - return false; + // If the previous axis is a separating axis + if (penetrationDepth <= decimal(0.0)) { + + // Return no collision + return false; + } + + // The two shapes are overlapping as in the previous frame and on the same axis, therefore + // we will skip the entire SAT algorithm because the minimum separating axis did not change + isTemporalCoherenceValid = lastFrameInfo.wasColliding; + + if (isTemporalCoherenceValid) { + + minPenetrationDepth = penetrationDepth; + minFaceIndex = lastFrameInfo.satMinAxisFaceIndex; + isMinPenetrationFaceNormal = true; + separatingAxisCapsuleSpace = outFaceNormalCapsuleSpace; + } } + else { // If the previous minimum separation axis the cross product of the capsule inner segment and an edge of the polyhedron - // The two shapes are overlapping as in the previous frame and on the same axis, therefore - // we will skip the entire SAT algorithm because the minimum separating axis did not change - isTemporalCoherenceValid = lastFrameInfo.wasColliding; + // Get an edge from the polyhedron (convert it into the capsule local-space) + HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(lastFrameInfo.satMinEdge1Index); + const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex); + const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex); + const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1); - if (isTemporalCoherenceValid) { + Vector3 outAxis; - minPenetrationDepth = penetrationDepth; - minFaceIndex = lastFrameInfo.satMinAxisFaceIndex; - isMinPenetrationFaceNormal = true; - separatingAxisCapsuleSpace = outFaceNormalCapsuleSpace; - } - } - else { // If the previous minimum separation axis the cross product of the capsule inner segment and an edge of the polyhedron + // Compute the penetration depth along this axis + const decimal penetrationDepth = computeEdgeVsCapsuleInnerSegmentPenetrationDepth(polyhedron, capsuleShape, + capsuleSegmentAxis, edgeVertex1, + edgeDirectionCapsuleSpace, + polyhedronToCapsuleTransform, + outAxis); - // Get an edge from the polyhedron (convert it into the capsule local-space) - HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(lastFrameInfo.satMinEdge1Index); - const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex); - const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex); - const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1); + // If the previous axis is a separating axis + if (penetrationDepth <= decimal(0.0)) { - Vector3 outAxis; + // Return no collision + return false; + } - // Compute the penetration depth along this axis - const decimal penetrationDepth = computeEdgeVsCapsuleInnerSegmentPenetrationDepth(polyhedron, capsuleShape, - capsuleSegmentAxis, edgeVertex1, - edgeDirectionCapsuleSpace, - polyhedronToCapsuleTransform, - outAxis); + // The two shapes are overlapping as in the previous frame and on the same axis, therefore + // we will skip the entire SAT algorithm because the minimum separating axis did not change + isTemporalCoherenceValid = lastFrameInfo.wasColliding; - // If the previous axis is a separating axis - if (penetrationDepth <= decimal(0.0)) { + if (isTemporalCoherenceValid) { - // Return no collision - return false; - } - - // The two shapes are overlapping as in the previous frame and on the same axis, therefore - // we will skip the entire SAT algorithm because the minimum separating axis did not change - isTemporalCoherenceValid = lastFrameInfo.wasColliding; - - if (isTemporalCoherenceValid) { - - minPenetrationDepth = penetrationDepth; - minEdgeIndex = lastFrameInfo.satMinEdge1Index; - isMinPenetrationFaceNormal = false; - separatingAxisCapsuleSpace = outAxis; - separatingPolyhedronEdgeVertex1 = edgeVertex1; - separatingPolyhedronEdgeVertex2 = edgeVertex2; + minPenetrationDepth = penetrationDepth; + minEdgeIndex = lastFrameInfo.satMinEdge1Index; + isMinPenetrationFaceNormal = false; + separatingAxisCapsuleSpace = outAxis; + separatingPolyhedronEdgeVertex1 = edgeVertex1; + separatingPolyhedronEdgeVertex2 = edgeVertex2; + } } } } @@ -527,6 +541,8 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { + PROFILE("SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron()"); + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); diff --git a/src/collision/shapes/ConvexPolyhedronShape.h b/src/collision/shapes/ConvexPolyhedronShape.h index f6d2a34b..6c1de4d5 100644 --- a/src/collision/shapes/ConvexPolyhedronShape.h +++ b/src/collision/shapes/ConvexPolyhedronShape.h @@ -42,10 +42,6 @@ class ConvexPolyhedronShape : public ConvexShape { protected : - // -------------------- Attributes -------------------- // - - // -------------------- Methods -------------------- // - public : // -------------------- Methods -------------------- // diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp index 4767435b..9c0deb38 100644 --- a/src/collision/shapes/TriangleShape.cpp +++ b/src/collision/shapes/TriangleShape.cpp @@ -40,10 +40,16 @@ using namespace reactphysics3d; * @param margin The collision margin (in meters) around the collision shape */ TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, decimal margin) - : ConvexShape(CollisionShapeType::TRIANGLE, margin) { + : ConvexPolyhedronShape(margin) { + mPoints[0] = point1; mPoints[1] = point2; mPoints[2] = point3; + + // Compute the triangle normal + mNormal = (point3 - point1).cross(point2 - point1); + mNormal.normalize(); + mRaycastTestType = TriangleRaycastSide::FRONT; } @@ -120,3 +126,51 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape return true; } +// Return a given half-edge of the polyhedron +HalfEdgeStructure::Edge TriangleShape::getHalfEdge(uint edgeIndex) const { + assert(edgeIndex < getNbHalfEdges()); + + HalfEdgeStructure::Edge edge; + + switch(edgeIndex) { + case 0: + edge.vertexIndex = 0; + edge.twinEdgeIndex = 1; + edge.faceIndex = 0; + edge.nextEdgeIndex = 2; + break; + case 1: + edge.vertexIndex = 1; + edge.twinEdgeIndex = 0; + edge.faceIndex = 1; + edge.nextEdgeIndex = 5; + break; + case 2: + edge.vertexIndex = 1; + edge.twinEdgeIndex = 3; + edge.faceIndex = 0; + edge.nextEdgeIndex = 4; + break; + case 3: + edge.vertexIndex = 2; + edge.twinEdgeIndex = 2; + edge.faceIndex = 1; + edge.nextEdgeIndex = 1; + break; + case 4: + edge.vertexIndex = 2; + edge.twinEdgeIndex = 5; + edge.faceIndex = 0; + edge.nextEdgeIndex = 0; + break; + case 5: + edge.vertexIndex = 0; + edge.twinEdgeIndex = 4; + edge.faceIndex = 1; + edge.nextEdgeIndex = 3; + break; + } + + return edge; + +} diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h index 29732f8f..e25086b8 100644 --- a/src/collision/shapes/TriangleShape.h +++ b/src/collision/shapes/TriangleShape.h @@ -28,7 +28,7 @@ // Libraries #include "mathematics/mathematics.h" -#include "ConvexShape.h" +#include "ConvexPolyhedronShape.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -51,7 +51,7 @@ enum class TriangleRaycastSide { * This class represents a triangle collision shape that is centered * at the origin and defined three points. */ -class TriangleShape : public ConvexShape { +class TriangleShape : public ConvexPolyhedronShape { protected: @@ -60,6 +60,9 @@ class TriangleShape : public ConvexShape { /// Three points of the triangle Vector3 mPoints[3]; + /// Normal of the triangle + Vector3 mNormal; + /// Raycast test type for the triangle (front, back, front-back) TriangleRaycastSide mRaycastTestType; @@ -113,11 +116,32 @@ class TriangleShape : public ConvexShape { // Set the raycast test type (front, back, front-back) void setRaycastTestType(TriangleRaycastSide testType); - /// Return the coordinates of a given vertex of the triangle - Vector3 getVertex(int index) const; + /// Return the number of faces of the polyhedron + virtual uint getNbFaces() const override; - /// Return true if the collision shape is a polyhedron - virtual bool isPolyhedron() 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 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 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 centroid of the polyhedron + virtual Vector3 getCentroid() const override; // ---------- Friendship ---------- // @@ -199,6 +223,74 @@ inline bool TriangleShape::testPointInside(const Vector3& localPoint, ProxyShape return false; } +// Return the number of faces of the polyhedron +inline uint TriangleShape::getNbFaces() const { + return 2; +} + +// Return a given face of the polyhedron +inline HalfEdgeStructure::Face TriangleShape::getFace(uint faceIndex) const { + assert(faceIndex < 2); + + HalfEdgeStructure::Face face; + + if (faceIndex == 0) { + face.faceVertices.push_back(0); + face.faceVertices.push_back(1); + face.faceVertices.push_back(2); + face.edgeIndex = 0; + + } + else { + face.faceVertices.push_back(0); + face.faceVertices.push_back(2); + face.faceVertices.push_back(1); + face.edgeIndex = 1; + } + + return face; +} + +// Return the number of vertices of the polyhedron +inline uint TriangleShape::getNbVertices() const { + return 3; +} + +// Return a given vertex of the polyhedron +inline HalfEdgeStructure::Vertex TriangleShape::getVertex(uint vertexIndex) const { + assert(vertexIndex < 3); + + HalfEdgeStructure::Vertex vertex(vertexIndex); + switch (vertexIndex) { + case 0: vertex.edgeIndex = 0; break; + case 1: vertex.edgeIndex = 2; break; + case 2: vertex.edgeIndex = 4; break; + } + return vertex; +} + +// Return the position of a given vertex +inline Vector3 TriangleShape::getVertexPosition(uint vertexIndex) const { + assert(vertexIndex < 3); + return mPoints[vertexIndex]; +} + +// Return the normal vector of a given face of the polyhedron +inline Vector3 TriangleShape::getFaceNormal(uint faceIndex) const { + assert(faceIndex < 2); + return faceIndex == 0 ? mNormal : -mNormal; +} + +// Return the centroid of the box +inline Vector3 TriangleShape::getCentroid() const { + return (mPoints[0] + mPoints[1] + mPoints[2]) / decimal(3.0); +} + +// Return the number of half-edges of the polyhedron +inline uint TriangleShape::getNbHalfEdges() const { + return 6; +} + // Return the raycast test type (front, back, front-back) inline TriangleRaycastSide TriangleShape::getRaycastTestType() const { return mRaycastTestType; @@ -212,20 +304,6 @@ inline void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) { mRaycastTestType = testType; } -// Return the coordinates of a given vertex of the triangle -/** - * @param index Index (0 to 2) of a vertex of the triangle - */ -inline Vector3 TriangleShape::getVertex(int index) const { - assert(index >= 0 && index < 3); - return mPoints[index]; -} - -// Return true if the collision shape is a polyhedron -inline bool TriangleShape::isPolyhedron() const { - return true; -} - } #endif