From c4eee4fb1f42f7d51e4f952ae838539dc0dbc8a7 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 2 May 2013 22:41:57 +0200 Subject: [PATCH] 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;