mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Start working on the slider joint

Browse Source
This commit is contained in:
Daniel Chappuis 2013-05-08 23:33:04 +02:00
parent da78e5d79a
commit b87f981827
15 changed files with 446 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/fallingcubes/Scene.cpp
View File

@ -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;

10
examples/joints/Scene.cpp
View File

@ -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<rp3d::BallAndSocketJoint*>(

23
src/configuration.h
View File

@ -51,6 +51,22 @@ typedef long unsigned int luint;
typedef luint bodyindex;
typedef std::pair<bodyindex, bodyindex> 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
}

32
src/constraint/BallAndSocketJoint.cpp
View File

@ -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) {
}

14
src/constraint/BallAndSocketJoint.h
View File

@ -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

3
src/constraint/Constraint.cpp
View File

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

23
src/constraint/Constraint.h
View File

@ -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

9
src/constraint/ContactPoint.cpp
View File

@ -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) {
}

7
src/constraint/ContactPoint.h
View File

@ -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)

105
src/constraint/SliderJoint.cpp Normal file
View File

@ -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) {
}

131
src/constraint/SliderJoint.h Normal file
View File

@ -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

28
src/engine/ConstraintSolver.cpp
View File

@ -37,7 +37,9 @@ ConstraintSolver::ConstraintSolver(std::set<Constraint*>& 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<Constraint*>::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<Constraint*>::iterator it;
for (it = mJoints.begin(); it != mJoints.end(); ++it) {
Constraint* joint = (*it);
// Solve the constraint
joint->solveVelocityConstraint(mConstraintSolverData);
}
}

24
src/engine/ConstraintSolver.h
View File

@ -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

38
src/engine/DynamicsWorld.cpp
View File

@ -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<mNbSolverIterations; i++) {
// For each iteration of the velocity solver
for (uint i=0; i<mNbVelocitySolverIterations; i++) {
// Solve the constraints
if (isConstraintsToSolve) mConstraintSolver.solve();
// Solve the velocity constraints
if (isConstraintsToSolve) mConstraintSolver.solveVelocityConstraints();
// Solve the contacts
if (isContactsToSolve) mContactSolver.solve();
@ -277,6 +281,21 @@ void DynamicsWorld::solveContactsAndConstraints() {
// Cache the lambda values in order to use them in the next step
if (isContactsToSolve) mContactSolver.storeImpulses();
// ---------- Solve positions constraints (error correction) for joints only ---------- //
// TODO : Integrate the bodies positions here
// If the Non-Linear-Gauss-Seidel position correction is active
if (mConstraintSolver.getIsNonLinearGaussSeidelPositionCorrectionActive()) {
// For each iteration of the position (error correction) solver
for (uint i=0; i<mNbPositionSolverIterations; i++) {
// Solve the position constraints
if (isConstraintsToSolve) mConstraintSolver.solvePositionConstraints();
}
}
}
// Cleanup the constrained velocities array at each step
@ -385,6 +404,15 @@ Constraint* DynamicsWorld::createJoint(const ConstraintInfo& jointInfo) {
break;
}
// Slider joint
case SLIDERJOINT:
{
void* allocatedMemory = mMemoryAllocator.allocate(sizeof(SliderJoint));
const SliderJointInfo& info = dynamic_cast<const SliderJointInfo&>(jointInfo);
newJoint = new (allocatedMemory) SliderJoint(info);
break;
}
default:
{
assert(false);

58
src/engine/DynamicsWorld.h
View File

@ -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