Merge branch 'sequentialimpulse2' into develop
This commit is contained in:
commit
33a084c816
|
@ -35,9 +35,9 @@ using namespace reactphysics3d;
|
||||||
CollisionShape *collisionShape, bodyindex id)
|
CollisionShape *collisionShape, bodyindex id)
|
||||||
: CollisionBody(transform, collisionShape, id), mInertiaTensorLocal(inertiaTensorLocal),
|
: CollisionBody(transform, collisionShape, id), mInertiaTensorLocal(inertiaTensorLocal),
|
||||||
mMass(mass), mInertiaTensorLocalInverse(inertiaTensorLocal.getInverse()),
|
mMass(mass), mInertiaTensorLocalInverse(inertiaTensorLocal.getInverse()),
|
||||||
mMassInverse(1.0/mass) {
|
mMassInverse(decimal(1.0) / mass), mFrictionCoefficient(DEFAULT_FRICTION_COEFFICIENT) {
|
||||||
|
|
||||||
mRestitution = 1.0;
|
mRestitution = decimal(1.0);
|
||||||
|
|
||||||
// Set the body pointer of the AABB and the collision shape
|
// Set the body pointer of the AABB and the collision shape
|
||||||
mAabb->setBodyPointer(this);
|
mAabb->setBodyPointer(this);
|
||||||
|
|
|
@ -77,6 +77,8 @@ class RigidBody : public CollisionBody {
|
||||||
// Coefficient of restitution (between 0 and 1) where 1 is for a very bouncy body
|
// Coefficient of restitution (between 0 and 1) where 1 is for a very bouncy body
|
||||||
decimal mRestitution;
|
decimal mRestitution;
|
||||||
|
|
||||||
|
// Friction coefficient
|
||||||
|
decimal mFrictionCoefficient;
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
|
||||||
|
@ -153,6 +155,12 @@ class RigidBody : public CollisionBody {
|
||||||
|
|
||||||
// Set the restitution coefficient
|
// Set the restitution coefficient
|
||||||
void setRestitution(decimal restitution) throw(std::invalid_argument);
|
void setRestitution(decimal restitution) throw(std::invalid_argument);
|
||||||
|
|
||||||
|
// Get the friction coefficient
|
||||||
|
decimal getFrictionCoefficient() const;
|
||||||
|
|
||||||
|
// Set the friction coefficient
|
||||||
|
void setFrictionCoefficient(decimal frictionCoefficient);
|
||||||
};
|
};
|
||||||
|
|
||||||
// Method that return the mass of the body
|
// Method that return the mass of the body
|
||||||
|
@ -277,6 +285,16 @@ inline void RigidBody::setRestitution(decimal restitution) throw(std::invalid_ar
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Get the friction coefficient
|
||||||
|
inline decimal RigidBody::getFrictionCoefficient() const {
|
||||||
|
return mFrictionCoefficient;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set the friction coefficient
|
||||||
|
inline void RigidBody::setFrictionCoefficient(decimal frictionCoefficient) {
|
||||||
|
mFrictionCoefficient = frictionCoefficient;
|
||||||
|
}
|
||||||
|
|
||||||
} // End of the ReactPhyscis3D namespace
|
} // End of the ReactPhyscis3D namespace
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -105,6 +105,7 @@ inline int EdgeEPA::getIndex() const {
|
||||||
inline EdgeEPA& EdgeEPA::operator=(const EdgeEPA& edge) {
|
inline EdgeEPA& EdgeEPA::operator=(const EdgeEPA& edge) {
|
||||||
mOwnerTriangle = edge.mOwnerTriangle;
|
mOwnerTriangle = edge.mOwnerTriangle;
|
||||||
mIndex = edge.mIndex;
|
mIndex = edge.mIndex;
|
||||||
|
return *this;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the index of the next counter-clockwise edge of the ownver triangle
|
// Return the index of the next counter-clockwise edge of the ownver triangle
|
||||||
|
|
|
@ -26,7 +26,7 @@
|
||||||
// Libraries
|
// Libraries
|
||||||
#include "GJKAlgorithm.h"
|
#include "GJKAlgorithm.h"
|
||||||
#include "Simplex.h"
|
#include "Simplex.h"
|
||||||
#include "../../../constraint/Contact.h"
|
#include "../../../constraint/ContactPoint.h"
|
||||||
#include "../../../configuration.h"
|
#include "../../../configuration.h"
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
|
|
|
@ -37,7 +37,7 @@
|
||||||
namespace reactphysics3d {
|
namespace reactphysics3d {
|
||||||
|
|
||||||
// Constants
|
// Constants
|
||||||
const decimal REL_ERROR = 1.0e-3;
|
const decimal REL_ERROR = decimal(1.0e-3);
|
||||||
const decimal REL_ERROR_SQUARE = REL_ERROR * REL_ERROR;
|
const decimal REL_ERROR_SQUARE = REL_ERROR * REL_ERROR;
|
||||||
|
|
||||||
/* -------------------------------------------------------------------
|
/* -------------------------------------------------------------------
|
||||||
|
|
|
@ -57,7 +57,7 @@ BoxShape::~BoxShape() {
|
||||||
|
|
||||||
// Return the local inertia tensor of the collision shape
|
// Return the local inertia tensor of the collision shape
|
||||||
void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
|
void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
|
||||||
decimal factor = (1.0 / 3.0) * mass;
|
decimal factor = (decimal(1.0) / decimal(3.0)) * mass;
|
||||||
decimal xSquare = mExtent.getX() * mExtent.getX();
|
decimal xSquare = mExtent.getX() * mExtent.getX();
|
||||||
decimal ySquare = mExtent.getY() * mExtent.getY();
|
decimal ySquare = mExtent.getY() * mExtent.getY();
|
||||||
decimal zSquare = mExtent.getZ() * mExtent.getZ();
|
decimal zSquare = mExtent.getZ() * mExtent.getZ();
|
||||||
|
|
|
@ -45,7 +45,7 @@ using namespace reactphysics3d;
|
||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
ConeShape::ConeShape(decimal radius, decimal height)
|
ConeShape::ConeShape(decimal radius, decimal height)
|
||||||
: CollisionShape(CONE), mRadius(radius), mHalfHeight(height/2.0) {
|
: CollisionShape(CONE), mRadius(radius), mHalfHeight(height / decimal(2.0)) {
|
||||||
assert(radius > 0.0);
|
assert(radius > 0.0);
|
||||||
assert(mHalfHeight > 0.0);
|
assert(mHalfHeight > 0.0);
|
||||||
|
|
||||||
|
|
|
@ -118,12 +118,12 @@ inline void ConeShape::setRadius(decimal radius) {
|
||||||
|
|
||||||
// Return the height
|
// Return the height
|
||||||
inline decimal ConeShape::getHeight() const {
|
inline decimal ConeShape::getHeight() const {
|
||||||
return 2.0 * mHalfHeight;
|
return decimal(2.0) * mHalfHeight;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Set the height
|
// Set the height
|
||||||
inline void ConeShape::setHeight(decimal height) {
|
inline void ConeShape::setHeight(decimal height) {
|
||||||
mHalfHeight = height / 2.0;
|
mHalfHeight = height * decimal(0.5);
|
||||||
|
|
||||||
// Update the sine of the semi-angle at the apex point
|
// Update the sine of the semi-angle at the apex point
|
||||||
mSinTheta = mRadius / (sqrt(mRadius * mRadius + height * height));
|
mSinTheta = mRadius / (sqrt(mRadius * mRadius + height * height));
|
||||||
|
@ -137,8 +137,10 @@ inline Vector3 ConeShape::getLocalExtents(decimal margin) const {
|
||||||
// Return the local inertia tensor of the collision shape
|
// Return the local inertia tensor of the collision shape
|
||||||
inline void ConeShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
|
inline void ConeShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
|
||||||
decimal rSquare = mRadius * mRadius;
|
decimal rSquare = mRadius * mRadius;
|
||||||
decimal diagXZ = 0.15 * mass * (rSquare + mHalfHeight);
|
decimal diagXZ = decimal(0.15) * mass * (rSquare + mHalfHeight);
|
||||||
tensor.setAllValues(diagXZ, 0.0, 0.0, 0.0, 0.3 * mass * rSquare, 0.0, 0.0, 0.0, diagXZ);
|
tensor.setAllValues(diagXZ, 0.0, 0.0,
|
||||||
|
0.0, decimal(0.3) * mass * rSquare,
|
||||||
|
0.0, 0.0, 0.0, diagXZ);
|
||||||
}
|
}
|
||||||
|
|
||||||
}; // End of the ReactPhysics3D namespace
|
}; // End of the ReactPhysics3D namespace
|
||||||
|
|
|
@ -116,7 +116,7 @@ inline decimal CylinderShape::getHeight() const {
|
||||||
|
|
||||||
// Set the height
|
// Set the height
|
||||||
inline void CylinderShape::setHeight(decimal height) {
|
inline void CylinderShape::setHeight(decimal height) {
|
||||||
this->mHalfHeight = height / 2.0;
|
mHalfHeight = height * decimal(0.5);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the local extents in x,y and z direction
|
// Return the local extents in x,y and z direction
|
||||||
|
@ -126,9 +126,11 @@ inline Vector3 CylinderShape::getLocalExtents(decimal margin) const {
|
||||||
|
|
||||||
// Return the local inertia tensor of the cylinder
|
// Return the local inertia tensor of the cylinder
|
||||||
inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
|
inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
|
||||||
decimal height = 2.0 * mHalfHeight;
|
decimal height = decimal(2.0) * mHalfHeight;
|
||||||
decimal diag = (1.0 / 12.0) * mass * (3 * mRadius * mRadius + height * height);
|
decimal diag = (decimal(1.0) / decimal(12.0)) * mass * (3 * mRadius * mRadius + height * height);
|
||||||
tensor.setAllValues(diag, 0.0, 0.0, 0.0, 0.5 * mass * mRadius * mRadius, 0.0, 0.0, 0.0, diag);
|
tensor.setAllValues(diag, 0.0, 0.0, 0.0,
|
||||||
|
decimal(0.5) * mass * mRadius * mRadius, 0.0,
|
||||||
|
0.0, 0.0, diag);
|
||||||
}
|
}
|
||||||
|
|
||||||
}; // End of the ReactPhysics3D namespace
|
}; // End of the ReactPhysics3D namespace
|
||||||
|
|
|
@ -121,8 +121,10 @@ inline Vector3 SphereShape::getLocalExtents(decimal margin) const {
|
||||||
|
|
||||||
// Return the local inertia tensor of the sphere
|
// Return the local inertia tensor of the sphere
|
||||||
inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
|
inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
|
||||||
decimal diag = 0.4 * mass * mRadius * mRadius;
|
decimal diag = decimal(0.4) * mass * mRadius * mRadius;
|
||||||
tensor.setAllValues(diag, 0.0, 0.0, 0.0, diag, 0.0, 0.0, 0.0, diag);
|
tensor.setAllValues(diag, 0.0, 0.0,
|
||||||
|
0.0, diag, 0.0,
|
||||||
|
0.0, 0.0, diag);
|
||||||
}
|
}
|
||||||
|
|
||||||
}; // End of the ReactPhysics3D namespace
|
}; // End of the ReactPhysics3D namespace
|
||||||
|
|
|
@ -53,52 +53,36 @@ typedef std::pair<bodyindex, bodyindex> bodyindexpair;
|
||||||
|
|
||||||
// ------------------- Constants ------------------- //
|
// ------------------- Constants ------------------- //
|
||||||
|
|
||||||
const reactphysics3d::decimal DECIMAL_SMALLEST = - std::numeric_limits<decimal>::max();
|
const decimal DECIMAL_SMALLEST = - std::numeric_limits<decimal>::max();
|
||||||
|
|
||||||
// Maximum decimal value
|
// Maximum decimal value
|
||||||
const reactphysics3d::decimal DECIMAL_LARGEST = std::numeric_limits<decimal>::max();
|
const decimal DECIMAL_LARGEST = std::numeric_limits<decimal>::max();
|
||||||
|
|
||||||
// Machine epsilon
|
// Machine epsilon
|
||||||
const reactphysics3d::decimal MACHINE_EPSILON = std::numeric_limits<decimal>::epsilon();
|
const decimal MACHINE_EPSILON = std::numeric_limits<decimal>::epsilon();
|
||||||
|
|
||||||
// Infinity
|
|
||||||
const reactphysics3d::decimal DECIMAL_INFINITY = std::numeric_limits<decimal>::infinity();
|
|
||||||
|
|
||||||
// Pi constant
|
// Pi constant
|
||||||
const reactphysics3d::decimal PI = 3.14159265;
|
const decimal PI = decimal(3.14159265);
|
||||||
|
|
||||||
// Default internal constant timestep in seconds
|
// Default internal constant timestep in seconds
|
||||||
const reactphysics3d::decimal DEFAULT_TIMESTEP = 1.0 / 60.0;
|
const decimal DEFAULT_TIMESTEP = decimal(1.0 / 60.0);
|
||||||
|
|
||||||
|
// Default friction coefficient for a rigid body
|
||||||
|
const decimal DEFAULT_FRICTION_COEFFICIENT = decimal(0.3);
|
||||||
|
|
||||||
// True if the deactivation (sleeping) of inactive bodies is enabled
|
// True if the deactivation (sleeping) of inactive bodies is enabled
|
||||||
const bool DEACTIVATION_ENABLED = true;
|
const bool DEACTIVATION_ENABLED = true;
|
||||||
|
|
||||||
// // Object margin for collision detection in cm (For GJK-EPA Algorithm)
|
// // Object margin for collision detection in cm (For GJK-EPA Algorithm)
|
||||||
const reactphysics3d::decimal OBJECT_MARGIN = 0.04;
|
const decimal OBJECT_MARGIN = decimal(0.04);
|
||||||
|
|
||||||
// Friction coefficient
|
// Distance threshold for two contact points for a valid persistent contact (in meters)
|
||||||
const reactphysics3d::decimal FRICTION_COEFFICIENT = 0.4;
|
const decimal PERSISTENT_CONTACT_DIST_THRESHOLD = decimal(0.03);
|
||||||
|
|
||||||
// Distance threshold for two contact points for a valid persistent contact
|
const decimal RESTITUTION_VELOCITY_THRESHOLD = decimal(1.0);
|
||||||
const reactphysics3d::decimal PERSISTENT_CONTACT_DIST_THRESHOLD = 0.02;
|
|
||||||
|
|
||||||
// Maximum number of bodies
|
|
||||||
const int NB_MAX_BODIES = 100000;
|
|
||||||
|
|
||||||
// Maximum number of constraints
|
|
||||||
const int NB_MAX_CONSTRAINTS = 100000;
|
|
||||||
|
|
||||||
// Number of iterations when solving a LCP problem
|
// Number of iterations when solving a LCP problem
|
||||||
const uint DEFAULT_LCP_ITERATIONS = 15;
|
const uint DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS = 15;
|
||||||
|
|
||||||
// Number of iterations when solving a LCP problem for error correction
|
|
||||||
const uint DEFAULT_LCP_ITERATIONS_ERROR_CORRECTION = 5;
|
|
||||||
|
|
||||||
// True if the error correction projection (first order world) is active in the constraint solver
|
|
||||||
const bool ERROR_CORRECTION_PROJECTION_ENABLED = true;
|
|
||||||
|
|
||||||
// Contacts with penetration depth (in meters) larger that this use error correction with projection
|
|
||||||
const reactphysics3d::decimal PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION = 0.20;
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -100,21 +100,6 @@ class Constraint {
|
||||||
// Return the type of the constraint
|
// Return the type of the constraint
|
||||||
ConstraintType getType() const;
|
ConstraintType getType() const;
|
||||||
|
|
||||||
// Compute the jacobian matrix for all mathematical constraints
|
|
||||||
virtual void computeJacobian(int noConstraint,
|
|
||||||
decimal J_sp[NB_MAX_CONSTRAINTS][2*6]) const=0;
|
|
||||||
|
|
||||||
// Compute the lowerbounds values for all the mathematical constraints
|
|
||||||
virtual void computeLowerBound(int noConstraint,
|
|
||||||
decimal lowerBounds[NB_MAX_CONSTRAINTS]) const=0;
|
|
||||||
|
|
||||||
// Compute the upperbounds values for all the mathematical constraints
|
|
||||||
virtual void computeUpperBound(int noConstraint,
|
|
||||||
decimal upperBounds[NB_MAX_CONSTRAINTS]) const=0;
|
|
||||||
|
|
||||||
// Compute the error values for all the mathematical constraints
|
|
||||||
virtual void computeErrorValue(int noConstraint, decimal errorValues[]) const=0;
|
|
||||||
|
|
||||||
// Return the number of mathematical constraints
|
// Return the number of mathematical constraints
|
||||||
unsigned int getNbConstraints() const;
|
unsigned int getNbConstraints() const;
|
||||||
|
|
||||||
|
|
|
@ -1,176 +0,0 @@
|
||||||
/********************************************************************************
|
|
||||||
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
|
|
||||||
* Copyright (c) 2010-2012 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 "Contact.h"
|
|
||||||
|
|
||||||
using namespace reactphysics3d;
|
|
||||||
using namespace std;
|
|
||||||
|
|
||||||
// Constructor
|
|
||||||
Contact::Contact(RigidBody* const body1, RigidBody* const body2, const ContactInfo* contactInfo)
|
|
||||||
: Constraint(body1, body2, 3, true, CONTACT), mNormal(contactInfo->normal),
|
|
||||||
mPenetrationDepth(contactInfo->penetrationDepth),
|
|
||||||
mLocalPointOnBody1(contactInfo->localPoint1),
|
|
||||||
mLocalPointOnBody2(contactInfo->localPoint2),
|
|
||||||
mWorldPointOnBody1(body1->getTransform() * contactInfo->localPoint1),
|
|
||||||
mWorldPointOnBody2(body2->getTransform() * contactInfo->localPoint2) {
|
|
||||||
assert(mPenetrationDepth > 0.0);
|
|
||||||
|
|
||||||
// Compute the auxiliary lower and upper bounds
|
|
||||||
// TODO : Now mC is only the mass of the first body but it is probably wrong
|
|
||||||
// TODO : Now g is 9.81 but we should use the true gravity value of the physics world.
|
|
||||||
mMu_mc_g = FRICTION_COEFFICIENT * body1->getMass() * 9.81;
|
|
||||||
|
|
||||||
// Compute the friction vectors that span the tangential friction plane
|
|
||||||
computeFrictionVectors();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Destructor
|
|
||||||
Contact::~Contact() {
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
// This method computes the jacobian matrix for all mathematical constraints
|
|
||||||
// The argument "J_sp" is the jacobian matrix of the constraint solver. This method
|
|
||||||
// fill in this matrix with all the jacobian matrix of the mathematical constraint
|
|
||||||
// of the contact. The argument "noConstraint", is the row were the method have
|
|
||||||
// to start to fill in the J_sp matrix.
|
|
||||||
void Contact::computeJacobian(int noConstraint, decimal J_sp[NB_MAX_CONSTRAINTS][2*6]) const {
|
|
||||||
assert(mBody1);
|
|
||||||
assert(mBody2);
|
|
||||||
|
|
||||||
Vector3 body1Position = mBody1->getTransform().getPosition();
|
|
||||||
Vector3 body2Position = mBody2->getTransform().getPosition();
|
|
||||||
int currentIndex = noConstraint; // Current constraint index
|
|
||||||
|
|
||||||
Vector3 r1 = mWorldPointOnBody1 - body1Position;
|
|
||||||
Vector3 r2 = mWorldPointOnBody2 - body2Position;
|
|
||||||
Vector3 r1CrossN = r1.cross(mNormal);
|
|
||||||
Vector3 r2CrossN = r2.cross(mNormal);
|
|
||||||
|
|
||||||
// Compute the jacobian matrix for the body 1 for the contact constraint
|
|
||||||
J_sp[currentIndex][0] = -mNormal.getX();
|
|
||||||
J_sp[currentIndex][1] = -mNormal.getY();
|
|
||||||
J_sp[currentIndex][2] = -mNormal.getZ();
|
|
||||||
J_sp[currentIndex][3] = -r1CrossN.getX();
|
|
||||||
J_sp[currentIndex][4] = -r1CrossN.getY();
|
|
||||||
J_sp[currentIndex][5] = -r1CrossN.getZ();
|
|
||||||
|
|
||||||
// Compute the jacobian matrix for the body 2 for the contact constraint
|
|
||||||
J_sp[currentIndex][6] = mNormal.getX();
|
|
||||||
J_sp[currentIndex][7] = mNormal.getY();
|
|
||||||
J_sp[currentIndex][8] = mNormal.getZ();
|
|
||||||
J_sp[currentIndex][9] = r2CrossN.getX();
|
|
||||||
J_sp[currentIndex][10] = r2CrossN.getY();
|
|
||||||
J_sp[currentIndex][11] = r2CrossN.getZ();
|
|
||||||
|
|
||||||
currentIndex++;
|
|
||||||
|
|
||||||
// Compute the jacobian matrix for the body 1 for the first friction constraint
|
|
||||||
Vector3 r1CrossU1 = r1.cross(mFrictionVectors[0]);
|
|
||||||
Vector3 r2CrossU1 = r2.cross(mFrictionVectors[0]);
|
|
||||||
Vector3 r1CrossU2 = r1.cross(mFrictionVectors[1]);
|
|
||||||
Vector3 r2CrossU2 = r2.cross(mFrictionVectors[1]);
|
|
||||||
J_sp[currentIndex][0] = -mFrictionVectors[0].getX();
|
|
||||||
J_sp[currentIndex][1] = -mFrictionVectors[0].getY();
|
|
||||||
J_sp[currentIndex][2] = -mFrictionVectors[0].getZ();
|
|
||||||
J_sp[currentIndex][3] = -r1CrossU1.getX();
|
|
||||||
J_sp[currentIndex][4] = -r1CrossU1.getY();
|
|
||||||
J_sp[currentIndex][5] = -r1CrossU1.getZ();
|
|
||||||
|
|
||||||
// Compute the jacobian matrix for the body 2 for the first friction constraint
|
|
||||||
J_sp[currentIndex][6] = mFrictionVectors[0].getX();
|
|
||||||
J_sp[currentIndex][7] = mFrictionVectors[0].getY();
|
|
||||||
J_sp[currentIndex][8] = mFrictionVectors[0].getZ();
|
|
||||||
J_sp[currentIndex][9] = r2CrossU1.getX();
|
|
||||||
J_sp[currentIndex][10] = r2CrossU1.getY();
|
|
||||||
J_sp[currentIndex][11] = r2CrossU1.getZ();
|
|
||||||
|
|
||||||
currentIndex++;
|
|
||||||
|
|
||||||
// Compute the jacobian matrix for the body 1 for the second friction constraint
|
|
||||||
J_sp[currentIndex][0] = -mFrictionVectors[1].getX();
|
|
||||||
J_sp[currentIndex][1] = -mFrictionVectors[1].getY();
|
|
||||||
J_sp[currentIndex][2] = -mFrictionVectors[1].getZ();
|
|
||||||
J_sp[currentIndex][3] = -r1CrossU2.getX();
|
|
||||||
J_sp[currentIndex][4] = -r1CrossU2.getY();
|
|
||||||
J_sp[currentIndex][5] = -r1CrossU2.getZ();
|
|
||||||
|
|
||||||
// Compute the jacobian matrix for the body 2 for the second friction constraint
|
|
||||||
J_sp[currentIndex][6] = mFrictionVectors[1].getX();
|
|
||||||
J_sp[currentIndex][7] = mFrictionVectors[1].getY();
|
|
||||||
J_sp[currentIndex][8] = mFrictionVectors[1].getZ();
|
|
||||||
J_sp[currentIndex][9] = r2CrossU2.getX();
|
|
||||||
J_sp[currentIndex][10] = r2CrossU2.getY();
|
|
||||||
J_sp[currentIndex][11] = r2CrossU2.getZ();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Compute the lowerbounds values for all the mathematical constraints. The
|
|
||||||
// argument "lowerBounds" is the lowerbounds values vector of the constraint solver and
|
|
||||||
// this methods has to fill in this vector starting from the row "noConstraint"
|
|
||||||
void Contact::computeLowerBound(int noConstraint, decimal lowerBounds[NB_MAX_CONSTRAINTS]) const {
|
|
||||||
assert(noConstraint >= 0 && noConstraint + mNbConstraints <= NB_MAX_CONSTRAINTS);
|
|
||||||
|
|
||||||
lowerBounds[noConstraint] = 0.0; // Lower bound for the contact constraint
|
|
||||||
lowerBounds[noConstraint + 1] = -mMu_mc_g; // Lower bound for the first friction constraint
|
|
||||||
lowerBounds[noConstraint + 2] = -mMu_mc_g; // Lower bound for the second friction constraint
|
|
||||||
}
|
|
||||||
|
|
||||||
// Compute the upperbounds values for all the mathematical constraints. The
|
|
||||||
// argument "upperBounds" is the upperbounds values vector of the constraint solver and
|
|
||||||
// this methods has to fill in this vector starting from the row "noConstraint"
|
|
||||||
void Contact::computeUpperBound(int noConstraint, decimal upperBounds[NB_MAX_CONSTRAINTS]) const {
|
|
||||||
assert(noConstraint >= 0 && noConstraint + mNbConstraints <= NB_MAX_CONSTRAINTS);
|
|
||||||
|
|
||||||
upperBounds[noConstraint] = DECIMAL_INFINITY; // Upper bound for the contact constraint
|
|
||||||
upperBounds[noConstraint + 1] = mMu_mc_g; // Upper bound for the first friction constraint
|
|
||||||
upperBounds[noConstraint + 2] = mMu_mc_g; // Upper bound for the second friction constraint
|
|
||||||
}
|
|
||||||
|
|
||||||
// Compute the error values for all the mathematical constraints. The argument
|
|
||||||
// "errorValues" is the error values vector of the constraint solver and this
|
|
||||||
// method has to fill in this vector starting from the row "noConstraint"
|
|
||||||
void Contact::computeErrorValue(int noConstraint, decimal errorValues[]) const {
|
|
||||||
assert(mBody1);
|
|
||||||
assert(mBody2);
|
|
||||||
|
|
||||||
// TODO : Do we need this casting anymore ?
|
|
||||||
RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(mBody1);
|
|
||||||
RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(mBody2);
|
|
||||||
|
|
||||||
assert(noConstraint >= 0 && noConstraint + mNbConstraints <= NB_MAX_CONSTRAINTS);
|
|
||||||
|
|
||||||
// Compute the error value for the contact constraint
|
|
||||||
Vector3 velocity1 = rigidBody1->getLinearVelocity();
|
|
||||||
Vector3 velocity2 = rigidBody2->getLinearVelocity();
|
|
||||||
decimal restitutionCoeff = rigidBody1->getRestitution() * rigidBody2->getRestitution();
|
|
||||||
decimal errorValue = restitutionCoeff * (mNormal.dot(velocity1) - mNormal.dot(velocity2));
|
|
||||||
|
|
||||||
// Assign the error value to the vector of error values
|
|
||||||
errorValues[noConstraint] = errorValue; // Error value for contact constraint
|
|
||||||
errorValues[noConstraint + 1] = 0.0; // Error value for friction constraint
|
|
||||||
errorValues[noConstraint + 2] = 0.0; // Error value for friction constraint
|
|
||||||
}
|
|
50
src/constraint/ContactPoint.cpp
Normal file
50
src/constraint/ContactPoint.cpp
Normal file
|
@ -0,0 +1,50 @@
|
||||||
|
/********************************************************************************
|
||||||
|
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
|
||||||
|
* Copyright (c) 2010-2012 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 "ContactPoint.h"
|
||||||
|
|
||||||
|
using namespace reactphysics3d;
|
||||||
|
using namespace std;
|
||||||
|
|
||||||
|
// Constructor
|
||||||
|
ContactPoint::ContactPoint(RigidBody* const body1, RigidBody* const body2,
|
||||||
|
const ContactInfo* contactInfo)
|
||||||
|
: Constraint(body1, body2, 3, 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)) {
|
||||||
|
|
||||||
|
assert(mPenetrationDepth > 0.0);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// Destructor
|
||||||
|
ContactPoint::~ContactPoint() {
|
||||||
|
|
||||||
|
}
|
|
@ -23,8 +23,8 @@
|
||||||
* *
|
* *
|
||||||
********************************************************************************/
|
********************************************************************************/
|
||||||
|
|
||||||
#ifndef CONTACT_H
|
#ifndef CONTACT_POINT_H
|
||||||
#define CONTACT_H
|
#define CONTACT_POINT_H
|
||||||
|
|
||||||
// Libraries
|
// Libraries
|
||||||
#include "Constraint.h"
|
#include "Constraint.h"
|
||||||
|
@ -52,16 +52,13 @@
|
||||||
namespace reactphysics3d {
|
namespace reactphysics3d {
|
||||||
|
|
||||||
/* -------------------------------------------------------------------
|
/* -------------------------------------------------------------------
|
||||||
Class Contact :
|
Class ContactPoint :
|
||||||
This class represents a collision contact between two bodies in
|
This class represents a collision contact point between two
|
||||||
the physics engine. The contact class inherits from the
|
bodies in the physics engine. The ContactPoint class inherits from
|
||||||
Constraint class. Each Contact represent a contact between two bodies
|
the Constraint class.
|
||||||
and contains the two contact points on each body. The contact has 3
|
|
||||||
mathematical constraints (1 for the contact constraint, and 2
|
|
||||||
for the friction constraints).
|
|
||||||
-------------------------------------------------------------------
|
-------------------------------------------------------------------
|
||||||
*/
|
*/
|
||||||
class Contact : public Constraint {
|
class ContactPoint : public Constraint {
|
||||||
|
|
||||||
protected :
|
protected :
|
||||||
|
|
||||||
|
@ -85,31 +82,29 @@ class Contact : public Constraint {
|
||||||
// Contact point on body 2 in world space
|
// Contact point on body 2 in world space
|
||||||
Vector3 mWorldPointOnBody2;
|
Vector3 mWorldPointOnBody2;
|
||||||
|
|
||||||
|
// True if the contact is a resting contact (exists for more than one time step)
|
||||||
|
bool mIsRestingContact;
|
||||||
|
|
||||||
// Two orthogonal vectors that span the tangential friction plane
|
// Two orthogonal vectors that span the tangential friction plane
|
||||||
std::vector<Vector3> mFrictionVectors;
|
std::vector<Vector3> mFrictionVectors;
|
||||||
|
|
||||||
decimal mMu_mc_g;
|
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
|
||||||
// Private copy-constructor
|
// Private copy-constructor
|
||||||
Contact(const Contact& contact);
|
ContactPoint(const ContactPoint& contact);
|
||||||
|
|
||||||
// Private assignment operator
|
// Private assignment operator
|
||||||
Contact& operator=(const Contact& contact);
|
ContactPoint& operator=(const ContactPoint& contact);
|
||||||
|
|
||||||
// Compute the two friction vectors that span the tangential friction plane
|
|
||||||
void computeFrictionVectors();
|
|
||||||
|
|
||||||
public :
|
public :
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
Contact(RigidBody* const body1, RigidBody* const body2, const ContactInfo* contactInfo);
|
ContactPoint(RigidBody* const body1, RigidBody* const body2, const ContactInfo* contactInfo);
|
||||||
|
|
||||||
// Destructor
|
// Destructor
|
||||||
virtual ~Contact();
|
virtual ~ContactPoint();
|
||||||
|
|
||||||
// Return the normal vector of the contact
|
// Return the normal vector of the contact
|
||||||
Vector3 getNormal() const;
|
Vector3 getNormal() const;
|
||||||
|
@ -135,20 +130,23 @@ class Contact : public Constraint {
|
||||||
// Set the contact world point on body 2
|
// Set the contact world point on body 2
|
||||||
void setWorldPointOnBody2(const Vector3& worldPoint);
|
void setWorldPointOnBody2(const Vector3& worldPoint);
|
||||||
|
|
||||||
// Compute the jacobian matrix for all mathematical constraints
|
// Return true if the contact is a resting contact
|
||||||
virtual void computeJacobian(int noConstraint,
|
bool getIsRestingContact() const;
|
||||||
decimal J_SP[NB_MAX_CONSTRAINTS][2*6]) const;
|
|
||||||
|
|
||||||
// Compute the lowerbounds values for all the mathematical constraints
|
// Set the mIsRestingContact variable
|
||||||
virtual void computeLowerBound(int noConstraint,
|
void setIsRestingContact(bool isRestingContact);
|
||||||
decimal lowerBounds[NB_MAX_CONSTRAINTS]) const;
|
|
||||||
|
|
||||||
// Compute the upperbounds values for all the mathematical constraints
|
// Get the first friction vector
|
||||||
virtual void computeUpperBound(int noConstraint,
|
Vector3 getFrictionVector1() const;
|
||||||
decimal upperBounds[NB_MAX_CONSTRAINTS]) const;
|
|
||||||
|
|
||||||
// Compute the error values for all the mathematical constraints
|
// Set the first friction vector
|
||||||
virtual void computeErrorValue(int noConstraint, decimal errorValues[]) const;
|
void setFrictionVector1(const Vector3& frictionVector1);
|
||||||
|
|
||||||
|
// Get the second friction vector
|
||||||
|
Vector3 getFrictionVector2() const;
|
||||||
|
|
||||||
|
// Set the second friction vector
|
||||||
|
void setFrictionVector2(const Vector3& frictionVector2);
|
||||||
|
|
||||||
// Return the penetration depth
|
// Return the penetration depth
|
||||||
decimal getPenetrationDepth() const;
|
decimal getPenetrationDepth() const;
|
||||||
|
@ -159,73 +157,89 @@ class Contact : public Constraint {
|
||||||
#endif
|
#endif
|
||||||
};
|
};
|
||||||
|
|
||||||
// Compute the two unit orthogonal vectors "v1" and "v2" that span the tangential friction plane
|
|
||||||
// The two vectors have to be such that : v1 x v2 = contactNormal
|
|
||||||
inline void Contact::computeFrictionVectors() {
|
|
||||||
// Delete the current friction vectors
|
|
||||||
mFrictionVectors.clear();
|
|
||||||
|
|
||||||
// Compute the first orthogonal vector
|
|
||||||
Vector3 vector1 = mNormal.getOneOrthogonalVector();
|
|
||||||
mFrictionVectors.push_back(vector1);
|
|
||||||
|
|
||||||
// Compute the second orthogonal vector using the cross product
|
|
||||||
mFrictionVectors.push_back(mNormal.cross(vector1));
|
|
||||||
}
|
|
||||||
|
|
||||||
// Return the normal vector of the contact
|
// Return the normal vector of the contact
|
||||||
inline Vector3 Contact::getNormal() const {
|
inline Vector3 ContactPoint::getNormal() const {
|
||||||
return mNormal;
|
return mNormal;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Set the penetration depth of the contact
|
// Set the penetration depth of the contact
|
||||||
inline void Contact::setPenetrationDepth(decimal penetrationDepth) {
|
inline void ContactPoint::setPenetrationDepth(decimal penetrationDepth) {
|
||||||
this->mPenetrationDepth = penetrationDepth;
|
this->mPenetrationDepth = penetrationDepth;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the contact point on body 1
|
// Return the contact point on body 1
|
||||||
inline Vector3 Contact::getLocalPointOnBody1() const {
|
inline Vector3 ContactPoint::getLocalPointOnBody1() const {
|
||||||
return mLocalPointOnBody1;
|
return mLocalPointOnBody1;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the contact point on body 2
|
// Return the contact point on body 2
|
||||||
inline Vector3 Contact::getLocalPointOnBody2() const {
|
inline Vector3 ContactPoint::getLocalPointOnBody2() const {
|
||||||
return mLocalPointOnBody2;
|
return mLocalPointOnBody2;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the contact world point on body 1
|
// Return the contact world point on body 1
|
||||||
inline Vector3 Contact::getWorldPointOnBody1() const {
|
inline Vector3 ContactPoint::getWorldPointOnBody1() const {
|
||||||
return mWorldPointOnBody1;
|
return mWorldPointOnBody1;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the contact world point on body 2
|
// Return the contact world point on body 2
|
||||||
inline Vector3 Contact::getWorldPointOnBody2() const {
|
inline Vector3 ContactPoint::getWorldPointOnBody2() const {
|
||||||
return mWorldPointOnBody2;
|
return mWorldPointOnBody2;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Set the contact world point on body 1
|
// Set the contact world point on body 1
|
||||||
inline void Contact::setWorldPointOnBody1(const Vector3& worldPoint) {
|
inline void ContactPoint::setWorldPointOnBody1(const Vector3& worldPoint) {
|
||||||
mWorldPointOnBody1 = worldPoint;
|
mWorldPointOnBody1 = worldPoint;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Set the contact world point on body 2
|
// Set the contact world point on body 2
|
||||||
inline void Contact::setWorldPointOnBody2(const Vector3& worldPoint) {
|
inline void ContactPoint::setWorldPointOnBody2(const Vector3& worldPoint) {
|
||||||
mWorldPointOnBody2 = worldPoint;
|
mWorldPointOnBody2 = worldPoint;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Return true if the contact is a resting contact
|
||||||
|
inline bool ContactPoint::getIsRestingContact() const {
|
||||||
|
return mIsRestingContact;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set the mIsRestingContact variable
|
||||||
|
inline void ContactPoint::setIsRestingContact(bool isRestingContact) {
|
||||||
|
mIsRestingContact = isRestingContact;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Get the first friction vector
|
||||||
|
inline Vector3 ContactPoint::getFrictionVector1() const {
|
||||||
|
return mFrictionVectors[0];
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set the first friction vector
|
||||||
|
inline void ContactPoint::setFrictionVector1(const Vector3& frictionVector1) {
|
||||||
|
mFrictionVectors[0] = frictionVector1;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Get the second friction vector
|
||||||
|
inline Vector3 ContactPoint::getFrictionVector2() const {
|
||||||
|
return mFrictionVectors[1];
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set the second friction vector
|
||||||
|
inline void ContactPoint::setFrictionVector2(const Vector3& frictionVector2) {
|
||||||
|
mFrictionVectors[1] = frictionVector2;
|
||||||
|
}
|
||||||
|
|
||||||
// Return the penetration depth of the contact
|
// Return the penetration depth of the contact
|
||||||
inline decimal Contact::getPenetrationDepth() const {
|
inline decimal ContactPoint::getPenetrationDepth() const {
|
||||||
return mPenetrationDepth;
|
return mPenetrationDepth;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
#ifdef VISUAL_DEBUG
|
#ifdef VISUAL_DEBUG
|
||||||
inline void Contact::draw() const {
|
inline void ContactPoint::draw() const {
|
||||||
glColor3f(1.0, 0.0, 0.0);
|
glColor3f(1.0, 0.0, 0.0);
|
||||||
glutSolidSphere(0.3, 20, 20);
|
glutSolidSphere(0.3, 20, 20);
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
} // End of the ReactPhysics3D namespace
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
|
@ -101,7 +101,7 @@ void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) {
|
||||||
collisionBody->CollisionBody::~CollisionBody();
|
collisionBody->CollisionBody::~CollisionBody();
|
||||||
|
|
||||||
// Remove the collision body from the list of bodies
|
// Remove the collision body from the list of bodies
|
||||||
mBodies.erase(collisionBody); // TOOD : Maybe use a set to make this faster
|
mBodies.erase(collisionBody); // TODO : Maybe use a set to make this faster
|
||||||
|
|
||||||
// Free the object from the memory pool
|
// Free the object from the memory pool
|
||||||
mMemoryPoolCollisionBodies.freeObject(collisionBody);
|
mMemoryPoolCollisionBodies.freeObject(collisionBody);
|
||||||
|
|
|
@ -35,7 +35,7 @@
|
||||||
#include "OverlappingPair.h"
|
#include "OverlappingPair.h"
|
||||||
#include "../collision/CollisionDetection.h"
|
#include "../collision/CollisionDetection.h"
|
||||||
#include "../constraint/Constraint.h"
|
#include "../constraint/Constraint.h"
|
||||||
#include "../constraint/Contact.h"
|
#include "../constraint/ContactPoint.h"
|
||||||
#include "../memory/MemoryPool.h"
|
#include "../memory/MemoryPool.h"
|
||||||
|
|
||||||
// Namespace reactphysics3d
|
// Namespace reactphysics3d
|
||||||
|
|
|
@ -1,540 +0,0 @@
|
||||||
/********************************************************************************
|
|
||||||
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
|
|
||||||
* Copyright (c) 2010-2012 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"
|
|
||||||
#include "DynamicsWorld.h"
|
|
||||||
#include "../body/RigidBody.h"
|
|
||||||
|
|
||||||
using namespace reactphysics3d;
|
|
||||||
using namespace std;
|
|
||||||
|
|
||||||
|
|
||||||
// Constructor
|
|
||||||
ConstraintSolver::ConstraintSolver(DynamicsWorld* world)
|
|
||||||
:world(world), nbConstraints(0), nbIterationsLCP(DEFAULT_LCP_ITERATIONS),
|
|
||||||
nbIterationsLCPErrorCorrection(DEFAULT_LCP_ITERATIONS_ERROR_CORRECTION),
|
|
||||||
isErrorCorrectionActive(false) {
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
// Destructor
|
|
||||||
ConstraintSolver::~ConstraintSolver() {
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
// Initialize the constraint solver before each solving
|
|
||||||
void ConstraintSolver::initialize() {
|
|
||||||
Constraint* constraint;
|
|
||||||
|
|
||||||
nbConstraints = 0;
|
|
||||||
nbConstraintsError = 0;
|
|
||||||
|
|
||||||
// For each constraint
|
|
||||||
vector<Constraint*>::iterator it;
|
|
||||||
for (it = world->getConstraintsBeginIterator(); it != world->getConstraintsEndIterator(); ++it) {
|
|
||||||
constraint = *it;
|
|
||||||
|
|
||||||
// If the constraint is active
|
|
||||||
if (constraint->isActive()) {
|
|
||||||
activeConstraints.push_back(constraint);
|
|
||||||
|
|
||||||
// Add the two bodies of the constraint in the constraintBodies list
|
|
||||||
constraintBodies.insert(constraint->getBody1());
|
|
||||||
constraintBodies.insert(constraint->getBody2());
|
|
||||||
|
|
||||||
// Fill in the body number maping
|
|
||||||
bodyNumberMapping.insert(pair<Body*, unsigned int>(constraint->getBody1(), bodyNumberMapping.size()));
|
|
||||||
bodyNumberMapping.insert(pair<Body*, unsigned int>(constraint->getBody2(), bodyNumberMapping.size()));
|
|
||||||
|
|
||||||
// Update the size of the jacobian matrix
|
|
||||||
nbConstraints += constraint->getNbConstraints();
|
|
||||||
|
|
||||||
// Update the size of the jacobian matrix for error correction projection
|
|
||||||
if (constraint->getType() == CONTACT) {
|
|
||||||
nbConstraintsError++;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Compute the number of bodies that are part of some active constraint
|
|
||||||
nbBodies = bodyNumberMapping.size();
|
|
||||||
|
|
||||||
assert(nbConstraints > 0 && nbConstraints <= NB_MAX_CONSTRAINTS);
|
|
||||||
assert(nbConstraintsError > 0 && nbConstraintsError <= NB_MAX_CONSTRAINTS);
|
|
||||||
assert(nbBodies > 0 && nbBodies <= NB_MAX_BODIES);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Fill in all the matrices needed to solve the LCP problem
|
|
||||||
// Notice that all the active constraints should have been evaluated first
|
|
||||||
void ConstraintSolver::fillInMatrices(decimal dt) {
|
|
||||||
Constraint* constraint;
|
|
||||||
decimal oneOverDt = 1.0 / dt;
|
|
||||||
|
|
||||||
// For each active constraint
|
|
||||||
int noConstraint = 0;
|
|
||||||
int noConstraintError = 0;
|
|
||||||
|
|
||||||
for (int c=0; c<activeConstraints.size(); c++) {
|
|
||||||
|
|
||||||
constraint = activeConstraints.at(c);
|
|
||||||
|
|
||||||
// Fill in the J_sp matrix
|
|
||||||
constraint->computeJacobian(noConstraint, J_sp);
|
|
||||||
|
|
||||||
// Fill in the body mapping matrix
|
|
||||||
for(int i=0; i<constraint->getNbConstraints(); i++) {
|
|
||||||
bodyMapping[noConstraint+i][0] = constraint->getBody1();
|
|
||||||
bodyMapping[noConstraint+i][1] = constraint->getBody2();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Fill in the limit vectors for the constraint
|
|
||||||
constraint->computeLowerBound(noConstraint, lowerBounds);
|
|
||||||
constraint->computeUpperBound(noConstraint, upperBounds);
|
|
||||||
|
|
||||||
// Fill in the error vector
|
|
||||||
constraint->computeErrorValue(noConstraint, errorValues);
|
|
||||||
|
|
||||||
// Get the cached lambda values of the constraint
|
|
||||||
for (int i=0; i<constraint->getNbConstraints(); i++) {
|
|
||||||
lambdaInit[noConstraint + i] = constraint->getCachedLambda(i);
|
|
||||||
}
|
|
||||||
|
|
||||||
// If the constraint is a contact
|
|
||||||
if (constraint->getType() == CONTACT) {
|
|
||||||
Contact* contact = dynamic_cast<Contact*>(constraint);
|
|
||||||
decimal penetrationDepth = contact->getPenetrationDepth();
|
|
||||||
|
|
||||||
// If error correction with projection is active
|
|
||||||
if (isErrorCorrectionActive) {
|
|
||||||
|
|
||||||
// Fill in the error correction projection parameters
|
|
||||||
lowerBoundsError[noConstraintError] = lowerBounds[noConstraint];
|
|
||||||
upperBoundsError[noConstraintError] = upperBounds[noConstraint];
|
|
||||||
for (int i=0; i<12; i++) {
|
|
||||||
J_spError[noConstraintError][i] = J_sp[noConstraint][i];
|
|
||||||
}
|
|
||||||
bodyMappingError[noConstraintError][0] = constraint->getBody1();
|
|
||||||
bodyMappingError[noConstraintError][1] = constraint->getBody2();
|
|
||||||
penetrationDepths[noConstraintError] = contact->getPenetrationDepth();
|
|
||||||
|
|
||||||
// If the penetration depth is small
|
|
||||||
if (penetrationDepth < PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION) {
|
|
||||||
// Use the Baumgarte error correction term for contacts instead of
|
|
||||||
// first order world projection
|
|
||||||
errorValues[noConstraint] += 0.1 * oneOverDt * contact->getPenetrationDepth();
|
|
||||||
}
|
|
||||||
|
|
||||||
noConstraintError++;
|
|
||||||
}
|
|
||||||
else { // If error correction with projection is not active
|
|
||||||
// Add the Baumgarte error correction term for contacts
|
|
||||||
errorValues[noConstraint] += 0.1 * oneOverDt * contact->getPenetrationDepth();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
noConstraint += constraint->getNbConstraints();
|
|
||||||
}
|
|
||||||
|
|
||||||
// For each current body that is implied in some constraint
|
|
||||||
RigidBody* rigidBody;
|
|
||||||
Body* body;
|
|
||||||
uint b=0;
|
|
||||||
for (set<Body*>::iterator it = constraintBodies.begin(); it != constraintBodies.end(); ++it, b++) {
|
|
||||||
body = *it;
|
|
||||||
uint bodyNumber = bodyNumberMapping[body];
|
|
||||||
|
|
||||||
// TODO : Use polymorphism and remove this downcasting
|
|
||||||
rigidBody = dynamic_cast<RigidBody*>(body);
|
|
||||||
assert(rigidBody);
|
|
||||||
|
|
||||||
// Compute the vector V1 with initial velocities values
|
|
||||||
Vector3 linearVelocity = rigidBody->getLinearVelocity();
|
|
||||||
Vector3 angularVelocity = rigidBody->getAngularVelocity();
|
|
||||||
int bodyIndexArray = 6 * bodyNumber;
|
|
||||||
V1[bodyIndexArray] = linearVelocity[0];
|
|
||||||
V1[bodyIndexArray + 1] = linearVelocity[1];
|
|
||||||
V1[bodyIndexArray + 2] = linearVelocity[2];
|
|
||||||
V1[bodyIndexArray + 3] = angularVelocity[0];
|
|
||||||
V1[bodyIndexArray + 4] = angularVelocity[1];
|
|
||||||
V1[bodyIndexArray + 5] = angularVelocity[2];
|
|
||||||
|
|
||||||
// Compute the vector Vconstraint with final constraint velocities
|
|
||||||
Vconstraint[bodyIndexArray] = 0.0;
|
|
||||||
Vconstraint[bodyIndexArray + 1] = 0.0;
|
|
||||||
Vconstraint[bodyIndexArray + 2] = 0.0;
|
|
||||||
Vconstraint[bodyIndexArray + 3] = 0.0;
|
|
||||||
Vconstraint[bodyIndexArray + 4] = 0.0;
|
|
||||||
Vconstraint[bodyIndexArray + 5] = 0.0;
|
|
||||||
|
|
||||||
// Compute the vector Vconstraint with final constraint velocities
|
|
||||||
if (isErrorCorrectionActive) {
|
|
||||||
VconstraintError[bodyIndexArray] = 0.0;
|
|
||||||
VconstraintError[bodyIndexArray + 1] = 0.0;
|
|
||||||
VconstraintError[bodyIndexArray + 2] = 0.0;
|
|
||||||
VconstraintError[bodyIndexArray + 3] = 0.0;
|
|
||||||
VconstraintError[bodyIndexArray + 4] = 0.0;
|
|
||||||
VconstraintError[bodyIndexArray + 5] = 0.0;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Compute the vector with forces and torques values
|
|
||||||
Vector3 externalForce = rigidBody->getExternalForce();
|
|
||||||
Vector3 externalTorque = rigidBody->getExternalTorque();
|
|
||||||
Fext[bodyIndexArray] = externalForce[0];
|
|
||||||
Fext[bodyIndexArray + 1] = externalForce[1];
|
|
||||||
Fext[bodyIndexArray + 2] = externalForce[2];
|
|
||||||
Fext[bodyIndexArray + 3] = externalTorque[0];
|
|
||||||
Fext[bodyIndexArray + 4] = externalTorque[1];
|
|
||||||
Fext[bodyIndexArray + 5] = externalTorque[2];
|
|
||||||
|
|
||||||
// Initialize the mass and inertia tensor matrices
|
|
||||||
Minv_sp_inertia[bodyNumber].setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
|
|
||||||
Minv_sp_mass_diag[bodyNumber] = 0.0;
|
|
||||||
|
|
||||||
// If the motion of the rigid body is enabled
|
|
||||||
if (rigidBody->getIsMotionEnabled()) {
|
|
||||||
Minv_sp_inertia[bodyNumber] = rigidBody->getInertiaTensorInverseWorld();
|
|
||||||
Minv_sp_mass_diag[bodyNumber] = rigidBody->getMassInverse();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Compute the vector b
|
|
||||||
void ConstraintSolver::computeVectorB(decimal dt) {
|
|
||||||
uint indexBody1, indexBody2;
|
|
||||||
decimal oneOverDT = 1.0 / dt;
|
|
||||||
|
|
||||||
for (uint c = 0; c<nbConstraints; c++) {
|
|
||||||
|
|
||||||
// b = errorValues * oneOverDT;
|
|
||||||
b[c] = errorValues[c] * oneOverDT;
|
|
||||||
|
|
||||||
// Substract 1.0/dt*J*V to the vector b
|
|
||||||
indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
|
|
||||||
indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
|
|
||||||
decimal multiplication = 0.0;
|
|
||||||
int body1ArrayIndex = 6 * indexBody1;
|
|
||||||
int body2ArrayIndex = 6 * indexBody2;
|
|
||||||
for (uint i=0; i<6; i++) {
|
|
||||||
multiplication += J_sp[c][i] * V1[body1ArrayIndex + i];
|
|
||||||
multiplication += J_sp[c][6 + i] * V1[body2ArrayIndex + i];
|
|
||||||
}
|
|
||||||
b[c] -= multiplication * oneOverDT ;
|
|
||||||
|
|
||||||
// Substract J*M^-1*F_ext to the vector b
|
|
||||||
decimal value1 = 0.0;
|
|
||||||
decimal value2 = 0.0;
|
|
||||||
decimal sum1, sum2;
|
|
||||||
value1 += J_sp[c][0] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex] +
|
|
||||||
J_sp[c][1] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex + 1] +
|
|
||||||
J_sp[c][2] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex + 2];
|
|
||||||
value2 += J_sp[c][6] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex] +
|
|
||||||
J_sp[c][7] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex + 1] +
|
|
||||||
J_sp[c][8] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex + 2];
|
|
||||||
for (uint i=0; i<3; i++) {
|
|
||||||
sum1 = 0.0;
|
|
||||||
sum2 = 0.0;
|
|
||||||
for (uint j=0; j<3; j++) {
|
|
||||||
sum1 += J_sp[c][3 + j] * Minv_sp_inertia[indexBody1].getValue(j, i);
|
|
||||||
sum2 += J_sp[c][9 + j] * Minv_sp_inertia[indexBody2].getValue(j, i);
|
|
||||||
}
|
|
||||||
value1 += sum1 * Fext[body1ArrayIndex + 3 + i];
|
|
||||||
value2 += sum2 * Fext[body2ArrayIndex + 3 + i];
|
|
||||||
}
|
|
||||||
|
|
||||||
b[c] -= value1 + value2;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Compute the vector b for error correction projection
|
|
||||||
void ConstraintSolver::computeVectorBError(decimal dt) {
|
|
||||||
decimal oneOverDT = 1.0 / dt;
|
|
||||||
|
|
||||||
for (uint c = 0; c<nbConstraintsError; c++) {
|
|
||||||
|
|
||||||
// b = errorValues * oneOverDT;
|
|
||||||
if (penetrationDepths[c] >= PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION) {
|
|
||||||
bError[c] = penetrationDepths[c] * oneOverDT;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
bError[c] = 0.0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Compute the matrix B_sp
|
|
||||||
void ConstraintSolver::computeMatrixB_sp() {
|
|
||||||
uint indexConstraintArray, indexBody1, indexBody2;
|
|
||||||
|
|
||||||
// For each constraint
|
|
||||||
for (uint c = 0; c<nbConstraints; c++) {
|
|
||||||
|
|
||||||
indexConstraintArray = 6 * c;
|
|
||||||
indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
|
|
||||||
indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
|
|
||||||
B_sp[0][indexConstraintArray] = Minv_sp_mass_diag[indexBody1] * J_sp[c][0];
|
|
||||||
B_sp[0][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody1] * J_sp[c][1];
|
|
||||||
B_sp[0][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody1] * J_sp[c][2];
|
|
||||||
B_sp[1][indexConstraintArray] = Minv_sp_mass_diag[indexBody2] * J_sp[c][6];
|
|
||||||
B_sp[1][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody2] * J_sp[c][7];
|
|
||||||
B_sp[1][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody2] * J_sp[c][8];
|
|
||||||
|
|
||||||
for (uint i=0; i<3; i++) {
|
|
||||||
B_sp[0][indexConstraintArray + 3 + i] = 0.0;
|
|
||||||
B_sp[1][indexConstraintArray + 3 + i] = 0.0;
|
|
||||||
for (uint j=0; j<3; j++) {
|
|
||||||
B_sp[0][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody1].getValue(i, j) * J_sp[c][3 + j];
|
|
||||||
B_sp[1][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody2].getValue(i, j) * J_sp[c][9 + j];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Same as B_sp matrix but for error correction projection and
|
|
||||||
// therefore, only for contact constraints
|
|
||||||
void ConstraintSolver::computeMatrixB_spErrorCorrection() {
|
|
||||||
uint indexConstraintArray, indexBody1, indexBody2;
|
|
||||||
|
|
||||||
// For each contact constraint
|
|
||||||
for (uint c = 0; c<nbConstraintsError; c++) {
|
|
||||||
|
|
||||||
indexConstraintArray = 6 * c;
|
|
||||||
indexBody1 = bodyNumberMapping[bodyMappingError[c][0]];
|
|
||||||
indexBody2 = bodyNumberMapping[bodyMappingError[c][1]];
|
|
||||||
B_spError[0][indexConstraintArray] = Minv_sp_mass_diag[indexBody1] * J_spError[c][0];
|
|
||||||
B_spError[0][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody1] * J_spError[c][1];
|
|
||||||
B_spError[0][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody1] * J_spError[c][2];
|
|
||||||
B_spError[1][indexConstraintArray] = Minv_sp_mass_diag[indexBody2] * J_spError[c][6];
|
|
||||||
B_spError[1][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody2] * J_spError[c][7];
|
|
||||||
B_spError[1][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody2] * J_spError[c][8];
|
|
||||||
|
|
||||||
for (uint i=0; i<3; i++) {
|
|
||||||
B_spError[0][indexConstraintArray + 3 + i] = 0.0;
|
|
||||||
B_spError[1][indexConstraintArray + 3 + i] = 0.0;
|
|
||||||
|
|
||||||
for (uint j=0; j<3; j++) {
|
|
||||||
B_spError[0][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody1].getValue(i, j) * J_spError[c][3 + j];
|
|
||||||
B_spError[1][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody2].getValue(i, j) * J_spError[c][9 + j];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Compute the vector V_constraint (which corresponds to the constraint part of
|
|
||||||
// the final V2 vector) according to the formula
|
|
||||||
// V_constraint = dt * (M^-1 * J^T * lambda)
|
|
||||||
// Note that we use the vector V to store both V1 and V_constraint.
|
|
||||||
// Note that M^-1 * J^T = B.
|
|
||||||
// This method is called after that the LCP solver has computed lambda
|
|
||||||
void ConstraintSolver::computeVectorVconstraint(decimal dt) {
|
|
||||||
uint indexBody1Array, indexBody2Array, indexConstraintArray;
|
|
||||||
uint j;
|
|
||||||
|
|
||||||
// Compute dt * (M^-1 * J^T * lambda
|
|
||||||
for (uint i=0; i<nbConstraints; i++) {
|
|
||||||
indexBody1Array = 6 * bodyNumberMapping[bodyMapping[i][0]];
|
|
||||||
indexBody2Array = 6 * bodyNumberMapping[bodyMapping[i][1]];
|
|
||||||
indexConstraintArray = 6 * i;
|
|
||||||
for (j=0; j<6; j++) {
|
|
||||||
Vconstraint[indexBody1Array + j] += B_sp[0][indexConstraintArray + j] * lambda[i] * dt;
|
|
||||||
Vconstraint[indexBody2Array + j] += B_sp[1][indexConstraintArray + j] * lambda[i] * dt;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Same as computeVectorVconstraint() but for error correction projection
|
|
||||||
void ConstraintSolver::computeVectorVconstraintError(decimal dt) {
|
|
||||||
uint indexBody1Array, indexBody2Array, indexConstraintArray;
|
|
||||||
uint j;
|
|
||||||
|
|
||||||
// Compute M^-1 * J^T * lambda
|
|
||||||
for (uint i=0; i<nbConstraintsError; i++) {
|
|
||||||
indexBody1Array = 6 * bodyNumberMapping[bodyMappingError[i][0]];
|
|
||||||
indexBody2Array = 6 * bodyNumberMapping[bodyMappingError[i][1]];
|
|
||||||
indexConstraintArray = 6 * i;
|
|
||||||
for (j=0; j<6; j++) {
|
|
||||||
VconstraintError[indexBody1Array + j] += B_spError[0][indexConstraintArray + j] * lambdaError[i];
|
|
||||||
VconstraintError[indexBody2Array + j] += B_spError[1][indexConstraintArray + j] * lambdaError[i];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// Solve a LCP problem using the Projected-Gauss-Seidel algorithm
|
|
||||||
// This method outputs the result in the lambda vector
|
|
||||||
void ConstraintSolver::solveLCP() {
|
|
||||||
|
|
||||||
for (uint i=0; i<nbConstraints; i++) {
|
|
||||||
lambda[i] = lambdaInit[i];
|
|
||||||
}
|
|
||||||
|
|
||||||
uint indexBody1Array, indexBody2Array;
|
|
||||||
decimal deltaLambda;
|
|
||||||
decimal lambdaTemp;
|
|
||||||
uint i, iter;
|
|
||||||
|
|
||||||
// Compute the vector a
|
|
||||||
computeVectorA();
|
|
||||||
|
|
||||||
|
|
||||||
// For each constraint
|
|
||||||
for (i=0; i<nbConstraints; i++) {
|
|
||||||
uint indexConstraintArray = 6 * i;
|
|
||||||
d[i] = 0.0;
|
|
||||||
for (uint j=0; j<6; j++) {
|
|
||||||
d[i] += J_sp[i][j] * B_sp[0][indexConstraintArray + j] + J_sp[i][6 + j] * B_sp[1][indexConstraintArray + j];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
for(iter=0; iter<nbIterationsLCP; iter++) {
|
|
||||||
for (i=0; i<nbConstraints; i++) {
|
|
||||||
indexBody1Array = 6 * bodyNumberMapping[bodyMapping[i][0]];
|
|
||||||
indexBody2Array = 6 * bodyNumberMapping[bodyMapping[i][1]];
|
|
||||||
uint indexConstraintArray = 6 * i;
|
|
||||||
deltaLambda = b[i];
|
|
||||||
for (uint j=0; j<6; j++) {
|
|
||||||
deltaLambda -= (J_sp[i][j] * a[indexBody1Array + j] + J_sp[i][6 + j] * a[indexBody2Array + j]);
|
|
||||||
}
|
|
||||||
deltaLambda /= d[i];
|
|
||||||
lambdaTemp = lambda[i];
|
|
||||||
lambda[i] = std::max(lowerBounds[i], std::min(lambda[i] + deltaLambda, upperBounds[i]));
|
|
||||||
deltaLambda = lambda[i] - lambdaTemp;
|
|
||||||
for (uint j=0; j<6; j++) {
|
|
||||||
a[indexBody1Array + j] += B_sp[0][indexConstraintArray + j] * deltaLambda;
|
|
||||||
a[indexBody2Array + j] += B_sp[1][indexConstraintArray + j] * deltaLambda;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Solve a LCP problem for error correction projection
|
|
||||||
// using the Projected-Gauss-Seidel algorithm
|
|
||||||
// This method outputs the result in the lambda vector
|
|
||||||
void ConstraintSolver::solveLCPErrorCorrection() {
|
|
||||||
|
|
||||||
for (uint i=0; i<nbConstraintsError; i++) {
|
|
||||||
lambdaError[i] = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint indexBody1Array, indexBody2Array;
|
|
||||||
decimal deltaLambda;
|
|
||||||
decimal lambdaTemp;
|
|
||||||
uint i, iter;
|
|
||||||
|
|
||||||
// Compute the vector a
|
|
||||||
computeVectorAError();
|
|
||||||
|
|
||||||
|
|
||||||
// For each constraint
|
|
||||||
for (i=0; i<nbConstraintsError; i++) {
|
|
||||||
uint indexConstraintArray = 6 * i;
|
|
||||||
d[i] = 0.0;
|
|
||||||
for (uint j=0; j<6; j++) {
|
|
||||||
d[i] += J_spError[i][j] * B_spError[0][indexConstraintArray + j] + J_spError[i][6 + j] * B_spError[1][indexConstraintArray + j];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
for(iter=0; iter<nbIterationsLCPErrorCorrection; iter++) {
|
|
||||||
for (i=0; i<nbConstraintsError; i++) {
|
|
||||||
indexBody1Array = 6 * bodyNumberMapping[bodyMappingError[i][0]];
|
|
||||||
indexBody2Array = 6 * bodyNumberMapping[bodyMappingError[i][1]];
|
|
||||||
uint indexConstraintArray = 6 * i;
|
|
||||||
deltaLambda = bError[i];
|
|
||||||
for (uint j=0; j<6; j++) {
|
|
||||||
deltaLambda -= (J_spError[i][j] * aError[indexBody1Array + j] + J_spError[i][6 + j] * aError[indexBody2Array + j]);
|
|
||||||
}
|
|
||||||
deltaLambda /= d[i];
|
|
||||||
lambdaTemp = lambdaError[i];
|
|
||||||
lambdaError[i] = std::max(lowerBoundsError[i], std::min(lambdaError[i] + deltaLambda, upperBoundsError[i]));
|
|
||||||
deltaLambda = lambdaError[i] - lambdaTemp;
|
|
||||||
for (uint j=0; j<6; j++) {
|
|
||||||
aError[indexBody1Array + j] += B_spError[0][indexConstraintArray + j] * deltaLambda;
|
|
||||||
aError[indexBody2Array + j] += B_spError[1][indexConstraintArray + j] * deltaLambda;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Compute the vector a used in the solve() method
|
|
||||||
// Note that a = B * lambda
|
|
||||||
void ConstraintSolver::computeVectorA() {
|
|
||||||
uint i;
|
|
||||||
uint indexBody1Array, indexBody2Array;
|
|
||||||
|
|
||||||
// Init the vector a with zero values
|
|
||||||
for (i=0; i<6*nbBodies; i++) {
|
|
||||||
a[i] = 0.0;
|
|
||||||
}
|
|
||||||
|
|
||||||
for(i=0; i<nbConstraints; i++) {
|
|
||||||
indexBody1Array = 6 * bodyNumberMapping[bodyMapping[i][0]];
|
|
||||||
indexBody2Array = 6 * bodyNumberMapping[bodyMapping[i][1]];
|
|
||||||
uint indexConstraintArray = 6 * i;
|
|
||||||
for (uint j=0; j<6; j++) {
|
|
||||||
a[indexBody1Array + j] += B_sp[0][indexConstraintArray + j] * lambda[i];
|
|
||||||
a[indexBody2Array + j] += B_sp[1][indexConstraintArray + j] * lambda[i];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Same as computeVectorA() but for error correction projection
|
|
||||||
void ConstraintSolver::computeVectorAError() {
|
|
||||||
uint i;
|
|
||||||
uint indexBody1Array, indexBody2Array;
|
|
||||||
|
|
||||||
// Init the vector a with zero values
|
|
||||||
for (i=0; i<6*nbBodies; i++) {
|
|
||||||
aError[i] = 0.0;
|
|
||||||
}
|
|
||||||
|
|
||||||
for(i=0; i<nbConstraintsError; i++) {
|
|
||||||
indexBody1Array = 6 * bodyNumberMapping[bodyMappingError[i][0]];
|
|
||||||
indexBody2Array = 6 * bodyNumberMapping[bodyMappingError[i][1]];
|
|
||||||
uint indexConstraintArray = 6 * i;
|
|
||||||
for (uint j=0; j<6; j++) {
|
|
||||||
aError[indexBody1Array + j] += B_spError[0][indexConstraintArray + j] * lambdaError[i];
|
|
||||||
aError[indexBody2Array + j] += B_spError[1][indexConstraintArray + j] * lambdaError[i];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// Cache the lambda values in order to reuse them in the next step
|
|
||||||
// to initialize the lambda vector
|
|
||||||
void ConstraintSolver::cacheLambda() {
|
|
||||||
|
|
||||||
// For each active constraint
|
|
||||||
int noConstraint = 0;
|
|
||||||
for (int c=0; c<activeConstraints.size(); c++) {
|
|
||||||
|
|
||||||
// For each constraint of the contact
|
|
||||||
for (int i=0; i<activeConstraints[c]->getNbConstraints(); i++) {
|
|
||||||
// Get the lambda value that have just been computed
|
|
||||||
activeConstraints[c]->setCachedLambda(i, lambda[noConstraint + i]);
|
|
||||||
}
|
|
||||||
|
|
||||||
noConstraint += activeConstraints[c]->getNbConstraints();
|
|
||||||
}
|
|
||||||
}
|
|
|
@ -1,237 +0,0 @@
|
||||||
/********************************************************************************
|
|
||||||
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
|
|
||||||
* Copyright (c) 2010-2012 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 CONSTRAINT_SOLVER_H
|
|
||||||
#define CONSTRAINT_SOLVER_H
|
|
||||||
|
|
||||||
// Libraries
|
|
||||||
#include "../configuration.h"
|
|
||||||
#include "../constraint/Constraint.h"
|
|
||||||
#include <map>
|
|
||||||
#include <set>
|
|
||||||
|
|
||||||
// ReactPhysics3D namespace
|
|
||||||
namespace reactphysics3d {
|
|
||||||
|
|
||||||
// Declarations
|
|
||||||
class DynamicsWorld;
|
|
||||||
|
|
||||||
|
|
||||||
/* -------------------------------------------------------------------
|
|
||||||
Class ConstrainSolver :
|
|
||||||
This class represents the constraint solver. The constraint solver
|
|
||||||
is based on the theory from the paper "Iterative Dynamics with
|
|
||||||
Temporal Coherence" from Erin Catto. We keep the same notations as
|
|
||||||
in the paper. The idea is to construct a LCP problem and then solve
|
|
||||||
it using a Projected Gauss Seidel (PGS) solver.
|
|
||||||
|
|
||||||
The idea is to solve the following system for lambda :
|
|
||||||
JM^-1J^T * lamdba - 1/dt * b_error + 1/dt * JV^1 + JM^-1F_ext >= 0
|
|
||||||
|
|
||||||
By default, error correction using first world order projections (described
|
|
||||||
by Erleben in section 4.16 in his PhD thesis "Stable, Robust, and
|
|
||||||
Versatile Multibody Dynamics Animation") is used for very large penetration
|
|
||||||
depths. Error correction with projection requires to solve another LCP problem
|
|
||||||
that is simpler than the above one and by considering only the contact
|
|
||||||
constraints. The LCP problem for error correction is the following one :
|
|
||||||
J_contact M^-1 J_contact^T * lambda + 1/dt * -d >= 0
|
|
||||||
|
|
||||||
where "d" is a vector with penetration depths. If the penetration depth of
|
|
||||||
a given contact is not very large, we use the Baumgarte error correction (see
|
|
||||||
the paper from Erin Catto).
|
|
||||||
|
|
||||||
-------------------------------------------------------------------
|
|
||||||
*/
|
|
||||||
class ConstraintSolver {
|
|
||||||
private:
|
|
||||||
DynamicsWorld* world; // Reference to the world
|
|
||||||
std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world
|
|
||||||
bool isErrorCorrectionActive; // True if error correction (with world order) is active
|
|
||||||
uint nbIterationsLCP; // Number of iterations of the LCP solver
|
|
||||||
uint nbIterationsLCPErrorCorrection; // Number of iterations of the LCP solver for error correction
|
|
||||||
uint nbConstraints; // Total number of constraints (with the auxiliary constraints)
|
|
||||||
uint nbConstraintsError; // Number of constraints for error correction projection (only contact constraints)
|
|
||||||
uint nbBodies; // Current number of bodies in the physics world
|
|
||||||
std::set<Body*> constraintBodies; // Bodies that are implied in some constraint
|
|
||||||
std::map<Body*, uint> bodyNumberMapping; // Map a body pointer with its index number
|
|
||||||
Body* bodyMapping[NB_MAX_CONSTRAINTS][2]; // 2-dimensional array that contains the mapping of body reference
|
|
||||||
// in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains
|
|
||||||
// the pointer to the body that correspond to the 1x6 J_ij matrix in the
|
|
||||||
// J_sp matrix. An integer body index refers to its index in the "bodies" std::vector
|
|
||||||
Body* bodyMappingError[NB_MAX_CONSTRAINTS][2]; // Same as bodyMapping but for error correction projection
|
|
||||||
decimal J_sp[NB_MAX_CONSTRAINTS][2*6]; // 2-dimensional array that correspond to the sparse representation of the jacobian matrix of all constraints
|
|
||||||
// This array contains for each constraint two 1x6 Jacobian matrices (one for each body of the constraint)
|
|
||||||
// a 1x6 matrix
|
|
||||||
decimal B_sp[2][6*NB_MAX_CONSTRAINTS]; // 2-dimensional array that correspond to a useful matrix in sparse representation
|
|
||||||
// This array contains for each constraint two 6x1 matrices (one for each body of the constraint)
|
|
||||||
// a 6x1 matrix
|
|
||||||
decimal J_spError[NB_MAX_CONSTRAINTS][2*6]; // Same as J_sp for error correction projection (only contact constraints)
|
|
||||||
decimal B_spError[2][6*NB_MAX_CONSTRAINTS]; // Same as B_sp for error correction projection (only contact constraints)
|
|
||||||
decimal b[NB_MAX_CONSTRAINTS]; // Vector "b" of the LCP problem
|
|
||||||
decimal bError[NB_MAX_CONSTRAINTS]; // Vector "b" of the LCP problem for error correction projection
|
|
||||||
decimal d[NB_MAX_CONSTRAINTS]; // Vector "d"
|
|
||||||
decimal a[6*NB_MAX_BODIES]; // Vector "a"
|
|
||||||
decimal aError[6*NB_MAX_BODIES]; // Vector "a" for error correction projection
|
|
||||||
decimal penetrationDepths[NB_MAX_CONSTRAINTS]; // Array of penetration depths for error correction projection
|
|
||||||
decimal lambda[NB_MAX_CONSTRAINTS]; // Lambda vector of the LCP problem
|
|
||||||
decimal lambdaError[NB_MAX_CONSTRAINTS]; // Lambda vector of the LCP problem for error correction projections
|
|
||||||
decimal lambdaInit[NB_MAX_CONSTRAINTS]; // Lambda init vector for the LCP solver
|
|
||||||
decimal errorValues[NB_MAX_CONSTRAINTS]; // Error vector of all constraints
|
|
||||||
decimal lowerBounds[NB_MAX_CONSTRAINTS]; // Vector that contains the low limits for the variables of the LCP problem
|
|
||||||
decimal upperBounds[NB_MAX_CONSTRAINTS]; // Vector that contains the high limits for the variables of the LCP problem
|
|
||||||
decimal lowerBoundsError[NB_MAX_CONSTRAINTS]; // Same as "lowerBounds" but for error correction projections
|
|
||||||
decimal upperBoundsError[NB_MAX_CONSTRAINTS]; // Same as "upperBounds" but for error correction projections
|
|
||||||
Matrix3x3 Minv_sp_inertia[NB_MAX_BODIES]; // 3x3 world inertia tensor matrix I for each body (from the Minv_sp matrix)
|
|
||||||
decimal Minv_sp_mass_diag[NB_MAX_BODIES]; // Array that contains for each body the inverse of its mass
|
|
||||||
// This is an array of size nbBodies that contains in each cell a 6x6 matrix
|
|
||||||
decimal V1[6*NB_MAX_BODIES]; // Array that contains for each body the 6x1 vector that contains linear and angular velocities
|
|
||||||
// Each cell contains a 6x1 vector with linear and angular velocities
|
|
||||||
decimal Vconstraint[6*NB_MAX_BODIES]; // Same kind of vector as V1 but contains the final constraint velocities
|
|
||||||
decimal VconstraintError[6*NB_MAX_BODIES]; // Same kind of vector as V1 but contains the final constraint velocities
|
|
||||||
decimal Fext[6*NB_MAX_BODIES]; // Array that contains for each body the 6x1 vector that contains external forces and torques
|
|
||||||
// Each cell contains a 6x1 vector with external force and torque.
|
|
||||||
void initialize(); // Initialize the constraint solver before each solving
|
|
||||||
void fillInMatrices(decimal dt); // Fill in all the matrices needed to solve the LCP problem
|
|
||||||
void computeVectorB(decimal dt); // Compute the vector b
|
|
||||||
void computeVectorBError(decimal dt); // Compute the vector b for error correction projection
|
|
||||||
void computeMatrixB_sp(); // Compute the matrix B_sp
|
|
||||||
void computeMatrixB_spErrorCorrection(); // Compute the matrix B_spError for error correction projection
|
|
||||||
void computeVectorVconstraint(decimal dt); // Compute the vector V2
|
|
||||||
void computeVectorVconstraintError(decimal dt); // Same as computeVectorVconstraint() but for error correction projection
|
|
||||||
void cacheLambda(); // Cache the lambda values in order to reuse them in the next step to initialize the lambda vector
|
|
||||||
void computeVectorA(); // Compute the vector a used in the solve() method
|
|
||||||
void computeVectorAError(); // Same as computeVectorA() but for error correction projection
|
|
||||||
void solveLCP(); // Solve a LCP problem using Projected-Gauss-Seidel algorithm
|
|
||||||
void solveLCPErrorCorrection(); // Solve the LCP problem for error correction projection
|
|
||||||
|
|
||||||
public:
|
|
||||||
ConstraintSolver(DynamicsWorld* world); // Constructor
|
|
||||||
virtual ~ConstraintSolver(); // Destructor
|
|
||||||
void solve(decimal dt); // Solve the current LCP problem
|
|
||||||
bool isConstrainedBody(Body* body) const; // Return true if the body is in at least one constraint
|
|
||||||
Vector3 getConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem
|
|
||||||
Vector3 getConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem
|
|
||||||
Vector3 getErrorConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem for error correction
|
|
||||||
Vector3 getErrorConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem for error correction
|
|
||||||
void cleanup(); // Cleanup of the constraint solver
|
|
||||||
void setNbLCPIterations(uint nbIterations); // Set the number of iterations of the LCP solver
|
|
||||||
void setIsErrorCorrectionActive(bool isErrorCorrectionActive); // Set the isErrorCorrectionActive value
|
|
||||||
};
|
|
||||||
|
|
||||||
// Return true if the body is in at least one constraint
|
|
||||||
inline bool ConstraintSolver::isConstrainedBody(Body* body) const {
|
|
||||||
if(constraintBodies.find(body) != constraintBodies.end()) {
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Return the constrained linear velocity of a body after solving the LCP problem
|
|
||||||
inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(Body* body) {
|
|
||||||
assert(isConstrainedBody(body));
|
|
||||||
uint indexBodyArray = 6 * bodyNumberMapping[body];
|
|
||||||
return Vector3(Vconstraint[indexBodyArray], Vconstraint[indexBodyArray + 1], Vconstraint[indexBodyArray + 2]);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Return the constrained angular velocity of a body after solving the LCP problem
|
|
||||||
inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body) {
|
|
||||||
assert(isConstrainedBody(body));
|
|
||||||
uint indexBodyArray = 6 * bodyNumberMapping[body];
|
|
||||||
return Vector3(Vconstraint[indexBodyArray + 3], Vconstraint[indexBodyArray + 4], Vconstraint[indexBodyArray + 5]);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Return the constrained linear velocity of a body after solving the LCP problem for error correction
|
|
||||||
inline Vector3 ConstraintSolver::getErrorConstrainedLinearVelocityOfBody(Body* body) {
|
|
||||||
//assert(isConstrainedBody(body));
|
|
||||||
uint indexBodyArray = 6 * bodyNumberMapping[body];
|
|
||||||
return Vector3(VconstraintError[indexBodyArray], VconstraintError[indexBodyArray + 1], VconstraintError[indexBodyArray + 2]);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Return the constrained angular velocity of a body after solving the LCP problem for error correction
|
|
||||||
inline Vector3 ConstraintSolver::getErrorConstrainedAngularVelocityOfBody(Body* body) {
|
|
||||||
//assert(isConstrainedBody(body));
|
|
||||||
uint indexBodyArray = 6 * bodyNumberMapping[body];
|
|
||||||
return Vector3(VconstraintError[indexBodyArray + 3], VconstraintError[indexBodyArray + 4], VconstraintError[indexBodyArray + 5]);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Cleanup of the constraint solver
|
|
||||||
inline void ConstraintSolver::cleanup() {
|
|
||||||
bodyNumberMapping.clear();
|
|
||||||
constraintBodies.clear();
|
|
||||||
activeConstraints.clear();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Set the number of iterations of the LCP solver
|
|
||||||
inline void ConstraintSolver::setNbLCPIterations(uint nbIterations) {
|
|
||||||
nbIterationsLCP = nbIterations;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// Set the isErrorCorrectionActive value
|
|
||||||
inline void ConstraintSolver::setIsErrorCorrectionActive(bool isErrorCorrectionActive) {
|
|
||||||
this->isErrorCorrectionActive = isErrorCorrectionActive;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// Solve the current LCP problem
|
|
||||||
inline void ConstraintSolver::solve(decimal dt) {
|
|
||||||
|
|
||||||
// Initialize the solver
|
|
||||||
initialize();
|
|
||||||
|
|
||||||
// Fill-in all the matrices needed to solve the LCP problem
|
|
||||||
fillInMatrices(dt);
|
|
||||||
|
|
||||||
// Compute the vector b
|
|
||||||
computeVectorB(dt);
|
|
||||||
if (isErrorCorrectionActive) {
|
|
||||||
computeVectorBError(dt);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Compute the matrix B
|
|
||||||
computeMatrixB_sp();
|
|
||||||
if (isErrorCorrectionActive) {
|
|
||||||
computeMatrixB_spErrorCorrection();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Solve the LCP problem (computation of lambda)
|
|
||||||
solveLCP();
|
|
||||||
if (isErrorCorrectionActive) {
|
|
||||||
solveLCPErrorCorrection();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Cache the lambda values in order to use them in the next step
|
|
||||||
cacheLambda();
|
|
||||||
|
|
||||||
// Compute the vector Vconstraint
|
|
||||||
computeVectorVconstraint(dt);
|
|
||||||
if (isErrorCorrectionActive) {
|
|
||||||
computeVectorVconstraintError(dt);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
} // End of ReactPhysics3D namespace
|
|
||||||
|
|
||||||
#endif
|
|
247
src/engine/ContactManifold.cpp
Normal file
247
src/engine/ContactManifold.cpp
Normal file
|
@ -0,0 +1,247 @@
|
||||||
|
/********************************************************************************
|
||||||
|
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
|
||||||
|
* Copyright (c) 2010-2012 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 "ContactManifold.h"
|
||||||
|
|
||||||
|
using namespace reactphysics3d;
|
||||||
|
|
||||||
|
// Constructor
|
||||||
|
ContactManifold::ContactManifold(Body* const body1, Body* const body2,
|
||||||
|
MemoryPool<ContactPoint>& memoryPoolContacts)
|
||||||
|
: mBody1(body1), mBody2(body2), mNbContactPoints(0), mFrictionImpulse1(0.0),
|
||||||
|
mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0),
|
||||||
|
mMemoryPoolContacts(memoryPoolContacts) {
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// Destructor
|
||||||
|
ContactManifold::~ContactManifold() {
|
||||||
|
clear();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Add a contact point in the manifold
|
||||||
|
void ContactManifold::addContactPoint(ContactPoint* contact) {
|
||||||
|
|
||||||
|
// For contact already in the manifold
|
||||||
|
for (uint i=0; i<mNbContactPoints; i++) {
|
||||||
|
|
||||||
|
// Check if the new point point does not correspond to a same contact point
|
||||||
|
// already in the manifold.
|
||||||
|
decimal distance = (mContactPoints[i]->getWorldPointOnBody1() - contact->getWorldPointOnBody1()).lengthSquare();
|
||||||
|
if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) {
|
||||||
|
|
||||||
|
// Delete the new contact
|
||||||
|
contact->ContactPoint::~ContactPoint();
|
||||||
|
mMemoryPoolContacts.freeObject(contact);
|
||||||
|
//removeContact(i);
|
||||||
|
|
||||||
|
return;
|
||||||
|
//break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// If the contact manifold is full
|
||||||
|
if (mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) {
|
||||||
|
int indexMaxPenetration = getIndexOfDeepestPenetration(contact);
|
||||||
|
int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1());
|
||||||
|
removeContactPoint(indexToRemove);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Add the new contact point in the manifold
|
||||||
|
mContactPoints[mNbContactPoints] = contact;
|
||||||
|
mNbContactPoints++;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Remove a contact point from the manifold
|
||||||
|
void ContactManifold::removeContactPoint(int index) {
|
||||||
|
assert(index >= 0 && index < mNbContactPoints);
|
||||||
|
assert(mNbContactPoints > 0);
|
||||||
|
|
||||||
|
// Call the destructor explicitly and tell the memory pool that
|
||||||
|
// the corresponding memory block is now free
|
||||||
|
mContactPoints[index]->ContactPoint::~ContactPoint();
|
||||||
|
mMemoryPoolContacts.freeObject(mContactPoints[index]);
|
||||||
|
|
||||||
|
// If we don't remove the last index
|
||||||
|
if (index < mNbContactPoints - 1) {
|
||||||
|
mContactPoints[index] = mContactPoints[mNbContactPoints - 1];
|
||||||
|
}
|
||||||
|
|
||||||
|
mNbContactPoints--;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Update the contact manifold
|
||||||
|
// First the world space coordinates of the current contacts in the manifold are recomputed from
|
||||||
|
// the corresponding transforms of the bodies because they have moved. Then we remove the contacts
|
||||||
|
// with a negative penetration depth (meaning that the bodies are not penetrating anymore) and also
|
||||||
|
// the contacts with a too large distance between the contact points in the plane orthogonal to the
|
||||||
|
// contact normal
|
||||||
|
void ContactManifold::update(const Transform& transform1, const Transform& transform2) {
|
||||||
|
if (mNbContactPoints == 0) return;
|
||||||
|
|
||||||
|
// Update the world coordinates and penetration depth of the contact points in the manifold
|
||||||
|
for (int i=0; i<mNbContactPoints; i++) {
|
||||||
|
mContactPoints[i]->setWorldPointOnBody1(transform1 * mContactPoints[i]->getLocalPointOnBody1());
|
||||||
|
mContactPoints[i]->setWorldPointOnBody2(transform2 * mContactPoints[i]->getLocalPointOnBody2());
|
||||||
|
mContactPoints[i]->setPenetrationDepth((mContactPoints[i]->getWorldPointOnBody1() - mContactPoints[i]->getWorldPointOnBody2()).dot(mContactPoints[i]->getNormal()));
|
||||||
|
}
|
||||||
|
|
||||||
|
const decimal squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD *
|
||||||
|
PERSISTENT_CONTACT_DIST_THRESHOLD;
|
||||||
|
|
||||||
|
// Remove the contact points that don't represent very well the contact manifold
|
||||||
|
for (int i=mNbContactPoints-1; i>=0; i--) {
|
||||||
|
assert(i>= 0 && i < mNbContactPoints);
|
||||||
|
|
||||||
|
// Compute the distance between contact points in the normal direction
|
||||||
|
decimal distanceNormal = -mContactPoints[i]->getPenetrationDepth();
|
||||||
|
|
||||||
|
// If the contacts points are too far from each other in the normal direction
|
||||||
|
if (distanceNormal > squarePersistentContactThreshold) {
|
||||||
|
removeContactPoint(i);
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
// Compute the distance of the two contact points in the plane
|
||||||
|
// orthogonal to the contact normal
|
||||||
|
Vector3 projOfPoint1 = mContactPoints[i]->getWorldPointOnBody1() +
|
||||||
|
mContactPoints[i]->getNormal() * distanceNormal;
|
||||||
|
Vector3 projDifference = mContactPoints[i]->getWorldPointOnBody2() - projOfPoint1;
|
||||||
|
|
||||||
|
// If the orthogonal distance is larger than the valid distance
|
||||||
|
// threshold, we remove the contact
|
||||||
|
if (projDifference.lengthSquare() > squarePersistentContactThreshold) {
|
||||||
|
removeContactPoint(i);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Return the index of the contact point with the larger penetration depth. This
|
||||||
|
// corresponding contact will be kept in the cache. The method returns -1 is
|
||||||
|
// the new contact is the deepest.
|
||||||
|
int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) const {
|
||||||
|
assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
|
||||||
|
int indexMaxPenetrationDepth = -1;
|
||||||
|
decimal maxPenetrationDepth = newContact->getPenetrationDepth();
|
||||||
|
|
||||||
|
// For each contact in the cache
|
||||||
|
for (uint i=0; i<mNbContactPoints; i++) {
|
||||||
|
// If the current contact has a larger penetration depth
|
||||||
|
if (mContactPoints[i]->getPenetrationDepth() > maxPenetrationDepth) {
|
||||||
|
maxPenetrationDepth = mContactPoints[i]->getPenetrationDepth();
|
||||||
|
indexMaxPenetrationDepth = i;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Return the index of largest penetration depth
|
||||||
|
return indexMaxPenetrationDepth;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Return the index that will be removed. The index of the contact point with the larger penetration
|
||||||
|
// depth is given as a parameter. This contact won't be removed. Given this contact, we compute
|
||||||
|
// the different area and we want to keep the contacts with the largest area. The new point is also
|
||||||
|
// kept. In order to compute the area of a quadrilateral, we use the formula :
|
||||||
|
// Area = 0.5 * | AC x BD | where AC and BD form the diagonals of the quadrilateral. Note that
|
||||||
|
// when we compute this area, we do not calculate it exactly but we
|
||||||
|
// only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore,
|
||||||
|
// this is only a guess that is faster to compute.
|
||||||
|
int ContactManifold::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const {
|
||||||
|
|
||||||
|
assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
|
||||||
|
|
||||||
|
decimal area0 = 0.0; // Area with contact 1,2,3 and newPoint
|
||||||
|
decimal area1 = 0.0; // Area with contact 0,2,3 and newPoint
|
||||||
|
decimal area2 = 0.0; // Area with contact 0,1,3 and newPoint
|
||||||
|
decimal area3 = 0.0; // Area with contact 0,1,2 and newPoint
|
||||||
|
|
||||||
|
if (indexMaxPenetration != 0) {
|
||||||
|
// Compute the area
|
||||||
|
Vector3 vector1 = newPoint - mContactPoints[1]->getLocalPointOnBody1();
|
||||||
|
Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[2]->getLocalPointOnBody1();
|
||||||
|
Vector3 crossProduct = vector1.cross(vector2);
|
||||||
|
area0 = crossProduct.lengthSquare();
|
||||||
|
}
|
||||||
|
if (indexMaxPenetration != 1) {
|
||||||
|
// Compute the area
|
||||||
|
Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
|
||||||
|
Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[2]->getLocalPointOnBody1();
|
||||||
|
Vector3 crossProduct = vector1.cross(vector2);
|
||||||
|
area1 = crossProduct.lengthSquare();
|
||||||
|
}
|
||||||
|
if (indexMaxPenetration != 2) {
|
||||||
|
// Compute the area
|
||||||
|
Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
|
||||||
|
Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[1]->getLocalPointOnBody1();
|
||||||
|
Vector3 crossProduct = vector1.cross(vector2);
|
||||||
|
area2 = crossProduct.lengthSquare();
|
||||||
|
}
|
||||||
|
if (indexMaxPenetration != 3) {
|
||||||
|
// Compute the area
|
||||||
|
Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
|
||||||
|
Vector3 vector2 = mContactPoints[2]->getLocalPointOnBody1() - mContactPoints[1]->getLocalPointOnBody1();
|
||||||
|
Vector3 crossProduct = vector1.cross(vector2);
|
||||||
|
area3 = crossProduct.lengthSquare();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Return the index of the contact to remove
|
||||||
|
return getMaxArea(area0, area1, area2, area3);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Return the index of maximum area
|
||||||
|
int ContactManifold::getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const {
|
||||||
|
if (area0 < area1) {
|
||||||
|
if (area1 < area2) {
|
||||||
|
if (area2 < area3) return 3;
|
||||||
|
else return 2;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
if (area1 < area3) return 3;
|
||||||
|
else return 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
if (area0 < area2) {
|
||||||
|
if (area2 < area3) return 3;
|
||||||
|
else return 2;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
if (area0 < area3) return 3;
|
||||||
|
else return 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Clear the contact manifold
|
||||||
|
void ContactManifold::clear() {
|
||||||
|
for (uint i=0; i<mNbContactPoints; i++) {
|
||||||
|
|
||||||
|
// Call the destructor explicitly and tell the memory pool that
|
||||||
|
// the corresponding memory block is now free
|
||||||
|
mContactPoints[i]->ContactPoint::~ContactPoint();
|
||||||
|
mMemoryPoolContacts.freeObject(mContactPoints[i]);
|
||||||
|
}
|
||||||
|
mNbContactPoints = 0;
|
||||||
|
}
|
244
src/engine/ContactManifold.h
Normal file
244
src/engine/ContactManifold.h
Normal file
|
@ -0,0 +1,244 @@
|
||||||
|
/********************************************************************************
|
||||||
|
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
|
||||||
|
* Copyright (c) 2010-2012 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 CONTACT_MANIFOLD_H
|
||||||
|
#define CONTACT_MANIFOLD_H
|
||||||
|
|
||||||
|
// Libraries
|
||||||
|
#include <vector>
|
||||||
|
#include "../body/Body.h"
|
||||||
|
#include "../constraint/ContactPoint.h"
|
||||||
|
|
||||||
|
// ReactPhysics3D namespace
|
||||||
|
namespace reactphysics3d {
|
||||||
|
|
||||||
|
// Constants
|
||||||
|
const uint MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold
|
||||||
|
|
||||||
|
/* -------------------------------------------------------------------
|
||||||
|
Class ContactManifold :
|
||||||
|
This class represents the set of contact points between two bodies.
|
||||||
|
The contact manifold is implemented in a way to cache the contact
|
||||||
|
points among the frames for better stability following the
|
||||||
|
"Contact Generation" presentation of Erwin Coumans at GDC 2010
|
||||||
|
conference (bullet.googlecode.com/files/GDC10_Coumans_Erwin_Contact.pdf).
|
||||||
|
Some code of this class is based on the implementation of the
|
||||||
|
btPersistentManifold class from Bullet physics engine (www.http://bulletphysics.org).
|
||||||
|
The contacts between two bodies are added one after the other in the cache.
|
||||||
|
When the cache is full, we have to remove one point. The idea is to keep
|
||||||
|
the point with the deepest penetration depth and also to keep the
|
||||||
|
points producing the larger area (for a more stable contact manifold).
|
||||||
|
The new added point is always kept.
|
||||||
|
-------------------------------------------------------------------
|
||||||
|
*/
|
||||||
|
class ContactManifold {
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
// -------------------- Attributes -------------------- //
|
||||||
|
|
||||||
|
// Pointer to the first body
|
||||||
|
Body* const mBody1;
|
||||||
|
|
||||||
|
// Pointer to the second body
|
||||||
|
Body* const mBody2;
|
||||||
|
|
||||||
|
// Contact points in the manifold
|
||||||
|
ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD];
|
||||||
|
|
||||||
|
// Number of contacts in the cache
|
||||||
|
uint mNbContactPoints;
|
||||||
|
|
||||||
|
// First friction vector of the contact manifold
|
||||||
|
Vector3 mFrictionVector1;
|
||||||
|
|
||||||
|
// Second friction vector of the contact manifold
|
||||||
|
Vector3 mFrictionVector2;
|
||||||
|
|
||||||
|
// First friction constraint accumulated impulse
|
||||||
|
decimal mFrictionImpulse1;
|
||||||
|
|
||||||
|
// Second friction constraint accumulated impulse
|
||||||
|
decimal mFrictionImpulse2;
|
||||||
|
|
||||||
|
// Twist friction constraint accumulated impulse
|
||||||
|
decimal mFrictionTwistImpulse;
|
||||||
|
|
||||||
|
// Reference to the memory pool with the contacts
|
||||||
|
MemoryPool<ContactPoint>& mMemoryPoolContacts;
|
||||||
|
|
||||||
|
// -------------------- Methods -------------------- //
|
||||||
|
|
||||||
|
// Private copy-constructor
|
||||||
|
ContactManifold(const ContactManifold& contactManifold);
|
||||||
|
|
||||||
|
// Private assignment operator
|
||||||
|
ContactManifold& operator=(const ContactManifold& contactManifold);
|
||||||
|
|
||||||
|
// Return the index of maximum area
|
||||||
|
int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const;
|
||||||
|
|
||||||
|
// Return the index of the contact with the larger penetration depth
|
||||||
|
int getIndexOfDeepestPenetration(ContactPoint* newContact) const;
|
||||||
|
|
||||||
|
// Return the index that will be removed
|
||||||
|
int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const;
|
||||||
|
|
||||||
|
// Remove a contact point from the manifold
|
||||||
|
void removeContactPoint(int index);
|
||||||
|
|
||||||
|
// Return true if two vectors are approximatively equal
|
||||||
|
bool isApproxEqual(const Vector3& vector1, const Vector3& vector2) const;
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
// -------------------- Methods -------------------- //
|
||||||
|
|
||||||
|
// Constructor
|
||||||
|
ContactManifold(Body* const mBody1, Body* const mBody2,
|
||||||
|
MemoryPool<ContactPoint>& mMemoryPoolContacts);
|
||||||
|
|
||||||
|
// Destructor
|
||||||
|
~ContactManifold();
|
||||||
|
|
||||||
|
// Add a contact point to the manifold
|
||||||
|
void addContactPoint(ContactPoint* contact);
|
||||||
|
|
||||||
|
// Update the contact manifold
|
||||||
|
void update(const Transform& transform1, const Transform& transform2);
|
||||||
|
|
||||||
|
// Clear the contact manifold
|
||||||
|
void clear();
|
||||||
|
|
||||||
|
// Return the number of contact points in the manifold
|
||||||
|
uint getNbContactPoints() const;
|
||||||
|
|
||||||
|
// Return the first friction vector at the center of the contact manifold
|
||||||
|
const Vector3& getFrictionVector1() const;
|
||||||
|
|
||||||
|
// set the first friction vector at the center of the contact manifold
|
||||||
|
void setFrictionVector1(const Vector3& mFrictionVector1);
|
||||||
|
|
||||||
|
// Return the second friction vector at the center of the contact manifold
|
||||||
|
const Vector3& getFrictionVector2() const;
|
||||||
|
|
||||||
|
// set the second friction vector at the center of the contact manifold
|
||||||
|
void setFrictionVector2(const Vector3& mFrictionVector2);
|
||||||
|
|
||||||
|
// Return the first friction accumulated impulse
|
||||||
|
decimal getFrictionImpulse1() const;
|
||||||
|
|
||||||
|
// Set the first friction accumulated impulse
|
||||||
|
void setFrictionImpulse1(decimal frictionImpulse1);
|
||||||
|
|
||||||
|
// Return the second friction accumulated impulse
|
||||||
|
decimal getFrictionImpulse2() const;
|
||||||
|
|
||||||
|
// Set the second friction accumulated impulse
|
||||||
|
void setFrictionImpulse2(decimal frictionImpulse2);
|
||||||
|
|
||||||
|
// Return the friction twist accumulated impulse
|
||||||
|
decimal getFrictionTwistImpulse() const;
|
||||||
|
|
||||||
|
// Set the friction twist accumulated impulse
|
||||||
|
void setFrictionTwistImpulse(decimal frictionTwistImpulse);
|
||||||
|
|
||||||
|
// Return a contact point of the manifold
|
||||||
|
ContactPoint* getContactPoint(uint index) const;
|
||||||
|
};
|
||||||
|
|
||||||
|
// Return the number of contact points in the manifold
|
||||||
|
inline uint ContactManifold::getNbContactPoints() const {
|
||||||
|
return mNbContactPoints;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Return the first friction vector at the center of the contact manifold
|
||||||
|
inline const Vector3& ContactManifold::getFrictionVector1() const {
|
||||||
|
return mFrictionVector1;
|
||||||
|
}
|
||||||
|
|
||||||
|
// set the first friction vector at the center of the contact manifold
|
||||||
|
inline void ContactManifold::setFrictionVector1(const Vector3& frictionVector1) {
|
||||||
|
mFrictionVector1 = frictionVector1;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Return the second friction vector at the center of the contact manifold
|
||||||
|
inline const Vector3& ContactManifold::getFrictionVector2() const {
|
||||||
|
return mFrictionVector2;
|
||||||
|
}
|
||||||
|
|
||||||
|
// set the second friction vector at the center of the contact manifold
|
||||||
|
inline void ContactManifold::setFrictionVector2(const Vector3& frictionVector2) {
|
||||||
|
mFrictionVector2 = frictionVector2;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Return the first friction accumulated impulse
|
||||||
|
inline decimal ContactManifold::getFrictionImpulse1() const {
|
||||||
|
return mFrictionImpulse1;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set the first friction accumulated impulse
|
||||||
|
inline void ContactManifold::setFrictionImpulse1(decimal frictionImpulse1) {
|
||||||
|
mFrictionImpulse1 = frictionImpulse1;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Return the second friction accumulated impulse
|
||||||
|
inline decimal ContactManifold::getFrictionImpulse2() const {
|
||||||
|
return mFrictionImpulse2;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set the second friction accumulated impulse
|
||||||
|
inline void ContactManifold::setFrictionImpulse2(decimal frictionImpulse2) {
|
||||||
|
mFrictionImpulse2 = frictionImpulse2;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Return the friction twist accumulated impulse
|
||||||
|
inline decimal ContactManifold::getFrictionTwistImpulse() const {
|
||||||
|
return mFrictionTwistImpulse;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set the friction twist accumulated impulse
|
||||||
|
inline void ContactManifold::setFrictionTwistImpulse(decimal frictionTwistImpulse) {
|
||||||
|
mFrictionTwistImpulse = frictionTwistImpulse;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Return a contact point of the manifold
|
||||||
|
inline ContactPoint* ContactManifold::getContactPoint(uint index) const {
|
||||||
|
assert(index >= 0 && index < mNbContactPoints);
|
||||||
|
return mContactPoints[index];
|
||||||
|
}
|
||||||
|
|
||||||
|
// Return true if two vectors are approximatively equal
|
||||||
|
inline bool ContactManifold::isApproxEqual(const Vector3& vector1,
|
||||||
|
const Vector3& vector2) const {
|
||||||
|
const decimal epsilon = decimal(0.1);
|
||||||
|
return (approxEqual(vector1.getX(), vector2.getX(), epsilon) &&
|
||||||
|
approxEqual(vector1.getY(), vector2.getY(), epsilon) &&
|
||||||
|
approxEqual(vector1.getZ(), vector2.getZ(), epsilon));
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
915
src/engine/ContactSolver.cpp
Normal file
915
src/engine/ContactSolver.cpp
Normal file
|
@ -0,0 +1,915 @@
|
||||||
|
/********************************************************************************
|
||||||
|
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
|
||||||
|
* Copyright (c) 2010-2012 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 "ContactSolver.h"
|
||||||
|
#include "DynamicsWorld.h"
|
||||||
|
#include "../body/RigidBody.h"
|
||||||
|
|
||||||
|
using namespace reactphysics3d;
|
||||||
|
using namespace std;
|
||||||
|
|
||||||
|
// Constants initialization
|
||||||
|
const decimal ContactSolver::BETA = decimal(0.2);
|
||||||
|
const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
|
||||||
|
const decimal ContactSolver::SLOP = decimal(0.01);
|
||||||
|
|
||||||
|
// Constructor
|
||||||
|
ContactSolver::ContactSolver(DynamicsWorld& world,std::vector<Vector3>& constrainedLinearVelocities,
|
||||||
|
std::vector<Vector3>& constrainedAngularVelocities,
|
||||||
|
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
|
||||||
|
:mWorld(world), mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS),
|
||||||
|
mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL),
|
||||||
|
mContactConstraints(NULL),
|
||||||
|
mConstrainedLinearVelocities(constrainedLinearVelocities),
|
||||||
|
mConstrainedAngularVelocities(constrainedAngularVelocities),
|
||||||
|
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
|
||||||
|
mIsWarmStartingActive(true), mIsSplitImpulseActive(true),
|
||||||
|
mIsSolveFrictionAtContactManifoldCenterActive(true) {
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// Destructor
|
||||||
|
ContactSolver::~ContactSolver() {
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// Initialize the constraint solver
|
||||||
|
void ContactSolver::initialize() {
|
||||||
|
|
||||||
|
// TODO : Use better memory allocation here
|
||||||
|
mContactConstraints = new ContactManifoldSolver[mWorld.getNbContactManifolds()];
|
||||||
|
|
||||||
|
mNbContactManifolds = 0;
|
||||||
|
|
||||||
|
// For each contact manifold of the world
|
||||||
|
vector<ContactManifold*>::iterator it;
|
||||||
|
for (it = mWorld.getContactManifoldsBeginIterator();
|
||||||
|
it != mWorld.getContactManifoldsEndIterator(); ++it) {
|
||||||
|
|
||||||
|
ContactManifold* externalManifold = *it;
|
||||||
|
|
||||||
|
ContactManifoldSolver& internalManifold = mContactConstraints[mNbContactManifolds];
|
||||||
|
|
||||||
|
assert(externalManifold->getNbContactPoints() > 0);
|
||||||
|
|
||||||
|
// Get the two bodies of the contact
|
||||||
|
RigidBody* body1 = externalManifold->getContactPoint(0)->getBody1();
|
||||||
|
RigidBody* body2 = externalManifold->getContactPoint(0)->getBody2();
|
||||||
|
|
||||||
|
// Add the two bodies of the constraint in the constraintBodies list
|
||||||
|
mConstraintBodies.insert(body1);
|
||||||
|
mConstraintBodies.insert(body2);
|
||||||
|
|
||||||
|
// Get the position of the two bodies
|
||||||
|
Vector3 x1 = body1->getTransform().getPosition();
|
||||||
|
Vector3 x2 = body2->getTransform().getPosition();
|
||||||
|
|
||||||
|
// Initialize the internal contact manifold structure using the external
|
||||||
|
// contact manifold
|
||||||
|
internalManifold.indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second;
|
||||||
|
internalManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second;
|
||||||
|
internalManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
|
||||||
|
internalManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
|
||||||
|
internalManifold.isBody1Moving = body1->getIsMotionEnabled();
|
||||||
|
internalManifold.isBody2Moving = body2->getIsMotionEnabled();
|
||||||
|
internalManifold.massInverseBody1 = body1->getMassInverse();
|
||||||
|
internalManifold.massInverseBody2 = body2->getMassInverse();
|
||||||
|
internalManifold.nbContacts = externalManifold->getNbContactPoints();
|
||||||
|
internalManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2);
|
||||||
|
internalManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
|
||||||
|
internalManifold.externalContactManifold = externalManifold;
|
||||||
|
|
||||||
|
// If we solve the friction constraints at the center of the contact manifold
|
||||||
|
if (mIsSolveFrictionAtContactManifoldCenterActive) {
|
||||||
|
internalManifold.frictionPointBody1 = Vector3(0.0, 0.0, 0.0);
|
||||||
|
internalManifold.frictionPointBody2 = Vector3(0.0, 0.0, 0.0);
|
||||||
|
}
|
||||||
|
|
||||||
|
// For each contact point of the contact manifold
|
||||||
|
for (uint c=0; c<externalManifold->getNbContactPoints(); c++) {
|
||||||
|
|
||||||
|
ContactPointSolver& contactPoint = internalManifold.contacts[c];
|
||||||
|
|
||||||
|
// Get a contact point
|
||||||
|
ContactPoint* externalContact = externalManifold->getContactPoint(c);
|
||||||
|
|
||||||
|
// Get the contact point on the two bodies
|
||||||
|
Vector3 p1 = externalContact->getWorldPointOnBody1();
|
||||||
|
Vector3 p2 = externalContact->getWorldPointOnBody2();
|
||||||
|
|
||||||
|
contactPoint.externalContact = externalContact;
|
||||||
|
contactPoint.normal = externalContact->getNormal();
|
||||||
|
contactPoint.r1 = p1 - x1;
|
||||||
|
contactPoint.r2 = p2 - x2;
|
||||||
|
contactPoint.penetrationDepth = externalContact->getPenetrationDepth();
|
||||||
|
contactPoint.isRestingContact = externalContact->getIsRestingContact();
|
||||||
|
externalContact->setIsRestingContact(true);
|
||||||
|
contactPoint.oldFrictionVector1 = externalContact->getFrictionVector1();
|
||||||
|
contactPoint.oldFrictionVector2 = externalContact->getFrictionVector2();
|
||||||
|
contactPoint.penetrationImpulse = 0.0;
|
||||||
|
contactPoint.friction1Impulse = 0.0;
|
||||||
|
contactPoint.friction2Impulse = 0.0;
|
||||||
|
|
||||||
|
// If we solve the friction constraints at the center of the contact manifold
|
||||||
|
if (mIsSolveFrictionAtContactManifoldCenterActive) {
|
||||||
|
internalManifold.frictionPointBody1 += p1;
|
||||||
|
internalManifold.frictionPointBody2 += p2;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// If we solve the friction constraints at the center of the contact manifold
|
||||||
|
if (mIsSolveFrictionAtContactManifoldCenterActive) {
|
||||||
|
|
||||||
|
internalManifold.frictionPointBody1 /=static_cast<decimal>(internalManifold.nbContacts);
|
||||||
|
internalManifold.frictionPointBody2 /=static_cast<decimal>(internalManifold.nbContacts);
|
||||||
|
internalManifold.r1Friction = internalManifold.frictionPointBody1 - x1;
|
||||||
|
internalManifold.r2Friction = internalManifold.frictionPointBody2 - x2;
|
||||||
|
internalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1();
|
||||||
|
internalManifold.oldFrictionVector2 = externalManifold->getFrictionVector2();
|
||||||
|
|
||||||
|
// If warm starting is active
|
||||||
|
if (mIsWarmStartingActive) {
|
||||||
|
|
||||||
|
// Initialize the accumulated impulses with the previous step accumulated impulses
|
||||||
|
internalManifold.friction1Impulse = externalManifold->getFrictionImpulse1();
|
||||||
|
internalManifold.friction2Impulse = externalManifold->getFrictionImpulse2();
|
||||||
|
internalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse();
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
|
||||||
|
// Initialize the accumulated impulses to zero
|
||||||
|
internalManifold.friction1Impulse = 0.0;
|
||||||
|
internalManifold.friction2Impulse = 0.0;
|
||||||
|
internalManifold.frictionTwistImpulse = 0.0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
mNbContactManifolds++;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Allocated memory for split impulse velocities
|
||||||
|
// TODO : Use better memory allocation here
|
||||||
|
mSplitLinearVelocities = new Vector3[mWorld.getNbRigidBodies()];
|
||||||
|
mSplitAngularVelocities = new Vector3[mWorld.getNbRigidBodies()];
|
||||||
|
assert(mSplitLinearVelocities != NULL);
|
||||||
|
assert(mSplitAngularVelocities != NULL);
|
||||||
|
|
||||||
|
assert(mConstraintBodies.size() > 0);
|
||||||
|
assert(mMapBodyToConstrainedVelocityIndex.size() >= mConstraintBodies.size());
|
||||||
|
assert(mConstrainedLinearVelocities.size() >= mConstraintBodies.size());
|
||||||
|
assert(mConstrainedAngularVelocities.size() >= mConstraintBodies.size());
|
||||||
|
|
||||||
|
// Initialize the split impulse velocities
|
||||||
|
initializeSplitImpulseVelocities();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Initialize the split impulse velocities
|
||||||
|
void ContactSolver::initializeSplitImpulseVelocities() {
|
||||||
|
|
||||||
|
// For each current body that is implied in some constraint
|
||||||
|
set<RigidBody*>::iterator it;
|
||||||
|
for (it = mConstraintBodies.begin(); it != mConstraintBodies.end(); ++it) {
|
||||||
|
RigidBody* rigidBody = *it;
|
||||||
|
assert(rigidBody);
|
||||||
|
|
||||||
|
uint bodyNumber = mMapBodyToConstrainedVelocityIndex.find(rigidBody)->second;
|
||||||
|
|
||||||
|
// Initialize the split impulse velocities to zero
|
||||||
|
mSplitLinearVelocities[bodyNumber] = Vector3(0, 0, 0);
|
||||||
|
mSplitAngularVelocities[bodyNumber] = Vector3(0, 0, 0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Initialize the contact constraints before solving the system
|
||||||
|
void ContactSolver::initializeContactConstraints() {
|
||||||
|
|
||||||
|
// For each contact constraint
|
||||||
|
for (uint c=0; c<mNbContactManifolds; c++) {
|
||||||
|
|
||||||
|
ContactManifoldSolver& manifold = mContactConstraints[c];
|
||||||
|
|
||||||
|
// Get the inertia tensors of both bodies
|
||||||
|
Matrix3x3& I1 = manifold.inverseInertiaTensorBody1;
|
||||||
|
Matrix3x3& I2 = manifold.inverseInertiaTensorBody2;
|
||||||
|
|
||||||
|
// If we solve the friction constraints at the center of the contact manifold
|
||||||
|
if (mIsSolveFrictionAtContactManifoldCenterActive) {
|
||||||
|
manifold.normal = Vector3(0.0, 0.0, 0.0);
|
||||||
|
}
|
||||||
|
|
||||||
|
// 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];
|
||||||
|
|
||||||
|
// For each contact point constraint
|
||||||
|
for (uint i=0; i<manifold.nbContacts; i++) {
|
||||||
|
|
||||||
|
ContactPointSolver& contactPoint = manifold.contacts[i];
|
||||||
|
ContactPoint* externalContact = contactPoint.externalContact;
|
||||||
|
|
||||||
|
// Compute the velocity difference
|
||||||
|
Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
|
||||||
|
|
||||||
|
contactPoint.r1CrossN = contactPoint.r1.cross(contactPoint.normal);
|
||||||
|
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 +
|
||||||
|
((I2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal);
|
||||||
|
}
|
||||||
|
massPenetration > 0.0 ? contactPoint.inversePenetrationMass = decimal(1.0) /
|
||||||
|
massPenetration :
|
||||||
|
decimal(0.0);
|
||||||
|
|
||||||
|
// If we do not solve the friction constraints at the center of the contact manifold
|
||||||
|
if (!mIsSolveFrictionAtContactManifoldCenterActive) {
|
||||||
|
|
||||||
|
// Compute the friction vectors
|
||||||
|
computeFrictionVectors(deltaV, contactPoint);
|
||||||
|
|
||||||
|
contactPoint.r1CrossT1 = contactPoint.r1.cross(contactPoint.frictionVector1);
|
||||||
|
contactPoint.r1CrossT2 = contactPoint.r1.cross(contactPoint.frictionVector2);
|
||||||
|
contactPoint.r2CrossT1 = contactPoint.r2.cross(contactPoint.frictionVector1);
|
||||||
|
contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionVector2);
|
||||||
|
|
||||||
|
// 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);
|
||||||
|
}
|
||||||
|
friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = decimal(1.0) /
|
||||||
|
friction1Mass :
|
||||||
|
decimal(0.0);
|
||||||
|
friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = decimal(1.0) /
|
||||||
|
friction2Mass :
|
||||||
|
decimal(0.0);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute the restitution velocity bias "b". We compute this here instead
|
||||||
|
// of inside the solve() method because we need to use the velocity difference
|
||||||
|
// at the beginning of the contact. Note that if it is a resting contact (normal
|
||||||
|
// velocity under a given threshold), we don't add a restitution velocity bias
|
||||||
|
contactPoint.restitutionBias = 0.0;
|
||||||
|
decimal deltaVDotN = deltaV.dot(contactPoint.normal);
|
||||||
|
if (deltaVDotN < RESTITUTION_VELOCITY_THRESHOLD) {
|
||||||
|
contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN;
|
||||||
|
}
|
||||||
|
|
||||||
|
// If the warm starting of the contact solver is active
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Initialize the split impulses to zero
|
||||||
|
contactPoint.penetrationSplitImpulse = 0.0;
|
||||||
|
|
||||||
|
// If we solve the friction constraints at the center of the contact manifold
|
||||||
|
if (mIsSolveFrictionAtContactManifoldCenterActive) {
|
||||||
|
manifold.normal += contactPoint.normal;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// If we solve the friction constraints at the center of the contact manifold
|
||||||
|
if (mIsSolveFrictionAtContactManifoldCenterActive) {
|
||||||
|
|
||||||
|
manifold.normal.normalize();
|
||||||
|
|
||||||
|
Vector3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) -
|
||||||
|
v1 - w1.cross(manifold.r1Friction);
|
||||||
|
|
||||||
|
// Compute the friction vectors
|
||||||
|
computeFrictionVectors(deltaVFrictionPoint, manifold);
|
||||||
|
|
||||||
|
// Compute the inverse mass matrix K for the friction constraints at the center of
|
||||||
|
// the contact manifold
|
||||||
|
manifold.r1CrossT1 = manifold.r1Friction.cross(manifold.frictionVector1);
|
||||||
|
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 *
|
||||||
|
manifold.normal) +
|
||||||
|
manifold.normal.dot(
|
||||||
|
manifold.inverseInertiaTensorBody2 *
|
||||||
|
manifold.normal);
|
||||||
|
friction1Mass > 0.0 ? manifold.inverseFriction1Mass = decimal(1.0)/friction1Mass
|
||||||
|
: decimal(0.0);
|
||||||
|
friction2Mass > 0.0 ? manifold.inverseFriction2Mass = decimal(1.0)/friction2Mass
|
||||||
|
: decimal(0.0);
|
||||||
|
frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = decimal(1.0) /
|
||||||
|
frictionTwistMass :
|
||||||
|
decimal(0.0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Warm start the solver
|
||||||
|
// For each constraint, we apply the previous impulse (from the previous step)
|
||||||
|
// at the beginning. With this technique, we will converge faster towards
|
||||||
|
// the solution of the linear system
|
||||||
|
void ContactSolver::warmStart() {
|
||||||
|
|
||||||
|
// For each constraint
|
||||||
|
for (uint c=0; c<mNbContactManifolds; c++) {
|
||||||
|
|
||||||
|
ContactManifoldSolver& contactManifold = mContactConstraints[c];
|
||||||
|
|
||||||
|
bool atLeastOneRestingContactPoint = false;
|
||||||
|
|
||||||
|
for (uint i=0; i<contactManifold.nbContacts; i++) {
|
||||||
|
|
||||||
|
ContactPointSolver& contactPoint = contactManifold.contacts[i];
|
||||||
|
|
||||||
|
// If it is not a new contact (this contact was already existing at last time step)
|
||||||
|
if (contactPoint.isRestingContact) {
|
||||||
|
|
||||||
|
atLeastOneRestingContactPoint = true;
|
||||||
|
|
||||||
|
// --------- Penetration --------- //
|
||||||
|
|
||||||
|
// Compute the impulse P = J^T * lambda
|
||||||
|
const Impulse impulsePenetration = computePenetrationImpulse(
|
||||||
|
contactPoint.penetrationImpulse, contactPoint);
|
||||||
|
|
||||||
|
// Apply the impulse to the bodies of the constraint
|
||||||
|
applyImpulse(impulsePenetration, contactManifold);
|
||||||
|
|
||||||
|
// If we do not solve the friction constraints at the center of the contact manifold
|
||||||
|
if (!mIsSolveFrictionAtContactManifoldCenterActive) {
|
||||||
|
|
||||||
|
// Project the old friction impulses (with old friction vectors) into
|
||||||
|
// the new friction vectors to get the new friction impulses
|
||||||
|
Vector3 oldFrictionImpulse = contactPoint.friction1Impulse *
|
||||||
|
contactPoint.oldFrictionVector1 +
|
||||||
|
contactPoint.friction2Impulse *
|
||||||
|
contactPoint.oldFrictionVector2;
|
||||||
|
contactPoint.friction1Impulse = oldFrictionImpulse.dot(
|
||||||
|
contactPoint.frictionVector1);
|
||||||
|
contactPoint.friction2Impulse = oldFrictionImpulse.dot(
|
||||||
|
contactPoint.frictionVector2);
|
||||||
|
|
||||||
|
// --------- Friction 1 --------- //
|
||||||
|
|
||||||
|
// Compute the impulse P = J^T * lambda
|
||||||
|
const Impulse impulseFriction1 = computeFriction1Impulse(
|
||||||
|
contactPoint.friction1Impulse, contactPoint);
|
||||||
|
|
||||||
|
// Apply the impulses to the bodies of the constraint
|
||||||
|
applyImpulse(impulseFriction1, contactManifold);
|
||||||
|
|
||||||
|
// --------- Friction 2 --------- //
|
||||||
|
|
||||||
|
// Compute the impulse P=J^T * lambda
|
||||||
|
const Impulse impulseFriction2 = computeFriction2Impulse(
|
||||||
|
contactPoint.friction2Impulse, contactPoint);
|
||||||
|
|
||||||
|
// Apply the impulses to the bodies of the constraint
|
||||||
|
applyImpulse(impulseFriction2, contactManifold);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else { // If it is a new contact point
|
||||||
|
|
||||||
|
// Initialize the accumulated impulses to zero
|
||||||
|
contactPoint.penetrationImpulse = 0.0;
|
||||||
|
contactPoint.friction1Impulse = 0.0;
|
||||||
|
contactPoint.friction2Impulse = 0.0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// If we solve the friction constraints at the center of the contact manifold and there is
|
||||||
|
// at least one resting contact point in the contact manifold
|
||||||
|
if (mIsSolveFrictionAtContactManifoldCenterActive && atLeastOneRestingContactPoint) {
|
||||||
|
|
||||||
|
// Project the old friction impulses (with old friction vectors) into the new friction
|
||||||
|
// vectors to get the new friction impulses
|
||||||
|
Vector3 oldFrictionImpulse = contactManifold.friction1Impulse *
|
||||||
|
contactManifold.oldFrictionVector1 +
|
||||||
|
contactManifold.friction2Impulse *
|
||||||
|
contactManifold.oldFrictionVector2;
|
||||||
|
contactManifold.friction1Impulse = oldFrictionImpulse.dot(
|
||||||
|
contactManifold.frictionVector1);
|
||||||
|
contactManifold.friction2Impulse = oldFrictionImpulse.dot(
|
||||||
|
contactManifold.frictionVector2);
|
||||||
|
|
||||||
|
// ------ First friction constraint at the center of the contact manifold ------ //
|
||||||
|
|
||||||
|
// Compute the impulse P = J^T * lambda
|
||||||
|
Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 *
|
||||||
|
contactManifold.friction1Impulse;
|
||||||
|
Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 *
|
||||||
|
contactManifold.friction1Impulse;
|
||||||
|
Vector3 linearImpulseBody2 = contactManifold.frictionVector1 *
|
||||||
|
contactManifold.friction1Impulse;
|
||||||
|
Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 *
|
||||||
|
contactManifold.friction1Impulse;
|
||||||
|
const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1,
|
||||||
|
linearImpulseBody2, angularImpulseBody2);
|
||||||
|
|
||||||
|
// Apply the impulses to the bodies of the constraint
|
||||||
|
applyImpulse(impulseFriction1, contactManifold);
|
||||||
|
|
||||||
|
// ------ Second friction constraint at the center of the contact manifold ----- //
|
||||||
|
|
||||||
|
// Compute the impulse P = J^T * lambda
|
||||||
|
linearImpulseBody1 = -contactManifold.frictionVector2 *
|
||||||
|
contactManifold.friction2Impulse;
|
||||||
|
angularImpulseBody1 = -contactManifold.r1CrossT2 *
|
||||||
|
contactManifold.friction2Impulse;
|
||||||
|
linearImpulseBody2 = contactManifold.frictionVector2 *
|
||||||
|
contactManifold.friction2Impulse;
|
||||||
|
angularImpulseBody2 = contactManifold.r2CrossT2 *
|
||||||
|
contactManifold.friction2Impulse;
|
||||||
|
const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1,
|
||||||
|
linearImpulseBody2, angularImpulseBody2);
|
||||||
|
|
||||||
|
// Apply the impulses to the bodies of the constraint
|
||||||
|
applyImpulse(impulseFriction2, contactManifold);
|
||||||
|
|
||||||
|
// ------ Twist friction constraint at the center of the contact manifold ------ //
|
||||||
|
|
||||||
|
// Compute the impulse P = J^T * lambda
|
||||||
|
linearImpulseBody1 = Vector3(0.0, 0.0, 0.0);
|
||||||
|
angularImpulseBody1 = -contactManifold.normal * contactManifold.frictionTwistImpulse;
|
||||||
|
linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);
|
||||||
|
angularImpulseBody2 = contactManifold.normal * contactManifold.frictionTwistImpulse;
|
||||||
|
const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
|
||||||
|
linearImpulseBody2, angularImpulseBody2);
|
||||||
|
|
||||||
|
// Apply the impulses to the bodies of the constraint
|
||||||
|
applyImpulse(impulseTwistFriction, contactManifold);
|
||||||
|
}
|
||||||
|
else { // If it is a new contact manifold
|
||||||
|
|
||||||
|
// Initialize the accumulated impulses to zero
|
||||||
|
contactManifold.friction1Impulse = 0.0;
|
||||||
|
contactManifold.friction2Impulse = 0.0;
|
||||||
|
contactManifold.frictionTwistImpulse = 0.0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Solve the contact constraints by applying sequential impulses
|
||||||
|
void ContactSolver::solveContactConstraints() {
|
||||||
|
|
||||||
|
decimal deltaLambda;
|
||||||
|
decimal lambdaTemp;
|
||||||
|
uint iter;
|
||||||
|
|
||||||
|
// For each iteration of the contact solver
|
||||||
|
for(iter=0; iter<mNbIterations; iter++) {
|
||||||
|
|
||||||
|
// For each contact manifold
|
||||||
|
for (uint c=0; c<mNbContactManifolds; c++) {
|
||||||
|
|
||||||
|
ContactManifoldSolver& contactManifold = mContactConstraints[c];
|
||||||
|
|
||||||
|
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];
|
||||||
|
|
||||||
|
for (uint i=0; i<contactManifold.nbContacts; i++) {
|
||||||
|
|
||||||
|
ContactPointSolver& contactPoint = contactManifold.contacts[i];
|
||||||
|
|
||||||
|
// --------- Penetration --------- //
|
||||||
|
|
||||||
|
// Compute J*v
|
||||||
|
Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
|
||||||
|
decimal deltaVDotN = deltaV.dot(contactPoint.normal);
|
||||||
|
decimal Jv = deltaVDotN;
|
||||||
|
|
||||||
|
// Compute the bias "b" of the constraint
|
||||||
|
decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
|
||||||
|
decimal biasPenetrationDepth = 0.0;
|
||||||
|
if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) *
|
||||||
|
max(0.0f, float(contactPoint.penetrationDepth - SLOP));
|
||||||
|
decimal b = biasPenetrationDepth + contactPoint.restitutionBias;
|
||||||
|
|
||||||
|
// Compute the Lagrange multiplier lambda
|
||||||
|
if (mIsSplitImpulseActive) {
|
||||||
|
deltaLambda = - (Jv + contactPoint.restitutionBias) *
|
||||||
|
contactPoint.inversePenetrationMass;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass;
|
||||||
|
}
|
||||||
|
lambdaTemp = contactPoint.penetrationImpulse;
|
||||||
|
contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse +
|
||||||
|
deltaLambda, decimal(0.0));
|
||||||
|
deltaLambda = contactPoint.penetrationImpulse - lambdaTemp;
|
||||||
|
|
||||||
|
// Compute the impulse P=J^T * lambda
|
||||||
|
const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda,
|
||||||
|
contactPoint);
|
||||||
|
|
||||||
|
// Apply the impulse to the bodies of the constraint
|
||||||
|
applyImpulse(impulsePenetration, contactManifold);
|
||||||
|
|
||||||
|
sumPenetrationImpulse += contactPoint.penetrationImpulse;
|
||||||
|
|
||||||
|
// If the split impulse position correction is active
|
||||||
|
if (mIsSplitImpulseActive) {
|
||||||
|
|
||||||
|
// Split impulse (position correction)
|
||||||
|
const Vector3& v1Split = mSplitLinearVelocities[contactManifold.indexBody1];
|
||||||
|
const Vector3& w1Split = mSplitAngularVelocities[contactManifold.indexBody1];
|
||||||
|
const Vector3& v2Split = mSplitLinearVelocities[contactManifold.indexBody2];
|
||||||
|
const Vector3& w2Split = mSplitAngularVelocities[contactManifold.indexBody2];
|
||||||
|
Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) -
|
||||||
|
v1Split - w1Split.cross(contactPoint.r1);
|
||||||
|
decimal JvSplit = deltaVSplit.dot(contactPoint.normal);
|
||||||
|
decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) *
|
||||||
|
contactPoint.inversePenetrationMass;
|
||||||
|
decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse;
|
||||||
|
contactPoint.penetrationSplitImpulse = std::max(
|
||||||
|
contactPoint.penetrationSplitImpulse +
|
||||||
|
deltaLambdaSplit, decimal(0.0));
|
||||||
|
deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit;
|
||||||
|
|
||||||
|
// Compute the impulse P=J^T * lambda
|
||||||
|
const Impulse splitImpulsePenetration = computePenetrationImpulse(
|
||||||
|
deltaLambdaSplit, contactPoint);
|
||||||
|
|
||||||
|
applySplitImpulse(splitImpulsePenetration, contactManifold);
|
||||||
|
}
|
||||||
|
|
||||||
|
// If we do not solve the friction constraints at the center of the contact manifold
|
||||||
|
if (!mIsSolveFrictionAtContactManifoldCenterActive) {
|
||||||
|
|
||||||
|
// --------- Friction 1 --------- //
|
||||||
|
|
||||||
|
// Compute J*v
|
||||||
|
deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
|
||||||
|
Jv = deltaV.dot(contactPoint.frictionVector1);
|
||||||
|
|
||||||
|
// Compute the Lagrange multiplier lambda
|
||||||
|
deltaLambda = -Jv;
|
||||||
|
deltaLambda *= contactPoint.inverseFriction1Mass;
|
||||||
|
decimal frictionLimit = contactManifold.frictionCoefficient *
|
||||||
|
contactPoint.penetrationImpulse;
|
||||||
|
lambdaTemp = contactPoint.friction1Impulse;
|
||||||
|
contactPoint.friction1Impulse = std::max(-frictionLimit,
|
||||||
|
std::min(contactPoint.friction1Impulse
|
||||||
|
+ deltaLambda, frictionLimit));
|
||||||
|
deltaLambda = contactPoint.friction1Impulse - lambdaTemp;
|
||||||
|
|
||||||
|
// Compute the impulse P=J^T * lambda
|
||||||
|
const Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda,
|
||||||
|
contactPoint);
|
||||||
|
|
||||||
|
// Apply the impulses to the bodies of the constraint
|
||||||
|
applyImpulse(impulseFriction1, contactManifold);
|
||||||
|
|
||||||
|
// --------- Friction 2 --------- //
|
||||||
|
|
||||||
|
// Compute J*v
|
||||||
|
deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
|
||||||
|
Jv = deltaV.dot(contactPoint.frictionVector2);
|
||||||
|
|
||||||
|
// Compute the Lagrange multiplier lambda
|
||||||
|
deltaLambda = -Jv;
|
||||||
|
deltaLambda *= contactPoint.inverseFriction2Mass;
|
||||||
|
frictionLimit = contactManifold.frictionCoefficient *
|
||||||
|
contactPoint.penetrationImpulse;
|
||||||
|
lambdaTemp = contactPoint.friction2Impulse;
|
||||||
|
contactPoint.friction2Impulse = std::max(-frictionLimit,
|
||||||
|
std::min(contactPoint.friction2Impulse
|
||||||
|
+ deltaLambda, frictionLimit));
|
||||||
|
deltaLambda = contactPoint.friction2Impulse - lambdaTemp;
|
||||||
|
|
||||||
|
// Compute the impulse P=J^T * lambda
|
||||||
|
const Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda,
|
||||||
|
contactPoint);
|
||||||
|
|
||||||
|
// Apply the impulses to the bodies of the constraint
|
||||||
|
applyImpulse(impulseFriction2, contactManifold);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// If we solve the friction constraints at the center of the contact manifold
|
||||||
|
if (mIsSolveFrictionAtContactManifoldCenterActive) {
|
||||||
|
|
||||||
|
// ------ First friction constraint at the center of the contact manifol ------ //
|
||||||
|
|
||||||
|
// Compute J*v
|
||||||
|
Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction)
|
||||||
|
- v1 - w1.cross(contactManifold.r1Friction);
|
||||||
|
decimal Jv = deltaV.dot(contactManifold.frictionVector1);
|
||||||
|
|
||||||
|
// Compute the Lagrange multiplier lambda
|
||||||
|
decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass;
|
||||||
|
decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
|
||||||
|
lambdaTemp = contactManifold.friction1Impulse;
|
||||||
|
contactManifold.friction1Impulse = std::max(-frictionLimit,
|
||||||
|
std::min(contactManifold.friction1Impulse +
|
||||||
|
deltaLambda, frictionLimit));
|
||||||
|
deltaLambda = contactManifold.friction1Impulse - lambdaTemp;
|
||||||
|
|
||||||
|
// Compute the impulse P=J^T * lambda
|
||||||
|
Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda;
|
||||||
|
Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda;
|
||||||
|
Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda;
|
||||||
|
Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda;
|
||||||
|
const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1,
|
||||||
|
linearImpulseBody2, angularImpulseBody2);
|
||||||
|
|
||||||
|
// Apply the impulses to the bodies of the constraint
|
||||||
|
applyImpulse(impulseFriction1, contactManifold);
|
||||||
|
|
||||||
|
// ------ Second friction constraint at the center of the contact manifol ----- //
|
||||||
|
|
||||||
|
// Compute J*v
|
||||||
|
deltaV = v2 + w2.cross(contactManifold.r2Friction)
|
||||||
|
- v1 - w1.cross(contactManifold.r1Friction);
|
||||||
|
Jv = deltaV.dot(contactManifold.frictionVector2);
|
||||||
|
|
||||||
|
// Compute the Lagrange multiplier lambda
|
||||||
|
deltaLambda = -Jv * contactManifold.inverseFriction2Mass;
|
||||||
|
frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
|
||||||
|
lambdaTemp = contactManifold.friction2Impulse;
|
||||||
|
contactManifold.friction2Impulse = std::max(-frictionLimit,
|
||||||
|
std::min(contactManifold.friction2Impulse +
|
||||||
|
deltaLambda, frictionLimit));
|
||||||
|
deltaLambda = contactManifold.friction2Impulse - lambdaTemp;
|
||||||
|
|
||||||
|
// Compute the impulse P=J^T * lambda
|
||||||
|
linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda;
|
||||||
|
angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda;
|
||||||
|
linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda;
|
||||||
|
angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda;
|
||||||
|
const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1,
|
||||||
|
linearImpulseBody2, angularImpulseBody2);
|
||||||
|
|
||||||
|
// Apply the impulses to the bodies of the constraint
|
||||||
|
applyImpulse(impulseFriction2, contactManifold);
|
||||||
|
|
||||||
|
// ------ Twist friction constraint at the center of the contact manifol ------ //
|
||||||
|
|
||||||
|
// Compute J*v
|
||||||
|
deltaV = w2 - w1;
|
||||||
|
Jv = deltaV.dot(contactManifold.normal);
|
||||||
|
|
||||||
|
deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass);
|
||||||
|
frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
|
||||||
|
lambdaTemp = contactManifold.frictionTwistImpulse;
|
||||||
|
contactManifold.frictionTwistImpulse = std::max(-frictionLimit,
|
||||||
|
std::min(contactManifold.frictionTwistImpulse
|
||||||
|
+ deltaLambda, frictionLimit));
|
||||||
|
deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp;
|
||||||
|
|
||||||
|
// Compute the impulse P=J^T * lambda
|
||||||
|
linearImpulseBody1 = Vector3(0.0, 0.0, 0.0);
|
||||||
|
angularImpulseBody1 = -contactManifold.normal * deltaLambda;
|
||||||
|
linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);;
|
||||||
|
angularImpulseBody2 = contactManifold.normal * deltaLambda;
|
||||||
|
const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
|
||||||
|
linearImpulseBody2, angularImpulseBody2);
|
||||||
|
|
||||||
|
// Apply the impulses to the bodies of the constraint
|
||||||
|
applyImpulse(impulseTwistFriction, contactManifold);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Solve the constraints
|
||||||
|
void ContactSolver::solve(decimal timeStep) {
|
||||||
|
|
||||||
|
// Set the current time step
|
||||||
|
mTimeStep = timeStep;
|
||||||
|
|
||||||
|
// Initialize the solver
|
||||||
|
initialize();
|
||||||
|
|
||||||
|
// Fill-in all the matrices needed to solve the LCP problem
|
||||||
|
initializeContactConstraints();
|
||||||
|
|
||||||
|
// Warm start the solver
|
||||||
|
if (mIsWarmStartingActive) {
|
||||||
|
warmStart();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Solve the contact constraints
|
||||||
|
solveContactConstraints();
|
||||||
|
|
||||||
|
// Cache the lambda values in order to use them in the next step
|
||||||
|
storeImpulses();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Store the computed impulses to use them to
|
||||||
|
// warm start the solver at the next iteration
|
||||||
|
void ContactSolver::storeImpulses() {
|
||||||
|
|
||||||
|
// For each contact manifold
|
||||||
|
for (uint c=0; c<mNbContactManifolds; c++) {
|
||||||
|
|
||||||
|
ContactManifoldSolver& manifold = mContactConstraints[c];
|
||||||
|
|
||||||
|
for (uint i=0; i<manifold.nbContacts; i++) {
|
||||||
|
|
||||||
|
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->setFrictionVector1(contactPoint.frictionVector1);
|
||||||
|
contactPoint.externalContact->setFrictionVector2(contactPoint.frictionVector2);
|
||||||
|
}
|
||||||
|
|
||||||
|
manifold.externalContactManifold->setFrictionImpulse1(manifold.friction1Impulse);
|
||||||
|
manifold.externalContactManifold->setFrictionImpulse2(manifold.friction2Impulse);
|
||||||
|
manifold.externalContactManifold->setFrictionTwistImpulse(manifold.frictionTwistImpulse);
|
||||||
|
manifold.externalContactManifold->setFrictionVector1(manifold.frictionVector1);
|
||||||
|
manifold.externalContactManifold->setFrictionVector2(manifold.frictionVector2);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Apply an impulse to the two bodies of a constraint
|
||||||
|
void ContactSolver::applyImpulse(const Impulse& impulse,
|
||||||
|
const ContactManifoldSolver& manifold) {
|
||||||
|
|
||||||
|
// Update the velocities of the bodies by applying the impulse P
|
||||||
|
if (manifold.isBody1Moving) {
|
||||||
|
mConstrainedLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 *
|
||||||
|
impulse.linearImpulseBody1;
|
||||||
|
mConstrainedAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 *
|
||||||
|
impulse.angularImpulseBody1;
|
||||||
|
}
|
||||||
|
if (manifold.isBody2Moving) {
|
||||||
|
mConstrainedLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 *
|
||||||
|
impulse.linearImpulseBody2;
|
||||||
|
mConstrainedAngularVelocities[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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane
|
||||||
|
// for a contact point constraint. The two vectors have to be such that : t1 x t2 = contactNormal.
|
||||||
|
void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
|
||||||
|
ContactPointSolver& contactPoint) const {
|
||||||
|
|
||||||
|
assert(contactPoint.normal.length() > 0.0);
|
||||||
|
|
||||||
|
// Compute the velocity difference vector in the tangential plane
|
||||||
|
Vector3 normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal;
|
||||||
|
Vector3 tangentVelocity = deltaVelocity - normalVelocity;
|
||||||
|
|
||||||
|
// If the velocty difference in the tangential plane is not zero
|
||||||
|
decimal lengthTangenVelocity = tangentVelocity.length();
|
||||||
|
if (lengthTangenVelocity > 0.0) {
|
||||||
|
|
||||||
|
// Compute the first friction vector in the direction of the tangent
|
||||||
|
// velocity difference
|
||||||
|
contactPoint.frictionVector1 = tangentVelocity / lengthTangenVelocity;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
|
||||||
|
// Get any orthogonal vector to the normal as the first friction vector
|
||||||
|
contactPoint.frictionVector1 = contactPoint.normal.getOneUnitOrthogonalVector();
|
||||||
|
}
|
||||||
|
|
||||||
|
// The second friction vector is computed by the cross product of the firs
|
||||||
|
// friction vector and the contact normal
|
||||||
|
contactPoint.frictionVector2 =contactPoint.normal.cross(contactPoint.frictionVector1).getUnit();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane
|
||||||
|
// for a contact constraint. The two vectors have to be such that : t1 x t2 = contactNormal.
|
||||||
|
void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
|
||||||
|
ContactManifoldSolver& contact) const {
|
||||||
|
|
||||||
|
assert(contact.normal.length() > 0.0);
|
||||||
|
|
||||||
|
// Compute the velocity difference vector in the tangential plane
|
||||||
|
Vector3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal;
|
||||||
|
Vector3 tangentVelocity = deltaVelocity - normalVelocity;
|
||||||
|
|
||||||
|
// If the velocty difference in the tangential plane is not zero
|
||||||
|
decimal lengthTangenVelocity = tangentVelocity.length();
|
||||||
|
if (lengthTangenVelocity > 0.0) {
|
||||||
|
|
||||||
|
// Compute the first friction vector in the direction of the tangent
|
||||||
|
// velocity difference
|
||||||
|
contact.frictionVector1 = tangentVelocity / lengthTangenVelocity;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
|
||||||
|
// Get any orthogonal vector to the normal as the first friction vector
|
||||||
|
contact.frictionVector1 = contact.normal.getOneUnitOrthogonalVector();
|
||||||
|
}
|
||||||
|
|
||||||
|
// The second friction vector is computed by the cross product of the firs
|
||||||
|
// friction vector and the contact normal
|
||||||
|
contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Clean up the constraint solver
|
||||||
|
void ContactSolver::cleanup() {
|
||||||
|
|
||||||
|
mConstraintBodies.clear();
|
||||||
|
|
||||||
|
if (mContactConstraints != NULL) {
|
||||||
|
delete[] mContactConstraints;
|
||||||
|
mContactConstraints = NULL;
|
||||||
|
}
|
||||||
|
if (mSplitLinearVelocities != NULL) {
|
||||||
|
delete[] mSplitLinearVelocities;
|
||||||
|
mSplitLinearVelocities = NULL;
|
||||||
|
}
|
||||||
|
if (mSplitAngularVelocities != NULL) {
|
||||||
|
delete[] mSplitAngularVelocities;
|
||||||
|
mSplitAngularVelocities = NULL;
|
||||||
|
}
|
||||||
|
}
|
455
src/engine/ContactSolver.h
Normal file
455
src/engine/ContactSolver.h
Normal file
|
@ -0,0 +1,455 @@
|
||||||
|
/********************************************************************************
|
||||||
|
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
|
||||||
|
* Copyright (c) 2010-2012 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 CONTACT_SOLVER_H
|
||||||
|
#define CONTACT_SOLVER_H
|
||||||
|
|
||||||
|
// Libraries
|
||||||
|
#include "../constraint/ContactPoint.h"
|
||||||
|
#include "../configuration.h"
|
||||||
|
#include "../constraint/Constraint.h"
|
||||||
|
#include "ContactManifold.h"
|
||||||
|
#include <map>
|
||||||
|
#include <set>
|
||||||
|
|
||||||
|
// ReactPhysics3D namespace
|
||||||
|
namespace reactphysics3d {
|
||||||
|
|
||||||
|
// Declarations
|
||||||
|
class DynamicsWorld;
|
||||||
|
|
||||||
|
// Structure Impulse
|
||||||
|
struct Impulse {
|
||||||
|
|
||||||
|
public:
|
||||||
|
const Vector3 linearImpulseBody1;
|
||||||
|
const Vector3 linearImpulseBody2;
|
||||||
|
const Vector3 angularImpulseBody1;
|
||||||
|
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 ContactSolver :
|
||||||
|
This class represents the contact solver that is used is solve rigid bodies contacts.
|
||||||
|
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 ContactSolver {
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
// Structure ContactPointSolver
|
||||||
|
// Contact solver internal data structure that to store all the
|
||||||
|
// information relative to a contact point
|
||||||
|
struct ContactPointSolver {
|
||||||
|
|
||||||
|
decimal penetrationImpulse; // Accumulated normal impulse
|
||||||
|
decimal friction1Impulse; // Accumulated impulse in the 1st friction direction
|
||||||
|
decimal friction2Impulse; // Accumulated impulse in the 2nd friction direction
|
||||||
|
decimal penetrationSplitImpulse; // Accumulated split impulse for penetration correction
|
||||||
|
Vector3 normal; // Normal vector of the contact
|
||||||
|
Vector3 frictionVector1; // First friction vector in the tangent plane
|
||||||
|
Vector3 frictionVector2; // Second friction vector in the tangent plane
|
||||||
|
Vector3 oldFrictionVector1; // Old first friction vector in the tangent plane
|
||||||
|
Vector3 oldFrictionVector2; // Old second friction vector in the tangent plane
|
||||||
|
Vector3 r1; // Vector from the body 1 center to the contact point
|
||||||
|
Vector3 r2; // Vector from the body 2 center to the contact point
|
||||||
|
Vector3 r1CrossT1; // Cross product of r1 with 1st friction vector
|
||||||
|
Vector3 r1CrossT2; // Cross product of r1 with 2nd friction vector
|
||||||
|
Vector3 r2CrossT1; // Cross product of r2 with 1st friction vector
|
||||||
|
Vector3 r2CrossT2; // Cross product of r2 with 2nd friction vector
|
||||||
|
Vector3 r1CrossN; // Cross product of r1 with the contact normal
|
||||||
|
Vector3 r2CrossN; // Cross product of r2 with the contact normal
|
||||||
|
decimal penetrationDepth; // Penetration depth
|
||||||
|
decimal restitutionBias; // Velocity restitution bias
|
||||||
|
decimal inversePenetrationMass; // Inverse of the matrix K for the penenetration
|
||||||
|
decimal inverseFriction1Mass; // Inverse of the matrix K for the 1st friction
|
||||||
|
decimal inverseFriction2Mass; // Inverse of the matrix K for the 2nd friction
|
||||||
|
bool isRestingContact; // True if the contact was existing last time step
|
||||||
|
ContactPoint* externalContact; // Pointer to the external contact
|
||||||
|
};
|
||||||
|
|
||||||
|
// Structure ContactManifoldSolver
|
||||||
|
// Contact solver internal data structure to store all the
|
||||||
|
// information relative to a contact manifold
|
||||||
|
struct ContactManifoldSolver {
|
||||||
|
|
||||||
|
uint indexBody1; // Index of body 1 in the constraint solver
|
||||||
|
uint indexBody2; // Index of body 2 in the constraint solver
|
||||||
|
decimal massInverseBody1; // Inverse of the mass of body 1
|
||||||
|
decimal massInverseBody2; // Inverse of the mass of body 2
|
||||||
|
Matrix3x3 inverseInertiaTensorBody1; // Inverse inertia tensor of body 1
|
||||||
|
Matrix3x3 inverseInertiaTensorBody2; // Inverse inertia tensor of body 2
|
||||||
|
bool isBody1Moving; // True if the body 1 is allowed to move
|
||||||
|
bool isBody2Moving; // True if the body 2 is allowed to move
|
||||||
|
ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD];// Contact point constraints
|
||||||
|
uint nbContacts; // Number of contact points
|
||||||
|
decimal restitutionFactor; // Mix of the restitution factor for two bodies
|
||||||
|
decimal frictionCoefficient; // Mix friction coefficient for the two bodies
|
||||||
|
ContactManifold* externalContactManifold; // Pointer to the external contact manifold
|
||||||
|
|
||||||
|
// Variables used when friction constraints are apply at the center of the manifold //
|
||||||
|
|
||||||
|
Vector3 normal; // Average normal vector of the contact manifold
|
||||||
|
Vector3 frictionPointBody1; // Point on body 1 where to apply the friction constraints
|
||||||
|
Vector3 frictionPointBody2; // Point on body 2 where to apply the friction constraints
|
||||||
|
Vector3 r1Friction; // R1 vector for the friction constraints
|
||||||
|
Vector3 r2Friction; // R2 vector for the friction constraints
|
||||||
|
Vector3 r1CrossT1; // Cross product of r1 with 1st friction vector
|
||||||
|
Vector3 r1CrossT2; // Cross product of r1 with 2nd friction vector
|
||||||
|
Vector3 r2CrossT1; // Cross product of r2 with 1st friction vector
|
||||||
|
Vector3 r2CrossT2; // Cross product of r2 with 2nd friction vector
|
||||||
|
decimal inverseFriction1Mass; // Matrix K for the first friction constraint
|
||||||
|
decimal inverseFriction2Mass; // Matrix K for the second friction constraint
|
||||||
|
decimal inverseTwistFrictionMass; // Matrix K for the twist friction constraint
|
||||||
|
Vector3 frictionVector1; // First friction direction at contact manifold center
|
||||||
|
Vector3 frictionVector2; // Second friction direction at contact manifold center
|
||||||
|
Vector3 oldFrictionVector1; // Old 1st friction direction at contact manifold center
|
||||||
|
Vector3 oldFrictionVector2; // Old 2nd friction direction at contact manifold center
|
||||||
|
decimal friction1Impulse; // First friction direction impulse at manifold center
|
||||||
|
decimal friction2Impulse; // Second friction direction impulse at manifold center
|
||||||
|
decimal frictionTwistImpulse; // Twist friction impulse at contact manifold center
|
||||||
|
};
|
||||||
|
|
||||||
|
// -------------------- Constants --------------------- //
|
||||||
|
|
||||||
|
// Beta value for the penetration depth position correction without split impulses
|
||||||
|
static const decimal BETA;
|
||||||
|
|
||||||
|
// Beta value for the penetration depth position correction with split impulses
|
||||||
|
static const decimal BETA_SPLIT_IMPULSE;
|
||||||
|
|
||||||
|
// Slop distance (allowed penetration distance between bodies)
|
||||||
|
static const decimal SLOP;
|
||||||
|
|
||||||
|
// -------------------- Attributes -------------------- //
|
||||||
|
|
||||||
|
// Reference to the world
|
||||||
|
DynamicsWorld& mWorld;
|
||||||
|
|
||||||
|
// Number of iterations of the constraints solver
|
||||||
|
uint mNbIterations;
|
||||||
|
|
||||||
|
// Split linear velocities for the position contact solver (split impulse)
|
||||||
|
Vector3* mSplitLinearVelocities;
|
||||||
|
|
||||||
|
// Split angular velocities for the position contact solver (split impulse)
|
||||||
|
Vector3* mSplitAngularVelocities;
|
||||||
|
|
||||||
|
// Current time step
|
||||||
|
decimal mTimeStep;
|
||||||
|
|
||||||
|
// Contact constraints
|
||||||
|
ContactManifoldSolver* mContactConstraints;
|
||||||
|
|
||||||
|
// Number of contact constraints
|
||||||
|
uint mNbContactManifolds;
|
||||||
|
|
||||||
|
// Constrained bodies
|
||||||
|
std::set<RigidBody*> mConstraintBodies;
|
||||||
|
|
||||||
|
// Pointer to the array of constrained linear velocities (state of the linear velocities
|
||||||
|
// after solving the constraints)
|
||||||
|
std::vector<Vector3>& mConstrainedLinearVelocities;
|
||||||
|
|
||||||
|
// Pointer to the array of constrained angular velocities (state of the angular velocities
|
||||||
|
// after solving the constraints)
|
||||||
|
std::vector<Vector3>& mConstrainedAngularVelocities;
|
||||||
|
|
||||||
|
// Reference to the map of rigid body to their index in the constrained velocities array
|
||||||
|
const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
|
||||||
|
|
||||||
|
// True if the warm starting of the solver is active
|
||||||
|
bool mIsWarmStartingActive;
|
||||||
|
|
||||||
|
// True if the split impulse position correction is active
|
||||||
|
bool mIsSplitImpulseActive;
|
||||||
|
|
||||||
|
// True if we solve 3 friction constraints at the contact manifold center only
|
||||||
|
// instead of 2 friction constraints at each contact point
|
||||||
|
bool mIsSolveFrictionAtContactManifoldCenterActive;
|
||||||
|
|
||||||
|
// -------------------- Methods -------------------- //
|
||||||
|
|
||||||
|
// Initialize the constraint solver
|
||||||
|
void initialize();
|
||||||
|
|
||||||
|
// Initialize the split impulse velocities
|
||||||
|
void initializeSplitImpulseVelocities();
|
||||||
|
|
||||||
|
// Initialize the contact constraints before solving the system
|
||||||
|
void initializeContactConstraints();
|
||||||
|
|
||||||
|
// Store the computed impulses to use them to
|
||||||
|
// warm start the solver at the next iteration
|
||||||
|
void storeImpulses();
|
||||||
|
|
||||||
|
// Warm start the solver
|
||||||
|
void warmStart();
|
||||||
|
|
||||||
|
// Solve the contact constraints by applying sequential impulses
|
||||||
|
void solveContactConstraints();
|
||||||
|
|
||||||
|
// Apply an impulse to the two bodies of a constraint
|
||||||
|
void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold);
|
||||||
|
|
||||||
|
// Apply an impulse to the two bodies of a constraint
|
||||||
|
void applySplitImpulse(const Impulse& impulse,
|
||||||
|
const ContactManifoldSolver& manifold);
|
||||||
|
|
||||||
|
// Compute the collision restitution factor from the restitution factor of each body
|
||||||
|
decimal computeMixedRestitutionFactor(const RigidBody* body1,
|
||||||
|
const RigidBody* body2) const;
|
||||||
|
|
||||||
|
// Compute the mixed friction coefficient from the friction coefficient of each body
|
||||||
|
decimal computeMixedFrictionCoefficient(const RigidBody* body1,
|
||||||
|
const RigidBody* body2)const;
|
||||||
|
|
||||||
|
// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction
|
||||||
|
// plane for a contact point constraint. The two vectors have to be
|
||||||
|
// such that : t1 x t2 = contactNormal.
|
||||||
|
void computeFrictionVectors(const Vector3& deltaVelocity,
|
||||||
|
ContactPointSolver &contactPoint) const;
|
||||||
|
|
||||||
|
// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction
|
||||||
|
// plane for a contact constraint. The two vectors have to be
|
||||||
|
// such that : t1 x t2 = contactNormal.
|
||||||
|
void computeFrictionVectors(const Vector3& deltaVelocity,
|
||||||
|
ContactManifoldSolver& contactPoint) const;
|
||||||
|
|
||||||
|
// Compute a penetration constraint impulse
|
||||||
|
const Impulse computePenetrationImpulse(decimal deltaLambda,
|
||||||
|
const ContactPointSolver& contactPoint) const;
|
||||||
|
|
||||||
|
// Compute the first friction constraint impulse
|
||||||
|
const Impulse computeFriction1Impulse(decimal deltaLambda,
|
||||||
|
const ContactPointSolver& contactPoint) const;
|
||||||
|
|
||||||
|
// Compute the second friction constraint impulse
|
||||||
|
const Impulse computeFriction2Impulse(decimal deltaLambda,
|
||||||
|
const ContactPointSolver& contactPoint) const;
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
// -------------------- Methods -------------------- //
|
||||||
|
|
||||||
|
// Constructor
|
||||||
|
ContactSolver(DynamicsWorld& mWorld, std::vector<Vector3>& constrainedLinearVelocities,
|
||||||
|
std::vector<Vector3>& constrainedAngularVelocities,
|
||||||
|
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
|
||||||
|
|
||||||
|
// Destructor
|
||||||
|
virtual ~ContactSolver();
|
||||||
|
|
||||||
|
// Solve the constraints
|
||||||
|
void solve(decimal timeStep);
|
||||||
|
|
||||||
|
// Return true if the body is in at least one constraint
|
||||||
|
bool isConstrainedBody(RigidBody* body) const;
|
||||||
|
|
||||||
|
// Return the constrained linear velocity of a body after solving the constraints
|
||||||
|
Vector3 getConstrainedLinearVelocityOfBody(RigidBody *body);
|
||||||
|
|
||||||
|
// Return the split linear velocity
|
||||||
|
Vector3 getSplitLinearVelocityOfBody(RigidBody* body);
|
||||||
|
|
||||||
|
// Return the constrained angular velocity of a body after solving the constraints
|
||||||
|
Vector3 getConstrainedAngularVelocityOfBody(RigidBody* body);
|
||||||
|
|
||||||
|
// Return the split angular velocity
|
||||||
|
Vector3 getSplitAngularVelocityOfBody(RigidBody* body);
|
||||||
|
|
||||||
|
// Clean up the constraint solver
|
||||||
|
void cleanup();
|
||||||
|
|
||||||
|
// Set the number of iterations of the constraint solver
|
||||||
|
void setNbIterationsSolver(uint nbIterations);
|
||||||
|
|
||||||
|
// Activate or Deactivate the split impulses for contacts
|
||||||
|
void setIsSplitImpulseActive(bool isActive);
|
||||||
|
|
||||||
|
// Activate or deactivate the solving of friction constraints at the center of
|
||||||
|
// the contact manifold instead of solving them at each contact point
|
||||||
|
void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
|
||||||
|
};
|
||||||
|
|
||||||
|
// Return true if the body is in at least one constraint
|
||||||
|
inline bool ContactSolver::isConstrainedBody(RigidBody* body) const {
|
||||||
|
return mConstraintBodies.count(body) == 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Return the split linear velocity
|
||||||
|
inline Vector3 ContactSolver::getSplitLinearVelocityOfBody(RigidBody* body) {
|
||||||
|
assert(isConstrainedBody(body));
|
||||||
|
const uint indexBody = mMapBodyToConstrainedVelocityIndex.find(body)->second;
|
||||||
|
return mSplitLinearVelocities[indexBody];
|
||||||
|
}
|
||||||
|
|
||||||
|
// Return the split angular velocity
|
||||||
|
inline Vector3 ContactSolver::getSplitAngularVelocityOfBody(RigidBody* body) {
|
||||||
|
assert(isConstrainedBody(body));
|
||||||
|
const uint indexBody = mMapBodyToConstrainedVelocityIndex.find(body)->second;
|
||||||
|
return mSplitAngularVelocities[indexBody];
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set the number of iterations of the constraint solver
|
||||||
|
inline void ContactSolver::setNbIterationsSolver(uint nbIterations) {
|
||||||
|
mNbIterations = nbIterations;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Activate or Deactivate the split impulses for contacts
|
||||||
|
inline void ContactSolver::setIsSplitImpulseActive(bool isActive) {
|
||||||
|
mIsSplitImpulseActive = isActive;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Activate or deactivate the solving of friction constraints at the center of
|
||||||
|
// the contact manifold instead of solving them at each contact point
|
||||||
|
inline void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) {
|
||||||
|
mIsSolveFrictionAtContactManifoldCenterActive = isActive;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute the collision restitution factor from the restitution factor of each body
|
||||||
|
inline decimal ContactSolver::computeMixedRestitutionFactor(const RigidBody* body1,
|
||||||
|
const RigidBody* body2) const {
|
||||||
|
decimal restitution1 = body1->getRestitution();
|
||||||
|
decimal restitution2 = body2->getRestitution();
|
||||||
|
|
||||||
|
// Return the largest restitution factor
|
||||||
|
return (restitution1 > restitution2) ? restitution1 : restitution2;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute the mixed friction coefficient from the friction coefficient of each body
|
||||||
|
inline decimal ContactSolver::computeMixedFrictionCoefficient(const RigidBody* body1,
|
||||||
|
const RigidBody* body2) const {
|
||||||
|
// Use the geometric mean to compute the mixed friction coefficient
|
||||||
|
return sqrt(body1->getFrictionCoefficient() * body2->getFrictionCoefficient());
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute a penetration constraint impulse
|
||||||
|
inline const Impulse ContactSolver::computePenetrationImpulse(decimal deltaLambda,
|
||||||
|
const ContactPointSolver& contactPoint)
|
||||||
|
const {
|
||||||
|
return Impulse(-contactPoint.normal * deltaLambda, -contactPoint.r1CrossN * deltaLambda,
|
||||||
|
contactPoint.normal * deltaLambda, contactPoint.r2CrossN * deltaLambda);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute the first friction constraint impulse
|
||||||
|
inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda,
|
||||||
|
const ContactPointSolver& contactPoint)
|
||||||
|
const {
|
||||||
|
return Impulse(-contactPoint.frictionVector1 * deltaLambda,
|
||||||
|
-contactPoint.r1CrossT1 * deltaLambda,
|
||||||
|
contactPoint.frictionVector1 * deltaLambda,
|
||||||
|
contactPoint.r2CrossT1 * deltaLambda);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute the second friction constraint impulse
|
||||||
|
inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda,
|
||||||
|
const ContactPointSolver& contactPoint)
|
||||||
|
const {
|
||||||
|
return Impulse(-contactPoint.frictionVector2 * deltaLambda,
|
||||||
|
-contactPoint.r1CrossT2 * deltaLambda,
|
||||||
|
contactPoint.frictionVector2 * deltaLambda,
|
||||||
|
contactPoint.r2CrossT2 * deltaLambda);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
|
@ -26,13 +26,15 @@
|
||||||
// Libraries
|
// Libraries
|
||||||
#include "DynamicsWorld.h"
|
#include "DynamicsWorld.h"
|
||||||
|
|
||||||
// We want to use the ReactPhysics3D namespace
|
// Namespaces
|
||||||
using namespace reactphysics3d;
|
using namespace reactphysics3d;
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP)
|
DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP)
|
||||||
: CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true), mConstraintSolver(this),
|
: CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true),
|
||||||
|
mContactSolver(*this, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
|
||||||
|
mMapBodyToConstrainedVelocityIndex),
|
||||||
mIsDeactivationActive(DEACTIVATION_ENABLED) {
|
mIsDeactivationActive(DEACTIVATION_ENABLED) {
|
||||||
|
|
||||||
}
|
}
|
||||||
|
@ -46,11 +48,13 @@ DynamicsWorld::~DynamicsWorld() {
|
||||||
(*it).second->OverlappingPair::~OverlappingPair();
|
(*it).second->OverlappingPair::~OverlappingPair();
|
||||||
mMemoryPoolOverlappingPairs.freeObject((*it).second);
|
mMemoryPoolOverlappingPairs.freeObject((*it).second);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Free the allocated memory for the constrained velocities
|
||||||
|
cleanupConstrainedVelocitiesArray();
|
||||||
}
|
}
|
||||||
|
|
||||||
// Update the physics simulation
|
// Update the physics simulation
|
||||||
void DynamicsWorld::update() {
|
void DynamicsWorld::update() {
|
||||||
bool existCollision = false;
|
|
||||||
|
|
||||||
assert(mTimer.getIsRunning());
|
assert(mTimer.getIsRunning());
|
||||||
|
|
||||||
|
@ -63,16 +67,20 @@ void DynamicsWorld::update() {
|
||||||
// While the time accumulator is not empty
|
// While the time accumulator is not empty
|
||||||
while(mTimer.isPossibleToTakeStep()) {
|
while(mTimer.isPossibleToTakeStep()) {
|
||||||
|
|
||||||
existCollision = false;
|
// Remove all contact manifolds
|
||||||
|
mContactManifolds.clear();
|
||||||
removeAllContactConstraints();
|
|
||||||
|
|
||||||
// Compute the collision detection
|
// Compute the collision detection
|
||||||
if (mCollisionDetection.computeCollisionDetection()) {
|
mCollisionDetection.computeCollisionDetection();
|
||||||
existCollision = true;
|
|
||||||
|
|
||||||
// Solve constraints
|
// Initialize the constrained velocities
|
||||||
mConstraintSolver.solve(mTimer.getTimeStep());
|
initConstrainedVelocitiesArray();
|
||||||
|
|
||||||
|
// If there are contacts
|
||||||
|
if (!mContactManifolds.empty()) {
|
||||||
|
|
||||||
|
// Solve the contacts
|
||||||
|
mContactSolver.solve(mTimer.getTimeStep());
|
||||||
}
|
}
|
||||||
|
|
||||||
// Update the timer
|
// Update the timer
|
||||||
|
@ -82,69 +90,61 @@ void DynamicsWorld::update() {
|
||||||
resetBodiesMovementVariable();
|
resetBodiesMovementVariable();
|
||||||
|
|
||||||
// Update the position and orientation of each body
|
// Update the position and orientation of each body
|
||||||
updateAllBodiesMotion();
|
updateRigidBodiesPositionAndOrientation();
|
||||||
|
|
||||||
// Cleanup of the constraint solver
|
// Cleanup of the contact solver
|
||||||
if (existCollision) {
|
mContactSolver.cleanup();
|
||||||
mConstraintSolver.cleanup();
|
|
||||||
}
|
// Cleanup the constrained velocities
|
||||||
|
cleanupConstrainedVelocitiesArray();
|
||||||
}
|
}
|
||||||
|
|
||||||
// Compute and set the interpolation factor to all the bodies
|
// Compute and set the interpolation factor to all the bodies
|
||||||
setInterpolationFactorToAllBodies();
|
setInterpolationFactorToAllBodies();
|
||||||
}
|
}
|
||||||
|
|
||||||
// Compute the motion of all bodies and update their positions and orientations
|
// Update the position and orientation of the rigid bodies
|
||||||
// First this method compute the vector V2 = V_constraint + V_forces + V1 where
|
void DynamicsWorld::updateRigidBodiesPositionAndOrientation() {
|
||||||
// V_constraint = dt * (M^-1 * J^T * lambda) and V_forces = dt * (M^-1 * F_ext)
|
|
||||||
// V2 is the final velocity after the timestep and V1 is the velocity before the
|
|
||||||
// timestep.
|
|
||||||
// After having computed the velocity V2, this method will update the position
|
|
||||||
// and orientation of each body.
|
|
||||||
// This method uses the semi-implicit Euler method to update the position and
|
|
||||||
// orientation of the body
|
|
||||||
void DynamicsWorld::updateAllBodiesMotion() {
|
|
||||||
decimal dt = mTimer.getTimeStep();
|
decimal dt = mTimer.getTimeStep();
|
||||||
Vector3 newLinearVelocity;
|
|
||||||
Vector3 newAngularVelocity;
|
|
||||||
Vector3 linearVelocityErrorCorrection;
|
|
||||||
Vector3 angularVelocityErrorCorrection;
|
|
||||||
|
|
||||||
// For each body of thephysics world
|
// For each rigid body of the world
|
||||||
for (set<RigidBody*>::iterator it=getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) {
|
for (set<RigidBody*>::iterator it=getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) {
|
||||||
|
|
||||||
RigidBody* rigidBody = *it;
|
RigidBody* rigidBody = *it;
|
||||||
assert(rigidBody);
|
assert(rigidBody);
|
||||||
|
|
||||||
// If the body is able to move
|
// If the body is allowed to move
|
||||||
if (rigidBody->getIsMotionEnabled()) {
|
if (rigidBody->getIsMotionEnabled()) {
|
||||||
newLinearVelocity.setAllValues(0.0, 0.0, 0.0);
|
|
||||||
newAngularVelocity.setAllValues(0.0, 0.0, 0.0);
|
|
||||||
linearVelocityErrorCorrection.setAllValues(0.0, 0.0, 0.0);
|
|
||||||
angularVelocityErrorCorrection.setAllValues(0.0, 0.0, 0.0);
|
|
||||||
|
|
||||||
// If it's a constrained body
|
// Update the old Transform of the body
|
||||||
if (mConstraintSolver.isConstrainedBody(*it)) {
|
rigidBody->updateOldTransform();
|
||||||
// Get the constrained linear and angular velocities from the constraint solver
|
|
||||||
newLinearVelocity = mConstraintSolver.getConstrainedLinearVelocityOfBody(*it);
|
// Get the constrained velocity
|
||||||
newAngularVelocity = mConstraintSolver.getConstrainedAngularVelocityOfBody(*it);
|
uint indexArray = mMapBodyToConstrainedVelocityIndex[rigidBody];
|
||||||
|
Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
|
||||||
linearVelocityErrorCorrection = mConstraintSolver.getErrorConstrainedLinearVelocityOfBody(rigidBody);
|
Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray];
|
||||||
angularVelocityErrorCorrection = mConstraintSolver.getErrorConstrainedAngularVelocityOfBody(rigidBody);
|
|
||||||
|
// Update the linear and angular velocity of the body
|
||||||
|
rigidBody->setLinearVelocity(newLinVelocity);
|
||||||
|
rigidBody->setAngularVelocity(newAngVelocity);
|
||||||
|
|
||||||
|
// Add the split impulse velocity from Contact Solver (only used to update the position)
|
||||||
|
if (mContactSolver.isConstrainedBody(rigidBody)) {
|
||||||
|
newLinVelocity += mContactSolver.getSplitLinearVelocityOfBody(rigidBody);
|
||||||
|
newAngVelocity += mContactSolver.getSplitAngularVelocityOfBody(rigidBody);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the
|
// Get current position and orientation of the body
|
||||||
// external forces and torques.
|
const Vector3& currentPosition = rigidBody->getTransform().getPosition();
|
||||||
newLinearVelocity += dt * rigidBody->getMassInverse() * rigidBody->getExternalForce();
|
const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation();
|
||||||
newAngularVelocity += dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
|
|
||||||
|
|
||||||
// Add the velocity V1 to the new velocity
|
// Compute the new position of the body
|
||||||
newLinearVelocity += rigidBody->getLinearVelocity();
|
Vector3 newPosition = currentPosition + newLinVelocity * dt;
|
||||||
newAngularVelocity += rigidBody->getAngularVelocity();
|
Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * currentOrientation * 0.5 * dt;
|
||||||
|
|
||||||
// Update the position and the orientation of the body according to the new velocity
|
// Update the Transform of the body
|
||||||
updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity,
|
Transform newTransform(newPosition, newOrientation.getUnit());
|
||||||
linearVelocityErrorCorrection, angularVelocityErrorCorrection);
|
rigidBody->setTransform(newTransform);
|
||||||
|
|
||||||
// Update the AABB of the rigid body
|
// Update the AABB of the rigid body
|
||||||
rigidBody->updateAABB();
|
rigidBody->updateAABB();
|
||||||
|
@ -152,37 +152,6 @@ void DynamicsWorld::updateAllBodiesMotion() {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Update the position and orientation of a body
|
|
||||||
// Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new
|
|
||||||
// orientation of the body
|
|
||||||
void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody, const Vector3& newLinVelocity, const Vector3& newAngVelocity,
|
|
||||||
const Vector3& linearVelocityErrorCorrection, const Vector3& angularVelocityErrorCorrection) {
|
|
||||||
decimal dt = mTimer.getTimeStep();
|
|
||||||
|
|
||||||
assert(rigidBody);
|
|
||||||
|
|
||||||
// Update the old position and orientation of the body
|
|
||||||
rigidBody->updateOldTransform();
|
|
||||||
|
|
||||||
// Update the linear and angular velocity of the body
|
|
||||||
rigidBody->setLinearVelocity(newLinVelocity);
|
|
||||||
rigidBody->setAngularVelocity(newAngVelocity);
|
|
||||||
|
|
||||||
// Get current position and orientation of the body
|
|
||||||
const Vector3& currentPosition = rigidBody->getTransform().getPosition();
|
|
||||||
const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation();
|
|
||||||
|
|
||||||
// Error correction projection
|
|
||||||
Vector3 correctedPosition = currentPosition + dt * linearVelocityErrorCorrection;
|
|
||||||
Quaternion correctedOrientation = currentOrientation + Quaternion(angularVelocityErrorCorrection.getX(), angularVelocityErrorCorrection.getY(), angularVelocityErrorCorrection.getZ(), 0) * currentOrientation * 0.5 * dt;
|
|
||||||
|
|
||||||
Vector3 newPosition = correctedPosition + newLinVelocity * dt;
|
|
||||||
Quaternion newOrientation = correctedOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * correctedOrientation * 0.5 * dt;
|
|
||||||
|
|
||||||
Transform newTransform(newPosition, newOrientation.getUnit());
|
|
||||||
rigidBody->setTransform(newTransform);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Compute and set the interpolation factor to all bodies
|
// Compute and set the interpolation factor to all bodies
|
||||||
void DynamicsWorld::setInterpolationFactorToAllBodies() {
|
void DynamicsWorld::setInterpolationFactorToAllBodies() {
|
||||||
|
|
||||||
|
@ -200,6 +169,41 @@ void DynamicsWorld::setInterpolationFactorToAllBodies() {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Initialize the constrained velocities array at each step
|
||||||
|
void DynamicsWorld::initConstrainedVelocitiesArray() {
|
||||||
|
|
||||||
|
// TODO : Use better memory allocation here
|
||||||
|
mConstrainedLinearVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
|
||||||
|
mConstrainedAngularVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
|
||||||
|
|
||||||
|
double dt = mTimer.getTimeStep();
|
||||||
|
|
||||||
|
// Fill in the mapping of rigid body to their index in the constrained
|
||||||
|
// velocities arrays
|
||||||
|
uint i = 0;
|
||||||
|
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
|
||||||
|
RigidBody* rigidBody = *it;
|
||||||
|
mMapBodyToConstrainedVelocityIndex.insert(std::make_pair<RigidBody*, uint>(rigidBody, i));
|
||||||
|
|
||||||
|
// TODO : Move it somewhere else
|
||||||
|
mConstrainedLinearVelocities[i] = rigidBody->getLinearVelocity() + dt * rigidBody->getMassInverse() *rigidBody->getExternalForce();
|
||||||
|
mConstrainedAngularVelocities[i] = rigidBody->getAngularVelocity() + dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
|
||||||
|
|
||||||
|
i++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Cleanup the constrained velocities array at each step
|
||||||
|
void DynamicsWorld::cleanupConstrainedVelocitiesArray() {
|
||||||
|
|
||||||
|
// Clear the constrained velocites
|
||||||
|
mConstrainedLinearVelocities.clear();
|
||||||
|
mConstrainedAngularVelocities.clear();
|
||||||
|
|
||||||
|
// Clear the rigid body to velocities array index mapping
|
||||||
|
mMapBodyToConstrainedVelocityIndex.clear();
|
||||||
|
}
|
||||||
|
|
||||||
// Apply the gravity force to all bodies of the physics world
|
// Apply the gravity force to all bodies of the physics world
|
||||||
void DynamicsWorld::applyGravity() {
|
void DynamicsWorld::applyGravity() {
|
||||||
|
|
||||||
|
@ -262,26 +266,6 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
|
||||||
mMemoryPoolRigidBodies.freeObject(rigidBody);
|
mMemoryPoolRigidBodies.freeObject(rigidBody);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Remove all collision contacts constraints
|
|
||||||
// TODO : This method should be in the collision detection class
|
|
||||||
void DynamicsWorld::removeAllContactConstraints() {
|
|
||||||
// For all constraints
|
|
||||||
for (vector<Constraint*>::iterator it = mConstraints.begin(); it != mConstraints.end(); ) {
|
|
||||||
|
|
||||||
// Try a downcasting
|
|
||||||
Contact* contact = dynamic_cast<Contact*>(*it);
|
|
||||||
|
|
||||||
// If the constraint is a contact
|
|
||||||
if (contact) {
|
|
||||||
// Remove it from the constraints of the physics world
|
|
||||||
it = mConstraints.erase(it);
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
++it;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Remove all constraints in the physics world
|
// Remove all constraints in the physics world
|
||||||
void DynamicsWorld::removeAllConstraints() {
|
void DynamicsWorld::removeAllConstraints() {
|
||||||
mConstraints.clear();
|
mConstraints.clear();
|
||||||
|
@ -321,7 +305,7 @@ void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, const
|
||||||
assert(rigidBody2);
|
assert(rigidBody2);
|
||||||
|
|
||||||
// Create a new contact
|
// Create a new contact
|
||||||
Contact* contact = new (mMemoryPoolContacts.allocateObject()) Contact(rigidBody1, rigidBody2, contactInfo);
|
ContactPoint* contact = new (mMemoryPoolContacts.allocateObject()) ContactPoint(rigidBody1, rigidBody2, contactInfo);
|
||||||
assert(contact);
|
assert(contact);
|
||||||
|
|
||||||
// Get the corresponding overlapping pair
|
// Get the corresponding overlapping pair
|
||||||
|
@ -332,9 +316,20 @@ void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, const
|
||||||
// Add the contact to the contact cache of the corresponding overlapping pair
|
// Add the contact to the contact cache of the corresponding overlapping pair
|
||||||
overlappingPair->addContact(contact);
|
overlappingPair->addContact(contact);
|
||||||
|
|
||||||
|
// TODO : Remove this
|
||||||
|
/*
|
||||||
|
// Create a contact manifold with the contact points of the two bodies
|
||||||
|
ContactManifold contactManifold;
|
||||||
|
contactManifold.nbContacts = 0;
|
||||||
|
|
||||||
// Add all the contacts in the contact cache of the two bodies
|
// Add all the contacts in the contact cache of the two bodies
|
||||||
// to the set of constraints in the physics world
|
// to the set of constraints in the physics world
|
||||||
for (uint i=0; i<overlappingPair->getNbContacts(); i++) {
|
for (uint i=0; i<overlappingPair->getNbContacts(); i++) {
|
||||||
addConstraint(overlappingPair->getContact(i));
|
contactManifold.contacts[i] = overlappingPair->getContact(i);
|
||||||
|
contactManifold.nbContacts++;
|
||||||
}
|
}
|
||||||
|
*/
|
||||||
|
|
||||||
|
// Add the contact manifold to the world
|
||||||
|
mContactManifolds.push_back(overlappingPair->getContactManifold());
|
||||||
}
|
}
|
||||||
|
|
|
@ -29,7 +29,7 @@
|
||||||
// Libraries
|
// Libraries
|
||||||
#include "CollisionWorld.h"
|
#include "CollisionWorld.h"
|
||||||
#include "../collision/CollisionDetection.h"
|
#include "../collision/CollisionDetection.h"
|
||||||
#include "ConstraintSolver.h"
|
#include "ContactSolver.h"
|
||||||
#include "../body/RigidBody.h"
|
#include "../body/RigidBody.h"
|
||||||
#include "Timer.h"
|
#include "Timer.h"
|
||||||
#include "../configuration.h"
|
#include "../configuration.h"
|
||||||
|
@ -53,8 +53,8 @@ class DynamicsWorld : public CollisionWorld {
|
||||||
// Timer of the physics engine
|
// Timer of the physics engine
|
||||||
Timer mTimer;
|
Timer mTimer;
|
||||||
|
|
||||||
// Constraint solver
|
// Contact solver
|
||||||
ConstraintSolver mConstraintSolver;
|
ContactSolver mContactSolver;
|
||||||
|
|
||||||
// True if the deactivation (sleeping) of inactive bodies is enabled
|
// True if the deactivation (sleeping) of inactive bodies is enabled
|
||||||
bool mIsDeactivationActive;
|
bool mIsDeactivationActive;
|
||||||
|
@ -62,7 +62,10 @@ class DynamicsWorld : public CollisionWorld {
|
||||||
// All the rigid bodies of the physics world
|
// All the rigid bodies of the physics world
|
||||||
std::set<RigidBody*> mRigidBodies;
|
std::set<RigidBody*> mRigidBodies;
|
||||||
|
|
||||||
// List that contains all the current constraints
|
// All the contact constraints
|
||||||
|
std::vector<ContactManifold*> mContactManifolds;
|
||||||
|
|
||||||
|
// All the constraints (except contact constraints)
|
||||||
std::vector<Constraint*> mConstraints;
|
std::vector<Constraint*> mConstraints;
|
||||||
|
|
||||||
// Gravity vector of the world
|
// Gravity vector of the world
|
||||||
|
@ -78,7 +81,18 @@ class DynamicsWorld : public CollisionWorld {
|
||||||
MemoryPool<RigidBody> mMemoryPoolRigidBodies;
|
MemoryPool<RigidBody> mMemoryPoolRigidBodies;
|
||||||
|
|
||||||
// Memory pool for the contacts
|
// Memory pool for the contacts
|
||||||
MemoryPool<Contact> mMemoryPoolContacts;
|
MemoryPool<ContactPoint> mMemoryPoolContacts;
|
||||||
|
|
||||||
|
// Array of constrained linear velocities (state of the linear velocities
|
||||||
|
// after solving the constraints)
|
||||||
|
std::vector<Vector3> mConstrainedLinearVelocities;
|
||||||
|
|
||||||
|
// Array of constrained angular velocities (state of the angular velocities
|
||||||
|
// after solving the constraints)
|
||||||
|
std::vector<Vector3> mConstrainedAngularVelocities;
|
||||||
|
|
||||||
|
// Map body to their index in the constrained velocities array
|
||||||
|
std::map<RigidBody*, uint> mMapBodyToConstrainedVelocityIndex;
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
|
||||||
|
@ -89,17 +103,21 @@ class DynamicsWorld : public CollisionWorld {
|
||||||
DynamicsWorld& operator=(const DynamicsWorld& world);
|
DynamicsWorld& operator=(const DynamicsWorld& world);
|
||||||
|
|
||||||
// Compute the motion of all bodies and update their positions and orientations
|
// Compute the motion of all bodies and update their positions and orientations
|
||||||
void updateAllBodiesMotion();
|
void updateRigidBodiesPositionAndOrientation();
|
||||||
|
|
||||||
// Update the position and orientation of a body
|
// Update the position and orientation of a body
|
||||||
void updatePositionAndOrientationOfBody(RigidBody* body, const Vector3& newLinVelocity,
|
void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity,
|
||||||
const Vector3& newAngVelocity,
|
Vector3 newAngVelocity);
|
||||||
const Vector3& linearVelocityErrorCorrection,
|
|
||||||
const Vector3& angularVelocityErrorCorrection);
|
|
||||||
|
|
||||||
// Compute and set the interpolation factor to all bodies
|
// Compute and set the interpolation factor to all bodies
|
||||||
void setInterpolationFactorToAllBodies();
|
void setInterpolationFactorToAllBodies();
|
||||||
|
|
||||||
|
// Initialize the constrained velocities array at each step
|
||||||
|
void initConstrainedVelocitiesArray();
|
||||||
|
|
||||||
|
// Cleanup the constrained velocities array at each step
|
||||||
|
void cleanupConstrainedVelocitiesArray();
|
||||||
|
|
||||||
// Apply the gravity force to all bodies
|
// Apply the gravity force to all bodies
|
||||||
void applyGravity();
|
void applyGravity();
|
||||||
|
|
||||||
|
@ -137,8 +155,15 @@ public :
|
||||||
// Update the physics simulation
|
// Update the physics simulation
|
||||||
void update();
|
void update();
|
||||||
|
|
||||||
// Set the number of iterations of the LCP solver
|
// Set the number of iterations of the constraint solver
|
||||||
void setNbLCPIterations(uint nbIterations);
|
void setNbIterationsSolver(uint nbIterations);
|
||||||
|
|
||||||
|
// Activate or Deactivate the split impulses for contacts
|
||||||
|
void setIsSplitImpulseActive(bool isActive);
|
||||||
|
|
||||||
|
// Activate or deactivate the solving of friction constraints at the center of
|
||||||
|
// the contact manifold instead of solving them at each contact point
|
||||||
|
void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
|
||||||
|
|
||||||
// Set the isErrorCorrectionActive value
|
// Set the isErrorCorrectionActive value
|
||||||
void setIsErrorCorrectionActive(bool isErrorCorrectionActive);
|
void setIsErrorCorrectionActive(bool isErrorCorrectionActive);
|
||||||
|
@ -160,24 +185,33 @@ public :
|
||||||
// Set the isGravityOn attribute
|
// Set the isGravityOn attribute
|
||||||
void setIsGratityOn(bool isGravityOn);
|
void setIsGratityOn(bool isGravityOn);
|
||||||
|
|
||||||
|
// Return the number of rigid bodies in the world
|
||||||
|
uint getNbRigidBodies() const;
|
||||||
|
|
||||||
// Add a constraint
|
// Add a constraint
|
||||||
void addConstraint(Constraint* constraint);
|
void addConstraint(Constraint* constraint);
|
||||||
|
|
||||||
// Remove a constraint
|
// Remove a constraint
|
||||||
void removeConstraint(Constraint* constraint);
|
void removeConstraint(Constraint* constraint);
|
||||||
|
|
||||||
// Remove all collision contacts constraints
|
|
||||||
void removeAllContactConstraints();
|
|
||||||
|
|
||||||
// Remove all constraints and delete them (free their memory)
|
// Remove all constraints and delete them (free their memory)
|
||||||
void removeAllConstraints();
|
void removeAllConstraints();
|
||||||
|
|
||||||
|
// Return the number of contact constraints in the world
|
||||||
|
uint getNbContactManifolds() const;
|
||||||
|
|
||||||
// Return a start iterator on the constraint list
|
// Return a start iterator on the constraint list
|
||||||
std::vector<Constraint*>::iterator getConstraintsBeginIterator();
|
std::vector<Constraint*>::iterator getConstraintsBeginIterator();
|
||||||
|
|
||||||
// Return a end iterator on the constraint list
|
// Return a end iterator on the constraint list
|
||||||
std::vector<Constraint*>::iterator getConstraintsEndIterator();
|
std::vector<Constraint*>::iterator getConstraintsEndIterator();
|
||||||
|
|
||||||
|
// Return a start iterator on the contact manifolds list
|
||||||
|
std::vector<ContactManifold*>::iterator getContactManifoldsBeginIterator();
|
||||||
|
|
||||||
|
// Return a end iterator on the contact manifolds list
|
||||||
|
std::vector<ContactManifold*>::iterator getContactManifoldsEndIterator();
|
||||||
|
|
||||||
// Return an iterator to the beginning of the rigid bodies of the physics world
|
// Return an iterator to the beginning of the rigid bodies of the physics world
|
||||||
std::set<RigidBody*>::iterator getRigidBodiesBeginIterator();
|
std::set<RigidBody*>::iterator getRigidBodiesBeginIterator();
|
||||||
|
|
||||||
|
@ -197,14 +231,20 @@ inline void DynamicsWorld::stop() {
|
||||||
mTimer.stop();
|
mTimer.stop();
|
||||||
}
|
}
|
||||||
|
|
||||||
// Set the number of iterations of the LCP solver
|
// Set the number of iterations of the constraint solver
|
||||||
inline void DynamicsWorld::setNbLCPIterations(uint nbIterations) {
|
inline void DynamicsWorld::setNbIterationsSolver(uint nbIterations) {
|
||||||
mConstraintSolver.setNbLCPIterations(nbIterations);
|
mContactSolver.setNbIterationsSolver(nbIterations);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Set the isErrorCorrectionActive value
|
// Activate or Deactivate the split impulses for contacts
|
||||||
inline void DynamicsWorld::setIsErrorCorrectionActive(bool isErrorCorrectionActive) {
|
inline void DynamicsWorld::setIsSplitImpulseActive(bool isActive) {
|
||||||
mConstraintSolver.setIsErrorCorrectionActive(isErrorCorrectionActive);
|
mContactSolver.setIsSplitImpulseActive(isActive);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Activate or deactivate the solving of friction constraints at the center of
|
||||||
|
// the contact manifold instead of solving them at each contact point
|
||||||
|
inline void DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) {
|
||||||
|
mContactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Reset the boolean movement variable of each body
|
// Reset the boolean movement variable of each body
|
||||||
|
@ -265,6 +305,11 @@ inline void DynamicsWorld::setIsGratityOn(bool isGravityOn) {
|
||||||
mIsGravityOn = isGravityOn;
|
mIsGravityOn = isGravityOn;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Return the number of rigid bodies in the world
|
||||||
|
inline uint DynamicsWorld::getNbRigidBodies() const {
|
||||||
|
return mRigidBodies.size();
|
||||||
|
}
|
||||||
|
|
||||||
// Return an iterator to the beginning of the bodies of the physics world
|
// Return an iterator to the beginning of the bodies of the physics world
|
||||||
inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesBeginIterator() {
|
inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesBeginIterator() {
|
||||||
return mRigidBodies.begin();
|
return mRigidBodies.begin();
|
||||||
|
@ -275,6 +320,11 @@ inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesEndIterator()
|
||||||
return mRigidBodies.end();
|
return mRigidBodies.end();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Return the number of contact manifolds in the world
|
||||||
|
inline uint DynamicsWorld::getNbContactManifolds() const {
|
||||||
|
return mContactManifolds.size();
|
||||||
|
}
|
||||||
|
|
||||||
// Return a start iterator on the constraint list
|
// Return a start iterator on the constraint list
|
||||||
inline std::vector<Constraint*>::iterator DynamicsWorld::getConstraintsBeginIterator() {
|
inline std::vector<Constraint*>::iterator DynamicsWorld::getConstraintsBeginIterator() {
|
||||||
return mConstraints.begin();
|
return mConstraints.begin();
|
||||||
|
@ -285,6 +335,16 @@ inline std::vector<Constraint*>::iterator DynamicsWorld::getConstraintsEndIterat
|
||||||
return mConstraints.end();
|
return mConstraints.end();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Return a start iterator on the contact manifolds list
|
||||||
|
inline std::vector<ContactManifold*>::iterator DynamicsWorld::getContactManifoldsBeginIterator() {
|
||||||
|
return mContactManifolds.begin();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Return a end iterator on the contact manifolds list
|
||||||
|
inline std::vector<ContactManifold*>::iterator DynamicsWorld::getContactManifoldsEndIterator() {
|
||||||
|
return mContactManifolds.end();
|
||||||
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -31,11 +31,11 @@ using namespace reactphysics3d;
|
||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
OverlappingPair::OverlappingPair(CollisionBody* body1, CollisionBody* body2,
|
OverlappingPair::OverlappingPair(CollisionBody* body1, CollisionBody* body2,
|
||||||
MemoryPool<Contact>& memoryPoolContacts)
|
MemoryPool<ContactPoint>& memoryPoolContacts)
|
||||||
: mBody1(body1), mBody2(body2), mContactsCache(body1, body2, memoryPoolContacts),
|
: mBody1(body1), mBody2(body2), mContactManifold(body1, body2, memoryPoolContacts),
|
||||||
mCachedSeparatingAxis(1.0, 1.0, 1.0) {
|
mCachedSeparatingAxis(1.0, 1.0, 1.0) {
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Destructor
|
// Destructor
|
||||||
OverlappingPair::~OverlappingPair() {
|
OverlappingPair::~OverlappingPair() {
|
||||||
|
|
|
@ -27,7 +27,7 @@
|
||||||
#define OVERLAPPING_PAIR_H
|
#define OVERLAPPING_PAIR_H
|
||||||
|
|
||||||
// Libraries
|
// Libraries
|
||||||
#include "PersistentContactCache.h"
|
#include "ContactManifold.h"
|
||||||
|
|
||||||
// ReactPhysics3D namespace
|
// ReactPhysics3D namespace
|
||||||
namespace reactphysics3d {
|
namespace reactphysics3d {
|
||||||
|
@ -53,8 +53,8 @@ class OverlappingPair {
|
||||||
// Pointer to the second body of the contact
|
// Pointer to the second body of the contact
|
||||||
CollisionBody* const mBody2;
|
CollisionBody* const mBody2;
|
||||||
|
|
||||||
// Persistent contact cache
|
// Persistent contact manifold
|
||||||
PersistentContactCache mContactsCache;
|
ContactManifold mContactManifold;
|
||||||
|
|
||||||
// Cached previous separating axis
|
// Cached previous separating axis
|
||||||
Vector3 mCachedSeparatingAxis;
|
Vector3 mCachedSeparatingAxis;
|
||||||
|
@ -73,7 +73,7 @@ class OverlappingPair {
|
||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
OverlappingPair(CollisionBody* body1, CollisionBody* body2,
|
OverlappingPair(CollisionBody* body1, CollisionBody* body2,
|
||||||
MemoryPool<Contact>& memoryPoolContacts);
|
MemoryPool<ContactPoint>& memoryPoolContacts);
|
||||||
|
|
||||||
// Destructor
|
// Destructor
|
||||||
~OverlappingPair();
|
~OverlappingPair();
|
||||||
|
@ -85,7 +85,7 @@ class OverlappingPair {
|
||||||
CollisionBody* const getBody2() const;
|
CollisionBody* const getBody2() const;
|
||||||
|
|
||||||
// Add a contact to the contact cache
|
// Add a contact to the contact cache
|
||||||
void addContact(Contact* contact);
|
void addContact(ContactPoint* contact);
|
||||||
|
|
||||||
// Update the contact cache
|
// Update the contact cache
|
||||||
void update();
|
void update();
|
||||||
|
@ -99,8 +99,12 @@ class OverlappingPair {
|
||||||
// Return the number of contacts in the cache
|
// Return the number of contacts in the cache
|
||||||
uint getNbContacts() const;
|
uint getNbContacts() const;
|
||||||
|
|
||||||
|
// Return the contact manifold
|
||||||
|
ContactManifold* getContactManifold();
|
||||||
|
|
||||||
// Return a contact of the cache
|
// Return a contact of the cache
|
||||||
Contact* getContact(uint index) const;
|
// TODO : Maybe remove this method
|
||||||
|
ContactPoint* getContact(uint index) const;
|
||||||
};
|
};
|
||||||
|
|
||||||
// Return the pointer to first body
|
// Return the pointer to first body
|
||||||
|
@ -113,14 +117,14 @@ inline CollisionBody* const OverlappingPair::getBody2() const {
|
||||||
return mBody2;
|
return mBody2;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Add a contact to the contact cache
|
// Add a contact to the contact manifold
|
||||||
inline void OverlappingPair::addContact(Contact* contact) {
|
inline void OverlappingPair::addContact(ContactPoint* contact) {
|
||||||
mContactsCache.addContact(contact);
|
mContactManifold.addContactPoint(contact);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Update the contact cache
|
// Update the contact manifold
|
||||||
inline void OverlappingPair::update() {
|
inline void OverlappingPair::update() {
|
||||||
mContactsCache.update(mBody1->getTransform(), mBody2->getTransform());
|
mContactManifold.update(mBody1->getTransform(), mBody2->getTransform());
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the cached separating axis
|
// Return the cached separating axis
|
||||||
|
@ -136,12 +140,17 @@ inline void OverlappingPair::setCachedSeparatingAxis(const Vector3& axis) {
|
||||||
|
|
||||||
// Return the number of contacts in the cache
|
// Return the number of contacts in the cache
|
||||||
inline uint OverlappingPair::getNbContacts() const {
|
inline uint OverlappingPair::getNbContacts() const {
|
||||||
return mContactsCache.getNbContacts();
|
return mContactManifold.getNbContactPoints();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Return the contact manifold
|
||||||
|
inline ContactManifold* OverlappingPair::getContactManifold() {
|
||||||
|
return &mContactManifold;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return a contact of the cache
|
// Return a contact of the cache
|
||||||
inline Contact* OverlappingPair::getContact(uint index) const {
|
inline ContactPoint* OverlappingPair::getContact(uint index) const {
|
||||||
return mContactsCache.getContact(index);
|
return mContactManifold.getContactPoint(index);
|
||||||
}
|
}
|
||||||
|
|
||||||
} // End of the ReactPhysics3D namespace
|
} // End of the ReactPhysics3D namespace
|
||||||
|
|
|
@ -1,227 +0,0 @@
|
||||||
/********************************************************************************
|
|
||||||
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
|
|
||||||
* Copyright (c) 2010-2012 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 "PersistentContactCache.h"
|
|
||||||
|
|
||||||
using namespace reactphysics3d;
|
|
||||||
|
|
||||||
// Constructor
|
|
||||||
PersistentContactCache::PersistentContactCache(Body* const body1, Body* const body2, MemoryPool<Contact>& memoryPoolContacts)
|
|
||||||
: mBody1(body1), mBody2(body2), mNbContacts(0), mMemoryPoolContacts(memoryPoolContacts) {
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
// Destructor
|
|
||||||
PersistentContactCache::~PersistentContactCache() {
|
|
||||||
clear();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Add a contact in the cache
|
|
||||||
void PersistentContactCache::addContact(Contact* contact) {
|
|
||||||
|
|
||||||
int indexNewContact = mNbContacts;
|
|
||||||
|
|
||||||
// For contact already in the cache
|
|
||||||
for (uint i=0; i<mNbContacts; i++) {
|
|
||||||
// Check if the new point point does not correspond to a same contact point
|
|
||||||
// already in the cache. If it's the case, we do not add the new contact
|
|
||||||
if (isApproxEqual(contact->getLocalPointOnBody1(), mContacts[i]->getLocalPointOnBody1())) {
|
|
||||||
// Delete the new contact
|
|
||||||
contact->Contact::~Contact();
|
|
||||||
mMemoryPoolContacts.freeObject(contact);
|
|
||||||
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// If the contact cache is full
|
|
||||||
if (mNbContacts == MAX_CONTACTS_IN_CACHE) {
|
|
||||||
int indexMaxPenetration = getIndexOfDeepestPenetration(contact);
|
|
||||||
int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1());
|
|
||||||
removeContact(indexToRemove);
|
|
||||||
indexNewContact = indexToRemove;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Add the new contact in the cache
|
|
||||||
mContacts[indexNewContact] = contact;
|
|
||||||
mNbContacts++;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Remove a contact from the cache
|
|
||||||
void PersistentContactCache::removeContact(int index) {
|
|
||||||
assert(index >= 0 && index < mNbContacts);
|
|
||||||
assert(mNbContacts > 0);
|
|
||||||
|
|
||||||
// Call the destructor explicitly and tell the memory pool that
|
|
||||||
// the corresponding memory block is now free
|
|
||||||
mContacts[index]->Contact::~Contact();
|
|
||||||
mMemoryPoolContacts.freeObject(mContacts[index]);
|
|
||||||
|
|
||||||
// If we don't remove the last index
|
|
||||||
if (index < mNbContacts - 1) {
|
|
||||||
mContacts[index] = mContacts[mNbContacts - 1];
|
|
||||||
}
|
|
||||||
|
|
||||||
mNbContacts--;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Update the contact cache
|
|
||||||
// First the world space coordinates of the current contacts in the cache are recomputed from
|
|
||||||
// the corresponding transforms of the bodies because they have moved. Then we remove the contacts
|
|
||||||
// with a negative penetration depth (meaning that the bodies are not penetrating anymore) and also
|
|
||||||
// the contacts with a too large distance between the contact points in the plane orthogonal to the
|
|
||||||
// contact normal
|
|
||||||
void PersistentContactCache::update(const Transform& transform1, const Transform& transform2) {
|
|
||||||
if (mNbContacts == 0) return;
|
|
||||||
|
|
||||||
// Update the world coordinates and penetration depth of the contacts in the cache
|
|
||||||
for (int i=0; i<mNbContacts; i++) {
|
|
||||||
mContacts[i]->setWorldPointOnBody1(transform1 * mContacts[i]->getLocalPointOnBody1());
|
|
||||||
mContacts[i]->setWorldPointOnBody2(transform2 * mContacts[i]->getLocalPointOnBody2());
|
|
||||||
mContacts[i]->setPenetrationDepth((mContacts[i]->getWorldPointOnBody1() - mContacts[i]->getWorldPointOnBody2()).dot(mContacts[i]->getNormal()));
|
|
||||||
}
|
|
||||||
|
|
||||||
// Remove the contacts that don't represent very well the persistent contact
|
|
||||||
for (int i=mNbContacts-1; i>=0; i--) {
|
|
||||||
assert(i>= 0 && i < mNbContacts);
|
|
||||||
|
|
||||||
// Remove the contacts with a negative penetration depth (meaning that the bodies are not penetrating anymore)
|
|
||||||
if (mContacts[i]->getPenetrationDepth() <= 0.0) {
|
|
||||||
removeContact(i);
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
// Compute the distance of the two contact points in the place orthogonal to the contact normal
|
|
||||||
Vector3 projOfPoint1 = mContacts[i]->getWorldPointOnBody1() - mContacts[i]->getNormal() * mContacts[i]->getPenetrationDepth();
|
|
||||||
Vector3 projDifference = mContacts[i]->getWorldPointOnBody2() - projOfPoint1;
|
|
||||||
|
|
||||||
// If the orthogonal distance is larger than the valid distance threshold, we remove the contact
|
|
||||||
if (projDifference.lengthSquare() > PERSISTENT_CONTACT_DIST_THRESHOLD * PERSISTENT_CONTACT_DIST_THRESHOLD) {
|
|
||||||
removeContact(i);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Return the index of the contact with the larger penetration depth. This
|
|
||||||
// corresponding contact will be kept in the cache. The method returns -1 is
|
|
||||||
// the new contact is the deepest.
|
|
||||||
int PersistentContactCache::getIndexOfDeepestPenetration(Contact* newContact) const {
|
|
||||||
assert(mNbContacts == MAX_CONTACTS_IN_CACHE);
|
|
||||||
int indexMaxPenetrationDepth = -1;
|
|
||||||
decimal maxPenetrationDepth = newContact->getPenetrationDepth();
|
|
||||||
|
|
||||||
// For each contact in the cache
|
|
||||||
for (uint i=0; i<mNbContacts; i++) {
|
|
||||||
// If the current contact has a larger penetration depth
|
|
||||||
if (mContacts[i]->getPenetrationDepth() > maxPenetrationDepth) {
|
|
||||||
maxPenetrationDepth = mContacts[i]->getPenetrationDepth();
|
|
||||||
indexMaxPenetrationDepth = i;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Return the index of largest penetration depth
|
|
||||||
return indexMaxPenetrationDepth;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Return the index that will be removed. The index of the contact with the larger penetration
|
|
||||||
// depth is given as a parameter. This contact won't be removed. Given this contact, we compute
|
|
||||||
// the different area and we want to keep the contacts with the largest area. The new point is also
|
|
||||||
// kept.
|
|
||||||
int PersistentContactCache::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const {
|
|
||||||
assert(mNbContacts == MAX_CONTACTS_IN_CACHE);
|
|
||||||
decimal area0 = 0.0; // Area with contact 1,2,3 and newPoint
|
|
||||||
decimal area1 = 0.0; // Area with contact 0,2,3 and newPoint
|
|
||||||
decimal area2 = 0.0; // Area with contact 0,1,3 and newPoint
|
|
||||||
decimal area3 = 0.0; // Area with contact 0,1,2 and newPoint
|
|
||||||
|
|
||||||
if (indexMaxPenetration != 0) {
|
|
||||||
// Compute the area
|
|
||||||
Vector3 vector1 = newPoint - mContacts[1]->getLocalPointOnBody1();
|
|
||||||
Vector3 vector2 = mContacts[3]->getLocalPointOnBody1() - mContacts[2]->getLocalPointOnBody1();
|
|
||||||
Vector3 crossProduct = vector1.cross(vector2);
|
|
||||||
area0 = crossProduct.lengthSquare();
|
|
||||||
}
|
|
||||||
if (indexMaxPenetration != 1) {
|
|
||||||
// Compute the area
|
|
||||||
Vector3 vector1 = newPoint - mContacts[0]->getLocalPointOnBody1();
|
|
||||||
Vector3 vector2 = mContacts[3]->getLocalPointOnBody1() - mContacts[2]->getLocalPointOnBody1();
|
|
||||||
Vector3 crossProduct = vector1.cross(vector2);
|
|
||||||
area1 = crossProduct.lengthSquare();
|
|
||||||
}
|
|
||||||
if (indexMaxPenetration != 2) {
|
|
||||||
// Compute the area
|
|
||||||
Vector3 vector1 = newPoint - mContacts[0]->getLocalPointOnBody1();
|
|
||||||
Vector3 vector2 = mContacts[3]->getLocalPointOnBody1() - mContacts[1]->getLocalPointOnBody1();
|
|
||||||
Vector3 crossProduct = vector1.cross(vector2);
|
|
||||||
area2 = crossProduct.lengthSquare();
|
|
||||||
}
|
|
||||||
if (indexMaxPenetration != 3) {
|
|
||||||
// Compute the area
|
|
||||||
Vector3 vector1 = newPoint - mContacts[0]->getLocalPointOnBody1();
|
|
||||||
Vector3 vector2 = mContacts[2]->getLocalPointOnBody1() - mContacts[1]->getLocalPointOnBody1();
|
|
||||||
Vector3 crossProduct = vector1.cross(vector2);
|
|
||||||
area3 = crossProduct.lengthSquare();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Return the index of the contact to remove
|
|
||||||
return getMaxArea(area0, area1, area2, area3);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Return the index of maximum area
|
|
||||||
int PersistentContactCache::getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const {
|
|
||||||
if (area0 < area1) {
|
|
||||||
if (area1 < area2) {
|
|
||||||
if (area2 < area3) return 3;
|
|
||||||
else return 2;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
if (area1 < area3) return 3;
|
|
||||||
else return 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
if (area0 < area2) {
|
|
||||||
if (area2 < area3) return 3;
|
|
||||||
else return 2;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
if (area0 < area3) return 3;
|
|
||||||
else return 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Clear the cache
|
|
||||||
void PersistentContactCache::clear() {
|
|
||||||
for (uint i=0; i<mNbContacts; i++) {
|
|
||||||
|
|
||||||
// Call the destructor explicitly and tell the memory pool that
|
|
||||||
// the corresponding memory block is now free
|
|
||||||
mContacts[i]->Contact::~Contact();
|
|
||||||
mMemoryPoolContacts.freeObject(mContacts[i]);
|
|
||||||
}
|
|
||||||
mNbContacts = 0;
|
|
||||||
}
|
|
|
@ -1,147 +0,0 @@
|
||||||
/********************************************************************************
|
|
||||||
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
|
|
||||||
* Copyright (c) 2010-2012 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 PERSISTENT_CONTACT_CACHE_H
|
|
||||||
#define PERSISTENT_CONTACT_CACHE_H
|
|
||||||
|
|
||||||
// Libraries
|
|
||||||
#include <vector>
|
|
||||||
#include "../body/Body.h"
|
|
||||||
#include "../constraint/Contact.h"
|
|
||||||
|
|
||||||
// ReactPhysics3D namespace
|
|
||||||
namespace reactphysics3d {
|
|
||||||
|
|
||||||
// Constants
|
|
||||||
const uint MAX_CONTACTS_IN_CACHE = 4; // Maximum number of contacts in the persistent cache
|
|
||||||
|
|
||||||
|
|
||||||
/* -------------------------------------------------------------------
|
|
||||||
Class PersistentContactCache :
|
|
||||||
This class represents a cache of at most 4 contact points between
|
|
||||||
two given bodies. The contacts between two bodies are added one
|
|
||||||
after the other in the cache. When the cache is full, we have
|
|
||||||
to remove one point. The idea is to keep the point with the deepest
|
|
||||||
penetration depth and also to keep the points producing the larger
|
|
||||||
area (for a more stable contact manifold). The new added point is
|
|
||||||
always kept. This kind of persistent cache has been explained for
|
|
||||||
instance in the presentation from Erin Catto about contact manifolds
|
|
||||||
at the GDC 2007 conference.
|
|
||||||
-------------------------------------------------------------------
|
|
||||||
*/
|
|
||||||
class PersistentContactCache {
|
|
||||||
|
|
||||||
private:
|
|
||||||
|
|
||||||
// -------------------- Attributes -------------------- //
|
|
||||||
|
|
||||||
// Pointer to the first body
|
|
||||||
Body* const mBody1;
|
|
||||||
|
|
||||||
// Pointer to the second body
|
|
||||||
Body* const mBody2;
|
|
||||||
|
|
||||||
// Contacts in the cache
|
|
||||||
Contact* mContacts[MAX_CONTACTS_IN_CACHE];
|
|
||||||
|
|
||||||
// Number of contacts in the cache
|
|
||||||
uint mNbContacts;
|
|
||||||
|
|
||||||
// Reference to the memory pool with the contacts
|
|
||||||
MemoryPool<Contact>& mMemoryPoolContacts;
|
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
|
||||||
|
|
||||||
// Private copy-constructor
|
|
||||||
PersistentContactCache(const PersistentContactCache& persistentContactCache);
|
|
||||||
|
|
||||||
// Private assignment operator
|
|
||||||
PersistentContactCache& operator=(const PersistentContactCache& persistentContactCache);
|
|
||||||
|
|
||||||
// Return the index of maximum area
|
|
||||||
int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const;
|
|
||||||
|
|
||||||
// Return the index of the contact with the larger penetration depth
|
|
||||||
int getIndexOfDeepestPenetration(Contact* newContact) const;
|
|
||||||
|
|
||||||
// Return the index that will be removed
|
|
||||||
int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const;
|
|
||||||
|
|
||||||
// Remove a contact from the cache
|
|
||||||
void removeContact(int index);
|
|
||||||
|
|
||||||
// Return true if two vectors are approximatively equal
|
|
||||||
bool isApproxEqual(const Vector3& vector1, const Vector3& vector2) const;
|
|
||||||
|
|
||||||
public:
|
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
|
||||||
|
|
||||||
// Constructor
|
|
||||||
PersistentContactCache(Body* const mBody1, Body* const mBody2,
|
|
||||||
MemoryPool<Contact>& mMemoryPoolContacts);
|
|
||||||
|
|
||||||
// Destructor
|
|
||||||
~PersistentContactCache();
|
|
||||||
|
|
||||||
// Add a contact
|
|
||||||
void addContact(Contact* contact);
|
|
||||||
|
|
||||||
// Update the contact cache
|
|
||||||
void update(const Transform& transform1, const Transform& transform2);
|
|
||||||
|
|
||||||
// Clear the cache
|
|
||||||
void clear();
|
|
||||||
|
|
||||||
// Return the number of contacts in the cache
|
|
||||||
uint getNbContacts() const;
|
|
||||||
|
|
||||||
// Return a contact of the cache
|
|
||||||
Contact* getContact(uint index) const;
|
|
||||||
};
|
|
||||||
|
|
||||||
// Return the number of contacts in the cache
|
|
||||||
inline uint PersistentContactCache::getNbContacts() const {
|
|
||||||
return mNbContacts;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Return a contact of the cache
|
|
||||||
inline Contact* PersistentContactCache::getContact(uint index) const {
|
|
||||||
assert(index >= 0 && index < mNbContacts);
|
|
||||||
return mContacts[index];
|
|
||||||
}
|
|
||||||
|
|
||||||
// Return true if two vectors are approximatively equal
|
|
||||||
inline bool PersistentContactCache::isApproxEqual(const Vector3& vector1,
|
|
||||||
const Vector3& vector2) const {
|
|
||||||
const decimal epsilon = 0.1;
|
|
||||||
return (approxEqual(vector1.getX(), vector2.getX(), epsilon) &&
|
|
||||||
approxEqual(vector1.getY(), vector2.getY(), epsilon) &&
|
|
||||||
approxEqual(vector1.getZ(), vector2.getZ(), epsilon));
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
|
@ -153,7 +153,7 @@ inline void Timer::start() {
|
||||||
LARGE_INTEGER ticks;
|
LARGE_INTEGER ticks;
|
||||||
QueryPerformanceFrequency(&ticksPerSecond);
|
QueryPerformanceFrequency(&ticksPerSecond);
|
||||||
QueryPerformanceCounter(&ticks);
|
QueryPerformanceCounter(&ticks);
|
||||||
lastUpdateTime = double(ticks.QuadPart) / double(ticksPerSecond.QuadPart);
|
mLastUpdateTime = double(ticks.QuadPart) / double(ticksPerSecond.QuadPart);
|
||||||
#else
|
#else
|
||||||
// Initialize the lastUpdateTime with the current time in seconds
|
// Initialize the lastUpdateTime with the current time in seconds
|
||||||
timeval timeValue;
|
timeval timeValue;
|
||||||
|
|
|
@ -176,7 +176,7 @@ inline Transform Transform::interpolateTransforms(const Transform& oldTransform,
|
||||||
const Transform& newTransform,
|
const Transform& newTransform,
|
||||||
decimal interpolationFactor) {
|
decimal interpolationFactor) {
|
||||||
|
|
||||||
Vector3 interPosition = oldTransform.mPosition * (1.0 - interpolationFactor) +
|
Vector3 interPosition = oldTransform.mPosition * (decimal(1.0) - interpolationFactor) +
|
||||||
newTransform.mPosition * interpolationFactor;
|
newTransform.mPosition * interpolationFactor;
|
||||||
|
|
||||||
Quaternion interOrientation = Quaternion::slerp(oldTransform.mOrientation,
|
Quaternion interOrientation = Quaternion::slerp(oldTransform.mOrientation,
|
||||||
|
|
|
@ -26,7 +26,6 @@
|
||||||
// Libraries
|
// Libraries
|
||||||
#include "Vector3.h"
|
#include "Vector3.h"
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <cassert>
|
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
// Namespaces
|
// Namespaces
|
||||||
|
@ -97,3 +96,27 @@ Vector3 Vector3::getOneOrthogonalVector() const {
|
||||||
//assert(vector1.isUnit());
|
//assert(vector1.isUnit());
|
||||||
return vector1;
|
return vector1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Return one unit orthogonal vector of the current vector
|
||||||
|
Vector3 Vector3::getOneUnitOrthogonalVector() const {
|
||||||
|
assert(!this->isZero());
|
||||||
|
|
||||||
|
decimal x = mValues[0];
|
||||||
|
decimal y = mValues[1];
|
||||||
|
decimal z = mValues[2];
|
||||||
|
|
||||||
|
// Get the minimum element of the vector
|
||||||
|
Vector3 vectorAbs(fabs(x), fabs(y), fabs(z));
|
||||||
|
int minElement = vectorAbs.getMinAxis();
|
||||||
|
|
||||||
|
if (minElement == 0) {
|
||||||
|
return Vector3(0.0, -z, y) / sqrt(y*y + z*z);
|
||||||
|
}
|
||||||
|
else if (minElement == 1) {
|
||||||
|
return Vector3(-z, 0.0, x) / sqrt(x*x + z*z);
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
return Vector3(-y, x, 0.0) / sqrt(x*x + y*y);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
|
@ -28,6 +28,7 @@
|
||||||
|
|
||||||
// Libraries
|
// Libraries
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
|
#include <cassert>
|
||||||
#include "mathematics_functions.h"
|
#include "mathematics_functions.h"
|
||||||
#include "../decimal.h"
|
#include "../decimal.h"
|
||||||
|
|
||||||
|
@ -95,6 +96,9 @@ class Vector3 {
|
||||||
// Return the corresponding unit vector
|
// Return the corresponding unit vector
|
||||||
Vector3 getUnit() const;
|
Vector3 getUnit() const;
|
||||||
|
|
||||||
|
// Return one unit orthogonal vector of the current vector
|
||||||
|
Vector3 getOneUnitOrthogonalVector() const;
|
||||||
|
|
||||||
// Return true if the vector is unit and false otherwise
|
// Return true if the vector is unit and false otherwise
|
||||||
bool isUnit() const;
|
bool isUnit() const;
|
||||||
|
|
||||||
|
@ -110,6 +114,9 @@ class Vector3 {
|
||||||
// Cross product of two vectors
|
// Cross product of two vectors
|
||||||
Vector3 cross(const Vector3& vector) const;
|
Vector3 cross(const Vector3& vector) const;
|
||||||
|
|
||||||
|
// Normalize the vector
|
||||||
|
void normalize();
|
||||||
|
|
||||||
// Return the corresponding absolute value vector
|
// Return the corresponding absolute value vector
|
||||||
Vector3 getAbsoluteVector() const;
|
Vector3 getAbsoluteVector() const;
|
||||||
|
|
||||||
|
@ -137,6 +144,9 @@ class Vector3 {
|
||||||
// Overloaded operator for multiplication with a number with assignment
|
// Overloaded operator for multiplication with a number with assignment
|
||||||
Vector3& operator*=(decimal number);
|
Vector3& operator*=(decimal number);
|
||||||
|
|
||||||
|
// Overloaded operator for division by a number with assignment
|
||||||
|
Vector3& operator/=(decimal number);
|
||||||
|
|
||||||
// Overloaded operator for value access
|
// Overloaded operator for value access
|
||||||
decimal& operator[] (int index);
|
decimal& operator[] (int index);
|
||||||
|
|
||||||
|
@ -153,6 +163,7 @@ class Vector3 {
|
||||||
friend Vector3 operator-(const Vector3& vector);
|
friend Vector3 operator-(const Vector3& vector);
|
||||||
friend Vector3 operator*(const Vector3& vector, decimal number);
|
friend Vector3 operator*(const Vector3& vector, decimal number);
|
||||||
friend Vector3 operator*(decimal number, const Vector3& vector);
|
friend Vector3 operator*(decimal number, const Vector3& vector);
|
||||||
|
friend Vector3 operator/(const Vector3& vector, decimal number);
|
||||||
};
|
};
|
||||||
|
|
||||||
// Get the x component of the vector
|
// Get the x component of the vector
|
||||||
|
@ -217,6 +228,15 @@ inline Vector3 Vector3::cross(const Vector3& vector) const {
|
||||||
mValues[0] * vector.mValues[1] - mValues[1] * vector.mValues[0]);
|
mValues[0] * vector.mValues[1] - mValues[1] * vector.mValues[0]);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Normalize the vector
|
||||||
|
inline void Vector3::normalize() {
|
||||||
|
decimal l = length();
|
||||||
|
assert(l != 0.0);
|
||||||
|
mValues[0] /= l;
|
||||||
|
mValues[1] /= l;
|
||||||
|
mValues[2] /= l;
|
||||||
|
}
|
||||||
|
|
||||||
// Return the corresponding absolute value vector
|
// Return the corresponding absolute value vector
|
||||||
inline Vector3 Vector3::getAbsoluteVector() const {
|
inline Vector3 Vector3::getAbsoluteVector() const {
|
||||||
return Vector3(std::abs(mValues[0]), std::abs(mValues[1]), std::abs(mValues[2]));
|
return Vector3(std::abs(mValues[0]), std::abs(mValues[1]), std::abs(mValues[2]));
|
||||||
|
@ -283,6 +303,14 @@ inline Vector3& Vector3::operator*=(decimal number) {
|
||||||
return *this;
|
return *this;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Overloaded operator for division by a number with assignment
|
||||||
|
inline Vector3& Vector3::operator/=(decimal number) {
|
||||||
|
mValues[0] /= number;
|
||||||
|
mValues[1] /= number;
|
||||||
|
mValues[2] /= number;
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
|
||||||
// Overloaded operator for value access
|
// Overloaded operator for value access
|
||||||
inline decimal& Vector3::operator[] (int index) {
|
inline decimal& Vector3::operator[] (int index) {
|
||||||
return mValues[index];
|
return mValues[index];
|
||||||
|
@ -313,6 +341,11 @@ inline Vector3 operator*(const Vector3& vector, decimal number) {
|
||||||
return Vector3(number * vector.mValues[0], number * vector.mValues[1], number * vector.mValues[2]);
|
return Vector3(number * vector.mValues[0], number * vector.mValues[1], number * vector.mValues[2]);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Overloaded operator for division by a number
|
||||||
|
inline Vector3 operator/(const Vector3& vector, decimal number) {
|
||||||
|
return Vector3(vector.mValues[0] / number, vector.mValues[1] / number, vector.mValues[2] / number);
|
||||||
|
}
|
||||||
|
|
||||||
// Overloaded operator for multiplication with a number
|
// Overloaded operator for multiplication with a number
|
||||||
inline Vector3 operator*(decimal number, const Vector3& vector) {
|
inline Vector3 operator*(decimal number, const Vector3& vector) {
|
||||||
return vector * number;
|
return vector * number;
|
||||||
|
|
Loading…
Reference in New Issue
Block a user