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

Start to implement the joints

Browse Source
This commit is contained in:
Daniel Chappuis 2013-04-22 23:32:52 +02:00
parent f692f7ef12
commit 471e4f7afc
10 changed files with 345 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
src/constraint/BallAndSocketJoint.cpp Normal file
View File

@ -0,0 +1,42 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "BallAndSocketJoint.h"
using namespace reactphysics3d;
// Constructor
BallAndSocketJoint::BallAndSocketJoint(RigidBody* const body1, RigidBody* const body2,
bool active, ConstraintType type)
: Constraint(body1, body2, active, type){
}
// Destructor
BallAndSocketJoint::~BallAndSocketJoint() {
}

59
src/constraint/BallAndSocketJoint.h Normal file
View File

@ -0,0 +1,59 @@
/********************************************************************************
* 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_BALL_AND_SOCKET_JOINT_H
#define REACTPHYSICS3D_BALL_AND_SOCKET_JOINT_H
// Libraries
#include "Constraint.h"
namespace reactphysics3d {
// Class BallAndSocketJoint
/**
* This class represents a ball-and-socket joint that allows arbitrary rotation
* between two bodies.
*/
class BallAndSocketJoint : public Constraint {
private :
// -------------------- Attributes -------------------- //
public :
// -------------------- Methods -------------------- //
/// Constructor
BallAndSocketJoint(RigidBody* const body1, RigidBody* const body2,
bool active, ConstraintType type);
/// Destructor
virtual ~BallAndSocketJoint();
};
}
#endif

11
src/constraint/Constraint.cpp
View File

@ -30,14 +30,9 @@ using namespace reactphysics3d;
// Constructor
Constraint::Constraint(RigidBody* const body1, RigidBody* const body2,
uint nbConstraints, bool active, ConstraintType type)
:mBody1(body1), mBody2(body2), mActive(active),
mNbConstraints(nbConstraints), mType(type) {
// Initialize the cached lambda values
for (uint i=0; i<nbConstraints; i++) {
mCachedLambdas.push_back(0.0);
}
bool active, ConstraintType type)
:mBody1(body1), mBody2(body2), mActive(active), mType(type) {
}
// Destructor

40
src/constraint/Constraint.h
View File

@ -34,7 +34,7 @@
namespace reactphysics3d {
// Enumeration for the type of a constraint
enum ConstraintType {CONTACT};
enum ConstraintType {CONTACT, BALLSOCKETJOINT};
// Class Constraint
/**
@ -58,16 +58,9 @@ class Constraint {
/// True if the constraint is active
bool mActive;
/// Number mathematical constraints associated with this Constraint
uint mNbConstraints;
/// Type of the constraint
const ConstraintType mType;
/// Cached lambda values of each mathematical constraint for
/// more precise initializaton of LCP solver
std::vector<decimal> mCachedLambdas;
// -------------------- Methods -------------------- //
/// Private copy-constructor
@ -81,7 +74,7 @@ class Constraint {
// -------------------- Methods -------------------- //
/// Constructor
Constraint(RigidBody* const body1, RigidBody* const body2, uint nbConstraints,
Constraint(RigidBody* const body1, RigidBody* const body2,
bool active, ConstraintType type);
/// Destructor
@ -98,15 +91,6 @@ class Constraint {
/// Return the type of the constraint
ConstraintType getType() const;
/// Return the number of mathematical constraints
unsigned int getNbConstraints() const;
/// Get one cached lambda value
decimal getCachedLambda(uint index) const;
/// Set on cached lambda value
void setCachedLambda(uint index, decimal lambda);
};
// Return the reference to the body 1
@ -127,25 +111,7 @@ inline bool Constraint::isActive() const {
// Return the type of the constraint
inline ConstraintType Constraint::getType() const {
return mType;
}
// Return the number auxiliary constraints
inline uint Constraint::getNbConstraints() const {
return mNbConstraints;
}
// Get one previous lambda value
inline decimal Constraint::getCachedLambda(uint index) const {
assert(index < mNbConstraints);
return mCachedLambdas[index];
}
// Set on cached lambda value
inline void Constraint::setCachedLambda(uint index, decimal lambda) {
assert(index < mNbConstraints);
mCachedLambdas[index] = lambda;
}
}
}

9
src/constraint/ContactPoint.cpp
View File

@ -32,13 +32,16 @@ using namespace std;
// Constructor
ContactPoint::ContactPoint(RigidBody* const body1, RigidBody* const body2,
const ContactInfo* contactInfo)
: Constraint(body1, body2, 3, true, CONTACT), mNormal(contactInfo->normal),
: Constraint(body1, body2, true, CONTACT), mNormal(contactInfo->normal),
mPenetrationDepth(contactInfo->penetrationDepth),
mLocalPointOnBody1(contactInfo->localPoint1),
mLocalPointOnBody2(contactInfo->localPoint2),
mWorldPointOnBody1(body1->getTransform() * contactInfo->localPoint1),
mWorldPointOnBody2(body2->getTransform() * contactInfo->localPoint2),
mIsRestingContact(false), mFrictionVectors(2, Vector3(0, 0, 0)) {
mWorldPointOnBody2(body2->getTransform() * contactInfo->localPoint2),
mIsRestingContact(false) {
mFrictionVectors[0] = Vector3(0, 0, 0);
mFrictionVectors[1] = Vector3(0, 0, 0);
assert(mPenetrationDepth > 0.0);

59
src/constraint/ContactPoint.h
View File

@ -84,7 +84,16 @@ class ContactPoint : public Constraint {
bool mIsRestingContact;
/// Two orthogonal vectors that span the tangential friction plane
std::vector<Vector3> mFrictionVectors;
Vector3 mFrictionVectors[2];
/// Cached penetration impulse
decimal mPenetrationImpulse;
/// Cached first friction impulse
decimal mFrictionImpulse1;
/// Cached second friction impulse
decimal mFrictionImpulse2;
// -------------------- Methods -------------------- //
@ -122,6 +131,24 @@ class ContactPoint : public Constraint {
/// Return the contact world point on body 2
Vector3 getWorldPointOnBody2() const;
/// Return the cached penetration impulse
decimal getPenetrationImpulse() const;
/// Return the cached first friction impulse
decimal getFrictionImpulse1() const;
/// Return the cached second friction impulse
decimal getFrictionImpulse2() const;
/// Set the cached penetration impulse
void setPenetrationImpulse(decimal impulse);
/// Set the first cached friction impulse
void setFrictionImpulse1(decimal impulse);
/// Set the second cached friction impulse
void setFrictionImpulse2(decimal impulse);
/// Set the contact world point on body 1
void setWorldPointOnBody1(const Vector3& worldPoint);
@ -185,6 +212,36 @@ inline Vector3 ContactPoint::getWorldPointOnBody2() const {
return mWorldPointOnBody2;
}
// Return the cached penetration impulse
inline decimal ContactPoint::getPenetrationImpulse() const {
return mPenetrationImpulse;
}
// Return the cached first friction impulse
inline decimal ContactPoint::getFrictionImpulse1() const {
return mFrictionImpulse1;
}
// Return the cached second friction impulse
inline decimal ContactPoint::getFrictionImpulse2() const {
return mFrictionImpulse2;
}
// Set the cached penetration impulse
inline void ContactPoint::setPenetrationImpulse(decimal impulse) {
mPenetrationImpulse = impulse;
}
// Set the first cached friction impulse
inline void ContactPoint::setFrictionImpulse1(decimal impulse) {
mFrictionImpulse1 = impulse;
}
// Set the second cached friction impulse
inline void ContactPoint::setFrictionImpulse2(decimal impulse) {
mFrictionImpulse2 = impulse;
}
// Set the contact world point on body 1
inline void ContactPoint::setWorldPointOnBody1(const Vector3& worldPoint) {
mWorldPointOnBody1 = worldPoint;

39
src/engine/ConstraintSolver.cpp Normal file
View File

@ -0,0 +1,39 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "ConstraintSolver.h"
using namespace reactphysics3d;
// Constructor
ConstraintSolver::ConstraintSolver() {
}
// Destructor
ConstraintSolver::~ConstraintSolver() {
}

128
src/engine/ConstraintSolver.h Normal file
View File

@ -0,0 +1,128 @@
/********************************************************************************
* 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_CONSTRAINT_SOLVER_H
#define REACTPHYSICS3D_CONSTRAINT_SOLVER_H
// Libraries
namespace reactphysics3d {
// Class ConstraintSolver
/**
* This class represents the constraint solver that is used to solve constraints between
* the rigid bodies. The constraint solver is based on the "Sequential Impulse" technique
* described by Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list).
*
* A constraint between two bodies is represented by a function C(x) which is equal to zero
* when the constraint is satisfied. The condition C(x)=0 describes a valid position and the
* condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is
* the Jacobian matrix of the constraint, v is a vector that contains the velocity of both
* bodies and b is the constraint bias. We are looking for a force F_c that will act on the
* bodies to keep the constraint satisfied. Note that from the virtual work principle, we have
* 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.
*
* --- Step 1 ---
*
* First, we integrate the applied force F_a acting of each rigid body (like gravity, ...) and
* we obtain some new velocities v2' that tends to violate the constraints.
*
* v2' = v1 + dt * M^-1 * F_a
*
* where M is a matrix that contains mass and inertia tensor information.
*
* --- Step 2 ---
*
* During the second step, we iterate over all the constraints for a certain number of
* iterations and for each constraint we compute the impulse to apply to the bodies needed
* so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that
* M * deltaV = P_c where M is the mass of the body, deltaV is the difference of velocity and
* P_c is the constraint impulse to apply to the body. Therefore, we have
* v2 = v2' + M^-1 * P_c. For each constraint, we can compute the Lagrange multiplier lambda
* using : lambda = -m_c (Jv2' + b) where m_c = 1 / (J * M^-1 * J^t). Now that we have the
* Lagrange multiplier lambda, we can compute the impulse P_c = J^t * lambda * dt to apply to
* the bodies to satisfy the constraint.
*
* --- Step 3 ---
*
* In the third step, we integrate the new position x2 of the bodies using the new velocities
* v2 computed in the second step with : x2 = x1 + dt * v2.
*
* Note that in the following code (as it is also explained in the slides from Erin Catto),
* the value lambda is not only the lagrange multiplier but is the multiplication of the
* Lagrange multiplier with the timestep dt. Therefore, in the following code, when we use
* lambda, we mean (lambda * dt).
*
* We are using the accumulated impulse technique that is also described in the slides from
* Erin Catto.
*
* We are also using warm starting. The idea is to warm start the solver at the beginning of
* each step by applying the last impulstes for the constraints that we already existing at the
* previous step. This allows the iterative solver to converge faster towards the solution.
*
* For contact constraints, we are also using split impulses so that the position correction
* that uses Baumgarte stabilization does not change the momentum of the bodies.
*
* There are two ways to apply the friction constraints. Either the friction constraints are
* applied at each contact point or they are applied only at the center of the contact manifold
* between two bodies. If we solve the friction constraints at each contact point, we need
* two constraints (two tangential friction directions) and if we solve the friction
* constraints at the center of the contact manifold, we need two constraints for tangential
* friction but also another twist friction constraint to prevent spin of the body around the
* contact manifold center.
*/
class ConstraintSolver {
private :
// -------------------- Attributes -------------------- //
/// Number of iterations of the contact solver
uint mNbIterations;
/// Current time step
decimal mTimeStep;
public :
// -------------------- Methods -------------------- //
/// Constructor
ConstraintSolver();
/// Destructor
~ConstraintSolver();
};
}
#endif

12
src/engine/ContactSolver.cpp
View File

@ -307,9 +307,9 @@ void ContactSolver::initializeContactConstraints() {
if (mIsWarmStartingActive) {
// Get the cached accumulated impulses from the previous step
contactPoint.penetrationImpulse = externalContact->getCachedLambda(0);
contactPoint.friction1Impulse = externalContact->getCachedLambda(1);
contactPoint.friction2Impulse = externalContact->getCachedLambda(2);
contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse();
contactPoint.friction1Impulse = externalContact->getFrictionImpulse1();
contactPoint.friction2Impulse = externalContact->getFrictionImpulse2();
}
// Initialize the split impulses to zero
@ -785,9 +785,9 @@ void ContactSolver::storeImpulses() {
ContactPointSolver& contactPoint = manifold.contacts[i];
contactPoint.externalContact->setCachedLambda(0, contactPoint.penetrationImpulse);
contactPoint.externalContact->setCachedLambda(1, contactPoint.friction1Impulse);
contactPoint.externalContact->setCachedLambda(2, contactPoint.friction2Impulse);
contactPoint.externalContact->setPenetrationImpulse(contactPoint.penetrationImpulse);
contactPoint.externalContact->setFrictionImpulse1(contactPoint.friction1Impulse);
contactPoint.externalContact->setFrictionImpulse2(contactPoint.friction2Impulse);
contactPoint.externalContact->setFrictionVector1(contactPoint.frictionVector1);
contactPoint.externalContact->setFrictionVector2(contactPoint.frictionVector2);

2
src/engine/ContactSolver.h
View File

@ -345,7 +345,7 @@ class ContactSolver {
/// Reference to the world
DynamicsWorld& mWorld;
/// Number of iterations of the constraints solver
/// Number of iterations of the contact solver
uint mNbIterations;
/// Split linear velocities for the position contact solver (split impulse)