TorqueGameEngines/Torque3D
1
0
Fork 0
mirror of https://github.com/TorqueGameEngines/Torque3D.git synced 2026-07-13 15:44:36 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions 3

Bullet 2.85 update

Browse source
This commit is contained in:
rextimmy 2016-12-28 18:32:21 +10:00
parent 38bf2b8175
commit 540c9b72c0
391 changed files with 32183 additions and 58559 deletions
Download patch file Download diff file Expand all files Collapse all files

19
Engine/lib/bullet/src/BulletDynamics/CMakeLists.txt
View file

@ -10,18 +10,22 @@ SET(BulletDynamics_SRCS
ConstraintSolver/btGearConstraint.cpp
ConstraintSolver/btGeneric6DofConstraint.cpp
ConstraintSolver/btGeneric6DofSpringConstraint.cpp
ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
ConstraintSolver/btHinge2Constraint.cpp
ConstraintSolver/btHingeConstraint.cpp
ConstraintSolver/btPoint2PointConstraint.cpp
ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
ConstraintSolver/btNNCGConstraintSolver.cpp
ConstraintSolver/btSliderConstraint.cpp
ConstraintSolver/btSolve2LinearConstraint.cpp
ConstraintSolver/btTypedConstraint.cpp
ConstraintSolver/btUniversalConstraint.cpp
Dynamics/btDiscreteDynamicsWorld.cpp
Dynamics/btDiscreteDynamicsWorldMt.cpp
Dynamics/btSimulationIslandManagerMt.cpp
Dynamics/btRigidBody.cpp
Dynamics/btSimpleDynamicsWorld.cpp
Dynamics/Bullet-C-API.cpp
# Dynamics/Bullet-C-API.cpp
Vehicle/btRaycastVehicle.cpp
Vehicle/btWheelInfo.cpp
Featherstone/btMultiBody.cpp
@ -30,9 +34,12 @@ SET(BulletDynamics_SRCS
Featherstone/btMultiBodyJointLimitConstraint.cpp
Featherstone/btMultiBodyConstraint.cpp
Featherstone/btMultiBodyPoint2Point.cpp
Featherstone/btMultiBodyFixedConstraint.cpp
Featherstone/btMultiBodySliderConstraint.cpp
Featherstone/btMultiBodyJointMotor.cpp
MLCPSolvers/btDantzigLCP.cpp
MLCPSolvers/btMLCPSolver.cpp
MLCPSolvers/btLemkeAlgorithm.cpp
)
SET(Root_HDRS
@ -48,11 +55,13 @@ SET(ConstraintSolver_HDRS
ConstraintSolver/btGearConstraint.h
ConstraintSolver/btGeneric6DofConstraint.h
ConstraintSolver/btGeneric6DofSpringConstraint.h
ConstraintSolver/btGeneric6DofSpring2Constraint.h
ConstraintSolver/btHinge2Constraint.h
ConstraintSolver/btHingeConstraint.h
ConstraintSolver/btJacobianEntry.h
ConstraintSolver/btPoint2PointConstraint.h
ConstraintSolver/btSequentialImpulseConstraintSolver.h
ConstraintSolver/btNNCGConstraintSolver.h
ConstraintSolver/btSliderConstraint.h
ConstraintSolver/btSolve2LinearConstraint.h
ConstraintSolver/btSolverBody.h
@ -63,6 +72,8 @@ SET(ConstraintSolver_HDRS
SET(Dynamics_HDRS
Dynamics/btActionInterface.h
Dynamics/btDiscreteDynamicsWorld.h
Dynamics/btDiscreteDynamicsWorldMt.h
Dynamics/btSimulationIslandManagerMt.h
Dynamics/btDynamicsWorld.h
Dynamics/btSimpleDynamicsWorld.h
Dynamics/btRigidBody.h
@ -84,6 +95,8 @@ SET(Featherstone_HDRS
Featherstone/btMultiBodyJointLimitConstraint.h
Featherstone/btMultiBodyConstraint.h
Featherstone/btMultiBodyPoint2Point.h
Featherstone/btMultiBodyFixedConstraint.h
Featherstone/btMultiBodySliderConstraint.h
Featherstone/btMultiBodyJointMotor.h
)
@ -93,7 +106,9 @@ SET(MLCPSolvers_HDRS
MLCPSolvers/btMLCPSolver.h
MLCPSolvers/btMLCPSolverInterface.h
MLCPSolvers/btPATHSolver.h
MLCPSolvers/btSolveProjectedGaussSeidel.h
MLCPSolvers/btSolveProjectedGaussSeidel.h
MLCPSolvers/btLemkeSolver.h
MLCPSolvers/btLemkeAlgorithm.h
)
SET(Character_HDRS

2
Engine/lib/bullet/src/BulletDynamics/Character/btCharacterControllerInterface.h
View file

@ -37,7 +37,7 @@ public:
virtual void preStep ( btCollisionWorld* collisionWorld) = 0;
virtual void playerStep (btCollisionWorld* collisionWorld, btScalar dt) = 0;
virtual bool canJump () const = 0;
virtual void jump () = 0;
virtual void jump(const btVector3& dir = btVector3()) = 0;
virtual bool onGround () const = 0;
virtual void setUpInterpolate (bool value) = 0;

483
Engine/lib/bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp
View file

@ -29,9 +29,10 @@ subject to the following restrictions:
static btVector3
getNormalizedVector(const btVector3& v)
{
btVector3 n = v.normalized();
if (n.length() < SIMD_EPSILON) {
n.setValue(0, 0, 0);
btVector3 n(0, 0, 0);
if (v.length() > SIMD_EPSILON) {
n = v.normalized();
}
return n;
}
@ -131,30 +132,37 @@ btVector3 btKinematicCharacterController::perpindicularComponent (const btVector
return direction - parallelComponent(direction, normal);
}
btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis)
btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up)
{
m_upAxis = upAxis;
m_addedMargin = 0.02;
m_walkDirection.setValue(0,0,0);
m_useGhostObjectSweepTest = true;
m_ghostObject = ghostObject;
m_stepHeight = stepHeight;
m_up.setValue(0.0f, 0.0f, 1.0f);
m_jumpAxis.setValue(0.0f, 0.0f, 1.0f);
setUp(up);
setStepHeight(stepHeight);
m_addedMargin = 0.02;
m_walkDirection.setValue(0.0,0.0,0.0);
m_AngVel.setValue(0.0, 0.0, 0.0);
m_useGhostObjectSweepTest = true;
m_turnAngle = btScalar(0.0);
m_convexShape=convexShape;
m_useWalkDirection = true; // use walk direction by default, legacy behavior
m_velocityTimeInterval = 0.0;
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
m_gravity = 9.8 * 3 ; // 3G acceleration.
m_gravity = 9.8 * 3.0 ; // 3G acceleration.
m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s.
m_jumpSpeed = 10.0; // ?
m_SetjumpSpeed = m_jumpSpeed;
m_wasOnGround = false;
m_wasJumping = false;
m_interpolateUp = true;
setMaxSlope(btRadians(45.0));
m_currentStepOffset = 0;
m_currentStepOffset = 0.0;
m_maxPenetrationDepth = 0.2;
full_drop = false;
bounce_fix = false;
m_linearDamping = btScalar(0.0);
m_angularDamping = btScalar(0.0);
}
btKinematicCharacterController::~btKinematicCharacterController ()
@ -189,7 +197,7 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
btScalar maxPen = btScalar(0.0);
// btScalar maxPen = btScalar(0.0);
for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++)
{
m_manifoldArray.resize(0);
@ -197,10 +205,13 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
btCollisionObject* obj0 = static_cast<btCollisionObject*>(collisionPair->m_pProxy0->m_clientObject);
btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse()))
continue;
if (!needsCollision(obj0, obj1))
continue;
if (collisionPair->m_algorithm)
collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);
@ -216,14 +227,15 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
btScalar dist = pt.getDistance();
if (dist < 0.0)
if (dist < -m_maxPenetrationDepth)
{
if (dist < maxPen)
{
maxPen = dist;
m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
// TODO: cause problems on slopes, not sure if it is needed
//if (dist < maxPen)
//{
// maxPen = dist;
// m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
}
//}
m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2);
penetration = true;
} else {
@ -243,18 +255,28 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
{
btScalar stepHeight = 0.0f;
if (m_verticalVelocity < 0.0)
stepHeight = m_stepHeight;
// phase 1: up
btTransform start, end;
m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.f?m_verticalOffset:0.f));
start.setIdentity ();
end.setIdentity ();
/* FIXME: Handle penetration properly */
start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_convexShape->getMargin() + m_addedMargin));
start.setOrigin(m_currentPosition);
m_targetPosition = m_currentPosition + m_up * (stepHeight) + m_jumpAxis * ((m_verticalOffset > 0.f ? m_verticalOffset : 0.f));
m_currentPosition = m_targetPosition;
end.setOrigin (m_targetPosition);
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.7071));
start.setRotation(m_currentOrientation);
end.setRotation(m_targetOrientation);
btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, -m_up, m_maxSlopeCosine);
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
@ -264,29 +286,61 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
}
else
{
world->convexSweepTest (m_convexShape, start, end, callback);
world->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
}
if (callback.hasHit())
if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
{
// Only modify the position if the hit was a slope and not a wall or ceiling.
if(callback.m_hitNormalWorld.dot(getUpAxisDirections()[m_upAxis]) > 0.0)
if (callback.m_hitNormalWorld.dot(m_up) > 0.0)
{
// we moved up only a fraction of the step height
m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
m_currentStepOffset = stepHeight * callback.m_closestHitFraction;
if (m_interpolateUp == true)
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
else
m_currentPosition = m_targetPosition;
}
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
btTransform& xform = m_ghostObject->getWorldTransform();
xform.setOrigin(m_currentPosition);
m_ghostObject->setWorldTransform(xform);
// fix penetration if we hit a ceiling for example
int numPenetrationLoops = 0;
m_touchingContact = false;
while (recoverFromPenetration(world))
{
numPenetrationLoops++;
m_touchingContact = true;
if (numPenetrationLoops > 4)
{
//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
break;
}
}
m_targetPosition = m_ghostObject->getWorldTransform().getOrigin();
m_currentPosition = m_targetPosition;
if (m_verticalOffset > 0)
{
m_verticalOffset = 0.0;
m_verticalVelocity = 0.0;
m_currentStepOffset = m_stepHeight;
}
} else {
m_currentStepOffset = m_stepHeight;
m_currentStepOffset = stepHeight;
m_currentPosition = m_targetPosition;
}
}
bool btKinematicCharacterController::needsCollision(const btCollisionObject* body0, const btCollisionObject* body1)
{
bool collides = (body0->getBroadphaseHandle()->m_collisionFilterGroup & body1->getBroadphaseHandle()->m_collisionFilterMask) != 0;
collides = collides && (body1->getBroadphaseHandle()->m_collisionFilterGroup & body0->getBroadphaseHandle()->m_collisionFilterMask);
return collides;
}
void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag)
{
btVector3 movementDirection = m_targetPosition - m_currentPosition;
@ -329,6 +383,7 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
// m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
// phase 2: forward and strafe
btTransform start, end;
m_targetPosition = m_currentPosition + walkMove;
start.setIdentity ();
@ -338,15 +393,6 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
// printf("distance2=%f\n",distance2);
if (m_touchingContact)
{
if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0))
{
//interferes with step movement
//updateTargetPositionBasedOnCollision (m_touchingNormal);
}
}
int maxIter = 10;
while (fraction > btScalar(0.01) && maxIter-- > 0)
@ -355,6 +401,9 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
end.setOrigin (m_targetPosition);
btVector3 sweepDirNegative(m_currentPosition - m_targetPosition);
start.setRotation(m_currentOrientation);
end.setRotation(m_targetOrientation);
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, sweepDirNegative, btScalar(0.0));
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
@ -363,25 +412,27 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
btScalar margin = m_convexShape->getMargin();
m_convexShape->setMargin(margin + m_addedMargin);
if (m_useGhostObjectSweepTest)
if (!(start == end))
{
m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
} else
{
collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
if (m_useGhostObjectSweepTest)
{
m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
}
else
{
collisionWorld->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
}
}
m_convexShape->setMargin(margin);
fraction -= callback.m_closestHitFraction;
if (callback.hasHit())
if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
{
// we moved only a fraction
btScalar hitDistance;
hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
//btScalar hitDistance;
//hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
// m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
@ -402,14 +453,11 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
break;
}
} else {
// we moved whole way
m_currentPosition = m_targetPosition;
}
// if (callback.m_closestHitFraction == 0.f)
// break;
else
{
m_currentPosition = m_targetPosition;
}
}
}
@ -420,27 +468,30 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
// phase 3: down
/*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep);
btVector3 step_drop = m_up * (m_currentStepOffset + additionalDownStep);
btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt;
btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity;
btVector3 gravity_drop = m_up * downVelocity;
m_targetPosition -= (step_drop + gravity_drop);*/
btVector3 orig_position = m_targetPosition;
btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
if (m_verticalVelocity > 0.0)
return;
if(downVelocity > 0.0 && downVelocity > m_fallSpeed
&& (m_wasOnGround || !m_wasJumping))
downVelocity = m_fallSpeed;
btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, m_up, m_maxSlopeCosine);
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
btKinematicClosestNotMeConvexResultCallback callback2 (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
btKinematicClosestNotMeConvexResultCallback callback2(m_ghostObject, m_up, m_maxSlopeCosine);
callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
@ -454,6 +505,9 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
start.setOrigin (m_currentPosition);
end.setOrigin (m_targetPosition);
start.setRotation(m_currentOrientation);
end.setRotation(m_targetOrientation);
//set double test for 2x the step drop, to check for a large drop vs small drop
end_double.setOrigin (m_targetPosition - step_drop);
@ -461,7 +515,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
{
m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
if (!callback.hasHit())
if (!callback.hasHit() && m_ghostObject->hasContactResponse())
{
//test a double fall height, to see if the character should interpolate it's fall (full) or not (partial)
m_ghostObject->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
@ -470,30 +524,34 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
{
collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
if (!callback.hasHit())
{
//test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
}
if (!callback.hasHit() && m_ghostObject->hasContactResponse())
{
//test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
}
}
btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
bool has_hit = false;
bool has_hit;
if (bounce_fix == true)
has_hit = callback.hasHit() || callback2.hasHit();
has_hit = (callback.hasHit() || callback2.hasHit()) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject);
else
has_hit = callback2.hasHit();
has_hit = callback2.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback2.m_hitCollisionObject);
if(downVelocity2 > 0.0 && downVelocity2 < m_stepHeight && has_hit == true && runonce == false
btScalar stepHeight = 0.0f;
if (m_verticalVelocity < 0.0)
stepHeight = m_stepHeight;
if (downVelocity2 > 0.0 && downVelocity2 < stepHeight && has_hit == true && runonce == false
&& (m_wasOnGround || !m_wasJumping))
{
//redo the velocity calculation when falling a small amount, for fast stairs motion
//for larger falls, use the smoother/slower interpolated movement by not touching the target position
m_targetPosition = orig_position;
downVelocity = m_stepHeight;
downVelocity = stepHeight;
btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
step_drop = m_up * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
runonce = true;
continue; //re-run previous tests
@ -501,10 +559,9 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
break;
}
if (callback.hasHit() || runonce == true)
if (m_ghostObject->hasContactResponse() && (callback.hasHit() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) || runonce == true)
{
// we dropped a fraction of the height -> hit floor
btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2;
//printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY());
@ -512,10 +569,10 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
if (bounce_fix == true)
{
if (full_drop == true)
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
else
//due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction);
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
else
//due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction);
}
else
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
@ -527,7 +584,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
m_wasJumping = false;
} else {
// we dropped the full height
full_drop = true;
if (bounce_fix == true)
@ -537,7 +594,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
{
m_targetPosition += step_drop; //undo previous target change
downVelocity = m_fallSpeed;
step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
step_drop = m_up * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
}
}
@ -578,21 +635,63 @@ btScalar timeInterval
m_velocityTimeInterval += timeInterval;
}
void btKinematicCharacterController::setAngularVelocity(const btVector3& velocity)
{
m_AngVel = velocity;
}
const btVector3& btKinematicCharacterController::getAngularVelocity() const
{
return m_AngVel;
}
void btKinematicCharacterController::setLinearVelocity(const btVector3& velocity)
{
m_walkDirection = velocity;
// HACK: if we are moving in the direction of the up, treat it as a jump :(
if (m_walkDirection.length2() > 0)
{
btVector3 w = velocity.normalized();
btScalar c = w.dot(m_up);
if (c != 0)
{
//there is a component in walkdirection for vertical velocity
btVector3 upComponent = m_up * (sinf(SIMD_HALF_PI - acosf(c)) * m_walkDirection.length());
m_walkDirection -= upComponent;
m_verticalVelocity = (c < 0.0f ? -1 : 1) * upComponent.length();
if (c > 0.0f)
{
m_wasJumping = true;
m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin();
}
}
}
else
m_verticalVelocity = 0.0f;
}
btVector3 btKinematicCharacterController::getLinearVelocity() const
{
return m_walkDirection + (m_verticalVelocity * m_up);
}
void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld )
{
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
m_wasOnGround = false;
m_wasJumping = false;
m_walkDirection.setValue(0,0,0);
m_velocityTimeInterval = 0.0;
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
m_wasOnGround = false;
m_wasJumping = false;
m_walkDirection.setValue(0,0,0);
m_velocityTimeInterval = 0.0;
//clear pair cache
btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache();
while (cache->getOverlappingPairArray().size() > 0)
{
cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
}
//clear pair cache
btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache();
while (cache->getOverlappingPairArray().size() > 0)
{
cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
}
}
void btKinematicCharacterController::warp (const btVector3& origin)
@ -606,62 +705,99 @@ void btKinematicCharacterController::warp (const btVector3& origin)
void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld)
{
int numPenetrationLoops = 0;
m_touchingContact = false;
while (recoverFromPenetration (collisionWorld))
{
numPenetrationLoops++;
m_touchingContact = true;
if (numPenetrationLoops > 4)
{
//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
break;
}
}
m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
m_targetPosition = m_currentPosition;
m_currentOrientation = m_ghostObject->getWorldTransform().getRotation();
m_targetOrientation = m_currentOrientation;
// printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);
}
#include <stdio.h>
void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt)
{
// printf("playerStep(): ");
// printf(" dt = %f", dt);
if (m_AngVel.length2() > 0.0f)
{
m_AngVel *= btPow(btScalar(1) - m_angularDamping, dt);
}
// integrate for angular velocity
if (m_AngVel.length2() > 0.0f)
{
btTransform xform;
xform = m_ghostObject->getWorldTransform();
btQuaternion rot(m_AngVel.normalized(), m_AngVel.length() * dt);
btQuaternion orn = rot * xform.getRotation();
xform.setRotation(orn);
m_ghostObject->setWorldTransform(xform);
m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
m_targetPosition = m_currentPosition;
m_currentOrientation = m_ghostObject->getWorldTransform().getRotation();
m_targetOrientation = m_currentOrientation;
}
// quick check...
if (!m_useWalkDirection && m_velocityTimeInterval <= 0.0) {
if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0)) {
// printf("\n");
return; // no motion
}
m_wasOnGround = onGround();
//btVector3 lvel = m_walkDirection;
btScalar c = 0.0f;
if (m_walkDirection.length2() > 0)
{
// apply damping
m_walkDirection *= btPow(btScalar(1) - m_linearDamping, dt);
}
m_verticalVelocity *= btPow(btScalar(1) - m_linearDamping, dt);
// Update fall velocity.
m_verticalVelocity -= m_gravity * dt;
if(m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
{
m_verticalVelocity = m_jumpSpeed;
}
if(m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
{
m_verticalVelocity = -btFabs(m_fallSpeed);
}
m_verticalOffset = m_verticalVelocity * dt;
btTransform xform;
xform = m_ghostObject->getWorldTransform ();
xform = m_ghostObject->getWorldTransform();
// printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
// printf("walkSpeed=%f\n",walkSpeed);
stepUp (collisionWorld);
stepUp(collisionWorld);
//todo: Experimenting with behavior of controller when it hits a ceiling..
//bool hitUp = stepUp (collisionWorld);
//if (hitUp)
//{
// m_verticalVelocity -= m_gravity * dt;
// if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
// {
// m_verticalVelocity = m_jumpSpeed;
// }
// if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
// {
// m_verticalVelocity = -btFabs(m_fallSpeed);
// }
// m_verticalOffset = m_verticalVelocity * dt;
// xform = m_ghostObject->getWorldTransform();
//}
if (m_useWalkDirection) {
stepForwardAndStrafe (collisionWorld, m_walkDirection);
} else {
@ -681,10 +817,38 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo
}
stepDown (collisionWorld, dt);
//todo: Experimenting with max jump height
//if (m_wasJumping)
//{
// btScalar ds = m_currentPosition[m_upAxis] - m_jumpPosition[m_upAxis];
// if (ds > m_maxJumpHeight)
// {
// // substract the overshoot
// m_currentPosition[m_upAxis] -= ds - m_maxJumpHeight;
// // max height was reached, so potential energy is at max
// // and kinematic energy is 0, thus velocity is 0.
// if (m_verticalVelocity > 0.0)
// m_verticalVelocity = 0.0;
// }
//}
// printf("\n");
xform.setOrigin (m_currentPosition);
m_ghostObject->setWorldTransform (xform);
int numPenetrationLoops = 0;
m_touchingContact = false;
while (recoverFromPenetration(collisionWorld))
{
numPenetrationLoops++;
m_touchingContact = true;
if (numPenetrationLoops > 4)
{
//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
break;
}
}
}
void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
@ -695,6 +859,7 @@ void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed)
{
m_jumpSpeed = jumpSpeed;
m_SetjumpSpeed = m_jumpSpeed;
}
void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight)
@ -707,14 +872,16 @@ bool btKinematicCharacterController::canJump () const
return onGround();
}
void btKinematicCharacterController::jump ()
void btKinematicCharacterController::jump(const btVector3& v)
{
if (!canJump())
return;
m_jumpSpeed = v.length2() == 0 ? m_SetjumpSpeed : v.length();
m_verticalVelocity = m_jumpSpeed;
m_wasJumping = true;
m_jumpAxis = v.length2() == 0 ? m_up : v.normalized();
m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin();
#if 0
currently no jumping.
btTransform xform;
@ -726,14 +893,16 @@ void btKinematicCharacterController::jump ()
#endif
}
void btKinematicCharacterController::setGravity(btScalar gravity)
void btKinematicCharacterController::setGravity(const btVector3& gravity)
{
m_gravity = gravity;
if (gravity.length2() > 0) setUpVector(-gravity);
m_gravity = gravity.length();
}
btScalar btKinematicCharacterController::getGravity() const
btVector3 btKinematicCharacterController::getGravity() const
{
return m_gravity;
return -m_gravity * m_up;
}
void btKinematicCharacterController::setMaxSlope(btScalar slopeRadians)
@ -747,11 +916,25 @@ btScalar btKinematicCharacterController::getMaxSlope() const
return m_maxSlopeRadians;
}
bool btKinematicCharacterController::onGround () const
void btKinematicCharacterController::setMaxPenetrationDepth(btScalar d)
{
return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0;
m_maxPenetrationDepth = d;
}
btScalar btKinematicCharacterController::getMaxPenetrationDepth() const
{
return m_maxPenetrationDepth;
}
bool btKinematicCharacterController::onGround () const
{
return (fabs(m_verticalVelocity) < SIMD_EPSILON) && (fabs(m_verticalOffset) < SIMD_EPSILON);
}
void btKinematicCharacterController::setStepHeight(btScalar h)
{
m_stepHeight = h;
}
btVector3* btKinematicCharacterController::getUpAxisDirections()
{
@ -768,3 +951,49 @@ void btKinematicCharacterController::setUpInterpolate(bool value)
{
m_interpolateUp = value;
}
void btKinematicCharacterController::setUp(const btVector3& up)
{
if (up.length2() > 0 && m_gravity > 0.0f)
{
setGravity(-m_gravity * up.normalized());
return;
}
setUpVector(up);
}
void btKinematicCharacterController::setUpVector(const btVector3& up)
{
if (m_up == up)
return;
btVector3 u = m_up;
if (up.length2() > 0)
m_up = up.normalized();
else
m_up = btVector3(0.0, 0.0, 0.0);
if (!m_ghostObject) return;
btQuaternion rot = getRotation(m_up, u);
//set orientation with new up
btTransform xform;
xform = m_ghostObject->getWorldTransform();
btQuaternion orn = rot.inverse() * xform.getRotation();
xform.setRotation(orn);
m_ghostObject->setWorldTransform(xform);
}
btQuaternion btKinematicCharacterController::getRotation(btVector3& v0, btVector3& v1) const
{
if (v0.length2() == 0.0f || v1.length2() == 0.0f)
{
btQuaternion q;
return q;
}
return shortestArcQuatNormalize2(v0, v1);
}

60
Engine/lib/bullet/src/BulletDynamics/Character/btKinematicCharacterController.h
View file

@ -43,10 +43,12 @@ protected:
btPairCachingGhostObject* m_ghostObject;
btConvexShape* m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast
btScalar m_maxPenetrationDepth;
btScalar m_verticalVelocity;
btScalar m_verticalOffset;
btScalar m_fallSpeed;
btScalar m_jumpSpeed;
btScalar m_SetjumpSpeed;
btScalar m_maxJumpHeight;
btScalar m_maxSlopeRadians; // Slope angle that is set (used for returning the exact value)
btScalar m_maxSlopeCosine; // Cosine equivalent of m_maxSlopeRadians (calculated once when set, for optimization)
@ -61,24 +63,34 @@ protected:
///this is the desired walk direction, set by the user
btVector3 m_walkDirection;
btVector3 m_normalizedDirection;
btVector3 m_AngVel;
btVector3 m_jumpPosition;
//some internal variables
btVector3 m_currentPosition;
btScalar m_currentStepOffset;
btVector3 m_targetPosition;
btQuaternion m_currentOrientation;
btQuaternion m_targetOrientation;
///keep track of the contact manifolds
btManifoldArray m_manifoldArray;
bool m_touchingContact;
btVector3 m_touchingNormal;
btScalar m_linearDamping;
btScalar m_angularDamping;
bool m_wasOnGround;
bool m_wasJumping;
bool m_useGhostObjectSweepTest;
bool m_useWalkDirection;
btScalar m_velocityTimeInterval;
int m_upAxis;
btVector3 m_up;
btVector3 m_jumpAxis;
static btVector3* getUpAxisDirections();
bool m_interpolateUp;
@ -94,11 +106,18 @@ protected:
void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
void stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove);
void stepDown (btCollisionWorld* collisionWorld, btScalar dt);
virtual bool needsCollision(const btCollisionObject* body0, const btCollisionObject* body1);
void setUpVector(const btVector3& up);
btQuaternion getRotation(btVector3& v0, btVector3& v1) const;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1);
btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up = btVector3(1.0,0.0,0.0));
~btKinematicCharacterController ();
@ -112,14 +131,9 @@ public:
///btActionInterface interface
void debugDraw(btIDebugDraw* debugDrawer);
void setUpAxis (int axis)
{
if (axis < 0)
axis = 0;
if (axis > 2)
axis = 2;
m_upAxis = axis;
}
void setUp(const btVector3& up);
const btVector3& getUp() { return m_up; }
/// This should probably be called setPositionIncrementPerSimulatorStep.
/// This is neither a direction nor a velocity, but the amount to
@ -136,27 +150,47 @@ public:
virtual void setVelocityForTimeInterval(const btVector3& velocity,
btScalar timeInterval);
virtual void setAngularVelocity(const btVector3& velocity);
virtual const btVector3& getAngularVelocity() const;
virtual void setLinearVelocity(const btVector3& velocity);
virtual btVector3 getLinearVelocity() const;
void setLinearDamping(btScalar d) { m_linearDamping = btClamped(d, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); }
btScalar getLinearDamping() const { return m_linearDamping; }
void setAngularDamping(btScalar d) { m_angularDamping = btClamped(d, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); }
btScalar getAngularDamping() const { return m_angularDamping; }
void reset ( btCollisionWorld* collisionWorld );
void warp (const btVector3& origin);
void preStep ( btCollisionWorld* collisionWorld);
void playerStep ( btCollisionWorld* collisionWorld, btScalar dt);
void setStepHeight(btScalar h);
btScalar getStepHeight() const { return m_stepHeight; }
void setFallSpeed (btScalar fallSpeed);
btScalar getFallSpeed() const { return m_fallSpeed; }
void setJumpSpeed (btScalar jumpSpeed);
btScalar getJumpSpeed() const { return m_jumpSpeed; }
void setMaxJumpHeight (btScalar maxJumpHeight);
bool canJump () const;
void jump ();
void jump(const btVector3& v = btVector3());
void setGravity(btScalar gravity);
btScalar getGravity() const;
void applyImpulse(const btVector3& v) { jump(v); }
void setGravity(const btVector3& gravity);
btVector3 getGravity() const;
/// The max slope determines the maximum angle that the controller can walk up.
/// The slope angle is measured in radians.
void setMaxSlope(btScalar slopeRadians);
btScalar getMaxSlope() const;
void setMaxPenetrationDepth(btScalar d);
btScalar getMaxPenetrationDepth() const;
btPairCachingGhostObject* getGhostObject();
void setUseGhostSweepTest(bool useGhostObjectSweepTest)
{

16
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
View file

@ -214,7 +214,7 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt
}
// m_swingCorrection is always positive or 0
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = SIMD_INFINITY;
info->m_upperLimit[srow] = (m_bMotorEnabled && m_maxMotorImpulse >= 0.0f) ? m_maxMotorImpulse : SIMD_INFINITY;
srow += info->rowskip;
}
}
@ -540,8 +540,8 @@ void btConeTwistConstraint::calcAngleInfo()
m_solveTwistLimit = false;
m_solveSwingLimit = false;
btVector3 b1Axis1,b1Axis2,b1Axis3;
btVector3 b2Axis1,b2Axis2;
btVector3 b1Axis1(0,0,0),b1Axis2(0,0,0),b1Axis3(0,0,0);
btVector3 b2Axis1(0,0,0),b2Axis2(0,0,0);
b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
@ -778,8 +778,10 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2];
target.normalize();
m_swingAxis = -ivB.cross(target);
m_swingCorrection = m_swingAxis.length();
m_swingAxis.normalize();
m_swingCorrection = m_swingAxis.length();
if (!btFuzzyZero(m_swingCorrection))
m_swingAxis.normalize();
}
}
@ -983,8 +985,8 @@ void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingA
void btConeTwistConstraint::setMotorTarget(const btQuaternion &q)
{
btTransform trACur = m_rbA.getCenterOfMassTransform();
btTransform trBCur = m_rbB.getCenterOfMassTransform();
//btTransform trACur = m_rbA.getCenterOfMassTransform();
//btTransform trBCur = m_rbB.getCenterOfMassTransform();
// btTransform trABCur = trBCur.inverse() * trACur;
// btQuaternion qABCur = trABCur.getRotation();
// btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);

66
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
View file

@ -170,6 +170,11 @@ public:
{
m_angularOnly = angularOnly;
}
bool getAngularOnly() const
{
return m_angularOnly;
}
void setLimit(int limitIndex,btScalar limitValue)
{
@ -196,6 +201,33 @@ public:
};
}
btScalar getLimit(int limitIndex) const
{
switch (limitIndex)
{
case 3:
{
return m_twistSpan;
break;
}
case 4:
{
return m_swingSpan2;
break;
}
case 5:
{
return m_swingSpan1;
break;
}
default:
{
btAssert(0 && "Invalid limitIndex specified for btConeTwistConstraint");
return 0.0;
}
};
}
// setLimit(), a few notes:
// _softness:
// 0->1, recommend ~0.8->1.
@ -218,8 +250,8 @@ public:
m_relaxationFactor = _relaxationFactor;
}
const btTransform& getAFrame() { return m_rbAFrame; };
const btTransform& getBFrame() { return m_rbBFrame; };
const btTransform& getAFrame() const { return m_rbAFrame; };
const btTransform& getBFrame() const { return m_rbBFrame; };
inline int getSolveTwistLimit()
{
@ -239,27 +271,43 @@ public:
void calcAngleInfo();
void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
inline btScalar getSwingSpan1()
inline btScalar getSwingSpan1() const
{
return m_swingSpan1;
}
inline btScalar getSwingSpan2()
inline btScalar getSwingSpan2() const
{
return m_swingSpan2;
}
inline btScalar getTwistSpan()
inline btScalar getTwistSpan() const
{
return m_twistSpan;
}
inline btScalar getTwistAngle()
inline btScalar getLimitSoftness() const
{
return m_limitSoftness;
}
inline btScalar getBiasFactor() const
{
return m_biasFactor;
}
inline btScalar getRelaxationFactor() const
{
return m_relaxationFactor;
}
inline btScalar getTwistAngle() const
{
return m_twistAngle;
}
bool isPastSwingLimit() { return m_solveSwingLimit; }
btScalar getDamping() const { return m_damping; }
void setDamping(btScalar damping) { m_damping = damping; }
void enableMotor(bool b) { m_bMotorEnabled = b; }
bool isMotorEnabled() const { return m_bMotorEnabled; }
btScalar getMaxMotorImpulse() const { return m_maxMotorImpulse; }
bool isMaxMotorImpulseNormalized() const { return m_bNormalizedMotorStrength; }
void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; }
void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; }
@ -271,6 +319,7 @@ public:
// note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability)
// note: don't forget to enableMotor()
void setMotorTarget(const btQuaternion &q);
const btQuaternion& getMotorTarget() const { return m_qTarget; }
// same as above, but q is the desired rotation of frameA wrt frameB in constraint space
void setMotorTargetInConstraintSpace(const btQuaternion &q);
@ -297,6 +346,11 @@ public:
///return the local value of parameter
virtual btScalar getParam(int num, int axis = -1) const;
int getFlags() const
{
return m_flags;
}
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)

3
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
View file

@ -33,7 +33,8 @@ class btDispatcher;
enum btConstraintSolverType
{
BT_SEQUENTIAL_IMPULSE_SOLVER=1,
BT_MLCP_SOLVER=2
BT_MLCP_SOLVER=2,
BT_NNCG_SOLVER=4
};
class btConstraintSolver

3
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
View file

@ -155,8 +155,7 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
body2.getLinearVelocity(),
body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity());
btScalar a;
a=jacDiagABInv;
rel_vel = normal.dot(vel);

9
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
View file

@ -58,7 +58,7 @@ struct btContactSolverInfoData
int m_minimumSolverBatchSize;
btScalar m_maxGyroscopicForce;
btScalar m_singleAxisRollingFrictionThreshold;
btScalar m_leastSquaresResidualThreshold;
};
@ -77,7 +77,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
m_maxErrorReduction = btScalar(20.);
m_numIterations = 10;
m_erp = btScalar(0.2);
m_erp2 = btScalar(0.8);
m_erp2 = btScalar(0.2);
m_globalCfm = btScalar(0.);
m_sor = btScalar(1.);
m_splitImpulse = true;
@ -89,8 +89,9 @@ struct btContactSolverInfo : public btContactSolverInfoData
m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER;
m_restingContactRestitutionThreshold = 2;//unused as of 2.81
m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
m_maxGyroscopicForce = 100.f; ///only used to clamp forces for bodies that have their BT_ENABLE_GYROPSCOPIC_FORCE flag set (using btRigidBody::setFlag)
m_maxGyroscopicForce = 100.f; ///it is only used for 'explicit' version of gyroscopic force
m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
m_leastSquaresResidualThreshold = 0.f;
}
};
@ -111,7 +112,7 @@ struct btContactSolverInfoDoubleData
double m_splitImpulseTurnErp;
double m_linearSlop;
double m_warmstartingFactor;
double m_maxGyroscopicForce;
double m_maxGyroscopicForce;///it is only used for 'explicit' version of gyroscopic force
double m_singleAxisRollingFrictionThreshold;
int m_numIterations;

108
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp
View file

@ -21,109 +21,17 @@ subject to the following restrictions:
btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB)
:btTypedConstraint(FIXED_CONSTRAINT_TYPE,rbA,rbB)
:btGeneric6DofSpring2Constraint(rbA,rbB,frameInA,frameInB)
{
m_pivotInA = frameInA.getOrigin();
m_pivotInB = frameInB.getOrigin();
m_relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse();
setAngularLowerLimit(btVector3(0,0,0));
setAngularUpperLimit(btVector3(0,0,0));
setLinearLowerLimit(btVector3(0,0,0));
setLinearUpperLimit(btVector3(0,0,0));
}
btFixedConstraint::~btFixedConstraint ()
{
}
void btFixedConstraint::getInfo1 (btConstraintInfo1* info)
{
info->m_numConstraintRows = 6;
info->nub = 6;
}
void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
{
//fix the 3 linear degrees of freedom
const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin();
const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis();
const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin();
const btMatrix3x3& worldOrnB = m_rbB.getCenterOfMassTransform().getBasis();
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[info->rowskip+1] = 1;
info->m_J1linearAxis[2*info->rowskip+2] = 1;
btVector3 a1 = worldOrnA*m_pivotInA;
{
btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
btVector3 a1neg = -a1;
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
if (info->m_J2linearAxis)
{
info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[info->rowskip+1] = -1;
info->m_J2linearAxis[2*info->rowskip+2] = -1;
}
btVector3 a2 = worldOrnB*m_pivotInB;
{
// btVector3 a2n = -a2;
btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
// set right hand side for the linear dofs
btScalar k = info->fps * info->erp;
btVector3 linearError = k*(a2+worldPosB-a1-worldPosA);
int j;
for (j=0; j<3; j++)
{
info->m_constraintError[j*info->rowskip] = linearError[j];
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
}
//fix the 3 angular degrees of freedom
int start_row = 3;
int s = info->rowskip;
int start_index = start_row * s;
// 3 rows to make body rotations equal
info->m_J1angularAxis[start_index] = 1;
info->m_J1angularAxis[start_index + s + 1] = 1;
info->m_J1angularAxis[start_index + s*2+2] = 1;
if ( info->m_J2angularAxis)
{
info->m_J2angularAxis[start_index] = -1;
info->m_J2angularAxis[start_index + s+1] = -1;
info->m_J2angularAxis[start_index + s*2+2] = -1;
}
// set right hand side for the angular dofs
btVector3 diff;
btScalar angle;
btMatrix3x3 mrelCur = worldOrnA *worldOrnB.inverse();
btQuaternion qrelCur;
mrelCur.getRotation(qrelCur);
btTransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB,qrelCur,diff,angle);
diff*=-angle;
for (j=0; j<3; j++)
{
info->m_constraintError[(3+j)*info->rowskip] = k * diff[j];
}
}

24
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h
View file

@ -16,34 +16,18 @@ subject to the following restrictions:
#ifndef BT_FIXED_CONSTRAINT_H
#define BT_FIXED_CONSTRAINT_H
#include "btTypedConstraint.h"
#include "btGeneric6DofSpring2Constraint.h"
ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btTypedConstraint
ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btGeneric6DofSpring2Constraint
{
btVector3 m_pivotInA;
btVector3 m_pivotInB;
btQuaternion m_relTargetAB;
public:
btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB);
virtual ~btFixedConstraint();
virtual void getInfo1 (btConstraintInfo1* info);
virtual void getInfo2 (btConstraintInfo2* info);
virtual void setParam(int num, btScalar value, int axis = -1)
{
btAssert(0);
}
virtual btScalar getParam(int num, int axis = -1) const
{
btAssert(0);
return 0.f;
}
};
#endif //BT_FIXED_CONSTRAINT_H

27
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
View file

@ -111,14 +111,14 @@ public:
//! Is limited
bool isLimited()
bool isLimited() const
{
if(m_loLimit > m_hiLimit) return false;
return true;
}
//! Need apply correction
bool needApplyTorques()
bool needApplyTorques() const
{
if(m_currentLimit == 0 && m_enableMotor == false) return false;
return true;
@ -207,11 +207,11 @@ public:
- limited means upper > lower
- limitIndex: first 3 are linear, next 3 are angular
*/
inline bool isLimited(int limitIndex)
inline bool isLimited(int limitIndex) const
{
return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
}
inline bool needApplyForce(int limitIndex)
inline bool needApplyForce(int limitIndex) const
{
if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
return true;
@ -457,7 +457,7 @@ public:
m_linearLimits.m_lowerLimit = linearLower;
}
void getLinearLowerLimit(btVector3& linearLower)
void getLinearLowerLimit(btVector3& linearLower) const
{
linearLower = m_linearLimits.m_lowerLimit;
}
@ -467,7 +467,7 @@ public:
m_linearLimits.m_upperLimit = linearUpper;
}
void getLinearUpperLimit(btVector3& linearUpper)
void getLinearUpperLimit(btVector3& linearUpper) const
{
linearUpper = m_linearLimits.m_upperLimit;
}
@ -478,7 +478,7 @@ public:
m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
}
void getAngularLowerLimit(btVector3& angularLower)
void getAngularLowerLimit(btVector3& angularLower) const
{
for(int i = 0; i < 3; i++)
angularLower[i] = m_angularLimits[i].m_loLimit;
@ -490,7 +490,7 @@ public:
m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
}
void getAngularUpperLimit(btVector3& angularUpper)
void getAngularUpperLimit(btVector3& angularUpper) const
{
for(int i = 0; i < 3; i++)
angularUpper[i] = m_angularLimits[i].m_hiLimit;
@ -532,7 +532,7 @@ public:
- limited means upper > lower
- limitIndex: first 3 are linear, next 3 are angular
*/
bool isLimited(int limitIndex)
bool isLimited(int limitIndex) const
{
if(limitIndex<3)
{
@ -549,8 +549,11 @@ public:
btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
// access for UseFrameOffset
bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
bool getUseFrameOffset() const { return m_useOffsetForConstraintFrame; }
void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
bool getUseLinearReferenceFrameA() const { return m_useLinearReferenceFrameA; }
void setUseLinearReferenceFrameA(bool linearReferenceFrameA) { m_useLinearReferenceFrameA = linearReferenceFrameA; }
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
@ -560,6 +563,10 @@ public:
void setAxis( const btVector3& axis1, const btVector3& axis2);
virtual int getFlags() const
{
return m_flags;
}
virtual int calculateSerializeBufferSize() const;

1121
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp Normal file
View file

File diff suppressed because it is too large Load diff

674
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h Normal file
View file

@ -0,0 +1,674 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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.
*/
/*
2014 May: btGeneric6DofSpring2Constraint is created from the original (2.82.2712) btGeneric6DofConstraint by Gabor Puhr and Tamas Umenhoffer
Pros:
- Much more accurate and stable in a lot of situation. (Especially when a sleeping chain of RBs connected with 6dof2 is pulled)
- Stable and accurate spring with minimal energy loss that works with all of the solvers. (latter is not true for the original 6dof spring)
- Servo motor functionality
- Much more accurate bouncing. 0 really means zero bouncing (not true for the original 6odf) and there is only a minimal energy loss when the value is 1 (because of the solvers' precision)
- Rotation order for the Euler system can be set. (One axis' freedom is still limited to pi/2)
Cons:
- It is slower than the original 6dof. There is no exact ratio, but half speed is a good estimation.
- At bouncing the correct velocity is calculated, but not the correct position. (it is because of the solver can correct position or velocity, but not both.)
*/
/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev
/// Added support for generic constraint solver through getInfo1/getInfo2 methods
/*
2007-09-09
btGeneric6DofConstraint Refactored by Francisco Le?n
email: projectileman@yahoo.com
http://gimpact.sf.net
*/
#ifndef BT_GENERIC_6DOF_CONSTRAINT2_H
#define BT_GENERIC_6DOF_CONSTRAINT2_H
#include "LinearMath/btVector3.h"
#include "btJacobianEntry.h"
#include "btTypedConstraint.h"
class btRigidBody;
#ifdef BT_USE_DOUBLE_PRECISION
#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintDoubleData2
#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintDoubleData2"
#else
#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintData
#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintData"
#endif //BT_USE_DOUBLE_PRECISION
enum RotateOrder
{
RO_XYZ=0,
RO_XZY,
RO_YXZ,
RO_YZX,
RO_ZXY,
RO_ZYX
};
class btRotationalLimitMotor2
{
public:
// upper < lower means free
// upper == lower means locked
// upper > lower means limited
btScalar m_loLimit;
btScalar m_hiLimit;
btScalar m_bounce;
btScalar m_stopERP;
btScalar m_stopCFM;
btScalar m_motorERP;
btScalar m_motorCFM;
bool m_enableMotor;
btScalar m_targetVelocity;
btScalar m_maxMotorForce;
bool m_servoMotor;
btScalar m_servoTarget;
bool m_enableSpring;
btScalar m_springStiffness;
bool m_springStiffnessLimited;
btScalar m_springDamping;
bool m_springDampingLimited;
btScalar m_equilibriumPoint;
btScalar m_currentLimitError;
btScalar m_currentLimitErrorHi;
btScalar m_currentPosition;
int m_currentLimit;
btRotationalLimitMotor2()
{
m_loLimit = 1.0f;
m_hiLimit = -1.0f;
m_bounce = 0.0f;
m_stopERP = 0.2f;
m_stopCFM = 0.f;
m_motorERP = 0.9f;
m_motorCFM = 0.f;
m_enableMotor = false;
m_targetVelocity = 0;
m_maxMotorForce = 0.1f;
m_servoMotor = false;
m_servoTarget = 0;
m_enableSpring = false;
m_springStiffness = 0;
m_springStiffnessLimited = false;
m_springDamping = 0;
m_springDampingLimited = false;
m_equilibriumPoint = 0;
m_currentLimitError = 0;
m_currentLimitErrorHi = 0;
m_currentPosition = 0;
m_currentLimit = 0;
}
btRotationalLimitMotor2(const btRotationalLimitMotor2 & limot)
{
m_loLimit = limot.m_loLimit;
m_hiLimit = limot.m_hiLimit;
m_bounce = limot.m_bounce;
m_stopERP = limot.m_stopERP;
m_stopCFM = limot.m_stopCFM;
m_motorERP = limot.m_motorERP;
m_motorCFM = limot.m_motorCFM;
m_enableMotor = limot.m_enableMotor;
m_targetVelocity = limot.m_targetVelocity;
m_maxMotorForce = limot.m_maxMotorForce;
m_servoMotor = limot.m_servoMotor;
m_servoTarget = limot.m_servoTarget;
m_enableSpring = limot.m_enableSpring;
m_springStiffness = limot.m_springStiffness;
m_springStiffnessLimited = limot.m_springStiffnessLimited;
m_springDamping = limot.m_springDamping;
m_springDampingLimited = limot.m_springDampingLimited;
m_equilibriumPoint = limot.m_equilibriumPoint;
m_currentLimitError = limot.m_currentLimitError;
m_currentLimitErrorHi = limot.m_currentLimitErrorHi;
m_currentPosition = limot.m_currentPosition;
m_currentLimit = limot.m_currentLimit;
}
bool isLimited()
{
if(m_loLimit > m_hiLimit) return false;
return true;
}
void testLimitValue(btScalar test_value);
};
class btTranslationalLimitMotor2
{
public:
// upper < lower means free
// upper == lower means locked
// upper > lower means limited
btVector3 m_lowerLimit;
btVector3 m_upperLimit;
btVector3 m_bounce;
btVector3 m_stopERP;
btVector3 m_stopCFM;
btVector3 m_motorERP;
btVector3 m_motorCFM;
bool m_enableMotor[3];
bool m_servoMotor[3];
bool m_enableSpring[3];
btVector3 m_servoTarget;
btVector3 m_springStiffness;
bool m_springStiffnessLimited[3];
btVector3 m_springDamping;
bool m_springDampingLimited[3];
btVector3 m_equilibriumPoint;
btVector3 m_targetVelocity;
btVector3 m_maxMotorForce;
btVector3 m_currentLimitError;
btVector3 m_currentLimitErrorHi;
btVector3 m_currentLinearDiff;
int m_currentLimit[3];
btTranslationalLimitMotor2()
{
m_lowerLimit .setValue(0.f , 0.f , 0.f );
m_upperLimit .setValue(0.f , 0.f , 0.f );
m_bounce .setValue(0.f , 0.f , 0.f );
m_stopERP .setValue(0.2f, 0.2f, 0.2f);
m_stopCFM .setValue(0.f , 0.f , 0.f );
m_motorERP .setValue(0.9f, 0.9f, 0.9f);
m_motorCFM .setValue(0.f , 0.f , 0.f );
m_currentLimitError .setValue(0.f , 0.f , 0.f );
m_currentLimitErrorHi.setValue(0.f , 0.f , 0.f );
m_currentLinearDiff .setValue(0.f , 0.f , 0.f );
for(int i=0; i < 3; i++)
{
m_enableMotor[i] = false;
m_servoMotor[i] = false;
m_enableSpring[i] = false;
m_servoTarget[i] = btScalar(0.f);
m_springStiffness[i] = btScalar(0.f);
m_springStiffnessLimited[i] = false;
m_springDamping[i] = btScalar(0.f);
m_springDampingLimited[i] = false;
m_equilibriumPoint[i] = btScalar(0.f);
m_targetVelocity[i] = btScalar(0.f);
m_maxMotorForce[i] = btScalar(0.f);
m_currentLimit[i] = 0;
}
}
btTranslationalLimitMotor2(const btTranslationalLimitMotor2 & other )
{
m_lowerLimit = other.m_lowerLimit;
m_upperLimit = other.m_upperLimit;
m_bounce = other.m_bounce;
m_stopERP = other.m_stopERP;
m_stopCFM = other.m_stopCFM;
m_motorERP = other.m_motorERP;
m_motorCFM = other.m_motorCFM;
m_currentLimitError = other.m_currentLimitError;
m_currentLimitErrorHi = other.m_currentLimitErrorHi;
m_currentLinearDiff = other.m_currentLinearDiff;
for(int i=0; i < 3; i++)
{
m_enableMotor[i] = other.m_enableMotor[i];
m_servoMotor[i] = other.m_servoMotor[i];
m_enableSpring[i] = other.m_enableSpring[i];
m_servoTarget[i] = other.m_servoTarget[i];
m_springStiffness[i] = other.m_springStiffness[i];
m_springStiffnessLimited[i] = other.m_springStiffnessLimited[i];
m_springDamping[i] = other.m_springDamping[i];
m_springDampingLimited[i] = other.m_springDampingLimited[i];
m_equilibriumPoint[i] = other.m_equilibriumPoint[i];
m_targetVelocity[i] = other.m_targetVelocity[i];
m_maxMotorForce[i] = other.m_maxMotorForce[i];
m_currentLimit[i] = other.m_currentLimit[i];
}
}
inline bool isLimited(int limitIndex)
{
return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
}
void testLimitValue(int limitIndex, btScalar test_value);
};
enum bt6DofFlags2
{
BT_6DOF_FLAGS_CFM_STOP2 = 1,
BT_6DOF_FLAGS_ERP_STOP2 = 2,
BT_6DOF_FLAGS_CFM_MOTO2 = 4,
BT_6DOF_FLAGS_ERP_MOTO2 = 8
};
#define BT_6DOF_FLAGS_AXIS_SHIFT2 4 // bits per axis
ATTRIBUTE_ALIGNED16(class) btGeneric6DofSpring2Constraint : public btTypedConstraint
{
protected:
btTransform m_frameInA;
btTransform m_frameInB;
btJacobianEntry m_jacLinear[3];
btJacobianEntry m_jacAng[3];
btTranslationalLimitMotor2 m_linearLimits;
btRotationalLimitMotor2 m_angularLimits[3];
RotateOrder m_rotateOrder;
protected:
btTransform m_calculatedTransformA;
btTransform m_calculatedTransformB;
btVector3 m_calculatedAxisAngleDiff;
btVector3 m_calculatedAxis[3];
btVector3 m_calculatedLinearDiff;
btScalar m_factA;
btScalar m_factB;
bool m_hasStaticBody;
int m_flags;
btGeneric6DofSpring2Constraint& operator=(btGeneric6DofSpring2Constraint&)
{
btAssert(0);
return *this;
}
int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
void calculateLinearInfo();
void calculateAngleInfo();
void testAngularLimitMotor(int axis_index);
void calculateJacobi(btRotationalLimitMotor2* limot, const btTransform& transA,const btTransform& transB, btConstraintInfo2* info, int srow, btVector3& ax1, int rotational, int rotAllowed);
int get_limit_motor_info2(btRotationalLimitMotor2* limot,
const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ);
btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ);
virtual void buildJacobian() {}
virtual void getInfo1 (btConstraintInfo1* info);
virtual void getInfo2 (btConstraintInfo2* info);
virtual int calculateSerializeBufferSize() const;
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
btRotationalLimitMotor2* getRotationalLimitMotor(int index) { return &m_angularLimits[index]; }
btTranslationalLimitMotor2* getTranslationalLimitMotor() { return &m_linearLimits; }
// Calculates the global transform for the joint offset for body A an B, and also calculates the angle differences between the bodies.
void calculateTransforms(const btTransform& transA,const btTransform& transB);
void calculateTransforms();
// Gets the global transform of the offset for body A
const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; }
// Gets the global transform of the offset for body B
const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; }
const btTransform & getFrameOffsetA() const { return m_frameInA; }
const btTransform & getFrameOffsetB() const { return m_frameInB; }
btTransform & getFrameOffsetA() { return m_frameInA; }
btTransform & getFrameOffsetB() { return m_frameInB; }
// Get the rotation axis in global coordinates ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously )
btVector3 getAxis(int axis_index) const { return m_calculatedAxis[axis_index]; }
// Get the relative Euler angle ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously )
btScalar getAngle(int axis_index) const { return m_calculatedAxisAngleDiff[axis_index]; }
// Get the relative position of the constraint pivot ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously )
btScalar getRelativePivotPosition(int axis_index) const { return m_calculatedLinearDiff[axis_index]; }
void setFrames(const btTransform & frameA, const btTransform & frameB);
void setLinearLowerLimit(const btVector3& linearLower) { m_linearLimits.m_lowerLimit = linearLower; }
void getLinearLowerLimit(btVector3& linearLower) { linearLower = m_linearLimits.m_lowerLimit; }
void setLinearUpperLimit(const btVector3& linearUpper) { m_linearLimits.m_upperLimit = linearUpper; }
void getLinearUpperLimit(btVector3& linearUpper) { linearUpper = m_linearLimits.m_upperLimit; }
void setAngularLowerLimit(const btVector3& angularLower)
{
for(int i = 0; i < 3; i++)
m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
}
void setAngularLowerLimitReversed(const btVector3& angularLower)
{
for(int i = 0; i < 3; i++)
m_angularLimits[i].m_hiLimit = btNormalizeAngle(-angularLower[i]);
}
void getAngularLowerLimit(btVector3& angularLower)
{
for(int i = 0; i < 3; i++)
angularLower[i] = m_angularLimits[i].m_loLimit;
}
void getAngularLowerLimitReversed(btVector3& angularLower)
{
for(int i = 0; i < 3; i++)
angularLower[i] = -m_angularLimits[i].m_hiLimit;
}
void setAngularUpperLimit(const btVector3& angularUpper)
{
for(int i = 0; i < 3; i++)
m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
}
void setAngularUpperLimitReversed(const btVector3& angularUpper)
{
for(int i = 0; i < 3; i++)
m_angularLimits[i].m_loLimit = btNormalizeAngle(-angularUpper[i]);
}
void getAngularUpperLimit(btVector3& angularUpper)
{
for(int i = 0; i < 3; i++)
angularUpper[i] = m_angularLimits[i].m_hiLimit;
}
void getAngularUpperLimitReversed(btVector3& angularUpper)
{
for(int i = 0; i < 3; i++)
angularUpper[i] = -m_angularLimits[i].m_loLimit;
}
//first 3 are linear, next 3 are angular
void setLimit(int axis, btScalar lo, btScalar hi)
{
if(axis<3)
{
m_linearLimits.m_lowerLimit[axis] = lo;
m_linearLimits.m_upperLimit[axis] = hi;
}
else
{
lo = btNormalizeAngle(lo);
hi = btNormalizeAngle(hi);
m_angularLimits[axis-3].m_loLimit = lo;
m_angularLimits[axis-3].m_hiLimit = hi;
}
}
void setLimitReversed(int axis, btScalar lo, btScalar hi)
{
if(axis<3)
{
m_linearLimits.m_lowerLimit[axis] = lo;
m_linearLimits.m_upperLimit[axis] = hi;
}
else
{
lo = btNormalizeAngle(lo);
hi = btNormalizeAngle(hi);
m_angularLimits[axis-3].m_hiLimit = -lo;
m_angularLimits[axis-3].m_loLimit = -hi;
}
}
bool isLimited(int limitIndex)
{
if(limitIndex<3)
{
return m_linearLimits.isLimited(limitIndex);
}
return m_angularLimits[limitIndex-3].isLimited();
}
void setRotationOrder(RotateOrder order) { m_rotateOrder = order; }
RotateOrder getRotationOrder() { return m_rotateOrder; }
void setAxis( const btVector3& axis1, const btVector3& axis2);
void setBounce(int index, btScalar bounce);
void enableMotor(int index, bool onOff);
void setServo(int index, bool onOff); // set the type of the motor (servo or not) (the motor has to be turned on for servo also)
void setTargetVelocity(int index, btScalar velocity);
void setServoTarget(int index, btScalar target);
void setMaxMotorForce(int index, btScalar force);
void enableSpring(int index, bool onOff);
void setStiffness(int index, btScalar stiffness, bool limitIfNeeded = true); // if limitIfNeeded is true the system will automatically limit the stiffness in necessary situations where otherwise the spring would move unrealistically too widely
void setDamping(int index, btScalar damping, bool limitIfNeeded = true); // if limitIfNeeded is true the system will automatically limit the damping in necessary situations where otherwise the spring would blow up
void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF
void setEquilibriumPoint(int index); // set the current constraint position/orientation as an equilibrium point for given DOF
void setEquilibriumPoint(int index, btScalar val);
//override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
//If no axis is provided, it uses the default axis for this constraint.
virtual void setParam(int num, btScalar value, int axis = -1);
virtual btScalar getParam(int num, int axis = -1) const;
static btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
static bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
static bool matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz);
static bool matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz);
static bool matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz);
static bool matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz);
static bool matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz);
};
struct btGeneric6DofSpring2ConstraintData
{
btTypedConstraintData m_typeConstraintData;
btTransformFloatData m_rbAFrame;
btTransformFloatData m_rbBFrame;
btVector3FloatData m_linearUpperLimit;
btVector3FloatData m_linearLowerLimit;
btVector3FloatData m_linearBounce;
btVector3FloatData m_linearStopERP;
btVector3FloatData m_linearStopCFM;
btVector3FloatData m_linearMotorERP;
btVector3FloatData m_linearMotorCFM;
btVector3FloatData m_linearTargetVelocity;
btVector3FloatData m_linearMaxMotorForce;
btVector3FloatData m_linearServoTarget;
btVector3FloatData m_linearSpringStiffness;
btVector3FloatData m_linearSpringDamping;
btVector3FloatData m_linearEquilibriumPoint;
char m_linearEnableMotor[4];
char m_linearServoMotor[4];
char m_linearEnableSpring[4];
char m_linearSpringStiffnessLimited[4];
char m_linearSpringDampingLimited[4];
char m_padding1[4];
btVector3FloatData m_angularUpperLimit;
btVector3FloatData m_angularLowerLimit;
btVector3FloatData m_angularBounce;
btVector3FloatData m_angularStopERP;
btVector3FloatData m_angularStopCFM;
btVector3FloatData m_angularMotorERP;
btVector3FloatData m_angularMotorCFM;
btVector3FloatData m_angularTargetVelocity;
btVector3FloatData m_angularMaxMotorForce;
btVector3FloatData m_angularServoTarget;
btVector3FloatData m_angularSpringStiffness;
btVector3FloatData m_angularSpringDamping;
btVector3FloatData m_angularEquilibriumPoint;
char m_angularEnableMotor[4];
char m_angularServoMotor[4];
char m_angularEnableSpring[4];
char m_angularSpringStiffnessLimited[4];
char m_angularSpringDampingLimited[4];
int m_rotateOrder;
};
struct btGeneric6DofSpring2ConstraintDoubleData2
{
btTypedConstraintDoubleData m_typeConstraintData;
btTransformDoubleData m_rbAFrame;
btTransformDoubleData m_rbBFrame;
btVector3DoubleData m_linearUpperLimit;
btVector3DoubleData m_linearLowerLimit;
btVector3DoubleData m_linearBounce;
btVector3DoubleData m_linearStopERP;
btVector3DoubleData m_linearStopCFM;
btVector3DoubleData m_linearMotorERP;
btVector3DoubleData m_linearMotorCFM;
btVector3DoubleData m_linearTargetVelocity;
btVector3DoubleData m_linearMaxMotorForce;
btVector3DoubleData m_linearServoTarget;
btVector3DoubleData m_linearSpringStiffness;
btVector3DoubleData m_linearSpringDamping;
btVector3DoubleData m_linearEquilibriumPoint;
char m_linearEnableMotor[4];
char m_linearServoMotor[4];
char m_linearEnableSpring[4];
char m_linearSpringStiffnessLimited[4];
char m_linearSpringDampingLimited[4];
char m_padding1[4];
btVector3DoubleData m_angularUpperLimit;
btVector3DoubleData m_angularLowerLimit;
btVector3DoubleData m_angularBounce;
btVector3DoubleData m_angularStopERP;
btVector3DoubleData m_angularStopCFM;
btVector3DoubleData m_angularMotorERP;
btVector3DoubleData m_angularMotorCFM;
btVector3DoubleData m_angularTargetVelocity;
btVector3DoubleData m_angularMaxMotorForce;
btVector3DoubleData m_angularServoTarget;
btVector3DoubleData m_angularSpringStiffness;
btVector3DoubleData m_angularSpringDamping;
btVector3DoubleData m_angularEquilibriumPoint;
char m_angularEnableMotor[4];
char m_angularServoMotor[4];
char m_angularEnableSpring[4];
char m_angularSpringStiffnessLimited[4];
char m_angularSpringDampingLimited[4];
int m_rotateOrder;
};
SIMD_FORCE_INLINE int btGeneric6DofSpring2Constraint::calculateSerializeBufferSize() const
{
return sizeof(btGeneric6DofSpring2ConstraintData2);
}
SIMD_FORCE_INLINE const char* btGeneric6DofSpring2Constraint::serialize(void* dataBuffer, btSerializer* serializer) const
{
btGeneric6DofSpring2ConstraintData2* dof = (btGeneric6DofSpring2ConstraintData2*)dataBuffer;
btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
m_frameInA.serialize(dof->m_rbAFrame);
m_frameInB.serialize(dof->m_rbBFrame);
int i;
for (i=0;i<3;i++)
{
dof->m_angularLowerLimit.m_floats[i] = m_angularLimits[i].m_loLimit;
dof->m_angularUpperLimit.m_floats[i] = m_angularLimits[i].m_hiLimit;
dof->m_angularBounce.m_floats[i] = m_angularLimits[i].m_bounce;
dof->m_angularStopERP.m_floats[i] = m_angularLimits[i].m_stopERP;
dof->m_angularStopCFM.m_floats[i] = m_angularLimits[i].m_stopCFM;
dof->m_angularMotorERP.m_floats[i] = m_angularLimits[i].m_motorERP;
dof->m_angularMotorCFM.m_floats[i] = m_angularLimits[i].m_motorCFM;
dof->m_angularTargetVelocity.m_floats[i] = m_angularLimits[i].m_targetVelocity;
dof->m_angularMaxMotorForce.m_floats[i] = m_angularLimits[i].m_maxMotorForce;
dof->m_angularServoTarget.m_floats[i] = m_angularLimits[i].m_servoTarget;
dof->m_angularSpringStiffness.m_floats[i] = m_angularLimits[i].m_springStiffness;
dof->m_angularSpringDamping.m_floats[i] = m_angularLimits[i].m_springDamping;
dof->m_angularEquilibriumPoint.m_floats[i] = m_angularLimits[i].m_equilibriumPoint;
}
dof->m_angularLowerLimit.m_floats[3] = 0;
dof->m_angularUpperLimit.m_floats[3] = 0;
dof->m_angularBounce.m_floats[3] = 0;
dof->m_angularStopERP.m_floats[3] = 0;
dof->m_angularStopCFM.m_floats[3] = 0;
dof->m_angularMotorERP.m_floats[3] = 0;
dof->m_angularMotorCFM.m_floats[3] = 0;
dof->m_angularTargetVelocity.m_floats[3] = 0;
dof->m_angularMaxMotorForce.m_floats[3] = 0;
dof->m_angularServoTarget.m_floats[3] = 0;
dof->m_angularSpringStiffness.m_floats[3] = 0;
dof->m_angularSpringDamping.m_floats[3] = 0;
dof->m_angularEquilibriumPoint.m_floats[3] = 0;
for (i=0;i<4;i++)
{
dof->m_angularEnableMotor[i] = i < 3 ? ( m_angularLimits[i].m_enableMotor ? 1 : 0 ) : 0;
dof->m_angularServoMotor[i] = i < 3 ? ( m_angularLimits[i].m_servoMotor ? 1 : 0 ) : 0;
dof->m_angularEnableSpring[i] = i < 3 ? ( m_angularLimits[i].m_enableSpring ? 1 : 0 ) : 0;
dof->m_angularSpringStiffnessLimited[i] = i < 3 ? ( m_angularLimits[i].m_springStiffnessLimited ? 1 : 0 ) : 0;
dof->m_angularSpringDampingLimited[i] = i < 3 ? ( m_angularLimits[i].m_springDampingLimited ? 1 : 0 ) : 0;
}
m_linearLimits.m_lowerLimit.serialize( dof->m_linearLowerLimit );
m_linearLimits.m_upperLimit.serialize( dof->m_linearUpperLimit );
m_linearLimits.m_bounce.serialize( dof->m_linearBounce );
m_linearLimits.m_stopERP.serialize( dof->m_linearStopERP );
m_linearLimits.m_stopCFM.serialize( dof->m_linearStopCFM );
m_linearLimits.m_motorERP.serialize( dof->m_linearMotorERP );
m_linearLimits.m_motorCFM.serialize( dof->m_linearMotorCFM );
m_linearLimits.m_targetVelocity.serialize( dof->m_linearTargetVelocity );
m_linearLimits.m_maxMotorForce.serialize( dof->m_linearMaxMotorForce );
m_linearLimits.m_servoTarget.serialize( dof->m_linearServoTarget );
m_linearLimits.m_springStiffness.serialize( dof->m_linearSpringStiffness );
m_linearLimits.m_springDamping.serialize( dof->m_linearSpringDamping );
m_linearLimits.m_equilibriumPoint.serialize( dof->m_linearEquilibriumPoint );
for (i=0;i<4;i++)
{
dof->m_linearEnableMotor[i] = i < 3 ? ( m_linearLimits.m_enableMotor[i] ? 1 : 0 ) : 0;
dof->m_linearServoMotor[i] = i < 3 ? ( m_linearLimits.m_servoMotor[i] ? 1 : 0 ) : 0;
dof->m_linearEnableSpring[i] = i < 3 ? ( m_linearLimits.m_enableSpring[i] ? 1 : 0 ) : 0;
dof->m_linearSpringStiffnessLimited[i] = i < 3 ? ( m_linearLimits.m_springStiffnessLimited[i] ? 1 : 0 ) : 0;
dof->m_linearSpringDampingLimited[i] = i < 3 ? ( m_linearLimits.m_springDampingLimited[i] ? 1 : 0 ) : 0;
}
dof->m_rotateOrder = m_rotateOrder;
return btGeneric6DofSpring2ConstraintDataName;
}
#endif //BT_GENERIC_6DOF_CONSTRAINT_H

20
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h
View file

@ -63,6 +63,26 @@ public:
void setEquilibriumPoint(int index); // set the current constraint position/orientation as an equilibrium point for given DOF
void setEquilibriumPoint(int index, btScalar val);
bool isSpringEnabled(int index) const
{
return m_springEnabled[index];
}
btScalar getStiffness(int index) const
{
return m_springStiffness[index];
}
btScalar getDamping(int index) const
{
return m_springDamping[index];
}
btScalar getEquilibriumPoint(int index) const
{
return m_equilibriumPoint[index];
}
virtual void setAxis( const btVector3& axis1, const btVector3& axis2);
virtual void getInfo2 (btConstraintInfo2* info);

4
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp
View file

@ -25,7 +25,7 @@ subject to the following restrictions:
// anchor, axis1 and axis2 are in world coordinate system
// axis1 must be orthogonal to axis2
btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2)
: btGeneric6DofSpringConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true),
: btGeneric6DofSpring2Constraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(),RO_XYZ),
m_anchor(anchor),
m_axis1(axis1),
m_axis2(axis2)
@ -59,7 +59,7 @@ btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVec
setAngularUpperLimit(btVector3(-1.f, 0.f, SIMD_HALF_PI * 0.5f));
// enable suspension
enableSpring(2, true);
setStiffness(2, SIMD_PI * SIMD_PI * 4.f); // period 1 sec for 1 kilogramm weel :-)
setStiffness(2, SIMD_PI * SIMD_PI * 4.f);
setDamping(2, 0.01f);
setEquilibriumPoint();
}

4
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h
View file

@ -20,7 +20,7 @@ subject to the following restrictions:
#include "LinearMath/btVector3.h"
#include "btTypedConstraint.h"
#include "btGeneric6DofSpringConstraint.h"
#include "btGeneric6DofSpring2Constraint.h"
@ -29,7 +29,7 @@ subject to the following restrictions:
// 2 rotational degrees of freedom, similar to Euler rotations around Z (axis 1) and X (axis 2)
// 1 translational (along axis Z) with suspension spring
ATTRIBUTE_ALIGNED16(class) btHinge2Constraint : public btGeneric6DofSpringConstraint
ATTRIBUTE_ALIGNED16(class) btHinge2Constraint : public btGeneric6DofSpring2Constraint
{
protected:
btVector3 m_anchor;

100
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
View file

@ -45,7 +45,11 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA),
m_flags(0)
m_flags(0),
m_normalCFM(0),
m_normalERP(0),
m_stopCFM(0),
m_stopERP(0)
{
m_rbAFrame.getOrigin() = pivotInA;
@ -101,7 +105,11 @@ m_angularOnly(false), m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA),
m_flags(0)
m_flags(0),
m_normalCFM(0),
m_normalERP(0),
m_stopCFM(0),
m_stopERP(0)
{
// since no frame is given, assume this to be zero angle and just pick rb transform axis
@ -151,7 +159,11 @@ m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA),
m_flags(0)
m_flags(0),
m_normalCFM(0),
m_normalERP(0),
m_stopCFM(0),
m_stopERP(0)
{
#ifndef _BT_USE_CENTER_LIMIT_
//start with free
@ -177,7 +189,11 @@ m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA),
m_flags(0)
m_flags(0),
m_normalCFM(0),
m_normalERP(0),
m_stopCFM(0),
m_stopERP(0)
{
///not providing rigidbody B means implicitly using worldspace for body B
@ -285,8 +301,60 @@ void btHingeConstraint::buildJacobian()
#endif //__SPU__
static inline btScalar btNormalizeAnglePositive(btScalar angle)
{
return btFmod(btFmod(angle, btScalar(2.0*SIMD_PI)) + btScalar(2.0*SIMD_PI), btScalar(2.0*SIMD_PI));
}
static btScalar btShortestAngularDistance(btScalar accAngle, btScalar curAngle)
{
btScalar result = btNormalizeAngle(btNormalizeAnglePositive(btNormalizeAnglePositive(curAngle) -
btNormalizeAnglePositive(accAngle)));
return result;
}
static btScalar btShortestAngleUpdate(btScalar accAngle, btScalar curAngle)
{
btScalar tol(0.3);
btScalar result = btShortestAngularDistance(accAngle, curAngle);
if (btFabs(result) > tol)
return curAngle;
else
return accAngle + result;
return curAngle;
}
btScalar btHingeAccumulatedAngleConstraint::getAccumulatedHingeAngle()
{
btScalar hingeAngle = getHingeAngle();
m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,hingeAngle);
return m_accumulatedAngle;
}
void btHingeAccumulatedAngleConstraint::setAccumulatedHingeAngle(btScalar accAngle)
{
m_accumulatedAngle = accAngle;
}
void btHingeAccumulatedAngleConstraint::getInfo1(btConstraintInfo1* info)
{
//update m_accumulatedAngle
btScalar curHingeAngle = getHingeAngle();
m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,curHingeAngle);
btHingeConstraint::getInfo1(info);
}
void btHingeConstraint::getInfo1(btConstraintInfo1* info)
{
if (m_useSolveConstraintObsolete)
{
info->m_numConstraintRows = 0;
@ -413,7 +481,9 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
// linear RHS
btScalar k = info->fps * info->erp;
btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp;
btScalar k = info->fps * normalErp;
if (!m_angularOnly)
{
for(i = 0; i < 3; i++)
@ -510,7 +580,7 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
powered = 0;
}
info->m_constraintError[srow] = btScalar(0.0f);
btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
if(powered)
{
if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
@ -606,6 +676,8 @@ void btHingeConstraint::updateRHS(btScalar timeStep)
}
btScalar btHingeConstraint::getHingeAngle()
{
return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
@ -798,7 +870,8 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
btScalar k = info->fps * info->erp;
btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM)? m_normalERP : info->erp;
btScalar k = info->fps * normalErp;
if (!m_angularOnly)
{
@ -856,7 +929,8 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
k = info->fps * info->erp;
k = info->fps * normalErp;//??
btVector3 u = ax1A.cross(ax1B);
info->m_constraintError[s3] = k * u.dot(p);
info->m_constraintError[s4] = k * u.dot(q);
@ -901,7 +975,7 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
powered = 0;
}
info->m_constraintError[srow] = btScalar(0.0f);
btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
if(powered)
{
if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
@ -1002,6 +1076,10 @@ void btHingeConstraint::setParam(int num, btScalar value, int axis)
m_normalCFM = value;
m_flags |= BT_HINGE_FLAGS_CFM_NORM;
break;
case BT_CONSTRAINT_ERP:
m_normalERP = value;
m_flags |= BT_HINGE_FLAGS_ERP_NORM;
break;
default :
btAssertConstrParams(0);
}
@ -1032,6 +1110,10 @@ btScalar btHingeConstraint::getParam(int num, int axis) const
btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM);
retVal = m_normalCFM;
break;
case BT_CONSTRAINT_ERP:
btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_NORM);
retVal = m_normalERP;
break;
default :
btAssertConstrParams(0);
}

87
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
View file

@ -41,7 +41,8 @@ enum btHingeFlags
{
BT_HINGE_FLAGS_CFM_STOP = 1,
BT_HINGE_FLAGS_ERP_STOP = 2,
BT_HINGE_FLAGS_CFM_NORM = 4
BT_HINGE_FLAGS_CFM_NORM = 4,
BT_HINGE_FLAGS_ERP_NORM = 8
};
@ -94,6 +95,7 @@ public:
int m_flags;
btScalar m_normalCFM;
btScalar m_normalERP;
btScalar m_stopCFM;
btScalar m_stopERP;
@ -175,6 +177,7 @@ public:
// maintain a given angular target.
void enableMotor(bool enableMotor) { m_enableAngularMotor = enableMotor; }
void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; }
void setMotorTargetVelocity(btScalar motorTargetVelocity) { m_motorTargetVelocity = motorTargetVelocity; }
void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B.
void setMotorTarget(btScalar targetAngle, btScalar dt);
@ -191,6 +194,33 @@ public:
m_relaxationFactor = _relaxationFactor;
#endif
}
btScalar getLimitSoftness() const
{
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.getSoftness();
#else
return m_limitSoftness;
#endif
}
btScalar getLimitBiasFactor() const
{
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.getBiasFactor();
#else
return m_biasFactor;
#endif
}
btScalar getLimitRelaxationFactor() const
{
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.getRelaxationFactor();
#else
return m_relaxationFactor;
#endif
}
void setAxis(btVector3& axisInA)
{
@ -217,6 +247,14 @@ public:
}
bool hasLimit() const {
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.getHalfRange() > 0;
#else
return m_lowerLimit <= m_upperLimit;
#endif
}
btScalar getLowerLimit() const
{
#ifdef _BT_USE_CENTER_LIMIT_
@ -236,6 +274,7 @@ public:
}
///The getHingeAngle gives the hinge angle in range [-PI,PI]
btScalar getHingeAngle();
btScalar getHingeAngle(const btTransform& transA,const btTransform& transB);
@ -286,13 +325,20 @@ public:
// access for UseFrameOffset
bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
// access for UseReferenceFrameA
bool getUseReferenceFrameA() const { return m_useReferenceFrameA; }
void setUseReferenceFrameA(bool useReferenceFrameA) { m_useReferenceFrameA = useReferenceFrameA; }
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
virtual void setParam(int num, btScalar value, int axis = -1);
///return the local value of parameter
virtual btScalar getParam(int num, int axis = -1) const;
virtual int getFlags() const
{
return m_flags;
}
virtual int calculateSerializeBufferSize() const;
@ -326,6 +372,43 @@ struct btHingeConstraintDoubleData
};
#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
///The getAccumulatedHingeAngle returns the accumulated hinge angle, taking rotation across the -PI/PI boundary into account
ATTRIBUTE_ALIGNED16(class) btHingeAccumulatedAngleConstraint : public btHingeConstraint
{
protected:
btScalar m_accumulatedAngle;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false)
:btHingeConstraint(rbA,rbB,pivotInA,pivotInB, axisInA,axisInB, useReferenceFrameA )
{
m_accumulatedAngle=getHingeAngle();
}
btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false)
:btHingeConstraint(rbA,pivotInA,axisInA, useReferenceFrameA)
{
m_accumulatedAngle=getHingeAngle();
}
btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false)
:btHingeConstraint(rbA,rbB, rbAFrame, rbBFrame, useReferenceFrameA )
{
m_accumulatedAngle=getHingeAngle();
}
btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false)
:btHingeConstraint(rbA,rbAFrame, useReferenceFrameA )
{
m_accumulatedAngle=getHingeAngle();
}
btScalar getAccumulatedHingeAngle();
void setAccumulatedHingeAngle(btScalar accAngle);
virtual void getInfo1 (btConstraintInfo1* info);
};
struct btHingeConstraintFloatData
{

463
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp Normal file
View file

@ -0,0 +1,463 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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.
*/
#include "btNNCGConstraintSolver.h"
btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
{
btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer);
m_pNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
m_pC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
m_pCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size());
m_pCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size());
m_deltafNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
m_deltafC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
m_deltafCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size());
m_deltafCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size());
return val;
}
btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
{
int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
int numConstraintPool = m_tmpSolverContactConstraintPool.size();
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
{
if (1) // uncomment this for a bit less random ((iteration & 7) == 0)
{
for (int j=0; j<numNonContactPool; ++j) {
int tmp = m_orderNonContactConstraintPool[j];
int swapi = btRandInt2(j+1);
m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
m_orderNonContactConstraintPool[swapi] = tmp;
}
//contact/friction constraints are not solved more than
if (iteration< infoGlobal.m_numIterations)
{
for (int j=0; j<numConstraintPool; ++j) {
int tmp = m_orderTmpConstraintPool[j];
int swapi = btRandInt2(j+1);
m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
m_orderTmpConstraintPool[swapi] = tmp;
}
for (int j=0; j<numFrictionPool; ++j) {
int tmp = m_orderFrictionConstraintPool[j];
int swapi = btRandInt2(j+1);
m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
m_orderFrictionConstraintPool[swapi] = tmp;
}
}
}
}
btScalar deltaflengthsqr = 0;
if (infoGlobal.m_solverMode & SOLVER_SIMD)
{
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
{
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
if (iteration < constraint.m_overrideNumSolverIterations)
{
btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
m_deltafNC[j] = deltaf;
deltaflengthsqr += deltaf * deltaf;
}
}
} else
{
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
{
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
if (iteration < constraint.m_overrideNumSolverIterations)
{
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
m_deltafNC[j] = deltaf;
deltaflengthsqr += deltaf * deltaf;
}
}
}
if (m_onlyForNoneContact)
{
if (iteration==0)
{
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
} else {
// deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
if (beta>1)
{
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
} else
{
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
{
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
if (iteration < constraint.m_overrideNumSolverIterations)
{
btScalar additionaldeltaimpulse = beta * m_pNC[j];
constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
const btSolverConstraint& c = constraint;
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
}
}
}
}
m_deltafLengthSqrPrev = deltaflengthsqr;
}
if (infoGlobal.m_solverMode & SOLVER_SIMD)
{
if (iteration< infoGlobal.m_numIterations)
{
for (int j=0;j<numConstraints;j++)
{
if (constraints[j]->isEnabled())
{
int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
}
}
///solve all contact constraints using SIMD, if available
if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
for (int c=0;c<numPoolConstraints;c++)
{
btScalar totalImpulse =0;
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
btScalar deltaf = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafC[c] = deltaf;
deltaflengthsqr += deltaf*deltaf;
totalImpulse = solveManifold.m_appliedImpulse;
}
bool applyFriction = true;
if (applyFriction)
{
{
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
if (totalImpulse>btScalar(0))
{
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafCF[c*multiplier] = deltaf;
deltaflengthsqr += deltaf*deltaf;
} else {
m_deltafCF[c*multiplier] = 0;
}
}
if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
{
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
if (totalImpulse>btScalar(0))
{
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafCF[c*multiplier+1] = deltaf;
deltaflengthsqr += deltaf*deltaf;
} else {
m_deltafCF[c*multiplier+1] = 0;
}
}
}
}
}
else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
{
//solve the friction constraints after all contact constraints, don't interleave them
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int j;
for (j=0;j<numPoolConstraints;j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
//resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafC[j] = deltaf;
deltaflengthsqr += deltaf*deltaf;
}
///solve all friction constraints, using SIMD, if available
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
for (j=0;j<numFrictionPoolConstraints;j++)
{
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
if (totalImpulse>btScalar(0))
{
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
//resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafCF[j] = deltaf;
deltaflengthsqr += deltaf*deltaf;
} else {
m_deltafCF[j] = 0;
}
}
int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
for (j=0;j<numRollingFrictionPoolConstraints;j++)
{
btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
if (totalImpulse>btScalar(0))
{
btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
m_deltafCRF[j] = deltaf;
deltaflengthsqr += deltaf*deltaf;
} else {
m_deltafCRF[j] = 0;
}
}
}
}
} else
{
if (iteration< infoGlobal.m_numIterations)
{
for (int j=0;j<numConstraints;j++)
{
if (constraints[j]->isEnabled())
{
int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
}
}
///solve all contact constraints
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
for (int j=0;j<numPoolConstraints;j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafC[j] = deltaf;
deltaflengthsqr += deltaf*deltaf;
}
///solve all friction constraints
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
for (int j=0;j<numFrictionPoolConstraints;j++)
{
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
if (totalImpulse>btScalar(0))
{
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafCF[j] = deltaf;
deltaflengthsqr += deltaf*deltaf;
} else {
m_deltafCF[j] = 0;
}
}
int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
for (int j=0;j<numRollingFrictionPoolConstraints;j++)
{
btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
if (totalImpulse>btScalar(0))
{
btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
m_deltafCRF[j] = deltaf;
deltaflengthsqr += deltaf*deltaf;
} else {
m_deltafCRF[j] = 0;
}
}
}
}
if (!m_onlyForNoneContact)
{
if (iteration==0)
{
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = m_deltafC[j];
for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = m_deltafCF[j];
if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 )
{
for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = m_deltafCRF[j];
}
} else
{
// deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
if (beta>1) {
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = 0;
for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = 0;
if ( (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) {
for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = 0;
}
} else {
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
{
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
if (iteration < constraint.m_overrideNumSolverIterations) {
btScalar additionaldeltaimpulse = beta * m_pNC[j];
constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
const btSolverConstraint& c = constraint;
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
}
}
for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++)
{
btSolverConstraint& constraint = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
if (iteration< infoGlobal.m_numIterations) {
btScalar additionaldeltaimpulse = beta * m_pC[j];
constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
m_pC[j] = beta * m_pC[j] + m_deltafC[j];
btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
const btSolverConstraint& c = constraint;
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
}
}
for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++)
{
btSolverConstraint& constraint = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
if (iteration< infoGlobal.m_numIterations) {
btScalar additionaldeltaimpulse = beta * m_pCF[j];
constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
m_pCF[j] = beta * m_pCF[j] + m_deltafCF[j];
btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
const btSolverConstraint& c = constraint;
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
}
}
if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) {
for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++)
{
btSolverConstraint& constraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
if (iteration< infoGlobal.m_numIterations) {
btScalar additionaldeltaimpulse = beta * m_pCRF[j];
constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
m_pCRF[j] = beta * m_pCRF[j] + m_deltafCRF[j];
btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
const btSolverConstraint& c = constraint;
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
}
}
}
}
}
m_deltafLengthSqrPrev = deltaflengthsqr;
}
return deltaflengthsqr;
}
btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
{
m_pNC.resizeNoInitialize(0);
m_pC.resizeNoInitialize(0);
m_pCF.resizeNoInitialize(0);
m_pCRF.resizeNoInitialize(0);
m_deltafNC.resizeNoInitialize(0);
m_deltafC.resizeNoInitialize(0);
m_deltafCF.resizeNoInitialize(0);
m_deltafCRF.resizeNoInitialize(0);
return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
}

64
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h Normal file
View file

@ -0,0 +1,64 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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 BT_NNCG_CONSTRAINT_SOLVER_H
#define BT_NNCG_CONSTRAINT_SOLVER_H
#include "btSequentialImpulseConstraintSolver.h"
ATTRIBUTE_ALIGNED16(class) btNNCGConstraintSolver : public btSequentialImpulseConstraintSolver
{
protected:
btScalar m_deltafLengthSqrPrev;
btAlignedObjectArray<btScalar> m_pNC; // p for None Contact constraints
btAlignedObjectArray<btScalar> m_pC; // p for Contact constraints
btAlignedObjectArray<btScalar> m_pCF; // p for ContactFriction constraints
btAlignedObjectArray<btScalar> m_pCRF; // p for ContactRollingFriction constraints
//These are recalculated in every iterations. We just keep these to prevent reallocation in each iteration.
btAlignedObjectArray<btScalar> m_deltafNC; // deltaf for NoneContact constraints
btAlignedObjectArray<btScalar> m_deltafC; // deltaf for Contact constraints
btAlignedObjectArray<btScalar> m_deltafCF; // deltaf for ContactFriction constraints
btAlignedObjectArray<btScalar> m_deltafCRF; // deltaf for ContactRollingFriction constraints
protected:
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btNNCGConstraintSolver() : btSequentialImpulseConstraintSolver(), m_onlyForNoneContact(false) {}
virtual btConstraintSolverType getSolverType() const
{
return BT_NNCG_SOLVER;
}
bool m_onlyForNoneContact;
};
#endif //BT_NNCG_CONSTRAINT_SOLVER_H

5
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
View file

@ -116,6 +116,11 @@ public:
virtual void setParam(int num, btScalar value, int axis = -1);
///return the local value of parameter
virtual btScalar getParam(int num, int axis = -1) const;
virtual int getFlags() const
{
return m_flags;
}
virtual int calculateSerializeBufferSize() const;

808
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View file

File diff suppressed because it is too large Load diff

68
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
View file

@ -27,6 +27,8 @@ class btCollisionObject;
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
typedef btSimdScalar(*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&);
///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolver : public btConstraintSolver
{
@ -43,18 +45,32 @@ protected:
btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
int m_maxOverrideNumSolverIterations;
int m_fixedBodyId;
// When running solvers on multiple threads, a race condition exists for Kinematic objects that
// participate in more than one solver.
// The getOrInitSolverBody() function writes the companionId of each body (storing the index of the solver body
// for the current solver). For normal dynamic bodies it isn't an issue because they can only be in one island
// (and therefore one thread) at a time. But kinematic bodies can be in multiple islands at once.
// To avoid this race condition, this solver does not write the companionId, instead it stores the solver body
// index in this solver-local table, indexed by the uniqueId of the body.
btAlignedObjectArray<int> m_kinematicBodyUniqueIdToSolverBodyTable; // only used for multithreading
btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric;
btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit;
btScalar m_leastSquaresResidual;
void setupFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
btScalar desiredVelocity=0., btScalar cfmSlip=0.);
void setupRollingFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
void setupTorsionalFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
btScalar desiredVelocity=0., btScalar cfmSlip=0.);
btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
btSolverConstraint& addRollingFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f);
btSolverConstraint& addTorsionalFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,btScalar torsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f);
void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp,
@ -76,11 +92,11 @@ protected:
void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
void resolveSplitPenetrationSIMD(
btSimdScalar resolveSplitPenetrationSIMD(
btSolverBody& bodyA,btSolverBody& bodyB,
const btSolverConstraint& contactConstraint);
void resolveSplitPenetrationImpulseCacheFriendly(
btScalar resolveSplitPenetrationImpulseCacheFriendly(
btSolverBody& bodyA,btSolverBody& bodyB,
const btSolverConstraint& contactConstraint);
@ -88,13 +104,10 @@ protected:
int getOrInitSolverBody(btCollisionObject& body,btScalar timeStep);
void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep);
void resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
void resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
void resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
void resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
btSimdScalar resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
btSimdScalar resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
btSimdScalar resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
btSimdScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
protected:
@ -115,9 +128,7 @@ public:
virtual ~btSequentialImpulseConstraintSolver();
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
///clear internal cached data and reset random seed
virtual void reset();
@ -139,6 +150,33 @@ public:
{
return BT_SEQUENTIAL_IMPULSE_SOLVER;
}
btSingleConstraintRowSolver getActiveConstraintRowSolverGeneric()
{
return m_resolveSingleConstraintRowGeneric;
}
void setConstraintRowSolverGeneric(btSingleConstraintRowSolver rowSolver)
{
m_resolveSingleConstraintRowGeneric = rowSolver;
}
btSingleConstraintRowSolver getActiveConstraintRowSolverLowerLimit()
{
return m_resolveSingleConstraintRowLowerLimit;
}
void setConstraintRowSolverLowerLimit(btSingleConstraintRowSolver rowSolver)
{
m_resolveSingleConstraintRowLowerLimit = rowSolver;
}
///Various implementations of solving a single constraint row using a generic equality constraint, using scalar reference, SSE2 or SSE4
btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric();
btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric();
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric();
///Various implementations of solving a single constraint row using an inequality (lower limit) constraint, using scalar reference, SSE2 or SSE4
btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit();
btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit();
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit();
};

8
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
View file

@ -539,8 +539,8 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
btScalar tag_vel = getTargetLinMotorVelocity();
btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP);
info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps;
info->m_lowerLimit[srow] += -getMaxLinMotorForce() / info->fps;
info->m_upperLimit[srow] += getMaxLinMotorForce() / info->fps;
}
if(limit)
{
@ -641,8 +641,8 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
}
btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP);
info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps;
info->m_lowerLimit[srow] = -getMaxAngMotorForce() / info->fps;
info->m_upperLimit[srow] = getMaxAngMotorForce() / info->fps;
}
if(limit)
{

7
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
View file

@ -25,6 +25,8 @@ TODO:
#ifndef BT_SLIDER_CONSTRAINT_H
#define BT_SLIDER_CONSTRAINT_H
#include "LinearMath/btScalar.h"//for BT_USE_DOUBLE_PRECISION
#ifdef BT_USE_DOUBLE_PRECISION
#define btSliderConstraintData2 btSliderConstraintDoubleData
#define btSliderConstraintDataName "btSliderConstraintDoubleData"
@ -280,6 +282,11 @@ public:
virtual void setParam(int num, btScalar value, int axis = -1);
///return the local value of parameter
virtual btScalar getParam(int num, int axis = -1) const;
virtual int getFlags() const
{
return m_flags;
}
virtual int calculateSerializeBufferSize() const;

4
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
View file

@ -24,7 +24,7 @@ subject to the following restrictions:
btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
:btTypedObject(type),
m_userConstraintType(-1),
m_userConstraintId(-1),
m_userConstraintPtr((void*)-1),
m_breakingImpulseThreshold(SIMD_INFINITY),
m_isEnabled(true),
m_needsFeedback(false),
@ -41,7 +41,7 @@ m_jointFeedback(0)
btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB)
:btTypedObject(type),
m_userConstraintType(-1),
m_userConstraintId(-1),
m_userConstraintPtr((void*)-1),
m_breakingImpulseThreshold(SIMD_INFINITY),
m_isEnabled(true),
m_needsFeedback(false),

7
Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
View file

@ -44,6 +44,7 @@ enum btTypedConstraintType
D6_SPRING_CONSTRAINT_TYPE,
GEAR_CONSTRAINT_TYPE,
FIXED_CONSTRAINT_TYPE,
D6_SPRING_2_CONSTRAINT_TYPE,
MAX_CONSTRAINT_TYPE
};
@ -65,6 +66,7 @@ enum btConstraintParams
ATTRIBUTE_ALIGNED16(struct) btJointFeedback
{
BT_DECLARE_ALIGNED_ALLOCATOR();
btVector3 m_appliedForceBodyA;
btVector3 m_appliedTorqueBodyA;
btVector3 m_appliedForceBodyB;
@ -143,11 +145,6 @@ public:
// lo and hi limits for variables (set to -/+ infinity on entry).
btScalar *m_lowerLimit,*m_upperLimit;
// findex vector for variables. see the LCP solver interface for a
// description of what this does. this is set to -1 on entry.
// note that the returned indexes are relative to the first index of
// the constraint.
int *findex;
// number of solver iterations
int m_numIterations;

405
Engine/lib/bullet/src/BulletDynamics/Dynamics/Bullet-C-API.cpp
View file

@ -1,405 +0,0 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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.
*/
/*
Draft high-level generic physics C-API. For low-level access, use the physics SDK native API's.
Work in progress, functionality will be added on demand.
If possible, use the richer Bullet C++ API, by including <src/btBulletDynamicsCommon.h>
*/
#include "Bullet-C-Api.h"
#include "btBulletDynamicsCommon.h"
#include "LinearMath/btAlignedAllocator.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btScalar.h"
#include "LinearMath/btMatrix3x3.h"
#include "LinearMath/btTransform.h"
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "BulletCollision/CollisionShapes/btTriangleShape.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
#include "BulletCollision/NarrowPhaseCollision/btPointCollector.h"
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
/*
Create and Delete a Physics SDK
*/
struct btPhysicsSdk
{
// btDispatcher* m_dispatcher;
// btOverlappingPairCache* m_pairCache;
// btConstraintSolver* m_constraintSolver
btVector3 m_worldAabbMin;
btVector3 m_worldAabbMax;
//todo: version, hardware/optimization settings etc?
btPhysicsSdk()
:m_worldAabbMin(-1000,-1000,-1000),
m_worldAabbMax(1000,1000,1000)
{
}
};
plPhysicsSdkHandle plNewBulletSdk()
{
void* mem = btAlignedAlloc(sizeof(btPhysicsSdk),16);
return (plPhysicsSdkHandle)new (mem)btPhysicsSdk;
}
void plDeletePhysicsSdk(plPhysicsSdkHandle physicsSdk)
{
btPhysicsSdk* phys = reinterpret_cast<btPhysicsSdk*>(physicsSdk);
btAlignedFree(phys);
}
/* Dynamics World */
plDynamicsWorldHandle plCreateDynamicsWorld(plPhysicsSdkHandle physicsSdkHandle)
{
btPhysicsSdk* physicsSdk = reinterpret_cast<btPhysicsSdk*>(physicsSdkHandle);
void* mem = btAlignedAlloc(sizeof(btDefaultCollisionConfiguration),16);
btDefaultCollisionConfiguration* collisionConfiguration = new (mem)btDefaultCollisionConfiguration();
mem = btAlignedAlloc(sizeof(btCollisionDispatcher),16);
btDispatcher* dispatcher = new (mem)btCollisionDispatcher(collisionConfiguration);
mem = btAlignedAlloc(sizeof(btAxisSweep3),16);
btBroadphaseInterface* pairCache = new (mem)btAxisSweep3(physicsSdk->m_worldAabbMin,physicsSdk->m_worldAabbMax);
mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16);
btConstraintSolver* constraintSolver = new(mem) btSequentialImpulseConstraintSolver();
mem = btAlignedAlloc(sizeof(btDiscreteDynamicsWorld),16);
return (plDynamicsWorldHandle) new (mem)btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration);
}
void plDeleteDynamicsWorld(plDynamicsWorldHandle world)
{
//todo: also clean up the other allocations, axisSweep, pairCache,dispatcher,constraintSolver,collisionConfiguration
btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
btAlignedFree(dynamicsWorld);
}
void plStepSimulation(plDynamicsWorldHandle world, plReal timeStep)
{
btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
btAssert(dynamicsWorld);
dynamicsWorld->stepSimulation(timeStep);
}
void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object)
{
btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
btAssert(dynamicsWorld);
btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
btAssert(body);
dynamicsWorld->addRigidBody(body);
}
void plRemoveRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object)
{
btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
btAssert(dynamicsWorld);
btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
btAssert(body);
dynamicsWorld->removeRigidBody(body);
}
/* Rigid Body */
plRigidBodyHandle plCreateRigidBody( void* user_data, float mass, plCollisionShapeHandle cshape )
{
btTransform trans;
trans.setIdentity();
btVector3 localInertia(0,0,0);
btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
btAssert(shape);
if (mass)
{
shape->calculateLocalInertia(mass,localInertia);
}
void* mem = btAlignedAlloc(sizeof(btRigidBody),16);
btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0,shape,localInertia);
btRigidBody* body = new (mem)btRigidBody(rbci);
body->setWorldTransform(trans);
body->setUserPointer(user_data);
return (plRigidBodyHandle) body;
}
void plDeleteRigidBody(plRigidBodyHandle cbody)
{
btRigidBody* body = reinterpret_cast< btRigidBody* >(cbody);
btAssert(body);
btAlignedFree( body);
}
/* Collision Shape definition */
plCollisionShapeHandle plNewSphereShape(plReal radius)
{
void* mem = btAlignedAlloc(sizeof(btSphereShape),16);
return (plCollisionShapeHandle) new (mem)btSphereShape(radius);
}
plCollisionShapeHandle plNewBoxShape(plReal x, plReal y, plReal z)
{
void* mem = btAlignedAlloc(sizeof(btBoxShape),16);
return (plCollisionShapeHandle) new (mem)btBoxShape(btVector3(x,y,z));
}
plCollisionShapeHandle plNewCapsuleShape(plReal radius, plReal height)
{
//capsule is convex hull of 2 spheres, so use btMultiSphereShape
const int numSpheres = 2;
btVector3 positions[numSpheres] = {btVector3(0,height,0),btVector3(0,-height,0)};
btScalar radi[numSpheres] = {radius,radius};
void* mem = btAlignedAlloc(sizeof(btMultiSphereShape),16);
return (plCollisionShapeHandle) new (mem)btMultiSphereShape(positions,radi,numSpheres);
}
plCollisionShapeHandle plNewConeShape(plReal radius, plReal height)
{
void* mem = btAlignedAlloc(sizeof(btConeShape),16);
return (plCollisionShapeHandle) new (mem)btConeShape(radius,height);
}
plCollisionShapeHandle plNewCylinderShape(plReal radius, plReal height)
{
void* mem = btAlignedAlloc(sizeof(btCylinderShape),16);
return (plCollisionShapeHandle) new (mem)btCylinderShape(btVector3(radius,height,radius));
}
/* Convex Meshes */
plCollisionShapeHandle plNewConvexHullShape()
{
void* mem = btAlignedAlloc(sizeof(btConvexHullShape),16);
return (plCollisionShapeHandle) new (mem)btConvexHullShape();
}
/* Concave static triangle meshes */
plMeshInterfaceHandle plNewMeshInterface()
{
return 0;
}
plCollisionShapeHandle plNewCompoundShape()
{
void* mem = btAlignedAlloc(sizeof(btCompoundShape),16);
return (plCollisionShapeHandle) new (mem)btCompoundShape();
}
void plAddChildShape(plCollisionShapeHandle compoundShapeHandle,plCollisionShapeHandle childShapeHandle, plVector3 childPos,plQuaternion childOrn)
{
btCollisionShape* colShape = reinterpret_cast<btCollisionShape*>(compoundShapeHandle);
btAssert(colShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE);
btCompoundShape* compoundShape = reinterpret_cast<btCompoundShape*>(colShape);
btCollisionShape* childShape = reinterpret_cast<btCollisionShape*>(childShapeHandle);
btTransform localTrans;
localTrans.setIdentity();
localTrans.setOrigin(btVector3(childPos[0],childPos[1],childPos[2]));
localTrans.setRotation(btQuaternion(childOrn[0],childOrn[1],childOrn[2],childOrn[3]));
compoundShape->addChildShape(localTrans,childShape);
}
void plSetEuler(plReal yaw,plReal pitch,plReal roll, plQuaternion orient)
{
btQuaternion orn;
orn.setEuler(yaw,pitch,roll);
orient[0] = orn.getX();
orient[1] = orn.getY();
orient[2] = orn.getZ();
orient[3] = orn.getW();
}
// extern void plAddTriangle(plMeshInterfaceHandle meshHandle, plVector3 v0,plVector3 v1,plVector3 v2);
// extern plCollisionShapeHandle plNewStaticTriangleMeshShape(plMeshInterfaceHandle);
void plAddVertex(plCollisionShapeHandle cshape, plReal x,plReal y,plReal z)
{
btCollisionShape* colShape = reinterpret_cast<btCollisionShape*>( cshape);
(void)colShape;
btAssert(colShape->getShapeType()==CONVEX_HULL_SHAPE_PROXYTYPE);
btConvexHullShape* convexHullShape = reinterpret_cast<btConvexHullShape*>( cshape);
convexHullShape->addPoint(btVector3(x,y,z));
}
void plDeleteShape(plCollisionShapeHandle cshape)
{
btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
btAssert(shape);
btAlignedFree(shape);
}
void plSetScaling(plCollisionShapeHandle cshape, plVector3 cscaling)
{
btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
btAssert(shape);
btVector3 scaling(cscaling[0],cscaling[1],cscaling[2]);
shape->setLocalScaling(scaling);
}
void plSetPosition(plRigidBodyHandle object, const plVector3 position)
{
btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
btAssert(body);
btVector3 pos(position[0],position[1],position[2]);
btTransform worldTrans = body->getWorldTransform();
worldTrans.setOrigin(pos);
body->setWorldTransform(worldTrans);
}
void plSetOrientation(plRigidBodyHandle object, const plQuaternion orientation)
{
btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
btAssert(body);
btQuaternion orn(orientation[0],orientation[1],orientation[2],orientation[3]);
btTransform worldTrans = body->getWorldTransform();
worldTrans.setRotation(orn);
body->setWorldTransform(worldTrans);
}
void plSetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix)
{
btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
btAssert(body);
btTransform& worldTrans = body->getWorldTransform();
worldTrans.setFromOpenGLMatrix(matrix);
}
void plGetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix)
{
btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
btAssert(body);
body->getWorldTransform().getOpenGLMatrix(matrix);
}
void plGetPosition(plRigidBodyHandle object,plVector3 position)
{
btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
btAssert(body);
const btVector3& pos = body->getWorldTransform().getOrigin();
position[0] = pos.getX();
position[1] = pos.getY();
position[2] = pos.getZ();
}
void plGetOrientation(plRigidBodyHandle object,plQuaternion orientation)
{
btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
btAssert(body);
const btQuaternion& orn = body->getWorldTransform().getRotation();
orientation[0] = orn.getX();
orientation[1] = orn.getY();
orientation[2] = orn.getZ();
orientation[3] = orn.getW();
}
//plRigidBodyHandle plRayCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal);
// extern plRigidBodyHandle plObjectCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal);
double plNearestPoints(float p1[3], float p2[3], float p3[3], float q1[3], float q2[3], float q3[3], float *pa, float *pb, float normal[3])
{
btVector3 vp(p1[0], p1[1], p1[2]);
btTriangleShape trishapeA(vp,
btVector3(p2[0], p2[1], p2[2]),
btVector3(p3[0], p3[1], p3[2]));
trishapeA.setMargin(0.000001f);
btVector3 vq(q1[0], q1[1], q1[2]);
btTriangleShape trishapeB(vq,
btVector3(q2[0], q2[1], q2[2]),
btVector3(q3[0], q3[1], q3[2]));
trishapeB.setMargin(0.000001f);
// btVoronoiSimplexSolver sGjkSimplexSolver;
// btGjkEpaPenetrationDepthSolver penSolverPtr;
static btSimplexSolverInterface sGjkSimplexSolver;
sGjkSimplexSolver.reset();
static btGjkEpaPenetrationDepthSolver Solver0;
static btMinkowskiPenetrationDepthSolver Solver1;
btConvexPenetrationDepthSolver* Solver = NULL;
Solver = &Solver1;
btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,Solver);
convexConvex.m_catchDegeneracies = 1;
// btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,0);
btPointCollector gjkOutput;
btGjkPairDetector::ClosestPointInput input;
btTransform tr;
tr.setIdentity();
input.m_transformA = tr;
input.m_transformB = tr;
convexConvex.getClosestPoints(input, gjkOutput, 0);
if (gjkOutput.m_hasResult)
{
pb[0] = pa[0] = gjkOutput.m_pointInWorld[0];
pb[1] = pa[1] = gjkOutput.m_pointInWorld[1];
pb[2] = pa[2] = gjkOutput.m_pointInWorld[2];
pb[0]+= gjkOutput.m_normalOnBInWorld[0] * gjkOutput.m_distance;
pb[1]+= gjkOutput.m_normalOnBInWorld[1] * gjkOutput.m_distance;
pb[2]+= gjkOutput.m_normalOnBInWorld[2] * gjkOutput.m_distance;
normal[0] = gjkOutput.m_normalOnBInWorld[0];
normal[1] = gjkOutput.m_normalOnBInWorld[1];
normal[2] = gjkOutput.m_normalOnBInWorld[2];
return gjkOutput.m_distance;
}
return -1.0f;
}

257
Engine/lib/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
View file

@ -4,8 +4,8 @@ Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
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,
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.
@ -34,6 +34,7 @@ subject to the following restrictions:
#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
@ -58,7 +59,7 @@ int firstHit=startHit;
SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs)
{
int islandId;
const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
@ -88,7 +89,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
int m_numConstraints;
btIDebugDraw* m_debugDrawer;
btDispatcher* m_dispatcher;
btAlignedObjectArray<btCollisionObject*> m_bodies;
btAlignedObjectArray<btPersistentManifold*> m_manifolds;
btAlignedObjectArray<btTypedConstraint*> m_constraints;
@ -127,7 +128,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
m_constraints.resize (0);
}
virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId)
{
if (islandId<0)
@ -140,7 +141,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
btTypedConstraint** startConstraint = 0;
int numCurConstraints = 0;
int i;
//find the first constraint for this island
for (i=0;i<m_numConstraints;i++)
{
@ -164,7 +165,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
} else
{
for (i=0;i<numBodies;i++)
m_bodies.push_back(bodies[i]);
for (i=0;i<numManifolds;i++)
@ -187,7 +188,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,*m_solverInfo,m_debugDrawer,m_dispatcher);
m_bodies.resize(0);
m_manifolds.resize(0);
@ -206,10 +207,10 @@ m_solverIslandCallback ( NULL ),
m_constraintSolver(constraintSolver),
m_gravity(0,-10,0),
m_localTime(0),
m_fixedTimeStep(0),
m_synchronizeAllMotionStates(false),
m_applySpeculativeContactRestitution(false),
m_profileTimings(0),
m_fixedTimeStep(0),
m_latencyMotionStateInterpolation(true)
{
@ -317,6 +318,9 @@ void btDiscreteDynamicsWorld::debugDrawWorld()
}
}
}
if (getDebugDrawer())
getDebugDrawer()->flushLines();
}
void btDiscreteDynamicsWorld::clearForces()
@ -329,7 +333,7 @@ void btDiscreteDynamicsWorld::clearForces()
//it might break backward compatibility (people applying forces on sleeping objects get never cleared and accumulate on wake-up
body->clearForces();
}
}
}
///apply gravity, call this once per timestep
void btDiscreteDynamicsWorld::applyGravity()
@ -445,7 +449,7 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
applyGravity();
for (int i=0;i<clampedSimulationSteps;i++)
{
@ -463,18 +467,18 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
#ifndef BT_NO_PROFILE
CProfileManager::Increment_Frame_Counter();
#endif //BT_NO_PROFILE
return numSimulationSubSteps;
}
void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
{
BT_PROFILE("internalSingleStepSimulation");
if(0 != m_internalPreTickCallback) {
(*m_internalPreTickCallback)(this, timeStep);
}
}
///apply gravity, predict motion
predictUnconstraintMotion(timeStep);
@ -487,20 +491,20 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
createPredictiveContacts(timeStep);
///perform collision detection
performDiscreteCollisionDetection();
calculateSimulationIslands();
getSolverInfo().m_timeStep = timeStep;
///solve contact and other joint constraints
solveConstraints(getSolverInfo());
///CallbackTriggers();
///integrate transforms
@ -509,12 +513,12 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
///update vehicle simulation
updateActions(timeStep);
updateActivationState( timeStep );
if(0 != m_internalTickCallback) {
(*m_internalTickCallback)(this, timeStep);
}
}
}
void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
@ -606,14 +610,14 @@ void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short
void btDiscreteDynamicsWorld::updateActions(btScalar timeStep)
{
BT_PROFILE("updateActions");
for ( int i=0;i<m_actions.size();i++)
{
m_actions[i]->updateAction( this, timeStep);
}
}
void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
{
BT_PROFILE("updateActivationState");
@ -634,7 +638,7 @@ void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
{
if (body->getActivationState() == ACTIVE_TAG)
body->setActivationState( WANTS_DEACTIVATION );
if (body->getActivationState() == ISLAND_SLEEPING)
if (body->getActivationState() == ISLAND_SLEEPING)
{
body->setAngularVelocity(btVector3(0,0,0));
body->setLinearVelocity(btVector3(0,0,0));
@ -653,6 +657,9 @@ void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies)
{
m_constraints.push_back(constraint);
//Make sure the two bodies of a type constraint are different (possibly add this to the btTypedConstraint constructor?)
btAssert(&constraint->getRigidBodyA()!=&constraint->getRigidBodyB());
if (disableCollisionsBetweenLinkedBodies)
{
constraint->getRigidBodyA().addConstraintRef(constraint);
@ -704,25 +711,25 @@ void btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character)
void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
BT_PROFILE("solveConstraints");
m_sortedConstraints.resize( m_constraints.size());
int i;
int i;
for (i=0;i<getNumConstraints();i++)
{
m_sortedConstraints[i] = m_constraints[i];
}
// btAssert(0);
m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
m_solverIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),getDebugDrawer());
m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
/// solve all the constraints for this island
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverIslandCallback);
@ -743,10 +750,10 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands()
for (int i=0;i<this->m_predictiveManifolds.size();i++)
{
btPersistentManifold* manifold = m_predictiveManifolds[i];
const btCollisionObject* colObj0 = manifold->getBody0();
const btCollisionObject* colObj1 = manifold->getBody1();
if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
{
@ -754,7 +761,7 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands()
}
}
}
{
int i;
int numConstraints = int(m_constraints.size());
@ -778,7 +785,7 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands()
//Store the island id in each body
getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
}
@ -794,7 +801,7 @@ public:
btDispatcher* m_dispatcher;
public:
btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
m_me(me),
m_allowedPenetration(0.0f),
@ -871,32 +878,19 @@ public:
int gNumClampedCcdMotions=0;
void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
void btDiscreteDynamicsWorld::createPredictiveContactsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep)
{
BT_PROFILE("createPredictiveContacts");
{
BT_PROFILE("release predictive contact manifolds");
for (int i=0;i<m_predictiveManifolds.size();i++)
{
btPersistentManifold* manifold = m_predictiveManifolds[i];
this->m_dispatcher1->releaseManifold(manifold);
}
m_predictiveManifolds.clear();
}
btTransform predictedTrans;
for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
for ( int i=0;i<numBodies;i++)
{
btRigidBody* body = m_nonStaticRigidBodies[i];
btRigidBody* body = bodies[i];
body->setHitFraction(1.f);
if (body->isActive() && (!body->isStaticOrKinematicObject()))
{
body->predictIntegratedTransform(timeStep, predictedTrans);
btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
@ -910,7 +904,7 @@ void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
{
public:
StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher)
{
}
@ -940,14 +934,16 @@ void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
{
btVector3 distVec = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin())*sweepResults.m_closestHitFraction;
btScalar distance = distVec.dot(-sweepResults.m_hitNormalWorld);
btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body,sweepResults.m_hitCollisionObject);
btMutexLock( &m_predictiveManifoldsMutex );
m_predictiveManifolds.push_back(manifold);
btMutexUnlock( &m_predictiveManifoldsMutex );
btVector3 worldPointB = body->getWorldTransform().getOrigin()+distVec;
btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse()*worldPointB;
@ -967,23 +963,45 @@ void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
}
}
}
void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
void btDiscreteDynamicsWorld::releasePredictiveContacts()
{
BT_PROFILE( "release predictive contact manifolds" );
for ( int i = 0; i < m_predictiveManifolds.size(); i++ )
{
btPersistentManifold* manifold = m_predictiveManifolds[ i ];
this->m_dispatcher1->releaseManifold( manifold );
}
m_predictiveManifolds.clear();
}
void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
{
BT_PROFILE("createPredictiveContacts");
releasePredictiveContacts();
if (m_nonStaticRigidBodies.size() > 0)
{
createPredictiveContactsInternal( &m_nonStaticRigidBodies[ 0 ], m_nonStaticRigidBodies.size(), timeStep );
}
}
void btDiscreteDynamicsWorld::integrateTransformsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep )
{
BT_PROFILE("integrateTransforms");
btTransform predictedTrans;
for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
for (int i=0;i<numBodies;i++)
{
btRigidBody* body = m_nonStaticRigidBodies[i];
btRigidBody* body = bodies[i];
body->setHitFraction(1.f);
if (body->isActive() && (!body->isStaticOrKinematicObject()))
{
body->predictIntegratedTransform(timeStep, predictedTrans);
btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
{
@ -996,7 +1014,7 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
{
public:
StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher)
{
}
@ -1026,7 +1044,7 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
{
//printf("clamped integration to hit fraction = %f\n",fraction);
body->setHitFraction(sweepResults.m_closestHitFraction);
body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
@ -1051,13 +1069,13 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
printf("sm2=%f\n",sm2);
}
#else
//don't apply the collision response right now, it will happen next frame
//if you really need to, you can uncomment next 3 lines. Note that is uses zero restitution.
//btScalar appliedImpulse = 0.f;
//btScalar depth = 0.f;
//appliedImpulse = resolveSingleCollision(body,(btCollisionObject*)sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth);
#endif
@ -1065,15 +1083,25 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
}
}
}
body->proceedToTransform( predictedTrans);
}
}
///this should probably be switched on by default, but it is not well tested yet
}
void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
{
BT_PROFILE("integrateTransforms");
if (m_nonStaticRigidBodies.size() > 0)
{
integrateTransformsInternal(&m_nonStaticRigidBodies[0], m_nonStaticRigidBodies.size(), timeStep);
}
///this should probably be switched on by default, but it is not well tested yet
if (m_applySpeculativeContactRestitution)
{
BT_PROFILE("apply speculative contact restitution");
@ -1082,7 +1110,7 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
btPersistentManifold* manifold = m_predictiveManifolds[i];
btRigidBody* body0 = btRigidBody::upcast((btCollisionObject*)manifold->getBody0());
btRigidBody* body1 = btRigidBody::upcast((btCollisionObject*)manifold->getBody1());
for (int p=0;p<manifold->getNumContacts();p++)
{
const btManifoldPoint& pt = manifold->getContactPoint(p);
@ -1092,11 +1120,11 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
//if (pt.getDistance()>0 && combinedRestitution>0 && pt.m_appliedImpulse != 0.f)
{
btVector3 imp = -pt.m_normalWorldOnB * pt.m_appliedImpulse* combinedRestitution;
const btVector3& pos1 = pt.getPositionWorldOnA();
const btVector3& pos2 = pt.getPositionWorldOnB();
btVector3 rel_pos0 = pos1 - body0->getWorldTransform().getOrigin();
btVector3 rel_pos0 = pos1 - body0->getWorldTransform().getOrigin();
btVector3 rel_pos1 = pos2 - body1->getWorldTransform().getOrigin();
if (body0)
@ -1107,14 +1135,12 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
}
}
}
}
void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
{
BT_PROFILE("predictUnconstraintMotion");
@ -1146,7 +1172,7 @@ void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
{
@ -1166,12 +1192,12 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
btTransform tr;
tr.setIdentity();
btVector3 pivot = p2pC->getPivotInA();
pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot;
pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot;
tr.setOrigin(pivot);
getDebugDrawer()->drawTransform(tr, dbgDrawSize);
// that ideally should draw the same frame
// that ideally should draw the same frame
pivot = p2pC->getPivotInB();
pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot;
pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot;
tr.setOrigin(pivot);
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
}
@ -1190,13 +1216,13 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
break;
}
bool drawSect = true;
if(minAng > maxAng)
if(!pHinge->hasLimit())
{
minAng = btScalar(0.f);
maxAng = SIMD_2_PI;
drawSect = false;
}
if(drawLimits)
if(drawLimits)
{
btVector3& center = tr.getOrigin();
btVector3 normal = tr.getBasis().getColumn(2);
@ -1231,7 +1257,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0));
pPrev = pCur;
}
}
btScalar tws = pCT->getTwistSpan();
btScalar twa = pCT->getTwistAngle();
bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f));
@ -1259,7 +1285,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
tr = p6DOF->getCalculatedTransformB();
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
if(drawLimits)
if(drawLimits)
{
tr = p6DOF->getCalculatedTransformA();
const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
@ -1300,6 +1326,57 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
}
}
break;
///note: the code for D6_SPRING_2_CONSTRAINT_TYPE is identical to D6_CONSTRAINT_TYPE, the D6_CONSTRAINT_TYPE+D6_SPRING_CONSTRAINT_TYPE will likely become obsolete/deprecated at some stage
case D6_SPRING_2_CONSTRAINT_TYPE:
{
{
btGeneric6DofSpring2Constraint* p6DOF = (btGeneric6DofSpring2Constraint*)constraint;
btTransform tr = p6DOF->getCalculatedTransformA();
if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
tr = p6DOF->getCalculatedTransformB();
if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
if (drawLimits)
{
tr = p6DOF->getCalculatedTransformA();
const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
btVector3 up = tr.getBasis().getColumn(2);
btVector3 axis = tr.getBasis().getColumn(0);
btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0));
axis = tr.getBasis().getColumn(1);
btScalar ay = p6DOF->getAngle(1);
btScalar az = p6DOF->getAngle(2);
btScalar cy = btCos(ay);
btScalar sy = btSin(ay);
btScalar cz = btCos(az);
btScalar sz = btSin(az);
btVector3 ref;
ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2];
ref[1] = -sz*axis[0] + cz*axis[1];
ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2];
tr = p6DOF->getCalculatedTransformB();
btVector3 normal = -tr.getBasis().getColumn(0);
btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
if (minFi > maxFi)
{
getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0, 0, 0), false);
}
else if (minFi < maxFi)
{
getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0, 0, 0), true);
}
tr = p6DOF->getCalculatedTransformA();
btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0, 0, 0));
}
}
break;
}
case SLIDER_CONSTRAINT_TYPE:
{
btSliderConstraint* pSlider = (btSliderConstraint*)constraint;
@ -1322,7 +1399,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
}
}
break;
default :
default :
break;
}
return;
@ -1422,19 +1499,19 @@ void btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serialize
worldInfo->m_solverInfo.m_globalCfm = getSolverInfo().m_globalCfm;
worldInfo->m_solverInfo.m_splitImpulsePenetrationThreshold = getSolverInfo().m_splitImpulsePenetrationThreshold;
worldInfo->m_solverInfo.m_splitImpulseTurnErp = getSolverInfo().m_splitImpulseTurnErp;
worldInfo->m_solverInfo.m_linearSlop = getSolverInfo().m_linearSlop;
worldInfo->m_solverInfo.m_warmstartingFactor = getSolverInfo().m_warmstartingFactor;
worldInfo->m_solverInfo.m_maxGyroscopicForce = getSolverInfo().m_maxGyroscopicForce;
worldInfo->m_solverInfo.m_singleAxisRollingFrictionThreshold = getSolverInfo().m_singleAxisRollingFrictionThreshold;
worldInfo->m_solverInfo.m_numIterations = getSolverInfo().m_numIterations;
worldInfo->m_solverInfo.m_solverMode = getSolverInfo().m_solverMode;
worldInfo->m_solverInfo.m_restingContactRestitutionThreshold = getSolverInfo().m_restingContactRestitutionThreshold;
worldInfo->m_solverInfo.m_minimumSolverBatchSize = getSolverInfo().m_minimumSolverBatchSize;
worldInfo->m_solverInfo.m_splitImpulse = getSolverInfo().m_splitImpulse;
#ifdef BT_USE_DOUBLE_PRECISION
const char* structType = "btDynamicsWorldDoubleData";
#else//BT_USE_DOUBLE_PRECISION
@ -1450,10 +1527,10 @@ void btDiscreteDynamicsWorld::serialize(btSerializer* serializer)
serializeDynamicsWorldInfo(serializer);
serializeRigidBodies(serializer);
serializeCollisionObjects(serializer);
serializeRigidBodies(serializer);
serializer->finishSerialization();
}

9
Engine/lib/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
View file

@ -30,6 +30,7 @@ class btIDebugDraw;
struct InplaceSolverIslandCallback;
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btThreads.h"
///btDiscreteDynamicsWorld provides discrete rigid body simulation
@ -68,9 +69,11 @@ protected:
bool m_latencyMotionStateInterpolation;
btAlignedObjectArray<btPersistentManifold*> m_predictiveManifolds;
btSpinMutex m_predictiveManifoldsMutex; // used to synchronize threads creating predictive contacts
virtual void predictUnconstraintMotion(btScalar timeStep);
void integrateTransformsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep ); // can be called in parallel
virtual void integrateTransforms(btScalar timeStep);
virtual void calculateSimulationIslands();
@ -85,7 +88,9 @@ protected:
virtual void internalSingleStepSimulation( btScalar timeStep);
void createPredictiveContacts(btScalar timeStep);
void releasePredictiveContacts();
void createPredictiveContactsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep ); // can be called in parallel
virtual void createPredictiveContacts(btScalar timeStep);
virtual void saveKinematicState(btScalar timeStep);
@ -151,7 +156,7 @@ public:
virtual void removeCollisionObject(btCollisionObject* collisionObject);
void debugDrawConstraint(btTypedConstraint* constraint);
virtual void debugDrawConstraint(btTypedConstraint* constraint);
virtual void debugDrawWorld();

162
Engine/lib/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp Normal file
View file

@ -0,0 +1,162 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
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.
*/
#include "btDiscreteDynamicsWorldMt.h"
//collision detection
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
#include "BulletCollision/CollisionShapes/btCollisionShape.h"
#include "btSimulationIslandManagerMt.h"
#include "LinearMath/btTransformUtil.h"
#include "LinearMath/btQuickprof.h"
//rigidbody & constraints
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
#include "LinearMath/btIDebugDraw.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "BulletDynamics/Dynamics/btActionInterface.h"
#include "LinearMath/btQuickprof.h"
#include "LinearMath/btMotionState.h"
#include "LinearMath/btSerializer.h"
struct InplaceSolverIslandCallbackMt : public btSimulationIslandManagerMt::IslandCallback
{
btContactSolverInfo* m_solverInfo;
btConstraintSolver* m_solver;
btIDebugDraw* m_debugDrawer;
btDispatcher* m_dispatcher;
InplaceSolverIslandCallbackMt(
btConstraintSolver* solver,
btStackAlloc* stackAlloc,
btDispatcher* dispatcher)
:m_solverInfo(NULL),
m_solver(solver),
m_debugDrawer(NULL),
m_dispatcher(dispatcher)
{
}
InplaceSolverIslandCallbackMt& operator=(InplaceSolverIslandCallbackMt& other)
{
btAssert(0);
(void)other;
return *this;
}
SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btIDebugDraw* debugDrawer)
{
btAssert(solverInfo);
m_solverInfo = solverInfo;
m_debugDrawer = debugDrawer;
}
virtual void processIsland( btCollisionObject** bodies,
int numBodies,
btPersistentManifold** manifolds,
int numManifolds,
btTypedConstraint** constraints,
int numConstraints,
int islandId
)
{
m_solver->solveGroup( bodies,
numBodies,
manifolds,
numManifolds,
constraints,
numConstraints,
*m_solverInfo,
m_debugDrawer,
m_dispatcher
);
}
};
btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
: btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration)
{
if (m_ownsIslandManager)
{
m_islandManager->~btSimulationIslandManager();
btAlignedFree( m_islandManager);
}
{
void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallbackMt),16);
m_solverIslandCallbackMt = new (mem) InplaceSolverIslandCallbackMt (m_constraintSolver, 0, dispatcher);
}
{
void* mem = btAlignedAlloc(sizeof(btSimulationIslandManagerMt),16);
btSimulationIslandManagerMt* im = new (mem) btSimulationIslandManagerMt();
m_islandManager = im;
im->setMinimumSolverBatchSize( m_solverInfo.m_minimumSolverBatchSize );
}
}
btDiscreteDynamicsWorldMt::~btDiscreteDynamicsWorldMt()
{
if (m_solverIslandCallbackMt)
{
m_solverIslandCallbackMt->~InplaceSolverIslandCallbackMt();
btAlignedFree(m_solverIslandCallbackMt);
}
if (m_ownsConstraintSolver)
{
m_constraintSolver->~btConstraintSolver();
btAlignedFree(m_constraintSolver);
}
}
void btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo)
{
BT_PROFILE("solveConstraints");
m_solverIslandCallbackMt->setup(&solverInfo, getDebugDrawer());
m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
/// solve all the constraints for this island
btSimulationIslandManagerMt* im = static_cast<btSimulationIslandManagerMt*>(m_islandManager);
im->buildAndProcessIslands( getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, m_solverIslandCallbackMt );
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
}

42
Engine/lib/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h Normal file
View file

@ -0,0 +1,42 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
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 BT_DISCRETE_DYNAMICS_WORLD_MT_H
#define BT_DISCRETE_DYNAMICS_WORLD_MT_H
#include "btDiscreteDynamicsWorld.h"
struct InplaceSolverIslandCallbackMt;
///
/// btDiscreteDynamicsWorldMt -- a version of DiscreteDynamicsWorld with some minor changes to support
/// solving simulation islands on multiple threads.
///
ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorldMt : public btDiscreteDynamicsWorld
{
protected:
InplaceSolverIslandCallbackMt* m_solverIslandCallbackMt;
virtual void solveConstraints(btContactSolverInfo& solverInfo);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
virtual ~btDiscreteDynamicsWorldMt();
};
#endif //BT_DISCRETE_DYNAMICS_WORLD_H

3
Engine/lib/bullet/src/BulletDynamics/Dynamics/btDynamicsWorld.h
View file

@ -34,7 +34,8 @@ enum btDynamicsWorldType
BT_DISCRETE_DYNAMICS_WORLD=2,
BT_CONTINUOUS_DYNAMICS_WORLD=3,
BT_SOFT_RIGID_DYNAMICS_WORLD=4,
BT_GPU_DYNAMICS_WORLD=5
BT_GPU_DYNAMICS_WORLD=5,
BT_SOFT_MULTIBODY_DYNAMICS_WORLD=6
};
///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc.

176
Engine/lib/bullet/src/BulletDynamics/Dynamics/btRigidBody.cpp
View file

@ -79,6 +79,8 @@ void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo&
//moved to btCollisionObject
m_friction = constructionInfo.m_friction;
m_rollingFriction = constructionInfo.m_rollingFriction;
m_spinningFriction = constructionInfo.m_spinningFriction;
m_restitution = constructionInfo.m_restitution;
setCollisionShape( constructionInfo.m_collisionShape );
@ -87,7 +89,7 @@ void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo&
setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia);
updateInertiaTensor();
m_rigidbodyFlags = 0;
m_rigidbodyFlags = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY;
m_deltaLinearVelocity.setZero();
@ -257,12 +259,41 @@ void btRigidBody::updateInertiaTensor()
}
btVector3 btRigidBody::computeGyroscopicForce(btScalar maxGyroscopicForce) const
btVector3 btRigidBody::getLocalInertia() const
{
btVector3 inertiaLocal;
inertiaLocal[0] = 1.f/getInvInertiaDiagLocal()[0];
inertiaLocal[1] = 1.f/getInvInertiaDiagLocal()[1];
inertiaLocal[2] = 1.f/getInvInertiaDiagLocal()[2];
const btVector3 inertia = m_invInertiaLocal;
inertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0),
inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0),
inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0));
return inertiaLocal;
}
inline btVector3 evalEulerEqn(const btVector3& w1, const btVector3& w0, const btVector3& T, const btScalar dt,
const btMatrix3x3 &I)
{
const btVector3 w2 = I*w1 + w1.cross(I*w1)*dt - (T*dt + I*w0);
return w2;
}
inline btMatrix3x3 evalEulerEqnDeriv(const btVector3& w1, const btVector3& w0, const btScalar dt,
const btMatrix3x3 &I)
{
btMatrix3x3 w1x, Iw1x;
const btVector3 Iwi = (I*w1);
w1.getSkewSymmetricMatrix(&w1x[0], &w1x[1], &w1x[2]);
Iwi.getSkewSymmetricMatrix(&Iw1x[0], &Iw1x[1], &Iw1x[2]);
const btMatrix3x3 dfw1 = I + (w1x*I - Iw1x)*dt;
return dfw1;
}
btVector3 btRigidBody::computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const
{
btVector3 inertiaLocal = getLocalInertia();
btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose();
btVector3 tmp = inertiaTensorWorld*getAngularVelocity();
btVector3 gf = getAngularVelocity().cross(tmp);
@ -274,6 +305,85 @@ btVector3 btRigidBody::computeGyroscopicForce(btScalar maxGyroscopicForce) const
return gf;
}
btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Body(btScalar step) const
{
btVector3 idl = getLocalInertia();
btVector3 omega1 = getAngularVelocity();
btQuaternion q = getWorldTransform().getRotation();
// Convert to body coordinates
btVector3 omegab = quatRotate(q.inverse(), omega1);
btMatrix3x3 Ib;
Ib.setValue(idl.x(),0,0,
0,idl.y(),0,
0,0,idl.z());
btVector3 ibo = Ib*omegab;
// Residual vector
btVector3 f = step * omegab.cross(ibo);
btMatrix3x3 skew0;
omegab.getSkewSymmetricMatrix(&skew0[0], &skew0[1], &skew0[2]);
btVector3 om = Ib*omegab;
btMatrix3x3 skew1;
om.getSkewSymmetricMatrix(&skew1[0],&skew1[1],&skew1[2]);
// Jacobian
btMatrix3x3 J = Ib + (skew0*Ib - skew1)*step;
// btMatrix3x3 Jinv = J.inverse();
// btVector3 omega_div = Jinv*f;
btVector3 omega_div = J.solve33(f);
// Single Newton-Raphson update
omegab = omegab - omega_div;//Solve33(J, f);
// Back to world coordinates
btVector3 omega2 = quatRotate(q,omegab);
btVector3 gf = omega2-omega1;
return gf;
}
btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) const
{
// use full newton-euler equations. common practice to drop the wxIw term. want it for better tumbling behavior.
// calculate using implicit euler step so it's stable.
const btVector3 inertiaLocal = getLocalInertia();
const btVector3 w0 = getAngularVelocity();
btMatrix3x3 I;
I = m_worldTransform.getBasis().scaled(inertiaLocal) *
m_worldTransform.getBasis().transpose();
// use newtons method to find implicit solution for new angular velocity (w')
// f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0
// df/dw' = I + 1xIw'*step + w'xI*step
btVector3 w1 = w0;
// one step of newton's method
{
const btVector3 fw = evalEulerEqn(w1, w0, btVector3(0, 0, 0), step, I);
const btMatrix3x3 dfw = evalEulerEqnDeriv(w1, w0, step, I);
btVector3 dw;
dw = dfw.solve33(fw);
//const btMatrix3x3 dfw_inv = dfw.inverse();
//dw = dfw_inv*fw;
w1 -= dw;
}
btVector3 gf = (w1 - w0);
return gf;
}
void btRigidBody::integrateVelocities(btScalar step)
{
if (isStaticOrKinematicObject())
@ -317,38 +427,50 @@ void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
}
bool btRigidBody::checkCollideWithOverride(const btCollisionObject* co) const
{
const btRigidBody* otherRb = btRigidBody::upcast(co);
if (!otherRb)
return true;
for (int i = 0; i < m_constraintRefs.size(); ++i)
{
const btTypedConstraint* c = m_constraintRefs[i];
if (c->isEnabled())
if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb)
return false;
}
return true;
}
void btRigidBody::addConstraintRef(btTypedConstraint* c)
{
int index = m_constraintRefs.findLinearSearch(c);
if (index == m_constraintRefs.size())
m_constraintRefs.push_back(c);
///disable collision with the 'other' body
m_checkCollideWith = true;
int index = m_constraintRefs.findLinearSearch(c);
//don't add constraints that are already referenced
//btAssert(index == m_constraintRefs.size());
if (index == m_constraintRefs.size())
{
m_constraintRefs.push_back(c);
btCollisionObject* colObjA = &c->getRigidBodyA();
btCollisionObject* colObjB = &c->getRigidBodyB();
if (colObjA == this)
{
colObjA->setIgnoreCollisionCheck(colObjB, true);
}
else
{
colObjB->setIgnoreCollisionCheck(colObjA, true);
}
}
}
void btRigidBody::removeConstraintRef(btTypedConstraint* c)
{
m_constraintRefs.remove(c);
m_checkCollideWith = m_constraintRefs.size() > 0;
int index = m_constraintRefs.findLinearSearch(c);
//don't remove constraints that are not referenced
if(index < m_constraintRefs.size())
{
m_constraintRefs.remove(c);
btCollisionObject* colObjA = &c->getRigidBodyA();
btCollisionObject* colObjB = &c->getRigidBodyB();
if (colObjA == this)
{
colObjA->setIgnoreCollisionCheck(colObjB, false);
}
else
{
colObjB->setIgnoreCollisionCheck(colObjA, false);
}
}
}
int btRigidBody::calculateSerializeBufferSize() const

31
Engine/lib/bullet/src/BulletDynamics/Dynamics/btRigidBody.h
View file

@ -41,10 +41,13 @@ extern bool gDisableDeactivation;
enum btRigidBodyFlags
{
BT_DISABLE_WORLD_GRAVITY = 1,
///The BT_ENABLE_GYROPSCOPIC_FORCE can easily introduce instability
///So generally it is best to not enable it.
///If really needed, run at a high frequency like 1000 Hertz: ///See Demos/GyroscopicDemo for an example use
BT_ENABLE_GYROPSCOPIC_FORCE = 2
///BT_ENABLE_GYROPSCOPIC_FORCE flags is enabled by default in Bullet 2.83 and onwards.
///and it BT_ENABLE_GYROPSCOPIC_FORCE becomes equivalent to BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY
///See Demos/GyroscopicDemo and computeGyroscopicImpulseImplicit
BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT = 2,
BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD=4,
BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY=8,
BT_ENABLE_GYROPSCOPIC_FORCE = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY,
};
@ -87,7 +90,7 @@ class btRigidBody : public btCollisionObject
//m_optionalMotionState allows to automatic synchronize the world transform for active objects
btMotionState* m_optionalMotionState;
//keep track of typed constraints referencing this rigid body
//keep track of typed constraints referencing this rigid body, to disable collision between linked bodies
btAlignedObjectArray<btTypedConstraint*> m_constraintRefs;
int m_rigidbodyFlags;
@ -132,6 +135,8 @@ public:
///the m_rollingFriction prevents rounded shapes, such as spheres, cylinders and capsules from rolling forever.
///See Bullet/Demos/RollingFrictionDemo for usage
btScalar m_rollingFriction;
btScalar m_spinningFriction;//torsional friction around contact normal
///best simulation results using zero restitution.
btScalar m_restitution;
@ -155,6 +160,7 @@ public:
m_angularDamping(btScalar(0.)),
m_friction(btScalar(0.5)),
m_rollingFriction(btScalar(0)),
m_spinningFriction(btScalar(0)),
m_restitution(btScalar(0.)),
m_linearSleepingThreshold(btScalar(0.8)),
m_angularSleepingThreshold(btScalar(1.f)),
@ -506,8 +512,6 @@ public:
return (getBroadphaseProxy() != 0);
}
virtual bool checkCollideWithOverride(const btCollisionObject* co) const;
void addConstraintRef(btTypedConstraint* c);
void removeConstraintRef(btTypedConstraint* c);
@ -531,7 +535,18 @@ public:
return m_rigidbodyFlags;
}
btVector3 computeGyroscopicForce(btScalar maxGyroscopicForce) const;
///perform implicit force computation in world space
btVector3 computeGyroscopicImpulseImplicit_World(btScalar dt) const;
///perform implicit force computation in body space (inertial frame)
btVector3 computeGyroscopicImpulseImplicit_Body(btScalar step) const;
///explicit version is best avoided, it gains energy
btVector3 computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const;
btVector3 getLocalInertia() const;
///////////////////////////////////////////////

641
Engine/lib/bullet/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp Normal file
View file

@ -0,0 +1,641 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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.
*/
#include "LinearMath/btScalar.h"
#include "btSimulationIslandManagerMt.h"
#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
//#include <stdio.h>
#include "LinearMath/btQuickprof.h"
SIMD_FORCE_INLINE int calcBatchCost( int bodies, int manifolds, int constraints )
{
// rough estimate of the cost of a batch, used for merging
int batchCost = bodies + 8 * manifolds + 4 * constraints;
return batchCost;
}
SIMD_FORCE_INLINE int calcBatchCost( const btSimulationIslandManagerMt::Island* island )
{
return calcBatchCost( island->bodyArray.size(), island->manifoldArray.size(), island->constraintArray.size() );
}
btSimulationIslandManagerMt::btSimulationIslandManagerMt()
{
m_minimumSolverBatchSize = calcBatchCost(0, 128, 0);
m_batchIslandMinBodyCount = 32;
m_islandDispatch = defaultIslandDispatch;
m_batchIsland = NULL;
}
btSimulationIslandManagerMt::~btSimulationIslandManagerMt()
{
for ( int i = 0; i < m_allocatedIslands.size(); ++i )
{
delete m_allocatedIslands[ i ];
}
m_allocatedIslands.resize( 0 );
m_activeIslands.resize( 0 );
m_freeIslands.resize( 0 );
}
inline int getIslandId(const btPersistentManifold* lhs)
{
const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
int islandId = rcolObj0->getIslandTag() >= 0 ? rcolObj0->getIslandTag() : rcolObj1->getIslandTag();
return islandId;
}
SIMD_FORCE_INLINE int btGetConstraintIslandId( const btTypedConstraint* lhs )
{
const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
int islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag();
return islandId;
}
/// function object that routes calls to operator<
class IslandBatchSizeSortPredicate
{
public:
bool operator() ( const btSimulationIslandManagerMt::Island* lhs, const btSimulationIslandManagerMt::Island* rhs ) const
{
int lCost = calcBatchCost( lhs );
int rCost = calcBatchCost( rhs );
return lCost > rCost;
}
};
class IslandBodyCapacitySortPredicate
{
public:
bool operator() ( const btSimulationIslandManagerMt::Island* lhs, const btSimulationIslandManagerMt::Island* rhs ) const
{
return lhs->bodyArray.capacity() > rhs->bodyArray.capacity();
}
};
void btSimulationIslandManagerMt::Island::append( const Island& other )
{
// append bodies
for ( int i = 0; i < other.bodyArray.size(); ++i )
{
bodyArray.push_back( other.bodyArray[ i ] );
}
// append manifolds
for ( int i = 0; i < other.manifoldArray.size(); ++i )
{
manifoldArray.push_back( other.manifoldArray[ i ] );
}
// append constraints
for ( int i = 0; i < other.constraintArray.size(); ++i )
{
constraintArray.push_back( other.constraintArray[ i ] );
}
}
bool btIsBodyInIsland( const btSimulationIslandManagerMt::Island& island, const btCollisionObject* obj )
{
for ( int i = 0; i < island.bodyArray.size(); ++i )
{
if ( island.bodyArray[ i ] == obj )
{
return true;
}
}
return false;
}
void btSimulationIslandManagerMt::initIslandPools()
{
// reset island pools
int numElem = getUnionFind().getNumElements();
m_lookupIslandFromId.resize( numElem );
for ( int i = 0; i < m_lookupIslandFromId.size(); ++i )
{
m_lookupIslandFromId[ i ] = NULL;
}
m_activeIslands.resize( 0 );
m_freeIslands.resize( 0 );
// check whether allocated islands are sorted by body capacity (largest to smallest)
int lastCapacity = 0;
bool isSorted = true;
for ( int i = 0; i < m_allocatedIslands.size(); ++i )
{
Island* island = m_allocatedIslands[ i ];
int cap = island->bodyArray.capacity();
if ( cap > lastCapacity )
{
isSorted = false;
break;
}
lastCapacity = cap;
}
if ( !isSorted )
{
m_allocatedIslands.quickSort( IslandBodyCapacitySortPredicate() );
}
m_batchIsland = NULL;
// mark all islands free (but avoid deallocation)
for ( int i = 0; i < m_allocatedIslands.size(); ++i )
{
Island* island = m_allocatedIslands[ i ];
island->bodyArray.resize( 0 );
island->manifoldArray.resize( 0 );
island->constraintArray.resize( 0 );
island->id = -1;
island->isSleeping = true;
m_freeIslands.push_back( island );
}
}
btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::getIsland( int id )
{
Island* island = m_lookupIslandFromId[ id ];
if ( island == NULL )
{
// search for existing island
for ( int i = 0; i < m_activeIslands.size(); ++i )
{
if ( m_activeIslands[ i ]->id == id )
{
island = m_activeIslands[ i ];
break;
}
}
m_lookupIslandFromId[ id ] = island;
}
return island;
}
btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::allocateIsland( int id, int numBodies )
{
Island* island = NULL;
int allocSize = numBodies;
if ( numBodies < m_batchIslandMinBodyCount )
{
if ( m_batchIsland )
{
island = m_batchIsland;
m_lookupIslandFromId[ id ] = island;
// if we've made a large enough batch,
if ( island->bodyArray.size() + numBodies >= m_batchIslandMinBodyCount )
{
// next time start a new batch
m_batchIsland = NULL;
}
return island;
}
else
{
// need to allocate a batch island
allocSize = m_batchIslandMinBodyCount * 2;
}
}
btAlignedObjectArray<Island*>& freeIslands = m_freeIslands;
// search for free island
if ( freeIslands.size() > 0 )
{
// try to reuse a previously allocated island
int iFound = freeIslands.size();
// linear search for smallest island that can hold our bodies
for ( int i = freeIslands.size() - 1; i >= 0; --i )
{
if ( freeIslands[ i ]->bodyArray.capacity() >= allocSize )
{
iFound = i;
island = freeIslands[ i ];
island->id = id;
break;
}
}
// if found, shrink array while maintaining ordering
if ( island )
{
int iDest = iFound;
int iSrc = iDest + 1;
while ( iSrc < freeIslands.size() )
{
freeIslands[ iDest++ ] = freeIslands[ iSrc++ ];
}
freeIslands.pop_back();
}
}
if ( island == NULL )
{
// no free island found, allocate
island = new Island(); // TODO: change this to use the pool allocator
island->id = id;
island->bodyArray.reserve( allocSize );
m_allocatedIslands.push_back( island );
}
m_lookupIslandFromId[ id ] = island;
if ( numBodies < m_batchIslandMinBodyCount )
{
m_batchIsland = island;
}
m_activeIslands.push_back( island );
return island;
}
void btSimulationIslandManagerMt::buildIslands( btDispatcher* dispatcher, btCollisionWorld* collisionWorld )
{
BT_PROFILE("islandUnionFindAndQuickSort");
btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
//we are going to sort the unionfind array, and store the element id in the size
//afterwards, we clean unionfind, to make sure no-one uses it anymore
getUnionFind().sortIslands();
int numElem = getUnionFind().getNumElements();
int endIslandIndex=1;
int startIslandIndex;
//update the sleeping state for bodies, if all are sleeping
for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
{
int islandId = getUnionFind().getElement(startIslandIndex).m_id;
for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
{
}
//int numSleeping = 0;
bool allSleeping = true;
int idx;
for (idx=startIslandIndex;idx<endIslandIndex;idx++)
{
int i = getUnionFind().getElement(idx).m_sz;
btCollisionObject* colObj0 = collisionObjects[i];
if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
{
// printf("error in island management\n");
}
btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
if (colObj0->getIslandTag() == islandId)
{
if (colObj0->getActivationState()== ACTIVE_TAG)
{
allSleeping = false;
}
if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
{
allSleeping = false;
}
}
}
if (allSleeping)
{
int idx;
for (idx=startIslandIndex;idx<endIslandIndex;idx++)
{
int i = getUnionFind().getElement(idx).m_sz;
btCollisionObject* colObj0 = collisionObjects[i];
if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
{
// printf("error in island management\n");
}
btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
if (colObj0->getIslandTag() == islandId)
{
colObj0->setActivationState( ISLAND_SLEEPING );
}
}
} else
{
int idx;
for (idx=startIslandIndex;idx<endIslandIndex;idx++)
{
int i = getUnionFind().getElement(idx).m_sz;
btCollisionObject* colObj0 = collisionObjects[i];
if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
{
// printf("error in island management\n");
}
btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
if (colObj0->getIslandTag() == islandId)
{
if ( colObj0->getActivationState() == ISLAND_SLEEPING)
{
colObj0->setActivationState( WANTS_DEACTIVATION);
colObj0->setDeactivationTime(0.f);
}
}
}
}
}
}
void btSimulationIslandManagerMt::addBodiesToIslands( btCollisionWorld* collisionWorld )
{
btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
int endIslandIndex = 1;
int startIslandIndex;
int numElem = getUnionFind().getNumElements();
// create explicit islands and add bodies to each
for ( startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex )
{
int islandId = getUnionFind().getElement( startIslandIndex ).m_id;
// find end index
for ( endIslandIndex = startIslandIndex; ( endIslandIndex < numElem ) && ( getUnionFind().getElement( endIslandIndex ).m_id == islandId ); endIslandIndex++ )
{
}
// check if island is sleeping
bool islandSleeping = true;
for ( int iElem = startIslandIndex; iElem < endIslandIndex; iElem++ )
{
int i = getUnionFind().getElement( iElem ).m_sz;
btCollisionObject* colObj = collisionObjects[ i ];
if ( colObj->isActive() )
{
islandSleeping = false;
}
}
if ( !islandSleeping )
{
// want to count the number of bodies before allocating the island to optimize memory usage of the Island structures
int numBodies = endIslandIndex - startIslandIndex;
Island* island = allocateIsland( islandId, numBodies );
island->isSleeping = false;
// add bodies to island
for ( int iElem = startIslandIndex; iElem < endIslandIndex; iElem++ )
{
int i = getUnionFind().getElement( iElem ).m_sz;
btCollisionObject* colObj = collisionObjects[ i ];
island->bodyArray.push_back( colObj );
}
}
}
}
void btSimulationIslandManagerMt::addManifoldsToIslands( btDispatcher* dispatcher )
{
// walk all the manifolds, activating bodies touched by kinematic objects, and add each manifold to its Island
int maxNumManifolds = dispatcher->getNumManifolds();
for ( int i = 0; i < maxNumManifolds; i++ )
{
btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal( i );
const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>( manifold->getBody0() );
const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>( manifold->getBody1() );
///@todo: check sleeping conditions!
if ( ( ( colObj0 ) && colObj0->getActivationState() != ISLAND_SLEEPING ) ||
( ( colObj1 ) && colObj1->getActivationState() != ISLAND_SLEEPING ) )
{
//kinematic objects don't merge islands, but wake up all connected objects
if ( colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING )
{
if ( colObj0->hasContactResponse() )
colObj1->activate();
}
if ( colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING )
{
if ( colObj1->hasContactResponse() )
colObj0->activate();
}
//filtering for response
if ( dispatcher->needsResponse( colObj0, colObj1 ) )
{
// scatter manifolds into various islands
int islandId = getIslandId( manifold );
// if island not sleeping,
if ( Island* island = getIsland( islandId ) )
{
island->manifoldArray.push_back( manifold );
}
}
}
}
}
void btSimulationIslandManagerMt::addConstraintsToIslands( btAlignedObjectArray<btTypedConstraint*>& constraints )
{
// walk constraints
for ( int i = 0; i < constraints.size(); i++ )
{
// scatter constraints into various islands
btTypedConstraint* constraint = constraints[ i ];
if ( constraint->isEnabled() )
{
int islandId = btGetConstraintIslandId( constraint );
// if island is not sleeping,
if ( Island* island = getIsland( islandId ) )
{
island->constraintArray.push_back( constraint );
}
}
}
}
void btSimulationIslandManagerMt::mergeIslands()
{
// sort islands in order of decreasing batch size
m_activeIslands.quickSort( IslandBatchSizeSortPredicate() );
// merge small islands to satisfy minimum batch size
// find first small batch island
int destIslandIndex = m_activeIslands.size();
for ( int i = 0; i < m_activeIslands.size(); ++i )
{
Island* island = m_activeIslands[ i ];
int batchSize = calcBatchCost( island );
if ( batchSize < m_minimumSolverBatchSize )
{
destIslandIndex = i;
break;
}
}
int lastIndex = m_activeIslands.size() - 1;
while ( destIslandIndex < lastIndex )
{
// merge islands from the back of the list
Island* island = m_activeIslands[ destIslandIndex ];
int numBodies = island->bodyArray.size();
int numManifolds = island->manifoldArray.size();
int numConstraints = island->constraintArray.size();
int firstIndex = lastIndex;
// figure out how many islands we want to merge and find out how many bodies, manifolds and constraints we will have
while ( true )
{
Island* src = m_activeIslands[ firstIndex ];
numBodies += src->bodyArray.size();
numManifolds += src->manifoldArray.size();
numConstraints += src->constraintArray.size();
int batchCost = calcBatchCost( numBodies, numManifolds, numConstraints );
if ( batchCost >= m_minimumSolverBatchSize )
{
break;
}
if ( firstIndex - 1 == destIslandIndex )
{
break;
}
firstIndex--;
}
// reserve space for these pointers to minimize reallocation
island->bodyArray.reserve( numBodies );
island->manifoldArray.reserve( numManifolds );
island->constraintArray.reserve( numConstraints );
// merge islands
for ( int i = firstIndex; i <= lastIndex; ++i )
{
island->append( *m_activeIslands[ i ] );
}
// shrink array to exclude the islands that were merged from
m_activeIslands.resize( firstIndex );
lastIndex = firstIndex - 1;
destIslandIndex++;
}
}
void btSimulationIslandManagerMt::defaultIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback )
{
// serial dispatch
btAlignedObjectArray<Island*>& islands = *islandsPtr;
for ( int i = 0; i < islands.size(); ++i )
{
Island* island = islands[ i ];
btPersistentManifold** manifolds = island->manifoldArray.size() ? &island->manifoldArray[ 0 ] : NULL;
btTypedConstraint** constraintsPtr = island->constraintArray.size() ? &island->constraintArray[ 0 ] : NULL;
callback->processIsland( &island->bodyArray[ 0 ],
island->bodyArray.size(),
manifolds,
island->manifoldArray.size(),
constraintsPtr,
island->constraintArray.size(),
island->id
);
}
}
///@todo: this is random access, it can be walked 'cache friendly'!
void btSimulationIslandManagerMt::buildAndProcessIslands( btDispatcher* dispatcher,
btCollisionWorld* collisionWorld,
btAlignedObjectArray<btTypedConstraint*>& constraints,
IslandCallback* callback
)
{
btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
buildIslands(dispatcher,collisionWorld);
BT_PROFILE("processIslands");
if(!getSplitIslands())
{
btPersistentManifold** manifolds = dispatcher->getInternalManifoldPointer();
int maxNumManifolds = dispatcher->getNumManifolds();
for ( int i = 0; i < maxNumManifolds; i++ )
{
btPersistentManifold* manifold = manifolds[ i ];
const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>( manifold->getBody0() );
const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>( manifold->getBody1() );
///@todo: check sleeping conditions!
if ( ( ( colObj0 ) && colObj0->getActivationState() != ISLAND_SLEEPING ) ||
( ( colObj1 ) && colObj1->getActivationState() != ISLAND_SLEEPING ) )
{
//kinematic objects don't merge islands, but wake up all connected objects
if ( colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING )
{
if ( colObj0->hasContactResponse() )
colObj1->activate();
}
if ( colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING )
{
if ( colObj1->hasContactResponse() )
colObj0->activate();
}
}
}
btTypedConstraint** constraintsPtr = constraints.size() ? &constraints[ 0 ] : NULL;
callback->processIsland(&collisionObjects[0],
collisionObjects.size(),
manifolds,
maxNumManifolds,
constraintsPtr,
constraints.size(),
-1
);
}
else
{
initIslandPools();
//traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
addBodiesToIslands( collisionWorld );
addManifoldsToIslands( dispatcher );
addConstraintsToIslands( constraints );
// m_activeIslands array should now contain all non-sleeping Islands, and each Island should
// have all the necessary bodies, manifolds and constraints.
// if we want to merge islands with small batch counts,
if ( m_minimumSolverBatchSize > 1 )
{
mergeIslands();
}
// dispatch islands to solver
m_islandDispatch( &m_activeIslands, callback );
}
}

109
Engine/lib/bullet/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h Normal file
View file

@ -0,0 +1,109 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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 BT_SIMULATION_ISLAND_MANAGER_MT_H
#define BT_SIMULATION_ISLAND_MANAGER_MT_H
#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
class btTypedConstraint;
///
/// SimulationIslandManagerMt -- Multithread capable version of SimulationIslandManager
/// Splits the world up into islands which can be solved in parallel.
/// In order to solve islands in parallel, an IslandDispatch function
/// must be provided which will dispatch calls to multiple threads.
/// The amount of parallelism that can be achieved depends on the number
/// of islands. If only a single island exists, then no parallelism is
/// possible.
///
class btSimulationIslandManagerMt : public btSimulationIslandManager
{
public:
struct Island
{
// a simulation island consisting of bodies, manifolds and constraints,
// to be passed into a constraint solver.
btAlignedObjectArray<btCollisionObject*> bodyArray;
btAlignedObjectArray<btPersistentManifold*> manifoldArray;
btAlignedObjectArray<btTypedConstraint*> constraintArray;
int id; // island id
bool isSleeping;
void append( const Island& other ); // add bodies, manifolds, constraints to my own
};
struct IslandCallback
{
virtual ~IslandCallback() {};
virtual void processIsland( btCollisionObject** bodies,
int numBodies,
btPersistentManifold** manifolds,
int numManifolds,
btTypedConstraint** constraints,
int numConstraints,
int islandId
) = 0;
};
typedef void( *IslandDispatchFunc ) ( btAlignedObjectArray<Island*>* islands, IslandCallback* callback );
static void defaultIslandDispatch( btAlignedObjectArray<Island*>* islands, IslandCallback* callback );
protected:
btAlignedObjectArray<Island*> m_allocatedIslands; // owner of all Islands
btAlignedObjectArray<Island*> m_activeIslands; // islands actively in use
btAlignedObjectArray<Island*> m_freeIslands; // islands ready to be reused
btAlignedObjectArray<Island*> m_lookupIslandFromId; // big lookup table to map islandId to Island pointer
Island* m_batchIsland;
int m_minimumSolverBatchSize;
int m_batchIslandMinBodyCount;
IslandDispatchFunc m_islandDispatch;
Island* getIsland( int id );
virtual Island* allocateIsland( int id, int numBodies );
virtual void initIslandPools();
virtual void addBodiesToIslands( btCollisionWorld* collisionWorld );
virtual void addManifoldsToIslands( btDispatcher* dispatcher );
virtual void addConstraintsToIslands( btAlignedObjectArray<btTypedConstraint*>& constraints );
virtual void mergeIslands();
public:
btSimulationIslandManagerMt();
virtual ~btSimulationIslandManagerMt();
virtual void buildAndProcessIslands( btDispatcher* dispatcher, btCollisionWorld* collisionWorld, btAlignedObjectArray<btTypedConstraint*>& constraints, IslandCallback* callback );
virtual void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld);
int getMinimumSolverBatchSize() const
{
return m_minimumSolverBatchSize;
}
void setMinimumSolverBatchSize( int sz )
{
m_minimumSolverBatchSize = sz;
}
IslandDispatchFunc getIslandDispatchFunction() const
{
return m_islandDispatch;
}
// allow users to set their own dispatch function for multithreaded dispatch
void setIslandDispatchFunction( IslandDispatchFunc func )
{
m_islandDispatch = func;
}
};
#endif //BT_SIMULATION_ISLAND_MANAGER_H

1982
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp Normal file
View file

File diff suppressed because it is too large Load diff

797
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBody.h Normal file
View file

@ -0,0 +1,797 @@
/*
* PURPOSE:
* Class representing an articulated rigid body. Stores the body's
* current state, allows forces and torques to be set, handles
* timestepping and implements Featherstone's algorithm.
*
* COPYRIGHT:
* Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013
* Portions written By Erwin Coumans: connection to LCP solver, various multibody constraints, replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
* Portions written By Jakub Stepien: support for multi-DOF constraints, introduction of spatial algebra and several other improvements
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 BT_MULTIBODY_H
#define BT_MULTIBODY_H
#include "LinearMath/btScalar.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btQuaternion.h"
#include "LinearMath/btMatrix3x3.h"
#include "LinearMath/btAlignedObjectArray.h"
///serialization data, don't change them if you are not familiar with the details of the serialization mechanisms
#ifdef BT_USE_DOUBLE_PRECISION
#define btMultiBodyData btMultiBodyDoubleData
#define btMultiBodyDataName "btMultiBodyDoubleData"
#define btMultiBodyLinkData btMultiBodyLinkDoubleData
#define btMultiBodyLinkDataName "btMultiBodyLinkDoubleData"
#else
#define btMultiBodyData btMultiBodyFloatData
#define btMultiBodyDataName "btMultiBodyFloatData"
#define btMultiBodyLinkData btMultiBodyLinkFloatData
#define btMultiBodyLinkDataName "btMultiBodyLinkFloatData"
#endif //BT_USE_DOUBLE_PRECISION
#include "btMultiBodyLink.h"
class btMultiBodyLinkCollider;
ATTRIBUTE_ALIGNED16(class) btMultiBody
{
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
//
// initialization
//
btMultiBody(int n_links, // NOT including the base
btScalar mass, // mass of base
const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal
bool fixedBase, // whether the base is fixed (true) or can move (false)
bool canSleep, bool deprecatedMultiDof=true);
virtual ~btMultiBody();
//note: fixed link collision with parent is always disabled
void setupFixed(int linkIndex,
btScalar mass,
const btVector3 &inertia,
int parent,
const btQuaternion &rotParentToThis,
const btVector3 &parentComToThisPivotOffset,
const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision=true);
void setupPrismatic(int i,
btScalar mass,
const btVector3 &inertia,
int parent,
const btQuaternion &rotParentToThis,
const btVector3 &jointAxis,
const btVector3 &parentComToThisPivotOffset,
const btVector3 &thisPivotToThisComOffset,
bool disableParentCollision);
void setupRevolute(int linkIndex, // 0 to num_links-1
btScalar mass,
const btVector3 &inertia,
int parentIndex,
const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0
const btVector3 &jointAxis, // in my frame
const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame
const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame
bool disableParentCollision=false);
void setupSpherical(int linkIndex, // 0 to num_links-1
btScalar mass,
const btVector3 &inertia,
int parent,
const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0
const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame
const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame
bool disableParentCollision=false);
void setupPlanar(int i, // 0 to num_links-1
btScalar mass,
const btVector3 &inertia,
int parent,
const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0
const btVector3 &rotationAxis,
const btVector3 &parentComToThisComOffset, // vector from parent COM to this COM, in PARENT frame
bool disableParentCollision=false);
const btMultibodyLink& getLink(int index) const
{
return m_links[index];
}
btMultibodyLink& getLink(int index)
{
return m_links[index];
}
void setBaseCollider(btMultiBodyLinkCollider* collider)//collider can be NULL to disable collision for the base
{
m_baseCollider = collider;
}
const btMultiBodyLinkCollider* getBaseCollider() const
{
return m_baseCollider;
}
btMultiBodyLinkCollider* getBaseCollider()
{
return m_baseCollider;
}
//
// get parent
// input: link num from 0 to num_links-1
// output: link num from 0 to num_links-1, OR -1 to mean the base.
//
int getParent(int link_num) const;
//
// get number of m_links, masses, moments of inertia
//
int getNumLinks() const { return m_links.size(); }
int getNumDofs() const { return m_dofCount; }
int getNumPosVars() const { return m_posVarCnt; }
btScalar getBaseMass() const { return m_baseMass; }
const btVector3 & getBaseInertia() const { return m_baseInertia; }
btScalar getLinkMass(int i) const;
const btVector3 & getLinkInertia(int i) const;
//
// change mass (incomplete: can only change base mass and inertia at present)
//
void setBaseMass(btScalar mass) { m_baseMass = mass; }
void setBaseInertia(const btVector3 &inertia) { m_baseInertia = inertia; }
//
// get/set pos/vel/rot/omega for the base link
//
const btVector3 & getBasePos() const { return m_basePos; } // in world frame
const btVector3 getBaseVel() const
{
return btVector3(m_realBuf[3],m_realBuf[4],m_realBuf[5]);
} // in world frame
const btQuaternion & getWorldToBaseRot() const
{
return m_baseQuat;
} // rotates world vectors into base frame
btVector3 getBaseOmega() const { return btVector3(m_realBuf[0],m_realBuf[1],m_realBuf[2]); } // in world frame
void setBasePos(const btVector3 &pos)
{
m_basePos = pos;
}
void setBaseWorldTransform(const btTransform& tr)
{
setBasePos(tr.getOrigin());
setWorldToBaseRot(tr.getRotation().inverse());
}
btTransform getBaseWorldTransform() const
{
btTransform tr;
tr.setOrigin(getBasePos());
tr.setRotation(getWorldToBaseRot().inverse());
return tr;
}
void setBaseVel(const btVector3 &vel)
{
m_realBuf[3]=vel[0]; m_realBuf[4]=vel[1]; m_realBuf[5]=vel[2];
}
void setWorldToBaseRot(const btQuaternion &rot)
{
m_baseQuat = rot; //m_baseQuat asumed to ba alias!?
}
void setBaseOmega(const btVector3 &omega)
{
m_realBuf[0]=omega[0];
m_realBuf[1]=omega[1];
m_realBuf[2]=omega[2];
}
//
// get/set pos/vel for child m_links (i = 0 to num_links-1)
//
btScalar getJointPos(int i) const;
btScalar getJointVel(int i) const;
btScalar * getJointVelMultiDof(int i);
btScalar * getJointPosMultiDof(int i);
const btScalar * getJointVelMultiDof(int i) const ;
const btScalar * getJointPosMultiDof(int i) const ;
void setJointPos(int i, btScalar q);
void setJointVel(int i, btScalar qdot);
void setJointPosMultiDof(int i, btScalar *q);
void setJointVelMultiDof(int i, btScalar *qdot);
//
// direct access to velocities as a vector of 6 + num_links elements.
// (omega first, then v, then joint velocities.)
//
const btScalar * getVelocityVector() const
{
return &m_realBuf[0];
}
/* btScalar * getVelocityVector()
{
return &real_buf[0];
}
*/
//
// get the frames of reference (positions and orientations) of the child m_links
// (i = 0 to num_links-1)
//
const btVector3 & getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords
const btQuaternion & getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i.
//
// transform vectors in local frame of link i to world frame (or vice versa)
//
btVector3 localPosToWorld(int i, const btVector3 &vec) const;
btVector3 localDirToWorld(int i, const btVector3 &vec) const;
btVector3 worldPosToLocal(int i, const btVector3 &vec) const;
btVector3 worldDirToLocal(int i, const btVector3 &vec) const;
//
// transform a frame in local coordinate to a frame in world coordinate
//
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const;
//
// calculate kinetic energy and angular momentum
// useful for debugging.
//
btScalar getKineticEnergy() const;
btVector3 getAngularMomentum() const;
//
// set external forces and torques. Note all external forces/torques are given in the WORLD frame.
//
void clearForcesAndTorques();
void clearConstraintForces();
void clearVelocities();
void addBaseForce(const btVector3 &f)
{
m_baseForce += f;
}
void addBaseTorque(const btVector3 &t) { m_baseTorque += t; }
void addLinkForce(int i, const btVector3 &f);
void addLinkTorque(int i, const btVector3 &t);
void addBaseConstraintForce(const btVector3 &f)
{
m_baseConstraintForce += f;
}
void addBaseConstraintTorque(const btVector3 &t) { m_baseConstraintTorque += t; }
void addLinkConstraintForce(int i, const btVector3 &f);
void addLinkConstraintTorque(int i, const btVector3 &t);
void addJointTorque(int i, btScalar Q);
void addJointTorqueMultiDof(int i, int dof, btScalar Q);
void addJointTorqueMultiDof(int i, const btScalar *Q);
const btVector3 & getBaseForce() const { return m_baseForce; }
const btVector3 & getBaseTorque() const { return m_baseTorque; }
const btVector3 & getLinkForce(int i) const;
const btVector3 & getLinkTorque(int i) const;
btScalar getJointTorque(int i) const;
btScalar * getJointTorqueMultiDof(int i);
//
// dynamics routines.
//
// timestep the velocities (given the external forces/torques set using addBaseForce etc).
// also sets up caches for calcAccelerationDeltas.
//
// Note: the caller must provide three vectors which are used as
// temporary scratch space. The idea here is to reduce dynamic
// memory allocation: the same scratch vectors can be re-used
// again and again for different Multibodies, instead of each
// btMultiBody allocating (and then deallocating) their own
// individual scratch buffers. This gives a considerable speed
// improvement, at least on Windows (where dynamic memory
// allocation appears to be fairly slow).
//
void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m,
bool isConstraintPass=false
);
///stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instead
void stepVelocitiesMultiDof(btScalar dt,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m,
bool isConstraintPass=false)
{
computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt,scratch_r,scratch_v,scratch_m,isConstraintPass);
}
// calcAccelerationDeltasMultiDof
// input: force vector (in same format as jacobian, i.e.:
// 3 torque values, 3 force values, num_links joint torque values)
// output: 3 omegadot values, 3 vdot values, num_links q_double_dot values
// (existing contents of output array are replaced)
// calcAccelerationDeltasMultiDof must have been called first.
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v) const;
void applyDeltaVeeMultiDof2(const btScalar * delta_vee, btScalar multiplier)
{
for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
{
m_deltaV[dof] += delta_vee[dof] * multiplier;
}
}
void processDeltaVeeMultiDof2()
{
applyDeltaVeeMultiDof(&m_deltaV[0],1);
for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
{
m_deltaV[dof] = 0.f;
}
}
void applyDeltaVeeMultiDof(const btScalar * delta_vee, btScalar multiplier)
{
//for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
// printf("%.4f ", delta_vee[dof]*multiplier);
//printf("\n");
//btScalar sum = 0;
//for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
//{
// sum += delta_vee[dof]*multiplier*delta_vee[dof]*multiplier;
//}
//btScalar l = btSqrt(sum);
//if (l>m_maxAppliedImpulse)
//{
// multiplier *= m_maxAppliedImpulse/l;
//}
for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
{
m_realBuf[dof] += delta_vee[dof] * multiplier;
btClamp(m_realBuf[dof],-m_maxCoordinateVelocity,m_maxCoordinateVelocity);
}
}
// timestep the positions (given current velocities).
void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
//
// contacts
//
// This routine fills out a contact constraint jacobian for this body.
// the 'normal' supplied must be -n for body1 or +n for body2 of the contact.
// 'normal' & 'contact_point' are both given in world coordinates.
void fillContactJacobianMultiDof(int link,
const btVector3 &contact_point,
const btVector3 &normal,
btScalar *jac,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m) const { fillConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); }
//a more general version of fillContactJacobianMultiDof which does not assume..
//.. that the constraint in question is contact or, to be more precise, constrains linear velocity only
void fillConstraintJacobianMultiDof(int link,
const btVector3 &contact_point,
const btVector3 &normal_ang,
const btVector3 &normal_lin,
btScalar *jac,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
//
// sleeping
//
void setCanSleep(bool canSleep)
{
m_canSleep = canSleep;
}
bool getCanSleep()const
{
return m_canSleep;
}
bool isAwake() const { return m_awake; }
void wakeUp();
void goToSleep();
void checkMotionAndSleepIfRequired(btScalar timestep);
bool hasFixedBase() const
{
return m_fixedBase;
}
int getCompanionId() const
{
return m_companionId;
}
void setCompanionId(int id)
{
//printf("for %p setCompanionId(%d)\n",this, id);
m_companionId = id;
}
void setNumLinks(int numLinks)//careful: when changing the number of m_links, make sure to re-initialize or update existing m_links
{
m_links.resize(numLinks);
}
btScalar getLinearDamping() const
{
return m_linearDamping;
}
void setLinearDamping( btScalar damp)
{
m_linearDamping = damp;
}
btScalar getAngularDamping() const
{
return m_angularDamping;
}
void setAngularDamping( btScalar damp)
{
m_angularDamping = damp;
}
bool getUseGyroTerm() const
{
return m_useGyroTerm;
}
void setUseGyroTerm(bool useGyro)
{
m_useGyroTerm = useGyro;
}
btScalar getMaxCoordinateVelocity() const
{
return m_maxCoordinateVelocity ;
}
void setMaxCoordinateVelocity(btScalar maxVel)
{
m_maxCoordinateVelocity = maxVel;
}
btScalar getMaxAppliedImpulse() const
{
return m_maxAppliedImpulse;
}
void setMaxAppliedImpulse(btScalar maxImp)
{
m_maxAppliedImpulse = maxImp;
}
void setHasSelfCollision(bool hasSelfCollision)
{
m_hasSelfCollision = hasSelfCollision;
}
bool hasSelfCollision() const
{
return m_hasSelfCollision;
}
void finalizeMultiDof();
void useRK4Integration(bool use) { m_useRK4 = use; }
bool isUsingRK4Integration() const { return m_useRK4; }
void useGlobalVelocities(bool use) { m_useGlobalVelocities = use; }
bool isUsingGlobalVelocities() const { return m_useGlobalVelocities; }
bool isPosUpdated() const
{
return __posUpdated;
}
void setPosUpdated(bool updated)
{
__posUpdated = updated;
}
//internalNeedsJointFeedback is for internal use only
bool internalNeedsJointFeedback() const
{
return m_internalNeedsJointFeedback;
}
void forwardKinematics(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);
void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
const char* getBaseName() const
{
return m_baseName;
}
///memory of setBaseName needs to be manager by user
void setBaseName(const char* name)
{
m_baseName = name;
}
///users can point to their objects, userPointer is not used by Bullet
void* getUserPointer() const
{
return m_userObjectPointer;
}
int getUserIndex() const
{
return m_userIndex;
}
int getUserIndex2() const
{
return m_userIndex2;
}
///users can point to their objects, userPointer is not used by Bullet
void setUserPointer(void* userPointer)
{
m_userObjectPointer = userPointer;
}
///users can point to their objects, userPointer is not used by Bullet
void setUserIndex(int index)
{
m_userIndex = index;
}
void setUserIndex2(int index)
{
m_userIndex2 = index;
}
private:
btMultiBody(const btMultiBody &); // not implemented
void operator=(const btMultiBody &); // not implemented
void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const;
void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const;
void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const;
void updateLinksDofOffsets()
{
int dofOffset = 0, cfgOffset = 0;
for(int bidx = 0; bidx < m_links.size(); ++bidx)
{
m_links[bidx].m_dofOffset = dofOffset; m_links[bidx].m_cfgOffset = cfgOffset;
dofOffset += m_links[bidx].m_dofCount; cfgOffset += m_links[bidx].m_posVarCount;
}
}
void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
private:
btMultiBodyLinkCollider* m_baseCollider;//can be NULL
const char* m_baseName;//memory needs to be manager by user!
btVector3 m_basePos; // position of COM of base (world frame)
btQuaternion m_baseQuat; // rotates world points into base frame
btScalar m_baseMass; // mass of the base
btVector3 m_baseInertia; // inertia of the base (in local frame; diagonal)
btVector3 m_baseForce; // external force applied to base. World frame.
btVector3 m_baseTorque; // external torque applied to base. World frame.
btVector3 m_baseConstraintForce; // external force applied to base. World frame.
btVector3 m_baseConstraintTorque; // external torque applied to base. World frame.
btAlignedObjectArray<btMultibodyLink> m_links; // array of m_links, excluding the base. index from 0 to num_links-1.
btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders;
//
// realBuf:
// offset size array
// 0 6 + num_links v (base_omega; base_vel; joint_vels) MULTIDOF [sysdof x sysdof for D matrices (TOO MUCH!) + pos_delta which is sys-cfg sized]
// 6+num_links num_links D
//
// vectorBuf:
// offset size array
// 0 num_links h_top
// num_links num_links h_bottom
//
// matrixBuf:
// offset size array
// 0 num_links+1 rot_from_parent
//
btAlignedObjectArray<btScalar> m_deltaV;
btAlignedObjectArray<btScalar> m_realBuf;
btAlignedObjectArray<btVector3> m_vectorBuf;
btAlignedObjectArray<btMatrix3x3> m_matrixBuf;
btMatrix3x3 m_cachedInertiaTopLeft;
btMatrix3x3 m_cachedInertiaTopRight;
btMatrix3x3 m_cachedInertiaLowerLeft;
btMatrix3x3 m_cachedInertiaLowerRight;
bool m_cachedInertiaValid;
bool m_fixedBase;
// Sleep parameters.
bool m_awake;
bool m_canSleep;
btScalar m_sleepTimer;
void* m_userObjectPointer;
int m_userIndex2;
int m_userIndex;
int m_companionId;
btScalar m_linearDamping;
btScalar m_angularDamping;
bool m_useGyroTerm;
btScalar m_maxAppliedImpulse;
btScalar m_maxCoordinateVelocity;
bool m_hasSelfCollision;
bool __posUpdated;
int m_dofCount, m_posVarCnt;
bool m_useRK4, m_useGlobalVelocities;
///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only
bool m_internalNeedsJointFeedback;
};
struct btMultiBodyLinkDoubleData
{
btQuaternionDoubleData m_zeroRotParentToThis;
btVector3DoubleData m_parentComToThisComOffset;
btVector3DoubleData m_thisPivotToThisComOffset;
btVector3DoubleData m_jointAxisTop[6];
btVector3DoubleData m_jointAxisBottom[6];
btVector3DoubleData m_linkInertia; // inertia of the base (in local frame; diagonal)
double m_linkMass;
int m_parentIndex;
int m_jointType;
int m_dofCount;
int m_posVarCount;
double m_jointPos[7];
double m_jointVel[6];
double m_jointTorque[6];
double m_jointDamping;
double m_jointFriction;
char *m_linkName;
char *m_jointName;
btCollisionObjectDoubleData *m_linkCollider;
char *m_paddingPtr;
};
struct btMultiBodyLinkFloatData
{
btQuaternionFloatData m_zeroRotParentToThis;
btVector3FloatData m_parentComToThisComOffset;
btVector3FloatData m_thisPivotToThisComOffset;
btVector3FloatData m_jointAxisTop[6];
btVector3FloatData m_jointAxisBottom[6];
btVector3FloatData m_linkInertia; // inertia of the base (in local frame; diagonal)
int m_dofCount;
float m_linkMass;
int m_parentIndex;
int m_jointType;
float m_jointPos[7];
float m_jointVel[6];
float m_jointTorque[6];
int m_posVarCount;
float m_jointDamping;
float m_jointFriction;
char *m_linkName;
char *m_jointName;
btCollisionObjectFloatData *m_linkCollider;
char *m_paddingPtr;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btMultiBodyDoubleData
{
btTransformDoubleData m_baseWorldTransform;
btVector3DoubleData m_baseInertia; // inertia of the base (in local frame; diagonal)
double m_baseMass;
char *m_baseName;
btMultiBodyLinkDoubleData *m_links;
btCollisionObjectDoubleData *m_baseCollider;
char *m_paddingPtr;
int m_numLinks;
char m_padding[4];
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btMultiBodyFloatData
{
char *m_baseName;
btMultiBodyLinkFloatData *m_links;
btCollisionObjectFloatData *m_baseCollider;
btTransformFloatData m_baseWorldTransform;
btVector3FloatData m_baseInertia; // inertia of the base (in local frame; diagonal)
float m_baseMass;
int m_numLinks;
};
#endif

417
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp Normal file
View file

@ -0,0 +1,417 @@
#include "btMultiBodyConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
:m_bodyA(bodyA),
m_bodyB(bodyB),
m_linkA(linkA),
m_linkB(linkB),
m_numRows(numRows),
m_jacSizeA(0),
m_jacSizeBoth(0),
m_isUnilateral(isUnilateral),
m_numDofsFinalized(-1),
m_maxAppliedImpulse(100)
{
}
void btMultiBodyConstraint::updateJacobianSizes()
{
if(m_bodyA)
{
m_jacSizeA = (6 + m_bodyA->getNumDofs());
}
if(m_bodyB)
{
m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs();
}
else
m_jacSizeBoth = m_jacSizeA;
}
void btMultiBodyConstraint::allocateJacobiansMultiDof()
{
updateJacobianSizes();
m_posOffset = ((1 + m_jacSizeBoth)*m_numRows);
m_data.resize((2 + m_jacSizeBoth) * m_numRows);
}
btMultiBodyConstraint::~btMultiBodyConstraint()
{
}
void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
{
for (int i = 0; i < ndof; ++i)
data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
}
btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint,
btMultiBodyJacobianData& data,
btScalar* jacOrgA, btScalar* jacOrgB,
const btVector3& constraintNormalAng,
const btVector3& constraintNormalLin,
const btVector3& posAworld, const btVector3& posBworld,
btScalar posError,
const btContactSolverInfo& infoGlobal,
btScalar lowerLimit, btScalar upperLimit,
bool angConstraint,
btScalar relaxation,
bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
{
solverConstraint.m_multiBodyA = m_bodyA;
solverConstraint.m_multiBodyB = m_bodyB;
solverConstraint.m_linkA = m_linkA;
solverConstraint.m_linkB = m_linkB;
btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
if (bodyA)
rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
if (bodyB)
rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
if (multiBodyA)
{
if (solverConstraint.m_linkA<0)
{
rel_pos1 = posAworld - multiBodyA->getBasePos();
} else
{
rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
}
const int ndofA = multiBodyA->getNumDofs() + 6;
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
if (solverConstraint.m_deltaVelAindex <0)
{
solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
} else
{
btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
}
//determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom
//resize..
solverConstraint.m_jacAindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
//copy/determine
if(jacOrgA)
{
for (int i=0;i<ndofA;i++)
data.m_jacobians[solverConstraint.m_jacAindex+i] = jacOrgA[i];
}
else
{
btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
//multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalAng, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
}
//determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
//resize..
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
//determine..
multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
btVector3 torqueAxis0;
if (angConstraint) {
torqueAxis0 = constraintNormalAng;
}
else {
torqueAxis0 = rel_pos1.cross(constraintNormalLin);
}
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_contactNormal1 = constraintNormalLin;
}
else //if(rb0)
{
btVector3 torqueAxis0;
if (angConstraint) {
torqueAxis0 = constraintNormalAng;
}
else {
torqueAxis0 = rel_pos1.cross(constraintNormalLin);
}
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_contactNormal1 = constraintNormalLin;
}
if (multiBodyB)
{
if (solverConstraint.m_linkB<0)
{
rel_pos2 = posBworld - multiBodyB->getBasePos();
} else
{
rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
}
const int ndofB = multiBodyB->getNumDofs() + 6;
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
if (solverConstraint.m_deltaVelBindex <0)
{
solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
}
//determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
//resize..
solverConstraint.m_jacBindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
//copy/determine..
if(jacOrgB)
{
for (int i=0;i<ndofB;i++)
data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i];
}
else
{
//multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalAng, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
}
//determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
//resize..
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
//determine..
multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
btVector3 torqueAxis1;
if (angConstraint) {
torqueAxis1 = constraintNormalAng;
}
else {
torqueAxis1 = rel_pos2.cross(constraintNormalLin);
}
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_contactNormal2 = -constraintNormalLin;
}
else //if(rb1)
{
btVector3 torqueAxis1;
if (angConstraint) {
torqueAxis1 = constraintNormalAng;
}
else {
torqueAxis1 = rel_pos2.cross(constraintNormalLin);
}
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_contactNormal2 = -constraintNormalLin;
}
{
btVector3 vec;
btScalar denom0 = 0.f;
btScalar denom1 = 0.f;
btScalar* jacB = 0;
btScalar* jacA = 0;
btScalar* deltaVelA = 0;
btScalar* deltaVelB = 0;
int ndofA = 0;
//determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
if (multiBodyA)
{
ndofA = multiBodyA->getNumDofs() + 6;
jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA; ++i)
{
btScalar j = jacA[i] ;
btScalar l = deltaVelA[i];
denom0 += j*l;
}
}
else if(rb0)
{
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
if (angConstraint) {
denom0 = rb0->getInvMass() + constraintNormalAng.dot(vec);
}
else {
denom0 = rb0->getInvMass() + constraintNormalLin.dot(vec);
}
}
//
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumDofs() + 6;
jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB; ++i)
{
btScalar j = jacB[i] ;
btScalar l = deltaVelB[i];
denom1 += j*l;
}
}
else if(rb1)
{
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
if (angConstraint) {
denom1 = rb1->getInvMass() + constraintNormalAng.dot(vec);
}
else {
denom1 = rb1->getInvMass() + constraintNormalLin.dot(vec);
}
}
//
btScalar d = denom0+denom1;
if (d>SIMD_EPSILON)
{
solverConstraint.m_jacDiagABInv = relaxation/(d);
}
else
{
//disable the constraint row to handle singularity/redundant constraint
solverConstraint.m_jacDiagABInv = 0.f;
}
}
//compute rhs and remaining solverConstraint fields
btScalar penetration = isFriction? 0 : posError;
btScalar rel_vel = 0.f;
int ndofA = 0;
int ndofB = 0;
{
btVector3 vel1,vel2;
if (multiBodyA)
{
ndofA = multiBodyA->getNumDofs() + 6;
btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA ; ++i)
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
}
else if(rb0)
{
rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
}
if (multiBodyB)
{
ndofB = multiBodyB->getNumDofs() + 6;
btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB ; ++i)
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
}
else if(rb1)
{
rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
}
solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
}
///warm starting (or zero if disabled)
/*
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
if (solverConstraint.m_appliedImpulse)
{
if (multiBodyA)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
multiBodyA->applyDeltaVee(deltaV,impulse);
applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
} else
{
if (rb0)
bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
}
if (multiBodyB)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
multiBodyB->applyDeltaVee(deltaV,impulse);
applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
} else
{
if (rb1)
bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
}
}
} else
*/
solverConstraint.m_appliedImpulse = 0.f;
solverConstraint.m_appliedPushImpulse = 0.f;
{
btScalar positionalError = 0.f;
btScalar velocityError = desiredVelocity - rel_vel;// * damping;
btScalar erp = infoGlobal.m_erp2;
//split impulse is not implemented yet for btMultiBody*
//if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
erp = infoGlobal.m_erp;
}
positionalError = -penetration * erp/infoGlobal.m_timeStep;
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
//split impulse is not implemented yet for btMultiBody*
// if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
//combine position and velocity into rhs
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
solverConstraint.m_rhsPenetration = 0.f;
}
/*else
{
//split position and velocity into rhs and m_rhsPenetration
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_rhsPenetration = penetrationImpulse;
}
*/
solverConstraint.m_cfm = 0.f;
solverConstraint.m_lowerLimit = lowerLimit;
solverConstraint.m_upperLimit = upperLimit;
}
return rel_vel;
}

183
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h Normal file
View file

@ -0,0 +1,183 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
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 BT_MULTIBODY_CONSTRAINT_H
#define BT_MULTIBODY_CONSTRAINT_H
#include "LinearMath/btScalar.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "btMultiBody.h"
class btMultiBody;
struct btSolverInfo;
#include "btMultiBodySolverConstraint.h"
struct btMultiBodyJacobianData
{
btAlignedObjectArray<btScalar> m_jacobians;
btAlignedObjectArray<btScalar> m_deltaVelocitiesUnitImpulse; //holds the joint-space response of the corresp. tree to the test impulse in each constraint space dimension
btAlignedObjectArray<btScalar> m_deltaVelocities; //holds joint-space vectors of all the constrained trees accumulating the effect of corrective impulses applied in SI
btAlignedObjectArray<btScalar> scratch_r;
btAlignedObjectArray<btVector3> scratch_v;
btAlignedObjectArray<btMatrix3x3> scratch_m;
btAlignedObjectArray<btSolverBody>* m_solverBodyPool;
int m_fixedBodyId;
};
class btMultiBodyConstraint
{
protected:
btMultiBody* m_bodyA;
btMultiBody* m_bodyB;
int m_linkA;
int m_linkB;
int m_numRows;
int m_jacSizeA;
int m_jacSizeBoth;
int m_posOffset;
bool m_isUnilateral;
int m_numDofsFinalized;
btScalar m_maxAppliedImpulse;
// warning: the data block lay out is not consistent for all constraints
// data block laid out as follows:
// cached impulses. (one per row.)
// jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc)
// positions. (one per row.)
btAlignedObjectArray<btScalar> m_data;
void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof);
btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint,
btMultiBodyJacobianData& data,
btScalar* jacOrgA, btScalar* jacOrgB,
const btVector3& constraintNormalAng,
const btVector3& constraintNormalLin,
const btVector3& posAworld, const btVector3& posBworld,
btScalar posError,
const btContactSolverInfo& infoGlobal,
btScalar lowerLimit, btScalar upperLimit,
bool angConstraint = false,
btScalar relaxation = 1.f,
bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0);
public:
btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral);
virtual ~btMultiBodyConstraint();
void updateJacobianSizes();
void allocateJacobiansMultiDof();
virtual void finalizeMultiDof()=0;
virtual int getIslandIdA() const =0;
virtual int getIslandIdB() const =0;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)=0;
int getNumRows() const
{
return m_numRows;
}
btMultiBody* getMultiBodyA()
{
return m_bodyA;
}
btMultiBody* getMultiBodyB()
{
return m_bodyB;
}
void internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
{
btAssert(dof>=0);
btAssert(dof < getNumRows());
m_data[dof] = appliedImpulse;
}
btScalar getAppliedImpulse(int dof)
{
btAssert(dof>=0);
btAssert(dof < getNumRows());
return m_data[dof];
}
// current constraint position
// constraint is pos >= 0 for unilateral, or pos = 0 for bilateral
// NOTE: ignored position for friction rows.
btScalar getPosition(int row) const
{
return m_data[m_posOffset + row];
}
void setPosition(int row, btScalar pos)
{
m_data[m_posOffset + row] = pos;
}
bool isUnilateral() const
{
return m_isUnilateral;
}
// jacobian blocks.
// each of size 6 + num_links. (jacobian2 is null if no body2.)
// format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients.
btScalar* jacobianA(int row)
{
return &m_data[m_numRows + row * m_jacSizeBoth];
}
const btScalar* jacobianA(int row) const
{
return &m_data[m_numRows + (row * m_jacSizeBoth)];
}
btScalar* jacobianB(int row)
{
return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
}
const btScalar* jacobianB(int row) const
{
return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
}
btScalar getMaxAppliedImpulse() const
{
return m_maxAppliedImpulse;
}
void setMaxAppliedImpulse(btScalar maxImp)
{
m_maxAppliedImpulse = maxImp;
}
virtual void debugDraw(class btIDebugDraw* drawer)=0;
};
#endif //BT_MULTIBODY_CONSTRAINT_H

1382
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp Normal file
View file

File diff suppressed because it is too large Load diff

100
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h Normal file
View file

@ -0,0 +1,100 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
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 BT_MULTIBODY_CONSTRAINT_SOLVER_H
#define BT_MULTIBODY_CONSTRAINT_SOLVER_H
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
#include "btMultiBodySolverConstraint.h"
#define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
class btMultiBody;
#include "btMultiBodyConstraint.h"
ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver
{
protected:
btMultiBodyConstraintArray m_multiBodyNonContactConstraints;
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints;
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints;
btMultiBodyJacobianData m_data;
//temp storage for multi body constraints for a specific island/group called by 'solveGroup'
btMultiBodyConstraint** m_tmpMultiBodyConstraints;
int m_tmpNumMultiBodyConstraints;
btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal);
btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);
btMultiBodySolverConstraint& addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,
btScalar combinedTorsionalFriction,
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);
void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow,
btScalar* jacA,btScalar* jacB,
btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
const btContactSolverInfo& infoGlobal);
void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint,
const btVector3& contactNormal,
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
btScalar& relaxation,
bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
//either rolling or spinning friction
void setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint,
const btVector3& contactNormal,
btManifoldPoint& cp,
btScalar combinedTorsionalFriction,
const btContactSolverInfo& infoGlobal,
btScalar& relaxation,
bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
void convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
// virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
void applyDeltaVee(btScalar* deltaV, btScalar impulse, int velocityIndex, int ndof);
void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& constraint, btScalar deltaTime);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
///this method should not be called, it was just used during porting/integration of Featherstone btMultiBody, providing backwards compatibility but no support for btMultiBodyConstraint (only contact constraints)
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
virtual void solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
};
#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H

989
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp Normal file
View file

@ -0,0 +1,989 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
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.
*/
#include "btMultiBodyDynamicsWorld.h"
#include "btMultiBodyConstraintSolver.h"
#include "btMultiBody.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
#include "LinearMath/btQuickprof.h"
#include "btMultiBodyConstraint.h"
#include "LinearMath/btIDebugDraw.h"
#include "LinearMath/btSerializer.h"
void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask)
{
m_multiBodies.push_back(body);
}
void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body)
{
m_multiBodies.remove(body);
}
void btMultiBodyDynamicsWorld::calculateSimulationIslands()
{
BT_PROFILE("calculateSimulationIslands");
getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
{
//merge islands based on speculative contact manifolds too
for (int i=0;i<this->m_predictiveManifolds.size();i++)
{
btPersistentManifold* manifold = m_predictiveManifolds[i];
const btCollisionObject* colObj0 = manifold->getBody0();
const btCollisionObject* colObj1 = manifold->getBody1();
if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
{
getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
}
}
}
{
int i;
int numConstraints = int(m_constraints.size());
for (i=0;i< numConstraints ; i++ )
{
btTypedConstraint* constraint = m_constraints[i];
if (constraint->isEnabled())
{
const btRigidBody* colObj0 = &constraint->getRigidBodyA();
const btRigidBody* colObj1 = &constraint->getRigidBodyB();
if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
{
getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
}
}
}
}
//merge islands linked by Featherstone link colliders
for (int i=0;i<m_multiBodies.size();i++)
{
btMultiBody* body = m_multiBodies[i];
{
btMultiBodyLinkCollider* prev = body->getBaseCollider();
for (int b=0;b<body->getNumLinks();b++)
{
btMultiBodyLinkCollider* cur = body->getLink(b).m_collider;
if (((cur) && (!(cur)->isStaticOrKinematicObject())) &&
((prev) && (!(prev)->isStaticOrKinematicObject())))
{
int tagPrev = prev->getIslandTag();
int tagCur = cur->getIslandTag();
getSimulationIslandManager()->getUnionFind().unite(tagPrev, tagCur);
}
if (cur && !cur->isStaticOrKinematicObject())
prev = cur;
}
}
}
//merge islands linked by multibody constraints
{
for (int i=0;i<this->m_multiBodyConstraints.size();i++)
{
btMultiBodyConstraint* c = m_multiBodyConstraints[i];
int tagA = c->getIslandIdA();
int tagB = c->getIslandIdB();
if (tagA>=0 && tagB>=0)
getSimulationIslandManager()->getUnionFind().unite(tagA, tagB);
}
}
//Store the island id in each body
getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
}
void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
{
BT_PROFILE("btMultiBodyDynamicsWorld::updateActivationState");
for ( int i=0;i<m_multiBodies.size();i++)
{
btMultiBody* body = m_multiBodies[i];
if (body)
{
body->checkMotionAndSleepIfRequired(timeStep);
if (!body->isAwake())
{
btMultiBodyLinkCollider* col = body->getBaseCollider();
if (col && col->getActivationState() == ACTIVE_TAG)
{
col->setActivationState( WANTS_DEACTIVATION);
col->setDeactivationTime(0.f);
}
for (int b=0;b<body->getNumLinks();b++)
{
btMultiBodyLinkCollider* col = body->getLink(b).m_collider;
if (col && col->getActivationState() == ACTIVE_TAG)
{
col->setActivationState( WANTS_DEACTIVATION);
col->setDeactivationTime(0.f);
}
}
} else
{
btMultiBodyLinkCollider* col = body->getBaseCollider();
if (col && col->getActivationState() != DISABLE_DEACTIVATION)
col->setActivationState( ACTIVE_TAG );
for (int b=0;b<body->getNumLinks();b++)
{
btMultiBodyLinkCollider* col = body->getLink(b).m_collider;
if (col && col->getActivationState() != DISABLE_DEACTIVATION)
col->setActivationState( ACTIVE_TAG );
}
}
}
}
btDiscreteDynamicsWorld::updateActivationState(timeStep);
}
SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs)
{
int islandId;
const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
return islandId;
}
class btSortConstraintOnIslandPredicate2
{
public:
bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) const
{
int rIslandId0,lIslandId0;
rIslandId0 = btGetConstraintIslandId2(rhs);
lIslandId0 = btGetConstraintIslandId2(lhs);
return lIslandId0 < rIslandId0;
}
};
SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs)
{
int islandId;
int islandTagA = lhs->getIslandIdA();
int islandTagB = lhs->getIslandIdB();
islandId= islandTagA>=0?islandTagA:islandTagB;
return islandId;
}
class btSortMultiBodyConstraintOnIslandPredicate
{
public:
bool operator() ( const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs ) const
{
int rIslandId0,lIslandId0;
rIslandId0 = btGetMultiBodyConstraintIslandId(rhs);
lIslandId0 = btGetMultiBodyConstraintIslandId(lhs);
return lIslandId0 < rIslandId0;
}
};
struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
{
btContactSolverInfo* m_solverInfo;
btMultiBodyConstraintSolver* m_solver;
btMultiBodyConstraint** m_multiBodySortedConstraints;
int m_numMultiBodyConstraints;
btTypedConstraint** m_sortedConstraints;
int m_numConstraints;
btIDebugDraw* m_debugDrawer;
btDispatcher* m_dispatcher;
btAlignedObjectArray<btCollisionObject*> m_bodies;
btAlignedObjectArray<btPersistentManifold*> m_manifolds;
btAlignedObjectArray<btTypedConstraint*> m_constraints;
btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
MultiBodyInplaceSolverIslandCallback( btMultiBodyConstraintSolver* solver,
btDispatcher* dispatcher)
:m_solverInfo(NULL),
m_solver(solver),
m_multiBodySortedConstraints(NULL),
m_numConstraints(0),
m_debugDrawer(NULL),
m_dispatcher(dispatcher)
{
}
MultiBodyInplaceSolverIslandCallback& operator=(MultiBodyInplaceSolverIslandCallback& other)
{
btAssert(0);
(void)other;
return *this;
}
SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer)
{
btAssert(solverInfo);
m_solverInfo = solverInfo;
m_multiBodySortedConstraints = sortedMultiBodyConstraints;
m_numMultiBodyConstraints = numMultiBodyConstraints;
m_sortedConstraints = sortedConstraints;
m_numConstraints = numConstraints;
m_debugDrawer = debugDrawer;
m_bodies.resize (0);
m_manifolds.resize (0);
m_constraints.resize (0);
m_multiBodyConstraints.resize(0);
}
virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId)
{
if (islandId<0)
{
///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
m_solver->solveMultiBodyGroup( bodies,numBodies,manifolds, numManifolds,m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
} else
{
//also add all non-contact constraints/joints for this island
btTypedConstraint** startConstraint = 0;
btMultiBodyConstraint** startMultiBodyConstraint = 0;
int numCurConstraints = 0;
int numCurMultiBodyConstraints = 0;
int i;
//find the first constraint for this island
for (i=0;i<m_numConstraints;i++)
{
if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
{
startConstraint = &m_sortedConstraints[i];
break;
}
}
//count the number of constraints in this island
for (;i<m_numConstraints;i++)
{
if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
{
numCurConstraints++;
}
}
for (i=0;i<m_numMultiBodyConstraints;i++)
{
if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
{
startMultiBodyConstraint = &m_multiBodySortedConstraints[i];
break;
}
}
//count the number of multi body constraints in this island
for (;i<m_numMultiBodyConstraints;i++)
{
if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
{
numCurMultiBodyConstraints++;
}
}
if (m_solverInfo->m_minimumSolverBatchSize<=1)
{
m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
} else
{
for (i=0;i<numBodies;i++)
m_bodies.push_back(bodies[i]);
for (i=0;i<numManifolds;i++)
m_manifolds.push_back(manifolds[i]);
for (i=0;i<numCurConstraints;i++)
m_constraints.push_back(startConstraint[i]);
for (i=0;i<numCurMultiBodyConstraints;i++)
m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]);
if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
{
processConstraints();
} else
{
//printf("deferred\n");
}
}
}
}
void processConstraints()
{
btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
//printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher);
m_bodies.resize(0);
m_manifolds.resize(0);
m_constraints.resize(0);
m_multiBodyConstraints.resize(0);
}
};
btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration),
m_multiBodyConstraintSolver(constraintSolver)
{
//split impulse is not yet supported for Featherstone hierarchies
// getSolverInfo().m_splitImpulse = false;
getSolverInfo().m_solverMode |=SOLVER_USE_2_FRICTION_DIRECTIONS;
m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver,dispatcher);
}
btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld ()
{
delete m_solverMultiBodyIslandCallback;
}
void btMultiBodyDynamicsWorld::forwardKinematics()
{
for (int b=0;b<m_multiBodies.size();b++)
{
btMultiBody* bod = m_multiBodies[b];
bod->forwardKinematics(m_scratch_world_to_local,m_scratch_local_origin);
}
}
void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
forwardKinematics();
BT_PROFILE("solveConstraints");
m_sortedConstraints.resize( m_constraints.size());
int i;
for (i=0;i<getNumConstraints();i++)
{
m_sortedConstraints[i] = m_constraints[i];
}
m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
for (i=0;i<m_multiBodyConstraints.size();i++)
{
m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
}
m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer());
m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
/// solve all the constraints for this island
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback);
#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
{
BT_PROFILE("btMultiBody addForce");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
m_scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
m_scratch_v.resize(bod->getNumLinks()+1);
m_scratch_m.resize(bod->getNumLinks()+1);
bod->addBaseForce(m_gravity * bod->getBaseMass());
for (int j = 0; j < bod->getNumLinks(); ++j)
{
bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
}
}//if (!isSleeping)
}
}
#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
{
BT_PROFILE("btMultiBody stepVelocities");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
m_scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
m_scratch_v.resize(bod->getNumLinks()+1);
m_scratch_m.resize(bod->getNumLinks()+1);
bool doNotUpdatePos = false;
{
if(!bod->isUsingRK4Integration())
{
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m);
}
else
{
//
int numDofs = bod->getNumDofs() + 6;
int numPosVars = bod->getNumPosVars() + 7;
btAlignedObjectArray<btScalar> scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs);
//convenience
btScalar *pMem = &scratch_r2[0];
btScalar *scratch_q0 = pMem; pMem += numPosVars;
btScalar *scratch_qx = pMem; pMem += numPosVars;
btScalar *scratch_qd0 = pMem; pMem += numDofs;
btScalar *scratch_qd1 = pMem; pMem += numDofs;
btScalar *scratch_qd2 = pMem; pMem += numDofs;
btScalar *scratch_qd3 = pMem; pMem += numDofs;
btScalar *scratch_qdd0 = pMem; pMem += numDofs;
btScalar *scratch_qdd1 = pMem; pMem += numDofs;
btScalar *scratch_qdd2 = pMem; pMem += numDofs;
btScalar *scratch_qdd3 = pMem; pMem += numDofs;
btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]);
/////
//copy q0 to scratch_q0 and qd0 to scratch_qd0
scratch_q0[0] = bod->getWorldToBaseRot().x();
scratch_q0[1] = bod->getWorldToBaseRot().y();
scratch_q0[2] = bod->getWorldToBaseRot().z();
scratch_q0[3] = bod->getWorldToBaseRot().w();
scratch_q0[4] = bod->getBasePos().x();
scratch_q0[5] = bod->getBasePos().y();
scratch_q0[6] = bod->getBasePos().z();
//
for(int link = 0; link < bod->getNumLinks(); ++link)
{
for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];
}
//
for(int dof = 0; dof < numDofs; ++dof)
scratch_qd0[dof] = bod->getVelocityVector()[dof];
////
struct
{
btMultiBody *bod;
btScalar *scratch_qx, *scratch_q0;
void operator()()
{
for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
scratch_qx[dof] = scratch_q0[dof];
}
} pResetQx = {bod, scratch_qx, scratch_q0};
//
struct
{
void operator()(btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size)
{
for(int i = 0; i < size; ++i)
pVal[i] = pCurVal[i] + dt * pDer[i];
}
} pEulerIntegrate;
//
struct
{
void operator()(btMultiBody *pBody, const btScalar *pData)
{
btScalar *pVel = const_cast<btScalar*>(pBody->getVelocityVector());
for(int i = 0; i < pBody->getNumDofs() + 6; ++i)
pVel[i] = pData[i];
}
} pCopyToVelocityVector;
//
struct
{
void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size)
{
for(int i = 0; i < size; ++i)
pDst[i] = pSrc[start + i];
}
} pCopy;
//
btScalar h = solverInfo.m_timeStep;
#define output &m_scratch_r[bod->getNumDofs()]
//calc qdd0 from: q0 & qd0
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
pCopy(output, scratch_qdd0, 0, numDofs);
//calc q1 = q0 + h/2 * qd0
pResetQx();
bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0);
//calc qd1 = qd0 + h/2 * qdd0
pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
//
//calc qdd1 from: q1 & qd1
pCopyToVelocityVector(bod, scratch_qd1);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
pCopy(output, scratch_qdd1, 0, numDofs);
//calc q2 = q0 + h/2 * qd1
pResetQx();
bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1);
//calc qd2 = qd0 + h/2 * qdd1
pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
//
//calc qdd2 from: q2 & qd2
pCopyToVelocityVector(bod, scratch_qd2);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
pCopy(output, scratch_qdd2, 0, numDofs);
//calc q3 = q0 + h * qd2
pResetQx();
bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
//calc qd3 = qd0 + h * qdd2
pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
//
//calc qdd3 from: q3 & qd3
pCopyToVelocityVector(bod, scratch_qd3);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
pCopy(output, scratch_qdd3, 0, numDofs);
//
//calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
//calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
btAlignedObjectArray<btScalar> delta_q; delta_q.resize(numDofs);
btAlignedObjectArray<btScalar> delta_qd; delta_qd.resize(numDofs);
for(int i = 0; i < numDofs; ++i)
{
delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]);
delta_qd[i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]);
//delta_q[i] = h*scratch_qd0[i];
//delta_qd[i] = h*scratch_qdd0[i];
}
//
pCopyToVelocityVector(bod, scratch_qd0);
bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
//
if(!doNotUpdatePos)
{
btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
for(int i = 0; i < numDofs; ++i)
pRealBuf[i] = delta_q[i];
//bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
bod->setPosUpdated(true);
}
//ugly hack which resets the cached data to t0 (needed for constraint solver)
{
for(int link = 0; link < bod->getNumLinks(); ++link)
bod->getLink(link).updateCacheMultiDof();
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m);
}
}
}
#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
bod->clearForcesAndTorques();
#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
}//if (!isSleeping)
}
}
clearMultiBodyConstraintForces();
m_solverMultiBodyIslandCallback->processConstraints();
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
{
BT_PROFILE("btMultiBody stepVelocities");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
m_scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
m_scratch_v.resize(bod->getNumLinks()+1);
m_scratch_m.resize(bod->getNumLinks()+1);
{
if(!bod->isUsingRK4Integration())
{
bool isConstraintPass = true;
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass);
}
}
}
}
}
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bod->processDeltaVeeMultiDof2();
}
}
void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
{
btDiscreteDynamicsWorld::integrateTransforms(timeStep);
{
BT_PROFILE("btMultiBody stepPositions");
//integrate and update the Featherstone hierarchies
for (int b=0;b<m_multiBodies.size();b++)
{
btMultiBody* bod = m_multiBodies[b];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
int nLinks = bod->getNumLinks();
///base + num m_links
{
if(!bod->isPosUpdated())
bod->stepPositionsMultiDof(timeStep);
else
{
btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
bod->stepPositionsMultiDof(1, 0, pRealBuf);
bod->setPosUpdated(false);
}
}
m_scratch_world_to_local.resize(nLinks+1);
m_scratch_local_origin.resize(nLinks+1);
bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local,m_scratch_local_origin);
} else
{
bod->clearVelocities();
}
}
}
}
void btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint)
{
m_multiBodyConstraints.push_back(constraint);
}
void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint)
{
m_multiBodyConstraints.remove(constraint);
}
void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint)
{
constraint->debugDraw(getDebugDrawer());
}
void btMultiBodyDynamicsWorld::debugDrawWorld()
{
BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
bool drawConstraints = false;
if (getDebugDrawer())
{
int mode = getDebugDrawer()->getDebugMode();
if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
{
drawConstraints = true;
}
if (drawConstraints)
{
BT_PROFILE("btMultiBody debugDrawWorld");
for (int c=0;c<m_multiBodyConstraints.size();c++)
{
btMultiBodyConstraint* constraint = m_multiBodyConstraints[c];
debugDrawMultiBodyConstraint(constraint);
}
for (int b = 0; b<m_multiBodies.size(); b++)
{
btMultiBody* bod = m_multiBodies[b];
bod->forwardKinematics(m_scratch_world_to_local1,m_scratch_local_origin1);
getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
for (int m = 0; m<bod->getNumLinks(); m++)
{
const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
getDebugDrawer()->drawTransform(tr, 0.1);
//draw the joint axis
if (bod->getLink(m).m_jointType==btMultibodyLink::eRevolute)
{
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec);
btVector4 color(0,0,0,1);//1,1,1);
btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
getDebugDrawer()->drawLine(from,to,color);
}
if (bod->getLink(m).m_jointType==btMultibodyLink::eFixed)
{
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
btVector4 color(0,0,0,1);//1,1,1);
btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
getDebugDrawer()->drawLine(from,to,color);
}
if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic)
{
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
btVector4 color(0,0,0,1);//1,1,1);
btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
getDebugDrawer()->drawLine(from,to,color);
}
}
}
}
}
btDiscreteDynamicsWorld::debugDrawWorld();
}
void btMultiBodyDynamicsWorld::applyGravity()
{
btDiscreteDynamicsWorld::applyGravity();
#ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
BT_PROFILE("btMultiBody addGravity");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
bod->addBaseForce(m_gravity * bod->getBaseMass());
for (int j = 0; j < bod->getNumLinks(); ++j)
{
bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
}
}//if (!isSleeping)
}
#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
}
void btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces()
{
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bod->clearConstraintForces();
}
}
void btMultiBodyDynamicsWorld::clearMultiBodyForces()
{
{
BT_PROFILE("clearMultiBodyForces");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
btMultiBody* bod = m_multiBodies[i];
bod->clearForcesAndTorques();
}
}
}
}
void btMultiBodyDynamicsWorld::clearForces()
{
btDiscreteDynamicsWorld::clearForces();
#ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
clearMultiBodyForces();
#endif
}
void btMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
{
serializer->startSerialization();
serializeDynamicsWorldInfo( serializer);
serializeMultiBodies(serializer);
serializeRigidBodies(serializer);
serializeCollisionObjects(serializer);
serializer->finishSerialization();
}
void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer)
{
int i;
//serialize all collision objects
for (i=0;i<m_multiBodies.size();i++)
{
btMultiBody* mb = m_multiBodies[i];
{
int len = mb->calculateSerializeBufferSize();
btChunk* chunk = serializer->allocate(len,1);
const char* structType = mb->serialize(chunk->m_oldPtr, serializer);
serializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb);
}
}
}

109
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h Normal file
View file

@ -0,0 +1,109 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
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 BT_MULTIBODY_DYNAMICS_WORLD_H
#define BT_MULTIBODY_DYNAMICS_WORLD_H
#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
#define BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
class btMultiBody;
class btMultiBodyConstraint;
class btMultiBodyConstraintSolver;
struct MultiBodyInplaceSolverIslandCallback;
///The btMultiBodyDynamicsWorld adds Featherstone multi body dynamics to Bullet
///This implementation is still preliminary/experimental.
class btMultiBodyDynamicsWorld : public btDiscreteDynamicsWorld
{
protected:
btAlignedObjectArray<btMultiBody*> m_multiBodies;
btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
btAlignedObjectArray<btMultiBodyConstraint*> m_sortedMultiBodyConstraints;
btMultiBodyConstraintSolver* m_multiBodyConstraintSolver;
MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback;
//cached data to avoid memory allocations
btAlignedObjectArray<btQuaternion> m_scratch_world_to_local;
btAlignedObjectArray<btVector3> m_scratch_local_origin;
btAlignedObjectArray<btQuaternion> m_scratch_world_to_local1;
btAlignedObjectArray<btVector3> m_scratch_local_origin1;
btAlignedObjectArray<btScalar> m_scratch_r;
btAlignedObjectArray<btVector3> m_scratch_v;
btAlignedObjectArray<btMatrix3x3> m_scratch_m;
virtual void calculateSimulationIslands();
virtual void updateActivationState(btScalar timeStep);
virtual void solveConstraints(btContactSolverInfo& solverInfo);
virtual void serializeMultiBodies(btSerializer* serializer);
public:
btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
virtual ~btMultiBodyDynamicsWorld ();
virtual void addMultiBody(btMultiBody* body, short group= btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter);
virtual void removeMultiBody(btMultiBody* body);
virtual int getNumMultibodies() const
{
return m_multiBodies.size();
}
btMultiBody* getMultiBody(int mbIndex)
{
return m_multiBodies[mbIndex];
}
virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint);
virtual int getNumMultiBodyConstraints() const
{
return m_multiBodyConstraints.size();
}
virtual btMultiBodyConstraint* getMultiBodyConstraint( int constraintIndex)
{
return m_multiBodyConstraints[constraintIndex];
}
virtual const btMultiBodyConstraint* getMultiBodyConstraint( int constraintIndex) const
{
return m_multiBodyConstraints[constraintIndex];
}
virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
virtual void integrateTransforms(btScalar timeStep);
virtual void debugDrawWorld();
virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);
void forwardKinematics();
virtual void clearForces();
virtual void clearMultiBodyConstraintForces();
virtual void clearMultiBodyForces();
virtual void applyGravity();
virtual void serialize(btSerializer* serializer);
};
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H

211
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp Normal file
View file

@ -0,0 +1,211 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
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.
*/
///This file was written by Erwin Coumans
#include "btMultiBodyFixedConstraint.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
#include "LinearMath/btIDebugDraw.h"
#define BTMBFIXEDCONSTRAINT_DIM 6
btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
:btMultiBodyConstraint(body,0,link,-1,BTMBFIXEDCONSTRAINT_DIM,false),
m_rigidBodyA(0),
m_rigidBodyB(bodyB),
m_pivotInA(pivotInA),
m_pivotInB(pivotInB),
m_frameInA(frameInA),
m_frameInB(frameInB)
{
m_data.resize(BTMBFIXEDCONSTRAINT_DIM);//at least store the applied impulses
}
btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBFIXEDCONSTRAINT_DIM,false),
m_rigidBodyA(0),
m_rigidBodyB(0),
m_pivotInA(pivotInA),
m_pivotInB(pivotInB),
m_frameInA(frameInA),
m_frameInB(frameInB)
{
m_data.resize(BTMBFIXEDCONSTRAINT_DIM);//at least store the applied impulses
}
void btMultiBodyFixedConstraint::finalizeMultiDof()
{
//not implemented yet
btAssert(0);
}
btMultiBodyFixedConstraint::~btMultiBodyFixedConstraint()
{
}
int btMultiBodyFixedConstraint::getIslandIdA() const
{
if (m_rigidBodyA)
return m_rigidBodyA->getIslandTag();
if (m_bodyA)
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
}
}
return -1;
}
int btMultiBodyFixedConstraint::getIslandIdB() const
{
if (m_rigidBodyB)
return m_rigidBodyB->getIslandTag();
if (m_bodyB)
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
{
col = m_bodyB->getLink(i).m_collider;
if (col)
return col->getIslandTag();
}
}
return -1;
}
void btMultiBodyFixedConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
{
int numDim = BTMBFIXEDCONSTRAINT_DIM;
for (int i=0;i<numDim;i++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = i;
constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
constraintRow.m_contactNormal1.setValue(0,0,0);
constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
constraintRow.m_contactNormal2.setValue(0,0,0);
constraintRow.m_angularComponentA.setValue(0,0,0);
constraintRow.m_angularComponentB.setValue(0,0,0);
constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
// Convert local points back to world
btVector3 pivotAworld = m_pivotInA;
btMatrix3x3 frameAworld = m_frameInA;
if (m_rigidBodyA)
{
constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
frameAworld = frameAworld.transpose()*btMatrix3x3(m_rigidBodyA->getOrientation());
} else
{
if (m_bodyA) {
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
}
}
btVector3 pivotBworld = m_pivotInB;
btMatrix3x3 frameBworld = m_frameInB;
if (m_rigidBodyB)
{
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
frameBworld = frameBworld.transpose()*btMatrix3x3(m_rigidBodyB->getOrientation());
} else
{
if (m_bodyB) {
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
frameBworld = m_bodyB->localFrameToWorld(m_linkB, frameBworld);
}
}
btMatrix3x3 relRot = frameAworld.inverse()*frameBworld;
btVector3 angleDiff;
btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot,angleDiff);
btVector3 constraintNormalLin(0,0,0);
btVector3 constraintNormalAng(0,0,0);
btScalar posError = 0.0;
if (i < 3) {
constraintNormalLin[i] = -1;
posError = (pivotAworld-pivotBworld).dot(constraintNormalLin);
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
constraintNormalLin, pivotAworld, pivotBworld,
posError,
infoGlobal,
-m_maxAppliedImpulse, m_maxAppliedImpulse
);
}
else { //i>=3
constraintNormalAng = frameAworld.getColumn(i%3);
posError = angleDiff[i%3];
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
constraintNormalLin, pivotAworld, pivotBworld,
posError,
infoGlobal,
-m_maxAppliedImpulse, m_maxAppliedImpulse, true
);
}
}
}
void btMultiBodyFixedConstraint::debugDraw(class btIDebugDraw* drawer)
{
btTransform tr;
tr.setIdentity();
if (m_rigidBodyA)
{
btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
tr.setOrigin(pivot);
drawer->drawTransform(tr, 0.1);
}
if (m_bodyA)
{
btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
tr.setOrigin(pivotAworld);
drawer->drawTransform(tr, 0.1);
}
if (m_rigidBodyB)
{
// that ideally should draw the same frame
btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
tr.setOrigin(pivot);
drawer->drawTransform(tr, 0.1);
}
if (m_bodyB)
{
btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
tr.setOrigin(pivotBworld);
drawer->drawTransform(tr, 0.1);
}
}

94
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h Normal file
View file

@ -0,0 +1,94 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
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.
*/
///This file was written by Erwin Coumans
#ifndef BT_MULTIBODY_FIXED_CONSTRAINT_H
#define BT_MULTIBODY_FIXED_CONSTRAINT_H
#include "btMultiBodyConstraint.h"
class btMultiBodyFixedConstraint : public btMultiBodyConstraint
{
protected:
btRigidBody* m_rigidBodyA;
btRigidBody* m_rigidBodyB;
btVector3 m_pivotInA;
btVector3 m_pivotInB;
btMatrix3x3 m_frameInA;
btMatrix3x3 m_frameInB;
public:
btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
virtual ~btMultiBodyFixedConstraint();
virtual void finalizeMultiDof();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
const btVector3& getPivotInA() const
{
return m_pivotInA;
}
void setPivotInA(const btVector3& pivotInA)
{
m_pivotInA = pivotInA;
}
const btVector3& getPivotInB() const
{
return m_pivotInB;
}
void setPivotInB(const btVector3& pivotInB)
{
m_pivotInB = pivotInB;
}
const btMatrix3x3& getFrameInA() const
{
return m_frameInA;
}
void setFrameInA(const btMatrix3x3& frameInA)
{
m_frameInA = frameInA;
}
const btMatrix3x3& getFrameInB() const
{
return m_frameInB;
}
void setFrameInB(const btMatrix3x3& frameInB)
{
m_frameInB = frameInB;
}
virtual void debugDraw(class btIDebugDraw* drawer);
};
#endif //BT_MULTIBODY_FIXED_CONSTRAINT_H

27
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h Normal file
View file

@ -0,0 +1,27 @@
/*
Copyright (c) 2015 Google Inc.
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 BT_MULTIBODY_JOINT_FEEDBACK_H
#define BT_MULTIBODY_JOINT_FEEDBACK_H
#include "LinearMath/btSpatialAlgebra.h"
struct btMultiBodyJointFeedback
{
btSpatialForceVector m_reactionForces;
};
#endif //BT_MULTIBODY_JOINT_FEEDBACK_H

199
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp Normal file
View file

@ -0,0 +1,199 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
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.
*/
///This file was written by Erwin Coumans
#include "btMultiBodyJointLimitConstraint.h"
#include "btMultiBody.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
//:btMultiBodyConstraint(body,0,link,-1,2,true),
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,2,true),
m_lowerBound(lower),
m_upperBound(upper)
{
}
void btMultiBodyJointLimitConstraint::finalizeMultiDof()
{
// the data.m_jacobians never change, so may as well
// initialize them here
allocateJacobiansMultiDof();
unsigned int offset = 6 + m_bodyA->getLink(m_linkA).m_dofOffset;
// row 0: the lower bound
jacobianA(0)[offset] = 1;
// row 1: the upper bound
//jacobianA(1)[offset] = -1;
jacobianB(1)[offset] = -1;
m_numDofsFinalized = m_jacSizeBoth;
}
btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
{
}
int btMultiBodyJointLimitConstraint::getIslandIdA() const
{
if(m_bodyA)
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
}
}
return -1;
}
int btMultiBodyJointLimitConstraint::getIslandIdB() const
{
if(m_bodyB)
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
{
col = m_bodyB->getLink(i).m_collider;
if (col)
return col->getIslandTag();
}
}
return -1;
}
void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)
{
// only positions need to be updated -- data.m_jacobians and force
// directions were set in the ctor and never change.
if (m_numDofsFinalized != m_jacSizeBoth)
{
finalizeMultiDof();
}
// row 0: the lower bound
setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); //multidof: this is joint-type dependent
// row 1: the upper bound
setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
for (int row=0;row<getNumRows();row++)
{
btScalar direction = row? -1 : 1;
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = row;
constraintRow.m_multiBodyA = m_bodyA;
constraintRow.m_multiBodyB = m_bodyB;
const btScalar posError = 0; //why assume it's zero?
const btVector3 dummy(0, 0, 0);
btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
{
//expect either prismatic or revolute joint type for now
btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
switch (m_bodyA->getLink(m_linkA).m_jointType)
{
case btMultibodyLink::eRevolute:
{
constraintRow.m_contactNormal1.setZero();
constraintRow.m_contactNormal2.setZero();
btVector3 revoluteAxisInWorld = direction*quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
break;
}
case btMultibodyLink::ePrismatic:
{
btVector3 prismaticAxisInWorld = direction* quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
constraintRow.m_contactNormal1=prismaticAxisInWorld;
constraintRow.m_contactNormal2=-prismaticAxisInWorld;
constraintRow.m_relpos1CrossNormal.setZero();
constraintRow.m_relpos2CrossNormal.setZero();
break;
}
default:
{
btAssert(0);
}
};
}
{
btScalar penetration = getPosition(row);
btScalar positionalError = 0.f;
btScalar velocityError = - rel_vel;// * damping;
btScalar erp = infoGlobal.m_erp2;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
erp = infoGlobal.m_erp;
}
if (penetration>0)
{
positionalError = 0;
velocityError = -penetration / infoGlobal.m_timeStep;
} else
{
positionalError = -penetration * erp/infoGlobal.m_timeStep;
}
btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
//combine position and velocity into rhs
constraintRow.m_rhs = penetrationImpulse+velocityImpulse;
constraintRow.m_rhsPenetration = 0.f;
} else
{
//split position and velocity into rhs and m_rhsPenetration
constraintRow.m_rhs = velocityImpulse;
constraintRow.m_rhsPenetration = penetrationImpulse;
}
}
}
}

50
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h Normal file
View file

@ -0,0 +1,50 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
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 BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
#define BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
#include "btMultiBodyConstraint.h"
struct btSolverInfo;
class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint
{
protected:
btScalar m_lowerBound;
btScalar m_upperBound;
public:
btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper);
virtual ~btMultiBodyJointLimitConstraint();
virtual void finalizeMultiDof();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
virtual void debugDraw(class btIDebugDraw* drawer)
{
//todo(erwincoumans)
}
};
#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H

183
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp Normal file
View file

@ -0,0 +1,183 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
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.
*/
///This file was written by Erwin Coumans
#include "btMultiBodyJointMotor.h"
#include "btMultiBody.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
m_desiredVelocity(desiredVelocity),
m_desiredPosition(0),
m_kd(1.),
m_kp(0),
m_erp(1),
m_rhsClamp(SIMD_INFINITY)
{
m_maxAppliedImpulse = maxMotorImpulse;
// the data.m_jacobians never change, so may as well
// initialize them here
}
void btMultiBodyJointMotor::finalizeMultiDof()
{
allocateJacobiansMultiDof();
// note: we rely on the fact that data.m_jacobians are
// always initialized to zero by the Constraint ctor
int linkDoF = 0;
unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
// row 0: the lower bound
// row 0: the lower bound
jacobianA(0)[offset] = 1;
m_numDofsFinalized = m_jacSizeBoth;
}
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
//:btMultiBodyConstraint(body,0,link,-1,1,true),
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
m_desiredVelocity(desiredVelocity),
m_desiredPosition(0),
m_kd(1.),
m_kp(0),
m_erp(1),
m_rhsClamp(SIMD_INFINITY)
{
btAssert(linkDoF < body->getLink(link).m_dofCount);
m_maxAppliedImpulse = maxMotorImpulse;
}
btMultiBodyJointMotor::~btMultiBodyJointMotor()
{
}
int btMultiBodyJointMotor::getIslandIdA() const
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
}
return -1;
}
int btMultiBodyJointMotor::getIslandIdB() const
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
{
col = m_bodyB->getLink(i).m_collider;
if (col)
return col->getIslandTag();
}
return -1;
}
void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)
{
// only positions need to be updated -- data.m_jacobians and force
// directions were set in the ctor and never change.
if (m_numDofsFinalized != m_jacSizeBoth)
{
finalizeMultiDof();
}
//don't crash
if (m_numDofsFinalized != m_jacSizeBoth)
return;
const btScalar posError = 0;
const btVector3 dummy(0, 0, 0);
for (int row=0;row<getNumRows();row++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
int dof = 0;
btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
btScalar positionStabiliationTerm = m_erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
btScalar velocityError = (m_desiredVelocity - currentVelocity);
btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError;
if (rhs>m_rhsClamp)
{
rhs=m_rhsClamp;
}
if (rhs<-m_rhsClamp)
{
rhs=-m_rhsClamp;
}
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,rhs);
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = row;
{
//expect either prismatic or revolute joint type for now
btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
switch (m_bodyA->getLink(m_linkA).m_jointType)
{
case btMultibodyLink::eRevolute:
{
constraintRow.m_contactNormal1.setZero();
constraintRow.m_contactNormal2.setZero();
btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
break;
}
case btMultibodyLink::ePrismatic:
{
btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
constraintRow.m_contactNormal1=prismaticAxisInWorld;
constraintRow.m_contactNormal2=-prismaticAxisInWorld;
constraintRow.m_relpos1CrossNormal.setZero();
constraintRow.m_relpos2CrossNormal.setZero();
break;
}
default:
{
btAssert(0);
}
};
}
}
}

81
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h Normal file
View file

@ -0,0 +1,81 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
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.
*/
///This file was written by Erwin Coumans
#ifndef BT_MULTIBODY_JOINT_MOTOR_H
#define BT_MULTIBODY_JOINT_MOTOR_H
#include "btMultiBodyConstraint.h"
struct btSolverInfo;
class btMultiBodyJointMotor : public btMultiBodyConstraint
{
protected:
btScalar m_desiredVelocity;
btScalar m_desiredPosition;
btScalar m_kd;
btScalar m_kp;
btScalar m_erp;
btScalar m_rhsClamp;//maximum error
public:
btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse);
btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse);
virtual ~btMultiBodyJointMotor();
virtual void finalizeMultiDof();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
virtual void setVelocityTarget(btScalar velTarget, btScalar kd = 1.f)
{
m_desiredVelocity = velTarget;
m_kd = kd;
}
virtual void setPositionTarget(btScalar posTarget, btScalar kp = 1.f)
{
m_desiredPosition = posTarget;
m_kp = kp;
}
virtual void setErp(btScalar erp)
{
m_erp = erp;
}
virtual btScalar getErp() const
{
return m_erp;
}
virtual void setRhsClamp(btScalar rhsClamp)
{
m_rhsClamp = rhsClamp;
}
virtual void debugDraw(class btIDebugDraw* drawer)
{
//todo(erwincoumans)
}
};
#endif //BT_MULTIBODY_JOINT_MOTOR_H

226
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyLink.h Normal file
View file

@ -0,0 +1,226 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
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 BT_MULTIBODY_LINK_H
#define BT_MULTIBODY_LINK_H
#include "LinearMath/btQuaternion.h"
#include "LinearMath/btVector3.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
enum btMultiBodyLinkFlags
{
BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1
};
//both defines are now permanently enabled
#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
#define TEST_SPATIAL_ALGEBRA_LAYER
//
// Various spatial helper functions
//
//namespace {
#include "LinearMath/btSpatialAlgebra.h"
//}
//
// Link struct
//
struct btMultibodyLink
{
BT_DECLARE_ALIGNED_ALLOCATOR();
btScalar m_mass; // mass of link
btVector3 m_inertiaLocal; // inertia of link (local frame; diagonal)
int m_parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
btQuaternion m_zeroRotParentToThis; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
btVector3 m_dVector; // vector from the inboard joint pos to this link's COM. (local frame.) constant.
//this is set to zero for planar joint (see also m_eVector comment)
// m_eVector is constant, but depends on the joint type:
// revolute, fixed, prismatic, spherical: vector from parent's COM to the pivot point, in PARENT's frame.
// planar: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.)
// todo: fix the planar so it is consistent with the other joints
btVector3 m_eVector;
btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;
enum eFeatherstoneJointType
{
eRevolute = 0,
ePrismatic = 1,
eSpherical = 2,
ePlanar = 3,
eFixed = 4,
eInvalid
};
// "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant.
// for prismatic: m_axesTop[0] = zero;
// m_axesBottom[0] = unit vector along the joint axis.
// for revolute: m_axesTop[0] = unit vector along the rotation axis (u);
// m_axesBottom[0] = u cross m_dVector (i.e. COM linear motion due to the rotation at the joint)
//
// for spherical: m_axesTop[0][1][2] (u1,u2,u3) form a 3x3 identity matrix (3 rotation axes)
// m_axesBottom[0][1][2] cross u1,u2,u3 (i.e. COM linear motion due to the rotation at the joint)
//
// for planar: m_axesTop[0] = unit vector along the rotation axis (u); defines the plane of motion
// m_axesTop[1][2] = zero
// m_axesBottom[0] = zero
// m_axesBottom[1][2] = unit vectors along the translational axes on that plane
btSpatialMotionVector m_axes[6];
void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; }
void setAxisBottom(int dof, const btVector3 &axis) { m_axes[dof].m_bottomVec = axis; }
void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_topVec.setValue(x, y, z); }
void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_bottomVec.setValue(x, y, z); }
const btVector3 & getAxisTop(int dof) const { return m_axes[dof].m_topVec; }
const btVector3 & getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; }
int m_dofOffset, m_cfgOffset;
btQuaternion m_cachedRotParentToThis; // rotates vectors in parent frame to vectors in local frame
btVector3 m_cachedRVector; // vector from COM of parent to COM of this link, in local frame.
btVector3 m_appliedForce; // In WORLD frame
btVector3 m_appliedTorque; // In WORLD frame
btVector3 m_appliedConstraintForce; // In WORLD frame
btVector3 m_appliedConstraintTorque; // In WORLD frame
btScalar m_jointPos[7];
//m_jointTorque is the joint torque applied by the user using 'addJointTorque'.
//It gets set to zero after each internal stepSimulation call
btScalar m_jointTorque[6];
class btMultiBodyLinkCollider* m_collider;
int m_flags;
int m_dofCount, m_posVarCount; //redundant but handy
eFeatherstoneJointType m_jointType;
struct btMultiBodyJointFeedback* m_jointFeedback;
btTransform m_cachedWorldTransform;//this cache is updated when calling btMultiBody::forwardKinematics
const char* m_linkName;//m_linkName memory needs to be managed by the developer/user!
const char* m_jointName;//m_jointName memory needs to be managed by the developer/user!
const void* m_userPtr;//m_userPtr ptr needs to be managed by the developer/user!
btScalar m_jointDamping; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual damping.
btScalar m_jointFriction; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual friction using a velocity motor.
// ctor: set some sensible defaults
btMultibodyLink()
: m_mass(1),
m_parent(-1),
m_zeroRotParentToThis(0, 0, 0, 1),
m_cachedRotParentToThis(0, 0, 0, 1),
m_collider(0),
m_flags(0),
m_dofCount(0),
m_posVarCount(0),
m_jointType(btMultibodyLink::eInvalid),
m_jointFeedback(0),
m_linkName(0),
m_jointName(0),
m_userPtr(0),
m_jointDamping(0),
m_jointFriction(0)
{
m_inertiaLocal.setValue(1, 1, 1);
setAxisTop(0, 0., 0., 0.);
setAxisBottom(0, 1., 0., 0.);
m_dVector.setValue(0, 0, 0);
m_eVector.setValue(0, 0, 0);
m_cachedRVector.setValue(0, 0, 0);
m_appliedForce.setValue( 0, 0, 0);
m_appliedTorque.setValue(0, 0, 0);
//
m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f;
m_jointPos[3] = 1.f; //"quat.w"
m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f;
m_cachedWorldTransform.setIdentity();
}
// routine to update m_cachedRotParentToThis and m_cachedRVector
void updateCacheMultiDof(btScalar *pq = 0)
{
btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
switch(m_jointType)
{
case eRevolute:
{
m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}
case ePrismatic:
{
// m_cachedRotParentToThis never changes, so no need to update
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector) + pJointPos[0] * getAxisBottom(0);
break;
}
case eSpherical:
{
m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}
case ePlanar:
{
m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}
case eFixed:
{
m_cachedRotParentToThis = m_zeroRotParentToThis;
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}
default:
{
//invalid type
btAssert(0);
}
}
}
};
#endif //BT_MULTIBODY_LINK_H

92
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h Normal file
View file

@ -0,0 +1,92 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
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 BT_FEATHERSTONE_LINK_COLLIDER_H
#define BT_FEATHERSTONE_LINK_COLLIDER_H
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "btMultiBody.h"
class btMultiBodyLinkCollider : public btCollisionObject
{
//protected:
public:
btMultiBody* m_multiBody;
int m_link;
btMultiBodyLinkCollider (btMultiBody* multiBody,int link)
:m_multiBody(multiBody),
m_link(link)
{
m_checkCollideWith = true;
//we need to remove the 'CF_STATIC_OBJECT' flag, otherwise links/base doesn't merge islands
//this means that some constraints might point to bodies that are not in the islands, causing crashes
//if (link>=0 || (multiBody && !multiBody->hasFixedBase()))
{
m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
}
// else
//{
// m_collisionFlags |= (btCollisionObject::CF_STATIC_OBJECT);
//}
m_internalType = CO_FEATHERSTONE_LINK;
}
static btMultiBodyLinkCollider* upcast(btCollisionObject* colObj)
{
if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
return (btMultiBodyLinkCollider*)colObj;
return 0;
}
static const btMultiBodyLinkCollider* upcast(const btCollisionObject* colObj)
{
if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
return (btMultiBodyLinkCollider*)colObj;
return 0;
}
virtual bool checkCollideWithOverride(const btCollisionObject* co) const
{
const btMultiBodyLinkCollider* other = btMultiBodyLinkCollider::upcast(co);
if (!other)
return true;
if (other->m_multiBody != this->m_multiBody)
return true;
if (!m_multiBody->hasSelfCollision())
return false;
//check if 'link' has collision disabled
if (m_link>=0)
{
const btMultibodyLink& link = m_multiBody->getLink(this->m_link);
if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.m_parent == other->m_link)
return false;
}
if (other->m_link>=0)
{
const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link);
if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.m_parent == this->m_link)
return false;
}
return true;
}
};
#endif //BT_FEATHERSTONE_LINK_COLLIDER_H

221
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp Normal file
View file

@ -0,0 +1,221 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
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.
*/
///This file was written by Erwin Coumans
#include "btMultiBodyPoint2Point.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btIDebugDraw.h"
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
#define BTMBP2PCONSTRAINT_DIM 3
#else
#define BTMBP2PCONSTRAINT_DIM 6
#endif
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB)
:btMultiBodyConstraint(body,0,link,-1,BTMBP2PCONSTRAINT_DIM,false),
m_rigidBodyA(0),
m_rigidBodyB(bodyB),
m_pivotInA(pivotInA),
m_pivotInB(pivotInB)
{
m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses
}
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBP2PCONSTRAINT_DIM,false),
m_rigidBodyA(0),
m_rigidBodyB(0),
m_pivotInA(pivotInA),
m_pivotInB(pivotInB)
{
m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses
}
void btMultiBodyPoint2Point::finalizeMultiDof()
{
//not implemented yet
btAssert(0);
}
btMultiBodyPoint2Point::~btMultiBodyPoint2Point()
{
}
int btMultiBodyPoint2Point::getIslandIdA() const
{
if (m_rigidBodyA)
return m_rigidBodyA->getIslandTag();
if (m_bodyA)
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
}
}
return -1;
}
int btMultiBodyPoint2Point::getIslandIdB() const
{
if (m_rigidBodyB)
return m_rigidBodyB->getIslandTag();
if (m_bodyB)
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
{
col = m_bodyB->getLink(i).m_collider;
if (col)
return col->getIslandTag();
}
}
return -1;
}
void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)
{
// int i=1;
int numDim = BTMBP2PCONSTRAINT_DIM;
for (int i=0;i<numDim;i++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
//memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = i;
constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
constraintRow.m_contactNormal1.setValue(0,0,0);
constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
constraintRow.m_contactNormal2.setValue(0,0,0);
constraintRow.m_angularComponentA.setValue(0,0,0);
constraintRow.m_angularComponentB.setValue(0,0,0);
constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
btVector3 contactNormalOnB(0,0,0);
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
contactNormalOnB[i] = -1;
#else
contactNormalOnB[i%3] = -1;
#endif
// Convert local points back to world
btVector3 pivotAworld = m_pivotInA;
if (m_rigidBodyA)
{
constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
} else
{
if (m_bodyA)
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
}
btVector3 pivotBworld = m_pivotInB;
if (m_rigidBodyB)
{
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
} else
{
if (m_bodyB)
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
}
btScalar posError = i < 3 ? (pivotAworld-pivotBworld).dot(contactNormalOnB) : 0;
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
fillMultiBodyConstraint(constraintRow, data, 0, 0, btVector3(0,0,0),
contactNormalOnB, pivotAworld, pivotBworld, //sucks but let it be this way "for the time being"
posError,
infoGlobal,
-m_maxAppliedImpulse, m_maxAppliedImpulse
);
//@todo: support the case of btMultiBody versus btRigidBody,
//see btPoint2PointConstraint::getInfo2NonVirtual
#else
const btVector3 dummy(0, 0, 0);
btAssert(m_bodyA->isMultiDof());
btScalar* jac1 = jacobianA(i);
const btVector3 &normalAng = i >= 3 ? contactNormalOnB : dummy;
const btVector3 &normalLin = i < 3 ? contactNormalOnB : dummy;
m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, normalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
fillMultiBodyConstraint(constraintRow, data, jac1, 0,
dummy, dummy, dummy, //sucks but let it be this way "for the time being"
posError,
infoGlobal,
-m_maxAppliedImpulse, m_maxAppliedImpulse
);
#endif
}
}
void btMultiBodyPoint2Point::debugDraw(class btIDebugDraw* drawer)
{
btTransform tr;
tr.setIdentity();
if (m_rigidBodyA)
{
btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
tr.setOrigin(pivot);
drawer->drawTransform(tr, 0.1);
}
if (m_bodyA)
{
btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
tr.setOrigin(pivotAworld);
drawer->drawTransform(tr, 0.1);
}
if (m_rigidBodyB)
{
// that ideally should draw the same frame
btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
tr.setOrigin(pivot);
drawer->drawTransform(tr, 0.1);
}
if (m_bodyB)
{
btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
tr.setOrigin(pivotBworld);
drawer->drawTransform(tr, 0.1);
}
}

65
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h Normal file
View file

@ -0,0 +1,65 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
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.
*/
///This file was written by Erwin Coumans
#ifndef BT_MULTIBODY_POINT2POINT_H
#define BT_MULTIBODY_POINT2POINT_H
#include "btMultiBodyConstraint.h"
//#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
class btMultiBodyPoint2Point : public btMultiBodyConstraint
{
protected:
btRigidBody* m_rigidBodyA;
btRigidBody* m_rigidBodyB;
btVector3 m_pivotInA;
btVector3 m_pivotInB;
public:
btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB);
btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB);
virtual ~btMultiBodyPoint2Point();
virtual void finalizeMultiDof();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
const btVector3& getPivotInB() const
{
return m_pivotInB;
}
void setPivotInB(const btVector3& pivotInB)
{
m_pivotInB = pivotInB;
}
virtual void debugDraw(class btIDebugDraw* drawer);
};
#endif //BT_MULTIBODY_POINT2POINT_H

230
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp Normal file
View file

@ -0,0 +1,230 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
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.
*/
///This file was written by Erwin Coumans
#include "btMultiBodySliderConstraint.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
#include "LinearMath/btIDebugDraw.h"
#define BTMBSLIDERCONSTRAINT_DIM 5
#define EPSILON 0.000001
btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
:btMultiBodyConstraint(body,0,link,-1,BTMBSLIDERCONSTRAINT_DIM,false),
m_rigidBodyA(0),
m_rigidBodyB(bodyB),
m_pivotInA(pivotInA),
m_pivotInB(pivotInB),
m_frameInA(frameInA),
m_frameInB(frameInB),
m_jointAxis(jointAxis)
{
m_data.resize(BTMBSLIDERCONSTRAINT_DIM);//at least store the applied impulses
}
btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBSLIDERCONSTRAINT_DIM,false),
m_rigidBodyA(0),
m_rigidBodyB(0),
m_pivotInA(pivotInA),
m_pivotInB(pivotInB),
m_frameInA(frameInA),
m_frameInB(frameInB),
m_jointAxis(jointAxis)
{
m_data.resize(BTMBSLIDERCONSTRAINT_DIM);//at least store the applied impulses
}
void btMultiBodySliderConstraint::finalizeMultiDof()
{
//not implemented yet
btAssert(0);
}
btMultiBodySliderConstraint::~btMultiBodySliderConstraint()
{
}
int btMultiBodySliderConstraint::getIslandIdA() const
{
if (m_rigidBodyA)
return m_rigidBodyA->getIslandTag();
if (m_bodyA)
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
}
}
return -1;
}
int btMultiBodySliderConstraint::getIslandIdB() const
{
if (m_rigidBodyB)
return m_rigidBodyB->getIslandTag();
if (m_bodyB)
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
{
col = m_bodyB->getLink(i).m_collider;
if (col)
return col->getIslandTag();
}
}
return -1;
}
void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
{
// Convert local points back to world
btVector3 pivotAworld = m_pivotInA;
btMatrix3x3 frameAworld = m_frameInA;
btVector3 jointAxis = m_jointAxis;
if (m_rigidBodyA)
{
pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
frameAworld = m_frameInA.transpose()*btMatrix3x3(m_rigidBodyA->getOrientation());
jointAxis = quatRotate(m_rigidBodyA->getOrientation(),m_jointAxis);
} else if (m_bodyA) {
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
frameAworld = m_bodyA->localFrameToWorld(m_linkA, m_frameInA);
jointAxis = m_bodyA->localDirToWorld(m_linkA, m_jointAxis);
}
btVector3 pivotBworld = m_pivotInB;
btMatrix3x3 frameBworld = m_frameInB;
if (m_rigidBodyB)
{
pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
frameBworld = m_frameInB.transpose()*btMatrix3x3(m_rigidBodyB->getOrientation());
} else if (m_bodyB) {
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
frameBworld = m_bodyB->localFrameToWorld(m_linkB, m_frameInB);
}
btVector3 constraintAxis[2];
for (int i = 0; i < 3; ++i)
{
constraintAxis[0] = frameAworld.getColumn(i).cross(jointAxis);
if (constraintAxis[0].safeNorm() > EPSILON)
{
constraintAxis[0] = constraintAxis[0].normalized();
constraintAxis[1] = jointAxis.cross(constraintAxis[0]);
constraintAxis[1] = constraintAxis[1].normalized();
break;
}
}
btMatrix3x3 relRot = frameAworld.inverse()*frameBworld;
btVector3 angleDiff;
btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot,angleDiff);
int numDim = BTMBSLIDERCONSTRAINT_DIM;
for (int i=0;i<numDim;i++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = i;
constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
constraintRow.m_contactNormal1.setValue(0,0,0);
constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
constraintRow.m_contactNormal2.setValue(0,0,0);
constraintRow.m_angularComponentA.setValue(0,0,0);
constraintRow.m_angularComponentB.setValue(0,0,0);
constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
if (m_rigidBodyA)
{
constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
}
if (m_rigidBodyB)
{
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
}
btVector3 constraintNormalLin(0,0,0);
btVector3 constraintNormalAng(0,0,0);
btScalar posError = 0.0;
if (i < 2) {
constraintNormalLin = constraintAxis[i];
posError = (pivotAworld-pivotBworld).dot(constraintNormalLin);
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
constraintNormalLin, pivotAworld, pivotBworld,
posError,
infoGlobal,
-m_maxAppliedImpulse, m_maxAppliedImpulse
);
}
else { //i>=2
constraintNormalAng = frameAworld.getColumn(i%3);
posError = angleDiff[i%3];
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
constraintNormalLin, pivotAworld, pivotBworld,
posError,
infoGlobal,
-m_maxAppliedImpulse, m_maxAppliedImpulse, true
);
}
}
}
void btMultiBodySliderConstraint::debugDraw(class btIDebugDraw* drawer)
{
btTransform tr;
tr.setIdentity();
if (m_rigidBodyA)
{
btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
tr.setOrigin(pivot);
drawer->drawTransform(tr, 0.1);
}
if (m_bodyA)
{
btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
tr.setOrigin(pivotAworld);
drawer->drawTransform(tr, 0.1);
}
if (m_rigidBodyB)
{
// that ideally should draw the same frame
btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
tr.setOrigin(pivot);
drawer->drawTransform(tr, 0.1);
}
if (m_bodyB)
{
btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
tr.setOrigin(pivotBworld);
drawer->drawTransform(tr, 0.1);
}
}

105
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h Normal file
View file

@ -0,0 +1,105 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
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.
*/
///This file was written by Erwin Coumans
#ifndef BT_MULTIBODY_SLIDER_CONSTRAINT_H
#define BT_MULTIBODY_SLIDER_CONSTRAINT_H
#include "btMultiBodyConstraint.h"
class btMultiBodySliderConstraint : public btMultiBodyConstraint
{
protected:
btRigidBody* m_rigidBodyA;
btRigidBody* m_rigidBodyB;
btVector3 m_pivotInA;
btVector3 m_pivotInB;
btMatrix3x3 m_frameInA;
btMatrix3x3 m_frameInB;
btVector3 m_jointAxis;
public:
btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis);
btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis);
virtual ~btMultiBodySliderConstraint();
virtual void finalizeMultiDof();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
const btVector3& getPivotInA() const
{
return m_pivotInA;
}
void setPivotInA(const btVector3& pivotInA)
{
m_pivotInA = pivotInA;
}
const btVector3& getPivotInB() const
{
return m_pivotInB;
}
void setPivotInB(const btVector3& pivotInB)
{
m_pivotInB = pivotInB;
}
const btMatrix3x3& getFrameInA() const
{
return m_frameInA;
}
void setFrameInA(const btMatrix3x3& frameInA)
{
m_frameInA = frameInA;
}
const btMatrix3x3& getFrameInB() const
{
return m_frameInB;
}
void setFrameInB(const btMatrix3x3& frameInB)
{
m_frameInB = frameInB;
}
const btVector3& getJointAxis() const
{
return m_jointAxis;
}
void setJointAxis(const btVector3& jointAxis)
{
m_jointAxis = jointAxis;
}
virtual void debugDraw(class btIDebugDraw* drawer);
};
#endif //BT_MULTIBODY_SLIDER_CONSTRAINT_H

90
Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h Normal file
View file

@ -0,0 +1,90 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
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 BT_MULTIBODY_SOLVER_CONSTRAINT_H
#define BT_MULTIBODY_SOLVER_CONSTRAINT_H
#include "LinearMath/btVector3.h"
#include "LinearMath/btAlignedObjectArray.h"
class btMultiBody;
class btMultiBodyConstraint;
#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint
{
BT_DECLARE_ALIGNED_ALLOCATOR();
btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1),m_orgConstraint(0), m_orgDofIndex(-1)
{}
int m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1
int m_jacAindex;
int m_deltaVelBindex;
int m_jacBindex;
btVector3 m_relpos1CrossNormal;
btVector3 m_contactNormal1;
btVector3 m_relpos2CrossNormal;
btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always
btVector3 m_angularComponentA;
btVector3 m_angularComponentB;
mutable btSimdScalar m_appliedPushImpulse;
mutable btSimdScalar m_appliedImpulse;
btScalar m_friction;
btScalar m_jacDiagABInv;
btScalar m_rhs;
btScalar m_cfm;
btScalar m_lowerLimit;
btScalar m_upperLimit;
btScalar m_rhsPenetration;
union
{
void* m_originalContactPoint;
btScalar m_unusedPadding4;
};
int m_overrideNumSolverIterations;
int m_frictionIndex;
int m_solverBodyIdA;
btMultiBody* m_multiBodyA;
int m_linkA;
int m_solverBodyIdB;
btMultiBody* m_multiBodyB;
int m_linkB;
//for writing back applied impulses
btMultiBodyConstraint* m_orgConstraint;
int m_orgDofIndex;
enum btSolverConstraintType
{
BT_SOLVER_CONTACT_1D = 0,
BT_SOLVER_FRICTION_1D
};
};
typedef btAlignedObjectArray<btMultiBodySolverConstraint> btMultiBodyConstraintArray;
#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_H

2080
Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp Normal file
View file

File diff suppressed because it is too large Load diff

77
Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btDantzigLCP.h Normal file
View file

@ -0,0 +1,77 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of *
* The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
/*
given (A,b,lo,hi), solve the LCP problem: A*x = b+w, where each x(i),w(i)
satisfies one of
(1) x = lo, w >= 0
(2) x = hi, w <= 0
(3) lo < x < hi, w = 0
A is a matrix of dimension n*n, everything else is a vector of size n*1.
lo and hi can be +/- dInfinity as needed. the first `nub' variables are
unbounded, i.e. hi and lo are assumed to be +/- dInfinity.
we restrict lo(i) <= 0 and hi(i) >= 0.
the original data (A,b) may be modified by this function.
if the `findex' (friction index) parameter is nonzero, it points to an array
of index values. in this case constraints that have findex[i] >= 0 are
special. all non-special constraints are solved for, then the lo and hi values
for the special constraints are set:
hi[i] = abs( hi[i] * x[findex[i]] )
lo[i] = -hi[i]
and the solution continues. this mechanism allows a friction approximation
to be implemented. the first `nub' variables are assumed to have findex < 0.
*/
#ifndef _BT_LCP_H_
#define _BT_LCP_H_
#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
#include "LinearMath/btScalar.h"
#include "LinearMath/btAlignedObjectArray.h"
struct btDantzigScratchMemory
{
btAlignedObjectArray<btScalar> m_scratch;
btAlignedObjectArray<btScalar> L;
btAlignedObjectArray<btScalar> d;
btAlignedObjectArray<btScalar> delta_w;
btAlignedObjectArray<btScalar> delta_x;
btAlignedObjectArray<btScalar> Dell;
btAlignedObjectArray<btScalar> ell;
btAlignedObjectArray<btScalar*> Arows;
btAlignedObjectArray<int> p;
btAlignedObjectArray<int> C;
btAlignedObjectArray<bool> state;
};
//return false if solving failed
bool btSolveDantzigLCP (int n, btScalar *A, btScalar *x, btScalar *b, btScalar *w,
int nub, btScalar *lo, btScalar *hi, int *findex,btDantzigScratchMemory& scratch);
#endif //_BT_LCP_H_

112
Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h Normal file
View file

@ -0,0 +1,112 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
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.
*/
///original version written by Erwin Coumans, October 2013
#ifndef BT_DANTZIG_SOLVER_H
#define BT_DANTZIG_SOLVER_H
#include "btMLCPSolverInterface.h"
#include "btDantzigLCP.h"
class btDantzigSolver : public btMLCPSolverInterface
{
protected:
btScalar m_acceptableUpperLimitSolution;
btAlignedObjectArray<char> m_tempBuffer;
btAlignedObjectArray<btScalar> m_A;
btAlignedObjectArray<btScalar> m_b;
btAlignedObjectArray<btScalar> m_x;
btAlignedObjectArray<btScalar> m_lo;
btAlignedObjectArray<btScalar> m_hi;
btAlignedObjectArray<int> m_dependencies;
btDantzigScratchMemory m_scratchMemory;
public:
btDantzigSolver()
:m_acceptableUpperLimitSolution(btScalar(1000))
{
}
virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
{
bool result = true;
int n = b.rows();
if (n)
{
int nub = 0;
btAlignedObjectArray<btScalar> ww;
ww.resize(n);
const btScalar* Aptr = A.getBufferPointer();
m_A.resize(n*n);
for (int i=0;i<n*n;i++)
{
m_A[i] = Aptr[i];
}
m_b.resize(n);
m_x.resize(n);
m_lo.resize(n);
m_hi.resize(n);
m_dependencies.resize(n);
for (int i=0;i<n;i++)
{
m_lo[i] = lo[i];
m_hi[i] = hi[i];
m_b[i] = b[i];
m_x[i] = x[i];
m_dependencies[i] = limitDependency[i];
}
result = btSolveDantzigLCP (n,&m_A[0],&m_x[0],&m_b[0],&ww[0],nub,&m_lo[0],&m_hi[0],&m_dependencies[0],m_scratchMemory);
if (!result)
return result;
// printf("numAllocas = %d\n",numAllocas);
for (int i=0;i<n;i++)
{
volatile btScalar xx = m_x[i];
if (xx != m_x[i])
return false;
if (x[i] >= m_acceptableUpperLimitSolution)
{
return false;
}
if (x[i] <= -m_acceptableUpperLimitSolution)
{
return false;
}
}
for (int i=0;i<n;i++)
{
x[i] = m_x[i];
}
}
return result;
}
};
#endif //BT_DANTZIG_SOLVER_H

371
Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp Normal file
View file

@ -0,0 +1,371 @@
/* Copyright (C) 2004-2013 MBSim Development Team
Code was converted for the Bullet Continuous Collision Detection and Physics Library
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.
*/
//The original version is here
//https://code.google.com/p/mbsim-env/source/browse/trunk/kernel/mbsim/numerics/linear_complementarity_problem/lemke_algorithm.cc
//This file is re-distributed under the ZLib license, with permission of the original author
//Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h
//STL/std::vector replaced by btAlignedObjectArray
#include "btLemkeAlgorithm.h"
#undef BT_DEBUG_OSTREAM
#ifdef BT_DEBUG_OSTREAM
using namespace std;
#endif //BT_DEBUG_OSTREAM
btScalar btMachEps()
{
static bool calculated=false;
static btScalar machEps = btScalar(1.);
if (!calculated)
{
do {
machEps /= btScalar(2.0);
// If next epsilon yields 1, then break, because current
// epsilon is the machine epsilon.
}
while ((btScalar)(1.0 + (machEps/btScalar(2.0))) != btScalar(1.0));
// printf( "\nCalculated Machine epsilon: %G\n", machEps );
calculated=true;
}
return machEps;
}
btScalar btEpsRoot() {
static btScalar epsroot = 0.;
static bool alreadyCalculated = false;
if (!alreadyCalculated) {
epsroot = btSqrt(btMachEps());
alreadyCalculated = true;
}
return epsroot;
}
btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/)
{
steps = 0;
int dim = m_q.size();
#ifdef BT_DEBUG_OSTREAM
if(DEBUGLEVEL >= 1) {
cout << "Dimension = " << dim << endl;
}
#endif //BT_DEBUG_OSTREAM
btVectorXu solutionVector(2 * dim);
solutionVector.setZero();
//, INIT, 0.);
btMatrixXu ident(dim, dim);
ident.setIdentity();
#ifdef BT_DEBUG_OSTREAM
cout << m_M << std::endl;
#endif
btMatrixXu mNeg = m_M.negative();
btMatrixXu A(dim, 2 * dim + 2);
//
A.setSubMatrix(0, 0, dim - 1, dim - 1,ident);
A.setSubMatrix(0, dim, dim - 1, 2 * dim - 1,mNeg);
A.setSubMatrix(0, 2 * dim, dim - 1, 2 * dim, -1.f);
A.setSubMatrix(0, 2 * dim + 1, dim - 1, 2 * dim + 1,m_q);
#ifdef BT_DEBUG_OSTREAM
cout << A << std::endl;
#endif //BT_DEBUG_OSTREAM
// btVectorXu q_;
// q_ >> A(0, 2 * dim + 1, dim - 1, 2 * dim + 1);
btAlignedObjectArray<int> basis;
//At first, all w-values are in the basis
for (int i = 0; i < dim; i++)
basis.push_back(i);
int pivotRowIndex = -1;
btScalar minValue = 1e30f;
bool greaterZero = true;
for (int i=0;i<dim;i++)
{
btScalar v =A(i,2*dim+1);
if (v<minValue)
{
minValue=v;
pivotRowIndex = i;
}
if (v<0)
greaterZero = false;
}
// int pivotRowIndex = q_.minIndex();//minIndex(q_); // first row is that with lowest q-value
int z0Row = pivotRowIndex; // remember the col of z0 for ending algorithm afterwards
int pivotColIndex = 2 * dim; // first col is that of z0
#ifdef BT_DEBUG_OSTREAM
if (DEBUGLEVEL >= 3)
{
// cout << "A: " << A << endl;
cout << "pivotRowIndex " << pivotRowIndex << endl;
cout << "pivotColIndex " << pivotColIndex << endl;
cout << "Basis: ";
for (int i = 0; i < basis.size(); i++)
cout << basis[i] << " ";
cout << endl;
}
#endif //BT_DEBUG_OSTREAM
if (!greaterZero)
{
if (maxloops == 0) {
maxloops = 100;
// maxloops = UINT_MAX; //TODO: not a really nice way, problem is: maxloops should be 2^dim (=1<<dim), but this could exceed UINT_MAX and thus the result would be 0 and therefore the lemke algorithm wouldn't start but probably would find a solution within less then UINT_MAX steps. Therefore this constant is used as a upper border right now...
}
/*start looping*/
for(steps = 0; steps < maxloops; steps++) {
GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
#ifdef BT_DEBUG_OSTREAM
if (DEBUGLEVEL >= 3) {
// cout << "A: " << A << endl;
cout << "pivotRowIndex " << pivotRowIndex << endl;
cout << "pivotColIndex " << pivotColIndex << endl;
cout << "Basis: ";
for (int i = 0; i < basis.size(); i++)
cout << basis[i] << " ";
cout << endl;
}
#endif //BT_DEBUG_OSTREAM
int pivotColIndexOld = pivotColIndex;
/*find new column index */
if (basis[pivotRowIndex] < dim) //if a w-value left the basis get in the correspondent z-value
pivotColIndex = basis[pivotRowIndex] + dim;
else
//else do it the other way round and get in the corresponding w-value
pivotColIndex = basis[pivotRowIndex] - dim;
/*the column becomes part of the basis*/
basis[pivotRowIndex] = pivotColIndexOld;
pivotRowIndex = findLexicographicMinimum(A, pivotColIndex);
if(z0Row == pivotRowIndex) { //if z0 leaves the basis the solution is found --> one last elimination step is necessary
GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
basis[pivotRowIndex] = pivotColIndex; //update basis
break;
}
}
#ifdef BT_DEBUG_OSTREAM
if(DEBUGLEVEL >= 1) {
cout << "Number of loops: " << steps << endl;
cout << "Number of maximal loops: " << maxloops << endl;
}
#endif //BT_DEBUG_OSTREAM
if(!validBasis(basis)) {
info = -1;
#ifdef BT_DEBUG_OSTREAM
if(DEBUGLEVEL >= 1)
cerr << "Lemke-Algorithm ended with Ray-Termination (no valid solution)." << endl;
#endif //BT_DEBUG_OSTREAM
return solutionVector;
}
}
#ifdef BT_DEBUG_OSTREAM
if (DEBUGLEVEL >= 2) {
// cout << "A: " << A << endl;
cout << "pivotRowIndex " << pivotRowIndex << endl;
cout << "pivotColIndex " << pivotColIndex << endl;
}
#endif //BT_DEBUG_OSTREAM
for (int i = 0; i < basis.size(); i++)
{
solutionVector[basis[i]] = A(i,2*dim+1);//q_[i];
}
info = 0;
return solutionVector;
}
int btLemkeAlgorithm::findLexicographicMinimum(const btMatrixXu& A, const int & pivotColIndex) {
int RowIndex = 0;
int dim = A.rows();
btAlignedObjectArray<btVectorXu> Rows;
for (int row = 0; row < dim; row++)
{
btVectorXu vec(dim + 1);
vec.setZero();//, INIT, 0.)
Rows.push_back(vec);
btScalar a = A(row, pivotColIndex);
if (a > 0) {
Rows[row][0] = A(row, 2 * dim + 1) / a;
Rows[row][1] = A(row, 2 * dim) / a;
for (int j = 2; j < dim + 1; j++)
Rows[row][j] = A(row, j - 1) / a;
#ifdef BT_DEBUG_OSTREAM
// if (DEBUGLEVEL) {
// cout << "Rows(" << row << ") = " << Rows[row] << endl;
// }
#endif
}
}
for (int i = 0; i < Rows.size(); i++)
{
if (Rows[i].nrm2() > 0.) {
int j = 0;
for (; j < Rows.size(); j++)
{
if(i != j)
{
if(Rows[j].nrm2() > 0.)
{
btVectorXu test(dim + 1);
for (int ii=0;ii<dim+1;ii++)
{
test[ii] = Rows[j][ii] - Rows[i][ii];
}
//=Rows[j] - Rows[i]
if (! LexicographicPositive(test))
break;
}
}
}
if (j == Rows.size())
{
RowIndex += i;
break;
}
}
}
return RowIndex;
}
bool btLemkeAlgorithm::LexicographicPositive(const btVectorXu & v)
{
int i = 0;
// if (DEBUGLEVEL)
// cout << "v " << v << endl;
while(i < v.size()-1 && fabs(v[i]) < btMachEps())
i++;
if (v[i] > 0)
return true;
return false;
}
void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis)
{
btScalar a = -1 / A(pivotRowIndex, pivotColumnIndex);
#ifdef BT_DEBUG_OSTREAM
cout << A << std::endl;
#endif
for (int i = 0; i < A.rows(); i++)
{
if (i != pivotRowIndex)
{
for (int j = 0; j < A.cols(); j++)
{
if (j != pivotColumnIndex)
{
btScalar v = A(i, j);
v += A(pivotRowIndex, j) * A(i, pivotColumnIndex) * a;
A.setElem(i, j, v);
}
}
}
}
#ifdef BT_DEBUG_OSTREAM
cout << A << std::endl;
#endif //BT_DEBUG_OSTREAM
for (int i = 0; i < A.cols(); i++)
{
A.mulElem(pivotRowIndex, i,-a);
}
#ifdef BT_DEBUG_OSTREAM
cout << A << std::endl;
#endif //#ifdef BT_DEBUG_OSTREAM
for (int i = 0; i < A.rows(); i++)
{
if (i != pivotRowIndex)
{
A.setElem(i, pivotColumnIndex,0);
}
}
#ifdef BT_DEBUG_OSTREAM
cout << A << std::endl;
#endif //#ifdef BT_DEBUG_OSTREAM
}
bool btLemkeAlgorithm::greaterZero(const btVectorXu & vector)
{
bool isGreater = true;
for (int i = 0; i < vector.size(); i++) {
if (vector[i] < 0) {
isGreater = false;
break;
}
}
return isGreater;
}
bool btLemkeAlgorithm::validBasis(const btAlignedObjectArray<int>& basis)
{
bool isValid = true;
for (int i = 0; i < basis.size(); i++) {
if (basis[i] >= basis.size() * 2) { //then z0 is in the base
isValid = false;
break;
}
}
return isValid;
}

108
Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h Normal file
View file

@ -0,0 +1,108 @@
/* Copyright (C) 2004-2013 MBSim Development Team
Code was converted for the Bullet Continuous Collision Detection and Physics Library
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.
*/
//The original version is here
//https://code.google.com/p/mbsim-env/source/browse/trunk/kernel/mbsim/numerics/linear_complementarity_problem/lemke_algorithm.cc
//This file is re-distributed under the ZLib license, with permission of the original author (Kilian Grundl)
//Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h
//STL/std::vector replaced by btAlignedObjectArray
#ifndef BT_NUMERICS_LEMKE_ALGORITHM_H_
#define BT_NUMERICS_LEMKE_ALGORITHM_H_
#include "LinearMath/btMatrixX.h"
#include <vector> //todo: replace by btAlignedObjectArray
class btLemkeAlgorithm
{
public:
btLemkeAlgorithm(const btMatrixXu& M_, const btVectorXu& q_, const int & DEBUGLEVEL_ = 0) :
DEBUGLEVEL(DEBUGLEVEL_)
{
setSystem(M_, q_);
}
/* GETTER / SETTER */
/**
* \brief return info of solution process
*/
int getInfo() {
return info;
}
/**
* \brief get the number of steps until the solution was found
*/
int getSteps(void) {
return steps;
}
/**
* \brief set system with Matrix M and vector q
*/
void setSystem(const btMatrixXu & M_, const btVectorXu & q_)
{
m_M = M_;
m_q = q_;
}
/***************************************************/
/**
* \brief solve algorithm adapted from : Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd)
*/
btVectorXu solve(unsigned int maxloops = 0);
virtual ~btLemkeAlgorithm() {
}
protected:
int findLexicographicMinimum(const btMatrixXu &A, const int & pivotColIndex);
bool LexicographicPositive(const btVectorXu & v);
void GaussJordanEliminationStep(btMatrixXu &A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis);
bool greaterZero(const btVectorXu & vector);
bool validBasis(const btAlignedObjectArray<int>& basis);
btMatrixXu m_M;
btVectorXu m_q;
/**
* \brief number of steps until the Lemke algorithm found a solution
*/
unsigned int steps;
/**
* \brief define level of debug output
*/
int DEBUGLEVEL;
/**
* \brief did the algorithm find a solution
*
* -1 : not successful
* 0 : successful
*/
int info;
};
#endif /* BT_NUMERICS_LEMKE_ALGORITHM_H_ */

350
Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h Normal file
View file

@ -0,0 +1,350 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
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.
*/
///original version written by Erwin Coumans, October 2013
#ifndef BT_LEMKE_SOLVER_H
#define BT_LEMKE_SOLVER_H
#include "btMLCPSolverInterface.h"
#include "btLemkeAlgorithm.h"
///The btLemkeSolver is based on "Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd) "
///It is a slower but more accurate solver. Increase the m_maxLoops for better convergence, at the cost of more CPU time.
///The original implementation of the btLemkeAlgorithm was done by Kilian Grundl from the MBSim team
class btLemkeSolver : public btMLCPSolverInterface
{
protected:
public:
btScalar m_maxValue;
int m_debugLevel;
int m_maxLoops;
bool m_useLoHighBounds;
btLemkeSolver()
:m_maxValue(100000),
m_debugLevel(0),
m_maxLoops(1000),
m_useLoHighBounds(true)
{
}
virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
{
if (m_useLoHighBounds)
{
BT_PROFILE("btLemkeSolver::solveMLCP");
int n = A.rows();
if (0==n)
return true;
bool fail = false;
btVectorXu solution(n);
btVectorXu q1;
q1.resize(n);
for (int row=0;row<n;row++)
{
q1[row] = -b[row];
}
// cout << "A" << endl;
// cout << A << endl;
/////////////////////////////////////
//slow matrix inversion, replace with LU decomposition
btMatrixXu A1;
btMatrixXu B(n,n);
{
BT_PROFILE("inverse(slow)");
A1.resize(A.rows(),A.cols());
for (int row=0;row<A.rows();row++)
{
for (int col=0;col<A.cols();col++)
{
A1.setElem(row,col,A(row,col));
}
}
btMatrixXu matrix;
matrix.resize(n,2*n);
for (int row=0;row<n;row++)
{
for (int col=0;col<n;col++)
{
matrix.setElem(row,col,A1(row,col));
}
}
btScalar ratio,a;
int i,j,k;
for(i = 0; i < n; i++){
for(j = n; j < 2*n; j++){
if(i==(j-n))
matrix.setElem(i,j,1.0);
else
matrix.setElem(i,j,0.0);
}
}
for(i = 0; i < n; i++){
for(j = 0; j < n; j++){
if(i!=j)
{
btScalar v = matrix(i,i);
if (btFuzzyZero(v))
{
a = 0.000001f;
}
ratio = matrix(j,i)/matrix(i,i);
for(k = 0; k < 2*n; k++){
matrix.addElem(j,k,- ratio * matrix(i,k));
}
}
}
}
for(i = 0; i < n; i++){
a = matrix(i,i);
if (btFuzzyZero(a))
{
a = 0.000001f;
}
btScalar invA = 1.f/a;
for(j = 0; j < 2*n; j++){
matrix.mulElem(i,j,invA);
}
}
for (int row=0;row<n;row++)
{
for (int col=0;col<n;col++)
{
B.setElem(row,col,matrix(row,n+col));
}
}
}
btMatrixXu b1(n,1);
btMatrixXu M(n*2,n*2);
for (int row=0;row<n;row++)
{
b1.setElem(row,0,-b[row]);
for (int col=0;col<n;col++)
{
btScalar v =B(row,col);
M.setElem(row,col,v);
M.setElem(n+row,n+col,v);
M.setElem(n+row,col,-v);
M.setElem(row,n+col,-v);
}
}
btMatrixXu Bb1 = B*b1;
// q = [ (-B*b1 - lo)' (hi + B*b1)' ]'
btVectorXu qq;
qq.resize(n*2);
for (int row=0;row<n;row++)
{
qq[row] = -Bb1(row,0)-lo[row];
qq[n+row] = Bb1(row,0)+hi[row];
}
btVectorXu z1;
btMatrixXu y1;
y1.resize(n,1);
btLemkeAlgorithm lemke(M,qq,m_debugLevel);
{
BT_PROFILE("lemke.solve");
lemke.setSystem(M,qq);
z1 = lemke.solve(m_maxLoops);
}
for (int row=0;row<n;row++)
{
y1.setElem(row,0,z1[2*n+row]-z1[3*n+row]);
}
btMatrixXu y1_b1(n,1);
for (int i=0;i<n;i++)
{
y1_b1.setElem(i,0,y1(i,0)-b1(i,0));
}
btMatrixXu x1;
x1 = B*(y1_b1);
for (int row=0;row<n;row++)
{
solution[row] = x1(row,0);//n];
}
int errorIndexMax = -1;
int errorIndexMin = -1;
float errorValueMax = -1e30;
float errorValueMin = 1e30;
for (int i=0;i<n;i++)
{
x[i] = solution[i];
volatile btScalar check = x[i];
if (x[i] != check)
{
//printf("Lemke result is #NAN\n");
x.setZero();
return false;
}
//this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver
//we need to figure out why it happens, and fix it, or detect it properly)
if (x[i]>m_maxValue)
{
if (x[i]> errorValueMax)
{
fail = true;
errorIndexMax = i;
errorValueMax = x[i];
}
////printf("x[i] = %f,",x[i]);
}
if (x[i]<-m_maxValue)
{
if (x[i]<errorValueMin)
{
errorIndexMin = i;
errorValueMin = x[i];
fail = true;
//printf("x[i] = %f,",x[i]);
}
}
}
if (fail)
{
int m_errorCountTimes = 0;
if (errorIndexMin<0)
errorValueMin = 0.f;
if (errorIndexMax<0)
errorValueMax = 0.f;
m_errorCountTimes++;
// printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
for (int i=0;i<n;i++)
{
x[i]=0.f;
}
}
return !fail;
} else
{
int dimension = A.rows();
if (0==dimension)
return true;
// printf("================ solving using Lemke/Newton/Fixpoint\n");
btVectorXu q;
q.resize(dimension);
for (int row=0;row<dimension;row++)
{
q[row] = -b[row];
}
btLemkeAlgorithm lemke(A,q,m_debugLevel);
lemke.setSystem(A,q);
btVectorXu solution = lemke.solve(m_maxLoops);
//check solution
bool fail = false;
int errorIndexMax = -1;
int errorIndexMin = -1;
float errorValueMax = -1e30;
float errorValueMin = 1e30;
for (int i=0;i<dimension;i++)
{
x[i] = solution[i+dimension];
volatile btScalar check = x[i];
if (x[i] != check)
{
x.setZero();
return false;
}
//this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver
//we need to figure out why it happens, and fix it, or detect it properly)
if (x[i]>m_maxValue)
{
if (x[i]> errorValueMax)
{
fail = true;
errorIndexMax = i;
errorValueMax = x[i];
}
////printf("x[i] = %f,",x[i]);
}
if (x[i]<-m_maxValue)
{
if (x[i]<errorValueMin)
{
errorIndexMin = i;
errorValueMin = x[i];
fail = true;
//printf("x[i] = %f,",x[i]);
}
}
}
if (fail)
{
static int errorCountTimes = 0;
if (errorIndexMin<0)
errorValueMin = 0.f;
if (errorIndexMax<0)
errorValueMax = 0.f;
printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
for (int i=0;i<dimension;i++)
{
x[i]=0.f;
}
}
return !fail;
}
return true;
}
};
#endif //BT_LEMKE_SOLVER_H

639
Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp Normal file
View file

@ -0,0 +1,639 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
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.
*/
///original version written by Erwin Coumans, October 2013
#include "btMLCPSolver.h"
#include "LinearMath/btMatrixX.h"
#include "LinearMath/btQuickprof.h"
#include "btSolveProjectedGaussSeidel.h"
btMLCPSolver::btMLCPSolver( btMLCPSolverInterface* solver)
:m_solver(solver),
m_fallback(0)
{
}
btMLCPSolver::~btMLCPSolver()
{
}
bool gUseMatrixMultiply = false;
bool interleaveContactAndFriction = false;
btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodiesUnUsed, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
{
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies, numBodiesUnUsed, manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
{
BT_PROFILE("gather constraint data");
int numFrictionPerContact = m_tmpSolverContactConstraintPool.size()==m_tmpSolverContactFrictionConstraintPool.size()? 1 : 2;
// int numBodies = m_tmpSolverBodyPool.size();
m_allConstraintPtrArray.resize(0);
m_limitDependencies.resize(m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size());
btAssert(m_limitDependencies.size() == m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size());
// printf("m_limitDependencies.size() = %d\n",m_limitDependencies.size());
int dindex = 0;
for (int i=0;i<m_tmpSolverNonContactConstraintPool.size();i++)
{
m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]);
m_limitDependencies[dindex++] = -1;
}
///The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
int firstContactConstraintOffset=dindex;
if (interleaveContactAndFriction)
{
for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
{
m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
m_limitDependencies[dindex++] = -1;
m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact]);
int findex = (m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact].m_frictionIndex*(1+numFrictionPerContact));
m_limitDependencies[dindex++] = findex +firstContactConstraintOffset;
if (numFrictionPerContact==2)
{
m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]);
m_limitDependencies[dindex++] = findex+firstContactConstraintOffset;
}
}
} else
{
for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
{
m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
m_limitDependencies[dindex++] = -1;
}
for (int i=0;i<m_tmpSolverContactFrictionConstraintPool.size();i++)
{
m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]);
m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex+firstContactConstraintOffset;
}
}
if (!m_allConstraintPtrArray.size())
{
m_A.resize(0,0);
m_b.resize(0);
m_x.resize(0);
m_lo.resize(0);
m_hi.resize(0);
return 0.f;
}
}
if (gUseMatrixMultiply)
{
BT_PROFILE("createMLCP");
createMLCP(infoGlobal);
}
else
{
BT_PROFILE("createMLCPFast");
createMLCPFast(infoGlobal);
}
return 0.f;
}
bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
{
bool result = true;
if (m_A.rows()==0)
return true;
//if using split impulse, we solve 2 separate (M)LCPs
if (infoGlobal.m_splitImpulse)
{
btMatrixXu Acopy = m_A;
btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies;
// printf("solve first LCP\n");
result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations );
if (result)
result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo,m_hi, limitDependenciesCopy,infoGlobal.m_numIterations );
} else
{
result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations );
}
return result;
}
struct btJointNode
{
int jointIndex; // pointer to enclosing dxJoint object
int otherBodyIndex; // *other* body this joint is connected to
int nextJointNodeIndex;//-1 for null
int constraintRowIndex;
};
void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
{
int numContactRows = interleaveContactAndFriction ? 3 : 1;
int numConstraintRows = m_allConstraintPtrArray.size();
int n = numConstraintRows;
{
BT_PROFILE("init b (rhs)");
m_b.resize(numConstraintRows);
m_bSplit.resize(numConstraintRows);
m_b.setZero();
m_bSplit.setZero();
for (int i=0;i<numConstraintRows ;i++)
{
btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv;
if (!btFuzzyZero(jacDiag))
{
btScalar rhs = m_allConstraintPtrArray[i]->m_rhs;
btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration;
m_b[i]=rhs/jacDiag;
m_bSplit[i] = rhsPenetration/jacDiag;
}
}
}
// btScalar* w = 0;
// int nub = 0;
m_lo.resize(numConstraintRows);
m_hi.resize(numConstraintRows);
{
BT_PROFILE("init lo/ho");
for (int i=0;i<numConstraintRows;i++)
{
if (0)//m_limitDependencies[i]>=0)
{
m_lo[i] = -BT_INFINITY;
m_hi[i] = BT_INFINITY;
} else
{
m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
}
}
}
//
int m=m_allConstraintPtrArray.size();
int numBodies = m_tmpSolverBodyPool.size();
btAlignedObjectArray<int> bodyJointNodeArray;
{
BT_PROFILE("bodyJointNodeArray.resize");
bodyJointNodeArray.resize(numBodies,-1);
}
btAlignedObjectArray<btJointNode> jointNodeArray;
{
BT_PROFILE("jointNodeArray.reserve");
jointNodeArray.reserve(2*m_allConstraintPtrArray.size());
}
btMatrixXu& J3 = m_scratchJ3;
{
BT_PROFILE("J3.resize");
J3.resize(2*m,8);
}
btMatrixXu& JinvM3 = m_scratchJInvM3;
{
BT_PROFILE("JinvM3.resize/setZero");
JinvM3.resize(2*m,8);
JinvM3.setZero();
J3.setZero();
}
int cur=0;
int rowOffset = 0;
btAlignedObjectArray<int>& ofs = m_scratchOfs;
{
BT_PROFILE("ofs resize");
ofs.resize(0);
ofs.resizeNoInitialize(m_allConstraintPtrArray.size());
}
{
BT_PROFILE("Compute J and JinvM");
int c=0;
int numRows = 0;
for (int i=0;i<m_allConstraintPtrArray.size();i+=numRows,c++)
{
ofs[c] = rowOffset;
int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
numRows = i<m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows ;
if (orgBodyA)
{
{
int slotA=-1;
//find free jointNode slot for sbA
slotA =jointNodeArray.size();
jointNodeArray.expand();//NonInitializing();
int prevSlot = bodyJointNodeArray[sbA];
bodyJointNodeArray[sbA] = slotA;
jointNodeArray[slotA].nextJointNodeIndex = prevSlot;
jointNodeArray[slotA].jointIndex = c;
jointNodeArray[slotA].constraintRowIndex = i;
jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1;
}
for (int row=0;row<numRows;row++,cur++)
{
btVector3 normalInvMass = m_allConstraintPtrArray[i+row]->m_contactNormal1 * orgBodyA->getInvMass();
btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld();
for (int r=0;r<3;r++)
{
J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal1[r]);
J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal[r]);
JinvM3.setElem(cur,r,normalInvMass[r]);
JinvM3.setElem(cur,r+4,relPosCrossNormalInvInertia[r]);
}
J3.setElem(cur,3,0);
JinvM3.setElem(cur,3,0);
J3.setElem(cur,7,0);
JinvM3.setElem(cur,7,0);
}
} else
{
cur += numRows;
}
if (orgBodyB)
{
{
int slotB=-1;
//find free jointNode slot for sbA
slotB =jointNodeArray.size();
jointNodeArray.expand();//NonInitializing();
int prevSlot = bodyJointNodeArray[sbB];
bodyJointNodeArray[sbB] = slotB;
jointNodeArray[slotB].nextJointNodeIndex = prevSlot;
jointNodeArray[slotB].jointIndex = c;
jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1;
jointNodeArray[slotB].constraintRowIndex = i;
}
for (int row=0;row<numRows;row++,cur++)
{
btVector3 normalInvMassB = m_allConstraintPtrArray[i+row]->m_contactNormal2*orgBodyB->getInvMass();
btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld();
for (int r=0;r<3;r++)
{
J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal2[r]);
J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal[r]);
JinvM3.setElem(cur,r,normalInvMassB[r]);
JinvM3.setElem(cur,r+4,relPosInvInertiaB[r]);
}
J3.setElem(cur,3,0);
JinvM3.setElem(cur,3,0);
J3.setElem(cur,7,0);
JinvM3.setElem(cur,7,0);
}
}
else
{
cur += numRows;
}
rowOffset+=numRows;
}
}
//compute JinvM = J*invM.
const btScalar* JinvM = JinvM3.getBufferPointer();
const btScalar* Jptr = J3.getBufferPointer();
{
BT_PROFILE("m_A.resize");
m_A.resize(n,n);
}
{
BT_PROFILE("m_A.setZero");
m_A.setZero();
}
int c=0;
{
int numRows = 0;
BT_PROFILE("Compute A");
for (int i=0;i<m_allConstraintPtrArray.size();i+= numRows,c++)
{
int row__ = ofs[c];
int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
// btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
// btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
numRows = i<m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows ;
const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__;
{
int startJointNodeA = bodyJointNodeArray[sbA];
while (startJointNodeA>=0)
{
int j0 = jointNodeArray[startJointNodeA].jointIndex;
int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
if (j0<c)
{
int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows;
size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8*numRowsOther : 0;
//printf("%d joint i %d and j0: %d: ",count++,i,j0);
m_A.multiplyAdd2_p8r ( JinvMrow,
Jptr + 2*8*(size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__,ofs[j0]);
}
startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
}
}
{
int startJointNodeB = bodyJointNodeArray[sbB];
while (startJointNodeB>=0)
{
int j1 = jointNodeArray[startJointNodeB].jointIndex;
int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
if (j1<c)
{
int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows;
size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8*numRowsOther : 0;
m_A.multiplyAdd2_p8r ( JinvMrow + 8*(size_t)numRows,
Jptr + 2*8*(size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__,ofs[j1]);
}
startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex;
}
}
}
{
BT_PROFILE("compute diagonal");
// compute diagonal blocks of m_A
int row__ = 0;
int numJointRows = m_allConstraintPtrArray.size();
int jj=0;
for (;row__<numJointRows;)
{
//int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA;
int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB;
// btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows;
const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__;
const btScalar *Jrow = Jptr + 2*8*(size_t)row__;
m_A.multiply2_p8r (JinvMrow, Jrow, infom, infom, row__,row__);
if (orgBodyB)
{
m_A.multiplyAdd2_p8r (JinvMrow + 8*(size_t)infom, Jrow + 8*(size_t)infom, infom, infom, row__,row__);
}
row__ += infom;
jj++;
}
}
}
if (1)
{
// add cfm to the diagonal of m_A
for ( int i=0; i<m_A.rows(); ++i)
{
m_A.setElem(i,i,m_A(i,i)+ infoGlobal.m_globalCfm/ infoGlobal.m_timeStep);
}
}
///fill the upper triangle of the matrix, to make it symmetric
{
BT_PROFILE("fill the upper triangle ");
m_A.copyLowerToUpperTriangle();
}
{
BT_PROFILE("resize/init x");
m_x.resize(numConstraintRows);
m_xSplit.resize(numConstraintRows);
if (infoGlobal.m_solverMode&SOLVER_USE_WARMSTARTING)
{
for (int i=0;i<m_allConstraintPtrArray.size();i++)
{
const btSolverConstraint& c = *m_allConstraintPtrArray[i];
m_x[i]=c.m_appliedImpulse;
m_xSplit[i] = c.m_appliedPushImpulse;
}
} else
{
m_x.setZero();
m_xSplit.setZero();
}
}
}
void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
{
int numBodies = this->m_tmpSolverBodyPool.size();
int numConstraintRows = m_allConstraintPtrArray.size();
m_b.resize(numConstraintRows);
if (infoGlobal.m_splitImpulse)
m_bSplit.resize(numConstraintRows);
m_bSplit.setZero();
m_b.setZero();
for (int i=0;i<numConstraintRows ;i++)
{
if (m_allConstraintPtrArray[i]->m_jacDiagABInv)
{
m_b[i]=m_allConstraintPtrArray[i]->m_rhs/m_allConstraintPtrArray[i]->m_jacDiagABInv;
if (infoGlobal.m_splitImpulse)
m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration/m_allConstraintPtrArray[i]->m_jacDiagABInv;
}
}
btMatrixXu& Minv = m_scratchMInv;
Minv.resize(6*numBodies,6*numBodies);
Minv.setZero();
for (int i=0;i<numBodies;i++)
{
const btSolverBody& rb = m_tmpSolverBodyPool[i];
const btVector3& invMass = rb.m_invMass;
setElem(Minv,i*6+0,i*6+0,invMass[0]);
setElem(Minv,i*6+1,i*6+1,invMass[1]);
setElem(Minv,i*6+2,i*6+2,invMass[2]);
btRigidBody* orgBody = m_tmpSolverBodyPool[i].m_originalBody;
for (int r=0;r<3;r++)
for (int c=0;c<3;c++)
setElem(Minv,i*6+3+r,i*6+3+c,orgBody? orgBody->getInvInertiaTensorWorld()[r][c] : 0);
}
btMatrixXu& J = m_scratchJ;
J.resize(numConstraintRows,6*numBodies);
J.setZero();
m_lo.resize(numConstraintRows);
m_hi.resize(numConstraintRows);
for (int i=0;i<numConstraintRows;i++)
{
m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
int bodyIndex0 = m_allConstraintPtrArray[i]->m_solverBodyIdA;
int bodyIndex1 = m_allConstraintPtrArray[i]->m_solverBodyIdB;
if (m_tmpSolverBodyPool[bodyIndex0].m_originalBody)
{
setElem(J,i,6*bodyIndex0+0,m_allConstraintPtrArray[i]->m_contactNormal1[0]);
setElem(J,i,6*bodyIndex0+1,m_allConstraintPtrArray[i]->m_contactNormal1[1]);
setElem(J,i,6*bodyIndex0+2,m_allConstraintPtrArray[i]->m_contactNormal1[2]);
setElem(J,i,6*bodyIndex0+3,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[0]);
setElem(J,i,6*bodyIndex0+4,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[1]);
setElem(J,i,6*bodyIndex0+5,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[2]);
}
if (m_tmpSolverBodyPool[bodyIndex1].m_originalBody)
{
setElem(J,i,6*bodyIndex1+0,m_allConstraintPtrArray[i]->m_contactNormal2[0]);
setElem(J,i,6*bodyIndex1+1,m_allConstraintPtrArray[i]->m_contactNormal2[1]);
setElem(J,i,6*bodyIndex1+2,m_allConstraintPtrArray[i]->m_contactNormal2[2]);
setElem(J,i,6*bodyIndex1+3,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[0]);
setElem(J,i,6*bodyIndex1+4,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[1]);
setElem(J,i,6*bodyIndex1+5,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[2]);
}
}
btMatrixXu& J_transpose = m_scratchJTranspose;
J_transpose= J.transpose();
btMatrixXu& tmp = m_scratchTmp;
{
{
BT_PROFILE("J*Minv");
tmp = J*Minv;
}
{
BT_PROFILE("J*tmp");
m_A = tmp*J_transpose;
}
}
if (1)
{
// add cfm to the diagonal of m_A
for ( int i=0; i<m_A.rows(); ++i)
{
m_A.setElem(i,i,m_A(i,i)+ infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
}
}
m_x.resize(numConstraintRows);
if (infoGlobal.m_splitImpulse)
m_xSplit.resize(numConstraintRows);
// m_x.setZero();
for (int i=0;i<m_allConstraintPtrArray.size();i++)
{
const btSolverConstraint& c = *m_allConstraintPtrArray[i];
m_x[i]=c.m_appliedImpulse;
if (infoGlobal.m_splitImpulse)
m_xSplit[i] = c.m_appliedPushImpulse;
}
}
btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
{
bool result = true;
{
BT_PROFILE("solveMLCP");
// printf("m_A(%d,%d)\n", m_A.rows(),m_A.cols());
result = solveMLCP(infoGlobal);
}
//check if solution is valid, and otherwise fallback to btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations
if (result)
{
BT_PROFILE("process MLCP results");
for (int i=0;i<m_allConstraintPtrArray.size();i++)
{
{
btSolverConstraint& c = *m_allConstraintPtrArray[i];
int sbA = c.m_solverBodyIdA;
int sbB = c.m_solverBodyIdB;
//btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
// btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
{
btScalar deltaImpulse = m_x[i]-c.m_appliedImpulse;
c.m_appliedImpulse = m_x[i];
solverBodyA.internalApplyImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
solverBodyB.internalApplyImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
}
if (infoGlobal.m_splitImpulse)
{
btScalar deltaImpulse = m_xSplit[i] - c.m_appliedPushImpulse;
solverBodyA.internalApplyPushImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
solverBodyB.internalApplyPushImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
c.m_appliedPushImpulse = m_xSplit[i];
}
}
}
}
else
{
// printf("m_fallback = %d\n",m_fallback);
m_fallback++;
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
}
return 0.f;
}

94
Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h Normal file
View file

@ -0,0 +1,94 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
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.
*/
///original version written by Erwin Coumans, October 2013
#ifndef BT_MLCP_SOLVER_H
#define BT_MLCP_SOLVER_H
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
#include "LinearMath/btMatrixX.h"
#include "BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h"
class btMLCPSolver : public btSequentialImpulseConstraintSolver
{
protected:
btMatrixXu m_A;
btVectorXu m_b;
btVectorXu m_x;
btVectorXu m_lo;
btVectorXu m_hi;
///when using 'split impulse' we solve two separate (M)LCPs
btVectorXu m_bSplit;
btVectorXu m_xSplit;
btVectorXu m_bSplit1;
btVectorXu m_xSplit2;
btAlignedObjectArray<int> m_limitDependencies;
btAlignedObjectArray<btSolverConstraint*> m_allConstraintPtrArray;
btMLCPSolverInterface* m_solver;
int m_fallback;
/// The following scratch variables are not stateful -- contents are cleared prior to each use.
/// They are only cached here to avoid extra memory allocations and deallocations and to ensure
/// that multiple instances of the solver can be run in parallel.
btMatrixXu m_scratchJ3;
btMatrixXu m_scratchJInvM3;
btAlignedObjectArray<int> m_scratchOfs;
btMatrixXu m_scratchMInv;
btMatrixXu m_scratchJ;
btMatrixXu m_scratchJTranspose;
btMatrixXu m_scratchTmp;
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
virtual void createMLCP(const btContactSolverInfo& infoGlobal);
virtual void createMLCPFast(const btContactSolverInfo& infoGlobal);
//return true is it solves the problem successfully
virtual bool solveMLCP(const btContactSolverInfo& infoGlobal);
public:
btMLCPSolver( btMLCPSolverInterface* solver);
virtual ~btMLCPSolver();
void setMLCPSolver(btMLCPSolverInterface* solver)
{
m_solver = solver;
}
int getNumFallbacks() const
{
return m_fallback;
}
void setNumFallbacks(int num)
{
m_fallback = num;
}
virtual btConstraintSolverType getSolverType() const
{
return BT_MLCP_SOLVER;
}
};
#endif //BT_MLCP_SOLVER_H

33
Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h Normal file
View file

@ -0,0 +1,33 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
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.
*/
///original version written by Erwin Coumans, October 2013
#ifndef BT_MLCP_SOLVER_INTERFACE_H
#define BT_MLCP_SOLVER_INTERFACE_H
#include "LinearMath/btMatrixX.h"
class btMLCPSolverInterface
{
public:
virtual ~btMLCPSolverInterface()
{
}
//return true is it solves the problem successfully
virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)=0;
};
#endif //BT_MLCP_SOLVER_INTERFACE_H

151
Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btPATHSolver.h Normal file
View file

@ -0,0 +1,151 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
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.
*/
///original version written by Erwin Coumans, October 2013
#ifndef BT_PATH_SOLVER_H
#define BT_PATH_SOLVER_H
//#define BT_USE_PATH
#ifdef BT_USE_PATH
extern "C" {
#include "PATH/SimpleLCP.h"
#include "PATH/License.h"
#include "PATH/Error_Interface.h"
};
void __stdcall MyError(Void *data, Char *msg)
{
printf("Path Error: %s\n",msg);
}
void __stdcall MyWarning(Void *data, Char *msg)
{
printf("Path Warning: %s\n",msg);
}
Error_Interface e;
#include "btMLCPSolverInterface.h"
#include "Dantzig/lcp.h"
class btPathSolver : public btMLCPSolverInterface
{
public:
btPathSolver()
{
License_SetString("2069810742&Courtesy_License&&&USR&2013&14_12_2011&1000&PATH&GEN&31_12_2013&0_0_0&0&0_0");
e.error_data = 0;
e.warning = MyWarning;
e.error = MyError;
Error_SetInterface(&e);
}
virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
{
MCP_Termination status;
int numVariables = b.rows();
if (0==numVariables)
return true;
/* - variables - the number of variables in the problem
- m_nnz - the number of nonzeros in the M matrix
- m_i - a vector of size m_nnz containing the row indices for M
- m_j - a vector of size m_nnz containing the column indices for M
- m_ij - a vector of size m_nnz containing the data for M
- q - a vector of size variables
- lb - a vector of size variables containing the lower bounds on x
- ub - a vector of size variables containing the upper bounds on x
*/
btAlignedObjectArray<double> values;
btAlignedObjectArray<int> rowIndices;
btAlignedObjectArray<int> colIndices;
for (int i=0;i<A.rows();i++)
{
for (int j=0;j<A.cols();j++)
{
if (A(i,j)!=0.f)
{
//add 1, because Path starts at 1, instead of 0
rowIndices.push_back(i+1);
colIndices.push_back(j+1);
values.push_back(A(i,j));
}
}
}
int numNonZero = rowIndices.size();
btAlignedObjectArray<double> zResult;
zResult.resize(numVariables);
btAlignedObjectArray<double> rhs;
btAlignedObjectArray<double> upperBounds;
btAlignedObjectArray<double> lowerBounds;
for (int i=0;i<numVariables;i++)
{
upperBounds.push_back(hi[i]);
lowerBounds.push_back(lo[i]);
rhs.push_back(-b[i]);
}
SimpleLCP(numVariables,numNonZero,&rowIndices[0],&colIndices[0],&values[0],&rhs[0],&lowerBounds[0],&upperBounds[0], &status, &zResult[0]);
if (status != MCP_Solved)
{
static const char* gReturnMsgs[] = {
"Invalid return",
"MCP_Solved: The problem was solved",
"MCP_NoProgress: A stationary point was found",
"MCP_MajorIterationLimit: Major iteration limit met",
"MCP_MinorIterationLimit: Cumulative minor iteration limit met",
"MCP_TimeLimit: Ran out of time",
"MCP_UserInterrupt: Control-C, typically",
"MCP_BoundError: Problem has a bound error",
"MCP_DomainError: Could not find starting point",
"MCP_Infeasible: Problem has no solution",
"MCP_Error: An error occurred within the code",
"MCP_LicenseError: License could not be found",
"MCP_OK"
};
printf("ERROR: The PATH MCP solver failed: %s\n", gReturnMsgs[(unsigned int)status]);// << std::endl;
printf("using Projected Gauss Seidel fallback\n");
return false;
} else
{
for (int i=0;i<numVariables;i++)
{
x[i] = zResult[i];
//check for #NAN
if (x[i] != zResult[i])
return false;
}
return true;
}
}
};
#endif //BT_USE_PATH
#endif //BT_PATH_SOLVER_H

110
Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h Normal file
View file

@ -0,0 +1,110 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
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.
*/
///original version written by Erwin Coumans, October 2013
#ifndef BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
#define BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
#include "btMLCPSolverInterface.h"
///This solver is mainly for debug/learning purposes: it is functionally equivalent to the btSequentialImpulseConstraintSolver solver, but much slower (it builds the full LCP matrix)
class btSolveProjectedGaussSeidel : public btMLCPSolverInterface
{
public:
btScalar m_leastSquaresResidualThreshold;
btScalar m_leastSquaresResidual;
btSolveProjectedGaussSeidel()
:m_leastSquaresResidualThreshold(0),
m_leastSquaresResidual(0)
{
}
virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
{
if (!A.rows())
return true;
//the A matrix is sparse, so compute the non-zero elements
A.rowComputeNonZeroElements();
//A is a m-n matrix, m rows, n columns
btAssert(A.rows() == b.rows());
int i, j, numRows = A.rows();
btScalar delta;
for (int k = 0; k <numIterations; k++)
{
m_leastSquaresResidual = 0.f;
for (i = 0; i <numRows; i++)
{
delta = 0.0f;
if (useSparsity)
{
for (int h=0;h<A.m_rowNonZeroElements1[i].size();h++)
{
int j = A.m_rowNonZeroElements1[i][h];
if (j != i)//skip main diagonal
{
delta += A(i,j) * x[j];
}
}
} else
{
for (j = 0; j <i; j++)
delta += A(i,j) * x[j];
for (j = i+1; j<numRows; j++)
delta += A(i,j) * x[j];
}
btScalar aDiag = A(i,i);
btScalar xOld = x[i];
x [i] = (b [i] - delta) / aDiag;
btScalar s = 1.f;
if (limitDependency[i]>=0)
{
s = x[limitDependency[i]];
if (s<0)
s=1;
}
if (x[i]<lo[i]*s)
x[i]=lo[i]*s;
if (x[i]>hi[i]*s)
x[i]=hi[i]*s;
btScalar diff = x[i] - xOld;
m_leastSquaresResidual += diff*diff;
}
btScalar eps = m_leastSquaresResidualThreshold;
if ((m_leastSquaresResidual < eps) || (k >=(numIterations-1)))
{
#ifdef VERBOSE_PRINTF_RESIDUAL
printf("totalLenSqr = %f at iteration #%d\n", m_leastSquaresResidual,k);
#endif
break;
}
}
return true;
}
};
#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H

5
Engine/lib/bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
View file

@ -296,8 +296,9 @@ void btRaycastVehicle::updateVehicle( btScalar step )
int i=0;
for (i=0;i<m_wheelInfo.size();i++)
{
btScalar depth;
depth = rayCast( m_wheelInfo[i]);
//btScalar depth;
//depth =
rayCast( m_wheelInfo[i]);
}
updateSuspension(step);

2
Engine/lib/bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.h
View file

@ -58,8 +58,6 @@ public:
};
private:
btScalar m_tau;
btScalar m_damping;
btVehicleRaycaster* m_vehicleRaycaster;
btScalar m_pitchControl;
btScalar m_steeringValue;

20
Engine/lib/bullet/src/BulletDynamics/premake4.lua
View file

@ -1,11 +1,21 @@
project "BulletDynamics"
kind "StaticLib"
targetdir "../../lib"
includedirs {
"..",
}
files {
"**.cpp",
"**.h"
}
"Dynamics/*.cpp",
"Dynamics/*.h",
"ConstraintSolver/*.cpp",
"ConstraintSolver/*.h",
"Featherstone/*.cpp",
"Featherstone/*.h",
"MLCPSolvers/*.cpp",
"MLCPSolvers/*.h",
"Vehicle/*.cpp",
"Vehicle/*.h",
"Character/*.cpp",
"Character/*.h"
}