From c1eabd3e2bc29fb3f045a106d03ab0b1def09c0f Mon Sep 17 00:00:00 2001 From: "chappuis.daniel" Date: Wed, 18 Jan 2012 23:06:33 +0000 Subject: [PATCH] Add error correction using first world order projection (not active by default), change the Shape class into Collider class, add the new decimal type in order to easily switch between single and double precision git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@460 92aac97c-a6ce-11dd-a772-7fcde58d38e6 --- src/body/Body.cpp | 10 +- src/body/Body.h | 70 ++-- src/body/RigidBody.cpp | 12 +- src/body/RigidBody.h | 36 +- src/collision/CollisionDetection.cpp | 14 +- src/collision/CollisionDetection.h | 12 +- src/collision/ContactInfo.cpp | 2 +- src/collision/ContactInfo.h | 6 +- src/collision/PersistentContactCache.cpp | 12 +- src/collision/PersistentContactCache.h | 12 +- .../broadphase/SweepAndPruneAlgorithm.cpp | 8 +- .../broadphase/SweepAndPruneAlgorithm.h | 5 +- .../narrowphase/EPA/EPAAlgorithm.cpp | 78 ++-- src/collision/narrowphase/EPA/EPAAlgorithm.h | 10 +- src/collision/narrowphase/EPA/TriangleEPA.cpp | 12 +- src/collision/narrowphase/EPA/TriangleEPA.h | 16 +- .../narrowphase/EPA/TrianglesStore.h | 2 +- .../narrowphase/GJK/GJKAlgorithm.cpp | 84 ++-- src/collision/narrowphase/GJK/GJKAlgorithm.h | 14 +- src/collision/narrowphase/GJK/Simplex.cpp | 12 +- src/collision/narrowphase/GJK/Simplex.h | 12 +- .../narrowphase/NarrowPhaseAlgorithm.h | 4 +- src/configuration.h | 39 ++ src/constants.h | 72 ---- src/constraint/Constraint.cpp | 4 +- src/constraint/Constraint.h | 40 +- src/constraint/Contact.cpp | 20 +- src/constraint/Contact.h | 25 +- src/engine/ConstraintSolver.cpp | 378 +++++++++++++----- src/engine/ConstraintSolver.h | 143 +++++-- src/engine/PhysicsEngine.cpp | 38 +- src/engine/PhysicsEngine.h | 35 +- src/engine/PhysicsWorld.cpp | 14 +- src/engine/PhysicsWorld.h | 4 +- src/mathematics/Matrix3x3.cpp | 8 +- src/mathematics/Matrix3x3.h | 44 +- src/mathematics/Quaternion.cpp | 59 +-- src/mathematics/Quaternion.h | 95 ++--- src/mathematics/Transform.h | 20 +- src/mathematics/Vector3.cpp | 8 +- src/mathematics/Vector3.h | 69 ++-- src/mathematics/mathematics.h | 55 +-- src/mathematics/mathematics_functions.h | 7 +- src/memory/MemoryPool.h | 6 +- src/reactphysics3d.h | 10 +- src/shapes/AABB.h | 14 +- src/shapes/{BoxShape.cpp => BoxCollider.cpp} | 26 +- src/shapes/{BoxShape.h => BoxCollider.h} | 32 +- src/shapes/{Shape.cpp => Collider.cpp} | 6 +- src/shapes/{Shape.h => Collider.h} | 26 +- .../{ConeShape.cpp => ConeCollider..cpp} | 16 +- src/shapes/{ConeShape.h => ConeCollider.h} | 54 +-- ...CylinderShape.cpp => CylinderCollider.cpp} | 14 +- .../{CylinderShape.h => CylinderCollider.h} | 50 +-- .../{SphereShape.cpp => SphereCollider.cpp} | 8 +- .../{SphereShape.h => SphereCollider.h} | 44 +- 56 files changed, 1092 insertions(+), 834 deletions(-) delete mode 100644 src/constants.h rename src/shapes/{BoxShape.cpp => BoxCollider.cpp} (85%) rename src/shapes/{BoxShape.h => BoxCollider.h} (79%) rename src/shapes/{Shape.cpp => Collider.cpp} (96%) rename src/shapes/{Shape.h => Collider.h} (81%) rename src/shapes/{ConeShape.cpp => ConeCollider..cpp} (87%) rename src/shapes/{ConeShape.h => ConeCollider.h} (65%) rename src/shapes/{CylinderShape.cpp => CylinderCollider.cpp} (87%) rename src/shapes/{CylinderShape.h => CylinderCollider.h} (64%) rename src/shapes/{SphereShape.cpp => SphereCollider.cpp} (93%) rename src/shapes/{SphereShape.h => SphereCollider.h} (72%) diff --git a/src/body/Body.cpp b/src/body/Body.cpp index 3a507248..a1d8e57e 100644 --- a/src/body/Body.cpp +++ b/src/body/Body.cpp @@ -25,16 +25,16 @@ // Libraries #include "Body.h" -#include "../shapes/Shape.h" +#include "../shapes/Collider.h" // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; // Constructor -Body::Body(const Transform& transform, Shape* shape, double mass, long unsigned int id) - : shape(shape), mass(mass), transform(transform), id(id) { +Body::Body(const Transform& transform, Collider* collider, decimal mass, long unsigned int id) + : collider(collider), mass(mass), transform(transform), id(id) { assert(mass > 0.0); - assert(shape); + assert(collider); isMotionEnabled = true; isCollisionEnabled = true; @@ -44,7 +44,7 @@ Body::Body(const Transform& transform, Shape* shape, double mass, long unsigned oldTransform = transform; // Create the AABB for broad-phase collision detection - aabb = new AABB(transform, shape->getLocalExtents(OBJECT_MARGIN)); + aabb = new AABB(transform, collider->getLocalExtents(OBJECT_MARGIN)); } // Destructor diff --git a/src/body/Body.h b/src/body/Body.h index 0bf49957..e4c6bb4e 100644 --- a/src/body/Body.h +++ b/src/body/Body.h @@ -31,8 +31,8 @@ #include #include "../mathematics/Transform.h" #include "../shapes/AABB.h" -#include "../shapes/Shape.h" -#include "../constants.h" +#include "../shapes/Collider.h" +#include "../configuration.h" // Namespace reactphysics3d namespace reactphysics3d { @@ -46,36 +46,36 @@ namespace reactphysics3d { */ class Body { protected : - Shape* shape; // Collision shape of the body - double mass; // Mass of the body + Collider* collider; // Collider of the body + decimal mass; // Mass of the body Transform transform; // Position and orientation of the body Transform oldTransform; // Last position and orientation of the body - double interpolationFactor; // Interpolation factor used for the state interpolation + decimal interpolationFactor; // Interpolation factor used for the state interpolation bool isMotionEnabled; // True if the body is able to move bool isCollisionEnabled; // True if the body can collide with others bodies AABB* aabb; // Axis-Aligned Bounding Box for Broad-Phase collision detection luint id; // ID of the body public : - Body(const Transform& transform, Shape* shape, double mass, long unsigned int id); // Constructor - virtual ~Body(); // Destructor + Body(const Transform& transform, Collider* collider, decimal mass, long unsigned int id); // Constructor + virtual ~Body(); // Destructor - luint getID() const; // Return the id of the body - Shape* getShape() const; // Return the collision shape - void setShape(Shape* shape); // Set the collision shape - double getMass() const; // Return the mass of the body - void setMass(double mass); // Set the mass of the body - const Transform& getTransform() const; // Return the current position and orientation - void setTransform(const Transform& transform); // Set the current position and orientation - const AABB* getAABB() const; // Return the AAABB of the body - Transform getInterpolatedTransform() const; // Return the interpolated transform for rendering - void setInterpolationFactor(double factor); // Set the interpolation factor of the body - bool getIsMotionEnabled() const; // Return if the rigid body can move - void setIsMotionEnabled(bool isMotionEnabled); // Set the value to true if the body can move - bool getIsCollisionEnabled() const; // Return true if the body can collide with others bodies - void setIsCollisionEnabled(bool isCollisionEnabled); // Set the isCollisionEnabled value - void updateOldTransform(); // Update the old transform with the current one - void updateAABB(); // Update the Axis-Aligned Bounding Box coordinates + luint getID() const; // Return the id of the body + Collider* getCollider() const; // Return the collision collider + void setCollider(Collider* collider); // Set the collision collider + decimal getMass() const; // Return the mass of the body + void setMass(decimal mass); // Set the mass of the body + const Transform& getTransform() const; // Return the current position and orientation + void setTransform(const Transform& transform); // Set the current position and orientation + const AABB* getAABB() const; // Return the AAABB of the body + Transform getInterpolatedTransform() const; // Return the interpolated transform for rendering + void setInterpolationFactor(decimal factor); // Set the interpolation factor of the body + bool getIsMotionEnabled() const; // Return if the rigid body can move + void setIsMotionEnabled(bool isMotionEnabled); // Set the value to true if the body can move + bool getIsCollisionEnabled() const; // Return true if the body can collide with others bodies + void setIsCollisionEnabled(bool isCollisionEnabled); // Set the isCollisionEnabled value + void updateOldTransform(); // Update the old transform with the current one + void updateAABB(); // Update the Axis-Aligned Bounding Box coordinates }; // Return the id of the body @@ -83,20 +83,20 @@ inline luint Body::getID() const { return id; } -// Return the collision shape -inline Shape* Body::getShape() const { - assert(shape); - return shape; +// Return the collider +inline Collider* Body::getCollider() const { + assert(collider); + return collider; } -// Set the collision shape -inline void Body::setShape(Shape* shape) { - assert(shape); - this->shape = shape; +// Set the collider +inline void Body::setCollider(Collider* collider) { + assert(collider); + this->collider = collider; } // Method that return the mass of the body -inline double Body::getMass() const { +inline decimal Body::getMass() const { return mass; }; @@ -106,7 +106,7 @@ inline Transform Body::getInterpolatedTransform() const { } // Set the interpolation factor of the body -inline void Body::setInterpolationFactor(double factor) { +inline void Body::setInterpolationFactor(decimal factor) { // Set the factor interpolationFactor = factor; } @@ -122,7 +122,7 @@ inline void Body::setIsMotionEnabled(bool isMotionEnabled) { } // Method that set the mass of the body -inline void Body::setMass(double mass) { +inline void Body::setMass(decimal mass) { this->mass = mass; } @@ -160,7 +160,7 @@ inline void Body::updateOldTransform() { // Update the rigid body in order to reflect a change in the body state inline void Body::updateAABB() { // Update the AABB - aabb->update(transform, shape->getLocalExtents(OBJECT_MARGIN)); + aabb->update(transform, collider->getLocalExtents(OBJECT_MARGIN)); } diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 48db850b..88fb35cf 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -25,23 +25,23 @@ // Libraries #include "RigidBody.h" -#include "../shapes/Shape.h" +#include "../shapes/Collider.h" // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; // Constructor - RigidBody::RigidBody(const Transform& transform, double mass, const Matrix3x3& inertiaTensorLocal, Shape* shape, long unsigned id) - : Body(transform, shape, mass, id), inertiaTensorLocal(inertiaTensorLocal), + RigidBody::RigidBody(const Transform& transform, decimal mass, const Matrix3x3& inertiaTensorLocal, Collider* collider, long unsigned id) + : Body(transform, collider, mass, id), inertiaTensorLocal(inertiaTensorLocal), inertiaTensorLocalInverse(inertiaTensorLocal.getInverse()), massInverse(1.0/mass) { restitution = 1.0; - // Set the body pointer of the AABB and the shape + // Set the body pointer of the AABB and the collider aabb->setBodyPointer(this); - shape->setBodyPointer(this); + collider->setBodyPointer(this); - assert(shape); + assert(collider); assert(aabb); } diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 454bb213..7c978de5 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -29,7 +29,7 @@ // Libraries #include #include "Body.h" -#include "../shapes/Shape.h" +#include "../shapes/Collider.h" #include "../mathematics/mathematics.h" // Namespace reactphysics3d @@ -50,32 +50,32 @@ class RigidBody : public Body { Vector3 externalTorque; // Current external torque on the body Matrix3x3 inertiaTensorLocal; // Local inertia tensor of the body (in body coordinates) Matrix3x3 inertiaTensorLocalInverse; // Inverse of the inertia tensor of the body (in body coordinates) - double massInverse; // Inverse of the mass of the body - double restitution; // Coefficient of restitution (between 0 and 1), 1 for a very boucing body + decimal massInverse; // Inverse of the mass of the body + decimal restitution; // Coefficient of restitution (between 0 and 1), 1 for a very boucing body public : - RigidBody(const Transform& transform, double mass, const Matrix3x3& inertiaTensorLocal, - Shape* shape, long unsigned int id); // Constructor // Copy-constructor + RigidBody(const Transform& transform, decimal mass, const Matrix3x3& inertiaTensorLocal, + Collider* collider, long unsigned int id); // Constructor // Copy-constructor virtual ~RigidBody(); // Destructor Vector3 getLinearVelocity() const; // Return the linear velocity void setLinearVelocity(const Vector3& linearVelocity); // Set the linear velocity of the body Vector3 getAngularVelocity() const; // Return the angular velocity void setAngularVelocity(const Vector3& angularVelocity); // Set the angular velocity - void setMassInverse(double massInverse); // Set the inverse of the mass + void setMassInverse(decimal massInverse); // Set the inverse of the mass Vector3 getExternalForce() const; // Return the current external force of the body void setExternalForce(const Vector3& force); // Set the current external force on the body Vector3 getExternalTorque() const; // Return the current external torque of the body void setExternalTorque(const Vector3& torque); // Set the current external torque of the body - double getMassInverse() const; // Return the inverse of the mass of the body - Matrix3x3 getInertiaTensorLocal() const; // Return the local inertia tensor of the body (in body coordinates) - void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal); // Set the local inertia tensor of the body (in body coordinates) - Matrix3x3 getInertiaTensorLocalInverse() const; // Get the inverse of the inertia tensor - Matrix3x3 getInertiaTensorWorld() const; // Return the inertia tensor in world coordinates - Matrix3x3 getInertiaTensorInverseWorld() const; // Return the inverse of the inertia tensor in world coordinates + decimal getMassInverse() const; // Return the inverse of the mass of the body + Matrix3x3 getInertiaTensorLocal() const; // Return the local inertia tensor of the body (in body coordinates) + void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal); // Set the local inertia tensor of the body (in body coordinates) + Matrix3x3 getInertiaTensorLocalInverse() const; // Get the inverse of the inertia tensor + Matrix3x3 getInertiaTensorWorld() const; // Return the inertia tensor in world coordinates + Matrix3x3 getInertiaTensorInverseWorld() const; // Return the inverse of the inertia tensor in world coordinates - double getRestitution() const; // Get the restitution coefficient - void setRestitution(double restitution) throw(std::invalid_argument); // Set the restitution coefficient + decimal getRestitution() const; // Get the restitution coefficient + void setRestitution(decimal restitution) throw(std::invalid_argument); // Set the restitution coefficient }; // Return the linear velocity @@ -93,7 +93,7 @@ inline void RigidBody::setAngularVelocity(const Vector3& angularVelocity) { } // Set the inverse of the mass -inline void RigidBody::setMassInverse(double massInverse) { +inline void RigidBody::setMassInverse(decimal massInverse) { this->massInverse = massInverse; } @@ -123,7 +123,7 @@ inline void RigidBody::setExternalTorque(const Vector3& torque) { } // Return the inverse of the mass of the body -inline double RigidBody::getMassInverse() const { +inline decimal RigidBody::getMassInverse() const { return massInverse; } @@ -165,12 +165,12 @@ inline void RigidBody::setLinearVelocity(const Vector3& linearVelocity) { } // Get the restitution coeffficient of the rigid body -inline double RigidBody::getRestitution() const { +inline decimal RigidBody::getRestitution() const { return restitution; } // Set the restitution coefficient -inline void RigidBody::setRestitution(double restitution) throw(std::invalid_argument) { +inline void RigidBody::setRestitution(decimal restitution) throw(std::invalid_argument) { // Check if the restitution coefficient is between 0 and 1 if (restitution >= 0.0 && restitution <= 1.0) { this->restitution = restitution; diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 9b0b265f..f1222fba 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -28,9 +28,9 @@ #include "broadphase/SweepAndPruneAlgorithm.h" #include "narrowphase/GJK/GJKAlgorithm.h" #include "../body/Body.h" -#include "../shapes/BoxShape.h" +#include "../shapes/BoxCollider.h" #include "../body/RigidBody.h" -#include "../constants.h" +#include "../configuration.h" #include #include #include @@ -99,7 +99,6 @@ void CollisionDetection::computeBroadPhase() { overlappingPairs.erase(*it); } - // The current overlapping pairs become the last step overlapping pairs lastStepOverlappingPairs = currentStepOverlappingPairs; } @@ -120,14 +119,17 @@ bool CollisionDetection::computeNarrowPhase() { (*it).second->update(); // Use the narrow-phase collision detection algorithm to check if there really are a contact - if (narrowPhaseAlgorithm->testCollision(body1->getShape(), body1->getTransform(), - body2->getShape(), body2->getTransform(), contactInfo)) { + if (narrowPhaseAlgorithm->testCollision(body1->getCollider(), body1->getTransform(), + body2->getCollider(), body2->getTransform(), contactInfo)) { assert(contactInfo); collisionExists = true; // Create a new contact Contact* contact = new(memoryPoolContacts.allocateObject()) Contact(contactInfo); + // Free the contact info memory + delete contactInfo; + // Add the contact to the contact cache of the corresponding overlapping pair (*it).second->addContact(contact); @@ -146,7 +148,7 @@ bool CollisionDetection::computeNarrowPhase() { // Allow the broadphase to notify the collision detection about an overlapping pair // This method is called by a broad-phase collision detection algorithm void CollisionDetection::broadPhaseNotifyOverlappingPair(Body* body1, Body* body2) { - // Construct the pair of index + // Construct the pair of body index pair indexPair = body1->getID() < body2->getID() ? make_pair(body1->getID(), body2->getID()) : make_pair(body2->getID(), body1->getID()); assert(indexPair.first != indexPair.second); diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 6b195a1e..d163a4cf 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -68,12 +68,12 @@ class CollisionDetection { bool computeNarrowPhase(); // Compute the narrow-phase collision detection public : - CollisionDetection(PhysicsWorld* physicsWorld); // Constructor - ~CollisionDetection(); // Destructor - - OverlappingPair* getOverlappingPair(luint body1ID, luint body2ID); // Return an overlapping pair or null - bool computeCollisionDetection(); // Compute the collision detection - void broadPhaseNotifyOverlappingPair(Body* body1, Body* body2); // Allow the broadphase to notify the collision detection about an overlapping pair + CollisionDetection(PhysicsWorld* physicsWorld); // Constructor + ~CollisionDetection(); // Destructor + + OverlappingPair* getOverlappingPair(luint body1ID, luint body2ID); // Return an overlapping pair or null + bool computeCollisionDetection(); // Compute the collision detection + void broadPhaseNotifyOverlappingPair(Body* body1, Body* body2); // Allow the broadphase to notify the collision detection about an overlapping pair }; // Return an overlapping pair of bodies according to the given bodies ID diff --git a/src/collision/ContactInfo.cpp b/src/collision/ContactInfo.cpp index 6ca70af1..8ff4ddf0 100644 --- a/src/collision/ContactInfo.cpp +++ b/src/collision/ContactInfo.cpp @@ -30,7 +30,7 @@ using namespace reactphysics3d; // Constructor for GJK -ContactInfo::ContactInfo(Body* body1, Body* body2, const Vector3& normal, double penetrationDepth, +ContactInfo::ContactInfo(Body* body1, Body* body2, const Vector3& normal, decimal penetrationDepth, const Vector3& localPoint1, const Vector3& localPoint2, const Transform& transform1, const Transform& transform2) : body1(body1), body2(body2), normal(normal), penetrationDepth(penetrationDepth), diff --git a/src/collision/ContactInfo.h b/src/collision/ContactInfo.h index 12ab2b3d..51e2aecf 100644 --- a/src/collision/ContactInfo.h +++ b/src/collision/ContactInfo.h @@ -27,7 +27,7 @@ #define CONTACT_INFO_H // Libraries -#include "../shapes/BoxShape.h" +#include "../shapes/BoxCollider.h" #include "../mathematics/mathematics.h" // ReactPhysics3D namespace @@ -46,13 +46,13 @@ struct ContactInfo { Body* const body1; // Pointer to the first body of the contact Body* const body2; // Pointer to the second body of the contact const Vector3 normal; // Normal vector the the collision contact in world space - const double penetrationDepth; // Penetration depth of the contact + const decimal penetrationDepth; // Penetration depth of the contact const Vector3 localPoint1; // Contact point of body 1 in local space of body 1 const Vector3 localPoint2; // Contact point of body 2 in local space of body 2 const Vector3 worldPoint1; // Contact point of body 1 in world space const Vector3 worldPoint2; // Contact point of body 2 in world space - ContactInfo(Body* body1, Body* body2, const Vector3& normal, double penetrationDepth, + ContactInfo(Body* body1, Body* body2, const Vector3& normal, decimal penetrationDepth, const Vector3& localPoint1, const Vector3& localPoint2, const Transform& transform1, const Transform& transform2); // Constructor for GJK }; diff --git a/src/collision/PersistentContactCache.cpp b/src/collision/PersistentContactCache.cpp index 51c2efd7..9f6b2401 100644 --- a/src/collision/PersistentContactCache.cpp +++ b/src/collision/PersistentContactCache.cpp @@ -131,7 +131,7 @@ void PersistentContactCache::update(const Transform& transform1, const Transform int PersistentContactCache::getIndexOfDeepestPenetration(Contact* newContact) const { assert(nbContacts == MAX_CONTACTS_IN_CACHE); int indexMaxPenetrationDepth = -1; - double maxPenetrationDepth = newContact->getPenetrationDepth(); + decimal maxPenetrationDepth = newContact->getPenetrationDepth(); // For each contact in the cache for (uint i=0; i& memoryPoolContacts; // Reference to the memory pool with the contacts - int getMaxArea(double area0, double area1, double area2, double area3) const; // Return the index of maximum area - int getIndexOfDeepestPenetration(Contact* newContact) const; // Return the index of the contact with the larger penetration depth - int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const; // Return the index that will be removed - void removeContact(int index); // Remove a contact from the cache - bool isApproxEqual(const Vector3& vector1, const Vector3& vector2) const; // Return true if two vectors are approximatively equal + int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const; // Return the index of maximum area + int getIndexOfDeepestPenetration(Contact* newContact) const; // Return the index of the contact with the larger penetration depth + int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const; // Return the index that will be removed + void removeContact(int index); // Remove a contact from the cache + bool isApproxEqual(const Vector3& vector1, const Vector3& vector2) const; // Return true if two vectors are approximatively equal public: PersistentContactCache(Body* const body1, Body* const body2, MemoryPool& memoryPoolContacts); // Constructor @@ -88,7 +88,7 @@ inline Contact* PersistentContactCache::getContact(uint index) const { // Return true if two vectors are approximatively equal inline bool PersistentContactCache::isApproxEqual(const Vector3& vector1, const Vector3& vector2) const { - const double epsilon = 0.1; + const decimal epsilon = 0.1; return (approxEqual(vector1.getX(), vector2.getX(), epsilon) && approxEqual(vector1.getY(), vector2.getY(), epsilon) && approxEqual(vector1.getZ(), vector2.getZ(), epsilon)); diff --git a/src/collision/broadphase/SweepAndPruneAlgorithm.cpp b/src/collision/broadphase/SweepAndPruneAlgorithm.cpp index 5d96a5d1..21ff4686 100644 --- a/src/collision/broadphase/SweepAndPruneAlgorithm.cpp +++ b/src/collision/broadphase/SweepAndPruneAlgorithm.cpp @@ -79,13 +79,13 @@ void SweepAndPruneAlgorithm::notifyAddedBodies(vector bodies) { // the collision detection object about overlapping pairs using the // broadPhaseNotifyOverlappingPair() method from the CollisionDetection class void SweepAndPruneAlgorithm::computePossibleCollisionPairs() { - double variance[3]; // Variance of the distribution of the AABBs on the three x, y and z axis - double esperance[] = {0.0, 0.0, 0.0}; // Esperance of the distribution of the AABBs on the three x, y and z axis - double esperanceSquare[] = {0.0, 0.0, 0.0}; // Esperance of the square of the distribution values of the AABBs on the three x, y and z axis + decimal variance[3]; // Variance of the distribution of the AABBs on the three x, y and z axis + decimal esperance[] = {0.0, 0.0, 0.0}; // Esperance of the distribution of the AABBs on the three x, y and z axis + decimal esperanceSquare[] = {0.0, 0.0, 0.0}; // Esperance of the square of the distribution values of the AABBs on the three x, y and z axis vector::iterator it; // Iterator on the sortedAABBs set vector::iterator it2; // Second iterator Vector3 center3D; // Center of the current AABB - double center[3]; // Coordinates of the center of the current AABB + decimal center[3]; // Coordinates of the center of the current AABB int i; const Body* body; // Body pointer on the body corresponding to an AABB uint nbAABBs = sortedAABBs.size(); // Number of AABBs diff --git a/src/collision/broadphase/SweepAndPruneAlgorithm.h b/src/collision/broadphase/SweepAndPruneAlgorithm.h index 4e84291f..ce4554ac 100644 --- a/src/collision/broadphase/SweepAndPruneAlgorithm.h +++ b/src/collision/broadphase/SweepAndPruneAlgorithm.h @@ -23,14 +23,13 @@ * * ********************************************************************************/ -#ifndef SAP_ALGORITHM_H -#define SAP_ALGORITHM_H +#ifndef SWEEP_AND_PRUNE_ALGORITHM_H +#define SWEEP_AND_PRUNE_ALGORITHM_H // Libraries #include "BroadPhaseAlgorithm.h" #include "../../shapes/AABB.h" -// TODO : Rename this class SweepAndPruneAlgorithm // Namespace ReactPhysics3D namespace reactphysics3d { diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp index 764837d0..324af996 100644 --- a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp +++ b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp @@ -80,26 +80,26 @@ int EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2, co // 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 Shape* shape1, const Transform& transform1, - const Shape* shape2, const Transform& transform2, +bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, const Collider* collider1, const Transform& transform1, + const Collider* collider2, const Transform& transform2, Vector3& v, ContactInfo*& contactInfo) { Vector3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates Vector3 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates Vector3 points[MAX_SUPPORT_POINTS]; // Current points - TrianglesStore triangleStore; // Store the triangles - TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face candidate of the EPA algorithm + TrianglesStore triangleStore; // Store the triangles + TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face candidate of the EPA algorithm - // Transform a point from body space of shape 2 to body space of shape 1 (the GJK algorithm is done in body space of shape 1) - Transform shape2ToShape1 = transform1.inverse() * transform2; + // Transform a point from local space of body 2 to local space of body 1 (the GJK algorithm is done in local space of body 1) + Transform body2Tobody1 = transform1.inverse() * transform2; - // Matrix that transform a direction from body space of shape 1 into body space of shape 2 - Matrix3x3 rotateToShape2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix(); + // Matrix that transform a direction from local space of body 1 into local space of body 2 + Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix(); // 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(); + decimal tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint(); // Number of triangles in the polytope unsigned int nbTriangles = 0; @@ -132,7 +132,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons int minAxis = d.getAbsoluteVector().getMinAxis(); // Compute sin(60) - const double sin60 = sqrt(3.0) * 0.5; + const decimal sin60 = sqrt(3.0) * 0.5; // Create a rotation quaternion to rotate the vector v1 to get the vectors // v2 and v3 @@ -147,18 +147,18 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons Vector3 v3 = rotationMat * v2; // Compute the support point in the direction of v1 - suppPointsA[2] = shape1->getLocalSupportPoint(v1, OBJECT_MARGIN); - suppPointsB[2] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * (-v1), OBJECT_MARGIN); + suppPointsA[2] = collider1->getLocalSupportPoint(v1, OBJECT_MARGIN); + suppPointsB[2] = body2Tobody1 * collider2->getLocalSupportPoint(rotateToBody2 * (-v1), OBJECT_MARGIN); points[2] = suppPointsA[2] - suppPointsB[2]; // Compute the support point in the direction of v2 - suppPointsA[3] = shape1->getLocalSupportPoint(v2, OBJECT_MARGIN); - suppPointsB[3] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * (-v2), OBJECT_MARGIN); + suppPointsA[3] = collider1->getLocalSupportPoint(v2, OBJECT_MARGIN); + suppPointsB[3] = body2Tobody1 * collider2->getLocalSupportPoint(rotateToBody2 * (-v2), OBJECT_MARGIN); points[3] = suppPointsA[3] - suppPointsB[3]; // Compute the support point in the direction of v3 - suppPointsA[4] = shape1->getLocalSupportPoint(v3, OBJECT_MARGIN); - suppPointsB[4] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * (-v3), OBJECT_MARGIN); + suppPointsA[4] = collider1->getLocalSupportPoint(v3, OBJECT_MARGIN); + suppPointsB[4] = body2Tobody1 * collider2->getLocalSupportPoint(rotateToBody2 * (-v3), OBJECT_MARGIN); points[4] = suppPointsA[4] - suppPointsB[4]; // Now we have an hexahedron (two tetrahedron glued together). We can simply keep the @@ -221,10 +221,10 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons link(EdgeEPA(face2, 1), EdgeEPA(face3, 1)); // Add the triangle faces in the candidate heap - addFaceCandidate(face0, triangleHeap, nbTriangles, DBL_MAX); - addFaceCandidate(face1, triangleHeap, nbTriangles, DBL_MAX); - addFaceCandidate(face2, triangleHeap, nbTriangles, DBL_MAX); - addFaceCandidate(face3, triangleHeap, nbTriangles, DBL_MAX); + addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_MAX); + addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_MAX); + addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_MAX); + addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_MAX); break; } @@ -252,11 +252,11 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons Vector3 n = v1.cross(v2); // Compute the two new vertices to obtain a hexahedron - suppPointsA[3] = shape1->getLocalSupportPoint(n, OBJECT_MARGIN); - suppPointsB[3] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * (-n), OBJECT_MARGIN); + suppPointsA[3] = collider1->getLocalSupportPoint(n, OBJECT_MARGIN); + suppPointsB[3] = body2Tobody1 * collider2->getLocalSupportPoint(rotateToBody2 * (-n), OBJECT_MARGIN); points[3] = suppPointsA[3] - suppPointsB[3]; - suppPointsA[4] = shape1->getLocalSupportPoint(-n, OBJECT_MARGIN); - suppPointsB[4] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * n, OBJECT_MARGIN); + suppPointsA[4] = collider1->getLocalSupportPoint(-n, OBJECT_MARGIN); + suppPointsB[4] = body2Tobody1 * collider2->getLocalSupportPoint(rotateToBody2 * n, OBJECT_MARGIN); points[4] = suppPointsA[4] - suppPointsB[4]; // Construct the triangle faces @@ -287,12 +287,12 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons link(EdgeEPA(face5, 1), EdgeEPA(face3, 2)); // Add the candidate faces in the heap - addFaceCandidate(face0, triangleHeap, nbTriangles, DBL_MAX); - addFaceCandidate(face1, triangleHeap, nbTriangles, DBL_MAX); - addFaceCandidate(face2, triangleHeap, nbTriangles, DBL_MAX); - addFaceCandidate(face3, triangleHeap, nbTriangles, DBL_MAX); - addFaceCandidate(face4, triangleHeap, nbTriangles, DBL_MAX); - addFaceCandidate(face5, triangleHeap, nbTriangles, DBL_MAX); + addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_MAX); + addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_MAX); + addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_MAX); + addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_MAX); + addFaceCandidate(face4, triangleHeap, nbTriangles, DECIMAL_MAX); + addFaceCandidate(face5, triangleHeap, nbTriangles, DECIMAL_MAX); nbVertices = 5; } @@ -307,7 +307,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons } TriangleEPA* triangle = 0; - double upperBoundSquarePenDepth = DBL_MAX; + decimal upperBoundSquarePenDepth = DECIMAL_MAX; do { triangle = triangleHeap[0]; @@ -325,23 +325,23 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons } // Compute the support point of the Minkowski difference (A-B) in the closest point direction - suppPointsA[nbVertices] = shape1->getLocalSupportPoint(triangle->getClosestPoint(), OBJECT_MARGIN); - suppPointsB[nbVertices] = shape2ToShape1 *shape2->getLocalSupportPoint(rotateToShape2 * (-triangle->getClosestPoint()), OBJECT_MARGIN); + suppPointsA[nbVertices] = collider1->getLocalSupportPoint(triangle->getClosestPoint(), OBJECT_MARGIN); + suppPointsB[nbVertices] = body2Tobody1 * collider2->getLocalSupportPoint(rotateToBody2 * (-triangle->getClosestPoint()), OBJECT_MARGIN); points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices]; int indexNewVertex = nbVertices; nbVertices++; // Update the upper bound of the penetration depth - double wDotv = points[indexNewVertex].dot(triangle->getClosestPoint()); + decimal wDotv = points[indexNewVertex].dot(triangle->getClosestPoint()); assert(wDotv > 0.0); - double wDotVSquare = wDotv * wDotv / triangle->getDistSquare(); + decimal wDotVSquare = wDotv * wDotv / triangle->getDistSquare(); if (wDotVSquare < upperBoundSquarePenDepth) { upperBoundSquarePenDepth = wDotVSquare; } // Compute the error - double error = wDotv - triangle->getDistSquare(); + decimal error = wDotv - triangle->getDistSquare(); if (error <= std::max(tolerance, REL_ERROR_SQUARE * wDotv) || points[indexNewVertex] == points[(*triangle)[0]] || points[indexNewVertex] == points[(*triangle)[1]] || @@ -370,11 +370,11 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons // Compute the contact info v = transform1.getOrientation().getMatrix() * triangle->getClosestPoint(); Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA); - Vector3 pBLocal = shape2ToShape1.inverse() * triangle->computeClosestPointOfObject(suppPointsB); + Vector3 pBLocal = body2Tobody1.inverse() * triangle->computeClosestPointOfObject(suppPointsB); Vector3 normal = v.getUnit(); - double penetrationDepth = v.length(); + decimal penetrationDepth = v.length(); assert(penetrationDepth > 0.0); - contactInfo = new ContactInfo(shape1->getBodyPointer(), shape2->getBodyPointer(), normal, + contactInfo = new ContactInfo(collider1->getBodyPointer(), collider2->getBodyPointer(), normal, penetrationDepth, pALocal, pBLocal, transform1, transform2); return true; diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.h b/src/collision/narrowphase/EPA/EPAAlgorithm.h index a01555c4..90bb07f0 100644 --- a/src/collision/narrowphase/EPA/EPAAlgorithm.h +++ b/src/collision/narrowphase/EPA/EPAAlgorithm.h @@ -28,7 +28,7 @@ // Libraries #include "../GJK/Simplex.h" -#include "../../../shapes/Shape.h" +#include "../../../shapes/Collider.h" #include "../../ContactInfo.h" #include "../../../mathematics/mathematics.h" #include "TriangleEPA.h" @@ -75,7 +75,7 @@ class EPAAlgorithm { TriangleComparison triangleComparison; // Triangle comparison operator void addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap, - uint& nbTriangles, double upperBoundSquarePenDepth); // Add a triangle face in the candidate triangle heap + uint& nbTriangles, decimal upperBoundSquarePenDepth); // Add a triangle face in the candidate triangle heap int isOriginInTetrahedron(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& p4) const; // Decide if the origin is in the tetrahedron @@ -83,14 +83,14 @@ class EPAAlgorithm { EPAAlgorithm(); // Constructor ~EPAAlgorithm(); // Destructor - bool computePenetrationDepthAndContactPoints(Simplex simplex, const Shape* shape1, const Transform& transform1, - const Shape* shape2, const Transform& transform2, + bool computePenetrationDepthAndContactPoints(Simplex simplex, const Collider* collider1, const Transform& transform1, + const Collider* collider2, const Transform& transform2, Vector3& v, ContactInfo*& contactInfo); // Compute the penetration depth with EPA algorithm }; // Add a triangle face in the candidate triangle heap in the EPA algorithm inline void EPAAlgorithm::addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap, - uint& nbTriangles, double upperBoundSquarePenDepth) { + uint& nbTriangles, decimal upperBoundSquarePenDepth) { // If the closest point of the affine hull of triangle points is internal to the triangle and // if the distance of the closest point from the origin is at most the penetration depth upper bound diff --git a/src/collision/narrowphase/EPA/TriangleEPA.cpp b/src/collision/narrowphase/EPA/TriangleEPA.cpp index 8cb26b2c..6063322c 100644 --- a/src/collision/narrowphase/EPA/TriangleEPA.cpp +++ b/src/collision/narrowphase/EPA/TriangleEPA.cpp @@ -56,11 +56,11 @@ bool TriangleEPA::computeClosestPoint(const Vector3* vertices) { Vector3 v1 = vertices[indicesVertices[1]] - p0; Vector3 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); + decimal v1Dotv1 = v1.dot(v1); + decimal v1Dotv2 = v1.dot(v2); + decimal v2Dotv2 = v2.dot(v2); + decimal p0Dotv1 = p0.dot(v1); + decimal p0Dotv2 = p0.dot(v2); // Compute determinant det = v1Dotv1 * v2Dotv2 - v1Dotv2 * v1Dotv2; @@ -126,7 +126,7 @@ bool TriangleEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex // Mark the current triangle as obsolete because it setIsObsolete(true); - // Execute recursively the silhouette algorithm for the ajdacent edges of neighbouring + // Execute recursively the silhouette algorithm for the adjacent edges of neighboring // triangles of the current triangle bool result = adjacentEdges[0].computeSilhouette(vertices, indexNewVertex, triangleStore) && adjacentEdges[1].computeSilhouette(vertices, indexNewVertex, triangleStore) && diff --git a/src/collision/narrowphase/EPA/TriangleEPA.h b/src/collision/narrowphase/EPA/TriangleEPA.h index 700222bb..4acf9c40 100644 --- a/src/collision/narrowphase/EPA/TriangleEPA.h +++ b/src/collision/narrowphase/EPA/TriangleEPA.h @@ -28,7 +28,7 @@ // Libraries #include "../../../mathematics/mathematics.h" -#include "../../../constants.h" +#include "../../../configuration.h" #include "EdgeEPA.h" #include @@ -51,11 +51,11 @@ class TriangleEPA { 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 - Vector3 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 + decimal det; // Determinant + Vector3 closestPoint; // Point v closest to the origin on the affine hull of the triangle + decimal lambda1; // Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2 + decimal lambda2; // Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2 + decimal distSquare; // Square distance of the point closest point v to the origin public: TriangleEPA(); // Constructor @@ -64,7 +64,7 @@ class TriangleEPA { EdgeEPA& getAdjacentEdge(int index); // 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 + decimal 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 Vector3& getClosestPoint() const; // Return the point closest to the origin @@ -93,7 +93,7 @@ inline void TriangleEPA::setAdjacentEdge(int index, EdgeEPA& edge) { } // Return the square distance of the closest point to origin -inline double TriangleEPA::getDistSquare() const { +inline decimal TriangleEPA::getDistSquare() const { return distSquare; } diff --git a/src/collision/narrowphase/EPA/TrianglesStore.h b/src/collision/narrowphase/EPA/TrianglesStore.h index 691a238d..de4ed8e1 100644 --- a/src/collision/narrowphase/EPA/TrianglesStore.h +++ b/src/collision/narrowphase/EPA/TrianglesStore.h @@ -89,7 +89,7 @@ inline TriangleEPA& TrianglesStore::last() { inline TriangleEPA* TrianglesStore::newTriangle(const Vector3* vertices, uint v0, uint v1, uint v2) { TriangleEPA* newTriangle = 0; - // If we have not reach the maximum number of triangles + // If we have not reached the maximum number of triangles if (nbTriangles != MAX_TRIANGLES) { newTriangle = &triangles[nbTriangles++]; new (newTriangle) TriangleEPA(v0, v1, v2); diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index a0a364c1..8895b254 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -27,7 +27,7 @@ #include "GJKAlgorithm.h" #include "Simplex.h" #include "../../../constraint/Contact.h" -#include "../../../constants.h" +#include "../../../configuration.h" #include #include #include @@ -58,31 +58,31 @@ GJKAlgorithm::~GJKAlgorithm() { // 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 Shape* shape1, const Transform& transform1, - const Shape* shape2, const Transform& transform2, +bool GJKAlgorithm::testCollision(const Collider* collider1, const Transform& transform1, + const Collider* collider2, const Transform& transform2, ContactInfo*& contactInfo) { - assert(shape1 != shape2); + assert(collider1 != collider2); Vector3 suppA; // Support point of object A Vector3 suppB; // Support point of object B Vector3 w; // Support point of Minkowski difference A-B Vector3 pA; // Closest point of object A Vector3 pB; // Closest point of object B - double vDotw; - double prevDistSquare; - Body* const body1 = shape1->getBodyPointer(); - Body* const body2 = shape2->getBodyPointer(); + decimal vDotw; + decimal prevDistSquare; + Body* const body1 = collider1->getBodyPointer(); + Body* const body2 = collider2->getBodyPointer(); - // Transform a point from body space of shape 2 to body space of shape 1 (the GJK algorithm is done in body space of shape 1) - Transform shape2ToShape1 = transform1.inverse() * transform2; + // Transform a point from local space of body 2 to local space of body 1 (the GJK algorithm is done in local space of body 1) + Transform body2Tobody1 = transform1.inverse() * transform2; - // Matrix that transform a direction from body space of shape 1 into body space of shape 2 - Matrix3x3 rotateToShape2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix(); + // Matrix that transform a direction from local space of body 1 into local space of body 2 + Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix(); // Initialize the margin (sum of margins of both objects) - double margin = 2 * OBJECT_MARGIN; - double marginSquare = margin * margin; + decimal margin = 2 * OBJECT_MARGIN; + decimal marginSquare = margin * margin; assert(margin > 0.0); // Create a simplex set @@ -93,12 +93,12 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform Vector3 v = (overlappingPair) ? overlappingPair->getCachedSeparatingAxis() : Vector3(1.0, 1.0, 1.0); // Initialize the upper bound for the square distance - double distSquare = DBL_MAX; + decimal distSquare = DECIMAL_MAX; do { // Compute the support points for original objects (without margins) A and B - suppA = shape1->getLocalSupportPoint(-v); - suppB = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * v); + suppA = collider1->getLocalSupportPoint(-v); + suppB = body2Tobody1 * collider2->getLocalSupportPoint(rotateToBody2 * v); // Compute the support point for the Minkowski difference A-B w = suppA - suppB; @@ -118,14 +118,14 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform // Project those two points on the margins to have the closest points of both // object with the margins - double dist = sqrt(distSquare); + decimal dist = sqrt(distSquare); assert(dist > 0.0); pA = (pA - (OBJECT_MARGIN / dist) * v); - pB = shape2ToShape1.inverse() * (pB + (OBJECT_MARGIN / dist) * v); + pB = body2Tobody1.inverse() * (pB + (OBJECT_MARGIN / dist) * v); // Compute the contact info Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit()); - double penetrationDepth = margin - dist; + decimal penetrationDepth = margin - dist; // Reject the contact if the penetration depth is negative (due too numerical errors) if (penetrationDepth <= 0.0) return false; @@ -146,14 +146,14 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform // Project those two points on the margins to have the closest points of both // object with the margins - double dist = sqrt(distSquare); + decimal dist = sqrt(distSquare); assert(dist > 0.0); pA = (pA - (OBJECT_MARGIN / dist) * v); - pB = shape2ToShape1.inverse() * (pB + (OBJECT_MARGIN / dist) * v); + pB = body2Tobody1.inverse() * (pB + (OBJECT_MARGIN / dist) * v); // Compute the contact info Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit()); - double penetrationDepth = margin - dist; + decimal penetrationDepth = margin - dist; // Reject the contact if the penetration depth is negative (due too numerical errors) if (penetrationDepth <= 0.0) return false; @@ -172,14 +172,14 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform // Project those two points on the margins to have the closest points of both // object with the margins - double dist = sqrt(distSquare); + decimal dist = sqrt(distSquare); assert(dist > 0.0); pA = (pA - (OBJECT_MARGIN / dist) * v); - pB = shape2ToShape1.inverse() * (pB + (OBJECT_MARGIN / dist) * v); + pB = body2Tobody1.inverse() * (pB + (OBJECT_MARGIN / dist) * v); // Compute the contact info Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit()); - double penetrationDepth = margin - dist; + decimal penetrationDepth = margin - dist; // Reject the contact if the penetration depth is negative (due too numerical errors) if (penetrationDepth <= 0.0) return false; @@ -206,14 +206,14 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform // Project those two points on the margins to have the closest points of both // object with the margins - double dist = sqrt(distSquare); + decimal dist = sqrt(distSquare); assert(dist > 0.0); pA = (pA - (OBJECT_MARGIN / dist) * v); - pB = shape2ToShape1.inverse() * (pB + (OBJECT_MARGIN / dist) * v); + pB = body2Tobody1.inverse() * (pB + (OBJECT_MARGIN / dist) * v); // Compute the contact info Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit()); - double penetrationDepth = margin - dist; + decimal penetrationDepth = margin - dist; // Reject the contact if the penetration depth is negative (due too numerical errors) if (penetrationDepth <= 0.0) return false; @@ -229,7 +229,7 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform // 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(shape1, transform1, shape2, transform2, contactInfo, v); + return computePenetrationDepthForEnlargedObjects(collider1, transform1, collider2, transform2, contactInfo, v); } // This method runs the GJK algorithm on the two enlarged objects (with margin) @@ -237,27 +237,27 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform // 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 Shape* const shape1, const Transform& transform1, - const Shape* const shape2, const Transform& transform2, +bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const Collider* const collider1, const Transform& transform1, + const Collider* const collider2, const Transform& transform2, ContactInfo*& contactInfo, Vector3& v) { Simplex simplex; Vector3 suppA; Vector3 suppB; Vector3 w; - double vDotw; - double distSquare = DBL_MAX; - double prevDistSquare; + decimal vDotw; + decimal distSquare = DECIMAL_MAX; + decimal prevDistSquare; - // Transform a point from body space of shape 2 to body space of shape 1 (the GJK algorithm is done in body space of shape 1) - Transform shape2ToShape1 = transform1.inverse() * transform2; + // Transform a point from local space of body 2 to local space of body 1 (the GJK algorithm is done in local space of body 1) + Transform body2ToBody1 = transform1.inverse() * transform2; - // Matrix that transform a direction from body space of shape 1 into body space of shape 2 - Matrix3x3 rotateToShape2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix(); + // Matrix that transform a direction from local space of body 1 into local space of body 2 + Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix(); do { // Compute the support points for the enlarged object A and B - suppA = shape1->getLocalSupportPoint(-v, OBJECT_MARGIN); - suppB = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * v, OBJECT_MARGIN); + suppA = collider1->getLocalSupportPoint(-v, OBJECT_MARGIN); + suppB = body2ToBody1 * collider2->getLocalSupportPoint(rotateToBody2 * v, OBJECT_MARGIN); // Compute the support point for the Minkowski difference A-B w = suppA - suppB; @@ -293,5 +293,5 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const Shape* const // 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, shape1, transform1, shape2, transform2, v, contactInfo); + return algoEPA.computePenetrationDepthAndContactPoints(simplex, collider1, transform1, collider2, transform2, v, contactInfo); } diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h index 2c95e2ca..098cb173 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.h +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h @@ -29,7 +29,7 @@ // Libraries #include "../NarrowPhaseAlgorithm.h" #include "../../ContactInfo.h" -#include "../../../shapes/Shape.h" +#include "../../../shapes/Collider.h" #include "../EPA/EPAAlgorithm.h" @@ -37,8 +37,8 @@ namespace reactphysics3d { // Constants -const double REL_ERROR = 1.0e-3; -const double REL_ERROR_SQUARE = REL_ERROR * REL_ERROR; +const decimal REL_ERROR = 1.0e-3; +const decimal REL_ERROR_SQUARE = REL_ERROR * REL_ERROR; /* ------------------------------------------------------------------- Class GJKAlgorithm : @@ -61,16 +61,16 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm { private : EPAAlgorithm algoEPA; // EPA Algorithm - bool computePenetrationDepthForEnlargedObjects(const Shape* shape1, const Transform& transform1, - const Shape* shape2, const Transform& transform2, + bool computePenetrationDepthForEnlargedObjects(const Collider* collider1, const Transform& transform1, + const Collider* collider2, const Transform& transform2, ContactInfo*& contactInfo, Vector3& v); // Compute the penetration depth for enlarged objects public : GJKAlgorithm(CollisionDetection& collisionDetection); // Constructor ~GJKAlgorithm(); // Destructor - virtual bool testCollision(const Shape* shape1, const Transform& transform1, - const Shape* shape2, const Transform& transform2, + virtual bool testCollision(const Collider* collider1, const Transform& transform1, + const Collider* collider2, const Transform& transform2, ContactInfo*& contactInfo); // Return true and compute a contact info if the two bounding volume collide }; diff --git a/src/collision/narrowphase/GJK/Simplex.cpp b/src/collision/narrowphase/GJK/Simplex.cpp index 07b94369..e8a46fee 100644 --- a/src/collision/narrowphase/GJK/Simplex.cpp +++ b/src/collision/narrowphase/GJK/Simplex.cpp @@ -222,7 +222,7 @@ bool Simplex::isProperSubset(Bits subset) const { // Return true if the set is affinely dependent meaning that a point of the set // is an affine combination of other points in the set bool Simplex::isAffinelyDependent() const { - double sum = 0.0; + decimal sum = 0.0; int i; Bits bit; @@ -273,7 +273,7 @@ bool Simplex::isValidSubset(Bits subset) const { // 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(Vector3& pA, Vector3& pB) const { - double deltaX = 0.0; + decimal deltaX = 0.0; pA.setAllValues(0.0, 0.0, 0.0); pB.setAllValues(0.0, 0.0, 0.0); int i; @@ -290,7 +290,7 @@ void Simplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const { } assert(deltaX > 0.0); - double factor = 1.0 / deltaX; + decimal factor = 1.0 / deltaX; pA *= factor; pB *= factor; } @@ -327,13 +327,13 @@ bool Simplex::computeClosestPoint(Vector3& v) { // Backup the closest point void Simplex::backupClosestPointInSimplex(Vector3& v) { - double minDistSquare = DBL_MAX; + decimal minDistSquare = DECIMAL_MAX; Bits bit; for (bit = allBits; bit != 0x0; bit--) { if (isSubset(bit, allBits) && isProperSubset(bit)) { Vector3 u = computeClosestPointForSubset(bit); - double distSquare = u.dot(u); + decimal distSquare = u.dot(u); if (distSquare < minDistSquare) { minDistSquare = distSquare; bitsCurrentSimplex = bit; @@ -348,7 +348,7 @@ void Simplex::backupClosestPointInSimplex(Vector3& v) { Vector3 Simplex::computeClosestPointForSubset(Bits subset) { Vector3 v(0.0, 0.0, 0.0); // Closet point v = sum(lambda_i * points[i]) maxLengthSquare = 0.0; - double deltaX = 0.0; // deltaX = sum of all det[subset][i] + decimal deltaX = 0.0; // deltaX = sum of all det[subset][i] int i; Bits bit; diff --git a/src/collision/narrowphase/GJK/Simplex.h b/src/collision/narrowphase/GJK/Simplex.h index ba50e3fb..e69ba152 100644 --- a/src/collision/narrowphase/GJK/Simplex.h +++ b/src/collision/narrowphase/GJK/Simplex.h @@ -49,13 +49,13 @@ typedef unsigned int Bits; class Simplex { private: Vector3 points[4]; // Current points - double pointsLengthSquare[4]; // pointsLengthSquare[i] = (points[i].length)^2 - double maxLengthSquare; // Maximum length of pointsLengthSquare[i] + decimal pointsLengthSquare[4]; // pointsLengthSquare[i] = (points[i].length)^2 + decimal maxLengthSquare; // Maximum length of pointsLengthSquare[i] Vector3 suppPointsA[4]; // Support points of object A in local coordinates Vector3 suppPointsB[4]; // Support points of object B in local coordinates Vector3 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 + decimal det[16][4]; // Cached determinant values + decimal 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 @@ -77,7 +77,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(Vector3* suppPointsA, Vector3* suppPointsB, Vector3* points) const; // Return the points of the simplex - double getMaxLengthSquareOfAPoint() const; // Return the maximum squared length of a point + decimal getMaxLengthSquareOfAPoint() const; // Return the maximum squared length of a point void addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB); // Addd a point to the simplex bool isPointInSimplex(const Vector3& point) const; // Return true if the point is in the simplex bool isAffinelyDependent() const; // Return true if the set is affinely dependent @@ -107,7 +107,7 @@ inline bool Simplex::isEmpty() const { } // Return the maximum squared length of a point -inline double Simplex::getMaxLengthSquareOfAPoint() const { +inline decimal Simplex::getMaxLengthSquareOfAPoint() const { return maxLengthSquare; } diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h index b95e46c2..351258bc 100644 --- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h +++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h @@ -50,8 +50,8 @@ class NarrowPhaseAlgorithm { NarrowPhaseAlgorithm(CollisionDetection& collisionDetection); // Constructor virtual ~NarrowPhaseAlgorithm(); // Destructor - virtual bool testCollision(const Shape* shape1, const Transform& transform1, - const Shape* shape2, const Transform& transform2, + virtual bool testCollision(const Collider* collider1, const Transform& transform1, + const Collider* collider2, const Transform& transform2, ContactInfo*& contactInfo)=0; // Return true and compute a contact info if the two bounding volume collide }; diff --git a/src/configuration.h b/src/configuration.h index 76c312b7..7f0f0cf1 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -26,6 +26,11 @@ #ifndef CONFIGURATION_H #define CONFIGURATION_H +// Libraries +#include +#include +#include "decimal.h" + // Windows platform #if defined(WIN32) || defined(_WIN32) || defined(_WIN64) || defined(__WIN32__) || defined(__WINDOWS__) #define WINDOWS_OS @@ -35,4 +40,38 @@ #define LINUX_OS #endif +// Type definitions +typedef unsigned int uint; +typedef long unsigned int luint; + +// Mathematical constants +const reactphysics3d::decimal DECIMAL_MIN = std::numeric_limits::min(); // Minimun decimal value +const reactphysics3d::decimal DECIMAL_MAX = std::numeric_limits::max(); // Maximum decimal value +const reactphysics3d::decimal MACHINE_EPSILON = std::numeric_limits::epsilon(); // Machine epsilon +const reactphysics3d::decimal DECIMAL_INFINITY = std::numeric_limits::infinity(); // Infinity +const reactphysics3d::decimal PI = 3.14159265; // Pi constant + +// Physics Engine constants +const reactphysics3d::decimal DEFAULT_TIMESTEP = 1.0 / 60.0; // Default internal constant timestep in seconds + +// GJK Algorithm parameters +const reactphysics3d::decimal OBJECT_MARGIN = 0.04; // Object margin for collision detection in cm + +// Contact constants +const reactphysics3d::decimal FRICTION_COEFFICIENT = 0.4; // Friction coefficient +const reactphysics3d::decimal PERSISTENT_CONTACT_DIST_THRESHOLD = 0.02; // Distance threshold for two contact points for a valid persistent contact + +// Constraint solver constants +const int NB_MAX_BODIES = 100000; // Maximum number of bodies +const int NB_MAX_CONTACTS = 100000; // Maximum number of contacts (for memory pool allocation) +const int NB_MAX_CONSTRAINTS = 100000; // Maximum number of constraints +const int NB_MAX_COLLISION_PAIRS = 10000; // Maximum number of collision pairs of bodies (for memory pool allocation) + +// Constraint solver constants +const uint DEFAULT_LCP_ITERATIONS = 15; // Number of iterations when solving a LCP problem +const uint DEFAULT_LCP_ITERATIONS_ERROR_CORRECTION = 5; // Number of iterations when solving a LCP problem for error correction +const bool ERROR_CORRECTION_PROJECTION_ENABLED = true; // True if the error correction projection (first order world) is active in the constraint solver +const reactphysics3d::decimal PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION = 0.20; // Contacts with penetration depth (in meters) larger that this use error correction with projection + + #endif diff --git a/src/constants.h b/src/constants.h deleted file mode 100644 index 00291a15..00000000 --- a/src/constants.h +++ /dev/null @@ -1,72 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * -* Copyright (c) 2010-2012 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef CONSTANTS_H -#define CONSTANTS_H - -// Libraries -#include -#include - -// Type definitions -typedef unsigned int uint; -typedef long unsigned int luint; - -// Mathematical constants -const double EPSILON = 1.0e-10; // Epsilon value -const double MACHINE_EPSILON = DBL_EPSILON; // Machine epsilon -const double EPSILON_TEST = 0.00001; // TODO : Try not to use this value -const double ONE_MINUS_EPSILON_TEST = 0.99999; // TODO : Try not to use this value -const double INFINITY_CONST = std::numeric_limits::infinity(); // Infinity constant -const double PI = 3.14159265; // Pi constant - -// Physics Engine constants -const double DEFAULT_TIMESTEP = 1.0 / 60.0; // Default internal constant timestep in seconds - -// GJK Algorithm parameters -const double OBJECT_MARGIN = 0.04; // Object margin for collision detection in cm - -// Contact constants -const double FRICTION_COEFFICIENT = 0.4; // Friction coefficient -const double DEFAULT_PENETRATION_FACTOR = 5.0; // Penetration factor (between 0 and 1) which specify the importance of the - // penetration depth in order to calculate the correct impulse for the contact -const double PERSISTENT_CONTACT_DIST_THRESHOLD = 0.02; // Distance threshold for two contact points for a valid persistent contact - -const int NB_MAX_BODIES = 100000; // Maximum number of bodies -const int NB_MAX_CONTACTS = 100000; // Maximum number of contacts (for memory pool allocation) -const int NB_MAX_CONSTRAINTS = 100000; // Maximum number of constraints -const int NB_MAX_COLLISION_PAIRS = 10000; // Maximum number of collision pairs of bodies (for memory pool allocation) - -// Constraint solver constants -const uint DEFAULT_LCP_ITERATIONS = 15; // Number of iterations when solving a LCP problem -const double AV_COUNTER_LIMIT = 500; // Maximum number value of the avBodiesCounter or avConstraintsCounter -const double AV_PERCENT_TO_FREE = 0.5; // We will free the memory if the current nb of bodies (or constraints) is - // less than AV_PERCENT_TO_FREE * bodiesCapacity (or constraintsCapacity). This - // is used to avoid to keep to much memory for a long time if the system doesn't - // need that memory. This value is between 0.0 and 1.0 - - -#endif - diff --git a/src/constraint/Constraint.cpp b/src/constraint/Constraint.cpp index 958f8124..a7c90da1 100644 --- a/src/constraint/Constraint.cpp +++ b/src/constraint/Constraint.cpp @@ -30,8 +30,8 @@ using namespace reactphysics3d; // Constructor -Constraint::Constraint(Body* const body1, Body* const body2, uint nbConstraints, bool active) - :body1(body1), body2(body2), active(active), nbConstraints(nbConstraints) { +Constraint::Constraint(Body* const body1, Body* const body2, uint nbConstraints, bool active, ConstraintType type) + :body1(body1), body2(body2), active(active), nbConstraints(nbConstraints), type(type) { // Initialize the cached lambda values for (int i=0; i cachedLambdas; // Cached lambda values of each mathematical constraint for more precise initializaton of LCP solver + const ConstraintType type; // Type of the constraint + std::vector cachedLambdas; // Cached lambda values of each mathematical constraint for more precise initializaton of LCP solver public : - Constraint(Body* const body1, Body* const body2, uint nbConstraints, bool active); // Constructor // Constructor - virtual ~Constraint(); // Destructor - Body* const getBody1() const; // Return the reference to the body 1 - Body* const getBody2() const; // Return the reference to the body 2 // Evaluate the constraint - bool isActive() const; // Return true if the constraint is active // Return the jacobian matrix of body 2 - virtual void computeJacobian(int noConstraint, double J_sp[NB_MAX_CONSTRAINTS][2*6]) const=0; // Compute the jacobian matrix for all mathematical constraints - virtual void computeLowerBound(int noConstraint, double lowerBounds[NB_MAX_CONSTRAINTS]) const=0; // Compute the lowerbounds values for all the mathematical constraints - virtual void computeUpperBound(int noConstraint, double upperBounds[NB_MAX_CONSTRAINTS]) const=0; // Compute the upperbounds values for all the mathematical constraints - virtual void computeErrorValue(int noConstraint, double errorValues[], double penetrationFactor) const=0; // Compute the error values for all the mathematical constraints + Constraint(Body* const body1, Body* const body2, uint nbConstraints, + bool active, ConstraintType type); // Constructor // Constructor + virtual ~Constraint(); // Destructor + Body* const getBody1() const; // Return the reference to the body 1 + Body* const getBody2() const; // Return the reference to the body 2 // Evaluate the constraint + bool isActive() const; // Return true if the constraint is active // Return the jacobian matrix of body 2 + ConstraintType getType() const; // Return the type of the constraint + virtual void computeJacobian(int noConstraint, decimal J_sp[NB_MAX_CONSTRAINTS][2*6]) const=0; // Compute the jacobian matrix for all mathematical constraints + virtual void computeLowerBound(int noConstraint, decimal lowerBounds[NB_MAX_CONSTRAINTS]) const=0; // Compute the lowerbounds values for all the mathematical constraints + virtual void computeUpperBound(int noConstraint, decimal upperBounds[NB_MAX_CONSTRAINTS]) const=0; // Compute the upperbounds values for all the mathematical constraints + virtual void computeErrorValue(int noConstraint, decimal errorValues[]) const=0; // Compute the error values for all the mathematical constraints unsigned int getNbConstraints() const; // Return the number of mathematical constraints // Return the number of auxiliary constraints - double getCachedLambda(int index) const; // Get one cached lambda value - void setCachedLambda(int index, double lambda); // Set on cached lambda value + decimal getCachedLambda(int index) const; // Get one cached lambda value + void setCachedLambda(int index, decimal lambda); // Set on cached lambda value }; // Return the reference to the body 1 @@ -79,19 +85,25 @@ inline bool Constraint::isActive() const { return active; } +// Return the type of the constraint +inline ConstraintType Constraint::getType() const { + return type; +} + + // Return the number auxiliary constraints inline uint Constraint::getNbConstraints() const { return nbConstraints; } // Get one previous lambda value -inline double Constraint::getCachedLambda(int index) const { +inline decimal Constraint::getCachedLambda(int index) const { assert(index >= 0 && index < nbConstraints); return cachedLambdas[index]; } // Set on cached lambda value -inline void Constraint::setCachedLambda(int index, double lambda) { +inline void Constraint::setCachedLambda(int index, decimal lambda) { assert(index >= 0 && index < nbConstraints); cachedLambdas[index] = lambda; } diff --git a/src/constraint/Contact.cpp b/src/constraint/Contact.cpp index dfa5c8a6..c0126189 100644 --- a/src/constraint/Contact.cpp +++ b/src/constraint/Contact.cpp @@ -31,7 +31,7 @@ using namespace std; // Constructor Contact::Contact(const ContactInfo* contactInfo) - : Constraint(contactInfo->body1, contactInfo->body2, 3, true), normal(contactInfo->normal), penetrationDepth(contactInfo->penetrationDepth), + : Constraint(contactInfo->body1, contactInfo->body2, 3, true, CONTACT), normal(contactInfo->normal), penetrationDepth(contactInfo->penetrationDepth), localPointOnBody1(contactInfo->localPoint1), localPointOnBody2(contactInfo->localPoint2), worldPointOnBody1(contactInfo->worldPoint1), worldPointOnBody2(contactInfo->worldPoint2) { assert(penetrationDepth > 0.0); @@ -55,7 +55,7 @@ Contact::~Contact() { // fill in this matrix with all the jacobian matrix of the mathematical constraint // of the contact. The argument "noConstraint", is the row were the method have // to start to fill in the J_sp matrix. -void Contact::computeJacobian(int noConstraint, double J_sp[NB_MAX_CONSTRAINTS][2*6]) const { +void Contact::computeJacobian(int noConstraint, decimal J_sp[NB_MAX_CONSTRAINTS][2*6]) const { assert(body1); assert(body2); @@ -128,7 +128,7 @@ void Contact::computeJacobian(int noConstraint, double J_sp[NB_MAX_CONSTRAINTS][ // Compute the lowerbounds values for all the mathematical constraints. The // argument "lowerBounds" is the lowerbounds values vector of the constraint solver and // this methods has to fill in this vector starting from the row "noConstraint" -void Contact::computeLowerBound(int noConstraint, double lowerBounds[NB_MAX_CONSTRAINTS]) const { +void Contact::computeLowerBound(int noConstraint, decimal lowerBounds[NB_MAX_CONSTRAINTS]) const { assert(noConstraint >= 0 && noConstraint + nbConstraints <= NB_MAX_CONSTRAINTS); lowerBounds[noConstraint] = 0.0; // Lower bound for the contact constraint @@ -139,10 +139,10 @@ void Contact::computeLowerBound(int noConstraint, double lowerBounds[NB_MAX_CONS // Compute the upperbounds values for all the mathematical constraints. The // argument "upperBounds" is the upperbounds values vector of the constraint solver and // this methods has to fill in this vector starting from the row "noConstraint" -void Contact::computeUpperBound(int noConstraint, double upperBounds[NB_MAX_CONSTRAINTS]) const { +void Contact::computeUpperBound(int noConstraint, decimal upperBounds[NB_MAX_CONSTRAINTS]) const { assert(noConstraint >= 0 && noConstraint + nbConstraints <= NB_MAX_CONSTRAINTS); - upperBounds[noConstraint] = INFINITY_CONST; // Upper bound for the contact constraint + upperBounds[noConstraint] = DECIMAL_INFINITY; // Upper bound for the contact constraint upperBounds[noConstraint + 1] = mu_mc_g; // Upper bound for the first friction constraint upperBounds[noConstraint + 2] = mu_mc_g; // Upper bound for the second friction constraint } @@ -150,11 +150,11 @@ void Contact::computeUpperBound(int noConstraint, double upperBounds[NB_MAX_CONS // Compute the error values for all the mathematical constraints. The argument // "errorValues" is the error values vector of the constraint solver and this // method has to fill in this vector starting from the row "noConstraint" -void Contact::computeErrorValue(int noConstraint, double errorValues[], double penetrationFactor) const { +void Contact::computeErrorValue(int noConstraint, decimal errorValues[]) const { assert(body1); assert(body2); - // TODO : Do we need this casting anymore + // TODO : Do we need this casting anymore ? RigidBody* rigidBody1 = dynamic_cast(body1); RigidBody* rigidBody2 = dynamic_cast(body2); @@ -163,9 +163,9 @@ void Contact::computeErrorValue(int noConstraint, double errorValues[], double p // Compute the error value for the contact constraint Vector3 velocity1 = rigidBody1->getLinearVelocity(); Vector3 velocity2 = rigidBody2->getLinearVelocity(); - double restitutionCoeff = rigidBody1->getRestitution() * rigidBody2->getRestitution(); - double errorValue = restitutionCoeff * (normal.dot(velocity1) - normal.dot(velocity2)) + penetrationFactor * penetrationDepth; - + decimal restitutionCoeff = rigidBody1->getRestitution() * rigidBody2->getRestitution(); + decimal errorValue = restitutionCoeff * (normal.dot(velocity1) - normal.dot(velocity2)); + // Assign the error value to the vector of error values errorValues[noConstraint] = errorValue; // Error value for contact constraint errorValues[noConstraint + 1] = 0.0; // Error value for friction constraint diff --git a/src/constraint/Contact.h b/src/constraint/Contact.h index 470aaa1d..0208ec90 100644 --- a/src/constraint/Contact.h +++ b/src/constraint/Contact.h @@ -30,11 +30,10 @@ #include "Constraint.h" #include "../collision/ContactInfo.h" #include "../body/RigidBody.h" -#include "../constants.h" +#include "../configuration.h" #include "../mathematics/mathematics.h" #include "../memory/MemoryPool.h" #include "../configuration.h" -#include #if defined(VISUAL_DEBUG) #if defined(APPLE_OS) @@ -65,13 +64,13 @@ namespace reactphysics3d { class Contact : public Constraint { protected : const Vector3 normal; // Normal vector of the contact (From body1 toward body2) in world space - double penetrationDepth; // Penetration depth + decimal penetrationDepth; // Penetration depth const Vector3 localPointOnBody1; // Contact point on body 1 in local space of body 1 const Vector3 localPointOnBody2; // Contact point on body 2 in local space of body 2 Vector3 worldPointOnBody1; // Contact point on body 1 in world space Vector3 worldPointOnBody2; // Contact point on body 2 in world space std::vector frictionVectors; // Two orthogonal vectors that span the tangential friction plane - double mu_mc_g; + decimal mu_mc_g; void computeFrictionVectors(); // Compute the two friction vectors that span the tangential friction plane @@ -80,18 +79,18 @@ class Contact : public Constraint { virtual ~Contact(); // Destructor Vector3 getNormal() const; // Return the normal vector of the contact - void setPenetrationDepth(double penetrationDepth); // Set the penetration depth of the contact + void setPenetrationDepth(decimal penetrationDepth); // Set the penetration depth of the contact Vector3 getLocalPointOnBody1() const; // Return the contact local point on body 1 Vector3 getLocalPointOnBody2() const; // Return the contact local point on body 2 Vector3 getWorldPointOnBody1() const; // Return the contact world point on body 1 Vector3 getWorldPointOnBody2() const; // Return the contact world point on body 2 void setWorldPointOnBody1(const Vector3& worldPoint); // Set the contact world point on body 1 void setWorldPointOnBody2(const Vector3& worldPoint); // Set the contact world point on body 2 - virtual void computeJacobian(int noConstraint, double J_SP[NB_MAX_CONSTRAINTS][2*6]) const; // Compute the jacobian matrix for all mathematical constraints - virtual void computeLowerBound(int noConstraint, double lowerBounds[NB_MAX_CONSTRAINTS]) const; // Compute the lowerbounds values for all the mathematical constraints - virtual void computeUpperBound(int noConstraint, double upperBounds[NB_MAX_CONSTRAINTS]) const; // Compute the upperbounds values for all the mathematical constraints - virtual void computeErrorValue(int noConstraint, double errorValues[], double penetrationFactor) const; // Compute the error values for all the mathematical constraints - double getPenetrationDepth() const; // Return the penetration depth + virtual void computeJacobian(int noConstraint, decimal J_SP[NB_MAX_CONSTRAINTS][2*6]) const; // Compute the jacobian matrix for all mathematical constraints + virtual void computeLowerBound(int noConstraint, decimal lowerBounds[NB_MAX_CONSTRAINTS]) const; // Compute the lowerbounds values for all the mathematical constraints + virtual void computeUpperBound(int noConstraint, decimal upperBounds[NB_MAX_CONSTRAINTS]) const; // Compute the upperbounds values for all the mathematical constraints + virtual void computeErrorValue(int noConstraint, decimal errorValues[]) const; // Compute the error values for all the mathematical constraints + decimal getPenetrationDepth() const; // Return the penetration depth #ifdef VISUAL_DEBUG void draw() const; // Draw the contact (for debugging) #endif @@ -117,7 +116,7 @@ inline Vector3 Contact::getNormal() const { } // Set the penetration depth of the contact -inline void Contact::setPenetrationDepth(double penetrationDepth) { +inline void Contact::setPenetrationDepth(decimal penetrationDepth) { this->penetrationDepth = penetrationDepth; } @@ -152,7 +151,7 @@ inline void Contact::setWorldPointOnBody2(const Vector3& worldPoint) { } // Return the penetration depth of the contact -inline double Contact::getPenetrationDepth() const { +inline decimal Contact::getPenetrationDepth() const { return penetrationDepth; } @@ -162,7 +161,7 @@ inline void Contact::draw() const { glColor3f(1.0, 0.0, 0.0); glutSolidSphere(0.3, 20, 20); } -#endif +#endif } // End of the ReactPhysics3D namespace diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index ebbe240a..4e8592d4 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -34,7 +34,8 @@ using namespace std; // Constructor ConstraintSolver::ConstraintSolver(PhysicsWorld* world) :physicsWorld(world), nbConstraints(0), nbIterationsLCP(DEFAULT_LCP_ITERATIONS), - penetrationFactor(DEFAULT_PENETRATION_FACTOR) { + nbIterationsLCPErrorCorrection(DEFAULT_LCP_ITERATIONS_ERROR_CORRECTION), + isErrorCorrectionActive(false) { } @@ -46,8 +47,9 @@ ConstraintSolver::~ConstraintSolver() { // Initialize the constraint solver before each solving void ConstraintSolver::initialize() { Constraint* constraint; - + nbConstraints = 0; + nbConstraintsError = 0; // For each constraint vector::iterator it; @@ -68,6 +70,11 @@ void ConstraintSolver::initialize() { // Update the size of the jacobian matrix nbConstraints += constraint->getNbConstraints(); + + // Update the size of the jacobian matrix for error correction projection + if (constraint->getType() == CONTACT) { + nbConstraintsError++; + } } } @@ -75,16 +82,19 @@ void ConstraintSolver::initialize() { nbBodies = bodyNumberMapping.size(); assert(nbConstraints > 0 && nbConstraints <= NB_MAX_CONSTRAINTS); + assert(nbConstraintsError > 0 && nbConstraintsError <= NB_MAX_CONSTRAINTS); assert(nbBodies > 0 && nbBodies <= NB_MAX_BODIES); } // Fill in all the matrices needed to solve the LCP problem // Notice that all the active constraints should have been evaluated first -void ConstraintSolver::fillInMatrices() { +void ConstraintSolver::fillInMatrices(decimal dt) { Constraint* constraint; + decimal oneOverDt = 1.0 / dt; // For each active constraint int noConstraint = 0; + int noConstraintError = 0; for (int c=0; ccomputeUpperBound(noConstraint, upperBounds); // Fill in the error vector - constraint->computeErrorValue(noConstraint, errorValues, penetrationFactor); + constraint->computeErrorValue(noConstraint, errorValues); // Get the cached lambda values of the constraint for (int i=0; igetNbConstraints(); i++) { lambdaInit[noConstraint + i] = constraint->getCachedLambda(i); } + // If the constraint is a contact + if (constraint->getType() == CONTACT) { + Contact* contact = dynamic_cast(constraint); + decimal penetrationDepth = contact->getPenetrationDepth(); + + // If error correction with projection is active + if (isErrorCorrectionActive) { + + // Fill in the error correction projection parameters + lowerBoundsError[noConstraintError] = lowerBounds[noConstraint]; + upperBoundsError[noConstraintError] = upperBounds[noConstraint]; + for (int i=0; i<12; i++) { + J_spError[noConstraintError][i] = J_sp[noConstraint][i]; + } + bodyMappingError[noConstraintError][0] = constraint->getBody1(); + bodyMappingError[noConstraintError][1] = constraint->getBody2(); + penetrationDepths[noConstraintError] = contact->getPenetrationDepth(); + + // If the penetration depth is small + if (penetrationDepth < PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION) { + // Use the Baumgarte error correction term for contacts instead of + // first order world projection + errorValues[noConstraint] += 0.1 * oneOverDt * contact->getPenetrationDepth(); + } + + noConstraintError++; + } + else { // If error correction with projection is not active + // Add the Baumgarte error correction term for contacts + errorValues[noConstraint] += 0.1 * oneOverDt * contact->getPenetrationDepth(); + } + } + noConstraint += constraint->getNbConstraints(); } @@ -129,21 +172,31 @@ void ConstraintSolver::fillInMatrices() { // Compute the vector V1 with initial velocities values Vector3 linearVelocity = rigidBody->getLinearVelocity(); Vector3 angularVelocity = rigidBody->getAngularVelocity(); - int bodyIndexArray = 6 * bodyNumber; - V1[bodyIndexArray] = linearVelocity[0]; - V1[bodyIndexArray + 1] = linearVelocity[1]; - V1[bodyIndexArray + 2] = linearVelocity[2]; - V1[bodyIndexArray + 3] = angularVelocity[0]; - V1[bodyIndexArray + 4] = angularVelocity[1]; - V1[bodyIndexArray + 5] = angularVelocity[2]; + int bodyIndexArray = 6 * bodyNumber; + V1[bodyIndexArray] = linearVelocity[0]; + V1[bodyIndexArray + 1] = linearVelocity[1]; + V1[bodyIndexArray + 2] = linearVelocity[2]; + V1[bodyIndexArray + 3] = angularVelocity[0]; + V1[bodyIndexArray + 4] = angularVelocity[1]; + V1[bodyIndexArray + 5] = angularVelocity[2]; // Compute the vector Vconstraint with final constraint velocities Vconstraint[bodyIndexArray] = 0.0; - Vconstraint[bodyIndexArray + 1] = 0.0; - Vconstraint[bodyIndexArray + 2] = 0.0; - Vconstraint[bodyIndexArray + 3] = 0.0; - Vconstraint[bodyIndexArray + 4] = 0.0; - Vconstraint[bodyIndexArray + 5] = 0.0; + Vconstraint[bodyIndexArray + 1] = 0.0; + Vconstraint[bodyIndexArray + 2] = 0.0; + Vconstraint[bodyIndexArray + 3] = 0.0; + Vconstraint[bodyIndexArray + 4] = 0.0; + Vconstraint[bodyIndexArray + 5] = 0.0; + + // Compute the vector Vconstraint with final constraint velocities + if (isErrorCorrectionActive) { + VconstraintError[bodyIndexArray] = 0.0; + VconstraintError[bodyIndexArray + 1] = 0.0; + VconstraintError[bodyIndexArray + 2] = 0.0; + VconstraintError[bodyIndexArray + 3] = 0.0; + VconstraintError[bodyIndexArray + 4] = 0.0; + VconstraintError[bodyIndexArray + 5] = 0.0; + } // Compute the vector with forces and torques values Vector3 externalForce = rigidBody->getExternalForce(); @@ -155,63 +208,78 @@ void ConstraintSolver::fillInMatrices() { Fext[bodyIndexArray + 4] = externalTorque[1]; Fext[bodyIndexArray + 5] = externalTorque[2]; - // Initialize the mass and inertia tensor matrices + // Initialize the mass and inertia tensor matrices Minv_sp_inertia[bodyNumber].setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); - Minv_sp_mass_diag[bodyNumber] = 0.0; - - // If the motion of the rigid body is enabled - if (rigidBody->getIsMotionEnabled()) { - Minv_sp_inertia[bodyNumber] = rigidBody->getInertiaTensorInverseWorld(); - Minv_sp_mass_diag[bodyNumber] = rigidBody->getMassInverse(); + Minv_sp_mass_diag[bodyNumber] = 0.0; + + // If the motion of the rigid body is enabled + if (rigidBody->getIsMotionEnabled()) { + Minv_sp_inertia[bodyNumber] = rigidBody->getInertiaTensorInverseWorld(); + Minv_sp_mass_diag[bodyNumber] = rigidBody->getMassInverse(); } - } } // Compute the vector b -void ConstraintSolver::computeVectorB(double dt) { +void ConstraintSolver::computeVectorB(decimal dt) { uint indexBody1, indexBody2; - double oneOverDT = 1.0 / dt; - - //b = errorValues * oneOverDT; + decimal oneOverDT = 1.0 / dt; for (uint c = 0; c= PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION) { + bError[c] = penetrationDepths[c] * oneOverDT; + } + else { + bError[c] = 0.0; + } } } @@ -222,24 +290,54 @@ void ConstraintSolver::computeMatrixB_sp() { // For each constraint for (uint c = 0; c @@ -44,63 +44,99 @@ namespace reactphysics3d { Temporal Coherence" from Erin Catto. We keep the same notations as in the paper. The idea is to construct a LCP problem and then solve it using a Projected Gauss Seidel (PGS) solver. + + The idea is to solve the following system for lambda : + JM^-1J^T * lamdba - 1/dt * b_error + 1/dt * JV^1 + JM^-1F_ext >= 0 + + By default, error correction using first world order projections (described + by Erleben in section 4.16 in his PhD thesis "Stable, Robust, and + Versatile Multibody Dynamics Animation") is used for very large penetration + depths. Error correction with projection requires to solve another LCP problem + that is simpler than the above one and by considering only the contact + constraints. The LCP problem for error correction is the following one : + J_contact M^-1 J_contact^T * lambda + 1/dt * -d >= 0 + + where "d" is a vector with penetration depths. If the penetration depth of + a given contact is not very large, we use the Baumgarte error correction (see + the paper from Erin Catto). + ------------------------------------------------------------------- */ class ConstraintSolver { private: PhysicsWorld* physicsWorld; // Reference to the physics world std::vector activeConstraints; // Current active constraints in the physics world + bool isErrorCorrectionActive; // True if error correction (with world order) is active uint nbIterationsLCP; // Number of iterations of the LCP solver + uint nbIterationsLCPErrorCorrection; // Number of iterations of the LCP solver for error correction uint nbConstraints; // Total number of constraints (with the auxiliary constraints) + uint nbConstraintsError; // Number of constraints for error correction projection (only contact constraints) uint nbBodies; // Current number of bodies in the physics world - double penetrationFactor; // Penetration factor "beta" for penetration correction std::set constraintBodies; // Bodies that are implied in some constraint std::map bodyNumberMapping; // Map a body pointer with its index number Body* bodyMapping[NB_MAX_CONSTRAINTS][2]; // 2-dimensional array that contains the mapping of body reference // in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains // the pointer to the body that correspond to the 1x6 J_ij matrix in the // J_sp matrix. An integer body index refers to its index in the "bodies" std::vector - double J_sp[NB_MAX_CONSTRAINTS][2*6]; // 2-dimensional array that correspond to the sparse representation of the jacobian matrix of all constraints + Body* bodyMappingError[NB_MAX_CONSTRAINTS][2]; // Same as bodyMapping but for error correction projection + decimal J_sp[NB_MAX_CONSTRAINTS][2*6]; // 2-dimensional array that correspond to the sparse representation of the jacobian matrix of all constraints // This array contains for each constraint two 1x6 Jacobian matrices (one for each body of the constraint) // a 1x6 matrix - double B_sp[2][6*NB_MAX_CONSTRAINTS]; // 2-dimensional array that correspond to a useful matrix in sparse representation + decimal B_sp[2][6*NB_MAX_CONSTRAINTS]; // 2-dimensional array that correspond to a useful matrix in sparse representation // This array contains for each constraint two 6x1 matrices (one for each body of the constraint) // a 6x1 matrix - double b[NB_MAX_CONSTRAINTS]; // Vector "b" of the LCP problem - double d[NB_MAX_CONSTRAINTS]; // Vector "d" - double a[6*NB_MAX_BODIES]; // Vector "a" - double lambda[NB_MAX_CONSTRAINTS]; // Lambda vector of the LCP problem - double lambdaInit[NB_MAX_CONSTRAINTS]; // Lambda init vector for the LCP solver - double errorValues[NB_MAX_CONSTRAINTS]; // Error vector of all constraints - double lowerBounds[NB_MAX_CONSTRAINTS]; // Vector that contains the low limits for the variables of the LCP problem - double upperBounds[NB_MAX_CONSTRAINTS]; // Vector that contains the high limits for the variables of the LCP problem + decimal J_spError[NB_MAX_CONSTRAINTS][2*6]; // Same as J_sp for error correction projection (only contact constraints) + decimal B_spError[2][6*NB_MAX_CONSTRAINTS]; // Same as B_sp for error correction projection (only contact constraints) + decimal b[NB_MAX_CONSTRAINTS]; // Vector "b" of the LCP problem + decimal bError[NB_MAX_CONSTRAINTS]; // Vector "b" of the LCP problem for error correction projection + decimal d[NB_MAX_CONSTRAINTS]; // Vector "d" + decimal a[6*NB_MAX_BODIES]; // Vector "a" + decimal aError[6*NB_MAX_BODIES]; // Vector "a" for error correction projection + decimal penetrationDepths[NB_MAX_CONSTRAINTS]; // Array of penetration depths for error correction projection + decimal lambda[NB_MAX_CONSTRAINTS]; // Lambda vector of the LCP problem + decimal lambdaError[NB_MAX_CONSTRAINTS]; // Lambda vector of the LCP problem for error correction projections + decimal lambdaInit[NB_MAX_CONSTRAINTS]; // Lambda init vector for the LCP solver + decimal errorValues[NB_MAX_CONSTRAINTS]; // Error vector of all constraints + decimal lowerBounds[NB_MAX_CONSTRAINTS]; // Vector that contains the low limits for the variables of the LCP problem + decimal upperBounds[NB_MAX_CONSTRAINTS]; // Vector that contains the high limits for the variables of the LCP problem + decimal lowerBoundsError[NB_MAX_CONSTRAINTS]; // Same as "lowerBounds" but for error correction projections + decimal upperBoundsError[NB_MAX_CONSTRAINTS]; // Same as "upperBounds" but for error correction projections Matrix3x3 Minv_sp_inertia[NB_MAX_BODIES]; // 3x3 world inertia tensor matrix I for each body (from the Minv_sp matrix) - double Minv_sp_mass_diag[NB_MAX_BODIES]; // Array that contains for each body the inverse of its mass + decimal Minv_sp_mass_diag[NB_MAX_BODIES]; // Array that contains for each body the inverse of its mass // This is an array of size nbBodies that contains in each cell a 6x6 matrix - double V1[6*NB_MAX_BODIES]; // Array that contains for each body the 6x1 vector that contains linear and angular velocities + decimal V1[6*NB_MAX_BODIES]; // Array that contains for each body the 6x1 vector that contains linear and angular velocities // Each cell contains a 6x1 vector with linear and angular velocities - double Vconstraint[6*NB_MAX_BODIES]; // Same kind of vector as V1 but contains the final constraint velocities - double Fext[6*NB_MAX_BODIES]; // Array that contains for each body the 6x1 vector that contains external forces and torques + decimal Vconstraint[6*NB_MAX_BODIES]; // Same kind of vector as V1 but contains the final constraint velocities + decimal VconstraintError[6*NB_MAX_BODIES]; // Same kind of vector as V1 but contains the final constraint velocities + decimal Fext[6*NB_MAX_BODIES]; // Array that contains for each body the 6x1 vector that contains external forces and torques // Each cell contains a 6x1 vector with external force and torque. void initialize(); // Initialize the constraint solver before each solving - void fillInMatrices(); // Fill in all the matrices needed to solve the LCP problem - void computeVectorB(double dt); // Compute the vector b + void fillInMatrices(decimal dt); // Fill in all the matrices needed to solve the LCP problem + void computeVectorB(decimal dt); // Compute the vector b + void computeVectorBError(decimal dt); // Compute the vector b for error correction projection void computeMatrixB_sp(); // Compute the matrix B_sp - void computeVectorVconstraint(double dt); // Compute the vector V2 + void computeMatrixB_spErrorCorrection(); // Compute the matrix B_spError for error correction projection + void computeVectorVconstraint(decimal dt); // Compute the vector V2 + void computeVectorVconstraintError(decimal dt); // Same as computeVectorVconstraint() but for error correction projection void cacheLambda(); // Cache the lambda values in order to reuse them in the next step to initialize the lambda vector void computeVectorA(); // Compute the vector a used in the solve() method + void computeVectorAError(); // Same as computeVectorA() but for error correction projection void solveLCP(); // Solve a LCP problem using Projected-Gauss-Seidel algorithm - - public: - ConstraintSolver(PhysicsWorld* world); // Constructor - virtual ~ConstraintSolver(); // Destructor - void solve(double dt); // Solve the current LCP problem - bool isConstrainedBody(Body* body) const; // Return true if the body is in at least one constraint - Vector3 getConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem - Vector3 getConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem - void cleanup(); // Cleanup of the constraint solver - void setPenetrationFactor(double penetrationFactor); // Set the penetration factor - void setNbLCPIterations(uint nbIterations); // Set the number of iterations of the LCP solver + void solveLCPErrorCorrection(); // Solve the LCP problem for error correction projection + + public: + ConstraintSolver(PhysicsWorld* world); // Constructor + virtual ~ConstraintSolver(); // Destructor + void solve(decimal dt); // Solve the current LCP problem + bool isConstrainedBody(Body* body) const; // Return true if the body is in at least one constraint + Vector3 getConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem + Vector3 getConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem + Vector3 getErrorConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem for error correction + Vector3 getErrorConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem for error correction + void cleanup(); // Cleanup of the constraint solver + void setPenetrationFactor(decimal penetrationFactor); // Set the penetration factor + void setNbLCPIterations(uint nbIterations); // Set the number of iterations of the LCP solver + void setIsErrorCorrectionActive(bool isErrorCorrectionActive); // Set the isErrorCorrectionActive value }; // Return true if the body is in at least one constraint @@ -118,7 +154,6 @@ inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(Body* body) return Vector3(Vconstraint[indexBodyArray], Vconstraint[indexBodyArray + 1], Vconstraint[indexBodyArray + 2]); } - // Return the constrained angular velocity of a body after solving the LCP problem inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body) { assert(isConstrainedBody(body)); @@ -126,6 +161,20 @@ inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body) return Vector3(Vconstraint[indexBodyArray + 3], Vconstraint[indexBodyArray + 4], Vconstraint[indexBodyArray + 5]); } +// Return the constrained linear velocity of a body after solving the LCP problem for error correction +inline Vector3 ConstraintSolver::getErrorConstrainedLinearVelocityOfBody(Body* body) { + //assert(isConstrainedBody(body)); + uint indexBodyArray = 6 * bodyNumberMapping[body]; + return Vector3(VconstraintError[indexBodyArray], VconstraintError[indexBodyArray + 1], VconstraintError[indexBodyArray + 2]); +} + +// Return the constrained angular velocity of a body after solving the LCP problem for error correction +inline Vector3 ConstraintSolver::getErrorConstrainedAngularVelocityOfBody(Body* body) { + //assert(isConstrainedBody(body)); + uint indexBodyArray = 6 * bodyNumberMapping[body]; + return Vector3(VconstraintError[indexBodyArray + 3], VconstraintError[indexBodyArray + 4], VconstraintError[indexBodyArray + 5]); +} + // Cleanup of the constraint solver inline void ConstraintSolver::cleanup() { bodyNumberMapping.clear(); @@ -133,39 +182,53 @@ inline void ConstraintSolver::cleanup() { activeConstraints.clear(); } -// Set the penetration factor -inline void ConstraintSolver::setPenetrationFactor(double factor) { - penetrationFactor = factor; -} - // Set the number of iterations of the LCP solver inline void ConstraintSolver::setNbLCPIterations(uint nbIterations) { nbIterationsLCP = nbIterations; -} +} + + +// Set the isErrorCorrectionActive value +inline void ConstraintSolver::setIsErrorCorrectionActive(bool isErrorCorrectionActive) { + this->isErrorCorrectionActive = isErrorCorrectionActive; +} + // Solve the current LCP problem -inline void ConstraintSolver::solve(double dt) { +inline void ConstraintSolver::solve(decimal dt) { - // Allocate memory for the matrices + // Initialize the solver initialize(); // Fill-in all the matrices needed to solve the LCP problem - fillInMatrices(); + fillInMatrices(dt); // Compute the vector b computeVectorB(dt); + if (isErrorCorrectionActive) { + computeVectorBError(dt); + } // Compute the matrix B computeMatrixB_sp(); + if (isErrorCorrectionActive) { + computeMatrixB_spErrorCorrection(); + } // Solve the LCP problem (computation of lambda) solveLCP(); + if (isErrorCorrectionActive) { + solveLCPErrorCorrection(); + } // Cache the lambda values in order to use them in the next step cacheLambda(); // Compute the vector Vconstraint computeVectorVconstraint(dt); + if (isErrorCorrectionActive) { + computeVectorVconstraintError(dt); + } } } // End of ReactPhysics3D namespace diff --git a/src/engine/PhysicsEngine.cpp b/src/engine/PhysicsEngine.cpp index c669f5ab..9ad96c45 100644 --- a/src/engine/PhysicsEngine.cpp +++ b/src/engine/PhysicsEngine.cpp @@ -31,7 +31,7 @@ using namespace reactphysics3d; using namespace std; // Constructor -PhysicsEngine::PhysicsEngine(PhysicsWorld* world, double timeStep = DEFAULT_TIMESTEP) +PhysicsEngine::PhysicsEngine(PhysicsWorld* world, decimal timeStep = DEFAULT_TIMESTEP) : world(world), timer(timeStep), collisionDetection(world), constraintSolver(world) { assert(world); assert(timeStep > 0.0); @@ -57,7 +57,6 @@ void PhysicsEngine::update() { // While the time accumulator is not empty while(timer.isPossibleToTakeStep()) { existCollision = false; - // Compute the collision detection if (collisionDetection.computeCollisionDetection()) { @@ -96,10 +95,12 @@ void PhysicsEngine::update() { // This method uses the semi-implicit Euler method to update the position and // orientation of the body void PhysicsEngine::updateAllBodiesMotion() { - double dt = timer.getTimeStep(); + decimal dt = timer.getTimeStep(); Vector3 newLinearVelocity; Vector3 newAngularVelocity; - + Vector3 linearVelocityErrorCorrection; + Vector3 angularVelocityErrorCorrection; + // For each body of thephysics world for (vector::iterator it=world->getRigidBodiesBeginIterator(); it != world->getRigidBodiesEndIterator(); ++it) { @@ -110,12 +111,17 @@ void PhysicsEngine::updateAllBodiesMotion() { if (rigidBody->getIsMotionEnabled()) { newLinearVelocity.setAllValues(0.0, 0.0, 0.0); newAngularVelocity.setAllValues(0.0, 0.0, 0.0); + linearVelocityErrorCorrection.setAllValues(0.0, 0.0, 0.0); + angularVelocityErrorCorrection.setAllValues(0.0, 0.0, 0.0); // If it's a constrained body if (constraintSolver.isConstrainedBody(*it)) { // Get the constrained linear and angular velocities from the constraint solver newLinearVelocity = constraintSolver.getConstrainedLinearVelocityOfBody(*it); newAngularVelocity = constraintSolver.getConstrainedAngularVelocityOfBody(*it); + + linearVelocityErrorCorrection = constraintSolver.getErrorConstrainedLinearVelocityOfBody(rigidBody); + angularVelocityErrorCorrection = constraintSolver.getErrorConstrainedAngularVelocityOfBody(rigidBody); } // Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the @@ -126,9 +132,10 @@ void PhysicsEngine::updateAllBodiesMotion() { // Add the velocity V1 to the new velocity newLinearVelocity += rigidBody->getLinearVelocity(); newAngularVelocity += rigidBody->getAngularVelocity(); - + // Update the position and the orientation of the body according to the new velocity - updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity); + updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity, + linearVelocityErrorCorrection, angularVelocityErrorCorrection); // Update the AABB of the rigid body rigidBody->updateAABB(); @@ -139,8 +146,9 @@ void PhysicsEngine::updateAllBodiesMotion() { // Update the position and orientation of a body // Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new // orientation of the body -void PhysicsEngine::updatePositionAndOrientationOfBody(RigidBody* rigidBody, const Vector3& newLinVelocity, const Vector3& newAngVelocity) { - double dt = timer.getTimeStep(); +void PhysicsEngine::updatePositionAndOrientationOfBody(RigidBody* rigidBody, const Vector3& newLinVelocity, const Vector3& newAngVelocity, + const Vector3& linearVelocityErrorCorrection, const Vector3& angularVelocityErrorCorrection) { + decimal dt = timer.getTimeStep(); assert(rigidBody); @@ -150,21 +158,27 @@ void PhysicsEngine::updatePositionAndOrientationOfBody(RigidBody* rigidBody, con // Update the linear and angular velocity of the body rigidBody->setLinearVelocity(newLinVelocity); rigidBody->setAngularVelocity(newAngVelocity); - + // Get current position and orientation of the body const Vector3& currentPosition = rigidBody->getTransform().getPosition(); const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation(); + + // Error correction projection + Vector3 correctedPosition = currentPosition + dt * linearVelocityErrorCorrection; + Quaternion correctedOrientation = currentOrientation + Quaternion(angularVelocityErrorCorrection.getX(), angularVelocityErrorCorrection.getY(), angularVelocityErrorCorrection.getZ(), 0) * currentOrientation * 0.5 * dt; + + Vector3 newPosition = correctedPosition + newLinVelocity * dt; + Quaternion newOrientation = correctedOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * correctedOrientation * 0.5 * dt; - Vector3 newPosition = currentPosition + newLinVelocity * dt; - Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * currentOrientation * 0.5 * dt; Transform newTransform(newPosition, newOrientation.getUnit()); rigidBody->setTransform(newTransform); } // Compute and set the interpolation factor to all bodies void PhysicsEngine::setInterpolationFactorToAllBodies() { + // Compute the interpolation factor - double factor = timer.computeInterpolationFactor(); + decimal factor = timer.computeInterpolationFactor(); assert(factor >= 0.0 && factor <= 1.0); // Set the factor to all bodies diff --git a/src/engine/PhysicsEngine.h b/src/engine/PhysicsEngine.h index 5c09428e..5f9e5807 100644 --- a/src/engine/PhysicsEngine.h +++ b/src/engine/PhysicsEngine.h @@ -32,7 +32,7 @@ #include "ConstraintSolver.h" #include "../body/RigidBody.h" #include "Timer.h" -#include "../constants.h" +#include "../configuration.h" // Namespace ReactPhysics3D namespace reactphysics3d { @@ -50,20 +50,23 @@ class PhysicsEngine { CollisionDetection collisionDetection; // Collision detection ConstraintSolver constraintSolver; // Constraint solver - void updateAllBodiesMotion(); // Compute the motion of all bodies and update their positions and orientations - void updatePositionAndOrientationOfBody(RigidBody* body, const Vector3& newLinVelocity, const Vector3& newAngVelocity); // Update the position and orientation of a body - void setInterpolationFactorToAllBodies(); // Compute and set the interpolation factor to all bodies - void applyGravity(); // Apply the gravity force to all bodies + void updateAllBodiesMotion(); // Compute the motion of all bodies and update their positions and orientations + void updatePositionAndOrientationOfBody(RigidBody* body, const Vector3& newLinVelocity, const Vector3& newAngVelocity, + const Vector3& linearVelocityErrorCorrection, const Vector3& angularVelocityErrorCorrection); // Update the position and orientation of a body + void setInterpolationFactorToAllBodies(); // Compute and set the interpolation factor to all bodies + void applyGravity(); // Apply the gravity force to all bodies public : - PhysicsEngine(PhysicsWorld* world, double timeStep); // Constructor + PhysicsEngine(PhysicsWorld* world, decimal timeStep); // Constructor ~PhysicsEngine(); // Destructor - void start(); // Start the physics simulation - void stop(); // Stop the physics simulation - void update(); // Update the physics simulation - void setPenetrationFactor(double factor); // Set the penetration factor for the constraint solver - void setNbLCPIterations(uint nbIterations); // Set the number of iterations of the LCP solver + void start(); // Start the physics simulation + void stop(); // Stop the physics simulation + void update(); // Update the physics simulation + void setPenetrationFactor(decimal factor); // Set the penetration factor for the constraint solver + void setNbLCPIterations(uint nbIterations); // Set the number of iterations of the LCP solver + void setIsErrorCorrectionActive(bool isErrorCorrectionActive); // Set the isErrorCorrectionActive value + }; // --- Inline functions --- // @@ -78,14 +81,20 @@ inline void PhysicsEngine::stop() { } // Set the penetration factor for the constraint solver -inline void PhysicsEngine::setPenetrationFactor(double factor) { +inline void PhysicsEngine::setPenetrationFactor(decimal factor) { constraintSolver.setPenetrationFactor(factor); } // Set the number of iterations of the LCP solver inline void PhysicsEngine::setNbLCPIterations(uint nbIterations) { constraintSolver.setNbLCPIterations(nbIterations); -} +} + +// Set the isErrorCorrectionActive value +inline void PhysicsEngine::setIsErrorCorrectionActive(bool isErrorCorrectionActive) { + constraintSolver.setIsErrorCorrectionActive(isErrorCorrectionActive); +} + } diff --git a/src/engine/PhysicsWorld.cpp b/src/engine/PhysicsWorld.cpp index 4bb67ffd..73bd790c 100644 --- a/src/engine/PhysicsWorld.cpp +++ b/src/engine/PhysicsWorld.cpp @@ -44,10 +44,10 @@ PhysicsWorld::~PhysicsWorld() { } // Create a rigid body into the physics world -RigidBody* PhysicsWorld::createRigidBody(const Transform& transform, double mass, const Matrix3x3& inertiaTensorLocal, Shape* shape) { +RigidBody* PhysicsWorld::createRigidBody(const Transform& transform, decimal mass, const Matrix3x3& inertiaTensorLocal, Collider* collider) { // Create the rigid body - RigidBody* rigidBody = new (memoryPoolRigidBodies.allocateObject()) RigidBody(transform, mass, inertiaTensorLocal, shape, currentBodyID); + RigidBody* rigidBody = new (memoryPoolRigidBodies.allocateObject()) RigidBody(transform, mass, inertiaTensorLocal, collider, currentBodyID); currentBodyID++; @@ -60,11 +60,11 @@ RigidBody* PhysicsWorld::createRigidBody(const Transform& transform, double mass void PhysicsWorld::destroyRigidBody(RigidBody* rigidBody) { removeRigidBody(rigidBody); - // Call the constructor of the rigid body - rigidBody->RigidBody::~RigidBody(); - - // Free the object from the memory pool - memoryPoolRigidBodies.freeObject(rigidBody); + // Call the constructor of the rigid body + rigidBody->RigidBody::~RigidBody(); + + // Free the object from the memory pool + memoryPoolRigidBodies.freeObject(rigidBody); } // Remove all collision contacts constraints diff --git a/src/engine/PhysicsWorld.h b/src/engine/PhysicsWorld.h index 5c3f8fd3..a79592d7 100644 --- a/src/engine/PhysicsWorld.h +++ b/src/engine/PhysicsWorld.h @@ -64,8 +64,8 @@ class PhysicsWorld { PhysicsWorld(const Vector3& gravity); // Constructor virtual ~PhysicsWorld(); // Destructor - RigidBody* createRigidBody(const Transform& transform, double mass, - const Matrix3x3& inertiaTensorLocal, Shape* shape); // Create a rigid body into the physics world + RigidBody* createRigidBody(const Transform& transform, decimal mass, + const Matrix3x3& inertiaTensorLocal, Collider* collider); // Create a rigid body into the physics world void destroyRigidBody(RigidBody* rigidBody); // Destroy a rigid body void clearAddedAndRemovedBodies(); // Clear the addedBodies and removedBodies sets Vector3 getGravity() const; // Return the gravity vector of the world diff --git a/src/mathematics/Matrix3x3.cpp b/src/mathematics/Matrix3x3.cpp index 10ffa53a..54f85ad9 100644 --- a/src/mathematics/Matrix3x3.cpp +++ b/src/mathematics/Matrix3x3.cpp @@ -37,12 +37,12 @@ Matrix3x3::Matrix3x3() { } // Constructor -Matrix3x3::Matrix3x3(double value) { +Matrix3x3::Matrix3x3(decimal value) { setAllValues(value, value, value, value, value, value, value, value, value); } // Constructor with arguments -Matrix3x3::Matrix3x3(double a1, double a2, double a3, double b1, double b2, double b3, double c1, double c2, double c3) { +Matrix3x3::Matrix3x3(decimal a1, decimal a2, decimal a3, decimal b1, decimal b2, decimal b3, decimal c1, decimal c2, decimal c3) { // Initialize the matrix with the values setAllValues(a1, a2, a3, b1, b2, b3, c1, c2, c3); } @@ -55,11 +55,11 @@ Matrix3x3::~Matrix3x3() { // Return the inverse matrix Matrix3x3 Matrix3x3::getInverse() const { // Compute the determinant of the matrix - double determinant = getDeterminant(); + decimal determinant = getDeterminant(); // Check if the determinant is equal to zero assert(determinant != 0.0); - double invDeterminant = 1.0 / determinant; + decimal invDeterminant = 1.0 / determinant; Matrix3x3 tempMatrix; // Compute the inverse of the matrix diff --git a/src/mathematics/Matrix3x3.h b/src/mathematics/Matrix3x3.h index 5b7628f5..6bd5b452 100644 --- a/src/mathematics/Matrix3x3.h +++ b/src/mathematics/Matrix3x3.h @@ -42,23 +42,23 @@ namespace reactphysics3d { */ class Matrix3x3 { private : - double array[3][3]; // Array with the values of the matrix + decimal array[3][3]; // Array with the values of the matrix public : Matrix3x3(); // Constructor - Matrix3x3(double value); // Constructor - Matrix3x3(double a1, double a2, double a3, double b1, double b2, double b3, - double c1, double c2, double c3); // Constructor + Matrix3x3(decimal value); // Constructor + Matrix3x3(decimal a1, decimal a2, decimal a3, decimal b1, decimal b2, decimal b3, + decimal c1, decimal c2, decimal c3); // Constructor virtual ~Matrix3x3(); // Destructor - double getValue(int i, int j) const; // Get a value in the matrix - void setValue(int i, int j, double value); // Set a value in the matrix - void setAllValues(double a1, double a2, double a3, double b1, double b2, double b3, - double c1, double c2, double c3); // Set all the values in the matrix + decimal getValue(int i, int j) const; // Get a value in the matrix + void setValue(int i, int j, decimal value); // Set a value in the matrix + void setAllValues(decimal a1, decimal a2, decimal a3, decimal b1, decimal b2, decimal b3, + decimal c1, decimal c2, decimal c3); // Set all the values in the matrix Vector3 getColumn(int i) const; // Return a column Matrix3x3 getTranspose() const; // Return the transpose matrix - double getDeterminant() const; // Return the determinant of the matrix - double getTrace() const; // Return the trace of the matrix + decimal getDeterminant() const; // Return the determinant of the matrix + decimal getTrace() const; // Return the trace of the matrix Matrix3x3 getInverse() const; // Return the inverse matrix Matrix3x3 getAbsoluteMatrix() const; // Return the matrix with absolute values void setToIdentity(); // Set the matrix to the identity matrix @@ -68,8 +68,8 @@ class Matrix3x3 { friend Matrix3x3 operator+(const Matrix3x3& matrix1, const Matrix3x3& matrix2); // Overloaded operator for addition friend Matrix3x3 operator-(const Matrix3x3& matrix1, const Matrix3x3& matrix2); // Overloaded operator for substraction friend Matrix3x3 operator-(const Matrix3x3& matrix); // Overloaded operator for the negative of the matrix - friend Matrix3x3 operator*(double nb, const Matrix3x3& matrix); // Overloaded operator for multiplication with a number - friend Matrix3x3 operator*(const Matrix3x3& matrix, double nb); // Overloaded operator for multiplication with a matrix + friend Matrix3x3 operator*(decimal nb, const Matrix3x3& matrix); // Overloaded operator for multiplication with a number + friend Matrix3x3 operator*(const Matrix3x3& matrix, decimal nb); // Overloaded operator for multiplication with a matrix friend Matrix3x3 operator*(const Matrix3x3& matrix1, const Matrix3x3& matrix2); // Overloaded operator for matrix multiplication friend Vector3 operator*(const Matrix3x3& matrix, const Vector3& vector); // Overloaded operator for multiplication with a vector @@ -77,25 +77,25 @@ class Matrix3x3 { bool operator!= (const Matrix3x3& matrix) const; // Overloaded operator for the is different condition Matrix3x3& operator+=(const Matrix3x3& matrix); // Overloaded operator for addition with assignment Matrix3x3& operator-=(const Matrix3x3& matrix); // Overloaded operator for substraction with assignment - Matrix3x3& operator*=(double nb); // Overloaded operator for multiplication with a number with assignment + Matrix3x3& operator*=(decimal nb); // Overloaded operator for multiplication with a number with assignment }; // Method to get a value in the matrix (inline) -inline double Matrix3x3::getValue(int i, int j) const { +inline decimal Matrix3x3::getValue(int i, int j) const { assert(i>=0 && i<3 && j>=0 && j<3); return array[i][j]; } // Method to set a value in the matrix (inline) -inline void Matrix3x3::setValue(int i, int j, double value) { +inline void Matrix3x3::setValue(int i, int j, decimal value) { assert(i>=0 && i<3 && j>=0 && j<3); array[i][j] = value; } // Method to set all the values in the matrix -inline void Matrix3x3::setAllValues(double a1, double a2, double a3, double b1, double b2, double b3, - double c1, double c2, double c3) { +inline void Matrix3x3::setAllValues(decimal a1, decimal a2, decimal a3, decimal b1, decimal b2, decimal b3, + decimal c1, decimal c2, decimal c3) { array[0][0] = a1; array[0][1] = a2; array[0][2] = a3; array[1][0] = b1; array[1][1] = b2; array[1][2] = b3; array[2][0] = c1; array[2][1] = c2; array[2][2] = c3; @@ -116,14 +116,14 @@ inline Matrix3x3 Matrix3x3::getTranspose() const { } // Return the determinant of the matrix -inline double Matrix3x3::getDeterminant() const { +inline decimal Matrix3x3::getDeterminant() const { // Compute and return the determinant of the matrix return (array[0][0]*(array[1][1]*array[2][2]-array[2][1]*array[1][2]) - array[0][1]*(array[1][0]*array[2][2]-array[2][0]*array[1][2]) + array[0][2]*(array[1][0]*array[2][1]-array[2][0]*array[1][1])); } // Return the trace of the matrix -inline double Matrix3x3::getTrace() const { +inline decimal Matrix3x3::getTrace() const { // Compute and return the trace return (array[0][0] + array[1][1] + array[2][2]); } @@ -170,14 +170,14 @@ inline Matrix3x3 operator-(const Matrix3x3& matrix) { } // Overloaded operator for multiplication with a number -inline Matrix3x3 operator*(double nb, const Matrix3x3& matrix) { +inline Matrix3x3 operator*(decimal nb, const Matrix3x3& matrix) { return Matrix3x3(matrix.array[0][0] * nb, matrix.array[0][1] * nb, matrix.array[0][2] * nb, matrix.array[1][0] * nb, matrix.array[1][1] * nb, matrix.array[1][2] * nb, matrix.array[2][0] * nb, matrix.array[2][1] * nb, matrix.array[2][2] * nb); } // Overloaded operator for multiplication with a matrix -inline Matrix3x3 operator*(const Matrix3x3& matrix, double nb) { +inline Matrix3x3 operator*(const Matrix3x3& matrix, decimal nb) { return nb * matrix; } @@ -230,7 +230,7 @@ inline Matrix3x3& Matrix3x3::operator-=(const Matrix3x3& matrix) { } // Overloaded operator for multiplication with a number with assignment -inline Matrix3x3& Matrix3x3::operator*=(double nb) { +inline Matrix3x3& Matrix3x3::operator*=(decimal nb) { array[0][0] *= nb; array[0][1] *= nb; array[0][2] *= nb; array[1][0] *= nb; array[1][1] *= nb; array[1][2] *= nb; array[2][0] *= nb; array[2][1] *= nb; array[2][2] *= nb; diff --git a/src/mathematics/Quaternion.cpp b/src/mathematics/Quaternion.cpp index 433cf7c5..2d95cfee 100755 --- a/src/mathematics/Quaternion.cpp +++ b/src/mathematics/Quaternion.cpp @@ -38,13 +38,13 @@ Quaternion::Quaternion() } // Constructor with arguments -Quaternion::Quaternion(double x, double y, double z, double w) +Quaternion::Quaternion(decimal x, decimal y, decimal z, decimal w) :x(x), y(y), z(z), w(w) { } // Constructor with the component w and the vector v=(x y z) -Quaternion::Quaternion(double w, const Vector3& v) +Quaternion::Quaternion(decimal w, const Vector3& v) :x(v.getX()), y(v.getY()), z(v.getZ()), w(w) { } @@ -59,17 +59,17 @@ Quaternion::Quaternion(const Quaternion& quaternion) Quaternion::Quaternion(const Matrix3x3& matrix) { // Get the trace of the matrix - double trace = matrix.getTrace(); + decimal trace = matrix.getTrace(); - double array[3][3]; + decimal array[3][3]; for (int i=0; i<3; i++) { for (int j=0; j<3; j++) { array[i][j] = matrix.getValue(i, j); } } - double r; - double s; + decimal r; + decimal s; if (trace < 0.0) { if (array[1][1] > array[0][0]) { @@ -135,7 +135,7 @@ Quaternion::~Quaternion() { // Compute the rotation angle (in radians) and the 3D rotation axis // This method is used to get the rotation angle (in radian) and the unit // rotation axis of an orientation quaternion. -void Quaternion::getRotationAngleAxis(double& angle, Vector3& axis) const { +void Quaternion::getRotationAngleAxis(decimal& angle, Vector3& axis) const { Quaternion quaternion; // If the quaternion is unit @@ -163,26 +163,26 @@ void Quaternion::getRotationAngleAxis(double& angle, Vector3& axis) const { // Return the orientation matrix corresponding to this quaternion Matrix3x3 Quaternion::getMatrix() const { - double nQ = x*x + y*y + z*z + w*w; - double s = 0.0; + decimal nQ = x*x + y*y + z*z + w*w; + decimal s = 0.0; if (nQ > 0.0) { s = 2.0/nQ; } // Computations used for optimization (less multiplications) - double xs = x*s; - double ys = y*s; - double zs = z*s; - double wxs = w*xs; - double wys = w*ys; - double wzs = w*zs; - double xxs = x*xs; - double xys = x*ys; - double xzs = x*zs; - double yys = y*ys; - double yzs = y*zs; - double zzs = z*zs; + decimal xs = x*s; + decimal ys = y*s; + decimal zs = z*s; + decimal wxs = w*xs; + decimal wys = w*ys; + decimal wzs = w*zs; + decimal xxs = x*xs; + decimal xys = x*ys; + decimal xzs = x*zs; + decimal yys = y*ys; + decimal yzs = y*zs; + decimal zzs = z*zs; // Create the matrix corresponding to the quaternion return Matrix3x3(1.0-yys-zzs, xys-wzs, xzs + wys, @@ -192,13 +192,13 @@ Matrix3x3 Quaternion::getMatrix() const { // Compute the spherical linear interpolation between two quaternions. // The t argument has to be such that 0 <= t <= 1. This method is static. -Quaternion Quaternion::slerp(const Quaternion& quaternion1, const Quaternion& quaternion2, double t) { +Quaternion Quaternion::slerp(const Quaternion& quaternion1, const Quaternion& quaternion2, decimal t) { assert(t >= 0.0 && t <= 1.0); - double invert = 1.0; + decimal invert = 1.0; // Compute cos(theta) using the quaternion scalar product - double cosineTheta = quaternion1.dot(quaternion2); + decimal cosineTheta = quaternion1.dot(quaternion2); // Take care of the sign of cosineTheta if (cosineTheta < 0.0) { @@ -208,19 +208,20 @@ Quaternion Quaternion::slerp(const Quaternion& quaternion1, const Quaternion& qu // Because of precision, if cos(theta) is nearly 1, therefore theta is nearly 0 and we can write // sin((1-t)*theta) as (1-t) and sin(t*theta) as t - if(1-cosineTheta < EPSILON_TEST) { + const decimal epsilon = 0.00001; + if(1-cosineTheta < epsilon) { return quaternion1 * (1.0-t) + quaternion2 * (t * invert); } // Compute the theta angle - double theta = acos(cosineTheta); + decimal theta = acos(cosineTheta); // Compute sin(theta) - double sineTheta = sin(theta); + decimal sineTheta = sin(theta); // Compute the two coefficients that are in the spherical linear interpolation formula - double coeff1 = sin((1.0-t)*theta) / sineTheta; - double coeff2 = sin(t*theta) / sineTheta * invert; + decimal coeff1 = sin((1.0-t)*theta) / sineTheta; + decimal coeff2 = sin(t*theta) / sineTheta * invert; // Compute and return the interpolated quaternion return quaternion1 * coeff1 + quaternion2 * coeff2; diff --git a/src/mathematics/Quaternion.h b/src/mathematics/Quaternion.h index 28923594..23730dad 100644 --- a/src/mathematics/Quaternion.h +++ b/src/mathematics/Quaternion.h @@ -30,6 +30,7 @@ #include #include "Vector3.h" #include "Matrix3x3.h" +#include "../decimal.h" // ReactPhysics3D namespace namespace reactphysics3d { @@ -42,86 +43,86 @@ namespace reactphysics3d { */ class Quaternion { private : - double x; // Component x of the quaternion - double y; // Component y of the quaternion - double z; // Component z of the quaternion - double w; // Component w of the quaternion + decimal x; // Component x of the quaternion + decimal y; // Component y of the quaternion + decimal z; // Component z of the quaternion + decimal w; // Component w of the quaternion public : - Quaternion(); // Constructor - Quaternion(double x, double y, double z, double w); // Constructor with arguments - Quaternion(double w, const Vector3& v); // Constructor with the component w and the vector v=(x y z) - Quaternion(const Quaternion& quaternion); // Copy-constructor - Quaternion(const Matrix3x3& matrix); // Create a unit quaternion from a rotation matrix - ~Quaternion(); // Destructor - double getX() const; // Return the component x of the quaternion - double getY() const; // Return the component y of the quaternion - double getZ() const; // Return the component z of the quaternion - double getW() const; // Return the component w of the quaternion - void setX(double x); // Set the value x - void setY(double y); // Set the value y - void setZ(double z); // Set the value z - void setW(double w); // Set the value w - Vector3 vectorV() const; // Return the vector v=(x y z) of the quaternion - double length() const; // Return the length of the quaternion - Quaternion getUnit() const; // Return the unit quaternion - Quaternion getConjugate() const; // Return the conjugate quaternion - Quaternion getInverse() const; // Return the inverse of the quaternion - Matrix3x3 getMatrix() const; // Return the orientation matrix corresponding to this quaternion - static Quaternion identity(); // Return the identity quaternion - double dot(const Quaternion& quaternion) const; // Dot product between two quaternions - void getRotationAngleAxis(double& angle, Vector3& axis) const; // Compute the rotation angle (in radians) and the axis + Quaternion(); // Constructor + Quaternion(decimal x, decimal y, decimal z, decimal w); // Constructor with arguments + Quaternion(decimal w, const Vector3& v); // Constructor with the component w and the vector v=(x y z) + Quaternion(const Quaternion& quaternion); // Copy-constructor + Quaternion(const Matrix3x3& matrix); // Create a unit quaternion from a rotation matrix + ~Quaternion(); // Destructor + decimal getX() const; // Return the component x of the quaternion + decimal getY() const; // Return the component y of the quaternion + decimal getZ() const; // Return the component z of the quaternion + decimal getW() const; // Return the component w of the quaternion + void setX(decimal x); // Set the value x + void setY(decimal y); // Set the value y + void setZ(decimal z); // Set the value z + void setW(decimal w); // Set the value w + Vector3 vectorV() const; // Return the vector v=(x y z) of the quaternion + decimal length() const; // Return the length of the quaternion + Quaternion getUnit() const; // Return the unit quaternion + Quaternion getConjugate() const; // Return the conjugate quaternion + Quaternion getInverse() const; // Return the inverse of the quaternion + Matrix3x3 getMatrix() const; // Return the orientation matrix corresponding to this quaternion + static Quaternion identity(); // Return the identity quaternion + decimal dot(const Quaternion& quaternion) const; // Dot product between two quaternions + void getRotationAngleAxis(decimal& angle, Vector3& axis) const; // Compute the rotation angle (in radians) and the axis static Quaternion slerp(const Quaternion& quaternion1, - const Quaternion& quaternion2, double t); // Compute the spherical linear interpolation between two quaternions + const Quaternion& quaternion2, decimal t); // Compute the spherical linear interpolation between two quaternions // --- Overloaded operators --- // - Quaternion operator+(const Quaternion& quaternion) const; // Overloaded operator for the addition - Quaternion operator-(const Quaternion& quaternion) const; // Overloaded operator for the substraction - Quaternion operator*(double nb) const; // Overloaded operator for the multiplication with a constant - Quaternion operator*(const Quaternion& quaternion) const; // Overloaded operator for the multiplication - Quaternion& operator=(const Quaternion& quaternion); // Overloaded operator for assignment - bool operator==(const Quaternion& quaternion) const; // Overloaded operator for equality condition + Quaternion operator+(const Quaternion& quaternion) const; // Overloaded operator for the addition + Quaternion operator-(const Quaternion& quaternion) const; // Overloaded operator for the substraction + Quaternion operator*(decimal nb) const; // Overloaded operator for the multiplication with a constant + Quaternion operator*(const Quaternion& quaternion) const; // Overloaded operator for the multiplication + Quaternion& operator=(const Quaternion& quaternion); // Overloaded operator for assignment + bool operator==(const Quaternion& quaternion) const; // Overloaded operator for equality condition }; // --- Inline functions --- // // Get the value x (inline) -inline double Quaternion::getX() const { +inline decimal Quaternion::getX() const { return x; } // Get the value y (inline) -inline double Quaternion::getY() const { +inline decimal Quaternion::getY() const { return y; } // Get the value z (inline) -inline double Quaternion::getZ() const { +inline decimal Quaternion::getZ() const { return z; } // Get the value w (inline) -inline double Quaternion::getW() const { +inline decimal Quaternion::getW() const { return w; } // Set the value x (inline) -inline void Quaternion::setX(double x) { +inline void Quaternion::setX(decimal x) { this->x = x; } // Set the value y (inline) -inline void Quaternion::setY(double y) { +inline void Quaternion::setY(decimal y) { this->y = y; } // Set the value z (inline) -inline void Quaternion::setZ(double z) { +inline void Quaternion::setZ(decimal z) { this->z = z; } // Set the value w (inline) -inline void Quaternion::setW(double w) { +inline void Quaternion::setW(decimal w) { this->w = w; } @@ -132,13 +133,13 @@ inline Vector3 Quaternion::vectorV() const { } // Return the length of the quaternion (inline) -inline double Quaternion::length() const { +inline decimal Quaternion::length() const { return sqrt(x*x + y*y + z*z + w*w); } // Return the unit quaternion inline Quaternion Quaternion::getUnit() const { - double lengthQuaternion = length(); + decimal lengthQuaternion = length(); // Check if the length is not equal to zero assert (lengthQuaternion != 0.0); @@ -159,7 +160,7 @@ inline Quaternion Quaternion::getConjugate() const { // Return the inverse of the quaternion (inline) inline Quaternion Quaternion::getInverse() const { - double lengthQuaternion = length(); + decimal lengthQuaternion = length(); lengthQuaternion = lengthQuaternion * lengthQuaternion; assert (lengthQuaternion != 0.0); @@ -169,7 +170,7 @@ inline Quaternion Quaternion::getInverse() const { } // Scalar product between two quaternions -inline double Quaternion::dot(const Quaternion& quaternion) const { +inline decimal Quaternion::dot(const Quaternion& quaternion) const { return (x*quaternion.x + y*quaternion.y + z*quaternion.z + w*quaternion.w); } @@ -186,7 +187,7 @@ inline Quaternion Quaternion::operator-(const Quaternion& quaternion) const { } // Overloaded operator for the multiplication with a constant -inline Quaternion Quaternion::operator*(double nb) const { +inline Quaternion Quaternion::operator*(decimal nb) const { // Return the result return Quaternion(nb*x, nb*y, nb*z, nb*w); } diff --git a/src/mathematics/Transform.h b/src/mathematics/Transform.h index 0e637189..87b34035 100644 --- a/src/mathematics/Transform.h +++ b/src/mathematics/Transform.h @@ -46,22 +46,22 @@ class Transform { Quaternion orientation; // Orientation public : - Transform(); // Constructor + Transform(); // Constructor Transform(const Vector3& position, const Matrix3x3& orientation); // Constructor Transform(const Vector3& position, const Quaternion& orientation); // Constructor - ~Transform(); // Destructor + ~Transform(); // Destructor - const Vector3& getPosition() const; // Return the origin of the transform - void setPosition(const Vector3& position); // Set the origin of the transform + const Vector3& getPosition() const; // Return the origin of the transform + void setPosition(const Vector3& position); // Set the origin of the transform const Quaternion& getOrientation() const; // Return the orientation quaternion void setOrientation(const Quaternion& orientation); // Set the rotation quaternion void setToIdentity(); // Set the transform to the identity transform - void setFromOpenGL(double* openglMatrix); // Set the transform from an OpenGL transform matrix - void getOpenGLMatrix(double* openglMatrix) const; // Get the OpenGL matrix of the transform + void setFromOpenGL(decimal* openglMatrix); // Set the transform from an OpenGL transform matrix + void getOpenGLMatrix(decimal* openglMatrix) const; // Get the OpenGL matrix of the transform Transform inverse() const; // Return the inverse of the transform static Transform interpolateTransforms(const Transform& oldTransform, const Transform& newTransform, - double interpolationFactor); // Return an interpolated transform + decimal interpolationFactor); // Return an interpolated transform Vector3 operator*(const Vector3& vector) const; // Return the transformed vector Transform operator*(const Transform& transform2) const; // Operator of multiplication of a transform with another one @@ -94,7 +94,7 @@ inline void Transform::setToIdentity() { } // Set the transform from an OpenGL transform matrix -inline void Transform::setFromOpenGL(double* openglMatrix) { +inline void Transform::setFromOpenGL(decimal* openglMatrix) { Matrix3x3 matrix(openglMatrix[0], openglMatrix[4], openglMatrix[8], openglMatrix[1], openglMatrix[5], openglMatrix[9], openglMatrix[2], openglMatrix[6], openglMatrix[10]); @@ -103,7 +103,7 @@ inline void Transform::setFromOpenGL(double* openglMatrix) { } // Get the OpenGL matrix of the transform -inline void Transform::getOpenGLMatrix(double* openglMatrix) const { +inline void Transform::getOpenGLMatrix(decimal* openglMatrix) const { const Matrix3x3& matrix = orientation.getMatrix(); openglMatrix[0] = matrix.getValue(0, 0); openglMatrix[1] = matrix.getValue(1, 0); openglMatrix[2] = matrix.getValue(2, 0); openglMatrix[3] = 0.0; openglMatrix[4] = matrix.getValue(0, 1); openglMatrix[5] = matrix.getValue(1, 1); openglMatrix[6] = matrix.getValue(2, 1); openglMatrix[7] = 0.0; @@ -119,7 +119,7 @@ inline Transform Transform::inverse() const { } // Return an interpolated transform -inline Transform Transform::interpolateTransforms(const Transform& oldTransform, const Transform& newTransform, double interpolationFactor) { +inline Transform Transform::interpolateTransforms(const Transform& oldTransform, const Transform& newTransform, decimal interpolationFactor) { Vector3 interPosition = oldTransform.position * (1.0 - interpolationFactor) + newTransform.position * interpolationFactor; Quaternion interOrientation = Quaternion::slerp(oldTransform.orientation, newTransform.orientation, interpolationFactor); return Transform(interPosition, interOrientation); diff --git a/src/mathematics/Vector3.cpp b/src/mathematics/Vector3.cpp index e954b89e..99120c69 100644 --- a/src/mathematics/Vector3.cpp +++ b/src/mathematics/Vector3.cpp @@ -40,7 +40,7 @@ Vector3::Vector3() { } // Constructor with arguments -Vector3::Vector3(double x, double y, double z) { +Vector3::Vector3(decimal x, decimal y, decimal z) { values[0] = x; values[1] = y; values[2] = z; @@ -60,12 +60,12 @@ Vector3::~Vector3() { // Return the corresponding unit vector Vector3 Vector3::getUnit() const { - double lengthVector = length(); + decimal lengthVector = length(); assert(lengthVector != 0.0); // Compute and return the unit vector - double lengthInv = 1.0 / lengthVector; + decimal lengthInv = 1.0 / lengthVector; return Vector3(values[0] * lengthInv, values[1] * lengthInv, values[2] * lengthInv); } @@ -91,6 +91,6 @@ Vector3 Vector3::getOneOrthogonalVector() const { vector1.setZ((-values[0]*values[2]-values[1]*vector1.getY())/values[2]); } - assert(vector1.isUnit()); + //assert(vector1.isUnit()); return vector1; } diff --git a/src/mathematics/Vector3.h b/src/mathematics/Vector3.h index d36ce36b..7c73e5f0 100644 --- a/src/mathematics/Vector3.h +++ b/src/mathematics/Vector3.h @@ -29,6 +29,7 @@ // Libraries #include #include "mathematics_functions.h" +#include "../decimal.h" // ReactPhysics3D namespace @@ -36,32 +37,32 @@ namespace reactphysics3d { /* ------------------------------------------------------------------- Class Vector3 : - This classrepresents 3 dimensionnal vector in space. + This class represents 3D vector in space. ------------------------------------------------------------------- */ class Vector3 { private : - double values[3]; // Values of the 3D vector + decimal values[3]; // Values of the 3D vector public : Vector3(); // Constructor of the class Vector3D - Vector3(double x, double y, double z); // Constructor with arguments + Vector3(decimal x, decimal y, decimal z); // Constructor with arguments Vector3(const Vector3& vector); // Copy-constructor virtual ~Vector3(); // Destructor - double getX() const; // Get the x component of the vector - double getY() const; // Get the y component of the vector - double getZ() const; // Get the z component of the vector - void setX(double x); // Set the x component of the vector - void setY(double y); // Set the y component of the vector - void setZ(double z); // Set the z component of the vector - void setAllValues(double x, double y, double z); // Set all the values of the vector - double length() const; // Return the lenght of the vector - double lengthSquare() const; // Return the square of the length of the vector + decimal getX() const; // Get the x component of the vector + decimal getY() const; // Get the y component of the vector + decimal getZ() const; // Get the z component of the vector + void setX(decimal x); // Set the x component of the vector + void setY(decimal y); // Set the y component of the vector + void setZ(decimal z); // Set the z component of the vector + void setAllValues(decimal x, decimal y, decimal z); // Set all the values of the vector + decimal length() const; // Return the lenght of the vector + decimal lengthSquare() const; // Return the square of the length of the vector Vector3 getUnit() const; // Return the corresponding unit vector bool isUnit() const; // Return true if the vector is unit and false otherwise bool isZero() const; // Return true if the current vector is the zero vector Vector3 getOneOrthogonalVector() const; // Return one unit orthogonal vectors of the current vector - double dot(const Vector3& vector) const; // Dot product of two vectors + decimal dot(const Vector3& vector) const; // Dot product of two vectors Vector3 cross(const Vector3& vector) const; // Cross product of two vectors Vector3 getAbsoluteVector() const; // Return the corresponding absolute value vector int getMinAxis() const; // Return the axis with the minimal value @@ -73,68 +74,68 @@ class Vector3 { bool operator!= (const Vector3& vector) const; // Overloaded operator for the is different condition Vector3& operator+=(const Vector3& vector); // Overloaded operator for addition with assignment Vector3& operator-=(const Vector3& vector); // Overloaded operator for substraction with assignment - Vector3& operator*=(double number); // Overloaded operator for multiplication with a number with assignment - double& operator[] (int index); // Overloaded operator for value access - const double& operator[] (int index) const; // Overloaded operator for value access + Vector3& operator*=(decimal number); // Overloaded operator for multiplication with a number with assignment + decimal& operator[] (int index); // Overloaded operator for value access + const decimal& operator[] (int index) const; // Overloaded operator for value access // Friend functions friend Vector3 operator+(const Vector3& vector1, const Vector3& vector2); friend Vector3 operator-(const Vector3& vector1, const Vector3& vector2); friend Vector3 operator-(const Vector3& vector); - friend Vector3 operator*(const Vector3& vector, double number); - friend Vector3 operator*(double number, const Vector3& vector); + friend Vector3 operator*(const Vector3& vector, decimal number); + friend Vector3 operator*(decimal number, const Vector3& vector); }; // Get the x component of the vector -inline double Vector3::getX() const { +inline decimal Vector3::getX() const { return values[0]; } // Get the y component of the vector -inline double Vector3::getY() const { +inline decimal Vector3::getY() const { return values[1]; } // Get the z component of the vector -inline double Vector3::getZ() const { +inline decimal Vector3::getZ() const { return values[2]; } // Set the x component of the vector -inline void Vector3::setX(double x) { +inline void Vector3::setX(decimal x) { this->values[0] = x; } // Set the y component of the vector -inline void Vector3::setY(double y) { +inline void Vector3::setY(decimal y) { this->values[1] = y; } // Set the z component of the vector -inline void Vector3::setZ(double z) { +inline void Vector3::setZ(decimal z) { this->values[2] = z; } // Set all the values of the vector (inline) -inline void Vector3::setAllValues(double x, double y, double z) { +inline void Vector3::setAllValues(decimal x, decimal y, decimal z) { values[0]= x; values[1] = y; values[2] = z; } // Return the length of the vector (inline) -inline double Vector3::length() const { +inline decimal Vector3::length() const { // Compute and return the length of the vector return sqrt(values[0]*values[0] + values[1]*values[1] + values[2]*values[2]); } // Return the square of the length of the vector -inline double Vector3::lengthSquare() const { +inline decimal Vector3::lengthSquare() const { return values[0]*values[0] + values[1]*values[1] + values[2]*values[2]; } // Scalar product of two vectors (inline) -inline double Vector3::dot(const Vector3& vector) const { +inline decimal Vector3::dot(const Vector3& vector) const { // Compute and return the result of the scalar product return (values[0] * vector.values[0] + values[1] * vector.values[1] + values[2] * vector.values[2]); } @@ -154,7 +155,7 @@ inline Vector3 Vector3::getAbsoluteVector() const { // Return true if two vectors are parallel inline bool Vector3::isParallelWith(const Vector3& vector) const { - double scalarProd = this->dot(vector); + decimal scalarProd = this->dot(vector); return approxEqual(std::abs(scalarProd), length() * vector.length()); } @@ -206,7 +207,7 @@ inline Vector3& Vector3::operator-=(const Vector3& vector) { } // Overloaded operator for multiplication with a number with assignment -inline Vector3& Vector3::operator*=(double number) { +inline Vector3& Vector3::operator*=(decimal number) { values[0] *= number; values[1] *= number; values[2] *= number; @@ -214,12 +215,12 @@ inline Vector3& Vector3::operator*=(double number) { } // Overloaded operator for value access -inline double& Vector3::operator[] (int index) { +inline decimal& Vector3::operator[] (int index) { return values[index]; } // Overloaded operator for value access -inline const double& Vector3::operator[] (int index) const { +inline const decimal& Vector3::operator[] (int index) const { return values[index]; } @@ -239,12 +240,12 @@ inline Vector3 operator-(const Vector3& vector) { } // Overloaded operator for multiplication with a number -inline Vector3 operator*(const Vector3& vector, double number) { +inline Vector3 operator*(const Vector3& vector, decimal number) { return Vector3(number * vector.values[0], number * vector.values[1], number * vector.values[2]); } // Overloaded operator for multiplication with a number -inline Vector3 operator*(double number, const Vector3& vector) { +inline Vector3 operator*(decimal number, const Vector3& vector) { return vector * number; } diff --git a/src/mathematics/mathematics.h b/src/mathematics/mathematics.h index 26453462..7b3cae1b 100644 --- a/src/mathematics/mathematics.h +++ b/src/mathematics/mathematics.h @@ -31,7 +31,7 @@ #include "Quaternion.h" #include "Vector3.h" #include "Transform.h" -#include "../constants.h" +#include "../configuration.h" #include "mathematics_functions.h" #include #include @@ -61,15 +61,15 @@ inline reactphysics3d::Vector3 rotateVectorWithQuaternion(const reactphysics3d:: // P1 = point1 + alpha * d1 // P2 = point2 + beta * d2 inline void closestPointsBetweenTwoLines(const reactphysics3d::Vector3& point1, const reactphysics3d::Vector3& d1, const reactphysics3d::Vector3& point2, - const reactphysics3d::Vector3& d2, double* alpha, double* beta) { + const reactphysics3d::Vector3& d2, decimal* alpha, decimal* beta) { reactphysics3d::Vector3 r = point1 - point2; - double a = d1.dot(d1); - double b = d1.dot(d2); - double c = d1.dot(r); - double e = d2.dot(d2); - double f = d2.dot(r); - double d = a*e-b*b; + decimal a = d1.dot(d1); + decimal b = d1.dot(d2); + decimal c = d1.dot(r); + decimal e = d2.dot(d2); + decimal f = d2.dot(r); + decimal d = a*e-b*b; // The two lines must not be parallel assert(!reactphysics3d::approxEqual(d, 0.0)); @@ -90,11 +90,11 @@ inline bool isPointOnSegment(const reactphysics3d::Vector3& segPointA, const rea } // Compute the length of the segment - double segmentLength = d.length(); + decimal segmentLength = d.length(); // Compute the distance from point "P" to points "segPointA" and "segPointB" - double distA = dP.length(); - double distB = (P - segPointB).length(); + decimal distA = dP.length(); + decimal distB = (P - segPointB).length(); // If one of the "distA" and "distB" is greather than the length of the segment, then P is not on the segment if (distA > segmentLength || distB > segmentLength) { @@ -111,7 +111,7 @@ inline bool isPointOnSegment(const reactphysics3d::Vector3& segPointA, const rea inline reactphysics3d::Vector3 computeLinesIntersection(const reactphysics3d::Vector3& p1, const reactphysics3d::Vector3& d1, const reactphysics3d::Vector3& p2, const reactphysics3d::Vector3& d2) { // Computes the two closest points on the lines - double alpha, beta; + decimal alpha, beta; closestPointsBetweenTwoLines(p1, d1, p2, d2, &alpha, &beta); reactphysics3d::Vector3 point1 = p1 + alpha * d1; reactphysics3d::Vector3 point2 = p2 + beta * d2; @@ -136,7 +136,7 @@ inline reactphysics3d::Vector3 computeNonParallelSegmentsIntersection(const reac assert(!d1.isParallelWith(d2)); // Compute the closet points between the two lines - double alpha, beta; + decimal alpha, beta; closestPointsBetweenTwoLines(seg1PointA, d1, seg2PointA, d2, &alpha, &beta); reactphysics3d::Vector3 point1 = seg1PointA + alpha * d1; reactphysics3d::Vector3 point2 = seg2PointA + beta * d2; @@ -147,7 +147,8 @@ inline reactphysics3d::Vector3 computeNonParallelSegmentsIntersection(const reac // If the two closest point aren't very close, there is no intersection between the segments reactphysics3d::Vector3 d = point2 - point1; - assert(d.length() <= EPSILON_TEST); + const decimal epsilon = 0.00001; + assert(d.length() <= epsilon); // They are very close so we return the intersection point (halfway between "point1" and "point2" return 0.5 * (point1 + point2); @@ -192,7 +193,7 @@ inline std::vector projectPointsOntoPlane(const std::ve // Compute the distance between a point "P" and a line (given by a point "A" and a vector "v") -inline double computeDistanceBetweenPointAndLine(const reactphysics3d::Vector3& P, const reactphysics3d::Vector3& A, const reactphysics3d::Vector3& v) { +inline decimal computeDistanceBetweenPointAndLine(const reactphysics3d::Vector3& P, const reactphysics3d::Vector3& A, const reactphysics3d::Vector3& v) { assert(v.length() != 0); return ((P-A).cross(v).length() / (v.length())); } @@ -208,12 +209,12 @@ inline reactphysics3d::Vector3 computeOrthogonalProjectionOfPointOntoALine(const // The result point Q will be in a segment of the rectangle inline reactphysics3d::Vector3 computeNearestPointOnRectangle(const reactphysics3d::Vector3& P, const std::vector rectangle) { assert(rectangle.size() == 4); - double distPSegment1 = computeDistanceBetweenPointAndLine(P, rectangle[0], rectangle[1] - rectangle[0]); - double distPSegment2 = computeDistanceBetweenPointAndLine(P, rectangle[1], rectangle[2] - rectangle[1]); - double distPSegment3 = computeDistanceBetweenPointAndLine(P, rectangle[2], rectangle[3] - rectangle[2]); - double distPSegment4 = computeDistanceBetweenPointAndLine(P, rectangle[3], rectangle[0] - rectangle[3]); - double distSegment1Segment3 = computeDistanceBetweenPointAndLine(rectangle[0], rectangle[3], rectangle[3] - rectangle[2]); - double distSegment2Segment4 = computeDistanceBetweenPointAndLine(rectangle[1], rectangle[3], rectangle[0] - rectangle[3]); + decimal distPSegment1 = computeDistanceBetweenPointAndLine(P, rectangle[0], rectangle[1] - rectangle[0]); + decimal distPSegment2 = computeDistanceBetweenPointAndLine(P, rectangle[1], rectangle[2] - rectangle[1]); + decimal distPSegment3 = computeDistanceBetweenPointAndLine(P, rectangle[2], rectangle[3] - rectangle[2]); + decimal distPSegment4 = computeDistanceBetweenPointAndLine(P, rectangle[3], rectangle[0] - rectangle[3]); + decimal distSegment1Segment3 = computeDistanceBetweenPointAndLine(rectangle[0], rectangle[3], rectangle[3] - rectangle[2]); + decimal distSegment2Segment4 = computeDistanceBetweenPointAndLine(rectangle[1], rectangle[3], rectangle[0] - rectangle[3]); Vector3 resultPoint; // Check if P is between the lines of the first pair of parallel segments of the rectangle @@ -280,8 +281,8 @@ inline void computeParallelSegmentsIntersection(const reactphysics3d::Vector3& s assert(d1.isParallelWith(d2)); // Compute the projection of the two points of the second segment onto the vector of segment 1 - double projSeg2PointA = d1.getUnit().dot(seg2PointA - seg1PointA); - double projSeg2PointB = d1.getUnit().dot(seg2PointB - seg1PointA); + decimal projSeg2PointA = d1.getUnit().dot(seg2PointA - seg1PointA); + decimal projSeg2PointB = d1.getUnit().dot(seg2PointB - seg1PointA); // The projections intervals should intersect assert(!(projSeg2PointA < 0.0 && projSeg2PointB < 0.0)); @@ -319,7 +320,7 @@ inline void computeParallelSegmentsIntersection(const reactphysics3d::Vector3& s // The segment is given by the two vertices in "segment" and the rectangle is given by the ordered vertices in "clipRectangle". // This method returns the clipped segment. inline std::vector clipSegmentWithRectangleInPlane(const std::vector& segment, const std::vector clipRectangle) { - double const epsilon = 0.01; + decimal const epsilon = 0.01; assert(segment.size() == 2); assert(clipRectangle.size() == 4); @@ -375,7 +376,7 @@ inline std::vector clipSegmentWithRectangleInPlane(cons // a rectangle polygon (given by the ordered 3D vertices in "clipRectangle"). The subject polygon and the clip rectangle are in 3D but we assumed that // they are on a same plane in 3D. The method returns the ordered 3D vertices of the subject polygon clipped using the rectangle polygon. inline std::vector clipPolygonWithRectangleInPlane(const std::vector& subjectPolygon, const std::vector& clipRectangle) { - double const epsilon = 0.1; + decimal const epsilon = 0.1; assert(clipRectangle.size() == 4); std::vector outputPolygon; @@ -432,10 +433,10 @@ inline reactphysics3d::Vector3 intersectLineWithPlane(const reactphysics3d::Vect assert(!approxEqual(lineVector.dot(planeNormal), 0.0)); // The plane is represented by the equation planeNormal dot X = d where X is a point of the plane - double d = planeNormal.dot(planePoint); + decimal d = planeNormal.dot(planePoint); // Compute the parameter t - double t = (d - planeNormal.dot(linePoint)) / planeNormal.dot(lineVector); + decimal t = (d - planeNormal.dot(linePoint)) / planeNormal.dot(lineVector); // Compute the intersection point return linePoint + lineVector * t; diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h index 5e6d0e11..0fcb40ee 100644 --- a/src/mathematics/mathematics_functions.h +++ b/src/mathematics/mathematics_functions.h @@ -27,7 +27,8 @@ #define MATHEMATICS_FUNCTIONS_H // Libraries -#include "../constants.h" +#include "../configuration.h" +#include "../decimal.h" // ReactPhysics3D namespace namespace reactphysics3d { @@ -36,8 +37,8 @@ namespace reactphysics3d { // function to test if two real numbers are (almost) equal // We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON] -inline bool approxEqual(double a, double b, double epsilon = EPSILON) { - double difference = a - b; +inline bool approxEqual(decimal a, decimal b, decimal epsilon = 1.0e-10) { + decimal difference = a - b; return (difference < epsilon && difference > -epsilon); } diff --git a/src/memory/MemoryPool.h b/src/memory/MemoryPool.h index d40050e2..1e96f128 100644 --- a/src/memory/MemoryPool.h +++ b/src/memory/MemoryPool.h @@ -27,7 +27,7 @@ #define MEMORY_POOL_H // Libraries -#include "../constants.h" +#include "../configuration.h" #include #include #include @@ -115,6 +115,10 @@ MemoryPool::MemoryPool(uint nbObjectsToAllocate) throw(std::bad_alloc) // Deallocate the block of memory that has been allocated previously template MemoryPool::~MemoryPool() { + + // Check if we have a memory leak + assert(currentNbObjects == 0); + // Release the whole memory block free(pMemoryBlock); } diff --git a/src/reactphysics3d.h b/src/reactphysics3d.h index f0a101b5..38733190 100644 --- a/src/reactphysics3d.h +++ b/src/reactphysics3d.h @@ -40,11 +40,11 @@ #include "body/RigidBody.h" #include "engine/PhysicsWorld.h" #include "engine/PhysicsEngine.h" -#include "shapes/Shape.h" -#include "shapes/BoxShape.h" -#include "shapes/SphereShape.h" -#include "shapes/ConeShape.h" -#include "shapes/CylinderShape.h" +#include "shapes/Collider.h" +#include "shapes/BoxCollider.h" +#include "shapes/SphereCollider.h" +#include "shapes/ConeCollider.h" +#include "shapes/CylinderCollider.h" #include "shapes/AABB.h" // Alias to the ReactPhysics3D namespace diff --git a/src/shapes/AABB.h b/src/shapes/AABB.h index ac222586..ecb983d5 100644 --- a/src/shapes/AABB.h +++ b/src/shapes/AABB.h @@ -47,22 +47,22 @@ class AABB { private : Vector3 minCoordinates; // Minimum world coordinates of the AABB on the x,y and z axis Vector3 maxCoordinates; // Maximum world coordinates of the AABB on the x,y and z axis - Body* bodyPointer; // Pointer to the owner body (not the abstract class Body but its derivative which is instanciable) + Body* bodyPointer; // Pointer to the owner body (not the abstract class Body but its derivative which is instanciable) public : - AABB(); // Constructor + AABB(); // Constructor AABB(const Transform& transform, const Vector3& extents); // Constructor - virtual ~AABB(); // Destructor + virtual ~AABB(); // Destructor Vector3 getCenter() const; // Return the center point const Vector3& getMinCoordinates() const; // Return the minimum coordinates of the AABB const Vector3& getMaxCoordinates() const; // Return the maximum coordinates of the AABB - Body* getBodyPointer() const; // Return a pointer to the owner body - void setBodyPointer(Body* bodyPointer); // Set the body pointer - bool testCollision(const AABB& aabb) const; // Return true if the current AABB is overlapping is the AABB in argument + Body* getBodyPointer() const; // Return a pointer to the owner body + void setBodyPointer(Body* bodyPointer); // Set the body pointer + bool testCollision(const AABB& aabb) const; // Return true if the current AABB is overlapping is the AABB in argument virtual void update(const Transform& newTransform, const Vector3& extents); // Update the oriented bounding box orientation according to a new orientation of the rigid body #ifdef VISUAL_DEBUG - virtual void draw() const; // Draw the AABB (only for testing purpose) + virtual void draw() const; // Draw the AABB (only for testing purpose) #endif }; diff --git a/src/shapes/BoxShape.cpp b/src/shapes/BoxCollider.cpp similarity index 85% rename from src/shapes/BoxShape.cpp rename to src/shapes/BoxCollider.cpp index 6ed7fc1c..7e355500 100644 --- a/src/shapes/BoxShape.cpp +++ b/src/shapes/BoxCollider.cpp @@ -24,7 +24,7 @@ ********************************************************************************/ // Libraries -#include "BoxShape.h" +#include "BoxCollider.h" #include "../configuration.h" #include #include @@ -46,21 +46,21 @@ using namespace reactphysics3d; using namespace std; // Constructor -BoxShape::BoxShape(const Vector3& extent) : extent(extent) { +BoxCollider::BoxCollider(const Vector3& extent) : extent(extent) { } // Destructor -BoxShape::~BoxShape() { +BoxCollider::~BoxCollider() { } -// Return the local inertia tensor of the shape -void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const { - double factor = (1.0 / 3.0) * mass; - double xSquare = extent.getX() * extent.getX(); - double ySquare = extent.getY() * extent.getY(); - double zSquare = extent.getZ() * extent.getZ(); +// Return the local inertia tensor of the collider +void BoxCollider::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { + decimal factor = (1.0 / 3.0) * mass; + decimal xSquare = extent.getX() * extent.getX(); + decimal ySquare = extent.getY() * extent.getY(); + decimal zSquare = extent.getZ() * extent.getZ(); tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0, 0.0, factor * (xSquare + zSquare), 0.0, 0.0, 0.0, factor * (xSquare + ySquare)); @@ -68,10 +68,10 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const { #ifdef VISUAL_DEBUG // Draw the Box (only for testing purpose) -void BoxShape::draw() const { - double e1 = extent.getX(); - double e2 = extent.getY(); - double e3 = extent.getZ(); +void BoxCollider::draw() const { + decimal e1 = extent.getX(); + decimal e2 = extent.getY(); + decimal e3 = extent.getZ(); // Draw in red glColor3f(1.0, 0.0, 0.0); diff --git a/src/shapes/BoxShape.h b/src/shapes/BoxCollider.h similarity index 79% rename from src/shapes/BoxShape.h rename to src/shapes/BoxCollider.h index d1b20355..43fbc320 100644 --- a/src/shapes/BoxShape.h +++ b/src/shapes/BoxCollider.h @@ -23,12 +23,12 @@ * * ********************************************************************************/ -#ifndef BOX_SHAPE_H -#define BOX_SHAPE_H +#ifndef BOX_COLLIDER_H +#define BOX_COLLIDER_H // Libraries #include -#include "Shape.h" +#include "Collider.h" #include "../mathematics/mathematics.h" @@ -36,26 +36,26 @@ namespace reactphysics3d { /* ------------------------------------------------------------------- - Class BoxShape : - This class represents a 3D box. Those axis are unit length. + Class BoxCollider : + This class represents a 3D box collider. Those axis are unit length. The three extents are half-widths of the box along the three axis x, y, z local axis. The "transform" of the corresponding rigid body gives an orientation and a position to the box. ------------------------------------------------------------------- */ -class BoxShape : public Shape { +class BoxCollider : public Collider { private : Vector3 extent; // Extent sizes of the box in the x, y and z direction public : - BoxShape(const Vector3& extent); // Constructor - virtual ~BoxShape(); // Destructor + BoxCollider(const Vector3& extent); // Constructor + virtual ~BoxCollider(); // Destructor const Vector3& getExtent() const; // Return the extents of the box void setExtent(const Vector3& extent); // Set the extents of the box - virtual Vector3 getLocalExtents(double margin=0.0) const; // Return the local extents in x,y and z direction - virtual Vector3 getLocalSupportPoint(const Vector3& direction, double margin=0.0) const; // Return a local support point in a given direction - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const; // Return the local inertia tensor of the shape + virtual Vector3 getLocalExtents(decimal margin=0.0) const; // Return the local extents in x,y and z direction + virtual Vector3 getLocalSupportPoint(const Vector3& direction, decimal margin=0.0) const; // Return a local support point in a given direction + virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; // Return the local inertia tensor of the collider #ifdef VISUAL_DEBUG virtual void draw() const; // Draw the Box (only for testing purpose) @@ -63,23 +63,23 @@ class BoxShape : public Shape { }; // Return the extents of the box -inline const Vector3& BoxShape::getExtent() const { +inline const Vector3& BoxCollider::getExtent() const { return extent; } // Set the extents of the box -inline void BoxShape::setExtent(const Vector3& extent) { +inline void BoxCollider::setExtent(const Vector3& extent) { this->extent = extent; } -// Return the local extents of the shape (half-width) in x,y and z local direction +// Return the local extents of the box (half-width) in x,y and z local direction // This method is used to compute the AABB of the box -inline Vector3 BoxShape::getLocalExtents(double margin) const { +inline Vector3 BoxCollider::getLocalExtents(decimal margin) const { return extent + Vector3(margin, margin, margin); } // Return a local support point in a given direction -inline Vector3 BoxShape::getLocalSupportPoint(const Vector3& direction, double margin) const { +inline Vector3 BoxCollider::getLocalSupportPoint(const Vector3& direction, decimal margin) const { assert(margin >= 0.0); return Vector3(direction.getX() < 0.0 ? -extent.getX()-margin : extent.getX()+margin, diff --git a/src/shapes/Shape.cpp b/src/shapes/Collider.cpp similarity index 96% rename from src/shapes/Shape.cpp rename to src/shapes/Collider.cpp index e90b73db..a6450553 100644 --- a/src/shapes/Shape.cpp +++ b/src/shapes/Collider.cpp @@ -24,17 +24,17 @@ ********************************************************************************/ // Libraries -#include "Shape.h" +#include "Collider.h" // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; // Constructor -Shape::Shape() { +Collider::Collider() { } // Destructor -Shape::~Shape() { +Collider::~Collider() { } diff --git a/src/shapes/Shape.h b/src/shapes/Collider.h similarity index 81% rename from src/shapes/Shape.h rename to src/shapes/Collider.h index 8b02cb57..1dafa038 100644 --- a/src/shapes/Shape.h +++ b/src/shapes/Collider.h @@ -23,8 +23,8 @@ * * ********************************************************************************/ -#ifndef SHAPE_H -#define SHAPE_H +#ifndef COLLIDER_H +#define COLLIDER_H // Libraries #include @@ -38,34 +38,34 @@ namespace reactphysics3d { class Body; /* ------------------------------------------------------------------- - Class Shape : - This abstract class represents the shape of a body that is used during - the narrow-phase collision detection. + Class Collider : + This abstract class represents the collider associated with a + body that is used during the narrow-phase collision detection. ------------------------------------------------------------------- */ -class Shape { +class Collider { protected : Body* bodyPointer; // Pointer to the owner body (not the abstract class Body but its derivative which is instanciable) public : - Shape(); // Constructor - virtual ~Shape(); // Destructor + Collider(); // Constructor + virtual ~Collider(); // Destructor Body* getBodyPointer() const; // Return the body pointer void setBodyPointer(Body* bodyPointer); // Set the body pointer - virtual Vector3 getLocalSupportPoint(const Vector3& direction, double margin=0.0) const=0; // Return a local support point in a given direction - virtual Vector3 getLocalExtents(double margin=0.0) const=0; // Return the local extents in x,y and z direction - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const=0; // Return the local inertia tensor of the shape + virtual Vector3 getLocalSupportPoint(const Vector3& direction, decimal margin=0.0) const=0; // Return a local support point in a given direction + virtual Vector3 getLocalExtents(decimal margin=0.0) const=0; // Return the local extents in x,y and z direction + virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const=0; // Return the local inertia tensor of the collider }; // Return the body pointer -inline Body* Shape::getBodyPointer() const { +inline Body* Collider::getBodyPointer() const { assert(bodyPointer != 0); return bodyPointer; } // Set the body pointer -inline void Shape::setBodyPointer(Body* bodyPointer) { +inline void Collider::setBodyPointer(Body* bodyPointer) { this->bodyPointer = bodyPointer; } diff --git a/src/shapes/ConeShape.cpp b/src/shapes/ConeCollider..cpp similarity index 87% rename from src/shapes/ConeShape.cpp rename to src/shapes/ConeCollider..cpp index 8fa46002..9bc4be26 100644 --- a/src/shapes/ConeShape.cpp +++ b/src/shapes/ConeCollider..cpp @@ -26,7 +26,7 @@ // Libraries #include #include "../configuration.h" -#include "ConeShape.h" +#include "ConeCollider.h" #if defined(VISUAL_DEBUG) #if defined(APPLE_OS) @@ -44,7 +44,7 @@ using namespace reactphysics3d; // Constructor -ConeShape::ConeShape(double radius, double height) : radius(radius), halfHeight(height/2.0) { +ConeCollider::ConeCollider(decimal radius, decimal height) : radius(radius), halfHeight(height/2.0) { assert(radius > 0.0); assert(halfHeight > 0.0); @@ -53,25 +53,25 @@ ConeShape::ConeShape(double radius, double height) : radius(radius), halfHeight( } // Destructor -ConeShape::~ConeShape() { +ConeCollider::~ConeCollider() { } // Return a local support point in a given direction -inline Vector3 ConeShape::getLocalSupportPoint(const Vector3& direction, double margin) const { +inline Vector3 ConeCollider::getLocalSupportPoint(const Vector3& direction, decimal margin) const { assert(margin >= 0.0); const Vector3& v = direction; - double sinThetaTimesLengthV = sinTheta * v.length(); + decimal sinThetaTimesLengthV = sinTheta * v.length(); Vector3 supportPoint; if (v.getY() >= sinThetaTimesLengthV) { supportPoint = Vector3(0.0, halfHeight, 0.0); } else { - double projectedLength = sqrt(v.getX() * v.getX() + v.getZ() * v.getZ()); + decimal projectedLength = sqrt(v.getX() * v.getX() + v.getZ() * v.getZ()); if (projectedLength > MACHINE_EPSILON) { - double d = radius / projectedLength; + decimal d = radius / projectedLength; supportPoint = Vector3(v.getX() * d, -halfHeight, v.getZ() * d); } else { @@ -93,7 +93,7 @@ inline Vector3 ConeShape::getLocalSupportPoint(const Vector3& direction, double #ifdef VISUAL_DEBUG // Draw the cone (only for debuging purpose) -void ConeShape::draw() const { +void ConeCollider::draw() const { // Draw in red glColor3f(1.0, 0.0, 0.0); diff --git a/src/shapes/ConeShape.h b/src/shapes/ConeCollider.h similarity index 65% rename from src/shapes/ConeShape.h rename to src/shapes/ConeCollider.h index 1e01ca1d..a716c4a4 100644 --- a/src/shapes/ConeShape.h +++ b/src/shapes/ConeCollider.h @@ -23,19 +23,19 @@ * * ********************************************************************************/ -#ifndef CONE_SHAPE_H -#define CONE_SHAPE_H +#ifndef CONE_COLLIDER_H +#define CONE_COLLIDER_H // Libraries -#include "Shape.h" +#include "Collider.h" #include "../mathematics/mathematics.h" // ReactPhysics3D namespace namespace reactphysics3d { /* ------------------------------------------------------------------- - Class ConeShape : - This class represents a cone collision shape centered at the + Class ConeCollider : + This class represents a cone collider centered at the origin and alligned with the Y axis. The cone is defined by its height and by the radius of its base. The center of the cone is at the half of the height. The "transform" of the @@ -43,23 +43,23 @@ namespace reactphysics3d { to the cone. ------------------------------------------------------------------- */ -class ConeShape : public Shape { +class ConeCollider : public Collider { private : - double radius; // Radius of the base - double halfHeight; // Half height of the cone - double sinTheta; // sine of the semi angle at the apex point + decimal radius; // Radius of the base + decimal halfHeight; // Half height of the cone + decimal sinTheta; // sine of the semi angle at the apex point public : - ConeShape(double radius, double height); // Constructor - virtual ~ConeShape(); // Destructor + ConeCollider(decimal radius, decimal height); // Constructor + virtual ~ConeCollider(); // Destructor - double getRadius() const; // Return the radius - void setRadius(double radius); // Set the radius - double getHeight() const; // Return the height - void setHeight(double height); // Set the height - virtual Vector3 getLocalSupportPoint(const Vector3& direction, double margin=0.0) const; // Return a support point in a given direction - virtual Vector3 getLocalExtents(double margin=0.0) const; // Return the local extents in x,y and z direction - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const; // Return the local inertia tensor of the shape + decimal getRadius() const; // Return the radius + void setRadius(decimal radius); // Set the radius + decimal getHeight() const; // Return the height + void setHeight(decimal height); // Set the height + virtual Vector3 getLocalSupportPoint(const Vector3& direction, decimal margin=0.0) const; // Return a support point in a given direction + virtual Vector3 getLocalExtents(decimal margin=0.0) const; // Return the local extents in x,y and z direction + virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; // Return the local inertia tensor of the collider #ifdef VISUAL_DEBUG @@ -68,12 +68,12 @@ class ConeShape : public Shape { }; // Return the radius -inline double ConeShape::getRadius() const { +inline decimal ConeCollider::getRadius() const { return radius; } // Set the radius -inline void ConeShape::setRadius(double radius) { +inline void ConeCollider::setRadius(decimal radius) { this->radius = radius; // Update sine of the semi-angle at the apex point @@ -81,12 +81,12 @@ inline void ConeShape::setRadius(double radius) { } // Return the height -inline double ConeShape::getHeight() const { +inline decimal ConeCollider::getHeight() const { return 2.0 * halfHeight; } // Set the height -inline void ConeShape::setHeight(double height) { +inline void ConeCollider::setHeight(decimal height) { this->halfHeight = height / 2.0; // Update the sine of the semi-angle at the apex point @@ -94,14 +94,14 @@ inline void ConeShape::setHeight(double height) { } // Return the local extents in x,y and z direction -inline Vector3 ConeShape::getLocalExtents(double margin) const { +inline Vector3 ConeCollider::getLocalExtents(decimal margin) const { return Vector3(radius + margin, halfHeight + margin, radius + margin); } -// Return the local inertia tensor of the shape -inline void ConeShape::computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const { - double rSquare = radius * radius; - double diagXZ = 0.15 * mass * (rSquare + halfHeight); +// Return the local inertia tensor of the collider +inline void ConeCollider::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { + decimal rSquare = radius * radius; + decimal diagXZ = 0.15 * mass * (rSquare + halfHeight); tensor.setAllValues(diagXZ, 0.0, 0.0, 0.0, 0.3 * mass * rSquare, 0.0, 0.0, 0.0, diagXZ); } diff --git a/src/shapes/CylinderShape.cpp b/src/shapes/CylinderCollider.cpp similarity index 87% rename from src/shapes/CylinderShape.cpp rename to src/shapes/CylinderCollider.cpp index 03adfc33..001b15c8 100644 --- a/src/shapes/CylinderShape.cpp +++ b/src/shapes/CylinderCollider.cpp @@ -24,7 +24,7 @@ ********************************************************************************/ // Libraries -#include "CylinderShape.h" +#include "CylinderCollider.h" #include "../configuration.h" #if defined(VISUAL_DEBUG) @@ -43,23 +43,23 @@ using namespace reactphysics3d; // Constructor -CylinderShape::CylinderShape(double radius, double height) : radius(radius), halfHeight(height/2.0) { +CylinderCollider::CylinderCollider(decimal radius, decimal height) : radius(radius), halfHeight(height/2.0) { } // Destructor -CylinderShape::~CylinderShape() { +CylinderCollider::~CylinderCollider() { } // Return a local support point in a given direction -Vector3 CylinderShape::getLocalSupportPoint(const Vector3& direction, double margin) const { +Vector3 CylinderCollider::getLocalSupportPoint(const Vector3& direction, decimal margin) const { assert(margin >= 0.0); Vector3 supportPoint(0.0, 0.0, 0.0); - double uDotv = direction.getY(); + decimal uDotv = direction.getY(); Vector3 w(direction.getX(), 0.0, direction.getZ()); - double lengthW = sqrt(direction.getX() * direction.getX() + direction.getZ() * direction.getZ()); + decimal lengthW = sqrt(direction.getX() * direction.getX() + direction.getZ() * direction.getZ()); if (lengthW != 0.0) { if (uDotv < 0.0) supportPoint.setY(-halfHeight); @@ -85,7 +85,7 @@ Vector3 CylinderShape::getLocalSupportPoint(const Vector3& direction, double mar #ifdef VISUAL_DEBUG // Draw the cone (only for debuging purpose) -void CylinderShape::draw() const { +void CylinderCollider::draw() const { // Draw in red glColor3f(1.0, 0.0, 0.0); diff --git a/src/shapes/CylinderShape.h b/src/shapes/CylinderCollider.h similarity index 64% rename from src/shapes/CylinderShape.h rename to src/shapes/CylinderCollider.h index 70d59ce2..e4559bab 100644 --- a/src/shapes/CylinderShape.h +++ b/src/shapes/CylinderCollider.h @@ -23,11 +23,11 @@ * * ********************************************************************************/ -#ifndef CYLINDER_SHAPE_H -#define CYLINDER_SHAPE_H +#ifndef CYLINDER_COLLIDER_H +#define CYLINDER_COLLIDER_H // Libraries -#include "Shape.h" +#include "Collider.h" #include "../mathematics/mathematics.h" @@ -35,29 +35,29 @@ namespace reactphysics3d { /* ------------------------------------------------------------------- - Class CylinderShape : - This class represents a cylinder collision shape around the Y axis + Class CylinderCollider : + This class represents a cylinder collision collider around the Y axis and centered at the origin. The cylinder is defined by its height and the radius of its base. The "transform" of the corresponding rigid body gives an orientation and a position to the cylinder. ------------------------------------------------------------------- */ -class CylinderShape : public Shape { +class CylinderCollider : public Collider { private : - double radius; // Radius of the base - double halfHeight; // Half height of the cone + decimal radius; // Radius of the base + decimal halfHeight; // Half height of the cone public : - CylinderShape(double radius, double height); // Constructor - virtual ~CylinderShape(); // Destructor + CylinderCollider(decimal radius, decimal height); // Constructor + virtual ~CylinderCollider(); // Destructor - double getRadius() const; // Return the radius - void setRadius(double radius); // Set the radius - double getHeight() const; // Return the height - void setHeight(double height); // Set the height - virtual Vector3 getLocalSupportPoint(const Vector3& direction, double margin=0.0) const; // Return a support point in a given direction - virtual Vector3 getLocalExtents(double margin=0.0) const; // Return the local extents in x,y and z direction - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const; // Return the local inertia tensor of the shape + decimal getRadius() const; // Return the radius + void setRadius(decimal radius); // Set the radius + decimal getHeight() const; // Return the height + void setHeight(decimal height); // Set the height + virtual Vector3 getLocalSupportPoint(const Vector3& direction, decimal margin=0.0) const; // Return a support point in a given direction + virtual Vector3 getLocalExtents(decimal margin=0.0) const; // Return the local extents in x,y and z direction + virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; // Return the local inertia tensor of the collider #ifdef VISUAL_DEBUG virtual void draw() const; // Draw the sphere (only for testing purpose) @@ -65,34 +65,34 @@ class CylinderShape : public Shape { }; // Return the radius -inline double CylinderShape::getRadius() const { +inline decimal CylinderCollider::getRadius() const { return radius; } // Set the radius -inline void CylinderShape::setRadius(double radius) { +inline void CylinderCollider::setRadius(decimal radius) { this->radius = radius; } // Return the height -inline double CylinderShape::getHeight() const { +inline decimal CylinderCollider::getHeight() const { return halfHeight * 2.0; } // Set the height -inline void CylinderShape::setHeight(double height) { +inline void CylinderCollider::setHeight(decimal height) { this->halfHeight = height / 2.0; } // Return the local extents in x,y and z direction -inline Vector3 CylinderShape::getLocalExtents(double margin) const { +inline Vector3 CylinderCollider::getLocalExtents(decimal margin) const { return Vector3(radius + margin, halfHeight + margin, radius + margin); } // Return the local inertia tensor of the cylinder -inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const { - double height = 2.0 * halfHeight; - double diag = (1.0 / 12.0) * mass * (3 * radius * radius + height * height); +inline void CylinderCollider::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { + decimal height = 2.0 * halfHeight; + decimal diag = (1.0 / 12.0) * mass * (3 * radius * radius + height * height); tensor.setAllValues(diag, 0.0, 0.0, 0.0, 0.5 * mass * radius * radius, 0.0, 0.0, 0.0, diag); } diff --git a/src/shapes/SphereShape.cpp b/src/shapes/SphereCollider.cpp similarity index 93% rename from src/shapes/SphereShape.cpp rename to src/shapes/SphereCollider.cpp index edac346f..cdc473f0 100644 --- a/src/shapes/SphereShape.cpp +++ b/src/shapes/SphereCollider.cpp @@ -24,7 +24,7 @@ ********************************************************************************/ // Libraries -#include "SphereShape.h" +#include "SphereCollider.h" #include "../configuration.h" #include @@ -45,18 +45,18 @@ using namespace reactphysics3d; using namespace std; // Constructor -SphereShape::SphereShape(double radius) : radius(radius) { +SphereCollider::SphereCollider(decimal radius) : radius(radius) { } // Destructor -SphereShape::~SphereShape() { +SphereCollider::~SphereCollider() { } #ifdef VISUAL_DEBUG // Draw the sphere (only for testing purpose) -void SphereShape::draw() const { +void SphereCollider::draw() const { // Draw in red glColor3f(1.0, 0.0, 0.0); diff --git a/src/shapes/SphereShape.h b/src/shapes/SphereCollider.h similarity index 72% rename from src/shapes/SphereShape.h rename to src/shapes/SphereCollider.h index 2eb1af72..f192a7ba 100644 --- a/src/shapes/SphereShape.h +++ b/src/shapes/SphereCollider.h @@ -23,35 +23,35 @@ * * ********************************************************************************/ -#ifndef SPHERE_SHAPE_H -#define SPHERE_SHAPE_H +#ifndef SPHERE_COLLIDER_H +#define SPHERE_COLLIDER_H // Libraries -#include "Shape.h" +#include "Collider.h" #include "../mathematics/mathematics.h" // ReactPhysics3D namespace namespace reactphysics3d { /* ------------------------------------------------------------------- - Class SphereShape : - This class represents a sphere collision shape that is centered + Class SphereCollider : + This class represents a sphere collider that is centered at the origin and defined by its radius. ------------------------------------------------------------------- */ -class SphereShape : public Shape { +class SphereCollider : public Collider { private : - double radius; // Radius of the sphere + decimal radius; // Radius of the sphere public : - SphereShape(double radius); // Constructor - virtual ~SphereShape(); // Destructor + SphereCollider(decimal radius); // Constructor + virtual ~SphereCollider(); // Destructor - double getRadius() const; // Return the radius of the sphere - void setRadius(double radius); // Set the radius of the sphere - virtual Vector3 getLocalSupportPoint(const Vector3& direction, double margin=0.0) const; // Return a local support point in a given direction - virtual Vector3 getLocalExtents(double margin=0.0) const; // Return the local extents in x,y and z direction - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const; // Return the local inertia tensor of the shape + decimal getRadius() const; // Return the radius of the sphere + void setRadius(decimal radius); // Set the radius of the sphere + virtual Vector3 getLocalSupportPoint(const Vector3& direction, decimal margin=0.0) const; // Return a local support point in a given direction + virtual Vector3 getLocalExtents(decimal margin=0.0) const; // Return the local extents in x,y and z direction + virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; // Return the local inertia tensor of the collider #ifdef VISUAL_DEBUG virtual void draw() const; // Draw the sphere (only for testing purpose) @@ -59,19 +59,19 @@ class SphereShape : public Shape { }; // Get the radius of the sphere -inline double SphereShape::getRadius() const { +inline decimal SphereCollider::getRadius() const { return radius; } // Set the radius of the sphere -inline void SphereShape::setRadius(double radius) { +inline void SphereCollider::setRadius(decimal radius) { this->radius = radius; } // Return a local support point in a given direction -inline Vector3 SphereShape::getLocalSupportPoint(const Vector3& direction, double margin) const { +inline Vector3 SphereCollider::getLocalSupportPoint(const Vector3& direction, decimal margin) const { assert(margin >= 0.0); - double length = direction.length(); + decimal length = direction.length(); // If the direction vector is not the zero vector if (length > 0.0) { @@ -84,15 +84,15 @@ inline Vector3 SphereShape::getLocalSupportPoint(const Vector3& direction, doubl return Vector3(0, radius + margin, 0); } -// Return the local extents of the shape (half-width) in x,y and z local direction +// Return the local extents of the collider (half-width) in x,y and z local direction // This method is used to compute the AABB of the box -inline Vector3 SphereShape::getLocalExtents(double margin) const { +inline Vector3 SphereCollider::getLocalExtents(decimal margin) const { return Vector3(radius + margin, radius + margin, radius + margin); } // Return the local inertia tensor of the sphere -inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const { - double diag = 0.4 * mass * radius * radius; +inline void SphereCollider::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { + decimal diag = 0.4 * mass * radius * radius; tensor.setAllValues(diag, 0.0, 0.0, 0.0, diag, 0.0, 0.0, 0.0, diag); }