diff --git a/src/body/BoundingSphere.h b/src/body/BoundingSphere.h index 8768b3b2..54346cf5 100644 --- a/src/body/BoundingSphere.h +++ b/src/body/BoundingSphere.h @@ -46,16 +46,18 @@ class BoundingSphere : public NarrowBoundingVolume { BoundingSphere(const Vector3D& center, double radius); // Constructor virtual ~BoundingSphere(); // Destructor - Vector3D getCenter() const; // Return the center point of the sphere - void setCenter(const Vector3D& center); // Set the center point of the sphere - double getRadius() const; // Return the radius of the sphere - void setRadius(double radius); // Set the radius of the sphere + Vector3D getCenter() const; // Return the center point of the sphere + void setCenter(const Vector3D& center); // Set the center point of the sphere + double getRadius() const; // Return the radius of the sphere + void setRadius(double radius); // Set the radius of the sphere virtual void update(const Vector3D& newCenter, - const Quaternion& rotationQuaternion); // Update the sphere orientation according to a new orientation of the rigid body - virtual AABB* computeAABB() const; // Return the corresponding AABB - #ifdef VISUAL_DEBUG + const Quaternion& rotationQuaternion); // Update the sphere orientation according to a new orientation of the rigid body + virtual AABB* computeAABB() const; // Return the corresponding AABB + virtual Vector3D getSupportPoint(const Vector3D& direction) const; // Return a support point in a given direction + +#ifdef VISUAL_DEBUG virtual void draw() const; // Draw the sphere (only for testing purpose) - #endif +#endif }; // Return the center point of the sphere @@ -84,6 +86,14 @@ inline void BoundingSphere::update(const Vector3D& newCenter, const Quaternion& center = newCenter; } +// Return a support point in a given direction +inline Vector3D BoundingSphere::getSupportPoint(const Vector3D& direction) const { + assert(direction.length() > 0.0); + + // Return the support point of the sphere in the given direction + return center + radius * direction.getUnit(); +} + }; // End of the ReactPhysics3D namespace #endif \ No newline at end of file diff --git a/src/body/NarrowBoundingVolume.h b/src/body/NarrowBoundingVolume.h index 64d07a30..8b0a9c55 100644 --- a/src/body/NarrowBoundingVolume.h +++ b/src/body/NarrowBoundingVolume.h @@ -45,10 +45,11 @@ class NarrowBoundingVolume : public BoundingVolume { protected : public : - NarrowBoundingVolume(); // Constructor - virtual ~NarrowBoundingVolume(); // Destructor + NarrowBoundingVolume(); // Constructor + virtual ~NarrowBoundingVolume(); // Destructor - virtual AABB* computeAABB() const=0; // Return the corresponding AABB + virtual AABB* computeAABB() const=0; // Return the corresponding AABB + virtual Vector3D getSupportPoint(const Vector3D& direction) const=0; // Return a support point in a given direction }; } diff --git a/src/body/OBB.h b/src/body/OBB.h index 744e3210..1d8d0c21 100644 --- a/src/body/OBB.h +++ b/src/body/OBB.h @@ -66,9 +66,11 @@ class OBB : public NarrowBoundingVolume { virtual std::vector getExtremeVertices(const Vector3D& axis) const; // Return all the vertices that are projected at the extreme of the projection of the bouding volume on the axis virtual void update(const Vector3D& newCenter, const Quaternion& rotationQuaternion); // Update the oriented bounding box orientation according to a new orientation of the rigid body virtual AABB* computeAABB() const; // Return the corresponding AABB - #ifdef VISUAL_DEBUG + virtual Vector3D getSupportPoint(const Vector3D& direction) const; // Return a support point in a given direction + +#ifdef VISUAL_DEBUG virtual void draw() const; // Draw the OBB (only for testing purpose) - #endif +#endif static OBB* computeFromVertices(const std::vector& vertices, const Vector3D& center); // Compute an OBB from a set of vertices }; @@ -225,6 +227,13 @@ inline void OBB::update(const Vector3D& newCenter, const Quaternion& rotationQua axis[2] = rotateVectorWithQuaternion(oldAxis[2], rotationQuaternion).getUnit(); } +// Return a support point in a given direction +inline Vector3D OBB::getSupportPoint(const Vector3D& direction) const { + // TODO : Implement this method + assert(false); + return Vector3D(); +} + }; // End of the ReactPhysics3D namespace #endif diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 82def353..baed6021 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -107,16 +107,33 @@ void CollisionDetection::computeAllContacts() { assert(contactInfo); // Compute one or several new contacts and add them into the physics world - computeContact(contactInfo); + computeContactSAT(contactInfo); } } -// Compute a contact (and add it to the physics world) for two colliding bodies -void CollisionDetection::computeContact(const ContactInfo* const contactInfo) { +// Compute a contact for GJK (and add it to the physics world) +void CollisionDetection::computeContactGJK(const ContactInfo* const contactInfo) { + // TODO : Compute PersisentContact here instead + + // Compute the set of contact points + vector contactPoints; + contactPoints.push_back(contactInfo->point1); + contactPoints.push_back(contactInfo->point2); + + // Create a new contact + Contact* contact = new Contact(contactInfo->body1, contactInfo->body2, contactInfo->normal, contactInfo->penetrationDepth, contactPoints); + + // Add the contact to the physics world + world->addConstraint(contact); +} + +// Compute a contact for the SAT algorithm (and add it to the physics world) for two colliding bodies +// This method only works for collision between two OBB bounding volumes +void CollisionDetection::computeContactSAT(const ContactInfo* const contactInfo) { // Extract informations from the contact info structure - const OBB* const obb1 = contactInfo->obb1; - const OBB* const obb2 = contactInfo->obb2; + const OBB* const obb1 = dynamic_cast(contactInfo->body1->getNarrowBoundingVolume()); + const OBB* const obb2 = dynamic_cast(contactInfo->body2->getNarrowBoundingVolume()); Vector3D normal = contactInfo->normal; double penetrationDepth = contactInfo->penetrationDepth; @@ -238,4 +255,5 @@ void CollisionDetection::computeContact(const ContactInfo* const contactInfo) { // Create a new contact and add it to the physics world world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedFace)); } -} \ No newline at end of file +} + diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 7487753a..20c50311 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -55,8 +55,8 @@ class CollisionDetection { void computeBroadPhase(); // Compute the broad-phase collision detection void computeNarrowPhase(); // Compute the narrow-phase collision detection void computeAllContacts(); // Compute all the contacts from the collision info list - void computeContact(const ContactInfo* const contactInfo); // Compute a contact (and add it to the physics world) for two colliding bodies - void computeContact2(const ContactInfo* const contactInfo); // Compute a contact (and add it to the physics world) for two colliding bodies + void computeContactSAT(const ContactInfo* const contactInfo); // Compute a contact for SAT (and add it to the physics world) for two colliding bodies + void computeContactGJK(const ContactInfo* const contactInfo); // Compute a contact for GJK (and add it to the physics world) public : CollisionDetection(PhysicsWorld* physicsWorld); // Constructor diff --git a/src/collision/ContactInfo.cpp b/src/collision/ContactInfo.cpp index 9d7c9016..46613abf 100644 --- a/src/collision/ContactInfo.cpp +++ b/src/collision/ContactInfo.cpp @@ -27,8 +27,15 @@ using namespace reactphysics3d; -// Constructor -ContactInfo::ContactInfo(const OBB* const obb1, const OBB* const obb2, const Vector3D& normal, double penetrationDepth) - : obb1(obb1), obb2(obb2), normal(normal), penetrationDepth(penetrationDepth) { +// Constructor for SAT +ContactInfo::ContactInfo(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth) + : body1(body1), body2(body2), normal(normal), penetrationDepth(penetrationDepth), point1(0.0, 0.0, 0.0), point2(0.0, 0.0, 0.0) { } + +// Constructor for GJK +ContactInfo::ContactInfo(Body* const body1, Body* const body2, const Vector3D& normal, + double penetrationDepth, const Vector3D& point1, const Vector3D& point2) + : body1(body1), body2(body2), normal(normal), penetrationDepth(penetrationDepth), point1(point1), point2(point2) { + +} diff --git a/src/collision/ContactInfo.h b/src/collision/ContactInfo.h index 54a765ca..3df5c885 100644 --- a/src/collision/ContactInfo.h +++ b/src/collision/ContactInfo.h @@ -42,13 +42,17 @@ namespace reactphysics3d { */ struct ContactInfo { public: - // TODO : Use polymorphism here (change OBB into BoundingVolume to be more general) - const OBB* const obb1; // Body pointer of the first bounding volume - const OBB* const obb2; // Body pointer of the second bounding volume - const Vector3D normal; // Normal vector the the collision contact - const double penetrationDepth; // Penetration depth of the contact + Body* const body1; // Pointer to the first body of the contact + Body* const body2; // Pointer to the second body of the contact + const Vector3D point1; // Contact point of body 1 + const Vector3D point2; // Contact point of body 2 + const Vector3D normal; // Normal vector the the collision contact + const double penetrationDepth; // Penetration depth of the contact - ContactInfo(const OBB* const obb1, const OBB* const obb2, const Vector3D& normal, double penetrationDepth); // Constructor + ContactInfo(Body* const body1, Body* const body2, const Vector3D& normal, + double penetrationDepth); // Constructor for SAT + ContactInfo(Body* const body1, Body* const body2, const Vector3D& normal, + double penetrationDepth, const Vector3D& point1, const Vector3D& point2); // Constructor for GJK }; } // End of the ReactPhysics3D namespace diff --git a/src/collision/GJKAlgorithm.cpp b/src/collision/GJKAlgorithm.cpp new file mode 100644 index 00000000..5b2aae81 --- /dev/null +++ b/src/collision/GJKAlgorithm.cpp @@ -0,0 +1,207 @@ + +/******************************************************************************** +* 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 "GJKAlgorithm.h" +#include "Simplex.h" +#include "../constraint/Contact.h" +#include "../constants.h" +#include +#include +#include +#include + + +// We want to use the ReactPhysics3D namespace +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 + + +} + +// Destructor +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. +bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolume1, + const NarrowBoundingVolume* const boundingVolume2, + ContactInfo*& contactInfo) { + Vector3D suppA; // Support point of object A + Vector3D suppB; // Support point of object B + Vector3D w; // Support point of Minkowski difference A-B + Vector3D pA; // Closest point of object A + Vector3D pB; // Closest point of object B + double vDotw; + double prevDistSquare; + + assert(boundingVolume1 != boundingVolume2); + + // Initialize the margin (sum of margins of both objects) + double margin = 2 * OBJECT_MARGIN; + double marginSquare = margin * margin; + assert(margin > 0.0); + + // Create a simplex set + Simplex simplex; + + // Get the last point V (last separating axis) + Vector3D v = lastSeparatingAxis; + + // Initialize the upper bound for the square distance + double distSquare = DBL_MAX; + + do { + // Compute the support points for object 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); + + // If the enlarge objects (with margins) do not intersect + if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) { + // No intersection, we return false + return false; + } + + // If the objects intersect only in the margins + if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * REL_ERROR_SQUARE) { + // Compute the closet points of both objects (without the margins) + simplex.computeClosestPointsOfAandB(pA, pB); + + // Project those two points on the margins to have the closest points of both + // object with the margins + double dist = sqrt(distSquare); + assert(dist > 0.0); + pA = pA - (OBJECT_MARGIN / dist) * v; + pB = pB + (OBJECT_MARGIN / dist) * v; + + // Compute the contact info + Vector3D normal = (pB - pA).getUnit(); + double penetrationDepth = margin - dist; + assert(penetrationDepth > 0.0); + contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(), + normal, penetrationDepth, pA, pB); + + // There is an intersection, therefore we return true + return true; + } + + // Add the current point to the simplex + simplex.addPoint(w, suppA, suppB); + + // If the simplex is affinely dependent + if (simplex.isAffinelyDependent()) { + // Compute the closet points of both objects (without the margins) + simplex.computeClosestPointsOfAandB(pA, pB); + + // Project those two points on the margins to have the closest points of both + // object with the margins + double dist = sqrt(distSquare); + assert(dist > 0.0); + pA = pA - (OBJECT_MARGIN / dist) * v; + pB = pB + (OBJECT_MARGIN / dist) * v; + + // Compute the contact info + Vector3D normal = (pB - pA).getUnit(); + double penetrationDepth = margin - dist; + assert(penetrationDepth > 0.0); + contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(), + normal, penetrationDepth, pA, pB); + + // There is an intersection, therefore we return true + return true; + } + + // Compute the point of the simplex closest to the origin + // If the computation of the closest point fail + if (!simplex.computeClosestPoint(v)) { + // Compute the closet points of both objects (without the margins) + simplex.computeClosestPointsOfAandB(pA, pB); + + // Project those two points on the margins to have the closest points of both + // object with the margins + double dist = sqrt(distSquare); + assert(dist > 0.0); + pA = pA - (OBJECT_MARGIN / dist) * v; + pB = pB + (OBJECT_MARGIN / dist) * v; + + // Compute the contact info + Vector3D normal = (pB - pA).getUnit(); + double penetrationDepth = margin - dist; + assert(penetrationDepth > 0.0); + contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(), + normal, penetrationDepth, pA, pB); + + // There is an intersection, therefore we return true + return true; + } + + // Store and update the squared distance of the closest point + prevDistSquare = distSquare; + distSquare = v.scalarProduct(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); + + // Compute the closet points of both objects (without the margins) + simplex.computeClosestPointsOfAandB(pA, pB); + + // Project those two points on the margins to have the closest points of both + // object with the margins + double dist = sqrt(distSquare); + assert(dist > 0.0); + pA = pA - (OBJECT_MARGIN / dist) * v; + pB = pB + (OBJECT_MARGIN / dist) * v; + + // Compute the contact info + Vector3D normal = (pB - pA).getUnit(); + double penetrationDepth = margin - dist; + assert(penetrationDepth > 0.0); + contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(), + normal, penetrationDepth, pA, pB); + + // There is an intersection, therefore we return true + return true; + } + + } while(!simplex.isFull() && distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint()); + + // The objects (without margins) intersect +} + + diff --git a/src/collision/GJKAlgorithm.h b/src/collision/GJKAlgorithm.h new file mode 100644 index 00000000..efe95df8 --- /dev/null +++ b/src/collision/GJKAlgorithm.h @@ -0,0 +1,69 @@ +/******************************************************************************** +* 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 GJKALGORITHM_H +#define GJKALGORITHM_H + +// Libraries +#include "NarrowPhaseAlgorithm.h" +#include "ContactInfo.h" +#include "../body/NarrowBoundingVolume.h" + + +// ReactPhysics3D namespace +namespace reactphysics3d { + +// Constants +const double REL_ERROR = 1.0e-3; +const double REL_ERROR_SQUARE = REL_ERROR * REL_ERROR; + + // TODO : Improve the description here +/* ------------------------------------------------------------------- + Class GJKAlgorithm : + This class implements a narrow-phase collision detection algorithm. This + 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". + ------------------------------------------------------------------- +*/ +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; + + public : + GJKAlgorithm(); // Constructor + ~GJKAlgorithm(); // Destructor + + virtual bool testCollision(const NarrowBoundingVolume* const boundingVolume1, + const NarrowBoundingVolume* const boundingVolume2, + ContactInfo*& contactInfo); // Return true and compute a contact info if the two bounding volume collide +}; + +} // End of the ReactPhysics3D namespace + +// TODO : Check what would be a correct value for the OBJECT_MARGIN constant + +#endif diff --git a/src/collision/NarrowPhaseAlgorithm.h b/src/collision/NarrowPhaseAlgorithm.h index 37b5f52a..7588ec27 100644 --- a/src/collision/NarrowPhaseAlgorithm.h +++ b/src/collision/NarrowPhaseAlgorithm.h @@ -47,7 +47,9 @@ class NarrowPhaseAlgorithm { NarrowPhaseAlgorithm(); // Constructor virtual ~NarrowPhaseAlgorithm(); // Destructor - virtual bool testCollision(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2, ContactInfo*& contactInfo)=0; // Return true and compute a contact info if the two bounding volume collide + virtual bool testCollision(const NarrowBoundingVolume* const boundingVolume1, + const NarrowBoundingVolume* const boundingVolume2, + ContactInfo*& contactInfo)=0; // Return true and compute a contact info if the two bounding volume collide }; } // End of reactphysics3d namespace diff --git a/src/collision/SATAlgorithm.cpp b/src/collision/SATAlgorithm.cpp index 0b3222d7..7401e0b1 100644 --- a/src/collision/SATAlgorithm.cpp +++ b/src/collision/SATAlgorithm.cpp @@ -48,7 +48,9 @@ SATAlgorithm::~SATAlgorithm() { // 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. -bool SATAlgorithm::testCollision(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2, ContactInfo*& contactInfo) { +bool SATAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolume1, + const NarrowBoundingVolume* const boundingVolume2, + ContactInfo*& contactInfo) { assert(boundingVolume1 != boundingVolume2); @@ -239,7 +241,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const // normals for separation. Therefore the OBBs must intersect // Compute the contact info - contactInfo = new ContactInfo(obb1, obb2, normal.getUnit(), minPenetrationDepth); + contactInfo = new ContactInfo(obb1->getBodyPointer(), obb2->getBodyPointer(), normal.getUnit(), minPenetrationDepth); return true; } @@ -398,7 +400,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const } // Compute the contact info - contactInfo = new ContactInfo(obb1, obb2, normal.getUnit(), minPenetrationDepth); + contactInfo = new ContactInfo(obb1->getBodyPointer(), obb2->getBodyPointer(), normal.getUnit(), minPenetrationDepth); return true; } diff --git a/src/collision/SATAlgorithm.h b/src/collision/SATAlgorithm.h index 4d465d95..7f1d0446 100644 --- a/src/collision/SATAlgorithm.h +++ b/src/collision/SATAlgorithm.h @@ -57,7 +57,9 @@ class SATAlgorithm : public NarrowPhaseAlgorithm { SATAlgorithm(); // Constructor ~SATAlgorithm(); // Destructor - virtual bool testCollision(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2, ContactInfo*& contactInfo); // Return true and compute a contact info if the two bounding volume collide + virtual bool testCollision(const NarrowBoundingVolume* const boundingVolume1, + const NarrowBoundingVolume* const boundingVolume2, + ContactInfo*& contactInfo); // Return true and compute a contact info if the two bounding volume collide }; // Return the contact normal with the correct sign (from obb1 toward obb2). "axis" is the axis vector direction where the diff --git a/src/collision/Simplex.h b/src/collision/Simplex.h index e1e51e18..3065a39b 100644 --- a/src/collision/Simplex.h +++ b/src/collision/Simplex.h @@ -67,7 +67,6 @@ class Simplex { 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: @@ -76,9 +75,11 @@ class Simplex { bool isFull() const; // Return true if the simplex contains 4 points bool isEmpty() const; // Return true if the simple is empty + 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 bool isAffinelyDependent() const; // Return true if the set is affinely dependent + void backupClosestPointInSimplex(Vector3D& point); // Backup the closest point 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 }; @@ -103,6 +104,11 @@ inline bool Simplex::isEmpty() const { return (bitsCurrentSimplex == 0x0); } +// Return the maximum squared length of a point +inline double Simplex::getMaxLengthSquareOfAPoint() const { + return maxLengthSquare; +} + } // End of the ReactPhysics3D namespace #endif \ No newline at end of file diff --git a/src/constants.h b/src/constants.h index e5c3e1b9..a4ddee22 100644 --- a/src/constants.h +++ b/src/constants.h @@ -27,6 +27,7 @@ // Libraries #include +#include // Type definitions typedef unsigned int uint; @@ -36,10 +37,14 @@ const double EPSILON = 0.00001; // E const double ONE_MINUS_EPSILON = 0.99999; // 1 - espilon const double INFINITY_CONST = std::numeric_limits::infinity(); // Infinity constant const double PI = 3.14159265; // Pi constant +const double MACHINE_EPSILON = DBL_EPSILON; // Physics Engine constants const double DEFAULT_TIMESTEP = 0.002; +// GJK Algorithm parameters +const double OBJECT_MARGIN = 0.1; // Object margin for the hybrid GJK algorithm + // Contact constants const double FRICTION_COEFFICIENT = 0.4; // Friction coefficient const double PENETRATION_FACTOR = 0.2; // Penetration factor (between 0 and 1) which specify the importance of the