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

Implementation of EPA Algorithm continued

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@417 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2011-02-07 14:51:54 +00:00
parent fdbb661df5
commit 844df20be0
14 changed files with 872 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
src/collision/EPA/EPAAlgorithm.cpp Normal file
View File

@ -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: {
}
}
}

74
src/collision/EPA/EPAAlgorithm.h Normal file
View File

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

86
src/collision/EPA/EdgeEPA.cpp Normal file
View File

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

90
src/collision/EPA/EdgeEPA.h Normal file
View File

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

99
src/collision/EPA/TriangleEPA.cpp Normal file
View File

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

133
src/collision/EPA/TriangleEPA.h Normal file
View File

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

39
src/collision/EPA/TrianglesStore.cpp Normal file
View File

@ -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() {
}

113
src/collision/EPA/TrianglesStore.h Normal file
View File

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

92
src/collision/GJKAlgorithm.cpp → src/collision/GJK/GJKAlgorithm.cpp
View File

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

20
src/collision/GJKAlgorithm.h → src/collision/GJK/GJKAlgorithm.h
View File

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

69
src/collision/Simplex.cpp → src/collision/GJK/Simplex.cpp
View File

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

1
src/collision/Simplex.h → src/collision/GJK/Simplex.h
View File

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

36
src/collision/SATAlgorithm.cpp
View File

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

2
src/collision/SATAlgorithm.h
View File

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