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

Add comments, modify method names, change solver nb of iterations, ...

Browse Source
This commit is contained in:
Daniel Chappuis 2013-09-27 18:43:45 +02:00
parent 97b0903a9d
commit b5ae655b0a
30 changed files with 282 additions and 327 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
examples/CMakeLists.txt
View File

@ -4,6 +4,11 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
# Set a variable for the directory of the opengl-framework # Set a variable for the directory of the opengl-framework
SET(OPENGLFRAMEWORK_DIR "${CMAKE_CURRENT_SOURCE_DIR}/common/opengl-framework") SET(OPENGLFRAMEWORK_DIR "${CMAKE_CURRENT_SOURCE_DIR}/common/opengl-framework")
# If we will use FREEGLUT
IF(NOT APPLE)
ADD_DEFINITIONS(-DUSE_FREEGLUT)
ENDIF()
ADD_SUBDIRECTORY(common/) ADD_SUBDIRECTORY(common/)
ADD_SUBDIRECTORY(cubes/) ADD_SUBDIRECTORY(cubes/)
ADD_SUBDIRECTORY(joints/) ADD_SUBDIRECTORY(joints/)

1
examples/collisionshapes/CollisionShapes.cpp
View File

@ -120,7 +120,6 @@ void keyboard(unsigned char key, int x, int y) {
// Escape key // Escape key
case 27: case 27:
#ifdef USE_FREEGLUT #ifdef USE_FREEGLUT
// TODO : Check if we need to call finish() here
glutLeaveMainLoop(); glutLeaveMainLoop();
#endif #endif
break; break;

14
examples/collisionshapes/Scene.cpp
View File

@ -74,7 +74,7 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0),
Box* box = new Box(BOX_SIZE, position , BOX_MASS, mDynamicsWorld); Box* box = new Box(BOX_SIZE, position , BOX_MASS, mDynamicsWorld);
// The sphere is a moving rigid body // The sphere is a moving rigid body
box->getRigidBody()->setIsMotionEnabled(true); box->getRigidBody()->enableMotion(true);
// Change the material properties of the rigid body // Change the material properties of the rigid body
rp3d::Material& material = box->getRigidBody()->getMaterial(); rp3d::Material& material = box->getRigidBody()->getMaterial();
@ -97,7 +97,7 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0),
Sphere* sphere = new Sphere(SPHERE_RADIUS, position , BOX_MASS, mDynamicsWorld); Sphere* sphere = new Sphere(SPHERE_RADIUS, position , BOX_MASS, mDynamicsWorld);
// The sphere is a moving rigid body // The sphere is a moving rigid body
sphere->getRigidBody()->setIsMotionEnabled(true); sphere->getRigidBody()->enableMotion(true);
// Change the material properties of the rigid body // Change the material properties of the rigid body
rp3d::Material& material = sphere->getRigidBody()->getMaterial(); rp3d::Material& material = sphere->getRigidBody()->getMaterial();
@ -120,7 +120,7 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0),
Cone* cone = new Cone(CONE_RADIUS, CONE_HEIGHT, position , CONE_MASS, mDynamicsWorld); Cone* cone = new Cone(CONE_RADIUS, CONE_HEIGHT, position , CONE_MASS, mDynamicsWorld);
// The cone is a moving rigid body // The cone is a moving rigid body
cone->getRigidBody()->setIsMotionEnabled(true); cone->getRigidBody()->enableMotion(true);
// Change the material properties of the rigid body // Change the material properties of the rigid body
rp3d::Material& material = cone->getRigidBody()->getMaterial(); rp3d::Material& material = cone->getRigidBody()->getMaterial();
@ -144,7 +144,7 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0),
CYLINDER_MASS, mDynamicsWorld); CYLINDER_MASS, mDynamicsWorld);
// The cylinder is a moving rigid body // The cylinder is a moving rigid body
cylinder->getRigidBody()->setIsMotionEnabled(true); cylinder->getRigidBody()->enableMotion(true);
// Change the material properties of the rigid body // Change the material properties of the rigid body
rp3d::Material& material = cylinder->getRigidBody()->getMaterial(); rp3d::Material& material = cylinder->getRigidBody()->getMaterial();
@ -168,7 +168,7 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0),
CAPSULE_MASS, mDynamicsWorld); CAPSULE_MASS, mDynamicsWorld);
// The cylinder is a moving rigid body // The cylinder is a moving rigid body
capsule->getRigidBody()->setIsMotionEnabled(true); capsule->getRigidBody()->enableMotion(true);
// Change the material properties of the rigid body // Change the material properties of the rigid body
rp3d::Material& material = capsule->getRigidBody()->getMaterial(); rp3d::Material& material = capsule->getRigidBody()->getMaterial();
@ -191,7 +191,7 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0),
ConvexMesh* mesh = new ConvexMesh(position, MESH_MASS, mDynamicsWorld); ConvexMesh* mesh = new ConvexMesh(position, MESH_MASS, mDynamicsWorld);
// The mesh is a moving rigid body // The mesh is a moving rigid body
mesh->getRigidBody()->setIsMotionEnabled(true); mesh->getRigidBody()->enableMotion(true);
// Change the material properties of the rigid body // Change the material properties of the rigid body
rp3d::Material& material = mesh->getRigidBody()->getMaterial(); rp3d::Material& material = mesh->getRigidBody()->getMaterial();
@ -206,7 +206,7 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0),
mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld); mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld);
// The floor must be a non-moving rigid body // The floor must be a non-moving rigid body
mFloor->getRigidBody()->setIsMotionEnabled(false); mFloor->getRigidBody()->enableMotion(false);
// Change the material properties of the rigid body // Change the material properties of the rigid body
rp3d::Material& material = mFloor->getRigidBody()->getMaterial(); rp3d::Material& material = mFloor->getRigidBody()->getMaterial();

2
examples/common/Box.cpp
View File

@ -121,7 +121,7 @@ void Box::render(openglframework::Shader& shader,
localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose();
shader.setMatrix3x3Uniform("normalMatrix", normalMatrix); shader.setMatrix3x3Uniform("normalMatrix", normalMatrix);
// TODO : REMOVE THIS // Set the vertex color
openglframework::Vector4 color(mColor.r, mColor.g, mColor.b, mColor.a); openglframework::Vector4 color(mColor.r, mColor.g, mColor.b, mColor.a);
shader.setVector4Uniform("vertexColor", color); shader.setVector4Uniform("vertexColor", color);

27
examples/common/opengl-framework/CMakeLists.txt
View File

@ -28,24 +28,23 @@ ENDIF()
# Find the GLUT/FREEGLUT library # Find the GLUT/FREEGLUT library
IF(APPLE) IF(APPLE)
# Find the GLUT library # Find the GLUT library
FIND_PACKAGE(GLUT REQUIRED) FIND_PACKAGE(GLUT REQUIRED)
IF(GLUT_FOUND) IF(GLUT_FOUND)
MESSAGE("GLUT found") MESSAGE("GLUT found")
ELSE(GLUT_FOUND) ELSE(GLUT_FOUND)
MESSAGE(SEND_ERROR "GLUT not found") MESSAGE(SEND_ERROR "GLUT not found")
ENDIF(GLUT_FOUND) ENDIF(GLUT_FOUND)
ELSE(APPLE) ELSE(APPLE)
# Find the FREEGLUT library # Find the FREEGLUT library
FIND_PACKAGE(FREEGLUT REQUIRED) FIND_PACKAGE(FREEGLUT REQUIRED)
IF(FREEGLUT_FOUND) IF(FREEGLUT_FOUND)
MESSAGE("FREEGLUT found") MESSAGE("FREEGLUT found")
ADD_DEFINITIONS(-DUSE_FREEGLUT) ELSE(FREEGLUT_FOUND)
ELSE(FREEGLUT_FOUND) MESSAGE(SEND_ERROR "FREEGLUT not found")
MESSAGE(SEND_ERROR "FREEGLUT not found")
ENDIF(FREEGLUT_FOUND) ENDIF(FREEGLUT_FOUND)
ENDIF(APPLE) ENDIF(APPLE)

1
examples/cubes/Cubes.cpp
View File

@ -120,7 +120,6 @@ void keyboard(unsigned char key, int x, int y) {
// Escape key // Escape key
case 27: case 27:
#ifdef USE_FREEGLUT #ifdef USE_FREEGLUT
// TODO : Check if we need to call finish() here
glutLeaveMainLoop(); glutLeaveMainLoop();
#endif #endif
break; break;

4
examples/cubes/Scene.cpp
View File

@ -70,7 +70,7 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0),
// Create a cube and a corresponding rigid in the dynamics world // Create a cube and a corresponding rigid in the dynamics world
Box* cube = new Box(BOX_SIZE, position , BOX_MASS, mDynamicsWorld); Box* cube = new Box(BOX_SIZE, position , BOX_MASS, mDynamicsWorld);
cube->getRigidBody()->setIsMotionEnabled(true); cube->getRigidBody()->enableMotion(true);
// Change the material properties of the rigid body // Change the material properties of the rigid body
rp3d::Material& material = cube->getRigidBody()->getMaterial(); rp3d::Material& material = cube->getRigidBody()->getMaterial();
@ -85,7 +85,7 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0),
mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld); mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld);
// The floor must be a non-moving rigid body // The floor must be a non-moving rigid body
mFloor->getRigidBody()->setIsMotionEnabled(false); mFloor->getRigidBody()->enableMotion(false);
// Change the material properties of the floor rigid body // Change the material properties of the floor rigid body
rp3d::Material& material = mFloor->getRigidBody()->getMaterial(); rp3d::Material& material = mFloor->getRigidBody()->getMaterial();

1
examples/joints/Joints.cpp
View File

@ -119,7 +119,6 @@ void keyboard(unsigned char key, int x, int y) {
// Escape key // Escape key
case 27: case 27:
#ifdef USE_FREEGLUT #ifdef USE_FREEGLUT
// TODO : Check if we need to call finish() here
glutLeaveMainLoop(); glutLeaveMainLoop();
#endif #endif
break; break;

16
examples/joints/Scene.cpp
View File

@ -205,8 +205,8 @@ void Scene::createBallAndSocketJoints() {
mDynamicsWorld); mDynamicsWorld);
// The fist box cannot move // The fist box cannot move
if (i == 0) mBallAndSocketJointChainBoxes[i]->getRigidBody()->setIsMotionEnabled(false); if (i == 0) mBallAndSocketJointChainBoxes[i]->getRigidBody()->enableMotion(false);
else mBallAndSocketJointChainBoxes[i]->getRigidBody()->setIsMotionEnabled(true); else mBallAndSocketJointChainBoxes[i]->getRigidBody()->enableMotion(true);
// Add some angular velocity damping // Add some angular velocity damping
mBallAndSocketJointChainBoxes[i]->getRigidBody()->setAngularDamping(rp3d::decimal(0.2)); mBallAndSocketJointChainBoxes[i]->getRigidBody()->setAngularDamping(rp3d::decimal(0.2));
@ -249,7 +249,7 @@ void Scene::createSliderJoint() {
mSliderJointBottomBox = new Box(box1Dimension, positionBox1 , BOX_MASS, mDynamicsWorld); mSliderJointBottomBox = new Box(box1Dimension, positionBox1 , BOX_MASS, mDynamicsWorld);
// The fist box cannot move // The fist box cannot move
mSliderJointBottomBox->getRigidBody()->setIsMotionEnabled(false); mSliderJointBottomBox->getRigidBody()->enableMotion(false);
// Change the material properties of the rigid body // Change the material properties of the rigid body
rp3d::Material& material1 = mSliderJointBottomBox->getRigidBody()->getMaterial(); rp3d::Material& material1 = mSliderJointBottomBox->getRigidBody()->getMaterial();
@ -265,7 +265,7 @@ void Scene::createSliderJoint() {
mSliderJointTopBox = new Box(box2Dimension, positionBox2 , BOX_MASS, mDynamicsWorld); mSliderJointTopBox = new Box(box2Dimension, positionBox2 , BOX_MASS, mDynamicsWorld);
// The second box is allowed to move // The second box is allowed to move
mSliderJointTopBox->getRigidBody()->setIsMotionEnabled(true); mSliderJointTopBox->getRigidBody()->enableMotion(true);
// Change the material properties of the rigid body // Change the material properties of the rigid body
rp3d::Material& material2 = mSliderJointTopBox->getRigidBody()->getMaterial(); rp3d::Material& material2 = mSliderJointTopBox->getRigidBody()->getMaterial();
@ -304,7 +304,7 @@ void Scene::createPropellerHingeJoint() {
mPropellerBox = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld); mPropellerBox = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld);
// The fist box cannot move // The fist box cannot move
mPropellerBox->getRigidBody()->setIsMotionEnabled(true); mPropellerBox->getRigidBody()->enableMotion(true);
// Change the material properties of the rigid body // Change the material properties of the rigid body
rp3d::Material& material = mPropellerBox->getRigidBody()->getMaterial(); rp3d::Material& material = mPropellerBox->getRigidBody()->getMaterial();
@ -342,7 +342,7 @@ void Scene::createFixedJoints() {
mFixedJointBox1 = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld); mFixedJointBox1 = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld);
// The fist box cannot move // The fist box cannot move
mFixedJointBox1->getRigidBody()->setIsMotionEnabled(true); mFixedJointBox1->getRigidBody()->enableMotion(true);
// Change the material properties of the rigid body // Change the material properties of the rigid body
rp3d::Material& material1 = mFixedJointBox1->getRigidBody()->getMaterial(); rp3d::Material& material1 = mFixedJointBox1->getRigidBody()->getMaterial();
@ -357,7 +357,7 @@ void Scene::createFixedJoints() {
mFixedJointBox2 = new Box(boxDimension, positionBox2 , BOX_MASS, mDynamicsWorld); mFixedJointBox2 = new Box(boxDimension, positionBox2 , BOX_MASS, mDynamicsWorld);
// The second box is allowed to move // The second box is allowed to move
mFixedJointBox2->getRigidBody()->setIsMotionEnabled(true); mFixedJointBox2->getRigidBody()->enableMotion(true);
// Change the material properties of the rigid body // Change the material properties of the rigid body
rp3d::Material& material2 = mFixedJointBox2->getRigidBody()->getMaterial(); rp3d::Material& material2 = mFixedJointBox2->getRigidBody()->getMaterial();
@ -396,7 +396,7 @@ void Scene::createFloor() {
mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld); mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld);
// The floor must be a non-moving rigid body // The floor must be a non-moving rigid body
mFloor->getRigidBody()->setIsMotionEnabled(false); mFloor->getRigidBody()->enableMotion(false);
// Change the material properties of the rigid body // Change the material properties of the rigid body
rp3d::Material& material = mFloor->getRigidBody()->getMaterial(); rp3d::Material& material = mFloor->getRigidBody()->getMaterial();

16
src/body/Body.h
View File

@ -83,12 +83,6 @@ class Body {
/// Return the id of the body /// Return the id of the body
bodyindex getID() const; bodyindex getID() const;
/// Return true if the body has already been added in an island (for the sleeping technique)
bool isAlreadyInIsland() const;
/// Set the value of to know if the body has already been added into an island
void setIsAlreadyInIsland(bool isAlreadyInIsland);
/// Return whether or not the body is allowed to sleep /// Return whether or not the body is allowed to sleep
bool isAllowedToSleep() const; bool isAllowedToSleep() const;
@ -126,16 +120,6 @@ inline bodyindex Body::getID() const {
return mID; return mID;
} }
// Return true if the body has already been added in an island (for the sleeping technique)
inline bool Body::isAlreadyInIsland() const {
return mIsAlreadyInIsland;
}
// Set the value of to know if the body has already been added into an island
inline void Body::setIsAlreadyInIsland(bool isAlreadyInIsland) {
mIsAlreadyInIsland = isAlreadyInIsland;
}
// Return whether or not the body is allowed to sleep // Return whether or not the body is allowed to sleep
inline bool Body::isAllowedToSleep() const { inline bool Body::isAllowedToSleep() const {
return mIsAllowedToSleep; return mIsAllowedToSleep;

57
src/body/CollisionBody.h
View File

@ -91,6 +91,12 @@ class CollisionBody : public Body {
/// Reset the contact manifold lists /// Reset the contact manifold lists
void resetContactManifoldsList(MemoryAllocator& memoryAllocator); void resetContactManifoldsList(MemoryAllocator& memoryAllocator);
/// Update the old transform with the current one.
void updateOldTransform();
/// Update the Axis-Aligned Bounding Box coordinates
void updateAABB();
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -101,12 +107,6 @@ class CollisionBody : public Body {
/// Destructor /// Destructor
virtual ~CollisionBody(); virtual ~CollisionBody();
/// Return true if the body has moved during the last frame
bool getHasMoved() const;
/// Set the hasMoved variable (true if the body has moved during the last frame)
void setHasMoved(bool hasMoved);
/// Return the collision shape /// Return the collision shape
CollisionShape* getCollisionShape() const; CollisionShape* getCollisionShape() const;
@ -128,23 +128,17 @@ class CollisionBody : public Body {
/// Set the interpolation factor of the body /// Set the interpolation factor of the body
void setInterpolationFactor(decimal factor); void setInterpolationFactor(decimal factor);
/// Return if the rigid body can move /// Return true if the rigid body is allowed to move
bool getIsMotionEnabled() const; bool isMotionEnabled() const;
/// Set the value to true if the body can move /// Enable/disable the motion of the body
void setIsMotionEnabled(bool isMotionEnabled); void enableMotion(bool isMotionEnabled);
/// Return true if the body can collide with others bodies /// Return true if the body can collide with others bodies
bool getIsCollisionEnabled() const; bool isCollisionEnabled() const;
/// Set the isCollisionEnabled value /// Enable/disable the collision with this body
void setIsCollisionEnabled(bool isCollisionEnabled); void enableCollision(bool isCollisionEnabled);
/// Update the old transform with the current one.
void updateOldTransform();
/// Update the Axis-Aligned Bounding Box coordinates
void updateAABB();
/// Return the first element of the linked list of contact manifolds involving this body /// Return the first element of the linked list of contact manifolds involving this body
const ContactManifoldListElement* getContactManifoldsLists() const; const ContactManifoldListElement* getContactManifoldsLists() const;
@ -152,18 +146,9 @@ class CollisionBody : public Body {
// -------------------- Friendship -------------------- // // -------------------- Friendship -------------------- //
friend class DynamicsWorld; friend class DynamicsWorld;
friend class CollisionDetection;
}; };
// Return true if the body has moved during the last frame
inline bool CollisionBody::getHasMoved() const {
return mHasMoved;
}
// Set the hasMoved variable (true if the body has moved during the last frame)
inline void CollisionBody::setHasMoved(bool hasMoved) {
mHasMoved = hasMoved;
}
// Return the collision shape // Return the collision shape
inline CollisionShape* CollisionBody::getCollisionShape() const { inline CollisionShape* CollisionBody::getCollisionShape() const {
assert(mCollisionShape); assert(mCollisionShape);
@ -187,13 +172,13 @@ inline void CollisionBody::setInterpolationFactor(decimal factor) {
mInterpolationFactor = factor; mInterpolationFactor = factor;
} }
// Return if the rigid body can move // Return true if the rigid body is allowed to move
inline bool CollisionBody::getIsMotionEnabled() const { inline bool CollisionBody::isMotionEnabled() const {
return mIsMotionEnabled; return mIsMotionEnabled;
} }
// Set the value to true if the body can move // Enable/disable the motion of the body
inline void CollisionBody::setIsMotionEnabled(bool isMotionEnabled) { inline void CollisionBody::enableMotion(bool isMotionEnabled) {
mIsMotionEnabled = isMotionEnabled; mIsMotionEnabled = isMotionEnabled;
} }
@ -219,12 +204,12 @@ inline const AABB& CollisionBody::getAABB() const {
} }
// Return true if the body can collide with others bodies // Return true if the body can collide with others bodies
inline bool CollisionBody::getIsCollisionEnabled() const { inline bool CollisionBody::isCollisionEnabled() const {
return mIsCollisionEnabled; return mIsCollisionEnabled;
} }
// Set the isCollisionEnabled value // Enable/disable the collision with this body
inline void CollisionBody::setIsCollisionEnabled(bool isCollisionEnabled) { inline void CollisionBody::enableCollision(bool isCollisionEnabled) {
mIsCollisionEnabled = isCollisionEnabled; mIsCollisionEnabled = isCollisionEnabled;
} }

6
src/body/RigidBody.h
View File

@ -105,6 +105,9 @@ class RigidBody : public CollisionBody {
/// Remove a joint from the joints list /// Remove a joint from the joints list
void removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint); void removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint);
/// Set the inverse of the mass
void setMassInverse(decimal massInverse);
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -134,9 +137,6 @@ class RigidBody : public CollisionBody {
/// Set the angular velocity /// Set the angular velocity
void setAngularVelocity(const Vector3& angularVelocity); void setAngularVelocity(const Vector3& angularVelocity);
/// Set the inverse of the mass
void setMassInverse(decimal massInverse);
/// Return the inverse of the mass of the body /// Return the inverse of the mass of the body
decimal getMassInverse() const; decimal getMassInverse() const;

2
src/collision/CollisionDetection.cpp
View File

@ -82,7 +82,7 @@ void CollisionDetection::computeBroadPhase() {
it != mWorld->getBodiesEndIterator(); it++) { it != mWorld->getBodiesEndIterator(); it++) {
// If the body has moved // If the body has moved
if ((*it)->getHasMoved()) { if ((*it)->mHasMoved) {
// Notify the broad-phase that the body has moved // Notify the broad-phase that the body has moved
mBroadPhaseAlgorithm->updateObject(*it, (*it)->getAABB()); mBroadPhaseAlgorithm->updateObject(*it, (*it)->getAABB());

2
src/collision/CollisionDetection.h
View File

@ -38,8 +38,6 @@
#include <map> #include <map>
#include <set> #include <set>
#include <utility> #include <utility>
#include <iostream> // TODO : Delete this
/// ReactPhysics3D namespace /// ReactPhysics3D namespace
namespace reactphysics3d { namespace reactphysics3d {

2
src/configuration.h
View File

@ -105,7 +105,7 @@ const decimal PERSISTENT_CONTACT_DIST_THRESHOLD = decimal(0.03);
const decimal RESTITUTION_VELOCITY_THRESHOLD = decimal(1.0); const decimal RESTITUTION_VELOCITY_THRESHOLD = decimal(1.0);
/// Number of iterations when solving the velocity constraints of the Sequential Impulse technique /// Number of iterations when solving the velocity constraints of the Sequential Impulse technique
const uint DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 15; const uint DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 10;
/// Number of iterations when solving the position constraints of the Sequential Impulse technique /// Number of iterations when solving the position constraints of the Sequential Impulse technique
const uint DEFAULT_POSITION_SOLVER_NB_ITERATIONS = 5; const uint DEFAULT_POSITION_SOLVER_NB_ITERATIONS = 5;

32
src/constraint/BallAndSocketJoint.cpp
View File

@ -73,25 +73,25 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
// Compute the matrix K=JM^-1J^t (3x3 matrix) // Compute the matrix K=JM^-1J^t (3x3 matrix)
decimal inverseMassBodies = 0.0; decimal inverseMassBodies = 0.0;
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
inverseMassBodies += mBody1->getMassInverse(); inverseMassBodies += mBody1->getMassInverse();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
inverseMassBodies += mBody2->getMassInverse(); inverseMassBodies += mBody2->getMassInverse();
} }
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies); 0, 0, inverseMassBodies);
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose(); massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
} }
// Compute the inverse mass matrix K^-1 // Compute the inverse mass matrix K^-1
mInverseMassMatrix.setToZero(); mInverseMassMatrix.setToZero();
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
mInverseMassMatrix = massMatrix.getInverse(); mInverseMassMatrix = massMatrix.getInverse();
} }
@ -123,7 +123,7 @@ void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverD
const decimal inverseMassBody1 = mBody1->getMassInverse(); const decimal inverseMassBody1 = mBody1->getMassInverse();
const decimal inverseMassBody2 = mBody2->getMassInverse(); const decimal inverseMassBody2 = mBody2->getMassInverse();
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda // Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody1 = -mImpulse; const Vector3 linearImpulseBody1 = -mImpulse;
@ -133,7 +133,7 @@ void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverD
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda // Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody2 = mImpulse; const Vector3 linearImpulseBody2 = mImpulse;
@ -165,7 +165,7 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con
const Vector3 deltaLambda = mInverseMassMatrix * (-Jv - mBiasVector); const Vector3 deltaLambda = mInverseMassMatrix * (-Jv - mBiasVector);
mImpulse += deltaLambda; mImpulse += deltaLambda;
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda // Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody1 = -deltaLambda; const Vector3 linearImpulseBody1 = -deltaLambda;
@ -175,7 +175,7 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda // Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody2 = deltaLambda; const Vector3 linearImpulseBody2 = deltaLambda;
@ -218,23 +218,23 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
// Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation constraints // Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation constraints
decimal inverseMassBodies = 0.0; decimal inverseMassBodies = 0.0;
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
inverseMassBodies += inverseMassBody1; inverseMassBodies += inverseMassBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
inverseMassBodies += inverseMassBody2; inverseMassBodies += inverseMassBody2;
} }
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies); 0, 0, inverseMassBodies);
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose(); massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
} }
mInverseMassMatrix.setToZero(); mInverseMassMatrix.setToZero();
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
mInverseMassMatrix = massMatrix.getInverse(); mInverseMassMatrix = massMatrix.getInverse();
} }
@ -247,7 +247,7 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
const Vector3 lambda = mInverseMassMatrix * (-constraintError); const Vector3 lambda = mInverseMassMatrix * (-constraintError);
// Apply the impulse to the bodies of the joint (directly update the position/orientation) // Apply the impulse to the bodies of the joint (directly update the position/orientation)
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse // Compute the impulse
const Vector3 linearImpulseBody1 = -lambda; const Vector3 linearImpulseBody1 = -lambda;
@ -262,7 +262,7 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize(); q1.normalize();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse // Compute the impulse
const Vector3 linearImpulseBody2 = lambda; const Vector3 linearImpulseBody2 = lambda;

23
src/constraint/BallAndSocketJoint.h
View File

@ -56,7 +56,8 @@ struct BallAndSocketJointInfo : public JointInfo {
// Class BallAndSocketJoint // Class BallAndSocketJoint
/** /**
* This class represents a ball-and-socket joint that allows arbitrary rotation * This class represents a ball-and-socket joint that allows arbitrary rotation
* between two bodies. * between two bodies. This joint has three degrees of freedom. It can be used to
* create a chain of bodies for instance.
*/ */
class BallAndSocketJoint : public Joint { class BallAndSocketJoint : public Joint {
@ -104,16 +105,6 @@ class BallAndSocketJoint : public Joint {
/// Private assignment operator /// Private assignment operator
BallAndSocketJoint& operator=(const BallAndSocketJoint& constraint); BallAndSocketJoint& operator=(const BallAndSocketJoint& constraint);
public :
// -------------------- Methods -------------------- //
/// Constructor
BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo);
/// Destructor
virtual ~BallAndSocketJoint();
/// Return the number of bytes used by the joint /// Return the number of bytes used by the joint
virtual size_t getSizeInBytes() const; virtual size_t getSizeInBytes() const;
@ -128,6 +119,16 @@ class BallAndSocketJoint : public Joint {
/// Solve the position constraint (for position error correction) /// Solve the position constraint (for position error correction)
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
public :
// -------------------- Methods -------------------- //
/// Constructor
BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo);
/// Destructor
virtual ~BallAndSocketJoint();
}; };
// Return the number of bytes used by the joint // Return the number of bytes used by the joint

52
src/constraint/FixedJoint.cpp
View File

@ -81,25 +81,25 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
decimal inverseMassBodies = 0.0; decimal inverseMassBodies = 0.0;
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
inverseMassBodies += mBody1->getMassInverse(); inverseMassBodies += mBody1->getMassInverse();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
inverseMassBodies += mBody2->getMassInverse(); inverseMassBodies += mBody2->getMassInverse();
} }
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies); 0, 0, inverseMassBodies);
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose(); massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
} }
// Compute the inverse mass matrix K^-1 for the 3 translation constraints // Compute the inverse mass matrix K^-1 for the 3 translation constraints
mInverseMassMatrixTranslation.setToZero(); mInverseMassMatrixTranslation.setToZero();
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
mInverseMassMatrixTranslation = massMatrix.getInverse(); mInverseMassMatrixTranslation = massMatrix.getInverse();
} }
@ -113,13 +113,13 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix) // contraints (3x3 matrix)
mInverseMassMatrixRotation.setToZero(); mInverseMassMatrixRotation.setToZero();
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
mInverseMassMatrixRotation += mI1; mInverseMassMatrixRotation += mI1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
mInverseMassMatrixRotation += mI2; mInverseMassMatrixRotation += mI2;
} }
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse(); mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse();
} }
@ -154,7 +154,7 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
const decimal inverseMassBody1 = mBody1->getMassInverse(); const decimal inverseMassBody1 = mBody1->getMassInverse();
const decimal inverseMassBody2 = mBody2->getMassInverse(); const decimal inverseMassBody2 = mBody2->getMassInverse();
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 translation constraints // Compute the impulse P=J^T * lambda for the 3 translation constraints
Vector3 linearImpulseBody1 = -mImpulseTranslation; Vector3 linearImpulseBody1 = -mImpulseTranslation;
@ -167,7 +167,7 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 translation constraints // Compute the impulse P=J^T * lambda for the 3 translation constraints
Vector3 linearImpulseBody2 = mImpulseTranslation; Vector3 linearImpulseBody2 = mImpulseTranslation;
@ -205,7 +205,7 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
(-JvTranslation - mBiasTranslation); (-JvTranslation - mBiasTranslation);
mImpulseTranslation += deltaLambda; mImpulseTranslation += deltaLambda;
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda // Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody1 = -deltaLambda; const Vector3 linearImpulseBody1 = -deltaLambda;
@ -215,7 +215,7 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda // Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody2 = deltaLambda; const Vector3 linearImpulseBody2 = deltaLambda;
@ -235,7 +235,7 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
Vector3 deltaLambda2 = mInverseMassMatrixRotation * (-JvRotation - mBiasRotation); Vector3 deltaLambda2 = mInverseMassMatrixRotation * (-JvRotation - mBiasRotation);
mImpulseRotation += deltaLambda2; mImpulseRotation += deltaLambda2;
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints // Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody1 = -deltaLambda2; const Vector3 angularImpulseBody1 = -deltaLambda2;
@ -243,7 +243,7 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
// Apply the impulse to the body // Apply the impulse to the body
w1 += mI1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints // Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody2 = deltaLambda2; const Vector3 angularImpulseBody2 = deltaLambda2;
@ -286,23 +286,23 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
decimal inverseMassBodies = 0.0; decimal inverseMassBodies = 0.0;
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
inverseMassBodies += mBody1->getMassInverse(); inverseMassBodies += mBody1->getMassInverse();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
inverseMassBodies += mBody2->getMassInverse(); inverseMassBodies += mBody2->getMassInverse();
} }
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies); 0, 0, inverseMassBodies);
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose(); massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
} }
mInverseMassMatrixTranslation.setToZero(); mInverseMassMatrixTranslation.setToZero();
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
mInverseMassMatrixTranslation = massMatrix.getInverse(); mInverseMassMatrixTranslation = massMatrix.getInverse();
} }
@ -313,7 +313,7 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
const Vector3 lambdaTranslation = mInverseMassMatrixTranslation * (-errorTranslation); const Vector3 lambdaTranslation = mInverseMassMatrixTranslation * (-errorTranslation);
// Apply the impulse to the bodies of the joint // Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse // Compute the impulse
Vector3 linearImpulseBody1 = -lambdaTranslation; Vector3 linearImpulseBody1 = -lambdaTranslation;
@ -328,7 +328,7 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize(); q1.normalize();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse // Compute the impulse
Vector3 linearImpulseBody2 = lambdaTranslation; Vector3 linearImpulseBody2 = lambdaTranslation;
@ -349,13 +349,13 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix) // contraints (3x3 matrix)
mInverseMassMatrixRotation.setToZero(); mInverseMassMatrixRotation.setToZero();
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
mInverseMassMatrixRotation += mI1; mInverseMassMatrixRotation += mI1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
mInverseMassMatrixRotation += mI2; mInverseMassMatrixRotation += mI2;
} }
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse(); mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse();
} }
@ -369,7 +369,7 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
Vector3 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation); Vector3 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation);
// Apply the impulse to the bodies of the joint // Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints // Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody1 = -lambdaRotation; const Vector3 angularImpulseBody1 = -lambdaRotation;
@ -381,7 +381,7 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize(); q1.normalize();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse // Compute the impulse
const Vector3 angularImpulseBody2 = lambdaRotation; const Vector3 angularImpulseBody2 = lambdaRotation;

20
src/constraint/FixedJoint.h
View File

@ -116,16 +116,6 @@ class FixedJoint : public Joint {
/// Private assignment operator /// Private assignment operator
FixedJoint& operator=(const FixedJoint& constraint); FixedJoint& operator=(const FixedJoint& constraint);
public :
// -------------------- Methods -------------------- //
/// Constructor
FixedJoint(const FixedJointInfo& jointInfo);
/// Destructor
virtual ~FixedJoint();
/// Return the number of bytes used by the joint /// Return the number of bytes used by the joint
virtual size_t getSizeInBytes() const; virtual size_t getSizeInBytes() const;
@ -140,6 +130,16 @@ class FixedJoint : public Joint {
/// Solve the position constraint (for position error correction) /// Solve the position constraint (for position error correction)
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
public :
// -------------------- Methods -------------------- //
/// Constructor
FixedJoint(const FixedJointInfo& jointInfo);
/// Destructor
virtual ~FixedJoint();
}; };
// Return the number of bytes used by the joint // Return the number of bytes used by the joint

80
src/constraint/HingeJoint.cpp
View File

@ -123,23 +123,23 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
// Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix) // Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix)
decimal inverseMassBodies = 0.0; decimal inverseMassBodies = 0.0;
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
inverseMassBodies += mBody1->getMassInverse(); inverseMassBodies += mBody1->getMassInverse();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
inverseMassBodies += mBody2->getMassInverse(); inverseMassBodies += mBody2->getMassInverse();
} }
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies); 0, 0, inverseMassBodies);
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose(); massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
} }
mInverseMassMatrixTranslation.setToZero(); mInverseMassMatrixTranslation.setToZero();
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
mInverseMassMatrixTranslation = massMatrix.getInverse(); mInverseMassMatrixTranslation = massMatrix.getInverse();
} }
@ -155,11 +155,11 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
Vector3 I1C2CrossA1(0, 0, 0); Vector3 I1C2CrossA1(0, 0, 0);
Vector3 I2B2CrossA1(0, 0, 0); Vector3 I2B2CrossA1(0, 0, 0);
Vector3 I2C2CrossA1(0, 0, 0); Vector3 I2C2CrossA1(0, 0, 0);
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
I1B2CrossA1 = mI1 * mB2CrossA1; I1B2CrossA1 = mI1 * mB2CrossA1;
I1C2CrossA1 = mI1 * mC2CrossA1; I1C2CrossA1 = mI1 * mC2CrossA1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
I2B2CrossA1 = mI2 * mB2CrossA1; I2B2CrossA1 = mI2 * mB2CrossA1;
I2C2CrossA1 = mI2 * mC2CrossA1; I2C2CrossA1 = mI2 * mC2CrossA1;
} }
@ -173,7 +173,7 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
mC2CrossA1.dot(I2C2CrossA1); mC2CrossA1.dot(I2C2CrossA1);
const Matrix2x2 matrixKRotation(el11, el12, el21, el22); const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
mInverseMassMatrixRotation.setToZero(); mInverseMassMatrixRotation.setToZero();
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
mInverseMassMatrixRotation = matrixKRotation.getInverse(); mInverseMassMatrixRotation = matrixKRotation.getInverse();
} }
@ -198,10 +198,10 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
mInverseMassMatrixLimitMotor = 0.0; mInverseMassMatrixLimitMotor = 0.0;
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
mInverseMassMatrixLimitMotor += mA1.dot(mI1 * mA1); mInverseMassMatrixLimitMotor += mA1.dot(mI1 * mA1);
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
mInverseMassMatrixLimitMotor += mA1.dot(mI2 * mA1); mInverseMassMatrixLimitMotor += mA1.dot(mI2 * mA1);
} }
mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ? mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ?
@ -243,7 +243,7 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Compute the impulse P=J^T * lambda for the motor constraint // Compute the impulse P=J^T * lambda for the motor constraint
const Vector3 motorImpulse = -mImpulseMotor * mA1; const Vector3 motorImpulse = -mImpulseMotor * mA1;
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 translation constraints // Compute the impulse P=J^T * lambda for the 3 translation constraints
Vector3 linearImpulseBody1 = -mImpulseTranslation; Vector3 linearImpulseBody1 = -mImpulseTranslation;
@ -262,7 +262,7 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 translation constraints // Compute the impulse P=J^T * lambda for the 3 translation constraints
Vector3 linearImpulseBody2 = mImpulseTranslation; Vector3 linearImpulseBody2 = mImpulseTranslation;
@ -306,7 +306,7 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
(-JvTranslation - mBTranslation); (-JvTranslation - mBTranslation);
mImpulseTranslation += deltaLambdaTranslation; mImpulseTranslation += deltaLambdaTranslation;
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda // Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody1 = -deltaLambdaTranslation; const Vector3 linearImpulseBody1 = -deltaLambdaTranslation;
@ -316,7 +316,7 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda // Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody2 = deltaLambdaTranslation; const Vector3 linearImpulseBody2 = deltaLambdaTranslation;
@ -337,7 +337,7 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
Vector2 deltaLambdaRotation = mInverseMassMatrixRotation * (-JvRotation - mBRotation); Vector2 deltaLambdaRotation = mInverseMassMatrixRotation * (-JvRotation - mBRotation);
mImpulseRotation += deltaLambdaRotation; mImpulseRotation += deltaLambdaRotation;
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 rotation constraints // Compute the impulse P=J^T * lambda for the 2 rotation constraints
const Vector3 angularImpulseBody1 = -mB2CrossA1 * deltaLambdaRotation.x - const Vector3 angularImpulseBody1 = -mB2CrossA1 * deltaLambdaRotation.x -
@ -346,7 +346,7 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
// Apply the impulse to the body // Apply the impulse to the body
w1 += mI1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 rotation constraints // Compute the impulse P=J^T * lambda for the 2 rotation constraints
const Vector3 angularImpulseBody2 = mB2CrossA1 * deltaLambdaRotation.x + const Vector3 angularImpulseBody2 = mB2CrossA1 * deltaLambdaRotation.x +
@ -372,7 +372,7 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0)); mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0));
deltaLambdaLower = mImpulseLowerLimit - lambdaTemp; deltaLambdaLower = mImpulseLowerLimit - lambdaTemp;
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the lower limit constraint // Compute the impulse P=J^T * lambda for the lower limit constraint
const Vector3 angularImpulseBody1 = -deltaLambdaLower * mA1; const Vector3 angularImpulseBody1 = -deltaLambdaLower * mA1;
@ -380,7 +380,7 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
// Apply the impulse to the body // Apply the impulse to the body
w1 += mI1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the lower limit constraint // Compute the impulse P=J^T * lambda for the lower limit constraint
const Vector3 angularImpulseBody2 = deltaLambdaLower * mA1; const Vector3 angularImpulseBody2 = deltaLambdaLower * mA1;
@ -402,7 +402,7 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0)); mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0));
deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp; deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp;
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the upper limit constraint // Compute the impulse P=J^T * lambda for the upper limit constraint
const Vector3 angularImpulseBody1 = deltaLambdaUpper * mA1; const Vector3 angularImpulseBody1 = deltaLambdaUpper * mA1;
@ -410,7 +410,7 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
// Apply the impulse to the body // Apply the impulse to the body
w1 += mI1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the upper limit constraint // Compute the impulse P=J^T * lambda for the upper limit constraint
const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mA1; const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mA1;
@ -435,7 +435,7 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
deltaLambdaMotor = mImpulseMotor - lambdaTemp; deltaLambdaMotor = mImpulseMotor - lambdaTemp;
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the motor // Compute the impulse P=J^T * lambda for the motor
const Vector3 angularImpulseBody1 = -deltaLambdaMotor * mA1; const Vector3 angularImpulseBody1 = -deltaLambdaMotor * mA1;
@ -443,7 +443,7 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
// Apply the impulse to the body // Apply the impulse to the body
w1 += mI1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the motor // Compute the impulse P=J^T * lambda for the motor
const Vector3 angularImpulseBody2 = deltaLambdaMotor * mA1; const Vector3 angularImpulseBody2 = deltaLambdaMotor * mA1;
@ -506,23 +506,23 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
decimal inverseMassBodies = 0.0; decimal inverseMassBodies = 0.0;
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
inverseMassBodies += mBody1->getMassInverse(); inverseMassBodies += mBody1->getMassInverse();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
inverseMassBodies += mBody2->getMassInverse(); inverseMassBodies += mBody2->getMassInverse();
} }
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies); 0, 0, inverseMassBodies);
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose(); massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
} }
mInverseMassMatrixTranslation.setToZero(); mInverseMassMatrixTranslation.setToZero();
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
mInverseMassMatrixTranslation = massMatrix.getInverse(); mInverseMassMatrixTranslation = massMatrix.getInverse();
} }
@ -533,7 +533,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
const Vector3 lambdaTranslation = mInverseMassMatrixTranslation * (-errorTranslation); const Vector3 lambdaTranslation = mInverseMassMatrixTranslation * (-errorTranslation);
// Apply the impulse to the bodies of the joint // Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse // Compute the impulse
Vector3 linearImpulseBody1 = -lambdaTranslation; Vector3 linearImpulseBody1 = -lambdaTranslation;
@ -548,7 +548,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize(); q1.normalize();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse // Compute the impulse
Vector3 linearImpulseBody2 = lambdaTranslation; Vector3 linearImpulseBody2 = lambdaTranslation;
@ -571,11 +571,11 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
Vector3 I1C2CrossA1(0, 0, 0); Vector3 I1C2CrossA1(0, 0, 0);
Vector3 I2B2CrossA1(0, 0, 0); Vector3 I2B2CrossA1(0, 0, 0);
Vector3 I2C2CrossA1(0, 0, 0); Vector3 I2C2CrossA1(0, 0, 0);
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
I1B2CrossA1 = mI1 * mB2CrossA1; I1B2CrossA1 = mI1 * mB2CrossA1;
I1C2CrossA1 = mI1 * mC2CrossA1; I1C2CrossA1 = mI1 * mC2CrossA1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
I2B2CrossA1 = mI2 * mB2CrossA1; I2B2CrossA1 = mI2 * mB2CrossA1;
I2C2CrossA1 = mI2 * mC2CrossA1; I2C2CrossA1 = mI2 * mC2CrossA1;
} }
@ -589,7 +589,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
mC2CrossA1.dot(I2C2CrossA1); mC2CrossA1.dot(I2C2CrossA1);
const Matrix2x2 matrixKRotation(el11, el12, el21, el22); const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
mInverseMassMatrixRotation.setToZero(); mInverseMassMatrixRotation.setToZero();
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
mInverseMassMatrixRotation = matrixKRotation.getInverse(); mInverseMassMatrixRotation = matrixKRotation.getInverse();
} }
@ -600,7 +600,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
Vector2 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation); Vector2 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation);
// Apply the impulse to the bodies of the joint // Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints // Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody1 = -mB2CrossA1 * lambdaRotation.x - const Vector3 angularImpulseBody1 = -mB2CrossA1 * lambdaRotation.x -
@ -613,7 +613,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize(); q1.normalize();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse // Compute the impulse
const Vector3 angularImpulseBody2 = mB2CrossA1 * lambdaRotation.x + const Vector3 angularImpulseBody2 = mB2CrossA1 * lambdaRotation.x +
@ -635,10 +635,10 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
mInverseMassMatrixLimitMotor = 0.0; mInverseMassMatrixLimitMotor = 0.0;
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
mInverseMassMatrixLimitMotor += mA1.dot(mI1 * mA1); mInverseMassMatrixLimitMotor += mA1.dot(mI1 * mA1);
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
mInverseMassMatrixLimitMotor += mA1.dot(mI2 * mA1); mInverseMassMatrixLimitMotor += mA1.dot(mI2 * mA1);
} }
mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ? mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ?
@ -652,7 +652,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
decimal lambdaLowerLimit = mInverseMassMatrixLimitMotor * (-lowerLimitError ); decimal lambdaLowerLimit = mInverseMassMatrixLimitMotor * (-lowerLimitError );
// Apply the impulse to the bodies of the joint // Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda // Compute the impulse P=J^T * lambda
const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mA1; const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mA1;
@ -664,7 +664,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize(); q1.normalize();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda // Compute the impulse P=J^T * lambda
const Vector3 angularImpulseBody2 = lambdaLowerLimit * mA1; const Vector3 angularImpulseBody2 = lambdaLowerLimit * mA1;
@ -685,7 +685,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
decimal lambdaUpperLimit = mInverseMassMatrixLimitMotor * (-upperLimitError); decimal lambdaUpperLimit = mInverseMassMatrixLimitMotor * (-upperLimitError);
// Apply the impulse to the bodies of the joint // Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda // Compute the impulse P=J^T * lambda
const Vector3 angularImpulseBody1 = lambdaUpperLimit * mA1; const Vector3 angularImpulseBody1 = lambdaUpperLimit * mA1;
@ -697,7 +697,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize(); q1.normalize();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda // Compute the impulse P=J^T * lambda
const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mA1; const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mA1;

33
src/constraint/HingeJoint.h
View File

@ -109,7 +109,8 @@ struct HingeJointInfo : public JointInfo {
// Class HingeJoint // Class HingeJoint
/** /**
* This class represents a hinge joint that allows arbitrary rotation * This class represents a hinge joint that allows arbitrary rotation
* between two bodies around a single axis. * between two bodies around a single axis. This joint has one degree of freedom. It
* can be useful to simulate doors or pendulumns.
*/ */
class HingeJoint : public Joint { class HingeJoint : public Joint {
@ -246,6 +247,21 @@ class HingeJoint : public Joint {
decimal computeCurrentHingeAngle(const Quaternion& orientationBody1, decimal computeCurrentHingeAngle(const Quaternion& orientationBody1,
const Quaternion& orientationBody2); const Quaternion& orientationBody2);
/// Return the number of bytes used by the joint
virtual size_t getSizeInBytes() const;
/// Initialize before solving the constraint
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData);
/// Warm start the constraint (apply the previous impulse at the beginning of the step)
virtual void warmstart(const ConstraintSolverData& constraintSolverData);
/// Solve the velocity constraint
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
/// Solve the position constraint (for position error correction)
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -294,21 +310,6 @@ class HingeJoint : public Joint {
/// Return the intensity of the current torque applied for the joint motor /// Return the intensity of the current torque applied for the joint motor
decimal getMotorTorque(decimal timeStep) const; decimal getMotorTorque(decimal timeStep) const;
/// Return the number of bytes used by the joint
virtual size_t getSizeInBytes() const;
/// Initialize before solving the constraint
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData);
/// Warm start the constraint (apply the previous impulse at the beginning of the step)
virtual void warmstart(const ConstraintSolverData& constraintSolverData);
/// Solve the velocity constraint
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
/// Solve the position constraint (for position error correction)
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
}; };
// Return true if the limits or the joint are enabled // Return true if the limits or the joint are enabled

37
src/constraint/Joint.h
View File

@ -152,6 +152,24 @@ class Joint {
/// Private assignment operator /// Private assignment operator
Joint& operator=(const Joint& constraint); Joint& operator=(const Joint& constraint);
/// Return true if the joint has already been added into an island
bool isAlreadyInIsland() const;
/// Return the number of bytes used by the joint
virtual size_t getSizeInBytes() const = 0;
/// Initialize before solving the joint
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData) = 0;
/// Warm start the joint (apply the previous impulse at the beginning of the step)
virtual void warmstart(const ConstraintSolverData& constraintSolverData) = 0;
/// Solve the velocity constraint
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) = 0;
/// Solve the position constraint
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData) = 0;
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -177,28 +195,11 @@ class Joint {
/// Return true if the collision between the two bodies of the joint is enabled /// Return true if the collision between the two bodies of the joint is enabled
bool isCollisionEnabled() const; bool isCollisionEnabled() const;
/// Return true if the joint has already been added into an island
bool isAlreadyInIsland() const;
/// Return the number of bytes used by the joint
virtual size_t getSizeInBytes() const = 0;
/// Initialize before solving the joint
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData) = 0;
/// Warm start the joint (apply the previous impulse at the beginning of the step)
virtual void warmstart(const ConstraintSolverData& constraintSolverData) = 0;
/// Solve the velocity constraint
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) = 0;
/// Solve the position constraint
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData) = 0;
// -------------------- Friendship -------------------- // // -------------------- Friendship -------------------- //
friend class DynamicsWorld; friend class DynamicsWorld;
friend class Island; friend class Island;
friend class ConstraintSolver;
}; };
// Return the reference to the body 1 // Return the reference to the body 1

76
src/constraint/SliderJoint.cpp
View File

@ -129,12 +129,12 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
Vector3 I1R1PlusUCrossN2(0, 0, 0); Vector3 I1R1PlusUCrossN2(0, 0, 0);
Vector3 I2R2CrossN1(0, 0, 0); Vector3 I2R2CrossN1(0, 0, 0);
Vector3 I2R2CrossN2(0, 0, 0); Vector3 I2R2CrossN2(0, 0, 0);
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
sumInverseMass += mBody1->getMassInverse(); sumInverseMass += mBody1->getMassInverse();
I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1; I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1;
I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2; I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
sumInverseMass += mBody2->getMassInverse(); sumInverseMass += mBody2->getMassInverse();
I2R2CrossN1 = mI2 * mR2CrossN1; I2R2CrossN1 = mI2 * mR2CrossN1;
I2R2CrossN2 = mI2 * mR2CrossN2; I2R2CrossN2 = mI2 * mR2CrossN2;
@ -149,7 +149,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
mR2CrossN2.dot(I2R2CrossN2); mR2CrossN2.dot(I2R2CrossN2);
Matrix2x2 matrixKTranslation(el11, el12, el21, el22); Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
mInverseMassMatrixTranslationConstraint.setToZero(); mInverseMassMatrixTranslationConstraint.setToZero();
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse();
} }
@ -165,13 +165,13 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix) // contraints (3x3 matrix)
mInverseMassMatrixRotationConstraint.setToZero(); mInverseMassMatrixRotationConstraint.setToZero();
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
mInverseMassMatrixRotationConstraint += mI1; mInverseMassMatrixRotationConstraint += mI1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
mInverseMassMatrixRotationConstraint += mI2; mInverseMassMatrixRotationConstraint += mI2;
} }
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse(); mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse();
} }
@ -188,11 +188,11 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
mInverseMassMatrixLimit = 0.0; mInverseMassMatrixLimit = 0.0;
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
mInverseMassMatrixLimit += mBody1->getMassInverse() + mInverseMassMatrixLimit += mBody1->getMassInverse() +
mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis); mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis);
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
mInverseMassMatrixLimit += mBody2->getMassInverse() + mInverseMassMatrixLimit += mBody2->getMassInverse() +
mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis); mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis);
} }
@ -214,10 +214,10 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
// Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix) // Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix)
mInverseMassMatrixMotor = 0.0; mInverseMassMatrixMotor = 0.0;
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
mInverseMassMatrixMotor += mBody1->getMassInverse(); mInverseMassMatrixMotor += mBody1->getMassInverse();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
mInverseMassMatrixMotor += mBody2->getMassInverse(); mInverseMassMatrixMotor += mBody2->getMassInverse();
} }
mInverseMassMatrixMotor = (mInverseMassMatrixMotor > 0.0) ? mInverseMassMatrixMotor = (mInverseMassMatrixMotor > 0.0) ?
@ -255,7 +255,7 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Compute the impulse P=J^T * lambda for the motor constraint // Compute the impulse P=J^T * lambda for the motor constraint
Vector3 impulseMotor = mImpulseMotor * mSliderAxisWorld; Vector3 impulseMotor = mImpulseMotor * mSliderAxisWorld;
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 translation constraints // Compute the impulse P=J^T * lambda for the 2 translation constraints
Vector3 linearImpulseBody1 = -mN1 * mImpulseTranslation.x - mN2 * mImpulseTranslation.y; Vector3 linearImpulseBody1 = -mN1 * mImpulseTranslation.x - mN2 * mImpulseTranslation.y;
@ -276,7 +276,7 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 translation constraints // Compute the impulse P=J^T * lambda for the 2 translation constraints
Vector3 linearImpulseBody2 = mN1 * mImpulseTranslation.x + mN2 * mImpulseTranslation.y; Vector3 linearImpulseBody2 = mN1 * mImpulseTranslation.x + mN2 * mImpulseTranslation.y;
@ -325,7 +325,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
Vector2 deltaLambda = mInverseMassMatrixTranslationConstraint * (-JvTranslation -mBTranslation); Vector2 deltaLambda = mInverseMassMatrixTranslationConstraint * (-JvTranslation -mBTranslation);
mImpulseTranslation += deltaLambda; mImpulseTranslation += deltaLambda;
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 translation constraints // Compute the impulse P=J^T * lambda for the 2 translation constraints
const Vector3 linearImpulseBody1 = -mN1 * deltaLambda.x - mN2 * deltaLambda.y; const Vector3 linearImpulseBody1 = -mN1 * deltaLambda.x - mN2 * deltaLambda.y;
@ -336,7 +336,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 translation constraints // Compute the impulse P=J^T * lambda for the 2 translation constraints
const Vector3 linearImpulseBody2 = mN1 * deltaLambda.x + mN2 * deltaLambda.y; const Vector3 linearImpulseBody2 = mN1 * deltaLambda.x + mN2 * deltaLambda.y;
@ -356,7 +356,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
Vector3 deltaLambda2 = mInverseMassMatrixRotationConstraint * (-JvRotation - mBRotation); Vector3 deltaLambda2 = mInverseMassMatrixRotationConstraint * (-JvRotation - mBRotation);
mImpulseRotation += deltaLambda2; mImpulseRotation += deltaLambda2;
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints // Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody1 = -deltaLambda2; const Vector3 angularImpulseBody1 = -deltaLambda2;
@ -364,7 +364,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
// Apply the impulse to the body // Apply the impulse to the body
w1 += mI1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints // Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody2 = deltaLambda2; const Vector3 angularImpulseBody2 = deltaLambda2;
@ -390,7 +390,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0)); mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0));
deltaLambdaLower = mImpulseLowerLimit - lambdaTemp; deltaLambdaLower = mImpulseLowerLimit - lambdaTemp;
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the lower limit constraint // Compute the impulse P=J^T * lambda for the lower limit constraint
const Vector3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld; const Vector3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld;
@ -400,7 +400,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the lower limit constraint // Compute the impulse P=J^T * lambda for the lower limit constraint
const Vector3 linearImpulseBody2 = deltaLambdaLower * mSliderAxisWorld; const Vector3 linearImpulseBody2 = deltaLambdaLower * mSliderAxisWorld;
@ -425,7 +425,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0)); mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0));
deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp; deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp;
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the upper limit constraint // Compute the impulse P=J^T * lambda for the upper limit constraint
const Vector3 linearImpulseBody1 = deltaLambdaUpper * mSliderAxisWorld; const Vector3 linearImpulseBody1 = deltaLambdaUpper * mSliderAxisWorld;
@ -435,7 +435,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the upper limit constraint // Compute the impulse P=J^T * lambda for the upper limit constraint
const Vector3 linearImpulseBody2 = -deltaLambdaUpper * mSliderAxisWorld; const Vector3 linearImpulseBody2 = -deltaLambdaUpper * mSliderAxisWorld;
@ -462,7 +462,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
deltaLambdaMotor = mImpulseMotor - lambdaTemp; deltaLambdaMotor = mImpulseMotor - lambdaTemp;
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the motor // Compute the impulse P=J^T * lambda for the motor
const Vector3 linearImpulseBody1 = deltaLambdaMotor * mSliderAxisWorld; const Vector3 linearImpulseBody1 = deltaLambdaMotor * mSliderAxisWorld;
@ -470,7 +470,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
// Apply the impulse to the body // Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the motor // Compute the impulse P=J^T * lambda for the motor
const Vector3 linearImpulseBody2 = -deltaLambdaMotor * mSliderAxisWorld; const Vector3 linearImpulseBody2 = -deltaLambdaMotor * mSliderAxisWorld;
@ -540,12 +540,12 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
Vector3 I1R1PlusUCrossN2(0, 0, 0); Vector3 I1R1PlusUCrossN2(0, 0, 0);
Vector3 I2R2CrossN1(0, 0, 0); Vector3 I2R2CrossN1(0, 0, 0);
Vector3 I2R2CrossN2(0, 0, 0); Vector3 I2R2CrossN2(0, 0, 0);
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
sumInverseMass += mBody1->getMassInverse(); sumInverseMass += mBody1->getMassInverse();
I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1; I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1;
I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2; I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
sumInverseMass += mBody2->getMassInverse(); sumInverseMass += mBody2->getMassInverse();
I2R2CrossN1 = mI2 * mR2CrossN1; I2R2CrossN1 = mI2 * mR2CrossN1;
I2R2CrossN2 = mI2 * mR2CrossN2; I2R2CrossN2 = mI2 * mR2CrossN2;
@ -560,7 +560,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
mR2CrossN2.dot(I2R2CrossN2); mR2CrossN2.dot(I2R2CrossN2);
Matrix2x2 matrixKTranslation(el11, el12, el21, el22); Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
mInverseMassMatrixTranslationConstraint.setToZero(); mInverseMassMatrixTranslationConstraint.setToZero();
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse();
} }
@ -570,7 +570,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Compute the Lagrange multiplier lambda for the 2 translation constraints // Compute the Lagrange multiplier lambda for the 2 translation constraints
Vector2 lambdaTranslation = mInverseMassMatrixTranslationConstraint * (-translationError); Vector2 lambdaTranslation = mInverseMassMatrixTranslationConstraint * (-translationError);
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 translation constraints // Compute the impulse P=J^T * lambda for the 2 translation constraints
const Vector3 linearImpulseBody1 = -mN1 * lambdaTranslation.x - mN2 * lambdaTranslation.y; const Vector3 linearImpulseBody1 = -mN1 * lambdaTranslation.x - mN2 * lambdaTranslation.y;
@ -586,7 +586,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize(); q1.normalize();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 translation constraints // Compute the impulse P=J^T * lambda for the 2 translation constraints
const Vector3 linearImpulseBody2 = mN1 * lambdaTranslation.x + mN2 * lambdaTranslation.y; const Vector3 linearImpulseBody2 = mN1 * lambdaTranslation.x + mN2 * lambdaTranslation.y;
@ -608,13 +608,13 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix) // contraints (3x3 matrix)
mInverseMassMatrixRotationConstraint.setToZero(); mInverseMassMatrixRotationConstraint.setToZero();
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
mInverseMassMatrixRotationConstraint += mI1; mInverseMassMatrixRotationConstraint += mI1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
mInverseMassMatrixRotationConstraint += mI2; mInverseMassMatrixRotationConstraint += mI2;
} }
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse(); mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse();
} }
@ -627,7 +627,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Compute the Lagrange multiplier lambda for the 3 rotation constraints // Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 lambdaRotation = mInverseMassMatrixRotationConstraint * (-errorRotation); Vector3 lambdaRotation = mInverseMassMatrixRotationConstraint * (-errorRotation);
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints // Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody1 = -lambdaRotation; const Vector3 angularImpulseBody1 = -lambdaRotation;
@ -639,7 +639,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize(); q1.normalize();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints // Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody2 = lambdaRotation; const Vector3 angularImpulseBody2 = lambdaRotation;
@ -660,11 +660,11 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
mInverseMassMatrixLimit = 0.0; mInverseMassMatrixLimit = 0.0;
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
mInverseMassMatrixLimit += mBody1->getMassInverse() + mInverseMassMatrixLimit += mBody1->getMassInverse() +
mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis); mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis);
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
mInverseMassMatrixLimit += mBody2->getMassInverse() + mInverseMassMatrixLimit += mBody2->getMassInverse() +
mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis); mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis);
} }
@ -678,7 +678,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Compute the Lagrange multiplier lambda for the lower limit constraint // Compute the Lagrange multiplier lambda for the lower limit constraint
decimal lambdaLowerLimit = mInverseMassMatrixLimit * (-lowerLimitError); decimal lambdaLowerLimit = mInverseMassMatrixLimit * (-lowerLimitError);
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the lower limit constraint // Compute the impulse P=J^T * lambda for the lower limit constraint
const Vector3 linearImpulseBody1 = -lambdaLowerLimit * mSliderAxisWorld; const Vector3 linearImpulseBody1 = -lambdaLowerLimit * mSliderAxisWorld;
@ -693,7 +693,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize(); q1.normalize();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the lower limit constraint // Compute the impulse P=J^T * lambda for the lower limit constraint
const Vector3 linearImpulseBody2 = lambdaLowerLimit * mSliderAxisWorld; const Vector3 linearImpulseBody2 = lambdaLowerLimit * mSliderAxisWorld;
@ -716,7 +716,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Compute the Lagrange multiplier lambda for the upper limit constraint // Compute the Lagrange multiplier lambda for the upper limit constraint
decimal lambdaUpperLimit = mInverseMassMatrixLimit * (-upperLimitError); decimal lambdaUpperLimit = mInverseMassMatrixLimit * (-upperLimitError);
if (mBody1->getIsMotionEnabled()) { if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the upper limit constraint // Compute the impulse P=J^T * lambda for the upper limit constraint
const Vector3 linearImpulseBody1 = lambdaUpperLimit * mSliderAxisWorld; const Vector3 linearImpulseBody1 = lambdaUpperLimit * mSliderAxisWorld;
@ -731,7 +731,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize(); q1.normalize();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the upper limit constraint // Compute the impulse P=J^T * lambda for the upper limit constraint
const Vector3 linearImpulseBody2 = -lambdaUpperLimit * mSliderAxisWorld; const Vector3 linearImpulseBody2 = -lambdaUpperLimit * mSliderAxisWorld;

34
src/constraint/SliderJoint.h
View File

@ -107,7 +107,9 @@ struct SliderJointInfo : public JointInfo {
// Class SliderJoint // Class SliderJoint
/** /**
* This class represents a slider joint. * This class represents a slider joint. This joint has a one degree of freedom.
* It only allows relative translation of the bodies along a single direction and no
* rotation.
*/ */
class SliderJoint : public Joint { class SliderJoint : public Joint {
@ -245,6 +247,21 @@ class SliderJoint : public Joint {
/// Reset the limits /// Reset the limits
void resetLimits(); void resetLimits();
/// Return the number of bytes used by the joint
virtual size_t getSizeInBytes() const;
/// Initialize before solving the constraint
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData);
/// Warm start the constraint (apply the previous impulse at the beginning of the step)
virtual void warmstart(const ConstraintSolverData& constraintSolverData);
/// Solve the velocity constraint
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
/// Solve the position constraint (for position error correction)
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -296,21 +313,6 @@ class SliderJoint : public Joint {
/// Return the intensity of the current force applied for the joint motor /// Return the intensity of the current force applied for the joint motor
decimal getMotorForce(decimal timeStep) const; decimal getMotorForce(decimal timeStep) const;
/// Return the number of bytes used by the joint
virtual size_t getSizeInBytes() const;
/// Initialize before solving the constraint
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData);
/// Warm start the constraint (apply the previous impulse at the beginning of the step)
virtual void warmstart(const ConstraintSolverData& constraintSolverData);
/// Solve the velocity constraint
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
/// Solve the position constraint (for position error correction)
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
}; };
// Return true if the limits or the joint are enabled // Return true if the limits or the joint are enabled

4
src/engine/ContactSolver.cpp
View File

@ -96,8 +96,8 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
internalManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second; internalManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second;
internalManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); internalManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
internalManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); internalManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
internalManifold.isBody1Moving = body1->getIsMotionEnabled(); internalManifold.isBody1Moving = body1->isMotionEnabled();
internalManifold.isBody2Moving = body2->getIsMotionEnabled(); internalManifold.isBody2Moving = body2->isMotionEnabled();
internalManifold.massInverseBody1 = body1->getMassInverse(); internalManifold.massInverseBody1 = body1->getMassInverse();
internalManifold.massInverseBody2 = body2->getMassInverse(); internalManifold.massInverseBody2 = body2->getMassInverse();
internalManifold.nbContacts = externalManifold->getNbContactPoints(); internalManifold.nbContacts = externalManifold->getNbContactPoints();

34
src/engine/DynamicsWorld.cpp
View File

@ -174,7 +174,7 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
for (uint b=0; b < mIslands[i]->getNbBodies(); b++) { for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
// If the body is allowed to move // If the body is allowed to move
if (bodies[b]->getIsMotionEnabled()) { if (bodies[b]->isMotionEnabled()) {
// Get the constrained velocity // Get the constrained velocity
uint indexArray = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; uint indexArray = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
@ -220,7 +220,7 @@ void DynamicsWorld::updateRigidBodiesAABB() {
for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
// If the body has moved // If the body has moved
if ((*it)->getHasMoved()) { if ((*it)->mHasMoved) {
// Update the AABB of the rigid body // Update the AABB of the rigid body
(*it)->updateAABB(); (*it)->updateAABB();
@ -314,7 +314,7 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
assert(mSplitAngularVelocities[indexBody] == Vector3(0, 0, 0)); assert(mSplitAngularVelocities[indexBody] == Vector3(0, 0, 0));
// If the body is allowed to move // If the body is allowed to move
if (bodies[b]->getIsMotionEnabled()) { if (bodies[b]->isMotionEnabled()) {
// Integrate the external force to get the new velocity of the body // Integrate the external force to get the new velocity of the body
mConstrainedLinearVelocities[indexBody] = bodies[b]->getLinearVelocity() + mConstrainedLinearVelocities[indexBody] = bodies[b]->getLinearVelocity() +
@ -528,12 +528,9 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
removeCollisionShape(rigidBody->getCollisionShape()); removeCollisionShape(rigidBody->getCollisionShape());
// Destroy all the joints in which the rigid body to be destroyed is involved // Destroy all the joints in which the rigid body to be destroyed is involved
// TODO : Iterate on the mJointList of the rigid body instead over all the joints of the world JointListElement* element;
bodyindex idToRemove = rigidBody->getID(); for (element = rigidBody->mJointsList; element != NULL; element = element->next) {
for (std::set<Joint*>::iterator it = mJoints.begin(); it != mJoints.end(); ++it) { destroyJoint(element->joint);
if ((*it)->getBody1()->getID() == idToRemove || (*it)->getBody2()->getID() == idToRemove) {
destroyJoint(*it);
}
} }
// Reset the contact manifold list of the body // Reset the contact manifold list of the body
@ -752,19 +749,17 @@ void DynamicsWorld::computeIslands() {
size_t nbBytesStack = sizeof(RigidBody*) * nbBodies; size_t nbBytesStack = sizeof(RigidBody*) * nbBodies;
RigidBody** stackBodiesToVisit = (RigidBody**)mMemoryAllocator.allocate(nbBytesStack); RigidBody** stackBodiesToVisit = (RigidBody**)mMemoryAllocator.allocate(nbBytesStack);
uint idIsland = 0; // TODO : REMOVE THIS
// For each rigid body of the world // For each rigid body of the world
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
RigidBody* body = *it; RigidBody* body = *it;
// If the body has already been added to an island, we go to the next body // If the body has already been added to an island, we go to the next body
if (body->isAlreadyInIsland()) continue; if (body->mIsAlreadyInIsland) continue;
// If the body is not moving, we go to the next body // If the body is not moving, we go to the next body
// TODO : When we will use STATIC bodies, we will need to take care of this case here // TODO : When we will use STATIC bodies, we will need to take care of this case here
if (!body->getIsMotionEnabled()) continue; if (!body->isMotionEnabled()) continue;
// If the body is sleeping or inactive, we go to the next body // If the body is sleeping or inactive, we go to the next body
if (body->isSleeping() || !body->isActive()) continue; if (body->isSleeping() || !body->isActive()) continue;
@ -777,9 +772,8 @@ void DynamicsWorld::computeIslands() {
// Create the new island // Create the new island
void* allocatedMemoryIsland = mMemoryAllocator.allocate(sizeof(Island)); void* allocatedMemoryIsland = mMemoryAllocator.allocate(sizeof(Island));
mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(idIsland, nbBodies,mContactManifolds.size(), mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies,mContactManifolds.size(),
mJoints.size(), mMemoryAllocator); mJoints.size(), mMemoryAllocator);
idIsland++;
// While there are still some bodies to visit in the stack // While there are still some bodies to visit in the stack
while (stackIndex > 0) { while (stackIndex > 0) {
@ -797,7 +791,7 @@ void DynamicsWorld::computeIslands() {
// If the current body is not moving, we do not want to perform the DFS // If the current body is not moving, we do not want to perform the DFS
// search across that body // search across that body
if (!bodyToVisit->getIsMotionEnabled()) continue; if (!bodyToVisit->isMotionEnabled()) continue;
// For each contact manifold in which the current body is involded // For each contact manifold in which the current body is involded
ContactManifoldListElement* contactElement; ContactManifoldListElement* contactElement;
@ -819,7 +813,7 @@ void DynamicsWorld::computeIslands() {
RigidBody* otherBody = (body1->getID() == bodyToVisit->getID()) ? body2 : body1; RigidBody* otherBody = (body1->getID() == bodyToVisit->getID()) ? body2 : body1;
// Check if the other body has already been added to the island // Check if the other body has already been added to the island
if (otherBody->isAlreadyInIsland()) continue; if (otherBody->mIsAlreadyInIsland) continue;
// Insert the other body into the stack of bodies to visit // Insert the other body into the stack of bodies to visit
stackBodiesToVisit[stackIndex] = otherBody; stackBodiesToVisit[stackIndex] = otherBody;
@ -847,7 +841,7 @@ void DynamicsWorld::computeIslands() {
RigidBody* otherBody = (body1->getID() == bodyToVisit->getID()) ? body2 : body1; RigidBody* otherBody = (body1->getID() == bodyToVisit->getID()) ? body2 : body1;
// Check if the other body has already been added to the island // Check if the other body has already been added to the island
if (otherBody->isAlreadyInIsland()) continue; if (otherBody->mIsAlreadyInIsland) continue;
// Insert the other body into the stack of bodies to visit // Insert the other body into the stack of bodies to visit
stackBodiesToVisit[stackIndex] = otherBody; stackBodiesToVisit[stackIndex] = otherBody;
@ -860,7 +854,7 @@ void DynamicsWorld::computeIslands() {
// can also be included in the other islands // can also be included in the other islands
for (uint i=0; i < mIslands[mNbIslands]->mNbBodies; i++) { for (uint i=0; i < mIslands[mNbIslands]->mNbBodies; i++) {
if (!mIslands[mNbIslands]->mBodies[i]->getIsMotionEnabled()) { if (!mIslands[mNbIslands]->mBodies[i]->isMotionEnabled()) {
mIslands[mNbIslands]->mBodies[i]->mIsAlreadyInIsland = false; mIslands[mNbIslands]->mBodies[i]->mIsAlreadyInIsland = false;
} }
} }
@ -893,7 +887,7 @@ void DynamicsWorld::updateSleepingBodies() {
for (uint b=0; b < mIslands[i]->getNbBodies(); b++) { for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
// Skip static bodies // Skip static bodies
if (!bodies[b]->getIsMotionEnabled()) continue; if (!bodies[b]->isMotionEnabled()) continue;
// If the body is velocity is large enough to stay awake // If the body is velocity is large enough to stay awake
if (bodies[b]->getLinearVelocity().lengthSquare() > sleepLinearVelocitySquare || if (bodies[b]->getLinearVelocity().lengthSquare() > sleepLinearVelocitySquare ||

14
src/engine/DynamicsWorld.h
View File

@ -194,7 +194,7 @@ class DynamicsWorld : public CollisionWorld {
virtual void notifyNewContact(const BroadPhasePair* pair, virtual void notifyNewContact(const BroadPhasePair* pair,
const ContactPointInfo* contactInfo); const ContactPointInfo* contactInfo);
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -246,8 +246,8 @@ public :
/// Add the joint to the list of joints of the two bodies involved in the joint /// Add the joint to the list of joints of the two bodies involved in the joint
void addJointToBody(Joint* joint); void addJointToBody(Joint* joint);
//// Add a contact manifold to the linked list of contact manifolds of the two bodies involed /// Add a contact manifold to the linked list of contact manifolds of the two bodies
//// in the corresponding contact. /// involed in the corresponding contact.
void addContactManifoldToBody(ContactManifold* contactManifold, void addContactManifoldToBody(ContactManifold* contactManifold,
CollisionBody *body1, CollisionBody *body2); CollisionBody *body1, CollisionBody *body2);
@ -310,12 +310,6 @@ public :
/// Set an event listener object to receive events callbacks. /// Set an event listener object to receive events callbacks.
void setEventListener(EventListener* eventListener); void setEventListener(EventListener* eventListener);
// TODO : REMOVE THIS
Island** getIslands() { return mIslands;}
// TODO : REMOVE THIS
uint getNbIslands() const {return mNbIslands;}
}; };
// Reset the external force and torque applied to the bodies // Reset the external force and torque applied to the bodies
@ -384,7 +378,7 @@ inline void DynamicsWorld::resetBodiesMovementVariable() {
it != getRigidBodiesEndIterator(); it++) { it != getRigidBodiesEndIterator(); it++) {
// Set the hasMoved variable to false // Set the hasMoved variable to false
(*it)->setHasMoved(false); (*it)->mHasMoved = false;
} }
} }

4
src/engine/Island.cpp
View File

@ -29,9 +29,9 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
Island::Island(uint id, uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, Island::Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints,
MemoryAllocator& memoryAllocator) MemoryAllocator& memoryAllocator)
: mID(id), mBodies(NULL), mContactManifolds(NULL), mJoints(NULL), mNbBodies(0), : mBodies(NULL), mContactManifolds(NULL), mJoints(NULL), mNbBodies(0),
mNbContactManifolds(0), mNbJoints(0), mMemoryAllocator(memoryAllocator) { mNbContactManifolds(0), mNbJoints(0), mMemoryAllocator(memoryAllocator) {
// Allocate memory for the arrays // Allocate memory for the arrays

8
src/engine/Island.h
View File

@ -45,9 +45,6 @@ class Island {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
// TODO : REMOVE THIS
uint mID;
/// Array with all the bodies of the island /// Array with all the bodies of the island
RigidBody** mBodies; RigidBody** mBodies;
@ -91,7 +88,7 @@ class Island {
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
Island(uint id, uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints,
MemoryAllocator& memoryAllocator); MemoryAllocator& memoryAllocator);
/// Destructor /// Destructor
@ -124,9 +121,6 @@ class Island {
/// Return a pointer to the array of joints /// Return a pointer to the array of joints
Joint** getJoints(); Joint** getJoints();
// TODO : REMOVE THIS
uint getID() const {return mID;}
// -------------------- Friendship -------------------- // // -------------------- Friendship -------------------- //
friend class DynamicsWorld; friend class DynamicsWorld;

2
src/reactphysics3d.h
View File

@ -26,7 +26,7 @@
/******************************************************************************** /********************************************************************************
* ReactPhysics3D * * ReactPhysics3D *
* Version 0.3.0 * * Version 0.4.0 *
* http://code.google.com/p/reactphysics3d/ * * http://code.google.com/p/reactphysics3d/ *
* Daniel Chappuis * * Daniel Chappuis *
********************************************************************************/ ********************************************************************************/