From 2acf563508bd20d4de88a1686818feb29b4b4c92 Mon Sep 17 00:00:00 2001 From: "chappuis.daniel" Date: Sat, 29 Jan 2011 17:48:48 +0000 Subject: [PATCH] Add the Simplex class git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@415 92aac97c-a6ce-11dd-a772-7fcde58d38e6 --- src/collision/Simplex.cpp | 351 ++++++++++++++++++++++++++++++++++++++ src/collision/Simplex.h | 108 ++++++++++++ 2 files changed, 459 insertions(+) create mode 100644 src/collision/Simplex.cpp create mode 100644 src/collision/Simplex.h diff --git a/src/collision/Simplex.cpp b/src/collision/Simplex.cpp new file mode 100644 index 00000000..f562a809 --- /dev/null +++ b/src/collision/Simplex.cpp @@ -0,0 +1,351 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 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 "Simplex.h" +#include + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor +Simplex::Simplex() : bitsCurrentSimplex(0x0), allBits(0x0) { + +} + +// Destructor +Simplex::~Simplex() { + +} + +// Add a new support point of (A-B) into the simplex +// suppPointA : support point of object A in a direction -v +// suppPointB : support point of object B in a direction v +// point : support point of object (A-B) => point = suppPointA - suppPointB +void Simplex::addPoint(const Vector3D& point, const Vector3D& suppPointA, const Vector3D& suppPointB) { + assert(!isFull()); + + lastFound = 0; + lastFoundBit = 0x1; + + // Look for the bit corresponding to one of the four point that is not in + // the current simplex + while (overlap(bitsCurrentSimplex, lastFoundBit)) { + lastFound++; + lastFoundBit <<= 1; + } + + assert(lastFound >= 0 && lastFound < 4); + + // Add the point into the simplex + points[lastFound] = point; + pointsLengthSquare[lastFound] = point.scalarProduct(point); + allBits = bitsCurrentSimplex | lastFoundBit; + + // Update the cached values + updateCache(); + + // Compute the cached determinant values + computeDeterminants(); + + // Add the support points of objects A and B + suppPointsA[lastFound] = suppPointA; + suppPointsB[lastFound] = suppPointB; +} + +// Return true if the point is in the simplex +bool Simplex::isPointInSimplex(const Vector3D& point) const { + int i; + Bits bit; + + // For each four possible points in the simplex + for (i=0, bit = 0x1; i<4; i++, bit <<= 1) { + // Check if the current point is in the simplex + if (overlap(allBits, bit) && point == points[i]) return true; + } + + return false; +} + +// Update the cached values used during the GJK algorithm +void Simplex::updateCache() { + int i; + Bits bit; + + // For each of the four possible points of the simplex + for (i=0, bit = 0x1; i<4; i++, bit <<= 1) { + // If the current points is in the simplex + if (overlap(bitsCurrentSimplex, bit)) { + + // Compute the distance between two points in the possible simplex set + diffLength[i][lastFound] = points[i] - points[lastFound]; + 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]); + } + } +} + +// Compute the cached determinant values +void Simplex::computeDeterminants() { + det[lastFoundBit][lastFound] = 1.0; + + // If the current simplex is not empty + if (!isEmpty()) { + int i; + Bits bitI; + + // For each possible four points in the simplex set + for (i=0, bitI = 0x1; i<4; i++, bitI <<= 1) { + + // If the current point is in the simplex + 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]); + + + int j; + Bits bitJ; + + for (j=0, bitJ = 0x1; j 0 for each "i" in I_x and +// 2. delta(X U {y_j})_j <= 0 for each "j" not in I_x_ +bool Simplex::isValidSubset(Bits subset) const { + int i; + Bits bit; + + // For each four point in the possible simplex set + for (i=0, bit=0x1; i<4; i++, bit <<= 1) { + if (overlap(allBits, bit)) { + // If the current point is in the subset + if (overlap(subset, bit)) { + // If one delta(X)_i is smaller or equal to zero + if (det[subset][i] <= 0.0) { + // The subset is not valid + return false; + } + } + // If the point is not in the subset and the value delta(X U {y_j})_j + // is bigger than zero + else if (det[subset | bit][i] > 0.0) { + // The subset is not valid + return false; + } + } + } + + return true; +} + +// Compute the closest points "pA" and "pB" of object A and B +// The points are computed as follows : +// pA = sum(lambda_i * a_i) where "a_i" are the support points of object A +// pB = sum(lambda_i * b_i) where "b_i" are the support points of object B +// with lambda_i = deltaX_i / deltaX +void Simplex::computeClosestPointsOfAandB(Vector3D& pA, Vector3D& pB) const { + double deltaX = 0.0; + pA.setAllValues(0.0, 0.0, 0.0); + pB.setAllValues(0.0, 0.0, 0.0); + int i; + Bits bit; + + // For each four points in the possible simplex set + for (i=0, bit=0x1; i<4; i++, bit <<= 1) { + // If the current point is part of the simplex + if (overlap(bitsCurrentSimplex, bit)) { + deltaX += det[bitsCurrentSimplex][i]; + pA = pA + det[bitsCurrentSimplex][i] * suppPointsA[i]; + pB = pB + det[bitsCurrentSimplex][i] * suppPointsB[i]; + } + } + + assert(deltaX > 0.0); + double factor = 1.0 / deltaX; + pA = factor * pA; + pB = factor * pB; +} + +// Compute the closest point "v" to the origin of the current simplex +// This method executes the Jonhnson's algorithm for computing the point +// "v" of simplex that is closest to the origin. The method returns true +// if a closest point has been found. +bool Simplex::computeClosestPoint(Vector3D& v) { + Bits subset; + + // For each possible simplex set + for (subset=bitsCurrentSimplex; subset != 0x0; subset--) { + // If the simplex is a subset of the current simplex and is valid for the Johnson's + // algorithm test + if (isSubset(subset, bitsCurrentSimplex) && isValidSubset(subset | lastFoundBit)) { + bitsCurrentSimplex = subset | lastFoundBit; // Add the last added point to the current simplex + v = computeClosestPointForSubset(bitsCurrentSimplex); // Compute the closest point in the simplex + return true; + } + } + + // If the simplex that contains only the last added point is valid for the + // Johnson's algorithm test + if (isValidSubset(lastFoundBit)) { + bitsCurrentSimplex = lastFoundBit; // Set the current simplex to the set that contains only the last added point + v = points[lastFound]; // The closest point of the simplex "v" is the last added point + return true; + } + + // The algorithm failed to found a point + return false; +} + +// Backup the closest point +void Simplex::backupClosestPointInSimplex(Vector3D& v) { + double minDistSquare = DBL_MAX; + Bits bit; + + for (bit = allBits; bit != 0x0; bit--) { + if (isSubset(bit, allBits) && isProperSubset(bit)) { + Vector3D u = computeClosestPointForSubset(bit); + double distSquare = u.scalarProduct(u); + if (distSquare < minDistSquare) { + minDistSquare = distSquare; + bitsCurrentSimplex = bit; + v = u; + } + } + } +} + +// Return the closest point "v" in the convex hull of the points in the subset +// represented by the bits "subset" +Vector3D Simplex::computeClosestPointForSubset(Bits subset) const { + Vector3D v(0.0, 0.0, 0.0); // Closet point v = sum(lambda_i * points[i]) + double maxLenSquare = 0.0; + double deltaX = 0.0; // deltaX = sum of all det[subset][i] + int i; + Bits bit; + + // For each four point in the possible simplex set + for (i=0, bit=0x1; i<4; i++, bit <<= 1) { + // If the current point is in the subset + if (overlap(subset, bit)) { + // deltaX = sum of all det[subset][i] + deltaX += det[subset][i]; + + if (maxLenSquare < pointsLengthSquare[i]) { + maxLenSquare = pointsLengthSquare[i]; + } + + // Closest point v = sum(lambda_i * points[i]) + v = v + det[subset][i] * points[i]; + } + } + + assert(deltaX > 0.0); + + // Return the closet point "v" in the convex hull for the given subset + return (1.0 / deltaX) * v; +} \ No newline at end of file diff --git a/src/collision/Simplex.h b/src/collision/Simplex.h new file mode 100644 index 00000000..e1e51e18 --- /dev/null +++ b/src/collision/Simplex.h @@ -0,0 +1,108 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010 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 SIMPLEX_H +#define SIMPLEX_H + +// Libraries +#include "../mathematics/mathematics.h" +#include + +// ReactPhysics3D namespace +namespace reactphysics3d { + +// Type definitions +typedef unsigned int Bits; + +/* ------------------------------------------------------------------- + Class Simplex : + This class represents a simplex which is a set of 3D points. This + class is used in the GJK algorithm. This implementation is based on + the implementation discussed in the book "Collision Detection in 3D + Environments". This class implements the Johnson's algorithm for + computing the point of a simplex that is closest to the origin and also + the smallest simplex needed to represent that closest point. + ------------------------------------------------------------------- +*/ +class Simplex { + private: + Vector3D points[4]; // Current points + double pointsLengthSquare[4]; // pointsLengthSquare[i] = (points[i].length)^2 + double maxLengthSquare; // Maximum length of pointsLengthSquare[i] + Vector3D suppPointsA[4]; // Support points of object A in local coordinates + Vector3D suppPointsB[4]; // Support points of object B in local coordinates + Vector3D diffLength[4][4]; // diff[i][j] contains points[i] - points[j] + double det[16][4]; // Cached determinant values + double normSquare[4][4]; // norm[i][j] = (diff[i][j].length())^2 + Bits bitsCurrentSimplex; // 4 bits that identify the current points of the simplex + // For instance, 0101 means that points[1] and points[3] are in the simplex + Bits lastFound; // Number between 1 and 4 that identify the last found support point + Bits lastFoundBit; // Position of the last found support point (lastFoundBit = 0x1 << lastFound) + Bits allBits; // allBits = bitsCurrentSimplex | lastFoundBit; + + bool overlap(Bits a, Bits b) const; // Return true if some bits of "a" overlap with bits of "b" + bool isSubset(Bits a, Bits b) const; // Return true if the bits of "b" is a subset of the bits of "a" + bool isValidSubset(Bits subset) const; // Return true if the subset is a valid one for the closest point computation + bool isProperSubset(Bits subset) const; // Return true if the subset is a proper subset + void updateCache(); // Update the cached values used during the GJK algorithm + void computeDeterminants(); // Compute the cached determinant values + void backupClosestPointInSimplex(Vector3D& point); // Backup the closest point + Vector3D computeClosestPointForSubset(Bits subset) const; // Return the closest point "v" in the convex hull of a subset of points + + public: + Simplex(); // Constructor + ~Simplex(); // Destructor + + bool isFull() const; // Return true if the simplex contains 4 points + bool isEmpty() const; // Return true if the simple is empty + 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 + bool isAffinelyDependent() const; // Return true if the set is affinely dependent + void computeClosestPointsOfAandB(Vector3D& pA, Vector3D& pB) const; // Compute the closest points of object A and B + bool computeClosestPoint(Vector3D& v); // Compute the closest point to the origin of the current simplex +}; + +// Return true if some bits of "a" overlap with bits of "b" +inline bool Simplex::overlap(Bits a, Bits b) const { + return ((a & b) != 0x0); +} + +// Return true if the bits of "b" is a subset of the bits of "a" +inline bool Simplex::isSubset(Bits a, Bits b) const { + return ((a & b) == a); +} + +// Return true if the simplex contains 4 points +inline bool Simplex::isFull() const { + return (bitsCurrentSimplex == 0xf); +} + +// Return true if the simple is empty +inline bool Simplex::isEmpty() const { + return (bitsCurrentSimplex == 0x0); +} + +} // End of the ReactPhysics3D namespace + +#endif \ No newline at end of file