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

Implement the Ball-And-Socket joint

Browse Source
This commit is contained in:
Daniel Chappuis 2013-05-02 22:41:57 +02:00
parent 44a26bdcab
commit c4eee4fb1f
22 changed files with 461 additions and 147 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
examples/fallingcubes/Scene.h
View File

@ -45,13 +45,13 @@ class Scene {
// -------------------- Attributes -------------------- //
// Pointer to the viewer
/// Pointer to the viewer
openglframework::GlutViewer* mViewer;
// Light 0
/// Light 0
openglframework::Light mLight0;
// Phong shader
/// Phong shader
openglframework::Shader mPhongShader;
/// All the boxes of the scene

126
examples/joints/Scene.cpp
View File

@ -56,39 +56,11 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0),
// Set the number of iterations of the constraint solver
mDynamicsWorld->setNbIterationsSolver(15);
float radius = 2.0f;
// Create all the cubes of the scene
for (int i=0; i<NB_BOXES; i++) {
// Position of the cubes
float angle = i * 30.0f;
openglframework::Vector3 position(radius * cos(angle),
1 + i * (BOX_SIZE.y + 0.3f),
radius * sin(angle));
// Create a cube and a corresponding rigid in the dynamics world
Box* cube = new Box(BOX_SIZE, position , BOX_MASS, mDynamicsWorld);
// The box is a moving rigid body
cube->getRigidBody()->setIsMotionEnabled(true);
// Set the bouncing factor of the box
cube->getRigidBody()->setRestitution(0.4);
// Add the box the list of box in the scene
mBoxes.push_back(cube);
}
// Create the Ball-and-Socket joint
createBallAndSocketJoints();
// Create the floor
openglframework::Vector3 floorPosition(0, 0, 0);
mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld);
// The floor must be a non-moving rigid body
mFloor->getRigidBody()->setIsMotionEnabled(false);
// Set the bouncing factor of the floor
mFloor->getRigidBody()->setRestitution(0.3);
createFloor();
// Start the simulation
startSimulation();
@ -103,20 +75,17 @@ Scene::~Scene() {
// Destroy the shader
mPhongShader.destroy();
// Destroy all the cubes of the scene
for (std::vector<Box*>::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) {
// Destroy the joints
mDynamicsWorld->destroyJoint(mBallAndSocketJoint);
// Destroy the corresponding rigid body from the dynamics world
mDynamicsWorld->destroyRigidBody((*it)->getRigidBody());
// Destroy the cube
delete (*it);
}
// Destroy the rigid body of the floor
mDynamicsWorld->destroyRigidBody(mFloor->getRigidBody());
// Destroy all the boxes of the scene
mDynamicsWorld->destroyRigidBody(mBallAndSocketJointBox1->getRigidBody());
mDynamicsWorld->destroyRigidBody(mBallAndSocketJointBox2->getRigidBody());
delete mBallAndSocketJointBox1;
delete mBallAndSocketJointBox2;
// Destroy the floor
mDynamicsWorld->destroyRigidBody(mFloor->getRigidBody());
delete mFloor;
// Destroy the dynamics world
@ -133,12 +102,10 @@ void Scene::simulate() {
mDynamicsWorld->update();
// Update the position and orientation of the boxes
for (std::vector<Box*>::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) {
// Update the transform used for the rendering
(*it)->updateTransform();
}
mBallAndSocketJointBox1->updateTransform();
mBallAndSocketJointBox2->updateTransform();
// Update the position and orientation of the floor
mFloor->updateTransform();
}
@ -169,10 +136,9 @@ void Scene::render() {
mPhongShader.setVector3Uniform("lightSpecularColor", Vector3(specCol.r, specCol.g, specCol.b));
mPhongShader.setFloatUniform("shininess", 60.0f);
// Render all the cubes of the scene
for (std::vector<Box*>::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) {
(*it)->render(mPhongShader);
}
// Render all the boxes
mBallAndSocketJointBox1->render(mPhongShader);
mBallAndSocketJointBox2->render(mPhongShader);
// Render the floor
mFloor->render(mPhongShader);
@ -180,3 +146,61 @@ void Scene::render() {
// Unbind the shader
mPhongShader.unbind();
}
// Create the boxes and joints for the Ball-and-Socket joint example
void Scene::createBallAndSocketJoints() {
// --------------- Create the first box --------------- //
// Position of the box
openglframework::Vector3 positionBox1(0, 15, 0);
// Create a box and a corresponding rigid in the dynamics world
mBallAndSocketJointBox1 = new Box(BOX_SIZE, positionBox1 , BOX_MASS, mDynamicsWorld);
// The fist box cannot move
mBallAndSocketJointBox1->getRigidBody()->setIsMotionEnabled(false);
// Set the bouncing factor of the box
mBallAndSocketJointBox1->getRigidBody()->setRestitution(0.4);
// --------------- Create the second box --------------- //
// Position of the box
openglframework::Vector3 positionBox2(0, 10, 0);
// Create a box and a corresponding rigid in the dynamics world
mBallAndSocketJointBox2 = new Box(BOX_SIZE, positionBox2 , BOX_MASS, mDynamicsWorld);
// The second box is allowed to move
mBallAndSocketJointBox2->getRigidBody()->setIsMotionEnabled(true);
// Set the bouncing factor of the box
mBallAndSocketJointBox2->getRigidBody()->setRestitution(0.4);
// --------------- Create the joint --------------- //
// Create the joint info object
rp3d::BallAndSocketJointInfo jointInfo;
jointInfo.body1 = mBallAndSocketJointBox1->getRigidBody();
jointInfo.body2 = mBallAndSocketJointBox2->getRigidBody();
jointInfo.anchorPointWorldSpace = rp3d::Vector3(0, 12.5, 0);
// Create the joint in the dynamics world
mBallAndSocketJoint = dynamic_cast<rp3d::BallAndSocketJoint*>(
mDynamicsWorld->createJoint(jointInfo));
}
// Create the floor
void Scene::createFloor() {
// Create the floor
openglframework::Vector3 floorPosition(0, 0, 0);
mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld);
// The floor must be a non-moving rigid body
mFloor->getRigidBody()->setIsMotionEnabled(false);
// Set the bouncing factor of the floor
mFloor->getRigidBody()->setRestitution(0.3);
}

25
examples/joints/Scene.h
View File

@ -32,7 +32,6 @@
#include "Box.h"
// Constants
const int NB_BOXES = 20; // Number of boxes in the scene
const openglframework::Vector3 BOX_SIZE(2, 2, 2); // Box dimensions in meters
const openglframework::Vector3 FLOOR_SIZE(20, 0.5f, 20); // Floor dimensions in meters
const float BOX_MASS = 1.0f; // Box mass in kilograms
@ -45,17 +44,23 @@ class Scene {
// -------------------- Attributes -------------------- //
// Pointer to the viewer
/// Pointer to the viewer
openglframework::GlutViewer* mViewer;
// Light 0
/// Light 0
openglframework::Light mLight0;
// Phong shader
/// Phong shader
openglframework::Shader mPhongShader;
/// All the boxes of the scene
std::vector<Box*> mBoxes;
/// Box 1 of Ball-And-Socket joint
Box* mBallAndSocketJointBox1;
/// Box 2 of Ball-And-Socket joint
Box* mBallAndSocketJointBox2;
/// Ball-and-Socket joint
rp3d::BallAndSocketJoint* mBallAndSocketJoint;
/// Box for the floor
Box* mFloor;
@ -66,6 +71,14 @@ class Scene {
/// True if the physics simulation is running
bool mIsRunning;
// -------------------- Methods -------------------- //
/// Create the boxes and joints for the Ball-and-Socket joint example
void createBallAndSocketJoints();
/// Create the floor
void createFloor();
public:
// -------------------- Methods -------------------- //

4
src/collision/narrowphase/EPA/EPAAlgorithm.cpp
View File

@ -98,7 +98,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// Transform a point from local space of body 2 to local
// space of body 1 (the GJK algorithm is done in local space of body 1)
Transform body2Tobody1 = transform1.inverse() * transform2;
Transform body2Tobody1 = transform1.getInverse() * transform2;
// Matrix that transform a direction from local
// space of body 1 into local space of body 2
@ -394,7 +394,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// Compute the contact info
v = transform1.getOrientation().getMatrix() * triangle->getClosestPoint();
Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA);
Vector3 pBLocal = body2Tobody1.inverse() * triangle->computeClosestPointOfObject(suppPointsB);
Vector3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB);
Vector3 normal = v.getUnit();
decimal penetrationDepth = v.length();
assert(penetrationDepth > 0.0);

12
src/collision/narrowphase/GJK/GJKAlgorithm.cpp
View File

@ -73,7 +73,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
// Transform a point from local space of body 2 to local
// space of body 1 (the GJK algorithm is done in local space of body 1)
Transform body2Tobody1 = transform1.inverse() * transform2;
Transform body2Tobody1 = transform1.getInverse() * transform2;
// Matrix that transform a direction from local
// space of body 1 into local space of body 2
@ -127,7 +127,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (collisionShape1->getMargin() / dist) * v);
pB = body2Tobody1.inverse() * (pB + (collisionShape2->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
@ -159,7 +159,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (collisionShape1->getMargin() / dist) * v);
pB = body2Tobody1.inverse() * (pB + (collisionShape2->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
@ -189,7 +189,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (collisionShape1->getMargin() / dist) * v);
pB = body2Tobody1.inverse() * (pB + (collisionShape2->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
@ -226,7 +226,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1,
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (collisionShape1->getMargin() / dist) * v);
pB = body2Tobody1.inverse() * (pB + (collisionShape2->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
@ -275,7 +275,7 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
// Transform a point from local space of body 2 to local space
// of body 1 (the GJK algorithm is done in local space of body 1)
Transform body2ToBody1 = transform1.inverse() * transform2;
Transform body2ToBody1 = transform1.getInverse() * transform2;
// Matrix that transform a direction from local space of body 1 into local space of body 2
Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() *

4
src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
View File

@ -60,8 +60,8 @@ bool SphereVsSphereAlgorithm::testCollision(const CollisionShape* collisionShape
// If the sphere collision shapes intersect
if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) {
Vector3 centerSphere2InBody1LocalSpace = transform1.inverse() * transform2.getPosition();
Vector3 centerSphere1InBody2LocalSpace = transform2.inverse() * transform1.getPosition();
Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
Vector3 intersectionOnBody1 = sphereShape1->getRadius() *
centerSphere2InBody1LocalSpace.getUnit();
Vector3 intersectionOnBody2 = sphereShape2->getRadius() *

95
src/constraint/BallAndSocketJoint.cpp
View File

@ -25,13 +25,17 @@
// Libraries
#include "BallAndSocketJoint.h"
#include "../engine/ConstraintSolver.h"
using namespace reactphysics3d;
// Constructor
BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo &jointInfo)
: Constraint(jointInfo){
: Constraint(jointInfo), mImpulse(Vector3(0, 0, 0)) {
// Compute the local-space anchor point for each body
mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
}
// Destructor
@ -39,3 +43,92 @@ BallAndSocketJoint::~BallAndSocketJoint() {
}
// Initialize before solving the constraint
void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
// Initialize the bodies index in the velocity array
mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
// Get the bodies positions and orientations
const Vector3& x1 = mBody1->getTransform().getPosition();
const Vector3& x2 = mBody2->getTransform().getPosition();
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
// Get the inertia tensor of bodies
Matrix3x3 inverseInertiaTensorBody1 = mBody1->getInertiaTensorInverseWorld();
Matrix3x3 inverseInertiaTensorBody2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to anchor point in local-space
const Vector3 u1Local = mLocalAnchorPointBody1 - x1;
const Vector3 u2Local = mLocalAnchorPointBody2 - x2;
// Compute the vector from body center to the anchor point in world-space
mU1World = orientationBody1 * u1Local;
mU2World = orientationBody2 * u2Local;
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mU1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mU2World);
// Compute the matrix JM^-1J^t
decimal inverseMassBodies = mBody1->getMassInverse() + mBody2->getMassInverse();
Matrix3x3 massMatrix= Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * inverseInertiaTensorBody1 * skewSymmetricMatrixU1+
skewSymmetricMatrixU2 * inverseInertiaTensorBody2 * skewSymmetricMatrixU2;
// Compute the inverse mass matrix K
mInverseMassMatrix = massMatrix.getInverse();
}
// Solve the constraint
void BallAndSocketJoint::solve(const ConstraintSolverData& constraintSolverData) {
// Get the body positions
const Vector3& x1 = mBody1->getTransform().getPosition();
const Vector3& x2 = mBody2->getTransform().getPosition();
// Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->getMassInverse();
decimal inverseMassBody2 = mBody2->getMassInverse();
Matrix3x3 inverseInertiaTensorBody1 = mBody1->getInertiaTensorInverseWorld();
Matrix3x3 inverseInertiaTensorBody2 = mBody2->getInertiaTensorInverseWorld();
// Compute J*v
Vector3 Jv = -v1 + mU1World.cross(w1) + v2 - mU2World.cross(w2);
// Compute the bias "b" of the constraint
decimal beta = 0.8; // TODO : Use a constant here
decimal biasFactor = -(beta/constraintSolverData.timeStep);
Vector3 b = biasFactor * (x2 + mU2World - x1 - mU1World);
// Compute the Lagrange multiplier lambda
Vector3 deltaLambda = mInverseMassMatrix * (-Jv - b);
mImpulse = mImpulse + deltaLambda;
// Compute the impulse P=J^T * lambda
Vector3 linearImpulseBody1 = -deltaLambda;
Vector3 angularImpulseBody1 = mU1World.cross(deltaLambda);
Vector3 linearImpulseBody2 = deltaLambda;
Vector3 angularImpulseBody2 = -mU2World.cross(deltaLambda);
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) {
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += inverseInertiaTensorBody1 * angularImpulseBody1;
}
if (mBody2->getIsMotionEnabled()) {
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += inverseInertiaTensorBody2 * angularImpulseBody2;
}
}

30
src/constraint/BallAndSocketJoint.h
View File

@ -44,7 +44,7 @@ struct BallAndSocketJointInfo : public ConstraintInfo {
// -------------------- Attributes -------------------- //
/// Anchor point (in world space coordinates)
Vector3 anchorPoint;
Vector3 anchorPointWorldSpace;
/// Constructor
BallAndSocketJointInfo() : ConstraintInfo(BALLSOCKETJOINT) {}
@ -62,10 +62,28 @@ class BallAndSocketJoint : public Constraint {
// -------------------- Attributes -------------------- //
/// Anchor point of body 1 (in local space coordinates)
Vector3 mLocalAnchorPoint1;
Vector3 mLocalAnchorPointBody1;
/// Anchor point of body 2 (in local space coordinates)
Vector3 mLocalAnchorPoint2;
Vector3 mLocalAnchorPointBody2;
/// Vector from center of body 2 to anchor point in world-space
Vector3 mU1World;
/// Vector from center of body 2 to anchor point in world-space
Vector3 mU2World;
/// Skew-Symmetric matrix for cross product with vector mU1World
Matrix3x3 mSkewSymmetricMatrixU1World;
/// Skew-Symmetric matrix for cross product with vector mU2World
Matrix3x3 mSkewSymmetricMatrixU2World;
/// Inverse mass matrix K=JM^-1J^-t of the constraint
Matrix3x3 mInverseMassMatrix;
/// Accumulated impulse
Vector3 mImpulse;
public :
@ -79,6 +97,12 @@ class BallAndSocketJoint : public Constraint {
/// Return the number of bytes used by the joint
virtual size_t getSizeInBytes() const;
/// Initialize before solving the constraint
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData);
/// Solve the constraint
virtual void solve(const ConstraintSolverData& constraintSolverData);
};
// Return the number of bytes used by the joint

15
src/constraint/Constraint.h
View File

@ -36,6 +36,9 @@ namespace reactphysics3d {
// Enumeration for the type of a constraint
enum ConstraintType {CONTACT, BALLSOCKETJOINT};
// Class declarations
struct ConstraintSolverData;
// Structure ConstraintInfo
/**
* This structure is used to gather the information needed to create a constraint.
@ -94,6 +97,12 @@ class Constraint {
/// Type of the constraint
const ConstraintType mType;
/// Body 1 index in the velocity array to solve the constraint
uint mIndexBody1;
/// Body 2 index in the velocity array to solve the constraint
uint mIndexBody2;
// -------------------- Methods -------------------- //
/// Private copy-constructor
@ -126,6 +135,12 @@ class Constraint {
/// Return the number of bytes used by the constraint
virtual size_t getSizeInBytes() const = 0;
/// Initialize before solving the constraint
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData) = 0;
/// Solve the constraint
virtual void solve(const ConstraintSolverData& constraintSolverData) = 0;
};
// Return the reference to the body 1

10
src/constraint/ContactPoint.cpp
View File

@ -50,3 +50,13 @@ ContactPoint::ContactPoint(const ContactPointInfo& contactInfo)
ContactPoint::~ContactPoint() {
}
// Initialize before solving the constraint
void ContactPoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
}
// Solve the constraint
void ContactPoint::solve(const ConstraintSolverData& constraintSolverData) {
}

6
src/constraint/ContactPoint.h
View File

@ -224,6 +224,12 @@ class ContactPoint : public Constraint {
/// Return the number of bytes used by the contact point
virtual size_t getSizeInBytes() const;
/// Initialize before solving the constraint
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData);
/// Solve the constraint
virtual void solve(const ConstraintSolverData& constraintSolverData);
#ifdef VISUAL_DEBUG
/// Draw the contact (for debugging)
void draw() const;

43
src/engine/ConstraintSolver.cpp
View File

@ -31,12 +31,14 @@ using namespace reactphysics3d;
// Constructor
ConstraintSolver::ConstraintSolver(std::set<Constraint*>& joints,
std::vector<Vector3>& constrainedLinearVelocities,
std::vector<Vector3>& constrainedAngularVelocities,
std::vector<Vector3>& linearVelocities,
std::vector<Vector3>& angularVelocities,
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
: mJoints(joints), mConstrainedLinearVelocities(constrainedLinearVelocities),
mConstrainedAngularVelocities(constrainedAngularVelocities),
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex) {
: mJoints(joints), mLinearVelocities(linearVelocities),
mAngularVelocities(angularVelocities),
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
mIsWarmStartingActive(false), mConstraintSolverData(linearVelocities,
angularVelocities, mapBodyToVelocityIndex){
}
@ -52,6 +54,28 @@ void ConstraintSolver::initialize(decimal dt) {
// Set the current time step
mTimeStep = dt;
// Initialize the constraint solver data used to initialize and solve the constraints
mConstraintSolverData.timeStep = mTimeStep;
mConstraintSolverData.isWarmStartingActive = mIsWarmStartingActive;
// For each joint
std::set<Constraint*>::iterator it;
for (it = mJoints.begin(); it != mJoints.end(); ++it) {
Constraint* joint = (*it);
// Get the rigid bodies of the joint
RigidBody* body1 = joint->getBody1();
RigidBody* body2 = joint->getBody2();
// Add the bodies to the set of constrained bodies
mConstraintBodies.insert(body1);
mConstraintBodies.insert(body2);
// Initialize the constraint before solving it
joint->initBeforeSolve(mConstraintSolverData);
}
}
// Solve the constraints
@ -59,4 +83,13 @@ void ConstraintSolver::solve() {
PROFILE("ConstraintSolver::solve()");
// For each joint
std::set<Constraint*>::iterator it;
for (it = mJoints.begin(); it != mJoints.end(); ++it) {
Constraint* joint = (*it);
// Solve the constraint
joint->solve(mConstraintSolverData);
}
}

60
src/engine/ConstraintSolver.h
View File

@ -35,6 +35,43 @@
namespace reactphysics3d {
// Structure ConstraintSolverData
/**
* This structure contains data from the constraint solver that are used to solve
* each joint constraint.
*/
struct ConstraintSolverData {
public :
/// Current time step of the simulation
decimal timeStep;
/// Reference to the bodies linear velocities
std::vector<Vector3>& linearVelocities;
/// Reference to the bodies angular velocities
std::vector<Vector3>& angularVelocities;
/// Reference to the map that associates rigid body to their index
/// in the constrained velocities array
const std::map<RigidBody*, uint>& mapBodyToConstrainedVelocityIndex;
/// True if warm starting of the solver is active
bool isWarmStartingActive;
/// Constructor
ConstraintSolverData(std::vector<Vector3>& refLinearVelocities,
std::vector<Vector3>& refAngularVelocities,
const std::map<RigidBody*, uint>& refMapBodyToConstrainedVelocityIndex)
:linearVelocities(refLinearVelocities),
angularVelocities(refAngularVelocities),
mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){
}
};
// Class ConstraintSolver
/**
* This class represents the constraint solver that is used to solve constraints between
@ -113,31 +150,38 @@ class ConstraintSolver {
/// Reference to all the joints of the world
std::set<Constraint*>& mJoints;
/// Constrained bodies
std::set<RigidBody*> mConstraintBodies;
/// Reference to the array of constrained linear velocities (state of the linear velocities
/// after solving the constraints)
std::vector<Vector3>& mConstrainedLinearVelocities;
std::vector<Vector3>& mLinearVelocities;
/// Reference to the array of constrained angular velocities (state of the angular velocities
/// after solving the constraints)
std::vector<Vector3>& mConstrainedAngularVelocities;
std::vector<Vector3>& mAngularVelocities;
/// Reference to the map of rigid body to their index in the constrained velocities array
/// Reference to the map that associates rigid body to their index in
/// the constrained velocities array
const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
/// Number of iterations of the contact solver
uint mNbIterations;
/// Current time step
decimal mTimeStep;
/// True if the warm starting of the solver is active
bool mIsWarmStartingActive;
/// Constraint solver data used to initialize and solve the constraints
ConstraintSolverData mConstraintSolverData;
public :
// -------------------- Methods -------------------- //
/// Constructor
ConstraintSolver(std::set<Constraint*>& joints,
std::vector<Vector3>& constrainedLinearVelocities,
std::vector<Vector3>& constrainedAngularVelocities,
std::vector<Vector3>& linearVelocities,
std::vector<Vector3>& angularVelocities,
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
/// Destructor

32
src/engine/ContactSolver.cpp
View File

@ -46,8 +46,8 @@ ContactSolver::ContactSolver(std::vector<ContactManifold*>& contactManifolds,
:mContactManifolds(contactManifolds),
mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL),
mContactConstraints(NULL),
mConstrainedLinearVelocities(constrainedLinearVelocities),
mConstrainedAngularVelocities(constrainedAngularVelocities),
mLinearVelocities(constrainedLinearVelocities),
mAngularVelocities(constrainedAngularVelocities),
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
mIsWarmStartingActive(true), mIsSplitImpulseActive(true),
mIsSolveFrictionAtContactManifoldCenterActive(true) {
@ -186,8 +186,8 @@ void ContactSolver::initialize(decimal dt) {
assert(mConstraintBodies.size() > 0);
assert(mMapBodyToConstrainedVelocityIndex.size() >= mConstraintBodies.size());
assert(mConstrainedLinearVelocities.size() >= mConstraintBodies.size());
assert(mConstrainedAngularVelocities.size() >= mConstraintBodies.size());
assert(mLinearVelocities.size() >= mConstraintBodies.size());
assert(mAngularVelocities.size() >= mConstraintBodies.size());
// Initialize the split impulse velocities
initializeSplitImpulseVelocities();
@ -231,10 +231,10 @@ void ContactSolver::initializeContactConstraints() {
}
// Get the velocities of the bodies
const Vector3& v1 = mConstrainedLinearVelocities[manifold.indexBody1];
const Vector3& w1 = mConstrainedAngularVelocities[manifold.indexBody1];
const Vector3& v2 = mConstrainedLinearVelocities[manifold.indexBody2];
const Vector3& w2 = mConstrainedAngularVelocities[manifold.indexBody2];
const Vector3& v1 = mLinearVelocities[manifold.indexBody1];
const Vector3& w1 = mAngularVelocities[manifold.indexBody1];
const Vector3& v2 = mLinearVelocities[manifold.indexBody2];
const Vector3& w2 = mAngularVelocities[manifold.indexBody2];
// For each contact point constraint
for (uint i=0; i<manifold.nbContacts; i++) {
@ -545,10 +545,10 @@ void ContactSolver::solve() {
decimal sumPenetrationImpulse = 0.0;
// Get the constrained velocities
const Vector3& v1 = mConstrainedLinearVelocities[contactManifold.indexBody1];
const Vector3& w1 = mConstrainedAngularVelocities[contactManifold.indexBody1];
const Vector3& v2 = mConstrainedLinearVelocities[contactManifold.indexBody2];
const Vector3& w2 = mConstrainedAngularVelocities[contactManifold.indexBody2];
const Vector3& v1 = mLinearVelocities[contactManifold.indexBody1];
const Vector3& w1 = mAngularVelocities[contactManifold.indexBody1];
const Vector3& v2 = mLinearVelocities[contactManifold.indexBody2];
const Vector3& w2 = mAngularVelocities[contactManifold.indexBody2];
for (uint i=0; i<contactManifold.nbContacts; i++) {
@ -789,15 +789,15 @@ void ContactSolver::applyImpulse(const Impulse& impulse,
// Update the velocities of the bodies by applying the impulse P
if (manifold.isBody1Moving) {
mConstrainedLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 *
mLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 *
impulse.linearImpulseBody1;
mConstrainedAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 *
mAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 *
impulse.angularImpulseBody1;
}
if (manifold.isBody2Moving) {
mConstrainedLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 *
mLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 *
impulse.linearImpulseBody2;
mConstrainedAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 *
mAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 *
impulse.angularImpulseBody2;
}
}

45
src/engine/ContactSolver.h
View File

@ -31,44 +31,13 @@
#include "../configuration.h"
#include "../constraint/Constraint.h"
#include "ContactManifold.h"
#include "Impulse.h"
#include <map>
#include <set>
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Declarations
class DynamicsWorld;
// Structure Impulse
/**
* Represents an impulse that we can apply to bodies in the contact or constraint solver.
*/
struct Impulse {
public:
/// Linear impulse applied to the first body
const Vector3 linearImpulseBody1;
/// Linear impulse applied to the second body
const Vector3 linearImpulseBody2;
/// Angular impulse applied to the first body
const Vector3 angularImpulseBody1;
/// Angular impulse applied to the second body
const Vector3 angularImpulseBody2;
/// Constructor
Impulse(const Vector3& linearImpulseBody1, const Vector3& angularImpulseBody1,
const Vector3& linearImpulseBody2, const Vector3& angularImpulseBody2)
: linearImpulseBody1(linearImpulseBody1), angularImpulseBody1(angularImpulseBody1),
linearImpulseBody2(linearImpulseBody2), angularImpulseBody2(angularImpulseBody2) {
}
};
// Class Contact Solver
/**
@ -85,7 +54,7 @@ struct Impulse {
* F_c = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a
* Lagrange multiplier. Therefore, finding the force F_c is equivalent to finding the Lagrange
* multiplier lambda.
*
* An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a
* body to change its velocity. The idea of the Sequential Impulse technique is to apply
* impulses to bodies of each constraints in order to keep the constraint satisfied.
@ -363,13 +332,11 @@ class ContactSolver {
/// Constrained bodies
std::set<RigidBody*> mConstraintBodies;
/// Reference to the array of constrained linear velocities (state of the linear velocities
/// after solving the constraints)
std::vector<Vector3>& mConstrainedLinearVelocities;
/// Reference to the array of linear velocities
std::vector<Vector3>& mLinearVelocities;
/// Reference to the array of constrained angular velocities (state of the angular velocities
/// after solving the constraints)
std::vector<Vector3>& mConstrainedAngularVelocities;
/// Reference to the array of angular velocities
std::vector<Vector3>& mAngularVelocities;
/// Reference to the map of rigid body to their index in the constrained velocities array
const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;

65
src/engine/Impulse.h Normal file
View File

@ -0,0 +1,65 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_IMPULSE_H
#define REACTPHYSICS3D_IMPULSE_H
// Libraries
#include "../mathematics/mathematics.h"
namespace reactphysics3d {
// Structure Impulse
/**
* Represents an impulse that we can apply to bodies in the contact or constraint solver.
*/
struct Impulse {
public:
/// Linear impulse applied to the first body
const Vector3 linearImpulseBody1;
/// Linear impulse applied to the second body
const Vector3 linearImpulseBody2;
/// Angular impulse applied to the first body
const Vector3 angularImpulseBody1;
/// Angular impulse applied to the second body
const Vector3 angularImpulseBody2;
/// Constructor
Impulse(const Vector3& linearImpulseBody1, const Vector3& angularImpulseBody1,
const Vector3& linearImpulseBody2, const Vector3& angularImpulseBody2)
: linearImpulseBody1(linearImpulseBody1), angularImpulseBody1(angularImpulseBody1),
linearImpulseBody2(linearImpulseBody2), angularImpulseBody2(angularImpulseBody2) {
}
};
}
#endif

10
src/mathematics/Matrix3x3.h
View File

@ -105,6 +105,10 @@ class Matrix3x3 {
/// Return the 3x3 identity matrix
static Matrix3x3 identity();
/// Return a skew-symmetric matrix using a given vector that can be used
/// to compute cross product with another vector using matrix multiplication
static Matrix3x3 computeSkewSymmetricMatrixForCrossProduct(const Vector3& vector);
/// Overloaded operator for addition
friend Matrix3x3 operator+(const Matrix3x3& matrix1, const Matrix3x3& matrix2);
@ -215,6 +219,12 @@ inline Matrix3x3 Matrix3x3::identity() {
return Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
}
// Return a skew-symmetric matrix using a given vector that can be used
// to compute cross product with another vector using matrix multiplication
inline Matrix3x3 Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(const Vector3& vector) {
return Matrix3x3(0, -vector.z, vector.y, vector.z, 0, -vector.x, -vector.y, vector.x, 0);
}
// Return the matrix with absolute values
inline Matrix3x3 Matrix3x3::getAbsoluteMatrix() const {
return Matrix3x3(fabs(mRows[0][0]), fabs(mRows[0][1]), fabs(mRows[0][2]),

4
src/mathematics/Quaternion.h
View File

@ -131,7 +131,7 @@ struct Quaternion {
Quaternion operator*(const Quaternion& quaternion) const;
/// Overloaded operator for the multiplication with a vector
Vector3 operator*(const Vector3& point);
Vector3 operator*(const Vector3& point) const;
/// Overloaded operator for assignment
Quaternion& operator=(const Quaternion& quaternion);
@ -250,7 +250,7 @@ inline Quaternion Quaternion::operator*(const Quaternion& quaternion) const {
// Overloaded operator for the multiplication with a vector.
/// This methods rotates a point given the rotation of a quaternion.
inline Vector3 Quaternion::operator*(const Vector3& point) {
inline Vector3 Quaternion::operator*(const Vector3& point) const {
Quaternion p(point.x, point.y, point.z, 0.0);
return (((*this) * p) * getConjugate()).getVectorV();
}

4
src/mathematics/Transform.h
View File

@ -92,7 +92,7 @@ class Transform {
void getOpenGLMatrix(decimal* openglMatrix) const;
/// Return the inverse of the transform
Transform inverse() const;
Transform getInverse() const;
/// Return an interpolated transform
static Transform interpolateTransforms(const Transform& oldTransform,
@ -167,7 +167,7 @@ inline void Transform::getOpenGLMatrix(decimal* openglMatrix) const {
}
// Return the inverse of the transform
inline Transform Transform::inverse() const {
inline Transform Transform::getInverse() const {
const Quaternion& invQuaternion = mOrientation.getInverse();
Matrix3x3 invMatrix = invQuaternion.getMatrix();
return Transform(invMatrix * (-mPosition), invQuaternion);

1
src/reactphysics3d.h
View File

@ -47,6 +47,7 @@
#include "collision/shapes/ConeShape.h"
#include "collision/shapes/CylinderShape.h"
#include "collision/shapes/AABB.h"
#include "constraint/BallAndSocketJoint.h"
/// Alias to the ReactPhysics3D namespace
namespace rp3d = reactphysics3d;

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

@ -196,6 +196,15 @@ class TestMatrix3x3 : public Test {
test(matrix.getAbsoluteMatrix() == Matrix3x3(24, 64, 253, 35, 52, 72, 21, 35, 363));
Matrix3x3 absoluteMatrix = matrix2.getAbsoluteMatrix();
test(absoluteMatrix == Matrix3x3(2, 3, 4, 5, 6, 7, 8, 9, 10));
// Test method that computes skew-symmetric matrix for cross product
Vector3 vector1(3, -5, 6);
Vector3 vector2(73, 42, 26);
Matrix3x3 skewMatrix = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(vector1);
test(skewMatrix == Matrix3x3(0, -6, -5, 6, 0, -3, 5, 3, 0));
Vector3 crossProduct1 = vector1.cross(vector2);
Vector3 crossProduct2 = skewMatrix * vector2;
test(crossProduct1 == crossProduct2);
}
/// Test the operators

2
test/tests/mathematics/TestTransform.h
View File

@ -114,7 +114,7 @@ class TestTransform : public Test {
/// Test the inverse
void testInverse() {
Transform inverseTransform = mTransform1.inverse();
Transform inverseTransform = mTransform1.getInverse();
Vector3 vector(2, 3, 4);
Vector3 tempVector = mTransform1 * vector;
Vector3 tempVector2 = inverseTransform * tempVector;