diff --git a/sources/reactphysics3d/body/OBB.cpp b/sources/reactphysics3d/body/OBB.cpp index b96c368c..37f4b100 100644 --- a/sources/reactphysics3d/body/OBB.cpp +++ b/sources/reactphysics3d/body/OBB.cpp @@ -120,18 +120,6 @@ std::vector OBB::getExtremeVertices(const Vector3D& directionAxis) con std::vector extremeVertices; - // TODO : Delete this - std::cout << "------------------------------------" << std::endl; - for (int i=0; i<8; i++) - { - Vector3D vertex = getVertex(i); - - // Compute the projection length of the current vertex onto the projection axis - double projectionLength = directionAxis.scalarProduct(vertex-center) / directionAxis.length(); - std::cout << "Point : x=" << vertex.getX() << " y=" << vertex.getY() << "z=" << vertex.getZ() << std::endl; - std::cout << "projection length = " << projectionLength << std::endl; - } - // Check if the given axis is parallel to an axis on the OBB if (axis[0].isParallelWith(directionAxis)) { if (axis[0].scalarProduct(directionAxis) >= 0) { // If both axis are in the same direction diff --git a/sources/reactphysics3d/body/OBB.h b/sources/reactphysics3d/body/OBB.h index f4695fef..3f6e2b45 100644 --- a/sources/reactphysics3d/body/OBB.h +++ b/sources/reactphysics3d/body/OBB.h @@ -157,7 +157,6 @@ inline void OBB::setExtent(unsigned int index, double extent) throw(std::invalid } } -// TODO : Test this method // Update the orientation of the OBB according to the orientation of the rigid body inline void OBB::updateOrientation(const Vector3D& newCenter, const Quaternion& rotationQuaternion) { // Update the center of the OBB diff --git a/sources/reactphysics3d/body/RigidBody.h b/sources/reactphysics3d/body/RigidBody.h index 75fac7cc..8aba50b8 100644 --- a/sources/reactphysics3d/body/RigidBody.h +++ b/sources/reactphysics3d/body/RigidBody.h @@ -64,7 +64,7 @@ class RigidBody : public Body { Quaternion getOrientation() const; // Return the orientation quaternion void setOrientation(const Quaternion& orientation); // Set the orientation quaternion Vector3D getLinearVelocity() const; // Return the linear velocity - void setLinearVelocity(const Vector3D& linearVelocity); // TODO : Delete this + void setLinearVelocity(const Vector3D& linearVelocity); // Set the linear velocity of the body Vector3D getAngularVelocity() const; // Return the angular velocity void setAngularVelocity(const Vector3D& angularVelocity); // Set the angular velocity void setMassInverse(double massInverse); // Set the inverse of the mass @@ -192,7 +192,7 @@ inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const { // Set the interpolation factor of the body inline void RigidBody::setInterpolationFactor(double factor) { - //assert(factor >= 0.0 && factor <= 1.0); // TODO : Remove the comment here + //assert(factor >= 0.0 && factor <= 1.0); // Set the factor interpolationFactor = factor; diff --git a/sources/reactphysics3d/collision/CollisionDetection.h b/sources/reactphysics3d/collision/CollisionDetection.h index 5b3fb6b2..a7076095 100644 --- a/sources/reactphysics3d/collision/CollisionDetection.h +++ b/sources/reactphysics3d/collision/CollisionDetection.h @@ -44,10 +44,8 @@ class CollisionDetection { PhysicsWorld* world; // Pointer to the physics world std::vector > possibleCollisionPairs; // Possible collision pairs of bodies (computed by broadphase) std::vector contactInfos; // Contact informations (computed by narrowphase) - - // TODO : Check if we can avoid pointers for the two following classes (to avoid dynamic alocation) - BroadPhaseAlgorithm* broadPhaseAlgorithm; // Broad-phase algorithm - NarrowPhaseAlgorithm* narrowPhaseAlgorithm; // Narrow-phase algorithm + BroadPhaseAlgorithm* broadPhaseAlgorithm; // Broad-phase algorithm + NarrowPhaseAlgorithm* narrowPhaseAlgorithm; // Narrow-phase algorithm void computeBroadPhase(); // Compute the broad-phase collision detection void computeNarrowPhase(); // Compute the narrow-phase collision detection diff --git a/sources/reactphysics3d/collision/NarrowPhaseAlgorithm.h b/sources/reactphysics3d/collision/NarrowPhaseAlgorithm.h index 61b6b84f..4f1a7d5b 100644 --- a/sources/reactphysics3d/collision/NarrowPhaseAlgorithm.h +++ b/sources/reactphysics3d/collision/NarrowPhaseAlgorithm.h @@ -27,8 +27,6 @@ // Namespace ReactPhysics3D namespace reactphysics3d { -// TODO : Remove the contact determination from this class. This class should only return pairs of bodies that intersect - /* ------------------------------------------------------------------- Class NarrowPhaseAlgorithm : This class is an abstract class that represents an algorithm diff --git a/sources/reactphysics3d/collision/SATAlgorithm.cpp b/sources/reactphysics3d/collision/SATAlgorithm.cpp index c44c4c06..8d1afbfe 100644 --- a/sources/reactphysics3d/collision/SATAlgorithm.cpp +++ b/sources/reactphysics3d/collision/SATAlgorithm.cpp @@ -30,9 +30,6 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; -// TODO : Check and modify all comments on this file in order that -//  everything have to do with the new SAT algorithm - // Constructor SATAlgorithm::SATAlgorithm() { @@ -67,7 +64,7 @@ bool SATAlgorithm::testCollision(const BoundingVolume* const boundingVolume1, co // This method returns true and computes a contact info if the two OBB intersect. -// This method implements the separating algorithm between two OBB. The goal of this method is to test if the +// This method implements the separating algorithm between two OBBs. The goal of this method is to test if the // two OBBs intersect or not. If they intersect we report a contact info and the method returns true. If // they don't intersect, the method returns false. The separation axis that have to be tested for two // OBB are the six face normals (3 for each OBB) and the nine vectors V = Ai x Bj where Ai is the ith face normal @@ -85,9 +82,8 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const double max2; // Maximum of interval 2 Vector3D normal; // Contact normal (correspond to the separation axis with the smallest positive penetration depth) // The contact normal point out of OBB1 toward OBB2 - bool side; // True if the interval 1 is at the left of interval 2 if a collision occurs and false otherwise double minPenetrationDepth = DBL_MAX; // Minimum penetration depth detected among all separated axis - const double cutoff = 0.99; // Cutoff for cosine of angles between box axes + const double cutoff = 0.99; // Cutoff for cosine of angles between box axes bool existsParallelPair = false; // True if there exists two face normals that are parallel. // This is used because if a parallel pair exists, it is sufficient // to test only the face normals of the OBBs for separation. Two nearly @@ -118,13 +114,11 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - bool sideTemp; // TODO : Check if we really need the "side" variable (maybee, we can remove it) - double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); + double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis - side = sideTemp; minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth normal = computeContactNormal(obb1->getAxis(0), boxDistance); // Compute the contact normal with the correct sign } @@ -145,12 +139,11 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis - side = sideTemp; minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth normal = computeContactNormal(obb1->getAxis(1), boxDistance); // Compute the contact normal with the correct sign } @@ -171,12 +164,11 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis - side = sideTemp; minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth normal = computeContactNormal(obb1->getAxis(2), boxDistance); // Compute the contact normal with the correct sign } @@ -190,12 +182,11 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis - side = sideTemp; minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth normal = computeContactNormal(obb2->getAxis(0), boxDistance); // Compute the contact normal with the correct sign } @@ -209,12 +200,11 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis - side = sideTemp; minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth normal = computeContactNormal(obb2->getAxis(1), boxDistance); // Compute the contact normal with the correct sign } @@ -228,12 +218,11 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis - side = sideTemp; minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth normal = computeContactNormal(obb2->getAxis(2), boxDistance); // Compute the contact normal with the correct sign } @@ -257,12 +246,11 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis - side = sideTemp; minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth normal = computeContactNormal(obb1->getAxis(0).crossProduct(obb2->getAxis(0)), boxDistance); // Compute the contact normal with the correct sign } @@ -275,12 +263,11 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis - side = sideTemp; minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth normal = computeContactNormal(obb1->getAxis(0).crossProduct(obb2->getAxis(1)), boxDistance); // Compute the contact normal with the correct sign } @@ -293,12 +280,11 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis - side = sideTemp; minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth normal = computeContactNormal(obb1->getAxis(0).crossProduct(obb2->getAxis(2)), boxDistance); // Compute the contact normal with the correct sign } @@ -311,12 +297,11 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis - side = sideTemp; minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth normal = computeContactNormal(obb1->getAxis(1).crossProduct(obb2->getAxis(0)), boxDistance); // Compute the contact normal with the correct sign } @@ -329,12 +314,11 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis - side = sideTemp; minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth normal = computeContactNormal(obb1->getAxis(1).crossProduct(obb2->getAxis(1)), boxDistance); // Compute the contact normal with the correct sign } @@ -347,12 +331,11 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis - side = sideTemp; minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth normal = computeContactNormal(obb1->getAxis(1).crossProduct(obb2->getAxis(2)), boxDistance); // Compute the contact normal with the correct sign } @@ -365,12 +348,11 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis - side = sideTemp; minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth normal = computeContactNormal(obb1->getAxis(2).crossProduct(obb2->getAxis(0)), boxDistance); // Compute the contact normal with the correct sign } @@ -383,12 +365,11 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis - side = sideTemp; minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth normal = computeContactNormal(obb1->getAxis(2).crossProduct(obb2->getAxis(1)), boxDistance); // Compute the contact normal with the correct sign } @@ -401,12 +382,11 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis - side = sideTemp; minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth normal = computeContactNormal(obb1->getAxis(2).crossProduct(obb2->getAxis(2)), boxDistance); // Compute the contact normal with the correct sign } @@ -418,10 +398,8 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const } // This method computes and returns the penetration depth between two intervals. This method returns the computed -// penetration depth (note that it could return a negative penetration depth if the intervals are separated. This -// method also find which interval is at the left of the other in order to know which extreme of interval 1 collides with -// which extreme of interval 2 if a collision occur. -double SATAlgorithm::computePenetrationDepth(double min1, double max1, double min2, double max2, bool& side) const { +// penetration depth (note that it could return a negative penetration depth if the intervals are separated. +double SATAlgorithm::computePenetrationDepth(double min1, double max1, double min2, double max2) const { // Compute the length of both intervals double lengthInterval1 = max1 - min1; @@ -433,18 +411,5 @@ double SATAlgorithm::computePenetrationDepth(double min1, double max1, double mi double lengthBothIntervals = maxExtreme - minExtreme; // Compute the current penetration depth - double penetrationDepth = (lengthInterval1 + lengthInterval2) - lengthBothIntervals; - - // Find which interval is at the left of the other - if (std::abs(max1-min2) <= std::abs(max2-min1)) { - // Right of interval 1 collides with the left of interval 2 - side = true; - } - else { - // Right of interval 2 collides with the left of interval 1 - side = false; - } - - // Return the computed penetration depth - return penetrationDepth; + return (lengthInterval1 + lengthInterval2) - lengthBothIntervals; } diff --git a/sources/reactphysics3d/collision/SATAlgorithm.h b/sources/reactphysics3d/collision/SATAlgorithm.h index 79677aed..6234445a 100644 --- a/sources/reactphysics3d/collision/SATAlgorithm.h +++ b/sources/reactphysics3d/collision/SATAlgorithm.h @@ -44,12 +44,9 @@ namespace reactphysics3d { */ class SATAlgorithm : public NarrowPhaseAlgorithm { private : - bool computeCollisionTest(const OBB* const obb1, const OBB* const obb2, ContactInfo*& contactInfo) const; // Return true and compute a contact info if the two OBB collide - double computePenetrationDepth(double min1, double max1, double min2, double max2, bool& side) const; // Compute the penetration depth of two projection intervals - // TODO : Remove the following method - //void computeContact(const OBB* const obb1, const OBB* const obb2, const Vector3D normal, double penetrationDepth, - // const std::vector& obb1ExtremePoints, const std::vector& obb2ExtremePoints, Contact** contact) const; // Compute a new contact // Compute a new collision contact between two projection intervals - Vector3D computeContactNormal(const Vector3D& axis, const Vector3D& distanceOfOBBs) const; // Compute a contact normal + bool computeCollisionTest(const OBB* const obb1, const OBB* const obb2, ContactInfo*& contactInfo) const; // Return true and compute a contact info if the two OBB collide + double computePenetrationDepth(double min1, double max1, double min2, double max2) const; // Compute the penetration depth of two projection intervals + Vector3D computeContactNormal(const Vector3D& axis, const Vector3D& distanceOfOBBs) const; // Compute a contact normal public : SATAlgorithm(); // Constructor @@ -58,8 +55,6 @@ class SATAlgorithm : public NarrowPhaseAlgorithm { virtual bool testCollision(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2, ContactInfo*& contactInfo); // Return true and compute a contact info if the two bounding volume collide }; -// --- Inlines function --- // - // Return the contact normal with the correct sign (from obb1 toward obb2). "axis" is the axis vector direction where the // collision occur and "distanceOfOBBs" is the vector (obb2.center - obb1.center). inline Vector3D SATAlgorithm::computeContactNormal(const Vector3D& axis, const Vector3D& distanceOfOBBs) const { diff --git a/sources/reactphysics3d/constraint/Contact.cpp b/sources/reactphysics3d/constraint/Contact.cpp index 3286984e..f478adfd 100644 --- a/sources/reactphysics3d/constraint/Contact.cpp +++ b/sources/reactphysics3d/constraint/Contact.cpp @@ -61,8 +61,8 @@ void Contact::evaluate() { // Compute the friction vectors that span the tangential friction plane computeFrictionVectors(); - Vector3D r1 = point - rigidBody1->getCurrentBodyState().getPosition(); - Vector3D r2 = point - rigidBody2->getCurrentBodyState().getPosition(); + Vector3D r1 = point - rigidBody1->getPosition(); + Vector3D r2 = point - rigidBody2->getPosition(); Vector3D r1CrossN = r1.crossProduct(normal); Vector3D r2CrossN = r2.crossProduct(normal); @@ -87,10 +87,10 @@ void Contact::evaluate() { upperBound = INFINITY_CONST; // Compute the error value of the constraint - Vector3D velocity1 = rigidBody1->getCurrentBodyState().getLinearVelocity(); - Vector3D velocity2 = rigidBody2->getCurrentBodyState().getLinearVelocity(); + Vector3D velocity1 = rigidBody1->getLinearVelocity(); + Vector3D velocity2 = rigidBody2->getLinearVelocity(); double restitutionCoeff = rigidBody1->getRestitution() * rigidBody2->getRestitution(); - errorValue = restitutionCoeff * (normal.scalarProduct(velocity1) - normal.scalarProduct(velocity2)) + PENETRATION_FACTOR * penetrationDepth; // TODO : Add penetration + errorValue = restitutionCoeff * (normal.scalarProduct(velocity1) - normal.scalarProduct(velocity2)) + PENETRATION_FACTOR * penetrationDepth; // Compute the auxiliary jacobian matrix (this corresponds to the friction constraint) Vector3D r1CrossU1 = r1.crossProduct(frictionVectors[0]); @@ -125,7 +125,7 @@ void Contact::evaluate() { // Compute the auxiliary lower and upper bounds // TODO : Now mC is only the mass of the first body but it is probably wrong // TODO : Now g is 9.81 but we should use the true gravity value of the physics world. - double mu_mc_g = FRICTION_COEFFICIENT * rigidBody1->getMass().getValue() * 9.81; + double mu_mc_g = FRICTION_COEFFICIENT * rigidBody1->getMass() * 9.81; auxLowerBounds.setValue(0, -mu_mc_g); auxLowerBounds.setValue(1, -mu_mc_g); auxUpperBounds.setValue(0, mu_mc_g); diff --git a/sources/reactphysics3d/constraint/Contact.h b/sources/reactphysics3d/constraint/Contact.h index 6710027a..9cf238db 100644 --- a/sources/reactphysics3d/constraint/Contact.h +++ b/sources/reactphysics3d/constraint/Contact.h @@ -62,7 +62,7 @@ class Contact : public Constraint { virtual void evaluate(); // Evaluate the constraint uint getNbAuxConstraints() const; // Return the number of auxiliary constraints - double getPenetrationDepth() const; // TODO : Delete this + double getPenetrationDepth() const; // Return the penetration depth void draw() const; // TODO : Delete this (Used to debug collision detection) }; diff --git a/sources/reactphysics3d/engine/ConstraintSolver.h b/sources/reactphysics3d/engine/ConstraintSolver.h index 3cf71743..3675c097 100644 --- a/sources/reactphysics3d/engine/ConstraintSolver.h +++ b/sources/reactphysics3d/engine/ConstraintSolver.h @@ -33,7 +33,6 @@ namespace reactphysics3d { // Constants -// TODO : Set this to 10 after debugging const uint MAX_LCP_ITERATIONS = 10; // Maximum number of iterations when solving a LCP problem /* ------------------------------------------------------------------- diff --git a/sources/reactphysics3d/engine/PhysicsEngine.cpp b/sources/reactphysics3d/engine/PhysicsEngine.cpp index 59b03940..c82d1eb5 100644 --- a/sources/reactphysics3d/engine/PhysicsEngine.cpp +++ b/sources/reactphysics3d/engine/PhysicsEngine.cpp @@ -24,8 +24,8 @@ using namespace reactphysics3d; // Constructor -PhysicsEngine::PhysicsEngine(PhysicsWorld* world, const Time& timeStep) throw (std::invalid_argument) - : world(world), timer(Time(0.0), timeStep), collisionDetection(world), constraintSolver(world) { +PhysicsEngine::PhysicsEngine(PhysicsWorld* world, double timeStep) throw (std::invalid_argument) + : world(world), timer(0.0, timeStep), collisionDetection(world), constraintSolver(world) { // Check if the pointer to the world is not NULL if (world == 0) { // Throw an exception @@ -46,14 +46,14 @@ void PhysicsEngine::update() { applyGravity(); // While the time accumulator is not empty - while(timer.getAccumulator() >= timer.getTimeStep().getValue()) { + while(timer.getAccumulator() >= timer.getTimeStep()) { existCollision = false; // Compute the collision detection if (collisionDetection.computeCollisionDetection()) { existCollision = true; // Solve constraints - constraintSolver.solve(timer.getTimeStep().getValue()); + constraintSolver.solve(timer.getTimeStep()); } // Update the timer @@ -80,7 +80,7 @@ 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().getValue(); + double dt = timer.getTimeStep(); Vector3D newLinearVelocity; Vector3D newAngularVelocity; @@ -128,7 +128,7 @@ void PhysicsEngine::updateAllBodiesMotion() { // Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new // orientation of the body void PhysicsEngine::updatePositionAndOrientationOfBody(Body* body, const Vector3D& newLinVelocity, const Vector3D& newAngVelocity) { - double dt = timer.getTimeStep().getValue(); + double dt = timer.getTimeStep(); RigidBody* rigidBody = dynamic_cast(body); assert(rigidBody); diff --git a/sources/reactphysics3d/engine/PhysicsEngine.h b/sources/reactphysics3d/engine/PhysicsEngine.h index c64e5c3d..68773571 100644 --- a/sources/reactphysics3d/engine/PhysicsEngine.h +++ b/sources/reactphysics3d/engine/PhysicsEngine.h @@ -42,21 +42,19 @@ class PhysicsEngine { Timer timer; // Timer of the physics engine CollisionDetection collisionDetection; // Collision detection ConstraintSolver constraintSolver; // Constraint solver - // Update the state of a rigid body // TODO : Delete this + void updateAllBodiesMotion(); // Compute the motion of all bodies and update their positions and orientations void updatePositionAndOrientationOfBody(Body* body, const Vector3D& newLinVelocity, const Vector3D& newAngVelocity); // Update the position and orientation of a body void applyGravity(); // Apply the gravity force to all bodies public : - PhysicsEngine(PhysicsWorld* world, const Time& timeStep) throw (std::invalid_argument); // Constructor + PhysicsEngine(PhysicsWorld* world, double timeStep) throw (std::invalid_argument); // Constructor ~PhysicsEngine(); // Destructor void start(); // Start the physics simulation void stop(); // Stop the physics simulation void update(); // Update the physics simulation - //void updateDynamic(); // TODO : Delete this method - void updateCollision(); // TODO : Delete this collision - void initializeDisplayTime(const Time& displayTime); // Initialize the display time - void updateDisplayTime(const Time& newDisplayTime); // Update the display time + void initializeDisplayTime(long double displayTime); // Initialize the display time + void updateDisplayTime(long double newDisplayTime); // Update the display time }; // --- Inline functions --- // @@ -71,12 +69,12 @@ inline void PhysicsEngine::stop() { } // Initialize the display time -inline void PhysicsEngine::initializeDisplayTime(const Time& displayTime) { +inline void PhysicsEngine::initializeDisplayTime(long double displayTime) { timer.setCurrentDisplayTime(displayTime); } // Update the display time -inline void PhysicsEngine::updateDisplayTime(const Time& newDisplayTime) { +inline void PhysicsEngine::updateDisplayTime(long double newDisplayTime) { timer.updateDisplayTime(newDisplayTime); } diff --git a/sources/reactphysics3d/engine/Timer.cpp b/sources/reactphysics3d/engine/Timer.cpp index 03f15963..d6caa7fe 100644 --- a/sources/reactphysics3d/engine/Timer.cpp +++ b/sources/reactphysics3d/engine/Timer.cpp @@ -24,10 +24,10 @@ using namespace reactphysics3d; // Constructor -Timer::Timer(const Time& initialTime, const Time& timeStep) throw(std::invalid_argument) - : timeStep(timeStep), time(initialTime), currentDisplayTime(Time(0.0)), deltaDisplayTime(Time(0.0)) { +Timer::Timer(long double initialTime, double timeStep) throw(std::invalid_argument) + : timeStep(timeStep), time(initialTime), currentDisplayTime(0.0), deltaDisplayTime(0.0) { // Check if the timestep is different from zero - if (timeStep.getValue() != 0.0) { + if (timeStep != 0.0) { accumulator = 0.0; isRunning = false; } diff --git a/sources/reactphysics3d/engine/Timer.h b/sources/reactphysics3d/engine/Timer.h index 76684b9f..2d4d4814 100644 --- a/sources/reactphysics3d/engine/Timer.h +++ b/sources/reactphysics3d/engine/Timer.h @@ -21,16 +21,12 @@ #define TIMER_H // Libraries - #include "../physics/physics.h" #include #include // Namespace ReactPhysics3D namespace reactphysics3d { -// TODO : Now we are using the "Time" class in this class but we should a -// time class from the standard library - /* ------------------------------------------------------------------- Class Timer : This class will take care of the time in the physics engine. @@ -38,43 +34,43 @@ */ class Timer { private : - Time timeStep; // Timestep dt of the physics engine (timestep > 0.0) - Time time; // Current time of the physics engine - Time currentDisplayTime; // Current display time - Time deltaDisplayTime; // Current time difference between two display frames - double accumulator; // Used to fix the time step and avoid strange time effects - bool isRunning; // True if the timer is running + double timeStep; // Timestep dt of the physics engine (timestep > 0.0) + long double time; // Current time of the physics engine + long double currentDisplayTime; // Current display time + long double deltaDisplayTime; // Current time difference between two display frames + double accumulator; // Used to fix the time step and avoid strange time effects + bool isRunning; // True if the timer is running public : - Timer(const Time& initialTime, const Time& timeStep) throw(std::invalid_argument); // Constructor - Timer(const Timer& timer); // Copy-constructor - virtual ~Timer(); // Destructor + Timer(long double initialTime, double timeStep) throw(std::invalid_argument); // Constructor + Timer(const Timer& timer); // Copy-constructor + virtual ~Timer(); // Destructor - Time getTimeStep() const; // Return the timestep of the physics engine - void setTimeStep(const Time& timeStep) throw(std::invalid_argument); // Set the timestep of the physics engine - Time getTime() const; // Return the current time - void setTime(const Time& time); // Set the current time + double getTimeStep() const; // Return the timestep of the physics engine + void setTimeStep(double timeStep) throw(std::invalid_argument); // Set the timestep of the physics engine + long double getTime() const; // Return the current time + void setTime(long double time); // Set the current time bool getIsRunning() const; // Return if the timer is running void setIsRunning(bool isRunning); // Set if the timer is running double getAccumulator() const; // Return the accumulator value - void setCurrentDisplayTime(const Time& displayTime); // Set the current display time + void setCurrentDisplayTime(long double displayTime); // Set the current display time void update(); // Update the timer by adding some time value (or timeStep by default) to the current time double getInterpolationFactor() const; // Compute and return the interpolation factor between two body states - void updateDisplayTime(const Time& newDisplayTime); // Set the new currentDisplayTime value + void updateDisplayTime(long double newDisplayTime); // Set the new currentDisplayTime value }; // --- Inline functions --- // // Return the timestep of the physics engine -inline Time Timer::getTimeStep() const { +inline double Timer::getTimeStep() const { return timeStep; } // Set the timestep of the physics engine -inline void Timer::setTimeStep(const Time& timeStep) throw(std::invalid_argument) { +inline void Timer::setTimeStep(double timeStep) throw(std::invalid_argument) { // Check if the timestep is different from zero - if (timeStep.getValue() != 0.0) { + if (timeStep != 0.0) { this->timeStep = timeStep; } else { @@ -84,12 +80,12 @@ inline void Timer::setTimeStep(const Time& timeStep) throw(std::invalid_argument } // Return the current time -inline Time Timer::getTime() const { +inline long double Timer::getTime() const { return time; } // Set the current time -inline void Timer::setTime(const Time& time) { +inline void Timer::setTime(long double time) { this->time = time; } @@ -109,7 +105,7 @@ inline double Timer::getAccumulator() const { } // Set the current display time -inline void Timer::setCurrentDisplayTime(const Time& currentDisplayTime) { +inline void Timer::setCurrentDisplayTime(long double currentDisplayTime) { this->currentDisplayTime = currentDisplayTime; } @@ -118,29 +114,29 @@ inline void Timer::update() { // Check if the timer is running if (isRunning) { // Update the current time of the physics engine - time.setValue(time.getValue() + timeStep.getValue()); + time += timeStep; // Update the accumulator value - accumulator -= timeStep.getValue(); + accumulator -= timeStep; } } // Compute and return the interpolation factor between two body states inline double Timer::getInterpolationFactor() const { // Compute and return the interpolation factor - return (accumulator / timeStep.getValue()); + return (accumulator / timeStep); } // Set the new currentDisplayTime value -inline void Timer::updateDisplayTime(const Time& newDisplayTime) { +inline void Timer::updateDisplayTime(long double newDisplayTime) { // Compute the delta display time between two display frames - deltaDisplayTime.setValue(newDisplayTime.getValue() - currentDisplayTime.getValue()); + deltaDisplayTime = newDisplayTime - currentDisplayTime; // Update the current display time - currentDisplayTime.setValue(newDisplayTime.getValue()); + currentDisplayTime = newDisplayTime; // Update the accumulator value - accumulator += deltaDisplayTime.getValue(); + accumulator += deltaDisplayTime; } } // End of the ReactPhysics3D namespace diff --git a/sources/reactphysics3d/mathematics/Matrix.cpp b/sources/reactphysics3d/mathematics/Matrix.cpp index 98c58b1e..360dd89d 100644 --- a/sources/reactphysics3d/mathematics/Matrix.cpp +++ b/sources/reactphysics3d/mathematics/Matrix.cpp @@ -350,7 +350,6 @@ Matrix Matrix::identity(int dimension) throw(std::invalid_argument) { } } -// TODO : Test this method // Fill in a sub-matrix of the current matrix with another matrix. // This method replaces the sub-matrix with the upper-left index (i,j) in the current matrix by another // "subMatrix" matrix. diff --git a/sources/reactphysics3d/mathematics/Vector3D.cpp b/sources/reactphysics3d/mathematics/Vector3D.cpp index 43e4c04a..a0cfeab1 100644 --- a/sources/reactphysics3d/mathematics/Vector3D.cpp +++ b/sources/reactphysics3d/mathematics/Vector3D.cpp @@ -66,7 +66,6 @@ Vector3D Vector3D::getUnit() const throw(MathematicsException) { } // Return two unit orthogonal vectors of the current vector -// TODO : Test this method Vector3D Vector3D::getOneOrthogonalVector() const { assert(!this->isZero()); Vector3D unitVector = this->getUnit(); diff --git a/sources/reactphysics3d/mathematics/Vector3D.h b/sources/reactphysics3d/mathematics/Vector3D.h index fb9ea4ef..abd7f3fa 100644 --- a/sources/reactphysics3d/mathematics/Vector3D.h +++ b/sources/reactphysics3d/mathematics/Vector3D.h @@ -114,7 +114,6 @@ inline double Vector3D::length() const { } // Return the vector in the opposite direction -// TODO : Test this function inline Vector3D Vector3D::getOpposite() const { return (Vector3D(0.0, 0.0, 0.0) - *this); } diff --git a/sources/reactphysics3d/mathematics/mathematics.h b/sources/reactphysics3d/mathematics/mathematics.h index 7dea1974..74414b02 100644 --- a/sources/reactphysics3d/mathematics/mathematics.h +++ b/sources/reactphysics3d/mathematics/mathematics.h @@ -17,8 +17,6 @@ * along with ReactPhysics3D. If not, see . * ***************************************************************************/ -// TODO : Mathematics library : Check everywhere that in member methods we use attributes access instead of getter and setter. - #ifndef MATHEMATICS_H #define MATHEMATICS_H @@ -41,7 +39,6 @@ namespace reactphysics3d { // ---------- Mathematics functions ---------- // -// TODO : Test this method // Rotate a vector according to a rotation quaternion. // The function returns the vector rotated according to the quaternion in argument inline reactphysics3d::Vector3D rotateVectorWithQuaternion(const reactphysics3d::Vector3D& vector, const reactphysics3d::Quaternion& quaternion) { @@ -55,7 +52,6 @@ inline reactphysics3d::Vector3D rotateVectorWithQuaternion(const reactphysics3d: return quaternionResult.vectorV(); } -// TODO : Test this method // Given two lines (given by the points "point1", "point2" and the vectors "d1" and "d2" that are not parallel, this method returns the values // "alpha" and "beta" such that the two points P1 and P2 are the two closest point between the two lines and such that // P1 = point1 + alpha * d1 @@ -79,7 +75,6 @@ inline void closestPointsBetweenTwoLines(const reactphysics3d::Vector3D& point1, *beta = (a*f-b*c)/d; } -// TODO : Test this method // This method returns true if the point "P" is on the segment between "segPointA" and "segPointB" and return false otherwise inline bool isPointOnSegment(const reactphysics3d::Vector3D& segPointA, const reactphysics3d::Vector3D& segPointB, const reactphysics3d::Vector3D& P) { @@ -106,7 +101,7 @@ inline bool isPointOnSegment(const reactphysics3d::Vector3D& segPointA, const re return true; } -// TODO : Test this method + // Given two lines in 3D that intersect, this method returns the intersection point between the two lines. // The first line is given by the point "p1" and the vector "d1", the second line is given by the point "p2" and the vector "d2". inline reactphysics3d::Vector3D computeLinesIntersection(const reactphysics3d::Vector3D& p1, const reactphysics3d::Vector3D& d1, @@ -124,7 +119,7 @@ inline reactphysics3d::Vector3D computeLinesIntersection(const reactphysics3d::V return 0.5 * (point1 + point2); } -// TODO : Test this method + // Given two segments in 3D that are not parallel and that intersect, this method computes the intersection point between the two segments. // This method returns the intersection point. inline reactphysics3d::Vector3D computeNonParallelSegmentsIntersection(const reactphysics3d::Vector3D& seg1PointA, const reactphysics3d::Vector3D& seg1PointB, @@ -155,7 +150,6 @@ inline reactphysics3d::Vector3D computeNonParallelSegmentsIntersection(const rea } -// TODO : Test this method // Move a set of points by a given vector. // The method returns a set of points moved by the given vector. inline std::vector movePoints(const std::vector& points, const reactphysics3d::Vector3D& vector) { @@ -172,7 +166,6 @@ inline std::vector movePoints(const std::vector projectPointsOntoPlane(const std::vector& points, const reactphysics3d::Vector3D& A, @@ -193,7 +186,7 @@ inline std::vector projectPointsOntoPlane(const std::v return projectedPoints; } -// TODO : Test this method + // Compute the distance between a point "P" and a line (given by a point "A" and a vector "v") inline double computeDistanceBetweenPointAndLine(const reactphysics3d::Vector3D& P, const reactphysics3d::Vector3D& A, const reactphysics3d::Vector3D& v) { assert(v.length() != 0);