From 844df20be095e2494e15a2ceab87269f3a5f85eb Mon Sep 17 00:00:00 2001
From: "chappuis.daniel" <chappuis.daniel@92aac97c-a6ce-11dd-a772-7fcde58d38e6>
Date: Mon, 7 Feb 2011 14:51:54 +0000
Subject: [PATCH] Implementation of EPA Algorithm continued
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@417 92aac97c-a6ce-11dd-a772-7fcde58d38e6
---
src/collision/EPA/EPAAlgorithm.cpp | 76 +++++++++++++
src/collision/EPA/EPAAlgorithm.h | 74 +++++++++++++
src/collision/EPA/EdgeEPA.cpp | 86 +++++++++++++++
src/collision/EPA/EdgeEPA.h | 90 +++++++++++++++
src/collision/EPA/TriangleEPA.cpp | 99 +++++++++++++++++
src/collision/EPA/TriangleEPA.h | 133 +++++++++++++++++++++++
src/collision/EPA/TrianglesStore.cpp | 39 +++++++
src/collision/EPA/TrianglesStore.h | 113 +++++++++++++++++++
src/collision/{ => GJK}/GJKAlgorithm.cpp | 92 ++++++++++++++--
src/collision/{ => GJK}/GJKAlgorithm.h | 20 +++-
src/collision/{ => GJK}/Simplex.cpp | 69 ++++++++----
src/collision/{ => GJK}/Simplex.h | 1 +
src/collision/SATAlgorithm.cpp | 36 +++---
src/collision/SATAlgorithm.h | 2 +-
14 files changed, 872 insertions(+), 58 deletions(-)
create mode 100644 src/collision/EPA/EPAAlgorithm.cpp
create mode 100644 src/collision/EPA/EPAAlgorithm.h
create mode 100644 src/collision/EPA/EdgeEPA.cpp
create mode 100644 src/collision/EPA/EdgeEPA.h
create mode 100644 src/collision/EPA/TriangleEPA.cpp
create mode 100644 src/collision/EPA/TriangleEPA.h
create mode 100644 src/collision/EPA/TrianglesStore.cpp
create mode 100644 src/collision/EPA/TrianglesStore.h
rename src/collision/{ => GJK}/GJKAlgorithm.cpp (67%)
rename src/collision/{ => GJK}/GJKAlgorithm.h (73%)
rename src/collision/{ => GJK}/Simplex.cpp (85%)
rename src/collision/{ => GJK}/Simplex.h (98%)
diff --git a/src/collision/EPA/EPAAlgorithm.cpp b/src/collision/EPA/EPAAlgorithm.cpp
new file mode 100644
index 00000000..88f8e601
--- /dev/null
+++ b/src/collision/EPA/EPAAlgorithm.cpp
@@ -0,0 +1,76 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
+* Copyright (c) 2011 Daniel Chappuis *
+*********************************************************************************
+* *
+* Permission is hereby granted, free of charge, to any person obtaining a copy *
+* of this software and associated documentation files (the "Software"), to deal *
+* in the Software without restriction, including without limitation the rights *
+* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
+* copies of the Software, and to permit persons to whom the Software is *
+* furnished to do so, subject to the following conditions: *
+* *
+* The above copyright notice and this permission notice shall be included in *
+* all copies or substantial portions of the Software. *
+* *
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
+* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
+* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
+* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
+* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
+* THE SOFTWARE. *
+********************************************************************************/
+
+// Libraries
+#include "EPAAlgorithm.h"
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Constructor
+EPAAlgorithm::EPAAlgorithm() {
+
+}
+
+// Destructor
+EPAAlgorithm::~EPAAlgorithm() {
+
+}
+
+// Compute the penetration depth with the EPA algorithms
+// This method computes the penetration depth and contact points between two
+// enlarged objects (with margin) where the original objects (without margin)
+// intersect. An initial simplex that contains origin has been computed with
+// GJK algorithm. The EPA Algorithm will extend this simplex polytope to find
+// the correct penetration depth
+bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, const NarrowBoundingVolume* const boundingVolume1,
+ const NarrowBoundingVolume* const boundingVolume2,
+ Vector3D& v, ContactInfo*& contactInfo) {
+ Vector3D suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates
+ Vector3D suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates
+ Vector3D points[MAX_SUPPORT_POINTS]; // Current points
+
+ // TODO : Check that we call all the supportPoint() function with a margin
+
+ // Get the simplex computed previously by the GJK algorithm
+ unsigned int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
+
+ // Compute the tolerance
+ double tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint();
+
+ // Number of triangles in the polytope
+ unsigned int nbTriangles = 0;
+
+ // Select an action according to the number of points in the simplex computed with GJK algorithm
+ switch(nbVertices) {
+ case 1:
+ // Only one point in the simplex (which should be the origin). We have a touching contact
+ // with zero penetration depth. We drop that kind of contact. Therefore, we return false
+ return false;
+
+ case 2: {
+
+ }
+ }
+}
diff --git a/src/collision/EPA/EPAAlgorithm.h b/src/collision/EPA/EPAAlgorithm.h
new file mode 100644
index 00000000..e9707b4b
--- /dev/null
+++ b/src/collision/EPA/EPAAlgorithm.h
@@ -0,0 +1,74 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
+* Copyright (c) 2011 Daniel Chappuis *
+*********************************************************************************
+* *
+* Permission is hereby granted, free of charge, to any person obtaining a copy *
+* of this software and associated documentation files (the "Software"), to deal *
+* in the Software without restriction, including without limitation the rights *
+* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
+* copies of the Software, and to permit persons to whom the Software is *
+* furnished to do so, subject to the following conditions: *
+* *
+* The above copyright notice and this permission notice shall be included in *
+* all copies or substantial portions of the Software. *
+* *
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
+* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
+* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
+* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
+* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
+* THE SOFTWARE. *
+********************************************************************************/
+
+#ifndef EPAAlgorithm_H
+#define EPAAlgorithm_H
+
+// Libraries
+#include "../GJK/Simplex.h"
+#include "../body/NarrowBoundingVolume.h"
+#include "ContactInfo.h"
+#include "../mathematics/mathematics.h"
+
+// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Constants
+const unsigned int MAX_SUPPORT_POINTS = 100; // Maximum number of support points of the polytope
+const unsigned int MAX_FACETS = 200; // Maximum number of facets of the polytope
+
+/* -------------------------------------------------------------------
+ Class EPAAlgorithm :
+ This class is the implementation of the Expanding Polytope Algorithm (EPA).
+ The EPA algorithm computes the penetration depth and contact points between
+ two enlarged objects (with margin) where the original objects (without margin)
+ intersect. The penetration depth of a pair of intersecting objects A and B is
+ the length of a point on the boundary of the Minkowski sum (A-B) closest to the
+ origin. The goal of the EPA algorithm is to start with an initial simplex polytope
+ that contains the origin and expend it in order to find the point on the boundary
+ of (A-B) that is closest to the origin. An initial simplex that contains origin
+ has been computed wit GJK algorithm. The EPA Algorithm will extend this simplex
+ polytope to find the correct penetration depth.
+ -------------------------------------------------------------------
+*/
+class EPAAlgorithm {
+ private:
+
+
+ bool isOrigininInTetrahedron(const Vector3D& p1, const Vector3D& p2,
+ const Vector3D& p3, const Vector3D& p4) const; // Return true if the origin is in the tetrahedron
+
+ public:
+ EPAAlgorithm(); // Constructor
+ ~EPAAlgorithm(); // Destructor
+
+ bool computePenetrationDepthAndContactPoints(Simplex simplex, const NarrowBoundingVolume* const boundingVolume1,
+ const NarrowBoundingVolume* const boundingVolume2,
+ Vector3D& v, ContactInfo*& contactInfo); // Compute the penetration depth with EPA algorithm
+};
+
+} // End of ReactPhysics3D namespace
+
+#endif
+
diff --git a/src/collision/EPA/EdgeEPA.cpp b/src/collision/EPA/EdgeEPA.cpp
new file mode 100644
index 00000000..844148cb
--- /dev/null
+++ b/src/collision/EPA/EdgeEPA.cpp
@@ -0,0 +1,86 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
+* Copyright (c) 2011 Daniel Chappuis *
+*********************************************************************************
+* *
+* Permission is hereby granted, free of charge, to any person obtaining a copy *
+* of this software and associated documentation files (the "Software"), to deal *
+* in the Software without restriction, including without limitation the rights *
+* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
+* copies of the Software, and to permit persons to whom the Software is *
+* furnished to do so, subject to the following conditions: *
+* *
+* The above copyright notice and this permission notice shall be included in *
+* all copies or substantial portions of the Software. *
+* *
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
+* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
+* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
+* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
+* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
+* THE SOFTWARE. *
+********************************************************************************/
+
+// Libraries
+#include "EdgeEPA.h"
+#include "TriangleEPA.h"
+#include "TrianglesStore.h"
+#include <cassert>
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Constructor
+EdgeEPA::EdgeEPA() {
+
+}
+
+// Constructor
+EdgeEPA::EdgeEPA(TriangleEPA* ownerTriangle, int index)
+ : ownerTriangle(ownerTriangle), index(index) {
+ assert(index >= 0 && index < 3);
+}
+
+// Destructor
+EdgeEPA::~EdgeEPA() {
+
+}
+
+// Return the index of the source vertex of the edge (vertex starting the edge)
+uint EdgeEPA::getSource() const {
+ return (*ownerTriangle)[index];
+}
+
+// Return the index of the target vertex of the edge (vertex ending the edge)
+uint EdgeEPA::getTarget() const {
+ return (*ownerTriangle)[IndexOfNextCounterClockwiseEdge(index)];
+}
+
+// Link the edge with another one (meaning that the current edge of a triangle will
+// is associated with the edge of another triangle in order that both triangles
+// are neighbour along both edges)
+bool EdgeEPA::link(EdgeEPA& edge) {
+ bool isPossible = (this->getSource() == edge.getTarget() &&
+ this->getTarget() == edge.getSource());
+
+ // If the link is possible
+ if (isPossible) {
+ this->getOwnerTriangle()->setAdjacentEdge(index, edge);
+ edge.getOwnerTriangle()->setAdjacentEdge(edge.getIndex(), *this);
+ }
+
+ // Return if the link has been made
+ return isPossible;
+}
+
+// Half link the edge with another one
+void EdgeEPA::halfLink(EdgeEPA& edge) const {
+
+}
+
+
+// Compute the silhouette
+bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesStore trianglesStore) {
+ // TODO : Implement this
+}
diff --git a/src/collision/EPA/EdgeEPA.h b/src/collision/EPA/EdgeEPA.h
new file mode 100644
index 00000000..177f7c43
--- /dev/null
+++ b/src/collision/EPA/EdgeEPA.h
@@ -0,0 +1,90 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
+* Copyright (c) 2011 Daniel Chappuis *
+*********************************************************************************
+* *
+* Permission is hereby granted, free of charge, to any person obtaining a copy *
+* of this software and associated documentation files (the "Software"), to deal *
+* in the Software without restriction, including without limitation the rights *
+* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
+* copies of the Software, and to permit persons to whom the Software is *
+* furnished to do so, subject to the following conditions: *
+* *
+* The above copyright notice and this permission notice shall be included in *
+* all copies or substantial portions of the Software. *
+* *
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
+* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
+* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
+* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
+* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
+* THE SOFTWARE. *
+********************************************************************************/
+
+#ifndef EDGEEPA_H
+#define EDGEEPA_H
+
+
+// Libraries
+#include "../../mathematics/mathematics.h"
+
+// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Class declarations
+class TriangleEPA;
+class TrianglesStore;
+
+/* -------------------------------------------------------------------
+ Class EdgeEPA :
+ This class represents an edge of the current polytope in the EPA
+ algorithm.
+ -------------------------------------------------------------------
+*/
+class EdgeEPA {
+ private:
+ TriangleEPA* ownerTriangle; // Pointer to the triangle that contains this edge
+ int index; // Index of the edge in the triangle (between 0 and 2).
+ // The edge with index i connect triangle vertices i and (i+1 % 3)
+
+ public:
+ EdgeEPA(); // Constructor
+ EdgeEPA(TriangleEPA* ownerTriangle, int index); // Constructor
+ ~EdgeEPA(); // Destructor
+
+ TriangleEPA* getOwnerTriangle() const; // Return the pointer to the owner triangle
+ int getIndex() const; // Return the index of the edge in the triangle
+ uint getSource() const; // Return index of the source vertex of the edge
+ uint getTarget() const; // Return the index of the target vertex of the edge
+ bool link(EdgeEPA& edge); // Link the edge with another one
+ void halfLink(EdgeEPA& edge) const; // Half link the edge with another one
+ bool computeSilhouette(const Vector3D* vertices, uint index,
+ TrianglesStore trianglesStore); // Compute the recursive silhouette algorithm
+};
+
+
+// Return the pointer to the owner triangle
+inline TriangleEPA* EdgeEPA::getOwnerTriangle() const {
+ return ownerTriangle;
+}
+
+// Return the edge index
+inline int EdgeEPA::getIndex() const {
+ return index;
+}
+
+// Return the index of the next counter-clockwise edge of the ownver triangle
+inline int IndexOfNextCounterClockwiseEdge(int i) {
+ return (i + 1) % 3;
+}
+
+// Return the index of the previous counter-clockwise edge of the ownver triangle
+inline int IndexOfPreviousCounterClockwiseEdge(int i) {
+ return (i + 2) % 3;
+}
+
+} // End of ReactPhysics3D namespace
+
+#endif
+
diff --git a/src/collision/EPA/TriangleEPA.cpp b/src/collision/EPA/TriangleEPA.cpp
new file mode 100644
index 00000000..7d395ebc
--- /dev/null
+++ b/src/collision/EPA/TriangleEPA.cpp
@@ -0,0 +1,99 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
+* Copyright (c) 2011 Daniel Chappuis *
+*********************************************************************************
+* *
+* Permission is hereby granted, free of charge, to any person obtaining a copy *
+* of this software and associated documentation files (the "Software"), to deal *
+* in the Software without restriction, including without limitation the rights *
+* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
+* copies of the Software, and to permit persons to whom the Software is *
+* furnished to do so, subject to the following conditions: *
+* *
+* The above copyright notice and this permission notice shall be included in *
+* all copies or substantial portions of the Software. *
+* *
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
+* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
+* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
+* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
+* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
+* THE SOFTWARE. *
+********************************************************************************/
+
+
+// Libraries
+#include "TriangleEPA.h"
+#include "EdgeEPA.h"
+#include "TrianglesStore.h"
+
+// We use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Constructor
+TriangleEPA::TriangleEPA() {
+
+}
+
+// Constructor
+TriangleEPA::TriangleEPA(uint indexVertex1, uint indexVertex2, uint indexVertex3)
+ : isObsolete(false) {
+ indicesVertices[0] = indexVertex1;
+ indicesVertices[1] = indexVertex2;
+ indicesVertices[2] = indexVertex3;
+}
+
+// Destructor
+TriangleEPA::~TriangleEPA() {
+
+}
+
+// Compute the point v closest to the origin of this triangle
+bool TriangleEPA::computeClosestPoint(const Vector3D* vertices) {
+ const Vector3D& p0 = vertices[indicesVertices[0]];
+
+ Vector3D v1 = vertices[indicesVertices[1]] - p0;
+ Vector3D v2 = vertices[indicesVertices[2]] - p0;
+ double v1Dotv1 = v1.dot(v1);
+ double v1Dotv2 = v1.dot(v2);
+ double v2Dotv2 = v2.dot(v2);
+ double p0Dotv1 = p0.dot(v1);
+ double p0Dotv2 = p0.dot(v2);
+
+ // Compute determinant
+ det = v1Dotv1 * v2Dotv2 - v1Dotv2 * v1Dotv2;
+
+ // Compute lambda values
+ lambda1 = p0Dotv2 * v1Dotv2 - p0Dotv1 * v2Dotv2;
+ lambda2 = p0Dotv1 * v1Dotv2 - p0Dotv2 * v1Dotv1;
+
+ // If the determinant is positive
+ if (det > 0.0) {
+ // Compute the closest point v
+ closestPoint = p0 + (lambda1 * v1 * lambda2 * v2) / det;
+
+ // Compute the square distance of closest point to the origin
+ distSquare = closestPoint.dot(closestPoint);
+
+ return true;
+ }
+
+ return false;
+}
+
+// Compute recursive silhouette algorithm for that triangle
+bool TriangleEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesStore& triangleStore) {
+
+ uint first = triangleStore.getNbTriangles();
+
+ // Mark the current triangle as obsolete
+ setIsObsolete(true);
+
+ // Execute recursively the silhouette algorithm for the ajdacent edges of neighbouring
+ // triangles of the current triangle
+ bool result = adjacentEdges[0].computeSilhouette(vertices, index, triangleStore) &&
+ adjacentEdges[1].computeSilhouette(vertices, index, triangleStore) &&
+ adjacentEdges[2].computeSilhouette(vertices, index, triangleStore);
+
+}
diff --git a/src/collision/EPA/TriangleEPA.h b/src/collision/EPA/TriangleEPA.h
new file mode 100644
index 00000000..20b17479
--- /dev/null
+++ b/src/collision/EPA/TriangleEPA.h
@@ -0,0 +1,133 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
+* Copyright (c) 2011 Daniel Chappuis *
+*********************************************************************************
+* *
+* Permission is hereby granted, free of charge, to any person obtaining a copy *
+* of this software and associated documentation files (the "Software"), to deal *
+* in the Software without restriction, including without limitation the rights *
+* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
+* copies of the Software, and to permit persons to whom the Software is *
+* furnished to do so, subject to the following conditions: *
+* *
+* The above copyright notice and this permission notice shall be included in *
+* all copies or substantial portions of the Software. *
+* *
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
+* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
+* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
+* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
+* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
+* THE SOFTWARE. *
+********************************************************************************/
+
+#ifndef TRIANGLEEPA_H
+#define TRIANGLEEPA_H
+
+// Libraries
+#include "../../mathematics/mathematics.h"
+#include "../../constants.h"
+#include "EdgeEPA.h"
+#include <cassert>
+
+// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+/* -------------------------------------------------------------------
+ Class TriangleEPA :
+ This class represents a triangle face of the current polytope in the EPA
+ algorithm.
+ -------------------------------------------------------------------
+*/
+class TriangleEPA {
+ private:
+ uint indicesVertices[3]; // Indices of the vertices y_i of the triangle
+ EdgeEPA adjacentEdges[3]; // Three adjacent edges of the triangle (edges of other triangles)
+ bool isObsolete; // True if the triangle face is visible from the new support point
+ double det; // Determinant
+ Vector3D closestPoint; // Point v closest to the origin on the affine hull of the triangle
+ double lambda1; // Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
+ double lambda2; // Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
+ double distSquare; // Square distance of the point closest point v to the origin
+
+ public:
+ TriangleEPA(); // Constructor
+ TriangleEPA(uint v1, uint v2, uint v3); // Constructor
+ ~TriangleEPA(); // Destructor
+
+ const EdgeEPA& getAdjacentEdge(int index) const; // Return an adjacent edge of the triangle
+ void setAdjacentEdge(int index, EdgeEPA& edge); // Set an adjacent edge of the triangle
+ double getDistSquare() const; // Return the square distance of the closest point to origin
+ void setIsObsolete(bool isObsolete); // Set the isObsolete value
+ bool getIsObsolete() const; // Return true if the triangle face is obsolete
+ const Vector3D& getClosestPoint() const; // Return the point closest to the origin
+ bool isClosestPointInternalToTriangle() const; // Return true if the closest point on affine hull is inside the triangle
+ bool isVisibleFromVertex(const Vector3D* vertices, uint index) const; // Return true if the triangle is visible from a given vertex
+ bool computeClosestPoint(const Vector3D* vertices); // Compute the point v closest to the origin of this triangle
+ Vector3D computeClosestPointOfObject(const Vector3D* supportPointsOfObject) const; // Compute the point of an object closest to the origin
+ bool computeSilhouette(const Vector3D* vertices, uint index,
+ TrianglesStore& triangleStore); // Compute recursive silhouette algorithm for that triangle
+
+ uint operator[](int i) const; // Access operator
+};
+
+// Return an edge of the triangle
+inline const EdgeEPA& TriangleEPA::getAdjacentEdge(int index) const {
+ assert(index >= 0 && index < 3);
+ return adjacentEdges[index];
+}
+
+// Set an adjacent edge of the triangle
+inline void TriangleEPA::setAdjacentEdge(int index, EdgeEPA& edge) {
+ assert(index >=0 && index < 3);
+ adjacentEdges[index] = edge;
+}
+
+// Return the square distance of the closest point to origin
+inline double TriangleEPA::getDistSquare() const {
+ return distSquare;
+}
+
+// Set the isObsolete value
+inline void TriangleEPA::setIsObsolete(bool isObsolete) {
+ this->isObsolete = isObsolete;
+}
+
+// Return true if the triangle face is obsolete
+inline bool TriangleEPA::getIsObsolete() const {
+ return isObsolete;
+}
+
+// Return the point closest to the origin
+inline const Vector3D& TriangleEPA::getClosestPoint() const {
+ return closestPoint;
+}
+
+// Return true if the closest point on affine hull is inside the triangle
+inline bool TriangleEPA::isClosestPointInternalToTriangle() const {
+ return (lambda1 >= 0.0 && lambda2 >= 0.0 && lambda1 + lambda2 <= det);
+}
+
+// Return true if the triangle is visible from a given vertex
+inline bool TriangleEPA::isVisibleFromVertex(const Vector3D* vertices, uint index) const {
+ Vector3D closestToVert = vertices[index] - closestPoint;
+ return (closestPoint.dot(closestToVert) > 0.0);
+}
+
+// Compute the point of an object closest to the origin
+inline Vector3D TriangleEPA::computeClosestPointOfObject(const Vector3D* supportPointsOfObject) const {
+ const Vector3D& p0 = supportPointsOfObject[indicesVertices[0]];
+ return p0 + (lambda1 * (supportPointsOfObject[indicesVertices[1]] - p0) +
+ lambda2 * 1.0/det * (supportPointsOfObject[indicesVertices[2]] - p0));
+}
+
+// Access operator
+inline uint TriangleEPA::operator[](int i) const {
+ assert(i >= 0 && i <3);
+ return indicesVertices[i];
+}
+
+} // End of ReactPhysics3D namespace
+
+#endif
\ No newline at end of file
diff --git a/src/collision/EPA/TrianglesStore.cpp b/src/collision/EPA/TrianglesStore.cpp
new file mode 100644
index 00000000..7c39c681
--- /dev/null
+++ b/src/collision/EPA/TrianglesStore.cpp
@@ -0,0 +1,39 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
+* Copyright (c) 2011 Daniel Chappuis *
+*********************************************************************************
+* *
+* Permission is hereby granted, free of charge, to any person obtaining a copy *
+* of this software and associated documentation files (the "Software"), to deal *
+* in the Software without restriction, including without limitation the rights *
+* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
+* copies of the Software, and to permit persons to whom the Software is *
+* furnished to do so, subject to the following conditions: *
+* *
+* The above copyright notice and this permission notice shall be included in *
+* all copies or substantial portions of the Software. *
+* *
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
+* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
+* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
+* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
+* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
+* THE SOFTWARE. *
+********************************************************************************/
+
+// Libraries
+#include "TrianglesStore.h"
+
+// We use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Constructor
+TrianglesStore::TrianglesStore() : nbTriangles(0) {
+
+}
+
+// Destructor
+TrianglesStore::~TrianglesStore() {
+
+}
diff --git a/src/collision/EPA/TrianglesStore.h b/src/collision/EPA/TrianglesStore.h
new file mode 100644
index 00000000..5b3b2966
--- /dev/null
+++ b/src/collision/EPA/TrianglesStore.h
@@ -0,0 +1,113 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
+* Copyright (c) 2011 Daniel Chappuis *
+*********************************************************************************
+* *
+* Permission is hereby granted, free of charge, to any person obtaining a copy *
+* of this software and associated documentation files (the "Software"), to deal *
+* in the Software without restriction, including without limitation the rights *
+* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
+* copies of the Software, and to permit persons to whom the Software is *
+* furnished to do so, subject to the following conditions: *
+* *
+* The above copyright notice and this permission notice shall be included in *
+* all copies or substantial portions of the Software. *
+* *
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
+* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
+* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
+* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
+* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
+* THE SOFTWARE. *
+********************************************************************************/
+
+#ifndef TRIANGLESSTORE_H
+#define TRIANGLESSTORE_H
+
+#include "TriangleEPA.h"
+
+
+// Libraries
+#include <cassert>
+
+// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Constants
+const unsigned int MAX_TRIANGLES = 200; // Maximum number of triangles
+
+
+/* -------------------------------------------------------------------
+ Class TrianglesStore :
+ This class stores several triangles of the polytope in the EPA
+ algorithm.
+ -------------------------------------------------------------------
+*/
+class TrianglesStore {
+ private:
+ TriangleEPA triangles[MAX_TRIANGLES]; // Triangles
+ int nbTriangles; // Number of triangles
+
+ public:
+ TrianglesStore(); // Constructor
+ ~TrianglesStore(); // Destructor
+
+ void clear(); // Clear all the storage
+ int getNbTriangles() const; // Return the number of triangles
+ void setNbTriangles(int backup); // Set the number of triangles
+ TriangleEPA& last(); // Return the last triangle
+
+ TriangleEPA* newTriangle(const Vector3D* vertices, uint v0, uint v1, uint v2); // Create a new triangle
+
+ TriangleEPA& operator[](int i); // Access operator
+};
+
+// Clear all the storage
+inline void TrianglesStore::clear() {
+ nbTriangles = 0;
+}
+
+// Return the number of triangles
+inline int TrianglesStore::getNbTriangles() const {
+ return nbTriangles;
+}
+
+
+inline void TrianglesStore::setNbTriangles(int backup) {
+ nbTriangles = backup;
+}
+
+// Return the last triangle
+inline TriangleEPA& TrianglesStore::last() {
+ assert(nbTriangles > 0);
+ return triangles[nbTriangles - 1];
+}
+
+// Create a new triangle
+inline TriangleEPA* TrianglesStore::newTriangle(const Vector3D* vertices, uint v0, uint v1, uint v2) {
+ TriangleEPA* newTriangle = 0;
+
+ // If we have not reach the maximum number of triangles
+ if (nbTriangles != MAX_TRIANGLES) {
+ newTriangle = &triangles[nbTriangles];
+ nbTriangles++;
+ new (newTriangle) TriangleEPA(v0, v1, v2);
+ if (!newTriangle->computeClosestPoint(vertices)) {
+ nbTriangles--;
+ newTriangle = 0;
+ }
+ }
+
+ // Return the new triangle
+ return newTriangle;
+}
+
+// Access operator
+inline TriangleEPA& TrianglesStore::operator[](int i) {
+ return triangles[i];
+}
+
+} // End of ReactPhysics3D namespace
+
+#endif
\ No newline at end of file
diff --git a/src/collision/GJKAlgorithm.cpp b/src/collision/GJK/GJKAlgorithm.cpp
similarity index 67%
rename from src/collision/GJKAlgorithm.cpp
rename to src/collision/GJK/GJKAlgorithm.cpp
index 5b2aae81..d86806f4 100644
--- a/src/collision/GJKAlgorithm.cpp
+++ b/src/collision/GJK/GJKAlgorithm.cpp
@@ -1,7 +1,7 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
-* Copyright (c) 2010 Daniel Chappuis *
+* Copyright (c) 2011 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
@@ -38,9 +38,7 @@
using namespace reactphysics3d;
// Constructor
-GJKAlgorithm::GJKAlgorithm() : lastSeparatingAxis(1.0, 1.0, 1.0) {
- // TODO : Check if we can initialize the last separating axis as above
-
+GJKAlgorithm::GJKAlgorithm() {
}
@@ -50,7 +48,15 @@ GJKAlgorithm::~GJKAlgorithm() {
}
// Return true and compute a contact info if the two bounding volume collide.
-// The method returns false if there is no collision between the two bounding volumes.
+// This method implements the Hybrid Technique for computing the penetration depth by
+// running the GJK algorithm on original objects (without margin).
+// If the objects don't intersect, this method returns false. If they intersect
+// only in the margins, the method compute the penetration depth and contact points
+// (of enlarged objects). If the original objects (without margin) intersect, we
+// call the computePenetrationDepthForEnlargedObjects() method that run the GJK
+// algorithm on the enlarged object to obtain a simplex polytope that contains the
+// origin, they we give that simplex polytope to the EPA algorithm which will compute
+// the correct penetration depth and contact points between the enlarged objects.
bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolume1,
const NarrowBoundingVolume* const boundingVolume2,
ContactInfo*& contactInfo) {
@@ -73,20 +79,22 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
Simplex simplex;
// Get the last point V (last separating axis)
- Vector3D v = lastSeparatingAxis;
+ // TODO : Implement frame coherence. For each pair of body, store
+ // the last separating axis and use it to initialize the v vector
+ Vector3D v(1.0, 1.0, 1.0);
// Initialize the upper bound for the square distance
double distSquare = DBL_MAX;
do {
- // Compute the support points for object A and B
+ // Compute the support points for original objects (without margins) A and B
suppA = boundingVolume1->getSupportPoint(v.getOpposite());
suppB = boundingVolume2->getSupportPoint(v);
// Compute the support point for the Minkowski difference A-B
w = suppA - suppB;
- vDotw = v.scalarProduct(w);
+ vDotw = v.dot(w);
// If the enlarge objects (with margins) do not intersect
if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) {
@@ -117,7 +125,7 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
return true;
}
- // Add the current point to the simplex
+ // Add the new support point to the simplex
simplex.addPoint(w, suppA, suppB);
// If the simplex is affinely dependent
@@ -169,14 +177,14 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
// Store and update the squared distance of the closest point
prevDistSquare = distSquare;
- distSquare = v.scalarProduct(v);
+ distSquare = v.dot(v);
// If the distance to the closest point doesn't improve a lot
if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
simplex.backupClosestPointInSimplex(v);
// Get the new squared distance
- distSquare = v.scalarProduct(v);
+ distSquare = v.dot(v);
// Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB);
@@ -201,7 +209,67 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
} while(!simplex.isFull() && distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint());
- // The objects (without margins) intersect
+ // The objects (without margins) intersect. Therefore, we run the GJK algorithm again but on the
+ // enlarged objects to compute a simplex polytope that contains the origin. Then, we give that simplex
+ // polytope to the EPA algorithm to compute the correct penetration depth and contact points between
+ // the enlarged objects.
+ return computePenetrationDepthForEnlargedObjects(boundingVolume1, boundingVolume2, contactInfo, v);
}
+// This method runs the GJK algorithm on the two enlarged objects (with margin)
+// to compute a simplex polytope that contains the origin. The two objects are
+// assumed to intersect in the original objects (without margin). Therefore such
+// a polytope must exist. Then, we give that polytope to the EPA algorithm to
+// compute the correct penetration depth and contact points of the enlarged objects.
+bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const NarrowBoundingVolume* const boundingVolume1,
+ const NarrowBoundingVolume* const boundingVolume2,
+ ContactInfo*& contactInfo, Vector3D& v) {
+ Simplex simplex;
+ Vector3D suppA;
+ Vector3D suppB;
+ Vector3D w;
+ double vDotw;
+ double distSquare = DBL_MAX;
+ double prevDistSquare;
+
+ do {
+ // Compute the support points for the enlarged object A and B
+ suppA = boundingVolume1->getSupportPoint(v.getOpposite(), OBJECT_MARGIN);
+ suppB = boundingVolume2->getSupportPoint(v, OBJECT_MARGIN);
+ // Compute the support point for the Minkowski difference A-B
+ w = suppA - suppB;
+
+ vDotw = v.dot(w);
+
+ // If the enlarge objects do not intersect
+ if (vDotw > 0.0) {
+ // No intersection, we return false
+ return false;
+ }
+
+ // Add the new support point to the simplex
+ simplex.addPoint(w, suppA, suppB);
+
+ if (simplex.isAffinelyDependent()) {
+ return false;
+ }
+
+ if (!simplex.computeClosestPoint(v)) {
+ return false;
+ }
+
+ // Store and update the square distance
+ prevDistSquare = distSquare;
+ distSquare = v.dot(v);
+
+ if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
+ return false;
+ }
+
+ } while(!simplex.isFull() && distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint());
+
+ // Give the simplex computed with GJK algorithm to the EPA algorithm which will compute the correct
+ // penetration depth and contact points between the two enlarged objects
+ return algoEPA.computePenetrationDepthAndContactPoints(simplex, boundingVolume1, boundingVolume2, v, contactInfo);
+}
diff --git a/src/collision/GJKAlgorithm.h b/src/collision/GJK/GJKAlgorithm.h
similarity index 73%
rename from src/collision/GJKAlgorithm.h
rename to src/collision/GJK/GJKAlgorithm.h
index efe95df8..32e0bde1 100644
--- a/src/collision/GJKAlgorithm.h
+++ b/src/collision/GJK/GJKAlgorithm.h
@@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
-* Copyright (c) 2010 Daniel Chappuis *
+* Copyright (c) 2011 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
@@ -29,6 +29,7 @@
#include "NarrowPhaseAlgorithm.h"
#include "ContactInfo.h"
#include "../body/NarrowBoundingVolume.h"
+#include "EPAAlgorithm.h"
// ReactPhysics3D namespace
@@ -45,13 +46,24 @@ const double REL_ERROR_SQUARE = REL_ERROR * REL_ERROR;
algorithm uses the ISA-GJK algorithm and the EPA algorithm. This
implementation is based on the implementation discussed in the book
"Collision Detection in 3D Environments".
+ This method implements the Hybrid Technique for calculating the
+ penetration depth. The two objects are enlarged with a small margin. If
+ the object intersection, the penetration depth is quickly computed using
+ GJK algorithm on the original objects (without margin). If the
+ original objects (without margin) intersect, we run again the GJK
+ algorithm on the enlarged objects (with margin) to compute simplex
+ polytope that contains the origin and give it to the EPA (Expanding
+ Polytope Algorithm) to compute the correct penetration depth between the
+ enlarged objects.
-------------------------------------------------------------------
*/
class GJKAlgorithm : public NarrowPhaseAlgorithm {
private :
- // TODO : Implement frame coherence. For each pair of body, store
- // the last separating axis and use it to initialize the v vector
- Vector3D lastSeparatingAxis;
+ EPAAlgorithm algoEPA; // EPA Algorithm
+
+ bool computePenetrationDepthForEnlargedObjects(const NarrowBoundingVolume* const boundingVolume1,
+ const NarrowBoundingVolume* const boundingVolume2,
+ ContactInfo*& contactInfo, Vector3D& v); // Compute the penetration depth for enlarged objects
public :
GJKAlgorithm(); // Constructor
diff --git a/src/collision/Simplex.cpp b/src/collision/GJK/Simplex.cpp
similarity index 85%
rename from src/collision/Simplex.cpp
rename to src/collision/GJK/Simplex.cpp
index f562a809..41bce576 100644
--- a/src/collision/Simplex.cpp
+++ b/src/collision/GJK/Simplex.cpp
@@ -60,7 +60,7 @@ void Simplex::addPoint(const Vector3D& point, const Vector3D& suppPointA, const
// Add the point into the simplex
points[lastFound] = point;
- pointsLengthSquare[lastFound] = point.scalarProduct(point);
+ pointsLengthSquare[lastFound] = point.dot(point);
allBits = bitsCurrentSimplex | lastFoundBit;
// Update the cached values
@@ -103,11 +103,34 @@ void Simplex::updateCache() {
diffLength[lastFound][i] = diffLength[i][lastFound].getOpposite();
// Compute the squared length of the vector distances from points in the possible simplex set
- normSquare[i][lastFound] = normSquare[lastFound][i] = diffLength[i][lastFound].scalarProduct(diffLength[i][lastFound]);
+ normSquare[i][lastFound] = normSquare[lastFound][i] = diffLength[i][lastFound].dot(diffLength[i][lastFound]);
}
}
}
+// Return the points of the simplex
+unsigned int Simplex::getSimplex(Vector3D* suppPointsA, Vector3D* suppPointsB, Vector3D* points) const {
+ unsigned int nbVertices = 0;
+ int i;
+ Bits bit;
+
+ // For each four point in the possible simplex
+ for (i=0, bit=0x1; i<4; i++, bit <<=1) {
+ // If the current point is in the simplex
+ if (overlap(bitsCurrentSimplex, bit)) {
+ // Store the points
+ suppPointsA[nbVertices] = this->suppPointsA[nbVertices];
+ suppPointsB[nbVertices] = this->suppPointsB[nbVertices];
+ points[nbVertices] = this->points[nbVertices];
+
+ nbVertices++;
+ }
+ }
+
+ // Return the number of points in the simplex
+ return nbVertices;
+}
+
// Compute the cached determinant values
void Simplex::computeDeterminants() {
det[lastFoundBit][lastFound] = 1.0;
@@ -124,8 +147,8 @@ void Simplex::computeDeterminants() {
if (overlap(bitsCurrentSimplex, bitI)) {
Bits bit2 = bitI | lastFoundBit;
- det[bit2][i] = diffLength[lastFound][i].scalarProduct(points[lastFound]);
- det[bit2][lastFound] = diffLength[i][lastFound].scalarProduct(points[i]);
+ det[bit2][i] = diffLength[lastFound][i].dot(points[lastFound]);
+ det[bit2][lastFound] = diffLength[i][lastFound].dot(points[i]);
int j;
@@ -137,16 +160,16 @@ void Simplex::computeDeterminants() {
Bits bit3 = bitJ | bit2;
k = normSquare[i][j] < normSquare[lastFound][j] ? i : lastFound;
- det[bit3][j] = det[bit2][i] * diffLength[k][j].scalarProduct(points[i]) +
- det[bit2][lastFound] * diffLength[k][j].scalarProduct(points[lastFound]);
+ det[bit3][j] = det[bit2][i] * diffLength[k][j].dot(points[i]) +
+ det[bit2][lastFound] * diffLength[k][j].dot(points[lastFound]);
k = normSquare[j][i] < normSquare[lastFound][i] ? j : lastFound;
- det[bit3][i] = det[bitJ | lastFoundBit][j] * diffLength[k][i].scalarProduct(points[j]) +
- det[bitJ | lastFoundBit][lastFound] * diffLength[k][i].scalarProduct(points[lastFound]);
+ det[bit3][i] = det[bitJ | lastFoundBit][j] * diffLength[k][i].dot(points[j]) +
+ det[bitJ | lastFoundBit][lastFound] * diffLength[k][i].dot(points[lastFound]);
k = normSquare[i][lastFound] < normSquare[j][lastFound] ? i : j;
- det[bit3][lastFound] = det[bitJ | bitI][j] * diffLength[k][lastFound].scalarProduct(points[j]) +
- det[bitJ | bitI][i] * diffLength[k][lastFound].scalarProduct(points[i]);
+ det[bit3][lastFound] = det[bitJ | bitI][j] * diffLength[k][lastFound].dot(points[j]) +
+ det[bitJ | bitI][i] * diffLength[k][lastFound].dot(points[i]);
}
}
}
@@ -156,24 +179,24 @@ void Simplex::computeDeterminants() {
int k;
k = normSquare[1][0] < normSquare[2][0] ? (normSquare[1][0] < normSquare[3][0] ? 1 : 3) : (normSquare[2][0] < normSquare[3][0] ? 2 : 3);
- det[0xf][0] = det[0xe][1] * diffLength[k][0].scalarProduct(points[1]) +
- det[0xe][2] * diffLength[k][0].scalarProduct(points[2]) +
- det[0xe][3] * diffLength[k][0].scalarProduct(points[3]);
+ det[0xf][0] = det[0xe][1] * diffLength[k][0].dot(points[1]) +
+ det[0xe][2] * diffLength[k][0].dot(points[2]) +
+ det[0xe][3] * diffLength[k][0].dot(points[3]);
k = normSquare[0][1] < normSquare[2][1] ? (normSquare[0][1] < normSquare[3][1] ? 0 : 3) : (normSquare[2][1] < normSquare[3][1] ? 2 : 3);
- det[0xf][1] = det[0xd][0] * diffLength[k][1].scalarProduct(points[0]) +
- det[0xd][2] * diffLength[k][1].scalarProduct(points[2]) +
- det[0xd][3] * diffLength[k][1].scalarProduct(points[3]);
+ det[0xf][1] = det[0xd][0] * diffLength[k][1].dot(points[0]) +
+ det[0xd][2] * diffLength[k][1].dot(points[2]) +
+ det[0xd][3] * diffLength[k][1].dot(points[3]);
k = normSquare[0][2] < normSquare[1][2] ? (normSquare[0][2] < normSquare[3][2] ? 0 : 3) : (normSquare[1][2] < normSquare[3][2] ? 1 : 3);
- det[0xf][2] = det[0xb][0] * diffLength[k][2].scalarProduct(points[0]) +
- det[0xb][1] * diffLength[k][2].scalarProduct(points[1]) +
- det[0xb][3] * diffLength[k][2].scalarProduct(points[3]);
+ det[0xf][2] = det[0xb][0] * diffLength[k][2].dot(points[0]) +
+ det[0xb][1] * diffLength[k][2].dot(points[1]) +
+ det[0xb][3] * diffLength[k][2].dot(points[3]);
k = normSquare[0][3] < normSquare[1][3] ? (normSquare[0][3] < normSquare[2][3] ? 0 : 2) : (normSquare[1][3] < normSquare[2][3] ? 1 : 2);
- det[0xf][3] = det[0x7][0] * diffLength[k][3].scalarProduct(points[0]) +
- det[0x7][1] * diffLength[k][3].scalarProduct(points[1]) +
- det[0x7][2] * diffLength[k][3].scalarProduct(points[2]);
+ det[0xf][3] = det[0x7][0] * diffLength[k][3].dot(points[0]) +
+ det[0x7][1] * diffLength[k][3].dot(points[1]) +
+ det[0x7][2] * diffLength[k][3].dot(points[2]);
}
}
}
@@ -309,7 +332,7 @@ void Simplex::backupClosestPointInSimplex(Vector3D& v) {
for (bit = allBits; bit != 0x0; bit--) {
if (isSubset(bit, allBits) && isProperSubset(bit)) {
Vector3D u = computeClosestPointForSubset(bit);
- double distSquare = u.scalarProduct(u);
+ double distSquare = u.dot(u);
if (distSquare < minDistSquare) {
minDistSquare = distSquare;
bitsCurrentSimplex = bit;
diff --git a/src/collision/Simplex.h b/src/collision/GJK/Simplex.h
similarity index 98%
rename from src/collision/Simplex.h
rename to src/collision/GJK/Simplex.h
index 3065a39b..ae93340c 100644
--- a/src/collision/Simplex.h
+++ b/src/collision/GJK/Simplex.h
@@ -75,6 +75,7 @@ class Simplex {
bool isFull() const; // Return true if the simplex contains 4 points
bool isEmpty() const; // Return true if the simple is empty
+ unsigned int getSimplex(Vector3D* suppPointsA, Vector3D* suppPointsB, Vector3D* points) const; // Return the points of the simplex
double getMaxLengthSquareOfAPoint() const; // Return the maximum squared length of a point
void addPoint(const Vector3D& point, const Vector3D& suppPointA, const Vector3D& suppPointB); // Addd a point to the simplex
bool isPointInSimplex(const Vector3D& point) const; // Return true if the point is in the simplex
diff --git a/src/collision/SATAlgorithm.cpp b/src/collision/SATAlgorithm.cpp
index 7401e0b1..f7dc6479 100644
--- a/src/collision/SATAlgorithm.cpp
+++ b/src/collision/SATAlgorithm.cpp
@@ -108,13 +108,13 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
// Axis A0
for (int i=0; i<3; ++i) {
- c[0][i] = obb1->getAxis(0).scalarProduct(obb2->getAxis(i));
+ c[0][i] = obb1->getAxis(0).dot(obb2->getAxis(i));
absC[0][i] = fabs(c[0][i]);
if (absC[0][i] > cutoff) {
existsParallelPair = true;
}
}
- udc1[0] = obb1->getAxis(0).scalarProduct(boxDistance);
+ udc1[0] = obb1->getAxis(0).dot(boxDistance);
center = udc1[0];
radius1 = obb1->getExtent(0);
radius2 = obb2->getExtent(0)*absC[0][0] + obb2->getExtent(1)*absC[0][1] + obb2->getExtent(2) * absC[0][2];
@@ -133,13 +133,13 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
// Axis A1
for (int i=0; i<3; ++i) {
- c[1][i] = obb1->getAxis(1).scalarProduct(obb2->getAxis(i));
+ c[1][i] = obb1->getAxis(1).dot(obb2->getAxis(i));
absC[1][i] = fabs(c[1][i]);
if (absC[1][i] > cutoff) {
existsParallelPair = true;
}
}
- udc1[1] = obb1->getAxis(1).scalarProduct(boxDistance);
+ udc1[1] = obb1->getAxis(1).dot(boxDistance);
center = udc1[1];
radius1 = obb1->getExtent(1);
radius2 = obb2->getExtent(0)*absC[1][0] + obb2->getExtent(1)*absC[1][1] + obb2->getExtent(2) * absC[1][2];
@@ -158,13 +158,13 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
// Axis A2
for (int i=0; i<3; ++i) {
- c[2][i] = obb1->getAxis(2).scalarProduct(obb2->getAxis(i));
+ c[2][i] = obb1->getAxis(2).dot(obb2->getAxis(i));
absC[2][i] = fabs(c[2][i]);
if (absC[2][i] > cutoff) {
existsParallelPair = true;
}
}
- udc1[2] = obb1->getAxis(2).scalarProduct(boxDistance);
+ udc1[2] = obb1->getAxis(2).dot(boxDistance);
center = udc1[2];
radius1 = obb1->getExtent(2);
radius2 = obb2->getExtent(0)*absC[2][0] + obb2->getExtent(1)*absC[2][1] + obb2->getExtent(2)*absC[2][2];
@@ -182,7 +182,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
}
// Axis B0
- udc2[0] = obb2->getAxis(0).scalarProduct(boxDistance);
+ udc2[0] = obb2->getAxis(0).dot(boxDistance);
center = udc2[0];
radius1 = obb1->getExtent(0)*absC[0][0] + obb1->getExtent(1)*absC[1][0] + obb1->getExtent(2) * absC[2][0];
radius2 = obb2->getExtent(0);
@@ -200,7 +200,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
}
// Axis B1
- udc2[1] = obb2->getAxis(1).scalarProduct(boxDistance);
+ udc2[1] = obb2->getAxis(1).dot(boxDistance);
center = udc2[1];
radius1 = obb1->getExtent(0)*absC[0][1] + obb1->getExtent(1)*absC[1][1] + obb1->getExtent(2) * absC[2][1];
radius2 = obb2->getExtent(1);
@@ -218,7 +218,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
}
// Axis B2
- udc2[2] = obb2->getAxis(2).scalarProduct(boxDistance);
+ udc2[2] = obb2->getAxis(2).dot(boxDistance);
center = udc2[2];
radius1 = obb1->getExtent(0)*absC[0][2] + obb1->getExtent(1)*absC[1][2] + obb1->getExtent(2)*absC[2][2];
radius2 = obb2->getExtent(2);
@@ -260,7 +260,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
- normal = computeContactNormal(obb1->getAxis(0).crossProduct(obb2->getAxis(0)), boxDistance); // Compute the contact normal with the correct sign
+ normal = computeContactNormal(obb1->getAxis(0).cross(obb2->getAxis(0)), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A0 x B1
@@ -277,7 +277,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
- normal = computeContactNormal(obb1->getAxis(0).crossProduct(obb2->getAxis(1)), boxDistance); // Compute the contact normal with the correct sign
+ normal = computeContactNormal(obb1->getAxis(0).cross(obb2->getAxis(1)), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A0 x B2
@@ -294,7 +294,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
- normal = computeContactNormal(obb1->getAxis(0).crossProduct(obb2->getAxis(2)), boxDistance); // Compute the contact normal with the correct sign
+ normal = computeContactNormal(obb1->getAxis(0).cross(obb2->getAxis(2)), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A1 x B0
@@ -311,7 +311,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
- normal = computeContactNormal(obb1->getAxis(1).crossProduct(obb2->getAxis(0)), boxDistance); // Compute the contact normal with the correct sign
+ normal = computeContactNormal(obb1->getAxis(1).cross(obb2->getAxis(0)), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A1 x B1
@@ -328,7 +328,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
- normal = computeContactNormal(obb1->getAxis(1).crossProduct(obb2->getAxis(1)), boxDistance); // Compute the contact normal with the correct sign
+ normal = computeContactNormal(obb1->getAxis(1).cross(obb2->getAxis(1)), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A1 x B2
@@ -345,7 +345,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
- normal = computeContactNormal(obb1->getAxis(1).crossProduct(obb2->getAxis(2)), boxDistance); // Compute the contact normal with the correct sign
+ normal = computeContactNormal(obb1->getAxis(1).cross(obb2->getAxis(2)), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A2 x B0
@@ -362,7 +362,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
- normal = computeContactNormal(obb1->getAxis(2).crossProduct(obb2->getAxis(0)), boxDistance); // Compute the contact normal with the correct sign
+ normal = computeContactNormal(obb1->getAxis(2).cross(obb2->getAxis(0)), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A2 x B1
@@ -379,7 +379,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
- normal = computeContactNormal(obb1->getAxis(2).crossProduct(obb2->getAxis(1)), boxDistance); // Compute the contact normal with the correct sign
+ normal = computeContactNormal(obb1->getAxis(2).cross(obb2->getAxis(1)), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A2 x B2
@@ -396,7 +396,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
- normal = computeContactNormal(obb1->getAxis(2).crossProduct(obb2->getAxis(2)), boxDistance); // Compute the contact normal with the correct sign
+ normal = computeContactNormal(obb1->getAxis(2).cross(obb2->getAxis(2)), boxDistance); // Compute the contact normal with the correct sign
}
// Compute the contact info
diff --git a/src/collision/SATAlgorithm.h b/src/collision/SATAlgorithm.h
index 7f1d0446..e693e980 100644
--- a/src/collision/SATAlgorithm.h
+++ b/src/collision/SATAlgorithm.h
@@ -65,7 +65,7 @@ class SATAlgorithm : public NarrowPhaseAlgorithm {
// Return the contact normal with the correct sign (from obb1 toward obb2). "axis" is the axis vector direction where the
// collision occur and "distanceOfOBBs" is the vector (obb2.center - obb1.center).
inline Vector3D SATAlgorithm::computeContactNormal(const Vector3D& axis, const Vector3D& distanceOfOBBs) const {
- if (distanceOfOBBs.scalarProduct(axis) >= 0.0) {
+ if (distanceOfOBBs.dot(axis) >= 0.0) {
return axis;
}
else {