diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index a2c3344d..c0b4d4e9 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -273,7 +273,7 @@ void CollisionDetection::computeNarrowPhase() { // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, true)) { + if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, true, mSingleFrameAllocator)) { // Add the contact points as a potential contact manifold into the pair currentNarrowPhaseInfo->addContactPointsAsPotentialContactManifold(); @@ -593,7 +593,7 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false); + isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false, mPoolAllocator); } } @@ -688,7 +688,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false); + isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false, mPoolAllocator); } } @@ -767,7 +767,7 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) { + if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mPoolAllocator)) { // Add the contact points as a potential contact manifold into the pair narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); @@ -859,7 +859,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) { + if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mPoolAllocator)) { // Add the contact points as a potential contact manifold into the pair narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); @@ -943,7 +943,7 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) { + if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mPoolAllocator)) { // Add the contact points as a potential contact manifold into the pair narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index fc33c00f..d728a95f 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -150,7 +150,6 @@ class CollisionDetection { /// Process the potential contacts where one collion is a concave shape void processSmoothMeshContacts(OverlappingPair* pair); - public : diff --git a/src/collision/HalfEdgeStructure.h b/src/collision/HalfEdgeStructure.h index 6b4e3aa1..ec4fff35 100644 --- a/src/collision/HalfEdgeStructure.h +++ b/src/collision/HalfEdgeStructure.h @@ -106,13 +106,13 @@ class HalfEdgeStructure { uint getNbVertices() const; /// Return a given face - Face getFace(uint index) const; + const Face& getFace(uint index) const; /// Return a given edge - Edge getHalfEdge(uint index) const; + const Edge& getHalfEdge(uint index) const; /// Return a given vertex - Vertex getVertex(uint index) const; + const Vertex& getVertex(uint index) const; }; @@ -133,33 +133,33 @@ inline void HalfEdgeStructure::addFace(std::vector faceVertices) { // Return the number of faces inline uint HalfEdgeStructure::getNbFaces() const { - return mFaces.size(); + return static_cast(mFaces.size()); } // Return the number of edges inline uint HalfEdgeStructure::getNbHalfEdges() const { - return mEdges.size(); + return static_cast(mEdges.size()); } // Return the number of vertices inline uint HalfEdgeStructure::getNbVertices() const { - return mVertices.size(); + return static_cast(mVertices.size()); } // Return a given face -inline HalfEdgeStructure::Face HalfEdgeStructure::getFace(uint index) const { +inline const HalfEdgeStructure::Face& HalfEdgeStructure::getFace(uint index) const { assert(index < mFaces.size()); return mFaces[index]; } // Return a given edge -inline HalfEdgeStructure::Edge HalfEdgeStructure::getHalfEdge(uint index) const { +inline const HalfEdgeStructure::Edge& HalfEdgeStructure::getHalfEdge(uint index) const { assert(index < mEdges.size()); return mEdges[index]; } // Return a given vertex -inline HalfEdgeStructure::Vertex HalfEdgeStructure::getVertex(uint index) const { +inline const HalfEdgeStructure::Vertex& HalfEdgeStructure::getVertex(uint index) const { assert(index < mVertices.size()); return mVertices[index]; } diff --git a/src/collision/PolyhedronMesh.cpp b/src/collision/PolyhedronMesh.cpp index b4f82fad..ec8b661d 100644 --- a/src/collision/PolyhedronMesh.cpp +++ b/src/collision/PolyhedronMesh.cpp @@ -123,7 +123,7 @@ void PolyhedronMesh::computeFacesNormals() { // For each face for (uint f=0; f < mHalfEdgeStructure.getNbFaces(); f++) { - HalfEdgeStructure::Face face = mHalfEdgeStructure.getFace(f); + const HalfEdgeStructure::Face& face = mHalfEdgeStructure.getFace(f); assert(face.faceVertices.size() >= 3); diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp index b15cb186..bf9eefc2 100755 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp @@ -33,7 +33,8 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between two capsules // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { +bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, + Allocator& memoryAllocator) { assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE); assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h index 0b19338f..df9a8854 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h @@ -61,7 +61,7 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between two capsules - virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override; }; } diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index 300ba5f2..f943312b 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -37,11 +37,12 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between a capsule and a polyhedron // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { +bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, + Allocator& memoryAllocator) { // First, we run the GJK algorithm GJKAlgorithm gjkAlgorithm; - SATAlgorithm satAlgorithm; + SATAlgorithm satAlgorithm(memoryAllocator); #ifdef IS_PROFILING_ACTIVE @@ -85,9 +86,6 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh // For each face of the polyhedron for (uint f = 0; f < polyhedron->getNbFaces(); f++) { - // Get the face - HalfEdgeStructure::Face face = polyhedron->getFace(f); - const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform; const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform; diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h index c0dd46d5..d7f96d63 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h @@ -61,7 +61,7 @@ class CapsuleVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm { CapsuleVsConvexPolyhedronAlgorithm& operator=(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between a capsule and a polyhedron - virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override; }; } diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp index 507f6043..5cf10b17 100644 --- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp @@ -34,10 +34,11 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between two convex polyhedra // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { +bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, + Allocator& memoryAllocator) { // Run the SAT algorithm to find the separating axis and compute contact point - SATAlgorithm satAlgorithm; + SATAlgorithm satAlgorithm(memoryAllocator); #ifdef IS_PROFILING_ACTIVE diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h index 1e0c9748..204bc269 100644 --- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h +++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h @@ -61,7 +61,7 @@ class ConvexPolyhedronVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm ConvexPolyhedronVsConvexPolyhedronAlgorithm& operator=(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between two convex polyhedra - virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override; }; } diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h index 34fa5962..40d3fbc6 100644 --- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h +++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h @@ -90,8 +90,9 @@ class NarrowPhaseAlgorithm { /// Deleted assignment operator NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete; - /// Compute a contact info if the two bounding volume collide - virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts)=0; + /// Compute a contact info if the two bounding volumes collide + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, + Allocator& memoryAllocator)=0; #ifdef IS_PROFILING_ACTIVE diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index f12964cf..7ef99e1f 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -44,6 +44,11 @@ using namespace reactphysics3d; // Static variables initialization const decimal SATAlgorithm::SAME_SEPARATING_AXIS_BIAS = decimal(0.001); +// Constructor +SATAlgorithm::SATAlgorithm(Allocator& memoryAllocator) : mMemoryAllocator(memoryAllocator) { + +} + // Test collision between a sphere and a convex mesh bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const { @@ -125,7 +130,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth(uint faceInd PROFILE("SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth)", mProfiler); // Get the face - HalfEdgeStructure::Face face = polyhedron->getFace(faceIndex); + const HalfEdgeStructure::Face& face = polyhedron->getFace(faceIndex); // Get the face normal const Vector3 faceNormal = polyhedron->getFaceNormal(faceIndex); @@ -200,12 +205,12 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro for (uint e = 0; e < polyhedron->getNbHalfEdges(); e += 2) { // Get an edge from the polyhedron (convert it into the capsule local-space) - HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(e); + const HalfEdgeStructure::Edge& edge = polyhedron->getHalfEdge(e); const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex); const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex); const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1); - HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex); + const HalfEdgeStructure::Edge& twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex); const Vector3 adjacentFace1Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(edge.faceIndex); const Vector3 adjacentFace2Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(twinEdge.faceIndex); @@ -336,7 +341,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhe PROFILE("SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth", mProfiler); // Get the face - HalfEdgeStructure::Face face = polyhedron->getFace(polyhedronFaceIndex); + const HalfEdgeStructure::Face& face = polyhedron->getFace(polyhedronFaceIndex); // Get the face normal const Vector3 faceNormal = polyhedron->getFaceNormal(polyhedronFaceIndex); @@ -361,7 +366,7 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI PROFILE("SATAlgorithm::computeCapsulePolyhedronFaceContactPoints", mProfiler); - HalfEdgeStructure::Face face = polyhedron->getFace(referenceFaceIndex); + const HalfEdgeStructure::Face& face = polyhedron->getFace(referenceFaceIndex); // Get the face normal Vector3 faceNormal = polyhedron->getFaceNormal(referenceFaceIndex); @@ -375,8 +380,8 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI // For each adjacent edge of the separating face of the polyhedron do { - HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(edgeIndex); - HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex); + const HalfEdgeStructure::Edge& edge = polyhedron->getHalfEdge(edgeIndex); + const HalfEdgeStructure::Edge& twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex); // Compute the edge vertices and edge direction Vector3 edgeV1 = polyhedron->getVertexPosition(edge.vertexIndex); @@ -575,8 +580,8 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn } else { // If the previous separating axis (or axis with minimum penetration depth) was the cross product of two edges - HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(lastFrameCollisionInfo->satMinEdge1Index); - HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(lastFrameCollisionInfo->satMinEdge2Index); + const HalfEdgeStructure::Edge& edge1 = polyhedron1->getHalfEdge(lastFrameCollisionInfo->satMinEdge1Index); + const HalfEdgeStructure::Edge& edge2 = polyhedron2->getHalfEdge(lastFrameCollisionInfo->satMinEdge2Index); Vector3 separatingAxisPolyhedron2Space; @@ -669,7 +674,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn for (uint i=0; i < polyhedron1->getNbHalfEdges(); i += 2) { // Get an edge of polyhedron 1 - HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(i); + const HalfEdgeStructure::Edge& edge1 = polyhedron1->getHalfEdge(i); const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex); const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex); @@ -678,7 +683,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn for (uint j=0; j < polyhedron2->getNbHalfEdges(); j += 2) { // Get an edge of polyhedron 2 - HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(j); + const HalfEdgeStructure::Edge& edge2 = polyhedron2->getHalfEdge(j); const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex); const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex); @@ -816,23 +821,24 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene -(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * axisReferenceSpace); // Get the reference face - HalfEdgeStructure::Face referenceFace = referencePolyhedron->getFace(minFaceIndex); + const HalfEdgeStructure::Face& referenceFace = referencePolyhedron->getFace(minFaceIndex); // Find the incident face on the other polyhedron (most anti-parallel face) uint incidentFaceIndex = findMostAntiParallelFaceOnPolyhedron(incidentPolyhedron, axisIncidentSpace); // Get the incident face - HalfEdgeStructure::Face incidentFace = incidentPolyhedron->getFace(incidentFaceIndex); + const HalfEdgeStructure::Face& incidentFace = incidentPolyhedron->getFace(incidentFaceIndex); - std::vector polygonVertices; // Vertices to clip of the incident face - std::vector planesNormals; // Normals of the clipping planes - std::vector planesPoints; // Points on the clipping planes + uint nbIncidentFaceVertices = static_cast(incidentFace.faceVertices.size()); + List polygonVertices(mMemoryAllocator, nbIncidentFaceVertices); // Vertices to clip of the incident face + List planesNormals(mMemoryAllocator, nbIncidentFaceVertices); // Normals of the clipping planes + List planesPoints(mMemoryAllocator, nbIncidentFaceVertices); // Points on the clipping planes // Get all the vertices of the incident face (in the reference local-space) std::vector::const_iterator it; for (it = incidentFace.faceVertices.begin(); it != incidentFace.faceVertices.end(); ++it) { const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(*it); - polygonVertices.push_back(incidentToReferenceTransform * faceVertexIncidentSpace); + polygonVertices.add(incidentToReferenceTransform * faceVertexIncidentSpace); } // Get the reference face clipping planes @@ -841,10 +847,10 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene do { // Get the adjacent edge - HalfEdgeStructure::Edge edge = referencePolyhedron->getHalfEdge(currentEdgeIndex); + const HalfEdgeStructure::Edge& edge = referencePolyhedron->getHalfEdge(currentEdgeIndex); // Get the twin edge - HalfEdgeStructure::Edge twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex); + const HalfEdgeStructure::Edge& twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex); // Compute the edge vertices and edge direction Vector3 edgeV1 = referencePolyhedron->getVertexPosition(edge.vertexIndex); @@ -855,8 +861,8 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene // The clipping plane is perpendicular to the edge direction and the reference face normal Vector3 clipPlaneNormal = axisReferenceSpace.cross(edgeDirection); - planesNormals.push_back(clipPlaneNormal); - planesPoints.push_back(edgeV1); + planesNormals.add(clipPlaneNormal); + planesPoints.add(edgeV1); // Go to the next adjacent edge of the reference face currentEdgeIndex = edge.nextEdgeIndex; @@ -867,17 +873,16 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene assert(planesNormals.size() == planesPoints.size()); // Clip the reference faces with the adjacent planes of the reference face - std::vector clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals); + List clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals, mMemoryAllocator); // We only keep the clipped points that are below the reference face const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(referencePolyhedron->getHalfEdge(firstEdgeIndex).vertexIndex); - std::vector::const_iterator itPoints; bool contactPointsFound = false; - for (itPoints = clipPolygonVertices.begin(); itPoints != clipPolygonVertices.end(); ++itPoints) { + for (uint i=0; i decimal(0.0)) { @@ -887,10 +892,10 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene Vector3 outWorldNormal = normalWorld; // Convert the clip incident polyhedron vertex into the incident polyhedron local-space - Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints); + Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * clipPolygonVertices[i]; // Project the contact point onto the reference face - Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(*itPoints, axisReferenceSpace, referenceFaceVertex); + Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(clipPolygonVertices[i], axisReferenceSpace, referenceFaceVertex); // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2, @@ -968,7 +973,7 @@ decimal SATAlgorithm::testSingleFaceDirectionPolyhedronVsPolyhedron(const Convex PROFILE("SATAlgorithm::testSingleFaceDirectionPolyhedronVsPolyhedron", mProfiler); - HalfEdgeStructure::Face face = polyhedron1->getFace(faceIndex); + const HalfEdgeStructure::Face& face = polyhedron1->getFace(faceIndex); // Get the face normal const Vector3 faceNormal = polyhedron1->getFaceNormal(faceIndex); diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index 60f7a091..af49ca09 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -50,6 +50,9 @@ class SATAlgorithm { /// make sure the contact manifold does not change too much between frames. static const decimal SAME_SEPARATING_AXIS_BIAS; + /// Memory allocator + Allocator& mMemoryAllocator; + #ifdef IS_PROFILING_ACTIVE /// Pointer to the profiler @@ -115,7 +118,7 @@ class SATAlgorithm { // -------------------- Methods -------------------- // /// Constructor - SATAlgorithm() = default; + SATAlgorithm(Allocator& memoryAllocator); /// Destructor ~SATAlgorithm() = default; diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp index a5d7a6ac..f5bfc11d 100755 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp @@ -34,7 +34,8 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between a sphere and a capsule // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { +bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, + Allocator& memoryAllocator) { bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE; diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h index bb9e9866..b51bba57 100644 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h @@ -61,7 +61,7 @@ class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { SphereVsCapsuleAlgorithm& operator=(const SphereVsCapsuleAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between a sphere and a capsule - virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override; }; } diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp index 28a565c1..6cc3e578 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp @@ -34,7 +34,8 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between a sphere and a convex polyhedron // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { +bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, + Allocator& memoryAllocator) { assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); @@ -69,7 +70,7 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPha if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) { // Run the SAT algorithm to find the separating axis and compute contact point - SATAlgorithm satAlgorithm; + SATAlgorithm satAlgorithm(memoryAllocator); #ifdef IS_PROFILING_ACTIVE diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h index 2d9fa801..17055431 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h @@ -61,7 +61,7 @@ class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm { SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between a sphere and a convex polyhedron - virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override; }; } diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp index 0b520213..f8820106 100755 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -30,7 +30,8 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; -bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { +bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, + Allocator& memoryAllocator) { assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE); assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE); diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.h b/src/collision/narrowphase/SphereVsSphereAlgorithm.h index 1fd885a5..e0741ab8 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.h +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.h @@ -61,7 +61,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm { SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete; /// Compute a contact info if the two bounding volume collide - virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override; }; } diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index 6b8c8d6b..37667958 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/src/collision/shapes/BoxShape.cpp @@ -84,10 +84,9 @@ BoxShape::BoxShape(const Vector3& extent) */ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { decimal factor = (decimal(1.0) / decimal(3.0)) * mass; - Vector3 realExtent = mExtent + Vector3(mMargin, mMargin, mMargin); - decimal xSquare = realExtent.x * realExtent.x; - decimal ySquare = realExtent.y * realExtent.y; - decimal zSquare = realExtent.z * realExtent.z; + decimal xSquare = mExtent.x * mExtent.x; + decimal ySquare = mExtent.y * mExtent.y; + decimal zSquare = mExtent.z * mExtent.z; tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0, 0.0, factor * (xSquare + zSquare), 0.0, 0.0, 0.0, factor * (xSquare + ySquare)); diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h index 5643a5c8..d1aa9810 100644 --- a/src/collision/shapes/BoxShape.h +++ b/src/collision/shapes/BoxShape.h @@ -101,7 +101,7 @@ class BoxShape : public ConvexPolyhedronShape { virtual uint getNbFaces() const override; /// Return a given face of the polyhedron - virtual HalfEdgeStructure::Face getFace(uint faceIndex) const override; + virtual const HalfEdgeStructure::Face& getFace(uint faceIndex) const override; /// Return the number of vertices of the polyhedron virtual uint getNbVertices() const override; @@ -113,7 +113,7 @@ class BoxShape : public ConvexPolyhedronShape { virtual uint getNbHalfEdges() const override; /// Return a given half-edge of the polyhedron - virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const override; + virtual const HalfEdgeStructure::Edge& getHalfEdge(uint edgeIndex) const override; /// Return the position of a given vertex virtual Vector3 getVertexPosition(uint vertexIndex) const override; @@ -130,7 +130,7 @@ class BoxShape : public ConvexPolyhedronShape { * @return The vector with the three extents of the box shape (in meters) */ inline Vector3 BoxShape::getExtent() const { - return mExtent + Vector3(mMargin, mMargin, mMargin); + return mExtent; } // Set the scaling vector of the collision shape @@ -150,7 +150,7 @@ inline void BoxShape::setLocalScaling(const Vector3& scaling) { inline void BoxShape::getLocalBounds(Vector3& min, Vector3& max) const { // Maximum bounds - max = mExtent + Vector3(mMargin, mMargin, mMargin); + max = mExtent; // Minimum bounds min = -max; @@ -161,7 +161,7 @@ inline size_t BoxShape::getSizeInBytes() const { return sizeof(BoxShape); } -// Return a local support point in a given direction without the objec margin +// Return a local support point in a given direction without the object margin inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { return Vector3(direction.x < decimal(0.0) ? -mExtent.x : mExtent.x, @@ -182,7 +182,7 @@ inline uint BoxShape::getNbFaces() const { } // Return a given face of the polyhedron -inline HalfEdgeStructure::Face BoxShape::getFace(uint faceIndex) const { +inline const HalfEdgeStructure::Face& BoxShape::getFace(uint faceIndex) const { assert(faceIndex < mHalfEdgeStructure.getNbFaces()); return mHalfEdgeStructure.getFace(faceIndex); } @@ -243,7 +243,7 @@ inline uint BoxShape::getNbHalfEdges() const { } // Return a given half-edge of the polyhedron -inline HalfEdgeStructure::Edge BoxShape::getHalfEdge(uint edgeIndex) const { +inline const HalfEdgeStructure::Edge& BoxShape::getHalfEdge(uint edgeIndex) const { assert(edgeIndex < getNbHalfEdges()); return mHalfEdgeStructure.getHalfEdge(edgeIndex); } diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index 71077af2..0ce696af 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/src/collision/shapes/ConvexMeshShape.h @@ -111,7 +111,7 @@ class ConvexMeshShape : public ConvexPolyhedronShape { virtual uint getNbFaces() const override; /// Return a given face of the polyhedron - virtual HalfEdgeStructure::Face getFace(uint faceIndex) const override; + virtual const HalfEdgeStructure::Face& getFace(uint faceIndex) const override; /// Return the number of vertices of the polyhedron virtual uint getNbVertices() const override; @@ -123,7 +123,7 @@ class ConvexMeshShape : public ConvexPolyhedronShape { virtual uint getNbHalfEdges() const override; /// Return a given half-edge of the polyhedron - virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const override; + virtual const HalfEdgeStructure::Edge& getHalfEdge(uint edgeIndex) const override; /// Return the position of a given vertex virtual Vector3 getVertexPosition(uint vertexIndex) const override; @@ -191,7 +191,7 @@ inline uint ConvexMeshShape::getNbFaces() const { } // Return a given face of the polyhedron -inline HalfEdgeStructure::Face ConvexMeshShape::getFace(uint faceIndex) const { +inline const HalfEdgeStructure::Face& ConvexMeshShape::getFace(uint faceIndex) const { assert(faceIndex < getNbFaces()); return mPolyhedronMesh->getHalfEdgeStructure().getFace(faceIndex); } @@ -213,7 +213,7 @@ inline uint ConvexMeshShape::getNbHalfEdges() const { } // Return a given half-edge of the polyhedron -inline HalfEdgeStructure::Edge ConvexMeshShape::getHalfEdge(uint edgeIndex) const { +inline const HalfEdgeStructure::Edge& ConvexMeshShape::getHalfEdge(uint edgeIndex) const { assert(edgeIndex < getNbHalfEdges()); return mPolyhedronMesh->getHalfEdgeStructure().getHalfEdge(edgeIndex); } diff --git a/src/collision/shapes/ConvexPolyhedronShape.h b/src/collision/shapes/ConvexPolyhedronShape.h index 96707d69..1d944cb8 100644 --- a/src/collision/shapes/ConvexPolyhedronShape.h +++ b/src/collision/shapes/ConvexPolyhedronShape.h @@ -62,7 +62,7 @@ class ConvexPolyhedronShape : public ConvexShape { virtual uint getNbFaces() const=0; /// Return a given face of the polyhedron - virtual HalfEdgeStructure::Face getFace(uint faceIndex) const=0; + virtual const HalfEdgeStructure::Face& getFace(uint faceIndex) const=0; /// Return the number of vertices of the polyhedron virtual uint getNbVertices() const=0; @@ -80,7 +80,7 @@ class ConvexPolyhedronShape : public ConvexShape { virtual uint getNbHalfEdges() const=0; /// Return a given half-edge of the polyhedron - virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const=0; + virtual const HalfEdgeStructure::Edge& getHalfEdge(uint edgeIndex) const=0; /// Return true if the collision shape is a polyhedron virtual bool isPolyhedron() const override; diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp index 77d0929c..e43cc674 100644 --- a/src/collision/shapes/TriangleShape.cpp +++ b/src/collision/shapes/TriangleShape.cpp @@ -58,6 +58,57 @@ TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNor mVerticesNormals[1] = verticesNormals[1]; mVerticesNormals[2] = verticesNormals[2]; + // Faces + for (uint i=0; i<2; i++) { + mFaces[i].faceVertices.push_back(0); + mFaces[i].faceVertices.push_back(1); + mFaces[i].faceVertices.push_back(2); + mFaces[i].edgeIndex = i; + } + + // Edges + for (uint i=0; i<6; i++) { + switch(i) { + case 0: + mEdges[0].vertexIndex = 0; + mEdges[0].twinEdgeIndex = 1; + mEdges[0].faceIndex = 0; + mEdges[0].nextEdgeIndex = 2; + break; + case 1: + mEdges[1].vertexIndex = 1; + mEdges[1].twinEdgeIndex = 0; + mEdges[1].faceIndex = 1; + mEdges[1].nextEdgeIndex = 5; + break; + case 2: + mEdges[2].vertexIndex = 1; + mEdges[2].twinEdgeIndex = 3; + mEdges[2].faceIndex = 0; + mEdges[2].nextEdgeIndex = 4; + break; + case 3: + mEdges[3].vertexIndex = 2; + mEdges[3].twinEdgeIndex = 2; + mEdges[3].faceIndex = 1; + mEdges[3].nextEdgeIndex = 1; + break; + case 4: + mEdges[4].vertexIndex = 2; + mEdges[4].twinEdgeIndex = 5; + mEdges[4].faceIndex = 0; + mEdges[4].nextEdgeIndex = 0; + break; + case 5: + mEdges[5].vertexIndex = 0; + mEdges[5].twinEdgeIndex = 4; + mEdges[5].faceIndex = 1; + mEdges[5].nextEdgeIndex = 3; + break; + } + } + + mRaycastTestType = TriangleRaycastSide::FRONT; mId = shapeId; @@ -227,51 +278,3 @@ 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 60ec216b..a4ce7726 100644 --- a/src/collision/shapes/TriangleShape.h +++ b/src/collision/shapes/TriangleShape.h @@ -72,6 +72,12 @@ class TriangleShape : public ConvexPolyhedronShape { /// Raycast test type for the triangle (front, back, front-back) TriangleRaycastSide mRaycastTestType; + /// Faces information for the two faces of the triangle + HalfEdgeStructure::Face mFaces[2]; + + /// Edges information for the six edges of the triangle + HalfEdgeStructure::Edge mEdges[6]; + // -------------------- Methods -------------------- // /// Return a local support point in a given direction without the object margin @@ -137,7 +143,7 @@ class TriangleShape : public ConvexPolyhedronShape { virtual uint getNbFaces() const override; /// Return a given face of the polyhedron - virtual HalfEdgeStructure::Face getFace(uint faceIndex) const override; + virtual const HalfEdgeStructure::Face& getFace(uint faceIndex) const override; /// Return the number of vertices of the polyhedron virtual uint getNbVertices() const override; @@ -155,7 +161,7 @@ class TriangleShape : public ConvexPolyhedronShape { virtual uint getNbHalfEdges() const override; /// Return a given half-edge of the polyhedron - virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const override; + virtual const HalfEdgeStructure::Edge& getHalfEdge(uint edgeIndex) const override; /// Return the centroid of the polyhedron virtual Vector3 getCentroid() const override; @@ -252,26 +258,9 @@ inline uint TriangleShape::getNbFaces() const { } // Return a given face of the polyhedron -inline HalfEdgeStructure::Face TriangleShape::getFace(uint faceIndex) const { +inline const 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 mFaces[faceIndex]; } // Return the number of vertices of the polyhedron @@ -292,6 +281,12 @@ inline HalfEdgeStructure::Vertex TriangleShape::getVertex(uint vertexIndex) cons return vertex; } +// Return a given half-edge of the polyhedron +inline const HalfEdgeStructure::Edge& TriangleShape::getHalfEdge(uint edgeIndex) const { + assert(edgeIndex < getNbHalfEdges()); + return mEdges[edgeIndex]; +} + // Return the position of a given vertex inline Vector3 TriangleShape::getVertexPosition(uint vertexIndex) const { assert(vertexIndex < 3); diff --git a/src/mathematics/Quaternion.h b/src/mathematics/Quaternion.h index 74415eb8..d5e771e2 100644 --- a/src/mathematics/Quaternion.h +++ b/src/mathematics/Quaternion.h @@ -306,16 +306,35 @@ inline Quaternion Quaternion::operator*(decimal nb) const { // Overloaded operator for the multiplication of two quaternions inline Quaternion Quaternion::operator*(const Quaternion& quaternion) const { + + /* The followin code is equivalent to this return Quaternion(w * quaternion.w - getVectorV().dot(quaternion.getVectorV()), - w * quaternion.getVectorV() + quaternion.w * getVectorV() + - getVectorV().cross(quaternion.getVectorV())); + w * quaternion.getVectorV() + quaternion.w * getVectorV() + + getVectorV().cross(quaternion.getVectorV())); + */ + + return Quaternion(w * quaternion.x + quaternion.w * x + y * quaternion.z - z * quaternion.y, + w * quaternion.y + quaternion.w * y + z * quaternion.x - x * quaternion.z, + w * quaternion.z + quaternion.w * z + x * quaternion.y - y * quaternion.x, + w * quaternion.w - x * quaternion.x - y * quaternion.y - z * quaternion.z); } // Overloaded operator for the multiplication with a vector. /// This methods rotates a point given the rotation of a quaternion. inline Vector3 Quaternion::operator*(const Vector3& point) const { - Quaternion p(point.x, point.y, point.z, 0.0); - return (((*this) * p) * getConjugate()).getVectorV(); + + /* The following code is equivalent to this + * Quaternion p(point.x, point.y, point.z, 0.0); + * return (((*this) * p) * getConjugate()).getVectorV(); + */ + + const decimal prodX = w * point.x + y * point.z - z * point.y; + const decimal prodY = w * point.y + z * point.x - x * point.z; + const decimal prodZ = w * point.z + x * point.y - y * point.x; + const decimal prodW = -x * point.x - y * point.y - z * point.z; + return Vector3(w * prodX - prodY * z + prodZ * y - prodW * x, + w * prodY - prodZ * x + prodX * z - prodW * y, + w * prodZ - prodX * y + prodY * x - prodW * z); } // Overloaded operator for the assignment diff --git a/src/mathematics/Transform.h b/src/mathematics/Transform.h index ab17a620..52ee663e 100644 --- a/src/mathematics/Transform.h +++ b/src/mathematics/Transform.h @@ -194,7 +194,7 @@ inline Transform Transform::identity() { // Return the transformed vector inline Vector3 Transform::operator*(const Vector3& vector) const { - return (mOrientation.getMatrix() * vector) + mPosition; + return (mOrientation * vector) + mPosition; } // Operator of multiplication of a transform with another one diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp index 37335abe..e7c9f371 100755 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -198,10 +198,7 @@ decimal reactphysics3d::computePlaneSegmentIntersection(const Vector3& segA, con const decimal parallelEpsilon = decimal(0.0001); decimal t = decimal(-1); - // Segment AB - const Vector3 ab = segB - segA; - - decimal nDotAB = planeNormal.dot(ab); + decimal nDotAB = planeNormal.dot(segB - segA); // If the segment is not parallel to the plane if (std::abs(nDotAB) > parallelEpsilon) { @@ -297,23 +294,27 @@ std::vector reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, // Clip a polygon against multiple planes and return the clipped polygon vertices // This method implements the Sutherland–Hodgman clipping algorithm -std::vector reactphysics3d::clipPolygonWithPlanes(const std::vector& polygonVertices, const std::vector& planesPoints, - const std::vector& planesNormals) { +List reactphysics3d::clipPolygonWithPlanes(const List& polygonVertices, const List& planesPoints, + const List& planesNormals, Allocator& allocator) { assert(planesPoints.size() == planesNormals.size()); - std::vector inputVertices(polygonVertices); - std::vector outputVertices; + uint nbMaxElements = polygonVertices.size() + planesPoints.size(); + List inputVertices(allocator, nbMaxElements); + List outputVertices(allocator, nbMaxElements); + + inputVertices.addRange(polygonVertices); // For each clipping plane for (uint p=0; p reactphysics3d::clipPolygonWithPlanes(const std::vector= decimal(0) && t <= decimal(1.0)) { - outputVertices.push_back(v1 + t * (v2 - v1)); + outputVertices.add(v1 + t * (v2 - v1)); } else { - outputVertices.push_back(v2); + outputVertices.add(v2); } } // Add the second vertex - outputVertices.push_back(v2); + outputVertices.add(v2); } else { // If the second vertex is behind the clipping plane @@ -350,10 +351,10 @@ std::vector reactphysics3d::clipPolygonWithPlanes(const std::vector= decimal(0.0) && t <= decimal(1.0)) { - outputVertices.push_back(v1 + t * (v2 - v1)); + outputVertices.add(v1 + t * (v2 - v1)); } else { - outputVertices.push_back(v1); + outputVertices.add(v1); } } } diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h index 9f062826..ff6ac6f4 100755 --- a/src/mathematics/mathematics_functions.h +++ b/src/mathematics/mathematics_functions.h @@ -33,6 +33,7 @@ #include #include #include +#include "containers/List.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -116,8 +117,8 @@ std::vector clipSegmentWithPlanes(const Vector3& segA, const Vector3& s const std::vector& planesNormals); /// Clip a polygon against multiple planes and return the clipped polygon vertices -std::vector clipPolygonWithPlanes(const std::vector& polygonVertices, const std::vector& planesPoints, - const std::vector& planesNormals); +List clipPolygonWithPlanes(const List& polygonVertices, const List& planesPoints, + const List& planesNormals, Allocator& allocator); /// Project a point onto a plane that is given by a point and its unit length normal Vector3 projectPointOntoPlane(const Vector3& point, const Vector3& planeNormal, const Vector3& planePoint); diff --git a/test/tests/mathematics/TestMathematicsFunctions.h b/test/tests/mathematics/TestMathematicsFunctions.h index b45415ee..793a79fb 100644 --- a/test/tests/mathematics/TestMathematicsFunctions.h +++ b/test/tests/mathematics/TestMathematicsFunctions.h @@ -27,6 +27,8 @@ #define TEST_MATHEMATICS_FUNCTIONS_H // Libraries +#include "containers/List.h" +#include "memory/DefaultAllocator.h" /// Reactphysics3D namespace namespace reactphysics3d { @@ -41,7 +43,7 @@ class TestMathematicsFunctions : public Test { // ---------- Atributes ---------- // - + DefaultAllocator mAllocator; public : @@ -223,37 +225,37 @@ class TestMathematicsFunctions : public Test { test(clipSegmentVertices.size() == 0); // Test clipPolygonWithPlanes() - std::vector polygonVertices; - polygonVertices.push_back(Vector3(-4, 2, 0)); - polygonVertices.push_back(Vector3(7, 2, 0)); - polygonVertices.push_back(Vector3(7, 4, 0)); - polygonVertices.push_back(Vector3(-4, 4, 0)); + List polygonVertices(mAllocator); + polygonVertices.add(Vector3(-4, 2, 0)); + polygonVertices.add(Vector3(7, 2, 0)); + polygonVertices.add(Vector3(7, 4, 0)); + polygonVertices.add(Vector3(-4, 4, 0)); - planesNormals.clear(); - planesPoints.clear(); - planesNormals.push_back(Vector3(1, 0, 0)); - planesPoints.push_back(Vector3(0, 0, 0)); - planesNormals.push_back(Vector3(0, 1, 0)); - planesPoints.push_back(Vector3(0, 0, 0)); - planesNormals.push_back(Vector3(-1, 0, 0)); - planesPoints.push_back(Vector3(10, 0, 0)); - planesNormals.push_back(Vector3(0, -1, 0)); - planesPoints.push_back(Vector3(10, 5, 0)); + List polygonPlanesNormals(mAllocator); + List polygonPlanesPoints(mAllocator); + polygonPlanesNormals.add(Vector3(1, 0, 0)); + polygonPlanesPoints.add(Vector3(0, 0, 0)); + polygonPlanesNormals.add(Vector3(0, 1, 0)); + polygonPlanesPoints.add(Vector3(0, 0, 0)); + polygonPlanesNormals.add(Vector3(-1, 0, 0)); + polygonPlanesPoints.add(Vector3(10, 0, 0)); + polygonPlanesNormals.add(Vector3(0, -1, 0)); + polygonPlanesPoints.add(Vector3(10, 5, 0)); - clipSegmentVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals); - test(clipSegmentVertices.size() == 4); - test(approxEqual(clipSegmentVertices[0].x, 0, 0.000001)); - test(approxEqual(clipSegmentVertices[0].y, 2, 0.000001)); - test(approxEqual(clipSegmentVertices[0].z, 0, 0.000001)); - test(approxEqual(clipSegmentVertices[1].x, 7, 0.000001)); - test(approxEqual(clipSegmentVertices[1].y, 2, 0.000001)); - test(approxEqual(clipSegmentVertices[1].z, 0, 0.000001)); - test(approxEqual(clipSegmentVertices[2].x, 7, 0.000001)); - test(approxEqual(clipSegmentVertices[2].y, 4, 0.000001)); - test(approxEqual(clipSegmentVertices[2].z, 0, 0.000001)); - test(approxEqual(clipSegmentVertices[3].x, 0, 0.000001)); - test(approxEqual(clipSegmentVertices[3].y, 4, 0.000001)); - test(approxEqual(clipSegmentVertices[3].z, 0, 0.000001)); + List clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, polygonPlanesPoints, polygonPlanesNormals, mAllocator); + test(clipPolygonVertices.size() == 4); + test(approxEqual(clipPolygonVertices[0].x, 0, 0.000001)); + test(approxEqual(clipPolygonVertices[0].y, 2, 0.000001)); + test(approxEqual(clipPolygonVertices[0].z, 0, 0.000001)); + test(approxEqual(clipPolygonVertices[1].x, 7, 0.000001)); + test(approxEqual(clipPolygonVertices[1].y, 2, 0.000001)); + test(approxEqual(clipPolygonVertices[1].z, 0, 0.000001)); + test(approxEqual(clipPolygonVertices[2].x, 7, 0.000001)); + test(approxEqual(clipPolygonVertices[2].y, 4, 0.000001)); + test(approxEqual(clipPolygonVertices[2].z, 0, 0.000001)); + test(approxEqual(clipPolygonVertices[3].x, 0, 0.000001)); + test(approxEqual(clipPolygonVertices[3].y, 4, 0.000001)); + test(approxEqual(clipPolygonVertices[3].z, 0, 0.000001)); }