From b87f981827807581a6142cb05e3b8cc3aaa3258a Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 8 May 2013 23:33:04 +0200 Subject: [PATCH] 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