mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Many small optimizations

Browse Source
This commit is contained in:
Daniel Chappuis 2017-12-04 22:14:52 +01:00
parent 4cc024b85e
commit 4f76553c59
30 changed files with 235 additions and 204 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/collision/CollisionDetection.cpp
View File

@ -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();

1
src/collision/CollisionDetection.h
View File

@ -151,7 +151,6 @@ class CollisionDetection {
/// Process the potential contacts where one collion is a concave shape
void processSmoothMeshContacts(OverlappingPair* pair);
public :
// -------------------- Methods -------------------- //

18
src/collision/HalfEdgeStructure.h
View File

@ -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<uint> faceVertices) {
// Return the number of faces
inline uint HalfEdgeStructure::getNbFaces() const {
return mFaces.size();
return static_cast<uint>(mFaces.size());
}
// Return the number of edges
inline uint HalfEdgeStructure::getNbHalfEdges() const {
return mEdges.size();
return static_cast<uint>(mEdges.size());
}
// Return the number of vertices
inline uint HalfEdgeStructure::getNbVertices() const {
return mVertices.size();
return static_cast<uint>(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];
}

2
src/collision/PolyhedronMesh.cpp
View File

@ -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);

3
src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
View File

@ -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);

2
src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
View File

@ -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;
};
}

8
src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
View File

@ -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;

2
src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h
View File

@ -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;
};
}

5
src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp
View File

@ -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

2
src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h
View File

@ -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;
};
}

5
src/collision/narrowphase/NarrowPhaseAlgorithm.h
View File

@ -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

61
src/collision/narrowphase/SAT/SATAlgorithm.cpp
View File

@ -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<Vector3> polygonVertices; // Vertices to clip of the incident face
std::vector<Vector3> planesNormals; // Normals of the clipping planes
std::vector<Vector3> planesPoints; // Points on the clipping planes
uint nbIncidentFaceVertices = static_cast<uint>(incidentFace.faceVertices.size());
List<Vector3> polygonVertices(mMemoryAllocator, nbIncidentFaceVertices); // Vertices to clip of the incident face
List<Vector3> planesNormals(mMemoryAllocator, nbIncidentFaceVertices); // Normals of the clipping planes
List<Vector3> planesPoints(mMemoryAllocator, nbIncidentFaceVertices); // Points on the clipping planes
// Get all the vertices of the incident face (in the reference local-space)
std::vector<uint>::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<Vector3> clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals);
List<Vector3> 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<Vector3>::const_iterator itPoints;
bool contactPointsFound = false;
for (itPoints = clipPolygonVertices.begin(); itPoints != clipPolygonVertices.end(); ++itPoints) {
for (uint i=0; i<clipPolygonVertices.size(); i++) {
// Compute the penetration depth of this contact point (can be different from the minPenetration depth which is
// the maximal penetration depth of any contact point for this separating axis
decimal penetrationDepth = (referenceFaceVertex - (*itPoints)).dot(axisReferenceSpace);
decimal penetrationDepth = (referenceFaceVertex - clipPolygonVertices[i]).dot(axisReferenceSpace);
// If the clip point is bellow the reference face
if (penetrationDepth > 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);

5
src/collision/narrowphase/SAT/SATAlgorithm.h
View File

@ -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;

3
src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
View File

@ -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;

2
src/collision/narrowphase/SphereVsCapsuleAlgorithm.h
View File

@ -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;
};
}

5
src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
View File

@ -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

2
src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
View File

@ -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;
};
}

3
src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
View File

@ -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);

2
src/collision/narrowphase/SphereVsSphereAlgorithm.h
View File

@ -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;
};
}

7
src/collision/shapes/BoxShape.cpp
View File

@ -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));

14
src/collision/shapes/BoxShape.h
View File

@ -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);
}

8
src/collision/shapes/ConvexMeshShape.h
View File

@ -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);
}

4
src/collision/shapes/ConvexPolyhedronShape.h
View File

@ -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;

99
src/collision/shapes/TriangleShape.cpp
View File

@ -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;
}

37
src/collision/shapes/TriangleShape.h
View File

@ -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);

23
src/mathematics/Quaternion.h
View File

@ -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()));
*/
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

2
src/mathematics/Transform.h
View File

@ -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

31
src/mathematics/mathematics_functions.cpp
View File

@ -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<Vector3> 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<Vector3> reactphysics3d::clipPolygonWithPlanes(const std::vector<Vector3>& polygonVertices, const std::vector<Vector3>& planesPoints,
const std::vector<Vector3>& planesNormals) {
List<Vector3> reactphysics3d::clipPolygonWithPlanes(const List<Vector3>& polygonVertices, const List<Vector3>& planesPoints,
const List<Vector3>& planesNormals, Allocator& allocator) {
assert(planesPoints.size() == planesNormals.size());
std::vector<Vector3> inputVertices(polygonVertices);
std::vector<Vector3> outputVertices;
uint nbMaxElements = polygonVertices.size() + planesPoints.size();
List<Vector3> inputVertices(allocator, nbMaxElements);
List<Vector3> outputVertices(allocator, nbMaxElements);
inputVertices.addRange(polygonVertices);
// For each clipping plane
for (uint p=0; p<planesPoints.size(); p++) {
outputVertices.clear();
uint vStart = inputVertices.size() - 1;
uint nbInputVertices = inputVertices.size();
uint vStart = nbInputVertices - 1;
// For each edge of the polygon
for (uint vEnd = 0; vEnd<inputVertices.size(); vEnd++) {
for (uint vEnd = 0; vEnd<nbInputVertices; vEnd++) {
Vector3& v1 = inputVertices[vStart];
Vector3& v2 = inputVertices[vEnd];
@ -331,15 +332,15 @@ std::vector<Vector3> reactphysics3d::clipPolygonWithPlanes(const std::vector<Vec
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);
}
}
// 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<Vector3> reactphysics3d::clipPolygonWithPlanes(const std::vector<Vec
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));
}
else {
outputVertices.push_back(v1);
outputVertices.add(v1);
}
}
}

5
src/mathematics/mathematics_functions.h
View File

@ -33,6 +33,7 @@
#include <cassert>
#include <cmath>
#include <vector>
#include "containers/List.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
@ -116,8 +117,8 @@ std::vector<Vector3> clipSegmentWithPlanes(const Vector3& segA, const Vector3& s
const std::vector<Vector3>& planesNormals);
/// Clip a polygon against multiple planes and return the clipped polygon vertices
std::vector<Vector3> clipPolygonWithPlanes(const std::vector<Vector3>& polygonVertices, const std::vector<Vector3>& planesPoints,
const std::vector<Vector3>& planesNormals);
List<Vector3> clipPolygonWithPlanes(const List<Vector3>& polygonVertices, const List<Vector3>& planesPoints,
const List<Vector3>& 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);

62
test/tests/mathematics/TestMathematicsFunctions.h
View File

@ -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<Vector3> 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<Vector3> 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<Vector3> polygonPlanesNormals(mAllocator);
List<Vector3> 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<Vector3> 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));
}