Minor constraint refactoring, to allow SPU-side processing for PLAYSTATION 3 (added non-virtual methods)
Also comment-out some code for __SPU__ to reduce code size Added btContactConstraint (only used on PS3 SPU right now, better to use btPersistentManifold directly for contact constraints) Improved readblend utility library (see also usage in http://gamekit.googlecode.com with Irrlicht) Fix for btConvexConvexAlgorithm, potential division by zero Thanks linzner http://code.google.com/p/bullet/issues/detail?id=260
This commit is contained in:
erwin.coumans
@@ -14,6 +14,59 @@ subject to the following restrictions:
|
||||
*/
|
||||
|
||||
|
||||
#include "btContactConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "btJacobianEntry.h"
|
||||
#include "btContactSolverInfo.h"
|
||||
#include "LinearMath/btMinMax.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
|
||||
|
||||
btContactConstraint::btContactConstraint()
|
||||
:btTypedConstraint(CONTACT_CONSTRAINT_TYPE)
|
||||
{
|
||||
}
|
||||
|
||||
btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB)
|
||||
:btTypedConstraint(CONTACT_CONSTRAINT_TYPE,rbA,rbB),
|
||||
m_contactManifold(*contactManifold)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
btContactConstraint::~btContactConstraint()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void btContactConstraint::setContactManifold(btPersistentManifold* contactManifold)
|
||||
{
|
||||
m_contactManifold = *contactManifold;
|
||||
}
|
||||
|
||||
void btContactConstraint::getInfo1 (btConstraintInfo1* info)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void btContactConstraint::getInfo2 (btConstraintInfo2* info)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void btContactConstraint::buildJacobian()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void btContactConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
#include "btContactConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "LinearMath/btVector3.h"
|
||||
@@ -85,345 +138,4 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
|
||||
|
||||
|
||||
|
||||
//response between two dynamic objects with friction
|
||||
btScalar resolveSingleCollision(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& solverInfo)
|
||||
{
|
||||
|
||||
const btVector3& pos1_ = contactPoint.getPositionWorldOnA();
|
||||
const btVector3& pos2_ = contactPoint.getPositionWorldOnB();
|
||||
const btVector3& normal = contactPoint.m_normalWorldOnB;
|
||||
|
||||
//constant over all iterations
|
||||
btVector3 rel_pos1 = pos1_ - body1.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pos2_ - body2.getCenterOfMassPosition();
|
||||
|
||||
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
btScalar rel_vel;
|
||||
rel_vel = normal.dot(vel);
|
||||
|
||||
btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;
|
||||
|
||||
// btScalar damping = solverInfo.m_damping ;
|
||||
btScalar Kerp = solverInfo.m_erp;
|
||||
btScalar Kcor = Kerp *Kfps;
|
||||
|
||||
btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
|
||||
btAssert(cpd);
|
||||
btScalar distance = cpd->m_penetration;
|
||||
btScalar positionalError = Kcor *-distance;
|
||||
btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
|
||||
|
||||
btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
|
||||
|
||||
btScalar velocityImpulse = velocityError * cpd->m_jacDiagABInv;
|
||||
|
||||
btScalar normalImpulse = penetrationImpulse+velocityImpulse;
|
||||
|
||||
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
|
||||
btScalar oldNormalImpulse = cpd->m_appliedImpulse;
|
||||
btScalar sum = oldNormalImpulse + normalImpulse;
|
||||
cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
|
||||
|
||||
normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
|
||||
|
||||
#ifdef USE_INTERNAL_APPLY_IMPULSE
|
||||
if (body1.getInvMass())
|
||||
{
|
||||
body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
|
||||
}
|
||||
if (body2.getInvMass())
|
||||
{
|
||||
body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
|
||||
}
|
||||
#else //USE_INTERNAL_APPLY_IMPULSE
|
||||
body1.applyImpulse(normal*(normalImpulse), rel_pos1);
|
||||
body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
|
||||
#endif //USE_INTERNAL_APPLY_IMPULSE
|
||||
|
||||
return normalImpulse;
|
||||
}
|
||||
|
||||
|
||||
btScalar resolveSingleFriction(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& solverInfo)
|
||||
{
|
||||
|
||||
(void)solverInfo;
|
||||
|
||||
const btVector3& pos1 = contactPoint.getPositionWorldOnA();
|
||||
const btVector3& pos2 = contactPoint.getPositionWorldOnB();
|
||||
|
||||
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
|
||||
|
||||
btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
|
||||
btAssert(cpd);
|
||||
|
||||
btScalar combinedFriction = cpd->m_friction;
|
||||
|
||||
btScalar limit = cpd->m_appliedImpulse * combinedFriction;
|
||||
|
||||
if (cpd->m_appliedImpulse>btScalar(0.))
|
||||
//friction
|
||||
{
|
||||
//apply friction in the 2 tangential directions
|
||||
|
||||
// 1st tangent
|
||||
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
btScalar j1,j2;
|
||||
|
||||
{
|
||||
|
||||
btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
|
||||
|
||||
// calculate j that moves us to zero relative velocity
|
||||
j1 = -vrel * cpd->m_jacDiagABInvTangent0;
|
||||
btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse0;
|
||||
cpd->m_accumulatedTangentImpulse0 = oldTangentImpulse + j1;
|
||||
btSetMin(cpd->m_accumulatedTangentImpulse0, limit);
|
||||
btSetMax(cpd->m_accumulatedTangentImpulse0, -limit);
|
||||
j1 = cpd->m_accumulatedTangentImpulse0 - oldTangentImpulse;
|
||||
|
||||
}
|
||||
{
|
||||
// 2nd tangent
|
||||
|
||||
btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
|
||||
|
||||
// calculate j that moves us to zero relative velocity
|
||||
j2 = -vrel * cpd->m_jacDiagABInvTangent1;
|
||||
btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse1;
|
||||
cpd->m_accumulatedTangentImpulse1 = oldTangentImpulse + j2;
|
||||
btSetMin(cpd->m_accumulatedTangentImpulse1, limit);
|
||||
btSetMax(cpd->m_accumulatedTangentImpulse1, -limit);
|
||||
j2 = cpd->m_accumulatedTangentImpulse1 - oldTangentImpulse;
|
||||
}
|
||||
|
||||
#ifdef USE_INTERNAL_APPLY_IMPULSE
|
||||
if (body1.getInvMass())
|
||||
{
|
||||
body1.internalApplyImpulse(cpd->m_frictionWorldTangential0*body1.getInvMass(),cpd->m_frictionAngularComponent0A,j1);
|
||||
body1.internalApplyImpulse(cpd->m_frictionWorldTangential1*body1.getInvMass(),cpd->m_frictionAngularComponent1A,j2);
|
||||
}
|
||||
if (body2.getInvMass())
|
||||
{
|
||||
body2.internalApplyImpulse(cpd->m_frictionWorldTangential0*body2.getInvMass(),cpd->m_frictionAngularComponent0B,-j1);
|
||||
body2.internalApplyImpulse(cpd->m_frictionWorldTangential1*body2.getInvMass(),cpd->m_frictionAngularComponent1B,-j2);
|
||||
}
|
||||
#else //USE_INTERNAL_APPLY_IMPULSE
|
||||
body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1);
|
||||
body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2);
|
||||
#endif //USE_INTERNAL_APPLY_IMPULSE
|
||||
|
||||
|
||||
}
|
||||
return cpd->m_appliedImpulse;
|
||||
}
|
||||
|
||||
|
||||
btScalar resolveSingleFrictionOriginal(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& solverInfo);
|
||||
|
||||
btScalar resolveSingleFrictionOriginal(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& solverInfo)
|
||||
{
|
||||
|
||||
(void)solverInfo;
|
||||
|
||||
const btVector3& pos1 = contactPoint.getPositionWorldOnA();
|
||||
const btVector3& pos2 = contactPoint.getPositionWorldOnB();
|
||||
|
||||
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
|
||||
|
||||
btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
|
||||
btAssert(cpd);
|
||||
|
||||
btScalar combinedFriction = cpd->m_friction;
|
||||
|
||||
btScalar limit = cpd->m_appliedImpulse * combinedFriction;
|
||||
//if (contactPoint.m_appliedImpulse>btScalar(0.))
|
||||
//friction
|
||||
{
|
||||
//apply friction in the 2 tangential directions
|
||||
|
||||
{
|
||||
// 1st tangent
|
||||
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
|
||||
|
||||
// calculate j that moves us to zero relative velocity
|
||||
btScalar j = -vrel * cpd->m_jacDiagABInvTangent0;
|
||||
btScalar total = cpd->m_accumulatedTangentImpulse0 + j;
|
||||
btSetMin(total, limit);
|
||||
btSetMax(total, -limit);
|
||||
j = total - cpd->m_accumulatedTangentImpulse0;
|
||||
cpd->m_accumulatedTangentImpulse0 = total;
|
||||
body1.applyImpulse(j * cpd->m_frictionWorldTangential0, rel_pos1);
|
||||
body2.applyImpulse(j * -cpd->m_frictionWorldTangential0, rel_pos2);
|
||||
}
|
||||
|
||||
|
||||
{
|
||||
// 2nd tangent
|
||||
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
|
||||
|
||||
// calculate j that moves us to zero relative velocity
|
||||
btScalar j = -vrel * cpd->m_jacDiagABInvTangent1;
|
||||
btScalar total = cpd->m_accumulatedTangentImpulse1 + j;
|
||||
btSetMin(total, limit);
|
||||
btSetMax(total, -limit);
|
||||
j = total - cpd->m_accumulatedTangentImpulse1;
|
||||
cpd->m_accumulatedTangentImpulse1 = total;
|
||||
body1.applyImpulse(j * cpd->m_frictionWorldTangential1, rel_pos1);
|
||||
body2.applyImpulse(j * -cpd->m_frictionWorldTangential1, rel_pos2);
|
||||
}
|
||||
}
|
||||
return cpd->m_appliedImpulse;
|
||||
}
|
||||
|
||||
|
||||
//velocity + friction
|
||||
//response between two dynamic objects with friction
|
||||
btScalar resolveSingleCollisionCombined(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& solverInfo)
|
||||
{
|
||||
|
||||
const btVector3& pos1 = contactPoint.getPositionWorldOnA();
|
||||
const btVector3& pos2 = contactPoint.getPositionWorldOnB();
|
||||
const btVector3& normal = contactPoint.m_normalWorldOnB;
|
||||
|
||||
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
|
||||
|
||||
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
btScalar rel_vel;
|
||||
rel_vel = normal.dot(vel);
|
||||
|
||||
btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;
|
||||
|
||||
//btScalar damping = solverInfo.m_damping ;
|
||||
btScalar Kerp = solverInfo.m_erp;
|
||||
btScalar Kcor = Kerp *Kfps;
|
||||
|
||||
btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
|
||||
btAssert(cpd);
|
||||
btScalar distance = cpd->m_penetration;
|
||||
btScalar positionalError = Kcor *-distance;
|
||||
btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
|
||||
|
||||
btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
|
||||
|
||||
btScalar velocityImpulse = velocityError * cpd->m_jacDiagABInv;
|
||||
|
||||
btScalar normalImpulse = penetrationImpulse+velocityImpulse;
|
||||
|
||||
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
|
||||
btScalar oldNormalImpulse = cpd->m_appliedImpulse;
|
||||
btScalar sum = oldNormalImpulse + normalImpulse;
|
||||
cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
|
||||
|
||||
normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
|
||||
|
||||
|
||||
#ifdef USE_INTERNAL_APPLY_IMPULSE
|
||||
if (body1.getInvMass())
|
||||
{
|
||||
body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
|
||||
}
|
||||
if (body2.getInvMass())
|
||||
{
|
||||
body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
|
||||
}
|
||||
#else //USE_INTERNAL_APPLY_IMPULSE
|
||||
body1.applyImpulse(normal*(normalImpulse), rel_pos1);
|
||||
body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
|
||||
#endif //USE_INTERNAL_APPLY_IMPULSE
|
||||
|
||||
{
|
||||
//friction
|
||||
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
rel_vel = normal.dot(vel);
|
||||
|
||||
|
||||
btVector3 lat_vel = vel - normal * rel_vel;
|
||||
btScalar lat_rel_vel = lat_vel.length();
|
||||
|
||||
btScalar combinedFriction = cpd->m_friction;
|
||||
|
||||
if (cpd->m_appliedImpulse > 0)
|
||||
if (lat_rel_vel > SIMD_EPSILON)
|
||||
{
|
||||
lat_vel /= lat_rel_vel;
|
||||
btVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel);
|
||||
btVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel);
|
||||
btScalar friction_impulse = lat_rel_vel /
|
||||
(body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
|
||||
btScalar normal_impulse = cpd->m_appliedImpulse * combinedFriction;
|
||||
|
||||
btSetMin(friction_impulse, normal_impulse);
|
||||
btSetMax(friction_impulse, -normal_impulse);
|
||||
body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
|
||||
body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
return normalImpulse;
|
||||
}
|
||||
|
||||
btScalar resolveSingleFrictionEmpty(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& solverInfo);
|
||||
|
||||
btScalar resolveSingleFrictionEmpty(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& solverInfo)
|
||||
{
|
||||
(void)contactPoint;
|
||||
(void)body1;
|
||||
(void)body2;
|
||||
(void)solverInfo;
|
||||
|
||||
|
||||
return btScalar(0.);
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user