From 4f76553c59ae7410afa89f649a7aa089bb34a302 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 4 Dec 2017 22:14:52 +0100 Subject: [PATCH 01/10] Many small optimizations --- src/collision/CollisionDetection.cpp | 12 +-- src/collision/CollisionDetection.h | 1 - src/collision/HalfEdgeStructure.h | 18 ++-- src/collision/PolyhedronMesh.cpp | 2 +- .../narrowphase/CapsuleVsCapsuleAlgorithm.cpp | 3 +- .../narrowphase/CapsuleVsCapsuleAlgorithm.h | 2 +- .../CapsuleVsConvexPolyhedronAlgorithm.cpp | 8 +- .../CapsuleVsConvexPolyhedronAlgorithm.h | 2 +- ...xPolyhedronVsConvexPolyhedronAlgorithm.cpp | 5 +- ...vexPolyhedronVsConvexPolyhedronAlgorithm.h | 2 +- .../narrowphase/NarrowPhaseAlgorithm.h | 5 +- .../narrowphase/SAT/SATAlgorithm.cpp | 61 ++++++------ src/collision/narrowphase/SAT/SATAlgorithm.h | 5 +- .../narrowphase/SphereVsCapsuleAlgorithm.cpp | 3 +- .../narrowphase/SphereVsCapsuleAlgorithm.h | 2 +- .../SphereVsConvexPolyhedronAlgorithm.cpp | 5 +- .../SphereVsConvexPolyhedronAlgorithm.h | 2 +- .../narrowphase/SphereVsSphereAlgorithm.cpp | 3 +- .../narrowphase/SphereVsSphereAlgorithm.h | 2 +- src/collision/shapes/BoxShape.cpp | 7 +- src/collision/shapes/BoxShape.h | 14 +-- src/collision/shapes/ConvexMeshShape.h | 8 +- src/collision/shapes/ConvexPolyhedronShape.h | 4 +- src/collision/shapes/TriangleShape.cpp | 99 ++++++++++--------- src/collision/shapes/TriangleShape.h | 37 +++---- src/mathematics/Quaternion.h | 27 ++++- src/mathematics/Transform.h | 2 +- src/mathematics/mathematics_functions.cpp | 31 +++--- src/mathematics/mathematics_functions.h | 5 +- .../mathematics/TestMathematicsFunctions.h | 62 ++++++------ 30 files changed, 235 insertions(+), 204 deletions(-) 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)); } From 9d761291d620efdf23749d0ed2d82daee054c45a Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 6 Dec 2017 21:55:50 +0100 Subject: [PATCH 02/10] Small optimizations --- CMakeLists.txt | 2 ++ .../narrowphase/SAT/SATAlgorithm.cpp | 24 +-------------- src/collision/narrowphase/SAT/SATAlgorithm.h | 3 -- .../shapes/ConvexPolyhedronShape.cpp | 22 ++++++++++++++ src/collision/shapes/ConvexPolyhedronShape.h | 5 ++++ src/constraint/ContactPoint.h | 8 ++--- src/mathematics/mathematics_functions.cpp | 29 ++++++++++--------- 7 files changed, 50 insertions(+), 43 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c4cfde6e..e7e2df87 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -192,8 +192,10 @@ SET (REACTPHYSICS3D_SOURCES "src/memory/PoolAllocator.cpp" "src/memory/SingleFrameAllocator.h" "src/memory/SingleFrameAllocator.cpp" + "src/memory/DefaultAllocator.h" "src/containers/Stack.h" "src/containers/LinkedList.h" + "src/containers/List.h" ) # Create the library diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 7ef99e1f..5e40f45f 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -824,7 +824,7 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene const HalfEdgeStructure::Face& referenceFace = referencePolyhedron->getFace(minFaceIndex); // Find the incident face on the other polyhedron (most anti-parallel face) - uint incidentFaceIndex = findMostAntiParallelFaceOnPolyhedron(incidentPolyhedron, axisIncidentSpace); + uint incidentFaceIndex = incidentPolyhedron->findMostAntiParallelFace(axisIncidentSpace); // Get the incident face const HalfEdgeStructure::Face& incidentFace = incidentPolyhedron->getFace(incidentFaceIndex); @@ -914,28 +914,6 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene return contactPointsFound; } -// Find and return the index of the polyhedron face with the most anti-parallel face normal given a direction vector -// This is used to find the incident face on a polyhedron of a given reference face of another polyhedron -uint SATAlgorithm::findMostAntiParallelFaceOnPolyhedron(const ConvexPolyhedronShape* polyhedron, const Vector3& direction) const { - - PROFILE("SATAlgorithm::findMostAntiParallelFaceOnPolyhedron", mProfiler); - - decimal minDotProduct = DECIMAL_LARGEST; - uint mostAntiParallelFace = 0; - - // For each face of the polyhedron - for (uint i=0; i < polyhedron->getNbFaces(); i++) { - - // Get the face normal - decimal dotProduct = polyhedron->getFaceNormal(i).dot(direction); - if (dotProduct < minDotProduct) { - minDotProduct = dotProduct; - mostAntiParallelFace = i; - } - } - - return mostAntiParallelFace; -} // Compute and return the distance between the two edges in the direction of the candidate separating axis decimal SATAlgorithm::computeDistanceBetweenEdges(const Vector3& edge1A, const Vector3& edge2A, const Vector3& polyhedron2Centroid, diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index af49ca09..52884f8e 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -72,9 +72,6 @@ class SATAlgorithm { const Vector3& c, const Vector3& d, const Vector3& bCrossA, const Vector3& dCrossC) const; - // Find and return the index of the polyhedron face with the most anti-parallel face normal given a direction vector - uint findMostAntiParallelFaceOnPolyhedron(const ConvexPolyhedronShape* polyhedron, const Vector3& direction) const; - /// Compute and return the distance between the two edges in the direction of the candidate separating axis decimal computeDistanceBetweenEdges(const Vector3& edge1A, const Vector3& edge2A, const Vector3& polyhedron2Centroid, const Vector3& edge1Direction, const Vector3& edge2Direction, diff --git a/src/collision/shapes/ConvexPolyhedronShape.cpp b/src/collision/shapes/ConvexPolyhedronShape.cpp index ab93584d..24f80de6 100644 --- a/src/collision/shapes/ConvexPolyhedronShape.cpp +++ b/src/collision/shapes/ConvexPolyhedronShape.cpp @@ -35,3 +35,25 @@ ConvexPolyhedronShape::ConvexPolyhedronShape(CollisionShapeName name) : ConvexShape(name, CollisionShapeType::CONVEX_POLYHEDRON) { } + +// Find and return the index of the polyhedron face with the most anti-parallel face +// normal given a direction vector. This is used to find the incident face on +// a polyhedron of a given reference face of another polyhedron +uint ConvexPolyhedronShape::findMostAntiParallelFace(const Vector3& direction) const { + + decimal minDotProduct = DECIMAL_LARGEST; + uint mostAntiParallelFace = 0; + + // For each face of the polyhedron + for (uint i=0; i < getNbFaces(); i++) { + + // Get the face normal + decimal dotProduct = getFaceNormal(i).dot(direction); + if (dotProduct < minDotProduct) { + minDotProduct = dotProduct; + mostAntiParallelFace = i; + } + } + + return mostAntiParallelFace; +} diff --git a/src/collision/shapes/ConvexPolyhedronShape.h b/src/collision/shapes/ConvexPolyhedronShape.h index 1d944cb8..50c6df89 100644 --- a/src/collision/shapes/ConvexPolyhedronShape.h +++ b/src/collision/shapes/ConvexPolyhedronShape.h @@ -87,6 +87,10 @@ class ConvexPolyhedronShape : public ConvexShape { /// Return the centroid of the polyhedron virtual Vector3 getCentroid() const=0; + + /// Find and return the index of the polyhedron face with the most anti-parallel face + /// normal given a direction vector + uint findMostAntiParallelFace(const Vector3& direction) const; }; // Return true if the collision shape is a polyhedron @@ -94,6 +98,7 @@ inline bool ConvexPolyhedronShape::isPolyhedron() const { return true; } + } #endif diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h index 40a3d13c..d78659f8 100644 --- a/src/constraint/ContactPoint.h +++ b/src/constraint/ContactPoint.h @@ -122,10 +122,10 @@ class ContactPoint { Vector3 getNormal() const; /// Return the contact point on the first proxy shape in the local-space of the proxy shape - Vector3 getLocalPointOnShape1() const; + const Vector3& getLocalPointOnShape1() const; /// Return the contact point on the second proxy shape in the local-space of the proxy shape - Vector3 getLocalPointOnShape2() const; + const Vector3& getLocalPointOnShape2() const; /// Return the cached penetration impulse decimal getPenetrationImpulse() const; @@ -157,12 +157,12 @@ inline Vector3 ContactPoint::getNormal() const { } // Return the contact point on the first proxy shape in the local-space of the proxy shape -inline Vector3 ContactPoint::getLocalPointOnShape1() const { +inline const Vector3& ContactPoint::getLocalPointOnShape1() const { return mLocalPointOnShape1; } // Return the contact point on the second proxy shape in the local-space of the proxy shape -inline Vector3 ContactPoint::getLocalPointOnShape2() const { +inline const Vector3& ContactPoint::getLocalPointOnShape2() const { return mLocalPointOnShape2; } diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp index e7c9f371..b82c9fbd 100755 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -300,24 +300,26 @@ List reactphysics3d::clipPolygonWithPlanes(const List& polygon assert(planesPoints.size() == planesNormals.size()); uint nbMaxElements = polygonVertices.size() + planesPoints.size(); - List inputVertices(allocator, nbMaxElements); - List outputVertices(allocator, nbMaxElements); + List list1(allocator, nbMaxElements); + List list2(allocator, nbMaxElements); + List dotProducts(allocator, nbMaxElements * 2); - inputVertices.addRange(polygonVertices); + const List* inputVertices = &polygonVertices; + List* outputVertices = &list2; // For each clipping plane for (uint p=0; pclear(); - uint nbInputVertices = inputVertices.size(); + uint nbInputVertices = inputVertices->size(); uint vStart = nbInputVertices - 1; // For each edge of the polygon for (uint vEnd = 0; vEnd reactphysics3d::clipPolygonWithPlanes(const List& polygon decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]); if (t >= decimal(0) && t <= decimal(1.0)) { - outputVertices.add(v1 + t * (v2 - v1)); + outputVertices->add(v1 + t * (v2 - v1)); } else { - outputVertices.add(v2); + outputVertices->add(v2); } } // Add the second vertex - outputVertices.add(v2); + outputVertices->add(v2); } else { // If the second vertex is behind the clipping plane @@ -351,10 +353,10 @@ List reactphysics3d::clipPolygonWithPlanes(const List& polygon decimal t = computePlaneSegmentIntersection(v1, v2, -planesNormals[p].dot(planesPoints[p]), -planesNormals[p]); if (t >= decimal(0.0) && t <= decimal(1.0)) { - outputVertices.add(v1 + t * (v2 - v1)); + outputVertices->add(v1 + t * (v2 - v1)); } else { - outputVertices.add(v1); + outputVertices->add(v1); } } } @@ -363,9 +365,10 @@ List reactphysics3d::clipPolygonWithPlanes(const List& polygon } inputVertices = outputVertices; + outputVertices = p % 2 == 0 ? &list1 : &list2; } - return outputVertices; + return *outputVertices; } // Project a point onto a plane that is given by a point and its unit length normal From cf42e9f04c35cdacb3b7ae54881812d8bd187d7d Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 12 Dec 2017 07:29:29 +0100 Subject: [PATCH 03/10] Optimizations in contact solver --- src/engine/ContactSolver.cpp | 300 +++++++++++++++++++++++++++-------- 1 file changed, 233 insertions(+), 67 deletions(-) diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 167d37b8..7bd74e8a 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -163,22 +163,48 @@ void ContactSolver::initializeForIsland(Island* island) { new (mContactPoints + mNbContactPoints) ContactPointSolver(); mContactPoints[mNbContactPoints].externalContact = externalContact; mContactPoints[mNbContactPoints].normal = externalContact->getNormal(); - mContactPoints[mNbContactPoints].r1 = p1 - x1; - mContactPoints[mNbContactPoints].r2 = p2 - x2; + mContactPoints[mNbContactPoints].r1.x = p1.x - x1.x; + mContactPoints[mNbContactPoints].r1.y = p1.y - x1.y; + mContactPoints[mNbContactPoints].r1.z = p1.z - x1.z; + mContactPoints[mNbContactPoints].r2.x = p2.x - x2.x; + mContactPoints[mNbContactPoints].r2.y = p2.y - x2.y; + mContactPoints[mNbContactPoints].r2.z = p2.z - x2.z; mContactPoints[mNbContactPoints].penetrationDepth = externalContact->getPenetrationDepth(); mContactPoints[mNbContactPoints].isRestingContact = externalContact->getIsRestingContact(); externalContact->setIsRestingContact(true); mContactPoints[mNbContactPoints].penetrationImpulse = externalContact->getPenetrationImpulse(); mContactPoints[mNbContactPoints].penetrationSplitImpulse = 0.0; - mContactConstraints[mNbContactManifolds].frictionPointBody1 += p1; - mContactConstraints[mNbContactManifolds].frictionPointBody2 += p2; + mContactConstraints[mNbContactManifolds].frictionPointBody1.x += p1.x; + mContactConstraints[mNbContactManifolds].frictionPointBody1.y += p1.y; + mContactConstraints[mNbContactManifolds].frictionPointBody1.z += p1.z; + mContactConstraints[mNbContactManifolds].frictionPointBody2.x += p2.x; + mContactConstraints[mNbContactManifolds].frictionPointBody2.y += p2.y; + mContactConstraints[mNbContactManifolds].frictionPointBody2.z += p2.z; // Compute the velocity difference - Vector3 deltaV = v2 + w2.cross(mContactPoints[mNbContactPoints].r2) - v1 - w1.cross(mContactPoints[mNbContactPoints].r1); + //deltaV = v2 + w2.cross(mContactPoints[mNbContactPoints].r2) - v1 - w1.cross(mContactPoints[mNbContactPoints].r1); + Vector3 deltaV(v2.x + w2.y * mContactPoints[mNbContactPoints].r2.z - w2.z * mContactPoints[mNbContactPoints].r2.y + - v1.x - w1.y * mContactPoints[mNbContactPoints].r1.z - w1.z * mContactPoints[mNbContactPoints].r1.y, + v2.y + w2.z * mContactPoints[mNbContactPoints].r2.x - w2.x * mContactPoints[mNbContactPoints].r2.z + - v1.y - w1.z * mContactPoints[mNbContactPoints].r1.x - w1.x * mContactPoints[mNbContactPoints].r1.z, + v2.z + w2.x * mContactPoints[mNbContactPoints].r2.y - w2.y * mContactPoints[mNbContactPoints].r2.x + - v1.z - w1.x * mContactPoints[mNbContactPoints].r1.y - w1.y * mContactPoints[mNbContactPoints].r1.x); - Vector3 r1CrossN = mContactPoints[mNbContactPoints].r1.cross(mContactPoints[mNbContactPoints].normal); - Vector3 r2CrossN = mContactPoints[mNbContactPoints].r2.cross(mContactPoints[mNbContactPoints].normal); + // r1CrossN = mContactPoints[mNbContactPoints].r1.cross(mContactPoints[mNbContactPoints].normal); + Vector3 r1CrossN(mContactPoints[mNbContactPoints].r1.y * mContactPoints[mNbContactPoints].normal.z - + mContactPoints[mNbContactPoints].r1.z * mContactPoints[mNbContactPoints].normal.y, + mContactPoints[mNbContactPoints].r1.z * mContactPoints[mNbContactPoints].normal.x - + mContactPoints[mNbContactPoints].r1.x * mContactPoints[mNbContactPoints].normal.z, + mContactPoints[mNbContactPoints].r1.x * mContactPoints[mNbContactPoints].normal.y - + mContactPoints[mNbContactPoints].r1.y * mContactPoints[mNbContactPoints].normal.x); + // r2CrossN = mContactPoints[mNbContactPoints].r2.cross(mContactPoints[mNbContactPoints].normal); + Vector3 r2CrossN(mContactPoints[mNbContactPoints].r2.y * mContactPoints[mNbContactPoints].normal.z - + mContactPoints[mNbContactPoints].r2.z * mContactPoints[mNbContactPoints].normal.y, + mContactPoints[mNbContactPoints].r2.z * mContactPoints[mNbContactPoints].normal.x - + mContactPoints[mNbContactPoints].r2.x * mContactPoints[mNbContactPoints].normal.z, + mContactPoints[mNbContactPoints].r2.x * mContactPoints[mNbContactPoints].normal.y - + mContactPoints[mNbContactPoints].r2.y * mContactPoints[mNbContactPoints].normal.x); mContactPoints[mNbContactPoints].i1TimesR1CrossN = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * r1CrossN; mContactPoints[mNbContactPoints].i2TimesR2CrossN = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * r2CrossN; @@ -194,13 +220,18 @@ void ContactSolver::initializeForIsland(Island* island) { // at the beginning of the contact. Note that if it is a resting contact (normal // velocity bellow a given threshold), we do not add a restitution velocity bias mContactPoints[mNbContactPoints].restitutionBias = 0.0; - decimal deltaVDotN = deltaV.dot(mContactPoints[mNbContactPoints].normal); + // deltaVDotN = deltaV.dot(mContactPoints[mNbContactPoints].normal); + decimal deltaVDotN = deltaV.x * mContactPoints[mNbContactPoints].normal.x + + deltaV.y * mContactPoints[mNbContactPoints].normal.y + + deltaV.z * mContactPoints[mNbContactPoints].normal.z; const decimal restitutionFactor = computeMixedRestitutionFactor(body1, body2); if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) { mContactPoints[mNbContactPoints].restitutionBias = restitutionFactor * deltaVDotN; } - mContactConstraints[mNbContactManifolds].normal += mContactPoints[mNbContactPoints].normal; + mContactConstraints[mNbContactManifolds].normal.x += mContactPoints[mNbContactPoints].normal.x; + mContactConstraints[mNbContactManifolds].normal.y += mContactPoints[mNbContactPoints].normal.y; + mContactConstraints[mNbContactManifolds].normal.z += mContactPoints[mNbContactPoints].normal.z; mNbContactPoints++; @@ -209,8 +240,12 @@ void ContactSolver::initializeForIsland(Island* island) { mContactConstraints[mNbContactManifolds].frictionPointBody1 /=static_cast(mContactConstraints[mNbContactManifolds].nbContacts); mContactConstraints[mNbContactManifolds].frictionPointBody2 /=static_cast(mContactConstraints[mNbContactManifolds].nbContacts); - mContactConstraints[mNbContactManifolds].r1Friction = mContactConstraints[mNbContactManifolds].frictionPointBody1 - x1; - mContactConstraints[mNbContactManifolds].r2Friction = mContactConstraints[mNbContactManifolds].frictionPointBody2 - x2; + mContactConstraints[mNbContactManifolds].r1Friction.x = mContactConstraints[mNbContactManifolds].frictionPointBody1.x - x1.x; + mContactConstraints[mNbContactManifolds].r1Friction.y = mContactConstraints[mNbContactManifolds].frictionPointBody1.y - x1.y; + mContactConstraints[mNbContactManifolds].r1Friction.z = mContactConstraints[mNbContactManifolds].frictionPointBody1.z - x1.z; + mContactConstraints[mNbContactManifolds].r2Friction.x = mContactConstraints[mNbContactManifolds].frictionPointBody2.x - x2.x; + mContactConstraints[mNbContactManifolds].r2Friction.y = mContactConstraints[mNbContactManifolds].frictionPointBody2.y - x2.y; + mContactConstraints[mNbContactManifolds].r2Friction.z = mContactConstraints[mNbContactManifolds].frictionPointBody2.z - x2.z; mContactConstraints[mNbContactManifolds].oldFrictionVector1 = externalManifold->getFrictionVector1(); mContactConstraints[mNbContactManifolds].oldFrictionVector2 = externalManifold->getFrictionVector2(); @@ -230,8 +265,20 @@ void ContactSolver::initializeForIsland(Island* island) { mContactConstraints[mNbContactManifolds].normal.normalize(); - Vector3 deltaVFrictionPoint = v2 + w2.cross(mContactConstraints[mNbContactManifolds].r2Friction) - - v1 - w1.cross(mContactConstraints[mNbContactManifolds].r1Friction); + // deltaVFrictionPoint = v2 + w2.cross(mContactConstraints[mNbContactManifolds].r2Friction) - + // v1 - w1.cross(mContactConstraints[mNbContactManifolds].r1Friction); + Vector3 deltaVFrictionPoint(v2.x + w2.y * mContactConstraints[mNbContactManifolds].r2Friction.z - + w2.z * mContactConstraints[mNbContactManifolds].r2Friction.y - + v1.x - w1.y * mContactConstraints[mNbContactManifolds].r1Friction.z - + w1.z * mContactConstraints[mNbContactManifolds].r1Friction.y, + v2.y + w2.z * mContactConstraints[mNbContactManifolds].r2Friction.x - + w2.x * mContactConstraints[mNbContactManifolds].r2Friction.z - + v1.y - w1.z * mContactConstraints[mNbContactManifolds].r1Friction.x - + w1.x * mContactConstraints[mNbContactManifolds].r1Friction.z, + v2.z + w2.x * mContactConstraints[mNbContactManifolds].r2Friction.y - + w2.y * mContactConstraints[mNbContactManifolds].r2Friction.x - + v1.z - w1.x * mContactConstraints[mNbContactManifolds].r1Friction.y - + w1.y * mContactConstraints[mNbContactManifolds].r1Friction.x); // Compute the friction vectors computeFrictionVectors(deltaVFrictionPoint, mContactConstraints[mNbContactManifolds]); @@ -289,13 +336,25 @@ void ContactSolver::warmStart() { // --------- Penetration --------- // // Update the velocities of the body 1 by applying the impulse P - Vector3 impulsePenetration = mContactPoints[contactPointIndex].normal * mContactPoints[contactPointIndex].penetrationImpulse; - mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * impulsePenetration; - mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactPoints[contactPointIndex].i1TimesR1CrossN * mContactPoints[contactPointIndex].penetrationImpulse; + Vector3 impulsePenetration(mContactPoints[contactPointIndex].normal.x * mContactPoints[contactPointIndex].penetrationImpulse, + mContactPoints[contactPointIndex].normal.y * mContactPoints[contactPointIndex].penetrationImpulse, + mContactPoints[contactPointIndex].normal.z * mContactPoints[contactPointIndex].penetrationImpulse); + mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * impulsePenetration.x; + mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * impulsePenetration.y; + mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * impulsePenetration.z; + + mAngularVelocities[mContactConstraints[c].indexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse; + mAngularVelocities[mContactConstraints[c].indexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse; + mAngularVelocities[mContactConstraints[c].indexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse; // Update the velocities of the body 2 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * impulsePenetration; - mAngularVelocities[mContactConstraints[c].indexBody2] += mContactPoints[contactPointIndex].i2TimesR2CrossN * mContactPoints[contactPointIndex].penetrationImpulse; + mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * impulsePenetration.x; + mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * impulsePenetration.y; + mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * impulsePenetration.z; + + mAngularVelocities[mContactConstraints[c].indexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse; + mAngularVelocities[mContactConstraints[c].indexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse; + mAngularVelocities[mContactConstraints[c].indexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse; } else { // If it is a new contact point @@ -312,20 +371,27 @@ void ContactSolver::warmStart() { // Project the old friction impulses (with old friction vectors) into the new friction // vectors to get the new friction impulses - Vector3 oldFrictionImpulse = mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1 + - mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2; + Vector3 oldFrictionImpulse(mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1.x + + mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2.x, + mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1.y + + mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2.y, + mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1.z + + mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2.z); mContactConstraints[c].friction1Impulse = oldFrictionImpulse.dot(mContactConstraints[c].frictionVector1); mContactConstraints[c].friction2Impulse = oldFrictionImpulse.dot(mContactConstraints[c].frictionVector2); // ------ First friction constraint at the center of the contact manifold ------ // // Compute the impulse P = J^T * lambda - Vector3 angularImpulseBody1 = -mContactConstraints[c].r1CrossT1 * - mContactConstraints[c].friction1Impulse; - Vector3 linearImpulseBody2 = mContactConstraints[c].frictionVector1 * - mContactConstraints[c].friction1Impulse; - Vector3 angularImpulseBody2 = mContactConstraints[c].r2CrossT1 * - mContactConstraints[c].friction1Impulse; + Vector3 angularImpulseBody1(-mContactConstraints[c].r1CrossT1.x * mContactConstraints[c].friction1Impulse, + -mContactConstraints[c].r1CrossT1.y * mContactConstraints[c].friction1Impulse, + -mContactConstraints[c].r1CrossT1.z * mContactConstraints[c].friction1Impulse); + Vector3 linearImpulseBody2(mContactConstraints[c].frictionVector1.x * mContactConstraints[c].friction1Impulse, + mContactConstraints[c].frictionVector1.y * mContactConstraints[c].friction1Impulse, + mContactConstraints[c].frictionVector1.z * mContactConstraints[c].friction1Impulse); + Vector3 angularImpulseBody2(mContactConstraints[c].r2CrossT1.x * mContactConstraints[c].friction1Impulse, + mContactConstraints[c].r2CrossT1.y * mContactConstraints[c].friction1Impulse, + mContactConstraints[c].r2CrossT1.z * mContactConstraints[c].friction1Impulse); // Update the velocities of the body 1 by applying the impulse P mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2; @@ -338,23 +404,40 @@ void ContactSolver::warmStart() { // ------ Second friction constraint at the center of the contact manifold ----- // // Compute the impulse P = J^T * lambda - angularImpulseBody1 = -mContactConstraints[c].r1CrossT2 * mContactConstraints[c].friction2Impulse; - linearImpulseBody2 = mContactConstraints[c].frictionVector2 * mContactConstraints[c].friction2Impulse; - angularImpulseBody2 = mContactConstraints[c].r2CrossT2 * mContactConstraints[c].friction2Impulse; + angularImpulseBody1.x = -mContactConstraints[c].r1CrossT2.x * mContactConstraints[c].friction2Impulse; + angularImpulseBody1.y = -mContactConstraints[c].r1CrossT2.y * mContactConstraints[c].friction2Impulse; + angularImpulseBody1.z = -mContactConstraints[c].r1CrossT2.z * mContactConstraints[c].friction2Impulse; + linearImpulseBody2.x = mContactConstraints[c].frictionVector2.x * mContactConstraints[c].friction2Impulse; + linearImpulseBody2.y = mContactConstraints[c].frictionVector2.y * mContactConstraints[c].friction2Impulse; + linearImpulseBody2.z = mContactConstraints[c].frictionVector2.z * mContactConstraints[c].friction2Impulse; + angularImpulseBody2.x = mContactConstraints[c].r2CrossT2.x * mContactConstraints[c].friction2Impulse; + angularImpulseBody2.y = mContactConstraints[c].r2CrossT2.y * mContactConstraints[c].friction2Impulse; + angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * mContactConstraints[c].friction2Impulse; // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2; + mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x; + mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y; + mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z; + mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; // Update the velocities of the body 2 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2; + mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x; + mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y; + mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z; + mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; // ------ Twist friction constraint at the center of the contact manifold ------ // // Compute the impulse P = J^T * lambda - angularImpulseBody1 = -mContactConstraints[c].normal * mContactConstraints[c].frictionTwistImpulse; - angularImpulseBody2 = mContactConstraints[c].normal * mContactConstraints[c].frictionTwistImpulse; + angularImpulseBody1.x = -mContactConstraints[c].normal.x * mContactConstraints[c].frictionTwistImpulse; + angularImpulseBody1.y = -mContactConstraints[c].normal.y * mContactConstraints[c].frictionTwistImpulse; + angularImpulseBody1.z = -mContactConstraints[c].normal.z * mContactConstraints[c].frictionTwistImpulse; + + angularImpulseBody2.x = mContactConstraints[c].normal.x * mContactConstraints[c].frictionTwistImpulse; + angularImpulseBody2.y = mContactConstraints[c].normal.y * mContactConstraints[c].frictionTwistImpulse; + angularImpulseBody2.z = mContactConstraints[c].normal.z * mContactConstraints[c].frictionTwistImpulse; // Update the velocities of the body 1 by applying the impulse P mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; @@ -409,8 +492,15 @@ void ContactSolver::solve() { // --------- Penetration --------- // // Compute J*v - Vector3 deltaV = v2 + w2.cross(mContactPoints[contactPointIndex].r2) - v1 - w1.cross(mContactPoints[contactPointIndex].r1); - decimal deltaVDotN = deltaV.dot(mContactPoints[contactPointIndex].normal); + //Vector3 deltaV = v2 + w2.cross(mContactPoints[contactPointIndex].r2) - v1 - w1.cross(mContactPoints[contactPointIndex].r1); + Vector3 deltaV(v2.x + w2.y * mContactPoints[contactPointIndex].r2.z - w2.z * mContactPoints[contactPointIndex].r2.y - v1.x - + w1.y * mContactPoints[contactPointIndex].r1.z + w1.z * mContactPoints[contactPointIndex].r1.y, + v2.y + w2.z * mContactPoints[contactPointIndex].r2.x - w2.x * mContactPoints[contactPointIndex].r2.z - v1.y - + w1.z * mContactPoints[contactPointIndex].r1.x + w1.x * mContactPoints[contactPointIndex].r1.z, + v2.z + w2.x * mContactPoints[contactPointIndex].r2.y - w2.y * mContactPoints[contactPointIndex].r2.x - v1.z - + w1.x * mContactPoints[contactPointIndex].r1.y + w1.y * mContactPoints[contactPointIndex].r1.x); + decimal deltaVDotN = deltaV.x * mContactPoints[contactPointIndex].normal.x + deltaV.y * mContactPoints[contactPointIndex].normal.y + + deltaV.z * mContactPoints[contactPointIndex].normal.z; decimal Jv = deltaVDotN; // Compute the bias "b" of the constraint @@ -433,15 +523,27 @@ void ContactSolver::solve() { deltaLambda, decimal(0.0)); deltaLambda = mContactPoints[contactPointIndex].penetrationImpulse - lambdaTemp; - Vector3 linearImpulse = mContactPoints[contactPointIndex].normal * deltaLambda; + Vector3 linearImpulse(mContactPoints[contactPointIndex].normal.x * deltaLambda, + mContactPoints[contactPointIndex].normal.y * deltaLambda, + mContactPoints[contactPointIndex].normal.z * deltaLambda); // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulse; - mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactPoints[contactPointIndex].i1TimesR1CrossN * deltaLambda; + mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x; + mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y; + mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z; + + mAngularVelocities[mContactConstraints[c].indexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambda; + mAngularVelocities[mContactConstraints[c].indexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambda; + mAngularVelocities[mContactConstraints[c].indexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambda; // Update the velocities of the body 2 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulse; - mAngularVelocities[mContactConstraints[c].indexBody2] += mContactPoints[contactPointIndex].i2TimesR2CrossN * deltaLambda; + mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x; + mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y; + mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z; + + mAngularVelocities[mContactConstraints[c].indexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambda; + mAngularVelocities[mContactConstraints[c].indexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambda; + mAngularVelocities[mContactConstraints[c].indexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambda; sumPenetrationImpulse += mContactPoints[contactPointIndex].penetrationImpulse; @@ -453,9 +555,17 @@ void ContactSolver::solve() { const Vector3& w1Split = mSplitAngularVelocities[mContactConstraints[c].indexBody1]; const Vector3& v2Split = mSplitLinearVelocities[mContactConstraints[c].indexBody2]; const Vector3& w2Split = mSplitAngularVelocities[mContactConstraints[c].indexBody2]; - Vector3 deltaVSplit = v2Split + w2Split.cross(mContactPoints[contactPointIndex].r2) - - v1Split - w1Split.cross(mContactPoints[contactPointIndex].r1); - decimal JvSplit = deltaVSplit.dot(mContactPoints[contactPointIndex].normal); + + //Vector3 deltaVSplit = v2Split + w2Split.cross(mContactPoints[contactPointIndex].r2) - v1Split - w1Split.cross(mContactPoints[contactPointIndex].r1); + Vector3 deltaVSplit(v2Split.x + w2Split.y * mContactPoints[contactPointIndex].r2.z - w2Split.z * mContactPoints[contactPointIndex].r2.y - v1Split.x - + w1Split.y * mContactPoints[contactPointIndex].r1.z + w1Split.z * mContactPoints[contactPointIndex].r1.y, + v2Split.y + w2Split.z * mContactPoints[contactPointIndex].r2.x - w2Split.x * mContactPoints[contactPointIndex].r2.z - v1Split.y - + w1Split.z * mContactPoints[contactPointIndex].r1.x + w1Split.x * mContactPoints[contactPointIndex].r1.z, + v2Split.z + w2Split.x * mContactPoints[contactPointIndex].r2.y - w2Split.y * mContactPoints[contactPointIndex].r2.x - v1Split.z - + w1Split.x * mContactPoints[contactPointIndex].r1.y + w1Split.y * mContactPoints[contactPointIndex].r1.x); + decimal JvSplit = deltaVSplit.x * mContactPoints[contactPointIndex].normal.x + + deltaVSplit.y * mContactPoints[contactPointIndex].normal.y + + deltaVSplit.z * mContactPoints[contactPointIndex].normal.z; decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * mContactPoints[contactPointIndex].inversePenetrationMass; decimal lambdaTempSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse; @@ -464,15 +574,27 @@ void ContactSolver::solve() { deltaLambdaSplit, decimal(0.0)); deltaLambdaSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse - lambdaTempSplit; - Vector3 linearImpulse = mContactPoints[contactPointIndex].normal * deltaLambdaSplit; + Vector3 linearImpulse(mContactPoints[contactPointIndex].normal.x * deltaLambdaSplit, + mContactPoints[contactPointIndex].normal.y * deltaLambdaSplit, + mContactPoints[contactPointIndex].normal.z * deltaLambdaSplit); // Update the velocities of the body 1 by applying the impulse P - mSplitLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulse; - mSplitAngularVelocities[mContactConstraints[c].indexBody1] -= mContactPoints[contactPointIndex].i1TimesR1CrossN * deltaLambdaSplit; + mSplitLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x; + mSplitLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y; + mSplitLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z; + + mSplitAngularVelocities[mContactConstraints[c].indexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambdaSplit; + mSplitAngularVelocities[mContactConstraints[c].indexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambdaSplit; + mSplitAngularVelocities[mContactConstraints[c].indexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambdaSplit; // Update the velocities of the body 1 by applying the impulse P - mSplitLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulse; - mSplitAngularVelocities[mContactConstraints[c].indexBody2] += mContactPoints[contactPointIndex].i2TimesR2CrossN * deltaLambdaSplit; + mSplitLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x; + mSplitLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y; + mSplitLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z; + + mSplitAngularVelocities[mContactConstraints[c].indexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambdaSplit; + mSplitAngularVelocities[mContactConstraints[c].indexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambdaSplit; + mSplitAngularVelocities[mContactConstraints[c].indexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambdaSplit; } contactPointIndex++; @@ -481,9 +603,16 @@ void ContactSolver::solve() { // ------ First friction constraint at the center of the contact manifol ------ // // Compute J*v - Vector3 deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - - v1 - w1.cross(mContactConstraints[c].r1Friction); - decimal Jv = deltaV.dot(mContactConstraints[c].frictionVector1); + // deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - v1 - w1.cross(mContactConstraints[c].r1Friction); + Vector3 deltaV(v2.x + w2.y * mContactConstraints[c].r2Friction.z - w2.z * mContactConstraints[c].r2Friction.y - v1.x - + w1.y * mContactConstraints[c].r1Friction.z + w1.z * mContactConstraints[c].r1Friction.y, + v2.y + w2.z * mContactConstraints[c].r2Friction.x - w2.x * mContactConstraints[c].r2Friction.z - v1.y - + w1.z * mContactConstraints[c].r1Friction.x + w1.x * mContactConstraints[c].r1Friction.z, + v2.z + w2.x * mContactConstraints[c].r2Friction.y - w2.y * mContactConstraints[c].r2Friction.x - v1.z - + w1.x * mContactConstraints[c].r1Friction.y + w1.y * mContactConstraints[c].r1Friction.x); + decimal Jv = deltaV.x * mContactConstraints[c].frictionVector1.x + + deltaV.y * mContactConstraints[c].frictionVector1.y + + deltaV.z * mContactConstraints[c].frictionVector1.z; // Compute the Lagrange multiplier lambda decimal deltaLambda = -Jv * mContactConstraints[c].inverseFriction1Mass; @@ -495,23 +624,42 @@ void ContactSolver::solve() { deltaLambda = mContactConstraints[c].friction1Impulse - lambdaTemp; // Compute the impulse P=J^T * lambda - Vector3 angularImpulseBody1 = -mContactConstraints[c].r1CrossT1 * deltaLambda; - Vector3 linearImpulseBody2 = mContactConstraints[c].frictionVector1 * deltaLambda; - Vector3 angularImpulseBody2 = mContactConstraints[c].r2CrossT1 * deltaLambda; + Vector3 angularImpulseBody1(-mContactConstraints[c].r1CrossT1.x * deltaLambda, + -mContactConstraints[c].r1CrossT1.y * deltaLambda, + -mContactConstraints[c].r1CrossT1.z * deltaLambda); + Vector3 linearImpulseBody2(mContactConstraints[c].frictionVector1.x * deltaLambda, + mContactConstraints[c].frictionVector1.y * deltaLambda, + mContactConstraints[c].frictionVector1.z * deltaLambda); + Vector3 angularImpulseBody2(mContactConstraints[c].r2CrossT1.x * deltaLambda, + mContactConstraints[c].r2CrossT1.y * deltaLambda, + mContactConstraints[c].r2CrossT1.z * deltaLambda); // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2; + mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x; + mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y; + mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z; + mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; // Update the velocities of the body 2 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2; + mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x; + mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y; + mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z; + mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; // ------ Second friction constraint at the center of the contact manifol ----- // // Compute J*v - deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - v1 - w1.cross(mContactConstraints[c].r1Friction); - Jv = deltaV.dot(mContactConstraints[c].frictionVector2); + //deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - v1 - w1.cross(mContactConstraints[c].r1Friction); + deltaV.x = v2.x + w2.y * mContactConstraints[c].r2Friction.z - v2.z * mContactConstraints[c].r2Friction.y - v1.x - + w1.y * mContactConstraints[c].r1Friction.z + w1.z * mContactConstraints[c].r1Friction.y; + deltaV.y = v2.y + w2.z * mContactConstraints[c].r2Friction.x - v2.x * mContactConstraints[c].r2Friction.z - v1.y - + w1.z * mContactConstraints[c].r1Friction.x + w1.x * mContactConstraints[c].r1Friction.z; + deltaV.z = v2.z + w2.x * mContactConstraints[c].r2Friction.y - v2.y * mContactConstraints[c].r2Friction.x - v1.z - + w1.x * mContactConstraints[c].r1Friction.y + w1.y * mContactConstraints[c].r1Friction.x; + Jv = deltaV.x * mContactConstraints[c].frictionVector2.x + deltaV.y * mContactConstraints[c].frictionVector2.y + + deltaV.z * mContactConstraints[c].frictionVector2.z; // Compute the Lagrange multiplier lambda deltaLambda = -Jv * mContactConstraints[c].inverseFriction2Mass; @@ -523,23 +671,36 @@ void ContactSolver::solve() { deltaLambda = mContactConstraints[c].friction2Impulse - lambdaTemp; // Compute the impulse P=J^T * lambda - angularImpulseBody1 = -mContactConstraints[c].r1CrossT2 * deltaLambda; - linearImpulseBody2 = mContactConstraints[c].frictionVector2 * deltaLambda; - angularImpulseBody2 = mContactConstraints[c].r2CrossT2 * deltaLambda; + angularImpulseBody1.x = -mContactConstraints[c].r1CrossT2.x * deltaLambda; + angularImpulseBody1.y = -mContactConstraints[c].r1CrossT2.y * deltaLambda; + angularImpulseBody1.z = -mContactConstraints[c].r1CrossT2.z * deltaLambda; + + linearImpulseBody2.x = mContactConstraints[c].frictionVector2.x * deltaLambda; + linearImpulseBody2.y = mContactConstraints[c].frictionVector2.y * deltaLambda; + linearImpulseBody2.z = mContactConstraints[c].frictionVector2.z * deltaLambda; + + angularImpulseBody2.x = mContactConstraints[c].r2CrossT2.x * deltaLambda; + angularImpulseBody2.y = mContactConstraints[c].r2CrossT2.y * deltaLambda; + angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * deltaLambda; // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2; + mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x; + mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y; + mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z; mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; // Update the velocities of the body 2 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2; + mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x; + mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y; + mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z; mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; // ------ Twist friction constraint at the center of the contact manifol ------ // // Compute J*v deltaV = w2 - w1; - Jv = deltaV.dot(mContactConstraints[c].normal); + Jv = deltaV.x * mContactConstraints[c].normal.x + deltaV.y * mContactConstraints[c].normal.y + + deltaV.z * mContactConstraints[c].normal.z; deltaLambda = -Jv * (mContactConstraints[c].inverseTwistFrictionMass); frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse; @@ -550,7 +711,9 @@ void ContactSolver::solve() { deltaLambda = mContactConstraints[c].frictionTwistImpulse - lambdaTemp; // Compute the impulse P=J^T * lambda - angularImpulseBody2 = mContactConstraints[c].normal * deltaLambda; + angularImpulseBody2.x = mContactConstraints[c].normal.x * deltaLambda; + angularImpulseBody2.y = mContactConstraints[c].normal.y * deltaLambda; + angularImpulseBody2.z = mContactConstraints[c].normal.z * deltaLambda; // Update the velocities of the body 1 by applying the impulse P mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2; @@ -619,8 +782,11 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, assert(contact.normal.length() > decimal(0.0)); // Compute the velocity difference vector in the tangential plane - Vector3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal; - Vector3 tangentVelocity = deltaVelocity - normalVelocity; + Vector3 normalVelocity(deltaVelocity.x * contact.normal.x * contact.normal.x, + deltaVelocity.y * contact.normal.y * contact.normal.y, + deltaVelocity.z * contact.normal.z * contact.normal.z); + Vector3 tangentVelocity(deltaVelocity.x - normalVelocity.x, deltaVelocity.y - normalVelocity.y, + deltaVelocity.z - normalVelocity.z); // If the velocty difference in the tangential plane is not zero decimal lengthTangenVelocity = tangentVelocity.length(); From 9066264189e27c85bd7a16b04cd3bb32c6a8ef8c Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 12 Dec 2017 22:36:19 +0100 Subject: [PATCH 04/10] Remove commented code --- src/engine/DynamicsWorld.cpp | 23 ----------------------- 1 file changed, 23 deletions(-) diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index bb824228..66b13a8e 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -358,20 +358,6 @@ void DynamicsWorld::solveContactsAndConstraints() { // Check if there are contacts and constraints to solve bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0; - //bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0; - //if (!isConstraintsToSolve && !isContactsToSolve) continue; - - // If there are contacts in the current island -// if (isContactsToSolve) { - -// // Initialize the solver -// mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]); - -// // Warm start the contact solver -// if (mContactSolver.IsWarmStartingActive()) { -// mContactSolver.warmStart(); -// } -// } // If there are constraints if (isConstraintsToSolve) { @@ -393,15 +379,6 @@ void DynamicsWorld::solveContactsAndConstraints() { } mContactSolver.solve(); - - // Solve the contacts -// if (isContactsToSolve) { - -// mContactSolver.resetTotalPenetrationImpulse(); - -// mContactSolver.solvePenetrationConstraints(); -// mContactSolver.solveFrictionConstraints(); -// } } mContactSolver.storeImpulses(); From 2d0cb2753832059abb460ebb317bbbfd9e8f0eab Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 12 Dec 2017 07:38:56 +0100 Subject: [PATCH 05/10] Add List and DefaultAllocator classes --- src/containers/List.h | 191 ++++++++++++++++++++++++++++++++++ src/memory/DefaultAllocator.h | 66 ++++++++++++ 2 files changed, 257 insertions(+) create mode 100644 src/containers/List.h create mode 100644 src/memory/DefaultAllocator.h diff --git a/src/containers/List.h b/src/containers/List.h new file mode 100644 index 00000000..4915f32d --- /dev/null +++ b/src/containers/List.h @@ -0,0 +1,191 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_LIST_H +#define REACTPHYSICS3D_LIST_H + +// Libraries +#include "configuration.h" +#include "memory/Allocator.h" +#include + +namespace reactphysics3d { + +// Class List +/** + * This class represents a simple generic list with custom memory allocator. + */ +template +class List { + + private: + + // -------------------- Attributes -------------------- // + + /// Pointer to the first element of the list + T* mElements; + + /// Number of elements in the list + uint mSize; + + /// Number of allocated elements in the list + uint mCapacity; + + /// Memory allocator + Allocator& mAllocator; + + // -------------------- Methods -------------------- // + + /// Allocate more memory for the elements of the list + void allocateMemory(uint nbElementsToAllocate) { + + assert(nbElementsToAllocate > mCapacity); + + // Allocate memory for the new array + void* newMemory = mAllocator.allocate(nbElementsToAllocate * sizeof(T)); + + if (mElements != nullptr) { + + // Copy the elements to the new allocated memory location + std::memcpy(newMemory, static_cast(mElements), mSize * sizeof(T)); + + // Release the previously allocated memory + mAllocator.release(mElements, mCapacity * sizeof(T)); + } + + mElements = static_cast(newMemory); + + mCapacity = nbElementsToAllocate; + } + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + List(Allocator& allocator, uint capacity = 0) + : mElements(nullptr), mSize(0), mCapacity(0), mAllocator(allocator) { + + if (capacity > 0) { + + // Allocate memory + allocateMemory(capacity); + } + } + + /// Copy constructor + List(const List& list) : mElements(nullptr), mSize(0), mCapacity(0), mAllocator(list.mAllocator) { + + // All all the elements of the list to the current one + addRange(list); + } + + /// Destructor + ~List() { + + // If elements have been allocated + if (mCapacity > 0) { + + // Clear the list + clear(); + + // Release the memory allocated on the heap + mAllocator.release(static_cast(mElements), mCapacity * sizeof(T)); + } + } + + /// Add an element into the list + void add(const T& element) { + + // If we need to allocate more memory + if (mSize == mCapacity) { + allocateMemory(mCapacity == 0 ? 1 : mCapacity * 2); + } + + mElements[mSize] = element; + mSize++; + } + + /// Append another list to the current one + void addRange(const List& list) { + + // If we need to allocate more memory + if (mSize + list.size() > mCapacity) { + + // Allocate memory + allocateMemory(mSize + list.size()); + } + + // Add the elements of the list to the current one + for(uint i=0; i= 0 && index < mSize); + return mElements[index]; + } + + /// Overloaded const index operator + const T& operator[](const uint index) const { + assert(index >= 0 && index < mSize); + return mElements[index]; + } + + /// Overloaded assignment operator + List& operator=(const List& list) { + + // Clear all the elements + clear(); + + // Add all the elements of the list to the current one + addRange(list); + + return *this; + } +}; + +} + +#endif diff --git a/src/memory/DefaultAllocator.h b/src/memory/DefaultAllocator.h new file mode 100644 index 00000000..367171bd --- /dev/null +++ b/src/memory/DefaultAllocator.h @@ -0,0 +1,66 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_DEFAULT_ALLOCATOR_H +#define REACTPHYSICS3D_DEFAULT_ALLOCATOR_H + +// Libraries +#include "memory/Allocator.h" +#include + +/// ReactPhysics3D namespace +namespace reactphysics3d { + +// Class DefaultAllocator +/** + * This class represents a default memory allocator that uses default malloc/free methods + */ +class DefaultAllocator : public Allocator { + + public: + + /// Destructor + virtual ~DefaultAllocator() = default; + + /// Allocate memory of a given size (in bytes) and return a pointer to the + /// allocated memory. + virtual void* allocate(size_t size) override { + return malloc(size); + } + + /// Release previously allocated memory. + virtual void release(void* pointer, size_t size) override { + free(pointer); + } + + /// Return true if memory needs to be release with this allocator + virtual bool isReleaseNeeded() const override { + return true; + } +}; + +} + +#endif From 5392948518ef415991feb55e0595bc15b48d737c Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 13 Dec 2017 17:51:38 +0100 Subject: [PATCH 06/10] Use inline constructors for mathematics objects (vectors, quaternion, matrices, ...) --- src/mathematics/Matrix2x2.cpp | 25 ------------------ src/mathematics/Matrix2x2.h | 25 ++++++++++++++++++ src/mathematics/Matrix3x3.cpp | 26 ------------------- src/mathematics/Matrix3x3.h | 26 +++++++++++++++++++ src/mathematics/Quaternion.cpp | 21 ---------------- src/mathematics/Quaternion.h | 25 ++++++++++++++++-- src/mathematics/Transform.cpp | 22 ---------------- src/mathematics/Transform.h | 46 ++++++++++++++++++++++++++++++++-- src/mathematics/Vector2.cpp | 15 ----------- src/mathematics/Vector2.h | 16 ++++++++++++ src/mathematics/Vector3.cpp | 15 ----------- src/mathematics/Vector3.h | 15 +++++++++++ 12 files changed, 149 insertions(+), 128 deletions(-) diff --git a/src/mathematics/Matrix2x2.cpp b/src/mathematics/Matrix2x2.cpp index c501c1db..bf4adbe5 100644 --- a/src/mathematics/Matrix2x2.cpp +++ b/src/mathematics/Matrix2x2.cpp @@ -28,31 +28,6 @@ using namespace reactphysics3d; -// Constructor of the class Matrix2x2 -Matrix2x2::Matrix2x2() { - - // Initialize all values in the matrix to zero - setAllValues(0.0, 0.0, 0.0, 0.0); -} - -// Constructor -Matrix2x2::Matrix2x2(decimal value) { - setAllValues(value, value, value, value); -} - -// Constructor with arguments -Matrix2x2::Matrix2x2(decimal a1, decimal a2, decimal b1, decimal b2) { - - // Initialize the matrix with the values - setAllValues(a1, a2, b1, b2); -} - -// Copy-constructor -Matrix2x2::Matrix2x2(const Matrix2x2& matrix) { - setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], - matrix.mRows[1][0], matrix.mRows[1][1]); -} - // Assignment operator Matrix2x2& Matrix2x2::operator=(const Matrix2x2& matrix) { diff --git a/src/mathematics/Matrix2x2.h b/src/mathematics/Matrix2x2.h index ee31b07a..8315eb99 100644 --- a/src/mathematics/Matrix2x2.h +++ b/src/mathematics/Matrix2x2.h @@ -147,6 +147,31 @@ class Matrix2x2 { Vector2& operator[](int row); }; +// Constructor of the class Matrix2x2 +inline Matrix2x2::Matrix2x2() { + + // Initialize all values in the matrix to zero + setAllValues(0.0, 0.0, 0.0, 0.0); +} + +// Constructor +inline Matrix2x2::Matrix2x2(decimal value) { + setAllValues(value, value, value, value); +} + +// Constructor with arguments +inline Matrix2x2::Matrix2x2(decimal a1, decimal a2, decimal b1, decimal b2) { + + // Initialize the matrix with the values + setAllValues(a1, a2, b1, b2); +} + +// Copy-constructor +inline Matrix2x2::Matrix2x2(const Matrix2x2& matrix) { + setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], + matrix.mRows[1][0], matrix.mRows[1][1]); +} + // Method to set all the values in the matrix inline void Matrix2x2::setAllValues(decimal a1, decimal a2, decimal b1, decimal b2) { diff --git a/src/mathematics/Matrix3x3.cpp b/src/mathematics/Matrix3x3.cpp index a5491ed4..e86e63a1 100644 --- a/src/mathematics/Matrix3x3.cpp +++ b/src/mathematics/Matrix3x3.cpp @@ -30,32 +30,6 @@ // Namespaces using namespace reactphysics3d; -// Constructor of the class Matrix3x3 -Matrix3x3::Matrix3x3() { - // Initialize all values in the matrix to zero - setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); -} - -// Constructor -Matrix3x3::Matrix3x3(decimal value) { - setAllValues(value, value, value, value, value, value, value, value, value); -} - -// Constructor with arguments -Matrix3x3::Matrix3x3(decimal a1, decimal a2, decimal a3, - decimal b1, decimal b2, decimal b3, - decimal c1, decimal c2, decimal c3) { - // Initialize the matrix with the values - setAllValues(a1, a2, a3, b1, b2, b3, c1, c2, c3); -} - -// Copy-constructor -Matrix3x3::Matrix3x3(const Matrix3x3& matrix) { - setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], matrix.mRows[0][2], - matrix.mRows[1][0], matrix.mRows[1][1], matrix.mRows[1][2], - matrix.mRows[2][0], matrix.mRows[2][1], matrix.mRows[2][2]); -} - // Assignment operator Matrix3x3& Matrix3x3::operator=(const Matrix3x3& matrix) { diff --git a/src/mathematics/Matrix3x3.h b/src/mathematics/Matrix3x3.h index ebb8792d..6897a2cf 100644 --- a/src/mathematics/Matrix3x3.h +++ b/src/mathematics/Matrix3x3.h @@ -155,6 +155,32 @@ class Matrix3x3 { Vector3& operator[](int row); }; +// Constructor of the class Matrix3x3 +inline Matrix3x3::Matrix3x3() { + // Initialize all values in the matrix to zero + setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); +} + +// Constructor +inline Matrix3x3::Matrix3x3(decimal value) { + setAllValues(value, value, value, value, value, value, value, value, value); +} + +// Constructor with arguments +inline Matrix3x3::Matrix3x3(decimal a1, decimal a2, decimal a3, + decimal b1, decimal b2, decimal b3, + decimal c1, decimal c2, decimal c3) { + // Initialize the matrix with the values + setAllValues(a1, a2, a3, b1, b2, b3, c1, c2, c3); +} + +// Copy-constructor +inline Matrix3x3::Matrix3x3(const Matrix3x3& matrix) { + setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], matrix.mRows[0][2], + matrix.mRows[1][0], matrix.mRows[1][1], matrix.mRows[1][2], + matrix.mRows[2][0], matrix.mRows[2][1], matrix.mRows[2][2]); +} + // Method to set all the values in the matrix inline void Matrix3x3::setAllValues(decimal a1, decimal a2, decimal a3, decimal b1, decimal b2, decimal b3, diff --git a/src/mathematics/Quaternion.cpp b/src/mathematics/Quaternion.cpp index d0e1ae61..0e877612 100644 --- a/src/mathematics/Quaternion.cpp +++ b/src/mathematics/Quaternion.cpp @@ -31,27 +31,6 @@ // Namespace using namespace reactphysics3d; -// Constructor of the class -Quaternion::Quaternion() : x(0.0), y(0.0), z(0.0), w(0.0) { - -} - -// Constructor with arguments -Quaternion::Quaternion(decimal newX, decimal newY, decimal newZ, decimal newW) - :x(newX), y(newY), z(newZ), w(newW) { - -} - -// Constructor with the component w and the vector v=(x y z) -Quaternion::Quaternion(decimal newW, const Vector3& v) : x(v.x), y(v.y), z(v.z), w(newW) { - -} - -// Constructor with the component w and the vector v=(x y z) -Quaternion::Quaternion(const Vector3& v, decimal newW) : x(v.x), y(v.y), z(v.z), w(newW) { - -} - // Return a quaternion constructed from Euler angles (in radians) Quaternion Quaternion::fromEulerAngles(decimal angleX, decimal angleY, decimal angleZ) { diff --git a/src/mathematics/Quaternion.h b/src/mathematics/Quaternion.h index d5e771e2..dd0726dc 100644 --- a/src/mathematics/Quaternion.h +++ b/src/mathematics/Quaternion.h @@ -169,7 +169,28 @@ struct Quaternion { void initWithEulerAngles(decimal angleX, decimal angleY, decimal angleZ); }; -/// Set all the values +// Constructor of the class +inline Quaternion::Quaternion() : x(0.0), y(0.0), z(0.0), w(0.0) { + +} + +// Constructor with arguments +inline Quaternion::Quaternion(decimal newX, decimal newY, decimal newZ, decimal newW) + :x(newX), y(newY), z(newZ), w(newW) { + +} + +// Constructor with the component w and the vector v=(x y z) +inline Quaternion::Quaternion(decimal newW, const Vector3& v) : x(v.x), y(v.y), z(v.z), w(newW) { + +} + +// Constructor with the component w and the vector v=(x y z) +inline Quaternion::Quaternion(const Vector3& v, decimal newW) : x(v.x), y(v.y), z(v.z), w(newW) { + +} + +// Set all the values inline void Quaternion::setAllValues(decimal newX, decimal newY, decimal newZ, decimal newW) { x = newX; y = newY; @@ -177,7 +198,7 @@ inline void Quaternion::setAllValues(decimal newX, decimal newY, decimal newZ, d w = newW; } -/// Set the quaternion to zero +// Set the quaternion to zero inline void Quaternion::setToZero() { x = 0; y = 0; diff --git a/src/mathematics/Transform.cpp b/src/mathematics/Transform.cpp index 4bc91253..d5cf0399 100644 --- a/src/mathematics/Transform.cpp +++ b/src/mathematics/Transform.cpp @@ -29,25 +29,3 @@ // Namespaces using namespace reactphysics3d; -// Constructor -Transform::Transform() : mPosition(Vector3(0.0, 0.0, 0.0)), mOrientation(Quaternion::identity()) { - -} - -// Constructor -Transform::Transform(const Vector3& position, const Matrix3x3& orientation) - : mPosition(position), mOrientation(Quaternion(orientation)) { - -} - -// Constructor -Transform::Transform(const Vector3& position, const Quaternion& orientation) - : mPosition(position), mOrientation(orientation) { - -} - -// Copy-constructor -Transform::Transform(const Transform& transform) - : mPosition(transform.mPosition), mOrientation(transform.mOrientation) { - -} diff --git a/src/mathematics/Transform.h b/src/mathematics/Transform.h index 52ee663e..a5248fc2 100644 --- a/src/mathematics/Transform.h +++ b/src/mathematics/Transform.h @@ -118,6 +118,29 @@ class Transform { Transform& operator=(const Transform& transform); }; +// Constructor +inline Transform::Transform() : mPosition(Vector3(0.0, 0.0, 0.0)), mOrientation(Quaternion::identity()) { + +} + +// Constructor +inline Transform::Transform(const Vector3& position, const Matrix3x3& orientation) + : mPosition(position), mOrientation(Quaternion(orientation)) { + +} + +// Constructor +inline Transform::Transform(const Vector3& position, const Quaternion& orientation) + : mPosition(position), mOrientation(orientation) { + +} + +// Copy-constructor +inline Transform::Transform(const Transform& transform) + : mPosition(transform.mPosition), mOrientation(transform.mOrientation) { + +} + // Return the position of the transform inline const Vector3& Transform::getPosition() const { return mPosition; @@ -199,8 +222,27 @@ inline Vector3 Transform::operator*(const Vector3& vector) const { // Operator of multiplication of a transform with another one inline Transform Transform::operator*(const Transform& transform2) const { - return Transform(mPosition + mOrientation * transform2.mPosition, - mOrientation * transform2.mOrientation); + + const decimal prodX = mOrientation.w * transform2.mPosition.x + mOrientation.w * transform2.mPosition.z + - mOrientation.z * transform2.mPosition.y; + const decimal prodY = mOrientation.w * transform2.mPosition.y + mOrientation.z * transform2.mPosition.x + - mOrientation.x * transform2.mPosition.z; + const decimal prodZ = mOrientation.w * transform2.mPosition.z + mOrientation.x * transform2.mPosition.y + - mOrientation.y * transform2.mPosition.x; + const decimal prodW = -mOrientation.x * transform2.mPosition.x - mOrientation.y * transform2.mPosition.y + - mOrientation.z * transform2.mPosition.z; + + return Transform(Vector3(mPosition.x + mOrientation.w * prodX - prodY * mOrientation.z + prodZ * mOrientation.y - prodW * mOrientation.x, + mPosition.y + mOrientation.w * prodY - prodZ * mOrientation.x + prodX * mOrientation.z - prodW * mOrientation.y, + mPosition.z + mOrientation.w * prodZ - prodX * mOrientation.y + prodY * mOrientation.x - prodW * mOrientation.z), + Quaternion(mOrientation.w * transform2.mOrientation.x + transform2.mOrientation.w * mOrientation.x + + mOrientation.y * transform2.mOrientation.z - mOrientation.z * transform2.mOrientation.y, + mOrientation.w * transform2.mOrientation.y + transform2.mOrientation.w * mOrientation.y + + mOrientation.z * transform2.mOrientation.x - mOrientation.x * transform2.mOrientation.z, + mOrientation.w * transform2.mOrientation.z + transform2.mOrientation.w * mOrientation.z + + mOrientation.x * transform2.mOrientation.y - mOrientation.y * transform2.mOrientation.x, + mOrientation.w * transform2.mOrientation.w - mOrientation.x * transform2.mOrientation.x + - mOrientation.y * transform2.mOrientation.y - mOrientation.z * transform2.mOrientation.z)); } // Return true if the two transforms are equal diff --git a/src/mathematics/Vector2.cpp b/src/mathematics/Vector2.cpp index 2f225636..271fd82f 100644 --- a/src/mathematics/Vector2.cpp +++ b/src/mathematics/Vector2.cpp @@ -30,21 +30,6 @@ // Namespaces using namespace reactphysics3d; -// Constructor -Vector2::Vector2() : x(0.0), y(0.0) { - -} - -// Constructor with arguments -Vector2::Vector2(decimal newX, decimal newY) : x(newX), y(newY) { - -} - -// Copy-constructor -Vector2::Vector2(const Vector2& vector) : x(vector.x), y(vector.y) { - -} - // Return the corresponding unit vector Vector2 Vector2::getUnit() const { decimal lengthVector = length(); diff --git a/src/mathematics/Vector2.h b/src/mathematics/Vector2.h index 4b0e32f1..bc5f8872 100644 --- a/src/mathematics/Vector2.h +++ b/src/mathematics/Vector2.h @@ -156,6 +156,22 @@ struct Vector2 { friend Vector2 operator/(const Vector2& vector1, const Vector2& vector2); }; +// Constructor +inline Vector2::Vector2() : x(0.0), y(0.0) { + +} + +// Constructor with arguments +inline Vector2::Vector2(decimal newX, decimal newY) : x(newX), y(newY) { + +} + +// Copy-constructor +inline Vector2::Vector2(const Vector2& vector) : x(vector.x), y(vector.y) { + +} + + // Set the vector to zero inline void Vector2::setToZero() { x = 0; diff --git a/src/mathematics/Vector3.cpp b/src/mathematics/Vector3.cpp index ab2d126d..33b760ba 100644 --- a/src/mathematics/Vector3.cpp +++ b/src/mathematics/Vector3.cpp @@ -31,21 +31,6 @@ // Namespaces using namespace reactphysics3d; -// Constructor of the class Vector3D -Vector3::Vector3() : x(0.0), y(0.0), z(0.0) { - -} - -// Constructor with arguments -Vector3::Vector3(decimal newX, decimal newY, decimal newZ) : x(newX), y(newY), z(newZ) { - -} - -// Copy-constructor -Vector3::Vector3(const Vector3& vector) : x(vector.x), y(vector.y), z(vector.z) { - -} - // Return the corresponding unit vector Vector3 Vector3::getUnit() const { decimal lengthVector = length(); diff --git a/src/mathematics/Vector3.h b/src/mathematics/Vector3.h index 1ca1adfd..95255fdb 100644 --- a/src/mathematics/Vector3.h +++ b/src/mathematics/Vector3.h @@ -168,6 +168,21 @@ struct Vector3 { friend Vector3 operator/(const Vector3& vector1, const Vector3& vector2); }; +// Constructor of the class Vector3D +inline Vector3::Vector3() : x(0.0), y(0.0), z(0.0) { + +} + +// Constructor with arguments +inline Vector3::Vector3(decimal newX, decimal newY, decimal newZ) : x(newX), y(newY), z(newZ) { + +} + +// Copy-constructor +inline Vector3::Vector3(const Vector3& vector) : x(vector.x), y(vector.y), z(vector.z) { + +} + // Set the vector to zero inline void Vector3::setToZero() { x = 0; From 7d20a746e9a8822d4530e6b7b44ef3286d8aed68 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 14 Dec 2017 15:09:56 +0100 Subject: [PATCH 07/10] Do not std::map to store mapping from rigid body to index in array --- src/body/RigidBody.cpp | 2 +- src/body/RigidBody.h | 7 ++++++- src/constraint/BallAndSocketJoint.cpp | 4 ++-- src/constraint/FixedJoint.cpp | 4 ++-- src/constraint/HingeJoint.cpp | 4 ++-- src/constraint/SliderJoint.cpp | 4 ++-- src/engine/ConstraintSolver.cpp | 4 +--- src/engine/ConstraintSolver.h | 16 +++------------- src/engine/ContactSolver.cpp | 8 +++----- src/engine/ContactSolver.h | 6 +----- src/engine/DynamicsWorld.cpp | 26 +++++++++----------------- src/engine/DynamicsWorld.h | 3 --- 12 files changed, 32 insertions(+), 56 deletions(-) diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index d0f0c782..01e1694b 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -42,7 +42,7 @@ RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyinde : CollisionBody(transform, world, id), mInitMass(decimal(1.0)), mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()), mIsGravityEnabled(true), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)), - mJointsList(nullptr) { + mJointsList(nullptr), mArrayIndex(0) { // Compute the inverse mass mMassInverse = decimal(1.0) / mInitMass; diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 6ba7a596..e12cab05 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -50,6 +50,11 @@ class DynamicsWorld; */ class RigidBody : public CollisionBody { + private : + + /// Index of the body in arrays for contact/constraint solver + uint mArrayIndex; + protected : // -------------------- Attributes -------------------- // @@ -102,7 +107,7 @@ class RigidBody : public CollisionBody { decimal mAngularDamping; /// First element of the linked list of joints involving this body - JointListElement* mJointsList; + JointListElement* mJointsList; // -------------------- Methods -------------------- // diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index 7a5fce1b..272d922e 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -45,8 +45,8 @@ BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo) void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { // Initialize the bodies index in the velocity array - mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; - mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; + mIndexBody1 = mBody1->mArrayIndex; + mIndexBody2 = mBody2->mArrayIndex; // Get the bodies center of mass and orientations const Vector3& x1 = mBody1->mCenterOfMassWorld; diff --git a/src/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp index 8f0b105c..01fe3957 100644 --- a/src/constraint/FixedJoint.cpp +++ b/src/constraint/FixedJoint.cpp @@ -53,8 +53,8 @@ FixedJoint::FixedJoint(const FixedJointInfo& jointInfo) void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { // Initialize the bodies index in the velocity array - mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; - mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; + mIndexBody1 = mBody1->mArrayIndex; + mIndexBody2 = mBody2->mArrayIndex; // Get the bodies positions and orientations const Vector3& x1 = mBody1->mCenterOfMassWorld; diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index 1634bf27..7b400a2a 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -68,8 +68,8 @@ HingeJoint::HingeJoint(const HingeJointInfo& jointInfo) void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { // Initialize the bodies index in the velocity array - mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; - mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; + mIndexBody1 = mBody1->mArrayIndex; + mIndexBody2 = mBody2->mArrayIndex; // Get the bodies positions and orientations const Vector3& x1 = mBody1->mCenterOfMassWorld; diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index 919d91c0..7697013d 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -67,8 +67,8 @@ SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { // Initialize the bodies index in the veloc ity array - mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; - mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; + mIndexBody1 = mBody1->mArrayIndex; + mIndexBody2 = mBody2->mArrayIndex; // Get the bodies positions and orientations const Vector3& x1 = mBody1->mCenterOfMassWorld; diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 458c39a7..5345cbaf 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -30,9 +30,7 @@ using namespace reactphysics3d; // Constructor -ConstraintSolver::ConstraintSolver(const std::map& mapBodyToVelocityIndex) - : mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), - mIsWarmStartingActive(true), mConstraintSolverData(mapBodyToVelocityIndex) { +ConstraintSolver::ConstraintSolver() : mIsWarmStartingActive(true) { #ifdef IS_PROFILING_ACTIVE diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index fbb54ed1..105045e0 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -60,18 +60,12 @@ struct ConstraintSolverData { /// Reference to the bodies orientations Quaternion* orientations; - /// Reference to the map that associates rigid body to their index - /// in the constrained velocities array - const std::map& mapBodyToConstrainedVelocityIndex; - /// True if warm starting of the solver is active bool isWarmStartingActive; /// Constructor - ConstraintSolverData(const std::map& refMapBodyToConstrainedVelocityIndex) - :linearVelocities(nullptr), angularVelocities(nullptr), - positions(nullptr), orientations(nullptr), - mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){ + ConstraintSolverData() :linearVelocities(nullptr), angularVelocities(nullptr), + positions(nullptr), orientations(nullptr) { } @@ -152,10 +146,6 @@ class ConstraintSolver { // -------------------- Attributes -------------------- // - /// Reference to the map that associates rigid body to their index in - /// the constrained velocities array - const std::map& mMapBodyToConstrainedVelocityIndex; - /// Current time step decimal mTimeStep; @@ -176,7 +166,7 @@ class ConstraintSolver { // -------------------- Methods -------------------- // /// Constructor - ConstraintSolver(const std::map& mapBodyToVelocityIndex); + ConstraintSolver(); /// Destructor ~ConstraintSolver() = default; diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 7bd74e8a..6361badd 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -39,12 +39,10 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2); const decimal ContactSolver::SLOP = decimal(0.01); // Constructor -ContactSolver::ContactSolver(const std::map& mapBodyToVelocityIndex, - SingleFrameAllocator& allocator) +ContactSolver::ContactSolver(SingleFrameAllocator& allocator) :mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr), mContactConstraints(nullptr), mSingleFrameAllocator(allocator), mLinearVelocities(nullptr), mAngularVelocities(nullptr), - mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), mIsSplitImpulseActive(true) { } @@ -131,8 +129,8 @@ void ContactSolver::initializeForIsland(Island* island) { // Initialize the internal contact manifold structure using the external // contact manifold new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver(); - mContactConstraints[mNbContactManifolds].indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second; - mContactConstraints[mNbContactManifolds].indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second; + mContactConstraints[mNbContactManifolds].indexBody1 = body1->mArrayIndex; + mContactConstraints[mNbContactManifolds].indexBody2 = body2->mArrayIndex; mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); mContactConstraints[mNbContactManifolds].massInverseBody1 = body1->mMassInverse; diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index 0a2850f5..05bd569c 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -302,9 +302,6 @@ class ContactSolver { /// Array of angular velocities Vector3* mAngularVelocities; - /// Reference to the map of rigid body to their index in the constrained velocities array - const std::map& mMapBodyToConstrainedVelocityIndex; - /// True if the split impulse position correction is active bool mIsSplitImpulseActive; @@ -342,8 +339,7 @@ class ContactSolver { // -------------------- Methods -------------------- // /// Constructor - ContactSolver(const std::map& mapBodyToVelocityIndex, - SingleFrameAllocator& allocator); + ContactSolver(SingleFrameAllocator& allocator); /// Destructor ~ContactSolver() = default; diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 66b13a8e..054c4dc5 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -41,8 +41,7 @@ using namespace std; */ DynamicsWorld::DynamicsWorld(const Vector3 &gravity) : CollisionWorld(), - mContactSolver(mMapBodyToConstrainedVelocityIndex, mSingleFrameAllocator), - mConstraintSolver(mMapBodyToConstrainedVelocityIndex), + mContactSolver(mSingleFrameAllocator), mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS), mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS), mIsSleepingEnabled(SLEEPING_ENABLED), mGravity(gravity), mTimeStep(decimal(1.0f / 60.0f)), @@ -167,7 +166,7 @@ void DynamicsWorld::integrateRigidBodiesPositions() { for (uint b=0; b < mIslands[i]->getNbBodies(); b++) { // Get the constrained velocity - uint indexArray = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; + uint indexArray = bodies[b]->mArrayIndex; Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray]; Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray]; @@ -205,7 +204,7 @@ void DynamicsWorld::updateBodiesState() { for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) { - uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; + uint index = bodies[b]->mArrayIndex; // Update the linear and angular velocity of the body bodies[b]->mLinearVelocity = mConstrainedLinearVelocities[index]; @@ -250,21 +249,14 @@ void DynamicsWorld::initVelocityArrays() { assert(mConstrainedPositions != nullptr); assert(mConstrainedOrientations != nullptr); - // Reset the velocities arrays - for (uint i=0; i::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { + mSplitLinearVelocities[i].setToZero(); mSplitAngularVelocities[i].setToZero(); - } - // Initialize the map of body indexes in the velocity arrays - mMapBodyToConstrainedVelocityIndex.clear(); - std::set::const_iterator it; - uint indexBody = 0; - for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { - - // Add the body into the map - mMapBodyToConstrainedVelocityIndex.insert(std::make_pair(*it, indexBody)); - indexBody++; + (*it)->mArrayIndex = i++; } } @@ -289,7 +281,7 @@ void DynamicsWorld::integrateRigidBodiesVelocities() { for (uint b=0; b < mIslands[i]->getNbBodies(); b++) { // Insert the body into the map of constrained velocities - uint indexBody = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; + uint indexBody = bodies[b]->mArrayIndex; assert(mSplitLinearVelocities[indexBody] == Vector3(0, 0, 0)); assert(mSplitAngularVelocities[indexBody] == Vector3(0, 0, 0)); diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 366b2770..0c614b65 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -100,9 +100,6 @@ class DynamicsWorld : public CollisionWorld { /// Array of constrained rigid bodies orientation (for position error correction) Quaternion* mConstrainedOrientations; - /// Map body to their index in the constrained velocities array - std::map mMapBodyToConstrainedVelocityIndex; - /// Number of islands in the world uint mNbIslands; From 47869627d13d71f3997adea53731890e55a10de4 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 14 Dec 2017 20:24:19 +0100 Subject: [PATCH 08/10] Fix issue in Transform --- src/mathematics/Transform.h | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/mathematics/Transform.h b/src/mathematics/Transform.h index a5248fc2..aeaf4234 100644 --- a/src/mathematics/Transform.h +++ b/src/mathematics/Transform.h @@ -223,7 +223,11 @@ inline Vector3 Transform::operator*(const Vector3& vector) const { // Operator of multiplication of a transform with another one inline Transform Transform::operator*(const Transform& transform2) const { - const decimal prodX = mOrientation.w * transform2.mPosition.x + mOrientation.w * transform2.mPosition.z + // The following code is equivalent to this + //return Transform(mPosition + mOrientation * transform2.mPosition, + // mOrientation * transform2.mOrientation); + + const decimal prodX = mOrientation.w * transform2.mPosition.x + mOrientation.y * transform2.mPosition.z - mOrientation.z * transform2.mPosition.y; const decimal prodY = mOrientation.w * transform2.mPosition.y + mOrientation.z * transform2.mPosition.x - mOrientation.x * transform2.mPosition.z; From f2ee00ca68b409234b84d134cdc9845c9c55e2c3 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 14 Dec 2017 22:25:52 +0100 Subject: [PATCH 09/10] Use List instead of std::vector compute segment clipping in SAT algorithm --- src/body/RigidBody.cpp | 4 +- .../narrowphase/SAT/SATAlgorithm.cpp | 10 ++--- src/mathematics/mathematics_functions.cpp | 43 +++++++++++-------- src/mathematics/mathematics_functions.h | 7 +-- 4 files changed, 36 insertions(+), 28 deletions(-) diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 01e1694b..e4ce4458 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -39,10 +39,10 @@ using namespace reactphysics3d; * @param id The ID of the body */ RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id) - : CollisionBody(transform, world, id), mInitMass(decimal(1.0)), + : CollisionBody(transform, world, id), mArrayIndex(0), mInitMass(decimal(1.0)), mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()), mIsGravityEnabled(true), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)), - mJointsList(nullptr), mArrayIndex(0) { + mJointsList(nullptr) { // Compute the inverse mass mMassInverse = decimal(1.0) / mInitMass; diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 5e40f45f..2605eaf9 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -374,8 +374,8 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI uint firstEdgeIndex = face.edgeIndex; uint edgeIndex = firstEdgeIndex; - std::vector planesPoints; - std::vector planesNormals; + List planesPoints(mMemoryAllocator, 2); + List planesNormals(mMemoryAllocator, 2); // For each adjacent edge of the separating face of the polyhedron do { @@ -393,15 +393,15 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI Vector3 clipPlaneNormal = faceNormal.cross(edgeDirection); // Construct a clipping plane for each adjacent edge of the separating face of the polyhedron - planesPoints.push_back(polyhedron->getVertexPosition(edge.vertexIndex)); - planesNormals.push_back(clipPlaneNormal); + planesPoints.add(polyhedron->getVertexPosition(edge.vertexIndex)); + planesNormals.add(clipPlaneNormal); edgeIndex = edge.nextEdgeIndex; } while(edgeIndex != firstEdgeIndex); // First we clip the inner segment of the capsule with the four planes of the adjacent faces - std::vector clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, planesPoints, planesNormals); + List clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, planesPoints, planesNormals, mMemoryAllocator); // Project the two clipped points into the polyhedron face const Vector3 delta = faceNormal * (penetrationDepth - capsuleRadius); diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp index b82c9fbd..0d8994f4 100755 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -222,27 +222,34 @@ decimal reactphysics3d::computePointToLineDistance(const Vector3& linePointA, co // Clip a segment against multiple planes and return the clipped segment vertices // This method implements the Sutherland–Hodgman clipping algorithm -std::vector reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB, - const std::vector& planesPoints, - const std::vector& planesNormals) { +List reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB, + const List& planesPoints, + const List& planesNormals, + Allocator& allocator) { assert(planesPoints.size() == planesNormals.size()); - std::vector inputVertices = {segA, segB}; - std::vector outputVertices; + List list1(allocator, 2); + List list2(allocator, 2); + + List* inputVertices = &list1; + List* outputVertices = &list2; + + inputVertices->add(segA); + inputVertices->add(segB); // For each clipping plane for (uint p=0; psize() == 0) return *inputVertices; - assert(inputVertices.size() == 2); + assert(inputVertices->size() == 2); - outputVertices.clear(); + outputVertices->clear(); - Vector3& v1 = inputVertices[0]; - Vector3& v2 = inputVertices[1]; + Vector3& v1 = (*inputVertices)[0]; + Vector3& v2 = (*inputVertices)[1]; decimal v1DotN = (v1 - planesPoints[p]).dot(planesNormals[p]); decimal v2DotN = (v2 - planesPoints[p]).dot(planesNormals[p]); @@ -257,39 +264,40 @@ std::vector reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]); if (t >= 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); } } else { - outputVertices.push_back(v1); + outputVertices->add(v1); } // Add the second vertex - outputVertices.push_back(v2); + outputVertices->add(v2); } else { // If the second vertex is behind the clipping plane // If the first vertex is in front of the clippling plane if (v1DotN >= decimal(0.0)) { - outputVertices.push_back(v1); + outputVertices->add(v1); // The first point we keep is the intersection between the segment v1, v2 and the clipping plane decimal t = computePlaneSegmentIntersection(v1, v2, -planesNormals[p].dot(planesPoints[p]), -planesNormals[p]); if (t >= decimal(0.0) && t <= decimal(1.0)) { - outputVertices.push_back(v1 + t * (v2 - v1)); + outputVertices->add(v1 + t * (v2 - v1)); } } } inputVertices = outputVertices; + outputVertices = p % 2 == 0 ? &list1 : &list2; } - return outputVertices; + return *outputVertices; } // Clip a polygon against multiple planes and return the clipped polygon vertices @@ -302,7 +310,6 @@ List reactphysics3d::clipPolygonWithPlanes(const List& polygon uint nbMaxElements = polygonVertices.size() + planesPoints.size(); List list1(allocator, nbMaxElements); List list2(allocator, nbMaxElements); - List dotProducts(allocator, nbMaxElements * 2); const List* inputVertices = &polygonVertices; List* outputVertices = &list2; diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h index ff6ac6f4..5081d741 100755 --- a/src/mathematics/mathematics_functions.h +++ b/src/mathematics/mathematics_functions.h @@ -112,9 +112,10 @@ decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB decimal computePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point); /// Clip a segment against multiple planes and return the clipped segment vertices -std::vector clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB, - const std::vector& planesPoints, - const std::vector& planesNormals); +List clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB, + const List& planesPoints, + const List& planesNormals, + Allocator& allocator); /// Clip a polygon against multiple planes and return the clipped polygon vertices List clipPolygonWithPlanes(const List& polygonVertices, const List& planesPoints, From 8f126a75d6d38dd4152739fab4257bd0f427e514 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 27 Dec 2017 20:53:09 +0100 Subject: [PATCH 10/10] Use List in HalfEdgeStructure with some changes in memory allocation --- CMakeLists.txt | 2 + src/body/CollisionBody.cpp | 2 +- src/body/RigidBody.cpp | 2 +- src/collision/CollisionDetection.cpp | 7 + src/collision/ContactManifoldSet.cpp | 1 - src/collision/HalfEdgeStructure.cpp | 4 +- src/collision/HalfEdgeStructure.h | 29 +- src/collision/MiddlePhaseTriangleCallback.cpp | 2 +- src/collision/NarrowPhaseInfo.cpp | 6 +- src/collision/PolyhedronMesh.cpp | 11 +- src/collision/PolyhedronMesh.h | 1 + src/collision/ProxyShape.cpp | 6 +- src/collision/ProxyShape.h | 5 +- .../narrowphase/SAT/SATAlgorithm.cpp | 5 +- src/collision/shapes/BoxShape.cpp | 33 +- src/collision/shapes/BoxShape.h | 4 +- src/collision/shapes/CapsuleShape.cpp | 2 +- src/collision/shapes/CapsuleShape.h | 2 +- src/collision/shapes/CollisionShape.h | 2 +- src/collision/shapes/ConcaveMeshShape.cpp | 8 +- src/collision/shapes/ConcaveMeshShape.h | 7 +- src/collision/shapes/ConvexMeshShape.cpp | 2 +- src/collision/shapes/ConvexMeshShape.h | 2 +- src/collision/shapes/HeightFieldShape.cpp | 8 +- src/collision/shapes/HeightFieldShape.h | 7 +- src/collision/shapes/SphereShape.cpp | 2 +- src/collision/shapes/SphereShape.h | 2 +- src/collision/shapes/TriangleShape.cpp | 15 +- src/collision/shapes/TriangleShape.h | 7 +- src/containers/List.h | 105 ++-- src/engine/CollisionWorld.cpp | 1 - src/engine/CollisionWorld.h | 1 + src/engine/OverlappingPair.cpp | 20 +- src/memory/MemoryManager.cpp | 32 ++ src/memory/MemoryManager.h | 74 +++ src/memory/PoolAllocator.cpp | 2 +- src/memory/PoolAllocator.h | 2 +- test/tests/collision/TestCollisionWorld.h | 499 +++++++++++++----- test/tests/collision/TestDynamicAABBTree.h | 29 + test/tests/collision/TestHalfEdgeStructure.h | 47 +- test/tests/collision/TestRaycast.h | 4 +- .../mathematics/TestMathematicsFunctions.h | 18 +- 42 files changed, 730 insertions(+), 290 deletions(-) create mode 100644 src/memory/MemoryManager.cpp create mode 100644 src/memory/MemoryManager.h mode change 100644 => 100755 test/tests/collision/TestCollisionWorld.h mode change 100644 => 100755 test/tests/collision/TestDynamicAABBTree.h diff --git a/CMakeLists.txt b/CMakeLists.txt index e7e2df87..e585e0d1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -193,6 +193,8 @@ SET (REACTPHYSICS3D_SOURCES "src/memory/SingleFrameAllocator.h" "src/memory/SingleFrameAllocator.cpp" "src/memory/DefaultAllocator.h" + "src/memory/MemoryManager.h" + "src/memory/MemoryManager.cpp" "src/containers/Stack.h" "src/containers/LinkedList.h" "src/containers/List.h" diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index e588f93c..56493d28 100644 --- a/src/body/CollisionBody.cpp +++ b/src/body/CollisionBody.cpp @@ -72,7 +72,7 @@ ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape, // Create a new proxy collision shape to attach the collision shape to the body ProxyShape* proxyShape = new (mWorld.mPoolAllocator.allocate( sizeof(ProxyShape))) ProxyShape(this, collisionShape, - transform, decimal(1)); + transform, decimal(1), mWorld.mPoolAllocator); #ifdef IS_PROFILING_ACTIVE diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index e4ce4458..7419fc4c 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -226,7 +226,7 @@ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape, // Create a new proxy collision shape to attach the collision shape to the body ProxyShape* proxyShape = new (mWorld.mPoolAllocator.allocate( sizeof(ProxyShape))) ProxyShape(this, collisionShape, - transform, mass); + transform, mass, mWorld.mPoolAllocator); #ifdef IS_PROFILING_ACTIVE diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index c0b4d4e9..e57c5fd8 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -288,7 +288,14 @@ void CollisionDetection::computeNarrowPhase() { lastCollisionFrameInfo->isValid = true; } + NarrowPhaseInfo* narrowPhaseInfoToDelete = currentNarrowPhaseInfo; currentNarrowPhaseInfo = currentNarrowPhaseInfo->next; + + // Call the destructor + narrowPhaseInfoToDelete->~NarrowPhaseInfo(); + + // Release the allocated memory for the narrow phase info + mSingleFrameAllocator.release(narrowPhaseInfoToDelete, sizeof(NarrowPhaseInfo)); } // Convert the potential contact into actual contacts diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp index ce3e4492..5f70bbc0 100644 --- a/src/collision/ContactManifoldSet.cpp +++ b/src/collision/ContactManifoldSet.cpp @@ -225,7 +225,6 @@ void ContactManifoldSet::removeManifold(ContactManifold* manifold) { // Delete the contact manifold manifold->~ContactManifold(); mMemoryAllocator.release(manifold, sizeof(ContactManifold)); - mNbManifolds--; } diff --git a/src/collision/HalfEdgeStructure.cpp b/src/collision/HalfEdgeStructure.cpp index 0f7b8f6e..70a731d5 100644 --- a/src/collision/HalfEdgeStructure.cpp +++ b/src/collision/HalfEdgeStructure.cpp @@ -96,8 +96,8 @@ void HalfEdgeStructure::init() { mapEdgeToIndex.insert(std::make_pair(pairV1V2, edgeIndex + 1)); mapEdgeToIndex.insert(std::make_pair(pairV2V1, edgeIndex)); - mEdges.push_back(itEdge->second); - mEdges.push_back(edge); + mEdges.add(itEdge->second); + mEdges.add(edge); } currentFaceEdges.push_back(pairV1V2); diff --git a/src/collision/HalfEdgeStructure.h b/src/collision/HalfEdgeStructure.h index ec4fff35..e398913f 100644 --- a/src/collision/HalfEdgeStructure.h +++ b/src/collision/HalfEdgeStructure.h @@ -50,14 +50,14 @@ class HalfEdgeStructure { }; struct Face { - uint edgeIndex; // Index of an half-edge of the face - std::vector faceVertices; // Index of the vertices of the face + uint edgeIndex; // Index of an half-edge of the face + List faceVertices; // Index of the vertices of the face /// Constructor - Face() {} + Face(Allocator& allocator) : faceVertices(allocator) {} /// Constructor - Face(std::vector vertices) : faceVertices(vertices) {} + Face(List vertices) : faceVertices(vertices) {} }; struct Vertex { @@ -70,19 +70,24 @@ class HalfEdgeStructure { private: + /// Reference to a memory allocator + Allocator& mAllocator; + /// All the faces - std::vector mFaces; + List mFaces; /// All the vertices - std::vector mVertices; + List mVertices; /// All the half-edges - std::vector mEdges; + List mEdges; public: /// Constructor - HalfEdgeStructure() = default; + HalfEdgeStructure(Allocator& allocator, uint facesCapacity, uint verticesCapacity, + uint edgesCapacity) :mAllocator(allocator), mFaces(allocator, facesCapacity), + mVertices(allocator, verticesCapacity), mEdges(allocator, edgesCapacity) {} /// Destructor ~HalfEdgeStructure() = default; @@ -94,7 +99,7 @@ class HalfEdgeStructure { uint addVertex(uint vertexPointIndex); /// Add a face - void addFace(std::vector faceVertices); + void addFace(List faceVertices); /// Return the number of faces uint getNbFaces() const; @@ -119,16 +124,16 @@ class HalfEdgeStructure { // Add a vertex inline uint HalfEdgeStructure::addVertex(uint vertexPointIndex) { Vertex vertex(vertexPointIndex); - mVertices.push_back(vertex); + mVertices.add(vertex); return mVertices.size() - 1; } // Add a face -inline void HalfEdgeStructure::addFace(std::vector faceVertices) { +inline void HalfEdgeStructure::addFace(List faceVertices) { // Create a new face Face face(faceVertices); - mFaces.push_back(face); + mFaces.add(face); } // Return the number of faces diff --git a/src/collision/MiddlePhaseTriangleCallback.cpp b/src/collision/MiddlePhaseTriangleCallback.cpp index f8736a0b..a53271a4 100644 --- a/src/collision/MiddlePhaseTriangleCallback.cpp +++ b/src/collision/MiddlePhaseTriangleCallback.cpp @@ -34,7 +34,7 @@ void MiddlePhaseTriangleCallback::testTriangle(const Vector3* trianglePoints, co // Create a triangle collision shape (the allocated memory for the TriangleShape will be released in the // destructor of the corresponding NarrowPhaseInfo. TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape))) - TriangleShape(trianglePoints, verticesNormals, shapeId); + TriangleShape(trianglePoints, verticesNormals, shapeId, mAllocator); #ifdef IS_PROFILING_ACTIVE diff --git a/src/collision/NarrowPhaseInfo.cpp b/src/collision/NarrowPhaseInfo.cpp index a6440367..0189db99 100644 --- a/src/collision/NarrowPhaseInfo.cpp +++ b/src/collision/NarrowPhaseInfo.cpp @@ -52,11 +52,13 @@ NarrowPhaseInfo::~NarrowPhaseInfo() { // Release the memory of the TriangleShape (this memory was allocated in the // MiddlePhaseTriangleCallback::testTriangle() method) if (collisionShape1->getName() == CollisionShapeName::TRIANGLE) { + collisionShape1->~CollisionShape(); collisionShapeAllocator.release(collisionShape1, sizeof(TriangleShape)); } if (collisionShape2->getName() == CollisionShapeName::TRIANGLE) { - collisionShapeAllocator.release(collisionShape2, sizeof(TriangleShape)); - } + collisionShape2->~CollisionShape(); + collisionShapeAllocator.release(collisionShape2, sizeof(TriangleShape)); + } } // Add a new contact point diff --git a/src/collision/PolyhedronMesh.cpp b/src/collision/PolyhedronMesh.cpp index ec8b661d..08a84e7d 100644 --- a/src/collision/PolyhedronMesh.cpp +++ b/src/collision/PolyhedronMesh.cpp @@ -25,6 +25,7 @@ // Libraries #include "PolyhedronMesh.h" +#include "memory/MemoryManager.h" using namespace reactphysics3d; @@ -34,7 +35,11 @@ using namespace reactphysics3d; * Create a polyhedron mesh given an array of polygons. * @param polygonVertexArray Pointer to the array of polygons and their vertices */ -PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray) { +PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray) + : mHalfEdgeStructure(MemoryManager::getDefaultAllocator(), + polygonVertexArray->getNbFaces(), + polygonVertexArray->getNbVertices(), + (polygonVertexArray->getNbFaces() + polygonVertexArray->getNbVertices() - 2) * 2) { mPolygonVertexArray = polygonVertexArray; @@ -70,11 +75,11 @@ void PolyhedronMesh::createHalfEdgeStructure() { // Get the polygon face PolygonVertexArray::PolygonFace* face = mPolygonVertexArray->getPolygonFace(f); - std::vector faceVertices; + List faceVertices(MemoryManager::getDefaultAllocator(), face->nbVertices); // For each vertex of the face for (uint v=0; v < face->nbVertices; v++) { - faceVertices.push_back(mPolygonVertexArray->getVertexIndexInFace(f, v)); + faceVertices.add(mPolygonVertexArray->getVertexIndexInFace(f, v)); } assert(faceVertices.size() >= 3); diff --git a/src/collision/PolyhedronMesh.h b/src/collision/PolyhedronMesh.h index a64db670..b459723a 100644 --- a/src/collision/PolyhedronMesh.h +++ b/src/collision/PolyhedronMesh.h @@ -30,6 +30,7 @@ #include "mathematics/mathematics.h" #include "HalfEdgeStructure.h" #include "collision/PolygonVertexArray.h" +#include "memory/DefaultAllocator.h" #include namespace reactphysics3d { diff --git a/src/collision/ProxyShape.cpp b/src/collision/ProxyShape.cpp index 6e6eefbd..f782b2a8 100644 --- a/src/collision/ProxyShape.cpp +++ b/src/collision/ProxyShape.cpp @@ -35,9 +35,9 @@ using namespace reactphysics3d; * @param transform Transformation from collision shape local-space to body local-space * @param mass Mass of the collision shape (in kilograms) */ -ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, decimal mass) +ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, decimal mass, Allocator& allocator) :mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass), - mNext(nullptr), mBroadPhaseID(-1), mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF) { + mNext(nullptr), mBroadPhaseID(-1), mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF), mAllocator(allocator) { } @@ -76,7 +76,7 @@ bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) { worldToLocalTransform * ray.point2, ray.maxFraction); - bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this); + bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this, mAllocator); // Convert the raycast info into world-space raycastInfo.worldPoint = localToWorldTransform * raycastInfo.worldPoint; diff --git a/src/collision/ProxyShape.h b/src/collision/ProxyShape.h index 8e9a9e9f..0ba34698 100644 --- a/src/collision/ProxyShape.h +++ b/src/collision/ProxyShape.h @@ -82,6 +82,9 @@ class ProxyShape { /// proxy shape will collide with every collision categories by default. unsigned short mCollideWithMaskBits; + /// Memory allocator + Allocator& mAllocator; + #ifdef IS_PROFILING_ACTIVE /// Pointer to the profiler @@ -100,7 +103,7 @@ class ProxyShape { /// Constructor ProxyShape(CollisionBody* body, CollisionShape* shape, - const Transform& transform, decimal mass); + const Transform& transform, decimal mass, Allocator& allocator); /// Destructor virtual ~ProxyShape(); diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 2605eaf9..1992cbc7 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -835,9 +835,8 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene 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); + for (uint i=0; i < incidentFace.faceVertices.size(); i++) { + const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(incidentFace.faceVertices[i]); polygonVertices.add(incidentToReferenceTransform * faceVertexIncidentSpace); } diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index 37667958..079b15c5 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/src/collision/shapes/BoxShape.cpp @@ -27,6 +27,7 @@ #include "BoxShape.h" #include "collision/ProxyShape.h" #include "configuration.h" +#include "memory/MemoryManager.h" #include #include @@ -37,7 +38,9 @@ using namespace reactphysics3d; * @param extent The vector with the three extents of the box (in meters) */ BoxShape::BoxShape(const Vector3& extent) - : ConvexPolyhedronShape(CollisionShapeName::BOX), mExtent(extent) { + : ConvexPolyhedronShape(CollisionShapeName::BOX), mExtent(extent), + mHalfEdgeStructure(MemoryManager::getDefaultAllocator(), 6, 8, 24) { + assert(extent.x > decimal(0.0)); assert(extent.y > decimal(0.0)); assert(extent.z > decimal(0.0)); @@ -52,19 +55,21 @@ BoxShape::BoxShape(const Vector3& extent) mHalfEdgeStructure.addVertex(6); mHalfEdgeStructure.addVertex(7); + DefaultAllocator& allocator = MemoryManager::getDefaultAllocator(); + // Faces - std::vector face0; - face0.push_back(0); face0.push_back(1); face0.push_back(2); face0.push_back(3); - std::vector face1; - face1.push_back(1); face1.push_back(5); face1.push_back(6); face1.push_back(2); - std::vector face2; - face2.push_back(4); face2.push_back(7); face2.push_back(6); face2.push_back(5); - std::vector face3; - face3.push_back(4); face3.push_back(0); face3.push_back(3); face3.push_back(7); - std::vector face4; - face4.push_back(4); face4.push_back(5); face4.push_back(1); face4.push_back(0); - std::vector face5; - face5.push_back(2); face5.push_back(6); face5.push_back(7); face5.push_back(3); + List face0(allocator, 4); + face0.add(0); face0.add(1); face0.add(2); face0.add(3); + List face1(allocator, 4); + face1.add(1); face1.add(5); face1.add(6); face1.add(2); + List face2(allocator, 4); + face2.add(4); face2.add(7); face2.add(6); face2.add(5); + List face3(allocator, 4); + face3.add(4); face3.add(0); face3.add(3); face3.add(7); + List face4(allocator, 4); + face4.add(4); face4.add(5); face4.add(1); face4.add(0); + List face5(allocator, 4); + face5.add(2); face5.add(6); face5.add(7); face5.add(3); mHalfEdgeStructure.addFace(face0); mHalfEdgeStructure.addFace(face1); @@ -93,7 +98,7 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const } // Raycast method with feedback information -bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { +bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const { Vector3 rayDirection = ray.point2 - ray.point1; decimal tMin = DECIMAL_SMALLEST; diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h index d1aa9810..b3e094bd 100644 --- a/src/collision/shapes/BoxShape.h +++ b/src/collision/shapes/BoxShape.h @@ -31,7 +31,7 @@ #include "ConvexPolyhedronShape.h" #include "body/CollisionBody.h" #include "mathematics/mathematics.h" - +#include "memory/DefaultAllocator.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -64,7 +64,7 @@ class BoxShape : public ConvexPolyhedronShape { virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; diff --git a/src/collision/shapes/CapsuleShape.cpp b/src/collision/shapes/CapsuleShape.cpp index 32ea146c..da29fede 100644 --- a/src/collision/shapes/CapsuleShape.cpp +++ b/src/collision/shapes/CapsuleShape.cpp @@ -85,7 +85,7 @@ bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyS } // Raycast method with feedback information -bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { +bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const { const Vector3 n = ray.point2 - ray.point1; diff --git a/src/collision/shapes/CapsuleShape.h b/src/collision/shapes/CapsuleShape.h index 8aebff58..0da2d1f4 100644 --- a/src/collision/shapes/CapsuleShape.h +++ b/src/collision/shapes/CapsuleShape.h @@ -62,7 +62,7 @@ class CapsuleShape : public ConvexShape { virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override; /// Raycasting method between a ray one of the two spheres end cap of the capsule bool raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2, diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index 6792cc09..4381b05c 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -87,7 +87,7 @@ class CollisionShape { virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0; /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const=0; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const=0; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const = 0; diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp index ee90ce46..0b49f20c 100644 --- a/src/collision/shapes/ConcaveMeshShape.cpp +++ b/src/collision/shapes/ConcaveMeshShape.cpp @@ -111,12 +111,12 @@ void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB& // Raycast method with feedback information /// Note that only the first triangle hit by the ray in the mesh will be returned, even if /// the ray hits many triangles. -bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { +bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const { PROFILE("ConcaveMeshShape::raycast()", mProfiler); // Create the callback object that will compute ray casting against triangles - ConcaveMeshRaycastCallback raycastCallback(mDynamicAABBTree, *this, proxyShape, raycastInfo, ray); + ConcaveMeshRaycastCallback raycastCallback(mDynamicAABBTree, *this, proxyShape, raycastInfo, ray, allocator); #ifdef IS_PROFILING_ACTIVE @@ -180,7 +180,7 @@ void ConcaveMeshRaycastCallback::raycastTriangles() { mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals); // Create a triangle collision shape - TriangleShape triangleShape(trianglePoints, verticesNormals, mConcaveMeshShape.computeTriangleShapeId(data[0], data[1])); + TriangleShape triangleShape(trianglePoints, verticesNormals, mConcaveMeshShape.computeTriangleShapeId(data[0], data[1]), mAllocator); triangleShape.setRaycastTestType(mConcaveMeshShape.getRaycastTestType()); #ifdef IS_PROFILING_ACTIVE @@ -192,7 +192,7 @@ void ConcaveMeshRaycastCallback::raycastTriangles() { // Ray casting test against the collision shape RaycastInfo raycastInfo; - bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape); + bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape, mAllocator); // If the ray hit the collision shape if (isTriangleHit && raycastInfo.hitFraction <= smallestHitFraction) { diff --git a/src/collision/shapes/ConcaveMeshShape.h b/src/collision/shapes/ConcaveMeshShape.h index 2d0a3e9c..52d49a02 100644 --- a/src/collision/shapes/ConcaveMeshShape.h +++ b/src/collision/shapes/ConcaveMeshShape.h @@ -77,6 +77,7 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback { RaycastInfo& mRaycastInfo; const Ray& mRay; bool mIsHit; + Allocator& mAllocator; #ifdef IS_PROFILING_ACTIVE @@ -89,9 +90,9 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback { // Constructor ConcaveMeshRaycastCallback(const DynamicAABBTree& dynamicAABBTree, const ConcaveMeshShape& concaveMeshShape, - ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray) + ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray, Allocator& allocator) : mDynamicAABBTree(dynamicAABBTree), mConcaveMeshShape(concaveMeshShape), mProxyShape(proxyShape), - mRaycastInfo(raycastInfo), mRay(ray), mIsHit(false) { + mRaycastInfo(raycastInfo), mRay(ray), mIsHit(false), mAllocator(allocator) { } @@ -141,7 +142,7 @@ class ConcaveMeshShape : public ConcaveShape { // -------------------- Methods -------------------- // /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp index f6011375..19ef5016 100644 --- a/src/collision/shapes/ConvexMeshShape.cpp +++ b/src/collision/shapes/ConvexMeshShape.cpp @@ -112,7 +112,7 @@ void ConvexMeshShape::recalculateBounds() { } // Raycast method with feedback information -bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { +bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const { return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast( ray, proxyShape, raycastInfo); } diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index 0ce696af..1c33d919 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/src/collision/shapes/ConvexMeshShape.h @@ -77,7 +77,7 @@ class ConvexMeshShape : public ConvexPolyhedronShape { virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; diff --git a/src/collision/shapes/HeightFieldShape.cpp b/src/collision/shapes/HeightFieldShape.cpp index 74276a75..f067a827 100644 --- a/src/collision/shapes/HeightFieldShape.cpp +++ b/src/collision/shapes/HeightFieldShape.cpp @@ -212,14 +212,14 @@ void HeightFieldShape::computeMinMaxGridCoordinates(int* minCoords, int* maxCoor // Raycast method with feedback information /// Note that only the first triangle hit by the ray in the mesh will be returned, even if /// the ray hits many triangles. -bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { +bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const { // TODO : Implement raycasting without using an AABB for the ray // but using a dynamic AABB tree or octree instead PROFILE("HeightFieldShape::raycast()", mProfiler); - TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this); + TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this, allocator); #ifdef IS_PROFILING_ACTIVE @@ -266,7 +266,7 @@ Vector3 HeightFieldShape::getVertexAt(int x, int y) const { void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) { // Create a triangle collision shape - TriangleShape triangleShape(trianglePoints, verticesNormals, shapeId); + TriangleShape triangleShape(trianglePoints, verticesNormals, shapeId, mAllocator); triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType()); #ifdef IS_PROFILING_ACTIVE @@ -278,7 +278,7 @@ void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints, const // Ray casting test against the collision shape RaycastInfo raycastInfo; - bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape); + bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape, mAllocator); // If the ray hit the collision shape if (isTriangleHit && raycastInfo.hitFraction <= mSmallestHitFraction) { diff --git a/src/collision/shapes/HeightFieldShape.h b/src/collision/shapes/HeightFieldShape.h index f8b498d5..4190c3c7 100644 --- a/src/collision/shapes/HeightFieldShape.h +++ b/src/collision/shapes/HeightFieldShape.h @@ -49,6 +49,7 @@ class TriangleOverlapCallback : public TriangleCallback { bool mIsHit; decimal mSmallestHitFraction; const HeightFieldShape& mHeightFieldShape; + Allocator& mAllocator; #ifdef IS_PROFILING_ACTIVE @@ -61,9 +62,9 @@ class TriangleOverlapCallback : public TriangleCallback { // Constructor TriangleOverlapCallback(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo, - const HeightFieldShape& heightFieldShape) + const HeightFieldShape& heightFieldShape, Allocator& allocator) : mRay(ray), mProxyShape(proxyShape), mRaycastInfo(raycastInfo), - mHeightFieldShape (heightFieldShape) { + mHeightFieldShape (heightFieldShape), mAllocator(allocator) { mIsHit = false; mSmallestHitFraction = mRay.maxFraction; } @@ -143,7 +144,7 @@ class HeightFieldShape : public ConcaveShape { // -------------------- Methods -------------------- // /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; diff --git a/src/collision/shapes/SphereShape.cpp b/src/collision/shapes/SphereShape.cpp index fa6b4f6a..7d155f97 100644 --- a/src/collision/shapes/SphereShape.cpp +++ b/src/collision/shapes/SphereShape.cpp @@ -41,7 +41,7 @@ SphereShape::SphereShape(decimal radius) } // Raycast method with feedback information -bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { +bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const { const Vector3 m = ray.point1; decimal c = m.dot(m) - mMargin * mMargin; diff --git a/src/collision/shapes/SphereShape.h b/src/collision/shapes/SphereShape.h index 70f651de..18883d79 100644 --- a/src/collision/shapes/SphereShape.h +++ b/src/collision/shapes/SphereShape.h @@ -55,7 +55,7 @@ class SphereShape : public ConvexShape { virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp index e43cc674..4a7eb013 100644 --- a/src/collision/shapes/TriangleShape.cpp +++ b/src/collision/shapes/TriangleShape.cpp @@ -33,6 +33,7 @@ using namespace reactphysics3d; + // Constructor /** * Do not use this constructor. It is supposed to be used internally only. @@ -43,8 +44,9 @@ using namespace reactphysics3d; * @param verticesNormals The three vertices normals for smooth mesh collision * @param margin The collision margin (in meters) around the collision shape */ -TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId) - : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE) { +TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId, + Allocator& allocator) + : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE), mFaces{HalfEdgeStructure::Face(allocator), HalfEdgeStructure::Face(allocator)} { mPoints[0] = vertices[0]; mPoints[1] = vertices[1]; @@ -60,9 +62,10 @@ TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNor // 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].faceVertices.reserve(3); + mFaces[i].faceVertices.add(0); + mFaces[i].faceVertices.add(1); + mFaces[i].faceVertices.add(2); mFaces[i].edgeIndex = i; } @@ -208,7 +211,7 @@ Vector3 TriangleShape::computeSmoothLocalContactNormalForTriangle(const Vector3& // Raycast method with feedback information /// This method use the line vs triangle raycasting technique described in /// Real-time Collision Detection by Christer Ericson. -bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { +bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const { PROFILE("TriangleShape::raycast()", mProfiler); diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h index a4ce7726..9a2c756e 100644 --- a/src/collision/shapes/TriangleShape.h +++ b/src/collision/shapes/TriangleShape.h @@ -60,6 +60,7 @@ class TriangleShape : public ConvexPolyhedronShape { // -------------------- Attribute -------------------- // + /// Three points of the triangle Vector3 mPoints[3]; @@ -90,7 +91,8 @@ class TriangleShape : public ConvexPolyhedronShape { virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, + Allocator& allocator) const override; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; @@ -110,7 +112,8 @@ class TriangleShape : public ConvexPolyhedronShape { // -------------------- Methods -------------------- // /// Constructor - TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId); + TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, + uint shapeId, Allocator& allocator); /// Destructor virtual ~TriangleShape() override = default; diff --git a/src/containers/List.h b/src/containers/List.h index 4915f32d..71ae7ef7 100644 --- a/src/containers/List.h +++ b/src/containers/List.h @@ -44,59 +44,38 @@ class List { // -------------------- Attributes -------------------- // - /// Pointer to the first element of the list - T* mElements; + /// Buffer for the list elements + void* mBuffer; /// Number of elements in the list - uint mSize; + size_t mSize; /// Number of allocated elements in the list - uint mCapacity; + size_t mCapacity; /// Memory allocator Allocator& mAllocator; // -------------------- Methods -------------------- // - /// Allocate more memory for the elements of the list - void allocateMemory(uint nbElementsToAllocate) { - - assert(nbElementsToAllocate > mCapacity); - - // Allocate memory for the new array - void* newMemory = mAllocator.allocate(nbElementsToAllocate * sizeof(T)); - - if (mElements != nullptr) { - - // Copy the elements to the new allocated memory location - std::memcpy(newMemory, static_cast(mElements), mSize * sizeof(T)); - - // Release the previously allocated memory - mAllocator.release(mElements, mCapacity * sizeof(T)); - } - - mElements = static_cast(newMemory); - - mCapacity = nbElementsToAllocate; - } public: // -------------------- Methods -------------------- // /// Constructor - List(Allocator& allocator, uint capacity = 0) - : mElements(nullptr), mSize(0), mCapacity(0), mAllocator(allocator) { + List(Allocator& allocator, size_t capacity = 0) + : mBuffer(nullptr), mSize(0), mCapacity(0), mAllocator(allocator) { if (capacity > 0) { // Allocate memory - allocateMemory(capacity); + reserve(capacity); } } /// Copy constructor - List(const List& list) : mElements(nullptr), mSize(0), mCapacity(0), mAllocator(list.mAllocator) { + List(const List& list) : mBuffer(nullptr), mSize(0), mCapacity(0), mAllocator(list.mAllocator) { // All all the elements of the list to the current one addRange(list); @@ -112,22 +91,66 @@ class List { clear(); // Release the memory allocated on the heap - mAllocator.release(static_cast(mElements), mCapacity * sizeof(T)); + mAllocator.release(mBuffer, mCapacity * sizeof(T)); } } + /// Allocate memory for a given number of elements + void reserve(size_t capacity) { + + if (capacity <= mCapacity) return; + + // Allocate memory for the new array + void* newMemory = mAllocator.allocate(capacity * sizeof(T)); + + if (mBuffer != nullptr) { + + // Copy the elements to the new allocated memory location + std::memcpy(newMemory, mBuffer, mSize * sizeof(T)); + + // Release the previously allocated memory + mAllocator.release(mBuffer, mCapacity * sizeof(T)); + } + + mBuffer = newMemory; + assert(mBuffer != nullptr); + + mCapacity = capacity; + } + /// Add an element into the list void add(const T& element) { // If we need to allocate more memory if (mSize == mCapacity) { - allocateMemory(mCapacity == 0 ? 1 : mCapacity * 2); + reserve(mCapacity == 0 ? 1 : mCapacity * 2); } - mElements[mSize] = element; + // Use the copy-constructor to construct the element + new (static_cast(mBuffer) + mSize * sizeof(T)) T(element); + mSize++; } + /// Remove an element from the list at a given index + void remove(uint index) { + + assert(index >= 0 && index < mSize); + + // Call the destructor + (static_cast(mBuffer)[index]).~T(); + + mSize--; + + if (index != mSize) { + + // Move the elements to fill in the empty slot + char* dest = static_cast(mBuffer) + index * sizeof(T); + char* src = dest + sizeof(T); + std::memcpy(static_cast(dest), static_cast(src), (mSize - index) * sizeof(T)); + } + } + /// Append another list to the current one void addRange(const List& list) { @@ -135,12 +158,13 @@ class List { if (mSize + list.size() > mCapacity) { // Allocate memory - allocateMemory(mSize + list.size()); + reserve(mSize + list.size()); } // Add the elements of the list to the current one for(uint i=0; i(mBuffer) + mSize * sizeof(T)) T(list[i]); mSize++; } } @@ -150,27 +174,32 @@ class List { // Call the destructor of each element for (uint i=0; i < mSize; i++) { - mElements[i].~T(); + (static_cast(mBuffer)[i]).~T(); } mSize = 0; } /// Return the number of elments in the list - uint size() const { + size_t size() const { return mSize; } + /// Return the capacity of the list + size_t capacity() const { + return mCapacity; + } + /// Overloaded index operator T& operator[](const uint index) { assert(index >= 0 && index < mSize); - return mElements[index]; + return (static_cast(mBuffer)[index]); } /// Overloaded const index operator const T& operator[](const uint index) const { assert(index >= 0 && index < mSize); - return mElements[index]; + return (static_cast(mBuffer)[index]); } /// Overloaded assignment operator diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index eeb06305..8b29e4a1 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -187,4 +187,3 @@ AABB CollisionWorld::getWorldAABB(const ProxyShape* proxyShape) const { return mCollisionDetection.getWorldAABB(proxyShape); } - diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index 6c558d6d..ead42710 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -39,6 +39,7 @@ #include "collision/CollisionDetection.h" #include "constraint/Joint.h" #include "constraint/ContactPoint.h" +#include "memory/DefaultAllocator.h" #include "memory/PoolAllocator.h" #include "EventListener.h" diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp index eba2fb80..431476ed 100644 --- a/src/engine/OverlappingPair.cpp +++ b/src/engine/OverlappingPair.cpp @@ -116,20 +116,16 @@ void OverlappingPair::addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo // Clear all the potential contact manifolds void OverlappingPair::clearPotentialContactManifolds() { - // Do we need to release memory - if (mTempMemoryAllocator.isReleaseNeeded()) { + ContactManifoldInfo* element = mPotentialContactManifolds; + while(element != nullptr) { - ContactManifoldInfo* element = mPotentialContactManifolds; - while(element != nullptr) { + // Remove the proxy collision shape + ContactManifoldInfo* elementToRemove = element; + element = element->getNext(); - // Remove the proxy collision shape - ContactManifoldInfo* elementToRemove = element; - element = element->getNext(); - - // Delete the element - elementToRemove->~ContactManifoldInfo(); - mTempMemoryAllocator.release(elementToRemove, sizeof(ContactManifoldInfo)); - } + // Delete the element + elementToRemove->~ContactManifoldInfo(); + mTempMemoryAllocator.release(elementToRemove, sizeof(ContactManifoldInfo)); } mPotentialContactManifolds = nullptr; diff --git a/src/memory/MemoryManager.cpp b/src/memory/MemoryManager.cpp new file mode 100644 index 00000000..85832cb1 --- /dev/null +++ b/src/memory/MemoryManager.cpp @@ -0,0 +1,32 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2015 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "MemoryManager.h" + +using namespace reactphysics3d; + +// Static variables +DefaultAllocator MemoryManager::mDefaultAllocator; diff --git a/src/memory/MemoryManager.h b/src/memory/MemoryManager.h new file mode 100644 index 00000000..922a5a17 --- /dev/null +++ b/src/memory/MemoryManager.h @@ -0,0 +1,74 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_MEMORY_MANAGER_H +#define REACTPHYSICS3D_MEMORY_MANAGER_H + +// Libraries +#include "memory/DefaultAllocator.h" + +/// Namespace ReactPhysics3D +namespace reactphysics3d { + +// Class MemoryManager +/** + * The memory manager is used to store the different memory allocators that are used + * by the library. + */ +class MemoryManager { + + private: + + /// Default memory allocator + static DefaultAllocator mDefaultAllocator; + + public: + + /// Memory allocation types + enum class AllocationType { + Default, // Default memory allocator + Pool, // Memory pool allocator + Frame, // Single frame memory allocator + }; + + /// Constructor + MemoryManager(); + + /// Destructor + ~MemoryManager(); + + /// Return the default memory allocator + static DefaultAllocator& getDefaultAllocator(); +}; + +// Return the default memory allocator +inline DefaultAllocator& MemoryManager::getDefaultAllocator() { + return mDefaultAllocator; +} + +} + +#endif + diff --git a/src/memory/PoolAllocator.cpp b/src/memory/PoolAllocator.cpp index dc1306bc..53cee0a1 100644 --- a/src/memory/PoolAllocator.cpp +++ b/src/memory/PoolAllocator.cpp @@ -126,7 +126,7 @@ void* PoolAllocator::allocate(size_t size) { } else { // If there is no more free memory units in the corresponding heap - // If we need to allocate more memory to containsthe blocks + // If we need to allocate more memory to contains the blocks if (mNbCurrentMemoryBlocks == mNbAllocatedMemoryBlocks) { // Allocate more memory to contain the blocks diff --git a/src/memory/PoolAllocator.h b/src/memory/PoolAllocator.h index 0664e559..b06778d5 100644 --- a/src/memory/PoolAllocator.h +++ b/src/memory/PoolAllocator.h @@ -94,7 +94,7 @@ class PoolAllocator : public Allocator { /// Size of the memory units that each heap is responsible to allocate static size_t mUnitSizes[NB_HEAPS]; - /// Lookup table that mape size to allocate to the index of the + /// Lookup table that map the size to allocate to the index of the /// corresponding heap we will use for the allocation. static int mMapSizeToHeapIndex[MAX_UNIT_SIZE + 1]; diff --git a/test/tests/collision/TestCollisionWorld.h b/test/tests/collision/TestCollisionWorld.h old mode 100644 new mode 100755 index 678b5f21..ad0c92cf --- a/test/tests/collision/TestCollisionWorld.h +++ b/test/tests/collision/TestCollisionWorld.h @@ -40,21 +40,108 @@ enum CollisionCategory { CATEGORY_3 = 0x0004 }; -// TODO : Add test for concave shape collision here +// Contact point collision data +struct CollisionPointData { + + Vector3 localPointBody1; + Vector3 localPointBody2; + decimal penetrationDepth; + + CollisionPointData(const Vector3& point1, const Vector3& point2, decimal penDepth) { + localPointBody1 = point1; + localPointBody2 = point2; + penetrationDepth = penDepth; + } + + bool isContactPointSimilarTo(const Vector3& pointBody1, const Vector3& pointBody2, decimal penDepth, decimal epsilon = 0.001) { + + return approxEqual(pointBody1, localPointBody1, epsilon) && + approxEqual(pointBody2, localPointBody2, epsilon) && + approxEqual(penetrationDepth, penDepth, epsilon); + } +}; + +// Contact manifold collision data +struct CollisionManifoldData { + + std::vector contactPoints; + + int getNbContactPoints() const { + return contactPoints.size(); + } + + bool hasContactPointSimilarTo(const Vector3& localPointBody1, const Vector3& localPointBody2, decimal penetrationDepth, decimal epsilon = 0.001) { + + std::vector::iterator it; + for (it = contactPoints.begin(); it != contactPoints.end(); ++it) { + + if (it->isContactPointSimilarTo(localPointBody1, localPointBody2, penetrationDepth)) { + return true; + } + } + + return false; + } + +}; + +// Collision data between two proxy shapes +struct CollisionData { + + std::pair proxyShapes; + std::pair bodies; + std::vector contactManifolds; + + int getNbContactManifolds() const { + return contactManifolds.size(); + } + + int getTotalNbContactPoints() const { + + int nbPoints = 0; + + std::vector::const_iterator it; + for (it = contactManifolds.begin(); it != contactManifolds.end(); ++it) { + + nbPoints += it->getNbContactPoints(); + } + + return nbPoints; + } + + bool hasContactPointSimilarTo(const Vector3& localPointBody1, const Vector3& localPointBody2, decimal penetrationDepth, decimal epsilon = 0.001) { + + std::vector::iterator it; + for (it = contactManifolds.begin(); it != contactManifolds.end(); ++it) { + + if (it->hasContactPointSimilarTo(localPointBody1, localPointBody2, penetrationDepth)) { + return true; + } + } + + return false; + } + +}; // Class class WorldCollisionCallback : public CollisionCallback { + private: + + std::vector> mCollisionData; + + std::pair getCollisionKeyPair(std::pair pair) const { + + if (pair.first > pair.second) { + return std::make_pair(pair.second, pair.first); + } + + return pair; + } + public: - bool boxCollideWithSphere1; - bool sphere1CollideWithSphere2; - - CollisionBody* boxBody; - CollisionBody* sphere1Body; - CollisionBody* sphere2Body; - CollisionBody* cylinderBody; - WorldCollisionCallback() { reset(); @@ -62,30 +149,79 @@ class WorldCollisionCallback : public CollisionCallback void reset() { - boxCollideWithSphere1 = false; - sphere1CollideWithSphere2 = false; + mCollisionData.clear(); } - // This method will be called for contact + bool hasContacts() const { + return mCollisionData.size() > 0; + } + + bool areProxyShapesColliding(const ProxyShape* proxyShape1, const ProxyShape* proxyShape2) { + return std::find(mCollisionData.begin(), mCollisionData.end(), getCollisionKeyPair(std::make_pair(proxyShape1, proxyShape2))) != mCollisionData.end(); + } + + // This method will be called for each contact virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override { - if (isContactBetweenBodies(boxBody, sphere1Body, collisionCallbackInfo)) { - boxCollideWithSphere1 = true; - } - else if (isContactBetweenBodies(sphere1Body, sphere2Body, collisionCallbackInfo)) { - sphere1CollideWithSphere2 = true; - } - } + CollisionData collisionData; + collisionData.bodies = std::make_pair(collisionCallbackInfo.body1, collisionCallbackInfo.body2); + collisionData.proxyShapes = std::make_pair(collisionCallbackInfo.proxyShape1, collisionCallbackInfo.proxyShape2); - bool isContactBetweenBodies(const CollisionBody* body1, const CollisionBody* body2, - const CollisionCallbackInfo& collisionCallbackInfo) { - return (collisionCallbackInfo.body1->getID() == body1->getID() && - collisionCallbackInfo.body2->getID() == body2->getID()) || - (collisionCallbackInfo.body2->getID() == body1->getID() && - collisionCallbackInfo.body1->getID() == body2->getID()); + ContactManifoldListElement* element = collisionCallbackInfo.contactManifoldElements; + while (element != nullptr) { + + ContactManifold* contactManifold = element->getContactManifold(); + + CollisionManifoldData collisionManifold; + + ContactPoint* contactPoint = contactManifold->getContactPoints(); + while (contactPoint != nullptr) { + + CollisionPointData collisionPoint(contactPoint->getLocalPointOnShape1(), contactPoint->getLocalPointOnShape2(), contactPoint->getPenetrationDepth()); + collisionManifold.contactPoints.push_back(collisionPoint); + + contactPoint = contactPoint->getNext(); + } + + collisionData.contactManifolds.push_back(collisionManifold); + + element = element->getNext(); + } } }; +/// Overlap callback +class WorldOverlapCallback : public OverlapCallback { + + private: + + CollisionBody* mOverlapBody; + + public: + + /// Destructor + virtual ~WorldOverlapCallback() { + reset(); + } + + /// This method will be called for each reported overlapping bodies + virtual void notifyOverlap(CollisionBody* collisionBody) override { + + } + + void reset() { + mOverlapBody = nullptr; + } + + bool hasOverlap() const { + return mOverlapBody != nullptr; + } + + CollisionBody* getOverlapBody() { + return mOverlapBody; + } +}; + // Class TestCollisionWorld /** * Unit test for the CollisionWorld class. @@ -100,22 +236,29 @@ class TestCollisionWorld : public Test { CollisionWorld* mWorld; // Bodies - CollisionBody* mBoxBody; - CollisionBody* mSphere1Body; - CollisionBody* mSphere2Body; + CollisionBody* mBoxBody1; + CollisionBody* mBoxBody2; + CollisionBody* mSphereBody1; + CollisionBody* mSphereBody2; // Collision shapes - BoxShape* mBoxShape; - SphereShape* mSphereShape; + BoxShape* mBoxShape1; + BoxShape* mBoxShape2; + SphereShape* mSphereShape1; + SphereShape* mSphereShape2; // Proxy shapes - ProxyShape* mBoxProxyShape; - ProxyShape* mSphere1ProxyShape; - ProxyShape* mSphere2ProxyShape; + ProxyShape* mBoxProxyShape1; + ProxyShape* mBoxProxyShape2; + ProxyShape* mSphereProxyShape1; + ProxyShape* mSphereProxyShape2; - // Collision callback class + // Collision callback WorldCollisionCallback mCollisionCallback; + // Overlap callback + WorldOverlapCallback mOverlapCallback; + public : // ---------- Methods ---------- // @@ -123,147 +266,243 @@ class TestCollisionWorld : public Test { /// Constructor TestCollisionWorld(const std::string& name) : Test(name) { - // Create the world + // Create the collision world mWorld = new CollisionWorld(); - // Create the bodies - Transform boxTransform(Vector3(10, 0, 0), Quaternion::identity()); - mBoxBody = mWorld->createCollisionBody(boxTransform); - mBoxShape = new BoxShape(Vector3(3, 3, 3)); - mBoxProxyShape = mBoxBody->addCollisionShape(mBoxShape, Transform::identity()); + // ---------- Boxes ---------- // + Transform boxTransform1(Vector3(-20, 20, 0), Quaternion::identity()); + mBoxBody1 = mWorld->createCollisionBody(boxTransform1); + mBoxShape1 = new BoxShape(Vector3(3, 3, 3)); + mBoxProxyShape1 = mBoxBody1->addCollisionShape(mBoxShape1, Transform::identity()); - mSphereShape = new SphereShape(3.0); - Transform sphere1Transform(Vector3(10,5, 0), Quaternion::identity()); - mSphere1Body = mWorld->createCollisionBody(sphere1Transform); - mSphere1ProxyShape = mSphere1Body->addCollisionShape(mSphereShape, Transform::identity()); + Transform boxTransform2(Vector3(-10, 20, 0), Quaternion::identity()); + mBoxBody2 = mWorld->createCollisionBody(boxTransform2); + mBoxShape2 = new BoxShape(Vector3(2, 2, 2)); + mBoxProxyShape2 = mBoxBody2->addCollisionShape(mBoxShape1, Transform::identity()); - Transform sphere2Transform(Vector3(30, 10, 10), Quaternion::identity()); - mSphere2Body = mWorld->createCollisionBody(sphere2Transform); - mSphere2ProxyShape = mSphere2Body->addCollisionShape(mSphereShape, Transform::identity()); + // ---------- Spheres ---------- // + mSphereShape1 = new SphereShape(3.0); + Transform sphereTransform1(Vector3(10, 20, 0), Quaternion::identity()); + mSphereBody1 = mWorld->createCollisionBody(sphereTransform1); + mSphereProxyShape1 = mSphereBody1->addCollisionShape(mSphereShape1, Transform::identity()); - // Assign collision categories to proxy shapes - mBoxProxyShape->setCollisionCategoryBits(CATEGORY_1); - mSphere1ProxyShape->setCollisionCategoryBits(CATEGORY_1); - mSphere2ProxyShape->setCollisionCategoryBits(CATEGORY_2); - - mCollisionCallback.boxBody = mBoxBody; - mCollisionCallback.sphere1Body = mSphere1Body; - mCollisionCallback.sphere2Body = mSphere2Body; + mSphereShape2 = new SphereShape(5.0); + Transform sphereTransform2(Vector3(20, 20, 0), Quaternion::identity()); + mSphereBody2 = mWorld->createCollisionBody(sphereTransform2); + mSphereProxyShape2 = mSphereBody2->addCollisionShape(mSphereShape2, Transform::identity()); } /// Destructor virtual ~TestCollisionWorld() { - delete mBoxShape; - delete mSphereShape; + + delete mBoxShape1; + delete mBoxShape2; + + delete mSphereShape1; + delete mSphereShape2; + + delete mWorld; } /// Run the tests void run() { - testCollisions(); + testNoCollisions(); + testNoOverlap(); + testNoAABBOverlap(); + + testAABBOverlap(); + + testSphereVsSphereCollision(); + testSphereVsBoxCollision(); + + testMultipleCollisions(); } - void testCollisions() { + void testNoCollisions() { - mCollisionCallback.reset(); - mWorld->testCollision(&mCollisionCallback); - test(mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.sphere1CollideWithSphere2); + // All the shapes of the world are not touching when they are created. + // Here we test that at the beginning, there is no collision at all. - test(mWorld->testAABBOverlap(mBoxBody, mSphere1Body)); - test(!mWorld->testAABBOverlap(mSphere1Body, mSphere2Body)); + // ---------- Global test ---------- // - test(mBoxProxyShape->testAABBOverlap(mSphere1ProxyShape->getWorldAABB())); - test(!mSphere1ProxyShape->testAABBOverlap(mSphere2ProxyShape->getWorldAABB())); + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + test(!mCollisionCallback.hasContacts()); - mCollisionCallback.reset(); - test(!mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.sphere1CollideWithSphere2); + // ---------- Single body test ---------- // - mCollisionCallback.reset(); - mWorld->testCollision(mBoxBody, mSphere1Body, &mCollisionCallback); - test(mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.sphere1CollideWithSphere2); + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody1, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); - mCollisionCallback.reset(); - test(!mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.sphere1CollideWithSphere2); + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody2, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); - // Move sphere 1 to collide with sphere 2 - mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity())); + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody1, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); - mCollisionCallback.reset(); - mWorld->testCollision(&mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.sphere1CollideWithSphere2); + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody2, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); - mCollisionCallback.reset(); - mWorld->testCollision(mBoxBody, mSphere1Body, &mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.sphere1CollideWithSphere2); + // Two bodies test - mCollisionCallback.reset(); - test(!mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.sphere1CollideWithSphere2); + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody1, mBoxBody2, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); + + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody1, mSphereBody2, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); + + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody1, mSphereBody1, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); + + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody1, mSphereBody2, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); + + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody2, mSphereBody1, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); + + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody2, mSphereBody2, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); + + } + + void testNoOverlap() { + + // All the shapes of the world are not touching when they are created. + // Here we test that at the beginning, there is no overlap at all. + + // ---------- Single body test ---------- // + + mOverlapCallback.reset(); + mWorld->testOverlap(mBoxBody1, &mOverlapCallback); + test(!mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mBoxBody2, &mOverlapCallback); + test(!mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mSphereBody1, &mOverlapCallback); + test(!mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mSphereBody2, &mOverlapCallback); + test(!mOverlapCallback.hasOverlap()); + + // Two bodies test + + test(!mWorld->testOverlap(mBoxBody1, mBoxBody2)); + test(!mWorld->testOverlap(mSphereBody1, mSphereBody2)); + test(!mWorld->testOverlap(mBoxBody1, mSphereBody1)); + test(!mWorld->testOverlap(mBoxBody1, mSphereBody2)); + test(!mWorld->testOverlap(mBoxBody2, mSphereBody1)); + test(!mWorld->testOverlap(mBoxBody2, mSphereBody2)); + } + + void testNoAABBOverlap() { + + // All the shapes of the world are not touching when they are created. + // Here we test that at the beginning, there is no AABB overlap at all. + + // Two bodies test + + test(!mWorld->testAABBOverlap(mBoxBody1, mBoxBody2)); + test(!mWorld->testAABBOverlap(mSphereBody1, mSphereBody2)); + test(!mWorld->testAABBOverlap(mBoxBody1, mSphereBody1)); + test(!mWorld->testAABBOverlap(mBoxBody1, mSphereBody2)); + test(!mWorld->testAABBOverlap(mBoxBody2, mSphereBody1)); + test(!mWorld->testAABBOverlap(mBoxBody2, mSphereBody2)); + } + + void testAABBOverlap() { + + // TODO : Test the CollisionWorld::testAABBOverlap() method here + } + + void testSphereVsSphereCollision() { + + + + // Move sphere 1 to collide with sphere 2 + mSphereBody1->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity())); + + } + + void testSphereVsBoxCollision() { // Move sphere 1 to collide with box - mSphere1Body->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity())); + mSphereBody1->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity())); // --------- Test collision with inactive bodies --------- // mCollisionCallback.reset(); - mBoxBody->setIsActive(false); - mSphere1Body->setIsActive(false); - mSphere2Body->setIsActive(false); + mBoxBody1->setIsActive(false); + mSphereBody1->setIsActive(false); + mSphereBody2->setIsActive(false); mWorld->testCollision(&mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.sphere1CollideWithSphere2); + - test(!mWorld->testAABBOverlap(mBoxBody, mSphere1Body)); - test(!mWorld->testAABBOverlap(mSphere1Body, mSphere2Body)); - - test(!mBoxProxyShape->testAABBOverlap(mSphere1ProxyShape->getWorldAABB())); - test(!mSphere1ProxyShape->testAABBOverlap(mSphere2ProxyShape->getWorldAABB())); - - mBoxBody->setIsActive(true); - mSphere1Body->setIsActive(true); - mSphere2Body->setIsActive(true); + mBoxBody1->setIsActive(true); + mSphereBody1->setIsActive(true); + mSphereBody2->setIsActive(true); // --------- Test collision with collision filtering -------- // - mBoxProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_3); - mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_2); - mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_1); + //mBoxProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_3); + //mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_2); + //mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_1); - mCollisionCallback.reset(); - mWorld->testCollision(&mCollisionCallback); - test(mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.sphere1CollideWithSphere2); + //mCollisionCallback.reset(); + //mWorld->testCollision(&mCollisionCallback); + //test(mCollisionCallback.boxCollideWithSphere1); + //test(!mCollisionCallback.sphere1CollideWithSphere2); - // Move sphere 1 to collide with sphere 2 - mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity())); + //// Move sphere 1 to collide with sphere 2 + //mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity())); - mCollisionCallback.reset(); - mWorld->testCollision(&mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.sphere1CollideWithSphere2); + //mCollisionCallback.reset(); + //mWorld->testCollision(&mCollisionCallback); + //test(!mCollisionCallback.boxCollideWithSphere1); + //test(mCollisionCallback.sphere1CollideWithSphere2); - mBoxProxyShape->setCollideWithMaskBits(CATEGORY_2); - mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_2); - mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_3); + //mBoxProxyShape->setCollideWithMaskBits(CATEGORY_2); + //mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_2); + //mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_3); - mCollisionCallback.reset(); - mWorld->testCollision(&mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.sphere1CollideWithSphere2); + //mCollisionCallback.reset(); + //mWorld->testCollision(&mCollisionCallback); + //test(!mCollisionCallback.boxCollideWithSphere1); + //test(!mCollisionCallback.sphere1CollideWithSphere2); - // Move sphere 1 to collide with box - mSphere1Body->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity())); + //// Move sphere 1 to collide with box + //mSphere1Body->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity())); - mBoxProxyShape->setCollideWithMaskBits(0xFFFF); - mSphere1ProxyShape->setCollideWithMaskBits(0xFFFF); - mSphere2ProxyShape->setCollideWithMaskBits(0xFFFF); + //mBoxProxyShape->setCollideWithMaskBits(0xFFFF); + //mSphere1ProxyShape->setCollideWithMaskBits(0xFFFF); + //mSphere2ProxyShape->setCollideWithMaskBits(0xFFFF); } + + void testMultipleCollisions() { + + // TODO : Test collisions without categories set + + // TODO : Test colliisons with categories set + + // Assign collision categories to proxy shapes + //mBoxProxyShape->setCollisionCategoryBits(CATEGORY_1); + //mSphere1ProxyShape->setCollisionCategoryBits(CATEGORY_1); + //mSphere2ProxyShape->setCollisionCategoryBits(CATEGORY_2); + } }; } diff --git a/test/tests/collision/TestDynamicAABBTree.h b/test/tests/collision/TestDynamicAABBTree.h old mode 100644 new mode 100755 index 7ab3202b..53f5e618 --- a/test/tests/collision/TestDynamicAABBTree.h +++ b/test/tests/collision/TestDynamicAABBTree.h @@ -114,6 +114,12 @@ class TestDynamicAABBTree : public Test { // Dynamic AABB Tree DynamicAABBTree tree; + +#ifdef IS_PROFILING_ACTIVE + /// Pointer to the profiler + Profiler* profiler = new Profiler(); + tree.setProfiler(profiler); +#endif int object1Data = 56; int object2Data = 23; @@ -152,6 +158,10 @@ class TestDynamicAABBTree : public Test { test(*(int*)(tree.getNodeDataPointer(object2Id)) == object2Data); test(*(int*)(tree.getNodeDataPointer(object3Id)) == object3Data); test(*(int*)(tree.getNodeDataPointer(object4Id)) == object4Data); + +#ifdef IS_PROFILING_ACTIVE + delete profiler; +#endif } void testOverlapping() { @@ -161,6 +171,12 @@ class TestDynamicAABBTree : public Test { // Dynamic AABB Tree DynamicAABBTree tree; +#ifdef IS_PROFILING_ACTIVE + /// Pointer to the profiler + Profiler* profiler = new Profiler(); + tree.setProfiler(profiler); +#endif + int object1Data = 56; int object2Data = 23; int object3Data = 13; @@ -342,6 +358,9 @@ class TestDynamicAABBTree : public Test { test(!mOverlapCallback.isOverlapping(object3Id)); test(!mOverlapCallback.isOverlapping(object4Id)); +#ifdef IS_PROFILING_ACTIVE + delete profiler; +#endif } void testRaycast() { @@ -351,6 +370,12 @@ class TestDynamicAABBTree : public Test { // Dynamic AABB Tree DynamicAABBTree tree; +#ifdef IS_PROFILING_ACTIVE + /// Pointer to the profiler + Profiler* profiler = new Profiler(); + tree.setProfiler(profiler); +#endif + int object1Data = 56; int object2Data = 23; int object3Data = 13; @@ -513,6 +538,10 @@ class TestDynamicAABBTree : public Test { test(!mRaycastCallback.isHit(object2Id)); test(mRaycastCallback.isHit(object3Id)); test(mRaycastCallback.isHit(object4Id)); + +#ifdef IS_PROFILING_ACTIVE + delete profiler; +#endif } }; diff --git a/test/tests/collision/TestHalfEdgeStructure.h b/test/tests/collision/TestHalfEdgeStructure.h index 4c4a2b39..35f357b3 100644 --- a/test/tests/collision/TestHalfEdgeStructure.h +++ b/test/tests/collision/TestHalfEdgeStructure.h @@ -19,6 +19,9 @@ class TestHalfEdgeStructure : public Test { // ---------- Atributes ---------- // + /// Memory allocator + DefaultAllocator mAllocator; + public : @@ -43,7 +46,7 @@ class TestHalfEdgeStructure : public Test { void testCube() { // Create the half-edge structure for a cube - rp3d::HalfEdgeStructure cubeStructure; + rp3d::HalfEdgeStructure cubeStructure(mAllocator, 6, 8, 24); rp3d::Vector3 vertices[8] = { rp3d::Vector3(-0.5, -0.5, 0.5), @@ -67,18 +70,18 @@ class TestHalfEdgeStructure : public Test { cubeStructure.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; - face1.push_back(1); face1.push_back(5); face1.push_back(6); face1.push_back(2); - std::vector face2; - face2.push_back(5); face2.push_back(4); face2.push_back(7); face2.push_back(6); - std::vector face3; - face3.push_back(4); face3.push_back(0); face3.push_back(3); face3.push_back(7); - std::vector face4; - face4.push_back(0); face4.push_back(4); face4.push_back(5); face4.push_back(1); - std::vector face5; - face5.push_back(2); face5.push_back(6); face5.push_back(7); face5.push_back(3); + List face0(mAllocator, 4); + face0.add(0); face0.add(1); face0.add(2); face0.add(3); + List face1(mAllocator, 4); + face1.add(1); face1.add(5); face1.add(6); face1.add(2); + List face2(mAllocator, 4); + face2.add(5); face2.add(4); face2.add(7); face2.add(6); + List face3(mAllocator, 4); + face3.add(4); face3.add(0); face3.add(3); face3.add(7); + List face4(mAllocator, 4); + face4.add(0); face4.add(4); face4.add(5); face4.add(1); + List face5(mAllocator, 4); + face5.add(2); face5.add(6); face5.add(7); face5.add(3); cubeStructure.addFace(face0); cubeStructure.addFace(face1); @@ -168,7 +171,7 @@ class TestHalfEdgeStructure : public Test { // Create the half-edge structure for a tetrahedron std::vector> faces; - rp3d::HalfEdgeStructure tetrahedron; + rp3d::HalfEdgeStructure tetrahedron(mAllocator, 4, 4, 12); // Vertices rp3d::Vector3 vertices[4] = { @@ -184,14 +187,14 @@ class TestHalfEdgeStructure : public Test { tetrahedron.addVertex(3); // Faces - std::vector face0; - face0.push_back(0); face0.push_back(1); face0.push_back(2); - std::vector face1; - face1.push_back(0); face1.push_back(3); face1.push_back(1); - std::vector face2; - face2.push_back(1); face2.push_back(3); face2.push_back(2); - std::vector face3; - face3.push_back(0); face3.push_back(2); face3.push_back(3); + List face0(mAllocator, 3); + face0.add(0); face0.add(1); face0.add(2); + List face1(mAllocator, 3); + face1.add(0); face1.add(3); face1.add(1); + List face2(mAllocator, 3); + face2.add(1); face2.add(3); face2.add(2); + List face3(mAllocator, 3); + face3.add(0); face3.add(2); face3.add(3); tetrahedron.addFace(face0); tetrahedron.addFace(face1); diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h index 97dd002a..d02ab847 100644 --- a/test/tests/collision/TestRaycast.h +++ b/test/tests/collision/TestRaycast.h @@ -99,6 +99,8 @@ class TestRaycast : public Test { // Raycast callback class WorldRaycastCallback mCallback; + DefaultAllocator mAllocator; + // Epsilon decimal epsilon; @@ -203,7 +205,7 @@ class TestRaycast : public Test { triangleVertices[1] = Vector3(105, 100, 0); triangleVertices[2] = Vector3(100, 103, 0); Vector3 triangleVerticesNormals[3] = {Vector3(0, 0, 1), Vector3(0, 0, 1), Vector3(0, 0, 1)}; - mTriangleShape = new TriangleShape(triangleVertices, triangleVerticesNormals, 0); + mTriangleShape = new TriangleShape(triangleVertices, triangleVerticesNormals, 0, mAllocator); mTriangleProxyShape = mTriangleBody->addCollisionShape(mTriangleShape, mShapeTransform); mCapsuleShape = new CapsuleShape(2, 5); diff --git a/test/tests/mathematics/TestMathematicsFunctions.h b/test/tests/mathematics/TestMathematicsFunctions.h index 793a79fb..b76cb78c 100644 --- a/test/tests/mathematics/TestMathematicsFunctions.h +++ b/test/tests/mathematics/TestMathematicsFunctions.h @@ -176,13 +176,13 @@ class TestMathematicsFunctions : public Test { segmentVertices.push_back(Vector3(-6, 3, 0)); segmentVertices.push_back(Vector3(8, 3, 0)); - std::vector planesNormals; - std::vector planesPoints; - planesNormals.push_back(Vector3(-1, 0, 0)); - planesPoints.push_back(Vector3(4, 0, 0)); + List planesNormals(mAllocator, 2); + List planesPoints(mAllocator, 2); + planesNormals.add(Vector3(-1, 0, 0)); + planesPoints.add(Vector3(4, 0, 0)); - std::vector clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], - planesPoints, planesNormals); + List clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], + planesPoints, planesNormals, mAllocator); test(clipSegmentVertices.size() == 2); test(approxEqual(clipSegmentVertices[0].x, -6, 0.000001)); test(approxEqual(clipSegmentVertices[0].y, 3, 0.000001)); @@ -195,7 +195,7 @@ class TestMathematicsFunctions : public Test { segmentVertices.push_back(Vector3(8, 3, 0)); segmentVertices.push_back(Vector3(-6, 3, 0)); - clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals); + clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals, mAllocator); test(clipSegmentVertices.size() == 2); test(approxEqual(clipSegmentVertices[0].x, 4, 0.000001)); test(approxEqual(clipSegmentVertices[0].y, 3, 0.000001)); @@ -208,7 +208,7 @@ class TestMathematicsFunctions : public Test { segmentVertices.push_back(Vector3(-6, 3, 0)); segmentVertices.push_back(Vector3(3, 3, 0)); - clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals); + clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals, mAllocator); test(clipSegmentVertices.size() == 2); test(approxEqual(clipSegmentVertices[0].x, -6, 0.000001)); test(approxEqual(clipSegmentVertices[0].y, 3, 0.000001)); @@ -221,7 +221,7 @@ class TestMathematicsFunctions : public Test { segmentVertices.push_back(Vector3(5, 3, 0)); segmentVertices.push_back(Vector3(8, 3, 0)); - clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals); + clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals, mAllocator); test(clipSegmentVertices.size() == 0); // Test clipPolygonWithPlanes()