diff --git a/include/reactphysics3d/collision/ContactPointInfo.h b/include/reactphysics3d/collision/ContactPointInfo.h index b75b9b97..0b296b5b 100644 --- a/include/reactphysics3d/collision/ContactPointInfo.h +++ b/include/reactphysics3d/collision/ContactPointInfo.h @@ -52,14 +52,15 @@ struct ContactPointInfo { /// Normalized normal vector of the collision contact in world space Vector3 normal; - /// Penetration depth of the contact - decimal penetrationDepth; - /// Contact point of body 1 in local space of body 1 Vector3 localPoint1; /// Contact point of body 2 in local space of body 2 Vector3 localPoint2; + + /// Penetration depth of the contact + decimal penetrationDepth; + }; } diff --git a/include/reactphysics3d/containers/Array.h b/include/reactphysics3d/containers/Array.h index f79ac094..74c9a507 100755 --- a/include/reactphysics3d/containers/Array.h +++ b/include/reactphysics3d/containers/Array.h @@ -292,7 +292,7 @@ class Array { reserve(mCapacity == 0 ? 1 : mCapacity * 2); } - // Use the copy-constructor to construct the element + // Use the constructor to construct the element new (reinterpret_cast(mBuffer + mSize)) T(element); mSize++; diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index bfc32951..835ff13a 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -520,7 +520,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn // was a face normal of polyhedron 1 if (lastFrameCollisionInfo->satIsAxisFacePolyhedron1) { - decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, + const decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, lastFrameCollisionInfo->satMinAxisFaceIndex); // If the previous axis was a separating axis and is still a separating axis in this frame @@ -912,7 +912,7 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace; // Compute the world normal - Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform.getOrientation() * axisReferenceSpace : + const Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform.getOrientation() * axisReferenceSpace : -(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * axisReferenceSpace); // Get the reference face @@ -936,43 +936,42 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene } // For each edge of the reference we use it to clip the incident face polygon using Sutherland-Hodgman algorithm - uint32 currentEdgeIndex = referenceFace.edgeIndex; - uint32 firstEdgeIndex = currentEdgeIndex; - Vector3 planeNormal; - Vector3 planePoint; + uint32 firstEdgeIndex = referenceFace.edgeIndex; bool areVertices1Input = false; uint32 nbOutputVertices; + uint currentEdgeIndex; + + // Get the adjacent edge + const HalfEdgeStructure::Edge* currentEdge = &(referencePolyhedron->getHalfEdge(firstEdgeIndex)); + Vector3 edgeV1 = referencePolyhedron->getVertexPosition(currentEdge->vertexIndex); + do { // Switch the input/output arrays of vertices areVertices1Input = !areVertices1Input; // Get the adjacent edge - const HalfEdgeStructure::Edge& edge = referencePolyhedron->getHalfEdge(currentEdgeIndex); - - // Get the twin edge - const HalfEdgeStructure::Edge& twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex); + const HalfEdgeStructure::Edge* nextEdge = &(referencePolyhedron->getHalfEdge(currentEdge->nextEdgeIndex)); // Compute the edge vertices and edge direction - Vector3 edgeV1 = referencePolyhedron->getVertexPosition(edge.vertexIndex); - Vector3 edgeV2 = referencePolyhedron->getVertexPosition(twinEdge.vertexIndex); - Vector3 edgeDirection = edgeV2 - edgeV1; + const Vector3 edgeV2 = referencePolyhedron->getVertexPosition(nextEdge->vertexIndex); + const Vector3 edgeDirection = edgeV2 - edgeV1; // Compute the normal of the clipping plane for this edge // The clipping plane is perpendicular to the edge direction and the reference face normal - Vector3 clipPlaneNormal = axisReferenceSpace.cross(edgeDirection); - - planeNormal = clipPlaneNormal; - planePoint = edgeV1; + const Vector3 planeNormal = axisReferenceSpace.cross(edgeDirection); assert(areVertices1Input && verticesTemp1.size() > 0 || !areVertices1Input); assert(!areVertices1Input && verticesTemp2.size() > 0 || areVertices1Input); // Clip the incident face with one adjacent plane (corresponding to one edge) of the reference face - clipPolygonWithPlane(areVertices1Input ? verticesTemp1 : verticesTemp2, planePoint, planeNormal, areVertices1Input ? verticesTemp2 : verticesTemp1); + clipPolygonWithPlane(areVertices1Input ? verticesTemp1 : verticesTemp2, edgeV1, planeNormal, areVertices1Input ? verticesTemp2 : verticesTemp1); + + currentEdgeIndex = currentEdge->nextEdgeIndex; // Go to the next adjacent edge of the reference face - currentEdgeIndex = edge.nextEdgeIndex; + currentEdge = nextEdge; + edgeV1 = edgeV2; // Clear the input array of vertices before the next loop if (areVertices1Input) { diff --git a/src/systems/CollisionDetectionSystem.cpp b/src/systems/CollisionDetectionSystem.cpp index 00e4d78c..b09c338a 100644 --- a/src/systems/CollisionDetectionSystem.cpp +++ b/src/systems/CollisionDetectionSystem.cpp @@ -832,7 +832,7 @@ void CollisionDetectionSystem::createContacts() { // Process the contact pairs in the order defined by the islands such that the contact manifolds and // contact points of a given island are packed together in the array of manifolds and contact points - uint32 nbContactPairsToProcess = mWorld->mProcessContactPairsOrderIslands.size(); + const uint32 nbContactPairsToProcess = mWorld->mProcessContactPairsOrderIslands.size(); for (uint p=0; p < nbContactPairsToProcess; p++) { uint32 contactPairIndex = mWorld->mProcessContactPairsOrderIslands[p]; @@ -988,6 +988,8 @@ void CollisionDetectionSystem::createSnapshotContacts(Array& contac // Initialize the current contacts with the contacts from the previous frame (for warmstarting) void CollisionDetectionSystem::initContactsWithPreviousOnes() { + const decimal persistentContactDistThresholdSqr = mWorld->mConfig.persistentContactDistanceThreshold * mWorld->mConfig.persistentContactDistanceThreshold; + // For each contact pair of the current frame const uint32 nbCurrentContactPairs = mCurrentContactPairs->size(); for (uint32 i=0; i < nbCurrentContactPairs; i++) { @@ -1052,16 +1054,18 @@ void CollisionDetectionSystem::initContactsWithPreviousOnes() { assert(c < mCurrentContactPoints->size()); ContactPoint& currentContactPoint = (*mCurrentContactPoints)[c]; + const Vector3& currentContactPointLocalShape1 = currentContactPoint.getLocalPointOnShape1(); + // Find a similar contact point among the contact points from the previous frame (for warmstarting) const uint previousContactPointsIndex = previousContactPair.contactPointsIndex; const uint previousNbContactPoints = previousContactPair.nbToTalContactPoints; for (uint p=previousContactPointsIndex; p < previousContactPointsIndex + previousNbContactPoints; p++) { - ContactPoint& previousContactPoint = (*mPreviousContactPoints)[p]; + const ContactPoint& previousContactPoint = (*mPreviousContactPoints)[p]; // If the previous contact point is very close to th current one - const decimal distSquare = (currentContactPoint.getLocalPointOnShape1() - previousContactPoint.getLocalPointOnShape1()).lengthSquare(); - if (distSquare <= mWorld->mConfig.persistentContactDistanceThreshold * mWorld->mConfig.persistentContactDistanceThreshold) { + const decimal distSquare = (currentContactPointLocalShape1 - previousContactPoint.getLocalPointOnShape1()).lengthSquare(); + if (distSquare <= persistentContactDistThresholdSqr) { // Transfer data from the previous contact point to the current one currentContactPoint.setPenetrationImpulse(previousContactPoint.getPenetrationImpulse());