From 21f9b39bc4432b139e33652e46770995f07e70ca Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sat, 26 Sep 2020 15:07:20 +0200 Subject: [PATCH] Small optimizations --- .../collision/shapes/TriangleShape.h | 65 ++++++++++++++++++ .../narrowphase/SAT/SATAlgorithm.cpp | 18 +++-- .../shapes/ConvexPolyhedronShape.cpp | 5 +- src/collision/shapes/TriangleShape.cpp | 66 ------------------- 4 files changed, 81 insertions(+), 73 deletions(-) diff --git a/include/reactphysics3d/collision/shapes/TriangleShape.h b/include/reactphysics3d/collision/shapes/TriangleShape.h index 823f33bd..5224f242 100644 --- a/include/reactphysics3d/collision/shapes/TriangleShape.h +++ b/include/reactphysics3d/collision/shapes/TriangleShape.h @@ -312,6 +312,71 @@ RP3D_FORCE_INLINE decimal TriangleShape::getVolume() const { return decimal(0.0); } +// Get a smooth contact normal for collision for a triangle of the mesh +/// This is used to avoid the internal edges issue that occurs when a shape is colliding with +/// several triangles of a concave mesh. If the shape collide with an edge of the triangle for instance, +/// the computed contact normal from this triangle edge is not necessarily in the direction of the surface +/// normal of the mesh at this point. The idea to solve this problem is to use the real (smooth) surface +/// normal of the mesh at this point as the contact normal. This technique is described in the chapter 5 +/// of the Game Physics Pearl book by Gino van der Bergen and Dirk Gregorius. The vertices normals of the +/// mesh are either provided by the user or precomputed if the user did not provide them. Note that we only +/// use the interpolated normal if the contact point is on an edge of the triangle. If the contact is in the +/// middle of the triangle, we return the true triangle normal. +RP3D_FORCE_INLINE Vector3 TriangleShape::computeSmoothLocalContactNormalForTriangle(const Vector3& localContactPoint) const { + + // Compute the barycentric coordinates of the point in the triangle + decimal u, v, w; + computeBarycentricCoordinatesInTriangle(mPoints[0], mPoints[1], mPoints[2], localContactPoint, u, v, w); + + // If the contact is in the middle of the triangle face (not on the edges) + if (u > MACHINE_EPSILON && v > MACHINE_EPSILON && w > MACHINE_EPSILON) { + + // We return the true triangle face normal (not the interpolated one) + return mNormal; + } + + // We compute the contact normal as the barycentric interpolation of the three vertices normals + const Vector3 interpolatedNormal = u * mVerticesNormals[0] + v * mVerticesNormals[1] + w * mVerticesNormals[2]; + + // If the interpolated normal is degenerated + if (interpolatedNormal.lengthSquare() < MACHINE_EPSILON) { + + // Return the original normal + return mNormal; + } + + return interpolatedNormal.getUnit(); +} + +// This method compute the smooth mesh contact with a triangle in case one of the two collision +// shapes is a triangle. The idea in this case is to use a smooth vertex normal of the triangle mesh +// at the contact point instead of the triangle normal to avoid the internal edge collision issue. +// This method will return the new smooth world contact +// normal of the triangle and the the local contact point on the other shape. +RP3D_FORCE_INLINE void TriangleShape::computeSmoothTriangleMeshContact(const CollisionShape* shape1, const CollisionShape* shape2, + Vector3& localContactPointShape1, Vector3& localContactPointShape2, + const Transform& shape1ToWorld, const Transform& shape2ToWorld, + decimal penetrationDepth, Vector3& outSmoothVertexNormal) { + + assert(shape1->getName() != CollisionShapeName::TRIANGLE || shape2->getName() != CollisionShapeName::TRIANGLE); + + // If one the shape is a triangle + bool isShape1Triangle = shape1->getName() == CollisionShapeName::TRIANGLE; + if (isShape1Triangle || shape2->getName() == CollisionShapeName::TRIANGLE) { + + const TriangleShape* triangleShape = isShape1Triangle ? static_cast(shape1): + static_cast(shape2); + + // Compute the smooth triangle mesh contact normal and recompute the local contact point on the other shape + triangleShape->computeSmoothMeshContact(isShape1Triangle ? localContactPointShape1 : localContactPointShape2, + isShape1Triangle ? shape1ToWorld : shape2ToWorld, + isShape1Triangle ? shape2ToWorld.getInverse() : shape1ToWorld.getInverse(), + penetrationDepth, isShape1Triangle, + isShape1Triangle ? localContactPointShape2 : localContactPointShape1, + outSmoothVertexNormal); + } +} + } #endif diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 3e1daa4c..aa7797bb 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -55,8 +55,7 @@ SATAlgorithm::SATAlgorithm(bool clipWithPreviousAxisIfStillColliding, MemoryAllo } // Test collision between a sphere and a convex mesh -bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, - uint batchStartIndex, uint batchNbItems) const { +bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems) const { bool isCollisionFound = false; @@ -895,11 +894,20 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene RP3D_PROFILE("SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints", mProfiler); - const ConvexPolyhedronShape* referencePolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1 : polyhedron2; - const ConvexPolyhedronShape* incidentPolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2 : polyhedron1; + const ConvexPolyhedronShape* referencePolyhedron; + const ConvexPolyhedronShape* incidentPolyhedron; const Transform& referenceToIncidentTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1ToPolyhedron2 : polyhedron2ToPolyhedron1; const Transform& incidentToReferenceTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2ToPolyhedron1 : polyhedron1ToPolyhedron2; + if (isMinPenetrationFaceNormalPolyhedron1) { + referencePolyhedron = polyhedron1; + incidentPolyhedron = polyhedron2; + } + else { + referencePolyhedron = polyhedron2; + incidentPolyhedron = polyhedron1; + } + const Vector3 axisReferenceSpace = referencePolyhedron->getFaceNormal(minFaceIndex); const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace; @@ -972,7 +980,7 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene // the maximal penetration depth of any contact point for this separating axis decimal penetrationDepth = (referenceFaceVertex - clipPolygonVertices[i]).dot(axisReferenceSpace); - // If the clip point is bellow the reference face + // If the clip point is below the reference face if (penetrationDepth > decimal(0.0)) { contactPointsFound = true; diff --git a/src/collision/shapes/ConvexPolyhedronShape.cpp b/src/collision/shapes/ConvexPolyhedronShape.cpp index bb907a2b..9ec5f1a8 100644 --- a/src/collision/shapes/ConvexPolyhedronShape.cpp +++ b/src/collision/shapes/ConvexPolyhedronShape.cpp @@ -45,10 +45,11 @@ uint ConvexPolyhedronShape::findMostAntiParallelFace(const Vector3& direction) c uint mostAntiParallelFace = 0; // For each face of the polyhedron - for (uint i=0; i < getNbFaces(); i++) { + uint32 nbFaces = getNbFaces(); + for (uint32 i=0; i < nbFaces; i++) { // Get the face normal - decimal dotProduct = getFaceNormal(i).dot(direction); + const decimal dotProduct = getFaceNormal(i).dot(direction); if (dotProduct < minDotProduct) { minDotProduct = dotProduct; mostAntiParallelFace = i; diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp index 2d1b1380..091d2507 100644 --- a/src/collision/shapes/TriangleShape.cpp +++ b/src/collision/shapes/TriangleShape.cpp @@ -122,36 +122,6 @@ TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNor mId = shapeId; } -// This method compute the smooth mesh contact with a triangle in case one of the two collision -// shapes is a triangle. The idea in this case is to use a smooth vertex normal of the triangle mesh -// at the contact point instead of the triangle normal to avoid the internal edge collision issue. -// This method will return the new smooth world contact -// normal of the triangle and the the local contact point on the other shape. -void TriangleShape::computeSmoothTriangleMeshContact(const CollisionShape* shape1, const CollisionShape* shape2, - Vector3& localContactPointShape1, Vector3& localContactPointShape2, - const Transform& shape1ToWorld, const Transform& shape2ToWorld, - decimal penetrationDepth, Vector3& outSmoothVertexNormal) { - - assert(shape1->getName() != CollisionShapeName::TRIANGLE || shape2->getName() != CollisionShapeName::TRIANGLE); - - // If one the shape is a triangle - bool isShape1Triangle = shape1->getName() == CollisionShapeName::TRIANGLE; - if (isShape1Triangle || shape2->getName() == CollisionShapeName::TRIANGLE) { - - const TriangleShape* triangleShape = isShape1Triangle ? static_cast(shape1): - static_cast(shape2); - - // Compute the smooth triangle mesh contact normal and recompute the local contact point on the other shape - triangleShape->computeSmoothMeshContact(isShape1Triangle ? localContactPointShape1 : localContactPointShape2, - isShape1Triangle ? shape1ToWorld : shape2ToWorld, - isShape1Triangle ? shape2ToWorld.getInverse() : shape1ToWorld.getInverse(), - penetrationDepth, isShape1Triangle, - isShape1Triangle ? localContactPointShape2 : localContactPointShape1, - outSmoothVertexNormal); - } -} - - // This method implements the technique described in Game Physics Pearl book // by Gino van der Bergen and Dirk Gregorius to get smooth triangle mesh collision. The idea is // to replace the contact normal of the triangle shape with the precomputed normal of the triangle @@ -186,42 +156,6 @@ void TriangleShape::computeSmoothMeshContact(Vector3 localContactPointTriangle, outNewLocalContactPointOtherShape.setAllValues(otherShapePoint.x, otherShapePoint.y, otherShapePoint.z); } -// Get a smooth contact normal for collision for a triangle of the mesh -/// This is used to avoid the internal edges issue that occurs when a shape is colliding with -/// several triangles of a concave mesh. If the shape collide with an edge of the triangle for instance, -/// the computed contact normal from this triangle edge is not necessarily in the direction of the surface -/// normal of the mesh at this point. The idea to solve this problem is to use the real (smooth) surface -/// normal of the mesh at this point as the contact normal. This technique is described in the chapter 5 -/// of the Game Physics Pearl book by Gino van der Bergen and Dirk Gregorius. The vertices normals of the -/// mesh are either provided by the user or precomputed if the user did not provide them. Note that we only -/// use the interpolated normal if the contact point is on an edge of the triangle. If the contact is in the -/// middle of the triangle, we return the true triangle normal. -Vector3 TriangleShape::computeSmoothLocalContactNormalForTriangle(const Vector3& localContactPoint) const { - - // Compute the barycentric coordinates of the point in the triangle - decimal u, v, w; - computeBarycentricCoordinatesInTriangle(mPoints[0], mPoints[1], mPoints[2], localContactPoint, u, v, w); - - // If the contact is in the middle of the triangle face (not on the edges) - if (u > MACHINE_EPSILON && v > MACHINE_EPSILON && w > MACHINE_EPSILON) { - - // We return the true triangle face normal (not the interpolated one) - return mNormal; - } - - // We compute the contact normal as the barycentric interpolation of the three vertices normals - Vector3 interpolatedNormal = u * mVerticesNormals[0] + v * mVerticesNormals[1] + w * mVerticesNormals[2]; - - // If the interpolated normal is degenerated - if (interpolatedNormal.lengthSquare() < MACHINE_EPSILON) { - - // Return the original normal - return mNormal; - } - - return interpolatedNormal.getUnit(); -} - // Update the AABB of a body using its collision shape /** * @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape