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