From 471e4f7afcf1b4e0b5de2b3fecd02e24ab21f72b Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 22 Apr 2013 23:32:52 +0200 Subject: [PATCH 01/24] Start to implement the joints --- src/constraint/BallAndSocketJoint.cpp | 42 +++++++++ src/constraint/BallAndSocketJoint.h | 59 ++++++++++++ src/constraint/Constraint.cpp | 11 +-- src/constraint/Constraint.h | 40 +------- src/constraint/ContactPoint.cpp | 9 +- src/constraint/ContactPoint.h | 59 +++++++++++- src/engine/ConstraintSolver.cpp | 39 ++++++++ src/engine/ConstraintSolver.h | 128 ++++++++++++++++++++++++++ src/engine/ContactSolver.cpp | 12 +-- src/engine/ContactSolver.h | 2 +- 10 files changed, 345 insertions(+), 56 deletions(-) create mode 100644 src/constraint/BallAndSocketJoint.cpp create mode 100644 src/constraint/BallAndSocketJoint.h create mode 100644 src/engine/ConstraintSolver.cpp create mode 100644 src/engine/ConstraintSolver.h diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp new file mode 100644 index 00000000..51b017c7 --- /dev/null +++ b/src/constraint/BallAndSocketJoint.cpp @@ -0,0 +1,42 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2013 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. * +* * +********************************************************************************/ + +// Libraries +#include "BallAndSocketJoint.h" + +using namespace reactphysics3d; + +// Constructor +BallAndSocketJoint::BallAndSocketJoint(RigidBody* const body1, RigidBody* const body2, + bool active, ConstraintType type) + : Constraint(body1, body2, active, type){ + +} + +// Destructor +BallAndSocketJoint::~BallAndSocketJoint() { + +} + diff --git a/src/constraint/BallAndSocketJoint.h b/src/constraint/BallAndSocketJoint.h new file mode 100644 index 00000000..7908dc8f --- /dev/null +++ b/src/constraint/BallAndSocketJoint.h @@ -0,0 +1,59 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2013 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 REACTPHYSICS3D_BALL_AND_SOCKET_JOINT_H +#define REACTPHYSICS3D_BALL_AND_SOCKET_JOINT_H + +// Libraries +#include "Constraint.h" + +namespace reactphysics3d { + +// Class BallAndSocketJoint +/** + * This class represents a ball-and-socket joint that allows arbitrary rotation + * between two bodies. + */ +class BallAndSocketJoint : public Constraint { + + private : + + // -------------------- Attributes -------------------- // + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + BallAndSocketJoint(RigidBody* const body1, RigidBody* const body2, + bool active, ConstraintType type); + + /// Destructor + virtual ~BallAndSocketJoint(); +}; + +} + +#endif diff --git a/src/constraint/Constraint.cpp b/src/constraint/Constraint.cpp index 7f40b3f7..a1bd2b76 100644 --- a/src/constraint/Constraint.cpp +++ b/src/constraint/Constraint.cpp @@ -30,14 +30,9 @@ using namespace reactphysics3d; // Constructor Constraint::Constraint(RigidBody* const body1, RigidBody* const body2, - uint nbConstraints, bool active, ConstraintType type) - :mBody1(body1), mBody2(body2), mActive(active), - mNbConstraints(nbConstraints), mType(type) { - - // Initialize the cached lambda values - for (uint i=0; i mCachedLambdas; - // -------------------- Methods -------------------- // /// Private copy-constructor @@ -81,7 +74,7 @@ class Constraint { // -------------------- Methods -------------------- // /// Constructor - Constraint(RigidBody* const body1, RigidBody* const body2, uint nbConstraints, + Constraint(RigidBody* const body1, RigidBody* const body2, bool active, ConstraintType type); /// Destructor @@ -98,15 +91,6 @@ class Constraint { /// Return the type of the constraint ConstraintType getType() const; - - /// Return the number of mathematical constraints - unsigned int getNbConstraints() const; - - /// Get one cached lambda value - decimal getCachedLambda(uint index) const; - - /// Set on cached lambda value - void setCachedLambda(uint index, decimal lambda); }; // Return the reference to the body 1 @@ -127,25 +111,7 @@ inline bool Constraint::isActive() const { // Return the type of the constraint inline ConstraintType Constraint::getType() const { return mType; -} - - -// Return the number auxiliary constraints -inline uint Constraint::getNbConstraints() const { - return mNbConstraints; -} - -// Get one previous lambda value -inline decimal Constraint::getCachedLambda(uint index) const { - assert(index < mNbConstraints); - return mCachedLambdas[index]; -} - -// Set on cached lambda value -inline void Constraint::setCachedLambda(uint index, decimal lambda) { - assert(index < mNbConstraints); - mCachedLambdas[index] = lambda; -} +} } diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp index d2f34781..df38e269 100644 --- a/src/constraint/ContactPoint.cpp +++ b/src/constraint/ContactPoint.cpp @@ -32,13 +32,16 @@ using namespace std; // Constructor ContactPoint::ContactPoint(RigidBody* const body1, RigidBody* const body2, const ContactInfo* contactInfo) - : Constraint(body1, body2, 3, true, CONTACT), mNormal(contactInfo->normal), + : Constraint(body1, body2, true, CONTACT), mNormal(contactInfo->normal), mPenetrationDepth(contactInfo->penetrationDepth), mLocalPointOnBody1(contactInfo->localPoint1), mLocalPointOnBody2(contactInfo->localPoint2), mWorldPointOnBody1(body1->getTransform() * contactInfo->localPoint1), - mWorldPointOnBody2(body2->getTransform() * contactInfo->localPoint2), - mIsRestingContact(false), mFrictionVectors(2, Vector3(0, 0, 0)) { + mWorldPointOnBody2(body2->getTransform() * contactInfo->localPoint2), + mIsRestingContact(false) { + + mFrictionVectors[0] = Vector3(0, 0, 0); + mFrictionVectors[1] = Vector3(0, 0, 0); assert(mPenetrationDepth > 0.0); diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h index fa6abe5c..8ad4eff9 100644 --- a/src/constraint/ContactPoint.h +++ b/src/constraint/ContactPoint.h @@ -84,7 +84,16 @@ class ContactPoint : public Constraint { bool mIsRestingContact; /// Two orthogonal vectors that span the tangential friction plane - std::vector mFrictionVectors; + Vector3 mFrictionVectors[2]; + + /// Cached penetration impulse + decimal mPenetrationImpulse; + + /// Cached first friction impulse + decimal mFrictionImpulse1; + + /// Cached second friction impulse + decimal mFrictionImpulse2; // -------------------- Methods -------------------- // @@ -122,6 +131,24 @@ class ContactPoint : public Constraint { /// Return the contact world point on body 2 Vector3 getWorldPointOnBody2() const; + /// Return the cached penetration impulse + decimal getPenetrationImpulse() const; + + /// Return the cached first friction impulse + decimal getFrictionImpulse1() const; + + /// Return the cached second friction impulse + decimal getFrictionImpulse2() const; + + /// Set the cached penetration impulse + void setPenetrationImpulse(decimal impulse); + + /// Set the first cached friction impulse + void setFrictionImpulse1(decimal impulse); + + /// Set the second cached friction impulse + void setFrictionImpulse2(decimal impulse); + /// Set the contact world point on body 1 void setWorldPointOnBody1(const Vector3& worldPoint); @@ -185,6 +212,36 @@ inline Vector3 ContactPoint::getWorldPointOnBody2() const { return mWorldPointOnBody2; } +// Return the cached penetration impulse +inline decimal ContactPoint::getPenetrationImpulse() const { + return mPenetrationImpulse; +} + +// Return the cached first friction impulse +inline decimal ContactPoint::getFrictionImpulse1() const { + return mFrictionImpulse1; +} + +// Return the cached second friction impulse +inline decimal ContactPoint::getFrictionImpulse2() const { + return mFrictionImpulse2; +} + +// Set the cached penetration impulse +inline void ContactPoint::setPenetrationImpulse(decimal impulse) { + mPenetrationImpulse = impulse; +} + +// Set the first cached friction impulse +inline void ContactPoint::setFrictionImpulse1(decimal impulse) { + mFrictionImpulse1 = impulse; +} + +// Set the second cached friction impulse +inline void ContactPoint::setFrictionImpulse2(decimal impulse) { + mFrictionImpulse2 = impulse; +} + // Set the contact world point on body 1 inline void ContactPoint::setWorldPointOnBody1(const Vector3& worldPoint) { mWorldPointOnBody1 = worldPoint; diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp new file mode 100644 index 00000000..a0131bac --- /dev/null +++ b/src/engine/ConstraintSolver.cpp @@ -0,0 +1,39 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2013 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. * +* * +********************************************************************************/ + +// Libraries +#include "ConstraintSolver.h" + +using namespace reactphysics3d; + +// Constructor +ConstraintSolver::ConstraintSolver() { + +} + +// Destructor +ConstraintSolver::~ConstraintSolver() { + +} diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h new file mode 100644 index 00000000..227c4c9a --- /dev/null +++ b/src/engine/ConstraintSolver.h @@ -0,0 +1,128 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2013 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 REACTPHYSICS3D_CONSTRAINT_SOLVER_H +#define REACTPHYSICS3D_CONSTRAINT_SOLVER_H + +// Libraries + +namespace reactphysics3d { + +// Class ConstraintSolver +/** + * This class represents the constraint solver that is used to solve constraints between + * the rigid bodies. The constraint solver is based on the "Sequential Impulse" technique + * described by Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list). + * + * A constraint between two bodies is represented by a function C(x) which is equal to zero + * when the constraint is satisfied. The condition C(x)=0 describes a valid position and the + * condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is + * the Jacobian matrix of the constraint, v is a vector that contains the velocity of both + * bodies and b is the constraint bias. We are looking for a force F_c that will act on the + * bodies to keep the constraint satisfied. Note that from the virtual work principle, we have + * F_c = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a + * Lagrange multiplier. Therefore, finding the force F_c is equivalent to finding the Lagrange + * multiplier lambda. + + * An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a + * body to change its velocity. The idea of the Sequential Impulse technique is to apply + * impulses to bodies of each constraints in order to keep the constraint satisfied. + * + * --- Step 1 --- + * + * First, we integrate the applied force F_a acting of each rigid body (like gravity, ...) and + * we obtain some new velocities v2' that tends to violate the constraints. + * + * v2' = v1 + dt * M^-1 * F_a + * + * where M is a matrix that contains mass and inertia tensor information. + * + * --- Step 2 --- + * + * During the second step, we iterate over all the constraints for a certain number of + * iterations and for each constraint we compute the impulse to apply to the bodies needed + * so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that + * M * deltaV = P_c where M is the mass of the body, deltaV is the difference of velocity and + * P_c is the constraint impulse to apply to the body. Therefore, we have + * v2 = v2' + M^-1 * P_c. For each constraint, we can compute the Lagrange multiplier lambda + * using : lambda = -m_c (Jv2' + b) where m_c = 1 / (J * M^-1 * J^t). Now that we have the + * Lagrange multiplier lambda, we can compute the impulse P_c = J^t * lambda * dt to apply to + * the bodies to satisfy the constraint. + * + * --- Step 3 --- + * + * In the third step, we integrate the new position x2 of the bodies using the new velocities + * v2 computed in the second step with : x2 = x1 + dt * v2. + * + * Note that in the following code (as it is also explained in the slides from Erin Catto), + * the value lambda is not only the lagrange multiplier but is the multiplication of the + * Lagrange multiplier with the timestep dt. Therefore, in the following code, when we use + * lambda, we mean (lambda * dt). + * + * We are using the accumulated impulse technique that is also described in the slides from + * Erin Catto. + * + * We are also using warm starting. The idea is to warm start the solver at the beginning of + * each step by applying the last impulstes for the constraints that we already existing at the + * previous step. This allows the iterative solver to converge faster towards the solution. + * + * For contact constraints, we are also using split impulses so that the position correction + * that uses Baumgarte stabilization does not change the momentum of the bodies. + * + * There are two ways to apply the friction constraints. Either the friction constraints are + * applied at each contact point or they are applied only at the center of the contact manifold + * between two bodies. If we solve the friction constraints at each contact point, we need + * two constraints (two tangential friction directions) and if we solve the friction + * constraints at the center of the contact manifold, we need two constraints for tangential + * friction but also another twist friction constraint to prevent spin of the body around the + * contact manifold center. + */ +class ConstraintSolver { + + private : + + // -------------------- Attributes -------------------- // + + /// Number of iterations of the contact solver + uint mNbIterations; + + /// Current time step + decimal mTimeStep; + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + ConstraintSolver(); + + /// Destructor + ~ConstraintSolver(); + +}; + +} + +#endif diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index e90ed978..7d786588 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -307,9 +307,9 @@ void ContactSolver::initializeContactConstraints() { if (mIsWarmStartingActive) { // Get the cached accumulated impulses from the previous step - contactPoint.penetrationImpulse = externalContact->getCachedLambda(0); - contactPoint.friction1Impulse = externalContact->getCachedLambda(1); - contactPoint.friction2Impulse = externalContact->getCachedLambda(2); + contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse(); + contactPoint.friction1Impulse = externalContact->getFrictionImpulse1(); + contactPoint.friction2Impulse = externalContact->getFrictionImpulse2(); } // Initialize the split impulses to zero @@ -785,9 +785,9 @@ void ContactSolver::storeImpulses() { ContactPointSolver& contactPoint = manifold.contacts[i]; - contactPoint.externalContact->setCachedLambda(0, contactPoint.penetrationImpulse); - contactPoint.externalContact->setCachedLambda(1, contactPoint.friction1Impulse); - contactPoint.externalContact->setCachedLambda(2, contactPoint.friction2Impulse); + contactPoint.externalContact->setPenetrationImpulse(contactPoint.penetrationImpulse); + contactPoint.externalContact->setFrictionImpulse1(contactPoint.friction1Impulse); + contactPoint.externalContact->setFrictionImpulse2(contactPoint.friction2Impulse); contactPoint.externalContact->setFrictionVector1(contactPoint.frictionVector1); contactPoint.externalContact->setFrictionVector2(contactPoint.frictionVector2); diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index 36997013..1153f306 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -345,7 +345,7 @@ class ContactSolver { /// Reference to the world DynamicsWorld& mWorld; - /// Number of iterations of the constraints solver + /// Number of iterations of the contact solver uint mNbIterations; /// Split linear velocities for the position contact solver (split impulse) From 2b2143d41e1bd5472c220ce5bf3d180902e041a6 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 24 Apr 2013 19:24:28 +0200 Subject: [PATCH 02/24] Continue the implementation of the constraint solver --- src/collision/CollisionDetection.cpp | 12 ++- src/collision/CollisionDetection.h | 2 +- src/collision/ContactInfo.cpp | 38 --------- src/collision/ContactInfo.h | 81 ------------------- .../narrowphase/EPA/EPAAlgorithm.cpp | 4 +- src/collision/narrowphase/EPA/EPAAlgorithm.h | 4 +- .../narrowphase/GJK/GJKAlgorithm.cpp | 12 +-- src/collision/narrowphase/GJK/GJKAlgorithm.h | 6 +- .../narrowphase/NarrowPhaseAlgorithm.h | 6 +- .../narrowphase/SphereVsSphereAlgorithm.cpp | 4 +- .../SphereVsSphereAlgorithm.cpp.autosave | 81 +++++++++++++++++++ .../narrowphase/SphereVsSphereAlgorithm.h | 4 +- src/constraint/BallAndSocketJoint.cpp | 5 +- src/constraint/BallAndSocketJoint.h | 36 ++++++++- src/constraint/Constraint.cpp | 8 +- src/constraint/Constraint.h | 39 ++++++++- src/constraint/ContactPoint.cpp | 15 ++-- src/constraint/ContactPoint.h | 56 ++++++++++++- src/engine/CollisionWorld.cpp | 7 +- src/engine/CollisionWorld.h | 2 +- src/engine/ConstraintSolver.cpp | 8 +- src/engine/ConstraintSolver.h | 25 +++++- src/engine/ContactSolver.cpp | 15 ++-- src/engine/ContactSolver.h | 11 +-- src/engine/DynamicsWorld.cpp | 72 +++++++++++++---- src/engine/DynamicsWorld.h | 78 ++++-------------- 26 files changed, 373 insertions(+), 258 deletions(-) delete mode 100644 src/collision/ContactInfo.cpp delete mode 100644 src/collision/ContactInfo.h create mode 100644 src/collision/narrowphase/SphereVsSphereAlgorithm.cpp.autosave diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index e5359860..46773a1c 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -99,7 +99,7 @@ void CollisionDetection::computeNarrowPhase() { // For each possible collision pair of bodies for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); it++) { - ContactInfo* contactInfo = NULL; + ContactPointInfo* contactInfo = NULL; BroadPhasePair* pair = (*it).second; assert(pair != NULL); @@ -125,12 +125,18 @@ void CollisionDetection::computeNarrowPhase() { contactInfo)) { assert(contactInfo != NULL); + // Set the bodies of the contact + contactInfo->body1 = dynamic_cast(body1); + contactInfo->body2 = dynamic_cast(body2); + assert(contactInfo->body1 != NULL); + assert(contactInfo->body2 != NULL); + // Notify the world about the new narrow-phase contact mWorld->notifyNewContact(pair, contactInfo); // Delete and remove the contact info from the memory allocator - contactInfo->ContactInfo::~ContactInfo(); - mMemoryAllocator.release(contactInfo, sizeof(ContactInfo)); + contactInfo->ContactPointInfo::~ContactPointInfo(); + mMemoryAllocator.release(contactInfo, sizeof(ContactPointInfo)); } } } diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 897103f9..2cb0879a 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -33,7 +33,7 @@ #include "narrowphase/GJK/GJKAlgorithm.h" #include "narrowphase/SphereVsSphereAlgorithm.h" #include "../memory/MemoryAllocator.h" -#include "ContactInfo.h" +#include "../constraint/ContactPoint.h" #include #include #include diff --git a/src/collision/ContactInfo.cpp b/src/collision/ContactInfo.cpp deleted file mode 100644 index b3d7486a..00000000 --- a/src/collision/ContactInfo.cpp +++ /dev/null @@ -1,38 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * -* Copyright (c) 2010-2013 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. * -* * -********************************************************************************/ - -// Libraries -#include "ContactInfo.h" - -using namespace reactphysics3d; - - -// Constructor -ContactInfo::ContactInfo(const Vector3& normal, decimal penetrationDepth, - const Vector3& localPoint1, const Vector3& localPoint2) - : normal(normal), penetrationDepth(penetrationDepth), localPoint1(localPoint1), - localPoint2(localPoint2) { - -} diff --git a/src/collision/ContactInfo.h b/src/collision/ContactInfo.h deleted file mode 100644 index a9d8245a..00000000 --- a/src/collision/ContactInfo.h +++ /dev/null @@ -1,81 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * -* Copyright (c) 2010-2013 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 REACTPHYSICS3D_CONTACT_INFO_H -#define REACTPHYSICS3D_CONTACT_INFO_H - -// Libraries -#include "../collision/shapes/BoxShape.h" -#include "../mathematics/mathematics.h" - -// ReactPhysics3D namespace -namespace reactphysics3d { - -// Structure ContactInfo -/** - * This structure contains informations about a collision contact - * computed during the narrow-phase collision detection. Those - * informations are used to compute the contact set for a contact - * between two bodies. - */ -struct ContactInfo { - - private: - - // -------------------- Methods -------------------- // - - /// Private copy-constructor - ContactInfo(const ContactInfo& contactInfo); - - /// Private assignment operator - ContactInfo& operator=(const ContactInfo& contactInfo); - - public: - - // -------------------- Attributes -------------------- // - - /// Normal vector the the collision contact in world space - const Vector3 normal; - - /// Penetration depth of the contact - const decimal penetrationDepth; - - /// Contact point of body 1 in local space of body 1 - const Vector3 localPoint1; - - /// Contact point of body 2 in local space of body 2 - const Vector3 localPoint2; - - // -------------------- Methods -------------------- // - - /// Constructor - ContactInfo(const Vector3& normal, decimal penetrationDepth, - const Vector3& localPoint1, const Vector3& localPoint2); -}; - -} - -#endif - diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp index d14aff7b..2439979a 100644 --- a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp +++ b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp @@ -87,7 +87,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple const Transform& transform1, const CollisionShape* collisionShape2, const Transform& transform2, - Vector3& v, ContactInfo*& contactInfo) { + Vector3& v, ContactPointInfo*& 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 @@ -400,7 +400,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple assert(penetrationDepth > 0.0); // Create the contact info object - contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactInfo))) ContactInfo(normal, + contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal, penetrationDepth, pALocal, pBLocal); diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.h b/src/collision/narrowphase/EPA/EPAAlgorithm.h index 5b11a2bf..8f13b8a5 100644 --- a/src/collision/narrowphase/EPA/EPAAlgorithm.h +++ b/src/collision/narrowphase/EPA/EPAAlgorithm.h @@ -29,7 +29,7 @@ // Libraries #include "../GJK/Simplex.h" #include "../../shapes/CollisionShape.h" -#include "../../ContactInfo.h" +#include "../../../constraint/ContactPoint.h" #include "../../../mathematics/mathematics.h" #include "TriangleEPA.h" #include "../../../memory/MemoryAllocator.h" @@ -123,7 +123,7 @@ class EPAAlgorithm { const Transform& transform1, const CollisionShape* collisionShape2, const Transform& transform2, - Vector3& v, ContactInfo*& contactInfo); + Vector3& v, ContactPointInfo*& contactInfo); }; // Add a triangle face in the candidate triangle heap in the EPA algorithm diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index b03f3953..16e8f671 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -61,7 +61,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1, const Transform& transform1, const CollisionShape* collisionShape2, const Transform& transform2, - ContactInfo*& contactInfo) { + ContactPointInfo*& contactInfo) { Vector3 suppA; // Support point of object A Vector3 suppB; // Support point of object B @@ -137,7 +137,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1, if (penetrationDepth <= 0.0) return false; // Create the contact info object - contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactInfo))) ContactInfo(normal, + contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal, penetrationDepth, pA, pB); @@ -169,7 +169,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1, if (penetrationDepth <= 0.0) return false; // Create the contact info object - contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactInfo))) ContactInfo(normal, + contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal, penetrationDepth, pA, pB); @@ -199,7 +199,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1, if (penetrationDepth <= 0.0) return false; // Create the contact info object - contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactInfo))) ContactInfo(normal, + contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal, penetrationDepth, pA, pB); @@ -236,7 +236,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1, if (penetrationDepth <= 0.0) return false; // Create the contact info object - contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactInfo))) ContactInfo(normal, + contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal, penetrationDepth, pA, pB); @@ -263,7 +263,7 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap const Transform& transform1, const CollisionShape* collisionShape2, const Transform& transform2, - ContactInfo*& contactInfo, + ContactPointInfo*& contactInfo, Vector3& v) { Simplex simplex; Vector3 suppA; diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h index 5e387b68..5ce89f2e 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.h +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h @@ -28,7 +28,7 @@ // Libraries #include "../NarrowPhaseAlgorithm.h" -#include "../../ContactInfo.h" +#include "../../../constraint/ContactPoint.h" #include "../../../collision/shapes/CollisionShape.h" #include "../EPA/EPAAlgorithm.h" @@ -78,7 +78,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm { const Transform& transform1, const CollisionShape* collisionShape2, const Transform& transform2, - ContactInfo*& contactInfo, Vector3& v); + ContactPointInfo*& contactInfo, Vector3& v); public : @@ -95,7 +95,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm { const Transform& transform1, const CollisionShape* collisionShape2, const Transform& transform2, - ContactInfo*& contactInfo); + ContactPointInfo*& contactInfo); }; } diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h index 998583a3..79ac19d9 100644 --- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h +++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h @@ -28,7 +28,7 @@ // Libraries #include "../../body/Body.h" -#include "../ContactInfo.h" +#include "../../constraint/ContactPoint.h" #include "../broadphase/PairManager.h" #include "../../memory/MemoryAllocator.h" #include "../BroadPhasePair.h" @@ -36,7 +36,7 @@ /// Namespace ReactPhysics3D namespace reactphysics3d { - + // Class NarrowPhaseAlgorithm /** * This class is an abstract class that represents an algorithm @@ -82,7 +82,7 @@ class NarrowPhaseAlgorithm { const Transform& transform1, const CollisionShape* collisionShape2, const Transform& transform2, - ContactInfo*& contactInfo)=0; + ContactPointInfo*& contactInfo)=0; }; // Set the current overlapping pair of bodies diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp index 2f950e04..1a67782a 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -45,7 +45,7 @@ bool SphereVsSphereAlgorithm::testCollision(const CollisionShape* collisionShape const Transform& transform1, const CollisionShape* collisionShape2, const Transform& transform2, - ContactInfo*& contactInfo) { + ContactPointInfo*& contactInfo) { // Get the sphere collision shapes const SphereShape* sphereShape1 = dynamic_cast(collisionShape1); @@ -69,7 +69,7 @@ bool SphereVsSphereAlgorithm::testCollision(const CollisionShape* collisionShape decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters); // Create the contact info object - contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactInfo))) ContactInfo( + contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo( vectorBetweenCenters.getUnit(), penetrationDepth, intersectionOnBody1, intersectionOnBody2); diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp.autosave b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp.autosave new file mode 100644 index 00000000..a4165d64 --- /dev/null +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp.autosave @@ -0,0 +1,81 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2013 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. * +* * +********************************************************************************/ + +// Libraries +#include "SphereVsSphereAlgorithm.h" +#include "../../collision/shapes/SphereShape.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + + +// Constructor +SphereVsSphereAlgorithm::SphereVsSphereAlgorithm(MemoryAllocator& memoryAllocator) + :NarrowPhaseAlgorithm(memoryAllocator) { + +} + +// Destructor +SphereVsSphereAlgorithm::~SphereVsSphereAlgorithm() { + +} + +bool SphereVsSphereAlgorithm::testCollision(const CollisionShape* collisionShape1, + const Transform& transform1, + const CollisionShape* collisionShape2, + const Transform& transform2, + ContactPointInfo*& contactInfo) { + + // Get the sphere collision shapes + const SphereShape* sphereShape1 = dynamic_cast(collisionShape1); + const SphereShape* sphereShape2 = dynamic_cast(collisionShape2); + + // Compute the distance between the centers + Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition(); + decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare(); + + // Compute the sum of the radius + decimal sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius(); + + // If the sphere collision shapes intersect + if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) { + Vector3 centerSphere2InBody1LocalSpace = transform1.inverse() * transform2.getPosition(); + Vector3 centerSphere1InBody2LocalSpace = transform2.inverse() * transform1.getPosition(); + Vector3 intersectionOnBody1 = sphereShape1->getRadius() * + centerSphere2InBody1LocalSpace.getUnit(); + Vector3 intersectionOnBody2 = sphereShape2->getRadius() * + centerSphere1InBody2LocalSpace.getUnit(); + decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters); + + // Create the contact info object + contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo( + vectorBetweenCenters.getUnit(), penetrationDepth, + intersectionOnBody1, intersectionOnBody2); + + return true; + } + + return false; +} diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.h b/src/collision/narrowphase/SphereVsSphereAlgorithm.h index e9320619..d47f55aa 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.h +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.h @@ -28,7 +28,7 @@ // Libraries #include "../../body/Body.h" -#include "../ContactInfo.h" +#include "../../constraint/ContactPoint.h" #include "NarrowPhaseAlgorithm.h" @@ -67,7 +67,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm { const Transform& transform1, const CollisionShape* collisionShape2, const Transform& transform2, - ContactInfo*& contactInfo); + ContactPointInfo*& contactInfo); }; } diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index 51b017c7..17de606d 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -29,9 +29,8 @@ using namespace reactphysics3d; // Constructor -BallAndSocketJoint::BallAndSocketJoint(RigidBody* const body1, RigidBody* const body2, - bool active, ConstraintType type) - : Constraint(body1, body2, active, type){ +BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo &jointInfo) + : Constraint(jointInfo){ } diff --git a/src/constraint/BallAndSocketJoint.h b/src/constraint/BallAndSocketJoint.h index 7908dc8f..e7e3f5ea 100644 --- a/src/constraint/BallAndSocketJoint.h +++ b/src/constraint/BallAndSocketJoint.h @@ -28,9 +28,28 @@ // Libraries #include "Constraint.h" +#include "../mathematics/mathematics.h" namespace reactphysics3d { +// Structure BallAndSocketJointInfo +/** + * This structure is used to gather the information needed to create a ball-and-socket + * joint. This structure will be used to create the actual ball-and-socket joint. + */ +struct BallAndSocketJointInfo : public ConstraintInfo { + + public : + + // -------------------- Attributes -------------------- // + + /// Anchor point (in world space coordinates) + Vector3 anchorPoint; + + /// Constructor + BallAndSocketJointInfo() : ConstraintInfo(BALLSOCKETJOINT) {} +}; + // Class BallAndSocketJoint /** * This class represents a ball-and-socket joint that allows arbitrary rotation @@ -42,18 +61,31 @@ class BallAndSocketJoint : public Constraint { // -------------------- Attributes -------------------- // + /// Anchor point of body 1 (in local space coordinates) + Vector3 mLocalAnchorPoint1; + + /// Anchor point of body 2 (in local space coordinates) + Vector3 mLocalAnchorPoint2; + public : // -------------------- Methods -------------------- // /// Constructor - BallAndSocketJoint(RigidBody* const body1, RigidBody* const body2, - bool active, ConstraintType type); + BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo); /// Destructor virtual ~BallAndSocketJoint(); + + /// Return the number of bytes used by the joint + virtual size_t getSizeInBytes() const; }; +// Return the number of bytes used by the joint +inline size_t BallAndSocketJoint::getSizeInBytes() const { + return sizeof(BallAndSocketJoint); +} + } #endif diff --git a/src/constraint/Constraint.cpp b/src/constraint/Constraint.cpp index a1bd2b76..7ecded94 100644 --- a/src/constraint/Constraint.cpp +++ b/src/constraint/Constraint.cpp @@ -29,10 +29,12 @@ using namespace reactphysics3d; // Constructor -Constraint::Constraint(RigidBody* const body1, RigidBody* const body2, - bool active, ConstraintType type) - :mBody1(body1), mBody2(body2), mActive(active), mType(type) { +Constraint::Constraint(const ConstraintInfo& constraintInfo) + :mBody1(constraintInfo.body1), mBody2(constraintInfo.body2), mActive(true), + mType(constraintInfo.type) { + assert(mBody1 != NULL); + assert(mBody2 != NULL); } // Destructor diff --git a/src/constraint/Constraint.h b/src/constraint/Constraint.h index 9399d84b..de09c956 100644 --- a/src/constraint/Constraint.h +++ b/src/constraint/Constraint.h @@ -36,6 +36,39 @@ namespace reactphysics3d { // Enumeration for the type of a constraint enum ConstraintType {CONTACT, BALLSOCKETJOINT}; +// Structure ConstraintInfo +/** + * This structure is used to gather the information needed to create a constraint. + */ +struct ConstraintInfo { + + public : + + // -------------------- Attributes -------------------- // + + /// First rigid body of the constraint + RigidBody* body1; + + /// Second rigid body of the constraint + RigidBody* body2; + + /// Type of the constraint + ConstraintType type; + + /// Constructor + ConstraintInfo(ConstraintType constraintType) + : body1(NULL), body2(NULL), type(constraintType) {} + + /// Constructor + ConstraintInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, ConstraintType constraintType) + : body1(rigidBody1), body2(rigidBody2), type(constraintType) { + } + + /// Destructor + virtual ~ConstraintInfo() {} + +}; + // Class Constraint /** * This abstract class represents a constraint in the physics engine. @@ -74,8 +107,7 @@ class Constraint { // -------------------- Methods -------------------- // /// Constructor - Constraint(RigidBody* const body1, RigidBody* const body2, - bool active, ConstraintType type); + Constraint(const ConstraintInfo& constraintInfo); /// Destructor virtual ~Constraint(); @@ -91,6 +123,9 @@ class Constraint { /// Return the type of the constraint ConstraintType getType() const; + + /// Return the number of bytes used by the constraint + virtual size_t getSizeInBytes() const = 0; }; // Return the reference to the body 1 diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp index df38e269..93a7ec3d 100644 --- a/src/constraint/ContactPoint.cpp +++ b/src/constraint/ContactPoint.cpp @@ -30,14 +30,13 @@ using namespace reactphysics3d; using namespace std; // Constructor -ContactPoint::ContactPoint(RigidBody* const body1, RigidBody* const body2, - const ContactInfo* contactInfo) - : Constraint(body1, body2, true, CONTACT), mNormal(contactInfo->normal), - mPenetrationDepth(contactInfo->penetrationDepth), - mLocalPointOnBody1(contactInfo->localPoint1), - mLocalPointOnBody2(contactInfo->localPoint2), - mWorldPointOnBody1(body1->getTransform() * contactInfo->localPoint1), - mWorldPointOnBody2(body2->getTransform() * contactInfo->localPoint2), +ContactPoint::ContactPoint(const ContactPointInfo& contactInfo) + : Constraint(contactInfo), mNormal(contactInfo.normal), + mPenetrationDepth(contactInfo.penetrationDepth), + mLocalPointOnBody1(contactInfo.localPoint1), + mLocalPointOnBody2(contactInfo.localPoint2), + mWorldPointOnBody1(contactInfo.body1->getTransform() * contactInfo.localPoint1), + mWorldPointOnBody2(contactInfo.body2->getTransform() * contactInfo.localPoint2), mIsRestingContact(false) { mFrictionVectors[0] = Vector3(0, 0, 0); diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h index 8ad4eff9..14852282 100644 --- a/src/constraint/ContactPoint.h +++ b/src/constraint/ContactPoint.h @@ -28,7 +28,6 @@ // Libraries #include "Constraint.h" -#include "../collision/ContactInfo.h" #include "../body/RigidBody.h" #include "../configuration.h" #include "../mathematics/mathematics.h" @@ -50,6 +49,52 @@ /// ReactPhysics3D namespace namespace reactphysics3d { +// Structure ContactPointInfo +/** + * This structure contains informations about a collision contact + * computed during the narrow-phase collision detection. Those + * informations are used to compute the contact set for a contact + * between two bodies. + */ +struct ContactPointInfo : public ConstraintInfo { + + private: + + // -------------------- Methods -------------------- // + + /// Private copy-constructor + ContactPointInfo(const ContactPointInfo& contactInfo); + + /// Private assignment operator + ContactPointInfo& operator=(const ContactPointInfo& contactInfo); + + public: + + // -------------------- Attributes -------------------- // + + /// Normal vector the the collision contact in world space + const Vector3 normal; + + /// Penetration depth of the contact + const decimal penetrationDepth; + + /// Contact point of body 1 in local space of body 1 + const Vector3 localPoint1; + + /// Contact point of body 2 in local space of body 2 + const Vector3 localPoint2; + + // -------------------- Methods -------------------- // + + /// Constructor + ContactPointInfo(const Vector3& normal, decimal penetrationDepth, + const Vector3& localPoint1, const Vector3& localPoint2) + : ConstraintInfo(CONTACT), normal(normal), penetrationDepth(penetrationDepth), + localPoint1(localPoint1), localPoint2(localPoint2) { + + } +}; + // Class ContactPoint /** * This class represents a collision contact point between two @@ -108,7 +153,7 @@ class ContactPoint : public Constraint { // -------------------- Methods -------------------- // /// Constructor - ContactPoint(RigidBody* const body1, RigidBody* const body2, const ContactInfo* contactInfo); + ContactPoint(const ContactPointInfo& contactInfo); /// Destructor virtual ~ContactPoint(); @@ -176,6 +221,9 @@ class ContactPoint : public Constraint { /// Return the penetration depth decimal getPenetrationDepth() const; + /// Return the number of bytes used by the contact point + virtual size_t getSizeInBytes() const; + #ifdef VISUAL_DEBUG /// Draw the contact (for debugging) void draw() const; @@ -287,6 +335,10 @@ inline decimal ContactPoint::getPenetrationDepth() const { return mPenetrationDepth; } +// Return the number of bytes used by the contact point +inline size_t ContactPoint::getSizeInBytes() const { + return sizeof(ContactPoint); +} #ifdef VISUAL_DEBUG inline void ContactPoint::draw() const { diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index 3a048d09..80e73e87 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -56,7 +56,7 @@ void CollisionWorld::notifyRemovedOverlappingPair(const BroadPhasePair* removedP // Notify the world about a new narrow-phase contact void CollisionWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, - const ContactInfo* contactInfo) { + const ContactPointInfo* contactInfo) { // TODO : Implement this method } @@ -101,7 +101,7 @@ void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) { // Add the body ID to the list of free IDs mFreeBodiesIDs.push_back(collisionBody->getID()); - // Call the constructor of the collision body + // Call the destructor of the collision body collisionBody->CollisionBody::~CollisionBody(); // Remove the collision body from the list of bodies @@ -181,6 +181,9 @@ void CollisionWorld::removeCollisionShape(CollisionShape* collisionShape) { // Remove the shape from the set of shapes in the world mCollisionShapes.remove(collisionShape); + // Call the destructor of the collision shape + collisionShape->CollisionShape::~CollisionShape(); + // Deallocate the memory used by the collision shape mMemoryAllocator.release(collisionShape, collisionShape->getSizeInBytes()); } diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index a120956b..89f38c5c 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -91,7 +91,7 @@ class CollisionWorld { virtual void notifyRemovedOverlappingPair(const BroadPhasePair* removedPair); /// Notify the world about a new narrow-phase contact - virtual void notifyNewContact(const BroadPhasePair* pair, const ContactInfo* contactInfo); + virtual void notifyNewContact(const BroadPhasePair* pair, const ContactPointInfo* contactInfo); /// Update the overlapping pair virtual void updateOverlappingPair(const BroadPhasePair* pair); diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index a0131bac..5fc41c2a 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -29,7 +29,13 @@ using namespace reactphysics3d; // Constructor -ConstraintSolver::ConstraintSolver() { +ConstraintSolver::ConstraintSolver(std::set& joints, + std::vector& constrainedLinearVelocities, + std::vector& constrainedAngularVelocities, + const std::map& mapBodyToVelocityIndex) + : mJoints(joints), mConstrainedLinearVelocities(constrainedLinearVelocities), + mConstrainedAngularVelocities(constrainedAngularVelocities), + mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex) { } diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index 227c4c9a..5dce0686 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -27,6 +27,11 @@ #define REACTPHYSICS3D_CONSTRAINT_SOLVER_H // Libraries +#include "../configuration.h" +#include "mathematics/mathematics.h" +#include "../constraint/Constraint.h" +#include +#include namespace reactphysics3d { @@ -105,6 +110,20 @@ class ConstraintSolver { // -------------------- Attributes -------------------- // + /// Reference to all the joints of the world + std::set& mJoints; + + /// Reference to the array of constrained linear velocities (state of the linear velocities + /// after solving the constraints) + std::vector& mConstrainedLinearVelocities; + + /// Reference to the array of constrained angular velocities (state of the angular velocities + /// after solving the constraints) + std::vector& mConstrainedAngularVelocities; + + /// Reference to the map of rigid body to their index in the constrained velocities array + const std::map& mMapBodyToConstrainedVelocityIndex; + /// Number of iterations of the contact solver uint mNbIterations; @@ -116,11 +135,13 @@ class ConstraintSolver { // -------------------- Methods -------------------- // /// Constructor - ConstraintSolver(); + ConstraintSolver(std::set& joints, + std::vector& constrainedLinearVelocities, + std::vector& constrainedAngularVelocities, + const std::map& mapBodyToVelocityIndex); /// Destructor ~ConstraintSolver(); - }; } diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 7d786588..94d58943 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -39,10 +39,12 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2); const decimal ContactSolver::SLOP = decimal(0.01); // Constructor -ContactSolver::ContactSolver(DynamicsWorld& world,std::vector& constrainedLinearVelocities, +ContactSolver::ContactSolver(std::vector& contactManifolds, + std::vector& constrainedLinearVelocities, std::vector& constrainedAngularVelocities, const std::map& mapBodyToVelocityIndex) - :mWorld(world), mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS), + :mContactManifolds(contactManifolds), + mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS), mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL), mContactConstraints(NULL), mConstrainedLinearVelocities(constrainedLinearVelocities), @@ -62,14 +64,13 @@ ContactSolver::~ContactSolver() { void ContactSolver::initialize() { // TODO : Use better memory allocation here - mContactConstraints = new ContactManifoldSolver[mWorld.getNbContactManifolds()]; + mContactConstraints = new ContactManifoldSolver[mContactManifolds.size()]; mNbContactManifolds = 0; // For each contact manifold of the world vector::iterator it; - for (it = mWorld.getContactManifoldsBeginIterator(); - it != mWorld.getContactManifoldsEndIterator(); ++it) { + for (it = mContactManifolds.begin(); it != mContactManifolds.end(); ++it) { ContactManifold* externalManifold = *it; @@ -174,8 +175,8 @@ void ContactSolver::initialize() { // Allocated memory for split impulse velocities // TODO : Use better memory allocation here - mSplitLinearVelocities = new Vector3[mWorld.getNbRigidBodies()]; - mSplitAngularVelocities = new Vector3[mWorld.getNbRigidBodies()]; + mSplitLinearVelocities = new Vector3[mMapBodyToConstrainedVelocityIndex.size()]; + mSplitAngularVelocities = new Vector3[mMapBodyToConstrainedVelocityIndex.size()]; assert(mSplitLinearVelocities != NULL); assert(mSplitAngularVelocities != NULL); diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index 1153f306..97ec39b5 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -342,8 +342,8 @@ class ContactSolver { // -------------------- Attributes -------------------- // - /// Reference to the world - DynamicsWorld& mWorld; + /// Reference to all the contact manifold of the world + std::vector& mContactManifolds; /// Number of iterations of the contact solver uint mNbIterations; @@ -366,11 +366,11 @@ class ContactSolver { /// Constrained bodies std::set mConstraintBodies; - /// Pointer to the array of constrained linear velocities (state of the linear velocities + /// Reference to the array of constrained linear velocities (state of the linear velocities /// after solving the constraints) std::vector& mConstrainedLinearVelocities; - /// Pointer to the array of constrained angular velocities (state of the angular velocities + /// Reference to the array of constrained angular velocities (state of the angular velocities /// after solving the constraints) std::vector& mConstrainedAngularVelocities; @@ -452,7 +452,8 @@ class ContactSolver { // -------------------- Methods -------------------- // /// Constructor - ContactSolver(DynamicsWorld& mWorld, std::vector& constrainedLinearVelocities, + ContactSolver(std::vector& contactManifolds, + std::vector& constrainedLinearVelocities, std::vector& constrainedAngularVelocities, const std::map& mapBodyToVelocityIndex); diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 13d79138..dd319483 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -25,6 +25,7 @@ // Libraries #include "DynamicsWorld.h" +#include "constraint/BallAndSocketJoint.h" // Namespaces using namespace reactphysics3d; @@ -33,8 +34,10 @@ using namespace std; // Constructor DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP) : CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true), - mContactSolver(*this, mConstrainedLinearVelocities, mConstrainedAngularVelocities, + mContactSolver(mContactManifolds, mConstrainedLinearVelocities, mConstrainedAngularVelocities, mMapBodyToConstrainedVelocityIndex), + mConstraintSolver(mJoints, mConstrainedLinearVelocities, mConstrainedAngularVelocities, + mMapBodyToConstrainedVelocityIndex), mIsDeactivationActive(DEACTIVATION_ENABLED) { } @@ -221,6 +224,8 @@ void DynamicsWorld::initConstrainedVelocitiesArray() { i++; } + + assert(mMapBodyToConstrainedVelocityIndex.size() == mRigidBodies.size()); } // Cleanup the constrained velocities array at each step @@ -230,6 +235,9 @@ void DynamicsWorld::cleanupConstrainedVelocitiesArray() { mConstrainedLinearVelocities.clear(); mConstrainedAngularVelocities.clear(); + // Clear the constrained bodies + mConstrainedBodies.clear(); + // Clear the rigid body to velocities array index mapping mMapBodyToConstrainedVelocityIndex.clear(); } @@ -300,7 +308,7 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { // Remove the collision shape from the world removeCollisionShape(rigidBody->getCollisionShape()); - // Call the constructor of the rigid body + // Call the destructor of the rigid body rigidBody->RigidBody::~RigidBody(); // Remove the rigid body from the list of rigid bodies @@ -311,9 +319,51 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { mMemoryAllocator.release(rigidBody, sizeof(RigidBody)); } -// Remove all constraints in the physics world -void DynamicsWorld::removeAllConstraints() { - mConstraints.clear(); +// Create a joint between two bodies in the world and return a pointer to the new joint +Constraint* DynamicsWorld::createJoint(const ConstraintInfo& jointInfo) { + + Constraint* newJoint = NULL; + + // Allocate memory to create the new joint + switch(jointInfo.type) { + + // Ball-and-Socket joint + case BALLSOCKETJOINT: + { + void* allocatedMemory = mMemoryAllocator.allocate(sizeof(BallAndSocketJoint)); + const BallAndSocketJointInfo& info = dynamic_cast( + jointInfo); + newJoint = new (allocatedMemory) BallAndSocketJoint(info); + break; + } + + default: + { + assert(false); + return NULL; + } + } + + // Add the joint into the world + mJoints.insert(newJoint); + + // Return the pointer to the created joint + return newJoint; +} + +// Destroy a joint +void DynamicsWorld::destroyJoint(Constraint* joint) { + + assert(joint != NULL); + + // Remove the joint from the world + mJoints.erase(joint); + + // Call the destructor of the joint + joint->Constraint::~Constraint(); + + // Release the allocated memory + mMemoryAllocator.release(joint, joint->getSizeInBytes()); } // Notify the world about a new broad-phase overlapping pair @@ -345,19 +395,11 @@ void DynamicsWorld::notifyRemovedOverlappingPair(const BroadPhasePair* removedPa // Notify the world about a new narrow-phase contact void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, - const ContactInfo* contactInfo) { - - RigidBody* const rigidBody1 = dynamic_cast(broadPhasePair->body1); - RigidBody* const rigidBody2 = dynamic_cast(broadPhasePair->body2); - - assert(rigidBody1 != NULL); - assert(rigidBody2 != NULL); + const ContactPointInfo* contactInfo) { // Create a new contact ContactPoint* contact = new (mMemoryAllocator.allocate(sizeof(ContactPoint))) ContactPoint( - rigidBody1, - rigidBody2, - contactInfo); + *contactInfo); assert(contact != NULL); // Get the corresponding overlapping pair diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 93b87bf2..de1edd7c 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -30,6 +30,7 @@ #include "CollisionWorld.h" #include "../collision/CollisionDetection.h" #include "ContactSolver.h" +#include "ConstraintSolver.h" #include "../body/RigidBody.h" #include "Timer.h" #include "../configuration.h" @@ -55,6 +56,9 @@ class DynamicsWorld : public CollisionWorld { /// Contact solver ContactSolver mContactSolver; + /// Constraint solver + ConstraintSolver mConstraintSolver; + /// True if the deactivation (sleeping) of inactive bodies is enabled bool mIsDeactivationActive; @@ -64,8 +68,11 @@ class DynamicsWorld : public CollisionWorld { /// All the contact constraints std::vector mContactManifolds; - /// All the constraints (except contact constraints) - std::vector mConstraints; + /// All the joints of the world + std::set mJoints; + + /// All the bodies that are part of contacts or constraints + std::set mConstrainedBodies; /// Gravity vector of the world Vector3 mGravity; @@ -124,7 +131,7 @@ class DynamicsWorld : public CollisionWorld { virtual void notifyRemovedOverlappingPair(const BroadPhasePair* removedPair); /// Notify the world about a new narrow-phase contact - virtual void notifyNewContact(const BroadPhasePair* pair, const ContactInfo* contactInfo); + virtual void notifyNewContact(const BroadPhasePair* pair, const ContactPointInfo* contactInfo); public : @@ -166,6 +173,12 @@ public : /// Destroy a rigid body void destroyRigidBody(RigidBody* rigidBody); + /// Create a joint between two bodies in the world and return a pointer to the new joint + Constraint* createJoint(const ConstraintInfo& jointInfo); + + /// Destroy a joint + void destroyJoint(Constraint* joint); + /// Return the gravity vector of the world Vector3 getGravity() const; @@ -178,30 +191,9 @@ public : /// Return the number of rigid bodies in the world uint getNbRigidBodies() const; - /// Add a constraint - void addConstraint(Constraint* constraint); - - /// Remove a constraint - void removeConstraint(Constraint* constraint); - - /// Remove all constraints and delete them (free their memory) - void removeAllConstraints(); - /// Return the number of contact constraints in the world uint getNbContactManifolds() const; - /// Return a start iterator on the constraint list - std::vector::iterator getConstraintsBeginIterator(); - - /// Return a end iterator on the constraint list - std::vector::iterator getConstraintsEndIterator(); - - /// Return a start iterator on the contact manifolds list - std::vector::iterator getContactManifoldsBeginIterator(); - - /// Return a end iterator on the contact manifolds list - std::vector::iterator getContactManifoldsEndIterator(); - /// Return an iterator to the beginning of the rigid bodies of the physics world std::set::iterator getRigidBodiesBeginIterator(); @@ -259,24 +251,6 @@ inline void DynamicsWorld::updateOverlappingPair(const BroadPhasePair* pair) { overlappingPair->update(); } - -// Add a constraint into the physics world -inline void DynamicsWorld::addConstraint(Constraint* constraint) { - assert(constraint != 0); - mConstraints.push_back(constraint); -} - -// Remove a constraint and free its memory -inline void DynamicsWorld::removeConstraint(Constraint* constraint) { - std::vector::iterator it; - - assert(constraint != NULL); - it = std::find(mConstraints.begin(), mConstraints.end(), constraint); - assert(*it == constraint); - delete *it; - mConstraints.erase(it); -} - // Return the gravity vector of the world inline Vector3 DynamicsWorld::getGravity() const { return mGravity; @@ -312,26 +286,6 @@ inline uint DynamicsWorld::getNbContactManifolds() const { return mContactManifolds.size(); } -// Return a start iterator on the constraint list -inline std::vector::iterator DynamicsWorld::getConstraintsBeginIterator() { - return mConstraints.begin(); -} - -// Return a end iterator on the constraint list -inline std::vector::iterator DynamicsWorld::getConstraintsEndIterator() { - return mConstraints.end(); -} - -// Return a start iterator on the contact manifolds list -inline std::vector::iterator DynamicsWorld::getContactManifoldsBeginIterator() { - return mContactManifolds.begin(); -} - -// Return a end iterator on the contact manifolds list -inline std::vector::iterator DynamicsWorld::getContactManifoldsEndIterator() { - return mContactManifolds.end(); -} - } #endif From ded465c10508f902fdac58506a841e11b97bbd85 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 24 Apr 2013 22:29:31 +0200 Subject: [PATCH 03/24] Remove a file --- .../SphereVsSphereAlgorithm.cpp.autosave | 81 ------------------- 1 file changed, 81 deletions(-) delete mode 100644 src/collision/narrowphase/SphereVsSphereAlgorithm.cpp.autosave diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp.autosave b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp.autosave deleted file mode 100644 index a4165d64..00000000 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp.autosave +++ /dev/null @@ -1,81 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * -* Copyright (c) 2010-2013 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. * -* * -********************************************************************************/ - -// Libraries -#include "SphereVsSphereAlgorithm.h" -#include "../../collision/shapes/SphereShape.h" - -// We want to use the ReactPhysics3D namespace -using namespace reactphysics3d; - - -// Constructor -SphereVsSphereAlgorithm::SphereVsSphereAlgorithm(MemoryAllocator& memoryAllocator) - :NarrowPhaseAlgorithm(memoryAllocator) { - -} - -// Destructor -SphereVsSphereAlgorithm::~SphereVsSphereAlgorithm() { - -} - -bool SphereVsSphereAlgorithm::testCollision(const CollisionShape* collisionShape1, - const Transform& transform1, - const CollisionShape* collisionShape2, - const Transform& transform2, - ContactPointInfo*& contactInfo) { - - // Get the sphere collision shapes - const SphereShape* sphereShape1 = dynamic_cast(collisionShape1); - const SphereShape* sphereShape2 = dynamic_cast(collisionShape2); - - // Compute the distance between the centers - Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition(); - decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare(); - - // Compute the sum of the radius - decimal sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius(); - - // If the sphere collision shapes intersect - if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) { - Vector3 centerSphere2InBody1LocalSpace = transform1.inverse() * transform2.getPosition(); - Vector3 centerSphere1InBody2LocalSpace = transform2.inverse() * transform1.getPosition(); - Vector3 intersectionOnBody1 = sphereShape1->getRadius() * - centerSphere2InBody1LocalSpace.getUnit(); - Vector3 intersectionOnBody2 = sphereShape2->getRadius() * - centerSphere1InBody2LocalSpace.getUnit(); - decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters); - - // Create the contact info object - contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo( - vectorBetweenCenters.getUnit(), penetrationDepth, - intersectionOnBody1, intersectionOnBody2); - - return true; - } - - return false; -} From fdda0b26a92c603e8b52b6b9aa4f94fddf3b29d7 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 25 Apr 2013 22:34:20 +0200 Subject: [PATCH 04/24] Modify the contact solver so that its main loop is outside the solver --- examples/fallingcubes/FallingCubes.cpp | 6 +- src/engine/CollisionWorld.cpp | 6 +- src/engine/ConstraintSolver.cpp | 17 ++ src/engine/ConstraintSolver.h | 6 + src/engine/ContactSolver.cpp | 405 ++++++++++++------------- src/engine/ContactSolver.h | 38 +-- src/engine/DynamicsWorld.cpp | 72 +++-- src/engine/DynamicsWorld.h | 19 +- 8 files changed, 301 insertions(+), 268 deletions(-) diff --git a/examples/fallingcubes/FallingCubes.cpp b/examples/fallingcubes/FallingCubes.cpp index 65f8f485..54bbfa62 100644 --- a/examples/fallingcubes/FallingCubes.cpp +++ b/examples/fallingcubes/FallingCubes.cpp @@ -66,12 +66,11 @@ int main(int argc, char** argv) { glutMouseFunc(mouseButton); glutMotionFunc(mouseMotion); glutKeyboardFunc(keyboard); + glutCloseFunc(finish); // Glut main looop glutMainLoop(); - finish(); - return 0; } @@ -115,8 +114,7 @@ void keyboard(unsigned char key, int x, int y) { // Escape key case 27: - finish(); - exit(0); + glutLeaveMainLoop(); break; // Space bar diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index 80e73e87..2f2d4d81 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -154,6 +154,7 @@ CollisionShape* CollisionWorld::createCollisionShape(const CollisionShape& colli // A similar collision shape does not already exist in the world, so we create a // new one and add it to the world void* allocatedMemory = mMemoryAllocator.allocate(collisionShape.getSizeInBytes()); + size_t test = collisionShape.getSizeInBytes(); CollisionShape* newCollisionShape = collisionShape.clone(allocatedMemory); mCollisionShapes.push_back(newCollisionShape); @@ -181,11 +182,14 @@ void CollisionWorld::removeCollisionShape(CollisionShape* collisionShape) { // Remove the shape from the set of shapes in the world mCollisionShapes.remove(collisionShape); + // Compute the size (in bytes) of the collision shape + size_t nbBytesShape = collisionShape->getSizeInBytes(); + // Call the destructor of the collision shape collisionShape->CollisionShape::~CollisionShape(); // Deallocate the memory used by the collision shape - mMemoryAllocator.release(collisionShape, collisionShape->getSizeInBytes()); + mMemoryAllocator.release(collisionShape, nbBytesShape); } } diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 5fc41c2a..0291eee4 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -25,6 +25,7 @@ // Libraries #include "ConstraintSolver.h" +#include "Profiler.h" using namespace reactphysics3d; @@ -43,3 +44,19 @@ ConstraintSolver::ConstraintSolver(std::set& joints, ConstraintSolver::~ConstraintSolver() { } + +// Initialize the constraint solver +void ConstraintSolver::initialize(decimal dt) { + + PROFILE("ConstraintSolver::initialize()"); + + // Set the current time step + mTimeStep = dt; +} + +// Solve the constraints +void ConstraintSolver::solve() { + + PROFILE("ConstraintSolver::solve()"); + +} diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index 5dce0686..08e4a08f 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -142,6 +142,12 @@ class ConstraintSolver { /// Destructor ~ConstraintSolver(); + + /// Initialize the constraint solver + void initialize(decimal dt); + + /// Solve the constraints + void solve(); }; } diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 94d58943..3399ac0d 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -44,7 +44,6 @@ ContactSolver::ContactSolver(std::vector& contactManifolds, std::vector& constrainedAngularVelocities, const std::map& mapBodyToVelocityIndex) :mContactManifolds(contactManifolds), - mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS), mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL), mContactConstraints(NULL), mConstrainedLinearVelocities(constrainedLinearVelocities), @@ -61,7 +60,12 @@ ContactSolver::~ContactSolver() { } // Initialize the constraint solver -void ContactSolver::initialize() { +void ContactSolver::initialize(decimal dt) { + + PROFILE("ContactSolver::initialize()"); + + // Set the current time step + mTimeStep = dt; // TODO : Use better memory allocation here mContactConstraints = new ContactManifoldSolver[mContactManifolds.size()]; @@ -187,6 +191,9 @@ void ContactSolver::initialize() { // Initialize the split impulse velocities initializeSplitImpulseVelocities(); + + // Fill-in all the matrices needed to solve the LCP problem + initializeContactConstraints(); } // Initialize the split impulse velocities @@ -380,6 +387,9 @@ void ContactSolver::initializeContactConstraints() { /// the solution of the linear system void ContactSolver::warmStart() { + // Check that warm starting is active + if (!mIsWarmStartingActive) return; + // For each constraint for (uint c=0; c SLOP) biasPenetrationDepth = -(beta/mTimeStep) * + max(0.0f, float(contactPoint.penetrationDepth - SLOP)); + decimal b = biasPenetrationDepth + contactPoint.restitutionBias; - // Compute the bias "b" of the constraint - decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA; - decimal biasPenetrationDepth = 0.0; - if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) * - max(0.0f, float(contactPoint.penetrationDepth - SLOP)); - decimal b = biasPenetrationDepth + contactPoint.restitutionBias; + // Compute the Lagrange multiplier lambda + if (mIsSplitImpulseActive) { + deltaLambda = - (Jv + contactPoint.restitutionBias) * + contactPoint.inversePenetrationMass; + } + else { + deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass; + } + lambdaTemp = contactPoint.penetrationImpulse; + contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse + + deltaLambda, decimal(0.0)); + deltaLambda = contactPoint.penetrationImpulse - lambdaTemp; - // Compute the Lagrange multiplier lambda - if (mIsSplitImpulseActive) { - deltaLambda = - (Jv + contactPoint.restitutionBias) * - contactPoint.inversePenetrationMass; - } - else { - deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass; - } - lambdaTemp = contactPoint.penetrationImpulse; - contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse + - deltaLambda, decimal(0.0)); - deltaLambda = contactPoint.penetrationImpulse - lambdaTemp; + // Compute the impulse P=J^T * lambda + const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda, + contactPoint); + + // Apply the impulse to the bodies of the constraint + applyImpulse(impulsePenetration, contactManifold); + + sumPenetrationImpulse += contactPoint.penetrationImpulse; + + // If the split impulse position correction is active + if (mIsSplitImpulseActive) { + + // Split impulse (position correction) + const Vector3& v1Split = mSplitLinearVelocities[contactManifold.indexBody1]; + const Vector3& w1Split = mSplitAngularVelocities[contactManifold.indexBody1]; + const Vector3& v2Split = mSplitLinearVelocities[contactManifold.indexBody2]; + const Vector3& w2Split = mSplitAngularVelocities[contactManifold.indexBody2]; + Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) - + v1Split - w1Split.cross(contactPoint.r1); + decimal JvSplit = deltaVSplit.dot(contactPoint.normal); + decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * + contactPoint.inversePenetrationMass; + decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse; + contactPoint.penetrationSplitImpulse = std::max( + contactPoint.penetrationSplitImpulse + + deltaLambdaSplit, decimal(0.0)); + deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit; // Compute the impulse P=J^T * lambda - const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda, - contactPoint); + const Impulse splitImpulsePenetration = computePenetrationImpulse( + deltaLambdaSplit, contactPoint); - // Apply the impulse to the bodies of the constraint - applyImpulse(impulsePenetration, contactManifold); - - sumPenetrationImpulse += contactPoint.penetrationImpulse; - - // If the split impulse position correction is active - if (mIsSplitImpulseActive) { - - // Split impulse (position correction) - const Vector3& v1Split = mSplitLinearVelocities[contactManifold.indexBody1]; - const Vector3& w1Split = mSplitAngularVelocities[contactManifold.indexBody1]; - const Vector3& v2Split = mSplitLinearVelocities[contactManifold.indexBody2]; - const Vector3& w2Split = mSplitAngularVelocities[contactManifold.indexBody2]; - Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) - - v1Split - w1Split.cross(contactPoint.r1); - decimal JvSplit = deltaVSplit.dot(contactPoint.normal); - decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * - contactPoint.inversePenetrationMass; - decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse; - contactPoint.penetrationSplitImpulse = std::max( - contactPoint.penetrationSplitImpulse + - deltaLambdaSplit, decimal(0.0)); - deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit; - - // Compute the impulse P=J^T * lambda - const Impulse splitImpulsePenetration = computePenetrationImpulse( - deltaLambdaSplit, contactPoint); - - applySplitImpulse(splitImpulsePenetration, contactManifold); - } - - // If we do not solve the friction constraints at the center of the contact manifold - if (!mIsSolveFrictionAtContactManifoldCenterActive) { - - // --------- Friction 1 --------- // - - // Compute J*v - deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); - Jv = deltaV.dot(contactPoint.frictionVector1); - - // Compute the Lagrange multiplier lambda - deltaLambda = -Jv; - deltaLambda *= contactPoint.inverseFriction1Mass; - decimal frictionLimit = contactManifold.frictionCoefficient * - contactPoint.penetrationImpulse; - lambdaTemp = contactPoint.friction1Impulse; - contactPoint.friction1Impulse = std::max(-frictionLimit, - std::min(contactPoint.friction1Impulse - + deltaLambda, frictionLimit)); - deltaLambda = contactPoint.friction1Impulse - lambdaTemp; - - // Compute the impulse P=J^T * lambda - const Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda, - contactPoint); - - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseFriction1, contactManifold); - - // --------- Friction 2 --------- // - - // Compute J*v - deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); - Jv = deltaV.dot(contactPoint.frictionVector2); - - // Compute the Lagrange multiplier lambda - deltaLambda = -Jv; - deltaLambda *= contactPoint.inverseFriction2Mass; - frictionLimit = contactManifold.frictionCoefficient * - contactPoint.penetrationImpulse; - lambdaTemp = contactPoint.friction2Impulse; - contactPoint.friction2Impulse = std::max(-frictionLimit, - std::min(contactPoint.friction2Impulse - + deltaLambda, frictionLimit)); - deltaLambda = contactPoint.friction2Impulse - lambdaTemp; - - // Compute the impulse P=J^T * lambda - const Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda, - contactPoint); - - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseFriction2, contactManifold); - } + applySplitImpulse(splitImpulsePenetration, contactManifold); } - // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { + // If we do not solve the friction constraints at the center of the contact manifold + if (!mIsSolveFrictionAtContactManifoldCenterActive) { - // ------ First friction constraint at the center of the contact manifol ------ // + // --------- Friction 1 --------- // // Compute J*v - Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction) - - v1 - w1.cross(contactManifold.r1Friction); - decimal Jv = deltaV.dot(contactManifold.frictionVector1); + deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); + Jv = deltaV.dot(contactPoint.frictionVector1); // Compute the Lagrange multiplier lambda - decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass; - decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; - lambdaTemp = contactManifold.friction1Impulse; - contactManifold.friction1Impulse = std::max(-frictionLimit, - std::min(contactManifold.friction1Impulse + - deltaLambda, frictionLimit)); - deltaLambda = contactManifold.friction1Impulse - lambdaTemp; + deltaLambda = -Jv; + deltaLambda *= contactPoint.inverseFriction1Mass; + decimal frictionLimit = contactManifold.frictionCoefficient * + contactPoint.penetrationImpulse; + lambdaTemp = contactPoint.friction1Impulse; + contactPoint.friction1Impulse = std::max(-frictionLimit, + std::min(contactPoint.friction1Impulse + + deltaLambda, frictionLimit)); + deltaLambda = contactPoint.friction1Impulse - lambdaTemp; // Compute the impulse P=J^T * lambda - Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda; - Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda; - Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda; - Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda; - const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); + const Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda, + contactPoint); // Apply the impulses to the bodies of the constraint applyImpulse(impulseFriction1, contactManifold); - // ------ Second friction constraint at the center of the contact manifol ----- // + // --------- Friction 2 --------- // // Compute J*v - deltaV = v2 + w2.cross(contactManifold.r2Friction) - - v1 - w1.cross(contactManifold.r1Friction); - Jv = deltaV.dot(contactManifold.frictionVector2); + deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); + Jv = deltaV.dot(contactPoint.frictionVector2); // Compute the Lagrange multiplier lambda - deltaLambda = -Jv * contactManifold.inverseFriction2Mass; - frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; - lambdaTemp = contactManifold.friction2Impulse; - contactManifold.friction2Impulse = std::max(-frictionLimit, - std::min(contactManifold.friction2Impulse + - deltaLambda, frictionLimit)); - deltaLambda = contactManifold.friction2Impulse - lambdaTemp; + deltaLambda = -Jv; + deltaLambda *= contactPoint.inverseFriction2Mass; + frictionLimit = contactManifold.frictionCoefficient * + contactPoint.penetrationImpulse; + lambdaTemp = contactPoint.friction2Impulse; + contactPoint.friction2Impulse = std::max(-frictionLimit, + std::min(contactPoint.friction2Impulse + + deltaLambda, frictionLimit)); + deltaLambda = contactPoint.friction2Impulse - lambdaTemp; // Compute the impulse P=J^T * lambda - linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda; - angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda; - linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda; - angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda; - const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); + const Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda, + contactPoint); // Apply the impulses to the bodies of the constraint applyImpulse(impulseFriction2, contactManifold); - - // ------ Twist friction constraint at the center of the contact manifol ------ // - - // Compute J*v - deltaV = w2 - w1; - Jv = deltaV.dot(contactManifold.normal); - - deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass); - frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; - lambdaTemp = contactManifold.frictionTwistImpulse; - contactManifold.frictionTwistImpulse = std::max(-frictionLimit, - std::min(contactManifold.frictionTwistImpulse - + deltaLambda, frictionLimit)); - deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp; - - // Compute the impulse P=J^T * lambda - linearImpulseBody1 = Vector3(0.0, 0.0, 0.0); - angularImpulseBody1 = -contactManifold.normal * deltaLambda; - linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);; - angularImpulseBody2 = contactManifold.normal * deltaLambda; - const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); - - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseTwistFriction, contactManifold); } } + + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + + // ------ First friction constraint at the center of the contact manifol ------ // + + // Compute J*v + Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction) + - v1 - w1.cross(contactManifold.r1Friction); + decimal Jv = deltaV.dot(contactManifold.frictionVector1); + + // Compute the Lagrange multiplier lambda + decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass; + decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; + lambdaTemp = contactManifold.friction1Impulse; + contactManifold.friction1Impulse = std::max(-frictionLimit, + std::min(contactManifold.friction1Impulse + + deltaLambda, frictionLimit)); + deltaLambda = contactManifold.friction1Impulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda; + Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda; + Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda; + Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda; + const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction1, contactManifold); + + // ------ Second friction constraint at the center of the contact manifol ----- // + + // Compute J*v + deltaV = v2 + w2.cross(contactManifold.r2Friction) + - v1 - w1.cross(contactManifold.r1Friction); + Jv = deltaV.dot(contactManifold.frictionVector2); + + // Compute the Lagrange multiplier lambda + deltaLambda = -Jv * contactManifold.inverseFriction2Mass; + frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; + lambdaTemp = contactManifold.friction2Impulse; + contactManifold.friction2Impulse = std::max(-frictionLimit, + std::min(contactManifold.friction2Impulse + + deltaLambda, frictionLimit)); + deltaLambda = contactManifold.friction2Impulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda; + angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda; + linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda; + angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda; + const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction2, contactManifold); + + // ------ Twist friction constraint at the center of the contact manifol ------ // + + // Compute J*v + deltaV = w2 - w1; + Jv = deltaV.dot(contactManifold.normal); + + deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass); + frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; + lambdaTemp = contactManifold.frictionTwistImpulse; + contactManifold.frictionTwistImpulse = std::max(-frictionLimit, + std::min(contactManifold.frictionTwistImpulse + + deltaLambda, frictionLimit)); + deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + linearImpulseBody1 = Vector3(0.0, 0.0, 0.0); + angularImpulseBody1 = -contactManifold.normal * deltaLambda; + linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);; + angularImpulseBody2 = contactManifold.normal * deltaLambda; + const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseTwistFriction, contactManifold); + } } } -// Solve the constraints -void ContactSolver::solve(decimal timeStep) { - - PROFILE("ContactSolver::solve()"); - - // Set the current time step - mTimeStep = timeStep; - - // Initialize the solver - initialize(); - - // Fill-in all the matrices needed to solve the LCP problem - initializeContactConstraints(); - - // Warm start the solver - if (mIsWarmStartingActive) { - warmStart(); - } - - // Solve the contact constraints - solveContactConstraints(); - - // Cache the lambda values in order to use them in the next step - storeImpulses(); -} - // Store the computed impulses to use them to // warm start the solver at the next iteration void ContactSolver::storeImpulses() { diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index 97ec39b5..68e0c14a 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -345,9 +345,6 @@ class ContactSolver { /// Reference to all the contact manifold of the world std::vector& mContactManifolds; - /// Number of iterations of the contact solver - uint mNbIterations; - /// Split linear velocities for the position contact solver (split impulse) Vector3* mSplitLinearVelocities; @@ -389,25 +386,12 @@ class ContactSolver { // -------------------- Methods -------------------- // - /// Initialize the constraint solver - void initialize(); - /// Initialize the split impulse velocities void initializeSplitImpulseVelocities(); /// Initialize the contact constraints before solving the system void initializeContactConstraints(); - /// Store the computed impulses to use them to - /// warm start the solver at the next iteration - void storeImpulses(); - - /// Warm start the solver. - void warmStart(); - - /// Solve the contact constraints by applying sequential impulses - void solveContactConstraints(); - /// Apply an impulse to the two bodies of a constraint void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold); @@ -460,8 +444,18 @@ class ContactSolver { /// Destructor virtual ~ContactSolver(); - /// Solve the constraints - void solve(decimal timeStep); + /// Initialize the constraint solver + void initialize(decimal dt); + + /// Warm start the solver. + void warmStart(); + + /// Store the computed impulses to use them to + /// warm start the solver at the next iteration + void storeImpulses(); + + /// Solve the contacts + void solve(); /// Return true if the body is in at least one constraint bool isConstrainedBody(RigidBody* body) const; @@ -481,9 +475,6 @@ class ContactSolver { /// Clean up the constraint solver void cleanup(); - /// Set the number of iterations of the constraint solver - void setNbIterationsSolver(uint nbIterations); - /// Activate or Deactivate the split impulses for contacts void setIsSplitImpulseActive(bool isActive); @@ -511,11 +502,6 @@ inline Vector3 ContactSolver::getSplitAngularVelocityOfBody(RigidBody* body) { return mSplitAngularVelocities[indexBody]; } -// Set the number of iterations of the constraint solver -inline void ContactSolver::setNbIterationsSolver(uint nbIterations) { - mNbIterations = nbIterations; -} - // Activate or Deactivate the split impulses for contacts inline void ContactSolver::setIsSplitImpulseActive(bool isActive) { mIsSplitImpulseActive = isActive; diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index dd319483..237d5236 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -38,6 +38,7 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_ mMapBodyToConstrainedVelocityIndex), mConstraintSolver(mJoints, mConstrainedLinearVelocities, mConstrainedAngularVelocities, mMapBodyToConstrainedVelocityIndex), + mNbSolverIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS), mIsDeactivationActive(DEACTIVATION_ENABLED) { } @@ -94,15 +95,11 @@ void DynamicsWorld::update() { // Compute the collision detection mCollisionDetection.computeCollisionDetection(); - // Initialize the constrained velocities - initConstrainedVelocitiesArray(); + // Integrate the velocities + integrateRigidBodiesVelocities(); - // If there are contacts - if (!mContactManifolds.empty()) { - - // Solve the contacts - mContactSolver.solve(static_cast(mTimer.getTimeStep())); - } + // Solve the contacts and constraints + solveContactsAndConstraints(); // Update the timer mTimer.nextStep(); @@ -110,8 +107,8 @@ void DynamicsWorld::update() { // Reset the movement boolean variable of each body to false resetBodiesMovementVariable(); - // Update the position and orientation of each body - updateRigidBodiesPositionAndOrientation(); + // Integrate the position and orientation of each body + integrateRigidBodiesPositions(); // Cleanup of the contact solver mContactSolver.cleanup(); @@ -124,8 +121,8 @@ void DynamicsWorld::update() { setInterpolationFactorToAllBodies(); } -// Update the position and orientation of the rigid bodies -void DynamicsWorld::updateRigidBodiesPositionAndOrientation() { +// Integrate position and orientation of the rigid bodies +void DynamicsWorld::integrateRigidBodiesPositions() { PROFILE("DynamicsWorld::updateRigidBodiesPositionAndOrientation()"); @@ -200,8 +197,8 @@ void DynamicsWorld::setInterpolationFactorToAllBodies() { } } -// Initialize the constrained velocities array at each step -void DynamicsWorld::initConstrainedVelocitiesArray() { +// Integrate the velocities of rigid bodies +void DynamicsWorld::integrateRigidBodiesVelocities() { // TODO : Use better memory allocation here mConstrainedLinearVelocities = std::vector(mRigidBodies.size(), Vector3(0, 0, 0)); @@ -228,6 +225,50 @@ void DynamicsWorld::initConstrainedVelocitiesArray() { assert(mMapBodyToConstrainedVelocityIndex.size() == mRigidBodies.size()); } +// Solve the contacts and constraints +void DynamicsWorld::solveContactsAndConstraints() { + + PROFILE("DynamicsWorld::solveContactsAndConstraints()"); + + // Get the current time step + decimal dt = static_cast(mTimer.getTimeStep()); + + // Check if there are contacts and constraints to solve + bool isConstraintsToSolve = !mJoints.empty(); + bool isContactsToSolve = !mContactManifolds.empty(); + if (!isConstraintsToSolve && !isContactsToSolve) return; + + // If there are contacts + if (isContactsToSolve) { + + // Initialize the solver + mContactSolver.initialize(dt); + + // Warm start the contact solver + mContactSolver.warmStart(); + } + + // If there are constraints + if (isConstraintsToSolve) { + + // Initialize the constraint solver + mConstraintSolver.initialize(dt); + } + + // For each iteration of the solver + for (uint i=0; i mJoints; - /// All the bodies that are part of contacts or constraints - std::set mConstrainedBodies; - /// Gravity vector of the world Vector3 mGravity; @@ -99,8 +99,8 @@ class DynamicsWorld : public CollisionWorld { /// Private assignment operator DynamicsWorld& operator=(const DynamicsWorld& world); - /// Compute the motion of all bodies and update their positions and orientations - void updateRigidBodiesPositionAndOrientation(); + /// Integrate the positions and orientations of rigid bodies + void integrateRigidBodiesPositions(); /// Update the position and orientation of a body void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity, @@ -109,8 +109,11 @@ class DynamicsWorld : public CollisionWorld { /// Compute and set the interpolation factor to all bodies void setInterpolationFactorToAllBodies(); - /// Initialize the constrained velocities array at each step - void initConstrainedVelocitiesArray(); + /// Integrate the velocities of rigid bodies + void integrateRigidBodiesVelocities(); + + /// Solve the contacts and constraints + void solveContactsAndConstraints(); /// Cleanup the constrained velocities array at each step void cleanupConstrainedVelocitiesArray(); @@ -212,7 +215,7 @@ inline void DynamicsWorld::stop() { // Set the number of iterations of the constraint solver inline void DynamicsWorld::setNbIterationsSolver(uint nbIterations) { - mContactSolver.setNbIterationsSolver(nbIterations); + mNbSolverIterations = nbIterations; } // Activate or Deactivate the split impulses for contacts From 434a1687857a397cc71a6d0288e5fbbf86ff99b1 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 26 Apr 2013 00:46:09 +0200 Subject: [PATCH 05/24] Add files for the joints example --- examples/CMakeLists.txt | 1 + examples/fallingcubes/Box.cpp | 180 ----------------- examples/joints/CMakeLists.txt | 17 ++ examples/joints/Joints.cpp | 151 +++++++++++++++ examples/joints/Scene.cpp | 182 ++++++++++++++++++ .../{fallingcubes/Box.h => joints/Scene.h} | 114 +++++------ 6 files changed, 411 insertions(+), 234 deletions(-) delete mode 100644 examples/fallingcubes/Box.cpp create mode 100644 examples/joints/CMakeLists.txt create mode 100644 examples/joints/Joints.cpp create mode 100644 examples/joints/Scene.cpp rename examples/{fallingcubes/Box.h => joints/Scene.h} (51%) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 568479fb..0740c9fc 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -3,3 +3,4 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6) add_subdirectory(common/) add_subdirectory(fallingcubes/) +add_subdirectory(joints/) diff --git a/examples/fallingcubes/Box.cpp b/examples/fallingcubes/Box.cpp deleted file mode 100644 index d4169c9a..00000000 --- a/examples/fallingcubes/Box.cpp +++ /dev/null @@ -1,180 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * -* Copyright (c) 2010-2013 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. * -* * -********************************************************************************/ - -// Libraries -#include "Box.h" - -// Macros -#define MEMBER_OFFSET(s,m) ((char *)NULL + (offsetof(s,m))) - -// Initialize static variables -openglframework::VertexBufferObject Box::mVBOVertices(GL_ARRAY_BUFFER); -openglframework::VertexBufferObject Box::mVBOIndices(GL_ELEMENT_ARRAY_BUFFER); -bool Box::areVBOsCreated = false; -VertexData Box::mCubeVertices[8] = { - {openglframework::Vector3(1,1,1),openglframework::Vector3(1,1,1),openglframework::Color(0,0,1,1)}, - {openglframework::Vector3(-1,1,1),openglframework::Vector3(-1,1,1),openglframework::Color(0,0,1,1)}, - {openglframework::Vector3(-1,-1,1),openglframework::Vector3(-1,-1,1),openglframework::Color(0,0,1,1)}, - {openglframework::Vector3(1,-1,1),openglframework::Vector3(1,-1,1),openglframework::Color(0,0,1,1)}, - {openglframework::Vector3(1,-1,-1),openglframework::Vector3(1,-1,-1),openglframework::Color(0,0,1,1)}, - {openglframework::Vector3(-1,-1,-1),openglframework::Vector3(-1,-1,-1),openglframework::Color(0,0,1,1)}, - {openglframework::Vector3(-1,1,-1),openglframework::Vector3(-1,1,-1),openglframework::Color(0,0,1,1)}, - {openglframework::Vector3(1,1,-1),openglframework::Vector3(1,1,-1),openglframework::Color(0,0,1,1)} -}; -GLuint Box::mCubeIndices[36] = { 0, 1, 2, - 2, 3, 0, - 7, 4, 5, - 5, 6, 7, - 6, 5, 2, - 2, 1, 6, - 7, 0, 3, - 3, 4, 7, - 7, 6, 1, - 1, 0, 7, - 3, 2, 5, - 5, 4, 3}; - -// Constructor -Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &position, - float mass, reactphysics3d::DynamicsWorld* dynamicsWorld) - : openglframework::Object3D() { - - // Initialize the size of the box - mSize[0] = size.x * 0.5f; - mSize[1] = size.y * 0.5f; - mSize[2] = size.z * 0.5f; - - // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mSize[0], 0, 0, 0, - 0, mSize[1], 0, 0, - 0, 0, mSize[2], 0, - 0, 0, 0, 1); - - // Initialize the position where the cube will be rendered - translateWorld(position); - - // Create the collision shape for the rigid body (box shape) - mCollisionShape = new rp3d::BoxShape(rp3d::Vector3(mSize[0], mSize[1], mSize[2])); - - // Compute the inertia tensor of the body using its collision shape - rp3d::Matrix3x3 inertiaTensor; - mCollisionShape->computeLocalInertiaTensor(inertiaTensor, mass); - - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); - - // Create a rigid body corresponding to the cube in the dynamics world - mRigidBody = dynamicsWorld->createRigidBody(transform, mass, inertiaTensor, mCollisionShape); - - // If the Vertex Buffer object has not been created yet - if (!areVBOsCreated) { - // Create the Vertex Buffer - createVBO(); - } -} - -// Destructor -Box::~Box() { - - // Destroy the collision shape - delete mCollisionShape; -} - -// Render the cube at the correct position and with the correct orientation -void Box::render(openglframework::Shader& shader) { - - // Bind the shader - shader.bind(); - - // Set the model to World matrix - shader.setMatrix4x4Uniform("modelToWorldMatrix", mTransformMatrix); - - // Bind the vertices VBO - mVBOVertices.bind(); - - // Enable the vertex, normal and color arrays - glEnableClientState(GL_VERTEX_ARRAY); - glEnableClientState(GL_COLOR_ARRAY); - glEnableClientState(GL_NORMAL_ARRAY); - - // Set the arrays pointers - glVertexPointer(3, GL_FLOAT, sizeof(VertexData), MEMBER_OFFSET(VertexData, position)); - glNormalPointer(GL_FLOAT, sizeof(VertexData), MEMBER_OFFSET(VertexData, normal)); - glColorPointer(3, GL_FLOAT, sizeof(VertexData), MEMBER_OFFSET(VertexData, color)); - - // Bind the indices VBO - mVBOIndices.bind(); - - // Draw the geometry of the box - glDrawElements(GL_TRIANGLES, 36, GL_UNSIGNED_INT, (char*)NULL); - - // Unbind the VBOs - mVBOVertices.unbind(); - mVBOIndices.unbind(); - - // Disable the arrays - glDisableClientState(GL_VERTEX_ARRAY); - glDisableClientState(GL_COLOR_ARRAY); - glDisableClientState(GL_NORMAL_ARRAY); - - // Unbind the shader - shader.unbind(); -} - -// Update the transform matrix of the box -void Box::updateTransform() { - - // Get the interpolated transform of the rigid body - rp3d::Transform transform = mRigidBody->getInterpolatedTransform(); - - // Compute the transform used for rendering the box - float matrix[16]; - transform.getOpenGLMatrix(matrix); - openglframework::Matrix4 newMatrix(matrix[0], matrix[4], matrix[8], matrix[12], - matrix[1], matrix[5], matrix[9], matrix[13], - matrix[2], matrix[6], matrix[10], matrix[14], - matrix[3], matrix[7], matrix[11], matrix[15]); - - // Apply the scaling matrix to have the correct box dimensions - mTransformMatrix = newMatrix * mScalingMatrix; -} - -// Create the Vertex Buffer Objects used to render to box with OpenGL. -/// We create two VBOs (one for vertices and one for indices) to render all the boxes -/// in the simulation. -void Box::createVBO() { - - // Create the VBOs - mVBOVertices.create(); - mVBOIndices.create(); - - // Copy the data into the VBOs - mVBOVertices.copyDataIntoVBO(sizeof(mCubeVertices), mCubeVertices, GL_STATIC_DRAW); - mVBOIndices.copyDataIntoVBO(sizeof(mCubeIndices), mCubeIndices, GL_STATIC_DRAW); - - areVBOsCreated = true; -} diff --git a/examples/joints/CMakeLists.txt b/examples/joints/CMakeLists.txt new file mode 100644 index 00000000..dc2eed0c --- /dev/null +++ b/examples/joints/CMakeLists.txt @@ -0,0 +1,17 @@ +# Minimum cmake version required +cmake_minimum_required(VERSION 2.6) + +# Project configuration +PROJECT(Joints) + +# Copy the shaders used for the demo into the build directory +FILE(COPY "../common/opengl-framework/src/shaders/" DESTINATION "${CMAKE_CURRENT_BINARY_DIR}/shaders/") + +# Headers +INCLUDE_DIRECTORIES("../common/opengl-framework/src/" "../common/") + +# Create the example executable using the +# compiled reactphysics3d static library +ADD_EXECUTABLE(joints Joints.cpp Scene.cpp Scene.h "../common/Box.cpp" "../common/Box.h" "../common/Viewer.cpp" "../common/Viewer.h") + +TARGET_LINK_LIBRARIES(joints reactphysics3d openglframework) diff --git a/examples/joints/Joints.cpp b/examples/joints/Joints.cpp new file mode 100644 index 00000000..f42ffb97 --- /dev/null +++ b/examples/joints/Joints.cpp @@ -0,0 +1,151 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2013 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. * +* * +********************************************************************************/ + +// Libraries +#include "Scene.h" +#include "Viewer.h" + +// Declarations +void simulate(); +void display(); +void finish(); +void reshape(int width, int height); +void mouseButton(int button, int state, int x, int y); +void mouseMotion(int x, int y); +void keyboard(unsigned char key, int x, int y); +void init(); + +// Namespaces +using namespace openglframework; + +// Global variables +Viewer* viewer; +Scene* scene; + +// Main function +int main(int argc, char** argv) { + + // Create and initialize the Viewer + viewer = new Viewer(); + Vector2 windowsSize = Vector2(800, 600); + Vector2 windowsPosition = Vector2(100, 100); + bool initOK = viewer->init(argc, argv, "ReactPhysics3D Examples - Joints", windowsSize, windowsPosition); + if (!initOK) return 1; + + // Create the scene + scene = new Scene(viewer); + + init(); + + // Glut Idle function that is continuously called + glutIdleFunc(simulate); + glutDisplayFunc(display); + glutReshapeFunc(reshape); + glutMouseFunc(mouseButton); + glutMotionFunc(mouseMotion); + glutKeyboardFunc(keyboard); + glutCloseFunc(finish); + + // Glut main looop + glutMainLoop(); + + return 0; +} + +// Simulate function +void simulate() { + + // Physics simulation + scene->simulate(); + + viewer->computeFPS(); + + // Ask GLUT to render the scene + glutPostRedisplay (); +} + +// Initialization +void init() { + + // Define the background color (black) + glClearColor(0.0, 0.0, 0.0, 1.0); +} + +// Reshape function +void reshape(int newWidth, int newHeight) { + viewer->reshape(newWidth, newHeight); +} + +// Called when a mouse button event occurs +void mouseButton(int button, int state, int x, int y) { + viewer->mouseButtonEvent(button, state, x, y); +} + +// Called when a mouse motion event occurs +void mouseMotion(int x, int y) { + viewer->mouseMotionEvent(x, y); +} + +// Called when the user hits a special key on the keyboard +void keyboard(unsigned char key, int x, int y) { + switch(key) { + + // Escape key + case 27: + glutLeaveMainLoop(); + break; + + // Space bar + case 32: + scene->pauseContinueSimulation(); + break; + } +} + +// End of the application +void finish() { + + // Destroy the viewer and the scene + delete viewer; + delete scene; +} + +// Display the scene +void display() { + + // Render the scene + scene->render(); + + // Display the FPS + viewer->displayGUI(); + + // Swap the buffers + glutSwapBuffers(); + + // Check the OpenGL errors + GlutViewer::checkOpenGLErrors(); +} + + diff --git a/examples/joints/Scene.cpp b/examples/joints/Scene.cpp new file mode 100644 index 00000000..693a0fa5 --- /dev/null +++ b/examples/joints/Scene.cpp @@ -0,0 +1,182 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2013 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. * +* * +********************************************************************************/ + +// Libraries +#include "Scene.h" + +// Namespaces +using namespace openglframework; + +// Constructor +Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0), + mPhongShader("shaders/phong.vert", + "shaders/phong.frag"), mIsRunning(false) { + + // Move the light 0 + mLight0.translateWorld(Vector3(7, 15, 15)); + + // Compute the radius and the center of the scene + float radiusScene = 10.0f; + openglframework::Vector3 center(0, 5, 0); + + // Set the center of the scene + mViewer->setScenePosition(center, radiusScene); + + // Gravity vector in the dynamics world + rp3d::Vector3 gravity(0, -9.81, 0); + + // Time step for the physics simulation + rp3d::decimal timeStep = 1.0f / 60.0f; + + // Create the dynamics world for the physics simulation + mDynamicsWorld = new rp3d::DynamicsWorld(gravity, timeStep); + + // Set the number of iterations of the constraint solver + mDynamicsWorld->setNbIterationsSolver(15); + + float radius = 2.0f; + + // Create all the cubes of the scene + for (int i=0; igetRigidBody()->setIsMotionEnabled(true); + + // Set the bouncing factor of the box + cube->getRigidBody()->setRestitution(0.4); + + // Add the box the list of box in the scene + mBoxes.push_back(cube); + } + + // Create the floor + openglframework::Vector3 floorPosition(0, 0, 0); + mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld); + + // The floor must be a non-moving rigid body + mFloor->getRigidBody()->setIsMotionEnabled(false); + + // Set the bouncing factor of the floor + mFloor->getRigidBody()->setRestitution(0.3); + + // Start the simulation + startSimulation(); +} + +// Destructor +Scene::~Scene() { + + // Stop the physics simulation + stopSimulation(); + + // Destroy the shader + mPhongShader.destroy(); + + // Destroy all the cubes of the scene + for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { + + // Destroy the corresponding rigid body from the dynamics world + mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); + + // Destroy the cube + delete (*it); + } + + // Destroy the rigid body of the floor + mDynamicsWorld->destroyRigidBody(mFloor->getRigidBody()); + + // Destroy the floor + delete mFloor; + + // Destroy the dynamics world + delete mDynamicsWorld; +} + +// Take a step for the simulation +void Scene::simulate() { + + // If the physics simulation is running + if (mIsRunning) { + + // Take a simulation step + mDynamicsWorld->update(); + + // Update the position and orientation of the boxes + for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { + + // Update the transform used for the rendering + (*it)->updateTransform(); + } + + mFloor->updateTransform(); + + } +} + +// Render the scene +void Scene::render() { + + glEnable(GL_DEPTH_TEST); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glEnable(GL_CULL_FACE); + + // Bind the shader + mPhongShader.bind(); + + // Set the variables of the shader + const Camera& camera = mViewer->getCamera(); + Matrix4 matrixIdentity; + matrixIdentity.setToIdentity(); + mPhongShader.setVector3Uniform("cameraWorldPosition", mViewer->getCamera().getOrigin()); + mPhongShader.setMatrix4x4Uniform("worldToCameraMatrix", camera.getTransformMatrix().getInverse()); + mPhongShader.setMatrix4x4Uniform("projectionMatrix", camera.getProjectionMatrix()); + mPhongShader.setVector3Uniform("lightWorldPosition", mLight0.getOrigin()); + mPhongShader.setVector3Uniform("lightAmbientColor", Vector3(0.3f, 0.3f, 0.3f)); + Color& diffCol = mLight0.getDiffuseColor(); + Color& specCol = mLight0.getSpecularColor(); + mPhongShader.setVector3Uniform("lightDiffuseColor", Vector3(diffCol.r, diffCol.g, diffCol.b)); + mPhongShader.setVector3Uniform("lightSpecularColor", Vector3(specCol.r, specCol.g, specCol.b)); + mPhongShader.setFloatUniform("shininess", 60.0f); + + // Render all the cubes of the scene + for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { + (*it)->render(mPhongShader); + } + + // Render the floor + mFloor->render(mPhongShader); + + // Unbind the shader + mPhongShader.unbind(); +} diff --git a/examples/fallingcubes/Box.h b/examples/joints/Scene.h similarity index 51% rename from examples/fallingcubes/Box.h rename to examples/joints/Scene.h index d287c3cb..3741962e 100644 --- a/examples/fallingcubes/Box.h +++ b/examples/joints/Scene.h @@ -23,89 +23,95 @@ * * ********************************************************************************/ -#ifndef BOX_H -#define BOX_H +#ifndef SCENE_H +#define SCENE_H // Libraries #include "openglframework.h" #include "reactphysics3d.h" +#include "Box.h" -// Structure VertexData -struct VertexData { +// Constants +const int NB_BOXES = 20; // Number of boxes in the scene +const openglframework::Vector3 BOX_SIZE(2, 2, 2); // Box dimensions in meters +const openglframework::Vector3 FLOOR_SIZE(20, 0.5f, 20); // Floor dimensions in meters +const float BOX_MASS = 1.0f; // Box mass in kilograms +const float FLOOR_MASS = 100.0f; // Floor mass in kilograms - /// Vertex position - openglframework::Vector3 position; - - /// Vertex normal - openglframework::Vector3 normal; - - // Vertex color - openglframework::Color color; -}; - -// Class Box -class Box : public openglframework::Object3D { +// Class Scene +class Scene { private : // -------------------- Attributes -------------------- // - /// Size of each side of the box - float mSize[3]; + // Pointer to the viewer + openglframework::GlutViewer* mViewer; - /// Rigid body used to simulate the dynamics of the box - rp3d::RigidBody* mRigidBody; + // Light 0 + openglframework::Light mLight0; - /// Collision shape of the rigid body - rp3d::BoxShape* mCollisionShape; + // Phong shader + openglframework::Shader mPhongShader; - /// Scaling matrix (applied to a cube to obtain the correct box dimensions) - openglframework::Matrix4 mScalingMatrix; + /// All the boxes of the scene + std::vector mBoxes; - /// Vertex Buffer Object for the vertices data used to render the box with OpenGL - static openglframework::VertexBufferObject mVBOVertices; + /// Box for the floor + Box* mFloor; - /// Vertex Buffer Object for the indices used to render the box with OpenGL - static openglframework::VertexBufferObject mVBOIndices; + /// Dynamics world used for the physics simulation + rp3d::DynamicsWorld* mDynamicsWorld; - /// Vertex data for each vertex of the cube (used to render the box) - static VertexData mCubeVertices[8]; + /// True if the physics simulation is running + bool mIsRunning; - /// Indices of the cube (used to render the box) - static GLuint mCubeIndices[36]; - - /// True if the VBOs have already been created - static bool areVBOsCreated; - - // -------------------- Methods -------------------- // - - /// Create a Vertex Buffer Object to render to box with OpenGL - static void createVBO(); - - public : + public: // -------------------- Methods -------------------- // /// Constructor - Box(const openglframework::Vector3& size, const openglframework::Vector3& position, - float mass, rp3d::DynamicsWorld* dynamicsWorld); + Scene(openglframework::GlutViewer* viewer); /// Destructor - ~Box(); + ~Scene(); - /// Return a pointer to the rigid body of the box - rp3d::RigidBody* getRigidBody(); + /// Take a step for the simulation + void simulate(); - /// Update the transform matrix of the box - void updateTransform(); + /// Stop the simulation + void stopSimulation(); - /// Render the cube at the correct position and with the correct orientation - void render(openglframework::Shader& shader); + /// Start the simulation + void startSimulation(); + + /// Pause or continue simulation + void pauseContinueSimulation(); + + /// Render the scene + void render(); }; -// Return a pointer to the rigid body of the box -inline rp3d::RigidBody* Box::getRigidBody() { - return mRigidBody; +// Stop the simulation +inline void Scene::stopSimulation() { + mDynamicsWorld->stop(); + mIsRunning = false; +} + +// Start the simulation +inline void Scene::startSimulation() { + mDynamicsWorld->start(); + mIsRunning = true; +} + +// Pause or continue simulation +inline void Scene::pauseContinueSimulation() { + if (mIsRunning) { + stopSimulation(); + } + else { + startSimulation(); + } } #endif From 44a26bdcab42d9ac115b323d4f83c2d5000b0f63 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sat, 27 Apr 2013 00:15:25 +0200 Subject: [PATCH 06/24] Add cmake configuration file to find GLEW library --- CMakeLists.txt | 3 ++ cmake/FindGLEW.cmake | 65 ++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 68 insertions(+) create mode 100644 cmake/FindGLEW.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index 7986b1ce..47bc8aaa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,6 +7,9 @@ PROJECT(REACTPHYSICS3D) # Where to build the library SET(LIBRARY_OUTPUT_PATH lib/) +# Where to find the module to find special packages/libraries +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/") + # Options OPTION(COMPILE_EXAMPLES "Select this if you want to build the examples" OFF) OPTION(COMPILE_TESTS "Select this if you want to build the tests" OFF) diff --git a/cmake/FindGLEW.cmake b/cmake/FindGLEW.cmake new file mode 100644 index 00000000..c29c4eb2 --- /dev/null +++ b/cmake/FindGLEW.cmake @@ -0,0 +1,65 @@ +# +# Try to find GLEW library and include path. +# Once done this will define +# +# GLEW_FOUND +# GLEW_INCLUDE_PATH +# GLEW_LIBRARY +# + +IF (WIN32) +FIND_PATH( GLEW_INCLUDE_PATH GL/glew.h +$ENV{PROGRAMFILES}/GLEW/include +${GLEW_ROOT_DIR}/include +DOC "The directory where GL/glew.h resides") + +IF (NV_SYSTEM_PROCESSOR STREQUAL "AMD64") +FIND_LIBRARY( GLEW_LIBRARY +NAMES glew64 glew64s +PATHS +$ENV{PROGRAMFILES}/GLEW/lib +${PROJECT_SOURCE_DIR}/src/nvgl/glew/bin +${PROJECT_SOURCE_DIR}/src/nvgl/glew/lib +DOC "The GLEW library (64-bit)" +) +ELSE(NV_SYSTEM_PROCESSOR STREQUAL "AMD64") +FIND_LIBRARY( GLEW_LIBRARY +NAMES glew GLEW glew32 glew32s +PATHS +$ENV{PROGRAMFILES}/GLEW/lib +${PROJECT_SOURCE_DIR}/src/nvgl/glew/bin +${PROJECT_SOURCE_DIR}/src/nvgl/glew/lib +DOC "The GLEW library" +) +ENDIF(NV_SYSTEM_PROCESSOR STREQUAL "AMD64") +ELSE (WIN32) +FIND_PATH( GLEW_INCLUDE_PATH GL/glew.h +/usr/include +/usr/local/include +/sw/include +/opt/local/include +${GLEW_ROOT_DIR}/include +DOC "The directory where GL/glew.h resides") + +FIND_LIBRARY( GLEW_LIBRARY +NAMES GLEW glew +PATHS +/usr/lib64 +/usr/lib +/usr/local/lib64 +/usr/local/lib +/sw/lib +/opt/local/lib +${GLEW_ROOT_DIR}/lib +DOC "The GLEW library") +ENDIF (WIN32) + +SET(GLEW_FOUND "NO") +IF (GLEW_INCLUDE_PATH AND GLEW_LIBRARY) +SET(GLEW_LIBRARIES ${GLEW_LIBRARY}) +SET(GLEW_FOUND "YES") +ENDIF (GLEW_INCLUDE_PATH AND GLEW_LIBRARY) + + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(GLEW DEFAULT_MSG GLEW_LIBRARY GLEW_INCLUDE_PATH) \ No newline at end of file From c4eee4fb1f42f7d51e4f952ae838539dc0dbc8a7 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 2 May 2013 22:41:57 +0200 Subject: [PATCH 07/24] Implement the Ball-And-Socket joint --- examples/fallingcubes/Scene.h | 6 +- examples/joints/Scene.cpp | 126 +++++++++++------- examples/joints/Scene.h | 25 +++- .../narrowphase/EPA/EPAAlgorithm.cpp | 4 +- .../narrowphase/GJK/GJKAlgorithm.cpp | 12 +- .../narrowphase/SphereVsSphereAlgorithm.cpp | 4 +- src/constraint/BallAndSocketJoint.cpp | 95 ++++++++++++- src/constraint/BallAndSocketJoint.h | 30 ++++- src/constraint/Constraint.h | 15 +++ src/constraint/ContactPoint.cpp | 10 ++ src/constraint/ContactPoint.h | 6 + src/engine/ConstraintSolver.cpp | 43 +++++- src/engine/ConstraintSolver.h | 60 +++++++-- src/engine/ContactSolver.cpp | 32 ++--- src/engine/ContactSolver.h | 45 +------ src/engine/Impulse.h | 65 +++++++++ src/mathematics/Matrix3x3.h | 10 ++ src/mathematics/Quaternion.h | 4 +- src/mathematics/Transform.h | 4 +- src/reactphysics3d.h | 1 + test/tests/mathematics/TestMatrix3x3.h | 9 ++ test/tests/mathematics/TestTransform.h | 2 +- 22 files changed, 461 insertions(+), 147 deletions(-) create mode 100644 src/engine/Impulse.h diff --git a/examples/fallingcubes/Scene.h b/examples/fallingcubes/Scene.h index 3741962e..d2dbfc18 100644 --- a/examples/fallingcubes/Scene.h +++ b/examples/fallingcubes/Scene.h @@ -45,13 +45,13 @@ class Scene { // -------------------- Attributes -------------------- // - // Pointer to the viewer + /// Pointer to the viewer openglframework::GlutViewer* mViewer; - // Light 0 + /// Light 0 openglframework::Light mLight0; - // Phong shader + /// Phong shader openglframework::Shader mPhongShader; /// All the boxes of the scene diff --git a/examples/joints/Scene.cpp b/examples/joints/Scene.cpp index 693a0fa5..5e93b09c 100644 --- a/examples/joints/Scene.cpp +++ b/examples/joints/Scene.cpp @@ -56,39 +56,11 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0), // Set the number of iterations of the constraint solver mDynamicsWorld->setNbIterationsSolver(15); - float radius = 2.0f; - - // Create all the cubes of the scene - for (int i=0; igetRigidBody()->setIsMotionEnabled(true); - - // Set the bouncing factor of the box - cube->getRigidBody()->setRestitution(0.4); - - // Add the box the list of box in the scene - mBoxes.push_back(cube); - } + // Create the Ball-and-Socket joint + createBallAndSocketJoints(); // Create the floor - openglframework::Vector3 floorPosition(0, 0, 0); - mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld); - - // The floor must be a non-moving rigid body - mFloor->getRigidBody()->setIsMotionEnabled(false); - - // Set the bouncing factor of the floor - mFloor->getRigidBody()->setRestitution(0.3); + createFloor(); // Start the simulation startSimulation(); @@ -103,20 +75,17 @@ Scene::~Scene() { // Destroy the shader mPhongShader.destroy(); - // Destroy all the cubes of the scene - for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { + // Destroy the joints + mDynamicsWorld->destroyJoint(mBallAndSocketJoint); - // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); - - // Destroy the cube - delete (*it); - } - - // Destroy the rigid body of the floor - mDynamicsWorld->destroyRigidBody(mFloor->getRigidBody()); + // Destroy all the boxes of the scene + mDynamicsWorld->destroyRigidBody(mBallAndSocketJointBox1->getRigidBody()); + mDynamicsWorld->destroyRigidBody(mBallAndSocketJointBox2->getRigidBody()); + delete mBallAndSocketJointBox1; + delete mBallAndSocketJointBox2; // Destroy the floor + mDynamicsWorld->destroyRigidBody(mFloor->getRigidBody()); delete mFloor; // Destroy the dynamics world @@ -133,12 +102,10 @@ void Scene::simulate() { mDynamicsWorld->update(); // Update the position and orientation of the boxes - for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { - - // Update the transform used for the rendering - (*it)->updateTransform(); - } + mBallAndSocketJointBox1->updateTransform(); + mBallAndSocketJointBox2->updateTransform(); + // Update the position and orientation of the floor mFloor->updateTransform(); } @@ -169,10 +136,9 @@ void Scene::render() { mPhongShader.setVector3Uniform("lightSpecularColor", Vector3(specCol.r, specCol.g, specCol.b)); mPhongShader.setFloatUniform("shininess", 60.0f); - // Render all the cubes of the scene - for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { - (*it)->render(mPhongShader); - } + // Render all the boxes + mBallAndSocketJointBox1->render(mPhongShader); + mBallAndSocketJointBox2->render(mPhongShader); // Render the floor mFloor->render(mPhongShader); @@ -180,3 +146,61 @@ void Scene::render() { // Unbind the shader mPhongShader.unbind(); } + +// Create the boxes and joints for the Ball-and-Socket joint example +void Scene::createBallAndSocketJoints() { + + // --------------- Create the first box --------------- // + + // Position of the box + openglframework::Vector3 positionBox1(0, 15, 0); + + // Create a box and a corresponding rigid in the dynamics world + mBallAndSocketJointBox1 = new Box(BOX_SIZE, positionBox1 , BOX_MASS, mDynamicsWorld); + + // The fist box cannot move + mBallAndSocketJointBox1->getRigidBody()->setIsMotionEnabled(false); + + // Set the bouncing factor of the box + mBallAndSocketJointBox1->getRigidBody()->setRestitution(0.4); + + // --------------- Create the second box --------------- // + + // Position of the box + openglframework::Vector3 positionBox2(0, 10, 0); + + // Create a box and a corresponding rigid in the dynamics world + mBallAndSocketJointBox2 = new Box(BOX_SIZE, positionBox2 , BOX_MASS, mDynamicsWorld); + + // The second box is allowed to move + mBallAndSocketJointBox2->getRigidBody()->setIsMotionEnabled(true); + + // Set the bouncing factor of the box + mBallAndSocketJointBox2->getRigidBody()->setRestitution(0.4); + + // --------------- Create the joint --------------- // + + // Create the joint info object + rp3d::BallAndSocketJointInfo jointInfo; + jointInfo.body1 = mBallAndSocketJointBox1->getRigidBody(); + jointInfo.body2 = mBallAndSocketJointBox2->getRigidBody(); + jointInfo.anchorPointWorldSpace = rp3d::Vector3(0, 12.5, 0); + + // Create the joint in the dynamics world + mBallAndSocketJoint = dynamic_cast( + mDynamicsWorld->createJoint(jointInfo)); +} + +// Create the floor +void Scene::createFloor() { + + // Create the floor + openglframework::Vector3 floorPosition(0, 0, 0); + mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld); + + // The floor must be a non-moving rigid body + mFloor->getRigidBody()->setIsMotionEnabled(false); + + // Set the bouncing factor of the floor + mFloor->getRigidBody()->setRestitution(0.3); +} diff --git a/examples/joints/Scene.h b/examples/joints/Scene.h index 3741962e..7d89dc3a 100644 --- a/examples/joints/Scene.h +++ b/examples/joints/Scene.h @@ -32,7 +32,6 @@ #include "Box.h" // Constants -const int NB_BOXES = 20; // Number of boxes in the scene const openglframework::Vector3 BOX_SIZE(2, 2, 2); // Box dimensions in meters const openglframework::Vector3 FLOOR_SIZE(20, 0.5f, 20); // Floor dimensions in meters const float BOX_MASS = 1.0f; // Box mass in kilograms @@ -45,17 +44,23 @@ class Scene { // -------------------- Attributes -------------------- // - // Pointer to the viewer + /// Pointer to the viewer openglframework::GlutViewer* mViewer; - // Light 0 + /// Light 0 openglframework::Light mLight0; - // Phong shader + /// Phong shader openglframework::Shader mPhongShader; - /// All the boxes of the scene - std::vector mBoxes; + /// Box 1 of Ball-And-Socket joint + Box* mBallAndSocketJointBox1; + + /// Box 2 of Ball-And-Socket joint + Box* mBallAndSocketJointBox2; + + /// Ball-and-Socket joint + rp3d::BallAndSocketJoint* mBallAndSocketJoint; /// Box for the floor Box* mFloor; @@ -66,6 +71,14 @@ class Scene { /// True if the physics simulation is running bool mIsRunning; + // -------------------- Methods -------------------- // + + /// Create the boxes and joints for the Ball-and-Socket joint example + void createBallAndSocketJoints(); + + /// Create the floor + void createFloor(); + public: // -------------------- Methods -------------------- // diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp index 2439979a..b6c70ba5 100644 --- a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp +++ b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp @@ -98,7 +98,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple // 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; + Transform body2Tobody1 = transform1.getInverse() * transform2; // Matrix that transform a direction from local // space of body 1 into local space of body 2 @@ -394,7 +394,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple // Compute the contact info v = transform1.getOrientation().getMatrix() * triangle->getClosestPoint(); Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA); - Vector3 pBLocal = body2Tobody1.inverse() * triangle->computeClosestPointOfObject(suppPointsB); + Vector3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB); Vector3 normal = v.getUnit(); decimal penetrationDepth = v.length(); assert(penetrationDepth > 0.0); diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 16e8f671..392c704d 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -73,7 +73,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1, // 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; + Transform body2Tobody1 = transform1.getInverse() * transform2; // Matrix that transform a direction from local // space of body 1 into local space of body 2 @@ -127,7 +127,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1, decimal dist = sqrt(distSquare); assert(dist > 0.0); pA = (pA - (collisionShape1->getMargin() / dist) * v); - pB = body2Tobody1.inverse() * (pB + (collisionShape2->getMargin() / dist) * v); + pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v); // Compute the contact info Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit()); @@ -159,7 +159,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1, decimal dist = sqrt(distSquare); assert(dist > 0.0); pA = (pA - (collisionShape1->getMargin() / dist) * v); - pB = body2Tobody1.inverse() * (pB + (collisionShape2->getMargin() / dist) * v); + pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v); // Compute the contact info Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit()); @@ -189,7 +189,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1, decimal dist = sqrt(distSquare); assert(dist > 0.0); pA = (pA - (collisionShape1->getMargin() / dist) * v); - pB = body2Tobody1.inverse() * (pB + (collisionShape2->getMargin() / dist) * v); + pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v); // Compute the contact info Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit()); @@ -226,7 +226,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1, decimal dist = sqrt(distSquare); assert(dist > 0.0); pA = (pA - (collisionShape1->getMargin() / dist) * v); - pB = body2Tobody1.inverse() * (pB + (collisionShape2->getMargin() / dist) * v); + pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v); // Compute the contact info Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit()); @@ -275,7 +275,7 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap // 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; + Transform body2ToBody1 = transform1.getInverse() * transform2; // Matrix that transform a direction from local space of body 1 into local space of body 2 Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp index 1a67782a..d50fe507 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -60,8 +60,8 @@ bool SphereVsSphereAlgorithm::testCollision(const CollisionShape* collisionShape // If the sphere collision shapes intersect if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) { - Vector3 centerSphere2InBody1LocalSpace = transform1.inverse() * transform2.getPosition(); - Vector3 centerSphere1InBody2LocalSpace = transform2.inverse() * transform1.getPosition(); + Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition(); + Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition(); Vector3 intersectionOnBody1 = sphereShape1->getRadius() * centerSphere2InBody1LocalSpace.getUnit(); Vector3 intersectionOnBody2 = sphereShape2->getRadius() * diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index 17de606d..214b7337 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -25,13 +25,17 @@ // Libraries #include "BallAndSocketJoint.h" +#include "../engine/ConstraintSolver.h" using namespace reactphysics3d; // Constructor BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo &jointInfo) - : Constraint(jointInfo){ + : Constraint(jointInfo), mImpulse(Vector3(0, 0, 0)) { + // Compute the local-space anchor point for each body + mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; + mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; } // Destructor @@ -39,3 +43,92 @@ BallAndSocketJoint::~BallAndSocketJoint() { } +// Initialize before solving the constraint +void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { + + // Initialize the bodies index in the velocity array + mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; + mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; + + // Get the bodies positions and orientations + const Vector3& x1 = mBody1->getTransform().getPosition(); + const Vector3& x2 = mBody2->getTransform().getPosition(); + const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); + const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); + + // Get the inertia tensor of bodies + Matrix3x3 inverseInertiaTensorBody1 = mBody1->getInertiaTensorInverseWorld(); + Matrix3x3 inverseInertiaTensorBody2 = mBody2->getInertiaTensorInverseWorld(); + + // Compute the vector from body center to anchor point in local-space + const Vector3 u1Local = mLocalAnchorPointBody1 - x1; + const Vector3 u2Local = mLocalAnchorPointBody2 - x2; + + // Compute the vector from body center to the anchor point in world-space + mU1World = orientationBody1 * u1Local; + mU2World = orientationBody2 * u2Local; + + // Compute the corresponding skew-symmetric matrices + Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mU1World); + Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mU2World); + + // Compute the matrix JM^-1J^t + decimal inverseMassBodies = mBody1->getMassInverse() + mBody2->getMassInverse(); + Matrix3x3 massMatrix= Matrix3x3(inverseMassBodies, 0, 0, + 0, inverseMassBodies, 0, + 0, 0, inverseMassBodies) + + skewSymmetricMatrixU1 * inverseInertiaTensorBody1 * skewSymmetricMatrixU1+ + skewSymmetricMatrixU2 * inverseInertiaTensorBody2 * skewSymmetricMatrixU2; + + // Compute the inverse mass matrix K + mInverseMassMatrix = massMatrix.getInverse(); +} + +// Solve the constraint +void BallAndSocketJoint::solve(const ConstraintSolverData& constraintSolverData) { + + // Get the body positions + const Vector3& x1 = mBody1->getTransform().getPosition(); + const Vector3& x2 = mBody2->getTransform().getPosition(); + + // Get the velocities + Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; + Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; + Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; + Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; + + // Get the inverse mass and inverse inertia tensors of the bodies + decimal inverseMassBody1 = mBody1->getMassInverse(); + decimal inverseMassBody2 = mBody2->getMassInverse(); + Matrix3x3 inverseInertiaTensorBody1 = mBody1->getInertiaTensorInverseWorld(); + Matrix3x3 inverseInertiaTensorBody2 = mBody2->getInertiaTensorInverseWorld(); + + // Compute J*v + Vector3 Jv = -v1 + mU1World.cross(w1) + v2 - mU2World.cross(w2); + + // Compute the bias "b" of the constraint + decimal beta = 0.8; // TODO : Use a constant here + decimal biasFactor = -(beta/constraintSolverData.timeStep); + Vector3 b = biasFactor * (x2 + mU2World - x1 - mU1World); + + // Compute the Lagrange multiplier lambda + Vector3 deltaLambda = mInverseMassMatrix * (-Jv - b); + mImpulse = mImpulse + deltaLambda; + + // Compute the impulse P=J^T * lambda + Vector3 linearImpulseBody1 = -deltaLambda; + Vector3 angularImpulseBody1 = mU1World.cross(deltaLambda); + Vector3 linearImpulseBody2 = deltaLambda; + Vector3 angularImpulseBody2 = -mU2World.cross(deltaLambda); + + // Apply the impulse to the bodies of the joint + if (mBody1->getIsMotionEnabled()) { + v1 += inverseMassBody1 * linearImpulseBody1; + w1 += inverseInertiaTensorBody1 * angularImpulseBody1; + } + if (mBody2->getIsMotionEnabled()) { + v2 += inverseMassBody2 * linearImpulseBody2; + w2 += inverseInertiaTensorBody2 * angularImpulseBody2; + } +} + diff --git a/src/constraint/BallAndSocketJoint.h b/src/constraint/BallAndSocketJoint.h index e7e3f5ea..a69507ca 100644 --- a/src/constraint/BallAndSocketJoint.h +++ b/src/constraint/BallAndSocketJoint.h @@ -44,7 +44,7 @@ struct BallAndSocketJointInfo : public ConstraintInfo { // -------------------- Attributes -------------------- // /// Anchor point (in world space coordinates) - Vector3 anchorPoint; + Vector3 anchorPointWorldSpace; /// Constructor BallAndSocketJointInfo() : ConstraintInfo(BALLSOCKETJOINT) {} @@ -62,10 +62,28 @@ class BallAndSocketJoint : public Constraint { // -------------------- Attributes -------------------- // /// Anchor point of body 1 (in local space coordinates) - Vector3 mLocalAnchorPoint1; + Vector3 mLocalAnchorPointBody1; /// Anchor point of body 2 (in local space coordinates) - Vector3 mLocalAnchorPoint2; + Vector3 mLocalAnchorPointBody2; + + /// Vector from center of body 2 to anchor point in world-space + Vector3 mU1World; + + /// Vector from center of body 2 to anchor point in world-space + Vector3 mU2World; + + /// Skew-Symmetric matrix for cross product with vector mU1World + Matrix3x3 mSkewSymmetricMatrixU1World; + + /// Skew-Symmetric matrix for cross product with vector mU2World + Matrix3x3 mSkewSymmetricMatrixU2World; + + /// Inverse mass matrix K=JM^-1J^-t of the constraint + Matrix3x3 mInverseMassMatrix; + + /// Accumulated impulse + Vector3 mImpulse; public : @@ -79,6 +97,12 @@ class BallAndSocketJoint : public Constraint { /// Return the number of bytes used by the joint virtual size_t getSizeInBytes() const; + + /// Initialize before solving the constraint + virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); + + /// Solve the constraint + virtual void solve(const ConstraintSolverData& constraintSolverData); }; // Return the number of bytes used by the joint diff --git a/src/constraint/Constraint.h b/src/constraint/Constraint.h index de09c956..ae3918e0 100644 --- a/src/constraint/Constraint.h +++ b/src/constraint/Constraint.h @@ -36,6 +36,9 @@ namespace reactphysics3d { // Enumeration for the type of a constraint enum ConstraintType {CONTACT, BALLSOCKETJOINT}; +// Class declarations +struct ConstraintSolverData; + // Structure ConstraintInfo /** * This structure is used to gather the information needed to create a constraint. @@ -94,6 +97,12 @@ class Constraint { /// Type of the constraint const ConstraintType mType; + /// Body 1 index in the velocity array to solve the constraint + uint mIndexBody1; + + /// Body 2 index in the velocity array to solve the constraint + uint mIndexBody2; + // -------------------- Methods -------------------- // /// Private copy-constructor @@ -126,6 +135,12 @@ class Constraint { /// Return the number of bytes used by the constraint virtual size_t getSizeInBytes() const = 0; + + /// Initialize before solving the constraint + virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData) = 0; + + /// Solve the constraint + virtual void solve(const ConstraintSolverData& constraintSolverData) = 0; }; // Return the reference to the body 1 diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp index 93a7ec3d..b945c8e3 100644 --- a/src/constraint/ContactPoint.cpp +++ b/src/constraint/ContactPoint.cpp @@ -50,3 +50,13 @@ ContactPoint::ContactPoint(const ContactPointInfo& contactInfo) ContactPoint::~ContactPoint() { } + +// Initialize before solving the constraint +void ContactPoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { + +} + +// Solve the constraint +void ContactPoint::solve(const ConstraintSolverData& constraintSolverData) { + +} diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h index 14852282..3aff1334 100644 --- a/src/constraint/ContactPoint.h +++ b/src/constraint/ContactPoint.h @@ -224,6 +224,12 @@ class ContactPoint : public Constraint { /// Return the number of bytes used by the contact point virtual size_t getSizeInBytes() const; + /// Initialize before solving the constraint + virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); + + /// Solve the constraint + virtual void solve(const ConstraintSolverData& constraintSolverData); + #ifdef VISUAL_DEBUG /// Draw the contact (for debugging) void draw() const; diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 0291eee4..53a271af 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -31,12 +31,14 @@ using namespace reactphysics3d; // Constructor ConstraintSolver::ConstraintSolver(std::set& joints, - std::vector& constrainedLinearVelocities, - std::vector& constrainedAngularVelocities, + std::vector& linearVelocities, + std::vector& angularVelocities, const std::map& mapBodyToVelocityIndex) - : mJoints(joints), mConstrainedLinearVelocities(constrainedLinearVelocities), - mConstrainedAngularVelocities(constrainedAngularVelocities), - mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex) { + : mJoints(joints), mLinearVelocities(linearVelocities), + mAngularVelocities(angularVelocities), + mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), + mIsWarmStartingActive(false), mConstraintSolverData(linearVelocities, + angularVelocities, mapBodyToVelocityIndex){ } @@ -52,6 +54,28 @@ void ConstraintSolver::initialize(decimal dt) { // Set the current time step mTimeStep = dt; + + // Initialize the constraint solver data used to initialize and solve the constraints + mConstraintSolverData.timeStep = mTimeStep; + mConstraintSolverData.isWarmStartingActive = mIsWarmStartingActive; + + // For each joint + std::set::iterator it; + for (it = mJoints.begin(); it != mJoints.end(); ++it) { + + Constraint* joint = (*it); + + // Get the rigid bodies of the joint + RigidBody* body1 = joint->getBody1(); + RigidBody* body2 = joint->getBody2(); + + // Add the bodies to the set of constrained bodies + mConstraintBodies.insert(body1); + mConstraintBodies.insert(body2); + + // Initialize the constraint before solving it + joint->initBeforeSolve(mConstraintSolverData); + } } // Solve the constraints @@ -59,4 +83,13 @@ void ConstraintSolver::solve() { PROFILE("ConstraintSolver::solve()"); + // For each joint + std::set::iterator it; + for (it = mJoints.begin(); it != mJoints.end(); ++it) { + + Constraint* joint = (*it); + + // Solve the constraint + joint->solve(mConstraintSolverData); + } } diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index 08e4a08f..d72f9e98 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -35,6 +35,43 @@ namespace reactphysics3d { +// Structure ConstraintSolverData +/** + * This structure contains data from the constraint solver that are used to solve + * each joint constraint. + */ +struct ConstraintSolverData { + + public : + + /// Current time step of the simulation + decimal timeStep; + + /// Reference to the bodies linear velocities + std::vector& linearVelocities; + + /// Reference to the bodies angular velocities + std::vector& angularVelocities; + + /// Reference to the map that associates rigid body to their index + /// in the constrained velocities array + const std::map& mapBodyToConstrainedVelocityIndex; + + /// True if warm starting of the solver is active + bool isWarmStartingActive; + + /// Constructor + ConstraintSolverData(std::vector& refLinearVelocities, + std::vector& refAngularVelocities, + const std::map& refMapBodyToConstrainedVelocityIndex) + :linearVelocities(refLinearVelocities), + angularVelocities(refAngularVelocities), + mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){ + + } + +}; + // Class ConstraintSolver /** * This class represents the constraint solver that is used to solve constraints between @@ -113,31 +150,38 @@ class ConstraintSolver { /// Reference to all the joints of the world std::set& mJoints; + /// Constrained bodies + std::set mConstraintBodies; + /// Reference to the array of constrained linear velocities (state of the linear velocities /// after solving the constraints) - std::vector& mConstrainedLinearVelocities; + std::vector& mLinearVelocities; /// Reference to the array of constrained angular velocities (state of the angular velocities /// after solving the constraints) - std::vector& mConstrainedAngularVelocities; + std::vector& mAngularVelocities; - /// Reference to the map of rigid body to their index in the constrained velocities array + /// Reference to the map that associates rigid body to their index in + /// the constrained velocities array const std::map& mMapBodyToConstrainedVelocityIndex; - /// Number of iterations of the contact solver - uint mNbIterations; - /// Current time step decimal mTimeStep; + /// True if the warm starting of the solver is active + bool mIsWarmStartingActive; + + /// Constraint solver data used to initialize and solve the constraints + ConstraintSolverData mConstraintSolverData; + public : // -------------------- Methods -------------------- // /// Constructor ConstraintSolver(std::set& joints, - std::vector& constrainedLinearVelocities, - std::vector& constrainedAngularVelocities, + std::vector& linearVelocities, + std::vector& angularVelocities, const std::map& mapBodyToVelocityIndex); /// Destructor diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 3399ac0d..8467a710 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -46,8 +46,8 @@ ContactSolver::ContactSolver(std::vector& contactManifolds, :mContactManifolds(contactManifolds), mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL), mContactConstraints(NULL), - mConstrainedLinearVelocities(constrainedLinearVelocities), - mConstrainedAngularVelocities(constrainedAngularVelocities), + mLinearVelocities(constrainedLinearVelocities), + mAngularVelocities(constrainedAngularVelocities), mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), mIsWarmStartingActive(true), mIsSplitImpulseActive(true), mIsSolveFrictionAtContactManifoldCenterActive(true) { @@ -186,8 +186,8 @@ void ContactSolver::initialize(decimal dt) { assert(mConstraintBodies.size() > 0); assert(mMapBodyToConstrainedVelocityIndex.size() >= mConstraintBodies.size()); - assert(mConstrainedLinearVelocities.size() >= mConstraintBodies.size()); - assert(mConstrainedAngularVelocities.size() >= mConstraintBodies.size()); + assert(mLinearVelocities.size() >= mConstraintBodies.size()); + assert(mAngularVelocities.size() >= mConstraintBodies.size()); // Initialize the split impulse velocities initializeSplitImpulseVelocities(); @@ -231,10 +231,10 @@ void ContactSolver::initializeContactConstraints() { } // Get the velocities of the bodies - const Vector3& v1 = mConstrainedLinearVelocities[manifold.indexBody1]; - const Vector3& w1 = mConstrainedAngularVelocities[manifold.indexBody1]; - const Vector3& v2 = mConstrainedLinearVelocities[manifold.indexBody2]; - const Vector3& w2 = mConstrainedAngularVelocities[manifold.indexBody2]; + const Vector3& v1 = mLinearVelocities[manifold.indexBody1]; + const Vector3& w1 = mAngularVelocities[manifold.indexBody1]; + const Vector3& v2 = mLinearVelocities[manifold.indexBody2]; + const Vector3& w2 = mAngularVelocities[manifold.indexBody2]; // For each contact point constraint for (uint i=0; i #include /// ReactPhysics3D namespace namespace reactphysics3d { -// Declarations -class DynamicsWorld; - -// Structure Impulse -/** - * Represents an impulse that we can apply to bodies in the contact or constraint solver. - */ -struct Impulse { - - public: - - /// Linear impulse applied to the first body - const Vector3 linearImpulseBody1; - - /// Linear impulse applied to the second body - const Vector3 linearImpulseBody2; - - /// Angular impulse applied to the first body - const Vector3 angularImpulseBody1; - - /// Angular impulse applied to the second body - const Vector3 angularImpulseBody2; - - /// Constructor - Impulse(const Vector3& linearImpulseBody1, const Vector3& angularImpulseBody1, - const Vector3& linearImpulseBody2, const Vector3& angularImpulseBody2) - : linearImpulseBody1(linearImpulseBody1), angularImpulseBody1(angularImpulseBody1), - linearImpulseBody2(linearImpulseBody2), angularImpulseBody2(angularImpulseBody2) { - - } -}; - // Class Contact Solver /** @@ -85,7 +54,7 @@ struct Impulse { * F_c = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a * Lagrange multiplier. Therefore, finding the force F_c is equivalent to finding the Lagrange * multiplier lambda. - + * * An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a * body to change its velocity. The idea of the Sequential Impulse technique is to apply * impulses to bodies of each constraints in order to keep the constraint satisfied. @@ -363,13 +332,11 @@ class ContactSolver { /// Constrained bodies std::set mConstraintBodies; - /// Reference to the array of constrained linear velocities (state of the linear velocities - /// after solving the constraints) - std::vector& mConstrainedLinearVelocities; + /// Reference to the array of linear velocities + std::vector& mLinearVelocities; - /// Reference to the array of constrained angular velocities (state of the angular velocities - /// after solving the constraints) - std::vector& mConstrainedAngularVelocities; + /// Reference to the array of angular velocities + std::vector& mAngularVelocities; /// Reference to the map of rigid body to their index in the constrained velocities array const std::map& mMapBodyToConstrainedVelocityIndex; diff --git a/src/engine/Impulse.h b/src/engine/Impulse.h new file mode 100644 index 00000000..5ed9e228 --- /dev/null +++ b/src/engine/Impulse.h @@ -0,0 +1,65 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2013 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 REACTPHYSICS3D_IMPULSE_H +#define REACTPHYSICS3D_IMPULSE_H + +// Libraries +#include "../mathematics/mathematics.h" + +namespace reactphysics3d { + +// Structure Impulse +/** + * Represents an impulse that we can apply to bodies in the contact or constraint solver. + */ +struct Impulse { + + public: + + /// Linear impulse applied to the first body + const Vector3 linearImpulseBody1; + + /// Linear impulse applied to the second body + const Vector3 linearImpulseBody2; + + /// Angular impulse applied to the first body + const Vector3 angularImpulseBody1; + + /// Angular impulse applied to the second body + const Vector3 angularImpulseBody2; + + /// Constructor + Impulse(const Vector3& linearImpulseBody1, const Vector3& angularImpulseBody1, + const Vector3& linearImpulseBody2, const Vector3& angularImpulseBody2) + : linearImpulseBody1(linearImpulseBody1), angularImpulseBody1(angularImpulseBody1), + linearImpulseBody2(linearImpulseBody2), angularImpulseBody2(angularImpulseBody2) { + + } +}; + +} + +#endif diff --git a/src/mathematics/Matrix3x3.h b/src/mathematics/Matrix3x3.h index 560d4031..d9382b04 100644 --- a/src/mathematics/Matrix3x3.h +++ b/src/mathematics/Matrix3x3.h @@ -105,6 +105,10 @@ class Matrix3x3 { /// Return the 3x3 identity matrix static Matrix3x3 identity(); + /// Return a skew-symmetric matrix using a given vector that can be used + /// to compute cross product with another vector using matrix multiplication + static Matrix3x3 computeSkewSymmetricMatrixForCrossProduct(const Vector3& vector); + /// Overloaded operator for addition friend Matrix3x3 operator+(const Matrix3x3& matrix1, const Matrix3x3& matrix2); @@ -215,6 +219,12 @@ inline Matrix3x3 Matrix3x3::identity() { return Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); } +// Return a skew-symmetric matrix using a given vector that can be used +// to compute cross product with another vector using matrix multiplication +inline Matrix3x3 Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(const Vector3& vector) { + return Matrix3x3(0, -vector.z, vector.y, vector.z, 0, -vector.x, -vector.y, vector.x, 0); +} + // Return the matrix with absolute values inline Matrix3x3 Matrix3x3::getAbsoluteMatrix() const { return Matrix3x3(fabs(mRows[0][0]), fabs(mRows[0][1]), fabs(mRows[0][2]), diff --git a/src/mathematics/Quaternion.h b/src/mathematics/Quaternion.h index c6fd64a1..eb957db7 100644 --- a/src/mathematics/Quaternion.h +++ b/src/mathematics/Quaternion.h @@ -131,7 +131,7 @@ struct Quaternion { Quaternion operator*(const Quaternion& quaternion) const; /// Overloaded operator for the multiplication with a vector - Vector3 operator*(const Vector3& point); + Vector3 operator*(const Vector3& point) const; /// Overloaded operator for assignment Quaternion& operator=(const Quaternion& quaternion); @@ -250,7 +250,7 @@ inline Quaternion Quaternion::operator*(const Quaternion& quaternion) const { // Overloaded operator for the multiplication with a vector. /// This methods rotates a point given the rotation of a quaternion. -inline Vector3 Quaternion::operator*(const Vector3& point) { +inline Vector3 Quaternion::operator*(const Vector3& point) const { Quaternion p(point.x, point.y, point.z, 0.0); return (((*this) * p) * getConjugate()).getVectorV(); } diff --git a/src/mathematics/Transform.h b/src/mathematics/Transform.h index 2d3933db..9f5e07e1 100644 --- a/src/mathematics/Transform.h +++ b/src/mathematics/Transform.h @@ -92,7 +92,7 @@ class Transform { void getOpenGLMatrix(decimal* openglMatrix) const; /// Return the inverse of the transform - Transform inverse() const; + Transform getInverse() const; /// Return an interpolated transform static Transform interpolateTransforms(const Transform& oldTransform, @@ -167,7 +167,7 @@ inline void Transform::getOpenGLMatrix(decimal* openglMatrix) const { } // Return the inverse of the transform -inline Transform Transform::inverse() const { +inline Transform Transform::getInverse() const { const Quaternion& invQuaternion = mOrientation.getInverse(); Matrix3x3 invMatrix = invQuaternion.getMatrix(); return Transform(invMatrix * (-mPosition), invQuaternion); diff --git a/src/reactphysics3d.h b/src/reactphysics3d.h index 09331956..5f463e13 100644 --- a/src/reactphysics3d.h +++ b/src/reactphysics3d.h @@ -47,6 +47,7 @@ #include "collision/shapes/ConeShape.h" #include "collision/shapes/CylinderShape.h" #include "collision/shapes/AABB.h" +#include "constraint/BallAndSocketJoint.h" /// Alias to the ReactPhysics3D namespace namespace rp3d = reactphysics3d; diff --git a/test/tests/mathematics/TestMatrix3x3.h b/test/tests/mathematics/TestMatrix3x3.h index ccdda9a9..ca0a2bb2 100644 --- a/test/tests/mathematics/TestMatrix3x3.h +++ b/test/tests/mathematics/TestMatrix3x3.h @@ -196,6 +196,15 @@ class TestMatrix3x3 : public Test { test(matrix.getAbsoluteMatrix() == Matrix3x3(24, 64, 253, 35, 52, 72, 21, 35, 363)); Matrix3x3 absoluteMatrix = matrix2.getAbsoluteMatrix(); test(absoluteMatrix == Matrix3x3(2, 3, 4, 5, 6, 7, 8, 9, 10)); + + // Test method that computes skew-symmetric matrix for cross product + Vector3 vector1(3, -5, 6); + Vector3 vector2(73, 42, 26); + Matrix3x3 skewMatrix = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(vector1); + test(skewMatrix == Matrix3x3(0, -6, -5, 6, 0, -3, 5, 3, 0)); + Vector3 crossProduct1 = vector1.cross(vector2); + Vector3 crossProduct2 = skewMatrix * vector2; + test(crossProduct1 == crossProduct2); } /// Test the operators diff --git a/test/tests/mathematics/TestTransform.h b/test/tests/mathematics/TestTransform.h index 4c700a0d..72804053 100644 --- a/test/tests/mathematics/TestTransform.h +++ b/test/tests/mathematics/TestTransform.h @@ -114,7 +114,7 @@ class TestTransform : public Test { /// Test the inverse void testInverse() { - Transform inverseTransform = mTransform1.inverse(); + Transform inverseTransform = mTransform1.getInverse(); Vector3 vector(2, 3, 4); Vector3 tempVector = mTransform1 * vector; Vector3 tempVector2 = inverseTransform * tempVector; From 0071ed16a8d3f89befe1309be491512d215e3506 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 2 May 2013 22:51:31 +0200 Subject: [PATCH 08/24] Add comments in the dynamics world code --- src/engine/DynamicsWorld.cpp | 10 ++++++++-- src/engine/DynamicsWorld.h | 4 ++-- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 237d5236..aac339ac 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -121,7 +121,9 @@ void DynamicsWorld::update() { setInterpolationFactorToAllBodies(); } -// Integrate position and orientation of the rigid bodies +// Integrate position and orientation of the rigid bodies. +/// The positions and orientations of the bodies are integrated using +/// the sympletic Euler time stepping scheme. void DynamicsWorld::integrateRigidBodiesPositions() { PROFILE("DynamicsWorld::updateRigidBodiesPositionAndOrientation()"); @@ -197,7 +199,11 @@ void DynamicsWorld::setInterpolationFactorToAllBodies() { } } -// Integrate the velocities of rigid bodies +// Integrate the velocities of rigid bodies. +/// This method only set the temporary velocities but does not update +/// the actual velocitiy of the bodies. The velocities updated in this method +/// might violate the constraints and will be corrected in the constraint and +/// contact solver. void DynamicsWorld::integrateRigidBodiesVelocities() { // TODO : Use better memory allocation here diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 22fbc495..0503ebe2 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -99,7 +99,7 @@ class DynamicsWorld : public CollisionWorld { /// Private assignment operator DynamicsWorld& operator=(const DynamicsWorld& world); - /// Integrate the positions and orientations of rigid bodies + /// Integrate the positions and orientations of rigid bodies. void integrateRigidBodiesPositions(); /// Update the position and orientation of a body @@ -109,7 +109,7 @@ class DynamicsWorld : public CollisionWorld { /// Compute and set the interpolation factor to all bodies void setInterpolationFactorToAllBodies(); - /// Integrate the velocities of rigid bodies + /// Integrate the velocities of rigid bodies. void integrateRigidBodiesVelocities(); /// Solve the contacts and constraints From af2fcaeb8213c2848b95f0943a2b4ad14af89121 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 2 May 2013 23:55:10 +0200 Subject: [PATCH 09/24] Fix two issues --- src/constraint/BallAndSocketJoint.cpp | 10 ++++++---- src/engine/DynamicsWorld.cpp | 14 +++++++++----- 2 files changed, 15 insertions(+), 9 deletions(-) diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index 214b7337..8d7ae17d 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -34,8 +34,10 @@ BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo &jointInfo) : Constraint(jointInfo), mImpulse(Vector3(0, 0, 0)) { // Compute the local-space anchor point for each body - mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; - mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; + mLocalAnchorPointBody1 = mBody1->getTransform().getOrientation().getInverse() * + jointInfo.anchorPointWorldSpace; + mLocalAnchorPointBody2 = mBody2->getTransform().getOrientation().getInverse() * + jointInfo.anchorPointWorldSpace; } // Destructor @@ -107,13 +109,13 @@ void BallAndSocketJoint::solve(const ConstraintSolverData& constraintSolverData) Vector3 Jv = -v1 + mU1World.cross(w1) + v2 - mU2World.cross(w2); // Compute the bias "b" of the constraint - decimal beta = 0.8; // TODO : Use a constant here + decimal beta = 0.7; // TODO : Use a constant here decimal biasFactor = -(beta/constraintSolverData.timeStep); Vector3 b = biasFactor * (x2 + mU2World - x1 - mU1World); // Compute the Lagrange multiplier lambda Vector3 deltaLambda = mInverseMassMatrix * (-Jv - b); - mImpulse = mImpulse + deltaLambda; + mImpulse += deltaLambda; // Compute the impulse P=J^T * lambda Vector3 linearImpulseBody1 = -deltaLambda; diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index aac339ac..8dce0ece 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -219,11 +219,15 @@ void DynamicsWorld::integrateRigidBodiesVelocities() { RigidBody* rigidBody = *it; mMapBodyToConstrainedVelocityIndex.insert(std::make_pair(rigidBody, i)); - // Integrate the external force to get the new velocity of the body - mConstrainedLinearVelocities[i] = rigidBody->getLinearVelocity() + - dt * rigidBody->getMassInverse() * rigidBody->getExternalForce(); - mConstrainedAngularVelocities[i] = rigidBody->getAngularVelocity() + - dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque(); + // If the body is allowed to move + if (rigidBody->getIsMotionEnabled()) { + + // Integrate the external force to get the new velocity of the body + mConstrainedLinearVelocities[i] = rigidBody->getLinearVelocity() + + dt * rigidBody->getMassInverse() * rigidBody->getExternalForce(); + mConstrainedAngularVelocities[i] = rigidBody->getAngularVelocity() + + dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque(); + } i++; } From da78e5d79a547b929341a6fedfa1b0d9d7dd924c Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sat, 4 May 2013 09:20:53 +0200 Subject: [PATCH 10/24] Modifications in the BallAndSocketJoint to make it work --- examples/joints/Scene.cpp | 4 ++-- src/body/RigidBody.h | 4 ++-- src/constraint/BallAndSocketJoint.cpp | 30 +++++++++++---------------- src/engine/DynamicsWorld.cpp | 5 ++++- 4 files changed, 20 insertions(+), 23 deletions(-) diff --git a/examples/joints/Scene.cpp b/examples/joints/Scene.cpp index 5e93b09c..8c1ea4d0 100644 --- a/examples/joints/Scene.cpp +++ b/examples/joints/Scene.cpp @@ -167,7 +167,7 @@ void Scene::createBallAndSocketJoints() { // --------------- Create the second box --------------- // // Position of the box - openglframework::Vector3 positionBox2(0, 10, 0); + openglframework::Vector3 positionBox2(5, 10, 0); // Create a box and a corresponding rigid in the dynamics world mBallAndSocketJointBox2 = new Box(BOX_SIZE, positionBox2 , BOX_MASS, mDynamicsWorld); @@ -184,7 +184,7 @@ void Scene::createBallAndSocketJoints() { rp3d::BallAndSocketJointInfo jointInfo; jointInfo.body1 = mBallAndSocketJointBox1->getRigidBody(); jointInfo.body2 = mBallAndSocketJointBox2->getRigidBody(); - jointInfo.anchorPointWorldSpace = rp3d::Vector3(0, 12.5, 0); + jointInfo.anchorPointWorldSpace = rp3d::Vector3(0, 10, 0); // Create the joint in the dynamics world mBallAndSocketJoint = dynamic_cast( diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 0128bb42..1f340961 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -135,7 +135,7 @@ class RigidBody : public CollisionBody { decimal getMassInverse() const; /// Return the local inertia tensor of the body (in body coordinates) - Matrix3x3 getInertiaTensorLocal() const; + const Matrix3x3& getInertiaTensorLocal() const; /// Set the local inertia tensor of the body (in body coordinates) void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal); @@ -222,7 +222,7 @@ inline decimal RigidBody::getMassInverse() const { } // Return the local inertia tensor of the body (in body coordinates) -inline Matrix3x3 RigidBody::getInertiaTensorLocal() const { +inline const Matrix3x3& RigidBody::getInertiaTensorLocal() const { return mInertiaTensorLocal; } diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index 8d7ae17d..dab548a8 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -34,10 +34,8 @@ BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo &jointInfo) : Constraint(jointInfo), mImpulse(Vector3(0, 0, 0)) { // Compute the local-space anchor point for each body - mLocalAnchorPointBody1 = mBody1->getTransform().getOrientation().getInverse() * - jointInfo.anchorPointWorldSpace; - mLocalAnchorPointBody2 = mBody2->getTransform().getOrientation().getInverse() * - jointInfo.anchorPointWorldSpace; + mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; + mLocalAnchorPointBody2 = mBody2->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; } // Destructor @@ -53,8 +51,6 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; // Get the bodies positions and orientations - const Vector3& x1 = mBody1->getTransform().getPosition(); - const Vector3& x2 = mBody2->getTransform().getPosition(); const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); @@ -62,13 +58,9 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS Matrix3x3 inverseInertiaTensorBody1 = mBody1->getInertiaTensorInverseWorld(); Matrix3x3 inverseInertiaTensorBody2 = mBody2->getInertiaTensorInverseWorld(); - // Compute the vector from body center to anchor point in local-space - const Vector3 u1Local = mLocalAnchorPointBody1 - x1; - const Vector3 u2Local = mLocalAnchorPointBody2 - x2; - // Compute the vector from body center to the anchor point in world-space - mU1World = orientationBody1 * u1Local; - mU2World = orientationBody2 * u2Local; + mU1World = orientationBody1 * mLocalAnchorPointBody1; + mU2World = orientationBody2 * mLocalAnchorPointBody2; // Compute the corresponding skew-symmetric matrices Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mU1World); @@ -79,8 +71,10 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS Matrix3x3 massMatrix= Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies) + - skewSymmetricMatrixU1 * inverseInertiaTensorBody1 * skewSymmetricMatrixU1+ - skewSymmetricMatrixU2 * inverseInertiaTensorBody2 * skewSymmetricMatrixU2; + skewSymmetricMatrixU1 * inverseInertiaTensorBody1 * + skewSymmetricMatrixU1.getTranspose() + + skewSymmetricMatrixU2 * inverseInertiaTensorBody2 * + skewSymmetricMatrixU2.getTranspose(); // Compute the inverse mass matrix K mInverseMassMatrix = massMatrix.getInverse(); @@ -109,8 +103,8 @@ void BallAndSocketJoint::solve(const ConstraintSolverData& constraintSolverData) Vector3 Jv = -v1 + mU1World.cross(w1) + v2 - mU2World.cross(w2); // Compute the bias "b" of the constraint - decimal beta = 0.7; // TODO : Use a constant here - decimal biasFactor = -(beta/constraintSolverData.timeStep); + decimal beta = decimal(0.2); // TODO : Use a constant here + decimal biasFactor = (beta / constraintSolverData.timeStep); Vector3 b = biasFactor * (x2 + mU2World - x1 - mU1World); // Compute the Lagrange multiplier lambda @@ -119,9 +113,9 @@ void BallAndSocketJoint::solve(const ConstraintSolverData& constraintSolverData) // Compute the impulse P=J^T * lambda Vector3 linearImpulseBody1 = -deltaLambda; - Vector3 angularImpulseBody1 = mU1World.cross(deltaLambda); + Vector3 angularImpulseBody1 = deltaLambda.cross(mU1World); Vector3 linearImpulseBody2 = deltaLambda; - Vector3 angularImpulseBody2 = -mU2World.cross(deltaLambda); + Vector3 angularImpulseBody2 = -deltaLambda.cross(mU2World); // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 8dce0ece..ed69e4a5 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -407,11 +407,14 @@ void DynamicsWorld::destroyJoint(Constraint* joint) { // Remove the joint from the world mJoints.erase(joint); + // Get the size in bytes of the joint + size_t nbBytes = joint->getSizeInBytes(); + // Call the destructor of the joint joint->Constraint::~Constraint(); // Release the allocated memory - mMemoryAllocator.release(joint, joint->getSizeInBytes()); + mMemoryAllocator.release(joint, nbBytes); } // Notify the world about a new broad-phase overlapping pair From b87f981827807581a6142cb05e3b8cc3aaa3258a Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 8 May 2013 23:33:04 +0200 Subject: [PATCH 11/24] Start working on the slider joint --- examples/fallingcubes/Scene.cpp | 2 +- examples/joints/Scene.cpp | 10 +- src/configuration.h | 23 ++++- src/constraint/BallAndSocketJoint.cpp | 32 ++++--- src/constraint/BallAndSocketJoint.h | 14 ++- src/constraint/Constraint.cpp | 3 +- src/constraint/Constraint.h | 23 ++++- src/constraint/ContactPoint.cpp | 9 +- src/constraint/ContactPoint.h | 7 +- src/constraint/SliderJoint.cpp | 105 +++++++++++++++++++++ src/constraint/SliderJoint.h | 131 ++++++++++++++++++++++++++ src/engine/ConstraintSolver.cpp | 28 +++++- src/engine/ConstraintSolver.h | 24 ++++- src/engine/DynamicsWorld.cpp | 38 +++++++- src/engine/DynamicsWorld.h | 58 +++++++++--- 15 files changed, 446 insertions(+), 61 deletions(-) create mode 100644 src/constraint/SliderJoint.cpp create mode 100644 src/constraint/SliderJoint.h diff --git a/examples/fallingcubes/Scene.cpp b/examples/fallingcubes/Scene.cpp index 693a0fa5..b11f3a84 100644 --- a/examples/fallingcubes/Scene.cpp +++ b/examples/fallingcubes/Scene.cpp @@ -54,7 +54,7 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0), mDynamicsWorld = new rp3d::DynamicsWorld(gravity, timeStep); // Set the number of iterations of the constraint solver - mDynamicsWorld->setNbIterationsSolver(15); + mDynamicsWorld->setNbIterationsVelocitySolver(15); float radius = 2.0f; diff --git a/examples/joints/Scene.cpp b/examples/joints/Scene.cpp index 8c1ea4d0..c1b5060b 100644 --- a/examples/joints/Scene.cpp +++ b/examples/joints/Scene.cpp @@ -54,7 +54,7 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0), mDynamicsWorld = new rp3d::DynamicsWorld(gravity, timeStep); // Set the number of iterations of the constraint solver - mDynamicsWorld->setNbIterationsSolver(15); + mDynamicsWorld->setNbIterationsVelocitySolver(15); // Create the Ball-and-Socket joint createBallAndSocketJoints(); @@ -181,10 +181,10 @@ void Scene::createBallAndSocketJoints() { // --------------- Create the joint --------------- // // Create the joint info object - rp3d::BallAndSocketJointInfo jointInfo; - jointInfo.body1 = mBallAndSocketJointBox1->getRigidBody(); - jointInfo.body2 = mBallAndSocketJointBox2->getRigidBody(); - jointInfo.anchorPointWorldSpace = rp3d::Vector3(0, 10, 0); + rp3d::RigidBody* body1 = mBallAndSocketJointBox1->getRigidBody(); + rp3d::RigidBody* body2 = mBallAndSocketJointBox2->getRigidBody(); + const rp3d::Vector3 anchorPointWorldSpace(0, 10, 0); + rp3d::BallAndSocketJointInfo jointInfo(body1, body2, anchorPointWorldSpace); // Create the joint in the dynamics world mBallAndSocketJoint = dynamic_cast( diff --git a/src/configuration.h b/src/configuration.h index 59319ae8..eae264f9 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -51,6 +51,22 @@ typedef long unsigned int luint; typedef luint bodyindex; typedef std::pair bodyindexpair; +// ------------------- Enumerations ------------------- // + +/// Position correction technique used in the constraint solver (for joints). +/// BAUMGARTE : Faster but can be innacurate in some situations. This is the option +/// used by default. +/// NON_LINEAR_GAUSS_SEIDEL : Slower but more precise. +enum JointsPositionCorrectionTechnique {BAUMGARTE_JOINTS, NON_LINEAR_GAUSS_SEIDEL}; + +/// Position correction technique used in the contact solver (for contacts) +/// BAUMGARTE : Faster but can be innacurate and can lead to unexpected bounciness +/// in some situations (due to error correction factor being added to +/// the bodies momentum). +/// SPLIT_IMPULSES : A bit slower but the error correction factor is not added to the +/// bodies momentum. This is the option used by default. +enum ContactsPositionCorrectionTechnique {BAUMGARTE_CONTACTS, SPLIT_IMPULSES}; + // ------------------- Constants ------------------- // /// Smallest decimal value (negative) @@ -83,8 +99,11 @@ const decimal PERSISTENT_CONTACT_DIST_THRESHOLD = decimal(0.03); /// Velocity threshold for contact velocity restitution const decimal RESTITUTION_VELOCITY_THRESHOLD = decimal(1.0); -/// Number of iterations when solving a LCP problem -const uint DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS = 15; +/// Number of iterations when solving the velocity constraints of the Sequential Impulse technique +const uint DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 15; + +/// Number of iterations when solving the position constraints of the Sequential Impulse technique +const uint DEFAULT_POSITION_SOLVER_NB_ITERATIONS = 3; // TODO : Maybe we can use less iterations here } diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index dab548a8..b4fc6575 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -55,8 +55,8 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); // Get the inertia tensor of bodies - Matrix3x3 inverseInertiaTensorBody1 = mBody1->getInertiaTensorInverseWorld(); - Matrix3x3 inverseInertiaTensorBody2 = mBody2->getInertiaTensorInverseWorld(); + const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); + const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); // Compute the vector from body center to the anchor point in world-space mU1World = orientationBody1 * mLocalAnchorPointBody1; @@ -66,22 +66,20 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mU1World); Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mU2World); - // Compute the matrix JM^-1J^t + // Compute the matrix K=JM^-1J^t (3x3 matrix) decimal inverseMassBodies = mBody1->getMassInverse() + mBody2->getMassInverse(); Matrix3x3 massMatrix= Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies) + - skewSymmetricMatrixU1 * inverseInertiaTensorBody1 * - skewSymmetricMatrixU1.getTranspose() + - skewSymmetricMatrixU2 * inverseInertiaTensorBody2 * - skewSymmetricMatrixU2.getTranspose(); + skewSymmetricMatrixU1 * I1 * skewSymmetricMatrixU1.getTranspose() + + skewSymmetricMatrixU2 * I2 * skewSymmetricMatrixU2.getTranspose(); - // Compute the inverse mass matrix K + // Compute the inverse mass matrix K^-1 mInverseMassMatrix = massMatrix.getInverse(); } -// Solve the constraint -void BallAndSocketJoint::solve(const ConstraintSolverData& constraintSolverData) { +// Solve the velocity constraint +void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { // Get the body positions const Vector3& x1 = mBody1->getTransform().getPosition(); @@ -103,9 +101,12 @@ void BallAndSocketJoint::solve(const ConstraintSolverData& constraintSolverData) Vector3 Jv = -v1 + mU1World.cross(w1) + v2 - mU2World.cross(w2); // Compute the bias "b" of the constraint - decimal beta = decimal(0.2); // TODO : Use a constant here - decimal biasFactor = (beta / constraintSolverData.timeStep); - Vector3 b = biasFactor * (x2 + mU2World - x1 - mU1World); + Vector3 b(0, 0, 0); + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + decimal beta = decimal(0.2); // TODO : Use a constant here + decimal biasFactor = (beta / constraintSolverData.timeStep); + b = biasFactor * (x2 + mU2World - x1 - mU1World); + } // Compute the Lagrange multiplier lambda Vector3 deltaLambda = mInverseMassMatrix * (-Jv - b); @@ -128,3 +129,8 @@ void BallAndSocketJoint::solve(const ConstraintSolverData& constraintSolverData) } } +// Solve the position constraint +void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) { + +} + diff --git a/src/constraint/BallAndSocketJoint.h b/src/constraint/BallAndSocketJoint.h index a69507ca..0280c28c 100644 --- a/src/constraint/BallAndSocketJoint.h +++ b/src/constraint/BallAndSocketJoint.h @@ -43,11 +43,14 @@ struct BallAndSocketJointInfo : public ConstraintInfo { // -------------------- Attributes -------------------- // - /// Anchor point (in world space coordinates) + /// Anchor point (in world-space coordinates) Vector3 anchorPointWorldSpace; /// Constructor - BallAndSocketJointInfo() : ConstraintInfo(BALLSOCKETJOINT) {} + BallAndSocketJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, + const Vector3& initAnchorPointWorldSpace) + : ConstraintInfo(rigidBody1, rigidBody2, BALLSOCKETJOINT), + anchorPointWorldSpace(initAnchorPointWorldSpace){} }; // Class BallAndSocketJoint @@ -101,8 +104,11 @@ class BallAndSocketJoint : public Constraint { /// Initialize before solving the constraint virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); - /// Solve the constraint - virtual void solve(const ConstraintSolverData& constraintSolverData); + /// Solve the velocity constraint + virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); + + /// Solve the position constraint + virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); }; // Return the number of bytes used by the joint diff --git a/src/constraint/Constraint.cpp b/src/constraint/Constraint.cpp index 7ecded94..234861e3 100644 --- a/src/constraint/Constraint.cpp +++ b/src/constraint/Constraint.cpp @@ -31,7 +31,8 @@ using namespace reactphysics3d; // Constructor Constraint::Constraint(const ConstraintInfo& constraintInfo) :mBody1(constraintInfo.body1), mBody2(constraintInfo.body2), mActive(true), - mType(constraintInfo.type) { + mType(constraintInfo.type), + mPositionCorrectionTechnique(constraintInfo.positionCorrectionTechnique) { assert(mBody1 != NULL); assert(mBody2 != NULL); diff --git a/src/constraint/Constraint.h b/src/constraint/Constraint.h index ae3918e0..58181768 100644 --- a/src/constraint/Constraint.h +++ b/src/constraint/Constraint.h @@ -27,6 +27,7 @@ #define REACTPHYSICS3D_CONSTRAINT_H // Libraries +#include "../configuration.h" #include "../body/RigidBody.h" #include "../mathematics/mathematics.h" @@ -34,7 +35,7 @@ namespace reactphysics3d { // Enumeration for the type of a constraint -enum ConstraintType {CONTACT, BALLSOCKETJOINT}; +enum ConstraintType {CONTACT, BALLSOCKETJOINT, SLIDERJOINT}; // Class declarations struct ConstraintSolverData; @@ -58,13 +59,19 @@ struct ConstraintInfo { /// Type of the constraint ConstraintType type; + /// Position correction technique used for the constraint (used for joints). + /// By default, the BAUMGARTE technique is used + JointsPositionCorrectionTechnique positionCorrectionTechnique; + /// Constructor ConstraintInfo(ConstraintType constraintType) - : body1(NULL), body2(NULL), type(constraintType) {} + : body1(NULL), body2(NULL), type(constraintType), + positionCorrectionTechnique(BAUMGARTE_JOINTS) {} /// Constructor ConstraintInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, ConstraintType constraintType) - : body1(rigidBody1), body2(rigidBody2), type(constraintType) { + : body1(rigidBody1), body2(rigidBody2), type(constraintType), + positionCorrectionTechnique(BAUMGARTE_JOINTS) { } /// Destructor @@ -103,6 +110,9 @@ class Constraint { /// Body 2 index in the velocity array to solve the constraint uint mIndexBody2; + /// Position correction technique used for the constraint (used for joints) + JointsPositionCorrectionTechnique mPositionCorrectionTechnique; + // -------------------- Methods -------------------- // /// Private copy-constructor @@ -139,8 +149,11 @@ class Constraint { /// Initialize before solving the constraint virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData) = 0; - /// Solve the constraint - virtual void solve(const ConstraintSolverData& constraintSolverData) = 0; + /// Solve the velocity constraint + virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) = 0; + + /// Solve the position constraint + virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData) = 0; }; // Return the reference to the body 1 diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp index b945c8e3..178a5e1d 100644 --- a/src/constraint/ContactPoint.cpp +++ b/src/constraint/ContactPoint.cpp @@ -56,7 +56,12 @@ void ContactPoint::initBeforeSolve(const ConstraintSolverData& constraintSolverD } -// Solve the constraint -void ContactPoint::solve(const ConstraintSolverData& constraintSolverData) { +// Solve the velocity constraint +void ContactPoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { + +} + +// Solve the position constraint +void ContactPoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) { } diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h index 3aff1334..0047c020 100644 --- a/src/constraint/ContactPoint.h +++ b/src/constraint/ContactPoint.h @@ -227,8 +227,11 @@ class ContactPoint : public Constraint { /// Initialize before solving the constraint virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); - /// Solve the constraint - virtual void solve(const ConstraintSolverData& constraintSolverData); + /// Solve the velocity constraint + virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); + + /// Solve the position constraint + virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); #ifdef VISUAL_DEBUG /// Draw the contact (for debugging) diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp new file mode 100644 index 00000000..0d763e9f --- /dev/null +++ b/src/constraint/SliderJoint.cpp @@ -0,0 +1,105 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2013 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. * +* * +********************************************************************************/ + +// Libraries +#include "SliderJoint.h" + +using namespace reactphysics3d; + +// Constructor +SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) : Constraint(jointInfo) { + + // Compute the local-space anchor point for each body + mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; + mLocalAnchorPointBody2 = mBody2->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; +} + +// Destructor +SliderJoint::~SliderJoint() { + +} + +// Initialize before solving the constraint +void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { + + // Initialize the bodies index in the velocity array + mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; + mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; + + // Get the bodies positions and orientations + const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); + const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); + + // Get the inertia tensor of bodies + const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); + const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); + + // Compute the vector from body center to the anchor point in world-space + mU1World = orientationBody1 * mLocalAnchorPointBody1; + mU2World = orientationBody2 * mLocalAnchorPointBody2; + + // Compute the two orthogonal vectors to vector mU1World in world-space + mN1 = mU1World.getOneUnitOrthogonalVector(); + mN2 = mU1World.cross(mN1); + + // Compute the cross product used in the Jacobian + mU1WorldCrossN1 = mN2; + mU1WorldCrossN2 = mU1World.cross(mN2); + mU2WorldCrossN1 = mU2World.cross(mN1); + mU2WorldCrossN2 = mU2World.cross(mN2); + + // Compute the mass matrix K=JM^-1J^t for the 2 translation constraints (2x2 matrix) + const decimal n1Dotn1 = mN1.lengthSquare(); + const decimal n2Dotn2 = mN2.lengthSquare(); + const decimal sumInverseMass = mBody1->getMassInverse() + mBody2->getMassInverse(); + +} + +// Solve the velocity constraint +void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { + + // Get the body positions + const Vector3& x1 = mBody1->getTransform().getPosition(); + const Vector3& x2 = mBody2->getTransform().getPosition(); + + // Get the velocities + Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; + Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; + Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; + Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; + + // Get the inverse mass and inverse inertia tensors of the bodies + decimal inverseMassBody1 = mBody1->getMassInverse(); + decimal inverseMassBody2 = mBody2->getMassInverse(); + Matrix3x3 inverseInertiaTensorBody1 = mBody1->getInertiaTensorInverseWorld(); + Matrix3x3 inverseInertiaTensorBody2 = mBody2->getInertiaTensorInverseWorld(); + + // Compute J*v +} + +// Solve the position constraint +void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) { + +} diff --git a/src/constraint/SliderJoint.h b/src/constraint/SliderJoint.h new file mode 100644 index 00000000..2d9ae4ec --- /dev/null +++ b/src/constraint/SliderJoint.h @@ -0,0 +1,131 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2013 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 REACTPHYSICS3D_SLIDER_JOINT_H +#define REACTPHYSICS3D_SLIDER_JOINT_H + +// Libraries +#include "../mathematics/mathematics.h" +#include "../engine/ConstraintSolver.h" + +namespace reactphysics3d { + +// Structure SliderJointInfo +/** + * This structure is used to gather the information needed to create a slider + * joint. This structure will be used to create the actual slider joint. + */ +struct SliderJointInfo : public ConstraintInfo { + + public : + + // -------------------- Attributes -------------------- // + + /// Anchor point (in world-space coordinates) + Vector3 anchorPointWorldSpace; + + /// Slider axis (in world-space coordinates) + Vector3 axisWorldSpace; + + /// Constructor + SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, + const Vector3& initAnchorPointWorldSpace, + const Vector3& initAxisWorldSpace) + : ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT), + anchorPointWorldSpace(initAnchorPointWorldSpace), + axisWorldSpace(initAxisWorldSpace) {} +}; + +// Class SliderJoint +/** + * This class represents a slider joint. + */ +class SliderJoint : public Constraint { + + private : + + // -------------------- Attributes -------------------- // + + /// Anchor point of body 1 (in local space coordinates) + Vector3 mLocalAnchorPointBody1; + + /// Anchor point of body 2 (in local space coordinates) + Vector3 mLocalAnchorPointBody2; + + /// Vector from center of body 2 to anchor point in world-space + Vector3 mU1World; + + /// Vector from center of body 2 to anchor point in world-space + Vector3 mU2World; + + /// First vector orthogonal to vector mU1World in world-space + Vector3 mN1; + + /// Second vector orthogonal to vector mU1World and mN1 in world-space + Vector3 mN2; + + /// Cross product of mU1World and mN1 + Vector3 mU1WorldCrossN1; + + /// Cross product of mU1World and mN2 + Vector3 mU1WorldCrossN2; + + /// Cross product of mU2World and mN1 + Vector3 mU2WorldCrossN1; + + /// Cross product of mU2World and mN2 + Vector3 mU2WorldCrossN2; + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + SliderJoint(const SliderJointInfo& jointInfo); + + /// Destructor + virtual ~SliderJoint(); + + /// Return the number of bytes used by the joint + virtual size_t getSizeInBytes() const; + + /// Initialize before solving the constraint + virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); + + /// Solve the velocity constraint + virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); + + /// Solve the position constraint + virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); +}; + +// Return the number of bytes used by the joint +inline size_t SliderJoint::getSizeInBytes() const { + return sizeof(SliderJoint); +} + +} + +#endif diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 53a271af..f825e110 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -37,7 +37,9 @@ ConstraintSolver::ConstraintSolver(std::set& joints, : mJoints(joints), mLinearVelocities(linearVelocities), mAngularVelocities(angularVelocities), mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), - mIsWarmStartingActive(false), mConstraintSolverData(linearVelocities, + mIsWarmStartingActive(false), + mIsNonLinearGaussSeidelPositionCorrectionActive(false), + mConstraintSolverData(linearVelocities, angularVelocities, mapBodyToVelocityIndex){ } @@ -78,10 +80,10 @@ void ConstraintSolver::initialize(decimal dt) { } } -// Solve the constraints -void ConstraintSolver::solve() { +// Solve the velocity constraints +void ConstraintSolver::solveVelocityConstraints() { - PROFILE("ConstraintSolver::solve()"); + PROFILE("ConstraintSolver::solveVelocityConstraints()"); // For each joint std::set::iterator it; @@ -90,6 +92,22 @@ void ConstraintSolver::solve() { Constraint* joint = (*it); // Solve the constraint - joint->solve(mConstraintSolverData); + joint->solveVelocityConstraint(mConstraintSolverData); + } +} + +// Solve the position constraints +void ConstraintSolver::solvePositionConstraints() { + + PROFILE("ConstraintSolver::solvePositionConstraints()"); + + // For each joint + std::set::iterator it; + for (it = mJoints.begin(); it != mJoints.end(); ++it) { + + Constraint* joint = (*it); + + // Solve the constraint + joint->solveVelocityConstraint(mConstraintSolverData); } } diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index d72f9e98..fd9b3249 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -171,6 +171,9 @@ class ConstraintSolver { /// True if the warm starting of the solver is active bool mIsWarmStartingActive; + /// True if the Non-Linear-Gauss-Seidel position correction technique is enabled + bool mIsNonLinearGaussSeidelPositionCorrectionActive; + /// Constraint solver data used to initialize and solve the constraints ConstraintSolverData mConstraintSolverData; @@ -191,9 +194,28 @@ class ConstraintSolver { void initialize(decimal dt); /// Solve the constraints - void solve(); + void solveVelocityConstraints(); + + /// Solve the position constraints + void solvePositionConstraints(); + + /// Return true if the Non-Linear-Gauss-Seidel position correction technique is active + bool getIsNonLinearGaussSeidelPositionCorrectionActive() const; + + /// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique. + void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive); }; +// Return true if the Non-Linear-Gauss-Seidel position correction technique is active +inline bool ConstraintSolver::getIsNonLinearGaussSeidelPositionCorrectionActive() const { + return mIsNonLinearGaussSeidelPositionCorrectionActive; +} + +// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique. +inline void ConstraintSolver::setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive) { + mIsNonLinearGaussSeidelPositionCorrectionActive = isActive; +} + } #endif diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index ed69e4a5..2c046e99 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -26,6 +26,7 @@ // Libraries #include "DynamicsWorld.h" #include "constraint/BallAndSocketJoint.h" +#include "constraint/SliderJoint.h" // Namespaces using namespace reactphysics3d; @@ -38,7 +39,8 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_ mMapBodyToConstrainedVelocityIndex), mConstraintSolver(mJoints, mConstrainedLinearVelocities, mConstrainedAngularVelocities, mMapBodyToConstrainedVelocityIndex), - mNbSolverIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS), + mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS), + mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS), mIsDeactivationActive(DEACTIVATION_ENABLED) { } @@ -248,6 +250,8 @@ void DynamicsWorld::solveContactsAndConstraints() { bool isContactsToSolve = !mContactManifolds.empty(); if (!isConstraintsToSolve && !isContactsToSolve) return; + // ---------- Solve velocity constraints for joints and contacts ---------- // + // If there are contacts if (isContactsToSolve) { @@ -265,11 +269,11 @@ void DynamicsWorld::solveContactsAndConstraints() { mConstraintSolver.initialize(dt); } - // For each iteration of the solver - for (uint i=0; i(jointInfo); + newJoint = new (allocatedMemory) SliderJoint(info); + break; + } + default: { assert(false); diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 0503ebe2..e9c7bcbb 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -59,8 +59,11 @@ class DynamicsWorld : public CollisionWorld { /// Constraint solver ConstraintSolver mConstraintSolver; - /// Number of solver iterations for the Sequential Impulses technique - uint mNbSolverIterations; + /// Number of iterations for the velocity solver of the Sequential Impulses technique + uint mNbVelocitySolverIterations; + + /// Number of iterations for the position solver of the Sequential Impulses technique + uint mNbPositionSolverIterations; /// True if the deactivation (sleeping) of inactive bodies is enabled bool mIsDeactivationActive; @@ -155,19 +158,22 @@ public : /// Update the physics simulation void update(); - /// Set the number of iterations of the constraint solver - void setNbIterationsSolver(uint nbIterations); + /// Set the number of iterations for the velocity constraint solver + void setNbIterationsVelocitySolver(uint nbIterations); - /// Activate or Deactivate the split impulses for contacts - void setIsSplitImpulseActive(bool isActive); + /// Set the number of iterations for the position constraint solver + void setNbIterationsPositionSolver(uint nbIterations); + + /// Set the position correction technique used for contacts + void setContactsPositionCorrectionTechnique(ContactsPositionCorrectionTechnique technique); + + /// Set the position correction technique used for joints + void setJointsPositionCorrectionTechnique(JointsPositionCorrectionTechnique technique); /// Activate or deactivate the solving of friction constraints at the center of /// the contact manifold instead of solving them at each contact point void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); - /// Set the isErrorCorrectionActive value - void setIsErrorCorrectionActive(bool isErrorCorrectionActive); - /// Create a rigid body into the physics world. RigidBody* createRigidBody(const Transform& transform, decimal mass, const Matrix3x3& inertiaTensorLocal, @@ -213,14 +219,36 @@ inline void DynamicsWorld::stop() { mTimer.stop(); } -// Set the number of iterations of the constraint solver -inline void DynamicsWorld::setNbIterationsSolver(uint nbIterations) { - mNbSolverIterations = nbIterations; +// Set the number of iterations for the velocity constraint solver +inline void DynamicsWorld::setNbIterationsVelocitySolver(uint nbIterations) { + mNbVelocitySolverIterations = nbIterations; } -// Activate or Deactivate the split impulses for contacts -inline void DynamicsWorld::setIsSplitImpulseActive(bool isActive) { - mContactSolver.setIsSplitImpulseActive(isActive); +// Set the number of iterations for the position constraint solver +inline void DynamicsWorld::setNbIterationsPositionSolver(uint nbIterations) { + mNbPositionSolverIterations = nbIterations; +} + +// Set the position correction technique used for contacts +inline void DynamicsWorld::setContactsPositionCorrectionTechnique( + ContactsPositionCorrectionTechnique technique) { + if (technique == BAUMGARTE_CONTACTS) { + mContactSolver.setIsSplitImpulseActive(false); + } + else { + mContactSolver.setIsSplitImpulseActive(true); + } +} + +// Set the position correction technique used for joints +inline void DynamicsWorld::setJointsPositionCorrectionTechnique( + JointsPositionCorrectionTechnique technique) { + if (technique == BAUMGARTE_JOINTS) { + mConstraintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(false); + } + else { + mConstraintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(true); + } } // Activate or deactivate the solving of friction constraints at the center of From 7a2c2bdbd57db1de5e3db876b2675fff41d85980 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 9 May 2013 19:02:09 +0200 Subject: [PATCH 12/24] Add Vector2 and Matrix2x3 classes --- src/mathematics/Matrix2x2.cpp | 89 +++++++ src/mathematics/Matrix2x2.h | 312 ++++++++++++++++++++++++ src/mathematics/Vector2.cpp | 71 ++++++ src/mathematics/Vector2.h | 296 ++++++++++++++++++++++ src/mathematics/Vector3.h | 3 - test/tests/mathematics/TestMatrix2x2.h | 239 ++++++++++++++++++ test/tests/mathematics/TestMatrix3x3.h | 7 +- test/tests/mathematics/TestQuaternion.h | 6 +- test/tests/mathematics/TestTransform.h | 6 +- test/tests/mathematics/TestVector2.h | 208 ++++++++++++++++ test/tests/mathematics/TestVector3.h | 6 +- 11 files changed, 1223 insertions(+), 20 deletions(-) create mode 100644 src/mathematics/Matrix2x2.cpp create mode 100644 src/mathematics/Matrix2x2.h create mode 100644 src/mathematics/Vector2.cpp create mode 100644 src/mathematics/Vector2.h create mode 100644 test/tests/mathematics/TestMatrix2x2.h create mode 100644 test/tests/mathematics/TestVector2.h diff --git a/src/mathematics/Matrix2x2.cpp b/src/mathematics/Matrix2x2.cpp new file mode 100644 index 00000000..87353a3d --- /dev/null +++ b/src/mathematics/Matrix2x2.cpp @@ -0,0 +1,89 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2013 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. * +* * +********************************************************************************/ + +// Libraries +#include "Matrix2x2.h" + +using namespace reactphysics3d; + +// Constructor of the class Matrix2x2 +Matrix2x2::Matrix2x2() { + + // Initialize all values in the matrix to zero + setAllValues(0.0, 0.0, 0.0, 0.0); +} + +// Constructor +Matrix2x2::Matrix2x2(decimal value) { + setAllValues(value, value, value, value); +} + +// Constructor with arguments +Matrix2x2::Matrix2x2(decimal a1, decimal a2, decimal b1, decimal b2) { + + // Initialize the matrix with the values + setAllValues(a1, a2, b1, b2); +} + +// Destructor +Matrix2x2::~Matrix2x2() { + +} + +// Copy-constructor +Matrix2x2::Matrix2x2(const Matrix2x2& matrix) { + setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], + matrix.mRows[1][0], matrix.mRows[1][1]); +} + +// Assignment operator +Matrix2x2& Matrix2x2::operator=(const Matrix2x2& matrix) { + + // Check for self-assignment + if (&matrix != this) { + setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], + matrix.mRows[1][0], matrix.mRows[1][1]); + } + return *this; +} + +// Return the inverse matrix +Matrix2x2 Matrix2x2::getInverse() const { + + // Compute the determinant of the matrix + decimal determinant = getDeterminant(); + + // Check if the determinant is equal to zero + assert(std::abs(determinant) > MACHINE_EPSILON); + + decimal invDeterminant = decimal(1.0) / determinant; + + // TODO : Implement this + assert(false); + Matrix2x2 tempMatrix; + + // Return the inverse matrix + return (invDeterminant * tempMatrix); +} diff --git a/src/mathematics/Matrix2x2.h b/src/mathematics/Matrix2x2.h new file mode 100644 index 00000000..ec9c28f1 --- /dev/null +++ b/src/mathematics/Matrix2x2.h @@ -0,0 +1,312 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2013 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 REACTPHYSICS3D_MATRIX2X2_H +#define REACTPHYSICS3D_MATRIX2X2_H + +// Libraries +#include +#include "Vector2.h" + +/// ReactPhysics3D namespace +namespace reactphysics3d { + +// Class Matrix2x2 +/** + * This class represents a 2x2 matrix. + */ +class Matrix2x2 { + + private : + + // -------------------- Attributes -------------------- // + + /// Rows of the matrix; + Vector2 mRows[3]; + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + Matrix2x2(); + + /// Constructor + Matrix2x2(decimal value); + + /// Constructor + Matrix2x2(decimal a1, decimal a2, decimal b1, decimal b2); + + /// Destructor + virtual ~Matrix2x2(); + + /// Copy-constructor + Matrix2x2(const Matrix2x2& matrix); + + /// Assignment operator + Matrix2x2& operator=(const Matrix2x2& matrix); + + /// Set all the values in the matrix + void setAllValues(decimal a1, decimal a2, decimal b1, decimal b2); + + /// Set the matrix to zero + void setToZero(); + + /// Return a column + Vector2 getColumn(int i) const; + + /// Return a row + Vector2 getRow(int i) const; + + /// Return the transpose matrix + Matrix2x2 getTranspose() const; + + /// Return the determinant of the matrix + decimal getDeterminant() const; + + /// Return the trace of the matrix + decimal getTrace() const; + + /// Return the inverse matrix + Matrix2x2 getInverse() const; + + /// Return the matrix with absolute values + Matrix2x2 getAbsoluteMatrix() const; + + /// Set the matrix to the identity matrix + void setToIdentity(); + + /// Return the 2x2 identity matrix + static Matrix2x2 identity(); + + /// Overloaded operator for addition + friend Matrix2x2 operator+(const Matrix2x2& matrix1, const Matrix2x2& matrix2); + + /// Overloaded operator for substraction + friend Matrix2x2 operator-(const Matrix2x2& matrix1, const Matrix2x2& matrix2); + + /// Overloaded operator for the negative of the matrix + friend Matrix2x2 operator-(const Matrix2x2& matrix); + + /// Overloaded operator for multiplication with a number + friend Matrix2x2 operator*(decimal nb, const Matrix2x2& matrix); + + /// Overloaded operator for multiplication with a matrix + friend Matrix2x2 operator*(const Matrix2x2& matrix, decimal nb); + + /// Overloaded operator for matrix multiplication + friend Matrix2x2 operator*(const Matrix2x2& matrix1, const Matrix2x2& matrix2); + + /// Overloaded operator for multiplication with a vector + friend Vector2 operator*(const Matrix2x2& matrix, const Vector2& vector); + + /// Overloaded operator for equality condition + bool operator==(const Matrix2x2& matrix) const; + + /// Overloaded operator for the is different condition + bool operator!= (const Matrix2x2& matrix) const; + + /// Overloaded operator for addition with assignment + Matrix2x2& operator+=(const Matrix2x2& matrix); + + /// Overloaded operator for substraction with assignment + Matrix2x2& operator-=(const Matrix2x2& matrix); + + /// Overloaded operator for multiplication with a number with assignment + Matrix2x2& operator*=(decimal nb); + + /// Overloaded operator to read element of the matrix. + const Vector2& operator[](int row) const; + + /// Overloaded operator to read/write element of the matrix. + Vector2& operator[](int row); +}; + +// Method to set all the values in the matrix +inline void Matrix2x2::setAllValues(decimal a1, decimal a2, + decimal b1, decimal b2) { + mRows[0][0] = a1; mRows[0][1] = a2; + mRows[1][0] = b1; mRows[1][1] = b2; +} + +// Set the matrix to zero +inline void Matrix2x2::setToZero() { + mRows[0].setToZero(); + mRows[1].setToZero(); +} + +// Return a column +inline Vector2 Matrix2x2::getColumn(int i) const { + assert(i>= 0 && i<2); + return Vector2(mRows[0][i], mRows[1][i]); +} + +// Return a row +inline Vector2 Matrix2x2::getRow(int i) const { + assert(i>= 0 && i<2); + return mRows[i]; +} + +// Return the transpose matrix +inline Matrix2x2 Matrix2x2::getTranspose() const { + + // Return the transpose matrix + return Matrix2x2(mRows[0][0], mRows[1][0], + mRows[0][1], mRows[1][1]); +} + +// Return the determinant of the matrix +inline decimal Matrix2x2::getDeterminant() const { + + // Compute and return the determinant of the matrix + return mRows[0][0] * mRows[1][1] - mRows[1][0] * mRows[0][1]; +} + +// Return the trace of the matrix +inline decimal Matrix2x2::getTrace() const { + + // Compute and return the trace + return (mRows[0][0] + mRows[1][1]); +} + +// Set the matrix to the identity matrix +inline void Matrix2x2::setToIdentity() { + mRows[0][0] = 1.0; mRows[0][1] = 0.0; + mRows[1][0] = 0.0; mRows[1][1] = 1.0; +} + +// Return the 2x2 identity matrix +inline Matrix2x2 Matrix2x2::identity() { + + // Return the isdentity matrix + return Matrix2x2(1.0, 0.0, 0.0, 1.0); +} + +// Return the matrix with absolute values +inline Matrix2x2 Matrix2x2::getAbsoluteMatrix() const { + return Matrix2x2(fabs(mRows[0][0]), fabs(mRows[0][1]), + fabs(mRows[1][0]), fabs(mRows[1][1])); +} + +// Overloaded operator for addition +inline Matrix2x2 operator+(const Matrix2x2& matrix1, const Matrix2x2& matrix2) { + return Matrix2x2(matrix1.mRows[0][0] + matrix2.mRows[0][0], + matrix1.mRows[0][1] + matrix2.mRows[0][1], + matrix1.mRows[1][0] + matrix2.mRows[1][0], + matrix1.mRows[1][1] + matrix2.mRows[1][1]); +} + +// Overloaded operator for substraction +inline Matrix2x2 operator-(const Matrix2x2& matrix1, const Matrix2x2& matrix2) { + return Matrix2x2(matrix1.mRows[0][0] - matrix2.mRows[0][0], + matrix1.mRows[0][1] - matrix2.mRows[0][1], + matrix1.mRows[1][0] - matrix2.mRows[1][0], + matrix1.mRows[1][1] - matrix2.mRows[1][1]); +} + +// Overloaded operator for the negative of the matrix +inline Matrix2x2 operator-(const Matrix2x2& matrix) { + return Matrix2x2(-matrix.mRows[0][0], -matrix.mRows[0][1], + -matrix.mRows[1][0], -matrix.mRows[1][1]); +} + +// Overloaded operator for multiplication with a number +inline Matrix2x2 operator*(decimal nb, const Matrix2x2& matrix) { + return Matrix2x2(matrix.mRows[0][0] * nb, matrix.mRows[0][1] * nb, + matrix.mRows[1][0] * nb, matrix.mRows[1][1] * nb); +} + +// Overloaded operator for multiplication with a matrix +inline Matrix2x2 operator*(const Matrix2x2& matrix, decimal nb) { + return nb * matrix; +} + +// Overloaded operator for matrix multiplication +inline Matrix2x2 operator*(const Matrix2x2& matrix1, const Matrix2x2& matrix2) { + return Matrix2x2(matrix1.mRows[0][0] * matrix2.mRows[0][0] + matrix1.mRows[0][1] * + matrix2.mRows[1][0], + matrix1.mRows[0][0] * matrix2.mRows[0][1] + matrix1.mRows[0][1] * + matrix2.mRows[1][1], + matrix1.mRows[1][0] * matrix2.mRows[0][0] + matrix1.mRows[1][1] * + matrix2.mRows[1][0], + matrix1.mRows[1][0] * matrix2.mRows[0][1] + matrix1.mRows[1][1] * + matrix2.mRows[1][1]); +} + +// Overloaded operator for multiplication with a vector +inline Vector2 operator*(const Matrix2x2& matrix, const Vector2& vector) { + return Vector2(matrix.mRows[0][0]*vector.x + matrix.mRows[0][1]*vector.y, + matrix.mRows[1][0]*vector.x + matrix.mRows[1][1]*vector.y); +} + +// Overloaded operator for equality condition +inline bool Matrix2x2::operator==(const Matrix2x2& matrix) const { + return (mRows[0][0] == matrix.mRows[0][0] && mRows[0][1] == matrix.mRows[0][1] && + mRows[1][0] == matrix.mRows[1][0] && mRows[1][1] == matrix.mRows[1][1]); +} + +// Overloaded operator for the is different condition +inline bool Matrix2x2::operator!= (const Matrix2x2& matrix) const { + return !(*this == matrix); +} + +// Overloaded operator for addition with assignment +inline Matrix2x2& Matrix2x2::operator+=(const Matrix2x2& matrix) { + mRows[0][0] += matrix.mRows[0][0]; mRows[0][1] += matrix.mRows[0][1]; + mRows[1][0] += matrix.mRows[1][0]; mRows[1][1] += matrix.mRows[1][1]; + return *this; +} + +// Overloaded operator for substraction with assignment +inline Matrix2x2& Matrix2x2::operator-=(const Matrix2x2& matrix) { + mRows[0][0] -= matrix.mRows[0][0]; mRows[0][1] -= matrix.mRows[0][1]; + mRows[1][0] -= matrix.mRows[1][0]; mRows[1][1] -= matrix.mRows[1][1]; + return *this; +} + +// Overloaded operator for multiplication with a number with assignment +inline Matrix2x2& Matrix2x2::operator*=(decimal nb) { + mRows[0][0] *= nb; mRows[0][1] *= nb; + mRows[1][0] *= nb; mRows[1][1] *= nb; + return *this; +} + +// Overloaded operator to return a row of the matrix. +/// This operator is also used to access a matrix value using the syntax +/// matrix[row][col]. +inline const Vector2& Matrix2x2::operator[](int row) const { + return mRows[row]; +} + +// Overloaded operator to return a row of the matrix. +/// This operator is also used to access a matrix value using the syntax +/// matrix[row][col]. +inline Vector2& Matrix2x2::operator[](int row) { + return mRows[row]; +} + +} + +#endif diff --git a/src/mathematics/Vector2.cpp b/src/mathematics/Vector2.cpp new file mode 100644 index 00000000..c7a2c9f1 --- /dev/null +++ b/src/mathematics/Vector2.cpp @@ -0,0 +1,71 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2013 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. * +* * +********************************************************************************/ + +// Libraries +#include "Vector2.h" +#include + +// Namespaces +using namespace reactphysics3d; + +// Constructor +Vector2::Vector2() : x(0.0), y(0.0) { + +} + +// Constructor with arguments +Vector2::Vector2(decimal newX, decimal newY) : x(newX), y(newY) { + +} + +// Copy-constructor +Vector2::Vector2(const Vector2& vector) : x(vector.x), y(vector.y) { + +} + +// Destructor +Vector2::~Vector2() { + +} + +// Return the corresponding unit vector +Vector2 Vector2::getUnit() const { + decimal lengthVector = length(); + + assert(lengthVector > MACHINE_EPSILON); + + // Compute and return the unit vector + decimal lengthInv = decimal(1.0) / lengthVector; + return Vector2(x * lengthInv, y * lengthInv); +} + +// Return one unit orthogonal vector of the current vector +Vector2 Vector2::getOneUnitOrthogonalVector() const { + + decimal l = length(); + assert(l > MACHINE_EPSILON); + + return Vector2(-y / l, x / l); +} diff --git a/src/mathematics/Vector2.h b/src/mathematics/Vector2.h new file mode 100644 index 00000000..6f1f518e --- /dev/null +++ b/src/mathematics/Vector2.h @@ -0,0 +1,296 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2013 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 REACTPHYSICS3D_VECTOR2_H +#define REACTPHYSICS3D_VECTOR2_H + +// Libraries +#include +#include +#include "mathematics_functions.h" +#include "../decimal.h" + + +/// ReactPhysics3D namespace +namespace reactphysics3d { + +// Class Vector2 +/** + * This class represents a 2D vector. + */ +struct Vector2 { + + public: + + // -------------------- Attributes -------------------- // + + /// Component x + decimal x; + + /// Component y + decimal y; + + // -------------------- Methods -------------------- // + + /// Constructor of the class Vector3D + Vector2(); + + /// Constructor with arguments + Vector2(decimal newX, decimal newY); + + /// Copy-constructor + Vector2(const Vector2& vector); + + /// Destructor + ~Vector2(); + + /// Set all the values of the vector + void setAllValues(decimal newX, decimal newY); + + /// Set the vector to zero + void setToZero(); + + /// Return the length of the vector + decimal length() const; + + /// Return the square of the length of the vector + decimal lengthSquare() const; + + /// Return the corresponding unit vector + Vector2 getUnit() const; + + /// Return one unit orthogonal vector of the current vector + Vector2 getOneUnitOrthogonalVector() const; + + /// Return true if the vector is unit and false otherwise + bool isUnit() const; + + /// Return true if the current vector is the zero vector + bool isZero() const; + + /// Dot product of two vectors + decimal dot(const Vector2& vector) const; + + /// Normalize the vector + void normalize(); + + /// Return the corresponding absolute value vector + Vector2 getAbsoluteVector() const; + + /// Return the axis with the minimal value + int getMinAxis() const; + + /// Return the axis with the maximal value + int getMaxAxis() const; + + /// Overloaded operator for the equality condition + bool operator== (const Vector2& vector) const; + + /// Overloaded operator for the is different condition + bool operator!= (const Vector2& vector) const; + + /// Overloaded operator for addition with assignment + Vector2& operator+=(const Vector2& vector); + + /// Overloaded operator for substraction with assignment + Vector2& operator-=(const Vector2& vector); + + /// Overloaded operator for multiplication with a number with assignment + Vector2& operator*=(decimal number); + + /// Overloaded operator for division by a number with assignment + Vector2& operator/=(decimal number); + + /// Overloaded operator for value access + decimal& operator[] (int index); + + /// Overloaded operator for value access + const decimal& operator[] (int index) const; + + /// Overloaded operator + Vector2& operator=(const Vector2& vector); + + // -------------------- Friends -------------------- // + + friend Vector2 operator+(const Vector2& vector1, const Vector2& vector2); + friend Vector2 operator-(const Vector2& vector1, const Vector2& vector2); + friend Vector2 operator-(const Vector2& vector); + friend Vector2 operator*(const Vector2& vector, decimal number); + friend Vector2 operator*(decimal number, const Vector2& vector); + friend Vector2 operator/(const Vector2& vector, decimal number); +}; + +// Set the vector to zero +inline void Vector2::setToZero() { + x = 0; + y = 0; +} + +// Set all the values of the vector +inline void Vector2::setAllValues(decimal newX, decimal newY) { + x = newX; + y = newY; +} + +// Return the length of the vector +inline decimal Vector2::length() const { + return sqrt(x*x + y*y); +} + +// Return the square of the length of the vector +inline decimal Vector2::lengthSquare() const { + return x*x + y*y; +} + +// Scalar product of two vectors (inline) +inline decimal Vector2::dot(const Vector2& vector) const { + return (x*vector.x + y*vector.y); +} + +// Normalize the vector +inline void Vector2::normalize() { + decimal l = length(); + assert(l > std::numeric_limits::epsilon()); + x /= l; + y /= l; +} + +// Return the corresponding absolute value vector +inline Vector2 Vector2::getAbsoluteVector() const { + return Vector2(std::abs(x), std::abs(y)); +} + +// Return the axis with the minimal value +inline int Vector2::getMinAxis() const { + return (x < y ? 0 : 1); +} + +// Return the axis with the maximal value +inline int Vector2::getMaxAxis() const { + return (x < y ? 1 : 0); +} + +// Return true if the vector is unit and false otherwise +inline bool Vector2::isUnit() const { + return approxEqual(lengthSquare(), 1.0); +} + +// Return true if the vector is the zero vector +inline bool Vector2::isZero() const { + return approxEqual(lengthSquare(), 0.0); +} + +// Overloaded operator for the equality condition +inline bool Vector2::operator== (const Vector2& vector) const { + return (x == vector.x && y == vector.y); +} + +// Overloaded operator for the is different condition +inline bool Vector2::operator!= (const Vector2& vector) const { + return !(*this == vector); +} + +// Overloaded operator for addition with assignment +inline Vector2& Vector2::operator+=(const Vector2& vector) { + x += vector.x; + y += vector.y; + return *this; +} + +// Overloaded operator for substraction with assignment +inline Vector2& Vector2::operator-=(const Vector2& vector) { + x -= vector.x; + y -= vector.y; + return *this; +} + +// Overloaded operator for multiplication with a number with assignment +inline Vector2& Vector2::operator*=(decimal number) { + x *= number; + y *= number; + return *this; +} + +// Overloaded operator for division by a number with assignment +inline Vector2& Vector2::operator/=(decimal number) { + assert(number > std::numeric_limits::epsilon()); + x /= number; + y /= number; + return *this; +} + +// Overloaded operator for value access +inline decimal& Vector2::operator[] (int index) { + return (&x)[index]; +} + +// Overloaded operator for value access +inline const decimal& Vector2::operator[] (int index) const { + return (&x)[index]; +} + +// Overloaded operator for addition +inline Vector2 operator+(const Vector2& vector1, const Vector2& vector2) { + return Vector2(vector1.x + vector2.x, vector1.y + vector2.y); +} + +// Overloaded operator for substraction +inline Vector2 operator-(const Vector2& vector1, const Vector2& vector2) { + return Vector2(vector1.x - vector2.x, vector1.y - vector2.y); +} + +// Overloaded operator for the negative of a vector +inline Vector2 operator-(const Vector2& vector) { + return Vector2(-vector.x, -vector.y); +} + +// Overloaded operator for multiplication with a number +inline Vector2 operator*(const Vector2& vector, decimal number) { + return Vector2(number * vector.x, number * vector.y); +} + +// Overloaded operator for division by a number +inline Vector2 operator/(const Vector2& vector, decimal number) { + assert(number > MACHINE_EPSILON); + return Vector2(vector.x / number, vector.y / number); +} + +// Overloaded operator for multiplication with a number +inline Vector2 operator*(decimal number, const Vector2& vector) { + return vector * number; +} + +// Assignment operator +inline Vector2& Vector2::operator=(const Vector2& vector) { + if (&vector != this) { + x = vector.x; + y = vector.y; + } + return *this; +} + +} + +#endif diff --git a/src/mathematics/Vector3.h b/src/mathematics/Vector3.h index 3a54b06a..24385fb1 100644 --- a/src/mathematics/Vector3.h +++ b/src/mathematics/Vector3.h @@ -111,9 +111,6 @@ struct Vector3 { /// Return the axis with the maximal value int getMaxAxis() const; - /// Return true if two vectors are parallel - bool isParallelWith(const Vector3& vector) const; - /// Overloaded operator for the equality condition bool operator== (const Vector3& vector) const; diff --git a/test/tests/mathematics/TestMatrix2x2.h b/test/tests/mathematics/TestMatrix2x2.h new file mode 100644 index 00000000..082950a8 --- /dev/null +++ b/test/tests/mathematics/TestMatrix2x2.h @@ -0,0 +1,239 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2013 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 TEST_MATRIX2X2_H +#define TEST_MATRIX2X2_H + +// Libraries +#include "../../Test.h" +#include "../../../src/mathematics/Matrix2x2.h" + +/// Reactphysics3D namespace +namespace reactphysics3d { + +// Class TestMatrix2x2 +/** + * Unit test for the Matrix2x2 class + */ +class TestMatrix2x2 : public Test { + + private : + + // ---------- Atributes ---------- // + + /// Identity transform + Matrix2x2 mIdentity; + + /// First example matrix + Matrix2x2 mMatrix1; + + public : + + // ---------- Methods ---------- // + + /// Constructor + TestMatrix2x2() : mIdentity(Matrix2x2::identity()), + mMatrix1(2, 24, -4, 5) { + + + } + + /// Run the tests + void run() { + testConstructors(); + testGetSet(); + testIdentity(); + testOthersMethods(); + testOperators(); + } + + /// Test the constructors + void testConstructors() { + + Matrix2x2 test1(5.0); + Matrix2x2 test2(2, 3, 4, 5); + Matrix2x2 test3(mMatrix1); + + test(test1[0][0] == 5); + test(test1[0][1] == 5); + test(test1[1][0] == 5); + test(test1[1][1] == 5); + + test(test2[0][0] == 2); + test(test2[0][1] == 3); + test(test2[1][0] == 4); + test(test2[1][1] == 5); + + test(test3 == mMatrix1); + } + + /// Test the getter and setter methods + void testGetSet() { + + // Test method to set all the values + Matrix2x2 test2; + test2.setAllValues(2, 24, -4, 5); + test(test2 == mMatrix1); + + // Test method to set to zero + test2.setToZero(); + test(test2 == Matrix2x2(0, 0, 0, 0)); + + // Test method that returns a column + Vector2 column1 = mMatrix1.getColumn(0); + Vector2 column2 = mMatrix1.getColumn(1); + test(column1 == Vector2(2, -4)); + test(column2 == Vector2(24, 5)); + + // Test method that returns a row + Vector3 row1 = mMatrix1.getRow(0); + Vector3 row2 = mMatrix1.getRow(1); + test(row1 == Vector3(2, 24)); + test(row2 == Vector3(-4, 5)); + } + + /// Test the identity methods + void testIdentity() { + + Matrix2x2 identity = Matrix2x2::identity(); + Matrix2x2 test1; + test1.setToIdentity(); + + test(identity[0][0] == 1); + test(identity[0][1] == 0); + test(identity[1][0] == 0); + test(identity[1][1] == 1); + + test(test1 == Matrix2x2::identity()); + } + + /// Test others methods + void testOthersMethods() { + + // Test transpose + Matrix2x2 transpose = mMatrix1.getTranspose(); + test(transpose == Matrix2x2(2, -4, 24, 5)); + + // Test trace + test(mMatrix1.getTrace() == 10); + test(Matrix2x2::identity().getTrace() == 2); + + // Test determinant + Matrix2x2 matrix(-24, 64, 253, -35); + test(mMatrix1.getDeterminant() == 106); + test(matrix.getDeterminant() == -15352); + test(mIdentity.getDeterminant() == 1); + + // Test inverse + Matrix2x2 inverseMatrix = matrix.getInverse(); + test(approxEqual(inverseMatrix[0][0], decimal(0.056369), decimal(10e-6))); + test(approxEqual(inverseMatrix[0][1], decimal(-0.049549), decimal(10e-6))); + test(approxEqual(inverseMatrix[1][0], decimal(0.029460), decimal(10e-6))); + test(approxEqual(inverseMatrix[1][1], decimal(0.038575), decimal(10e-6))); + Matrix2x2 inverseMatrix1 = mMatrix1.getInverse(); + test(approxEqual(inverseMatrix1[0][0], decimal(0.030232), decimal(10e-6))); + test(approxEqual(inverseMatrix1[0][1], decimal(0.015676), decimal(10e-6))); + test(approxEqual(inverseMatrix1[1][0], decimal(-0.057410), decimal(10e-6))); + test(approxEqual(inverseMatrix1[1][1], decimal(0.039088), decimal(10e-6))); + + // Test absolute matrix + Matrix2x2 matrix2(-2, -3, -4, -5); + test(matrix.getAbsoluteMatrix() == Matrix2x2(24, 64, 253, 35)); + Matrix2x2 absoluteMatrix = matrix2.getAbsoluteMatrix(); + test(absoluteMatrix == Matrix2x2(2, 3, 4, 5)); + } + + /// Test the operators + void testOperators() { + + // Test addition + Matrix2x2 matrix1(2, 3, 4, 5); + Matrix2x2 matrix2(-2, 3, -5, 10); + Matrix2x2 addition1 = matrix1 + matrix2; + Matrix2x2 addition2(matrix1); + addition2 += matrix2; + test(addition1 == Matrix2x2(0, 6, -1, 15)); + test(addition2 == Matrix2x2(0, 6, -1, 15)); + + // Test substraction + Matrix2x2 substraction1 = matrix1 - matrix2; + Matrix2x2 substraction2(matrix1); + substraction2 -= matrix2; + test(substraction1 == Matrix2x2(4, 0, 9, -5)); + test(substraction2 == Matrix2x2(4, 0, 9, -5)); + + // Test negative operator + Matrix2x2 negative = -matrix1; + test(negative == Matrix2x2(-2, -3, -4, -5)); + + // Test multiplication with a number + Matrix2x2 multiplication1 = 3 * matrix1; + Matrix2x2 multiplication2 = matrix1 * 3; + Matrix2x2 multiplication3(matrix1); + multiplication3 *= 3; + test(multiplication1 == Matrix2x2(6, 9, 12, 15)); + test(multiplication2 == Matrix2x2(6, 9, 12, 15)); + test(multiplication3 == Matrix2x2(6, 9, 12, 15)); + + // Test multiplication with a matrix + Matrix2x2 multiplication4 = matrix1 * matrix2; + Matrix2x2 multiplication5 = matrix2 * matrix1; + test(multiplication4 == Matrix2x2(-19, 36, -33, 62)); + test(multiplication5 == Matrix2x2(8, 9, 30, 35)); + + // Test multiplication with a vector + Vector2 vector1(3, -32); + Vector2 vector2(-31, -422); + Vector2 test1 = matrix1 * vector1; + Vector2 test2 = matrix2 * vector2; + test(test1 == Vector2(-762, -182)); + test(test2 == Vector2(-10190, -1986)); + + // Test equality operators + test(Matrix2x2(34, 38, 43, 64) == + Matrix2x2(34, 38, 43, 64)); + test(Matrix2x2(34, 64, 43, 7) != + Matrix2x2(34, 38, 43, 64)); + + // Test operator to read a value + test(mMatrix1[0][0] == 2); + test(mMatrix1[0][1] == 24); + test(mMatrix1[1][0] == -4); + test(mMatrix1[1][1] == 5); + + // Test operator to set a value + Matrix2x2 test3; + test3[0][0] = 2; + test3[0][1] = 24; + test3[1][0] = -4; + test3[1][1] = 5; + test(test3 == mMatrix1); + } + + }; + +} + +#endif diff --git a/test/tests/mathematics/TestMatrix3x3.h b/test/tests/mathematics/TestMatrix3x3.h index ca0a2bb2..78ae43aa 100644 --- a/test/tests/mathematics/TestMatrix3x3.h +++ b/test/tests/mathematics/TestMatrix3x3.h @@ -1,4 +1,3 @@ - /******************************************************************************** * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * * Copyright (c) 2010-2013 Daniel Chappuis * @@ -27,14 +26,10 @@ #ifndef TEST_MATRIX3X3_H #define TEST_MATRIX3X3_H -#endif - // Libraries #include "../../Test.h" #include "../../../src/mathematics/Matrix3x3.h" -using namespace reactphysics3d; - /// Reactphysics3D namespace namespace reactphysics3d { @@ -287,3 +282,5 @@ class TestMatrix3x3 : public Test { }; } + +#endif diff --git a/test/tests/mathematics/TestQuaternion.h b/test/tests/mathematics/TestQuaternion.h index 9007f9b4..ac01cb6c 100644 --- a/test/tests/mathematics/TestQuaternion.h +++ b/test/tests/mathematics/TestQuaternion.h @@ -27,14 +27,10 @@ #ifndef TEST_QUATERNION_H #define TEST_QUATERNION_H -#endif - // Libraries #include "../../Test.h" #include "../../../src/mathematics/Quaternion.h" -using namespace reactphysics3d; - /// Reactphysics3D namespace namespace reactphysics3d { @@ -229,3 +225,5 @@ class TestQuaternion : public Test { }; } + +#endif diff --git a/test/tests/mathematics/TestTransform.h b/test/tests/mathematics/TestTransform.h index 72804053..2d24aaa8 100644 --- a/test/tests/mathematics/TestTransform.h +++ b/test/tests/mathematics/TestTransform.h @@ -27,14 +27,10 @@ #ifndef TEST_TRANSFORM_H #define TEST_TRANSFORM_H -#endif - // Libraries #include "../../Test.h" #include "../../../src/mathematics/Transform.h" -using namespace reactphysics3d; - /// Reactphysics3D namespace namespace reactphysics3d { @@ -216,3 +212,5 @@ class TestTransform : public Test { }; } + +#endif diff --git a/test/tests/mathematics/TestVector2.h b/test/tests/mathematics/TestVector2.h new file mode 100644 index 00000000..0cb1f812 --- /dev/null +++ b/test/tests/mathematics/TestVector2.h @@ -0,0 +1,208 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2013 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 TEST_VECTOR2_H +#define TEST_VECTOR2_H + +// Libraries +#include "../../Test.h" +#include "../../../src/mathematics/Vector2.h" + +/// Reactphysics3D namespace +namespace reactphysics3d { + +// Class TestVector2 +/** + * Unit test for the Vector2 class + */ +class TestVector2 : public Test { + + private : + + // ---------- Atributes ---------- // + + /// Zero vector + Vector2 mVectorZero; + + // Vector (3, 4) + Vector2 mVector34; + + public : + + // ---------- Methods ---------- // + + /// Constructor + TestVector1() : mVectorZero(0, 0), mVector34(3, 4) {} + + /// Run the tests + void run() { + testConstructors(); + testLengthMethods(); + testDotProduct(); + testOthersMethods(); + testOperators(); + } + + /// Test the constructors, getter and setter + void testConstructors() { + + // Test constructor + test(mVectorZero.x == 0.0); + test(mVectorZero.y == 0.0); + test(mVector34.x == 3.0); + test(mVector34.y == 4.0); + + // Test copy-constructor + Vector2 newVector(mVector34); + test(newVector.x == 3.0); + test(newVector.y == 4.0); + + // Test method to set values + Vector2 newVector2; + newVector2.setAllValues(decimal(6.1), decimal(7.2)); + test(approxEqual(newVector2.x, decimal(6.1))); + test(approxEqual(newVector2.y, decimal(7.2))); + + // Test method to set to zero + newVector2.setToZero(); + test(newVector2 == Vector2(0, 0)); + } + + /// Test the length, unit vector and normalize methods + void testLengthMethods() { + + // Test length methods + test(mVectorZero.length() == 0.0); + test(mVectorZero.lengthSquare() == 0.0); + test(Vector2(1, 0).length() == 1.0); + test(Vector2(0, 1).length() == 1.0); + test(mVector345.lengthSquare() == 50.0); + + // Test unit vector methods + test(Vector2(1, 0).isUnit()); + test(Vector2(0, 1).isUnit()); + test(!mVector34.isUnit()); + test(Vector2(5, 0).getUnit() == Vector3(1, 0)); + test(Vector2(0, 5).getUnit() == Vector3(0, 1)); + + test(!mVector34.isZero()); + test(mVectorZero.isZero()); + + // Test normalization method + Vector2 mVector10(1, 0); + Vector2 mVector01(0, 1); + Vector2 mVector50(5, 0); + Vector2 mVector05(0, 5); + mVector10.normalize(); + mVector01.normalize(); + mVector50.normalize(); + mVector05.normalize(); + test(mVector10 == Vector2(1, 0)); + test(mVector01 == Vector2(0, 1)); + test(mVector50 == Vector2(1, 0)); + test(mVector05 == Vector2(0, 1)); + } + + /// Test the dot product + void testDotProduct() { + + // Test the dot product + test(Vector2(5, 0).dot(Vector2(0, 8)) == 0); + test(Vector2(5, 8).dot(Vector2(0, 0)) == 0); + test(Vector2(12, 45).dot(Vector2(0, 0)) == 0); + test(Vector2(5, 7).dot(Vector2(5, 7)) == 74); + test(Vector2(3, 6).dot(Vector2(-3, -6)) == -45); + test(Vector2(2, 3).dot(Vector2(-7, 4)) == -2); + test(Vector2(4, 3).dot(Vector2(8, 9)) == 59); + } + + /// Test others methods + void testOthersMethods() { + + // Test the method that returns the absolute vector + test(Vector2(4, 5).getAbsoluteVector() == Vector2(4, 5)); + test(Vector2(-7, -24).getAbsoluteVector() == Vector2(7, 24)); + + // Test the method that returns the minimal element + test(Vector2(6, 35).getMinAxis() == 0); + test(Vector2(564, 45).getMinAxis() == 1); + test(Vector2(98, 23).getMinAxis() == 1); + test(Vector2(-53, -25).getMinAxis() == 0); + + // Test the method that returns the maximal element + test(Vector2(6, 35).getMaxAxis() == 1); + test(Vector2(7, 537).getMaxAxis() == 1); + test(Vector2(98, 23).getMaxAxis() == 0); + test(Vector2(-53, -25).getMaxAxis() == 1); + } + + /// Test the operators + void testOperators() { + + // Test the [] operator + test(mVector34[0] == 3); + test(mVector34[1] == 4); + + // Assignment operator + Vector2 newVector(6, 4); + newVector = Vector2(7, 8); + test(newVector == Vector2(7, 8)); + + // Equality, inequality operators + test(Vector2(5, 7) == Vector2(5, 7)); + test(Vector2(63, 64) != Vector2(63, 84)); + test(Vector2(63, 64) != Vector2(12, 64)); + + // Addition, substraction + Vector2 vector1(6, 33); + Vector2 vector2(7, 68); + test(Vector2(63, 24) + Vector2(3, 4) == Vector2(66, 28)); + test(Vector2(63, 24) - Vector2(3, 4) == Vector2(60, 20)); + vector1 += Vector2(5, 10); + vector2 -= Vector2(10, 21); + test(vector1 == Vector2(11, 43)); + test(vector2 == Vector2(-3, 47)); + + // Multiplication, division + Vector2 vector3(6, 33); + Vector2 vector4(15, 60); + test(Vector2(63, 24) * 3 == Vector2(189, 72)); + test(3 * Vector2(63, 24) == Vector2(189, 72)); + test(Vector2(14, 8) / 2 == Vector2(7, 4)); + vector3 *= 10; + vector4 /= 3; + test(vector3 == Vector2(60, 330)); + test(vector4 == Vector2(5, 20)); + + // Negative operator + Vector2 vector5(-34, 5); + Vector2 negative = -vector5; + test(negative == Vector2(34, -5)); + } + }; + +} + +#endif diff --git a/test/tests/mathematics/TestVector3.h b/test/tests/mathematics/TestVector3.h index d8f42599..274624fb 100644 --- a/test/tests/mathematics/TestVector3.h +++ b/test/tests/mathematics/TestVector3.h @@ -26,14 +26,10 @@ #ifndef TEST_VECTOR3_H #define TEST_VECTOR3_H -#endif - // Libraries #include "../../Test.h" #include "../../../src/mathematics/Vector3.h" -using namespace reactphysics3d; - /// Reactphysics3D namespace namespace reactphysics3d { @@ -232,3 +228,5 @@ class TestVector3 : public Test { }; } + +#endif From f23096af503f09b7e57aecf1b200481155bda786 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 9 May 2013 19:47:09 +0200 Subject: [PATCH 13/24] modify code in the Matrix2x2 class --- src/mathematics/Matrix2x2.cpp | 4 +--- test/tests/mathematics/TestMatrix2x2.h | 24 ++++++++++++------------ 2 files changed, 13 insertions(+), 15 deletions(-) diff --git a/src/mathematics/Matrix2x2.cpp b/src/mathematics/Matrix2x2.cpp index 87353a3d..96aa5806 100644 --- a/src/mathematics/Matrix2x2.cpp +++ b/src/mathematics/Matrix2x2.cpp @@ -80,9 +80,7 @@ Matrix2x2 Matrix2x2::getInverse() const { decimal invDeterminant = decimal(1.0) / determinant; - // TODO : Implement this - assert(false); - Matrix2x2 tempMatrix; + Matrix2x2 tempMatrix(mRows[1][1], -mRows[0][1], -mRows[1][0], mRows[0][0]); // Return the inverse matrix return (invDeterminant * tempMatrix); diff --git a/test/tests/mathematics/TestMatrix2x2.h b/test/tests/mathematics/TestMatrix2x2.h index 082950a8..b4251ec6 100644 --- a/test/tests/mathematics/TestMatrix2x2.h +++ b/test/tests/mathematics/TestMatrix2x2.h @@ -57,7 +57,6 @@ class TestMatrix2x2 : public Test { TestMatrix2x2() : mIdentity(Matrix2x2::identity()), mMatrix1(2, 24, -4, 5) { - } /// Run the tests @@ -147,21 +146,22 @@ class TestMatrix2x2 : public Test { test(mIdentity.getDeterminant() == 1); // Test inverse - Matrix2x2 inverseMatrix = matrix.getInverse(); - test(approxEqual(inverseMatrix[0][0], decimal(0.056369), decimal(10e-6))); - test(approxEqual(inverseMatrix[0][1], decimal(-0.049549), decimal(10e-6))); - test(approxEqual(inverseMatrix[1][0], decimal(0.029460), decimal(10e-6))); - test(approxEqual(inverseMatrix[1][1], decimal(0.038575), decimal(10e-6))); + Matrix2x2 matrix2(1, 2, 3, 4); + Matrix2x2 inverseMatrix = matrix2.getInverse(); + test(approxEqual(inverseMatrix[0][0], decimal(-2), decimal(10e-6))); + test(approxEqual(inverseMatrix[0][1], decimal(1), decimal(10e-6))); + test(approxEqual(inverseMatrix[1][0], decimal(-0.75), decimal(10e-6))); + test(approxEqual(inverseMatrix[1][1], decimal(0.25), decimal(10e-6))); Matrix2x2 inverseMatrix1 = mMatrix1.getInverse(); - test(approxEqual(inverseMatrix1[0][0], decimal(0.030232), decimal(10e-6))); - test(approxEqual(inverseMatrix1[0][1], decimal(0.015676), decimal(10e-6))); - test(approxEqual(inverseMatrix1[1][0], decimal(-0.057410), decimal(10e-6))); - test(approxEqual(inverseMatrix1[1][1], decimal(0.039088), decimal(10e-6))); + test(approxEqual(inverseMatrix1[0][0], decimal(0.04716981), decimal(10e-6))); + test(approxEqual(inverseMatrix1[0][1], decimal(-0.2264150), decimal(10e-6))); + test(approxEqual(inverseMatrix1[1][0], decimal(0.0377358), decimal(10e-6))); + test(approxEqual(inverseMatrix1[1][1], decimal(0.0188679), decimal(10e-6))); // Test absolute matrix - Matrix2x2 matrix2(-2, -3, -4, -5); + Matrix2x2 matrix3(-2, -3, -4, -5); test(matrix.getAbsoluteMatrix() == Matrix2x2(24, 64, 253, 35)); - Matrix2x2 absoluteMatrix = matrix2.getAbsoluteMatrix(); + Matrix2x2 absoluteMatrix = matrix3.getAbsoluteMatrix(); test(absoluteMatrix == Matrix2x2(2, 3, 4, 5)); } From 0f17bd1b0b417c66de03da4557107a14ab0c171c Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 12 May 2013 12:43:07 +0200 Subject: [PATCH 14/24] continue working on the slider joint --- src/constraint/BallAndSocketJoint.cpp | 40 +++++++- src/constraint/BallAndSocketJoint.h | 3 + src/constraint/Constraint.h | 3 + src/constraint/ContactPoint.cpp | 5 + src/constraint/ContactPoint.h | 3 + src/constraint/SliderJoint.cpp | 127 ++++++++++++++++++++++++-- src/constraint/SliderJoint.h | 15 +++ src/mathematics/mathematics.h | 2 + 8 files changed, 188 insertions(+), 10 deletions(-) diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index b4fc6575..9b52ef22 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -78,6 +78,38 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS mInverseMassMatrix = massMatrix.getInverse(); } +// Warm start the constraint (apply the previous impulse at the beginning of the step) +void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverData) { + + // Get the velocities + Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; + Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; + Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; + Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; + + // Get the inverse mass and inverse inertia tensors of the bodies + decimal inverseMassBody1 = mBody1->getMassInverse(); + decimal inverseMassBody2 = mBody2->getMassInverse(); + Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); + Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); + + // Compute the impulse P=J^T * lambda + Vector3 linearImpulseBody1 = -mImpulse; + Vector3 angularImpulseBody1 = mImpulse.cross(mU1World); + Vector3 linearImpulseBody2 = mImpulse; + Vector3 angularImpulseBody2 = -mImpulse.cross(mU2World); + + // Apply the impulse to the bodies of the joint + if (mBody1->getIsMotionEnabled()) { + v1 += inverseMassBody1 * linearImpulseBody1; + w1 += I1 * angularImpulseBody1; + } + if (mBody2->getIsMotionEnabled()) { + v2 += inverseMassBody2 * linearImpulseBody2; + w2 += I2 * angularImpulseBody2; + } +} + // Solve the velocity constraint void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { @@ -94,8 +126,8 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con // Get the inverse mass and inverse inertia tensors of the bodies decimal inverseMassBody1 = mBody1->getMassInverse(); decimal inverseMassBody2 = mBody2->getMassInverse(); - Matrix3x3 inverseInertiaTensorBody1 = mBody1->getInertiaTensorInverseWorld(); - Matrix3x3 inverseInertiaTensorBody2 = mBody2->getInertiaTensorInverseWorld(); + Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); + Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); // Compute J*v Vector3 Jv = -v1 + mU1World.cross(w1) + v2 - mU2World.cross(w2); @@ -121,11 +153,11 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { v1 += inverseMassBody1 * linearImpulseBody1; - w1 += inverseInertiaTensorBody1 * angularImpulseBody1; + w1 += I1 * angularImpulseBody1; } if (mBody2->getIsMotionEnabled()) { v2 += inverseMassBody2 * linearImpulseBody2; - w2 += inverseInertiaTensorBody2 * angularImpulseBody2; + w2 += I2 * angularImpulseBody2; } } diff --git a/src/constraint/BallAndSocketJoint.h b/src/constraint/BallAndSocketJoint.h index 0280c28c..0c48cdad 100644 --- a/src/constraint/BallAndSocketJoint.h +++ b/src/constraint/BallAndSocketJoint.h @@ -104,6 +104,9 @@ class BallAndSocketJoint : public Constraint { /// Initialize before solving the constraint virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); + /// Warm start the constraint (apply the previous impulse at the beginning of the step) + virtual void warmstart(const ConstraintSolverData& constraintSolverData); + /// Solve the velocity constraint virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); diff --git a/src/constraint/Constraint.h b/src/constraint/Constraint.h index 58181768..1be00170 100644 --- a/src/constraint/Constraint.h +++ b/src/constraint/Constraint.h @@ -149,6 +149,9 @@ class Constraint { /// Initialize before solving the constraint virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData) = 0; + /// Warm start the constraint (apply the previous impulse at the beginning of the step) + virtual void warmstart(const ConstraintSolverData& constraintSolverData) = 0; + /// Solve the velocity constraint virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) = 0; diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp index 178a5e1d..1f068df3 100644 --- a/src/constraint/ContactPoint.cpp +++ b/src/constraint/ContactPoint.cpp @@ -56,6 +56,11 @@ void ContactPoint::initBeforeSolve(const ConstraintSolverData& constraintSolverD } +// Warm start the constraint (apply the previous impulse at the beginning of the step) +void ContactPoint::warmstart(const ConstraintSolverData& constraintSolverData) { + +} + // Solve the velocity constraint void ContactPoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h index 0047c020..dc66b8b0 100644 --- a/src/constraint/ContactPoint.h +++ b/src/constraint/ContactPoint.h @@ -227,6 +227,9 @@ class ContactPoint : public Constraint { /// Initialize before solving the constraint virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); + /// Warm start the constraint (apply the previous impulse at the beginning of the step) + virtual void warmstart(const ConstraintSolverData& constraintSolverData); + /// Solve the velocity constraint virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index 0d763e9f..73222eef 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -29,7 +29,8 @@ using namespace reactphysics3d; // Constructor -SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) : Constraint(jointInfo) { +SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) + : Constraint(jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0) { // Compute the local-space anchor point for each body mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; @@ -64,17 +65,76 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa mN1 = mU1World.getOneUnitOrthogonalVector(); mN2 = mU1World.cross(mN1); - // Compute the cross product used in the Jacobian + // Compute the cross products used in the Jacobian mU1WorldCrossN1 = mN2; mU1WorldCrossN2 = mU1World.cross(mN2); mU2WorldCrossN1 = mU2World.cross(mN1); mU2WorldCrossN2 = mU2World.cross(mN2); - // Compute the mass matrix K=JM^-1J^t for the 2 translation constraints (2x2 matrix) + // Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation + // constraints (2x2 matrix) const decimal n1Dotn1 = mN1.lengthSquare(); const decimal n2Dotn2 = mN2.lengthSquare(); + const decimal n1Dotn2 = mN1.dot(mN2); const decimal sumInverseMass = mBody1->getMassInverse() + mBody2->getMassInverse(); + const Vector3 I1U2CrossN1 = I1 * mU2WorldCrossN1; + const Vector3 I1U2CrossN2 = I1 * mU2WorldCrossN2; + const Vector3 I2U1CrossN1 = I2 * mU1WorldCrossN1; + const Vector3 I2U1CrossN2 = I2 * mU1WorldCrossN2; + const decimal el11 = sumInverseMass * (n1Dotn1) + mU2WorldCrossN1.dot(I1U2CrossN1) + + mU1WorldCrossN1.dot(I2U1CrossN1); + const decimal el12 = sumInverseMass * (n1Dotn2) + mU2WorldCrossN1.dot(I1U2CrossN2) + + mU1WorldCrossN1.dot(I2U1CrossN2); + const decimal el21 = sumInverseMass * (n1Dotn2) + mU2WorldCrossN2.dot(I1U2CrossN1) + + mU1WorldCrossN2.dot(I2U1CrossN1); + const decimal el22 = sumInverseMass * (n2Dotn2) + mU2WorldCrossN2.dot(I1U2CrossN2) + + mU1WorldCrossN2.dot(I2U1CrossN2); + Matrix2x2 matrixKTranslation(el11, el12, el21, el22); + mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); + // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation + // contraints (3x3 matrix) + mInverseMassMatrixRotationConstraint = I1 + I2; +} + +// Warm start the constraint (apply the previous impulse at the beginning of the step) +void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { + + // Get the velocities + Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; + Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; + Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; + Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; + + // Get the inverse mass and inverse inertia tensors of the bodies + decimal inverseMassBody1 = mBody1->getMassInverse(); + decimal inverseMassBody2 = mBody2->getMassInverse(); + const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); + const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); + + // Compute the impulse P=J^T * lambda for the 2 translation constraints + Vector3 linearImpulseBody1 = -mN1 * mImpulseTranslation.x - + mN2 * mImpulseTranslation.y; + Vector3 angularImpulseBody1 = mU2WorldCrossN1 * mImpulseTranslation.x + + mU2WorldCrossN2 * mImpulseTranslation.y; + Vector3 linearImpulseBody2 = mN1 * mImpulseTranslation.x + + mN2 * mImpulseTranslation.y; + Vector3 angularImpulseBody2 = -mU1WorldCrossN1 * mImpulseTranslation.x - + mU1WorldCrossN2 * mImpulseTranslation.y; + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints + angularImpulseBody1 += -mImpulseRotation; + angularImpulseBody2 += mImpulseRotation; + + // Apply the impulse to the bodies of the joint + if (mBody1->getIsMotionEnabled()) { + v1 += inverseMassBody1 * linearImpulseBody1; + w1 += I1 * angularImpulseBody1; + } + if (mBody2->getIsMotionEnabled()) { + v2 += inverseMassBody2 * linearImpulseBody2; + w2 += I2 * angularImpulseBody2; + } } // Solve the velocity constraint @@ -93,10 +153,65 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint // Get the inverse mass and inverse inertia tensors of the bodies decimal inverseMassBody1 = mBody1->getMassInverse(); decimal inverseMassBody2 = mBody2->getMassInverse(); - Matrix3x3 inverseInertiaTensorBody1 = mBody1->getInertiaTensorInverseWorld(); - Matrix3x3 inverseInertiaTensorBody2 = mBody2->getInertiaTensorInverseWorld(); + const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); + const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); - // Compute J*v + // Compute J*v for the 2 translation constraints + const decimal el1 = -mN1.dot(v1) + mU2WorldCrossN1.dot(w1) + + mN1.dot(v2) - mU1WorldCrossN1.dot(w2); + const decimal el2 = -mN2.dot(v1) + mU2WorldCrossN2.dot(w1) + + mN2.dot(v2) - mU1WorldCrossN2.dot(w2); + const Vector2 JvTranslation(el1, el2); + + // Compute J*v for the 3 translation constraints + const Vector3 JvRotation = w2 - w1; + + // Compute the bias "b" of the translation constraint + Vector2 bTranslation(0, 0); + decimal beta = decimal(0.2); // TODO : Use a constant here + decimal biasFactor = (beta / constraintSolverData.timeStep); + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + Vector3 deltaV = x2 + mU2World - x1 - mU1World; + bTranslation.x = biasFactor * deltaV.dot(mN1); + bTranslation.y = biasFactor * deltaV.dot(mN2); + } + + // Compute the bias "b" of the translation constraint + Vector3 bRotation(0, 0, 0); + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + Quaternion q1 = mBody1->getTransform().getOrientation(); + Quaternion q2 = mBody2->getTransform().getOrientation(); + Quaternion qDiff = q1 * q2.getInverse(); + bRotation = 2.0 * qDiff.getVectorV(); + } + + // Compute the Lagrange multiplier lambda for the 2 translation constraints + Vector2 deltaLambda = mInverseMassMatrixTranslationConstraint * (-JvTranslation - bTranslation); + mImpulseTranslation += deltaLambda; + + // Compute the Lagrange multiplier lambda for the 3 rotation constraints + Vector3 deltaLambda2 = mInverseMassMatrixRotationConstraint * (-JvRotation - bRotation); + mImpulseRotation += deltaLambda2; + + // Compute the impulse P=J^T * lambda for the 2 translation constraints + Vector3 linearImpulseBody1 = -mN1 * deltaLambda.x - mN2 * deltaLambda.y; + Vector3 angularImpulseBody1 = mU2WorldCrossN1 * deltaLambda.x + mU2WorldCrossN2 * deltaLambda.y; + Vector3 linearImpulseBody2 = mN1 * deltaLambda.x + mN2 * deltaLambda.y; + Vector3 angularImpulseBody2 = -mU1WorldCrossN1 * deltaLambda.x -mU1WorldCrossN2 * deltaLambda.y; + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints + angularImpulseBody1 += -deltaLambda2; + angularImpulseBody2 += deltaLambda2; + + // Apply the impulse to the bodies of the joint + if (mBody1->getIsMotionEnabled()) { + v1 += inverseMassBody1 * linearImpulseBody1; + w1 += I1 * angularImpulseBody1; + } + if (mBody2->getIsMotionEnabled()) { + v2 += inverseMassBody2 * linearImpulseBody2; + w2 += I2 * angularImpulseBody2; + } } // Solve the position constraint diff --git a/src/constraint/SliderJoint.h b/src/constraint/SliderJoint.h index 2d9ae4ec..d4aab31a 100644 --- a/src/constraint/SliderJoint.h +++ b/src/constraint/SliderJoint.h @@ -98,6 +98,18 @@ class SliderJoint : public Constraint { /// Cross product of mU2World and mN2 Vector3 mU2WorldCrossN2; + /// Inverse of mass matrix K=JM^-1J^t for the translation constraint (2x2 matrix) + Matrix2x2 mInverseMassMatrixTranslationConstraint; + + /// Inverse of mass matrix K=JM^-1J^t for the rotation constraint (3x3 matrix) + Matrix3x3 mInverseMassMatrixRotationConstraint; + + /// Impulse for the 2 translation constraints + Vector2 mImpulseTranslation; + + /// Impulse for the 3 rotation constraints + Vector3 mImpulseRotation; + public : // -------------------- Methods -------------------- // @@ -114,6 +126,9 @@ class SliderJoint : public Constraint { /// Initialize before solving the constraint virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); + /// Warm start the constraint (apply the previous impulse at the beginning of the step) + virtual void warmstart(const ConstraintSolverData& constraintSolverData); + /// Solve the velocity constraint virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); diff --git a/src/mathematics/mathematics.h b/src/mathematics/mathematics.h index cc5aa467..3697634f 100644 --- a/src/mathematics/mathematics.h +++ b/src/mathematics/mathematics.h @@ -28,8 +28,10 @@ // Libraries #include "Matrix3x3.h" +#include "Matrix2x2.h" #include "Quaternion.h" #include "Vector3.h" +#include "Vector2.h" #include "Transform.h" #include "../configuration.h" #include "mathematics_functions.h" From 74070ae400be34de0b273d4ce08797aaee381a22 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 12 May 2013 14:08:30 +0200 Subject: [PATCH 15/24] Modify CMakeLists.txt file --- CMakeLists.txt | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 47bc8aaa..805fb5d3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,7 +14,8 @@ set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/") OPTION(COMPILE_EXAMPLES "Select this if you want to build the examples" OFF) OPTION(COMPILE_TESTS "Select this if you want to build the tests" OFF) OPTION(PROFILING_ENABLED "Select this if you want to compile with enabled profiling" OFF) - +OPTION(DOUBLE_PRECISION_ENABLED "Select this if you want to compile using double precision floating + values" OFF) # Headers INCLUDE_DIRECTORIES(src) @@ -22,6 +23,10 @@ IF (PROFILING_ENABLED) ADD_DEFINITIONS(-DIS_PROFILING_ACTIVE) ENDIF (PROFILING_ENABLED) +IF (DOUBLE_PRECISION_ENABLED) + ADD_DEFINITIONS(-DIS_DOUBLE_PRECISION_ENABLED) +ENDIF (DOUBLE_PRECISION_ENABLED) + # Library configuration file ( GLOB_RECURSE From 8f37d4ac98e94302c57c7db21499a99a3b40f2f5 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 16 May 2013 21:42:13 +0200 Subject: [PATCH 16/24] Fix issues in the unit tests for Vector2 and Matrix2x2 --- src/mathematics/Matrix2x2.h | 2 +- test/main.cpp | 4 ++++ test/tests/mathematics/TestMatrix2x2.h | 26 +++++++++++++------------- test/tests/mathematics/TestVector2.h | 8 ++++---- 4 files changed, 22 insertions(+), 18 deletions(-) diff --git a/src/mathematics/Matrix2x2.h b/src/mathematics/Matrix2x2.h index ec9c28f1..53a313af 100644 --- a/src/mathematics/Matrix2x2.h +++ b/src/mathematics/Matrix2x2.h @@ -60,7 +60,7 @@ class Matrix2x2 { Matrix2x2(decimal a1, decimal a2, decimal b1, decimal b2); /// Destructor - virtual ~Matrix2x2(); + ~Matrix2x2(); /// Copy-constructor Matrix2x2(const Matrix2x2& matrix); diff --git a/test/main.cpp b/test/main.cpp index 11139b6f..76c2048c 100644 --- a/test/main.cpp +++ b/test/main.cpp @@ -25,9 +25,11 @@ // Libraries #include "TestSuite.h" +#include "tests/mathematics/TestVector2.h" #include "tests/mathematics/TestVector3.h" #include "tests/mathematics/TestTransform.h" #include "tests/mathematics/TestQuaternion.h" +#include "tests/mathematics/TestMatrix2x2.h" #include "tests/mathematics/TestMatrix3x3.h" using namespace reactphysics3d; @@ -38,10 +40,12 @@ int main() { // ---------- Mathematics tests ---------- // + testSuite.addTest(new TestVector2); testSuite.addTest(new TestVector3); testSuite.addTest(new TestTransform); testSuite.addTest(new TestQuaternion); testSuite.addTest(new TestMatrix3x3); + testSuite.addTest(new TestMatrix2x2); // ----------------------------- --------- // diff --git a/test/tests/mathematics/TestMatrix2x2.h b/test/tests/mathematics/TestMatrix2x2.h index b4251ec6..f9144cc9 100644 --- a/test/tests/mathematics/TestMatrix2x2.h +++ b/test/tests/mathematics/TestMatrix2x2.h @@ -107,10 +107,10 @@ class TestMatrix2x2 : public Test { test(column2 == Vector2(24, 5)); // Test method that returns a row - Vector3 row1 = mMatrix1.getRow(0); - Vector3 row2 = mMatrix1.getRow(1); - test(row1 == Vector3(2, 24)); - test(row2 == Vector3(-4, 5)); + Vector2 row1 = mMatrix1.getRow(0); + Vector2 row2 = mMatrix1.getRow(1); + test(row1 == Vector2(2, 24)); + test(row2 == Vector2(-4, 5)); } /// Test the identity methods @@ -136,7 +136,7 @@ class TestMatrix2x2 : public Test { test(transpose == Matrix2x2(2, -4, 24, 5)); // Test trace - test(mMatrix1.getTrace() == 10); + test(mMatrix1.getTrace() ==7); test(Matrix2x2::identity().getTrace() == 2); // Test determinant @@ -150,13 +150,13 @@ class TestMatrix2x2 : public Test { Matrix2x2 inverseMatrix = matrix2.getInverse(); test(approxEqual(inverseMatrix[0][0], decimal(-2), decimal(10e-6))); test(approxEqual(inverseMatrix[0][1], decimal(1), decimal(10e-6))); - test(approxEqual(inverseMatrix[1][0], decimal(-0.75), decimal(10e-6))); - test(approxEqual(inverseMatrix[1][1], decimal(0.25), decimal(10e-6))); + test(approxEqual(inverseMatrix[1][0], decimal(1.5), decimal(10e-6))); + test(approxEqual(inverseMatrix[1][1], decimal(-0.5), decimal(10e-6))); Matrix2x2 inverseMatrix1 = mMatrix1.getInverse(); - test(approxEqual(inverseMatrix1[0][0], decimal(0.04716981), decimal(10e-6))); - test(approxEqual(inverseMatrix1[0][1], decimal(-0.2264150), decimal(10e-6))); - test(approxEqual(inverseMatrix1[1][0], decimal(0.0377358), decimal(10e-6))); - test(approxEqual(inverseMatrix1[1][1], decimal(0.0188679), decimal(10e-6))); + test(approxEqual(inverseMatrix1[0][0], decimal(0.047169811), decimal(10e-6))); + test(approxEqual(inverseMatrix1[0][1], decimal(-0.226415094), decimal(10e-6))); + test(approxEqual(inverseMatrix1[1][0], decimal(0.037735849), decimal(10e-6))); + test(approxEqual(inverseMatrix1[1][1], decimal(0.018867925), decimal(10e-6))); // Test absolute matrix Matrix2x2 matrix3(-2, -3, -4, -5); @@ -208,8 +208,8 @@ class TestMatrix2x2 : public Test { Vector2 vector2(-31, -422); Vector2 test1 = matrix1 * vector1; Vector2 test2 = matrix2 * vector2; - test(test1 == Vector2(-762, -182)); - test(test2 == Vector2(-10190, -1986)); + test(test1 == Vector2(-90, -148)); + test(test2 == Vector2(-1204, -4065)); // Test equality operators test(Matrix2x2(34, 38, 43, 64) == diff --git a/test/tests/mathematics/TestVector2.h b/test/tests/mathematics/TestVector2.h index 0cb1f812..f001fa0a 100644 --- a/test/tests/mathematics/TestVector2.h +++ b/test/tests/mathematics/TestVector2.h @@ -54,7 +54,7 @@ class TestVector2 : public Test { // ---------- Methods ---------- // /// Constructor - TestVector1() : mVectorZero(0, 0), mVector34(3, 4) {} + TestVector2() : mVectorZero(0, 0), mVector34(3, 4) {} /// Run the tests void run() { @@ -98,14 +98,14 @@ class TestVector2 : public Test { test(mVectorZero.lengthSquare() == 0.0); test(Vector2(1, 0).length() == 1.0); test(Vector2(0, 1).length() == 1.0); - test(mVector345.lengthSquare() == 50.0); + test(mVector34.lengthSquare() == 25.0); // Test unit vector methods test(Vector2(1, 0).isUnit()); test(Vector2(0, 1).isUnit()); test(!mVector34.isUnit()); - test(Vector2(5, 0).getUnit() == Vector3(1, 0)); - test(Vector2(0, 5).getUnit() == Vector3(0, 1)); + test(Vector2(5, 0).getUnit() == Vector2(1, 0)); + test(Vector2(0, 5).getUnit() == Vector2(0, 1)); test(!mVector34.isZero()); test(mVectorZero.isZero()); From 78abbaac729038a91cb5e0994a3c23e84b0acd47 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 21 May 2013 23:03:14 +0200 Subject: [PATCH 17/24] Fix issues in the BallAndSocketJoint and SliderJoint --- examples/joints/Scene.cpp | 59 ++++++++- examples/joints/Scene.h | 12 ++ src/constraint/BallAndSocketJoint.cpp | 63 ++++++---- src/constraint/BallAndSocketJoint.h | 18 +-- src/constraint/SliderJoint.cpp | 175 ++++++++++++++++---------- src/constraint/SliderJoint.h | 43 ++++--- src/mathematics/Matrix2x2.h | 2 +- src/reactphysics3d.h | 1 + 8 files changed, 253 insertions(+), 120 deletions(-) diff --git a/examples/joints/Scene.cpp b/examples/joints/Scene.cpp index c1b5060b..419f5c24 100644 --- a/examples/joints/Scene.cpp +++ b/examples/joints/Scene.cpp @@ -59,6 +59,9 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0), // Create the Ball-and-Socket joint createBallAndSocketJoints(); + // Create the Slider joint + createSliderJoint(); + // Create the floor createFloor(); @@ -77,12 +80,17 @@ Scene::~Scene() { // Destroy the joints mDynamicsWorld->destroyJoint(mBallAndSocketJoint); + mDynamicsWorld->destroyJoint(mSliderJoint); // Destroy all the boxes of the scene mDynamicsWorld->destroyRigidBody(mBallAndSocketJointBox1->getRigidBody()); mDynamicsWorld->destroyRigidBody(mBallAndSocketJointBox2->getRigidBody()); + mDynamicsWorld->destroyRigidBody(mSliderJointBox1->getRigidBody()); + mDynamicsWorld->destroyRigidBody(mSliderJointBox2->getRigidBody()); delete mBallAndSocketJointBox1; delete mBallAndSocketJointBox2; + delete mSliderJointBox1; + delete mSliderJointBox2; // Destroy the floor mDynamicsWorld->destroyRigidBody(mFloor->getRigidBody()); @@ -104,10 +112,11 @@ void Scene::simulate() { // Update the position and orientation of the boxes mBallAndSocketJointBox1->updateTransform(); mBallAndSocketJointBox2->updateTransform(); + mSliderJointBox1->updateTransform(); + mSliderJointBox2->updateTransform(); // Update the position and orientation of the floor mFloor->updateTransform(); - } } @@ -139,6 +148,8 @@ void Scene::render() { // Render all the boxes mBallAndSocketJointBox1->render(mPhongShader); mBallAndSocketJointBox2->render(mPhongShader); + mSliderJointBox1->render(mPhongShader); + mSliderJointBox2->render(mPhongShader); // Render the floor mFloor->render(mPhongShader); @@ -191,6 +202,52 @@ void Scene::createBallAndSocketJoints() { mDynamicsWorld->createJoint(jointInfo)); } +/// Create the boxes and joint for the Slider joint example +void Scene::createSliderJoint() { + + // --------------- Create the first box --------------- // + + // Position of the box + openglframework::Vector3 positionBox1(-4, 6, 0); + + // Create a box and a corresponding rigid in the dynamics world + mSliderJointBox1 = new Box(BOX_SIZE, positionBox1 , BOX_MASS, mDynamicsWorld); + + // The fist box cannot move + mSliderJointBox1->getRigidBody()->setIsMotionEnabled(false); + + // Set the bouncing factor of the box + mSliderJointBox1->getRigidBody()->setRestitution(0.4); + + // --------------- Create the second box --------------- // + + // Position of the box + openglframework::Vector3 positionBox2(2, 4, 0); + + // Create a box and a corresponding rigid in the dynamics world + mSliderJointBox2 = new Box(BOX_SIZE, positionBox2 , BOX_MASS, mDynamicsWorld); + + // The second box is allowed to move + mSliderJointBox2->getRigidBody()->setIsMotionEnabled(true); + + // Set the bouncing factor of the box + mSliderJointBox2->getRigidBody()->setRestitution(0.4); + + // --------------- Create the joint --------------- // + + // Create the joint info object + rp3d::RigidBody* body1 = mSliderJointBox1->getRigidBody(); + rp3d::RigidBody* body2 = mSliderJointBox2->getRigidBody(); + const rp3d::Vector3& body1Position = body1->getTransform().getPosition(); + const rp3d::Vector3& body2Position = body2->getTransform().getPosition(); + const rp3d::Vector3 anchorPointWorldSpace = 0.5 * (body2Position + body1Position); + const rp3d::Vector3 sliderAxisWorldSpace = body2Position - body1Position; + rp3d::SliderJointInfo jointInfo(body1, body2, anchorPointWorldSpace, sliderAxisWorldSpace); + + // Create the joint in the dynamics world + mSliderJoint = dynamic_cast(mDynamicsWorld->createJoint(jointInfo)); +} + // Create the floor void Scene::createFloor() { diff --git a/examples/joints/Scene.h b/examples/joints/Scene.h index 7d89dc3a..3bc5cb5d 100644 --- a/examples/joints/Scene.h +++ b/examples/joints/Scene.h @@ -62,6 +62,15 @@ class Scene { /// Ball-and-Socket joint rp3d::BallAndSocketJoint* mBallAndSocketJoint; + /// Box 1 of Slider joint + Box* mSliderJointBox1; + + /// Box 2 of Slider joint + Box* mSliderJointBox2; + + /// Slider joint + rp3d::SliderJoint* mSliderJoint; + /// Box for the floor Box* mFloor; @@ -76,6 +85,9 @@ class Scene { /// Create the boxes and joints for the Ball-and-Socket joint example void createBallAndSocketJoints(); + /// Create the boxes and joint for the Slider joint example + void createSliderJoint(); + /// Create the floor void createFloor(); diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index 9b52ef22..aa3409f2 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -59,23 +59,36 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); // Compute the vector from body center to the anchor point in world-space - mU1World = orientationBody1 * mLocalAnchorPointBody1; - mU2World = orientationBody2 * mLocalAnchorPointBody2; + mR1World = orientationBody1 * mLocalAnchorPointBody1; + mR2World = orientationBody2 * mLocalAnchorPointBody2; // Compute the corresponding skew-symmetric matrices - Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mU1World); - Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mU2World); + Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); + Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); // Compute the matrix K=JM^-1J^t (3x3 matrix) - decimal inverseMassBodies = mBody1->getMassInverse() + mBody2->getMassInverse(); - Matrix3x3 massMatrix= Matrix3x3(inverseMassBodies, 0, 0, + decimal inverseMassBodies = 0.0; + if (mBody1->getIsMotionEnabled()) { + inverseMassBodies += mBody1->getMassInverse(); + } + if (mBody2->getIsMotionEnabled()) { + inverseMassBodies += mBody2->getMassInverse(); + } + Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, - 0, 0, inverseMassBodies) + - skewSymmetricMatrixU1 * I1 * skewSymmetricMatrixU1.getTranspose() + - skewSymmetricMatrixU2 * I2 * skewSymmetricMatrixU2.getTranspose(); + 0, 0, inverseMassBodies); + if (mBody1->getIsMotionEnabled()) { + massMatrix += skewSymmetricMatrixU1 * I1 * skewSymmetricMatrixU1.getTranspose(); + } + if (mBody2->getIsMotionEnabled()) { + massMatrix += skewSymmetricMatrixU2 * I2 * skewSymmetricMatrixU2.getTranspose(); + } // Compute the inverse mass matrix K^-1 - mInverseMassMatrix = massMatrix.getInverse(); + mInverseMassMatrix.setToZero(); + if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + mInverseMassMatrix = massMatrix.getInverse(); + } } // Warm start the constraint (apply the previous impulse at the beginning of the step) @@ -88,16 +101,16 @@ void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverD Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies - decimal inverseMassBody1 = mBody1->getMassInverse(); - decimal inverseMassBody2 = mBody2->getMassInverse(); - Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); - Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); + const decimal inverseMassBody1 = mBody1->getMassInverse(); + const decimal inverseMassBody2 = mBody2->getMassInverse(); + const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); + const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); // Compute the impulse P=J^T * lambda - Vector3 linearImpulseBody1 = -mImpulse; - Vector3 angularImpulseBody1 = mImpulse.cross(mU1World); - Vector3 linearImpulseBody2 = mImpulse; - Vector3 angularImpulseBody2 = -mImpulse.cross(mU2World); + const Vector3 linearImpulseBody1 = -mImpulse; + const Vector3 angularImpulseBody1 = mImpulse.cross(mR1World); + const Vector3 linearImpulseBody2 = mImpulse; + const Vector3 angularImpulseBody2 = -mImpulse.cross(mR2World); // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { @@ -130,25 +143,25 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); // Compute J*v - Vector3 Jv = -v1 + mU1World.cross(w1) + v2 - mU2World.cross(w2); + const Vector3 Jv = -v1 + mR1World.cross(w1) + v2 - mR2World.cross(w2); // Compute the bias "b" of the constraint Vector3 b(0, 0, 0); if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { decimal beta = decimal(0.2); // TODO : Use a constant here decimal biasFactor = (beta / constraintSolverData.timeStep); - b = biasFactor * (x2 + mU2World - x1 - mU1World); + b = biasFactor * (x2 + mR2World - x1 - mR1World); } // Compute the Lagrange multiplier lambda - Vector3 deltaLambda = mInverseMassMatrix * (-Jv - b); + const Vector3 deltaLambda = mInverseMassMatrix * (-Jv - b); mImpulse += deltaLambda; // Compute the impulse P=J^T * lambda - Vector3 linearImpulseBody1 = -deltaLambda; - Vector3 angularImpulseBody1 = deltaLambda.cross(mU1World); - Vector3 linearImpulseBody2 = deltaLambda; - Vector3 angularImpulseBody2 = -deltaLambda.cross(mU2World); + const Vector3 linearImpulseBody1 = -deltaLambda; + const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World); + const Vector3 linearImpulseBody2 = deltaLambda; + const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World); // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { diff --git a/src/constraint/BallAndSocketJoint.h b/src/constraint/BallAndSocketJoint.h index 0c48cdad..556e316b 100644 --- a/src/constraint/BallAndSocketJoint.h +++ b/src/constraint/BallAndSocketJoint.h @@ -50,7 +50,7 @@ struct BallAndSocketJointInfo : public ConstraintInfo { BallAndSocketJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, const Vector3& initAnchorPointWorldSpace) : ConstraintInfo(rigidBody1, rigidBody2, BALLSOCKETJOINT), - anchorPointWorldSpace(initAnchorPointWorldSpace){} + anchorPointWorldSpace(initAnchorPointWorldSpace) {} }; // Class BallAndSocketJoint @@ -64,23 +64,23 @@ class BallAndSocketJoint : public Constraint { // -------------------- Attributes -------------------- // - /// Anchor point of body 1 (in local space coordinates) + /// Anchor point of body 1 (in local-space coordinates of body 1) Vector3 mLocalAnchorPointBody1; - /// Anchor point of body 2 (in local space coordinates) + /// Anchor point of body 2 (in local-space coordinates of body 2) Vector3 mLocalAnchorPointBody2; /// Vector from center of body 2 to anchor point in world-space - Vector3 mU1World; + Vector3 mR1World; /// Vector from center of body 2 to anchor point in world-space - Vector3 mU2World; + Vector3 mR2World; - /// Skew-Symmetric matrix for cross product with vector mU1World - Matrix3x3 mSkewSymmetricMatrixU1World; + /// Skew-Symmetric matrix for cross product with vector mR1World + Matrix3x3 mSkewSymmetricMatrixR1World; - /// Skew-Symmetric matrix for cross product with vector mU2World - Matrix3x3 mSkewSymmetricMatrixU2World; + /// Skew-Symmetric matrix for cross product with vector mR2World + Matrix3x3 mSkewSymmetricMatrixR2World; /// Inverse mass matrix K=JM^-1J^-t of the constraint Matrix3x3 mInverseMassMatrix; diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index 73222eef..8d395b31 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -35,6 +35,11 @@ SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) // Compute the local-space anchor point for each body mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; mLocalAnchorPointBody2 = mBody2->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; + + // Compute the slider axis in local-space of body 1 + mSliderAxisBody1 = mBody1->getTransform().getOrientation().getInverse() * + jointInfo.sliderAxisWorldSpace; + mSliderAxisBody1.normalize(); } // Destructor @@ -49,6 +54,10 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; + // Get the body positions + const Vector3& x1 = mBody1->getTransform().getPosition(); + const Vector3& x2 = mBody2->getTransform().getPosition(); + // Get the bodies positions and orientations const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); @@ -57,44 +66,72 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); - // Compute the vector from body center to the anchor point in world-space - mU1World = orientationBody1 * mLocalAnchorPointBody1; - mU2World = orientationBody2 * mLocalAnchorPointBody2; + // Vector from body center to the anchor point + mR1 = orientationBody1 * mLocalAnchorPointBody1; + mR2 = orientationBody2 * mLocalAnchorPointBody2; - // Compute the two orthogonal vectors to vector mU1World in world-space - mN1 = mU1World.getOneUnitOrthogonalVector(); - mN2 = mU1World.cross(mN1); + // Compute the vector u + const Vector3 u = x2 + mR2 - x1 - mR1; + + // Compute the two orthogonal vectors to the slider axis in local-space of body 1 + Vector3 sliderAxisWorld = orientationBody1 * mSliderAxisBody1; + sliderAxisWorld.normalize(); + mN1 = sliderAxisWorld.getOneUnitOrthogonalVector(); + mN2 = sliderAxisWorld.cross(mN1); // Compute the cross products used in the Jacobian - mU1WorldCrossN1 = mN2; - mU1WorldCrossN2 = mU1World.cross(mN2); - mU2WorldCrossN1 = mU2World.cross(mN1); - mU2WorldCrossN2 = mU2World.cross(mN2); + mR2CrossN1 = mR2.cross(mN1); + mR2CrossN2 = mR2.cross(mN2); + const Vector3 r1PlusU = mR1 + u; + mR1PlusUCrossN1 = (r1PlusU).cross(mN1); + mR1PlusUCrossN2 = (r1PlusU).cross(mN2); // Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation // constraints (2x2 matrix) const decimal n1Dotn1 = mN1.lengthSquare(); const decimal n2Dotn2 = mN2.lengthSquare(); const decimal n1Dotn2 = mN1.dot(mN2); - const decimal sumInverseMass = mBody1->getMassInverse() + mBody2->getMassInverse(); - const Vector3 I1U2CrossN1 = I1 * mU2WorldCrossN1; - const Vector3 I1U2CrossN2 = I1 * mU2WorldCrossN2; - const Vector3 I2U1CrossN1 = I2 * mU1WorldCrossN1; - const Vector3 I2U1CrossN2 = I2 * mU1WorldCrossN2; - const decimal el11 = sumInverseMass * (n1Dotn1) + mU2WorldCrossN1.dot(I1U2CrossN1) + - mU1WorldCrossN1.dot(I2U1CrossN1); - const decimal el12 = sumInverseMass * (n1Dotn2) + mU2WorldCrossN1.dot(I1U2CrossN2) + - mU1WorldCrossN1.dot(I2U1CrossN2); - const decimal el21 = sumInverseMass * (n1Dotn2) + mU2WorldCrossN2.dot(I1U2CrossN1) + - mU1WorldCrossN2.dot(I2U1CrossN1); - const decimal el22 = sumInverseMass * (n2Dotn2) + mU2WorldCrossN2.dot(I1U2CrossN2) + - mU1WorldCrossN2.dot(I2U1CrossN2); + decimal sumInverseMass = 0.0; + Vector3 I1R1PlusUCrossN1(0, 0, 0); + Vector3 I1R1PlusUCrossN2(0, 0, 0); + Vector3 I2R2CrossN1(0, 0, 0); + Vector3 I2R2CrossN2(0, 0, 0); + if (mBody1->getIsMotionEnabled()) { + sumInverseMass += mBody1->getMassInverse(); + I1R1PlusUCrossN1 = I1 * mR1PlusUCrossN1; + I1R1PlusUCrossN2 = I1 * mR1PlusUCrossN2; + } + if (mBody2->getIsMotionEnabled()) { + sumInverseMass += mBody2->getMassInverse(); + I2R2CrossN1 = I2 * mR2CrossN1; + I2R2CrossN2 = I2 * mR2CrossN2; + } + const decimal el11 = sumInverseMass * (n1Dotn1) + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + + mR2CrossN1.dot(I2R2CrossN1); + const decimal el12 = sumInverseMass * (n1Dotn2) + mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + + mR2CrossN1.dot(I2R2CrossN2); + const decimal el21 = sumInverseMass * (n1Dotn2) + mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) + + mR2CrossN2.dot(I2R2CrossN1); + const decimal el22 = sumInverseMass * (n2Dotn2) + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + + mR2CrossN2.dot(I2R2CrossN2); Matrix2x2 matrixKTranslation(el11, el12, el21, el22); - mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); + mInverseMassMatrixTranslationConstraint.setToZero(); + if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); + } // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // contraints (3x3 matrix) - mInverseMassMatrixRotationConstraint = I1 + I2; + mInverseMassMatrixRotationConstraint.setToZero(); + if (mBody1->getIsMotionEnabled()) { + mInverseMassMatrixRotationConstraint += I1; + } + if (mBody2->getIsMotionEnabled()) { + mInverseMassMatrixRotationConstraint += I2; + } + if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse(); + } } // Warm start the constraint (apply the previous impulse at the beginning of the step) @@ -107,20 +144,18 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies - decimal inverseMassBody1 = mBody1->getMassInverse(); - decimal inverseMassBody2 = mBody2->getMassInverse(); + const decimal inverseMassBody1 = mBody1->getMassInverse(); + const decimal inverseMassBody2 = mBody2->getMassInverse(); const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); // Compute the impulse P=J^T * lambda for the 2 translation constraints - Vector3 linearImpulseBody1 = -mN1 * mImpulseTranslation.x - - mN2 * mImpulseTranslation.y; - Vector3 angularImpulseBody1 = mU2WorldCrossN1 * mImpulseTranslation.x + - mU2WorldCrossN2 * mImpulseTranslation.y; - Vector3 linearImpulseBody2 = mN1 * mImpulseTranslation.x + - mN2 * mImpulseTranslation.y; - Vector3 angularImpulseBody2 = -mU1WorldCrossN1 * mImpulseTranslation.x - - mU1WorldCrossN2 * mImpulseTranslation.y; + Vector3 linearImpulseBody1 = -mN1 * mImpulseTranslation.x - mN2 * mImpulseTranslation.y; + Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * mImpulseTranslation.x - + mR1PlusUCrossN2 * mImpulseTranslation.y; + Vector3 linearImpulseBody2 = -linearImpulseBody1; + Vector3 angularImpulseBody2 = mR2CrossN1 * mImpulseTranslation.x + + mR2CrossN2 * mImpulseTranslation.y; // Compute the impulse P=J^T * lambda for the 3 rotation constraints angularImpulseBody1 += -mImpulseRotation; @@ -156,52 +191,34 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); - // Compute J*v for the 2 translation constraints - const decimal el1 = -mN1.dot(v1) + mU2WorldCrossN1.dot(w1) + - mN1.dot(v2) - mU1WorldCrossN1.dot(w2); - const decimal el2 = -mN2.dot(v1) + mU2WorldCrossN2.dot(w1) + - mN2.dot(v2) - mU1WorldCrossN2.dot(w2); - const Vector2 JvTranslation(el1, el2); + // --------------- Translation Constraints --------------- // - // Compute J*v for the 3 translation constraints - const Vector3 JvRotation = w2 - w1; + // Compute J*v for the 2 translation constraints + const decimal el1 = -mN1.dot(v1) - w1.dot(mR1PlusUCrossN1) + + mN1.dot(v2) + w2.dot(mR2CrossN1); + const decimal el2 = -mN2.dot(v1) - w1.dot(mR1PlusUCrossN2) + + mN2.dot(v2) + w2.dot(mR2CrossN2); + const Vector2 JvTranslation(el1, el2); // Compute the bias "b" of the translation constraint Vector2 bTranslation(0, 0); decimal beta = decimal(0.2); // TODO : Use a constant here decimal biasFactor = (beta / constraintSolverData.timeStep); if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { - Vector3 deltaV = x2 + mU2World - x1 - mU1World; + Vector3 deltaV = x2 + mR2 - x1 - mR1; bTranslation.x = biasFactor * deltaV.dot(mN1); bTranslation.y = biasFactor * deltaV.dot(mN2); } - // Compute the bias "b" of the translation constraint - Vector3 bRotation(0, 0, 0); - if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { - Quaternion q1 = mBody1->getTransform().getOrientation(); - Quaternion q2 = mBody2->getTransform().getOrientation(); - Quaternion qDiff = q1 * q2.getInverse(); - bRotation = 2.0 * qDiff.getVectorV(); - } - // Compute the Lagrange multiplier lambda for the 2 translation constraints Vector2 deltaLambda = mInverseMassMatrixTranslationConstraint * (-JvTranslation - bTranslation); mImpulseTranslation += deltaLambda; - // Compute the Lagrange multiplier lambda for the 3 rotation constraints - Vector3 deltaLambda2 = mInverseMassMatrixRotationConstraint * (-JvRotation - bRotation); - mImpulseRotation += deltaLambda2; - // Compute the impulse P=J^T * lambda for the 2 translation constraints Vector3 linearImpulseBody1 = -mN1 * deltaLambda.x - mN2 * deltaLambda.y; - Vector3 angularImpulseBody1 = mU2WorldCrossN1 * deltaLambda.x + mU2WorldCrossN2 * deltaLambda.y; - Vector3 linearImpulseBody2 = mN1 * deltaLambda.x + mN2 * deltaLambda.y; - Vector3 angularImpulseBody2 = -mU1WorldCrossN1 * deltaLambda.x -mU1WorldCrossN2 * deltaLambda.y; - - // Compute the impulse P=J^T * lambda for the 3 rotation constraints - angularImpulseBody1 += -deltaLambda2; - angularImpulseBody2 += deltaLambda2; + Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * deltaLambda.x -mR1PlusUCrossN2 * deltaLambda.y; + Vector3 linearImpulseBody2 = -linearImpulseBody1; + Vector3 angularImpulseBody2 = mR2CrossN1 * deltaLambda.x + mR2CrossN2 * deltaLambda.y; // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { @@ -212,6 +229,36 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint v2 += inverseMassBody2 * linearImpulseBody2; w2 += I2 * angularImpulseBody2; } + + // --------------- Rotation Constraints --------------- // + + // Compute J*v for the 3 rotation constraints + const Vector3 JvRotation = w2 - w1; + + // Compute the bias "b" of the rotation constraint + Vector3 bRotation(0, 0, 0); + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + Quaternion q1 = mBody1->getTransform().getOrientation(); + Quaternion q2 = mBody2->getTransform().getOrientation(); + Quaternion qDiff = q1 * q2.getInverse(); + bRotation = 2.0 * qDiff.getVectorV(); + } + + // Compute the Lagrange multiplier lambda for the 3 rotation constraints + Vector3 deltaLambda2 = mInverseMassMatrixRotationConstraint * (-JvRotation - bRotation); + mImpulseRotation += deltaLambda2; + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints + angularImpulseBody1 = -deltaLambda2; + angularImpulseBody2 = deltaLambda2; + + // Apply the impulse to the bodies of the joint + if (mBody1->getIsMotionEnabled()) { + w1 += I1 * angularImpulseBody1; + } + if (mBody2->getIsMotionEnabled()) { + w2 += I2 * angularImpulseBody2; + } } // Solve the position constraint diff --git a/src/constraint/SliderJoint.h b/src/constraint/SliderJoint.h index d4aab31a..44742a15 100644 --- a/src/constraint/SliderJoint.h +++ b/src/constraint/SliderJoint.h @@ -47,15 +47,15 @@ struct SliderJointInfo : public ConstraintInfo { Vector3 anchorPointWorldSpace; /// Slider axis (in world-space coordinates) - Vector3 axisWorldSpace; + Vector3 sliderAxisWorldSpace; /// Constructor SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, const Vector3& initAnchorPointWorldSpace, - const Vector3& initAxisWorldSpace) + const Vector3& initSliderAxisWorldSpace) : ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT), anchorPointWorldSpace(initAnchorPointWorldSpace), - axisWorldSpace(initAxisWorldSpace) {} + sliderAxisWorldSpace(initSliderAxisWorldSpace) {} }; // Class SliderJoint @@ -68,35 +68,38 @@ class SliderJoint : public Constraint { // -------------------- Attributes -------------------- // - /// Anchor point of body 1 (in local space coordinates) + /// Anchor point of body 1 (in local-space coordinates of body 1) Vector3 mLocalAnchorPointBody1; - /// Anchor point of body 2 (in local space coordinates) + /// Anchor point of body 2 (in local-space coordinates of body 2) Vector3 mLocalAnchorPointBody2; - /// Vector from center of body 2 to anchor point in world-space - Vector3 mU1World; + /// Slider axis (in local-space coordinates of body 1) + Vector3 mSliderAxisBody1; - /// Vector from center of body 2 to anchor point in world-space - Vector3 mU2World; - - /// First vector orthogonal to vector mU1World in world-space + /// First vector orthogonal to the slider axis local-space of body 1 Vector3 mN1; - /// Second vector orthogonal to vector mU1World and mN1 in world-space + /// Second vector orthogonal to the slider axis and mN1 in local-space of body 1 Vector3 mN2; - /// Cross product of mU1World and mN1 - Vector3 mU1WorldCrossN1; + /// Vector r1 in world-space coordinates + Vector3 mR1; - /// Cross product of mU1World and mN2 - Vector3 mU1WorldCrossN2; + /// Vector r2 in world-space coordinates + Vector3 mR2; - /// Cross product of mU2World and mN1 - Vector3 mU2WorldCrossN1; + /// Cross product of r2 and n1 + Vector3 mR2CrossN1; - /// Cross product of mU2World and mN2 - Vector3 mU2WorldCrossN2; + /// Cross product of r2 and n2 + Vector3 mR2CrossN2; + + /// Cross product of vector (r1 + u) and n1 + Vector3 mR1PlusUCrossN1; + + /// Cross product of vector (r1 + u) and n2 + Vector3 mR1PlusUCrossN2; /// Inverse of mass matrix K=JM^-1J^t for the translation constraint (2x2 matrix) Matrix2x2 mInverseMassMatrixTranslationConstraint; diff --git a/src/mathematics/Matrix2x2.h b/src/mathematics/Matrix2x2.h index 53a313af..d19846bc 100644 --- a/src/mathematics/Matrix2x2.h +++ b/src/mathematics/Matrix2x2.h @@ -44,7 +44,7 @@ class Matrix2x2 { // -------------------- Attributes -------------------- // /// Rows of the matrix; - Vector2 mRows[3]; + Vector2 mRows[2]; public : diff --git a/src/reactphysics3d.h b/src/reactphysics3d.h index 5f463e13..df9e367c 100644 --- a/src/reactphysics3d.h +++ b/src/reactphysics3d.h @@ -48,6 +48,7 @@ #include "collision/shapes/CylinderShape.h" #include "collision/shapes/AABB.h" #include "constraint/BallAndSocketJoint.h" +#include "constraint/SliderJoint.h" /// Alias to the ReactPhysics3D namespace namespace rp3d = reactphysics3d; From c7aa6e7e0e7837d175d9fa40b618856b89c07225 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 22 May 2013 23:38:30 +0200 Subject: [PATCH 18/24] Start working of the SliderJoint limits --- src/constraint/SliderJoint.cpp | 142 +++++++++++++++++++++++---------- src/constraint/SliderJoint.h | 67 +++++++++++++++- 2 files changed, 164 insertions(+), 45 deletions(-) diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index 8d395b31..a9a322a2 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -30,11 +30,23 @@ using namespace reactphysics3d; // Constructor SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) - : Constraint(jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0) { + : Constraint(jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0), + mImpulseLowerLimit(0), mIsLimitsActive(jointInfo.isLimitsActive), + mLowerLimit(jointInfo.lowerLimit), mUpperLimit(jointInfo.upperLimit) { + + assert(mUpperLimit >= 0.0); + assert(mLowerLimit <= 0.0); // Compute the local-space anchor point for each body - mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; - mLocalAnchorPointBody2 = mBody2->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; + const Transform& transform1 = mBody1->getTransform(); + const Transform& transform2 = mBody2->getTransform(); + mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; + mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; + + // Compute the initial orientation difference between the two bodies + mInitOrientationDifference = transform2.getOrientation() * + transform1.getOrientation().getInverse(); + mInitOrientationDifference.normalize(); // Compute the slider axis in local-space of body 1 mSliderAxisBody1 = mBody1->getTransform().getOrientation().getInverse() * @@ -54,11 +66,9 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; - // Get the body positions + // Get the bodies positions and orientations const Vector3& x1 = mBody1->getTransform().getPosition(); const Vector3& x2 = mBody2->getTransform().getPosition(); - - // Get the bodies positions and orientations const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); @@ -74,23 +84,26 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa const Vector3 u = x2 + mR2 - x1 - mR1; // Compute the two orthogonal vectors to the slider axis in local-space of body 1 - Vector3 sliderAxisWorld = orientationBody1 * mSliderAxisBody1; - sliderAxisWorld.normalize(); - mN1 = sliderAxisWorld.getOneUnitOrthogonalVector(); - mN2 = sliderAxisWorld.cross(mN1); + mSliderAxisWorld = orientationBody1 * mSliderAxisBody1; + mSliderAxisWorld.normalize(); + mN1 = mSliderAxisWorld.getOneUnitOrthogonalVector(); + mN2 = mSliderAxisWorld.cross(mN1); - // Compute the cross products used in the Jacobian + // Check if the limit constraints are violated or not + decimal lowerLimitError = u.dot(mSliderAxisWorld) - mLowerLimit; + mIsLowerLimitViolated = (lowerLimitError <= 0); + + // Compute the cross products used in the Jacobians mR2CrossN1 = mR2.cross(mN1); mR2CrossN2 = mR2.cross(mN2); + mR2CrossSliderAxis = mR2.cross(mSliderAxisWorld); const Vector3 r1PlusU = mR1 + u; mR1PlusUCrossN1 = (r1PlusU).cross(mN1); mR1PlusUCrossN2 = (r1PlusU).cross(mN2); + mR1PlusUCrossSliderAxis = (r1PlusU).cross(mSliderAxisWorld); // Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation // constraints (2x2 matrix) - const decimal n1Dotn1 = mN1.lengthSquare(); - const decimal n2Dotn2 = mN2.lengthSquare(); - const decimal n1Dotn2 = mN1.dot(mN2); decimal sumInverseMass = 0.0; Vector3 I1R1PlusUCrossN1(0, 0, 0); Vector3 I1R1PlusUCrossN2(0, 0, 0); @@ -106,13 +119,13 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa I2R2CrossN1 = I2 * mR2CrossN1; I2R2CrossN2 = I2 * mR2CrossN2; } - const decimal el11 = sumInverseMass * (n1Dotn1) + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + + const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + mR2CrossN1.dot(I2R2CrossN1); - const decimal el12 = sumInverseMass * (n1Dotn2) + mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + + const decimal el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + mR2CrossN1.dot(I2R2CrossN2); - const decimal el21 = sumInverseMass * (n1Dotn2) + mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) + + const decimal el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) + mR2CrossN2.dot(I2R2CrossN1); - const decimal el22 = sumInverseMass * (n2Dotn2) + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + + const decimal el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + mR2CrossN2.dot(I2R2CrossN2); Matrix2x2 matrixKTranslation(el11, el12, el21, el22); mInverseMassMatrixTranslationConstraint.setToZero(); @@ -120,6 +133,16 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); } + // Compute the bias "b" of the translation constraint + mBTranslation.setToZero(); + decimal beta = decimal(0.2); // TODO : Use a constant here + decimal biasFactor = (beta / constraintSolverData.timeStep); + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + mBTranslation.x = u.dot(mN1); + mBTranslation.y = u.dot(mN2); + mBTranslation *= biasFactor; + } + // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // contraints (3x3 matrix) mInverseMassMatrixRotationConstraint.setToZero(); @@ -132,6 +155,27 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse(); } + + // Compute the bias "b" of the rotation constraint + mBRotation.setToZero(); + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); + currentOrientationDifference.normalize(); + const Quaternion qError = currentOrientationDifference * + mInitOrientationDifference.getInverse(); + mBRotation = biasFactor * decimal(2.0) * qError.getVectorV(); + } + + // Compute the inverse of the mass matrix K=JM^-1J^t for the lower limit (1x1 matrix) + mInverseMassMatrixLowerLimit = sumInverseMass + + mR1PlusUCrossSliderAxis.dot(I1 * mR1PlusUCrossSliderAxis) + + mR2CrossSliderAxis.dot(I2 * mR2CrossSliderAxis); + + // Compute the bias "b" of the lower limit constraint + mBLowerLimit = 0.0; + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + mBLowerLimit = biasFactor * lowerLimitError; + } } // Warm start the constraint (apply the previous impulse at the beginning of the step) @@ -175,10 +219,6 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { // Solve the velocity constraint void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { - // Get the body positions - const Vector3& x1 = mBody1->getTransform().getPosition(); - const Vector3& x2 = mBody2->getTransform().getPosition(); - // Get the velocities Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; @@ -200,18 +240,8 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint mN2.dot(v2) + w2.dot(mR2CrossN2); const Vector2 JvTranslation(el1, el2); - // Compute the bias "b" of the translation constraint - Vector2 bTranslation(0, 0); - decimal beta = decimal(0.2); // TODO : Use a constant here - decimal biasFactor = (beta / constraintSolverData.timeStep); - if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { - Vector3 deltaV = x2 + mR2 - x1 - mR1; - bTranslation.x = biasFactor * deltaV.dot(mN1); - bTranslation.y = biasFactor * deltaV.dot(mN2); - } - // Compute the Lagrange multiplier lambda for the 2 translation constraints - Vector2 deltaLambda = mInverseMassMatrixTranslationConstraint * (-JvTranslation - bTranslation); + Vector2 deltaLambda = mInverseMassMatrixTranslationConstraint * (-JvTranslation -mBTranslation); mImpulseTranslation += deltaLambda; // Compute the impulse P=J^T * lambda for the 2 translation constraints @@ -235,17 +265,8 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint // Compute J*v for the 3 rotation constraints const Vector3 JvRotation = w2 - w1; - // Compute the bias "b" of the rotation constraint - Vector3 bRotation(0, 0, 0); - if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { - Quaternion q1 = mBody1->getTransform().getOrientation(); - Quaternion q2 = mBody2->getTransform().getOrientation(); - Quaternion qDiff = q1 * q2.getInverse(); - bRotation = 2.0 * qDiff.getVectorV(); - } - // Compute the Lagrange multiplier lambda for the 3 rotation constraints - Vector3 deltaLambda2 = mInverseMassMatrixRotationConstraint * (-JvRotation - bRotation); + Vector3 deltaLambda2 = mInverseMassMatrixRotationConstraint * (-JvRotation - mBRotation); mImpulseRotation += deltaLambda2; // Compute the impulse P=J^T * lambda for the 3 rotation constraints @@ -259,6 +280,41 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint if (mBody2->getIsMotionEnabled()) { w2 += I2 * angularImpulseBody2; } + + // --------------- Limits Constraints --------------- // + + if (mIsLimitsActive) { + + // If the lower limit is violated + if (mIsLowerLimitViolated) { + + // Compute J*v for the lower limit constraint + const decimal JvLowerLimit = mSliderAxisWorld.dot(v2) + mR2CrossSliderAxis.dot(w2) - + mSliderAxisWorld.dot(v1) - mR1PlusUCrossSliderAxis.dot(w1); + + // Compute the Lagrange multiplier lambda for the lower limit constraint + decimal deltaLambdaLower = mInverseMassMatrixLowerLimit * (-JvLowerLimit -mBLowerLimit); + decimal lambdaTemp = mImpulseLowerLimit; + mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0)); + deltaLambdaLower = mImpulseLowerLimit - lambdaTemp; + + // Compute the impulse P=J^T * lambda for the lower limit constraint + Vector3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld; + Vector3 angularImpulseBody1 = -deltaLambdaLower * mR1PlusUCrossSliderAxis; + Vector3 linearImpulseBody2 = -linearImpulseBody1; + Vector3 angularImpulseBody2 = deltaLambdaLower * mR2CrossSliderAxis; + + // Apply the impulse to the bodies of the joint + if (mBody1->getIsMotionEnabled()) { + v1 += inverseMassBody1 * linearImpulseBody1; + w1 += I1 * angularImpulseBody1; + } + if (mBody2->getIsMotionEnabled()) { + v2 += inverseMassBody2 * linearImpulseBody2; + w2 += I2 * angularImpulseBody2; + } + } + } } // Solve the position constraint diff --git a/src/constraint/SliderJoint.h b/src/constraint/SliderJoint.h index 44742a15..1f9bef7c 100644 --- a/src/constraint/SliderJoint.h +++ b/src/constraint/SliderJoint.h @@ -49,13 +49,34 @@ struct SliderJointInfo : public ConstraintInfo { /// Slider axis (in world-space coordinates) Vector3 sliderAxisWorldSpace; - /// Constructor + /// True if the slider limits are active + bool isLimitsActive; + + /// Lower limit + decimal lowerLimit; + + /// Upper limit + decimal upperLimit; + + /// Constructor without limits SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, const Vector3& initAnchorPointWorldSpace, const Vector3& initSliderAxisWorldSpace) : ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT), anchorPointWorldSpace(initAnchorPointWorldSpace), - sliderAxisWorldSpace(initSliderAxisWorldSpace) {} + sliderAxisWorldSpace(initSliderAxisWorldSpace), + isLimitsActive(false), lowerLimit(-1.0), upperLimit(1.0) {} + + /// Constructor with limits + SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, + const Vector3& initAnchorPointWorldSpace, + const Vector3& initSliderAxisWorldSpace, + decimal initLowerLimit, decimal initUpperLimit) + : ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT), + anchorPointWorldSpace(initAnchorPointWorldSpace), + sliderAxisWorldSpace(initSliderAxisWorldSpace), + isLimitsActive(true), lowerLimit(initLowerLimit), + upperLimit(initUpperLimit) {} }; // Class SliderJoint @@ -77,6 +98,9 @@ class SliderJoint : public Constraint { /// Slider axis (in local-space coordinates of body 1) Vector3 mSliderAxisBody1; + /// Initial orientation difference between the two bodies + Quaternion mInitOrientationDifference; + /// First vector orthogonal to the slider axis local-space of body 1 Vector3 mN1; @@ -95,24 +119,63 @@ class SliderJoint : public Constraint { /// Cross product of r2 and n2 Vector3 mR2CrossN2; + /// Cross product of r2 and the slider axis + Vector3 mR2CrossSliderAxis; + /// Cross product of vector (r1 + u) and n1 Vector3 mR1PlusUCrossN1; /// Cross product of vector (r1 + u) and n2 Vector3 mR1PlusUCrossN2; + /// Cross product of vector (r1 + u) and the slider axis + Vector3 mR1PlusUCrossSliderAxis; + + /// Bias of the 2 translation constraints + Vector2 mBTranslation; + + /// Bias of the 3 rotation constraints + Vector3 mBRotation; + + /// Bias of the lower limit constraint + decimal mBLowerLimit; + /// Inverse of mass matrix K=JM^-1J^t for the translation constraint (2x2 matrix) Matrix2x2 mInverseMassMatrixTranslationConstraint; /// Inverse of mass matrix K=JM^-1J^t for the rotation constraint (3x3 matrix) Matrix3x3 mInverseMassMatrixRotationConstraint; + /// Inverse of mass matrix K=JM^-1J^t for the lower limit constraint (1x1 matrix) + decimal mInverseMassMatrixLowerLimit; + /// Impulse for the 2 translation constraints Vector2 mImpulseTranslation; /// Impulse for the 3 rotation constraints Vector3 mImpulseRotation; + /// Impulse for the lower limit constraint + decimal mImpulseLowerLimit; + + /// True if the slider limits are active + bool mIsLimitsActive; + + /// Slider axis in world-space coordinates + Vector3 mSliderAxisWorld; + + /// Lower limit + decimal mLowerLimit; + + /// Upper limit + decimal mUpperLimit; + + /// True if the lower limit is violated + bool mIsLowerLimitViolated; + + /// True if the upper limit is violated + bool mIsUpperLimitViolated; + public : // -------------------- Methods -------------------- // From 61562b3560b8795f37e1cef488432d3a4442c37c Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 23 May 2013 22:52:08 +0200 Subject: [PATCH 19/24] Implement the upper limit for the slider joint --- src/constraint/SliderJoint.cpp | 60 ++++++++++++++++++++++++++++------ src/constraint/SliderJoint.h | 10 ++++-- 2 files changed, 58 insertions(+), 12 deletions(-) diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index a9a322a2..74003790 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -31,8 +31,9 @@ using namespace reactphysics3d; // Constructor SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) : Constraint(jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0), - mImpulseLowerLimit(0), mIsLimitsActive(jointInfo.isLimitsActive), - mLowerLimit(jointInfo.lowerLimit), mUpperLimit(jointInfo.upperLimit) { + mImpulseLowerLimit(0), mImpulseUpperLimit(0), + mIsLimitsActive(jointInfo.isLimitsActive), mLowerLimit(jointInfo.lowerLimit), + mUpperLimit(jointInfo.upperLimit) { assert(mUpperLimit >= 0.0); assert(mLowerLimit <= 0.0); @@ -83,15 +84,18 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa // Compute the vector u const Vector3 u = x2 + mR2 - x1 - mR1; - // Compute the two orthogonal vectors to the slider axis in local-space of body 1 + // Compute the two orthogonal vectors to the slider axis in world-space mSliderAxisWorld = orientationBody1 * mSliderAxisBody1; mSliderAxisWorld.normalize(); mN1 = mSliderAxisWorld.getOneUnitOrthogonalVector(); mN2 = mSliderAxisWorld.cross(mN1); // Check if the limit constraints are violated or not - decimal lowerLimitError = u.dot(mSliderAxisWorld) - mLowerLimit; + decimal uDotSliderAxis = u.dot(mSliderAxisWorld); + decimal lowerLimitError = uDotSliderAxis - mLowerLimit; + decimal upperLimitError = mUpperLimit - uDotSliderAxis; mIsLowerLimitViolated = (lowerLimitError <= 0); + mIsUpperLimitViolated = (upperLimitError <= 0); // Compute the cross products used in the Jacobians mR2CrossN1 = mR2.cross(mN1); @@ -167,7 +171,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa } // Compute the inverse of the mass matrix K=JM^-1J^t for the lower limit (1x1 matrix) - mInverseMassMatrixLowerLimit = sumInverseMass + + mInverseMassMatrixLimit = sumInverseMass + mR1PlusUCrossSliderAxis.dot(I1 * mR1PlusUCrossSliderAxis) + mR2CrossSliderAxis.dot(I2 * mR2CrossSliderAxis); @@ -176,6 +180,12 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { mBLowerLimit = biasFactor * lowerLimitError; } + + // Compute the bias "b" of the upper limit constraint + mBUpperLimit = 0.0; + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + mBUpperLimit = biasFactor * upperLimitError; + } } // Warm start the constraint (apply the previous impulse at the beginning of the step) @@ -293,16 +303,46 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint mSliderAxisWorld.dot(v1) - mR1PlusUCrossSliderAxis.dot(w1); // Compute the Lagrange multiplier lambda for the lower limit constraint - decimal deltaLambdaLower = mInverseMassMatrixLowerLimit * (-JvLowerLimit -mBLowerLimit); + decimal deltaLambdaLower = mInverseMassMatrixLimit * (-JvLowerLimit -mBLowerLimit); decimal lambdaTemp = mImpulseLowerLimit; mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0)); deltaLambdaLower = mImpulseLowerLimit - lambdaTemp; // Compute the impulse P=J^T * lambda for the lower limit constraint - Vector3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld; - Vector3 angularImpulseBody1 = -deltaLambdaLower * mR1PlusUCrossSliderAxis; - Vector3 linearImpulseBody2 = -linearImpulseBody1; - Vector3 angularImpulseBody2 = deltaLambdaLower * mR2CrossSliderAxis; + const Vector3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld; + const Vector3 angularImpulseBody1 = -deltaLambdaLower * mR1PlusUCrossSliderAxis; + const Vector3 linearImpulseBody2 = -linearImpulseBody1; + const Vector3 angularImpulseBody2 = deltaLambdaLower * mR2CrossSliderAxis; + + // Apply the impulse to the bodies of the joint + if (mBody1->getIsMotionEnabled()) { + v1 += inverseMassBody1 * linearImpulseBody1; + w1 += I1 * angularImpulseBody1; + } + if (mBody2->getIsMotionEnabled()) { + v2 += inverseMassBody2 * linearImpulseBody2; + w2 += I2 * angularImpulseBody2; + } + } + + // If the upper limit is violated + if (mIsUpperLimitViolated) { + + // Compute J*v for the upper limit constraint + const decimal JvUpperLimit = mSliderAxisWorld.dot(v1) + mR1PlusUCrossSliderAxis.dot(w1) + - mSliderAxisWorld.dot(v2) - mR2CrossSliderAxis.dot(w2); + + // Compute the Lagrange multiplier lambda for the upper limit constraint + decimal deltaLambdaUpper = mInverseMassMatrixLimit * (-JvUpperLimit -mBUpperLimit); + decimal lambdaTemp = mImpulseUpperLimit; + mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0)); + deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp; + + // Compute the impulse P=J^T * lambda for the upper limit constraint + const Vector3 linearImpulseBody1 = deltaLambdaUpper * mSliderAxisWorld; + const Vector3 angularImpulseBody1 = deltaLambdaUpper * mR1PlusUCrossSliderAxis; + const Vector3 linearImpulseBody2 = -linearImpulseBody1; + const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mR2CrossSliderAxis; // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { diff --git a/src/constraint/SliderJoint.h b/src/constraint/SliderJoint.h index 1f9bef7c..d0429d25 100644 --- a/src/constraint/SliderJoint.h +++ b/src/constraint/SliderJoint.h @@ -140,14 +140,17 @@ class SliderJoint : public Constraint { /// Bias of the lower limit constraint decimal mBLowerLimit; + /// Bias of the upper limit constraint + decimal mBUpperLimit; + /// Inverse of mass matrix K=JM^-1J^t for the translation constraint (2x2 matrix) Matrix2x2 mInverseMassMatrixTranslationConstraint; /// Inverse of mass matrix K=JM^-1J^t for the rotation constraint (3x3 matrix) Matrix3x3 mInverseMassMatrixRotationConstraint; - /// Inverse of mass matrix K=JM^-1J^t for the lower limit constraint (1x1 matrix) - decimal mInverseMassMatrixLowerLimit; + /// Inverse of mass matrix K=JM^-1J^t for the upper and lower limit constraints (1x1 matrix) + decimal mInverseMassMatrixLimit; /// Impulse for the 2 translation constraints Vector2 mImpulseTranslation; @@ -158,6 +161,9 @@ class SliderJoint : public Constraint { /// Impulse for the lower limit constraint decimal mImpulseLowerLimit; + /// Impulse for the upper limit constraint + decimal mImpulseUpperLimit; + /// True if the slider limits are active bool mIsLimitsActive; From 9c0844cf1b8ec612ae7cf60dc4cc9ca0b7fd1ecf Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 29 May 2013 22:45:02 +0200 Subject: [PATCH 20/24] Finish the implementation of the slider joint --- src/constraint/BallAndSocketJoint.cpp | 37 +++-- src/constraint/BallAndSocketJoint.h | 8 + src/constraint/Constraint.h | 4 +- src/constraint/SliderJoint.cpp | 210 +++++++++++++++++++++++--- src/constraint/SliderJoint.h | 140 +++++++++++++++-- 5 files changed, 352 insertions(+), 47 deletions(-) diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index aa3409f2..edec02b1 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -29,8 +29,11 @@ using namespace reactphysics3d; +// Static variables definition +const decimal BallAndSocketJoint::BETA = 0.2; + // Constructor -BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo &jointInfo) +BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo) : Constraint(jointInfo), mImpulse(Vector3(0, 0, 0)) { // Compute the local-space anchor point for each body @@ -51,6 +54,8 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; // Get the bodies positions and orientations + const Vector3& x1 = mBody1->getTransform().getPosition(); + const Vector3& x2 = mBody2->getTransform().getPosition(); const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); @@ -89,6 +94,20 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { mInverseMassMatrix = massMatrix.getInverse(); } + + // Compute the bias "b" of the constraint + mBiasVector.setToZero(); + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + decimal biasFactor = (BETA / constraintSolverData.timeStep); + mBiasVector = biasFactor * (x2 + mR2World - x1 - mR1World); + } + + // If warm-starting is not enabled + if (!constraintSolverData.isWarmStartingActive) { + + // Reset the accumulated impulse + mImpulse.setToZero(); + } } // Warm start the constraint (apply the previous impulse at the beginning of the step) @@ -126,10 +145,6 @@ void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverD // Solve the velocity constraint void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { - // Get the body positions - const Vector3& x1 = mBody1->getTransform().getPosition(); - const Vector3& x2 = mBody2->getTransform().getPosition(); - // Get the velocities Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; @@ -143,18 +158,10 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); // Compute J*v - const Vector3 Jv = -v1 + mR1World.cross(w1) + v2 - mR2World.cross(w2); - - // Compute the bias "b" of the constraint - Vector3 b(0, 0, 0); - if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { - decimal beta = decimal(0.2); // TODO : Use a constant here - decimal biasFactor = (beta / constraintSolverData.timeStep); - b = biasFactor * (x2 + mR2World - x1 - mR1World); - } + const Vector3 Jv = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World); // Compute the Lagrange multiplier lambda - const Vector3 deltaLambda = mInverseMassMatrix * (-Jv - b); + const Vector3 deltaLambda = mInverseMassMatrix * (-Jv - mBiasVector); mImpulse += deltaLambda; // Compute the impulse P=J^T * lambda diff --git a/src/constraint/BallAndSocketJoint.h b/src/constraint/BallAndSocketJoint.h index 556e316b..dff9210d 100644 --- a/src/constraint/BallAndSocketJoint.h +++ b/src/constraint/BallAndSocketJoint.h @@ -62,6 +62,11 @@ class BallAndSocketJoint : public Constraint { private : + // -------------------- Constants -------------------- // + + // Beta value for the bias factor of position correction + static const decimal BETA; + // -------------------- Attributes -------------------- // /// Anchor point of body 1 (in local-space coordinates of body 1) @@ -76,6 +81,9 @@ class BallAndSocketJoint : public Constraint { /// Vector from center of body 2 to anchor point in world-space Vector3 mR2World; + /// Bias vector for the constraint + Vector3 mBiasVector; + /// Skew-Symmetric matrix for cross product with vector mR1World Matrix3x3 mSkewSymmetricMatrixR1World; diff --git a/src/constraint/Constraint.h b/src/constraint/Constraint.h index 1be00170..97b70150 100644 --- a/src/constraint/Constraint.h +++ b/src/constraint/Constraint.h @@ -33,9 +33,9 @@ // ReactPhysics3D namespace namespace reactphysics3d { - + // Enumeration for the type of a constraint -enum ConstraintType {CONTACT, BALLSOCKETJOINT, SLIDERJOINT}; +enum ConstraintType {CONTACT, BALLSOCKETJOINT, SLIDERJOINT, HINGEJOINT}; // Class declarations struct ConstraintSolverData; diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index 74003790..7825a1fc 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -28,15 +28,21 @@ using namespace reactphysics3d; +// Static variables definition +const decimal SliderJoint::BETA = decimal(0.2); + // Constructor SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) : Constraint(jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0), - mImpulseLowerLimit(0), mImpulseUpperLimit(0), - mIsLimitsActive(jointInfo.isLimitsActive), mLowerLimit(jointInfo.lowerLimit), - mUpperLimit(jointInfo.upperLimit) { + mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0), + mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled), + mLowerLimit(jointInfo.lowerLimit), mUpperLimit(jointInfo.upperLimit), + mIsLowerLimitViolated(false), mIsUpperLimitViolated(false), + mMotorSpeed(jointInfo.motorSpeed), mMaxMotorForce(jointInfo.maxMotorForce){ assert(mUpperLimit >= 0.0); assert(mLowerLimit <= 0.0); + assert(mMaxMotorForce >= 0.0); // Compute the local-space anchor point for each body const Transform& transform1 = mBody1->getTransform(); @@ -63,7 +69,7 @@ SliderJoint::~SliderJoint() { // Initialize before solving the constraint void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { - // Initialize the bodies index in the velocity array + // Initialize the bodies index in the veloc ity array mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; @@ -94,8 +100,16 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa decimal uDotSliderAxis = u.dot(mSliderAxisWorld); decimal lowerLimitError = uDotSliderAxis - mLowerLimit; decimal upperLimitError = mUpperLimit - uDotSliderAxis; - mIsLowerLimitViolated = (lowerLimitError <= 0); - mIsUpperLimitViolated = (upperLimitError <= 0); + bool oldIsLowerLimitViolated = mIsLowerLimitViolated; + mIsLowerLimitViolated = lowerLimitError <= 0; + if (mIsLowerLimitViolated != oldIsLowerLimitViolated) { + mImpulseLowerLimit = 0.0; + } + bool oldIsUpperLimitViolated = mIsUpperLimitViolated; + mIsUpperLimitViolated = upperLimitError <= 0; + if (mIsUpperLimitViolated != oldIsUpperLimitViolated) { + mImpulseUpperLimit = 0.0; + } // Compute the cross products used in the Jacobians mR2CrossN1 = mR2.cross(mN1); @@ -139,8 +153,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa // Compute the bias "b" of the translation constraint mBTranslation.setToZero(); - decimal beta = decimal(0.2); // TODO : Use a constant here - decimal biasFactor = (beta / constraintSolverData.timeStep); + decimal biasFactor = (BETA / constraintSolverData.timeStep); if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { mBTranslation.x = u.dot(mN1); mBTranslation.y = u.dot(mN2); @@ -170,21 +183,54 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa mBRotation = biasFactor * decimal(2.0) * qError.getVectorV(); } - // Compute the inverse of the mass matrix K=JM^-1J^t for the lower limit (1x1 matrix) - mInverseMassMatrixLimit = sumInverseMass + - mR1PlusUCrossSliderAxis.dot(I1 * mR1PlusUCrossSliderAxis) + - mR2CrossSliderAxis.dot(I2 * mR2CrossSliderAxis); + if (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated)) { - // Compute the bias "b" of the lower limit constraint - mBLowerLimit = 0.0; - if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { - mBLowerLimit = biasFactor * lowerLimitError; + // Compute the inverse of the mass matrix K=JM^-1J^t for the lower limit (1x1 matrix) + mInverseMassMatrixLimit = 0.0; + if (mBody1->getIsMotionEnabled()) { + mInverseMassMatrixLimit += mBody1->getMassInverse() + + mR1PlusUCrossSliderAxis.dot(I1 * mR1PlusUCrossSliderAxis); + } + if (mBody2->getIsMotionEnabled()) { + mInverseMassMatrixLimit += mBody2->getMassInverse() + + mR2CrossSliderAxis.dot(I2 * mR2CrossSliderAxis); + } + mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ? + decimal(1.0) / mInverseMassMatrixLimit : decimal(0.0); + + // Compute the bias "b" of the lower limit constraint + mBLowerLimit = 0.0; + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + mBLowerLimit = biasFactor * lowerLimitError; + } + + // Compute the bias "b" of the upper limit constraint + mBUpperLimit = 0.0; + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + mBUpperLimit = biasFactor * upperLimitError; + } } - // Compute the bias "b" of the upper limit constraint - mBUpperLimit = 0.0; - if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { - mBUpperLimit = biasFactor * upperLimitError; + // Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix) + mInverseMassMatrixMotor = 0.0; + if (mBody1->getIsMotionEnabled()) { + mInverseMassMatrixMotor += mBody1->getMassInverse(); + } + if (mBody2->getIsMotionEnabled()) { + mInverseMassMatrixMotor += mBody2->getMassInverse(); + } + mInverseMassMatrixMotor = (mInverseMassMatrixMotor > 0.0) ? + decimal(1.0) / mInverseMassMatrixMotor : decimal(0.0); + + // If warm-starting is not enabled + if (!constraintSolverData.isWarmStartingActive) { + + // Reset all the accumulated impulses + mImpulseTranslation.setToZero(); + mImpulseRotation.setToZero(); + mImpulseLowerLimit = 0.0; + mImpulseUpperLimit = 0.0; + mImpulseMotor = 0.0; } } @@ -215,6 +261,19 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { angularImpulseBody1 += -mImpulseRotation; angularImpulseBody2 += mImpulseRotation; + // Compute the impulse P=J^T * lambda for the lower and upper limits constraints + decimal impulseLimits = mImpulseUpperLimit - mImpulseLowerLimit; + Vector3 linearImpulseLimits = impulseLimits * mSliderAxisWorld; + linearImpulseBody1 += linearImpulseLimits; + angularImpulseBody1 += impulseLimits * mR1PlusUCrossSliderAxis; + linearImpulseBody2 += -linearImpulseLimits; + angularImpulseBody2 += -impulseLimits * mR2CrossSliderAxis; + + // Compute the impulse P=J^T * lambda for the motor constraint + Vector3 impulseMotor = mImpulseMotor * mSliderAxisWorld; + linearImpulseBody1 += impulseMotor; + linearImpulseBody2 += -impulseMotor; + // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { v1 += inverseMassBody1 * linearImpulseBody1; @@ -293,7 +352,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint // --------------- Limits Constraints --------------- // - if (mIsLimitsActive) { + if (mIsLimitEnabled) { // If the lower limit is violated if (mIsLowerLimitViolated) { @@ -355,9 +414,118 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint } } } + + // --------------- Motor --------------- // + + if (mIsMotorEnabled) { + + // Compute J*v for the motor + const decimal JvMotor = mSliderAxisWorld.dot(v1) - mSliderAxisWorld.dot(v2); + + // Compute the Lagrange multiplier lambda for the motor + const decimal maxMotorImpulse = mMaxMotorForce * constraintSolverData.timeStep; + decimal deltaLambdaMotor = mInverseMassMatrixMotor * (-JvMotor -mMotorSpeed); + decimal lambdaTemp = mImpulseMotor; + mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); + deltaLambdaMotor = mImpulseMotor - lambdaTemp; + + // Compute the impulse P=J^T * lambda for the motor + const Vector3 linearImpulseBody1 = deltaLambdaMotor * mSliderAxisWorld; + const Vector3 linearImpulseBody2 = -linearImpulseBody1; + + // Apply the impulse to the bodies of the joint + if (mBody1->getIsMotionEnabled()) { + v1 += inverseMassBody1 * linearImpulseBody1; + } + if (mBody2->getIsMotionEnabled()) { + v2 += inverseMassBody2 * linearImpulseBody2; + } + } } // Solve the position constraint void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) { } + +// Enable/Disable the limits of the joint +void SliderJoint::enableLimit(bool isLimitEnabled) { + + if (isLimitEnabled != mIsLimitEnabled) { + + mIsLimitEnabled = isLimitEnabled; + + // Reset the limits + resetLimits(); + } +} + +// Enable/Disable the motor of the joint +void SliderJoint::enableMotor(bool isMotorEnabled) { + + mIsMotorEnabled = isMotorEnabled; + mImpulseMotor = 0.0; + + // TODO : Wake up the bodies of the joint here when sleeping is implemented +} + +// Set the lower limit +void SliderJoint::setLowerLimit(decimal lowerLimit) { + + assert(lowerLimit <= mUpperLimit); + + if (lowerLimit != mLowerLimit) { + + mLowerLimit = lowerLimit; + + // Reset the limits + resetLimits(); + } +} + +// Set the upper limit +void SliderJoint::setUpperLimit(decimal upperLimit) { + + assert(mLowerLimit <= upperLimit); + + if (upperLimit != mUpperLimit) { + + mUpperLimit = upperLimit; + + // Reset the limits + resetLimits(); + } +} + +// Reset the limits +void SliderJoint::resetLimits() { + + // Reset the accumulated impulses for the limits + mImpulseLowerLimit = 0.0; + mImpulseUpperLimit = 0.0; + + // TODO : Wake up the bodies of the joint here when sleeping is implemented +} + +// Set the motor speed +void SliderJoint::setMotorSpeed(decimal motorSpeed) { + + if (motorSpeed != mMotorSpeed) { + + mMotorSpeed = motorSpeed; + + // TODO : Wake up the bodies of the joint here when sleeping is implemented + } +} + +// Set the maximum motor force +void SliderJoint::setMaxMotorForce(decimal maxMotorForce) { + + if (maxMotorForce != mMaxMotorForce) { + + assert(mMaxMotorForce >= 0.0); + mMaxMotorForce = maxMotorForce; + + // TODO : Wake up the bodies of the joint here when sleeping is implemented + } +} diff --git a/src/constraint/SliderJoint.h b/src/constraint/SliderJoint.h index d0429d25..fd54b5cb 100644 --- a/src/constraint/SliderJoint.h +++ b/src/constraint/SliderJoint.h @@ -49,8 +49,11 @@ struct SliderJointInfo : public ConstraintInfo { /// Slider axis (in world-space coordinates) Vector3 sliderAxisWorldSpace; - /// True if the slider limits are active - bool isLimitsActive; + /// True if the slider limits are enabled + bool isLimitEnabled; + + /// True if the slider motor is enabled + bool isMotorEnabled; /// Lower limit decimal lowerLimit; @@ -58,16 +61,23 @@ struct SliderJointInfo : public ConstraintInfo { /// Upper limit decimal upperLimit; - /// Constructor without limits + /// Motor speed + decimal motorSpeed; + + /// Maximum motor force (in Newton) that can be applied to reach to desired motor speed + decimal maxMotorForce; + + /// Constructor without limits and without motor SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, const Vector3& initAnchorPointWorldSpace, const Vector3& initSliderAxisWorldSpace) : ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT), anchorPointWorldSpace(initAnchorPointWorldSpace), sliderAxisWorldSpace(initSliderAxisWorldSpace), - isLimitsActive(false), lowerLimit(-1.0), upperLimit(1.0) {} + isLimitEnabled(false), isMotorEnabled(false), lowerLimit(-1.0), + upperLimit(1.0), motorSpeed(0), maxMotorForce(0) {} - /// Constructor with limits + /// Constructor with limits and no motor SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, const Vector3& initAnchorPointWorldSpace, const Vector3& initSliderAxisWorldSpace, @@ -75,8 +85,21 @@ struct SliderJointInfo : public ConstraintInfo { : ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT), anchorPointWorldSpace(initAnchorPointWorldSpace), sliderAxisWorldSpace(initSliderAxisWorldSpace), - isLimitsActive(true), lowerLimit(initLowerLimit), - upperLimit(initUpperLimit) {} + isLimitEnabled(true), isMotorEnabled(false), lowerLimit(initLowerLimit), + upperLimit(initUpperLimit), motorSpeed(0), maxMotorForce(0) {} + + /// Constructor with limits and motor + SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, + const Vector3& initAnchorPointWorldSpace, + const Vector3& initSliderAxisWorldSpace, + decimal initLowerLimit, decimal initUpperLimit, + decimal initMotorSpeed, decimal initMaxMotorForce) + : ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT), + anchorPointWorldSpace(initAnchorPointWorldSpace), + sliderAxisWorldSpace(initSliderAxisWorldSpace), + isLimitEnabled(true), isMotorEnabled(true), lowerLimit(initLowerLimit), + upperLimit(initUpperLimit), motorSpeed(initMotorSpeed), + maxMotorForce(initMaxMotorForce) {} }; // Class SliderJoint @@ -87,6 +110,11 @@ class SliderJoint : public Constraint { private : + // -------------------- Constants -------------------- // + + // Beta value for the position correction bias factor + static const decimal BETA; + // -------------------- Attributes -------------------- // /// Anchor point of body 1 (in local-space coordinates of body 1) @@ -152,6 +180,9 @@ class SliderJoint : public Constraint { /// Inverse of mass matrix K=JM^-1J^t for the upper and lower limit constraints (1x1 matrix) decimal mInverseMassMatrixLimit; + /// Inverse of mass matrix K=JM^-1J^t for the motor + decimal mInverseMassMatrixMotor; + /// Impulse for the 2 translation constraints Vector2 mImpulseTranslation; @@ -164,8 +195,14 @@ class SliderJoint : public Constraint { /// Impulse for the upper limit constraint decimal mImpulseUpperLimit; - /// True if the slider limits are active - bool mIsLimitsActive; + /// Impulse for the motor + decimal mImpulseMotor; + + /// True if the slider limits are enabled + bool mIsLimitEnabled; + + /// True if the motor of the joint in enabled + bool mIsMotorEnabled; /// Slider axis in world-space coordinates Vector3 mSliderAxisWorld; @@ -182,6 +219,17 @@ class SliderJoint : public Constraint { /// True if the upper limit is violated bool mIsUpperLimitViolated; + /// Motor speed + decimal mMotorSpeed; + + /// Maximum motor force (in Newton) that can be applied to reach to desired motor speed + decimal mMaxMotorForce; + + // -------------------- Methods -------------------- // + + /// Reset the limits + void resetLimits(); + public : // -------------------- Methods -------------------- // @@ -192,6 +240,45 @@ class SliderJoint : public Constraint { /// Destructor virtual ~SliderJoint(); + /// Return true if the limits or the joint are enabled + bool isLimitEnabled() const; + + /// Return true if the motor of the joint is enabled + bool isMotorEnabled() const; + + /// Enable/Disable the limits of the joint + void enableLimit(bool isLimitEnabled); + + /// Enable/Disable the motor of the joint + void enableMotor(bool isMotorEnabled); + + /// Return the lower limit + decimal getLowerLimit() const; + + /// Set the lower limit + void setLowerLimit(decimal lowerLimit); + + /// Return the upper limit + decimal getUpperLimit() const; + + /// Set the upper limit + void setUpperLimit(decimal upperLimit); + + /// Return the motor speed + decimal getMotorSpeed() const; + + /// Set the motor speed + void setMotorSpeed(decimal motorSpeed); + + /// Return the maximum motor force + decimal getMaxMotorForce() const; + + /// Set the maximum motor force + void setMaxMotorForce(decimal maxMotorForce); + + /// Return the intensity of the current force applied for the joint motor + decimal getMotorForce(decimal timeStep) const; + /// Return the number of bytes used by the joint virtual size_t getSizeInBytes() const; @@ -208,6 +295,41 @@ class SliderJoint : public Constraint { virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); }; +// Return true if the limits or the joint are enabled +inline bool SliderJoint::isLimitEnabled() const { + return mIsLimitEnabled; +} + +// Return true if the motor of the joint is enabled +inline bool SliderJoint::isMotorEnabled() const { + return mIsMotorEnabled; +} + +// Return the lower limit +inline decimal SliderJoint::getLowerLimit() const { + return mLowerLimit; +} + +// Return the upper limit +inline decimal SliderJoint::getUpperLimit() const { + return mUpperLimit; +} + +// Return the motor speed +inline decimal SliderJoint::getMotorSpeed() const { + return mMotorSpeed; +} + +// Return the maximum motor force +inline decimal SliderJoint::getMaxMotorForce() const { + return mMaxMotorForce; +} + +// Return the intensity of the current force applied for the joint motor +inline decimal SliderJoint::getMotorForce(decimal timeStep) const { + return mImpulseMotor / timeStep; +} + // Return the number of bytes used by the joint inline size_t SliderJoint::getSizeInBytes() const { return sizeof(SliderJoint); From c4d6206ee240c69fd0991013e43381c916ab1648 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 9 Jun 2013 16:31:01 +0200 Subject: [PATCH 21/24] Finish the implementation of the Hinge joint and some others modifications --- .../narrowphase/EPA/EPAAlgorithm.cpp | 2 +- src/configuration.h | 3 + src/constraint/BallAndSocketJoint.cpp | 4 +- src/constraint/HingeJoint.cpp | 591 ++++++++++++++++++ src/constraint/HingeJoint.h | 342 ++++++++++ src/constraint/SliderJoint.cpp | 28 +- src/constraint/SliderJoint.h | 69 +- src/engine/ConstraintSolver.cpp | 7 +- src/engine/DynamicsWorld.cpp | 12 +- src/mathematics/Quaternion.h | 46 +- src/mathematics/mathematics_functions.h | 8 + src/reactphysics3d.h | 1 + test/tests/mathematics/TestQuaternion.h | 15 +- 13 files changed, 1070 insertions(+), 58 deletions(-) create mode 100644 src/constraint/HingeJoint.cpp create mode 100644 src/constraint/HingeJoint.h diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp index b6c70ba5..2cdfc87b 100644 --- a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp +++ b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp @@ -143,7 +143,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple int minAxis = d.getAbsoluteVector().getMinAxis(); // Compute sin(60) - const decimal sin60 = sqrt(3.0) * 0.5; + const decimal sin60 = sqrt(3.0) * decimal(0.5); // Create a rotation quaternion to rotate the vector v1 to get the vectors // v2 and v3 diff --git a/src/configuration.h b/src/configuration.h index eae264f9..83e3c817 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -81,6 +81,9 @@ const decimal MACHINE_EPSILON = std::numeric_limits::epsilon(); /// Pi constant const decimal PI = decimal(3.14159265); +/// 2*Pi constant +const decimal PI_TIMES_2 = decimal(6.28318530); + /// Default internal constant timestep in seconds const decimal DEFAULT_TIMESTEP = decimal(1.0 / 60.0); diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index edec02b1..4a8db2ce 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -27,10 +27,12 @@ #include "BallAndSocketJoint.h" #include "../engine/ConstraintSolver.h" +// TODO : Solve 2x2 or 3x3 linear systems without inverting the A matrix (direct resolution) + using namespace reactphysics3d; // Static variables definition -const decimal BallAndSocketJoint::BETA = 0.2; +const decimal BallAndSocketJoint::BETA = decimal(0.2); // Constructor BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo) diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp new file mode 100644 index 00000000..aa0da0eb --- /dev/null +++ b/src/constraint/HingeJoint.cpp @@ -0,0 +1,591 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2013 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. * +* * +********************************************************************************/ + +// Libraries +#include "HingeJoint.h" +#include "../engine/ConstraintSolver.h" +#include + +// TODO : Solve 2x2 or 3x3 linear systems without inverting the A matrix (direct resolution) + +using namespace reactphysics3d; + +// Static variables definition +const decimal HingeJoint::BETA = decimal(0.2); + +// Constructor +HingeJoint::HingeJoint(const HingeJointInfo& jointInfo) + : Constraint(jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0), + mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0), + mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled), + mLowerLimit(jointInfo.minAngleLimit), mUpperLimit(jointInfo.maxAngleLimit), + mIsLowerLimitViolated(false), mIsUpperLimitViolated(false), + mMotorSpeed(jointInfo.motorSpeed), mMaxMotorForce(jointInfo.maxMotorForce) { + + assert(mLowerLimit <= 0 && mLowerLimit >= -2.0 * PI); + assert(mUpperLimit >= 0 && mUpperLimit <= 2.0 * PI); + + // Compute the local-space anchor point for each body + Transform transform1 = mBody1->getTransform(); + Transform transform2 = mBody2->getTransform(); + mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; + mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; + + // Compute the local-space hinge axis + mHingeLocalAxisBody1 = transform1.getOrientation().getInverse() * jointInfo.rotationAxisWorld; + mHingeLocalAxisBody2 = transform2.getOrientation().getInverse() * jointInfo.rotationAxisWorld; + mHingeLocalAxisBody1.normalize(); + mHingeLocalAxisBody2.normalize(); + + // Compute the inverse of the initial orientation difference between the two bodies + mInitOrientationDifferenceInv = transform2.getOrientation() * + transform1.getOrientation().getInverse(); + mInitOrientationDifferenceInv.normalize(); + mInitOrientationDifferenceInv.inverse(); +} + +// Destructor +HingeJoint::~HingeJoint() { + +} + +// Initialize before solving the constraint +void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { + + // Initialize the bodies index in the velocity array + mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; + mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; + + // Get the bodies positions and orientations + const Vector3& x1 = mBody1->getTransform().getPosition(); + const Vector3& x2 = mBody2->getTransform().getPosition(); + const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); + const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); + + // Get the inertia tensor of bodies + const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); + const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); + + // Compute the vector from body center to the anchor point in world-space + mR1World = orientationBody1 * mLocalAnchorPointBody1; + mR2World = orientationBody2 * mLocalAnchorPointBody2; + + // Compute the current angle around the hinge axis + decimal hingeAngle = computeCurrentHingeAngle(orientationBody1, orientationBody2); + + // Check if the limit constraints are violated or not + decimal lowerLimitError = hingeAngle - mLowerLimit; + decimal upperLimitError = mUpperLimit - hingeAngle; + bool oldIsLowerLimitViolated = mIsLowerLimitViolated; + mIsLowerLimitViolated = lowerLimitError <= 0; + if (mIsLowerLimitViolated != oldIsLowerLimitViolated) { + mImpulseLowerLimit = 0.0; + } + bool oldIsUpperLimitViolated = mIsUpperLimitViolated; + mIsUpperLimitViolated = upperLimitError <= 0; + if (mIsUpperLimitViolated != oldIsUpperLimitViolated) { + mImpulseUpperLimit = 0.0; + } + + decimal testAngle = computeCurrentHingeAngle(orientationBody1, orientationBody2); + + // Compute vectors needed in the Jacobian + mA1 = orientationBody1 * mHingeLocalAxisBody1; + Vector3 a2 = orientationBody2 * mHingeLocalAxisBody2; + mA1.normalize(); + a2.normalize(); + const Vector3 b2 = a2.getOneUnitOrthogonalVector(); + const Vector3 c2 = a2.cross(b2); + mB2CrossA1 = b2.cross(mA1); + mC2CrossA1 = c2.cross(mA1); + + // Compute the corresponding skew-symmetric matrices + Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); + Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); + + // Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix) + decimal inverseMassBodies = 0.0; + if (mBody1->getIsMotionEnabled()) { + inverseMassBodies += mBody1->getMassInverse(); + } + if (mBody2->getIsMotionEnabled()) { + inverseMassBodies += mBody2->getMassInverse(); + } + Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, + 0, inverseMassBodies, 0, + 0, 0, inverseMassBodies); + if (mBody1->getIsMotionEnabled()) { + massMatrix += skewSymmetricMatrixU1 * I1 * skewSymmetricMatrixU1.getTranspose(); + } + if (mBody2->getIsMotionEnabled()) { + massMatrix += skewSymmetricMatrixU2 * I2 * skewSymmetricMatrixU2.getTranspose(); + } + mInverseMassMatrixTranslation.setToZero(); + if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + mInverseMassMatrixTranslation = massMatrix.getInverse(); + } + + // Compute the bias "b" of the translation constraints + mBTranslation.setToZero(); + decimal biasFactor = (BETA / constraintSolverData.timeStep); + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + mBTranslation = biasFactor * (x2 + mR2World - x1 - mR1World); + } + + // Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix) + Vector3 I1B2CrossA1(0, 0, 0); + Vector3 I1C2CrossA1(0, 0, 0); + Vector3 I2B2CrossA1(0, 0, 0); + Vector3 I2C2CrossA1(0, 0, 0); + if (mBody1->getIsMotionEnabled()) { + I1B2CrossA1 = I1 * mB2CrossA1; + I1C2CrossA1 = I1 * mC2CrossA1; + } + if (mBody2->getIsMotionEnabled()) { + I2B2CrossA1 = I2 * mB2CrossA1; + I2C2CrossA1 = I2 * mC2CrossA1; + } + const decimal el11 = mB2CrossA1.dot(I1B2CrossA1) + + mB2CrossA1.dot(I2B2CrossA1); + const decimal el12 = mB2CrossA1.dot(I1C2CrossA1) + + mB2CrossA1.dot(I2C2CrossA1); + const decimal el21 = mC2CrossA1.dot(I1B2CrossA1) + + mC2CrossA1.dot(I2B2CrossA1); + const decimal el22 = mC2CrossA1.dot(I1C2CrossA1) + + mC2CrossA1.dot(I2C2CrossA1); + const Matrix2x2 matrixKRotation(el11, el12, el21, el22); + mInverseMassMatrixRotation.setToZero(); + if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + mInverseMassMatrixRotation = matrixKRotation.getInverse(); + } + + // Compute the bias "b" of the rotation constraints + mBRotation.setToZero(); + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + mBRotation = biasFactor * Vector2(mA1.dot(b2), mA1.dot(c2)); + } + + // If warm-starting is not enabled + if (!constraintSolverData.isWarmStartingActive) { + + // Reset all the accumulated impulses + mImpulseTranslation.setToZero(); + mImpulseRotation.setToZero(); + mImpulseLowerLimit = 0.0; + mImpulseUpperLimit = 0.0; + mImpulseMotor = 0.0; + } + + if (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated)) { + + // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) + mInverseMassMatrixLimitMotor = 0.0; + if (mBody1->getIsMotionEnabled()) { + mInverseMassMatrixLimitMotor += mA1.dot(I1 * mA1); + } + if (mBody2->getIsMotionEnabled()) { + mInverseMassMatrixLimitMotor += mA1.dot(I2 * mA1); + } + mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ? + decimal(1.0) / mInverseMassMatrixLimitMotor : decimal(0.0); + + // Compute the bias "b" of the lower limit constraint + mBLowerLimit = 0.0; + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + mBLowerLimit = biasFactor * lowerLimitError; + } + + // Compute the bias "b" of the upper limit constraint + mBUpperLimit = 0.0; + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + mBUpperLimit = biasFactor * upperLimitError; + } + } +} + +// Warm start the constraint (apply the previous impulse at the beginning of the step) +void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) { + + // Get the velocities + Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; + Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; + Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; + Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; + + // Get the inverse mass and inverse inertia tensors of the bodies + const decimal inverseMassBody1 = mBody1->getMassInverse(); + const decimal inverseMassBody2 = mBody2->getMassInverse(); + const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); + const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); + + // Compute the impulse P=J^T * lambda for the 3 translation constraints + Vector3 linearImpulseBody1 = -mImpulseTranslation; + Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World); + Vector3 linearImpulseBody2 = mImpulseTranslation; + Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World); + + // Compute the impulse P=J^T * lambda for the 2 rotation constraints + Vector3 rotationImpulse = -mB2CrossA1 * mImpulseRotation.x - mC2CrossA1 * mImpulseRotation.y; + angularImpulseBody1 += rotationImpulse; + angularImpulseBody2 += -rotationImpulse; + + // Compute the impulse P=J^T * lambda for the lower and upper limits constraints + const Vector3 limitsImpulse = (mImpulseUpperLimit - mImpulseLowerLimit) * mA1; + angularImpulseBody1 += limitsImpulse; + angularImpulseBody2 += -limitsImpulse; + + // Compute the impulse P=J^T * lambda for the motor constraint + const Vector3 motorImpulse = -mImpulseMotor * mA1; + angularImpulseBody1 += motorImpulse; + angularImpulseBody2 += -motorImpulse; + + // Apply the impulse to the bodies of the joint + if (mBody1->getIsMotionEnabled()) { + v1 += inverseMassBody1 * linearImpulseBody1; + w1 += I1 * angularImpulseBody1; + } + if (mBody2->getIsMotionEnabled()) { + v2 += inverseMassBody2 * linearImpulseBody2; + w2 += I2 * angularImpulseBody2; + } +} + +// Solve the velocity constraint +void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { + + // Get the velocities + Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; + Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; + Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; + Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; + + // Get the inverse mass and inverse inertia tensors of the bodies + decimal inverseMassBody1 = mBody1->getMassInverse(); + decimal inverseMassBody2 = mBody2->getMassInverse(); + Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); + Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); + + // --------------- Translation Constraints --------------- // + + // Compute J*v + const Vector3 JvTranslation = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World); + + // Compute the Lagrange multiplier lambda + const Vector3 deltaLambdaTranslation = mInverseMassMatrixTranslation * + (-JvTranslation - mBTranslation); + mImpulseTranslation += deltaLambdaTranslation; + + // Compute the impulse P=J^T * lambda + Vector3 linearImpulseBody1 = -deltaLambdaTranslation; + Vector3 angularImpulseBody1 = deltaLambdaTranslation.cross(mR1World); + Vector3 linearImpulseBody2 = deltaLambdaTranslation; + Vector3 angularImpulseBody2 = -deltaLambdaTranslation.cross(mR2World); + + // Apply the impulse to the bodies of the joint + if (mBody1->getIsMotionEnabled()) { + v1 += inverseMassBody1 * linearImpulseBody1; + w1 += I1 * angularImpulseBody1; + } + if (mBody2->getIsMotionEnabled()) { + v2 += inverseMassBody2 * linearImpulseBody2; + w2 += I2 * angularImpulseBody2; + } + + // --------------- Rotation Constraints --------------- // + + // Compute J*v for the 2 rotation constraints + const Vector2 JvRotation(-mB2CrossA1.dot(w1) + mB2CrossA1.dot(w2), + -mC2CrossA1.dot(w1) + mC2CrossA1.dot(w2)); + + // Compute the Lagrange multiplier lambda for the 2 rotation constraints + Vector2 deltaLambdaRotation = mInverseMassMatrixRotation * (-JvRotation - mBRotation); + mImpulseRotation += deltaLambdaRotation; + + // Compute the impulse P=J^T * lambda for the 2 rotation constraints + angularImpulseBody1 = -mB2CrossA1 * deltaLambdaRotation.x - mC2CrossA1 * deltaLambdaRotation.y; + angularImpulseBody2 = -angularImpulseBody1; + + // Apply the impulse to the bodies of the joint + if (mBody1->getIsMotionEnabled()) { + w1 += I1 * angularImpulseBody1; + } + if (mBody2->getIsMotionEnabled()) { + w2 += I2 * angularImpulseBody2; + } + + // --------------- Limits Constraints --------------- // + + if (mIsLimitEnabled) { + + // If the lower limit is violated + if (mIsLowerLimitViolated) { + + // Compute J*v for the lower limit constraint + const decimal JvLowerLimit = (w2 - w1).dot(mA1); + + // Compute the Lagrange multiplier lambda for the lower limit constraint + decimal deltaLambdaLower = mInverseMassMatrixLimitMotor * (-JvLowerLimit - mBLowerLimit); + decimal lambdaTemp = mImpulseLowerLimit; + mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0)); + deltaLambdaLower = mImpulseLowerLimit - lambdaTemp; + + // Compute the impulse P=J^T * lambda for the lower limit constraint + const Vector3 angularImpulseBody1 = -deltaLambdaLower * mA1; + const Vector3 angularImpulseBody2 = -angularImpulseBody1; + + // Apply the impulse to the bodies of the joint + if (mBody1->getIsMotionEnabled()) { + w1 += I1 * angularImpulseBody1; + } + if (mBody2->getIsMotionEnabled()) { + w2 += I2 * angularImpulseBody2; + } + } + + // If the upper limit is violated + if (mIsUpperLimitViolated) { + + // Compute J*v for the upper limit constraint + const decimal JvUpperLimit = -(w2 - w1).dot(mA1); + + // Compute the Lagrange multiplier lambda for the upper limit constraint + decimal deltaLambdaUpper = mInverseMassMatrixLimitMotor * (-JvUpperLimit -mBUpperLimit); + decimal lambdaTemp = mImpulseUpperLimit; + mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0)); + deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp; + + // Compute the impulse P=J^T * lambda for the upper limit constraint + const Vector3 angularImpulseBody1 = deltaLambdaUpper * mA1; + const Vector3 angularImpulseBody2 = -angularImpulseBody1; + + // Apply the impulse to the bodies of the joint + if (mBody1->getIsMotionEnabled()) { + w1 += I1 * angularImpulseBody1; + } + if (mBody2->getIsMotionEnabled()) { + w2 += I2 * angularImpulseBody2; + } + } + } + + // --------------- Motor --------------- // + + if (mIsMotorEnabled) { + + // Compute J*v for the motor + const decimal JvMotor = mA1.dot(w1 - w2); + + // Compute the Lagrange multiplier lambda for the motor + const decimal maxMotorImpulse = mMaxMotorForce * constraintSolverData.timeStep; + decimal deltaLambdaMotor = mInverseMassMatrixLimitMotor * (-JvMotor - mMotorSpeed); + decimal lambdaTemp = mImpulseMotor; + mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); + deltaLambdaMotor = mImpulseMotor - lambdaTemp; + + // Compute the impulse P=J^T * lambda for the motor + const Vector3 angularImpulseBody1 = -deltaLambdaMotor * mA1; + const Vector3 angularImpulseBody2 = -angularImpulseBody1; + + // Apply the impulse to the bodies of the joint + if (mBody1->getIsMotionEnabled()) { + w1 += I1 * angularImpulseBody1; + } + if (mBody2->getIsMotionEnabled()) { + w2 += I2 * angularImpulseBody2; + } + } +} + +// Solve the position constraint +void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) { + +} + + +// Enable/Disable the limits of the joint +void HingeJoint::enableLimit(bool isLimitEnabled) { + + if (isLimitEnabled != mIsLimitEnabled) { + + mIsLimitEnabled = isLimitEnabled; + + // Reset the limits + resetLimits(); + } +} + +// Enable/Disable the motor of the joint +void HingeJoint::enableMotor(bool isMotorEnabled) { + + mIsMotorEnabled = isMotorEnabled; + mImpulseMotor = 0.0; + + // TODO : Wake up the bodies of the joint here when sleeping is implemented +} + +// Set the minimum angle limit +void HingeJoint::setMinAngleLimit(decimal lowerLimit) { + + assert(mLowerLimit <= 0 && mLowerLimit >= -2.0 * PI); + + if (lowerLimit != mLowerLimit) { + + mLowerLimit = lowerLimit; + + // Reset the limits + resetLimits(); + } +} + +// Set the maximum angle limit +void HingeJoint::setMaxAngleLimit(decimal upperLimit) { + + assert(upperLimit >= 0 && upperLimit <= 2.0 * PI); + + if (upperLimit != mUpperLimit) { + + mUpperLimit = upperLimit; + + // Reset the limits + resetLimits(); + } +} + +// Reset the limits +void HingeJoint::resetLimits() { + + // Reset the accumulated impulses for the limits + mImpulseLowerLimit = 0.0; + mImpulseUpperLimit = 0.0; + + // TODO : Wake up the bodies of the joint here when sleeping is implemented +} + +// Set the motor speed +void HingeJoint::setMotorSpeed(decimal motorSpeed) { + + if (motorSpeed != mMotorSpeed) { + + mMotorSpeed = motorSpeed; + + // TODO : Wake up the bodies of the joint here when sleeping is implemented + } +} + +// Set the maximum motor force +void HingeJoint::setMaxMotorForce(decimal maxMotorForce) { + + if (maxMotorForce != mMaxMotorForce) { + + assert(mMaxMotorForce >= 0.0); + mMaxMotorForce = maxMotorForce; + + // TODO : Wake up the bodies of the joint here when sleeping is implemented + } +} + +// Given an angle in radian, this method returns the corresponding angle in the range [-pi; pi] +decimal HingeJoint::computeNormalizedAngle(decimal angle) const { + + // Convert it into the range [-2*pi; 2*pi] + angle = fmod(angle, PI_TIMES_2); + + // Convert it into the range [-pi; pi] + if (angle < -PI) { + return angle + PI_TIMES_2; + } + else if (angle > PI) { + return angle - PI_TIMES_2; + } + else { + return angle; + } +} + +// Given an "inputAngle" in the range [-pi, pi], this method returns an +// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the +// two angle limits in arguments. +decimal HingeJoint::computeCorrespondingAngleNearLimits(decimal inputAngle, decimal lowerLimitAngle, + decimal upperLimitAngle) const { + if (upperLimitAngle <= lowerLimitAngle) { + return inputAngle; + } + else if (inputAngle > upperLimitAngle) { + decimal diffToUpperLimit = fabs(computeNormalizedAngle(inputAngle - upperLimitAngle)); + decimal diffToLowerLimit = fabs(computeNormalizedAngle(inputAngle - lowerLimitAngle)); + return (diffToUpperLimit > diffToLowerLimit) ? (inputAngle - PI_TIMES_2) : inputAngle; + } + else if (inputAngle < lowerLimitAngle) { + decimal diffToUpperLimit = fabs(computeNormalizedAngle(upperLimitAngle - inputAngle)); + decimal diffToLowerLimit = fabs(computeNormalizedAngle(lowerLimitAngle - inputAngle)); + return (diffToUpperLimit > diffToLowerLimit) ? inputAngle : (inputAngle + PI_TIMES_2); + } + else { + return inputAngle; + } +} + +// Compute the current angle around the hinge axis +decimal HingeJoint::computeCurrentHingeAngle(const Quaternion& orientationBody1, + const Quaternion& orientationBody2) { + + decimal hingeAngle; + + // Compute the current orientation difference between the two bodies + Quaternion currentOrientationDiff = orientationBody2 * orientationBody1.getInverse(); + currentOrientationDiff.normalize(); + + // Compute the relative rotation considering the initial orientation difference + Quaternion relativeRotation = currentOrientationDiff * mInitOrientationDifferenceInv; + relativeRotation.normalize(); + + // A quaternion q = [cos(theta/2); sin(theta/2) * rotAxis] where rotAxis is a unit + // length vector. We can extract cos(theta/2) with q.w and we can extract |sin(theta/2)| with : + // |sin(theta/2)| = q.getVectorV().length() since rotAxis is unit length. Note that any + // rotation can be represented by a quaternion q and -q. Therefore, if the relative rotation + // axis is not pointing in the same direction as the hinge axis, we use the rotation -q which + // has the same |sin(theta/2)| value but the value cos(theta/2) is sign inverted. Some details + // about this trick is explained in the source code of OpenTissue (http://www.opentissue.org). + decimal cosHalfAngle = relativeRotation.w; + decimal sinHalfAngleAbs = relativeRotation.getVectorV().length(); + + // Compute the dot product of the relative rotation axis and the hinge axis + decimal dotProduct = relativeRotation.getVectorV().dot(mA1); + + // If the relative rotation axis and the hinge axis are pointing the same direction + if (dotProduct >= decimal(0.0)) { + hingeAngle = decimal(2.0) * std::atan2(sinHalfAngleAbs, cosHalfAngle); + } + else { + hingeAngle = decimal(2.0) * std::atan2(sinHalfAngleAbs, -cosHalfAngle); + } + + // Convert the angle from range [-2*pi; 2*pi] into the range [-pi; pi] + hingeAngle = computeNormalizedAngle(hingeAngle); + + // Compute and return the corresponding angle near one the two limits + return computeCorrespondingAngleNearLimits(hingeAngle, mLowerLimit, mUpperLimit); +} + diff --git a/src/constraint/HingeJoint.h b/src/constraint/HingeJoint.h new file mode 100644 index 00000000..aa258159 --- /dev/null +++ b/src/constraint/HingeJoint.h @@ -0,0 +1,342 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2013 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 REACTPHYSICS3D_HINGE_JOINT_H +#define REACTPHYSICS3D_HINGE_JOINT_H + +// Libraries +#include "Constraint.h" +#include "../mathematics/mathematics.h" + +namespace reactphysics3d { + +// Structure HingeJointInfo +/** + * This structure is used to gather the information needed to create a hinge joint. + * This structure will be used to create the actual hinge joint. + */ +struct HingeJointInfo : public ConstraintInfo { + + public : + + // -------------------- Attributes -------------------- // + + /// Anchor point (in world-space coordinates) + Vector3 anchorPointWorldSpace; + + /// Hinge rotation axis (in world-space coordinates) + Vector3 rotationAxisWorld; + + /// True if the slider limits are enabled + bool isLimitEnabled; + + /// True if the slider motor is enabled + bool isMotorEnabled; + + /// Minimum allowed rotation angle (in radian) if limits are enabled. + /// The angle must be in the range [-2*pi, 0] + decimal minAngleLimit; + + /// Maximum allowed rotation angle (in radian) if limits are enabled. + /// The angle must be in the range [0, 2*pi] + decimal maxAngleLimit; + + /// Motor speed (in radian/second) + decimal motorSpeed; + + /// Maximum motor force (in Newton) that can be applied to reach to desired motor speed + decimal maxMotorForce; + + /// Constructor without limits and without motor + HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, + const Vector3& initAnchorPointWorldSpace, + const Vector3& initRotationAxisWorld) + : ConstraintInfo(rigidBody1, rigidBody2, HINGEJOINT), + anchorPointWorldSpace(initAnchorPointWorldSpace), + rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(false), + isMotorEnabled(false), minAngleLimit(-1), maxAngleLimit(1), motorSpeed(0), + maxMotorForce(0) {} + + /// Constructor with limits but without motor + HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, + const Vector3& initAnchorPointWorldSpace, + const Vector3& initRotationAxisWorld, + decimal initMinAngleLimit, decimal initMaxAngleLimit) + : ConstraintInfo(rigidBody1, rigidBody2, HINGEJOINT), + anchorPointWorldSpace(initAnchorPointWorldSpace), + rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true), + isMotorEnabled(false), minAngleLimit(initMinAngleLimit), + maxAngleLimit(initMaxAngleLimit), motorSpeed(0), maxMotorForce(0) {} + + /// Constructor with limits and motor + HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, + const Vector3& initAnchorPointWorldSpace, + const Vector3& initRotationAxisWorld, + decimal initMinAngleLimit, decimal initMaxAngleLimit, + decimal initMotorSpeed, decimal initMaxMotorForce) + : ConstraintInfo(rigidBody1, rigidBody2, HINGEJOINT), + anchorPointWorldSpace(initAnchorPointWorldSpace), + rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true), + isMotorEnabled(false), minAngleLimit(initMinAngleLimit), + maxAngleLimit(initMaxAngleLimit), motorSpeed(initMotorSpeed), + maxMotorForce(initMaxMotorForce) {} +}; + +// Class HingeJoint +/** + * This class represents a hinge joint that allows arbitrary rotation + * between two bodies around a single axis. + */ +class HingeJoint : public Constraint { + + private : + + // -------------------- Constants -------------------- // + + // Beta value for the bias factor of position correction + static const decimal BETA; + + // -------------------- Attributes -------------------- // + + /// Anchor point of body 1 (in local-space coordinates of body 1) + Vector3 mLocalAnchorPointBody1; + + /// Anchor point of body 2 (in local-space coordinates of body 2) + Vector3 mLocalAnchorPointBody2; + + /// Hinge rotation axis (in local-space coordinates of body 1) + Vector3 mHingeLocalAxisBody1; + + /// Hinge rotation axis (in local-space coordiantes of body 2) + Vector3 mHingeLocalAxisBody2; + + /// Hinge rotation axis (in world-space coordinates) computed from body 1 + Vector3 mA1; + + /// Vector from center of body 2 to anchor point in world-space + Vector3 mR1World; + + /// Vector from center of body 2 to anchor point in world-space + Vector3 mR2World; + + /// Cross product of vector b2 and a1 + Vector3 mB2CrossA1; + + /// Cross product of vector c2 and a1; + Vector3 mC2CrossA1; + + /// Impulse for the 3 translation constraints + Vector3 mImpulseTranslation; + + /// Impulse for the 2 rotation constraints + Vector2 mImpulseRotation; + + /// Accumulated impulse for the lower limit constraint + decimal mImpulseLowerLimit; + + /// Accumulated impulse for the upper limit constraint + decimal mImpulseUpperLimit; + + /// Accumulated impulse for the motor constraint; + decimal mImpulseMotor; + + /// Inverse mass matrix K=JM^-1J^t for the 3 translation constraints + Matrix3x3 mInverseMassMatrixTranslation; + + /// Inverse mass matrix K=JM^-1J^t for the 2 rotation constraints + Matrix2x2 mInverseMassMatrixRotation; + + /// Inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix) + decimal mInverseMassMatrixLimitMotor; + + /// Inverse of mass matrix K=JM^-1J^t for the motor + decimal mInverseMassMatrixMotor; + + /// Bias vector for the error correction for the translation constraints + Vector3 mBTranslation; + + /// Bias vector for the error correction for the rotation constraints + Vector2 mBRotation; + + /// Bias of the lower limit constraint + decimal mBLowerLimit; + + /// Bias of the upper limit constraint + decimal mBUpperLimit; + + /// Inverse of the initial orientation difference between the bodies + Quaternion mInitOrientationDifferenceInv; + + /// True if the joint limits are enabled + bool mIsLimitEnabled; + + /// True if the motor of the joint in enabled + bool mIsMotorEnabled; + + /// Lower limit (minimum allowed rotation angle in radi) + decimal mLowerLimit; + + /// Upper limit (maximum translation distance) + decimal mUpperLimit; + + /// True if the lower limit is violated + bool mIsLowerLimitViolated; + + /// True if the upper limit is violated + bool mIsUpperLimitViolated; + + /// Motor speed + decimal mMotorSpeed; + + /// Maximum motor force (in Newton) that can be applied to reach to desired motor speed + decimal mMaxMotorForce; + + // -------------------- Methods -------------------- // + + /// Reset the limits + void resetLimits(); + + /// Given an angle in radian, this method returns the corresponding angle in the range [-pi; pi] + decimal computeNormalizedAngle(decimal angle) const; + + /// Given an "inputAngle" in the range [-pi, pi], this method returns an + /// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the + /// two angle limits in arguments. + decimal computeCorrespondingAngleNearLimits(decimal inputAngle, decimal lowerLimitAngle, + decimal upperLimitAngle) const; + + /// Compute the current angle around the hinge axis + decimal computeCurrentHingeAngle(const Quaternion& orientationBody1, + const Quaternion& orientationBody2); + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + HingeJoint(const HingeJointInfo& jointInfo); + + /// Destructor + virtual ~HingeJoint(); + + /// Return true if the limits or the joint are enabled + bool isLimitEnabled() const; + + /// Return true if the motor of the joint is enabled + bool isMotorEnabled() const; + + /// Enable/Disable the limits of the joint + void enableLimit(bool isLimitEnabled); + + /// Enable/Disable the motor of the joint + void enableMotor(bool isMotorEnabled); + + /// Return the minimum angle limit + decimal getMinAngleLimit() const; + + /// Set the minimum angle limit + void setMinAngleLimit(decimal lowerLimit); + + /// Return the maximum angle limit + decimal getMaxAngleLimit() const; + + /// Set the maximum angle limit + void setMaxAngleLimit(decimal upperLimit); + + /// Return the motor speed + decimal getMotorSpeed() const; + + /// Set the motor speed + void setMotorSpeed(decimal motorSpeed); + + /// Return the maximum motor force + decimal getMaxMotorForce() const; + + /// Set the maximum motor force + void setMaxMotorForce(decimal maxMotorForce); + + /// Return the intensity of the current force applied for the joint motor + decimal getMotorForce(decimal timeStep) const; + + /// Return the number of bytes used by the joint + virtual size_t getSizeInBytes() const; + + /// Initialize before solving the constraint + virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); + + /// Warm start the constraint (apply the previous impulse at the beginning of the step) + virtual void warmstart(const ConstraintSolverData& constraintSolverData); + + /// Solve the velocity constraint + virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); + + /// Solve the position constraint + virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); +}; + +// Return true if the limits or the joint are enabled +inline bool HingeJoint::isLimitEnabled() const { + return mIsLimitEnabled; +} + +// Return true if the motor of the joint is enabled +inline bool HingeJoint::isMotorEnabled() const { + return mIsMotorEnabled; +} + +// Return the minimum angle limit +inline decimal HingeJoint::getMinAngleLimit() const { + return mLowerLimit; +} + +// Return the maximum angle limit +inline decimal HingeJoint::getMaxAngleLimit() const { + return mUpperLimit; +} + +// Return the motor speed +inline decimal HingeJoint::getMotorSpeed() const { + return mMotorSpeed; +} + +// Return the maximum motor force +inline decimal HingeJoint::getMaxMotorForce() const { + return mMaxMotorForce; +} + +// Return the intensity of the current force applied for the joint motor +inline decimal HingeJoint::getMotorForce(decimal timeStep) const { + return mImpulseMotor / timeStep; +} + +// Return the number of bytes used by the joint +inline size_t HingeJoint::getSizeInBytes() const { + return sizeof(HingeJoint); +} + +} + + +#endif diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index 7825a1fc..dc148f6f 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -26,6 +26,8 @@ // Libraries #include "SliderJoint.h" +// TODO : Solve 2x2 or 3x3 linear systems without inverting the A matrix (direct resolution) + using namespace reactphysics3d; // Static variables definition @@ -36,7 +38,7 @@ SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) : Constraint(jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0), mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0), mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled), - mLowerLimit(jointInfo.lowerLimit), mUpperLimit(jointInfo.upperLimit), + mLowerLimit(jointInfo.minTranslationLimit), mUpperLimit(jointInfo.maxTranslationLimit), mIsLowerLimitViolated(false), mIsUpperLimitViolated(false), mMotorSpeed(jointInfo.motorSpeed), mMaxMotorForce(jointInfo.maxMotorForce){ @@ -50,10 +52,11 @@ SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; - // Compute the initial orientation difference between the two bodies - mInitOrientationDifference = transform2.getOrientation() * + // Compute the inverse of the initial orientation difference between the two bodies + mInitOrientationDifferenceInv = transform2.getOrientation() * transform1.getOrientation().getInverse(); - mInitOrientationDifference.normalize(); + mInitOrientationDifferenceInv.normalize(); + mInitOrientationDifferenceInv.inverse(); // Compute the slider axis in local-space of body 1 mSliderAxisBody1 = mBody1->getTransform().getOrientation().getInverse() * @@ -87,7 +90,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa mR1 = orientationBody1 * mLocalAnchorPointBody1; mR2 = orientationBody2 * mLocalAnchorPointBody2; - // Compute the vector u + // Compute the vector u (difference between anchor points) const Vector3 u = x2 + mR2 - x1 - mR1; // Compute the two orthogonal vectors to the slider axis in world-space @@ -178,14 +181,13 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); currentOrientationDifference.normalize(); - const Quaternion qError = currentOrientationDifference * - mInitOrientationDifference.getInverse(); + const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv; mBRotation = biasFactor * decimal(2.0) * qError.getVectorV(); } if (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated)) { - // Compute the inverse of the mass matrix K=JM^-1J^t for the lower limit (1x1 matrix) + // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) mInverseMassMatrixLimit = 0.0; if (mBody1->getIsMotionEnabled()) { mInverseMassMatrixLimit += mBody1->getMassInverse() + @@ -424,7 +426,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint // Compute the Lagrange multiplier lambda for the motor const decimal maxMotorImpulse = mMaxMotorForce * constraintSolverData.timeStep; - decimal deltaLambdaMotor = mInverseMassMatrixMotor * (-JvMotor -mMotorSpeed); + decimal deltaLambdaMotor = mInverseMassMatrixMotor * (-JvMotor - mMotorSpeed); decimal lambdaTemp = mImpulseMotor; mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); deltaLambdaMotor = mImpulseMotor - lambdaTemp; @@ -469,8 +471,8 @@ void SliderJoint::enableMotor(bool isMotorEnabled) { // TODO : Wake up the bodies of the joint here when sleeping is implemented } -// Set the lower limit -void SliderJoint::setLowerLimit(decimal lowerLimit) { +// Set the minimum translation limit +void SliderJoint::setMinTranslationLimit(decimal lowerLimit) { assert(lowerLimit <= mUpperLimit); @@ -483,8 +485,8 @@ void SliderJoint::setLowerLimit(decimal lowerLimit) { } } -// Set the upper limit -void SliderJoint::setUpperLimit(decimal upperLimit) { +// Set the maximum translation limit +void SliderJoint::setMaxTranslationLimit(decimal upperLimit) { assert(mLowerLimit <= upperLimit); diff --git a/src/constraint/SliderJoint.h b/src/constraint/SliderJoint.h index fd54b5cb..79834322 100644 --- a/src/constraint/SliderJoint.h +++ b/src/constraint/SliderJoint.h @@ -55,11 +55,11 @@ struct SliderJointInfo : public ConstraintInfo { /// True if the slider motor is enabled bool isMotorEnabled; - /// Lower limit - decimal lowerLimit; + /// Mininum allowed translation if limits are enabled + decimal minTranslationLimit; - /// Upper limit - decimal upperLimit; + /// Maximum allowed translation if limits are enabled + decimal maxTranslationLimit; /// Motor speed decimal motorSpeed; @@ -74,31 +74,34 @@ struct SliderJointInfo : public ConstraintInfo { : ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT), anchorPointWorldSpace(initAnchorPointWorldSpace), sliderAxisWorldSpace(initSliderAxisWorldSpace), - isLimitEnabled(false), isMotorEnabled(false), lowerLimit(-1.0), - upperLimit(1.0), motorSpeed(0), maxMotorForce(0) {} + isLimitEnabled(false), isMotorEnabled(false), minTranslationLimit(-1.0), + maxTranslationLimit(1.0), motorSpeed(0), maxMotorForce(0) {} /// Constructor with limits and no motor SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, const Vector3& initAnchorPointWorldSpace, const Vector3& initSliderAxisWorldSpace, - decimal initLowerLimit, decimal initUpperLimit) + decimal initMinTranslationLimit, decimal initMaxTranslationLimit) : ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT), anchorPointWorldSpace(initAnchorPointWorldSpace), sliderAxisWorldSpace(initSliderAxisWorldSpace), - isLimitEnabled(true), isMotorEnabled(false), lowerLimit(initLowerLimit), - upperLimit(initUpperLimit), motorSpeed(0), maxMotorForce(0) {} + isLimitEnabled(true), isMotorEnabled(false), + minTranslationLimit(initMinTranslationLimit), + maxTranslationLimit(initMaxTranslationLimit), motorSpeed(0), + maxMotorForce(0) {} /// Constructor with limits and motor SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, const Vector3& initAnchorPointWorldSpace, const Vector3& initSliderAxisWorldSpace, - decimal initLowerLimit, decimal initUpperLimit, + decimal initMinTranslationLimit, decimal initMaxTranslationLimit, decimal initMotorSpeed, decimal initMaxMotorForce) : ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT), anchorPointWorldSpace(initAnchorPointWorldSpace), sliderAxisWorldSpace(initSliderAxisWorldSpace), - isLimitEnabled(true), isMotorEnabled(true), lowerLimit(initLowerLimit), - upperLimit(initUpperLimit), motorSpeed(initMotorSpeed), + isLimitEnabled(true), isMotorEnabled(true), + minTranslationLimit(initMinTranslationLimit), + maxTranslationLimit(initMaxTranslationLimit), motorSpeed(initMotorSpeed), maxMotorForce(initMaxMotorForce) {} }; @@ -126,8 +129,8 @@ class SliderJoint : public Constraint { /// Slider axis (in local-space coordinates of body 1) Vector3 mSliderAxisBody1; - /// Initial orientation difference between the two bodies - Quaternion mInitOrientationDifference; + /// Inverse of the initial orientation difference between the two bodies + Quaternion mInitOrientationDifferenceInv; /// First vector orthogonal to the slider axis local-space of body 1 Vector3 mN1; @@ -183,19 +186,19 @@ class SliderJoint : public Constraint { /// Inverse of mass matrix K=JM^-1J^t for the motor decimal mInverseMassMatrixMotor; - /// Impulse for the 2 translation constraints + /// Accumulated impulse for the 2 translation constraints Vector2 mImpulseTranslation; - /// Impulse for the 3 rotation constraints + /// Accumulated impulse for the 3 rotation constraints Vector3 mImpulseRotation; - /// Impulse for the lower limit constraint + /// Accumulated impulse for the lower limit constraint decimal mImpulseLowerLimit; - /// Impulse for the upper limit constraint + /// Accumulated impulse for the upper limit constraint decimal mImpulseUpperLimit; - /// Impulse for the motor + /// Accumulated impulse for the motor decimal mImpulseMotor; /// True if the slider limits are enabled @@ -207,10 +210,10 @@ class SliderJoint : public Constraint { /// Slider axis in world-space coordinates Vector3 mSliderAxisWorld; - /// Lower limit + /// Lower limit (minimum translation distance) decimal mLowerLimit; - /// Upper limit + /// Upper limit (maximum translation distance) decimal mUpperLimit; /// True if the lower limit is violated @@ -252,17 +255,17 @@ class SliderJoint : public Constraint { /// Enable/Disable the motor of the joint void enableMotor(bool isMotorEnabled); - /// Return the lower limit - decimal getLowerLimit() const; + /// Return the minimum translation limit + decimal getMinTranslationLimit() const; - /// Set the lower limit - void setLowerLimit(decimal lowerLimit); + /// Set the minimum translation limit + void setMinTranslationLimit(decimal lowerLimit); - /// Return the upper limit - decimal getUpperLimit() const; + /// Return the maximum translation limit + decimal getMaxTranslationLimit() const; - /// Set the upper limit - void setUpperLimit(decimal upperLimit); + /// Set the maximum translation limit + void setMaxTranslationLimit(decimal upperLimit); /// Return the motor speed decimal getMotorSpeed() const; @@ -305,13 +308,13 @@ inline bool SliderJoint::isMotorEnabled() const { return mIsMotorEnabled; } -// Return the lower limit -inline decimal SliderJoint::getLowerLimit() const { +// Return the minimum translation limit +inline decimal SliderJoint::getMinTranslationLimit() const { return mLowerLimit; } -// Return the upper limit -inline decimal SliderJoint::getUpperLimit() const { +// Return the maximum translation limit +inline decimal SliderJoint::getMaxTranslationLimit() const { return mUpperLimit; } diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index f825e110..ff994a28 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -37,7 +37,7 @@ ConstraintSolver::ConstraintSolver(std::set& joints, : mJoints(joints), mLinearVelocities(linearVelocities), mAngularVelocities(angularVelocities), mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), - mIsWarmStartingActive(false), + mIsWarmStartingActive(true), mIsNonLinearGaussSeidelPositionCorrectionActive(false), mConstraintSolverData(linearVelocities, angularVelocities, mapBodyToVelocityIndex){ @@ -77,6 +77,11 @@ void ConstraintSolver::initialize(decimal dt) { // Initialize the constraint before solving it joint->initBeforeSolve(mConstraintSolverData); + + // Warm-start the constraint if warm-starting is enabled + if (mIsWarmStartingActive) { + joint->warmstart(mConstraintSolverData); + } } } diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 2c046e99..98a9966a 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -27,6 +27,7 @@ #include "DynamicsWorld.h" #include "constraint/BallAndSocketJoint.h" #include "constraint/SliderJoint.h" +#include "constraint/HingeJoint.h" // Namespaces using namespace reactphysics3d; @@ -212,7 +213,7 @@ void DynamicsWorld::integrateRigidBodiesVelocities() { mConstrainedLinearVelocities = std::vector(mRigidBodies.size(), Vector3(0, 0, 0)); mConstrainedAngularVelocities = std::vector(mRigidBodies.size(), Vector3(0, 0, 0)); - double dt = mTimer.getTimeStep(); + decimal dt = static_cast(mTimer.getTimeStep()); // Fill in the mapping of rigid body to their index in the constrained // velocities arrays @@ -413,6 +414,15 @@ Constraint* DynamicsWorld::createJoint(const ConstraintInfo& jointInfo) { break; } + // Hinge joint + case HINGEJOINT: + { + void* allocatedMemory = mMemoryAllocator.allocate(sizeof(HingeJoint)); + const HingeJointInfo& info = dynamic_cast(jointInfo); + newJoint = new (allocatedMemory) HingeJoint(info); + break; + } + default: { assert(false); diff --git a/src/mathematics/Quaternion.h b/src/mathematics/Quaternion.h index eb957db7..246d56b2 100644 --- a/src/mathematics/Quaternion.h +++ b/src/mathematics/Quaternion.h @@ -84,15 +84,24 @@ struct Quaternion { /// Set the quaternion to zero void setToZero(); + /// Set to the identity quaternion + void setToIdentity(); + /// Return the vector v=(x y z) of the quaternion Vector3 getVectorV() const; /// Return the length of the quaternion decimal length() const; + /// Return the square of the length of the quaternion + decimal lengthSquare() const; + /// Normalize the quaternion void normalize(); + /// Inverse the quaternion + void inverse(); + /// Return the unit quaternion Quaternion getUnit() const; @@ -156,6 +165,14 @@ inline void Quaternion::setToZero() { w = 0; } +// Set to the identity quaternion +inline void Quaternion::setToIdentity() { + x = 0; + y = 0; + z = 0; + w = 1; +} + // Return the vector v=(x y z) of the quaternion inline Vector3 Quaternion::getVectorV() const { @@ -168,6 +185,11 @@ inline decimal Quaternion::length() const { return sqrt(x*x + y*y + z*z + w*w); } +// Return the square of the length of the quaternion +inline decimal Quaternion::lengthSquare() const { + return x*x + y*y + z*z + w*w; +} + // Normalize the quaternion inline void Quaternion::normalize() { @@ -182,6 +204,21 @@ inline void Quaternion::normalize() { w /= l; } +// Inverse the quaternion +inline void Quaternion::inverse() { + + // Get the square length of the quaternion + decimal lengthSquareQuaternion = lengthSquare(); + + assert (lengthSquareQuaternion > MACHINE_EPSILON); + + // Compute and return the inverse quaternion + x /= -lengthSquareQuaternion; + y /= -lengthSquareQuaternion; + z /= -lengthSquareQuaternion; + w /= lengthSquareQuaternion; +} + // Return the unit quaternion inline Quaternion Quaternion::getUnit() const { decimal lengthQuaternion = length(); @@ -207,14 +244,13 @@ inline Quaternion Quaternion::getConjugate() const { // Return the inverse of the quaternion (inline) inline Quaternion Quaternion::getInverse() const { - decimal lengthQuaternion = length(); - lengthQuaternion = lengthQuaternion * lengthQuaternion; + decimal lengthSquareQuaternion = lengthSquare(); - assert (lengthQuaternion > MACHINE_EPSILON); + assert (lengthSquareQuaternion > MACHINE_EPSILON); // Compute and return the inverse quaternion - return Quaternion(-x / lengthQuaternion, -y / lengthQuaternion, - -z / lengthQuaternion, w / lengthQuaternion); + return Quaternion(-x / lengthSquareQuaternion, -y / lengthSquareQuaternion, + -z / lengthSquareQuaternion, w / lengthSquareQuaternion); } // Scalar product between two quaternions diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h index d5278abd..6ac380d8 100644 --- a/src/mathematics/mathematics_functions.h +++ b/src/mathematics/mathematics_functions.h @@ -29,6 +29,7 @@ // Libraries #include "../configuration.h" #include "../decimal.h" +#include /// ReactPhysics3D namespace namespace reactphysics3d { @@ -43,6 +44,13 @@ inline bool approxEqual(decimal a, decimal b, decimal epsilon = MACHINE_EPSILON) return (difference < epsilon && difference > -epsilon); } +/// Function that returns the result of the "value" clamped by +/// two others values "lowerLimit" and "upperLimit" +inline decimal clamp(decimal value, decimal lowerLimit, decimal upperLimit) { + assert(lowerLimit <= upperLimit); + return std::min(std::max(value, lowerLimit), upperLimit); +} + } diff --git a/src/reactphysics3d.h b/src/reactphysics3d.h index df9e367c..98a8b146 100644 --- a/src/reactphysics3d.h +++ b/src/reactphysics3d.h @@ -49,6 +49,7 @@ #include "collision/shapes/AABB.h" #include "constraint/BallAndSocketJoint.h" #include "constraint/SliderJoint.h" +#include "constraint/HingeJoint.h" /// Alias to the ReactPhysics3D namespace namespace rp3d = reactphysics3d; diff --git a/test/tests/mathematics/TestQuaternion.h b/test/tests/mathematics/TestQuaternion.h index ac01cb6c..ac7ad741 100644 --- a/test/tests/mathematics/TestQuaternion.h +++ b/test/tests/mathematics/TestQuaternion.h @@ -120,6 +120,12 @@ class TestQuaternion : public Test { quaternion.setToZero(); test(quaternion == Quaternion(0, 0, 0, 0)); + // Tes the methods to get or set to identity + Quaternion identity1(1, 2, 3, 4); + identity1.setToIdentity(); + test(identity1 == Quaternion(0, 0, 0, 1)); + test(Quaternion::identity() == Quaternion(0, 0, 0, 1)); + // Test the method to get the vector (x, y, z) Vector3 v = mQuaternion1.getVectorV(); test(v.x == mQuaternion1.x); @@ -133,9 +139,12 @@ class TestQuaternion : public Test { test(conjugate.z == -mQuaternion1.z); test(conjugate.w == mQuaternion1.w); - // Test the inverse method - Quaternion inverse = mQuaternion1.getInverse(); - Quaternion product = mQuaternion1 * inverse; + // Test the inverse methods + Quaternion inverse1 = mQuaternion1.getInverse(); + Quaternion inverse2(mQuaternion1); + inverse2.inverse(); + test(inverse2 == inverse1); + Quaternion product = mQuaternion1 * inverse1; test(approxEqual(product.x, mIdentity.x, decimal(10e-6))); test(approxEqual(product.y, mIdentity.y, decimal(10e-6))); test(approxEqual(product.z, mIdentity.z, decimal(10e-6))); From 1c0726d9d68e5c5b272f7db825481043a9f22511 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 12 Jun 2013 20:43:54 +0200 Subject: [PATCH 22/24] Add the fixed joint --- src/body/CollisionBody.h | 2 +- src/constraint/BallAndSocketJoint.cpp | 1 - src/constraint/BallAndSocketJoint.h | 6 - src/constraint/Constraint.h | 2 +- src/constraint/FixedJoint.cpp | 248 ++++++++++++++++++++++++++ src/constraint/FixedJoint.h | 138 ++++++++++++++ src/constraint/HingeJoint.cpp | 2 - src/constraint/SliderJoint.cpp | 2 - src/engine/DynamicsWorld.cpp | 10 ++ src/reactphysics3d.h | 1 + 10 files changed, 399 insertions(+), 13 deletions(-) create mode 100644 src/constraint/FixedJoint.cpp create mode 100644 src/constraint/FixedJoint.h diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h index c119925e..22b93c0b 100644 --- a/src/body/CollisionBody.h +++ b/src/body/CollisionBody.h @@ -208,7 +208,7 @@ inline const Transform& CollisionBody::getTransform() const { inline void CollisionBody::setTransform(const Transform& transform) { // Check if the body has moved - if (this->mTransform != transform) { + if (mTransform != transform) { mHasMoved = true; } diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index 4a8db2ce..f5c0c50c 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -27,7 +27,6 @@ #include "BallAndSocketJoint.h" #include "../engine/ConstraintSolver.h" -// TODO : Solve 2x2 or 3x3 linear systems without inverting the A matrix (direct resolution) using namespace reactphysics3d; diff --git a/src/constraint/BallAndSocketJoint.h b/src/constraint/BallAndSocketJoint.h index dff9210d..934f32eb 100644 --- a/src/constraint/BallAndSocketJoint.h +++ b/src/constraint/BallAndSocketJoint.h @@ -84,12 +84,6 @@ class BallAndSocketJoint : public Constraint { /// Bias vector for the constraint Vector3 mBiasVector; - /// Skew-Symmetric matrix for cross product with vector mR1World - Matrix3x3 mSkewSymmetricMatrixR1World; - - /// Skew-Symmetric matrix for cross product with vector mR2World - Matrix3x3 mSkewSymmetricMatrixR2World; - /// Inverse mass matrix K=JM^-1J^-t of the constraint Matrix3x3 mInverseMassMatrix; diff --git a/src/constraint/Constraint.h b/src/constraint/Constraint.h index 97b70150..0d3cd8f1 100644 --- a/src/constraint/Constraint.h +++ b/src/constraint/Constraint.h @@ -35,7 +35,7 @@ namespace reactphysics3d { // Enumeration for the type of a constraint -enum ConstraintType {CONTACT, BALLSOCKETJOINT, SLIDERJOINT, HINGEJOINT}; +enum ConstraintType {CONTACT, BALLSOCKETJOINT, SLIDERJOINT, HINGEJOINT, FIXEDJOINT}; // Class declarations struct ConstraintSolverData; diff --git a/src/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp new file mode 100644 index 00000000..8fcbe04f --- /dev/null +++ b/src/constraint/FixedJoint.cpp @@ -0,0 +1,248 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2013 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. * +* * +********************************************************************************/ + +// Libraries +#include "FixedJoint.h" +#include "../engine/ConstraintSolver.h" + +using namespace reactphysics3d; + +// Static variables definition +const decimal FixedJoint::BETA = decimal(0.2); + +// Constructor +FixedJoint::FixedJoint(const FixedJointInfo& jointInfo) + : Constraint(jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0, 0) { + + // Compute the local-space anchor point for each body + const Transform& transform1 = mBody1->getTransform(); + const Transform& transform2 = mBody2->getTransform(); + mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; + mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; + + // Compute the inverse of the initial orientation difference between the two bodies + mInitOrientationDifferenceInv = transform2.getOrientation() * + transform1.getOrientation().getInverse(); + mInitOrientationDifferenceInv.normalize(); + mInitOrientationDifferenceInv.inverse(); +} + +// Destructor +FixedJoint::~FixedJoint() { + +} + +// Initialize before solving the constraint +void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { + + // Initialize the bodies index in the velocity array + mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; + mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; + + // Get the bodies positions and orientations + const Vector3& x1 = mBody1->getTransform().getPosition(); + const Vector3& x2 = mBody2->getTransform().getPosition(); + const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); + const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); + + // Get the inertia tensor of bodies + const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); + const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); + + // Compute the vector from body center to the anchor point in world-space + mR1World = orientationBody1 * mLocalAnchorPointBody1; + mR2World = orientationBody2 * mLocalAnchorPointBody2; + + // Compute the corresponding skew-symmetric matrices + Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); + Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); + + // Compute the matrix K=JM^-1J^t (3x3 matrix) + decimal inverseMassBodies = 0.0; + if (mBody1->getIsMotionEnabled()) { + inverseMassBodies += mBody1->getMassInverse(); + } + if (mBody2->getIsMotionEnabled()) { + inverseMassBodies += mBody2->getMassInverse(); + } + Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, + 0, inverseMassBodies, 0, + 0, 0, inverseMassBodies); + if (mBody1->getIsMotionEnabled()) { + massMatrix += skewSymmetricMatrixU1 * I1 * skewSymmetricMatrixU1.getTranspose(); + } + if (mBody2->getIsMotionEnabled()) { + massMatrix += skewSymmetricMatrixU2 * I2 * skewSymmetricMatrixU2.getTranspose(); + } + + // Compute the inverse mass matrix K^-1 for the 3 translation constraints + mInverseMassMatrixTranslation.setToZero(); + if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + mInverseMassMatrixTranslation = massMatrix.getInverse(); + } + + // Compute the bias "b" of the constraint for the 3 translation constraints + decimal biasFactor = (BETA / constraintSolverData.timeStep); + mBiasTranslation.setToZero(); + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + mBiasTranslation = biasFactor * (x2 + mR2World - x1 - mR1World); + } + + // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation + // contraints (3x3 matrix) + mInverseMassMatrixRotation.setToZero(); + if (mBody1->getIsMotionEnabled()) { + mInverseMassMatrixRotation += I1; + } + if (mBody2->getIsMotionEnabled()) { + mInverseMassMatrixRotation += I2; + } + if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse(); + } + + // Compute the bias "b" for the 3 rotation constraints + mBiasRotation.setToZero(); + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); + currentOrientationDifference.normalize(); + const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv; + mBiasRotation = biasFactor * decimal(2.0) * qError.getVectorV(); + } + + // If warm-starting is not enabled + if (!constraintSolverData.isWarmStartingActive) { + + // Reset the accumulated impulses + mImpulseTranslation.setToZero(); + mImpulseRotation.setToZero(); + } +} + +// Warm start the constraint (apply the previous impulse at the beginning of the step) +void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) { + + // Get the velocities + Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; + Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; + Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; + Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; + + // Get the inverse mass and inverse inertia tensors of the bodies + const decimal inverseMassBody1 = mBody1->getMassInverse(); + const decimal inverseMassBody2 = mBody2->getMassInverse(); + const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); + const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); + + // Compute the impulse P=J^T * lambda for the 3 translation constraints + Vector3 linearImpulseBody1 = -mImpulseTranslation; + Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World); + Vector3 linearImpulseBody2 = mImpulseTranslation; + Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World); + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints + angularImpulseBody1 += -mImpulseRotation; + angularImpulseBody2 += mImpulseRotation; + + // Apply the impulse to the bodies of the joint + if (mBody1->getIsMotionEnabled()) { + v1 += inverseMassBody1 * linearImpulseBody1; + w1 += I1 * angularImpulseBody1; + } + if (mBody2->getIsMotionEnabled()) { + v2 += inverseMassBody2 * linearImpulseBody2; + w2 += I2 * angularImpulseBody2; + } + +} + +// Solve the velocity constraint +void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { + + // Get the velocities + Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; + Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; + Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; + Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; + + // Get the inverse mass and inverse inertia tensors of the bodies + decimal inverseMassBody1 = mBody1->getMassInverse(); + decimal inverseMassBody2 = mBody2->getMassInverse(); + Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); + Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); + + // --------------- Translation Constraints --------------- // + + // Compute J*v for the 3 translation constraints + const Vector3 JvTranslation = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World); + + // Compute the Lagrange multiplier lambda + const Vector3 deltaLambda = mInverseMassMatrixTranslation * + (-JvTranslation - mBiasTranslation); + mImpulseTranslation += deltaLambda; + + // Compute the impulse P=J^T * lambda + Vector3 linearImpulseBody1 = -deltaLambda; + Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World); + Vector3 linearImpulseBody2 = deltaLambda; + Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World); + + // Apply the impulse to the bodies of the joint + if (mBody1->getIsMotionEnabled()) { + v1 += inverseMassBody1 * linearImpulseBody1; + w1 += I1 * angularImpulseBody1; + } + if (mBody2->getIsMotionEnabled()) { + v2 += inverseMassBody2 * linearImpulseBody2; + w2 += I2 * angularImpulseBody2; + } + + // --------------- Rotation Constraints --------------- // + + // Compute J*v for the 3 rotation constraints + const Vector3 JvRotation = w2 - w1; + + // Compute the Lagrange multiplier lambda for the 3 rotation constraints + Vector3 deltaLambda2 = mInverseMassMatrixRotation * (-JvRotation - mBiasRotation); + mImpulseRotation += deltaLambda2; + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints + angularImpulseBody1 = -deltaLambda2; + angularImpulseBody2 = deltaLambda2; + + // Apply the impulse to the bodies of the joint + if (mBody1->getIsMotionEnabled()) { + w1 += I1 * angularImpulseBody1; + } + if (mBody2->getIsMotionEnabled()) { + w2 += I2 * angularImpulseBody2; + } +} + +// Solve the position constraint +void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) { + +} + diff --git a/src/constraint/FixedJoint.h b/src/constraint/FixedJoint.h new file mode 100644 index 00000000..f5e179e7 --- /dev/null +++ b/src/constraint/FixedJoint.h @@ -0,0 +1,138 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2013 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 REACTPHYSICS3D_FIXED_JOINT_H +#define REACTPHYSICS3D_FIXED_JOINT_H + +// Libraries +#include "Constraint.h" +#include "../mathematics/mathematics.h" + +namespace reactphysics3d { + +// Structure FixedJointInfo +/** + * This structure is used to gather the information needed to create a fixed + * joint. This structure will be used to create the actual fixed joint. + */ +struct FixedJointInfo : public ConstraintInfo { + + public : + + // -------------------- Attributes -------------------- // + + /// Anchor point (in world-space coordinates) + Vector3 anchorPointWorldSpace; + + /// Constructor + FixedJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, + const Vector3& initAnchorPointWorldSpace) + : ConstraintInfo(rigidBody1, rigidBody2, FIXEDJOINT), + anchorPointWorldSpace(initAnchorPointWorldSpace){} +}; + +// Class FixedJoint +/** + * This class represents a fixed joint that is used to forbid any translation or rotation + * between two bodies. + */ +class FixedJoint : public Constraint { + + private : + + // -------------------- Constants -------------------- // + + // Beta value for the bias factor of position correction + static const decimal BETA; + + // -------------------- Attributes -------------------- // + + /// Anchor point of body 1 (in local-space coordinates of body 1) + Vector3 mLocalAnchorPointBody1; + + /// Anchor point of body 2 (in local-space coordinates of body 2) + Vector3 mLocalAnchorPointBody2; + + /// Vector from center of body 2 to anchor point in world-space + Vector3 mR1World; + + /// Vector from center of body 2 to anchor point in world-space + Vector3 mR2World; + + /// Accumulated impulse for the 3 translation constraints + Vector3 mImpulseTranslation; + + /// Accumulate impulse for the 3 rotation constraints + Vector3 mImpulseRotation; + + /// Inverse mass matrix K=JM^-1J^-t of the 3 translation constraints (3x3 matrix) + Matrix3x3 mInverseMassMatrixTranslation; + + /// Inverse mass matrix K=JM^-1J^-t of the 3 rotation constraints (3x3 matrix) + Matrix3x3 mInverseMassMatrixRotation; + + /// Bias vector for the 3 translation constraints + Vector3 mBiasTranslation; + + /// Bias vector for the 3 rotation constraints + Vector3 mBiasRotation; + + /// Inverse of the initial orientation difference between the two bodies + Quaternion mInitOrientationDifferenceInv; + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + FixedJoint(const FixedJointInfo& jointInfo); + + /// Destructor + virtual ~FixedJoint(); + + /// Return the number of bytes used by the joint + virtual size_t getSizeInBytes() const; + + /// Initialize before solving the constraint + virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); + + /// Warm start the constraint (apply the previous impulse at the beginning of the step) + virtual void warmstart(const ConstraintSolverData& constraintSolverData); + + /// Solve the velocity constraint + virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); + + /// Solve the position constraint + virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); +}; + +// Return the number of bytes used by the joint +inline size_t FixedJoint::getSizeInBytes() const { + return sizeof(FixedJoint); +} + +} + +#endif diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index aa0da0eb..10b3589e 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -28,8 +28,6 @@ #include "../engine/ConstraintSolver.h" #include -// TODO : Solve 2x2 or 3x3 linear systems without inverting the A matrix (direct resolution) - using namespace reactphysics3d; // Static variables definition diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index dc148f6f..c9586a61 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -26,8 +26,6 @@ // Libraries #include "SliderJoint.h" -// TODO : Solve 2x2 or 3x3 linear systems without inverting the A matrix (direct resolution) - using namespace reactphysics3d; // Static variables definition diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 98a9966a..53cd3e35 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -28,6 +28,7 @@ #include "constraint/BallAndSocketJoint.h" #include "constraint/SliderJoint.h" #include "constraint/HingeJoint.h" +#include "constraint/FixedJoint.h" // Namespaces using namespace reactphysics3d; @@ -423,6 +424,15 @@ Constraint* DynamicsWorld::createJoint(const ConstraintInfo& jointInfo) { break; } + // Fixed joint + case FIXEDJOINT: + { + void* allocatedMemory = mMemoryAllocator.allocate(sizeof(FixedJoint)); + const FixedJointInfo& info = dynamic_cast(jointInfo); + newJoint = new (allocatedMemory) FixedJoint(info); + break; + } + default: { assert(false); diff --git a/src/reactphysics3d.h b/src/reactphysics3d.h index 98a8b146..e24c2b82 100644 --- a/src/reactphysics3d.h +++ b/src/reactphysics3d.h @@ -50,6 +50,7 @@ #include "constraint/BallAndSocketJoint.h" #include "constraint/SliderJoint.h" #include "constraint/HingeJoint.h" +#include "constraint/FixedJoint.h" /// Alias to the ReactPhysics3D namespace namespace rp3d = reactphysics3d; From ce0078c2a96a38bb41a523560edefa24e92a550f Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 26 Jun 2013 22:28:31 +0200 Subject: [PATCH 23/24] Implement the non-linear-gauss-seidel position error correction --- src/body/CollisionBody.h | 2 - src/collision/BroadPhasePair.h | 15 +- src/collision/CollisionDetection.cpp | 3 + src/collision/CollisionDetection.h | 23 +- .../narrowphase/EPA/EPAAlgorithm.cpp | 2 +- src/configuration.h | 15 +- src/constraint/BallAndSocketJoint.cpp | 152 ++++-- src/constraint/BallAndSocketJoint.h | 8 +- src/constraint/Constraint.cpp | 3 +- src/constraint/Constraint.h | 22 +- src/constraint/FixedJoint.cpp | 243 +++++++-- src/constraint/FixedJoint.h | 8 +- src/constraint/HingeJoint.cpp | 446 +++++++++++++--- src/constraint/HingeJoint.h | 53 +- src/constraint/SliderJoint.cpp | 482 +++++++++++++++--- src/constraint/SliderJoint.h | 15 +- src/engine/ConstraintSolver.cpp | 19 +- src/engine/ConstraintSolver.h | 34 +- src/engine/ContactSolver.h | 10 +- src/engine/DynamicsWorld.cpp | 135 ++++- src/engine/DynamicsWorld.h | 35 +- src/engine/Timer.h | 10 +- src/mathematics/Quaternion.h | 24 + test/tests/mathematics/TestQuaternion.h | 6 + 24 files changed, 1429 insertions(+), 336 deletions(-) diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h index 22b93c0b..df484e33 100644 --- a/src/body/CollisionBody.h +++ b/src/body/CollisionBody.h @@ -239,8 +239,6 @@ inline void CollisionBody::updateOldTransform() { // Update the rigid body in order to reflect a change in the body state inline void CollisionBody::updateAABB() { - // TODO : An AABB should not be updated every frame but only if the body has moved - // Update the AABB mCollisionShape->updateAABB(mAabb, mTransform); } diff --git a/src/collision/BroadPhasePair.h b/src/collision/BroadPhasePair.h index f0a2f9ca..4712c9e1 100644 --- a/src/collision/BroadPhasePair.h +++ b/src/collision/BroadPhasePair.h @@ -60,6 +60,9 @@ struct BroadPhasePair { /// Destructor ~BroadPhasePair(); + /// Return the pair of bodies index + static bodyindexpair computeBodiesIndexPair(CollisionBody* body1, CollisionBody* body2); + /// Return the pair of bodies index bodyindexpair getBodiesIndexPair() const; @@ -77,16 +80,22 @@ struct BroadPhasePair { }; // Return the pair of bodies index -inline bodyindexpair BroadPhasePair::getBodiesIndexPair() const { - +inline bodyindexpair BroadPhasePair::computeBodiesIndexPair(CollisionBody* body1, + CollisionBody* body2) { // Construct the pair of body index - bodyindexpair indexPair = body1->getID() < body2->getID() ? + bodyindexpair indexPair = body1->getID() < body2->getID() ? std::make_pair(body1->getID(), body2->getID()) : std::make_pair(body2->getID(), body1->getID()); assert(indexPair.first != indexPair.second); return indexPair; } +// Return the pair of bodies index +inline bodyindexpair BroadPhasePair::getBodiesIndexPair() const { + + return computeBodiesIndexPair(body1, body2); +} + // Smaller than operator inline bool BroadPhasePair::operator<(const BroadPhasePair& broadPhasePair2) const { return (body1 < broadPhasePair2.body1 ? true : (body2 < broadPhasePair2.body2)); diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 46773a1c..5f97d271 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -109,6 +109,9 @@ void CollisionDetection::computeNarrowPhase() { // Update the contact cache of the overlapping pair mWorld->updateOverlappingPair(pair); + + // Check if the two bodies are allowed to collide, otherwise, we do not test for collision + if (mNoCollisionPairs.count(pair->getBodiesIndexPair()) > 0) continue; // Select the narrow phase algorithm to use according to the two collision shapes NarrowPhaseAlgorithm& narrowPhaseAlgorithm = SelectNarrowPhaseAlgorithm( diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 2cb0879a..a2aa3427 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -79,6 +79,9 @@ class CollisionDetection { /// Narrow-phase Sphere vs Sphere algorithm SphereVsSphereAlgorithm mNarrowPhaseSphereVsSphereAlgorithm; + /// Set of pair of bodies that cannot collide between each other + std::set mNoCollisionPairs; + // -------------------- Methods -------------------- // /// Private copy-constructor @@ -113,6 +116,12 @@ class CollisionDetection { /// Remove a body from the collision detection void removeBody(CollisionBody* body); + /// Add a pair of bodies that cannot collide with each other + void addNoCollisionPair(CollisionBody* body1, CollisionBody* body2); + + /// Remove a pair of bodies that cannot collide with each other + void removeNoCollisionPair(CollisionBody *body1, CollisionBody *body2); + /// Compute the collision detection void computeCollisionDetection(); @@ -148,7 +157,19 @@ inline void CollisionDetection::removeBody(CollisionBody* body) { // Remove the body from the broad-phase mBroadPhaseAlgorithm->removeObject(body); -} +} + +// Add a pair of bodies that cannot collide with each other +inline void CollisionDetection::addNoCollisionPair(CollisionBody* body1, + CollisionBody* body2) { + mNoCollisionPairs.insert(BroadPhasePair::computeBodiesIndexPair(body1, body2)); +} + +// Remove a pair of bodies that cannot collide with each other +inline void CollisionDetection::removeNoCollisionPair(CollisionBody* body1, + CollisionBody* body2) { + mNoCollisionPairs.erase(BroadPhasePair::computeBodiesIndexPair(body1, body2)); +} } diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp index 2cdfc87b..a9cf7a38 100644 --- a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp +++ b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp @@ -143,7 +143,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple int minAxis = d.getAbsoluteVector().getMinAxis(); // Compute sin(60) - const decimal sin60 = sqrt(3.0) * decimal(0.5); + const decimal sin60 = decimal(sqrt(3.0)) * decimal(0.5); // Create a rotation quaternion to rotate the vector v1 to get the vectors // v2 and v3 diff --git a/src/configuration.h b/src/configuration.h index 83e3c817..4873ff96 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -54,15 +54,14 @@ typedef std::pair bodyindexpair; // ------------------- Enumerations ------------------- // /// Position correction technique used in the constraint solver (for joints). -/// BAUMGARTE : Faster but can be innacurate in some situations. This is the option -/// used by default. -/// NON_LINEAR_GAUSS_SEIDEL : Slower but more precise. +/// BAUMGARTE_JOINTS : Faster but can be innacurate in some situations. +/// NON_LINEAR_GAUSS_SEIDEL : Slower but more precise. This is the option used by default. enum JointsPositionCorrectionTechnique {BAUMGARTE_JOINTS, NON_LINEAR_GAUSS_SEIDEL}; /// Position correction technique used in the contact solver (for contacts) -/// BAUMGARTE : Faster but can be innacurate and can lead to unexpected bounciness -/// in some situations (due to error correction factor being added to -/// the bodies momentum). +/// BAUMGARTE_CONTACTS : Faster but can be innacurate and can lead to unexpected bounciness +/// in some situations (due to error correction factor being added to +/// the bodies momentum). /// SPLIT_IMPULSES : A bit slower but the error correction factor is not added to the /// bodies momentum. This is the option used by default. enum ContactsPositionCorrectionTechnique {BAUMGARTE_CONTACTS, SPLIT_IMPULSES}; @@ -93,7 +92,7 @@ const decimal DEFAULT_FRICTION_COEFFICIENT = decimal(0.3); /// True if the deactivation (sleeping) of inactive bodies is enabled const bool DEACTIVATION_ENABLED = true; -///Object margin for collision detection in cm (For GJK-EPA Algorithm) +/// Object margin for collision detection in cm (For GJK-EPA Algorithm) const decimal OBJECT_MARGIN = decimal(0.04); /// Distance threshold for two contact points for a valid persistent contact (in meters) @@ -106,7 +105,7 @@ const decimal RESTITUTION_VELOCITY_THRESHOLD = decimal(1.0); const uint DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 15; /// Number of iterations when solving the position constraints of the Sequential Impulse technique -const uint DEFAULT_POSITION_SOLVER_NB_ITERATIONS = 3; // TODO : Maybe we can use less iterations here +const uint DEFAULT_POSITION_SOLVER_NB_ITERATIONS = 5; } diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index f5c0c50c..6dc016ae 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -27,7 +27,6 @@ #include "BallAndSocketJoint.h" #include "../engine/ConstraintSolver.h" - using namespace reactphysics3d; // Static variables definition @@ -61,8 +60,8 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); // Get the inertia tensor of bodies - const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); - const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); + mI1 = mBody1->getInertiaTensorInverseWorld(); + mI2 = mBody2->getInertiaTensorInverseWorld(); // Compute the vector from body center to the anchor point in world-space mR1World = orientationBody1 * mLocalAnchorPointBody1; @@ -84,10 +83,10 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS 0, inverseMassBodies, 0, 0, 0, inverseMassBodies); if (mBody1->getIsMotionEnabled()) { - massMatrix += skewSymmetricMatrixU1 * I1 * skewSymmetricMatrixU1.getTranspose(); + massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose(); } if (mBody2->getIsMotionEnabled()) { - massMatrix += skewSymmetricMatrixU2 * I2 * skewSymmetricMatrixU2.getTranspose(); + massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); } // Compute the inverse mass matrix K^-1 @@ -120,26 +119,29 @@ void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverD Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; - // Get the inverse mass and inverse inertia tensors of the bodies + // Get the inverse mass of the bodies const decimal inverseMassBody1 = mBody1->getMassInverse(); const decimal inverseMassBody2 = mBody2->getMassInverse(); - const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); - const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); - // Compute the impulse P=J^T * lambda - const Vector3 linearImpulseBody1 = -mImpulse; - const Vector3 angularImpulseBody1 = mImpulse.cross(mR1World); - const Vector3 linearImpulseBody2 = mImpulse; - const Vector3 angularImpulseBody2 = -mImpulse.cross(mR2World); - - // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda + const Vector3 linearImpulseBody1 = -mImpulse; + const Vector3 angularImpulseBody1 = mImpulse.cross(mR1World); + + // Apply the impulse to the body v1 += inverseMassBody1 * linearImpulseBody1; - w1 += I1 * angularImpulseBody1; + w1 += mI1 * angularImpulseBody1; } if (mBody2->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda + const Vector3 linearImpulseBody2 = mImpulse; + const Vector3 angularImpulseBody2 = -mImpulse.cross(mR2World); + + // Apply the impulse to the body v2 += inverseMassBody2 * linearImpulseBody2; - w2 += I2 * angularImpulseBody2; + w2 += mI2 * angularImpulseBody2; } } @@ -152,11 +154,9 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; - // Get the inverse mass and inverse inertia tensors of the bodies + // Get the inverse mass of the bodies decimal inverseMassBody1 = mBody1->getMassInverse(); decimal inverseMassBody2 = mBody2->getMassInverse(); - Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); - Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); // Compute J*v const Vector3 Jv = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World); @@ -165,25 +165,117 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con const Vector3 deltaLambda = mInverseMassMatrix * (-Jv - mBiasVector); mImpulse += deltaLambda; - // Compute the impulse P=J^T * lambda - const Vector3 linearImpulseBody1 = -deltaLambda; - const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World); - const Vector3 linearImpulseBody2 = deltaLambda; - const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World); - - // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda + const Vector3 linearImpulseBody1 = -deltaLambda; + const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World); + + // Apply the impulse to the body v1 += inverseMassBody1 * linearImpulseBody1; - w1 += I1 * angularImpulseBody1; + w1 += mI1 * angularImpulseBody1; } if (mBody2->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda + const Vector3 linearImpulseBody2 = deltaLambda; + const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World); + + // Apply the impulse to the body v2 += inverseMassBody2 * linearImpulseBody2; - w2 += I2 * angularImpulseBody2; + w2 += mI2 * angularImpulseBody2; } } -// Solve the position constraint +// Solve the position constraint (for position error correction) void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) { + // If the error position correction technique is not the non-linear-gauss-seidel, we do + // do not execute this method + if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return; + + // Get the bodies positions and orientations + Vector3& x1 = constraintSolverData.positions[mIndexBody1]; + Vector3& x2 = constraintSolverData.positions[mIndexBody2]; + Quaternion& q1 = constraintSolverData.orientations[mIndexBody1]; + Quaternion& q2 = constraintSolverData.orientations[mIndexBody2]; + + // Get the inverse mass and inverse inertia tensors of the bodies + decimal inverseMassBody1 = mBody1->getMassInverse(); + decimal inverseMassBody2 = mBody2->getMassInverse(); + + // Recompute the inverse inertia tensors + mI1 = mBody1->getInertiaTensorInverseWorld(); + mI2 = mBody2->getInertiaTensorInverseWorld(); + + // Compute the vector from body center to the anchor point in world-space + mR1World = q1 * mLocalAnchorPointBody1; + mR2World = q2 * mLocalAnchorPointBody2; + + // Compute the corresponding skew-symmetric matrices + Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); + Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); + + // Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation constraints + decimal inverseMassBodies = 0.0; + if (mBody1->getIsMotionEnabled()) { + inverseMassBodies += inverseMassBody1; + } + if (mBody2->getIsMotionEnabled()) { + inverseMassBodies += inverseMassBody2; + } + Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, + 0, inverseMassBodies, 0, + 0, 0, inverseMassBodies); + if (mBody1->getIsMotionEnabled()) { + massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose(); + } + if (mBody2->getIsMotionEnabled()) { + massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); + } + mInverseMassMatrix.setToZero(); + if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + mInverseMassMatrix = massMatrix.getInverse(); + } + + // Compute the constraint error (value of the C(x) function) + const Vector3 constraintError = (x2 + mR2World - x1 - mR1World); + + // Compute the Lagrange multiplier lambda + // TODO : Do not solve the system by computing the inverse each time and multiplying with the + // right-hand side vector but instead use a method to directly solve the linear system. + const Vector3 lambda = mInverseMassMatrix * (-constraintError); + + // Apply the impulse to the bodies of the joint (directly update the position/orientation) + if (mBody1->getIsMotionEnabled()) { + + // Compute the impulse + const Vector3 linearImpulseBody1 = -lambda; + const Vector3 angularImpulseBody1 = lambda.cross(mR1World); + + // Compute the pseudo velocity + const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; + const Vector3 w1 = mI1 * angularImpulseBody1; + + // Update the body position/orientation + x1 += v1; + q1 += Quaternion(0, w1) * q1 * decimal(0.5); + q1.normalize(); + } + if (mBody2->getIsMotionEnabled()) { + + // Compute the impulse + const Vector3 linearImpulseBody2 = lambda; + const Vector3 angularImpulseBody2 = -lambda.cross(mR2World); + + // Compute the pseudo velocity + const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; + const Vector3 w2 = mI2 * angularImpulseBody2; + + // Update the body position/orientation + x2 += v2; + q2 += Quaternion(0, w2) * q2 * decimal(0.5); + q2.normalize(); + } } diff --git a/src/constraint/BallAndSocketJoint.h b/src/constraint/BallAndSocketJoint.h index 934f32eb..6cd47fa6 100644 --- a/src/constraint/BallAndSocketJoint.h +++ b/src/constraint/BallAndSocketJoint.h @@ -81,6 +81,12 @@ class BallAndSocketJoint : public Constraint { /// Vector from center of body 2 to anchor point in world-space Vector3 mR2World; + /// Inertia tensor of body 1 (in world-space coordinates) + Matrix3x3 mI1; + + /// Inertia tensor of body 2 (in world-space coordinates) + Matrix3x3 mI2; + /// Bias vector for the constraint Vector3 mBiasVector; @@ -112,7 +118,7 @@ class BallAndSocketJoint : public Constraint { /// Solve the velocity constraint virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); - /// Solve the position constraint + /// Solve the position constraint (for position error correction) virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); }; diff --git a/src/constraint/Constraint.cpp b/src/constraint/Constraint.cpp index 234861e3..5da1c4eb 100644 --- a/src/constraint/Constraint.cpp +++ b/src/constraint/Constraint.cpp @@ -32,7 +32,8 @@ using namespace reactphysics3d; Constraint::Constraint(const ConstraintInfo& constraintInfo) :mBody1(constraintInfo.body1), mBody2(constraintInfo.body2), mActive(true), mType(constraintInfo.type), - mPositionCorrectionTechnique(constraintInfo.positionCorrectionTechnique) { + mPositionCorrectionTechnique(constraintInfo.positionCorrectionTechnique), + mIsCollisionEnabled(constraintInfo.isCollisionEnabled){ assert(mBody1 != NULL); assert(mBody2 != NULL); diff --git a/src/constraint/Constraint.h b/src/constraint/Constraint.h index 0d3cd8f1..ae7bfa77 100644 --- a/src/constraint/Constraint.h +++ b/src/constraint/Constraint.h @@ -59,6 +59,9 @@ struct ConstraintInfo { /// Type of the constraint ConstraintType type; + /// True if the two bodies of the constraint are allowed to collide with each other + bool isCollisionEnabled; + /// Position correction technique used for the constraint (used for joints). /// By default, the BAUMGARTE technique is used JointsPositionCorrectionTechnique positionCorrectionTechnique; @@ -66,12 +69,14 @@ struct ConstraintInfo { /// Constructor ConstraintInfo(ConstraintType constraintType) : body1(NULL), body2(NULL), type(constraintType), - positionCorrectionTechnique(BAUMGARTE_JOINTS) {} + positionCorrectionTechnique(NON_LINEAR_GAUSS_SEIDEL), + isCollisionEnabled(true) {} /// Constructor ConstraintInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, ConstraintType constraintType) : body1(rigidBody1), body2(rigidBody2), type(constraintType), - positionCorrectionTechnique(BAUMGARTE_JOINTS) { + positionCorrectionTechnique(NON_LINEAR_GAUSS_SEIDEL), + isCollisionEnabled(true) { } /// Destructor @@ -113,6 +118,9 @@ class Constraint { /// Position correction technique used for the constraint (used for joints) JointsPositionCorrectionTechnique mPositionCorrectionTechnique; + /// True if the two bodies of the constraint are allowed to collide with each other + bool mIsCollisionEnabled; + // -------------------- Methods -------------------- // /// Private copy-constructor @@ -143,6 +151,9 @@ class Constraint { /// Return the type of the constraint ConstraintType getType() const; + /// Return true if the collision between the two bodies of the constraint is enabled + bool isCollisionEnabled() const; + /// Return the number of bytes used by the constraint virtual size_t getSizeInBytes() const = 0; @@ -177,7 +188,12 @@ inline bool Constraint::isActive() const { // Return the type of the constraint inline ConstraintType Constraint::getType() const { return mType; -} +} + +// Return true if the collision between the two bodies of the constraint is enabled +inline bool Constraint::isCollisionEnabled() const { + return mIsCollisionEnabled; +} } diff --git a/src/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp index 8fcbe04f..7a5acf82 100644 --- a/src/constraint/FixedJoint.cpp +++ b/src/constraint/FixedJoint.cpp @@ -68,8 +68,8 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); // Get the inertia tensor of bodies - const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); - const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); + mI1 = mBody1->getInertiaTensorInverseWorld(); + mI2 = mBody2->getInertiaTensorInverseWorld(); // Compute the vector from body center to the anchor point in world-space mR1World = orientationBody1 * mLocalAnchorPointBody1; @@ -79,7 +79,7 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); - // Compute the matrix K=JM^-1J^t (3x3 matrix) + // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints decimal inverseMassBodies = 0.0; if (mBody1->getIsMotionEnabled()) { inverseMassBodies += mBody1->getMassInverse(); @@ -91,10 +91,10 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat 0, inverseMassBodies, 0, 0, 0, inverseMassBodies); if (mBody1->getIsMotionEnabled()) { - massMatrix += skewSymmetricMatrixU1 * I1 * skewSymmetricMatrixU1.getTranspose(); + massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose(); } if (mBody2->getIsMotionEnabled()) { - massMatrix += skewSymmetricMatrixU2 * I2 * skewSymmetricMatrixU2.getTranspose(); + massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); } // Compute the inverse mass matrix K^-1 for the 3 translation constraints @@ -114,10 +114,10 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat // contraints (3x3 matrix) mInverseMassMatrixRotation.setToZero(); if (mBody1->getIsMotionEnabled()) { - mInverseMassMatrixRotation += I1; + mInverseMassMatrixRotation += mI1; } if (mBody2->getIsMotionEnabled()) { - mInverseMassMatrixRotation += I2; + mInverseMassMatrixRotation += mI2; } if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse(); @@ -150,32 +150,36 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) { Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; - // Get the inverse mass and inverse inertia tensors of the bodies + // Get the inverse mass of the bodies const decimal inverseMassBody1 = mBody1->getMassInverse(); const decimal inverseMassBody2 = mBody2->getMassInverse(); - const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); - const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); - // Compute the impulse P=J^T * lambda for the 3 translation constraints - Vector3 linearImpulseBody1 = -mImpulseTranslation; - Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World); - Vector3 linearImpulseBody2 = mImpulseTranslation; - Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World); - - // Compute the impulse P=J^T * lambda for the 3 rotation constraints - angularImpulseBody1 += -mImpulseRotation; - angularImpulseBody2 += mImpulseRotation; - - // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda for the 3 translation constraints + Vector3 linearImpulseBody1 = -mImpulseTranslation; + Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World); + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints + angularImpulseBody1 += -mImpulseRotation; + + // Apply the impulse to the body v1 += inverseMassBody1 * linearImpulseBody1; - w1 += I1 * angularImpulseBody1; + w1 += mI1 * angularImpulseBody1; } if (mBody2->getIsMotionEnabled()) { - v2 += inverseMassBody2 * linearImpulseBody2; - w2 += I2 * angularImpulseBody2; - } + // Compute the impulse P=J^T * lambda for the 3 translation constraints + Vector3 linearImpulseBody2 = mImpulseTranslation; + Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World); + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints + angularImpulseBody2 += mImpulseRotation; + + // Apply the impulse to the body + v2 += inverseMassBody2 * linearImpulseBody2; + w2 += mI2 * angularImpulseBody2; + } } // Solve the velocity constraint @@ -187,11 +191,9 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; - // Get the inverse mass and inverse inertia tensors of the bodies + // Get the inverse mass of the bodies decimal inverseMassBody1 = mBody1->getMassInverse(); decimal inverseMassBody2 = mBody2->getMassInverse(); - Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); - Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); // --------------- Translation Constraints --------------- // @@ -203,20 +205,25 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS (-JvTranslation - mBiasTranslation); mImpulseTranslation += deltaLambda; - // Compute the impulse P=J^T * lambda - Vector3 linearImpulseBody1 = -deltaLambda; - Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World); - Vector3 linearImpulseBody2 = deltaLambda; - Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World); - - // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda + const Vector3 linearImpulseBody1 = -deltaLambda; + const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World); + + // Apply the impulse to the body v1 += inverseMassBody1 * linearImpulseBody1; - w1 += I1 * angularImpulseBody1; + w1 += mI1 * angularImpulseBody1; } if (mBody2->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda + const Vector3 linearImpulseBody2 = deltaLambda; + const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World); + + // Apply the impulse to the body v2 += inverseMassBody2 * linearImpulseBody2; - w2 += I2 * angularImpulseBody2; + w2 += mI2 * angularImpulseBody2; } // --------------- Rotation Constraints --------------- // @@ -228,21 +235,163 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS Vector3 deltaLambda2 = mInverseMassMatrixRotation * (-JvRotation - mBiasRotation); mImpulseRotation += deltaLambda2; - // Compute the impulse P=J^T * lambda for the 3 rotation constraints - angularImpulseBody1 = -deltaLambda2; - angularImpulseBody2 = deltaLambda2; + if (mBody1->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints + const Vector3 angularImpulseBody1 = -deltaLambda2; + + // Apply the impulse to the body + w1 += mI1 * angularImpulseBody1; + } + if (mBody2->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints + const Vector3 angularImpulseBody2 = deltaLambda2; + + // Apply the impulse to the body + w2 += mI2 * angularImpulseBody2; + } +} + +// Solve the position constraint (for position error correction) +void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) { + + // If the error position correction technique is not the non-linear-gauss-seidel, we do + // do not execute this method + if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return; + + // Get the bodies positions and orientations + Vector3& x1 = constraintSolverData.positions[mIndexBody1]; + Vector3& x2 = constraintSolverData.positions[mIndexBody2]; + Quaternion& q1 = constraintSolverData.orientations[mIndexBody1]; + Quaternion& q2 = constraintSolverData.orientations[mIndexBody2]; + + // Get the inverse mass and inverse inertia tensors of the bodies + decimal inverseMassBody1 = mBody1->getMassInverse(); + decimal inverseMassBody2 = mBody2->getMassInverse(); + + // Recompute the inverse inertia tensors + mI1 = mBody1->getInertiaTensorInverseWorld(); + mI2 = mBody2->getInertiaTensorInverseWorld(); + + // Compute the vector from body center to the anchor point in world-space + mR1World = q1 * mLocalAnchorPointBody1; + mR2World = q2 * mLocalAnchorPointBody2; + + // Compute the corresponding skew-symmetric matrices + Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); + Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); + + // --------------- Translation Constraints --------------- // + + // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints + decimal inverseMassBodies = 0.0; + if (mBody1->getIsMotionEnabled()) { + inverseMassBodies += mBody1->getMassInverse(); + } + if (mBody2->getIsMotionEnabled()) { + inverseMassBodies += mBody2->getMassInverse(); + } + Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, + 0, inverseMassBodies, 0, + 0, 0, inverseMassBodies); + if (mBody1->getIsMotionEnabled()) { + massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose(); + } + if (mBody2->getIsMotionEnabled()) { + massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); + } + mInverseMassMatrixTranslation.setToZero(); + if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + mInverseMassMatrixTranslation = massMatrix.getInverse(); + } + + // Compute position error for the 3 translation constraints + const Vector3 errorTranslation = x2 + mR2World - x1 - mR1World; + + // Compute the Lagrange multiplier lambda + const Vector3 lambdaTranslation = mInverseMassMatrixTranslation * (-errorTranslation); // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { - w1 += I1 * angularImpulseBody1; + + // Compute the impulse + Vector3 linearImpulseBody1 = -lambdaTranslation; + Vector3 angularImpulseBody1 = lambdaTranslation.cross(mR1World); + + // Compute the pseudo velocity + const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; + const Vector3 w1 = mI1 * angularImpulseBody1; + + // Update the body position/orientation + x1 += v1; + q1 += Quaternion(0, w1) * q1 * decimal(0.5); + q1.normalize(); } if (mBody2->getIsMotionEnabled()) { - w2 += I2 * angularImpulseBody2; + + // Compute the impulse + Vector3 linearImpulseBody2 = lambdaTranslation; + Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mR2World); + + // Compute the pseudo velocity + const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; + const Vector3 w2 = mI2 * angularImpulseBody2; + + // Update the body position/orientation + x2 += v2; + q2 += Quaternion(0, w2) * q2 * decimal(0.5); + q2.normalize(); + } + + // --------------- Rotation Constraints --------------- // + + // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation + // contraints (3x3 matrix) + mInverseMassMatrixRotation.setToZero(); + if (mBody1->getIsMotionEnabled()) { + mInverseMassMatrixRotation += mI1; + } + if (mBody2->getIsMotionEnabled()) { + mInverseMassMatrixRotation += mI2; + } + if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse(); + } + + // Compute the position error for the 3 rotation constraints + Quaternion currentOrientationDifference = q2 * q1.getInverse(); + currentOrientationDifference.normalize(); + const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv; + const Vector3 errorRotation = decimal(2.0) * qError.getVectorV(); + + // Compute the Lagrange multiplier lambda for the 3 rotation constraints + Vector3 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation); + + // Apply the impulse to the bodies of the joint + if (mBody1->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints + const Vector3 angularImpulseBody1 = -lambdaRotation; + + // Compute the pseudo velocity + const Vector3 w1 = mI1 * angularImpulseBody1; + + // Update the body position/orientation + q1 += Quaternion(0, w1) * q1 * decimal(0.5); + q1.normalize(); + } + if (mBody2->getIsMotionEnabled()) { + + // Compute the impulse + const Vector3 angularImpulseBody2 = lambdaRotation; + + // Compute the pseudo velocity + const Vector3 w2 = mI2 * angularImpulseBody2; + + // Update the body position/orientation + q2 += Quaternion(0, w2) * q2 * decimal(0.5); + q2.normalize(); } } -// Solve the position constraint -void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) { - -} - diff --git a/src/constraint/FixedJoint.h b/src/constraint/FixedJoint.h index f5e179e7..7f4f987f 100644 --- a/src/constraint/FixedJoint.h +++ b/src/constraint/FixedJoint.h @@ -81,6 +81,12 @@ class FixedJoint : public Constraint { /// Vector from center of body 2 to anchor point in world-space Vector3 mR2World; + /// Inertia tensor of body 1 (in world-space coordinates) + Matrix3x3 mI1; + + /// Inertia tensor of body 2 (in world-space coordinates) + Matrix3x3 mI2; + /// Accumulated impulse for the 3 translation constraints Vector3 mImpulseTranslation; @@ -124,7 +130,7 @@ class FixedJoint : public Constraint { /// Solve the velocity constraint virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); - /// Solve the position constraint + /// Solve the position constraint (for position error correction) virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); }; diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index 10b3589e..57c8764f 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -40,7 +40,7 @@ HingeJoint::HingeJoint(const HingeJointInfo& jointInfo) mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled), mLowerLimit(jointInfo.minAngleLimit), mUpperLimit(jointInfo.maxAngleLimit), mIsLowerLimitViolated(false), mIsUpperLimitViolated(false), - mMotorSpeed(jointInfo.motorSpeed), mMaxMotorForce(jointInfo.maxMotorForce) { + mMotorSpeed(jointInfo.motorSpeed), mMaxMotorTorque(jointInfo.maxMotorTorque) { assert(mLowerLimit <= 0 && mLowerLimit >= -2.0 * PI); assert(mUpperLimit >= 0 && mUpperLimit <= 2.0 * PI); @@ -83,8 +83,8 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); // Get the inertia tensor of bodies - const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); - const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); + mI1 = mBody1->getInertiaTensorInverseWorld(); + mI2 = mBody2->getInertiaTensorInverseWorld(); // Compute the vector from body center to the anchor point in world-space mR1World = orientationBody1 * mLocalAnchorPointBody1; @@ -107,8 +107,6 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat mImpulseUpperLimit = 0.0; } - decimal testAngle = computeCurrentHingeAngle(orientationBody1, orientationBody2); - // Compute vectors needed in the Jacobian mA1 = orientationBody1 * mHingeLocalAxisBody1; Vector3 a2 = orientationBody2 * mHingeLocalAxisBody2; @@ -135,10 +133,10 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat 0, inverseMassBodies, 0, 0, 0, inverseMassBodies); if (mBody1->getIsMotionEnabled()) { - massMatrix += skewSymmetricMatrixU1 * I1 * skewSymmetricMatrixU1.getTranspose(); + massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose(); } if (mBody2->getIsMotionEnabled()) { - massMatrix += skewSymmetricMatrixU2 * I2 * skewSymmetricMatrixU2.getTranspose(); + massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); } mInverseMassMatrixTranslation.setToZero(); if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { @@ -158,12 +156,12 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat Vector3 I2B2CrossA1(0, 0, 0); Vector3 I2C2CrossA1(0, 0, 0); if (mBody1->getIsMotionEnabled()) { - I1B2CrossA1 = I1 * mB2CrossA1; - I1C2CrossA1 = I1 * mC2CrossA1; + I1B2CrossA1 = mI1 * mB2CrossA1; + I1C2CrossA1 = mI1 * mC2CrossA1; } if (mBody2->getIsMotionEnabled()) { - I2B2CrossA1 = I2 * mB2CrossA1; - I2C2CrossA1 = I2 * mC2CrossA1; + I2B2CrossA1 = mI2 * mB2CrossA1; + I2C2CrossA1 = mI2 * mC2CrossA1; } const decimal el11 = mB2CrossA1.dot(I1B2CrossA1) + mB2CrossA1.dot(I2B2CrossA1); @@ -201,10 +199,10 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) mInverseMassMatrixLimitMotor = 0.0; if (mBody1->getIsMotionEnabled()) { - mInverseMassMatrixLimitMotor += mA1.dot(I1 * mA1); + mInverseMassMatrixLimitMotor += mA1.dot(mI1 * mA1); } if (mBody2->getIsMotionEnabled()) { - mInverseMassMatrixLimitMotor += mA1.dot(I2 * mA1); + mInverseMassMatrixLimitMotor += mA1.dot(mI2 * mA1); } mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ? decimal(1.0) / mInverseMassMatrixLimitMotor : decimal(0.0); @@ -235,38 +233,53 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) { // Get the inverse mass and inverse inertia tensors of the bodies const decimal inverseMassBody1 = mBody1->getMassInverse(); const decimal inverseMassBody2 = mBody2->getMassInverse(); - const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); - const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); - - // Compute the impulse P=J^T * lambda for the 3 translation constraints - Vector3 linearImpulseBody1 = -mImpulseTranslation; - Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World); - Vector3 linearImpulseBody2 = mImpulseTranslation; - Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World); // Compute the impulse P=J^T * lambda for the 2 rotation constraints Vector3 rotationImpulse = -mB2CrossA1 * mImpulseRotation.x - mC2CrossA1 * mImpulseRotation.y; - angularImpulseBody1 += rotationImpulse; - angularImpulseBody2 += -rotationImpulse; // Compute the impulse P=J^T * lambda for the lower and upper limits constraints const Vector3 limitsImpulse = (mImpulseUpperLimit - mImpulseLowerLimit) * mA1; - angularImpulseBody1 += limitsImpulse; - angularImpulseBody2 += -limitsImpulse; // Compute the impulse P=J^T * lambda for the motor constraint const Vector3 motorImpulse = -mImpulseMotor * mA1; - angularImpulseBody1 += motorImpulse; - angularImpulseBody2 += -motorImpulse; - // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda for the 3 translation constraints + Vector3 linearImpulseBody1 = -mImpulseTranslation; + Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World); + + // Compute the impulse P=J^T * lambda for the 2 rotation constraints + angularImpulseBody1 += rotationImpulse; + + // Compute the impulse P=J^T * lambda for the lower and upper limits constraints + angularImpulseBody1 += limitsImpulse; + + // Compute the impulse P=J^T * lambda for the motor constraint + angularImpulseBody1 += motorImpulse; + + // Apply the impulse to the body v1 += inverseMassBody1 * linearImpulseBody1; - w1 += I1 * angularImpulseBody1; + w1 += mI1 * angularImpulseBody1; } if (mBody2->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda for the 3 translation constraints + Vector3 linearImpulseBody2 = mImpulseTranslation; + Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World); + + // Compute the impulse P=J^T * lambda for the 2 rotation constraints + angularImpulseBody2 += -rotationImpulse; + + // Compute the impulse P=J^T * lambda for the lower and upper limits constraints + angularImpulseBody2 += -limitsImpulse; + + // Compute the impulse P=J^T * lambda for the motor constraint + angularImpulseBody2 += -motorImpulse; + + // Apply the impulse to the body v2 += inverseMassBody2 * linearImpulseBody2; - w2 += I2 * angularImpulseBody2; + w2 += mI2 * angularImpulseBody2; } } @@ -282,8 +295,6 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS // Get the inverse mass and inverse inertia tensors of the bodies decimal inverseMassBody1 = mBody1->getMassInverse(); decimal inverseMassBody2 = mBody2->getMassInverse(); - Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); - Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); // --------------- Translation Constraints --------------- // @@ -295,20 +306,25 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS (-JvTranslation - mBTranslation); mImpulseTranslation += deltaLambdaTranslation; - // Compute the impulse P=J^T * lambda - Vector3 linearImpulseBody1 = -deltaLambdaTranslation; - Vector3 angularImpulseBody1 = deltaLambdaTranslation.cross(mR1World); - Vector3 linearImpulseBody2 = deltaLambdaTranslation; - Vector3 angularImpulseBody2 = -deltaLambdaTranslation.cross(mR2World); - - // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda + const Vector3 linearImpulseBody1 = -deltaLambdaTranslation; + const Vector3 angularImpulseBody1 = deltaLambdaTranslation.cross(mR1World); + + // Apply the impulse to the body v1 += inverseMassBody1 * linearImpulseBody1; - w1 += I1 * angularImpulseBody1; + w1 += mI1 * angularImpulseBody1; } if (mBody2->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda + const Vector3 linearImpulseBody2 = deltaLambdaTranslation; + const Vector3 angularImpulseBody2 = -deltaLambdaTranslation.cross(mR2World); + + // Apply the impulse to the body v2 += inverseMassBody2 * linearImpulseBody2; - w2 += I2 * angularImpulseBody2; + w2 += mI2 * angularImpulseBody2; } // --------------- Rotation Constraints --------------- // @@ -321,16 +337,23 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS Vector2 deltaLambdaRotation = mInverseMassMatrixRotation * (-JvRotation - mBRotation); mImpulseRotation += deltaLambdaRotation; - // Compute the impulse P=J^T * lambda for the 2 rotation constraints - angularImpulseBody1 = -mB2CrossA1 * deltaLambdaRotation.x - mC2CrossA1 * deltaLambdaRotation.y; - angularImpulseBody2 = -angularImpulseBody1; - - // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { - w1 += I1 * angularImpulseBody1; + + // Compute the impulse P=J^T * lambda for the 2 rotation constraints + const Vector3 angularImpulseBody1 = -mB2CrossA1 * deltaLambdaRotation.x - + mC2CrossA1 * deltaLambdaRotation.y; + + // Apply the impulse to the body + w1 += mI1 * angularImpulseBody1; } if (mBody2->getIsMotionEnabled()) { - w2 += I2 * angularImpulseBody2; + + // Compute the impulse P=J^T * lambda for the 2 rotation constraints + const Vector3 angularImpulseBody2 = mB2CrossA1 * deltaLambdaRotation.x + + mC2CrossA1 * deltaLambdaRotation.y; + + // Apply the impulse to the body + w2 += mI2 * angularImpulseBody2; } // --------------- Limits Constraints --------------- // @@ -344,21 +367,26 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS const decimal JvLowerLimit = (w2 - w1).dot(mA1); // Compute the Lagrange multiplier lambda for the lower limit constraint - decimal deltaLambdaLower = mInverseMassMatrixLimitMotor * (-JvLowerLimit - mBLowerLimit); + decimal deltaLambdaLower = mInverseMassMatrixLimitMotor * (-JvLowerLimit -mBLowerLimit); decimal lambdaTemp = mImpulseLowerLimit; mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0)); deltaLambdaLower = mImpulseLowerLimit - lambdaTemp; - // Compute the impulse P=J^T * lambda for the lower limit constraint - const Vector3 angularImpulseBody1 = -deltaLambdaLower * mA1; - const Vector3 angularImpulseBody2 = -angularImpulseBody1; - - // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { - w1 += I1 * angularImpulseBody1; + + // Compute the impulse P=J^T * lambda for the lower limit constraint + const Vector3 angularImpulseBody1 = -deltaLambdaLower * mA1; + + // Apply the impulse to the body + w1 += mI1 * angularImpulseBody1; } if (mBody2->getIsMotionEnabled()) { - w2 += I2 * angularImpulseBody2; + + // Compute the impulse P=J^T * lambda for the lower limit constraint + const Vector3 angularImpulseBody2 = deltaLambdaLower * mA1; + + // Apply the impulse to the body + w2 += mI2 * angularImpulseBody2; } } @@ -374,16 +402,21 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0)); deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp; - // Compute the impulse P=J^T * lambda for the upper limit constraint - const Vector3 angularImpulseBody1 = deltaLambdaUpper * mA1; - const Vector3 angularImpulseBody2 = -angularImpulseBody1; - - // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { - w1 += I1 * angularImpulseBody1; + + // Compute the impulse P=J^T * lambda for the upper limit constraint + const Vector3 angularImpulseBody1 = deltaLambdaUpper * mA1; + + // Apply the impulse to the body + w1 += mI1 * angularImpulseBody1; } if (mBody2->getIsMotionEnabled()) { - w2 += I2 * angularImpulseBody2; + + // Compute the impulse P=J^T * lambda for the upper limit constraint + const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mA1; + + // Apply the impulse to the body + w2 += mI2 * angularImpulseBody2; } } } @@ -396,29 +429,288 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS const decimal JvMotor = mA1.dot(w1 - w2); // Compute the Lagrange multiplier lambda for the motor - const decimal maxMotorImpulse = mMaxMotorForce * constraintSolverData.timeStep; + const decimal maxMotorImpulse = mMaxMotorTorque * constraintSolverData.timeStep; decimal deltaLambdaMotor = mInverseMassMatrixLimitMotor * (-JvMotor - mMotorSpeed); decimal lambdaTemp = mImpulseMotor; mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); deltaLambdaMotor = mImpulseMotor - lambdaTemp; - // Compute the impulse P=J^T * lambda for the motor - const Vector3 angularImpulseBody1 = -deltaLambdaMotor * mA1; - const Vector3 angularImpulseBody2 = -angularImpulseBody1; - - // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { - w1 += I1 * angularImpulseBody1; + + // Compute the impulse P=J^T * lambda for the motor + const Vector3 angularImpulseBody1 = -deltaLambdaMotor * mA1; + + // Apply the impulse to the body + w1 += mI1 * angularImpulseBody1; } if (mBody2->getIsMotionEnabled()) { - w2 += I2 * angularImpulseBody2; + + // Compute the impulse P=J^T * lambda for the motor + const Vector3 angularImpulseBody2 = deltaLambdaMotor * mA1; + + // Apply the impulse to the body + w2 += mI2 * angularImpulseBody2; } } } -// Solve the position constraint +// Solve the position constraint (for position error correction) void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) { + // If the error position correction technique is not the non-linear-gauss-seidel, we do + // do not execute this method + if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return; + + // Get the bodies positions and orientations + Vector3& x1 = constraintSolverData.positions[mIndexBody1]; + Vector3& x2 = constraintSolverData.positions[mIndexBody2]; + Quaternion& q1 = constraintSolverData.orientations[mIndexBody1]; + Quaternion& q2 = constraintSolverData.orientations[mIndexBody2]; + + // Get the inverse mass and inverse inertia tensors of the bodies + decimal inverseMassBody1 = mBody1->getMassInverse(); + decimal inverseMassBody2 = mBody2->getMassInverse(); + + // Recompute the inverse inertia tensors + mI1 = mBody1->getInertiaTensorInverseWorld(); + mI2 = mBody2->getInertiaTensorInverseWorld(); + + // Compute the vector from body center to the anchor point in world-space + mR1World = q1 * mLocalAnchorPointBody1; + mR2World = q2 * mLocalAnchorPointBody2; + + // Compute the current angle around the hinge axis + decimal hingeAngle = computeCurrentHingeAngle(q1, q2); + + // Check if the limit constraints are violated or not + decimal lowerLimitError = hingeAngle - mLowerLimit; + decimal upperLimitError = mUpperLimit - hingeAngle; + mIsLowerLimitViolated = lowerLimitError <= 0; + mIsUpperLimitViolated = upperLimitError <= 0; + + // Compute vectors needed in the Jacobian + mA1 = q1 * mHingeLocalAxisBody1; + Vector3 a2 = q2 * mHingeLocalAxisBody2; + mA1.normalize(); + a2.normalize(); + const Vector3 b2 = a2.getOneUnitOrthogonalVector(); + const Vector3 c2 = a2.cross(b2); + mB2CrossA1 = b2.cross(mA1); + mC2CrossA1 = c2.cross(mA1); + + // Compute the corresponding skew-symmetric matrices + Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); + Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); + + // --------------- Translation Constraints --------------- // + + // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints + decimal inverseMassBodies = 0.0; + if (mBody1->getIsMotionEnabled()) { + inverseMassBodies += mBody1->getMassInverse(); + } + if (mBody2->getIsMotionEnabled()) { + inverseMassBodies += mBody2->getMassInverse(); + } + Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, + 0, inverseMassBodies, 0, + 0, 0, inverseMassBodies); + if (mBody1->getIsMotionEnabled()) { + massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose(); + } + if (mBody2->getIsMotionEnabled()) { + massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); + } + mInverseMassMatrixTranslation.setToZero(); + if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + mInverseMassMatrixTranslation = massMatrix.getInverse(); + } + + // Compute position error for the 3 translation constraints + const Vector3 errorTranslation = x2 + mR2World - x1 - mR1World; + + // Compute the Lagrange multiplier lambda + const Vector3 lambdaTranslation = mInverseMassMatrixTranslation * (-errorTranslation); + + // Apply the impulse to the bodies of the joint + if (mBody1->getIsMotionEnabled()) { + + // Compute the impulse + Vector3 linearImpulseBody1 = -lambdaTranslation; + Vector3 angularImpulseBody1 = lambdaTranslation.cross(mR1World); + + // Compute the pseudo velocity + const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; + const Vector3 w1 = mI1 * angularImpulseBody1; + + // Update the body position/orientation + x1 += v1; + q1 += Quaternion(0, w1) * q1 * decimal(0.5); + q1.normalize(); + } + if (mBody2->getIsMotionEnabled()) { + + // Compute the impulse + Vector3 linearImpulseBody2 = lambdaTranslation; + Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mR2World); + + // Compute the pseudo velocity + const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; + const Vector3 w2 = mI2 * angularImpulseBody2; + + // Update the body position/orientation + x2 += v2; + q2 += Quaternion(0, w2) * q2 * decimal(0.5); + q2.normalize(); + } + + // --------------- Rotation Constraints --------------- // + + // Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix) + Vector3 I1B2CrossA1(0, 0, 0); + Vector3 I1C2CrossA1(0, 0, 0); + Vector3 I2B2CrossA1(0, 0, 0); + Vector3 I2C2CrossA1(0, 0, 0); + if (mBody1->getIsMotionEnabled()) { + I1B2CrossA1 = mI1 * mB2CrossA1; + I1C2CrossA1 = mI1 * mC2CrossA1; + } + if (mBody2->getIsMotionEnabled()) { + I2B2CrossA1 = mI2 * mB2CrossA1; + I2C2CrossA1 = mI2 * mC2CrossA1; + } + const decimal el11 = mB2CrossA1.dot(I1B2CrossA1) + + mB2CrossA1.dot(I2B2CrossA1); + const decimal el12 = mB2CrossA1.dot(I1C2CrossA1) + + mB2CrossA1.dot(I2C2CrossA1); + const decimal el21 = mC2CrossA1.dot(I1B2CrossA1) + + mC2CrossA1.dot(I2B2CrossA1); + const decimal el22 = mC2CrossA1.dot(I1C2CrossA1) + + mC2CrossA1.dot(I2C2CrossA1); + const Matrix2x2 matrixKRotation(el11, el12, el21, el22); + mInverseMassMatrixRotation.setToZero(); + if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + mInverseMassMatrixRotation = matrixKRotation.getInverse(); + } + + // Compute the position error for the 3 rotation constraints + const Vector2 errorRotation = Vector2(mA1.dot(b2), mA1.dot(c2)); + + // Compute the Lagrange multiplier lambda for the 3 rotation constraints + Vector2 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation); + + // Apply the impulse to the bodies of the joint + if (mBody1->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints + const Vector3 angularImpulseBody1 = -mB2CrossA1 * lambdaRotation.x - + mC2CrossA1 * lambdaRotation.y; + + // Compute the pseudo velocity + const Vector3 w1 = mI1 * angularImpulseBody1; + + // Update the body position/orientation + q1 += Quaternion(0, w1) * q1 * decimal(0.5); + q1.normalize(); + } + if (mBody2->getIsMotionEnabled()) { + + // Compute the impulse + const Vector3 angularImpulseBody2 = mB2CrossA1 * lambdaRotation.x + + mC2CrossA1 * lambdaRotation.y; + + // Compute the pseudo velocity + const Vector3 w2 = mI2 * angularImpulseBody2; + + // Update the body position/orientation + q2 += Quaternion(0, w2) * q2 * decimal(0.5); + q2.normalize(); + } + + // --------------- Limits Constraints --------------- // + + if (mIsLimitEnabled) { + + if (mIsLowerLimitViolated || mIsUpperLimitViolated) { + + // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) + mInverseMassMatrixLimitMotor = 0.0; + if (mBody1->getIsMotionEnabled()) { + mInverseMassMatrixLimitMotor += mA1.dot(mI1 * mA1); + } + if (mBody2->getIsMotionEnabled()) { + mInverseMassMatrixLimitMotor += mA1.dot(mI2 * mA1); + } + mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ? + decimal(1.0) / mInverseMassMatrixLimitMotor : decimal(0.0); + } + + // If the lower limit is violated + if (mIsLowerLimitViolated) { + + // Compute the Lagrange multiplier lambda for the lower limit constraint + decimal lambdaLowerLimit = mInverseMassMatrixLimitMotor * (-lowerLimitError ); + + // Apply the impulse to the bodies of the joint + if (mBody1->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda + const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mA1; + + // Compute the pseudo velocity + const Vector3 w1 = mI1 * angularImpulseBody1; + + // Update the body position/orientation + q1 += Quaternion(0, w1) * q1 * decimal(0.5); + q1.normalize(); + } + if (mBody2->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda + const Vector3 angularImpulseBody2 = lambdaLowerLimit * mA1; + + // Compute the pseudo velocity + const Vector3 w2 = mI2 * angularImpulseBody2; + + // Update the body position/orientation + q2 += Quaternion(0, w2) * q2 * decimal(0.5); + q2.normalize(); + } + } + + // If the upper limit is violated + if (mIsUpperLimitViolated) { + + // Compute the Lagrange multiplier lambda for the upper limit constraint + decimal lambdaUpperLimit = mInverseMassMatrixLimitMotor * (-upperLimitError); + + // Apply the impulse to the bodies of the joint + if (mBody1->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda + const Vector3 angularImpulseBody1 = lambdaUpperLimit * mA1; + + // Compute the pseudo velocity + const Vector3 w1 = mI1 * angularImpulseBody1; + + // Update the body position/orientation + q1 += Quaternion(0, w1) * q1 * decimal(0.5); + q1.normalize(); + } + if (mBody2->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda + const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mA1; + + // Compute the pseudo velocity + const Vector3 w2 = mI2 * angularImpulseBody2; + + // Update the body position/orientation + q2 += Quaternion(0, w2) * q2 * decimal(0.5); + q2.normalize(); + } + } + } } @@ -492,13 +784,13 @@ void HingeJoint::setMotorSpeed(decimal motorSpeed) { } } -// Set the maximum motor force -void HingeJoint::setMaxMotorForce(decimal maxMotorForce) { +// Set the maximum motor torque +void HingeJoint::setMaxMotorTorque(decimal maxMotorTorque) { - if (maxMotorForce != mMaxMotorForce) { + if (maxMotorTorque != mMaxMotorTorque) { - assert(mMaxMotorForce >= 0.0); - mMaxMotorForce = maxMotorForce; + assert(mMaxMotorTorque >= 0.0); + mMaxMotorTorque = maxMotorTorque; // TODO : Wake up the bodies of the joint here when sleeping is implemented } diff --git a/src/constraint/HingeJoint.h b/src/constraint/HingeJoint.h index aa258159..33962f0c 100644 --- a/src/constraint/HingeJoint.h +++ b/src/constraint/HingeJoint.h @@ -66,8 +66,9 @@ struct HingeJointInfo : public ConstraintInfo { /// Motor speed (in radian/second) decimal motorSpeed; - /// Maximum motor force (in Newton) that can be applied to reach to desired motor speed - decimal maxMotorForce; + /// Maximum motor torque (in Newtons * meters) that can be applied to reach + /// to desired motor speed + decimal maxMotorTorque; /// Constructor without limits and without motor HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, @@ -76,8 +77,8 @@ struct HingeJointInfo : public ConstraintInfo { : ConstraintInfo(rigidBody1, rigidBody2, HINGEJOINT), anchorPointWorldSpace(initAnchorPointWorldSpace), rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(false), - isMotorEnabled(false), minAngleLimit(-1), maxAngleLimit(1), motorSpeed(0), - maxMotorForce(0) {} + isMotorEnabled(false), minAngleLimit(-1), maxAngleLimit(1), + motorSpeed(0), maxMotorTorque(0) {} /// Constructor with limits but without motor HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, @@ -88,20 +89,21 @@ struct HingeJointInfo : public ConstraintInfo { anchorPointWorldSpace(initAnchorPointWorldSpace), rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true), isMotorEnabled(false), minAngleLimit(initMinAngleLimit), - maxAngleLimit(initMaxAngleLimit), motorSpeed(0), maxMotorForce(0) {} + maxAngleLimit(initMaxAngleLimit), motorSpeed(0), + maxMotorTorque(0) {} /// Constructor with limits and motor HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, const Vector3& initAnchorPointWorldSpace, const Vector3& initRotationAxisWorld, decimal initMinAngleLimit, decimal initMaxAngleLimit, - decimal initMotorSpeed, decimal initMaxMotorForce) + decimal initMotorSpeed, decimal initMaxMotorTorque) : ConstraintInfo(rigidBody1, rigidBody2, HINGEJOINT), anchorPointWorldSpace(initAnchorPointWorldSpace), rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true), isMotorEnabled(false), minAngleLimit(initMinAngleLimit), maxAngleLimit(initMaxAngleLimit), motorSpeed(initMotorSpeed), - maxMotorForce(initMaxMotorForce) {} + maxMotorTorque(initMaxMotorTorque) {} }; // Class HingeJoint @@ -132,6 +134,12 @@ class HingeJoint : public Constraint { /// Hinge rotation axis (in local-space coordiantes of body 2) Vector3 mHingeLocalAxisBody2; + /// Inertia tensor of body 1 (in world-space coordinates) + Matrix3x3 mI1; + + /// Inertia tensor of body 2 (in world-space coordinates) + Matrix3x3 mI2; + /// Hinge rotation axis (in world-space coordinates) computed from body 1 Vector3 mA1; @@ -210,15 +218,16 @@ class HingeJoint : public Constraint { /// Motor speed decimal mMotorSpeed; - /// Maximum motor force (in Newton) that can be applied to reach to desired motor speed - decimal mMaxMotorForce; + /// Maximum motor torque (in Newtons) that can be applied to reach to desired motor speed + decimal mMaxMotorTorque; // -------------------- Methods -------------------- // /// Reset the limits void resetLimits(); - /// Given an angle in radian, this method returns the corresponding angle in the range [-pi; pi] + /// Given an angle in radian, this method returns the corresponding + /// angle in the range [-pi; pi] decimal computeNormalizedAngle(decimal angle) const; /// Given an "inputAngle" in the range [-pi, pi], this method returns an @@ -271,14 +280,14 @@ class HingeJoint : public Constraint { /// Set the motor speed void setMotorSpeed(decimal motorSpeed); - /// Return the maximum motor force - decimal getMaxMotorForce() const; + /// Return the maximum motor torque + decimal getMaxMotorTorque() const; - /// Set the maximum motor force - void setMaxMotorForce(decimal maxMotorForce); + /// Set the maximum motor torque + void setMaxMotorTorque(decimal maxMotorTorque); - /// Return the intensity of the current force applied for the joint motor - decimal getMotorForce(decimal timeStep) const; + /// Return the intensity of the current torque applied for the joint motor + decimal getMotorTorque(decimal timeStep) const; /// Return the number of bytes used by the joint virtual size_t getSizeInBytes() const; @@ -292,7 +301,7 @@ class HingeJoint : public Constraint { /// Solve the velocity constraint virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); - /// Solve the position constraint + /// Solve the position constraint (for position error correction) virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); }; @@ -321,13 +330,13 @@ inline decimal HingeJoint::getMotorSpeed() const { return mMotorSpeed; } -// Return the maximum motor force -inline decimal HingeJoint::getMaxMotorForce() const { - return mMaxMotorForce; +// Return the maximum motor torque +inline decimal HingeJoint::getMaxMotorTorque() const { + return mMaxMotorTorque; } -// Return the intensity of the current force applied for the joint motor -inline decimal HingeJoint::getMotorForce(decimal timeStep) const { +// Return the intensity of the current torque applied for the joint motor +inline decimal HingeJoint::getMotorTorque(decimal timeStep) const { return mImpulseMotor / timeStep; } diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index c9586a61..e1f2daed 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -36,9 +36,10 @@ SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) : Constraint(jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0), mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0), mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled), - mLowerLimit(jointInfo.minTranslationLimit), mUpperLimit(jointInfo.maxTranslationLimit), - mIsLowerLimitViolated(false), mIsUpperLimitViolated(false), - mMotorSpeed(jointInfo.motorSpeed), mMaxMotorForce(jointInfo.maxMotorForce){ + mLowerLimit(jointInfo.minTranslationLimit), + mUpperLimit(jointInfo.maxTranslationLimit), mIsLowerLimitViolated(false), + mIsUpperLimitViolated(false), mMotorSpeed(jointInfo.motorSpeed), + mMaxMotorForce(jointInfo.maxMotorForce){ assert(mUpperLimit >= 0.0); assert(mLowerLimit <= 0.0); @@ -81,8 +82,8 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); // Get the inertia tensor of bodies - const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); - const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); + mI1 = mBody1->getInertiaTensorInverseWorld(); + mI2 = mBody2->getInertiaTensorInverseWorld(); // Vector from body center to the anchor point mR1 = orientationBody1 * mLocalAnchorPointBody1; @@ -130,13 +131,13 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa Vector3 I2R2CrossN2(0, 0, 0); if (mBody1->getIsMotionEnabled()) { sumInverseMass += mBody1->getMassInverse(); - I1R1PlusUCrossN1 = I1 * mR1PlusUCrossN1; - I1R1PlusUCrossN2 = I1 * mR1PlusUCrossN2; + I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1; + I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2; } if (mBody2->getIsMotionEnabled()) { sumInverseMass += mBody2->getMassInverse(); - I2R2CrossN1 = I2 * mR2CrossN1; - I2R2CrossN2 = I2 * mR2CrossN2; + I2R2CrossN1 = mI2 * mR2CrossN1; + I2R2CrossN2 = mI2 * mR2CrossN2; } const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + mR2CrossN1.dot(I2R2CrossN1); @@ -165,10 +166,10 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa // contraints (3x3 matrix) mInverseMassMatrixRotationConstraint.setToZero(); if (mBody1->getIsMotionEnabled()) { - mInverseMassMatrixRotationConstraint += I1; + mInverseMassMatrixRotationConstraint += mI1; } if (mBody2->getIsMotionEnabled()) { - mInverseMassMatrixRotationConstraint += I2; + mInverseMassMatrixRotationConstraint += mI2; } if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse(); @@ -189,11 +190,11 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa mInverseMassMatrixLimit = 0.0; if (mBody1->getIsMotionEnabled()) { mInverseMassMatrixLimit += mBody1->getMassInverse() + - mR1PlusUCrossSliderAxis.dot(I1 * mR1PlusUCrossSliderAxis); + mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis); } if (mBody2->getIsMotionEnabled()) { mInverseMassMatrixLimit += mBody2->getMassInverse() + - mR2CrossSliderAxis.dot(I2 * mR2CrossSliderAxis); + mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis); } mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ? decimal(1.0) / mInverseMassMatrixLimit : decimal(0.0); @@ -246,42 +247,55 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { // Get the inverse mass and inverse inertia tensors of the bodies const decimal inverseMassBody1 = mBody1->getMassInverse(); const decimal inverseMassBody2 = mBody2->getMassInverse(); - const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); - const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); - - // Compute the impulse P=J^T * lambda for the 2 translation constraints - Vector3 linearImpulseBody1 = -mN1 * mImpulseTranslation.x - mN2 * mImpulseTranslation.y; - Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * mImpulseTranslation.x - - mR1PlusUCrossN2 * mImpulseTranslation.y; - Vector3 linearImpulseBody2 = -linearImpulseBody1; - Vector3 angularImpulseBody2 = mR2CrossN1 * mImpulseTranslation.x + - mR2CrossN2 * mImpulseTranslation.y; - - // Compute the impulse P=J^T * lambda for the 3 rotation constraints - angularImpulseBody1 += -mImpulseRotation; - angularImpulseBody2 += mImpulseRotation; // Compute the impulse P=J^T * lambda for the lower and upper limits constraints decimal impulseLimits = mImpulseUpperLimit - mImpulseLowerLimit; Vector3 linearImpulseLimits = impulseLimits * mSliderAxisWorld; - linearImpulseBody1 += linearImpulseLimits; - angularImpulseBody1 += impulseLimits * mR1PlusUCrossSliderAxis; - linearImpulseBody2 += -linearImpulseLimits; - angularImpulseBody2 += -impulseLimits * mR2CrossSliderAxis; // Compute the impulse P=J^T * lambda for the motor constraint Vector3 impulseMotor = mImpulseMotor * mSliderAxisWorld; - linearImpulseBody1 += impulseMotor; - linearImpulseBody2 += -impulseMotor; - // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda for the 2 translation constraints + Vector3 linearImpulseBody1 = -mN1 * mImpulseTranslation.x - mN2 * mImpulseTranslation.y; + Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * mImpulseTranslation.x - + mR1PlusUCrossN2 * mImpulseTranslation.y; + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints + angularImpulseBody1 += -mImpulseRotation; + + // Compute the impulse P=J^T * lambda for the lower and upper limits constraints + linearImpulseBody1 += linearImpulseLimits; + angularImpulseBody1 += impulseLimits * mR1PlusUCrossSliderAxis; + + // Compute the impulse P=J^T * lambda for the motor constraint + linearImpulseBody1 += impulseMotor; + + // Apply the impulse to the body v1 += inverseMassBody1 * linearImpulseBody1; - w1 += I1 * angularImpulseBody1; + w1 += mI1 * angularImpulseBody1; } if (mBody2->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda for the 2 translation constraints + Vector3 linearImpulseBody2 = mN1 * mImpulseTranslation.x + mN2 * mImpulseTranslation.y; + Vector3 angularImpulseBody2 = mR2CrossN1 * mImpulseTranslation.x + + mR2CrossN2 * mImpulseTranslation.y; + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints + angularImpulseBody2 += mImpulseRotation; + + // Compute the impulse P=J^T * lambda for the lower and upper limits constraints + linearImpulseBody2 += -linearImpulseLimits; + angularImpulseBody2 += -impulseLimits * mR2CrossSliderAxis; + + // Compute the impulse P=J^T * lambda for the motor constraint + linearImpulseBody2 += -impulseMotor; + + // Apply the impulse to the body v2 += inverseMassBody2 * linearImpulseBody2; - w2 += I2 * angularImpulseBody2; + w2 += mI2 * angularImpulseBody2; } } @@ -297,8 +311,6 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint // Get the inverse mass and inverse inertia tensors of the bodies decimal inverseMassBody1 = mBody1->getMassInverse(); decimal inverseMassBody2 = mBody2->getMassInverse(); - const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); - const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); // --------------- Translation Constraints --------------- // @@ -313,20 +325,26 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint Vector2 deltaLambda = mInverseMassMatrixTranslationConstraint * (-JvTranslation -mBTranslation); mImpulseTranslation += deltaLambda; - // Compute the impulse P=J^T * lambda for the 2 translation constraints - Vector3 linearImpulseBody1 = -mN1 * deltaLambda.x - mN2 * deltaLambda.y; - Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * deltaLambda.x -mR1PlusUCrossN2 * deltaLambda.y; - Vector3 linearImpulseBody2 = -linearImpulseBody1; - Vector3 angularImpulseBody2 = mR2CrossN1 * deltaLambda.x + mR2CrossN2 * deltaLambda.y; - - // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda for the 2 translation constraints + const Vector3 linearImpulseBody1 = -mN1 * deltaLambda.x - mN2 * deltaLambda.y; + const Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * deltaLambda.x - + mR1PlusUCrossN2 * deltaLambda.y; + + // Apply the impulse to the body v1 += inverseMassBody1 * linearImpulseBody1; - w1 += I1 * angularImpulseBody1; + w1 += mI1 * angularImpulseBody1; } if (mBody2->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda for the 2 translation constraints + const Vector3 linearImpulseBody2 = mN1 * deltaLambda.x + mN2 * deltaLambda.y; + const Vector3 angularImpulseBody2 = mR2CrossN1 * deltaLambda.x + mR2CrossN2 * deltaLambda.y; + + // Apply the impulse to the body v2 += inverseMassBody2 * linearImpulseBody2; - w2 += I2 * angularImpulseBody2; + w2 += mI2 * angularImpulseBody2; } // --------------- Rotation Constraints --------------- // @@ -338,16 +356,21 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint Vector3 deltaLambda2 = mInverseMassMatrixRotationConstraint * (-JvRotation - mBRotation); mImpulseRotation += deltaLambda2; - // Compute the impulse P=J^T * lambda for the 3 rotation constraints - angularImpulseBody1 = -deltaLambda2; - angularImpulseBody2 = deltaLambda2; - - // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { - w1 += I1 * angularImpulseBody1; + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints + const Vector3 angularImpulseBody1 = -deltaLambda2; + + // Apply the impulse to the body + w1 += mI1 * angularImpulseBody1; } if (mBody2->getIsMotionEnabled()) { - w2 += I2 * angularImpulseBody2; + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints + const Vector3 angularImpulseBody2 = deltaLambda2; + + // Apply the impulse to the body + w2 += mI2 * angularImpulseBody2; } // --------------- Limits Constraints --------------- // @@ -367,20 +390,25 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0)); deltaLambdaLower = mImpulseLowerLimit - lambdaTemp; - // Compute the impulse P=J^T * lambda for the lower limit constraint - const Vector3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld; - const Vector3 angularImpulseBody1 = -deltaLambdaLower * mR1PlusUCrossSliderAxis; - const Vector3 linearImpulseBody2 = -linearImpulseBody1; - const Vector3 angularImpulseBody2 = deltaLambdaLower * mR2CrossSliderAxis; - - // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda for the lower limit constraint + const Vector3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld; + const Vector3 angularImpulseBody1 = -deltaLambdaLower * mR1PlusUCrossSliderAxis; + + // Apply the impulse to the body v1 += inverseMassBody1 * linearImpulseBody1; - w1 += I1 * angularImpulseBody1; + w1 += mI1 * angularImpulseBody1; } if (mBody2->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda for the lower limit constraint + const Vector3 linearImpulseBody2 = deltaLambdaLower * mSliderAxisWorld; + const Vector3 angularImpulseBody2 = deltaLambdaLower * mR2CrossSliderAxis; + + // Apply the impulse to the body v2 += inverseMassBody2 * linearImpulseBody2; - w2 += I2 * angularImpulseBody2; + w2 += mI2 * angularImpulseBody2; } } @@ -397,20 +425,25 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0)); deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp; - // Compute the impulse P=J^T * lambda for the upper limit constraint - const Vector3 linearImpulseBody1 = deltaLambdaUpper * mSliderAxisWorld; - const Vector3 angularImpulseBody1 = deltaLambdaUpper * mR1PlusUCrossSliderAxis; - const Vector3 linearImpulseBody2 = -linearImpulseBody1; - const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mR2CrossSliderAxis; - - // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda for the upper limit constraint + const Vector3 linearImpulseBody1 = deltaLambdaUpper * mSliderAxisWorld; + const Vector3 angularImpulseBody1 = deltaLambdaUpper * mR1PlusUCrossSliderAxis; + + // Apply the impulse to the body v1 += inverseMassBody1 * linearImpulseBody1; - w1 += I1 * angularImpulseBody1; + w1 += mI1 * angularImpulseBody1; } if (mBody2->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda for the upper limit constraint + const Vector3 linearImpulseBody2 = -deltaLambdaUpper * mSliderAxisWorld; + const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mR2CrossSliderAxis; + + // Apply the impulse to the body v2 += inverseMassBody2 * linearImpulseBody2; - w2 += I2 * angularImpulseBody2; + w2 += mI2 * angularImpulseBody2; } } } @@ -429,23 +462,292 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); deltaLambdaMotor = mImpulseMotor - lambdaTemp; - // Compute the impulse P=J^T * lambda for the motor - const Vector3 linearImpulseBody1 = deltaLambdaMotor * mSliderAxisWorld; - const Vector3 linearImpulseBody2 = -linearImpulseBody1; - - // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda for the motor + const Vector3 linearImpulseBody1 = deltaLambdaMotor * mSliderAxisWorld; + + // Apply the impulse to the body v1 += inverseMassBody1 * linearImpulseBody1; } if (mBody2->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda for the motor + const Vector3 linearImpulseBody2 = -deltaLambdaMotor * mSliderAxisWorld; + + // Apply the impulse to the body v2 += inverseMassBody2 * linearImpulseBody2; } } } -// Solve the position constraint +// Solve the position constraint (for position error correction) void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) { + // If the error position correction technique is not the non-linear-gauss-seidel, we do + // do not execute this method + if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return; + + // Get the bodies positions and orientations + Vector3& x1 = constraintSolverData.positions[mIndexBody1]; + Vector3& x2 = constraintSolverData.positions[mIndexBody2]; + Quaternion& q1 = constraintSolverData.orientations[mIndexBody1]; + Quaternion& q2 = constraintSolverData.orientations[mIndexBody2]; + + // Get the inverse mass and inverse inertia tensors of the bodies + decimal inverseMassBody1 = mBody1->getMassInverse(); + decimal inverseMassBody2 = mBody2->getMassInverse(); + + // Recompute the inertia tensor of bodies + mI1 = mBody1->getInertiaTensorInverseWorld(); + mI2 = mBody2->getInertiaTensorInverseWorld(); + + // Vector from body center to the anchor point + mR1 = q1 * mLocalAnchorPointBody1; + mR2 = q2 * mLocalAnchorPointBody2; + + // Compute the vector u (difference between anchor points) + const Vector3 u = x2 + mR2 - x1 - mR1; + + // Compute the two orthogonal vectors to the slider axis in world-space + mSliderAxisWorld = q1 * mSliderAxisBody1; + mSliderAxisWorld.normalize(); + mN1 = mSliderAxisWorld.getOneUnitOrthogonalVector(); + mN2 = mSliderAxisWorld.cross(mN1); + + // Check if the limit constraints are violated or not + decimal uDotSliderAxis = u.dot(mSliderAxisWorld); + decimal lowerLimitError = uDotSliderAxis - mLowerLimit; + decimal upperLimitError = mUpperLimit - uDotSliderAxis; + mIsLowerLimitViolated = lowerLimitError <= 0; + mIsUpperLimitViolated = upperLimitError <= 0; + + // Compute the cross products used in the Jacobians + mR2CrossN1 = mR2.cross(mN1); + mR2CrossN2 = mR2.cross(mN2); + mR2CrossSliderAxis = mR2.cross(mSliderAxisWorld); + const Vector3 r1PlusU = mR1 + u; + mR1PlusUCrossN1 = (r1PlusU).cross(mN1); + mR1PlusUCrossN2 = (r1PlusU).cross(mN2); + mR1PlusUCrossSliderAxis = (r1PlusU).cross(mSliderAxisWorld); + + // --------------- Translation Constraints --------------- // + + // Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation + // constraints (2x2 matrix) + decimal sumInverseMass = 0.0; + Vector3 I1R1PlusUCrossN1(0, 0, 0); + Vector3 I1R1PlusUCrossN2(0, 0, 0); + Vector3 I2R2CrossN1(0, 0, 0); + Vector3 I2R2CrossN2(0, 0, 0); + if (mBody1->getIsMotionEnabled()) { + sumInverseMass += mBody1->getMassInverse(); + I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1; + I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2; + } + if (mBody2->getIsMotionEnabled()) { + sumInverseMass += mBody2->getMassInverse(); + I2R2CrossN1 = mI2 * mR2CrossN1; + I2R2CrossN2 = mI2 * mR2CrossN2; + } + const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + + mR2CrossN1.dot(I2R2CrossN1); + const decimal el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + + mR2CrossN1.dot(I2R2CrossN2); + const decimal el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) + + mR2CrossN2.dot(I2R2CrossN1); + const decimal el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + + mR2CrossN2.dot(I2R2CrossN2); + Matrix2x2 matrixKTranslation(el11, el12, el21, el22); + mInverseMassMatrixTranslationConstraint.setToZero(); + if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); + } + + // Compute the position error for the 2 translation constraints + const Vector2 translationError(u.dot(mN1), u.dot(mN2)); + + // Compute the Lagrange multiplier lambda for the 2 translation constraints + Vector2 lambdaTranslation = mInverseMassMatrixTranslationConstraint * (-translationError); + + if (mBody1->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda for the 2 translation constraints + const Vector3 linearImpulseBody1 = -mN1 * lambdaTranslation.x - mN2 * lambdaTranslation.y; + const Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * lambdaTranslation.x - + mR1PlusUCrossN2 * lambdaTranslation.y; + + // Apply the impulse to the body + const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; + const Vector3 w1 = mI1 * angularImpulseBody1; + + // Update the body position/orientation + x1 += v1; + q1 += Quaternion(0, w1) * q1 * decimal(0.5); + q1.normalize(); + } + if (mBody2->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda for the 2 translation constraints + const Vector3 linearImpulseBody2 = mN1 * lambdaTranslation.x + mN2 * lambdaTranslation.y; + const Vector3 angularImpulseBody2 = mR2CrossN1 * lambdaTranslation.x + + mR2CrossN2 * lambdaTranslation.y; + + // Apply the impulse to the body + const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; + const Vector3 w2 = mI2 * angularImpulseBody2; + + // Update the body position/orientation + x2 += v2; + q2 += Quaternion(0, w2) * q2 * decimal(0.5); + q2.normalize(); + } + + // --------------- Rotation Constraints --------------- // + + // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation + // contraints (3x3 matrix) + mInverseMassMatrixRotationConstraint.setToZero(); + if (mBody1->getIsMotionEnabled()) { + mInverseMassMatrixRotationConstraint += mI1; + } + if (mBody2->getIsMotionEnabled()) { + mInverseMassMatrixRotationConstraint += mI2; + } + if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse(); + } + + // Compute the position error for the 3 rotation constraints + Quaternion currentOrientationDifference = q2 * q1.getInverse(); + currentOrientationDifference.normalize(); + const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv; + const Vector3 errorRotation = decimal(2.0) * qError.getVectorV(); + + // Compute the Lagrange multiplier lambda for the 3 rotation constraints + Vector3 lambdaRotation = mInverseMassMatrixRotationConstraint * (-errorRotation); + + if (mBody1->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints + const Vector3 angularImpulseBody1 = -lambdaRotation; + + // Apply the impulse to the body + const Vector3 w1 = mI1 * angularImpulseBody1; + + // Update the body position/orientation + q1 += Quaternion(0, w1) * q1 * decimal(0.5); + q1.normalize(); + } + if (mBody2->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints + const Vector3 angularImpulseBody2 = lambdaRotation; + + // Apply the impulse to the body + const Vector3 w2 = mI2 * angularImpulseBody2; + + // Update the body position/orientation + q2 += Quaternion(0, w2) * q2 * decimal(0.5); + q2.normalize(); + } + + // --------------- Limits Constraints --------------- // + + if (mIsLimitEnabled) { + + if (mIsLowerLimitViolated || mIsUpperLimitViolated) { + + // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) + mInverseMassMatrixLimit = 0.0; + if (mBody1->getIsMotionEnabled()) { + mInverseMassMatrixLimit += mBody1->getMassInverse() + + mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis); + } + if (mBody2->getIsMotionEnabled()) { + mInverseMassMatrixLimit += mBody2->getMassInverse() + + mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis); + } + mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ? + decimal(1.0) / mInverseMassMatrixLimit : decimal(0.0); + } + + // If the lower limit is violated + if (mIsLowerLimitViolated) { + + // Compute the Lagrange multiplier lambda for the lower limit constraint + decimal lambdaLowerLimit = mInverseMassMatrixLimit * (-lowerLimitError); + + if (mBody1->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda for the lower limit constraint + const Vector3 linearImpulseBody1 = -lambdaLowerLimit * mSliderAxisWorld; + const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mR1PlusUCrossSliderAxis; + + // Apply the impulse to the body + const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; + const Vector3 w1 = mI1 * angularImpulseBody1; + + // Update the body position/orientation + x1 += v1; + q1 += Quaternion(0, w1) * q1 * decimal(0.5); + q1.normalize(); + } + if (mBody2->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda for the lower limit constraint + const Vector3 linearImpulseBody2 = lambdaLowerLimit * mSliderAxisWorld; + const Vector3 angularImpulseBody2 = lambdaLowerLimit * mR2CrossSliderAxis; + + // Apply the impulse to the body + const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; + const Vector3 w2 = mI2 * angularImpulseBody2; + + // Update the body position/orientation + x2 += v2; + q2 += Quaternion(0, w2) * q2 * decimal(0.5); + q2.normalize(); + } + } + + // If the upper limit is violated + if (mIsUpperLimitViolated) { + + // Compute the Lagrange multiplier lambda for the upper limit constraint + decimal lambdaUpperLimit = mInverseMassMatrixLimit * (-upperLimitError); + + if (mBody1->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda for the upper limit constraint + const Vector3 linearImpulseBody1 = lambdaUpperLimit * mSliderAxisWorld; + const Vector3 angularImpulseBody1 = lambdaUpperLimit * mR1PlusUCrossSliderAxis; + + // Apply the impulse to the body + const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; + const Vector3 w1 = mI1 * angularImpulseBody1; + + // Update the body position/orientation + x1 += v1; + q1 += Quaternion(0, w1) * q1 * decimal(0.5); + q1.normalize(); + } + if (mBody2->getIsMotionEnabled()) { + + // Compute the impulse P=J^T * lambda for the upper limit constraint + const Vector3 linearImpulseBody2 = -lambdaUpperLimit * mSliderAxisWorld; + const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mR2CrossSliderAxis; + + // Apply the impulse to the body + const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; + const Vector3 w2 = mI2 * angularImpulseBody2; + + // Update the body position/orientation + x2 += v2; + q2 += Quaternion(0, w2) * q2 * decimal(0.5); + q2.normalize(); + } + } + } } // Enable/Disable the limits of the joint @@ -469,6 +771,30 @@ void SliderJoint::enableMotor(bool isMotorEnabled) { // TODO : Wake up the bodies of the joint here when sleeping is implemented } +// Return the current translation value of the joint +decimal SliderJoint::getTranslation() const { + + // Get the bodies positions and orientations + const Vector3& x1 = mBody1->getTransform().getPosition(); + const Vector3& x2 = mBody2->getTransform().getPosition(); + const Quaternion& q1 = mBody1->getTransform().getOrientation(); + const Quaternion& q2 = mBody2->getTransform().getOrientation(); + + // Compute the two anchor points in world-space coordinates + const Vector3 anchorBody1 = x1 + q1 * mLocalAnchorPointBody1; + const Vector3 anchorBody2 = x2 + q2 * mLocalAnchorPointBody2; + + // Compute the vector u (difference between anchor points) + const Vector3 u = anchorBody2 - anchorBody1; + + // Compute the slider axis in world-space + Vector3 sliderAxisWorld = q1 * mSliderAxisBody1; + sliderAxisWorld.normalize(); + + // Compute and return the translation value + return u.dot(sliderAxisWorld); +} + // Set the minimum translation limit void SliderJoint::setMinTranslationLimit(decimal lowerLimit) { diff --git a/src/constraint/SliderJoint.h b/src/constraint/SliderJoint.h index 79834322..6515ffff 100644 --- a/src/constraint/SliderJoint.h +++ b/src/constraint/SliderJoint.h @@ -64,7 +64,7 @@ struct SliderJointInfo : public ConstraintInfo { /// Motor speed decimal motorSpeed; - /// Maximum motor force (in Newton) that can be applied to reach to desired motor speed + /// Maximum motor force (in Newtons) that can be applied to reach to desired motor speed decimal maxMotorForce; /// Constructor without limits and without motor @@ -129,6 +129,12 @@ class SliderJoint : public Constraint { /// Slider axis (in local-space coordinates of body 1) Vector3 mSliderAxisBody1; + /// Inertia tensor of body 1 (in world-space coordinates) + Matrix3x3 mI1; + + /// Inertia tensor of body 2 (in world-space coordinates) + Matrix3x3 mI2; + /// Inverse of the initial orientation difference between the two bodies Quaternion mInitOrientationDifferenceInv; @@ -225,7 +231,7 @@ class SliderJoint : public Constraint { /// Motor speed decimal mMotorSpeed; - /// Maximum motor force (in Newton) that can be applied to reach to desired motor speed + /// Maximum motor force (in Newtons) that can be applied to reach to desired motor speed decimal mMaxMotorForce; // -------------------- Methods -------------------- // @@ -255,6 +261,9 @@ class SliderJoint : public Constraint { /// Enable/Disable the motor of the joint void enableMotor(bool isMotorEnabled); + /// Return the current translation value of the joint + decimal getTranslation() const; + /// Return the minimum translation limit decimal getMinTranslationLimit() const; @@ -294,7 +303,7 @@ class SliderJoint : public Constraint { /// Solve the velocity constraint virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); - /// Solve the position constraint + /// Solve the position constraint (for position error correction) virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); }; diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index ff994a28..2154ceb0 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -33,14 +33,15 @@ using namespace reactphysics3d; ConstraintSolver::ConstraintSolver(std::set& joints, std::vector& linearVelocities, std::vector& angularVelocities, + std::vector& positions, + std::vector& orientations, const std::map& mapBodyToVelocityIndex) : mJoints(joints), mLinearVelocities(linearVelocities), - mAngularVelocities(angularVelocities), + mAngularVelocities(angularVelocities), mPositions(positions), + mOrientations(orientations), mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), - mIsWarmStartingActive(true), - mIsNonLinearGaussSeidelPositionCorrectionActive(false), - mConstraintSolverData(linearVelocities, - angularVelocities, mapBodyToVelocityIndex){ + mIsWarmStartingActive(true), mConstraintSolverData(linearVelocities, + angularVelocities, positions, orientations, mapBodyToVelocityIndex){ } @@ -94,10 +95,8 @@ void ConstraintSolver::solveVelocityConstraints() { std::set::iterator it; for (it = mJoints.begin(); it != mJoints.end(); ++it) { - Constraint* joint = (*it); - // Solve the constraint - joint->solveVelocityConstraint(mConstraintSolverData); + (*it)->solveVelocityConstraint(mConstraintSolverData); } } @@ -110,9 +109,7 @@ void ConstraintSolver::solvePositionConstraints() { std::set::iterator it; for (it = mJoints.begin(); it != mJoints.end(); ++it) { - Constraint* joint = (*it); - // Solve the constraint - joint->solveVelocityConstraint(mConstraintSolverData); + (*it)->solvePositionConstraint(mConstraintSolverData); } } diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index fd9b3249..79cac7f7 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -53,6 +53,12 @@ struct ConstraintSolverData { /// Reference to the bodies angular velocities std::vector& angularVelocities; + /// Reference to the bodies positions + std::vector& positions; + + /// Reference to the bodies orientations + std::vector& orientations; + /// Reference to the map that associates rigid body to their index /// in the constrained velocities array const std::map& mapBodyToConstrainedVelocityIndex; @@ -63,9 +69,12 @@ struct ConstraintSolverData { /// Constructor ConstraintSolverData(std::vector& refLinearVelocities, std::vector& refAngularVelocities, + std::vector& refPositions, + std::vector& refOrientations, const std::map& refMapBodyToConstrainedVelocityIndex) :linearVelocities(refLinearVelocities), angularVelocities(refAngularVelocities), + positions(refPositions), orientations(refOrientations), mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){ } @@ -161,6 +170,12 @@ class ConstraintSolver { /// after solving the constraints) std::vector& mAngularVelocities; + /// Reference to the array of bodies positions (for position error correction) + std::vector& mPositions; + + /// Reference to the array of bodies orientations (for position error correction) + std::vector& mOrientations; + /// Reference to the map that associates rigid body to their index in /// the constrained velocities array const std::map& mMapBodyToConstrainedVelocityIndex; @@ -171,9 +186,6 @@ class ConstraintSolver { /// True if the warm starting of the solver is active bool mIsWarmStartingActive; - /// True if the Non-Linear-Gauss-Seidel position correction technique is enabled - bool mIsNonLinearGaussSeidelPositionCorrectionActive; - /// Constraint solver data used to initialize and solve the constraints ConstraintSolverData mConstraintSolverData; @@ -185,6 +197,8 @@ class ConstraintSolver { ConstraintSolver(std::set& joints, std::vector& linearVelocities, std::vector& angularVelocities, + std::vector& positions, + std::vector& orientations, const std::map& mapBodyToVelocityIndex); /// Destructor @@ -204,16 +218,14 @@ class ConstraintSolver { /// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique. void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive); + + /// Return true if the body is in at least one constraint + bool isConstrainedBody(RigidBody* body) const; }; -// Return true if the Non-Linear-Gauss-Seidel position correction technique is active -inline bool ConstraintSolver::getIsNonLinearGaussSeidelPositionCorrectionActive() const { - return mIsNonLinearGaussSeidelPositionCorrectionActive; -} - -// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique. -inline void ConstraintSolver::setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive) { - mIsNonLinearGaussSeidelPositionCorrectionActive = isActive; +// Return true if the body is in at least one constraint +inline bool ConstraintSolver::isConstrainedBody(RigidBody* body) const { + return mConstraintBodies.count(body) == 1; } } diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index e40fb977..d1a3971f 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -442,6 +442,9 @@ class ContactSolver { /// Clean up the constraint solver void cleanup(); + /// Return true if the split impulses position correction technique is used for contacts + bool isSplitImpulseActive() const; + /// Activate or Deactivate the split impulses for contacts void setIsSplitImpulseActive(bool isActive); @@ -469,6 +472,11 @@ inline Vector3 ContactSolver::getSplitAngularVelocityOfBody(RigidBody* body) { return mSplitAngularVelocities[indexBody]; } +// Return true if the split impulses position correction technique is used for contacts +inline bool ContactSolver::isSplitImpulseActive() const { + return mIsSplitImpulseActive; +} + // Activate or Deactivate the split impulses for contacts inline void ContactSolver::setIsSplitImpulseActive(bool isActive) { mIsSplitImpulseActive = isActive; @@ -482,7 +490,7 @@ inline void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool // Compute the collision restitution factor from the restitution factor of each body inline decimal ContactSolver::computeMixedRestitutionFactor(const RigidBody* body1, - const RigidBody* body2) const { + const RigidBody* body2) const { decimal restitution1 = body1->getRestitution(); decimal restitution2 = body2->getRestitution(); diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 53cd3e35..5a7062e0 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -40,6 +40,7 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_ mContactSolver(mContactManifolds, mConstrainedLinearVelocities, mConstrainedAngularVelocities, mMapBodyToConstrainedVelocityIndex), mConstraintSolver(mJoints, mConstrainedLinearVelocities, mConstrainedAngularVelocities, + mConstrainedPositions, mConstrainedOrientations, mMapBodyToConstrainedVelocityIndex), mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS), mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS), @@ -102,18 +103,24 @@ void DynamicsWorld::update() { // Integrate the velocities integrateRigidBodiesVelocities(); - // Solve the contacts and constraints - solveContactsAndConstraints(); + // Reset the movement boolean variable of each body to false + resetBodiesMovementVariable(); // Update the timer mTimer.nextStep(); - // Reset the movement boolean variable of each body to false - resetBodiesMovementVariable(); + // Solve the contacts and constraints + solveContactsAndConstraints(); // Integrate the position and orientation of each body integrateRigidBodiesPositions(); + // Solve the position correction for constraints + solvePositionCorrection(); + + // Update the AABBs of the bodies + updateRigidBodiesAABB(); + // Cleanup of the contact solver mContactSolver.cleanup(); @@ -130,7 +137,7 @@ void DynamicsWorld::update() { /// the sympletic Euler time stepping scheme. void DynamicsWorld::integrateRigidBodiesPositions() { - PROFILE("DynamicsWorld::updateRigidBodiesPositionAndOrientation()"); + PROFILE("DynamicsWorld::integrateRigidBodiesPositions()"); decimal dt = static_cast(mTimer.getTimeStep()); @@ -144,9 +151,6 @@ void DynamicsWorld::integrateRigidBodiesPositions() { // If the body is allowed to move if (rigidBody->getIsMotionEnabled()) { - // Update the old Transform of the body - rigidBody->updateOldTransform(); - // Get the constrained velocity uint indexArray = mMapBodyToConstrainedVelocityIndex.find(rigidBody)->second; Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray]; @@ -157,7 +161,9 @@ void DynamicsWorld::integrateRigidBodiesPositions() { rigidBody->setAngularVelocity(newAngVelocity); // Add the split impulse velocity from Contact Solver (only used to update the position) - if (mContactSolver.isConstrainedBody(rigidBody)) { + if (mContactSolver.isConstrainedBody(rigidBody) && + mContactSolver.isSplitImpulseActive()) { + newLinVelocity += mContactSolver.getSplitLinearVelocityOfBody(rigidBody); newAngVelocity += mContactSolver.getSplitAngularVelocityOfBody(rigidBody); } @@ -168,17 +174,30 @@ void DynamicsWorld::integrateRigidBodiesPositions() { // Compute the new position of the body Vector3 newPosition = currentPosition + newLinVelocity * dt; - Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.x, - newAngVelocity.y, - newAngVelocity.z, 0) * - currentOrientation * 0.5 * dt; + Quaternion newOrientation = currentOrientation + Quaternion(0, newAngVelocity) * + currentOrientation * decimal(0.5) * dt; // Update the Transform of the body Transform newTransform(newPosition, newOrientation.getUnit()); rigidBody->setTransform(newTransform); + } + } +} + +// Update the AABBs of the bodies +void DynamicsWorld::updateRigidBodiesAABB() { + + PROFILE("DynamicsWorld::updateRigidBodiesAABB()"); + + // For each rigid body of the world + set::iterator it; + for (it = getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) { + + // If the body has moved + if ((*it)->getHasMoved()) { // Update the AABB of the rigid body - rigidBody->updateAABB(); + (*it)->updateAABB(); } } } @@ -210,6 +229,8 @@ void DynamicsWorld::setInterpolationFactorToAllBodies() { /// contact solver. void DynamicsWorld::integrateRigidBodiesVelocities() { + PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()"); + // TODO : Use better memory allocation here mConstrainedLinearVelocities = std::vector(mRigidBodies.size(), Vector3(0, 0, 0)); mConstrainedAngularVelocities = std::vector(mRigidBodies.size(), Vector3(0, 0, 0)); @@ -231,6 +252,9 @@ void DynamicsWorld::integrateRigidBodiesVelocities() { dt * rigidBody->getMassInverse() * rigidBody->getExternalForce(); mConstrainedAngularVelocities[i] = rigidBody->getAngularVelocity() + dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque(); + + // Update the old Transform of the body + rigidBody->updateOldTransform(); } i++; @@ -274,7 +298,7 @@ void DynamicsWorld::solveContactsAndConstraints() { // For each iteration of the velocity solver for (uint i=0; i(mRigidBodies.size()); + mConstrainedOrientations = std::vector(mRigidBodies.size()); + for (std::set::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { + + // If it is a constrained bodies (by a joint) + if (mConstraintSolver.isConstrainedBody(*it)) { + + uint index = mMapBodyToConstrainedVelocityIndex.find(*it)->second; + + // Get the position/orientation of the rigid body + const Transform& transform = (*it)->getTransform(); + mConstrainedPositions[index] = transform.getPosition(); + mConstrainedOrientations[index]= transform.getOrientation(); + } + } + + // ---------- Solve the position error correction for the constraints ---------- // + + // For each iteration of the position (error correction) solver + for (uint i=0; i::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { + + // If it is a constrained bodies (by a joint) + if (mConstraintSolver.isConstrainedBody(*it)) { + + uint index = mMapBodyToConstrainedVelocityIndex.find(*it)->second; + + // Get the new position/orientation of the body + const Vector3& newPosition = mConstrainedPositions[index]; + const Quaternion& newOrientation = mConstrainedOrientations[index]; + + // Update the Transform of the body + Transform newTransform(newPosition, newOrientation.getUnit()); + (*it)->setTransform(newTransform); } } } @@ -365,7 +430,7 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal ma return rigidBody; } -// Destroy a rigid body +// Destroy a rigid body and all the joints which it belongs void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { // Remove the body from the collision detection @@ -377,6 +442,14 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { // Remove the collision shape from the world removeCollisionShape(rigidBody->getCollisionShape()); + // Destroy all the joints that contains the rigid body to be destroyed + bodyindex idToRemove = rigidBody->getID(); + for (std::set::iterator it = mJoints.begin(); it != mJoints.end(); ++it) { + if ((*it)->getBody1()->getID() == idToRemove || (*it)->getBody2()->getID() == idToRemove) { + destroyJoint(*it); + } + } + // Call the destructor of the rigid body rigidBody->RigidBody::~RigidBody(); @@ -440,6 +513,13 @@ Constraint* DynamicsWorld::createJoint(const ConstraintInfo& jointInfo) { } } + // If the collision between the two bodies of the constraint is disabled + if (!jointInfo.isCollisionEnabled) { + + // Add the pair of bodies in the set of body pairs that cannot collide with each other + mCollisionDetection.addNoCollisionPair(jointInfo.body1, jointInfo.body2); + } + // Add the joint into the world mJoints.insert(newJoint); @@ -452,6 +532,13 @@ void DynamicsWorld::destroyJoint(Constraint* joint) { assert(joint != NULL); + // If the collision between the two bodies of the constraint was disabled + if (!joint->isCollisionEnabled()) { + + // Remove the pair of bodies from the set of body pairs that cannot collide with each other + mCollisionDetection.removeNoCollisionPair(joint->getBody1(), joint->getBody2()); + } + // Remove the joint from the world mJoints.erase(joint); diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index e9c7bcbb..d5412971 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -91,6 +91,12 @@ class DynamicsWorld : public CollisionWorld { /// after solving the constraints) std::vector mConstrainedAngularVelocities; + /// Array of constrained rigid bodies position (for position error correction) + std::vector mConstrainedPositions; + + /// Array of constrained rigid bodies orientation (for position error correction) + std::vector mConstrainedOrientations; + /// Map body to their index in the constrained velocities array std::map mMapBodyToConstrainedVelocityIndex; @@ -105,6 +111,9 @@ class DynamicsWorld : public CollisionWorld { /// Integrate the positions and orientations of rigid bodies. void integrateRigidBodiesPositions(); + /// Update the AABBs of the bodies + void updateRigidBodiesAABB(); + /// Update the position and orientation of a body void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity, Vector3 newAngVelocity); @@ -118,6 +127,9 @@ class DynamicsWorld : public CollisionWorld { /// Solve the contacts and constraints void solveContactsAndConstraints(); + /// Solve the position error correction of the constraints + void solvePositionCorrection(); + /// Cleanup the constrained velocities array at each step void cleanupConstrainedVelocitiesArray(); @@ -137,7 +149,8 @@ class DynamicsWorld : public CollisionWorld { virtual void notifyRemovedOverlappingPair(const BroadPhasePair* removedPair); /// Notify the world about a new narrow-phase contact - virtual void notifyNewContact(const BroadPhasePair* pair, const ContactPointInfo* contactInfo); + virtual void notifyNewContact(const BroadPhasePair* pair, + const ContactPointInfo* contactInfo); public : @@ -179,7 +192,7 @@ public : const Matrix3x3& inertiaTensorLocal, const CollisionShape& collisionShape); - /// Destroy a rigid body + /// Destroy a rigid body and all the joints which it belongs void destroyRigidBody(RigidBody* rigidBody); /// Create a joint between two bodies in the world and return a pointer to the new joint @@ -200,9 +213,15 @@ public : /// Return the number of rigid bodies in the world uint getNbRigidBodies() const; - /// Return the number of contact constraints in the world + /// Return the number of joints in the world + uint getNbJoints() const; + + /// Return the number of contact manifolds in the world uint getNbContactManifolds() const; + /// Return the current physics time (in seconds) + long double getPhysicsTime() const; + /// Return an iterator to the beginning of the rigid bodies of the physics world std::set::iterator getRigidBodiesBeginIterator(); @@ -302,6 +321,11 @@ inline uint DynamicsWorld::getNbRigidBodies() const { return mRigidBodies.size(); } +/// Return the number of joints in the world +inline uint DynamicsWorld::getNbJoints() const { + return mJoints.size(); +} + // Return an iterator to the beginning of the bodies of the physics world inline std::set::iterator DynamicsWorld::getRigidBodiesBeginIterator() { return mRigidBodies.begin(); @@ -317,6 +341,11 @@ inline uint DynamicsWorld::getNbContactManifolds() const { return mContactManifolds.size(); } +/// Return the current physics time (in seconds) +inline long double DynamicsWorld::getPhysicsTime() const { + return mTimer.getPhysicsTime(); +} + } #endif diff --git a/src/engine/Timer.h b/src/engine/Timer.h index 22728537..7e1b3b7d 100644 --- a/src/engine/Timer.h +++ b/src/engine/Timer.h @@ -47,7 +47,7 @@ namespace reactphysics3d { // Class Timer /** * This class will take care of the time in the physics engine. It - * uses fuunctions that depend on the current platform to get the + * uses functions that depend on the current platform to get the * current time. */ class Timer { @@ -59,9 +59,6 @@ class Timer { /// Timestep dt of the physics engine (timestep > 0.0) double mTimeStep; - /// Current time of the physics engine - long double mTime; - /// Last time the timer has been updated long double mLastUpdateTime; @@ -139,7 +136,7 @@ inline void Timer::setTimeStep(double timeStep) { // Return the current time inline long double Timer::getPhysicsTime() const { - return mTime; + return mLastUpdateTime; } // Return if the timer is running @@ -173,9 +170,6 @@ inline bool Timer::isPossibleToTakeStep() const { inline void Timer::nextStep() { assert(mIsRunning); - // Update the current time of the physics engine - mTime += mTimeStep; - // Update the accumulator value mAccumulator -= mTimeStep; } diff --git a/src/mathematics/Quaternion.h b/src/mathematics/Quaternion.h index 246d56b2..5bc862da 100644 --- a/src/mathematics/Quaternion.h +++ b/src/mathematics/Quaternion.h @@ -133,6 +133,12 @@ struct Quaternion { /// Overloaded operator for the substraction Quaternion operator-(const Quaternion& quaternion) const; + /// Overloaded operator for addition with assignment + Quaternion& operator+=(const Quaternion& quaternion); + + /// Overloaded operator for substraction with assignment + Quaternion& operator-=(const Quaternion& quaternion); + /// Overloaded operator for the multiplication with a constant Quaternion operator*(decimal nb) const; @@ -272,6 +278,24 @@ inline Quaternion Quaternion::operator-(const Quaternion& quaternion) const { return Quaternion(x - quaternion.x, y - quaternion.y, z - quaternion.z, w - quaternion.w); } +// Overloaded operator for addition with assignment +inline Quaternion& Quaternion::operator+=(const Quaternion& quaternion) { + x += quaternion.x; + y += quaternion.y; + z += quaternion.z; + w += quaternion.w; + return *this; +} + +// Overloaded operator for substraction with assignment +inline Quaternion& Quaternion::operator-=(const Quaternion& quaternion) { + x -= quaternion.x; + y -= quaternion.y; + z -= quaternion.z; + w -= quaternion.w; + return *this; +} + // Overloaded operator for the multiplication with a constant inline Quaternion Quaternion::operator*(decimal nb) const { return Quaternion(nb * x, nb * y, nb * z, nb * w); diff --git a/test/tests/mathematics/TestQuaternion.h b/test/tests/mathematics/TestQuaternion.h index ac7ad741..a7f3d324 100644 --- a/test/tests/mathematics/TestQuaternion.h +++ b/test/tests/mathematics/TestQuaternion.h @@ -197,11 +197,17 @@ class TestQuaternion : public Test { Quaternion quat1(4, 5, 2, 10); Quaternion quat2(-2, 7, 8, 3); Quaternion test1 = quat1 + quat2; + Quaternion test11(-6, 52, 2, 8); + test11 += quat1; test(test1 == Quaternion(2, 12, 10, 13)); + test(test11 == Quaternion(-2, 57, 4, 18)); // Test substraction Quaternion test2 = quat1 - quat2; + Quaternion test22(-73, 62, 25, 9); + test22 -= quat1; test(test2 == Quaternion(6, -2, -6, 7)); + test(test22 == Quaternion(-77, 57, 23, -1)); // Test multiplication with a number Quaternion test3 = quat1 * 3.0; From d265a52b67e76f8b588143a1802f7ca217cbcf0b Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 26 Jun 2013 22:29:23 +0200 Subject: [PATCH 24/24] Modify the joints example --- examples/joints/Scene.cpp | 249 +++++++++++++++++++++++++++++--------- examples/joints/Scene.h | 49 ++++++-- 2 files changed, 229 insertions(+), 69 deletions(-) diff --git a/examples/joints/Scene.cpp b/examples/joints/Scene.cpp index 419f5c24..ce278b63 100644 --- a/examples/joints/Scene.cpp +++ b/examples/joints/Scene.cpp @@ -25,6 +25,7 @@ // Libraries #include "Scene.h" +#include // Namespaces using namespace openglframework; @@ -62,6 +63,12 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0), // Create the Slider joint createSliderJoint(); + // Create the Hinge joint + createPropellerHingeJoint(); + + // Create the Fixed joint + createFixedJoints(); + // Create the floor createFloor(); @@ -79,18 +86,32 @@ Scene::~Scene() { mPhongShader.destroy(); // Destroy the joints - mDynamicsWorld->destroyJoint(mBallAndSocketJoint); mDynamicsWorld->destroyJoint(mSliderJoint); + mDynamicsWorld->destroyJoint(mPropellerHingeJoint); + mDynamicsWorld->destroyJoint(mFixedJoint1); + mDynamicsWorld->destroyJoint(mFixedJoint2); + for (int i=0; idestroyJoint(mBallAndSocketJoints[i]); + } - // Destroy all the boxes of the scene - mDynamicsWorld->destroyRigidBody(mBallAndSocketJointBox1->getRigidBody()); - mDynamicsWorld->destroyRigidBody(mBallAndSocketJointBox2->getRigidBody()); - mDynamicsWorld->destroyRigidBody(mSliderJointBox1->getRigidBody()); - mDynamicsWorld->destroyRigidBody(mSliderJointBox2->getRigidBody()); - delete mBallAndSocketJointBox1; - delete mBallAndSocketJointBox2; - delete mSliderJointBox1; - delete mSliderJointBox2; + // Destroy all the rigid bodies of the scene + mDynamicsWorld->destroyRigidBody(mSliderJointBottomBox->getRigidBody()); + mDynamicsWorld->destroyRigidBody(mSliderJointTopBox->getRigidBody()); + mDynamicsWorld->destroyRigidBody(mPropellerBox->getRigidBody()); + mDynamicsWorld->destroyRigidBody(mFixedJointBox1->getRigidBody()); + mDynamicsWorld->destroyRigidBody(mFixedJointBox2->getRigidBody()); + for (int i=0; idestroyRigidBody(mBallAndSocketJointChainBoxes[i]->getRigidBody()); + } + + delete mSliderJointBottomBox; + delete mSliderJointTopBox; + delete mPropellerBox; + delete mFixedJointBox1; + delete mFixedJointBox2; + for (int i=0; idestroyRigidBody(mFloor->getRigidBody()); @@ -106,14 +127,22 @@ void Scene::simulate() { // If the physics simulation is running if (mIsRunning) { + // Update the motor speed of the Slider Joint (to move up and down) + long double motorSpeed = 3 * cos(mDynamicsWorld->getPhysicsTime() * 1.5); + mSliderJoint->setMotorSpeed(motorSpeed); + // Take a simulation step mDynamicsWorld->update(); // Update the position and orientation of the boxes - mBallAndSocketJointBox1->updateTransform(); - mBallAndSocketJointBox2->updateTransform(); - mSliderJointBox1->updateTransform(); - mSliderJointBox2->updateTransform(); + mSliderJointBottomBox->updateTransform(); + mSliderJointTopBox->updateTransform(); + mPropellerBox->updateTransform(); + mFixedJointBox1->updateTransform(); + mFixedJointBox2->updateTransform(); + for (int i=0; iupdateTransform(); + } // Update the position and orientation of the floor mFloor->updateTransform(); @@ -146,10 +175,14 @@ void Scene::render() { mPhongShader.setFloatUniform("shininess", 60.0f); // Render all the boxes - mBallAndSocketJointBox1->render(mPhongShader); - mBallAndSocketJointBox2->render(mPhongShader); - mSliderJointBox1->render(mPhongShader); - mSliderJointBox2->render(mPhongShader); + mSliderJointBottomBox->render(mPhongShader); + mSliderJointTopBox->render(mPhongShader); + mPropellerBox->render(mPhongShader); + mFixedJointBox1->render(mPhongShader); + mFixedJointBox2->render(mPhongShader); + for (int i=0; irender(mPhongShader); + } // Render the floor mFloor->render(mPhongShader); @@ -161,45 +194,44 @@ void Scene::render() { // Create the boxes and joints for the Ball-and-Socket joint example void Scene::createBallAndSocketJoints() { - // --------------- Create the first box --------------- // + // --------------- Create the boxes --------------- // - // Position of the box - openglframework::Vector3 positionBox1(0, 15, 0); + openglframework::Vector3 positionBox(0, 15, 5); + openglframework::Vector3 boxDimension(1, 1, 1); + const float boxMass = 0.5f; - // Create a box and a corresponding rigid in the dynamics world - mBallAndSocketJointBox1 = new Box(BOX_SIZE, positionBox1 , BOX_MASS, mDynamicsWorld); + for (int i=0; igetRigidBody()->setIsMotionEnabled(false); + // Create a box and a corresponding rigid in the dynamics world + mBallAndSocketJointChainBoxes[i] = new Box(boxDimension, positionBox , boxMass, + mDynamicsWorld); - // Set the bouncing factor of the box - mBallAndSocketJointBox1->getRigidBody()->setRestitution(0.4); + // The fist box cannot move + if (i == 0) mBallAndSocketJointChainBoxes[i]->getRigidBody()->setIsMotionEnabled(false); + else mBallAndSocketJointChainBoxes[i]->getRigidBody()->setIsMotionEnabled(true); - // --------------- Create the second box --------------- // + // Set the bouncing factor of the box + mBallAndSocketJointChainBoxes[i]->getRigidBody()->setRestitution(0.4); - // Position of the box - openglframework::Vector3 positionBox2(5, 10, 0); + positionBox.y -= boxDimension.y + 0.5; + } - // Create a box and a corresponding rigid in the dynamics world - mBallAndSocketJointBox2 = new Box(BOX_SIZE, positionBox2 , BOX_MASS, mDynamicsWorld); + // --------------- Create the joints --------------- // - // The second box is allowed to move - mBallAndSocketJointBox2->getRigidBody()->setIsMotionEnabled(true); + for (int i=0; igetRigidBody()->setRestitution(0.4); + // Create the joint info object + rp3d::RigidBody* body1 = mBallAndSocketJointChainBoxes[i]->getRigidBody(); + rp3d::RigidBody* body2 = mBallAndSocketJointChainBoxes[i+1]->getRigidBody(); + rp3d::Vector3 body1Position = body1->getTransform().getPosition(); + rp3d::Vector3 body2Position = body2->getTransform().getPosition(); + const rp3d::Vector3 anchorPointWorldSpace = 0.5 * (body1Position + body2Position); + rp3d::BallAndSocketJointInfo jointInfo(body1, body2, anchorPointWorldSpace); - // --------------- Create the joint --------------- // - - // Create the joint info object - rp3d::RigidBody* body1 = mBallAndSocketJointBox1->getRigidBody(); - rp3d::RigidBody* body2 = mBallAndSocketJointBox2->getRigidBody(); - const rp3d::Vector3 anchorPointWorldSpace(0, 10, 0); - rp3d::BallAndSocketJointInfo jointInfo(body1, body2, anchorPointWorldSpace); - - // Create the joint in the dynamics world - mBallAndSocketJoint = dynamic_cast( - mDynamicsWorld->createJoint(jointInfo)); + // Create the joint in the dynamics world + mBallAndSocketJoints[i] = dynamic_cast( + mDynamicsWorld->createJoint(jointInfo)); + } } /// Create the boxes and joint for the Slider joint example @@ -208,46 +240,145 @@ void Scene::createSliderJoint() { // --------------- Create the first box --------------- // // Position of the box - openglframework::Vector3 positionBox1(-4, 6, 0); + openglframework::Vector3 positionBox1(0, 2.1, 0); // Create a box and a corresponding rigid in the dynamics world - mSliderJointBox1 = new Box(BOX_SIZE, positionBox1 , BOX_MASS, mDynamicsWorld); + openglframework::Vector3 box1Dimension(2, 4, 2); + mSliderJointBottomBox = new Box(box1Dimension, positionBox1 , BOX_MASS, mDynamicsWorld); // The fist box cannot move - mSliderJointBox1->getRigidBody()->setIsMotionEnabled(false); + mSliderJointBottomBox->getRigidBody()->setIsMotionEnabled(false); // Set the bouncing factor of the box - mSliderJointBox1->getRigidBody()->setRestitution(0.4); + mSliderJointBottomBox->getRigidBody()->setRestitution(0.4); // --------------- Create the second box --------------- // // Position of the box - openglframework::Vector3 positionBox2(2, 4, 0); + openglframework::Vector3 positionBox2(0, 4.2, 0); // Create a box and a corresponding rigid in the dynamics world - mSliderJointBox2 = new Box(BOX_SIZE, positionBox2 , BOX_MASS, mDynamicsWorld); + openglframework::Vector3 box2Dimension(1.5, 4, 1.5); + mSliderJointTopBox = new Box(box2Dimension, positionBox2 , BOX_MASS, mDynamicsWorld); // The second box is allowed to move - mSliderJointBox2->getRigidBody()->setIsMotionEnabled(true); + mSliderJointTopBox->getRigidBody()->setIsMotionEnabled(true); // Set the bouncing factor of the box - mSliderJointBox2->getRigidBody()->setRestitution(0.4); + mSliderJointTopBox->getRigidBody()->setRestitution(0.4); // --------------- Create the joint --------------- // // Create the joint info object - rp3d::RigidBody* body1 = mSliderJointBox1->getRigidBody(); - rp3d::RigidBody* body2 = mSliderJointBox2->getRigidBody(); + rp3d::RigidBody* body1 = mSliderJointBottomBox->getRigidBody(); + rp3d::RigidBody* body2 = mSliderJointTopBox->getRigidBody(); const rp3d::Vector3& body1Position = body1->getTransform().getPosition(); const rp3d::Vector3& body2Position = body2->getTransform().getPosition(); const rp3d::Vector3 anchorPointWorldSpace = 0.5 * (body2Position + body1Position); - const rp3d::Vector3 sliderAxisWorldSpace = body2Position - body1Position; - rp3d::SliderJointInfo jointInfo(body1, body2, anchorPointWorldSpace, sliderAxisWorldSpace); + const rp3d::Vector3 sliderAxisWorldSpace = (body2Position - body1Position); + rp3d::SliderJointInfo jointInfo(body1, body2, anchorPointWorldSpace, sliderAxisWorldSpace, + -1.7, 1.7); + jointInfo.isMotorEnabled = true; + jointInfo.motorSpeed = 0.0; + jointInfo.maxMotorForce = 10000.0; + jointInfo.isCollisionEnabled = false; // Create the joint in the dynamics world mSliderJoint = dynamic_cast(mDynamicsWorld->createJoint(jointInfo)); } +/// Create the boxes and joint for the Hinge joint example +void Scene::createPropellerHingeJoint() { + + // --------------- Create the propeller box --------------- // + + // Position of the box + openglframework::Vector3 positionBox1(0, 7, 0); + + // Create a box and a corresponding rigid in the dynamics world + openglframework::Vector3 boxDimension(10, 1, 1); + mPropellerBox = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld); + + // The fist box cannot move + mPropellerBox->getRigidBody()->setIsMotionEnabled(true); + + // Set the bouncing factor of the box + mPropellerBox->getRigidBody()->setRestitution(0.4); + + // --------------- Create the Hinge joint --------------- // + + // Create the joint info object + rp3d::RigidBody* body1 = mPropellerBox->getRigidBody(); + rp3d::RigidBody* body2 = mSliderJointTopBox->getRigidBody(); + const rp3d::Vector3& body1Position = body1->getTransform().getPosition(); + const rp3d::Vector3& body2Position = body2->getTransform().getPosition(); + const rp3d::Vector3 anchorPointWorldSpace = 0.5 * (body2Position + body1Position); + const rp3d::Vector3 hingeAxisWorldSpace(0, 1, 0); + rp3d::HingeJointInfo jointInfo(body1, body2, anchorPointWorldSpace, hingeAxisWorldSpace); + jointInfo.isMotorEnabled = true; + jointInfo.motorSpeed = -0.5 * PI; + jointInfo.maxMotorTorque = 60.0; + jointInfo.isCollisionEnabled = false; + + // Create the joint in the dynamics world + mPropellerHingeJoint = dynamic_cast(mDynamicsWorld->createJoint(jointInfo)); +} + +/// Create the boxes and joints for the fixed joints +void Scene::createFixedJoints() { + + // --------------- Create the first box --------------- // + + // Position of the box + openglframework::Vector3 positionBox1(5, 7, 0); + + // Create a box and a corresponding rigid in the dynamics world + openglframework::Vector3 boxDimension(1.5, 1.5, 1.5); + mFixedJointBox1 = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld); + + // The fist box cannot move + mFixedJointBox1->getRigidBody()->setIsMotionEnabled(true); + + // Set the bouncing factor of the box + mFixedJointBox1->getRigidBody()->setRestitution(0.4); + + // --------------- Create the second box --------------- // + + // Position of the box + openglframework::Vector3 positionBox2(-5, 7, 0); + + // Create a box and a corresponding rigid in the dynamics world + mFixedJointBox2 = new Box(boxDimension, positionBox2 , BOX_MASS, mDynamicsWorld); + + // The second box is allowed to move + mFixedJointBox2->getRigidBody()->setIsMotionEnabled(true); + + // Set the bouncing factor of the box + mFixedJointBox2->getRigidBody()->setRestitution(0.4); + + // --------------- Create the first fixed joint --------------- // + + // Create the joint info object + rp3d::RigidBody* body1 = mFixedJointBox1->getRigidBody(); + rp3d::RigidBody* propellerBody = mPropellerBox->getRigidBody(); + const rp3d::Vector3 anchorPointWorldSpace1(5, 7, 0); + rp3d::FixedJointInfo jointInfo1(body1, propellerBody, anchorPointWorldSpace1); + + // Create the joint in the dynamics world + mFixedJoint1 = dynamic_cast(mDynamicsWorld->createJoint(jointInfo1)); + + // --------------- Create the second fixed joint --------------- // + + // Create the joint info object + rp3d::RigidBody* body2 = mFixedJointBox2->getRigidBody(); + const rp3d::Vector3 anchorPointWorldSpace2(-5, 7, 0); + rp3d::FixedJointInfo jointInfo2(body2, propellerBody, anchorPointWorldSpace2); + + // Create the joint in the dynamics world + mFixedJoint2 = dynamic_cast(mDynamicsWorld->createJoint(jointInfo2)); +} + + // Create the floor void Scene::createFloor() { diff --git a/examples/joints/Scene.h b/examples/joints/Scene.h index 3bc5cb5d..90430d1d 100644 --- a/examples/joints/Scene.h +++ b/examples/joints/Scene.h @@ -36,6 +36,8 @@ const openglframework::Vector3 BOX_SIZE(2, 2, 2); // Box dimensions in const openglframework::Vector3 FLOOR_SIZE(20, 0.5f, 20); // Floor dimensions in meters const float BOX_MASS = 1.0f; // Box mass in kilograms const float FLOOR_MASS = 100.0f; // Floor mass in kilograms +const int NB_BALLSOCKETJOINT_BOXES = 7; // Number of Ball-And-Socket chain boxes +const int NB_HINGE_BOXES = 7; // Number of Hinge chain boxes // Class Scene class Scene { @@ -53,24 +55,45 @@ class Scene { /// Phong shader openglframework::Shader mPhongShader; - /// Box 1 of Ball-And-Socket joint - Box* mBallAndSocketJointBox1; + /// Boxes of Ball-And-Socket joint chain + Box* mBallAndSocketJointChainBoxes[NB_BALLSOCKETJOINT_BOXES]; - /// Box 2 of Ball-And-Socket joint - Box* mBallAndSocketJointBox2; + /// Boxes of the Hinge joint chain + Box* mHingeJointChainBoxes[NB_HINGE_BOXES]; - /// Ball-and-Socket joint - rp3d::BallAndSocketJoint* mBallAndSocketJoint; + /// Ball-And-Socket joints of the chain + rp3d::BallAndSocketJoint* mBallAndSocketJoints[NB_BALLSOCKETJOINT_BOXES-1]; - /// Box 1 of Slider joint - Box* mSliderJointBox1; + /// Hinge joints of the chain + rp3d::HingeJoint* mHingeJoints[NB_HINGE_BOXES-1]; - /// Box 2 of Slider joint - Box* mSliderJointBox2; + /// Bottom box of the Slider joint + Box* mSliderJointBottomBox; + + /// Top box of the Slider joint + Box* mSliderJointTopBox; /// Slider joint rp3d::SliderJoint* mSliderJoint; + /// Propeller box + Box* mPropellerBox; + + /// Box 1 of Fixed joint + Box* mFixedJointBox1; + + /// Box 2 of Fixed joint + Box* mFixedJointBox2; + + /// Hinge joint + rp3d::HingeJoint* mPropellerHingeJoint; + + /// First Fixed joint + rp3d::FixedJoint* mFixedJoint1; + + /// Second Fixed joint + rp3d::FixedJoint* mFixedJoint2; + /// Box for the floor Box* mFloor; @@ -88,6 +111,12 @@ class Scene { /// Create the boxes and joint for the Slider joint example void createSliderJoint(); + /// Create the boxes and joint for the Hinge joint example + void createPropellerHingeJoint(); + + /// Create the boxes and joint for the Fixed joint example + void createFixedJoints(); + /// Create the floor void createFloor();