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

Add support for the different body types (static, kinematic, dynamic)

Browse Source
This commit is contained in:
Daniel Chappuis 2013-11-21 23:24:11 +01:00
parent 84a7812325
commit 1812402617
28 changed files with 817 additions and 1127 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
examples/collisionshapes/Scene.cpp
View File

@ -73,9 +73,6 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0),
// Create a sphere and a corresponding rigid in the dynamics world
Box* box = new Box(BOX_SIZE, position , BOX_MASS, mDynamicsWorld);
// The sphere is a moving rigid body
box->getRigidBody()->enableMotion(true);
// Change the material properties of the rigid body
rp3d::Material& material = box->getRigidBody()->getMaterial();
material.setBounciness(rp3d::decimal(0.2));
@ -96,9 +93,6 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0),
// Create a sphere and a corresponding rigid in the dynamics world
Sphere* sphere = new Sphere(SPHERE_RADIUS, position , BOX_MASS, mDynamicsWorld);
// The sphere is a moving rigid body
sphere->getRigidBody()->enableMotion(true);
// Change the material properties of the rigid body
rp3d::Material& material = sphere->getRigidBody()->getMaterial();
material.setBounciness(rp3d::decimal(0.2));
@ -119,9 +113,6 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0),
// Create a cone and a corresponding rigid in the dynamics world
Cone* cone = new Cone(CONE_RADIUS, CONE_HEIGHT, position , CONE_MASS, mDynamicsWorld);
// The cone is a moving rigid body
cone->getRigidBody()->enableMotion(true);
// Change the material properties of the rigid body
rp3d::Material& material = cone->getRigidBody()->getMaterial();
material.setBounciness(rp3d::decimal(0.2));
@ -143,9 +134,6 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0),
Cylinder* cylinder = new Cylinder(CYLINDER_RADIUS, CYLINDER_HEIGHT, position ,
CYLINDER_MASS, mDynamicsWorld);
// The cylinder is a moving rigid body
cylinder->getRigidBody()->enableMotion(true);
// Change the material properties of the rigid body
rp3d::Material& material = cylinder->getRigidBody()->getMaterial();
material.setBounciness(rp3d::decimal(0.2));
@ -167,9 +155,6 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0),
Capsule* capsule = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position ,
CAPSULE_MASS, mDynamicsWorld);
// The cylinder is a moving rigid body
capsule->getRigidBody()->enableMotion(true);
// Change the material properties of the rigid body
rp3d::Material& material = capsule->getRigidBody()->getMaterial();
material.setBounciness(rp3d::decimal(0.2));
@ -190,9 +175,6 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0),
// Create a convex mesh and a corresponding rigid in the dynamics world
ConvexMesh* mesh = new ConvexMesh(position, MESH_MASS, mDynamicsWorld);
// The mesh is a moving rigid body
mesh->getRigidBody()->enableMotion(true);
// Change the material properties of the rigid body
rp3d::Material& material = mesh->getRigidBody()->getMaterial();
material.setBounciness(rp3d::decimal(0.2));
@ -205,8 +187,8 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0),
openglframework::Vector3 floorPosition(0, 0, 0);
mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld);
// The floor must be a non-moving rigid body
mFloor->getRigidBody()->enableMotion(false);
// The floor must be a static rigid body
mFloor->getRigidBody()->setType(rp3d::STATIC);
// Change the material properties of the rigid body
rp3d::Material& material = mFloor->getRigidBody()->getMaterial();

6
examples/common/Box.cpp
View File

@ -80,17 +80,13 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &p
// it is OK if this object is destroyed right after calling Dynamics::createRigidBody()
const rp3d::BoxShape collisionShape(rp3d::Vector3(mSize[0], mSize[1], mSize[2]));
// Compute the inertia tensor of the body using its collision shape
rp3d::Matrix3x3 inertiaTensor;
collisionShape.computeLocalInertiaTensor(inertiaTensor, mass);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
rp3d::Transform transform(initPosition, initOrientation);
// Create a rigid body corresponding to the cube in the dynamics world
mRigidBody = dynamicsWorld->createRigidBody(transform, mass, inertiaTensor, collisionShape);
mRigidBody = dynamicsWorld->createRigidBody(transform, mass, collisionShape);
// If the Vertex Buffer object has not been created yet
if (!areVBOsCreated) {

6
examples/common/Capsule.cpp
View File

@ -52,17 +52,13 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3 &pos
// it is OK if this object is destroyed right after calling Dynamics::createRigidBody()
const rp3d::CapsuleShape collisionShape(mRadius, mHeight);
// Compute the inertia tensor of the body using its collision shape
rp3d::Matrix3x3 inertiaTensor;
collisionShape.computeLocalInertiaTensor(inertiaTensor, mass);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
rp3d::Transform transform(initPosition, initOrientation);
// Create a rigid body corresponding to the sphere in the dynamics world
mRigidBody = dynamicsWorld->createRigidBody(transform, mass, inertiaTensor, collisionShape);
mRigidBody = dynamicsWorld->createRigidBody(transform, mass, collisionShape);
}
// Destructor

6
examples/common/Cone.cpp
View File

@ -52,17 +52,13 @@ Cone::Cone(float radius, float height, const openglframework::Vector3 &position,
// it is OK if this object is destroyed right after calling Dynamics::createRigidBody()
const rp3d::ConeShape collisionShape(mRadius, mHeight);
// Compute the inertia tensor of the body using its collision shape
rp3d::Matrix3x3 inertiaTensor;
collisionShape.computeLocalInertiaTensor(inertiaTensor, mass);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
rp3d::Transform transform(initPosition, initOrientation);
// Create a rigid body corresponding to the cone in the dynamics world
mRigidBody = dynamicsWorld->createRigidBody(transform, mass, inertiaTensor, collisionShape);
mRigidBody = dynamicsWorld->createRigidBody(transform, mass, collisionShape);
}
// Destructor

6
examples/common/ConvexMesh.cpp
View File

@ -65,17 +65,13 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, float mass,
}
collisionShape.setIsEdgesInformationUsed(true);// Enable the fast collision detection with edges
// Compute the inertia tensor of the body using its collision shape
rp3d::Matrix3x3 inertiaTensor;
collisionShape.computeLocalInertiaTensor(inertiaTensor, mass);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
rp3d::Transform transform(initPosition, initOrientation);
// Create a rigid body corresponding to the sphere in the dynamics world
mRigidBody = dynamicsWorld->createRigidBody(transform, mass, inertiaTensor, collisionShape);
mRigidBody = dynamicsWorld->createRigidBody(transform, mass, collisionShape);
}
// Destructor

6
examples/common/Cylinder.cpp
View File

@ -52,17 +52,13 @@ Cylinder::Cylinder(float radius, float height, const openglframework::Vector3 &p
// it is OK if this object is destroyed right after calling Dynamics::createRigidBody()
const rp3d::CylinderShape collisionShape(mRadius, mHeight);
// Compute the inertia tensor of the body using its collision shape
rp3d::Matrix3x3 inertiaTensor;
collisionShape.computeLocalInertiaTensor(inertiaTensor, mass);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
rp3d::Transform transform(initPosition, initOrientation);
// Create a rigid body corresponding to the cylinder in the dynamics world
mRigidBody = dynamicsWorld->createRigidBody(transform, mass, inertiaTensor, collisionShape);
mRigidBody = dynamicsWorld->createRigidBody(transform, mass, collisionShape);
}
// Destructor

6
examples/common/Sphere.cpp
View File

@ -52,17 +52,13 @@ Sphere::Sphere(float radius, const openglframework::Vector3 &position,
// it is OK if this object is destroyed right after calling Dynamics::createRigidBody()
const rp3d::SphereShape collisionShape(mRadius);
// Compute the inertia tensor of the body using its collision shape
rp3d::Matrix3x3 inertiaTensor;
collisionShape.computeLocalInertiaTensor(inertiaTensor, mass);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
rp3d::Transform transform(initPosition, initOrientation);
// Create a rigid body corresponding to the sphere in the dynamics world
mRigidBody = dynamicsWorld->createRigidBody(transform, mass, inertiaTensor, collisionShape);
mRigidBody = dynamicsWorld->createRigidBody(transform, mass, collisionShape);
}
// Destructor

8
examples/cubes/Scene.cpp
View File

@ -64,14 +64,12 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0),
// Position of the cubes
float angle = i * 30.0f;
openglframework::Vector3 position(radius * cos(angle),
1 + i * (BOX_SIZE.y + 0.3f),
10 + i * (BOX_SIZE.y + 0.3f),
0);
// Create a cube and a corresponding rigid in the dynamics world
Box* cube = new Box(BOX_SIZE, position , BOX_MASS, mDynamicsWorld);
cube->getRigidBody()->enableMotion(true);
// Change the material properties of the rigid body
rp3d::Material& material = cube->getRigidBody()->getMaterial();
material.setBounciness(rp3d::decimal(0.4));
@ -84,8 +82,8 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0),
openglframework::Vector3 floorPosition(0, 0, 0);
mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld);
// The floor must be a non-moving rigid body
mFloor->getRigidBody()->enableMotion(false);
// The floor must be a static rigid body
mFloor->getRigidBody()->setType(rp3d::STATIC);
// Change the material properties of the floor rigid body
rp3d::Material& material = mFloor->getRigidBody()->getMaterial();

8
examples/cubes/Scene.h
View File

@ -32,11 +32,11 @@
#include "Box.h"
// Constants
const int NB_SPHERES = 20; // Number of boxes in the scene
const openglframework::Vector3 BOX_SIZE(2, 2, 2); // Box dimensions in meters
const openglframework::Vector3 FLOOR_SIZE(50, 0.5f, 50); // Floor dimensions in meters
const int NB_SPHERES = 20; // Number of boxes in the scene
const openglframework::Vector3 BOX_SIZE(2, 2, 2); // Box dimensions in meters
const openglframework::Vector3 FLOOR_SIZE(50, 0.5f, 50); // Floor dimensions in meters
const float BOX_MASS = 1.0f; // Box mass in kilograms
const float FLOOR_MASS = 100.0f; // Floor mass in kilograms
const float FLOOR_MASS = 100.0f; // Floor mass in kilograms
// Class Scene
class Scene {

26
examples/joints/Scene.cpp
View File

@ -204,9 +204,10 @@ void Scene::createBallAndSocketJoints() {
mBallAndSocketJointChainBoxes[i] = new Box(boxDimension, positionBox , boxMass,
mDynamicsWorld);
// The fist box cannot move
if (i == 0) mBallAndSocketJointChainBoxes[i]->getRigidBody()->enableMotion(false);
else mBallAndSocketJointChainBoxes[i]->getRigidBody()->enableMotion(true);
// The fist box cannot move (static body)
if (i == 0) {
mBallAndSocketJointChainBoxes[i]->getRigidBody()->setType(rp3d::STATIC);
}
// Add some angular velocity damping
mBallAndSocketJointChainBoxes[i]->getRigidBody()->setAngularDamping(rp3d::decimal(0.2));
@ -249,7 +250,7 @@ void Scene::createSliderJoint() {
mSliderJointBottomBox = new Box(box1Dimension, positionBox1 , BOX_MASS, mDynamicsWorld);
// The fist box cannot move
mSliderJointBottomBox->getRigidBody()->enableMotion(false);
mSliderJointBottomBox->getRigidBody()->setType(rp3d::STATIC);
// Change the material properties of the rigid body
rp3d::Material& material1 = mSliderJointBottomBox->getRigidBody()->getMaterial();
@ -264,9 +265,6 @@ void Scene::createSliderJoint() {
openglframework::Vector3 box2Dimension(1.5f, 4, 1.5f);
mSliderJointTopBox = new Box(box2Dimension, positionBox2 , BOX_MASS, mDynamicsWorld);
// The second box is allowed to move
mSliderJointTopBox->getRigidBody()->enableMotion(true);
// Change the material properties of the rigid body
rp3d::Material& material2 = mSliderJointTopBox->getRigidBody()->getMaterial();
material2.setBounciness(0.4f);
@ -303,9 +301,6 @@ void Scene::createPropellerHingeJoint() {
openglframework::Vector3 boxDimension(10, 1, 1);
mPropellerBox = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld);
// The fist box cannot move
mPropellerBox->getRigidBody()->enableMotion(true);
// Change the material properties of the rigid body
rp3d::Material& material = mPropellerBox->getRigidBody()->getMaterial();
material.setBounciness(rp3d::decimal(0.4));
@ -341,9 +336,6 @@ void Scene::createFixedJoints() {
openglframework::Vector3 boxDimension(1.5, 1.5, 1.5);
mFixedJointBox1 = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld);
// The fist box cannot move
mFixedJointBox1->getRigidBody()->enableMotion(true);
// Change the material properties of the rigid body
rp3d::Material& material1 = mFixedJointBox1->getRigidBody()->getMaterial();
material1.setBounciness(rp3d::decimal(0.4));
@ -356,9 +348,6 @@ void Scene::createFixedJoints() {
// Create a box and a corresponding rigid in the dynamics world
mFixedJointBox2 = new Box(boxDimension, positionBox2 , BOX_MASS, mDynamicsWorld);
// The second box is allowed to move
mFixedJointBox2->getRigidBody()->enableMotion(true);
// Change the material properties of the rigid body
rp3d::Material& material2 = mFixedJointBox2->getRigidBody()->getMaterial();
material2.setBounciness(rp3d::decimal(0.4));
@ -387,7 +376,6 @@ void Scene::createFixedJoints() {
mFixedJoint2 = dynamic_cast<rp3d::FixedJoint*>(mDynamicsWorld->createJoint(jointInfo2));
}
// Create the floor
void Scene::createFloor() {
@ -395,8 +383,8 @@ void Scene::createFloor() {
openglframework::Vector3 floorPosition(0, 0, 0);
mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld);
// The floor must be a non-moving rigid body
mFloor->getRigidBody()->enableMotion(false);
// The floor must be a static rigid body
mFloor->getRigidBody()->setType(rp3d::STATIC);
// Change the material properties of the rigid body
rp3d::Material& material = mFloor->getRigidBody()->getMaterial();

3
src/body/CollisionBody.cpp
View File

@ -33,12 +33,11 @@ using namespace reactphysics3d;
// Constructor
CollisionBody::CollisionBody(const Transform& transform, CollisionShape *collisionShape,
bodyindex id)
: Body(id), mCollisionShape(collisionShape), mTransform(transform),
: Body(id), mType(DYNAMIC), mCollisionShape(collisionShape), mTransform(transform),
mHasMoved(false), mContactManifoldsList(NULL) {
assert(collisionShape);
mIsMotionEnabled = true;
mIsCollisionEnabled = true;
mInterpolationFactor = 0.0;

49
src/body/CollisionBody.h
View File

@ -42,6 +42,17 @@ namespace reactphysics3d {
// Class declarations
struct ContactManifoldListElement;
/// Enumeration for the type of a body
/// STATIC : A static body has infinite mass, zero velocity but the position can be
/// changed manually. A static body does not collide with other static or kinematic bodies.
/// KINEMATIC : A kinematic body has infinite mass, the velocity can be changed manually and its
/// position is computed by the physics engine. A kinematic body does not collide with
/// other static or kinematic bodies.
/// DYNAMIC : A dynamic body has non-zero mass, non-zero velocity determined by forces and its
/// position is determined by the physics engine. A dynamic body can collide with other
/// dynamic, static or kinematic bodies.
enum BodyType {STATIC, KINEMATIC, DYNAMIC};
// Class CollisionBody
/**
* This class represents a body that is able to collide with others
@ -53,6 +64,9 @@ class CollisionBody : public Body {
// -------------------- Attributes -------------------- //
/// Type of body (static, kinematic or dynamic)
BodyType mType;
/// Collision shape of the body
CollisionShape* mCollisionShape;
@ -65,9 +79,6 @@ class CollisionBody : public Body {
/// Interpolation factor used for the state interpolation
decimal mInterpolationFactor;
/// True if the body is able to move
bool mIsMotionEnabled;
/// True if the body can collide with others bodies
bool mIsCollisionEnabled;
@ -107,6 +118,12 @@ class CollisionBody : public Body {
/// Destructor
virtual ~CollisionBody();
/// Return the type of the body
BodyType getType() const;
/// Set the type of the body
void setType(BodyType type);
/// Return the collision shape
CollisionShape* getCollisionShape() const;
@ -128,12 +145,6 @@ class CollisionBody : public Body {
/// Set the interpolation factor of the body
void setInterpolationFactor(decimal factor);
/// Return true if the rigid body is allowed to move
bool isMotionEnabled() const;
/// Enable/disable the motion of the body
void enableMotion(bool isMotionEnabled);
/// Return true if the body can collide with others bodies
bool isCollisionEnabled() const;
@ -149,6 +160,16 @@ class CollisionBody : public Body {
friend class CollisionDetection;
};
// Return the type of the body
inline BodyType CollisionBody::getType() const {
return mType;
}
// Set the type of the body
inline void CollisionBody::setType(BodyType type) {
mType = type;
}
// Return the collision shape
inline CollisionShape* CollisionBody::getCollisionShape() const {
assert(mCollisionShape);
@ -172,16 +193,6 @@ inline void CollisionBody::setInterpolationFactor(decimal factor) {
mInterpolationFactor = factor;
}
// Return true if the rigid body is allowed to move
inline bool CollisionBody::isMotionEnabled() const {
return mIsMotionEnabled;
}
// Enable/disable the motion of the body
inline void CollisionBody::enableMotion(bool isMotionEnabled) {
mIsMotionEnabled = isMotionEnabled;
}
// Return the current position and orientation
inline const Transform& CollisionBody::getTransform() const {
return mTransform;

83
src/body/RigidBody.cpp
View File

@ -32,14 +32,24 @@
using namespace reactphysics3d;
// Constructor
RigidBody::RigidBody(const Transform& transform, decimal mass, const Matrix3x3& inertiaTensorLocal,
CollisionShape *collisionShape, bodyindex id)
: CollisionBody(transform, collisionShape, id), mInertiaTensorLocal(inertiaTensorLocal),
mMass(mass), mInertiaTensorLocalInverse(inertiaTensorLocal.getInverse()),
mMassInverse(decimal(1.0) / mass), mIsGravityEnabled(true),
RigidBody::RigidBody(const Transform& transform, decimal mass, CollisionShape *collisionShape,
bodyindex id)
: CollisionBody(transform, collisionShape, id), mInitMass(mass), mIsGravityEnabled(true),
mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)), mJointsList(NULL) {
assert(collisionShape);
// If the mass is not positive, set it to one
if (mInitMass <= decimal(0.0)) {
mInitMass = decimal(1.0);
}
// Compute the inertia tensor using the collision shape of the body
mCollisionShape->computeLocalInertiaTensor(mInertiaTensorLocal, mInitMass);
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
// Compute the inverse mass
mMassInverse = decimal(1.0) / mass;
}
// Destructor
@ -47,6 +57,69 @@ RigidBody::~RigidBody() {
assert(mJointsList == NULL);
}
// Set the type of the body (static, kinematic or dynamic)
void RigidBody::setType(BodyType type) {
if (mType == type) return;
CollisionBody::setType(type);
// If it is a static body
if (mType == STATIC) {
// Reset the velocity to zero
mLinearVelocity.setToZero();
mAngularVelocity.setToZero();
}
// If it is a static or a kinematic body
if (mType == STATIC || mType == KINEMATIC) {
// Reset the inverse mass and inverse inertia tensor to zero
mMassInverse = decimal(0.0);
mInertiaTensorLocalInverse = Matrix3x3::zero();
}
else { // If it is a dynamic body
mMassInverse = decimal(1.0) / mInitMass;
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
}
// Awake the body
setIsSleeping(false);
}
// Method that set the mass of the body
void RigidBody::setMass(decimal mass) {
mInitMass = mass;
// If the mass is negative, set it to one
if (mInitMass <= decimal(0.0)) {
mInitMass = decimal(1.0);
}
// Recompute the inverse mass
if (mType == DYNAMIC) {
mMassInverse = decimal(1.0) / mInitMass;
}
else {
mMassInverse = decimal(0.0);
}
}
// Set the local inertia tensor of the body (in body coordinates)
void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
mInertiaTensorLocal = inertiaTensorLocal;
// Recompute the inverse local inertia tensor
if (mType == DYNAMIC) {
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
}
else {
mInertiaTensorLocalInverse = Matrix3x3::zero();
}
}
// Remove a joint from the joints list
void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint) {

93
src/body/RigidBody.h
View File

@ -40,6 +40,7 @@ namespace reactphysics3d {
struct JointListElement;
class Joint;
// Class RigidBody
/**
* This class represents a rigid body of the physics
@ -51,12 +52,10 @@ class RigidBody : public CollisionBody {
protected :
// TODO : Remove the mass variable (duplicate with inverseMass)
// -------------------- Attributes -------------------- //
/// Mass of the body
decimal mMass;
/// Intial mass of the body
decimal mInitMass;
/// Linear velocity of the body
Vector3 mLinearVelocity;
@ -105,20 +104,20 @@ class RigidBody : public CollisionBody {
/// Remove a joint from the joints list
void removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint);
/// Set the inverse of the mass
void setMassInverse(decimal massInverse);
public :
// -------------------- Methods -------------------- //
/// Constructor
RigidBody(const Transform& transform, decimal mass, const Matrix3x3& inertiaTensorLocal,
CollisionShape* collisionShape, bodyindex id);
RigidBody(const Transform& transform, decimal mass, CollisionShape* collisionShape,
bodyindex id);
/// Destructor
virtual ~RigidBody();
/// Set the type of the body (static, kinematic or dynamic)
void setType(BodyType type);
/// Return the mass of the body
decimal getMass() const;
@ -128,27 +127,21 @@ class RigidBody : public CollisionBody {
/// Return the linear velocity
Vector3 getLinearVelocity() const;
/// Set the linear velocity of the body
/// Set the linear velocity of the body.
void setLinearVelocity(const Vector3& linearVelocity);
/// Return the angular velocity
Vector3 getAngularVelocity() const;
/// Set the angular velocity
/// Set the angular velocity.
void setAngularVelocity(const Vector3& angularVelocity);
/// Return the inverse of the mass of the body
decimal getMassInverse() const;
/// Return the local inertia tensor of the body (in body coordinates)
const Matrix3x3& getInertiaTensorLocal() const;
/// Set the local inertia tensor of the body (in body coordinates)
void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal);
/// Get the inverse of the inertia tensor
Matrix3x3 getInertiaTensorLocalInverse() const;
/// Return the inertia tensor in world coordinates.
Matrix3x3 getInertiaTensorWorld() const;
@ -197,16 +190,16 @@ class RigidBody : public CollisionBody {
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
friend class ContactSolver;
friend class BallAndSocketJoint;
friend class SliderJoint;
friend class HingeJoint;
friend class FixedJoint;
};
// Method that return the mass of the body
inline decimal RigidBody::getMass() const {
return mMass;
}
// Method that set the mass of the body
inline void RigidBody::setMass(decimal mass) {
mMass = mass;
return mInitMass;
}
// Return the linear velocity
@ -219,23 +212,15 @@ inline Vector3 RigidBody::getAngularVelocity() const {
return mAngularVelocity;
}
// Set the angular velocity.
/// You should only call this method for a kinematic body. Otherwise, it
/// will do nothing.
inline void RigidBody::setAngularVelocity(const Vector3& angularVelocity) {
mAngularVelocity = angularVelocity;
}
// Set the inverse of the mass
inline void RigidBody::setMassInverse(decimal massInverse) {
mMassInverse = massInverse;
}
// Get the inverse of the inertia tensor
inline Matrix3x3 RigidBody::getInertiaTensorLocalInverse() const {
return mInertiaTensorLocalInverse;
}
// Return the inverse of the mass of the body
inline decimal RigidBody::getMassInverse() const {
return mMassInverse;
// If it is a kinematic body
if (mType == KINEMATIC) {
mAngularVelocity = angularVelocity;
}
}
// Return the local inertia tensor of the body (in body coordinates)
@ -243,11 +228,6 @@ inline const Matrix3x3& RigidBody::getInertiaTensorLocal() const {
return mInertiaTensorLocal;
}
// Set the local inertia tensor of the body (in body coordinates)
inline void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
mInertiaTensorLocal = inertiaTensorLocal;
}
// Return the inertia tensor in world coordinates.
/// The inertia tensor I_w in world coordinates is computed
/// with the local inertia tensor I_b in body coordinates
@ -267,6 +247,8 @@ inline Matrix3x3 RigidBody::getInertiaTensorWorld() const {
/// by I_w = R * I_b^-1 * R^T
/// where R is the rotation matrix (and R^T its transpose) of the
/// current orientation quaternion of the body
// TODO : DO NOT RECOMPUTE THE MATRIX MULTIPLICATION EVERY TIME. WE NEED TO STORE THE
// INVERSE WORLD TENSOR IN THE CLASS AND UPLDATE IT WHEN THE ORIENTATION OF THE BODY CHANGES
inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const {
// Compute and return the inertia tensor in world coordinates
@ -274,11 +256,14 @@ inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const {
mTransform.getOrientation().getMatrix().getTranspose();
}
// Set the linear velocity of the rigid body
// Set the linear velocity of the rigid body.
/// You should only call this method for a kinematic body. Otherwise, it
/// will do nothing.
inline void RigidBody::setLinearVelocity(const Vector3& linearVelocity) {
// If the body is able to move
if (mIsMotionEnabled) {
// If it is a kinematic body
if (mType == KINEMATIC) {
// Update the linear velocity of the current body state
mLinearVelocity = linearVelocity;
}
@ -348,9 +333,11 @@ inline void RigidBody::setIsSleeping(bool isSleeping) {
/// If the body is sleeping, calling this method will wake it up. Note that the
/// force will we added to the sum of the applied forces and that this sum will be
/// reset to zero at the end of each call of the DynamicsWorld::update() method.
/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
inline void RigidBody::applyForceToCenter(const Vector3& force) {
// If it is a static body, do not apply any force
if (!mIsMotionEnabled) return;
// If it is not a dynamic body, we do nothing
if (mType != DYNAMIC) return;
// Awake the body if it was sleeping
if (mIsSleeping) {
@ -367,10 +354,11 @@ inline void RigidBody::applyForceToCenter(const Vector3& force) {
/// If the body is sleeping, calling this method will wake it up. Note that the
/// force will we added to the sum of the applied forces and that this sum will be
/// reset to zero at the end of each call of the DynamicsWorld::update() method.
/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
inline void RigidBody::applyForce(const Vector3& force, const Vector3& point) {
// If it is a static body, do not apply any force
if (!mIsMotionEnabled) return;
// If it is not a dynamic body, we do nothing
if (mType != DYNAMIC) return;
// Awake the body if it was sleeping
if (mIsSleeping) {
@ -386,10 +374,11 @@ inline void RigidBody::applyForce(const Vector3& force, const Vector3& point) {
/// If the body is sleeping, calling this method will wake it up. Note that the
/// force will we added to the sum of the applied torques and that this sum will be
/// reset to zero at the end of each call of the DynamicsWorld::update() method.
/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
inline void RigidBody::applyTorque(const Vector3& torque) {
// If it is a static body, do not apply any force
if (!mIsMotionEnabled) return;
// If it is not a dynamic body, we do nothing
if (mType != DYNAMIC) return;
// Awake the body if it was sleeping
if (mIsSleeping) {

1
src/collision/CollisionDetection.cpp
View File

@ -111,6 +111,7 @@ void CollisionDetection::computeNarrowPhase() {
mWorld->updateOverlappingPair(pair);
// Check if the two bodies are allowed to collide, otherwise, we do not test for collision
if (pair->body1->getType() != DYNAMIC && pair->body2->getType() != DYNAMIC) continue;
if (mNoCollisionPairs.count(pair->getBodiesIndexPair()) > 0) continue;
// Check if the two bodies are sleeping, if so, we do no test collision between them

153
src/constraint/BallAndSocketJoint.cpp
View File

@ -72,26 +72,16 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Compute the matrix K=JM^-1J^t (3x3 matrix)
decimal inverseMassBodies = 0.0;
if (mBody1->isMotionEnabled()) {
inverseMassBodies += mBody1->getMassInverse();
}
if (mBody2->isMotionEnabled()) {
inverseMassBodies += mBody2->getMassInverse();
}
decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies);
if (mBody1->isMotionEnabled()) {
massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
}
if (mBody2->isMotionEnabled()) {
massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
}
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
// Compute the inverse mass matrix K^-1
mInverseMassMatrix.setToZero();
if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrix = massMatrix.getInverse();
}
@ -119,30 +109,20 @@ void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverD
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass of the bodies
const decimal inverseMassBody1 = mBody1->getMassInverse();
const decimal inverseMassBody2 = mBody2->getMassInverse();
// Compute the impulse P=J^T * lambda for the body 1
const Vector3 linearImpulseBody1 = -mImpulse;
const Vector3 angularImpulseBody1 = mImpulse.cross(mR1World);
if (mBody1->isMotionEnabled()) {
// Apply the impulse to the body 1
v1 += mBody1->mMassInverse * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody1 = -mImpulse;
const Vector3 angularImpulseBody1 = mImpulse.cross(mR1World);
// Compute the impulse P=J^T * lambda for the body 2
const Vector3 angularImpulseBody2 = -mImpulse.cross(mR2World);
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody2 = mImpulse;
const Vector3 angularImpulseBody2 = -mImpulse.cross(mR2World);
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
}
// Apply the impulse to the body to the body 2
v2 += mBody2->mMassInverse * mImpulse;
w2 += mI2 * angularImpulseBody2;
}
// Solve the velocity constraint
@ -154,10 +134,6 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass of the bodies
decimal inverseMassBody1 = mBody1->getMassInverse();
decimal inverseMassBody2 = mBody2->getMassInverse();
// Compute J*v
const Vector3 Jv = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World);
@ -165,26 +141,20 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con
const Vector3 deltaLambda = mInverseMassMatrix * (-Jv - mBiasVector);
mImpulse += deltaLambda;
if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the body 1
const Vector3 linearImpulseBody1 = -deltaLambda;
const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World);
// Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody1 = -deltaLambda;
const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World);
// Apply the impulse to the body 1
v1 += mBody1->mMassInverse * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the body 2
const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World);
// Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody2 = deltaLambda;
const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World);
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
}
// Apply the impulse to the body 2
v2 += mBody2->mMassInverse * deltaLambda;
w2 += mI2 * angularImpulseBody2;
}
// Solve the position constraint (for position error correction)
@ -201,8 +171,8 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->getMassInverse();
decimal inverseMassBody2 = mBody2->getMassInverse();
decimal inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse;
// Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld();
@ -217,24 +187,14 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation constraints
decimal inverseMassBodies = 0.0;
if (mBody1->isMotionEnabled()) {
inverseMassBodies += inverseMassBody1;
}
if (mBody2->isMotionEnabled()) {
inverseMassBodies += inverseMassBody2;
}
decimal inverseMassBodies = inverseMassBody1 + inverseMassBody2;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies);
if (mBody1->isMotionEnabled()) {
massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
}
if (mBody2->isMotionEnabled()) {
massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
}
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
mInverseMassMatrix.setToZero();
if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrix = massMatrix.getInverse();
}
@ -246,36 +206,29 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
// right-hand side vector but instead use a method to directly solve the linear system.
const Vector3 lambda = mInverseMassMatrix * (-constraintError);
// Apply the impulse to the bodies of the joint (directly update the position/orientation)
if (mBody1->isMotionEnabled()) {
// Compute the impulse of body 1
const Vector3 linearImpulseBody1 = -lambda;
const Vector3 angularImpulseBody1 = lambda.cross(mR1World);
// Compute the impulse
const Vector3 linearImpulseBody1 = -lambda;
const Vector3 angularImpulseBody1 = lambda.cross(mR1World);
// Compute the pseudo velocity of body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1;
// Compute the pseudo velocity
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Update the body position/orientation
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse of body 2
const Vector3 angularImpulseBody2 = -lambda.cross(mR2World);
// Compute the impulse
const Vector3 linearImpulseBody2 = lambda;
const Vector3 angularImpulseBody2 = -lambda.cross(mR2World);
// Compute the pseudo velocity of body 2
const Vector3 v2 = inverseMassBody2 * lambda;
const Vector3 w2 = mI2 * angularImpulseBody2;
// Compute the pseudo velocity
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// Update the body position/orientation of body 2
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}

234
src/constraint/FixedJoint.cpp
View File

@ -80,26 +80,16 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
decimal inverseMassBodies = 0.0;
if (mBody1->isMotionEnabled()) {
inverseMassBodies += mBody1->getMassInverse();
}
if (mBody2->isMotionEnabled()) {
inverseMassBodies += mBody2->getMassInverse();
}
decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies);
if (mBody1->isMotionEnabled()) {
massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
}
if (mBody2->isMotionEnabled()) {
massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
}
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
// Compute the inverse mass matrix K^-1 for the 3 translation constraints
mInverseMassMatrixTranslation.setToZero();
if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixTranslation = massMatrix.getInverse();
}
@ -112,14 +102,8 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix)
mInverseMassMatrixRotation.setToZero();
if (mBody1->isMotionEnabled()) {
mInverseMassMatrixRotation += mI1;
}
if (mBody2->isMotionEnabled()) {
mInverseMassMatrixRotation += mI2;
}
if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
mInverseMassMatrixRotation = mI1 + mI2;
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse();
}
@ -151,35 +135,29 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass of the bodies
const decimal inverseMassBody1 = mBody1->getMassInverse();
const decimal inverseMassBody2 = mBody2->getMassInverse();
const decimal inverseMassBody1 = mBody1->mMassInverse;
const decimal inverseMassBody2 = mBody2->mMassInverse;
if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1
Vector3 linearImpulseBody1 = -mImpulseTranslation;
Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World);
// Compute the impulse P=J^T * lambda for the 3 translation constraints
Vector3 linearImpulseBody1 = -mImpulseTranslation;
Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1
angularImpulseBody1 += -mImpulseRotation;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
angularImpulseBody1 += -mImpulseRotation;
// Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 translation constraints for body 2
Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World);
// Compute the impulse P=J^T * lambda for the 3 translation constraints
Vector3 linearImpulseBody2 = mImpulseTranslation;
Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 2
angularImpulseBody2 += mImpulseRotation;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
angularImpulseBody2 += mImpulseRotation;
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
}
// Apply the impulse to the body 2
v2 += inverseMassBody2 * mImpulseTranslation;
w2 += mI2 * angularImpulseBody2;
}
// Solve the velocity constraint
@ -192,8 +170,8 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass of the bodies
decimal inverseMassBody1 = mBody1->getMassInverse();
decimal inverseMassBody2 = mBody2->getMassInverse();
decimal inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse;
// --------------- Translation Constraints --------------- //
@ -205,26 +183,20 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
(-JvTranslation - mBiasTranslation);
mImpulseTranslation += deltaLambda;
if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for body 1
const Vector3 linearImpulseBody1 = -deltaLambda;
Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World);
// Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody1 = -deltaLambda;
const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World);
// Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for body 2
const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World);
// Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody2 = deltaLambda;
const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World);
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
}
// Apply the impulse to the body 2
v2 += inverseMassBody2 * deltaLambda;
w2 += mI2 * angularImpulseBody2;
// --------------- Rotation Constraints --------------- //
@ -235,22 +207,14 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
Vector3 deltaLambda2 = mInverseMassMatrixRotation * (-JvRotation - mBiasRotation);
mImpulseRotation += deltaLambda2;
if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1
angularImpulseBody1 = -deltaLambda2;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody1 = -deltaLambda2;
// Apply the impulse to the body 1
w1 += mI1 * angularImpulseBody1;
// Apply the impulse to the body
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody2 = deltaLambda2;
// Apply the impulse to the body
w2 += mI2 * angularImpulseBody2;
}
// Apply the impulse to the body 2
w2 += mI2 * deltaLambda2;
}
// Solve the position constraint (for position error correction)
@ -267,8 +231,8 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->getMassInverse();
decimal inverseMassBody2 = mBody2->getMassInverse();
decimal inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse;
// Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld();
@ -285,24 +249,14 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// --------------- Translation Constraints --------------- //
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
decimal inverseMassBodies = 0.0;
if (mBody1->isMotionEnabled()) {
inverseMassBodies += mBody1->getMassInverse();
}
if (mBody2->isMotionEnabled()) {
inverseMassBodies += mBody2->getMassInverse();
}
decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies);
if (mBody1->isMotionEnabled()) {
massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
}
if (mBody2->isMotionEnabled()) {
massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
}
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
mInverseMassMatrixTranslation.setToZero();
if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixTranslation = massMatrix.getInverse();
}
@ -312,50 +266,37 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// Compute the Lagrange multiplier lambda
const Vector3 lambdaTranslation = mInverseMassMatrixTranslation * (-errorTranslation);
// Apply the impulse to the bodies of the joint
if (mBody1->isMotionEnabled()) {
// Compute the impulse of body 1
Vector3 linearImpulseBody1 = -lambdaTranslation;
Vector3 angularImpulseBody1 = lambdaTranslation.cross(mR1World);
// Compute the impulse
Vector3 linearImpulseBody1 = -lambdaTranslation;
Vector3 angularImpulseBody1 = lambdaTranslation.cross(mR1World);
// Compute the pseudo velocity of body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
Vector3 w1 = mI1 * angularImpulseBody1;
// Compute the pseudo velocity
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Update the body position/orientation
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse of body 2
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mR2World);
// Compute the impulse
Vector3 linearImpulseBody2 = lambdaTranslation;
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mR2World);
// Compute the pseudo velocity of body 2
const Vector3 v2 = inverseMassBody2 * lambdaTranslation;
Vector3 w2 = mI2 * angularImpulseBody2;
// Compute the pseudo velocity
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// Update the body position/orientation of body 2
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
// --------------- Rotation Constraints --------------- //
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix)
mInverseMassMatrixRotation.setToZero();
if (mBody1->isMotionEnabled()) {
mInverseMassMatrixRotation += mI1;
}
if (mBody2->isMotionEnabled()) {
mInverseMassMatrixRotation += mI2;
}
if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
mInverseMassMatrixRotation = mI1 + mI2;
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse();
}
@ -368,30 +309,21 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation);
// Apply the impulse to the bodies of the joint
if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
angularImpulseBody1 = -lambdaRotation;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody1 = -lambdaRotation;
// Compute the pseudo velocity of body 1
w1 = mI1 * angularImpulseBody1;
// Compute the pseudo velocity
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Update the body position/orientation
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->isMotionEnabled()) {
// Compute the pseudo velocity of body 2
w2 = mI2 * lambdaRotation;
// Compute the impulse
const Vector3 angularImpulseBody2 = lambdaRotation;
// Compute the pseudo velocity
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}

421
src/constraint/HingeJoint.cpp
View File

@ -122,24 +122,14 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix)
decimal inverseMassBodies = 0.0;
if (mBody1->isMotionEnabled()) {
inverseMassBodies += mBody1->getMassInverse();
}
if (mBody2->isMotionEnabled()) {
inverseMassBodies += mBody2->getMassInverse();
}
decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies);
if (mBody1->isMotionEnabled()) {
massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
}
if (mBody2->isMotionEnabled()) {
massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
}
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
mInverseMassMatrixTranslation.setToZero();
if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixTranslation = massMatrix.getInverse();
}
@ -151,18 +141,10 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
}
// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix)
Vector3 I1B2CrossA1(0, 0, 0);
Vector3 I1C2CrossA1(0, 0, 0);
Vector3 I2B2CrossA1(0, 0, 0);
Vector3 I2C2CrossA1(0, 0, 0);
if (mBody1->isMotionEnabled()) {
I1B2CrossA1 = mI1 * mB2CrossA1;
I1C2CrossA1 = mI1 * mC2CrossA1;
}
if (mBody2->isMotionEnabled()) {
I2B2CrossA1 = mI2 * mB2CrossA1;
I2C2CrossA1 = mI2 * mC2CrossA1;
}
Vector3 I1B2CrossA1 = mI1 * mB2CrossA1;
Vector3 I1C2CrossA1 = mI1 * mC2CrossA1;
Vector3 I2B2CrossA1 = mI2 * mB2CrossA1;
Vector3 I2C2CrossA1 = mI2 * mC2CrossA1;
const decimal el11 = mB2CrossA1.dot(I1B2CrossA1) +
mB2CrossA1.dot(I2B2CrossA1);
const decimal el12 = mB2CrossA1.dot(I1C2CrossA1) +
@ -173,7 +155,7 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
mC2CrossA1.dot(I2C2CrossA1);
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
mInverseMassMatrixRotation.setToZero();
if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixRotation = matrixKRotation.getInverse();
}
@ -198,13 +180,7 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
if (mIsMotorEnabled || (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated))) {
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits and motor (1x1 matrix)
mInverseMassMatrixLimitMotor = 0.0;
if (mBody1->isMotionEnabled()) {
mInverseMassMatrixLimitMotor += mA1.dot(mI1 * mA1);
}
if (mBody2->isMotionEnabled()) {
mInverseMassMatrixLimitMotor += mA1.dot(mI2 * mA1);
}
mInverseMassMatrixLimitMotor = mA1.dot(mI1 * mA1) + mA1.dot(mI2 * mA1);
mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ?
decimal(1.0) / mInverseMassMatrixLimitMotor : decimal(0.0);
@ -235,8 +211,8 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
const decimal inverseMassBody1 = mBody1->getMassInverse();
const decimal inverseMassBody2 = mBody2->getMassInverse();
const decimal inverseMassBody1 = mBody1->mMassInverse;
const decimal inverseMassBody2 = mBody2->mMassInverse;
// Compute the impulse P=J^T * lambda for the 2 rotation constraints
Vector3 rotationImpulse = -mB2CrossA1 * mImpulseRotation.x - mC2CrossA1 * mImpulseRotation.y;
@ -247,44 +223,38 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Compute the impulse P=J^T * lambda for the motor constraint
const Vector3 motorImpulse = -mImpulseMotor * mA1;
if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 translation constraints of body 1
Vector3 linearImpulseBody1 = -mImpulseTranslation;
Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World);
// Compute the impulse P=J^T * lambda for the 3 translation constraints
Vector3 linearImpulseBody1 = -mImpulseTranslation;
Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World);
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1
angularImpulseBody1 += rotationImpulse;
// Compute the impulse P=J^T * lambda for the 2 rotation constraints
angularImpulseBody1 += rotationImpulse;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1
angularImpulseBody1 += limitsImpulse;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints
angularImpulseBody1 += limitsImpulse;
// Compute the impulse P=J^T * lambda for the motor constraint of body 1
angularImpulseBody1 += motorImpulse;
// Compute the impulse P=J^T * lambda for the motor constraint
angularImpulseBody1 += motorImpulse;
// Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 translation constraints of body 2
Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World);
// Compute the impulse P=J^T * lambda for the 3 translation constraints
Vector3 linearImpulseBody2 = mImpulseTranslation;
Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World);
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2
angularImpulseBody2 += -rotationImpulse;
// Compute the impulse P=J^T * lambda for the 2 rotation constraints
angularImpulseBody2 += -rotationImpulse;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 2
angularImpulseBody2 += -limitsImpulse;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints
angularImpulseBody2 += -limitsImpulse;
// Compute the impulse P=J^T * lambda for the motor constraint of body 2
angularImpulseBody2 += -motorImpulse;
// Compute the impulse P=J^T * lambda for the motor constraint
angularImpulseBody2 += -motorImpulse;
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
}
// Apply the impulse to the body 2
v2 += inverseMassBody2 * mImpulseTranslation;
w2 += mI2 * angularImpulseBody2;
}
// Solve the velocity constraint
@ -297,8 +267,8 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->getMassInverse();
decimal inverseMassBody2 = mBody2->getMassInverse();
decimal inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse;
// --------------- Translation Constraints --------------- //
@ -310,26 +280,20 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
(-JvTranslation - mBTranslation);
mImpulseTranslation += deltaLambdaTranslation;
if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda of body 1
const Vector3 linearImpulseBody1 = -deltaLambdaTranslation;
Vector3 angularImpulseBody1 = deltaLambdaTranslation.cross(mR1World);
// Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody1 = -deltaLambdaTranslation;
const Vector3 angularImpulseBody1 = deltaLambdaTranslation.cross(mR1World);
// Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda of body 2
Vector3 angularImpulseBody2 = -deltaLambdaTranslation.cross(mR2World);
// Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody2 = deltaLambdaTranslation;
const Vector3 angularImpulseBody2 = -deltaLambdaTranslation.cross(mR2World);
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
}
// Apply the impulse to the body 2
v2 += inverseMassBody2 * deltaLambdaTranslation;
w2 += mI2 * angularImpulseBody2;
// --------------- Rotation Constraints --------------- //
@ -341,24 +305,19 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
Vector2 deltaLambdaRotation = mInverseMassMatrixRotation * (-JvRotation - mBRotation);
mImpulseRotation += deltaLambdaRotation;
if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1
angularImpulseBody1 = -mB2CrossA1 * deltaLambdaRotation.x -
mC2CrossA1 * deltaLambdaRotation.y;
// Compute the impulse P=J^T * lambda for the 2 rotation constraints
const Vector3 angularImpulseBody1 = -mB2CrossA1 * deltaLambdaRotation.x -
mC2CrossA1 * deltaLambdaRotation.y;
// Apply the impulse to the body 1
w1 += mI1 * angularImpulseBody1;
// Apply the impulse to the body
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2
angularImpulseBody2 = mB2CrossA1 * deltaLambdaRotation.x +
mC2CrossA1 * deltaLambdaRotation.y;
// Compute the impulse P=J^T * lambda for the 2 rotation constraints
const Vector3 angularImpulseBody2 = mB2CrossA1 * deltaLambdaRotation.x +
mC2CrossA1 * deltaLambdaRotation.y;
// Apply the impulse to the body
w2 += mI2 * angularImpulseBody2;
}
// Apply the impulse to the body 2
w2 += mI2 * angularImpulseBody2;
// --------------- Limits Constraints --------------- //
@ -376,22 +335,17 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0));
deltaLambdaLower = mImpulseLowerLimit - lambdaTemp;
if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 1
const Vector3 angularImpulseBody1 = -deltaLambdaLower * mA1;
// Compute the impulse P=J^T * lambda for the lower limit constraint
const Vector3 angularImpulseBody1 = -deltaLambdaLower * mA1;
// Apply the impulse to the body 1
w1 += mI1 * angularImpulseBody1;
// Apply the impulse to the body
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 2
const Vector3 angularImpulseBody2 = deltaLambdaLower * mA1;
// Compute the impulse P=J^T * lambda for the lower limit constraint
const Vector3 angularImpulseBody2 = deltaLambdaLower * mA1;
// Apply the impulse to the body
w2 += mI2 * angularImpulseBody2;
}
// Apply the impulse to the body 2
w2 += mI2 * angularImpulseBody2;
}
// If the upper limit is violated
@ -406,22 +360,17 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0));
deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp;
if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 1
const Vector3 angularImpulseBody1 = deltaLambdaUpper * mA1;
// Compute the impulse P=J^T * lambda for the upper limit constraint
const Vector3 angularImpulseBody1 = deltaLambdaUpper * mA1;
// Apply the impulse to the body 1
w1 += mI1 * angularImpulseBody1;
// Apply the impulse to the body
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 2
const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mA1;
// Compute the impulse P=J^T * lambda for the upper limit constraint
const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mA1;
// Apply the impulse to the body
w2 += mI2 * angularImpulseBody2;
}
// Apply the impulse to the body 2
w2 += mI2 * angularImpulseBody2;
}
}
@ -440,22 +389,17 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
deltaLambdaMotor = mImpulseMotor - lambdaTemp;
if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the motor of body 1
const Vector3 angularImpulseBody1 = -deltaLambdaMotor * mA1;
// Compute the impulse P=J^T * lambda for the motor
const Vector3 angularImpulseBody1 = -deltaLambdaMotor * mA1;
// Apply the impulse to the body 1
w1 += mI1 * angularImpulseBody1;
// Apply the impulse to the body
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the motor of body 2
const Vector3 angularImpulseBody2 = deltaLambdaMotor * mA1;
// Compute the impulse P=J^T * lambda for the motor
const Vector3 angularImpulseBody2 = deltaLambdaMotor * mA1;
// Apply the impulse to the body
w2 += mI2 * angularImpulseBody2;
}
// Apply the impulse to the body 2
w2 += mI2 * angularImpulseBody2;
}
}
@ -473,8 +417,8 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->getMassInverse();
decimal inverseMassBody2 = mBody2->getMassInverse();
decimal inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse;
// Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld();
@ -510,24 +454,14 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// --------------- Translation Constraints --------------- //
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
decimal inverseMassBodies = 0.0;
if (mBody1->isMotionEnabled()) {
inverseMassBodies += mBody1->getMassInverse();
}
if (mBody2->isMotionEnabled()) {
inverseMassBodies += mBody2->getMassInverse();
}
decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies);
if (mBody1->isMotionEnabled()) {
massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
}
if (mBody2->isMotionEnabled()) {
massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
}
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
mInverseMassMatrixTranslation.setToZero();
if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixTranslation = massMatrix.getInverse();
}
@ -537,53 +471,38 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// Compute the Lagrange multiplier lambda
const Vector3 lambdaTranslation = mInverseMassMatrixTranslation * (-errorTranslation);
// Apply the impulse to the bodies of the joint
if (mBody1->isMotionEnabled()) {
// Compute the impulse of body 1
Vector3 linearImpulseBody1 = -lambdaTranslation;
Vector3 angularImpulseBody1 = lambdaTranslation.cross(mR1World);
// Compute the impulse
Vector3 linearImpulseBody1 = -lambdaTranslation;
Vector3 angularImpulseBody1 = lambdaTranslation.cross(mR1World);
// Compute the pseudo velocity of body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
Vector3 w1 = mI1 * angularImpulseBody1;
// Compute the pseudo velocity
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Update the body position/orientation
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse of body 2
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mR2World);
// Compute the impulse
Vector3 linearImpulseBody2 = lambdaTranslation;
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mR2World);
// Compute the pseudo velocity of body 2
const Vector3 v2 = inverseMassBody2 * lambdaTranslation;
Vector3 w2 = mI2 * angularImpulseBody2;
// Compute the pseudo velocity
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// Update the body position/orientation of body 2
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
// --------------- Rotation Constraints --------------- //
// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix)
Vector3 I1B2CrossA1(0, 0, 0);
Vector3 I1C2CrossA1(0, 0, 0);
Vector3 I2B2CrossA1(0, 0, 0);
Vector3 I2C2CrossA1(0, 0, 0);
if (mBody1->isMotionEnabled()) {
I1B2CrossA1 = mI1 * mB2CrossA1;
I1C2CrossA1 = mI1 * mC2CrossA1;
}
if (mBody2->isMotionEnabled()) {
I2B2CrossA1 = mI2 * mB2CrossA1;
I2C2CrossA1 = mI2 * mC2CrossA1;
}
Vector3 I1B2CrossA1 = mI1 * mB2CrossA1;
Vector3 I1C2CrossA1 = mI1 * mC2CrossA1;
Vector3 I2B2CrossA1 = mI2 * mB2CrossA1;
Vector3 I2C2CrossA1 = mI2 * mC2CrossA1;
const decimal el11 = mB2CrossA1.dot(I1B2CrossA1) +
mB2CrossA1.dot(I2B2CrossA1);
const decimal el12 = mB2CrossA1.dot(I1C2CrossA1) +
@ -594,7 +513,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
mC2CrossA1.dot(I2C2CrossA1);
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
mInverseMassMatrixRotation.setToZero();
if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixRotation = matrixKRotation.getInverse();
}
@ -604,33 +523,25 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector2 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation);
// Apply the impulse to the bodies of the joint
if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
angularImpulseBody1 = -mB2CrossA1 * lambdaRotation.x - mC2CrossA1 * lambdaRotation.y;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody1 = -mB2CrossA1 * lambdaRotation.x -
mC2CrossA1 * lambdaRotation.y;
// Compute the pseudo velocity of body 1
w1 = mI1 * angularImpulseBody1;
// Compute the pseudo velocity
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Update the body position/orientation
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse of body 2
angularImpulseBody2 = mB2CrossA1 * lambdaRotation.x + mC2CrossA1 * lambdaRotation.y;
// Compute the impulse
const Vector3 angularImpulseBody2 = mB2CrossA1 * lambdaRotation.x +
mC2CrossA1 * lambdaRotation.y;
// Compute the pseudo velocity of body 2
w2 = mI2 * angularImpulseBody2;
// Compute the pseudo velocity
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
// --------------- Limits Constraints --------------- //
@ -639,13 +550,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
if (mIsLowerLimitViolated || mIsUpperLimitViolated) {
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
mInverseMassMatrixLimitMotor = 0.0;
if (mBody1->isMotionEnabled()) {
mInverseMassMatrixLimitMotor += mA1.dot(mI1 * mA1);
}
if (mBody2->isMotionEnabled()) {
mInverseMassMatrixLimitMotor += mA1.dot(mI2 * mA1);
}
mInverseMassMatrixLimitMotor = mA1.dot(mI1 * mA1) + mA1.dot(mI2 * mA1);
mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ?
decimal(1.0) / mInverseMassMatrixLimitMotor : decimal(0.0);
}
@ -656,31 +561,25 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// Compute the Lagrange multiplier lambda for the lower limit constraint
decimal lambdaLowerLimit = mInverseMassMatrixLimitMotor * (-lowerLimitError );
// Apply the impulse to the bodies of the joint
if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda of body 1
const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mA1;
// Compute the impulse P=J^T * lambda
const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mA1;
// Compute the pseudo velocity of body 1
const Vector3 w1 = mI1 * angularImpulseBody1;
// Compute the pseudo velocity
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Update the body position/orientation
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda of body 2
const Vector3 angularImpulseBody2 = lambdaLowerLimit * mA1;
// Compute the impulse P=J^T * lambda
const Vector3 angularImpulseBody2 = lambdaLowerLimit * mA1;
// Compute the pseudo velocity of body 2
const Vector3 w2 = mI2 * angularImpulseBody2;
// Compute the pseudo velocity
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// If the upper limit is violated
@ -689,31 +588,25 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// Compute the Lagrange multiplier lambda for the upper limit constraint
decimal lambdaUpperLimit = mInverseMassMatrixLimitMotor * (-upperLimitError);
// Apply the impulse to the bodies of the joint
if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda of body 1
const Vector3 angularImpulseBody1 = lambdaUpperLimit * mA1;
// Compute the impulse P=J^T * lambda
const Vector3 angularImpulseBody1 = lambdaUpperLimit * mA1;
// Compute the pseudo velocity of body 1
const Vector3 w1 = mI1 * angularImpulseBody1;
// Compute the pseudo velocity
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Update the body position/orientation
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda of body 2
const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mA1;
// Compute the impulse P=J^T * lambda
const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mA1;
// Compute the pseudo velocity of body 2
const Vector3 w2 = mI2 * angularImpulseBody2;
// Compute the pseudo velocity
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
}
}

2
src/constraint/Joint.h
View File

@ -34,7 +34,7 @@
// ReactPhysics3D namespace
namespace reactphysics3d {
// Enumeration for the type of a constraint
/// Enumeration for the type of a constraint
enum JointType {BALLSOCKETJOINT, SLIDERJOINT, HINGEJOINT, FIXEDJOINT};
// Class declarations

476
src/constraint/SliderJoint.cpp
View File

@ -124,21 +124,11 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
// Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
// constraints (2x2 matrix)
decimal sumInverseMass = 0.0;
Vector3 I1R1PlusUCrossN1(0, 0, 0);
Vector3 I1R1PlusUCrossN2(0, 0, 0);
Vector3 I2R2CrossN1(0, 0, 0);
Vector3 I2R2CrossN2(0, 0, 0);
if (mBody1->isMotionEnabled()) {
sumInverseMass += mBody1->getMassInverse();
I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1;
I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2;
}
if (mBody2->isMotionEnabled()) {
sumInverseMass += mBody2->getMassInverse();
I2R2CrossN1 = mI2 * mR2CrossN1;
I2R2CrossN2 = mI2 * mR2CrossN2;
}
decimal sumInverseMass = mBody1->mMassInverse + mBody2->mMassInverse;
Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1;
Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2;
Vector3 I2R2CrossN1 = mI2 * mR2CrossN1;
Vector3 I2R2CrossN2 = mI2 * mR2CrossN2;
const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
mR2CrossN1.dot(I2R2CrossN1);
const decimal el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
@ -149,7 +139,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
mR2CrossN2.dot(I2R2CrossN2);
Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
mInverseMassMatrixTranslationConstraint.setToZero();
if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse();
}
@ -164,14 +154,8 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix)
mInverseMassMatrixRotationConstraint.setToZero();
if (mBody1->isMotionEnabled()) {
mInverseMassMatrixRotationConstraint += mI1;
}
if (mBody2->isMotionEnabled()) {
mInverseMassMatrixRotationConstraint += mI2;
}
if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
mInverseMassMatrixRotationConstraint = mI1 + mI2;
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse();
}
@ -188,15 +172,9 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
if (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated)) {
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
mInverseMassMatrixLimit = 0.0;
if (mBody1->isMotionEnabled()) {
mInverseMassMatrixLimit += mBody1->getMassInverse() +
mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis);
}
if (mBody2->isMotionEnabled()) {
mInverseMassMatrixLimit += mBody2->getMassInverse() +
mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis);
}
mInverseMassMatrixLimit = mBody1->mMassInverse + mBody2->mMassInverse +
mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) +
mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis);
mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ?
decimal(1.0) / mInverseMassMatrixLimit : decimal(0.0);
@ -217,13 +195,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
if (mIsMotorEnabled) {
// Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix)
mInverseMassMatrixMotor = 0.0;
if (mBody1->isMotionEnabled()) {
mInverseMassMatrixMotor += mBody1->getMassInverse();
}
if (mBody2->isMotionEnabled()) {
mInverseMassMatrixMotor += mBody2->getMassInverse();
}
mInverseMassMatrixMotor = mBody1->mMassInverse + mBody2->mMassInverse;
mInverseMassMatrixMotor = (mInverseMassMatrixMotor > 0.0) ?
decimal(1.0) / mInverseMassMatrixMotor : decimal(0.0);
}
@ -250,58 +222,53 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
const decimal inverseMassBody1 = mBody1->getMassInverse();
const decimal inverseMassBody2 = mBody2->getMassInverse();
const decimal inverseMassBody1 = mBody1->mMassInverse;
const decimal inverseMassBody2 = mBody2->mMassInverse;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1
decimal impulseLimits = mImpulseUpperLimit - mImpulseLowerLimit;
Vector3 linearImpulseLimits = impulseLimits * mSliderAxisWorld;
// Compute the impulse P=J^T * lambda for the motor constraint
// Compute the impulse P=J^T * lambda for the motor constraint of body 1
Vector3 impulseMotor = mImpulseMotor * mSliderAxisWorld;
if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
Vector3 linearImpulseBody1 = -mN1 * mImpulseTranslation.x - mN2 * mImpulseTranslation.y;
Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * mImpulseTranslation.x -
mR1PlusUCrossN2 * mImpulseTranslation.y;
// Compute the impulse P=J^T * lambda for the 2 translation constraints
Vector3 linearImpulseBody1 = -mN1 * mImpulseTranslation.x - mN2 * mImpulseTranslation.y;
Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * mImpulseTranslation.x -
mR1PlusUCrossN2 * mImpulseTranslation.y;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
angularImpulseBody1 += -mImpulseRotation;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
angularImpulseBody1 += -mImpulseRotation;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1
linearImpulseBody1 += linearImpulseLimits;
angularImpulseBody1 += impulseLimits * mR1PlusUCrossSliderAxis;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints
linearImpulseBody1 += linearImpulseLimits;
angularImpulseBody1 += impulseLimits * mR1PlusUCrossSliderAxis;
// Compute the impulse P=J^T * lambda for the motor constraint of body 1
linearImpulseBody1 += impulseMotor;
// Compute the impulse P=J^T * lambda for the motor constraint
linearImpulseBody1 += impulseMotor;
// Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2
Vector3 linearImpulseBody2 = mN1 * mImpulseTranslation.x + mN2 * mImpulseTranslation.y;
Vector3 angularImpulseBody2 = mR2CrossN1 * mImpulseTranslation.x +
mR2CrossN2 * mImpulseTranslation.y;
// Compute the impulse P=J^T * lambda for the 2 translation constraints
Vector3 linearImpulseBody2 = mN1 * mImpulseTranslation.x + mN2 * mImpulseTranslation.y;
Vector3 angularImpulseBody2 = mR2CrossN1 * mImpulseTranslation.x +
mR2CrossN2 * mImpulseTranslation.y;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2
angularImpulseBody2 += mImpulseRotation;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
angularImpulseBody2 += mImpulseRotation;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 2
linearImpulseBody2 += -linearImpulseLimits;
angularImpulseBody2 += -impulseLimits * mR2CrossSliderAxis;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints
linearImpulseBody2 += -linearImpulseLimits;
angularImpulseBody2 += -impulseLimits * mR2CrossSliderAxis;
// Compute the impulse P=J^T * lambda for the motor constraint of body 2
linearImpulseBody2 += -impulseMotor;
// Compute the impulse P=J^T * lambda for the motor constraint
linearImpulseBody2 += -impulseMotor;
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
}
// Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
}
// Solve the velocity constraint
@ -314,8 +281,8 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->getMassInverse();
decimal inverseMassBody2 = mBody2->getMassInverse();
decimal inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse;
// --------------- Translation Constraints --------------- //
@ -330,27 +297,22 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
Vector2 deltaLambda = mInverseMassMatrixTranslationConstraint * (-JvTranslation -mBTranslation);
mImpulseTranslation += deltaLambda;
if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
const Vector3 linearImpulseBody1 = -mN1 * deltaLambda.x - mN2 * deltaLambda.y;
Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * deltaLambda.x -
mR1PlusUCrossN2 * deltaLambda.y;
// Compute the impulse P=J^T * lambda for the 2 translation constraints
const Vector3 linearImpulseBody1 = -mN1 * deltaLambda.x - mN2 * deltaLambda.y;
const Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * deltaLambda.x -
mR1PlusUCrossN2 * deltaLambda.y;
// Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2
const Vector3 linearImpulseBody2 = mN1 * deltaLambda.x + mN2 * deltaLambda.y;
Vector3 angularImpulseBody2 = mR2CrossN1 * deltaLambda.x + mR2CrossN2 * deltaLambda.y;
// Compute the impulse P=J^T * lambda for the 2 translation constraints
const Vector3 linearImpulseBody2 = mN1 * deltaLambda.x + mN2 * deltaLambda.y;
const Vector3 angularImpulseBody2 = mR2CrossN1 * deltaLambda.x + mR2CrossN2 * deltaLambda.y;
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
}
// Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
// --------------- Rotation Constraints --------------- //
@ -361,22 +323,17 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
Vector3 deltaLambda2 = mInverseMassMatrixRotationConstraint * (-JvRotation - mBRotation);
mImpulseRotation += deltaLambda2;
if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
angularImpulseBody1 = -deltaLambda2;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody1 = -deltaLambda2;
// Apply the impulse to the body to body 1
w1 += mI1 * angularImpulseBody1;
// Apply the impulse to the body
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2
angularImpulseBody2 = deltaLambda2;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody2 = deltaLambda2;
// Apply the impulse to the body
w2 += mI2 * angularImpulseBody2;
}
// Apply the impulse to the body 2
w2 += mI2 * angularImpulseBody2;
// --------------- Limits Constraints --------------- //
@ -395,26 +352,21 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0));
deltaLambdaLower = mImpulseLowerLimit - lambdaTemp;
if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 1
const Vector3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld;
const Vector3 angularImpulseBody1 = -deltaLambdaLower * mR1PlusUCrossSliderAxis;
// Compute the impulse P=J^T * lambda for the lower limit constraint
const Vector3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld;
const Vector3 angularImpulseBody1 = -deltaLambdaLower * mR1PlusUCrossSliderAxis;
// Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 2
const Vector3 linearImpulseBody2 = deltaLambdaLower * mSliderAxisWorld;
const Vector3 angularImpulseBody2 = deltaLambdaLower * mR2CrossSliderAxis;
// Compute the impulse P=J^T * lambda for the lower limit constraint
const Vector3 linearImpulseBody2 = deltaLambdaLower * mSliderAxisWorld;
const Vector3 angularImpulseBody2 = deltaLambdaLower * mR2CrossSliderAxis;
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
}
// Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
}
// If the upper limit is violated
@ -430,26 +382,21 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0));
deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp;
if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 1
const Vector3 linearImpulseBody1 = deltaLambdaUpper * mSliderAxisWorld;
const Vector3 angularImpulseBody1 = deltaLambdaUpper * mR1PlusUCrossSliderAxis;
// Compute the impulse P=J^T * lambda for the upper limit constraint
const Vector3 linearImpulseBody1 = deltaLambdaUpper * mSliderAxisWorld;
const Vector3 angularImpulseBody1 = deltaLambdaUpper * mR1PlusUCrossSliderAxis;
// Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 2
const Vector3 linearImpulseBody2 = -deltaLambdaUpper * mSliderAxisWorld;
const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mR2CrossSliderAxis;
// Compute the impulse P=J^T * lambda for the upper limit constraint
const Vector3 linearImpulseBody2 = -deltaLambdaUpper * mSliderAxisWorld;
const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mR2CrossSliderAxis;
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
}
// Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
}
}
@ -467,22 +414,17 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
deltaLambdaMotor = mImpulseMotor - lambdaTemp;
if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the motor of body 1
const Vector3 linearImpulseBody1 = deltaLambdaMotor * mSliderAxisWorld;
// Compute the impulse P=J^T * lambda for the motor
const Vector3 linearImpulseBody1 = deltaLambdaMotor * mSliderAxisWorld;
// Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1;
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1;
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the motor of body 2
const Vector3 linearImpulseBody2 = -deltaLambdaMotor * mSliderAxisWorld;
// Compute the impulse P=J^T * lambda for the motor
const Vector3 linearImpulseBody2 = -deltaLambdaMotor * mSliderAxisWorld;
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2;
}
// Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2;
}
}
@ -500,8 +442,8 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->getMassInverse();
decimal inverseMassBody2 = mBody2->getMassInverse();
decimal inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse;
// Recompute the inertia tensor of bodies
mI1 = mBody1->getInertiaTensorInverseWorld();
@ -540,21 +482,11 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
// constraints (2x2 matrix)
decimal sumInverseMass = 0.0;
Vector3 I1R1PlusUCrossN1(0, 0, 0);
Vector3 I1R1PlusUCrossN2(0, 0, 0);
Vector3 I2R2CrossN1(0, 0, 0);
Vector3 I2R2CrossN2(0, 0, 0);
if (mBody1->isMotionEnabled()) {
sumInverseMass += mBody1->getMassInverse();
I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1;
I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2;
}
if (mBody2->isMotionEnabled()) {
sumInverseMass += mBody2->getMassInverse();
I2R2CrossN1 = mI2 * mR2CrossN1;
I2R2CrossN2 = mI2 * mR2CrossN2;
}
decimal sumInverseMass = mBody1->mMassInverse + mBody2->mMassInverse;
Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1;
Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2;
Vector3 I2R2CrossN1 = mI2 * mR2CrossN1;
Vector3 I2R2CrossN2 = mI2 * mR2CrossN2;
const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
mR2CrossN1.dot(I2R2CrossN1);
const decimal el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
@ -565,7 +497,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
mR2CrossN2.dot(I2R2CrossN2);
Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
mInverseMassMatrixTranslationConstraint.setToZero();
if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse();
}
@ -575,51 +507,40 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Compute the Lagrange multiplier lambda for the 2 translation constraints
Vector2 lambdaTranslation = mInverseMassMatrixTranslationConstraint * (-translationError);
if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
const Vector3 linearImpulseBody1 = -mN1 * lambdaTranslation.x - mN2 * lambdaTranslation.y;
Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * lambdaTranslation.x -
mR1PlusUCrossN2 * lambdaTranslation.y;
// Compute the impulse P=J^T * lambda for the 2 translation constraints
const Vector3 linearImpulseBody1 = -mN1 * lambdaTranslation.x - mN2 * lambdaTranslation.y;
const Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * lambdaTranslation.x -
mR1PlusUCrossN2 * lambdaTranslation.y;
// Apply the impulse to the body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
Vector3 w1 = mI1 * angularImpulseBody1;
// Apply the impulse to the body
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Update the body position/orientation
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2
const Vector3 linearImpulseBody2 = mN1 * lambdaTranslation.x + mN2 * lambdaTranslation.y;
Vector3 angularImpulseBody2 = mR2CrossN1 * lambdaTranslation.x +
mR2CrossN2 * lambdaTranslation.y;
// Compute the impulse P=J^T * lambda for the 2 translation constraints
const Vector3 linearImpulseBody2 = mN1 * lambdaTranslation.x + mN2 * lambdaTranslation.y;
const Vector3 angularImpulseBody2 = mR2CrossN1 * lambdaTranslation.x +
mR2CrossN2 * lambdaTranslation.y;
// Apply the impulse to the body 2
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
Vector3 w2 = mI2 * angularImpulseBody2;
// Apply the impulse to the body
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// Update the body position/orientation of body 2
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
// --------------- Rotation Constraints --------------- //
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix)
mInverseMassMatrixRotationConstraint.setToZero();
if (mBody1->isMotionEnabled()) {
mInverseMassMatrixRotationConstraint += mI1;
}
if (mBody2->isMotionEnabled()) {
mInverseMassMatrixRotationConstraint += mI2;
}
if (mBody1->isMotionEnabled() || mBody2->isMotionEnabled()) {
mInverseMassMatrixRotationConstraint = mI1 + mI2;
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse();
}
@ -632,30 +553,25 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 lambdaRotation = mInverseMassMatrixRotationConstraint * (-errorRotation);
if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
angularImpulseBody1 = -lambdaRotation;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody1 = -lambdaRotation;
// Apply the impulse to the body 1
w1 = mI1 * angularImpulseBody1;
// Apply the impulse to the body
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Update the body position/orientation
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2
angularImpulseBody2 = lambdaRotation;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody2 = lambdaRotation;
// Apply the impulse to the body 2
w2 = mI2 * angularImpulseBody2;
// Apply the impulse to the body
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
// --------------- Limits Constraints --------------- //
@ -664,15 +580,9 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
if (mIsLowerLimitViolated || mIsUpperLimitViolated) {
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
mInverseMassMatrixLimit = 0.0;
if (mBody1->isMotionEnabled()) {
mInverseMassMatrixLimit += mBody1->getMassInverse() +
mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis);
}
if (mBody2->isMotionEnabled()) {
mInverseMassMatrixLimit += mBody2->getMassInverse() +
mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis);
}
mInverseMassMatrixLimit = mBody1->mMassInverse + mBody2->mMassInverse +
mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) +
mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis);
mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ?
decimal(1.0) / mInverseMassMatrixLimit : decimal(0.0);
}
@ -683,36 +593,31 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Compute the Lagrange multiplier lambda for the lower limit constraint
decimal lambdaLowerLimit = mInverseMassMatrixLimit * (-lowerLimitError);
if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 1
const Vector3 linearImpulseBody1 = -lambdaLowerLimit * mSliderAxisWorld;
const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mR1PlusUCrossSliderAxis;
// Compute the impulse P=J^T * lambda for the lower limit constraint
const Vector3 linearImpulseBody1 = -lambdaLowerLimit * mSliderAxisWorld;
const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mR1PlusUCrossSliderAxis;
// Apply the impulse to the body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1;
// Apply the impulse to the body
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Update the body position/orientation
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 2
const Vector3 linearImpulseBody2 = lambdaLowerLimit * mSliderAxisWorld;
const Vector3 angularImpulseBody2 = lambdaLowerLimit * mR2CrossSliderAxis;
// Compute the impulse P=J^T * lambda for the lower limit constraint
const Vector3 linearImpulseBody2 = lambdaLowerLimit * mSliderAxisWorld;
const Vector3 angularImpulseBody2 = lambdaLowerLimit * mR2CrossSliderAxis;
// Apply the impulse to the body 2
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
const Vector3 w2 = mI2 * angularImpulseBody2;
// Apply the impulse to the body
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// Update the body position/orientation of body 2
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// If the upper limit is violated
@ -721,36 +626,31 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Compute the Lagrange multiplier lambda for the upper limit constraint
decimal lambdaUpperLimit = mInverseMassMatrixLimit * (-upperLimitError);
if (mBody1->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 1
const Vector3 linearImpulseBody1 = lambdaUpperLimit * mSliderAxisWorld;
const Vector3 angularImpulseBody1 = lambdaUpperLimit * mR1PlusUCrossSliderAxis;
// Compute the impulse P=J^T * lambda for the upper limit constraint
const Vector3 linearImpulseBody1 = lambdaUpperLimit * mSliderAxisWorld;
const Vector3 angularImpulseBody1 = lambdaUpperLimit * mR1PlusUCrossSliderAxis;
// Apply the impulse to the body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1;
// Apply the impulse to the body
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Update the body position/orientation
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->isMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 2
const Vector3 linearImpulseBody2 = -lambdaUpperLimit * mSliderAxisWorld;
const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mR2CrossSliderAxis;
// Compute the impulse P=J^T * lambda for the upper limit constraint
const Vector3 linearImpulseBody2 = -lambdaUpperLimit * mSliderAxisWorld;
const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mR2CrossSliderAxis;
// Apply the impulse to the body 2
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
const Vector3 w2 = mI2 * angularImpulseBody2;
// Apply the impulse to the body
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// Update the body position/orientation of body 2
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
}
}

126
src/engine/ContactSolver.cpp
View File

@ -96,10 +96,8 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
internalManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second;
internalManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
internalManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
internalManifold.isBody1Moving = body1->isMotionEnabled();
internalManifold.isBody2Moving = body2->isMotionEnabled();
internalManifold.massInverseBody1 = body1->getMassInverse();
internalManifold.massInverseBody2 = body2->getMassInverse();
internalManifold.massInverseBody1 = body1->mMassInverse;
internalManifold.massInverseBody2 = body2->mMassInverse;
internalManifold.nbContacts = externalManifold->getNbContactPoints();
internalManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2);
internalManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
@ -211,15 +209,9 @@ void ContactSolver::initializeContactConstraints() {
contactPoint.r2CrossN = contactPoint.r2.cross(contactPoint.normal);
// Compute the inverse mass matrix K for the penetration constraint
decimal massPenetration = 0.0;
if (manifold.isBody1Moving) {
massPenetration += manifold.massInverseBody1 +
((I1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal);
}
if (manifold.isBody2Moving) {
massPenetration += manifold.massInverseBody2 +
decimal massPenetration = manifold.massInverseBody1 + manifold.massInverseBody2 +
((I1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal) +
((I2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal);
}
massPenetration > 0.0 ? contactPoint.inversePenetrationMass = decimal(1.0) /
massPenetration :
decimal(0.0);
@ -237,24 +229,16 @@ void ContactSolver::initializeContactConstraints() {
// Compute the inverse mass matrix K for the friction
// constraints at each contact point
decimal friction1Mass = 0.0;
decimal friction2Mass = 0.0;
if (manifold.isBody1Moving) {
friction1Mass += manifold.massInverseBody1 +
((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot(
contactPoint.frictionVector1);
friction2Mass += manifold.massInverseBody1 +
((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot(
contactPoint.frictionVector2);
}
if (manifold.isBody2Moving) {
friction1Mass += manifold.massInverseBody2 +
((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot(
contactPoint.frictionVector1);
friction2Mass += manifold.massInverseBody2 +
((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot(
contactPoint.frictionVector2);
}
decimal friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot(
contactPoint.frictionVector1) +
((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot(
contactPoint.frictionVector1);
decimal friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot(
contactPoint.frictionVector2) +
((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot(
contactPoint.frictionVector2);
friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = decimal(1.0) /
friction1Mass :
decimal(0.0);
@ -308,29 +292,19 @@ void ContactSolver::initializeContactConstraints() {
manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionVector2);
manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1);
manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionVector2);
decimal friction1Mass = 0.0;
decimal friction2Mass = 0.0;
if (manifold.isBody1Moving) {
friction1Mass += manifold.massInverseBody1 +
((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot(
manifold.frictionVector1);
friction2Mass += manifold.massInverseBody1 +
((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot(
manifold.frictionVector2);
}
if (manifold.isBody2Moving) {
friction1Mass += manifold.massInverseBody2 +
((I2 * manifold.r2CrossT1).cross(manifold.r2Friction)).dot(
manifold.frictionVector1);
friction2Mass += manifold.massInverseBody2 +
((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot(
manifold.frictionVector2);
}
decimal frictionTwistMass = manifold.normal.dot(
manifold.inverseInertiaTensorBody1 *
decimal friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot(
manifold.frictionVector1) +
((I2 * manifold.r2CrossT1).cross(manifold.r2Friction)).dot(
manifold.frictionVector1);
decimal friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot(
manifold.frictionVector2) +
((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot(
manifold.frictionVector2);
decimal frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1 *
manifold.normal) +
manifold.normal.dot(
manifold.inverseInertiaTensorBody2 *
manifold.normal.dot(manifold.inverseInertiaTensorBody2 *
manifold.normal);
friction1Mass > 0.0 ? manifold.inverseFriction1Mass = decimal(1.0)/friction1Mass
: decimal(0.0);
@ -749,38 +723,34 @@ void ContactSolver::storeImpulses() {
void ContactSolver::applyImpulse(const Impulse& impulse,
const ContactManifoldSolver& manifold) {
// Update the velocities of the bodies by applying the impulse P
if (manifold.isBody1Moving) {
mLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 *
impulse.linearImpulseBody1;
mAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 *
impulse.angularImpulseBody1;
}
if (manifold.isBody2Moving) {
mLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 *
impulse.linearImpulseBody2;
mAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 *
impulse.angularImpulseBody2;
}
// Update the velocities of the body 1 by applying the impulse P
mLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 *
impulse.linearImpulseBody1;
mAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 *
impulse.angularImpulseBody1;
// Update the velocities of the body 1 by applying the impulse P
mLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 *
impulse.linearImpulseBody2;
mAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 *
impulse.angularImpulseBody2;
}
// Apply an impulse to the two bodies of a constraint
void ContactSolver::applySplitImpulse(const Impulse& impulse,
const ContactManifoldSolver& manifold) {
// Update the velocities of the bodies by applying the impulse P
if (manifold.isBody1Moving) {
mSplitLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 *
impulse.linearImpulseBody1;
mSplitAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 *
impulse.angularImpulseBody1;
}
if (manifold.isBody2Moving) {
mSplitLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 *
impulse.linearImpulseBody2;
mSplitAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 *
impulse.angularImpulseBody2;
}
// Update the velocities of the body 1 by applying the impulse P
mSplitLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 *
impulse.linearImpulseBody1;
mSplitAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 *
impulse.angularImpulseBody1;
// Update the velocities of the body 1 by applying the impulse P
mSplitLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 *
impulse.linearImpulseBody2;
mSplitAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 *
impulse.angularImpulseBody2;
}
// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane

6
src/engine/ContactSolver.h
View File

@ -218,12 +218,6 @@ class ContactSolver {
/// Inverse inertia tensor of body 2
Matrix3x3 inverseInertiaTensorBody2;
/// True if the body 1 is allowed to move
bool isBody1Moving;
/// True if the body 2 is allowed to move
bool isBody2Moving;
/// Contact point constraints
ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD];

149
src/engine/DynamicsWorld.cpp
View File

@ -173,39 +173,35 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
// For each body of the island
for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
// If the body is allowed to move
if (bodies[b]->isMotionEnabled()) {
// Get the constrained velocity
uint indexArray = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray];
// Get the constrained velocity
uint indexArray = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray];
// Update the linear and angular velocity of the body
bodies[b]->mLinearVelocity = newLinVelocity;
bodies[b]->mAngularVelocity = newAngVelocity;
// Update the linear and angular velocity of the body
bodies[b]->setLinearVelocity(newLinVelocity);
bodies[b]->setAngularVelocity(newAngVelocity);
// Add the split impulse velocity from Contact Solver (only used
// to update the position)
if (mContactSolver.isSplitImpulseActive()) {
// Add the split impulse velocity from Contact Solver (only used
// to update the position)
if (mContactSolver.isSplitImpulseActive()) {
newLinVelocity += mSplitLinearVelocities[indexArray];
newAngVelocity += mSplitAngularVelocities[indexArray];
}
// Get current position and orientation of the body
const Vector3& currentPosition = bodies[b]->getTransform().getPosition();
const Quaternion& currentOrientation = bodies[b]->getTransform().getOrientation();
// Compute the new position of the body
Vector3 newPosition = currentPosition + newLinVelocity * dt;
Quaternion newOrientation = currentOrientation + Quaternion(0, newAngVelocity) *
currentOrientation * decimal(0.5) * dt;
// Update the Transform of the body
Transform newTransform(newPosition, newOrientation.getUnit());
bodies[b]->setTransform(newTransform);
newLinVelocity += mSplitLinearVelocities[indexArray];
newAngVelocity += mSplitAngularVelocities[indexArray];
}
// Get current position and orientation of the body
const Vector3& currentPosition = bodies[b]->getTransform().getPosition();
const Quaternion& currentOrientation = bodies[b]->getTransform().getOrientation();
// Compute the new position of the body
Vector3 newPosition = currentPosition + newLinVelocity * dt;
Quaternion newOrientation = currentOrientation + Quaternion(0, newAngVelocity) *
currentOrientation * decimal(0.5) * dt;
// Update the Transform of the body
Transform newTransform(newPosition, newOrientation.getUnit());
bodies[b]->setTransform(newTransform);
}
}
}
@ -313,52 +309,48 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
assert(mSplitLinearVelocities[indexBody] == Vector3(0, 0, 0));
assert(mSplitAngularVelocities[indexBody] == Vector3(0, 0, 0));
// If the body is allowed to move
if (bodies[b]->isMotionEnabled()) {
// Integrate the external force to get the new velocity of the body
mConstrainedLinearVelocities[indexBody] = bodies[b]->getLinearVelocity() +
dt * bodies[b]->mMassInverse * bodies[b]->mExternalForce;
mConstrainedAngularVelocities[indexBody] = bodies[b]->getAngularVelocity() +
dt * bodies[b]->getInertiaTensorInverseWorld() *
bodies[b]->mExternalTorque;
// Integrate the external force to get the new velocity of the body
mConstrainedLinearVelocities[indexBody] = bodies[b]->getLinearVelocity() +
dt * bodies[b]->getMassInverse() * bodies[b]->mExternalForce;
mConstrainedAngularVelocities[indexBody] = bodies[b]->getAngularVelocity() +
dt * bodies[b]->getInertiaTensorInverseWorld() *
bodies[b]->mExternalTorque;
// If the gravity has to be applied to this rigid body
if (bodies[b]->isGravityEnabled() && mIsGravityEnabled) {
// If the gravity has to be applied to this rigid body
if (bodies[b]->isGravityEnabled() && mIsGravityEnabled) {
// Integrate the gravity force
mConstrainedLinearVelocities[indexBody] += dt * bodies[b]->getMassInverse() *
bodies[b]->getMass() * mGravity;
}
// Apply the velocity damping
// Damping force : F_c = -c' * v (c=damping factor)
// Equation : m * dv/dt = -c' * v
// => dv/dt = -c * v (with c=c'/m)
// => dv/dt + c * v = 0
// Solution : v(t) = v0 * e^(-c * t)
// => v(t + dt) = v0 * e^(-c(t + dt))
// = v0 * e^(-ct) * e^(-c * dt)
// = v(t) * e^(-c * dt)
// => v2 = v1 * e^(-c * dt)
// Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ...
// => e^(-x) ~ 1 - x
// => v2 = v1 * (1 - c * dt)
decimal linDampingFactor = bodies[b]->getLinearDamping();
decimal angDampingFactor = bodies[b]->getAngularDamping();
decimal linearDamping = clamp(decimal(1.0) - dt * linDampingFactor,
decimal(0.0), decimal(1.0));
decimal angularDamping = clamp(decimal(1.0) - dt * angDampingFactor,
decimal(0.0), decimal(1.0));
mConstrainedLinearVelocities[indexBody] *= clamp(linearDamping, decimal(0.0),
decimal(1.0));
mConstrainedAngularVelocities[indexBody] *= clamp(angularDamping, decimal(0.0),
decimal(1.0));
// Update the old Transform of the body
bodies[b]->updateOldTransform();
// Integrate the gravity force
mConstrainedLinearVelocities[indexBody] += dt * bodies[b]->mMassInverse *
bodies[b]->getMass() * mGravity;
}
// Apply the velocity damping
// Damping force : F_c = -c' * v (c=damping factor)
// Equation : m * dv/dt = -c' * v
// => dv/dt = -c * v (with c=c'/m)
// => dv/dt + c * v = 0
// Solution : v(t) = v0 * e^(-c * t)
// => v(t + dt) = v0 * e^(-c(t + dt))
// = v0 * e^(-ct) * e^(-c * dt)
// = v(t) * e^(-c * dt)
// => v2 = v1 * e^(-c * dt)
// Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ...
// => e^(-x) ~ 1 - x
// => v2 = v1 * (1 - c * dt)
decimal linDampingFactor = bodies[b]->getLinearDamping();
decimal angDampingFactor = bodies[b]->getAngularDamping();
decimal linearDamping = clamp(decimal(1.0) - dt * linDampingFactor,
decimal(0.0), decimal(1.0));
decimal angularDamping = clamp(decimal(1.0) - dt * angDampingFactor,
decimal(0.0), decimal(1.0));
mConstrainedLinearVelocities[indexBody] *= clamp(linearDamping, decimal(0.0),
decimal(1.0));
mConstrainedAngularVelocities[indexBody] *= clamp(angularDamping, decimal(0.0),
decimal(1.0));
// Update the old Transform of the body
bodies[b]->updateOldTransform();
indexBody++;
}
}
@ -484,7 +476,6 @@ void DynamicsWorld::solvePositionCorrection() {
// Create a rigid body into the physics world
RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal mass,
const Matrix3x3& inertiaTensorLocal,
const CollisionShape& collisionShape) {
// Compute the body ID
@ -499,7 +490,6 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal ma
// Create the rigid body
RigidBody* rigidBody = new (mMemoryAllocator.allocate(sizeof(RigidBody))) RigidBody(transform,
mass,
inertiaTensorLocal,
newCollisionShape,
bodyID);
assert(rigidBody != NULL);
@ -757,9 +747,8 @@ void DynamicsWorld::computeIslands() {
// If the body has already been added to an island, we go to the next body
if (body->mIsAlreadyInIsland) continue;
// 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
if (!body->isMotionEnabled()) continue;
// If the body is static, we go to the next body
if (body->getType() == STATIC) continue;
// If the body is sleeping or inactive, we go to the next body
if (body->isSleeping() || !body->isActive()) continue;
@ -789,9 +778,9 @@ void DynamicsWorld::computeIslands() {
// Add the body into the island
mIslands[mNbIslands]->addBody(bodyToVisit);
// If the current body is not moving, we do not want to perform the DFS
// If the current body is static, we do not want to perform the DFS
// search across that body
if (!bodyToVisit->isMotionEnabled()) continue;
if (bodyToVisit->getType() == STATIC) continue;
// For each contact manifold in which the current body is involded
ContactManifoldListElement* contactElement;
@ -854,7 +843,7 @@ void DynamicsWorld::computeIslands() {
// can also be included in the other islands
for (uint i=0; i < mIslands[mNbIslands]->mNbBodies; i++) {
if (!mIslands[mNbIslands]->mBodies[i]->isMotionEnabled()) {
if (mIslands[mNbIslands]->mBodies[i]->getType() == STATIC) {
mIslands[mNbIslands]->mBodies[i]->mIsAlreadyInIsland = false;
}
}
@ -887,7 +876,7 @@ void DynamicsWorld::updateSleepingBodies() {
for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
// Skip static bodies
if (!bodies[b]->isMotionEnabled()) continue;
if (bodies[b]->getType() == STATIC) continue;
// If the body is velocity is large enough to stay awake
if (bodies[b]->getLinearVelocity().lengthSquare() > sleepLinearVelocitySquare ||

1
src/engine/DynamicsWorld.h
View File

@ -231,7 +231,6 @@ class DynamicsWorld : public CollisionWorld {
/// Create a rigid body into the physics world.
RigidBody* createRigidBody(const Transform& transform, decimal mass,
const Matrix3x3& inertiaTensorLocal,
const CollisionShape& collisionShape);
/// Destroy a rigid body and all the joints which it belongs

8
src/mathematics/Matrix2x2.h
View File

@ -101,6 +101,9 @@ class Matrix2x2 {
/// Return the 2x2 identity matrix
static Matrix2x2 identity();
/// Return the 2x2 zero matrix
static Matrix2x2 zero();
/// Overloaded operator for addition
friend Matrix2x2 operator+(const Matrix2x2& matrix1, const Matrix2x2& matrix2);
@ -204,6 +207,11 @@ inline Matrix2x2 Matrix2x2::identity() {
return Matrix2x2(1.0, 0.0, 0.0, 1.0);
}
// Return the 2x2 zero matrix
inline Matrix2x2 Matrix2x2::zero() {
return Matrix2x2(0.0, 0.0, 0.0, 0.0);
}
// Return the matrix with absolute values
inline Matrix2x2 Matrix2x2::getAbsoluteMatrix() const {
return Matrix2x2(fabs(mRows[0][0]), fabs(mRows[0][1]),

10
src/mathematics/Matrix3x3.h
View File

@ -105,6 +105,9 @@ class Matrix3x3 {
/// Return the 3x3 identity matrix
static Matrix3x3 identity();
/// Return the 3x3 zero matrix
static Matrix3x3 zero();
/// Return a skew-symmetric matrix using a given vector that can be used
/// to compute cross product with another vector using matrix multiplication
static Matrix3x3 computeSkewSymmetricMatrixForCrossProduct(const Vector3& vector);
@ -214,11 +217,14 @@ inline void Matrix3x3::setToIdentity() {
// Return the 3x3 identity matrix
inline Matrix3x3 Matrix3x3::identity() {
// Return the isdentity matrix
return Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
}
// Return the 3x3 zero matrix
inline Matrix3x3 Matrix3x3::zero() {
return Matrix3x3(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
}
// Return a skew-symmetric matrix using a given vector that can be used
// to compute cross product with another vector using matrix multiplication
inline Matrix3x3 Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(const Vector3& vector) {

12
test/tests/mathematics/TestMatrix2x2.h
View File

@ -64,6 +64,7 @@ class TestMatrix2x2 : public Test {
testConstructors();
testGetSet();
testIdentity();
testZero();
testOthersMethods();
testOperators();
}
@ -128,6 +129,17 @@ class TestMatrix2x2 : public Test {
test(test1 == Matrix2x2::identity());
}
/// Test the zero method
void testZero() {
Matrix2x2 zero = Matrix2x2::zero();
test(zero[0][0] == 0);
test(zero[0][1] == 0);
test(zero[1][0] == 0);
test(zero[1][1] == 0);
}
/// Test others methods
void testOthersMethods() {

17
test/tests/mathematics/TestMatrix3x3.h
View File

@ -65,6 +65,7 @@ class TestMatrix3x3 : public Test {
testConstructors();
testGetSet();
testIdentity();
testZero();
testOthersMethods();
testOperators();
}
@ -147,6 +148,22 @@ class TestMatrix3x3 : public Test {
test(test1 == Matrix3x3::identity());
}
/// Test the zero method
void testZero() {
Matrix3x3 zero = Matrix3x3::zero();
test(zero[0][0] == 0);
test(zero[0][1] == 0);
test(zero[0][2] == 0);
test(zero[1][0] == 0);
test(zero[1][1] == 0);
test(zero[1][2] == 0);
test(zero[2][0] == 0);
test(zero[2][1] == 0);
test(zero[2][2] == 0);
}
/// Test others methods
void testOthersMethods() {