Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #1895 from erwincoumans/master

Browse Source 
Code-style consistency improvement: _clang-format applied
This commit is contained in:
erwincoumans
2018-09-23 19:22:09 -07:00
committed by GitHub
parent d54c8395ac ab8f16961e
commit cdd56e4641
1773 changed files with 1081087 additions and 474249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
src/BulletDynamics/Character/btCharacterControllerInterface.h
View File

@@ -26,22 +26,21 @@ class btCollisionWorld;
class btCharacterControllerInterface : public btActionInterface
{
public:
btCharacterControllerInterface () {};
virtual ~btCharacterControllerInterface () {};
virtual void setWalkDirection(const btVector3& walkDirection) = 0;
virtual void setVelocityForTimeInterval(const btVector3& velocity, btScalar timeInterval) = 0;
virtual void reset ( btCollisionWorld* collisionWorld ) = 0;
virtual void warp (const btVector3& origin) = 0;
btCharacterControllerInterface(){};
virtual ~btCharacterControllerInterface(){};
virtual void preStep ( btCollisionWorld* collisionWorld) = 0;
virtual void playerStep (btCollisionWorld* collisionWorld, btScalar dt) = 0;
virtual bool canJump () const = 0;
virtual void jump(const btVector3& dir = btVector3(0, 0, 0)) = 0;
virtual void setWalkDirection(const btVector3& walkDirection) = 0;
virtual void setVelocityForTimeInterval(const btVector3& velocity, btScalar timeInterval) = 0;
virtual void reset(btCollisionWorld* collisionWorld) = 0;
virtual void warp(const btVector3& origin) = 0;
virtual bool onGround () const = 0;
virtual void setUpInterpolate (bool value) = 0;
virtual void preStep(btCollisionWorld* collisionWorld) = 0;
virtual void playerStep(btCollisionWorld* collisionWorld, btScalar dt) = 0;
virtual bool canJump() const = 0;
virtual void jump(const btVector3& dir = btVector3(0, 0, 0)) = 0;
virtual bool onGround() const = 0;
virtual void setUpInterpolate(bool value) = 0;
};
#endif //BT_CHARACTER_CONTROLLER_INTERFACE_H
#endif //BT_CHARACTER_CONTROLLER_INTERFACE_H

364
src/BulletDynamics/Character/btKinematicCharacterController.cpp
View File

@@ -13,7 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include <stdio.h>
#include "LinearMath/btIDebugDraw.h"
#include "BulletCollision/CollisionDispatch/btGhostObject.h"
@@ -24,20 +23,19 @@ subject to the following restrictions:
#include "LinearMath/btDefaultMotionState.h"
#include "btKinematicCharacterController.h"
// static helper method
static btVector3
getNormalizedVector(const btVector3& v)
{
btVector3 n(0, 0, 0);
if (v.length() > SIMD_EPSILON) {
if (v.length() > SIMD_EPSILON)
{
n = v.normalized();
}
return n;
}
///@todo Interact with dynamic objects,
///Ride kinematicly animated platforms properly
///More realistic (or maybe just a config option) falling
@@ -47,18 +45,19 @@ getNormalizedVector(const btVector3& v)
class btKinematicClosestNotMeRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
{
public:
btKinematicClosestNotMeRayResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
btKinematicClosestNotMeRayResultCallback(btCollisionObject* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
{
m_me = me;
}
virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace)
virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace)
{
if (rayResult.m_collisionObject == m_me)
return 1.0;
return ClosestRayResultCallback::addSingleResult (rayResult, normalInWorldSpace);
return ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace);
}
protected:
btCollisionObject* m_me;
};
@@ -66,15 +65,12 @@ protected:
class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
{
public:
btKinematicClosestNotMeConvexResultCallback (btCollisionObject* me, const btVector3& up, btScalar minSlopeDot)
: btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
, m_me(me)
, m_up(up)
, m_minSlopeDot(minSlopeDot)
btKinematicClosestNotMeConvexResultCallback(btCollisionObject* me, const btVector3& up, btScalar minSlopeDot)
: btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0)), m_me(me), m_up(up), m_minSlopeDot(minSlopeDot)
{
}
virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult, bool normalInWorldSpace)
{
if (convexResult.m_hitCollisionObject == m_me)
return btScalar(1.0);
@@ -86,19 +82,22 @@ public:
if (normalInWorldSpace)
{
hitNormalWorld = convexResult.m_hitNormalLocal;
} else
}
else
{
///need to transform normal into worldspace
hitNormalWorld = convexResult.m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal;
hitNormalWorld = convexResult.m_hitCollisionObject->getWorldTransform().getBasis() * convexResult.m_hitNormalLocal;
}
btScalar dotUp = m_up.dot(hitNormalWorld);
if (dotUp < m_minSlopeDot) {
if (dotUp < m_minSlopeDot)
{
return btScalar(1.0);
}
return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
return ClosestConvexResultCallback::addSingleResult(convexResult, normalInWorldSpace);
}
protected:
btCollisionObject* m_me;
const btVector3 m_up;
@@ -110,7 +109,7 @@ protected:
*
* from: http://www-cs-students.stanford.edu/~adityagp/final/node3.html
*/
btVector3 btKinematicCharacterController::computeReflectionDirection (const btVector3& direction, const btVector3& normal)
btVector3 btKinematicCharacterController::computeReflectionDirection(const btVector3& direction, const btVector3& normal)
{
return direction - (btScalar(2.0) * direction.dot(normal)) * normal;
}
@@ -118,7 +117,7 @@ btVector3 btKinematicCharacterController::computeReflectionDirection (const btVe
/*
* Returns the portion of 'direction' that is parallel to 'normal'
*/
btVector3 btKinematicCharacterController::parallelComponent (const btVector3& direction, const btVector3& normal)
btVector3 btKinematicCharacterController::parallelComponent(const btVector3& direction, const btVector3& normal)
{
btScalar magnitude = direction.dot(normal);
return normal * magnitude;
@@ -127,29 +126,29 @@ btVector3 btKinematicCharacterController::parallelComponent (const btVector3& di
/*
* Returns the portion of 'direction' that is perpindicular to 'normal'
*/
btVector3 btKinematicCharacterController::perpindicularComponent (const btVector3& direction, const btVector3& normal)
btVector3 btKinematicCharacterController::perpindicularComponent(const btVector3& direction, const btVector3& normal)
{
return direction - parallelComponent(direction, normal);
}
btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up)
btKinematicCharacterController::btKinematicCharacterController(btPairCachingGhostObject* ghostObject, btConvexShape* convexShape, btScalar stepHeight, const btVector3& up)
{
m_ghostObject = ghostObject;
m_up.setValue(0.0f, 0.0f, 1.0f);
m_jumpAxis.setValue(0.0f, 0.0f, 1.0f);
m_addedMargin = 0.02;
m_walkDirection.setValue(0.0,0.0,0.0);
m_walkDirection.setValue(0.0, 0.0, 0.0);
m_AngVel.setValue(0.0, 0.0, 0.0);
m_useGhostObjectSweepTest = true;
m_useGhostObjectSweepTest = true;
m_turnAngle = btScalar(0.0);
m_convexShape=convexShape;
m_useWalkDirection = true; // use walk direction by default, legacy behavior
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.0 ; // 3G acceleration.
m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s.
m_jumpSpeed = 10.0; // ?
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;
@@ -166,7 +165,7 @@ btKinematicCharacterController::btKinematicCharacterController (btPairCachingGho
setMaxSlope(btRadians(45.0));
}
btKinematicCharacterController::~btKinematicCharacterController ()
btKinematicCharacterController::~btKinematicCharacterController()
{
}
@@ -175,7 +174,7 @@ btPairCachingGhostObject* btKinematicCharacterController::getGhostObject()
return m_ghostObject;
}
bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* collisionWorld)
bool btKinematicCharacterController::recoverFromPenetration(btCollisionWorld* collisionWorld)
{
// Here we must refresh the overlapping paircache as the penetrating movement itself or the
// previous recovery iteration might have used setWorldTransform and pushed us into an object
@@ -186,19 +185,19 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
// paircache and the ghostobject's internal paircache at the same time. /BW
btVector3 minAabb, maxAabb;
m_convexShape->getAabb(m_ghostObject->getWorldTransform(), minAabb,maxAabb);
collisionWorld->getBroadphase()->setAabb(m_ghostObject->getBroadphaseHandle(),
minAabb,
maxAabb,
collisionWorld->getDispatcher());
m_convexShape->getAabb(m_ghostObject->getWorldTransform(), minAabb, maxAabb);
collisionWorld->getBroadphase()->setAabb(m_ghostObject->getBroadphaseHandle(),
minAabb,
maxAabb,
collisionWorld->getDispatcher());
bool penetration = false;
collisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), collisionWorld->getDispatcher());
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);
@@ -206,25 +205,24 @@ 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);
for (int j=0;j<m_manifoldArray.size();j++)
for (int j = 0; j < m_manifoldArray.size(); j++)
{
btPersistentManifold* manifold = m_manifoldArray[j];
btScalar directionSign = manifold->getBody0() == m_ghostObject ? btScalar(-1.0) : btScalar(1.0);
for (int p=0;p<manifold->getNumContacts();p++)
for (int p = 0; p < manifold->getNumContacts(); p++)
{
const btManifoldPoint&pt = manifold->getContactPoint(p);
const btManifoldPoint& pt = manifold->getContactPoint(p);
btScalar dist = pt.getDistance();
@@ -239,22 +237,24 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
//}
m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2);
penetration = true;
} else {
}
else
{
//printf("touching %f\n", dist);
}
}
//manifold->clearManifold();
}
}
btTransform newTrans = m_ghostObject->getWorldTransform();
newTrans.setOrigin(m_currentPosition);
m_ghostObject->setWorldTransform(newTrans);
// printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
// printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
return penetration;
}
void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
void btKinematicCharacterController::stepUp(btCollisionWorld* world)
{
btScalar stepHeight = 0.0f;
if (m_verticalVelocity < 0.0)
@@ -263,8 +263,8 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
// phase 1: up
btTransform start, end;
start.setIdentity ();
end.setIdentity ();
start.setIdentity();
end.setIdentity();
/* FIXME: Handle penetration properly */
start.setOrigin(m_currentPosition);
@@ -272,7 +272,7 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
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);
end.setOrigin(m_targetPosition);
start.setRotation(m_currentOrientation);
end.setRotation(m_targetOrientation);
@@ -280,10 +280,10 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, -m_up, m_maxSlopeCosine);
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
if (m_useGhostObjectSweepTest)
{
m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
}
else
{
@@ -298,7 +298,7 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
// we moved up only a fraction of the step height
m_currentStepOffset = stepHeight * callback.m_closestHitFraction;
if (m_interpolateUp == true)
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
m_currentPosition.setInterpolate3(m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
else
m_currentPosition = m_targetPosition;
}
@@ -329,7 +329,9 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
m_verticalVelocity = 0.0;
m_currentStepOffset = m_stepHeight;
}
} else {
}
else
{
m_currentStepOffset = stepHeight;
m_currentPosition = m_targetPosition;
}
@@ -342,43 +344,44 @@ bool btKinematicCharacterController::needsCollision(const btCollisionObject* bod
return collides;
}
void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag)
void btKinematicCharacterController::updateTargetPositionBasedOnCollision(const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag)
{
btVector3 movementDirection = m_targetPosition - m_currentPosition;
btScalar movementLength = movementDirection.length();
if (movementLength>SIMD_EPSILON)
if (movementLength > SIMD_EPSILON)
{
movementDirection.normalize();
btVector3 reflectDir = computeReflectionDirection (movementDirection, hitNormal);
btVector3 reflectDir = computeReflectionDirection(movementDirection, hitNormal);
reflectDir.normalize();
btVector3 parallelDir, perpindicularDir;
parallelDir = parallelComponent (reflectDir, hitNormal);
perpindicularDir = perpindicularComponent (reflectDir, hitNormal);
parallelDir = parallelComponent(reflectDir, hitNormal);
perpindicularDir = perpindicularComponent(reflectDir, hitNormal);
m_targetPosition = m_currentPosition;
if (0)//tangentMag != 0.0)
if (0) //tangentMag != 0.0)
{
btVector3 parComponent = parallelDir * btScalar (tangentMag*movementLength);
// printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]);
m_targetPosition += parComponent;
btVector3 parComponent = parallelDir * btScalar(tangentMag * movementLength);
// printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]);
m_targetPosition += parComponent;
}
if (normalMag != 0.0)
{
btVector3 perpComponent = perpindicularDir * btScalar (normalMag*movementLength);
// printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]);
btVector3 perpComponent = perpindicularDir * btScalar(normalMag * movementLength);
// printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]);
m_targetPosition += perpComponent;
}
} else
}
else
{
// printf("movementLength don't normalize a zero vector\n");
// printf("movementLength don't normalize a zero vector\n");
}
}
void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* collisionWorld, const btVector3& walkMove)
void btKinematicCharacterController::stepForwardAndStrafe(btCollisionWorld* collisionWorld, const btVector3& walkMove)
{
// printf("m_normalizedDirection=%f,%f,%f\n",
// m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
@@ -387,29 +390,28 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
m_targetPosition = m_currentPosition + walkMove;
start.setIdentity ();
end.setIdentity ();
start.setIdentity();
end.setIdentity();
btScalar fraction = 1.0;
btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
// printf("distance2=%f\n",distance2);
btScalar distance2 = (m_currentPosition - m_targetPosition).length2();
// printf("distance2=%f\n",distance2);
int maxIter = 10;
while (fraction > btScalar(0.01) && maxIter-- > 0)
{
start.setOrigin (m_currentPosition);
end.setOrigin (m_targetPosition);
start.setOrigin(m_currentPosition);
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));
btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, sweepDirNegative, btScalar(0.0));
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
btScalar margin = m_convexShape->getMargin();
m_convexShape->setMargin(margin + m_addedMargin);
@@ -426,18 +428,17 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
}
m_convexShape->setMargin(margin);
fraction -= callback.m_closestHitFraction;
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();
// m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
// m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld);
updateTargetPositionBasedOnCollision(callback.m_hitNormalWorld);
btVector3 currentDir = m_targetPosition - m_currentPosition;
distance2 = currentDir.length2();
if (distance2 > SIMD_EPSILON)
@@ -448,21 +449,21 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
{
break;
}
} else
}
else
{
// printf("currentDir: don't normalize a zero vector\n");
// printf("currentDir: don't normalize a zero vector\n");
break;
}
}
else
{
m_currentPosition = m_targetPosition;
else
{
m_currentPosition = m_targetPosition;
}
}
}
void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt)
void btKinematicCharacterController::stepDown(btCollisionWorld* collisionWorld, btScalar dt)
{
btTransform start, end, end_double;
bool runonce = false;
@@ -475,64 +476,64 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
m_targetPosition -= (step_drop + gravity_drop);*/
btVector3 orig_position = m_targetPosition;
btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
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))
if (downVelocity > 0.0 && downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping))
downVelocity = m_fallSpeed;
btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, m_up, m_maxSlopeCosine);
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
btKinematicClosestNotMeConvexResultCallback callback2(m_ghostObject, m_up, m_maxSlopeCosine);
callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
while (1)
{
start.setIdentity ();
end.setIdentity ();
start.setIdentity();
end.setIdentity();
end_double.setIdentity ();
end_double.setIdentity();
start.setOrigin (m_currentPosition);
end.setOrigin (m_targetPosition);
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);
end_double.setOrigin(m_targetPosition - step_drop);
if (m_useGhostObjectSweepTest)
{
m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, 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 (full) or not (partial)
m_ghostObject->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
m_ghostObject->convexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
}
} else
}
else
{
collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
collisionWorld->convexSweepTest(m_convexShape, start, end, callback, 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);
collisionWorld->convexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
}
}
btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
btScalar downVelocity2 = (m_verticalVelocity < 0.f ? -m_verticalVelocity : 0.f) * dt;
bool has_hit;
if (bounce_fix == true)
has_hit = (callback.hasHit() || callback2.hasHit()) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject);
@@ -543,8 +544,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
if (m_verticalVelocity < 0.0)
stepHeight = m_stepHeight;
if (downVelocity2 > 0.0 && downVelocity2 < stepHeight && has_hit == true && runonce == false
&& (m_wasOnGround || !m_wasJumping))
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
@@ -555,7 +555,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
step_drop = m_up * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
runonce = true;
continue; //re-run previous tests
continue; //re-run previous tests
}
break;
}
@@ -570,30 +570,32 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
if (bounce_fix == true)
{
if (full_drop == true)
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
else
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, fraction);
}
else
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
m_currentPosition.setInterpolate3(m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
full_drop = false;
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
m_wasJumping = false;
} else {
}
else
{
// we dropped the full height
full_drop = true;
if (bounce_fix == true)
{
downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
downVelocity = (m_verticalVelocity < 0.f ? -m_verticalVelocity : 0.f) * dt;
if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping))
{
m_targetPosition += step_drop; //undo previous target change
m_targetPosition += step_drop; //undo previous target change
downVelocity = m_fallSpeed;
step_drop = m_up * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
@@ -605,30 +607,22 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
}
}
void btKinematicCharacterController::setWalkDirection
(
const btVector3& walkDirection
)
void btKinematicCharacterController::setWalkDirection(
const btVector3& walkDirection)
{
m_useWalkDirection = true;
m_walkDirection = walkDirection;
m_normalizedDirection = getNormalizedVector(m_walkDirection);
}
void btKinematicCharacterController::setVelocityForTimeInterval
(
const btVector3& velocity,
btScalar timeInterval
)
void btKinematicCharacterController::setVelocityForTimeInterval(
const btVector3& velocity,
btScalar timeInterval)
{
// printf("setVelocity!\n");
// printf(" interval: %f\n", timeInterval);
// printf(" velocity: (%f, %f, %f)\n",
// velocity.x(), velocity.y(), velocity.z());
// printf("setVelocity!\n");
// printf(" interval: %f\n", timeInterval);
// printf(" velocity: (%f, %f, %f)\n",
// velocity.x(), velocity.y(), velocity.z());
m_useWalkDirection = false;
m_walkDirection = velocity;
@@ -661,7 +655,7 @@ void btKinematicCharacterController::setLinearVelocity(const btVector3& velocity
btVector3 upComponent = m_up * (btSin(SIMD_HALF_PI - btAcos(c)) * m_walkDirection.length());
m_walkDirection -= upComponent;
m_verticalVelocity = (c < 0.0f ? -1 : 1) * upComponent.length();
if (c > 0.0f)
{
m_wasJumping = true;
@@ -678,46 +672,45 @@ btVector3 btKinematicCharacterController::getLinearVelocity() const
return m_walkDirection + (m_verticalVelocity * m_up);
}
void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld )
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)
void btKinematicCharacterController::warp(const btVector3& origin)
{
btTransform xform;
xform.setIdentity();
xform.setOrigin (origin);
m_ghostObject->setWorldTransform (xform);
xform.setOrigin(origin);
m_ghostObject->setWorldTransform(xform);
}
void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld)
void btKinematicCharacterController::preStep(btCollisionWorld* collisionWorld)
{
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]);
// printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);
}
void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt)
void btKinematicCharacterController::playerStep(btCollisionWorld* collisionWorld, btScalar dt)
{
// printf("playerStep(): ");
// printf(" dt = %f", dt);
// printf("playerStep(): ");
// printf(" dt = %f", dt);
if (m_AngVel.length2() > 0.0f)
{
@@ -744,16 +737,17 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo
}
// quick check...
if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0)) {
// printf("\n");
return; // no motion
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
@@ -761,7 +755,7 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo
}
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)
@@ -777,12 +771,12 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo
btTransform xform;
xform = m_ghostObject->getWorldTransform();
// printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
// printf("walkSpeed=%f\n",walkSpeed);
// printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
// printf("walkSpeed=%f\n",walkSpeed);
stepUp(collisionWorld);
//todo: Experimenting with behavior of controller when it hits a ceiling..
//bool hitUp = stepUp (collisionWorld);
//bool hitUp = stepUp (collisionWorld);
//if (hitUp)
//{
// m_verticalVelocity -= m_gravity * dt;
@@ -799,9 +793,12 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo
// xform = m_ghostObject->getWorldTransform();
//}
if (m_useWalkDirection) {
stepForwardAndStrafe (collisionWorld, m_walkDirection);
} else {
if (m_useWalkDirection)
{
stepForwardAndStrafe(collisionWorld, m_walkDirection);
}
else
{
//printf(" time: %f", m_velocityTimeInterval);
// still have some time left for moving!
btScalar dtMoving =
@@ -816,7 +813,7 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo
// okay, step
stepForwardAndStrafe(collisionWorld, move);
}
stepDown (collisionWorld, dt);
stepDown(collisionWorld, dt);
//todo: Experimenting with max jump height
//if (m_wasJumping)
@@ -827,7 +824,7 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo
// // substract the overshoot
// m_currentPosition[m_upAxis] -= ds - m_maxJumpHeight;
// // max height was reached, so potential energy is at max
// // 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;
@@ -835,8 +832,8 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo
//}
// printf("\n");
xform.setOrigin (m_currentPosition);
m_ghostObject->setWorldTransform (xform);
xform.setOrigin(m_currentPosition);
m_ghostObject->setWorldTransform(xform);
int numPenetrationLoops = 0;
m_touchingContact = false;
@@ -852,23 +849,23 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo
}
}
void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
void btKinematicCharacterController::setFallSpeed(btScalar fallSpeed)
{
m_fallSpeed = fallSpeed;
}
void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed)
void btKinematicCharacterController::setJumpSpeed(btScalar jumpSpeed)
{
m_jumpSpeed = jumpSpeed;
m_SetjumpSpeed = m_jumpSpeed;
}
void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight)
void btKinematicCharacterController::setMaxJumpHeight(btScalar maxJumpHeight)
{
m_maxJumpHeight = maxJumpHeight;
}
bool btKinematicCharacterController::canJump () const
bool btKinematicCharacterController::canJump() const
{
return onGround();
}
@@ -927,20 +924,20 @@ btScalar btKinematicCharacterController::getMaxPenetrationDepth() const
return m_maxPenetrationDepth;
}
bool btKinematicCharacterController::onGround () const
bool btKinematicCharacterController::onGround() const
{
return (fabs(m_verticalVelocity) < SIMD_EPSILON) && (fabs(m_verticalOffset) < SIMD_EPSILON);
}
void btKinematicCharacterController::setStepHeight(btScalar h)
void btKinematicCharacterController::setStepHeight(btScalar h)
{
m_stepHeight = h;
}
btVector3* btKinematicCharacterController::getUpAxisDirections()
{
static btVector3 sUpAxisDirection[3] = { btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f) };
static btVector3 sUpAxisDirection[3] = {btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f)};
return sUpAxisDirection;
}
@@ -997,4 +994,3 @@ btQuaternion btKinematicCharacterController::getRotation(btVector3& v0, btVector
return shortestArcQuatNormalize2(v0, v1);
}

114
src/BulletDynamics/Character/btKinematicCharacterController.h
View File

@@ -13,7 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_KINEMATIC_CHARACTER_CONTROLLER_H
#define BT_KINEMATIC_CHARACTER_CONTROLLER_H
@@ -23,7 +22,6 @@ subject to the following restrictions:
#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
class btCollisionShape;
class btConvexShape;
class btRigidBody;
@@ -34,15 +32,15 @@ class btPairCachingGhostObject;
///btKinematicCharacterController is an object that supports a sliding motion in a world.
///It uses a ghost object and convex sweep test to test for upcoming collisions. This is combined with discrete collision detection to recover from penetrations.
///Interaction between btKinematicCharacterController and dynamic rigid bodies needs to be explicity implemented by the user.
ATTRIBUTE_ALIGNED16(class) btKinematicCharacterController : public btCharacterControllerInterface
ATTRIBUTE_ALIGNED16(class)
btKinematicCharacterController : public btCharacterControllerInterface
{
protected:
btScalar m_halfHeight;
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
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;
@@ -50,33 +48,33 @@ protected:
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)
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)
btScalar m_gravity;
btScalar m_turnAngle;
btScalar m_stepHeight;
btScalar m_addedMargin;//@todo: remove this and fix the code
btScalar m_addedMargin; //@todo: remove this and fix the code
///this is the desired walk direction, set by the user
btVector3 m_walkDirection;
btVector3 m_normalizedDirection;
btVector3 m_AngVel;
btVector3 m_walkDirection;
btVector3 m_normalizedDirection;
btVector3 m_AngVel;
btVector3 m_jumpPosition;
btVector3 m_jumpPosition;
//some internal variables
btVector3 m_currentPosition;
btScalar m_currentStepOffset;
btScalar m_currentStepOffset;
btVector3 m_targetPosition;
btQuaternion m_currentOrientation;
btQuaternion m_targetOrientation;
///keep track of the contact manifolds
btManifoldArray m_manifoldArray;
btManifoldArray m_manifoldArray;
bool m_touchingContact;
btVector3 m_touchingNormal;
@@ -84,52 +82,50 @@ protected:
btScalar m_linearDamping;
btScalar m_angularDamping;
bool m_wasOnGround;
bool m_wasJumping;
bool m_useGhostObjectSweepTest;
bool m_useWalkDirection;
btScalar m_velocityTimeInterval;
bool m_wasOnGround;
bool m_wasJumping;
bool m_useGhostObjectSweepTest;
bool m_useWalkDirection;
btScalar m_velocityTimeInterval;
btVector3 m_up;
btVector3 m_jumpAxis;
static btVector3* getUpAxisDirections();
bool m_interpolateUp;
bool full_drop;
bool bounce_fix;
bool m_interpolateUp;
bool full_drop;
bool bounce_fix;
btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal);
btVector3 parallelComponent (const btVector3& direction, const btVector3& normal);
btVector3 perpindicularComponent (const btVector3& direction, const btVector3& normal);
btVector3 computeReflectionDirection(const btVector3& direction, const btVector3& normal);
btVector3 parallelComponent(const btVector3& direction, const btVector3& normal);
btVector3 perpindicularComponent(const btVector3& direction, const btVector3& normal);
bool recoverFromPenetration ( btCollisionWorld* collisionWorld);
void stepUp (btCollisionWorld* collisionWorld);
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);
bool recoverFromPenetration(btCollisionWorld * collisionWorld);
void stepUp(btCollisionWorld * collisionWorld);
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;
btQuaternion getRotation(btVector3 & v0, btVector3 & v1) const;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up = btVector3(1.0,0.0,0.0));
~btKinematicCharacterController ();
btKinematicCharacterController(btPairCachingGhostObject * ghostObject, btConvexShape * convexShape, btScalar stepHeight, const btVector3& up = btVector3(1.0, 0.0, 0.0));
~btKinematicCharacterController();
///btActionInterface interface
virtual void updateAction( btCollisionWorld* collisionWorld,btScalar deltaTime)
virtual void updateAction(btCollisionWorld * collisionWorld, btScalar deltaTime)
{
preStep ( collisionWorld);
playerStep (collisionWorld, deltaTime);
preStep(collisionWorld);
playerStep(collisionWorld, deltaTime);
}
///btActionInterface interface
void debugDraw(btIDebugDraw* debugDrawer);
void debugDraw(btIDebugDraw * debugDrawer);
void setUp(const btVector3& up);
@@ -140,7 +136,7 @@ public:
/// increment the position each simulation iteration, regardless
/// of dt.
/// This call will reset any velocity set by setVelocityForTimeInterval().
virtual void setWalkDirection(const btVector3& walkDirection);
virtual void setWalkDirection(const btVector3& walkDirection);
/// Caller provides a velocity with which the character should move for
/// the given time period. After the time period, velocity is reset
@@ -148,7 +144,7 @@ public:
/// This call will reset any walk direction set by setWalkDirection().
/// Negative time intervals will result in no motion.
virtual void setVelocityForTimeInterval(const btVector3& velocity,
btScalar timeInterval);
btScalar timeInterval);
virtual void setAngularVelocity(const btVector3& velocity);
virtual const btVector3& getAngularVelocity() const;
@@ -157,24 +153,24 @@ public:
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; }
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; }
btScalar getAngularDamping() const { return m_angularDamping; }
void reset ( btCollisionWorld* collisionWorld );
void warp (const btVector3& origin);
void reset(btCollisionWorld * collisionWorld);
void warp(const btVector3& origin);
void preStep ( btCollisionWorld* collisionWorld);
void playerStep ( btCollisionWorld* collisionWorld, btScalar dt);
void preStep(btCollisionWorld * collisionWorld);
void playerStep(btCollisionWorld * collisionWorld, btScalar dt);
void setStepHeight(btScalar h);
btScalar getStepHeight() const { return m_stepHeight; }
void setFallSpeed (btScalar fallSpeed);
void setFallSpeed(btScalar fallSpeed);
btScalar getFallSpeed() const { return m_fallSpeed; }
void setJumpSpeed (btScalar jumpSpeed);
void setJumpSpeed(btScalar jumpSpeed);
btScalar getJumpSpeed() const { return m_jumpSpeed; }
void setMaxJumpHeight (btScalar maxJumpHeight);
bool canJump () const;
void setMaxJumpHeight(btScalar maxJumpHeight);
bool canJump() const;
void jump(const btVector3& v = btVector3(0, 0, 0));
@@ -192,13 +188,13 @@ public:
btScalar getMaxPenetrationDepth() const;
btPairCachingGhostObject* getGhostObject();
void setUseGhostSweepTest(bool useGhostObjectSweepTest)
void setUseGhostSweepTest(bool useGhostObjectSweepTest)
{
m_useGhostObjectSweepTest = useGhostObjectSweepTest;
}
bool onGround () const;
void setUpInterpolate (bool value);
bool onGround() const;
void setUpInterpolate(bool value);
};
#endif // BT_KINEMATIC_CHARACTER_CONTROLLER_H
#endif // BT_KINEMATIC_CHARACTER_CONTROLLER_H

1740
src/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp
View File

File diff suppressed because it is too large Load Diff

62
src/BulletDynamics/ConstraintSolver/btBatchedConstraints.h
View File

@@ -21,46 +21,42 @@ subject to the following restrictions:
#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
#include "BulletDynamics/ConstraintSolver/btSolverConstraint.h"
class btIDebugDraw;
struct btBatchedConstraints
{
enum BatchingMethod
{
BATCHING_METHOD_SPATIAL_GRID_2D,
BATCHING_METHOD_SPATIAL_GRID_3D,
BATCHING_METHOD_COUNT
};
struct Range
{
int begin;
int end;
enum BatchingMethod
{
BATCHING_METHOD_SPATIAL_GRID_2D,
BATCHING_METHOD_SPATIAL_GRID_3D,
BATCHING_METHOD_COUNT
};
struct Range
{
int begin;
int end;
Range() : begin( 0 ), end( 0 ) {}
Range( int _beg, int _end ) : begin( _beg ), end( _end ) {}
};
Range() : begin(0), end(0) {}
Range(int _beg, int _end) : begin(_beg), end(_end) {}
};
btAlignedObjectArray<int> m_constraintIndices;
btAlignedObjectArray<Range> m_batches; // each batch is a range of indices in the m_constraintIndices array
btAlignedObjectArray<Range> m_phases; // each phase is range of indices in the m_batches array
btAlignedObjectArray<char> m_phaseGrainSize; // max grain size for each phase
btAlignedObjectArray<int> m_phaseOrder; // phases can be done in any order, so we can randomize the order here
btIDebugDraw* m_debugDrawer;
btAlignedObjectArray<int> m_constraintIndices;
btAlignedObjectArray<Range> m_batches; // each batch is a range of indices in the m_constraintIndices array
btAlignedObjectArray<Range> m_phases; // each phase is range of indices in the m_batches array
btAlignedObjectArray<char> m_phaseGrainSize; // max grain size for each phase
btAlignedObjectArray<int> m_phaseOrder; // phases can be done in any order, so we can randomize the order here
btIDebugDraw* m_debugDrawer;
static bool s_debugDrawBatches;
static bool s_debugDrawBatches;
btBatchedConstraints() {m_debugDrawer=NULL;}
void setup( btConstraintArray* constraints,
const btAlignedObjectArray<btSolverBody>& bodies,
BatchingMethod batchingMethod,
int minBatchSize,
int maxBatchSize,
btAlignedObjectArray<char>* scratchMemory
);
bool validate( btConstraintArray* constraints, const btAlignedObjectArray<btSolverBody>& bodies ) const;
btBatchedConstraints() { m_debugDrawer = NULL; }
void setup(btConstraintArray* constraints,
const btAlignedObjectArray<btSolverBody>& bodies,
BatchingMethod batchingMethod,
int minBatchSize,
int maxBatchSize,
btAlignedObjectArray<char>* scratchMemory);
bool validate(btConstraintArray* constraints, const btAlignedObjectArray<btSolverBody>& bodies) const;
};
#endif // BT_BATCHED_CONSTRAINTS_H
#endif // BT_BATCHED_CONSTRAINTS_H

610
src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
View File

File diff suppressed because it is too large Load Diff

264
src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
View File

@@ -15,8 +15,6 @@ subject to the following restrictions:
Written by: Marcus Hennix
*/
/*
Overview:
@@ -31,8 +29,6 @@ twist is along the x-axis,
and swing 1 and 2 are along the z and y axes respectively.
*/
#ifndef BT_CONETWISTCONSTRAINT_H
#define BT_CONETWISTCONSTRAINT_H
@@ -41,13 +37,12 @@ and swing 1 and 2 are along the z and y axes respectively.
#include "btTypedConstraint.h"
#ifdef BT_USE_DOUBLE_PRECISION
#define btConeTwistConstraintData2 btConeTwistConstraintDoubleData
#define btConeTwistConstraintDataName "btConeTwistConstraintDoubleData"
#define btConeTwistConstraintData2 btConeTwistConstraintDoubleData
#define btConeTwistConstraintDataName "btConeTwistConstraintDoubleData"
#else
#define btConeTwistConstraintData2 btConeTwistConstraintData
#define btConeTwistConstraintDataName "btConeTwistConstraintData"
#endif //BT_USE_DOUBLE_PRECISION
#define btConeTwistConstraintData2 btConeTwistConstraintData
#define btConeTwistConstraintDataName "btConeTwistConstraintData"
#endif //BT_USE_DOUBLE_PRECISION
class btRigidBody;
@@ -59,103 +54,99 @@ enum btConeTwistFlags
};
///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc)
ATTRIBUTE_ALIGNED16(class) btConeTwistConstraint : public btTypedConstraint
ATTRIBUTE_ALIGNED16(class)
btConeTwistConstraint : public btTypedConstraint
{
#ifdef IN_PARALLELL_SOLVER
public:
#endif
btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
btTransform m_rbAFrame;
btTransform m_rbAFrame;
btTransform m_rbBFrame;
btScalar m_limitSoftness;
btScalar m_biasFactor;
btScalar m_relaxationFactor;
btScalar m_limitSoftness;
btScalar m_biasFactor;
btScalar m_relaxationFactor;
btScalar m_damping;
btScalar m_damping;
btScalar m_swingSpan1;
btScalar m_swingSpan2;
btScalar m_twistSpan;
btScalar m_swingSpan1;
btScalar m_swingSpan2;
btScalar m_twistSpan;
btScalar m_fixThresh;
btScalar m_fixThresh;
btVector3 m_swingAxis;
btVector3 m_twistAxis;
btVector3 m_swingAxis;
btVector3 m_twistAxis;
btScalar m_kSwing;
btScalar m_kTwist;
btScalar m_kSwing;
btScalar m_kTwist;
btScalar m_twistLimitSign;
btScalar m_swingCorrection;
btScalar m_twistCorrection;
btScalar m_twistLimitSign;
btScalar m_swingCorrection;
btScalar m_twistCorrection;
btScalar m_twistAngle;
btScalar m_twistAngle;
btScalar m_accSwingLimitImpulse;
btScalar m_accTwistLimitImpulse;
btScalar m_accSwingLimitImpulse;
btScalar m_accTwistLimitImpulse;
bool m_angularOnly;
bool m_solveTwistLimit;
bool m_solveSwingLimit;
bool m_angularOnly;
bool m_solveTwistLimit;
bool m_solveSwingLimit;
bool m_useSolveConstraintObsolete;
bool m_useSolveConstraintObsolete;
// not yet used...
btScalar m_swingLimitRatio;
btScalar m_twistLimitRatio;
btVector3 m_twistAxisA;
btScalar m_swingLimitRatio;
btScalar m_twistLimitRatio;
btVector3 m_twistAxisA;
// motor
bool m_bMotorEnabled;
bool m_bNormalizedMotorStrength;
bool m_bMotorEnabled;
bool m_bNormalizedMotorStrength;
btQuaternion m_qTarget;
btScalar m_maxMotorImpulse;
btVector3 m_accMotorImpulse;
// parameters
int m_flags;
btScalar m_linCFM;
btScalar m_linERP;
btScalar m_angCFM;
protected:
btScalar m_maxMotorImpulse;
btVector3 m_accMotorImpulse;
// parameters
int m_flags;
btScalar m_linCFM;
btScalar m_linERP;
btScalar m_angCFM;
protected:
void init();
void computeConeLimitInfo(const btQuaternion& qCone, // in
btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
void computeConeLimitInfo(const btQuaternion& qCone, // in
btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
void computeTwistLimitInfo(const btQuaternion& qTwist, // in
btScalar& twistAngle, btVector3& vTwistAxis); // all outs
void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const;
void computeTwistLimitInfo(const btQuaternion& qTwist, // in
btScalar& twistAngle, btVector3& vTwistAxis); // all outs
void adjustSwingAxisToUseEllipseNormal(btVector3 & vSwingAxis) const;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,const btTransform& rbAFrame, const btTransform& rbBFrame);
btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
btConeTwistConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& rbAFrame, const btTransform& rbBFrame);
virtual void buildJacobian();
btConeTwistConstraint(btRigidBody & rbA, const btTransform& rbAFrame);
virtual void getInfo1 (btConstraintInfo1* info);
virtual void buildJacobian();
void getInfo1NonVirtual(btConstraintInfo1* info);
virtual void getInfo2 (btConstraintInfo2* info);
void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
virtual void getInfo1(btConstraintInfo1 * info);
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
void getInfo1NonVirtual(btConstraintInfo1 * info);
void updateRHS(btScalar timeStep);
virtual void getInfo2(btConstraintInfo2 * info);
void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB);
virtual void solveConstraintObsolete(btSolverBody & bodyA, btSolverBody & bodyB, btScalar timeStep);
void updateRHS(btScalar timeStep);
const btRigidBody& getRigidBodyA() const
{
@@ -166,64 +157,64 @@ public:
return m_rbB;
}
void setAngularOnly(bool angularOnly)
void setAngularOnly(bool angularOnly)
{
m_angularOnly = angularOnly;
}
bool getAngularOnly() const
bool getAngularOnly() const
{
return m_angularOnly;
return m_angularOnly;
}
void setLimit(int limitIndex,btScalar limitValue)
void setLimit(int limitIndex, btScalar limitValue)
{
switch (limitIndex)
{
case 3:
case 3:
{
m_twistSpan = limitValue;
break;
}
case 4:
case 4:
{
m_swingSpan2 = limitValue;
break;
}
case 5:
case 5:
{
m_swingSpan1 = limitValue;
break;
}
default:
default:
{
}
};
}
btScalar getLimit(int limitIndex) const
btScalar getLimit(int limitIndex) const
{
switch (limitIndex)
{
case 3:
case 3:
{
return m_twistSpan;
break;
}
case 4:
case 4:
{
return m_swingSpan2;
break;
}
case 5:
case 5:
{
return m_swingSpan1;
break;
}
default:
default:
{
btAssert(0 && "Invalid limitIndex specified for btConeTwistConstraint");
return 0.0;
btAssert(0 && "Invalid limitIndex specified for btConeTwistConstraint");
return 0.0;
}
};
}
@@ -239,18 +230,18 @@ public:
// __relaxationFactor:
// 0->1, recommend to stay near 1.
// the lower the value, the less the constraint will fight velocities which violate the angular limits.
void setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
void setLimit(btScalar _swingSpan1, btScalar _swingSpan2, btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
{
m_swingSpan1 = _swingSpan1;
m_swingSpan2 = _swingSpan2;
m_twistSpan = _twistSpan;
m_twistSpan = _twistSpan;
m_limitSoftness = _softness;
m_limitSoftness = _softness;
m_biasFactor = _biasFactor;
m_relaxationFactor = _relaxationFactor;
}
const btTransform& getAFrame() const { return m_rbAFrame; };
const btTransform& getAFrame() const { return m_rbAFrame; };
const btTransform& getBFrame() const { return m_rbBFrame; };
inline int getSolveTwistLimit()
@@ -269,7 +260,7 @@ public:
}
void calcAngleInfo();
void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
void calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB);
inline btScalar getSwingSpan1() const
{
@@ -308,8 +299,16 @@ public:
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; }
void setMaxMotorImpulse(btScalar maxMotorImpulse)
{
m_maxMotorImpulse = maxMotorImpulse;
m_bNormalizedMotorStrength = false;
}
void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse)
{
m_maxMotorImpulse = maxMotorImpulse;
m_bNormalizedMotorStrength = true;
}
btScalar getFixThresh() { return m_fixThresh; }
void setFixThresh(btScalar fixThresh) { m_fixThresh = fixThresh; }
@@ -318,17 +317,17 @@ public:
// q: the desired rotation of bodyA wrt bodyB.
// 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);
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);
void setMotorTargetInConstraintSpace(const btQuaternion& q);
btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///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 void setParam(int num, btScalar value, int axis = -1);
virtual void setFrames(const btTransform& frameA, const btTransform& frameB);
@@ -342,84 +341,74 @@ public:
return m_rbBFrame;
}
///return the local value of parameter
virtual btScalar getParam(int num, int axis = -1) const;
virtual btScalar getParam(int num, int axis = -1) const;
int getFlags() const
{
return m_flags;
}
virtual int calculateSerializeBufferSize() const;
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
};
struct btConeTwistConstraintDoubleData
struct btConeTwistConstraintDoubleData
{
btTypedConstraintDoubleData m_typeConstraintData;
btTypedConstraintDoubleData m_typeConstraintData;
btTransformDoubleData m_rbAFrame;
btTransformDoubleData m_rbBFrame;
//limits
double m_swingSpan1;
double m_swingSpan2;
double m_twistSpan;
double m_limitSoftness;
double m_biasFactor;
double m_relaxationFactor;
double m_damping;
double m_swingSpan1;
double m_swingSpan2;
double m_twistSpan;
double m_limitSoftness;
double m_biasFactor;
double m_relaxationFactor;
double m_damping;
};
#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
///this structure is not used, except for loading pre-2.82 .bullet files
struct btConeTwistConstraintData
struct btConeTwistConstraintData
{
btTypedConstraintData m_typeConstraintData;
btTypedConstraintData m_typeConstraintData;
btTransformFloatData m_rbAFrame;
btTransformFloatData m_rbBFrame;
//limits
float m_swingSpan1;
float m_swingSpan2;
float m_twistSpan;
float m_limitSoftness;
float m_biasFactor;
float m_relaxationFactor;
float m_swingSpan1;
float m_swingSpan2;
float m_twistSpan;
float m_limitSoftness;
float m_biasFactor;
float m_relaxationFactor;
float m_damping;
float m_damping;
char m_pad[4];
};
#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
//
SIMD_FORCE_INLINE int btConeTwistConstraint::calculateSerializeBufferSize() const
SIMD_FORCE_INLINE int btConeTwistConstraint::calculateSerializeBufferSize() const
{
return sizeof(btConeTwistConstraintData2);
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
{
btConeTwistConstraintData2* cone = (btConeTwistConstraintData2*) dataBuffer;
btTypedConstraint::serialize(&cone->m_typeConstraintData,serializer);
btConeTwistConstraintData2* cone = (btConeTwistConstraintData2*)dataBuffer;
btTypedConstraint::serialize(&cone->m_typeConstraintData, serializer);
m_rbAFrame.serialize(cone->m_rbAFrame);
m_rbBFrame.serialize(cone->m_rbBFrame);
cone->m_swingSpan1 = m_swingSpan1;
cone->m_swingSpan2 = m_swingSpan2;
cone->m_twistSpan = m_twistSpan;
@@ -431,5 +420,4 @@ SIMD_FORCE_INLINE const char* btConeTwistConstraint::serialize(void* dataBuffer,
return btConeTwistConstraintDataName;
}
#endif //BT_CONETWISTCONSTRAINT_H
#endif //BT_CONETWISTCONSTRAINT_H

32
src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
View File

@@ -26,41 +26,33 @@ struct btContactSolverInfo;
struct btBroadphaseProxy;
class btIDebugDraw;
class btStackAlloc;
class btDispatcher;
class btDispatcher;
/// btConstraintSolver provides solver interface
enum btConstraintSolverType
{
BT_SEQUENTIAL_IMPULSE_SOLVER=1,
BT_MLCP_SOLVER=2,
BT_NNCG_SOLVER=4,
BT_MULTIBODY_SOLVER=8,
BT_SEQUENTIAL_IMPULSE_SOLVER = 1,
BT_MLCP_SOLVER = 2,
BT_NNCG_SOLVER = 4,
BT_MULTIBODY_SOLVER = 8,
};
class btConstraintSolver
{
public:
virtual ~btConstraintSolver() {}
virtual void prepareSolve (int /* numBodies */, int /* numManifolds */) {;}
virtual void prepareSolve(int /* numBodies */, int /* numManifolds */) { ; }
///solve a group of constraints
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer,btDispatcher* dispatcher) = 0;
virtual btScalar solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, class btIDebugDraw* debugDrawer, btDispatcher* dispatcher) = 0;
virtual void allSolved (const btContactSolverInfo& /* info */,class btIDebugDraw* /* debugDrawer */) {;}
virtual void allSolved(const btContactSolverInfo& /* info */, class btIDebugDraw* /* debugDrawer */) { ; }
///clear internal cached data and reset random seed
virtual void reset() = 0;
virtual btConstraintSolverType getSolverType() const=0;
virtual void reset() = 0;
virtual btConstraintSolverType getSolverType() const = 0;
};
#endif //BT_CONSTRAINT_SOLVER_H
#endif //BT_CONSTRAINT_SOLVER_H

132
src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
View File

@@ -13,7 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btContactConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btVector3.h"
@@ -22,44 +21,33 @@ subject to the following restrictions:
#include "LinearMath/btMinMax.h"
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB)
:btTypedConstraint(CONTACT_CONSTRAINT_TYPE,rbA,rbB),
m_contactManifold(*contactManifold)
btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold, btRigidBody& rbA, btRigidBody& rbB)
: btTypedConstraint(CONTACT_CONSTRAINT_TYPE, rbA, rbB),
m_contactManifold(*contactManifold)
{
}
btContactConstraint::~btContactConstraint()
{
}
void btContactConstraint::setContactManifold(btPersistentManifold* contactManifold)
void btContactConstraint::setContactManifold(btPersistentManifold* contactManifold)
{
m_contactManifold = *contactManifold;
}
void btContactConstraint::getInfo1 (btConstraintInfo1* info)
void btContactConstraint::getInfo1(btConstraintInfo1* info)
{
}
void btContactConstraint::getInfo2 (btConstraintInfo2* info)
void btContactConstraint::getInfo2(btConstraintInfo2* info)
{
}
void btContactConstraint::buildJacobian()
void btContactConstraint::buildJacobian()
{
}
#include "btContactConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btVector3.h"
@@ -68,64 +56,59 @@ void btContactConstraint::buildJacobian()
#include "LinearMath/btMinMax.h"
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
//response between two dynamic objects without friction and no restitution, assuming 0 penetration depth
btScalar resolveSingleCollision(
btRigidBody* body1,
btCollisionObject* colObj2,
const btVector3& contactPositionWorld,
const btVector3& contactNormalOnB,
const btContactSolverInfo& solverInfo,
btScalar distance)
btRigidBody* body1,
btCollisionObject* colObj2,
const btVector3& contactPositionWorld,
const btVector3& contactNormalOnB,
const btContactSolverInfo& solverInfo,
btScalar distance)
{
btRigidBody* body2 = btRigidBody::upcast(colObj2);
const btVector3& normal = contactNormalOnB;
btVector3 rel_pos1 = contactPositionWorld - body1->getWorldTransform().getOrigin();
btVector3 rel_pos2 = contactPositionWorld - colObj2->getWorldTransform().getOrigin();
btVector3 vel1 = body1->getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = body2? body2->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
btVector3 vel = vel1 - vel2;
btScalar rel_vel;
rel_vel = normal.dot(vel);
btScalar combinedRestitution = 0.f;
btScalar restitution = combinedRestitution* -rel_vel;
const btVector3& normal = contactNormalOnB;
btScalar positionalError = solverInfo.m_erp *-distance /solverInfo.m_timeStep ;
btScalar velocityError = -(1.0f + restitution) * rel_vel;// * damping;
btScalar denom0 = body1->computeImpulseDenominator(contactPositionWorld,normal);
btScalar denom1 = body2? body2->computeImpulseDenominator(contactPositionWorld,normal) : 0.f;
btVector3 rel_pos1 = contactPositionWorld - body1->getWorldTransform().getOrigin();
btVector3 rel_pos2 = contactPositionWorld - colObj2->getWorldTransform().getOrigin();
btVector3 vel1 = body1->getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = body2 ? body2->getVelocityInLocalPoint(rel_pos2) : btVector3(0, 0, 0);
btVector3 vel = vel1 - vel2;
btScalar rel_vel;
rel_vel = normal.dot(vel);
btScalar combinedRestitution = 0.f;
btScalar restitution = combinedRestitution * -rel_vel;
btScalar positionalError = solverInfo.m_erp * -distance / solverInfo.m_timeStep;
btScalar velocityError = -(1.0f + restitution) * rel_vel; // * damping;
btScalar denom0 = body1->computeImpulseDenominator(contactPositionWorld, normal);
btScalar denom1 = body2 ? body2->computeImpulseDenominator(contactPositionWorld, normal) : 0.f;
btScalar relaxation = 1.f;
btScalar jacDiagABInv = relaxation/(denom0+denom1);
btScalar jacDiagABInv = relaxation / (denom0 + denom1);
btScalar penetrationImpulse = positionalError * jacDiagABInv;
btScalar velocityImpulse = velocityError * jacDiagABInv;
btScalar penetrationImpulse = positionalError * jacDiagABInv;
btScalar velocityImpulse = velocityError * jacDiagABInv;
btScalar normalImpulse = penetrationImpulse+velocityImpulse;
normalImpulse = 0.f > normalImpulse ? 0.f: normalImpulse;
btScalar normalImpulse = penetrationImpulse + velocityImpulse;
normalImpulse = 0.f > normalImpulse ? 0.f : normalImpulse;
body1->applyImpulse(normal*(normalImpulse), rel_pos1);
if (body2)
body2->applyImpulse(-normal*(normalImpulse), rel_pos2);
return normalImpulse;
body1->applyImpulse(normal * (normalImpulse), rel_pos1);
if (body2)
body2->applyImpulse(-normal * (normalImpulse), rel_pos2);
return normalImpulse;
}
//bilateral constraint between two dynamic objects
void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
btRigidBody& body2, const btVector3& pos2,
btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep)
btRigidBody& body2, const btVector3& pos2,
btScalar distance, const btVector3& normal, btScalar& impulse, btScalar timeStep)
{
(void)timeStep;
(void)distance;
btScalar normalLenSqr = normal.length2();
btAssert(btFabs(normalLenSqr) < btScalar(1.1));
if (normalLenSqr > btScalar(1.1))
@@ -133,45 +116,38 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
impulse = btScalar(0.);
return;
}
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
//this jacobian entry could be re-used for all iterations
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
btVector3 vel = vel1 - vel2;
btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
body2.getCenterOfMassTransform().getBasis().transpose(),
rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
body2.getInvInertiaDiagLocal(),body2.getInvMass());
btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
body2.getCenterOfMassTransform().getBasis().transpose(),
rel_pos1, rel_pos2, normal, body1.getInvInertiaDiagLocal(), body1.getInvMass(),
body2.getInvInertiaDiagLocal(), body2.getInvMass());
btScalar jacDiagAB = jac.getDiagonal();
btScalar jacDiagABInv = btScalar(1.) / jacDiagAB;
btScalar rel_vel = jac.getRelativeVelocity(
btScalar rel_vel = jac.getRelativeVelocity(
body1.getLinearVelocity(),
body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
body2.getLinearVelocity(),
body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity());
body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity());
rel_vel = normal.dot(vel);
//todo: move this into proper structure
btScalar contactDamping = btScalar(0.2);
#ifdef ONLY_USE_LINEAR_MASS
btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
impulse = - contactDamping * rel_vel * massTerm;
#else
impulse = -contactDamping * rel_vel * massTerm;
#else
btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
impulse = velocityImpulse;
#endif
}

30
src/BulletDynamics/ConstraintSolver/btContactConstraint.h
View File

@@ -22,20 +22,17 @@ subject to the following restrictions:
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
///btContactConstraint can be automatically created to solve contact constraints using the unified btTypedConstraint interface
ATTRIBUTE_ALIGNED16(class) btContactConstraint : public btTypedConstraint
ATTRIBUTE_ALIGNED16(class)
btContactConstraint : public btTypedConstraint
{
protected:
btPersistentManifold m_contactManifold;
protected:
btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB);
btContactConstraint(btPersistentManifold * contactManifold, btRigidBody & rbA, btRigidBody & rbB);
public:
void setContactManifold(btPersistentManifold* contactManifold);
void setContactManifold(btPersistentManifold * contactManifold);
btPersistentManifold* getContactManifold()
{
@@ -49,25 +46,20 @@ public:
virtual ~btContactConstraint();
virtual void getInfo1 (btConstraintInfo1* info);
virtual void getInfo1(btConstraintInfo1 * info);
virtual void getInfo2 (btConstraintInfo2* info);
virtual void getInfo2(btConstraintInfo2 * info);
///obsolete methods
virtual void buildJacobian();
virtual void buildJacobian();
};
///very basic collision resolution without friction
btScalar resolveSingleCollision(btRigidBody* body1, class btCollisionObject* colObj2, const btVector3& contactPositionWorld,const btVector3& contactNormalOnB, const struct btContactSolverInfo& solverInfo,btScalar distance);
btScalar resolveSingleCollision(btRigidBody* body1, class btCollisionObject* colObj2, const btVector3& contactPositionWorld, const btVector3& contactNormalOnB, const struct btContactSolverInfo& solverInfo, btScalar distance);
///resolveSingleBilateral is an obsolete methods used for vehicle friction between two dynamic objects
void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
btRigidBody& body2, const btVector3& pos2,
btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep);
btRigidBody& body2, const btVector3& pos2,
btScalar distance, const btVector3& normal, btScalar& impulse, btScalar timeStep);
#endif //BT_CONTACT_CONSTRAINT_H
#endif //BT_CONTACT_CONSTRAINT_H

169
src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
View File

@@ -18,7 +18,7 @@ subject to the following restrictions:
#include "LinearMath/btScalar.h"
enum btSolverMode
enum btSolverMode
{
SOLVER_RANDMIZE_ORDER = 1,
SOLVER_FRICTION_SEPARATE = 2,
@@ -35,134 +35,125 @@ enum btSolverMode
struct btContactSolverInfoData
{
btScalar m_tau;
btScalar m_damping; //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
btScalar m_friction;
btScalar m_timeStep;
btScalar m_restitution;
int m_numIterations;
btScalar m_maxErrorReduction;
btScalar m_sor; //successive over-relaxation term
btScalar m_erp; //error reduction for non-contact constraints
btScalar m_erp2; //error reduction for contact constraints
btScalar m_globalCfm; //constraint force mixing for contacts and non-contacts
btScalar m_frictionERP; //error reduction for friction constraints
btScalar m_frictionCFM; //constraint force mixing for friction constraints
btScalar m_tau;
btScalar m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
btScalar m_friction;
btScalar m_timeStep;
btScalar m_restitution;
int m_numIterations;
btScalar m_maxErrorReduction;
btScalar m_sor;//successive over-relaxation term
btScalar m_erp;//error reduction for non-contact constraints
btScalar m_erp2;//error reduction for contact constraints
btScalar m_globalCfm;//constraint force mixing for contacts and non-contacts
btScalar m_frictionERP;//error reduction for friction constraints
btScalar m_frictionCFM;//constraint force mixing for friction constraints
int m_splitImpulse;
btScalar m_splitImpulsePenetrationThreshold;
btScalar m_splitImpulseTurnErp;
btScalar m_linearSlop;
btScalar m_warmstartingFactor;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
btScalar m_maxGyroscopicForce;
btScalar m_singleAxisRollingFrictionThreshold;
btScalar m_leastSquaresResidualThreshold;
btScalar m_restitutionVelocityThreshold;
int m_splitImpulse;
btScalar m_splitImpulsePenetrationThreshold;
btScalar m_splitImpulseTurnErp;
btScalar m_linearSlop;
btScalar m_warmstartingFactor;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
btScalar m_maxGyroscopicForce;
btScalar m_singleAxisRollingFrictionThreshold;
btScalar m_leastSquaresResidualThreshold;
btScalar m_restitutionVelocityThreshold;
};
struct btContactSolverInfo : public btContactSolverInfoData
{
inline btContactSolverInfo()
{
m_tau = btScalar(0.6);
m_damping = btScalar(1.0);
m_friction = btScalar(0.3);
m_timeStep = btScalar(1.f/60.f);
m_timeStep = btScalar(1.f / 60.f);
m_restitution = btScalar(0.);
m_maxErrorReduction = btScalar(20.);
m_numIterations = 10;
m_erp = btScalar(0.2);
m_erp2 = btScalar(0.2);
m_globalCfm = btScalar(0.);
m_frictionERP = btScalar(0.2);//positional friction 'anchors' are disabled by default
m_frictionERP = btScalar(0.2); //positional friction 'anchors' are disabled by default
m_frictionCFM = btScalar(0.);
m_sor = btScalar(1.);
m_splitImpulse = true;
m_splitImpulsePenetrationThreshold = -.04f;
m_splitImpulseTurnErp = 0.1f;
m_linearSlop = btScalar(0.0);
m_warmstartingFactor=btScalar(0.85);
m_warmstartingFactor = btScalar(0.85);
//m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD | SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|SOLVER_USE_2_FRICTION_DIRECTIONS|SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | SOLVER_RANDMIZE_ORDER;
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; ///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_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; ///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;
m_restitutionVelocityThreshold = 0.2f;//if the relative velocity is below this threshold, there is zero restitution
m_restitutionVelocityThreshold = 0.2f; //if the relative velocity is below this threshold, there is zero restitution
}
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btContactSolverInfoDoubleData
{
double m_tau;
double m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
double m_friction;
double m_timeStep;
double m_restitution;
double m_maxErrorReduction;
double m_sor;
double m_erp;//used as Baumgarte factor
double m_erp2;//used in Split Impulse
double m_globalCfm;//constraint force mixing
double m_splitImpulsePenetrationThreshold;
double m_splitImpulseTurnErp;
double m_linearSlop;
double m_warmstartingFactor;
double m_maxGyroscopicForce;///it is only used for 'explicit' version of gyroscopic force
double m_singleAxisRollingFrictionThreshold;
int m_numIterations;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
int m_splitImpulse;
char m_padding[4];
double m_tau;
double m_damping; //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
double m_friction;
double m_timeStep;
double m_restitution;
double m_maxErrorReduction;
double m_sor;
double m_erp; //used as Baumgarte factor
double m_erp2; //used in Split Impulse
double m_globalCfm; //constraint force mixing
double m_splitImpulsePenetrationThreshold;
double m_splitImpulseTurnErp;
double m_linearSlop;
double m_warmstartingFactor;
double m_maxGyroscopicForce; ///it is only used for 'explicit' version of gyroscopic force
double m_singleAxisRollingFrictionThreshold;
int m_numIterations;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
int m_splitImpulse;
char m_padding[4];
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btContactSolverInfoFloatData
{
float m_tau;
float m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
float m_friction;
float m_timeStep;
float m_tau;
float m_damping; //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
float m_friction;
float m_timeStep;
float m_restitution;
float m_maxErrorReduction;
float m_sor;
float m_erp;//used as Baumgarte factor
float m_restitution;
float m_maxErrorReduction;
float m_sor;
float m_erp; //used as Baumgarte factor
float m_erp2;//used in Split Impulse
float m_globalCfm;//constraint force mixing
float m_splitImpulsePenetrationThreshold;
float m_splitImpulseTurnErp;
float m_erp2; //used in Split Impulse
float m_globalCfm; //constraint force mixing
float m_splitImpulsePenetrationThreshold;
float m_splitImpulseTurnErp;
float m_linearSlop;
float m_warmstartingFactor;
float m_maxGyroscopicForce;
float m_singleAxisRollingFrictionThreshold;
float m_linearSlop;
float m_warmstartingFactor;
float m_maxGyroscopicForce;
float m_singleAxisRollingFrictionThreshold;
int m_numIterations;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
int m_numIterations;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
int m_splitImpulse;
char m_padding[4];
int m_splitImpulse;
char m_padding[4];
};
#endif //BT_CONTACT_SOLVER_INFO
#endif //BT_CONTACT_SOLVER_INFO

19
src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp
View File

@@ -13,25 +13,20 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btFixedConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btTransformUtil.h"
#include <new>
btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB)
:btGeneric6DofSpring2Constraint(rbA,rbB,frameInA,frameInB)
btFixedConstraint::btFixedConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
: btGeneric6DofSpring2Constraint(rbA, rbB, frameInA, frameInB)
{
setAngularLowerLimit(btVector3(0,0,0));
setAngularUpperLimit(btVector3(0,0,0));
setLinearLowerLimit(btVector3(0,0,0));
setLinearUpperLimit(btVector3(0,0,0));
setAngularLowerLimit(btVector3(0, 0, 0));
setAngularUpperLimit(btVector3(0, 0, 0));
setLinearLowerLimit(btVector3(0, 0, 0));
setLinearUpperLimit(btVector3(0, 0, 0));
}
btFixedConstraint::~btFixedConstraint ()
btFixedConstraint::~btFixedConstraint()
{
}

11
src/BulletDynamics/ConstraintSolver/btFixedConstraint.h
View File

@@ -18,16 +18,13 @@ subject to the following restrictions:
#include "btGeneric6DofSpring2Constraint.h"
ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btGeneric6DofSpring2Constraint
ATTRIBUTE_ALIGNED16(class)
btFixedConstraint : public btGeneric6DofSpring2Constraint
{
public:
btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB);
btFixedConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& frameInA, const btTransform& frameInB);
virtual ~btFixedConstraint();
};
#endif //BT_FIXED_CONSTRAINT_H
#endif //BT_FIXED_CONSTRAINT_H

28
src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp
View File

@@ -17,38 +17,36 @@ subject to the following restrictions:
#include "btGearConstraint.h"
btGearConstraint::btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio)
:btTypedConstraint(GEAR_CONSTRAINT_TYPE,rbA,rbB),
m_axisInA(axisInA),
m_axisInB(axisInB),
m_ratio(ratio)
btGearConstraint::btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA, const btVector3& axisInB, btScalar ratio)
: btTypedConstraint(GEAR_CONSTRAINT_TYPE, rbA, rbB),
m_axisInA(axisInA),
m_axisInB(axisInB),
m_ratio(ratio)
{
}
btGearConstraint::~btGearConstraint ()
btGearConstraint::~btGearConstraint()
{
}
void btGearConstraint::getInfo1 (btConstraintInfo1* info)
void btGearConstraint::getInfo1(btConstraintInfo1* info)
{
info->m_numConstraintRows = 1;
info->nub = 1;
}
void btGearConstraint::getInfo2 (btConstraintInfo2* info)
void btGearConstraint::getInfo2(btConstraintInfo2* info)
{
btVector3 globalAxisA, globalAxisB;
globalAxisA = m_rbA.getWorldTransform().getBasis()*this->m_axisInA;
globalAxisB = m_rbB.getWorldTransform().getBasis()*this->m_axisInB;
globalAxisA = m_rbA.getWorldTransform().getBasis() * this->m_axisInA;
globalAxisB = m_rbB.getWorldTransform().getBasis() * this->m_axisInB;
info->m_J1angularAxis[0] = globalAxisA[0];
info->m_J1angularAxis[1] = globalAxisA[1];
info->m_J1angularAxis[2] = globalAxisA[2];
info->m_J2angularAxis[0] = m_ratio*globalAxisB[0];
info->m_J2angularAxis[1] = m_ratio*globalAxisB[1];
info->m_J2angularAxis[2] = m_ratio*globalAxisB[2];
info->m_J2angularAxis[0] = m_ratio * globalAxisB[0];
info->m_J2angularAxis[1] = m_ratio * globalAxisB[1];
info->m_J2angularAxis[2] = m_ratio * globalAxisB[2];
}

94
src/BulletDynamics/ConstraintSolver/btGearConstraint.h
View File

@@ -13,45 +13,40 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_GEAR_CONSTRAINT_H
#define BT_GEAR_CONSTRAINT_H
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
#ifdef BT_USE_DOUBLE_PRECISION
#define btGearConstraintData btGearConstraintDoubleData
#define btGearConstraintDataName "btGearConstraintDoubleData"
#define btGearConstraintData btGearConstraintDoubleData
#define btGearConstraintDataName "btGearConstraintDoubleData"
#else
#define btGearConstraintData btGearConstraintFloatData
#define btGearConstraintDataName "btGearConstraintFloatData"
#endif //BT_USE_DOUBLE_PRECISION
#define btGearConstraintData btGearConstraintFloatData
#define btGearConstraintDataName "btGearConstraintFloatData"
#endif //BT_USE_DOUBLE_PRECISION
///The btGeatConstraint will couple the angular velocity for two bodies around given local axis and ratio.
///See Bullet/Demos/ConstraintDemo for an example use.
class btGearConstraint : public btTypedConstraint
{
protected:
btVector3 m_axisInA;
btVector3 m_axisInB;
bool m_useFrameA;
btScalar m_ratio;
btVector3 m_axisInA;
btVector3 m_axisInB;
bool m_useFrameA;
btScalar m_ratio;
public:
btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio=1.f);
virtual ~btGearConstraint ();
btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA, const btVector3& axisInB, btScalar ratio = 1.f);
virtual ~btGearConstraint();
///internal method used by the constraint solver, don't use them directly
virtual void getInfo1 (btConstraintInfo1* info);
virtual void getInfo1(btConstraintInfo1* info);
///internal method used by the constraint solver, don't use them directly
virtual void getInfo2 (btConstraintInfo2* info);
virtual void getInfo2(btConstraintInfo2* info);
void setAxisA(btVector3& axisA)
void setAxisA(btVector3& axisA)
{
m_axisInA = axisA;
}
@@ -76,68 +71,64 @@ public:
return m_ratio;
}
virtual void setParam(int num, btScalar value, int axis = -1)
virtual void setParam(int num, btScalar value, int axis = -1)
{
(void) num;
(void) value;
(void) axis;
(void)num;
(void)value;
(void)axis;
btAssert(0);
}
///return the local value of parameter
virtual btScalar getParam(int num, int axis = -1) const
{
(void) num;
(void) axis;
virtual btScalar getParam(int num, int axis = -1) const
{
(void)num;
(void)axis;
btAssert(0);
return 0.f;
}
virtual int calculateSerializeBufferSize() const;
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btGearConstraintFloatData
{
btTypedConstraintFloatData m_typeConstraintData;
btTypedConstraintFloatData m_typeConstraintData;
btVector3FloatData m_axisInA;
btVector3FloatData m_axisInB;
btVector3FloatData m_axisInA;
btVector3FloatData m_axisInB;
float m_ratio;
char m_padding[4];
float m_ratio;
char m_padding[4];
};
struct btGearConstraintDoubleData
{
btTypedConstraintDoubleData m_typeConstraintData;
btTypedConstraintDoubleData m_typeConstraintData;
btVector3DoubleData m_axisInA;
btVector3DoubleData m_axisInB;
btVector3DoubleData m_axisInA;
btVector3DoubleData m_axisInB;
double m_ratio;
double m_ratio;
};
SIMD_FORCE_INLINE int btGearConstraint::calculateSerializeBufferSize() const
SIMD_FORCE_INLINE int btGearConstraint::calculateSerializeBufferSize() const
{
return sizeof(btGearConstraintData);
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* btGearConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* btGearConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
{
btGearConstraintData* gear = (btGearConstraintData*)dataBuffer;
btTypedConstraint::serialize(&gear->m_typeConstraintData,serializer);
btTypedConstraint::serialize(&gear->m_typeConstraintData, serializer);
m_axisInA.serialize( gear->m_axisInA );
m_axisInB.serialize( gear->m_axisInB );
m_axisInA.serialize(gear->m_axisInA);
m_axisInB.serialize(gear->m_axisInB);
gear->m_ratio = m_ratio;
@@ -152,9 +143,4 @@ SIMD_FORCE_INLINE const char* btGearConstraint::serialize(void* dataBuffer, btSe
return btGearConstraintDataName;
}
#endif //BT_GEAR_CONSTRAINT_H
#endif //BT_GEAR_CONSTRAINT_H

702
src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
View File

File diff suppressed because it is too large Load Diff

662
src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
View File

@@ -23,7 +23,6 @@ email: projectileman@yahoo.com
http://gimpact.sf.net
*/
#ifndef BT_GENERIC_6DOF_CONSTRAINT_H
#define BT_GENERIC_6DOF_CONSTRAINT_H
@@ -33,96 +32,91 @@ http://gimpact.sf.net
class btRigidBody;
#ifdef BT_USE_DOUBLE_PRECISION
#define btGeneric6DofConstraintData2 btGeneric6DofConstraintDoubleData2
#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintDoubleData2"
#define btGeneric6DofConstraintData2 btGeneric6DofConstraintDoubleData2
#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintDoubleData2"
#else
#define btGeneric6DofConstraintData2 btGeneric6DofConstraintData
#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintData"
#endif //BT_USE_DOUBLE_PRECISION
#define btGeneric6DofConstraintData2 btGeneric6DofConstraintData
#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintData"
#endif //BT_USE_DOUBLE_PRECISION
//! Rotation Limit structure for generic joints
class btRotationalLimitMotor
{
public:
//! limit_parameters
//!@{
btScalar m_loLimit;//!< joint limit
btScalar m_hiLimit;//!< joint limit
btScalar m_targetVelocity;//!< target motor velocity
btScalar m_maxMotorForce;//!< max force on motor
btScalar m_maxLimitForce;//!< max force on limit
btScalar m_damping;//!< Damping.
btScalar m_limitSoftness;//! Relaxation factor
btScalar m_normalCFM;//!< Constraint force mixing factor
btScalar m_stopERP;//!< Error tolerance factor when joint is at limit
btScalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit
btScalar m_bounce;//!< restitution factor
bool m_enableMotor;
//! limit_parameters
//!@{
btScalar m_loLimit; //!< joint limit
btScalar m_hiLimit; //!< joint limit
btScalar m_targetVelocity; //!< target motor velocity
btScalar m_maxMotorForce; //!< max force on motor
btScalar m_maxLimitForce; //!< max force on limit
btScalar m_damping; //!< Damping.
btScalar m_limitSoftness; //! Relaxation factor
btScalar m_normalCFM; //!< Constraint force mixing factor
btScalar m_stopERP; //!< Error tolerance factor when joint is at limit
btScalar m_stopCFM; //!< Constraint force mixing factor when joint is at limit
btScalar m_bounce; //!< restitution factor
bool m_enableMotor;
//!@}
//!@}
//! temp_variables
//!@{
btScalar m_currentLimitError;//! How much is violated this limit
btScalar m_currentPosition; //! current value of angle
int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
btScalar m_accumulatedImpulse;
//!@}
//! temp_variables
//!@{
btScalar m_currentLimitError; //! How much is violated this limit
btScalar m_currentPosition; //! current value of angle
int m_currentLimit; //!< 0=free, 1=at lo limit, 2=at hi limit
btScalar m_accumulatedImpulse;
//!@}
btRotationalLimitMotor()
{
m_accumulatedImpulse = 0.f;
m_targetVelocity = 0;
m_maxMotorForce = 6.0f;
m_maxLimitForce = 300.0f;
m_loLimit = 1.0f;
m_hiLimit = -1.0f;
btRotationalLimitMotor()
{
m_accumulatedImpulse = 0.f;
m_targetVelocity = 0;
m_maxMotorForce = 6.0f;
m_maxLimitForce = 300.0f;
m_loLimit = 1.0f;
m_hiLimit = -1.0f;
m_normalCFM = 0.f;
m_stopERP = 0.2f;
m_stopCFM = 0.f;
m_bounce = 0.0f;
m_damping = 1.0f;
m_limitSoftness = 0.5f;
m_currentLimit = 0;
m_currentLimitError = 0;
m_enableMotor = false;
}
m_bounce = 0.0f;
m_damping = 1.0f;
m_limitSoftness = 0.5f;
m_currentLimit = 0;
m_currentLimitError = 0;
m_enableMotor = false;
}
btRotationalLimitMotor(const btRotationalLimitMotor & limot)
{
m_targetVelocity = limot.m_targetVelocity;
m_maxMotorForce = limot.m_maxMotorForce;
m_limitSoftness = limot.m_limitSoftness;
m_loLimit = limot.m_loLimit;
m_hiLimit = limot.m_hiLimit;
btRotationalLimitMotor(const btRotationalLimitMotor& limot)
{
m_targetVelocity = limot.m_targetVelocity;
m_maxMotorForce = limot.m_maxMotorForce;
m_limitSoftness = limot.m_limitSoftness;
m_loLimit = limot.m_loLimit;
m_hiLimit = limot.m_hiLimit;
m_normalCFM = limot.m_normalCFM;
m_stopERP = limot.m_stopERP;
m_stopCFM = limot.m_stopCFM;
m_bounce = limot.m_bounce;
m_currentLimit = limot.m_currentLimit;
m_currentLimitError = limot.m_currentLimitError;
m_enableMotor = limot.m_enableMotor;
}
m_stopCFM = limot.m_stopCFM;
m_bounce = limot.m_bounce;
m_currentLimit = limot.m_currentLimit;
m_currentLimitError = limot.m_currentLimitError;
m_enableMotor = limot.m_enableMotor;
}
//! Is limited
bool isLimited() const
{
if(m_loLimit > m_hiLimit) return false;
return true;
}
bool isLimited() const
{
if (m_loLimit > m_hiLimit) return false;
return true;
}
//! Need apply correction
bool needApplyTorques() const
{
if(m_currentLimit == 0 && m_enableMotor == false) return false;
return true;
}
bool needApplyTorques() const
{
if (m_currentLimit == 0 && m_enableMotor == false) return false;
return true;
}
//! calculates error
/*!
@@ -131,104 +125,98 @@ public:
int testLimitValue(btScalar test_value);
//! apply the correction impulses for two bodies
btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
btScalar solveAngularLimits(btScalar timeStep, btVector3& axis, btScalar jacDiagABInv, btRigidBody* body0, btRigidBody* body1);
};
class btTranslationalLimitMotor
{
public:
btVector3 m_lowerLimit;//!< the constraint lower limits
btVector3 m_upperLimit;//!< the constraint upper limits
btVector3 m_accumulatedImpulse;
//! Linear_Limit_parameters
//!@{
btScalar m_limitSoftness;//!< Softness for linear limit
btScalar m_damping;//!< Damping for linear limit
btScalar m_restitution;//! Bounce parameter for linear limit
btVector3 m_normalCFM;//!< Constraint force mixing factor
btVector3 m_stopERP;//!< Error tolerance factor when joint is at limit
btVector3 m_stopCFM;//!< Constraint force mixing factor when joint is at limit
//!@}
bool m_enableMotor[3];
btVector3 m_targetVelocity;//!< target motor velocity
btVector3 m_maxMotorForce;//!< max force on motor
btVector3 m_currentLimitError;//! How much is violated this limit
btVector3 m_currentLinearDiff;//! Current relative offset of constraint frames
int m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
btVector3 m_lowerLimit; //!< the constraint lower limits
btVector3 m_upperLimit; //!< the constraint upper limits
btVector3 m_accumulatedImpulse;
//! Linear_Limit_parameters
//!@{
btScalar m_limitSoftness; //!< Softness for linear limit
btScalar m_damping; //!< Damping for linear limit
btScalar m_restitution; //! Bounce parameter for linear limit
btVector3 m_normalCFM; //!< Constraint force mixing factor
btVector3 m_stopERP; //!< Error tolerance factor when joint is at limit
btVector3 m_stopCFM; //!< Constraint force mixing factor when joint is at limit
//!@}
bool m_enableMotor[3];
btVector3 m_targetVelocity; //!< target motor velocity
btVector3 m_maxMotorForce; //!< max force on motor
btVector3 m_currentLimitError; //! How much is violated this limit
btVector3 m_currentLinearDiff; //! Current relative offset of constraint frames
int m_currentLimit[3]; //!< 0=free, 1=at lower limit, 2=at upper limit
btTranslationalLimitMotor()
{
m_lowerLimit.setValue(0.f,0.f,0.f);
m_upperLimit.setValue(0.f,0.f,0.f);
m_accumulatedImpulse.setValue(0.f,0.f,0.f);
btTranslationalLimitMotor()
{
m_lowerLimit.setValue(0.f, 0.f, 0.f);
m_upperLimit.setValue(0.f, 0.f, 0.f);
m_accumulatedImpulse.setValue(0.f, 0.f, 0.f);
m_normalCFM.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_limitSoftness = 0.7f;
m_damping = btScalar(1.0f);
m_restitution = btScalar(0.5f);
for(int i=0; i < 3; i++)
m_limitSoftness = 0.7f;
m_damping = btScalar(1.0f);
m_restitution = btScalar(0.5f);
for (int i = 0; i < 3; i++)
{
m_enableMotor[i] = false;
m_targetVelocity[i] = btScalar(0.f);
m_maxMotorForce[i] = btScalar(0.f);
}
}
}
btTranslationalLimitMotor(const btTranslationalLimitMotor & other )
{
m_lowerLimit = other.m_lowerLimit;
m_upperLimit = other.m_upperLimit;
m_accumulatedImpulse = other.m_accumulatedImpulse;
btTranslationalLimitMotor(const btTranslationalLimitMotor& other)
{
m_lowerLimit = other.m_lowerLimit;
m_upperLimit = other.m_upperLimit;
m_accumulatedImpulse = other.m_accumulatedImpulse;
m_limitSoftness = other.m_limitSoftness ;
m_damping = other.m_damping;
m_restitution = other.m_restitution;
m_limitSoftness = other.m_limitSoftness;
m_damping = other.m_damping;
m_restitution = other.m_restitution;
m_normalCFM = other.m_normalCFM;
m_stopERP = other.m_stopERP;
m_stopCFM = other.m_stopCFM;
for(int i=0; i < 3; i++)
for (int i = 0; i < 3; i++)
{
m_enableMotor[i] = other.m_enableMotor[i];
m_targetVelocity[i] = other.m_targetVelocity[i];
m_maxMotorForce[i] = other.m_maxMotorForce[i];
}
}
}
//! Test limit
//! Test limit
/*!
- free means upper < lower,
- locked means upper == lower
- limited means upper > lower
- limitIndex: first 3 are linear, next 3 are angular
*/
inline bool isLimited(int limitIndex) const
{
return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
}
inline bool needApplyForce(int limitIndex) const
{
if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
return true;
}
inline bool isLimited(int limitIndex) const
{
return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
}
inline bool needApplyForce(int limitIndex) const
{
if (m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
return true;
}
int testLimitValue(int limitIndex, btScalar test_value);
btScalar solveLinearAxis(
btScalar timeStep,
btScalar jacDiagABInv,
btRigidBody& body1,const btVector3 &pointInA,
btRigidBody& body2,const btVector3 &pointInB,
int limit_index,
const btVector3 & axis_normal_on_a,
const btVector3 & anchorPos);
btScalar solveLinearAxis(
btScalar timeStep,
btScalar jacDiagABInv,
btRigidBody& body1, const btVector3& pointInA,
btRigidBody& body2, const btVector3& pointInB,
int limit_index,
const btVector3& axis_normal_on_a,
const btVector3& anchorPos);
};
enum bt6DofFlags
@@ -237,8 +225,7 @@ enum bt6DofFlags
BT_6DOF_FLAGS_CFM_STOP = 2,
BT_6DOF_FLAGS_ERP_STOP = 4
};
#define BT_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
#define BT_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
/// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
/*!
@@ -276,254 +263,245 @@ This brings support for limit parameters and motors. </li>
</ul>
*/
ATTRIBUTE_ALIGNED16(class) btGeneric6DofConstraint : public btTypedConstraint
ATTRIBUTE_ALIGNED16(class)
btGeneric6DofConstraint : public btTypedConstraint
{
protected:
//! relative_frames
//!@{
btTransform m_frameInA;//!< the constraint space w.r.t body A
btTransform m_frameInB;//!< the constraint space w.r.t body B
//!@}
//! Jacobians
//!@{
btJacobianEntry m_jacLinear[3];//!< 3 orthogonal linear constraints
btJacobianEntry m_jacAng[3];//!< 3 orthogonal angular constraints
//!@}
//! Linear_Limit_parameters
//!@{
btTranslationalLimitMotor m_linearLimits;
//!@}
//! hinge_parameters
//!@{
btRotationalLimitMotor m_angularLimits[3];
//!@{
btTransform m_frameInA; //!< the constraint space w.r.t body A
btTransform m_frameInB; //!< the constraint space w.r.t body B
//!@}
//! Jacobians
//!@{
btJacobianEntry m_jacLinear[3]; //!< 3 orthogonal linear constraints
btJacobianEntry m_jacAng[3]; //!< 3 orthogonal angular constraints
//!@}
//! Linear_Limit_parameters
//!@{
btTranslationalLimitMotor m_linearLimits;
//!@}
//! hinge_parameters
//!@{
btRotationalLimitMotor m_angularLimits[3];
//!@}
protected:
//! temporal variables
//!@{
btScalar m_timeStep;
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;
btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
//! temporal variables
//!@{
btScalar m_timeStep;
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;
bool m_useLinearReferenceFrameA;
bool m_useOffsetForConstraintFrame;
int m_flags;
btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
//!@}
bool m_useLinearReferenceFrameA;
bool m_useOffsetForConstraintFrame;
btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other)
{
btAssert(0);
(void) other;
return *this;
}
int m_flags;
//!@}
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);
btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other)
{
btAssert(0);
(void)other;
return *this;
}
int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
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);
void buildLinearJacobian(
btJacobianEntry & jacLinear,const btVector3 & normalWorld,
const btVector3 & pivotAInW,const btVector3 & pivotBInW);
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 buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW);
void buildLinearJacobian(
btJacobianEntry & jacLinear, const btVector3& normalWorld,
const btVector3& pivotAInW, const btVector3& pivotBInW);
void buildAngularJacobian(btJacobianEntry & jacAngular, const btVector3& jointAxisW);
// tests linear limits
void calculateLinearInfo();
//! calcs the euler angles between the two bodies.
void calculateAngleInfo();
void calculateAngleInfo();
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
///for backwards compatibility during the transition to 'getInfo/getInfo2'
bool m_useSolveConstraintObsolete;
btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
///for backwards compatibility during the transition to 'getInfo/getInfo2'
bool m_useSolveConstraintObsolete;
btGeneric6DofConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA);
btGeneric6DofConstraint(btRigidBody & rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
//! Calcs global transform of the offsets
/*!
Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
\sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo
*/
void calculateTransforms(const btTransform& transA,const btTransform& transB);
void calculateTransforms(const btTransform& transA, const btTransform& transB);
void calculateTransforms();
//! Gets the global transform of the offset for body A
/*!
/*!
\sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
*/
const btTransform & getCalculatedTransformA() const
{
return m_calculatedTransformA;
}
const btTransform& getCalculatedTransformA() const
{
return m_calculatedTransformA;
}
//! Gets the global transform of the offset for body B
/*!
//! Gets the global transform of the offset for body B
/*!
\sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
*/
const btTransform & getCalculatedTransformB() const
{
return m_calculatedTransformB;
}
const btTransform& getCalculatedTransformB() const
{
return m_calculatedTransformB;
}
const btTransform & getFrameOffsetA() const
{
return m_frameInA;
}
const btTransform& getFrameOffsetA() const
{
return m_frameInA;
}
const btTransform & getFrameOffsetB() const
{
return m_frameInB;
}
const btTransform& getFrameOffsetB() const
{
return m_frameInB;
}
btTransform& getFrameOffsetA()
{
return m_frameInA;
}
btTransform & getFrameOffsetA()
{
return m_frameInA;
}
btTransform & getFrameOffsetB()
{
return m_frameInB;
}
btTransform& getFrameOffsetB()
{
return m_frameInB;
}
//! performs Jacobian calculation, and also calculates angle differences and axis
virtual void buildJacobian();
virtual void buildJacobian();
virtual void getInfo1 (btConstraintInfo1* info);
virtual void getInfo1(btConstraintInfo1 * info);
void getInfo1NonVirtual (btConstraintInfo1* info);
void getInfo1NonVirtual(btConstraintInfo1 * info);
virtual void getInfo2 (btConstraintInfo2* info);
virtual void getInfo2(btConstraintInfo2 * info);
void getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB);
void updateRHS(btScalar timeStep);
void updateRHS(btScalar timeStep);
//! Get the rotation axis in global coordinates
/*!
\pre btGeneric6DofConstraint.buildJacobian must be called previously.
*/
btVector3 getAxis(int axis_index) const;
btVector3 getAxis(int axis_index) const;
//! Get the relative Euler angle
/*!
//! Get the relative Euler angle
/*!
\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
*/
btScalar getAngle(int axis_index) const;
btScalar getAngle(int axis_index) const;
//! Get the relative position of the constraint pivot
/*!
/*!
\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
*/
btScalar getRelativePivotPosition(int axis_index) const;
void setFrames(const btTransform & frameA, const btTransform & frameB);
void setFrames(const btTransform& frameA, const btTransform& frameB);
//! Test angular limit.
/*!
Calculates angular correction and returns true if limit needs to be corrected.
\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
*/
bool testAngularLimitMotor(int axis_index);
bool testAngularLimitMotor(int axis_index);
void setLinearLowerLimit(const btVector3& linearLower)
{
m_linearLimits.m_lowerLimit = linearLower;
}
void setLinearLowerLimit(const btVector3& linearLower)
{
m_linearLimits.m_lowerLimit = linearLower;
}
void getLinearLowerLimit(btVector3& linearLower) const
void getLinearLowerLimit(btVector3 & linearLower) const
{
linearLower = m_linearLimits.m_lowerLimit;
}
void setLinearUpperLimit(const btVector3& linearUpper)
void setLinearUpperLimit(const btVector3& linearUpper)
{
m_linearLimits.m_upperLimit = linearUpper;
}
void getLinearUpperLimit(btVector3& linearUpper) const
void getLinearUpperLimit(btVector3 & linearUpper) const
{
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 getAngularLowerLimit(btVector3& angularLower) const
void setAngularLowerLimit(const btVector3& angularLower)
{
for(int i = 0; i < 3; i++)
for (int i = 0; i < 3; i++)
m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
}
void getAngularLowerLimit(btVector3 & angularLower) const
{
for (int i = 0; i < 3; i++)
angularLower[i] = m_angularLimits[i].m_loLimit;
}
void setAngularUpperLimit(const btVector3& angularUpper)
{
for(int i = 0; i < 3; i++)
m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
}
void getAngularUpperLimit(btVector3& angularUpper) const
void setAngularUpperLimit(const btVector3& angularUpper)
{
for(int i = 0; i < 3; i++)
for (int i = 0; i < 3; i++)
m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
}
void getAngularUpperLimit(btVector3 & angularUpper) const
{
for (int i = 0; i < 3; i++)
angularUpper[i] = m_angularLimits[i].m_hiLimit;
}
//! Retrieves the angular limit informacion
btRotationalLimitMotor * getRotationalLimitMotor(int index)
{
return &m_angularLimits[index];
}
btRotationalLimitMotor* getRotationalLimitMotor(int index)
{
return &m_angularLimits[index];
}
//! Retrieves the limit informacion
btTranslationalLimitMotor * getTranslationalLimitMotor()
{
return &m_linearLimits;
}
//! Retrieves the limit informacion
btTranslationalLimitMotor* getTranslationalLimitMotor()
{
return &m_linearLimits;
}
//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
{
//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;
}
}
m_angularLimits[axis - 3].m_loLimit = lo;
m_angularLimits[axis - 3].m_hiLimit = hi;
}
}
//! Test limit
/*!
@@ -532,116 +510,106 @@ public:
- limited means upper > lower
- limitIndex: first 3 are linear, next 3 are angular
*/
bool isLimited(int limitIndex) const
{
if(limitIndex<3)
{
bool isLimited(int limitIndex) const
{
if (limitIndex < 3)
{
return m_linearLimits.isLimited(limitIndex);
}
return m_angularLimits[limitIndex - 3].isLimited();
}
}
return m_angularLimits[limitIndex-3].isLimited();
}
virtual void calcAnchorPos(void); // overridable
virtual void calcAnchorPos(void); // overridable
int get_limit_motor_info2( btRotationalLimitMotor * 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);
int get_limit_motor_info2(btRotationalLimitMotor * 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);
// access for UseFrameOffset
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).
///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 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 btScalar getParam(int num, int axis = -1) const;
void setAxis( const btVector3& axis1, const btVector3& axis2);
void setAxis(const btVector3& axis1, const btVector3& axis2);
virtual int getFlags() const
{
return m_flags;
virtual int getFlags() const
{
return m_flags;
}
virtual int calculateSerializeBufferSize() const;
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
};
struct btGeneric6DofConstraintData
{
btTypedConstraintData m_typeConstraintData;
btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
btTypedConstraintData m_typeConstraintData;
btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
btTransformFloatData m_rbBFrame;
btVector3FloatData m_linearUpperLimit;
btVector3FloatData m_linearLowerLimit;
btVector3FloatData m_angularUpperLimit;
btVector3FloatData m_angularLowerLimit;
int m_useLinearReferenceFrameA;
btVector3FloatData m_linearUpperLimit;
btVector3FloatData m_linearLowerLimit;
btVector3FloatData m_angularUpperLimit;
btVector3FloatData m_angularLowerLimit;
int m_useLinearReferenceFrameA;
int m_useOffsetForConstraintFrame;
};
struct btGeneric6DofConstraintDoubleData2
{
btTypedConstraintDoubleData m_typeConstraintData;
btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
btTypedConstraintDoubleData m_typeConstraintData;
btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
btTransformDoubleData m_rbBFrame;
btVector3DoubleData m_linearUpperLimit;
btVector3DoubleData m_linearLowerLimit;
btVector3DoubleData m_angularUpperLimit;
btVector3DoubleData m_angularLowerLimit;
int m_useLinearReferenceFrameA;
btVector3DoubleData m_linearUpperLimit;
btVector3DoubleData m_linearLowerLimit;
btVector3DoubleData m_angularUpperLimit;
btVector3DoubleData m_angularLowerLimit;
int m_useLinearReferenceFrameA;
int m_useOffsetForConstraintFrame;
};
SIMD_FORCE_INLINE int btGeneric6DofConstraint::calculateSerializeBufferSize() const
SIMD_FORCE_INLINE int btGeneric6DofConstraint::calculateSerializeBufferSize() const
{
return sizeof(btGeneric6DofConstraintData2);
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
{
btGeneric6DofConstraintData2* dof = (btGeneric6DofConstraintData2*)dataBuffer;
btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
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++)
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_angularLowerLimit.m_floats[i] = m_angularLimits[i].m_loLimit;
dof->m_angularUpperLimit.m_floats[i] = m_angularLimits[i].m_hiLimit;
dof->m_linearLowerLimit.m_floats[i] = m_linearLimits.m_lowerLimit[i];
dof->m_linearUpperLimit.m_floats[i] = m_linearLimits.m_upperLimit[i];
}
dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0;
dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA ? 1 : 0;
dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0;
return btGeneric6DofConstraintDataName;
}
#endif //BT_GENERIC_6DOF_CONSTRAINT_H
#endif //BT_GENERIC_6DOF_CONSTRAINT_H

619
src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
View File

File diff suppressed because it is too large Load Diff

535
src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
View File

@@ -37,7 +37,6 @@ email: projectileman@yahoo.com
http://gimpact.sf.net
*/
#ifndef BT_GENERIC_6DOF_CONSTRAINT2_H
#define BT_GENERIC_6DOF_CONSTRAINT2_H
@@ -47,18 +46,17 @@ http://gimpact.sf.net
class btRigidBody;
#ifdef BT_USE_DOUBLE_PRECISION
#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintDoubleData2
#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintDoubleData2"
#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintDoubleData2
#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintDoubleData2"
#else
#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintData
#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintData"
#endif //BT_USE_DOUBLE_PRECISION
#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintData
#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintData"
#endif //BT_USE_DOUBLE_PRECISION
enum RotateOrder
{
RO_XYZ=0,
RO_XYZ = 0,
RO_XZY,
RO_YXZ,
RO_YZX,
@@ -69,9 +67,9 @@ enum RotateOrder
class btRotationalLimitMotor2
{
public:
// upper < lower means free
// upper == lower means locked
// upper > lower means limited
// upper < lower means free
// upper == lower means locked
// upper > lower means limited
btScalar m_loLimit;
btScalar m_hiLimit;
btScalar m_bounce;
@@ -79,95 +77,92 @@ public:
btScalar m_stopCFM;
btScalar m_motorERP;
btScalar m_motorCFM;
bool m_enableMotor;
bool m_enableMotor;
btScalar m_targetVelocity;
btScalar m_maxMotorForce;
bool m_servoMotor;
bool m_servoMotor;
btScalar m_servoTarget;
bool m_enableSpring;
bool m_enableSpring;
btScalar m_springStiffness;
bool m_springStiffnessLimited;
bool m_springStiffnessLimited;
btScalar m_springDamping;
bool m_springDampingLimited;
bool m_springDampingLimited;
btScalar m_equilibriumPoint;
btScalar m_currentLimitError;
btScalar m_currentLimitErrorHi;
btScalar m_currentPosition;
int m_currentLimit;
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 = 6.0f;
m_servoMotor = false;
m_servoTarget = 0;
m_enableSpring = false;
m_springStiffness = 0;
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 = 6.0f;
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_springDamping = 0;
m_springDampingLimited = false;
m_equilibriumPoint = 0;
m_currentLimitError = 0;
m_currentLimitError = 0;
m_currentLimitErrorHi = 0;
m_currentPosition = 0;
m_currentLimit = 0;
m_currentPosition = 0;
m_currentLimit = 0;
}
btRotationalLimitMotor2(const btRotationalLimitMotor2 & limot)
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_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_springDamping = limot.m_springDamping;
m_springDampingLimited = limot.m_springDampingLimited;
m_equilibriumPoint = limot.m_equilibriumPoint;
m_currentLimitError = limot.m_currentLimitError;
m_currentLimitError = limot.m_currentLimitError;
m_currentLimitErrorHi = limot.m_currentLimitErrorHi;
m_currentPosition = limot.m_currentPosition;
m_currentLimit = limot.m_currentLimit;
m_currentPosition = limot.m_currentPosition;
m_currentLimit = limot.m_currentLimit;
}
bool isLimited()
{
if(m_loLimit > m_hiLimit) return false;
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
// upper < lower means free
// upper == lower means locked
// upper > lower means limited
btVector3 m_lowerLimit;
btVector3 m_upperLimit;
btVector3 m_bounce;
@@ -175,14 +170,14 @@ public:
btVector3 m_stopCFM;
btVector3 m_motorERP;
btVector3 m_motorCFM;
bool m_enableMotor[3];
bool m_servoMotor[3];
bool m_enableSpring[3];
bool m_enableMotor[3];
bool m_servoMotor[3];
bool m_enableSpring[3];
btVector3 m_servoTarget;
btVector3 m_springStiffness;
bool m_springStiffnessLimited[3];
bool m_springStiffnessLimited[3];
btVector3 m_springDamping;
bool m_springDampingLimited[3];
bool m_springDampingLimited[3];
btVector3 m_equilibriumPoint;
btVector3 m_targetVelocity;
btVector3 m_maxMotorForce;
@@ -190,69 +185,69 @@ public:
btVector3 m_currentLimitError;
btVector3 m_currentLimitErrorHi;
btVector3 m_currentLinearDiff;
int m_currentLimit[3];
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_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 );
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++)
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_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;
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 )
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_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;
m_currentLinearDiff = other.m_currentLinearDiff;
for(int i=0; i < 3; i++)
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_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_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];
m_currentLimit[i] = other.m_currentLimit[i];
}
}
@@ -271,13 +266,12 @@ enum bt6DofFlags2
BT_6DOF_FLAGS_CFM_MOTO2 = 4,
BT_6DOF_FLAGS_ERP_MOTO2 = 8
};
#define BT_6DOF_FLAGS_AXIS_SHIFT2 4 // bits per axis
#define BT_6DOF_FLAGS_AXIS_SHIFT2 4 // bits per axis
ATTRIBUTE_ALIGNED16(class) btGeneric6DofSpring2Constraint : public btTypedConstraint
ATTRIBUTE_ALIGNED16(class)
btGeneric6DofSpring2Constraint : public btTypedConstraint
{
protected:
btTransform m_frameInA;
btTransform m_frameInB;
@@ -290,45 +284,43 @@ protected:
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;
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&)
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);
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);
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);
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 void getInfo1(btConstraintInfo1 * info);
virtual void getInfo2(btConstraintInfo2 * info);
virtual int calculateSerializeBufferSize() const;
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
@@ -336,19 +328,19 @@ public:
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(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; }
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& getCalculatedTransformB() const { return m_calculatedTransformB; }
const btTransform & getFrameOffsetA() const { return m_frameInA; }
const btTransform & getFrameOffsetB() const { return m_frameInB; }
const btTransform& getFrameOffsetA() const { return m_frameInA; }
const btTransform& getFrameOffsetB() const { return m_frameInB; }
btTransform & getFrameOffsetA() { return m_frameInA; }
btTransform & getFrameOffsetB() { 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]; }
@@ -359,58 +351,58 @@ public:
// 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 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 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 getLinearUpperLimit(btVector3 & linearUpper) { linearUpper = m_linearLimits.m_upperLimit; }
void setAngularLowerLimit(const btVector3& angularLower)
{
for(int i = 0; i < 3; i++)
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++)
for (int i = 0; i < 3; i++)
m_angularLimits[i].m_hiLimit = btNormalizeAngle(-angularLower[i]);
}
void getAngularLowerLimit(btVector3& angularLower)
void getAngularLowerLimit(btVector3 & angularLower)
{
for(int i = 0; i < 3; i++)
for (int i = 0; i < 3; i++)
angularLower[i] = m_angularLimits[i].m_loLimit;
}
void getAngularLowerLimitReversed(btVector3& angularLower)
void getAngularLowerLimitReversed(btVector3 & angularLower)
{
for(int i = 0; i < 3; i++)
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++)
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++)
for (int i = 0; i < 3; i++)
m_angularLimits[i].m_loLimit = btNormalizeAngle(-angularUpper[i]);
}
void getAngularUpperLimit(btVector3& angularUpper)
void getAngularUpperLimit(btVector3 & angularUpper)
{
for(int i = 0; i < 3; i++)
for (int i = 0; i < 3; i++)
angularUpper[i] = m_angularLimits[i].m_hiLimit;
}
void getAngularUpperLimitReversed(btVector3& angularUpper)
void getAngularUpperLimitReversed(btVector3 & angularUpper)
{
for(int i = 0; i < 3; i++)
for (int i = 0; i < 3; i++)
angularUpper[i] = -m_angularLimits[i].m_loLimit;
}
@@ -418,7 +410,7 @@ public:
void setLimit(int axis, btScalar lo, btScalar hi)
{
if(axis<3)
if (axis < 3)
{
m_linearLimits.m_lowerLimit[axis] = lo;
m_linearLimits.m_upperLimit[axis] = hi;
@@ -427,14 +419,14 @@ public:
{
lo = btNormalizeAngle(lo);
hi = btNormalizeAngle(hi);
m_angularLimits[axis-3].m_loLimit = lo;
m_angularLimits[axis-3].m_hiLimit = 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)
if (axis < 3)
{
m_linearLimits.m_lowerLimit[axis] = lo;
m_linearLimits.m_upperLimit[axis] = hi;
@@ -443,54 +435,53 @@ public:
{
lo = btNormalizeAngle(lo);
hi = btNormalizeAngle(hi);
m_angularLimits[axis-3].m_hiLimit = -lo;
m_angularLimits[axis-3].m_loLimit = -hi;
m_angularLimits[axis - 3].m_hiLimit = -lo;
m_angularLimits[axis - 3].m_loLimit = -hi;
}
}
bool isLimited(int limitIndex)
{
if(limitIndex<3)
if (limitIndex < 3)
{
return m_linearLimits.isLimited(limitIndex);
}
return m_angularLimits[limitIndex-3].isLimited();
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 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 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 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).
//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);
};
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
{
@@ -511,12 +502,12 @@ struct btGeneric6DofSpring2ConstraintData
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];
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;
@@ -531,13 +522,13 @@ struct btGeneric6DofSpring2ConstraintData
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];
char m_angularEnableMotor[4];
char m_angularServoMotor[4];
char m_angularEnableSpring[4];
char m_angularSpringStiffnessLimited[4];
char m_angularSpringDampingLimited[4];
int m_rotateOrder;
int m_rotateOrder;
};
struct btGeneric6DofSpring2ConstraintDoubleData2
@@ -559,12 +550,12 @@ struct btGeneric6DofSpring2ConstraintDoubleData2
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];
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;
@@ -579,13 +570,13 @@ struct btGeneric6DofSpring2ConstraintDoubleData2
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];
char m_angularEnableMotor[4];
char m_angularServoMotor[4];
char m_angularEnableSpring[4];
char m_angularSpringStiffnessLimited[4];
char m_angularSpringDampingLimited[4];
int m_rotateOrder;
int m_rotateOrder;
};
SIMD_FORCE_INLINE int btGeneric6DofSpring2Constraint::calculateSerializeBufferSize() const
@@ -596,84 +587,80 @@ SIMD_FORCE_INLINE int btGeneric6DofSpring2Constraint::calculateSerializeBufferSi
SIMD_FORCE_INLINE const char* btGeneric6DofSpring2Constraint::serialize(void* dataBuffer, btSerializer* serializer) const
{
btGeneric6DofSpring2ConstraintData2* dof = (btGeneric6DofSpring2ConstraintData2*)dataBuffer;
btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
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++)
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_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_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++)
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;
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++)
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_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;
dof->m_padding1[0] = 0;
dof->m_padding1[1] = 0;
dof->m_padding1[2] = 0;
dof->m_padding1[3] = 0;
dof->m_padding1[0] = 0;
dof->m_padding1[1] = 0;
dof->m_padding1[2] = 0;
dof->m_padding1[3] = 0;
return btGeneric6DofSpring2ConstraintDataName;
}
#endif //BT_GENERIC_6DOF_CONSTRAINT_H
#endif //BT_GENERIC_6DOF_CONSTRAINT_H

64
src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp
View File

@@ -17,26 +17,23 @@ subject to the following restrictions:
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btTransformUtil.h"
btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA)
btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
: btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA)
{
init();
init();
}
btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)
: btGeneric6DofConstraint(rbB, frameInB, useLinearReferenceFrameB)
: btGeneric6DofConstraint(rbB, frameInB, useLinearReferenceFrameB)
{
init();
init();
}
void btGeneric6DofSpringConstraint::init()
{
m_objectType = D6_SPRING_CONSTRAINT_TYPE;
for(int i = 0; i < 6; i++)
for (int i = 0; i < 6; i++)
{
m_springEnabled[i] = false;
m_equilibriumPoint[i] = btScalar(0.f);
@@ -45,12 +42,11 @@ void btGeneric6DofSpringConstraint::init()
}
}
void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff)
{
btAssert((index >= 0) && (index < 6));
m_springEnabled[index] = onOff;
if(index < 3)
if (index < 3)
{
m_linearLimits.m_enableMotor[index] = onOff;
}
@@ -60,44 +56,38 @@ void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff)
}
}
void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness)
{
btAssert((index >= 0) && (index < 6));
m_springStiffness[index] = stiffness;
}
void btGeneric6DofSpringConstraint::setDamping(int index, btScalar damping)
{
btAssert((index >= 0) && (index < 6));
m_springDamping[index] = damping;
}
void btGeneric6DofSpringConstraint::setEquilibriumPoint()
{
calculateTransforms();
int i;
for( i = 0; i < 3; i++)
for (i = 0; i < 3; i++)
{
m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
}
for(i = 0; i < 3; i++)
for (i = 0; i < 3; i++)
{
m_equilibriumPoint[i + 3] = m_calculatedAxisAngleDiff[i];
}
}
void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index)
{
btAssert((index >= 0) && (index < 6));
calculateTransforms();
if(index < 3)
if (index < 3)
{
m_equilibriumPoint[index] = m_calculatedLinearDiff[index];
}
@@ -113,15 +103,14 @@ void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index, btScalar val)
m_equilibriumPoint[index] = val;
}
void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* info)
{
// it is assumed that calculateTransforms() have been called before this call
int i;
//btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity();
for(i = 0; i < 3; i++)
for (i = 0; i < 3; i++)
{
if(m_springEnabled[i])
if (m_springEnabled[i])
{
// get current position of constraint
btScalar currPos = m_calculatedLinearDiff[i];
@@ -130,28 +119,27 @@ void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* inf
// spring force is (delta * m_stiffness) according to Hooke's Law
btScalar force = delta * m_springStiffness[i];
btScalar velFactor = info->fps * m_springDamping[i] / btScalar(info->m_numIterations);
m_linearLimits.m_targetVelocity[i] = velFactor * force;
m_linearLimits.m_maxMotorForce[i] = btFabs(force);
m_linearLimits.m_targetVelocity[i] = velFactor * force;
m_linearLimits.m_maxMotorForce[i] = btFabs(force);
}
}
for(i = 0; i < 3; i++)
for (i = 0; i < 3; i++)
{
if(m_springEnabled[i + 3])
if (m_springEnabled[i + 3])
{
// get current position of constraint
btScalar currPos = m_calculatedAxisAngleDiff[i];
// calculate difference
btScalar delta = currPos - m_equilibriumPoint[i+3];
btScalar delta = currPos - m_equilibriumPoint[i + 3];
// spring force is (-delta * m_stiffness) according to Hooke's Law
btScalar force = -delta * m_springStiffness[i+3];
btScalar velFactor = info->fps * m_springDamping[i+3] / btScalar(info->m_numIterations);
btScalar force = -delta * m_springStiffness[i + 3];
btScalar velFactor = info->fps * m_springDamping[i + 3] / btScalar(info->m_numIterations);
m_angularLimits[i].m_targetVelocity = velFactor * force;
m_angularLimits[i].m_maxMotorForce = btFabs(force);
}
}
}
void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info)
{
// this will be called by constraint solver at the constraint setup stage
@@ -161,25 +149,21 @@ void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info)
btGeneric6DofConstraint::getInfo2(info);
}
void btGeneric6DofSpringConstraint::setAxis(const btVector3& axis1,const btVector3& axis2)
void btGeneric6DofSpringConstraint::setAxis(const btVector3& axis1, const btVector3& axis2)
{
btVector3 zAxis = axis1.normalized();
btVector3 yAxis = axis2.normalized();
btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
btTransform frameInW;
frameInW.setIdentity();
frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
xAxis[1], yAxis[1], zAxis[1],
xAxis[2], yAxis[2], zAxis[2]);
frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
xAxis[1], yAxis[1], zAxis[1],
xAxis[2], yAxis[2], zAxis[2]);
// now get constraint frame in local coordinate systems
m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
calculateTransforms();
calculateTransforms();
}

96
src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h
View File

@@ -16,20 +16,17 @@ subject to the following restrictions:
#ifndef BT_GENERIC_6DOF_SPRING_CONSTRAINT_H
#define BT_GENERIC_6DOF_SPRING_CONSTRAINT_H
#include "LinearMath/btVector3.h"
#include "btTypedConstraint.h"
#include "btGeneric6DofConstraint.h"
#ifdef BT_USE_DOUBLE_PRECISION
#define btGeneric6DofSpringConstraintData2 btGeneric6DofSpringConstraintDoubleData2
#define btGeneric6DofSpringConstraintDataName "btGeneric6DofSpringConstraintDoubleData2"
#define btGeneric6DofSpringConstraintData2 btGeneric6DofSpringConstraintDoubleData2
#define btGeneric6DofSpringConstraintDataName "btGeneric6DofSpringConstraintDoubleData2"
#else
#define btGeneric6DofSpringConstraintData2 btGeneric6DofSpringConstraintData
#define btGeneric6DofSpringConstraintDataName "btGeneric6DofSpringConstraintData"
#endif //BT_USE_DOUBLE_PRECISION
#define btGeneric6DofSpringConstraintData2 btGeneric6DofSpringConstraintData
#define btGeneric6DofSpringConstraintDataName "btGeneric6DofSpringConstraintData"
#endif //BT_USE_DOUBLE_PRECISION
/// Generic 6 DOF constraint that allows to set spring motors to any translational and rotational DOF
@@ -41,101 +38,98 @@ subject to the following restrictions:
/// 4 : rotation Y (2nd Euler rotational around new position of Y axis, range [-PI/2+epsilon, PI/2-epsilon] )
/// 5 : rotation Z (1st Euler rotational around Z axis, range [-PI+epsilon, PI-epsilon] )
ATTRIBUTE_ALIGNED16(class) btGeneric6DofSpringConstraint : public btGeneric6DofConstraint
ATTRIBUTE_ALIGNED16(class)
btGeneric6DofSpringConstraint : public btGeneric6DofConstraint
{
protected:
bool m_springEnabled[6];
btScalar m_equilibriumPoint[6];
btScalar m_springStiffness[6];
btScalar m_springDamping[6]; // between 0 and 1 (1 == no damping)
bool m_springEnabled[6];
btScalar m_equilibriumPoint[6];
btScalar m_springStiffness[6];
btScalar m_springDamping[6]; // between 0 and 1 (1 == no damping)
void init();
void internalUpdateSprings(btConstraintInfo2* info);
public:
void internalUpdateSprings(btConstraintInfo2 * info);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
btGeneric6DofSpringConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
btGeneric6DofSpringConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA);
btGeneric6DofSpringConstraint(btRigidBody & rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
void enableSpring(int index, bool onOff);
void setStiffness(int index, btScalar stiffness);
void setDamping(int index, btScalar damping);
void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF
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);
bool isSpringEnabled(int index) const
{
return m_springEnabled[index];
return m_springEnabled[index];
}
btScalar getStiffness(int index) const
{
return m_springStiffness[index];
return m_springStiffness[index];
}
btScalar getDamping(int index) const
{
return m_springDamping[index];
return m_springDamping[index];
}
btScalar getEquilibriumPoint(int index) const
{
return m_equilibriumPoint[index];
return m_equilibriumPoint[index];
}
virtual void setAxis( const btVector3& axis1, const btVector3& axis2);
virtual void setAxis(const btVector3& axis1, const btVector3& axis2);
virtual void getInfo2 (btConstraintInfo2* info);
virtual void getInfo2(btConstraintInfo2 * info);
virtual int calculateSerializeBufferSize() const;
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
};
struct btGeneric6DofSpringConstraintData
{
btGeneric6DofConstraintData m_6dofData;
int m_springEnabled[6];
float m_equilibriumPoint[6];
float m_springStiffness[6];
float m_springDamping[6];
btGeneric6DofConstraintData m_6dofData;
int m_springEnabled[6];
float m_equilibriumPoint[6];
float m_springStiffness[6];
float m_springDamping[6];
};
struct btGeneric6DofSpringConstraintDoubleData2
{
btGeneric6DofConstraintDoubleData2 m_6dofData;
int m_springEnabled[6];
double m_equilibriumPoint[6];
double m_springStiffness[6];
double m_springDamping[6];
btGeneric6DofConstraintDoubleData2 m_6dofData;
int m_springEnabled[6];
double m_equilibriumPoint[6];
double m_springStiffness[6];
double m_springDamping[6];
};
SIMD_FORCE_INLINE int btGeneric6DofSpringConstraint::calculateSerializeBufferSize() const
SIMD_FORCE_INLINE int btGeneric6DofSpringConstraint::calculateSerializeBufferSize() const
{
return sizeof(btGeneric6DofSpringConstraintData2);
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* btGeneric6DofSpringConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* btGeneric6DofSpringConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
{
btGeneric6DofSpringConstraintData2* dof = (btGeneric6DofSpringConstraintData2*)dataBuffer;
btGeneric6DofConstraint::serialize(&dof->m_6dofData,serializer);
btGeneric6DofConstraint::serialize(&dof->m_6dofData, serializer);
int i;
for (i=0;i<6;i++)
for (i = 0; i < 6; i++)
{
dof->m_equilibriumPoint[i] = m_equilibriumPoint[i];
dof->m_springDamping[i] = m_springDamping[i];
dof->m_springEnabled[i] = m_springEnabled[i]? 1 : 0;
dof->m_springEnabled[i] = m_springEnabled[i] ? 1 : 0;
dof->m_springStiffness[i] = m_springStiffness[i];
}
return btGeneric6DofSpringConstraintDataName;
}
#endif // BT_GENERIC_6DOF_SPRING_CONSTRAINT_H
#endif // BT_GENERIC_6DOF_SPRING_CONSTRAINT_H

29
src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp
View File

@@ -13,54 +13,49 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btHinge2Constraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btTransformUtil.h"
// constructor
// 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)
: btGeneric6DofSpring2Constraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(),RO_XYZ),
m_anchor(anchor),
m_axis1(axis1),
m_axis2(axis2)
: btGeneric6DofSpring2Constraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), RO_XYZ),
m_anchor(anchor),
m_axis1(axis1),
m_axis2(axis2)
{
// build frame basis
// 6DOF constraint uses Euler angles and to define limits
// it is assumed that rotational order is :
// Z - first, allowed limits are (-PI,PI);
// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number
// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number
// used to prevent constraint from instability on poles;
// new position of X, allowed limits are (-PI,PI);
// So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
// Build the frame in world coordinate system first
btVector3 zAxis = axis1.normalize();
btVector3 xAxis = axis2.normalize();
btVector3 yAxis = zAxis.cross(xAxis); // we want right coordinate system
btVector3 yAxis = zAxis.cross(xAxis); // we want right coordinate system
btTransform frameInW;
frameInW.setIdentity();
frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
xAxis[1], yAxis[1], zAxis[1],
xAxis[2], yAxis[2], zAxis[2]);
frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
xAxis[1], yAxis[1], zAxis[1],
xAxis[2], yAxis[2], zAxis[2]);
frameInW.setOrigin(anchor);
// now get constraint frame in local coordinate systems
m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW;
m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW;
// sei limits
setLinearLowerLimit(btVector3(0.f, 0.f, -1.f));
setLinearUpperLimit(btVector3(0.f, 0.f, 1.f));
setLinearUpperLimit(btVector3(0.f, 0.f, 1.f));
// like front wheels of a car
setAngularLowerLimit(btVector3(1.f, 0.f, -SIMD_HALF_PI * 0.5f));
setAngularUpperLimit(btVector3(-1.f, 0.f, SIMD_HALF_PI * 0.5f));
setAngularLowerLimit(btVector3(1.f, 0.f, -SIMD_HALF_PI * 0.5f));
setAngularUpperLimit(btVector3(-1.f, 0.f, SIMD_HALF_PI * 0.5f));
// enable suspension
enableSpring(2, true);
setStiffness(2, SIMD_PI * SIMD_PI * 4.f);
setDamping(2, 0.01f);
setEquilibriumPoint();
}

27
src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h
View File

@@ -16,32 +16,30 @@ subject to the following restrictions:
#ifndef BT_HINGE2_CONSTRAINT_H
#define BT_HINGE2_CONSTRAINT_H
#include "LinearMath/btVector3.h"
#include "btTypedConstraint.h"
#include "btGeneric6DofSpring2Constraint.h"
// Constraint similar to ODE Hinge2 Joint
// has 3 degrees of frredom:
// 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 btGeneric6DofSpring2Constraint
ATTRIBUTE_ALIGNED16(class)
btHinge2Constraint : public btGeneric6DofSpring2Constraint
{
protected:
btVector3 m_anchor;
btVector3 m_axis1;
btVector3 m_axis2;
btVector3 m_anchor;
btVector3 m_axis1;
btVector3 m_axis2;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
BT_DECLARE_ALIGNED_ALLOCATOR();
// constructor
// anchor, axis1 and axis2 are in world coordinate system
// axis1 must be orthogonal to axis2
btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2);
btHinge2Constraint(btRigidBody & rbA, btRigidBody & rbB, btVector3 & anchor, btVector3 & axis1, btVector3 & axis2);
// access
const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); }
const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); }
@@ -51,10 +49,7 @@ public:
btScalar getAngle2() { return getAngle(0); }
// limits
void setUpperLimit(btScalar ang1max) { setAngularUpperLimit(btVector3(-1.f, 0.f, ang1max)); }
void setLowerLimit(btScalar ang1min) { setAngularLowerLimit(btVector3( 1.f, 0.f, ang1min)); }
void setLowerLimit(btScalar ang1min) { setAngularLowerLimit(btVector3(1.f, 0.f, ang1min)); }
};
#endif // BT_HINGE2_CONSTRAINT_H
#endif // BT_HINGE2_CONSTRAINT_H

669
src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
View File

File diff suppressed because it is too large Load Diff

415
src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
View File

@@ -20,7 +20,6 @@ subject to the following restrictions:
#define _BT_USE_CENTER_LIMIT_ 1
#include "LinearMath/btVector3.h"
#include "btJacobianEntry.h"
#include "btTypedConstraint.h"
@@ -28,14 +27,12 @@ subject to the following restrictions:
class btRigidBody;
#ifdef BT_USE_DOUBLE_PRECISION
#define btHingeConstraintData btHingeConstraintDoubleData2 //rename to 2 for backwards compatibility, so we can still load the 'btHingeConstraintDoubleData' version
#define btHingeConstraintDataName "btHingeConstraintDoubleData2"
#define btHingeConstraintData btHingeConstraintDoubleData2 //rename to 2 for backwards compatibility, so we can still load the 'btHingeConstraintDoubleData' version
#define btHingeConstraintDataName "btHingeConstraintDoubleData2"
#else
#define btHingeConstraintData btHingeConstraintFloatData
#define btHingeConstraintDataName "btHingeConstraintFloatData"
#endif //BT_USE_DOUBLE_PRECISION
#define btHingeConstraintData btHingeConstraintFloatData
#define btHingeConstraintDataName "btHingeConstraintFloatData"
#endif //BT_USE_DOUBLE_PRECISION
enum btHingeFlags
{
@@ -45,89 +42,83 @@ enum btHingeFlags
BT_HINGE_FLAGS_ERP_NORM = 8
};
/// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
/// axis defines the orientation of the hinge axis
ATTRIBUTE_ALIGNED16(class) btHingeConstraint : public btTypedConstraint
ATTRIBUTE_ALIGNED16(class)
btHingeConstraint : public btTypedConstraint
{
#ifdef IN_PARALLELL_SOLVER
public:
#endif
btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
btJacobianEntry m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
btJacobianEntry m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis.
btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis.
btTransform m_rbBFrame;
btScalar m_motorTargetVelocity;
btScalar m_maxMotorImpulse;
btScalar m_motorTargetVelocity;
btScalar m_maxMotorImpulse;
#ifdef _BT_USE_CENTER_LIMIT_
btAngularLimit m_limit;
#ifdef _BT_USE_CENTER_LIMIT_
btAngularLimit m_limit;
#else
btScalar m_lowerLimit;
btScalar m_upperLimit;
btScalar m_limitSign;
btScalar m_correction;
btScalar m_lowerLimit;
btScalar m_upperLimit;
btScalar m_limitSign;
btScalar m_correction;
btScalar m_limitSoftness;
btScalar m_biasFactor;
btScalar m_relaxationFactor;
btScalar m_limitSoftness;
btScalar m_biasFactor;
btScalar m_relaxationFactor;
bool m_solveLimit;
bool m_solveLimit;
#endif
btScalar m_kHinge;
btScalar m_kHinge;
btScalar m_accLimitImpulse;
btScalar m_hingeAngle;
btScalar m_referenceSign;
btScalar m_accLimitImpulse;
btScalar m_hingeAngle;
btScalar m_referenceSign;
bool m_angularOnly;
bool m_enableAngularMotor;
bool m_useSolveConstraintObsolete;
bool m_useOffsetForConstraintFrame;
bool m_useReferenceFrameA;
bool m_angularOnly;
bool m_enableAngularMotor;
bool m_useSolveConstraintObsolete;
bool m_useOffsetForConstraintFrame;
bool m_useReferenceFrameA;
btScalar m_accMotorImpulse;
btScalar m_accMotorImpulse;
int m_flags;
btScalar m_normalCFM;
btScalar m_normalERP;
btScalar m_stopCFM;
btScalar m_stopERP;
int m_flags;
btScalar m_normalCFM;
btScalar m_normalERP;
btScalar m_stopCFM;
btScalar m_stopERP;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false);
btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false);
btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
btHingeConstraint(btRigidBody & rbA, btRigidBody & rbB, const btVector3& pivotInA, const btVector3& pivotInB, const btVector3& axisInA, const btVector3& axisInB, bool useReferenceFrameA = false);
btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false);
btHingeConstraint(btRigidBody & rbA, const btVector3& pivotInA, const btVector3& axisInA, bool useReferenceFrameA = false);
btHingeConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
virtual void buildJacobian();
btHingeConstraint(btRigidBody & rbA, const btTransform& rbAFrame, bool useReferenceFrameA = false);
virtual void getInfo1 (btConstraintInfo1* info);
virtual void buildJacobian();
void getInfo1NonVirtual(btConstraintInfo1* info);
virtual void getInfo1(btConstraintInfo1 * info);
virtual void getInfo2 (btConstraintInfo2* info);
void getInfo1NonVirtual(btConstraintInfo1 * info);
void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
virtual void getInfo2(btConstraintInfo2 * info);
void getInfo2Internal(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
void getInfo2InternalUsingFrameOffset(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB);
void updateRHS(btScalar timeStep);
void getInfo2Internal(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB);
void getInfo2InternalUsingFrameOffset(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB);
void updateRHS(btScalar timeStep);
const btRigidBody& getRigidBodyA() const
{
@@ -138,19 +129,19 @@ public:
return m_rbB;
}
btRigidBody& getRigidBodyA()
{
return m_rbA;
}
btRigidBody& getRigidBodyA()
{
return m_rbA;
}
btRigidBody& getRigidBodyB()
{
return m_rbB;
btRigidBody& getRigidBodyB()
{
return m_rbB;
}
btTransform& getFrameOffsetA()
{
return m_rbAFrame;
return m_rbAFrame;
}
btTransform& getFrameOffsetB()
@@ -159,15 +150,15 @@ public:
}
void setFrames(const btTransform& frameA, const btTransform& frameB);
void setAngularOnly(bool angularOnly)
void setAngularOnly(bool angularOnly)
{
m_angularOnly = angularOnly;
}
void enableAngularMotor(bool enableMotor,btScalar targetVelocity,btScalar maxMotorImpulse)
void enableAngularMotor(bool enableMotor, btScalar targetVelocity, btScalar maxMotorImpulse)
{
m_enableAngularMotor = enableMotor;
m_enableAngularMotor = enableMotor;
m_motorTargetVelocity = targetVelocity;
m_maxMotorImpulse = maxMotorImpulse;
}
@@ -175,29 +166,28 @@ public:
// extra motor API, including ability to set a target rotation (as opposed to angular velocity)
// note: setMotorTarget sets angular velocity under the hood, so you must call it every tick to
// maintain a given angular target.
void enableMotor(bool enableMotor) { m_enableAngularMotor = enableMotor; }
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(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B.
void setMotorTarget(btScalar targetAngle, btScalar dt);
void setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
void setLimit(btScalar low, btScalar high, btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
{
#ifdef _BT_USE_CENTER_LIMIT_
#ifdef _BT_USE_CENTER_LIMIT_
m_limit.set(low, high, _softness, _biasFactor, _relaxationFactor);
#else
m_lowerLimit = btNormalizeAngle(low);
m_upperLimit = btNormalizeAngle(high);
m_limitSoftness = _softness;
m_limitSoftness = _softness;
m_biasFactor = _biasFactor;
m_relaxationFactor = _relaxationFactor;
#endif
}
btScalar getLimitSoftness() const
{
#ifdef _BT_USE_CENTER_LIMIT_
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.getSoftness();
#else
return m_limitSoftness;
@@ -206,7 +196,7 @@ public:
btScalar getLimitBiasFactor() const
{
#ifdef _BT_USE_CENTER_LIMIT_
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.getBiasFactor();
#else
return m_biasFactor;
@@ -215,112 +205,110 @@ public:
btScalar getLimitRelaxationFactor() const
{
#ifdef _BT_USE_CENTER_LIMIT_
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.getRelaxationFactor();
#else
return m_relaxationFactor;
#endif
}
void setAxis(btVector3& axisInA)
void setAxis(btVector3 & axisInA)
{
btVector3 rbAxisA1, rbAxisA2;
btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
btVector3 pivotInA = m_rbAFrame.getOrigin();
// m_rbAFrame.getOrigin() = pivotInA;
m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
// m_rbAFrame.getOrigin() = pivotInA;
m_rbAFrame.getBasis().setValue(rbAxisA1.getX(), rbAxisA2.getX(), axisInA.getX(),
rbAxisA1.getY(), rbAxisA2.getY(), axisInA.getY(),
rbAxisA1.getZ(), rbAxisA2.getZ(), axisInA.getZ());
btVector3 axisInB = m_rbA.getCenterOfMassTransform().getBasis() * axisInA;
btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
btQuaternion rotationArc = shortestArcQuat(axisInA, axisInB);
btVector3 rbAxisB1 = quatRotate(rotationArc, rbAxisA1);
btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
m_rbBFrame.getOrigin() = m_rbB.getCenterOfMassTransform().inverse()(m_rbA.getCenterOfMassTransform()(pivotInA));
m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
m_rbBFrame.getBasis().setValue(rbAxisB1.getX(), rbAxisB2.getX(), axisInB.getX(),
rbAxisB1.getY(), rbAxisB2.getY(), axisInB.getY(),
rbAxisB1.getZ(), rbAxisB2.getZ(), axisInB.getZ());
m_rbBFrame.getBasis() = m_rbB.getCenterOfMassTransform().getBasis().inverse() * m_rbBFrame.getBasis();
}
bool hasLimit() const {
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.getHalfRange() > 0;
#else
return m_lowerLimit <= m_upperLimit;
#endif
}
btScalar getLowerLimit() const
bool hasLimit() const
{
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.getLow();
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.getHalfRange() > 0;
#else
return m_lowerLimit;
return m_lowerLimit <= m_upperLimit;
#endif
}
btScalar getUpperLimit() const
btScalar getLowerLimit() const
{
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.getHigh();
#else
return m_upperLimit;
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.getLow();
#else
return m_lowerLimit;
#endif
}
btScalar getUpperLimit() const
{
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.getHigh();
#else
return m_upperLimit;
#endif
}
///The getHingeAngle gives the hinge angle in range [-PI,PI]
btScalar getHingeAngle();
btScalar getHingeAngle(const btTransform& transA,const btTransform& transB);
btScalar getHingeAngle(const btTransform& transA, const btTransform& transB);
void testLimit(const btTransform& transA,const btTransform& transB);
void testLimit(const btTransform& transA, const btTransform& transB);
const btTransform& getAFrame() const { return m_rbAFrame; };
const btTransform& getAFrame() const { return m_rbAFrame; };
const btTransform& getBFrame() const { return m_rbBFrame; };
btTransform& getAFrame() { return m_rbAFrame; };
btTransform& getAFrame() { return m_rbAFrame; };
btTransform& getBFrame() { return m_rbBFrame; };
inline int getSolveLimit()
{
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.isLimit();
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.isLimit();
#else
return m_solveLimit;
return m_solveLimit;
#endif
}
inline btScalar getLimitSign()
{
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.getSign();
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.getSign();
#else
return m_limitSign;
#endif
}
inline bool getAngularOnly()
{
return m_angularOnly;
inline bool getAngularOnly()
{
return m_angularOnly;
}
inline bool getEnableAngularMotor()
{
return m_enableAngularMotor;
inline bool getEnableAngularMotor()
{
return m_enableAngularMotor;
}
inline btScalar getMotorTargetVelocity()
{
return m_motorTargetVelocity;
inline btScalar getMotorTargetVelocity()
{
return m_motorTargetVelocity;
}
inline btScalar getMaxMotorImpulse()
{
return m_maxMotorImpulse;
inline btScalar getMaxMotorImpulse()
{
return m_maxMotorImpulse;
}
// access for UseFrameOffset
bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
@@ -329,143 +317,132 @@ public:
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).
///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 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
virtual btScalar getParam(int num, int axis = -1) const;
virtual int getFlags() const
{
return m_flags;
return m_flags;
}
virtual int calculateSerializeBufferSize() const;
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
};
//only for backward compatibility
#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
///this structure is not used, except for loading pre-2.82 .bullet files
struct btHingeConstraintDoubleData
struct btHingeConstraintDoubleData
{
btTypedConstraintData m_typeConstraintData;
btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
btTypedConstraintData m_typeConstraintData;
btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
btTransformDoubleData m_rbBFrame;
int m_useReferenceFrameA;
int m_angularOnly;
int m_enableAngularMotor;
float m_motorTargetVelocity;
float m_maxMotorImpulse;
float m_lowerLimit;
float m_upperLimit;
float m_limitSoftness;
float m_biasFactor;
float m_relaxationFactor;
int m_useReferenceFrameA;
int m_angularOnly;
int m_enableAngularMotor;
float m_motorTargetVelocity;
float m_maxMotorImpulse;
float m_lowerLimit;
float m_upperLimit;
float m_limitSoftness;
float m_biasFactor;
float m_relaxationFactor;
};
#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
#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
ATTRIBUTE_ALIGNED16(class)
btHingeAccumulatedAngleConstraint : public btHingeConstraint
{
protected:
btScalar m_accumulatedAngle;
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 )
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();
m_accumulatedAngle = getHingeAngle();
}
btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false)
:btHingeConstraint(rbA,pivotInA,axisInA, useReferenceFrameA)
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();
m_accumulatedAngle = getHingeAngle();
}
btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false)
:btHingeConstraint(rbA,rbAFrame, useReferenceFrameA )
btHingeAccumulatedAngleConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false)
: btHingeConstraint(rbA, rbB, rbAFrame, rbBFrame, useReferenceFrameA)
{
m_accumulatedAngle=getHingeAngle();
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);
void setAccumulatedHingeAngle(btScalar accAngle);
virtual void getInfo1(btConstraintInfo1 * info);
};
struct btHingeConstraintFloatData
struct btHingeConstraintFloatData
{
btTypedConstraintData m_typeConstraintData;
btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
btTypedConstraintData m_typeConstraintData;
btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
btTransformFloatData m_rbBFrame;
int m_useReferenceFrameA;
int m_angularOnly;
int m_enableAngularMotor;
float m_motorTargetVelocity;
float m_maxMotorImpulse;
int m_useReferenceFrameA;
int m_angularOnly;
float m_lowerLimit;
float m_upperLimit;
float m_limitSoftness;
float m_biasFactor;
float m_relaxationFactor;
int m_enableAngularMotor;
float m_motorTargetVelocity;
float m_maxMotorImpulse;
float m_lowerLimit;
float m_upperLimit;
float m_limitSoftness;
float m_biasFactor;
float m_relaxationFactor;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btHingeConstraintDoubleData2
struct btHingeConstraintDoubleData2
{
btTypedConstraintDoubleData m_typeConstraintData;
btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
btTypedConstraintDoubleData m_typeConstraintData;
btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
btTransformDoubleData m_rbBFrame;
int m_useReferenceFrameA;
int m_angularOnly;
int m_enableAngularMotor;
double m_motorTargetVelocity;
double m_maxMotorImpulse;
double m_lowerLimit;
double m_upperLimit;
double m_limitSoftness;
double m_biasFactor;
double m_relaxationFactor;
char m_padding1[4];
int m_useReferenceFrameA;
int m_angularOnly;
int m_enableAngularMotor;
double m_motorTargetVelocity;
double m_maxMotorImpulse;
double m_lowerLimit;
double m_upperLimit;
double m_limitSoftness;
double m_biasFactor;
double m_relaxationFactor;
char m_padding1[4];
};
SIMD_FORCE_INLINE int btHingeConstraint::calculateSerializeBufferSize() const
SIMD_FORCE_INLINE int btHingeConstraint::calculateSerializeBufferSize() const
{
return sizeof(btHingeConstraintData);
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
{
btHingeConstraintData* hingeData = (btHingeConstraintData*)dataBuffer;
btTypedConstraint::serialize(&hingeData->m_typeConstraintData,serializer);
btTypedConstraint::serialize(&hingeData->m_typeConstraintData, serializer);
m_rbAFrame.serialize(hingeData->m_rbAFrame);
m_rbBFrame.serialize(hingeData->m_rbBFrame);
@@ -475,7 +452,7 @@ SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btS
hingeData->m_maxMotorImpulse = float(m_maxMotorImpulse);
hingeData->m_motorTargetVelocity = float(m_motorTargetVelocity);
hingeData->m_useReferenceFrameA = m_useReferenceFrameA;
#ifdef _BT_USE_CENTER_LIMIT_
#ifdef _BT_USE_CENTER_LIMIT_
hingeData->m_lowerLimit = float(m_limit.getLow());
hingeData->m_upperLimit = float(m_limit.getHigh());
hingeData->m_limitSoftness = float(m_limit.getSoftness());
@@ -500,4 +477,4 @@ SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btS
return btHingeConstraintDataName;
}
#endif //BT_HINGECONSTRAINT_H
#endif //BT_HINGECONSTRAINT_H

103
src/BulletDynamics/ConstraintSolver/btJacobianEntry.h
View File

@@ -18,7 +18,6 @@ subject to the following restrictions:
#include "LinearMath/btMatrix3x3.h"
//notes:
// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components
// which makes the btJacobianEntry memory layout 16 bytes
@@ -27,25 +26,26 @@ subject to the following restrictions:
/// Jacobian entry is an abstraction that allows to describe constraints
/// it can be used in combination with a constraint solver
/// Can be used to relate the effect of an impulse to the constraint error
ATTRIBUTE_ALIGNED16(class) btJacobianEntry
ATTRIBUTE_ALIGNED16(class)
btJacobianEntry
{
public:
btJacobianEntry() {};
btJacobianEntry(){};
//constraint between two different rigidbodies
btJacobianEntry(
const btMatrix3x3& world2A,
const btMatrix3x3& world2B,
const btVector3& rel_pos1,const btVector3& rel_pos2,
const btVector3& rel_pos1, const btVector3& rel_pos2,
const btVector3& jointAxis,
const btVector3& inertiaInvA,
const btVector3& inertiaInvA,
const btScalar massInvA,
const btVector3& inertiaInvB,
const btScalar massInvB)
:m_linearJointAxis(jointAxis)
: m_linearJointAxis(jointAxis)
{
m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis));
m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis));
m_0MinvJt = inertiaInvA * m_aJ;
m_aJ = world2A * (rel_pos1.cross(m_linearJointAxis));
m_bJ = world2B * (rel_pos2.cross(-m_linearJointAxis));
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
@@ -54,33 +54,31 @@ public:
//angular constraint between two different rigidbodies
btJacobianEntry(const btVector3& jointAxis,
const btMatrix3x3& world2A,
const btMatrix3x3& world2B,
const btVector3& inertiaInvA,
const btVector3& inertiaInvB)
:m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
const btMatrix3x3& world2A,
const btMatrix3x3& world2B,
const btVector3& inertiaInvA,
const btVector3& inertiaInvB)
: m_linearJointAxis(btVector3(btScalar(0.), btScalar(0.), btScalar(0.)))
{
m_aJ= world2A*jointAxis;
m_bJ = world2B*-jointAxis;
m_0MinvJt = inertiaInvA * m_aJ;
m_aJ = world2A * jointAxis;
m_bJ = world2B * -jointAxis;
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
btAssert(m_Adiag > btScalar(0.0));
}
//angular constraint between two different rigidbodies
btJacobianEntry(const btVector3& axisInA,
const btVector3& axisInB,
const btVector3& inertiaInvA,
const btVector3& inertiaInvB)
: m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
, m_aJ(axisInA)
, m_bJ(-axisInB)
const btVector3& axisInB,
const btVector3& inertiaInvA,
const btVector3& inertiaInvB)
: m_linearJointAxis(btVector3(btScalar(0.), btScalar(0.), btScalar(0.))), m_aJ(axisInA), m_bJ(-axisInB)
{
m_0MinvJt = inertiaInvA * m_aJ;
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
btAssert(m_Adiag > btScalar(0.0));
}
@@ -88,25 +86,25 @@ public:
//constraint on one rigidbody
btJacobianEntry(
const btMatrix3x3& world2A,
const btVector3& rel_pos1,const btVector3& rel_pos2,
const btVector3& rel_pos1, const btVector3& rel_pos2,
const btVector3& jointAxis,
const btVector3& inertiaInvA,
const btVector3& inertiaInvA,
const btScalar massInvA)
:m_linearJointAxis(jointAxis)
: m_linearJointAxis(jointAxis)
{
m_aJ= world2A*(rel_pos1.cross(jointAxis));
m_bJ = world2A*(rel_pos2.cross(-jointAxis));
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = btVector3(btScalar(0.),btScalar(0.),btScalar(0.));
m_aJ = world2A * (rel_pos1.cross(jointAxis));
m_bJ = world2A * (rel_pos2.cross(-jointAxis));
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = btVector3(btScalar(0.), btScalar(0.), btScalar(0.));
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
btAssert(m_Adiag > btScalar(0.0));
}
btScalar getDiagonal() const { return m_Adiag; }
btScalar getDiagonal() const { return m_Adiag; }
// for two constraints on the same rigidbody (for example vehicle friction)
btScalar getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA) const
btScalar getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA) const
{
const btJacobianEntry& jacA = *this;
btScalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
@@ -114,42 +112,39 @@ public:
return lin + ang;
}
// for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
btScalar getNonDiagonal(const btJacobianEntry& jacB,const btScalar massInvA,const btScalar massInvB) const
btScalar getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA, const btScalar massInvB) const
{
const btJacobianEntry& jacA = *this;
btVector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
btVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
btVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
btVector3 lin0 = massInvA * lin ;
btVector3 lin0 = massInvA * lin;
btVector3 lin1 = massInvB * lin;
btVector3 sum = ang0+ang1+lin0+lin1;
return sum[0]+sum[1]+sum[2];
btVector3 sum = ang0 + ang1 + lin0 + lin1;
return sum[0] + sum[1] + sum[2];
}
btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB)
btScalar getRelativeVelocity(const btVector3& linvelA, const btVector3& angvelA, const btVector3& linvelB, const btVector3& angvelB)
{
btVector3 linrel = linvelA - linvelB;
btVector3 angvela = angvelA * m_aJ;
btVector3 angvelb = angvelB * m_bJ;
btVector3 angvela = angvelA * m_aJ;
btVector3 angvelb = angvelB * m_bJ;
linrel *= m_linearJointAxis;
angvela += angvelb;
angvela += linrel;
btScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
btScalar rel_vel2 = angvela[0] + angvela[1] + angvela[2];
return rel_vel2 + SIMD_EPSILON;
}
//private:
//private:
btVector3 m_linearJointAxis;
btVector3 m_aJ;
btVector3 m_bJ;
btVector3 m_0MinvJt;
btVector3 m_1MinvJt;
btVector3 m_linearJointAxis;
btVector3 m_aJ;
btVector3 m_bJ;
btVector3 m_0MinvJt;
btVector3 m_1MinvJt;
//Optimization: can be stored in the w/last component of one of the vectors
btScalar m_Adiag;
btScalar m_Adiag;
};
#endif //BT_JACOBIAN_ENTRY_H
#endif //BT_JACOBIAN_ENTRY_H

262
src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp
View File

@@ -15,14 +15,9 @@ subject to the following restrictions:
#include "btNNCGConstraintSolver.h"
btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
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);
btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
m_pNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
m_pC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
@@ -37,38 +32,39 @@ btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject*
return val;
}
btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
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)
if (1) // uncomment this for a bit less random ((iteration & 7) == 0)
{
for (int j=0; j<numNonContactPool; ++j) {
for (int j = 0; j < numNonContactPool; ++j)
{
int tmp = m_orderNonContactConstraintPool[j];
int swapi = btRandInt2(j+1);
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)
//contact/friction constraints are not solved more than
if (iteration < infoGlobal.m_numIterations)
{
for (int j=0; j<numConstraintPool; ++j) {
for (int j = 0; j < numConstraintPool; ++j)
{
int tmp = m_orderTmpConstraintPool[j];
int swapi = btRandInt2(j+1);
int swapi = btRandInt2(j + 1);
m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
m_orderTmpConstraintPool[swapi] = tmp;
}
for (int j=0; j<numFrictionPool; ++j) {
for (int j = 0; j < numFrictionPool; ++j)
{
int tmp = m_orderFrictionConstraintPool[j];
int swapi = btRandInt2(j+1);
int swapi = btRandInt2(j + 1);
m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
m_orderFrictionConstraintPool[swapi] = tmp;
}
@@ -76,39 +72,40 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
}
}
btScalar deltaflengthsqr = 0;
{
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
{
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
if (iteration < constraint.m_overrideNumSolverIterations)
if (iteration < constraint.m_overrideNumSolverIterations)
{
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
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 (m_onlyForNoneContact)
{
if (iteration==0)
if (iteration == 0)
{
for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) m_pNC[j] = m_deltafNC[j];
}
else
{
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)
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++) m_pNC[j] = 0;
}
else
{
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
{
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
if (iteration < constraint.m_overrideNumSolverIterations)
if (iteration < constraint.m_overrideNumSolverIterations)
{
btScalar additionaldeltaimpulse = beta * m_pNC[j];
constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
@@ -116,8 +113,8 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
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);
body1.internalApplyImpulse(c.m_contactNormal1 * body1.internalGetInvMass(), c.m_angularComponentA, additionaldeltaimpulse);
body2.internalApplyImpulse(c.m_contactNormal2 * body2.internalGetInvMass(), c.m_angularComponentB, additionaldeltaimpulse);
}
}
}
@@ -125,21 +122,18 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
m_deltafLengthSqrPrev = deltaflengthsqr;
}
{
if (iteration< infoGlobal.m_numIterations)
if (iteration < infoGlobal.m_numIterations)
{
for (int j=0;j<numConstraints;j++)
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);
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);
constraints[j]->solveConstraintObsolete(bodyA, bodyB, infoGlobal.m_timeStep);
}
}
@@ -147,203 +141,206 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
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;
int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1;
for (int c=0;c<numPoolConstraints;c++)
for (int c = 0; c < numPoolConstraints; c++)
{
btScalar totalImpulse =0;
btScalar totalImpulse = 0;
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
btScalar deltaf = resolveSingleConstraintRowLowerLimit(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[c] = deltaf;
deltaflengthsqr += deltaf*deltaf;
deltaflengthsqr += deltaf * deltaf;
totalImpulse = solveManifold.m_appliedImpulse;
}
bool applyFriction = true;
if (applyFriction)
{
{
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier]];
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
if (totalImpulse>btScalar(0))
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[c*multiplier] = deltaf;
deltaflengthsqr += deltaf*deltaf;
} else {
m_deltafCF[c*multiplier] = 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[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]];
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
if (totalImpulse>btScalar(0))
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[c*multiplier+1] = deltaf;
deltaflengthsqr += deltaf*deltaf;
} else {
m_deltafCF[c*multiplier+1] = 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[c * multiplier + 1] = deltaf;
deltaflengthsqr += deltaf * deltaf;
}
else
{
m_deltafCF[c * multiplier + 1] = 0;
}
}
}
}
}
else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
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++)
for (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);
btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
m_deltafC[j] = deltaf;
deltaflengthsqr += deltaf*deltaf;
deltaflengthsqr += deltaf * deltaf;
}
///solve all friction constraints
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
for (j=0;j<numFrictionPoolConstraints;j++)
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))
if (totalImpulse > btScalar(0))
{
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
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);
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
m_deltafCF[j] = deltaf;
deltaflengthsqr += deltaf*deltaf;
} else {
deltaflengthsqr += deltaf * deltaf;
}
else
{
m_deltafCF[j] = 0;
}
}
}
{
{
int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
for (int j=0;j<numRollingFrictionPoolConstraints;j++)
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))
if (totalImpulse > btScalar(0))
{
btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
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);
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
m_deltafCRF[j] = deltaf;
deltaflengthsqr += deltaf*deltaf;
} else {
deltaflengthsqr += deltaf * deltaf;
}
else
{
m_deltafCRF[j] = 0;
}
}
}
}
}
}
if (!m_onlyForNoneContact)
if (!m_onlyForNoneContact)
{
if (iteration==0)
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];
for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = m_deltafCRF[j];
} else
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];
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;
for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = 0;
} else {
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
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;
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) {
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);
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++)
for (int j = 0; j < m_tmpSolverContactConstraintPool.size(); j++)
{
btSolverConstraint& constraint = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
if (iteration< infoGlobal.m_numIterations) {
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);
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++)
for (int j = 0; j < m_tmpSolverContactFrictionConstraintPool.size(); j++)
{
btSolverConstraint& constraint = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
if (iteration< infoGlobal.m_numIterations) {
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);
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_tmpSolverContactRollingFrictionConstraintPool.size();j++)
for (int j = 0; j < m_tmpSolverContactRollingFrictionConstraintPool.size(); j++)
{
btSolverConstraint& constraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
if (iteration< infoGlobal.m_numIterations) {
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);
body1.internalApplyImpulse(c.m_contactNormal1 * body1.internalGetInvMass(), c.m_angularComponentA, additionaldeltaimpulse);
body2.internalApplyImpulse(c.m_contactNormal2 * body2.internalGetInvMass(), c.m_angularComponentB, additionaldeltaimpulse);
}
}
}
@@ -355,7 +352,7 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
return deltaflengthsqr;
}
btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
{
m_pNC.resizeNoInitialize(0);
m_pC.resizeNoInitialize(0);
@@ -369,6 +366,3 @@ btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject
return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
}

35
src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h
View File

@@ -18,33 +18,30 @@ subject to the following restrictions:
#include "btSequentialImpulseConstraintSolver.h"
ATTRIBUTE_ALIGNED16(class) btNNCGConstraintSolver : public btSequentialImpulseConstraintSolver
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
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
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 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);
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) {}
@@ -57,8 +54,4 @@ public:
bool m_onlyForNoneContact;
};
#endif //BT_NNCG_CONSTRAINT_SOLVER_H
#endif //BT_NNCG_CONSTRAINT_SOLVER_H

186
src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
View File

@@ -13,217 +13,193 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btPoint2PointConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include <new>
btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
m_flags(0),
m_useSolveConstraintObsolete(false)
btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& pivotInA, const btVector3& pivotInB)
: btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE, rbA, rbB), m_pivotInA(pivotInA), m_pivotInB(pivotInB), m_flags(0), m_useSolveConstraintObsolete(false)
{
}
btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
m_flags(0),
m_useSolveConstraintObsolete(false)
btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA, const btVector3& pivotInA)
: btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE, rbA), m_pivotInA(pivotInA), m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), m_flags(0), m_useSolveConstraintObsolete(false)
{
}
void btPoint2PointConstraint::buildJacobian()
void btPoint2PointConstraint::buildJacobian()
{
///we need it for both methods
{
m_appliedImpulse = btScalar(0.);
btVector3 normal(0,0,0);
btVector3 normal(0, 0, 0);
for (int i=0;i<3;i++)
for (int i = 0; i < 3; i++)
{
normal[i] = 1;
new (&m_jac[i]) btJacobianEntry(
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
normal,
m_rbA.getInvInertiaDiagLocal(),
m_rbA.getInvMass(),
m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvMass());
normal[i] = 0;
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
m_rbA.getCenterOfMassTransform() * m_pivotInA - m_rbA.getCenterOfMassPosition(),
m_rbB.getCenterOfMassTransform() * m_pivotInB - m_rbB.getCenterOfMassPosition(),
normal,
m_rbA.getInvInertiaDiagLocal(),
m_rbA.getInvMass(),
m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvMass());
normal[i] = 0;
}
}
}
void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
void btPoint2PointConstraint::getInfo1(btConstraintInfo1* info)
{
getInfo1NonVirtual(info);
}
void btPoint2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
void btPoint2PointConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
{
if (m_useSolveConstraintObsolete)
{
info->m_numConstraintRows = 0;
info->nub = 0;
} else
}
else
{
info->m_numConstraintRows = 3;
info->nub = 3;
}
}
void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
void btPoint2PointConstraint::getInfo2(btConstraintInfo2* info)
{
getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
}
void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans)
void btPoint2PointConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans)
{
btAssert(!m_useSolveConstraintObsolete);
//retrieve matrices
//retrieve matrices
// anchor points in global coordinates with respect to body PORs.
// set jacobian
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[info->rowskip+1] = 1;
info->m_J1linearAxis[2*info->rowskip+2] = 1;
btVector3 a1 = body0_trans.getBasis()*getPivotInA();
// set jacobian
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[info->rowskip + 1] = 1;
info->m_J1linearAxis[2 * info->rowskip + 2] = 1;
btVector3 a1 = body0_trans.getBasis() * getPivotInA();
{
btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
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);
a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
}
info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[info->rowskip+1] = -1;
info->m_J2linearAxis[2*info->rowskip+2] = -1;
btVector3 a2 = body1_trans.getBasis()*getPivotInB();
info->m_J2linearAxis[info->rowskip + 1] = -1;
info->m_J2linearAxis[2 * info->rowskip + 2] = -1;
btVector3 a2 = body1_trans.getBasis() * getPivotInB();
{
// btVector3 a2n = -a2;
// 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);
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
// set right hand side
btScalar currERP = (m_flags & BT_P2P_FLAGS_ERP) ? m_erp : info->erp;
btScalar k = info->fps * currERP;
int j;
for (j=0; j<3; j++)
{
info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
}
if(m_flags & BT_P2P_FLAGS_CFM)
btScalar k = info->fps * currERP;
int j;
for (j = 0; j < 3; j++)
{
for (j=0; j<3; j++)
info->m_constraintError[j * info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
}
if (m_flags & BT_P2P_FLAGS_CFM)
{
for (j = 0; j < 3; j++)
{
info->cfm[j*info->rowskip] = m_cfm;
info->cfm[j * info->rowskip] = m_cfm;
}
}
btScalar impulseClamp = m_setting.m_impulseClamp;//
for (j=0; j<3; j++)
{
btScalar impulseClamp = m_setting.m_impulseClamp; //
for (j = 0; j < 3; j++)
{
if (m_setting.m_impulseClamp > 0)
{
info->m_lowerLimit[j*info->rowskip] = -impulseClamp;
info->m_upperLimit[j*info->rowskip] = impulseClamp;
info->m_lowerLimit[j * info->rowskip] = -impulseClamp;
info->m_upperLimit[j * info->rowskip] = impulseClamp;
}
}
info->m_damping = m_setting.m_damping;
}
void btPoint2PointConstraint::updateRHS(btScalar timeStep)
void btPoint2PointConstraint::updateRHS(btScalar timeStep)
{
(void)timeStep;
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///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.
void btPoint2PointConstraint::setParam(int num, btScalar value, int axis)
{
if(axis != -1)
if (axis != -1)
{
btAssertConstrParams(0);
}
else
{
switch(num)
switch (num)
{
case BT_CONSTRAINT_ERP :
case BT_CONSTRAINT_STOP_ERP :
m_erp = value;
case BT_CONSTRAINT_ERP:
case BT_CONSTRAINT_STOP_ERP:
m_erp = value;
m_flags |= BT_P2P_FLAGS_ERP;
break;
case BT_CONSTRAINT_CFM :
case BT_CONSTRAINT_STOP_CFM :
m_cfm = value;
case BT_CONSTRAINT_CFM:
case BT_CONSTRAINT_STOP_CFM:
m_cfm = value;
m_flags |= BT_P2P_FLAGS_CFM;
break;
default:
default:
btAssertConstrParams(0);
}
}
}
///return the local value of parameter
btScalar btPoint2PointConstraint::getParam(int num, int axis) const
btScalar btPoint2PointConstraint::getParam(int num, int axis) const
{
btScalar retVal(SIMD_INFINITY);
if(axis != -1)
if (axis != -1)
{
btAssertConstrParams(0);
}
else
{
switch(num)
switch (num)
{
case BT_CONSTRAINT_ERP :
case BT_CONSTRAINT_STOP_ERP :
case BT_CONSTRAINT_ERP:
case BT_CONSTRAINT_STOP_ERP:
btAssertConstrParams(m_flags & BT_P2P_FLAGS_ERP);
retVal = m_erp;
retVal = m_erp;
break;
case BT_CONSTRAINT_CFM :
case BT_CONSTRAINT_STOP_CFM :
case BT_CONSTRAINT_CFM:
case BT_CONSTRAINT_STOP_CFM:
btAssertConstrParams(m_flags & BT_P2P_FLAGS_CFM);
retVal = m_cfm;
retVal = m_cfm;
break;
default:
default:
btAssertConstrParams(0);
}
}
return retVal;
}

133
src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
View File

@@ -22,26 +22,24 @@ subject to the following restrictions:
class btRigidBody;
#ifdef BT_USE_DOUBLE_PRECISION
#define btPoint2PointConstraintData2 btPoint2PointConstraintDoubleData2
#define btPoint2PointConstraintDataName "btPoint2PointConstraintDoubleData2"
#define btPoint2PointConstraintData2 btPoint2PointConstraintDoubleData2
#define btPoint2PointConstraintDataName "btPoint2PointConstraintDoubleData2"
#else
#define btPoint2PointConstraintData2 btPoint2PointConstraintFloatData
#define btPoint2PointConstraintDataName "btPoint2PointConstraintFloatData"
#endif //BT_USE_DOUBLE_PRECISION
#define btPoint2PointConstraintData2 btPoint2PointConstraintFloatData
#define btPoint2PointConstraintDataName "btPoint2PointConstraintFloatData"
#endif //BT_USE_DOUBLE_PRECISION
struct btConstraintSetting
struct btConstraintSetting
{
btConstraintSetting() :
m_tau(btScalar(0.3)),
m_damping(btScalar(1.)),
m_impulseClamp(btScalar(0.))
btConstraintSetting() : m_tau(btScalar(0.3)),
m_damping(btScalar(1.)),
m_impulseClamp(btScalar(0.))
{
}
btScalar m_tau;
btScalar m_damping;
btScalar m_impulseClamp;
btScalar m_tau;
btScalar m_damping;
btScalar m_impulseClamp;
};
enum btPoint2PointFlags
@@ -51,52 +49,51 @@ enum btPoint2PointFlags
};
/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
ATTRIBUTE_ALIGNED16(class) btPoint2PointConstraint : public btTypedConstraint
ATTRIBUTE_ALIGNED16(class)
btPoint2PointConstraint : public btTypedConstraint
{
#ifdef IN_PARALLELL_SOLVER
public:
#endif
btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
btVector3 m_pivotInA;
btVector3 m_pivotInB;
int m_flags;
btScalar m_erp;
btScalar m_cfm;
public:
btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
btVector3 m_pivotInA;
btVector3 m_pivotInB;
int m_flags;
btScalar m_erp;
btScalar m_cfm;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
///for backwards compatibility during the transition to 'getInfo/getInfo2'
bool m_useSolveConstraintObsolete;
bool m_useSolveConstraintObsolete;
btConstraintSetting m_setting;
btConstraintSetting m_setting;
btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB);
btPoint2PointConstraint(btRigidBody & rbA, btRigidBody & rbB, const btVector3& pivotInA, const btVector3& pivotInB);
btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA);
btPoint2PointConstraint(btRigidBody & rbA, const btVector3& pivotInA);
virtual void buildJacobian();
virtual void buildJacobian();
virtual void getInfo1(btConstraintInfo1 * info);
virtual void getInfo1 (btConstraintInfo1* info);
void getInfo1NonVirtual(btConstraintInfo1 * info);
void getInfo1NonVirtual (btConstraintInfo1* info);
virtual void getInfo2(btConstraintInfo2 * info);
virtual void getInfo2 (btConstraintInfo2* info);
void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& body0_trans, const btTransform& body1_trans);
void getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans);
void updateRHS(btScalar timeStep);
void updateRHS(btScalar timeStep);
void setPivotA(const btVector3& pivotA)
void setPivotA(const btVector3& pivotA)
{
m_pivotInA = pivotA;
}
void setPivotB(const btVector3& pivotB)
void setPivotB(const btVector3& pivotB)
{
m_pivotInB = pivotB;
}
@@ -111,70 +108,66 @@ public:
return m_pivotInB;
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///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 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 btScalar getParam(int num, int axis = -1) const;
virtual int calculateSerializeBufferSize() const;
virtual int getFlags() const
{
return m_flags;
}
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btPoint2PointConstraintFloatData
struct btPoint2PointConstraintFloatData
{
btTypedConstraintData m_typeConstraintData;
btVector3FloatData m_pivotInA;
btVector3FloatData m_pivotInB;
btTypedConstraintData m_typeConstraintData;
btVector3FloatData m_pivotInA;
btVector3FloatData m_pivotInB;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btPoint2PointConstraintDoubleData2
struct btPoint2PointConstraintDoubleData2
{
btTypedConstraintDoubleData m_typeConstraintData;
btVector3DoubleData m_pivotInA;
btVector3DoubleData m_pivotInB;
btTypedConstraintDoubleData m_typeConstraintData;
btVector3DoubleData m_pivotInA;
btVector3DoubleData m_pivotInB;
};
#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
///this structure is not used, except for loading pre-2.82 .bullet files
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btPoint2PointConstraintDoubleData
struct btPoint2PointConstraintDoubleData
{
btTypedConstraintData m_typeConstraintData;
btVector3DoubleData m_pivotInA;
btVector3DoubleData m_pivotInB;
btTypedConstraintData m_typeConstraintData;
btVector3DoubleData m_pivotInA;
btVector3DoubleData m_pivotInB;
};
#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
SIMD_FORCE_INLINE int btPoint2PointConstraint::calculateSerializeBufferSize() const
SIMD_FORCE_INLINE int btPoint2PointConstraint::calculateSerializeBufferSize() const
{
return sizeof(btPoint2PointConstraintData2);
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* btPoint2PointConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* btPoint2PointConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
{
btPoint2PointConstraintData2* p2pData = (btPoint2PointConstraintData2*)dataBuffer;
btTypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer);
btTypedConstraint::serialize(&p2pData->m_typeConstraintData, serializer);
m_pivotInA.serialize(p2pData->m_pivotInA);
m_pivotInB.serialize(p2pData->m_pivotInB);
return btPoint2PointConstraintDataName;
}
#endif //BT_POINT2POINTCONSTRAINT_H
#endif //BT_POINT2POINTCONSTRAINT_H

1783
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View File

File diff suppressed because it is too large Load Diff

183
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
View File

@@ -27,147 +27,142 @@ class btCollisionObject;
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
typedef btScalar(*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&);
typedef btScalar (*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
ATTRIBUTE_ALIGNED16(class)
btSequentialImpulseConstraintSolver : public btConstraintSolver
{
protected:
btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
btConstraintArray m_tmpSolverContactConstraintPool;
btConstraintArray m_tmpSolverNonContactConstraintPool;
btConstraintArray m_tmpSolverContactFrictionConstraintPool;
btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
btConstraintArray m_tmpSolverContactConstraintPool;
btConstraintArray m_tmpSolverNonContactConstraintPool;
btConstraintArray m_tmpSolverContactFrictionConstraintPool;
btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
btAlignedObjectArray<int> m_orderTmpConstraintPool;
btAlignedObjectArray<int> m_orderNonContactConstraintPool;
btAlignedObjectArray<int> m_orderFrictionConstraintPool;
btAlignedObjectArray<int> m_orderTmpConstraintPool;
btAlignedObjectArray<int> m_orderNonContactConstraintPool;
btAlignedObjectArray<int> m_orderFrictionConstraintPool;
btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
int m_maxOverrideNumSolverIterations;
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
// 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;
btSingleConstraintRowSolver m_resolveSplitPenetrationImpulse;
int m_cachedSolverMode; // used to check if SOLVER_SIMD flag has been changed
void setupSolverFunctions( bool useSimd );
btSingleConstraintRowSolver m_resolveSplitPenetrationImpulse;
int m_cachedSolverMode; // used to check if SOLVER_SIMD flag has been changed
void setupSolverFunctions(bool useSimd);
btScalar m_leastSquaresResidual;
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,
const btContactSolverInfo& infoGlobal,
btScalar desiredVelocity=0., btScalar cfmSlip=0.);
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,
const btContactSolverInfo& infoGlobal,
btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
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.);
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, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
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);
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, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
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,
const btContactSolverInfo& infoGlobal, btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2);
void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp,
const btContactSolverInfo& infoGlobal,btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2);
static void applyAnisotropicFriction(btCollisionObject * colObj, btVector3 & frictionDirection, int frictionMode);
static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode);
void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB,
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
void setFrictionConstraintImpulse(btSolverConstraint & solverConstraint, int solverBodyIdA, int solverBodyIdB,
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
unsigned long m_btSeed2;
unsigned long m_btSeed2;
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold);
virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
virtual void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
void convertContact(btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal);
virtual void convertJoints(btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal);
void convertJoint(btSolverConstraint* currentConstraintRow, btTypedConstraint* constraint, const btTypedConstraint::btConstraintInfo1& info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo& infoGlobal);
virtual void convertJoints(btTypedConstraint * *constraints, int numConstraints, const btContactSolverInfo& infoGlobal);
void convertJoint(btSolverConstraint * currentConstraintRow, btTypedConstraint * constraint, const btTypedConstraint::btConstraintInfo1& info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo& infoGlobal);
virtual void convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
virtual void convertBodies(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
btScalar resolveSplitPenetrationSIMD(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
{
return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint );
}
btScalar resolveSplitPenetrationSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint)
{
return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint);
}
btScalar resolveSplitPenetrationImpulseCacheFriendly(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
{
return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint );
}
btScalar resolveSplitPenetrationImpulseCacheFriendly(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint)
{
return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint);
}
//internal method
int getOrInitSolverBody(btCollisionObject& body,btScalar timeStep);
void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep);
int getOrInitSolverBody(btCollisionObject & body, btScalar timeStep);
void initSolverBody(btSolverBody * solverBody, btCollisionObject * collisionObject, btScalar timeStep);
btScalar resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
btScalar resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
btScalar resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
btScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
btScalar resolveSplitPenetrationImpulse(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
{
return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint );
}
btScalar resolveSingleConstraintRowGeneric(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint);
btScalar resolveSingleConstraintRowGenericSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint);
btScalar resolveSingleConstraintRowLowerLimit(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint);
btScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint);
btScalar resolveSplitPenetrationImpulse(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint)
{
return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint);
}
protected:
void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
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);
void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
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);
virtual btScalar solveGroupCacheFriendlyIterations(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);
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btSequentialImpulseConstraintSolver();
virtual ~btSequentialImpulseConstraintSolver();
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
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();
virtual void reset();
unsigned long btRand2();
int btRandInt2 (int n);
int btRandInt2(int n);
void setRandSeed(unsigned long seed)
void setRandSeed(unsigned long seed)
{
m_btSeed2 = seed;
}
unsigned long getRandSeed() const
unsigned long getRandSeed() const
{
return m_btSeed2;
}
virtual btConstraintSolverType getSolverType() const
virtual btConstraintSolverType getSolverType() const
{
return BT_SEQUENTIAL_IMPULSE_SOLVER;
}
btSingleConstraintRowSolver getActiveConstraintRowSolverGeneric()
btSingleConstraintRowSolver getActiveConstraintRowSolverGeneric()
{
return m_resolveSingleConstraintRowGeneric;
}
@@ -175,7 +170,7 @@ public:
{
m_resolveSingleConstraintRowGeneric = rowSolver;
}
btSingleConstraintRowSolver getActiveConstraintRowSolverLowerLimit()
btSingleConstraintRowSolver getActiveConstraintRowSolverLowerLimit()
{
return m_resolveSingleConstraintRowLowerLimit;
}
@@ -185,18 +180,14 @@ public:
}
///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();
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();
btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit();
btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit();
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit();
};
#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H

2419
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp
View File

File diff suppressed because it is too large Load Diff

152
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h
View File

@@ -53,102 +53,98 @@ subject to the following restrictions:
/// because floating point addition is not associative due to rounding errors.
/// The task scheduler can and should ensure that the result of any parallelSum operation is deterministic.
///
ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolverMt : public btSequentialImpulseConstraintSolver
ATTRIBUTE_ALIGNED16(class)
btSequentialImpulseConstraintSolverMt : public btSequentialImpulseConstraintSolver
{
public:
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) BT_OVERRIDE;
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) BT_OVERRIDE;
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) BT_OVERRIDE;
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) BT_OVERRIDE;
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) BT_OVERRIDE;
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) BT_OVERRIDE;
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
// temp struct used to collect info from persistent manifolds into a cache-friendly struct using multiple threads
struct btContactManifoldCachedInfo
{
static const int MAX_NUM_CONTACT_POINTS = 4;
// temp struct used to collect info from persistent manifolds into a cache-friendly struct using multiple threads
struct btContactManifoldCachedInfo
{
static const int MAX_NUM_CONTACT_POINTS = 4;
int numTouchingContacts;
int solverBodyIds[ 2 ];
int contactIndex;
int rollingFrictionIndex;
bool contactHasRollingFriction[ MAX_NUM_CONTACT_POINTS ];
btManifoldPoint* contactPoints[ MAX_NUM_CONTACT_POINTS ];
};
// temp struct used for setting up joint constraints in parallel
struct JointParams
{
int m_solverConstraint;
int m_solverBodyA;
int m_solverBodyB;
};
void internalInitMultipleJoints(btTypedConstraint** constraints, int iBegin, int iEnd);
void internalConvertMultipleJoints( const btAlignedObjectArray<JointParams>& jointParamsArray, btTypedConstraint** constraints, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal );
int numTouchingContacts;
int solverBodyIds[2];
int contactIndex;
int rollingFrictionIndex;
bool contactHasRollingFriction[MAX_NUM_CONTACT_POINTS];
btManifoldPoint* contactPoints[MAX_NUM_CONTACT_POINTS];
};
// temp struct used for setting up joint constraints in parallel
struct JointParams
{
int m_solverConstraint;
int m_solverBodyA;
int m_solverBodyB;
};
void internalInitMultipleJoints(btTypedConstraint * *constraints, int iBegin, int iEnd);
void internalConvertMultipleJoints(const btAlignedObjectArray<JointParams>& jointParamsArray, btTypedConstraint** constraints, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
// parameters to control batching
static bool s_allowNestedParallelForLoops; // whether to allow nested parallel operations
static int s_minimumContactManifoldsForBatching; // don't even try to batch if fewer manifolds than this
static btBatchedConstraints::BatchingMethod s_contactBatchingMethod;
static btBatchedConstraints::BatchingMethod s_jointBatchingMethod;
static int s_minBatchSize; // desired number of constraints per batch
static int s_maxBatchSize;
// parameters to control batching
static bool s_allowNestedParallelForLoops; // whether to allow nested parallel operations
static int s_minimumContactManifoldsForBatching; // don't even try to batch if fewer manifolds than this
static btBatchedConstraints::BatchingMethod s_contactBatchingMethod;
static btBatchedConstraints::BatchingMethod s_jointBatchingMethod;
static int s_minBatchSize; // desired number of constraints per batch
static int s_maxBatchSize;
protected:
static const int CACHE_LINE_SIZE = 64;
static const int CACHE_LINE_SIZE = 64;
btBatchedConstraints m_batchedContactConstraints;
btBatchedConstraints m_batchedJointConstraints;
int m_numFrictionDirections;
bool m_useBatching;
bool m_useObsoleteJointConstraints;
btAlignedObjectArray<btContactManifoldCachedInfo> m_manifoldCachedInfoArray;
btAlignedObjectArray<int> m_rollingFrictionIndexTable; // lookup table mapping contact index to rolling friction index
btSpinMutex m_bodySolverArrayMutex;
char m_antiFalseSharingPadding[CACHE_LINE_SIZE]; // padding to keep mutexes in separate cachelines
btSpinMutex m_kinematicBodyUniqueIdToSolverBodyTableMutex;
btAlignedObjectArray<char> m_scratchMemory;
btBatchedConstraints m_batchedContactConstraints;
btBatchedConstraints m_batchedJointConstraints;
int m_numFrictionDirections;
bool m_useBatching;
bool m_useObsoleteJointConstraints;
btAlignedObjectArray<btContactManifoldCachedInfo> m_manifoldCachedInfoArray;
btAlignedObjectArray<int> m_rollingFrictionIndexTable; // lookup table mapping contact index to rolling friction index
btSpinMutex m_bodySolverArrayMutex;
char m_antiFalseSharingPadding[CACHE_LINE_SIZE]; // padding to keep mutexes in separate cachelines
btSpinMutex m_kinematicBodyUniqueIdToSolverBodyTableMutex;
btAlignedObjectArray<char> m_scratchMemory;
virtual void randomizeConstraintOrdering( int iteration, int numIterations );
virtual btScalar resolveAllJointConstraints( int iteration );
virtual btScalar resolveAllContactConstraints();
virtual btScalar resolveAllContactFrictionConstraints();
virtual btScalar resolveAllContactConstraintsInterleaved();
virtual btScalar resolveAllRollingFrictionConstraints();
virtual void randomizeConstraintOrdering(int iteration, int numIterations);
virtual btScalar resolveAllJointConstraints(int iteration);
virtual btScalar resolveAllContactConstraints();
virtual btScalar resolveAllContactFrictionConstraints();
virtual btScalar resolveAllContactConstraintsInterleaved();
virtual btScalar resolveAllRollingFrictionConstraints();
virtual void setupBatchedContactConstraints();
virtual void setupBatchedJointConstraints();
virtual void convertJoints(btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
virtual void convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
virtual void setupBatchedContactConstraints();
virtual void setupBatchedJointConstraints();
virtual void convertJoints(btTypedConstraint * *constraints, int numConstraints, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
virtual void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
virtual void convertBodies(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
int getOrInitSolverBodyThreadsafe(btCollisionObject& body, btScalar timeStep);
void allocAllContactConstraints(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
void setupAllContactConstraints(const btContactSolverInfo& infoGlobal);
void randomizeBatchedConstraintOrdering( btBatchedConstraints* batchedConstraints );
int getOrInitSolverBodyThreadsafe(btCollisionObject & body, btScalar timeStep);
void allocAllContactConstraints(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
void setupAllContactConstraints(const btContactSolverInfo& infoGlobal);
void randomizeBatchedConstraintOrdering(btBatchedConstraints * batchedConstraints);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btSequentialImpulseConstraintSolverMt();
virtual ~btSequentialImpulseConstraintSolverMt();
btScalar resolveMultipleJointConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd, int iteration );
btScalar resolveMultipleContactConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
btScalar resolveMultipleContactSplitPenetrationImpulseConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
btScalar resolveMultipleContactFrictionConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
btScalar resolveMultipleContactRollingFrictionConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
btScalar resolveMultipleContactConstraintsInterleaved( const btAlignedObjectArray<int>& contactIndices, int batchBegin, int batchEnd );
btScalar resolveMultipleJointConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd, int iteration);
btScalar resolveMultipleContactConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd);
btScalar resolveMultipleContactSplitPenetrationImpulseConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd);
btScalar resolveMultipleContactFrictionConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd);
btScalar resolveMultipleContactRollingFrictionConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd);
btScalar resolveMultipleContactConstraintsInterleaved(const btAlignedObjectArray<int>& contactIndices, int batchBegin, int batchEnd);
void internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
void internalAllocContactConstraints(const btContactManifoldCachedInfo* cachedInfoArray, int numManifolds);
void internalSetupContactConstraints(int iContactConstraint, const btContactSolverInfo& infoGlobal);
void internalConvertBodies(btCollisionObject** bodies, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
void internalWriteBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
void internalWriteBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
void internalWriteBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
void internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo * cachedInfoArray, btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
void internalAllocContactConstraints(const btContactManifoldCachedInfo* cachedInfoArray, int numManifolds);
void internalSetupContactConstraints(int iContactConstraint, const btContactSolverInfo& infoGlobal);
void internalConvertBodies(btCollisionObject * *bodies, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
void internalWriteBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
void internalWriteBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
void internalWriteBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
};
#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H
#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H

640
src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
View File

File diff suppressed because it is too large Load Diff

211
src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
View File

@@ -25,31 +25,26 @@ TODO:
#ifndef BT_SLIDER_CONSTRAINT_H
#define BT_SLIDER_CONSTRAINT_H
#include "LinearMath/btScalar.h"//for BT_USE_DOUBLE_PRECISION
#include "LinearMath/btScalar.h" //for BT_USE_DOUBLE_PRECISION
#ifdef BT_USE_DOUBLE_PRECISION
#define btSliderConstraintData2 btSliderConstraintDoubleData
#define btSliderConstraintDataName "btSliderConstraintDoubleData"
#define btSliderConstraintData2 btSliderConstraintDoubleData
#define btSliderConstraintDataName "btSliderConstraintDoubleData"
#else
#define btSliderConstraintData2 btSliderConstraintData
#define btSliderConstraintDataName "btSliderConstraintData"
#endif //BT_USE_DOUBLE_PRECISION
#define btSliderConstraintData2 btSliderConstraintData
#define btSliderConstraintDataName "btSliderConstraintData"
#endif //BT_USE_DOUBLE_PRECISION
#include "LinearMath/btVector3.h"
#include "btJacobianEntry.h"
#include "btTypedConstraint.h"
class btRigidBody;
#define SLIDER_CONSTRAINT_DEF_SOFTNESS (btScalar(1.0))
#define SLIDER_CONSTRAINT_DEF_DAMPING (btScalar(1.0))
#define SLIDER_CONSTRAINT_DEF_RESTITUTION (btScalar(0.7))
#define SLIDER_CONSTRAINT_DEF_CFM (btScalar(0.f))
#define SLIDER_CONSTRAINT_DEF_SOFTNESS (btScalar(1.0))
#define SLIDER_CONSTRAINT_DEF_DAMPING (btScalar(1.0))
#define SLIDER_CONSTRAINT_DEF_RESTITUTION (btScalar(0.7))
#define SLIDER_CONSTRAINT_DEF_CFM (btScalar(0.f))
enum btSliderFlags
{
@@ -67,15 +62,15 @@ enum btSliderFlags
BT_SLIDER_FLAGS_ERP_LIMANG = (1 << 11)
};
ATTRIBUTE_ALIGNED16(class) btSliderConstraint : public btTypedConstraint
ATTRIBUTE_ALIGNED16(class)
btSliderConstraint : public btTypedConstraint
{
protected:
///for backwards compatibility during the transition to 'getInfo/getInfo2'
bool m_useSolveConstraintObsolete;
bool m_useOffsetForConstraintFrame;
btTransform m_frameInA;
btTransform m_frameInB;
bool m_useSolveConstraintObsolete;
bool m_useOffsetForConstraintFrame;
btTransform m_frameInA;
btTransform m_frameInB;
// use frameA fo define limits, if true
bool m_useLinearReferenceFrameA;
// linear limits
@@ -119,21 +114,21 @@ protected:
btScalar m_restitutionOrthoAng;
btScalar m_dampingOrthoAng;
btScalar m_cfmOrthoAng;
// for interlal use
bool m_solveLinLim;
bool m_solveAngLim;
int m_flags;
btJacobianEntry m_jacLin[3];
btScalar m_jacLinDiagABInv[3];
btJacobianEntry m_jacLin[3];
btScalar m_jacLinDiagABInv[3];
btJacobianEntry m_jacAng[3];
btJacobianEntry m_jacAng[3];
btScalar m_timeStep;
btTransform m_calculatedTransformA;
btTransform m_calculatedTransformB;
btTransform m_calculatedTransformA;
btTransform m_calculatedTransformB;
btVector3 m_sliderAxis;
btVector3 m_realPivotAInW;
@@ -150,57 +145,57 @@ protected:
btScalar m_angDepth;
btScalar m_kAngle;
bool m_poweredLinMotor;
btScalar m_targetLinMotorVelocity;
btScalar m_maxLinMotorForce;
btScalar m_accumulatedLinMotorImpulse;
bool m_poweredAngMotor;
btScalar m_targetAngMotorVelocity;
btScalar m_maxAngMotorForce;
btScalar m_accumulatedAngMotorImpulse;
bool m_poweredLinMotor;
btScalar m_targetLinMotorVelocity;
btScalar m_maxLinMotorForce;
btScalar m_accumulatedLinMotorImpulse;
//------------------------
bool m_poweredAngMotor;
btScalar m_targetAngMotorVelocity;
btScalar m_maxAngMotorForce;
btScalar m_accumulatedAngMotorImpulse;
//------------------------
void initParams();
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
// constructors
btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA);
btSliderConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA);
btSliderConstraint(btRigidBody & rbB, const btTransform& frameInB, bool useLinearReferenceFrameA);
// overrides
virtual void getInfo1 (btConstraintInfo1* info);
virtual void getInfo1(btConstraintInfo1 * info);
void getInfo1NonVirtual(btConstraintInfo1* info);
virtual void getInfo2 (btConstraintInfo2* info);
void getInfo1NonVirtual(btConstraintInfo1 * info);
void getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass);
virtual void getInfo2(btConstraintInfo2 * info);
void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, btScalar rbAinvMass, btScalar rbBinvMass);
// access
const btRigidBody& getRigidBodyA() const { return m_rbA; }
const btRigidBody& getRigidBodyB() const { return m_rbB; }
const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; }
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; }
btScalar getLowerLinLimit() { return m_lowerLinLimit; }
void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; }
btScalar getUpperLinLimit() { return m_upperLinLimit; }
void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; }
btScalar getLowerAngLimit() { return m_lowerAngLimit; }
void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = btNormalizeAngle(lowerLimit); }
btScalar getUpperAngLimit() { return m_upperAngLimit; }
void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = btNormalizeAngle(upperLimit); }
const btRigidBody& getRigidBodyA() const { return m_rbA; }
const btRigidBody& getRigidBodyB() const { return m_rbB; }
const btTransform& getCalculatedTransformA() const { return m_calculatedTransformA; }
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; }
btScalar getLowerLinLimit() { return m_lowerLinLimit; }
void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; }
btScalar getUpperLinLimit() { return m_upperLinLimit; }
void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; }
btScalar getLowerAngLimit() { return m_lowerAngLimit; }
void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = btNormalizeAngle(lowerLimit); }
btScalar getUpperAngLimit() { return m_upperAngLimit; }
void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = btNormalizeAngle(upperLimit); }
bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; }
btScalar getSoftnessDirLin() { return m_softnessDirLin; }
btScalar getRestitutionDirLin() { return m_restitutionDirLin; }
btScalar getDampingDirLin() { return m_dampingDirLin ; }
btScalar getDampingDirLin() { return m_dampingDirLin; }
btScalar getSoftnessDirAng() { return m_softnessDirAng; }
btScalar getRestitutionDirAng() { return m_restitutionDirAng; }
btScalar getDampingDirAng() { return m_dampingDirAng; }
@@ -249,8 +244,6 @@ public:
btScalar getLinearPos() const { return m_linPos; }
btScalar getAngularPos() const { return m_angPos; }
// access for ODE solver
bool getSolveLinLimit() { return m_solveLinLim; }
@@ -258,9 +251,9 @@ public:
bool getSolveAngLimit() { return m_solveAngLim; }
btScalar getAngDepth() { return m_angDepth; }
// shared code used by ODE solver
void calculateTransforms(const btTransform& transA,const btTransform& transB);
void testLinLimits();
void testAngLimits();
void calculateTransforms(const btTransform& transA, const btTransform& transB);
void testLinLimits();
void testAngLimits();
// access for PE Solver
btVector3 getAncorInA();
btVector3 getAncorInB();
@@ -268,84 +261,75 @@ public:
bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
void setFrames(const btTransform& frameA, const btTransform& frameB)
{
m_frameInA=frameA;
m_frameInB=frameB;
calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
void setFrames(const btTransform& frameA, const btTransform& frameB)
{
m_frameInA = frameA;
m_frameInB = frameB;
calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
buildJacobian();
}
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///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 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
{
virtual btScalar getParam(int num, int axis = -1) const;
virtual int getFlags() const
{
return m_flags;
}
virtual int calculateSerializeBufferSize() const;
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btSliderConstraintData
{
btTypedConstraintData m_typeConstraintData;
btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
btTypedConstraintData m_typeConstraintData;
btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
btTransformFloatData m_rbBFrame;
float m_linearUpperLimit;
float m_linearLowerLimit;
float m_angularUpperLimit;
float m_angularLowerLimit;
float m_linearUpperLimit;
float m_linearLowerLimit;
int m_useLinearReferenceFrameA;
float m_angularUpperLimit;
float m_angularLowerLimit;
int m_useLinearReferenceFrameA;
int m_useOffsetForConstraintFrame;
};
struct btSliderConstraintDoubleData
{
btTypedConstraintDoubleData m_typeConstraintData;
btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
btTypedConstraintDoubleData m_typeConstraintData;
btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
btTransformDoubleData m_rbBFrame;
double m_linearUpperLimit;
double m_linearLowerLimit;
double m_angularUpperLimit;
double m_angularLowerLimit;
double m_linearUpperLimit;
double m_linearLowerLimit;
int m_useLinearReferenceFrameA;
double m_angularUpperLimit;
double m_angularLowerLimit;
int m_useLinearReferenceFrameA;
int m_useOffsetForConstraintFrame;
};
SIMD_FORCE_INLINE int btSliderConstraint::calculateSerializeBufferSize() const
SIMD_FORCE_INLINE int btSliderConstraint::calculateSerializeBufferSize() const
{
return sizeof(btSliderConstraintData2);
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* btSliderConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* btSliderConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
{
btSliderConstraintData2* sliderData = (btSliderConstraintData2*) dataBuffer;
btTypedConstraint::serialize(&sliderData->m_typeConstraintData,serializer);
btSliderConstraintData2* sliderData = (btSliderConstraintData2*)dataBuffer;
btTypedConstraint::serialize(&sliderData->m_typeConstraintData, serializer);
m_frameInA.serialize(sliderData->m_rbAFrame);
m_frameInB.serialize(sliderData->m_rbBFrame);
@@ -362,7 +346,4 @@ SIMD_FORCE_INLINE const char* btSliderConstraint::serialize(void* dataBuffer, bt
return btSliderConstraintDataName;
}
#endif //BT_SLIDER_CONSTRAINT_H
#endif //BT_SLIDER_CONSTRAINT_H

164
src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp
View File

@@ -13,43 +13,38 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btSolve2LinearConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btVector3.h"
#include "btJacobianEntry.h"
void btSolve2LinearConstraint::resolveUnilateralPairConstraint(
btRigidBody* body1,
btRigidBody* body2,
btRigidBody* body1,
btRigidBody* body2,
const btMatrix3x3& world2A,
const btMatrix3x3& world2B,
const btVector3& invInertiaADiag,
const btScalar invMassA,
const btVector3& linvelA,const btVector3& angvelA,
const btVector3& rel_posA1,
const btVector3& invInertiaBDiag,
const btScalar invMassB,
const btVector3& linvelB,const btVector3& angvelB,
const btVector3& rel_posA2,
const btMatrix3x3& world2A,
const btMatrix3x3& world2B,
btScalar depthA, const btVector3& normalA,
const btVector3& rel_posB1,const btVector3& rel_posB2,
btScalar depthB, const btVector3& normalB,
btScalar& imp0,btScalar& imp1)
const btVector3& invInertiaADiag,
const btScalar invMassA,
const btVector3& linvelA, const btVector3& angvelA,
const btVector3& rel_posA1,
const btVector3& invInertiaBDiag,
const btScalar invMassB,
const btVector3& linvelB, const btVector3& angvelB,
const btVector3& rel_posA2,
btScalar depthA, const btVector3& normalA,
const btVector3& rel_posB1, const btVector3& rel_posB2,
btScalar depthB, const btVector3& normalB,
btScalar& imp0, btScalar& imp1)
{
(void)linvelA;
(void)linvelB;
(void)angvelB;
(void)angvelA;
imp0 = btScalar(0.);
imp1 = btScalar(0.);
@@ -59,86 +54,76 @@ void btSolve2LinearConstraint::resolveUnilateralPairConstraint(
btAssert(len < SIMD_EPSILON);
//this jacobian entry could be re-used for all iterations
btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
invInertiaBDiag,invMassB);
btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
invInertiaBDiag,invMassB);
btJacobianEntry jacA(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
invInertiaBDiag, invMassB);
btJacobianEntry jacB(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
invInertiaBDiag, invMassB);
//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
//const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1) - body2->getVelocityInLocalPoint(rel_posA1));
const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1) - body2->getVelocityInLocalPoint(rel_posB1));
// btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv
// btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv
btScalar massTerm = btScalar(1.) / (invMassA + invMassB);
// calculate rhs (or error) terms
const btScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping;
const btScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping;
const btScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping;
const btScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping;
// dC/dv * dv = -C
// jacobian * impulse = -error
//
//impulse = jacobianInverse * -error
// inverting 2x2 symmetric system (offdiagonal are equal!)
//
//
btScalar nonDiag = jacA.getNonDiagonal(jacB, invMassA, invMassB);
btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag);
btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * -nonDiag * invDet;
//[a b] [d -c]
//[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
//[jA nD] * [imp0] = [dv0]
//[nD jB] [imp1] [dv1]
}
void btSolve2LinearConstraint::resolveBilateralPairConstraint(
btRigidBody* body1,
btRigidBody* body2,
const btMatrix3x3& world2A,
const btMatrix3x3& world2B,
const btVector3& invInertiaADiag,
const btScalar invMassA,
const btVector3& linvelA,const btVector3& angvelA,
const btVector3& rel_posA1,
const btVector3& invInertiaBDiag,
const btScalar invMassB,
const btVector3& linvelB,const btVector3& angvelB,
const btVector3& rel_posA2,
btRigidBody* body1,
btRigidBody* body2,
const btMatrix3x3& world2A,
const btMatrix3x3& world2B,
btScalar depthA, const btVector3& normalA,
const btVector3& rel_posB1,const btVector3& rel_posB2,
btScalar depthB, const btVector3& normalB,
btScalar& imp0,btScalar& imp1)
const btVector3& invInertiaADiag,
const btScalar invMassA,
const btVector3& linvelA, const btVector3& angvelA,
const btVector3& rel_posA1,
const btVector3& invInertiaBDiag,
const btScalar invMassB,
const btVector3& linvelB, const btVector3& angvelB,
const btVector3& rel_posA2,
btScalar depthA, const btVector3& normalA,
const btVector3& rel_posB1, const btVector3& rel_posB2,
btScalar depthB, const btVector3& normalB,
btScalar& imp0, btScalar& imp1)
{
(void)linvelA;
(void)linvelB;
(void)angvelA;
(void)angvelB;
imp0 = btScalar(0.);
imp1 = btScalar(0.);
@@ -148,42 +133,40 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint(
btAssert(len < SIMD_EPSILON);
//this jacobian entry could be re-used for all iterations
btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
invInertiaBDiag,invMassB);
btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
invInertiaBDiag,invMassB);
btJacobianEntry jacA(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
invInertiaBDiag, invMassB);
btJacobianEntry jacB(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
invInertiaBDiag, invMassB);
//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
//const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1) - body2->getVelocityInLocalPoint(rel_posA1));
const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1) - body2->getVelocityInLocalPoint(rel_posB1));
// calculate rhs (or error) terms
const btScalar dv0 = depthA * m_tau - vel0 * m_damping;
const btScalar dv1 = depthB * m_tau - vel1 * m_damping;
const btScalar dv0 = depthA * m_tau - vel0 * m_damping;
const btScalar dv1 = depthB * m_tau - vel1 * m_damping;
// dC/dv * dv = -C
// jacobian * impulse = -error
//
//impulse = jacobianInverse * -error
// inverting 2x2 symmetric system (offdiagonal are equal!)
//
//
btScalar nonDiag = jacA.getNonDiagonal(jacB, invMassA, invMassB);
btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag);
btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * -nonDiag * invDet;
//[a b] [d -c]
//[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
@@ -191,9 +174,9 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint(
//[jA nD] * [imp0] = [dv0]
//[nD jB] [imp1] [dv1]
if ( imp0 > btScalar(0.0))
if (imp0 > btScalar(0.0))
{
if ( imp1 > btScalar(0.0) )
if (imp1 > btScalar(0.0))
{
//both positive
}
@@ -203,9 +186,10 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint(
// now imp0>0 imp1<0
imp0 = dv0 / jacA.getDiagonal();
if ( imp0 > btScalar(0.0) )
if (imp0 > btScalar(0.0))
{
} else
}
else
{
imp0 = btScalar(0.);
}
@@ -216,24 +200,25 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint(
imp0 = btScalar(0.);
imp1 = dv1 / jacB.getDiagonal();
if ( imp1 <= btScalar(0.0) )
if (imp1 <= btScalar(0.0))
{
imp1 = btScalar(0.);
// now imp0>0 imp1<0
imp0 = dv0 / jacA.getDiagonal();
if ( imp0 > btScalar(0.0) )
if (imp0 > btScalar(0.0))
{
} else
}
else
{
imp0 = btScalar(0.);
}
} else
}
else
{
}
}
}
/*
void btSolve2LinearConstraint::resolveAngularConstraint( const btMatrix3x3& invInertiaAWS,
const btScalar invMassA,
@@ -252,4 +237,3 @@ void btSolve2LinearConstraint::resolveAngularConstraint( const btMatrix3x3& invI
}
*/

80
src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h
View File

@@ -19,20 +19,16 @@ subject to the following restrictions:
#include "LinearMath/btMatrix3x3.h"
#include "LinearMath/btVector3.h"
class btRigidBody;
/// constraint class used for lateral tyre friction.
class btSolve2LinearConstraint
class btSolve2LinearConstraint
{
btScalar m_tau;
btScalar m_damping;
btScalar m_tau;
btScalar m_damping;
public:
btSolve2LinearConstraint(btScalar tau,btScalar damping)
btSolve2LinearConstraint(btScalar tau, btScalar damping)
{
m_tau = tau;
m_damping = damping;
@@ -40,52 +36,51 @@ public:
//
// solve unilateral constraint (equality, direct method)
//
void resolveUnilateralPairConstraint(
btRigidBody* body0,
void resolveUnilateralPairConstraint(
btRigidBody* body0,
btRigidBody* body1,
const btMatrix3x3& world2A,
const btMatrix3x3& world2B,
const btVector3& invInertiaADiag,
const btScalar invMassA,
const btVector3& linvelA,const btVector3& angvelA,
const btVector3& rel_posA1,
const btVector3& invInertiaBDiag,
const btScalar invMassB,
const btVector3& linvelB,const btVector3& angvelB,
const btVector3& rel_posA2,
const btMatrix3x3& world2B,
btScalar depthA, const btVector3& normalA,
const btVector3& rel_posB1,const btVector3& rel_posB2,
btScalar depthB, const btVector3& normalB,
btScalar& imp0,btScalar& imp1);
const btVector3& invInertiaADiag,
const btScalar invMassA,
const btVector3& linvelA, const btVector3& angvelA,
const btVector3& rel_posA1,
const btVector3& invInertiaBDiag,
const btScalar invMassB,
const btVector3& linvelB, const btVector3& angvelB,
const btVector3& rel_posA2,
btScalar depthA, const btVector3& normalA,
const btVector3& rel_posB1, const btVector3& rel_posB2,
btScalar depthB, const btVector3& normalB,
btScalar& imp0, btScalar& imp1);
//
// solving 2x2 lcp problem (inequality, direct solution )
//
void resolveBilateralPairConstraint(
btRigidBody* body0,
btRigidBody* body1,
btRigidBody* body0,
btRigidBody* body1,
const btMatrix3x3& world2A,
const btMatrix3x3& world2B,
const btVector3& invInertiaADiag,
const btScalar invMassA,
const btVector3& linvelA,const btVector3& angvelA,
const btVector3& rel_posA1,
const btVector3& invInertiaBDiag,
const btScalar invMassB,
const btVector3& linvelB,const btVector3& angvelB,
const btVector3& rel_posA2,
const btMatrix3x3& world2B,
btScalar depthA, const btVector3& normalA,
const btVector3& rel_posB1,const btVector3& rel_posB2,
btScalar depthB, const btVector3& normalB,
btScalar& imp0,btScalar& imp1);
const btVector3& invInertiaADiag,
const btScalar invMassA,
const btVector3& linvelA, const btVector3& angvelA,
const btVector3& rel_posA1,
const btVector3& invInertiaBDiag,
const btScalar invMassB,
const btVector3& linvelB, const btVector3& angvelB,
const btVector3& rel_posA2,
/*
btScalar depthA, const btVector3& normalA,
const btVector3& rel_posB1, const btVector3& rel_posB2,
btScalar depthB, const btVector3& normalB,
btScalar& imp0, btScalar& imp1);
/*
void resolveAngularConstraint( const btMatrix3x3& invInertiaAWS,
const btScalar invMassA,
const btVector3& linvelA,const btVector3& angvelA,
@@ -101,7 +96,6 @@ public:
btScalar& imp0,btScalar& imp1);
*/
};
#endif //BT_SOLVE_2LINEAR_CONSTRAINT_H
#endif //BT_SOLVE_2LINEAR_CONSTRAINT_H

209
src/BulletDynamics/ConstraintSolver/btSolverBody.h
View File

@@ -16,7 +16,7 @@ subject to the following restrictions:
#ifndef BT_SOLVER_BODY_H
#define BT_SOLVER_BODY_H
class btRigidBody;
class btRigidBody;
#include "LinearMath/btVector3.h"
#include "LinearMath/btMatrix3x3.h"
@@ -26,103 +26,99 @@ class btRigidBody;
///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
#ifdef BT_USE_SSE
#define USE_SIMD 1
#endif //
#endif //
#ifdef USE_SIMD
struct btSimdScalar
struct btSimdScalar
{
SIMD_FORCE_INLINE btSimdScalar()
{
}
SIMD_FORCE_INLINE btSimdScalar(float fl)
:m_vec128 (_mm_set1_ps(fl))
SIMD_FORCE_INLINE btSimdScalar()
{
}
SIMD_FORCE_INLINE btSimdScalar(__m128 v128)
:m_vec128(v128)
SIMD_FORCE_INLINE btSimdScalar(float fl)
: m_vec128(_mm_set1_ps(fl))
{
}
union
SIMD_FORCE_INLINE btSimdScalar(__m128 v128)
: m_vec128(v128)
{
__m128 m_vec128;
float m_floats[4];
int m_ints[4];
btScalar m_unusedPadding;
}
union {
__m128 m_vec128;
float m_floats[4];
int m_ints[4];
btScalar m_unusedPadding;
};
SIMD_FORCE_INLINE __m128 get128()
SIMD_FORCE_INLINE __m128 get128()
{
return m_vec128;
}
SIMD_FORCE_INLINE const __m128 get128() const
SIMD_FORCE_INLINE const __m128 get128() const
{
return m_vec128;
}
SIMD_FORCE_INLINE void set128(__m128 v128)
SIMD_FORCE_INLINE void set128(__m128 v128)
{
m_vec128 = v128;
}
SIMD_FORCE_INLINE operator __m128()
{
return m_vec128;
SIMD_FORCE_INLINE operator __m128()
{
return m_vec128;
}
SIMD_FORCE_INLINE operator const __m128() const
{
return m_vec128;
}
SIMD_FORCE_INLINE operator float() const
{
return m_floats[0];
SIMD_FORCE_INLINE operator const __m128() const
{
return m_vec128;
}
SIMD_FORCE_INLINE operator float() const
{
return m_floats[0];
}
};
///@brief Return the elementwise product of two btSimdScalar
SIMD_FORCE_INLINE btSimdScalar
operator*(const btSimdScalar& v1, const btSimdScalar& v2)
SIMD_FORCE_INLINE btSimdScalar
operator*(const btSimdScalar& v1, const btSimdScalar& v2)
{
return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
return btSimdScalar(_mm_mul_ps(v1.get128(), v2.get128()));
}
///@brief Return the elementwise product of two btSimdScalar
SIMD_FORCE_INLINE btSimdScalar
operator+(const btSimdScalar& v1, const btSimdScalar& v2)
SIMD_FORCE_INLINE btSimdScalar
operator+(const btSimdScalar& v1, const btSimdScalar& v2)
{
return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
return btSimdScalar(_mm_add_ps(v1.get128(), v2.get128()));
}
#else
#define btSimdScalar btScalar
#endif
///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
ATTRIBUTE_ALIGNED16 (struct) btSolverBody
ATTRIBUTE_ALIGNED16(struct)
btSolverBody
{
BT_DECLARE_ALIGNED_ALLOCATOR();
btTransform m_worldTransform;
btVector3 m_deltaLinearVelocity;
btVector3 m_deltaAngularVelocity;
btVector3 m_angularFactor;
btVector3 m_linearFactor;
btVector3 m_invMass;
btVector3 m_pushVelocity;
btVector3 m_turnVelocity;
btVector3 m_linearVelocity;
btVector3 m_angularVelocity;
btVector3 m_externalForceImpulse;
btVector3 m_externalTorqueImpulse;
btTransform m_worldTransform;
btVector3 m_deltaLinearVelocity;
btVector3 m_deltaAngularVelocity;
btVector3 m_angularFactor;
btVector3 m_linearFactor;
btVector3 m_invMass;
btVector3 m_pushVelocity;
btVector3 m_turnVelocity;
btVector3 m_linearVelocity;
btVector3 m_angularVelocity;
btVector3 m_externalForceImpulse;
btVector3 m_externalTorqueImpulse;
btRigidBody* m_originalBody;
void setWorldTransform(const btTransform& worldTransform)
btRigidBody* m_originalBody;
void setWorldTransform(const btTransform& worldTransform)
{
m_worldTransform = worldTransform;
}
@@ -131,56 +127,50 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody
{
return m_worldTransform;
}
SIMD_FORCE_INLINE void getVelocityInLocalPointNoDelta(const btVector3& rel_pos, btVector3& velocity ) const
SIMD_FORCE_INLINE void getVelocityInLocalPointNoDelta(const btVector3& rel_pos, btVector3& velocity) const
{
if (m_originalBody)
velocity = m_linearVelocity + m_externalForceImpulse + (m_angularVelocity+m_externalTorqueImpulse).cross(rel_pos);
velocity = m_linearVelocity + m_externalForceImpulse + (m_angularVelocity + m_externalTorqueImpulse).cross(rel_pos);
else
velocity.setValue(0,0,0);
velocity.setValue(0, 0, 0);
}
SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity) const
{
if (m_originalBody)
velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos);
else
velocity.setValue(0,0,0);
velocity.setValue(0, 0, 0);
}
SIMD_FORCE_INLINE void getAngularVelocity(btVector3& angVel) const
SIMD_FORCE_INLINE void getAngularVelocity(btVector3 & angVel) const
{
if (m_originalBody)
angVel =m_angularVelocity+m_deltaAngularVelocity;
angVel = m_angularVelocity + m_deltaAngularVelocity;
else
angVel.setValue(0,0,0);
angVel.setValue(0, 0, 0);
}
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent, const btScalar impulseMagnitude)
{
if (m_originalBody)
{
m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor;
m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
}
}
SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent, btScalar impulseMagnitude)
{
if (m_originalBody)
{
m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor;
m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
m_pushVelocity += linearComponent * impulseMagnitude * m_linearFactor;
m_turnVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
}
}
const btVector3& getDeltaLinearVelocity() const
{
return m_deltaLinearVelocity;
@@ -191,20 +181,19 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody
return m_deltaAngularVelocity;
}
const btVector3& getPushVelocity() const
const btVector3& getPushVelocity() const
{
return m_pushVelocity;
}
const btVector3& getTurnVelocity() const
const btVector3& getTurnVelocity() const
{
return m_turnVelocity;
}
////////////////////////////////////////////////
///some internal methods, don't use them
btVector3& internalGetDeltaLinearVelocity()
{
return m_deltaLinearVelocity;
@@ -229,7 +218,7 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody
{
m_invMass = invMass;
}
btVector3& internalGetPushVelocity()
{
return m_pushVelocity;
@@ -240,67 +229,57 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody
return m_turnVelocity;
}
SIMD_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
SIMD_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity) const
{
velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos);
}
SIMD_FORCE_INLINE void internalGetAngularVelocity(btVector3& angVel) const
SIMD_FORCE_INLINE void internalGetAngularVelocity(btVector3 & angVel) const
{
angVel = m_angularVelocity+m_deltaAngularVelocity;
angVel = m_angularVelocity + m_deltaAngularVelocity;
}
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent, const btScalar impulseMagnitude)
{
if (m_originalBody)
{
m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
}
}
void writebackVelocity()
{
if (m_originalBody)
{
m_linearVelocity +=m_deltaLinearVelocity;
m_angularVelocity += m_deltaAngularVelocity;
//m_originalBody->setCompanionId(-1);
m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor;
m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
}
}
void writebackVelocityAndTransform(btScalar timeStep, btScalar splitImpulseTurnErp)
void writebackVelocity()
{
(void) timeStep;
if (m_originalBody)
{
m_linearVelocity += m_deltaLinearVelocity;
m_angularVelocity += m_deltaAngularVelocity;
//m_originalBody->setCompanionId(-1);
}
}
void writebackVelocityAndTransform(btScalar timeStep, btScalar splitImpulseTurnErp)
{
(void)timeStep;
if (m_originalBody)
{
m_linearVelocity += m_deltaLinearVelocity;
m_angularVelocity += m_deltaAngularVelocity;
//correct the position/orientation based on push/turn recovery
btTransform newTransform;
if (m_pushVelocity[0]!=0.f || m_pushVelocity[1]!=0 || m_pushVelocity[2]!=0 || m_turnVelocity[0]!=0.f || m_turnVelocity[1]!=0 || m_turnVelocity[2]!=0)
if (m_pushVelocity[0] != 0.f || m_pushVelocity[1] != 0 || m_pushVelocity[2] != 0 || m_turnVelocity[0] != 0.f || m_turnVelocity[1] != 0 || m_turnVelocity[2] != 0)
{
// btQuaternion orn = m_worldTransform.getRotation();
btTransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity*splitImpulseTurnErp,timeStep,newTransform);
// btQuaternion orn = m_worldTransform.getRotation();
btTransformUtil::integrateTransform(m_worldTransform, m_pushVelocity, m_turnVelocity * splitImpulseTurnErp, timeStep, newTransform);
m_worldTransform = newTransform;
}
//m_worldTransform.setRotation(orn);
//m_originalBody->setCompanionId(-1);
}
}
};
#endif //BT_SOLVER_BODY_H
#endif //BT_SOLVER_BODY_H

64
src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
View File

@@ -16,7 +16,7 @@ subject to the following restrictions:
#ifndef BT_SOLVER_CONSTRAINT_H
#define BT_SOLVER_CONSTRAINT_H
class btRigidBody;
class btRigidBody;
#include "LinearMath/btVector3.h"
#include "LinearMath/btMatrix3x3.h"
#include "btJacobianEntry.h"
@@ -25,56 +25,50 @@ class btRigidBody;
//#define NO_FRICTION_TANGENTIALS 1
#include "btSolverBody.h"
///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
ATTRIBUTE_ALIGNED16(struct)
btSolverConstraint
{
BT_DECLARE_ALIGNED_ALLOCATOR();
btVector3 m_relpos1CrossNormal;
btVector3 m_contactNormal1;
btVector3 m_relpos1CrossNormal;
btVector3 m_contactNormal1;
btVector3 m_relpos2CrossNormal;
btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always
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;
btVector3 m_angularComponentA;
btVector3 m_angularComponentB;
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_numRowsForNonContactConstraint;
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_numRowsForNonContactConstraint;
};
int m_overrideNumSolverIterations;
int m_frictionIndex;
int m_overrideNumSolverIterations;
int m_frictionIndex;
int m_solverBodyIdA;
int m_solverBodyIdB;
enum btSolverConstraintType
enum btSolverConstraintType
{
BT_SOLVER_CONTACT_1D = 0,
BT_SOLVER_FRICTION_1D
};
};
typedef btAlignedObjectArray<btSolverConstraint> btConstraintArray;
#endif //BT_SOLVER_CONSTRAINT_H
typedef btAlignedObjectArray<btSolverConstraint> btConstraintArray;
#endif //BT_SOLVER_CONSTRAINT_H

102
src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
View File

@@ -13,69 +13,63 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btTypedConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btSerializer.h"
#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.05f)
btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
:btTypedObject(type),
m_userConstraintType(-1),
m_userConstraintPtr((void*)-1),
m_breakingImpulseThreshold(SIMD_INFINITY),
m_isEnabled(true),
m_needsFeedback(false),
m_overrideNumSolverIterations(-1),
m_rbA(rbA),
m_rbB(getFixedBody()),
m_appliedImpulse(btScalar(0.)),
m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
m_jointFeedback(0)
: btTypedObject(type),
m_userConstraintType(-1),
m_userConstraintPtr((void*)-1),
m_breakingImpulseThreshold(SIMD_INFINITY),
m_isEnabled(true),
m_needsFeedback(false),
m_overrideNumSolverIterations(-1),
m_rbA(rbA),
m_rbB(getFixedBody()),
m_appliedImpulse(btScalar(0.)),
m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
m_jointFeedback(0)
{
}
btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB)
:btTypedObject(type),
m_userConstraintType(-1),
m_userConstraintPtr((void*)-1),
m_breakingImpulseThreshold(SIMD_INFINITY),
m_isEnabled(true),
m_needsFeedback(false),
m_overrideNumSolverIterations(-1),
m_rbA(rbA),
m_rbB(rbB),
m_appliedImpulse(btScalar(0.)),
m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
m_jointFeedback(0)
btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA, btRigidBody& rbB)
: btTypedObject(type),
m_userConstraintType(-1),
m_userConstraintPtr((void*)-1),
m_breakingImpulseThreshold(SIMD_INFINITY),
m_isEnabled(true),
m_needsFeedback(false),
m_overrideNumSolverIterations(-1),
m_rbA(rbA),
m_rbB(rbB),
m_appliedImpulse(btScalar(0.)),
m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
m_jointFeedback(0)
{
}
btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
{
if(lowLim > uppLim)
if (lowLim > uppLim)
{
return btScalar(1.0f);
}
else if(lowLim == uppLim)
else if (lowLim == uppLim)
{
return btScalar(0.0f);
}
btScalar lim_fact = btScalar(1.0f);
btScalar delta_max = vel / timeFact;
if(delta_max < btScalar(0.0f))
if (delta_max < btScalar(0.0f))
{
if((pos >= lowLim) && (pos < (lowLim - delta_max)))
if ((pos >= lowLim) && (pos < (lowLim - delta_max)))
{
lim_fact = (lowLim - pos) / delta_max;
}
else if(pos < lowLim)
else if (pos < lowLim)
{
lim_fact = btScalar(0.0f);
}
@@ -84,13 +78,13 @@ btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScal
lim_fact = btScalar(1.0f);
}
}
else if(delta_max > btScalar(0.0f))
else if (delta_max > btScalar(0.0f))
{
if((pos <= uppLim) && (pos > (uppLim - delta_max)))
if ((pos <= uppLim) && (pos > (uppLim - delta_max)))
{
lim_fact = (uppLim - pos) / delta_max;
}
else if(pos > uppLim)
else if (pos > uppLim)
{
lim_fact = btScalar(0.0f);
}
@@ -101,19 +95,19 @@ btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScal
}
else
{
lim_fact = btScalar(0.0f);
lim_fact = btScalar(0.0f);
}
return lim_fact;
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
{
btTypedConstraintData2* tcd = (btTypedConstraintData2*) dataBuffer;
btTypedConstraintData2* tcd = (btTypedConstraintData2*)dataBuffer;
tcd->m_rbA = (btRigidBodyData*)serializer->getUniquePointer(&m_rbA);
tcd->m_rbB = (btRigidBodyData*)serializer->getUniquePointer(&m_rbB);
char* name = (char*) serializer->findNameForPointer(this);
char* name = (char*)serializer->findNameForPointer(this);
tcd->m_name = (char*)serializer->getUniquePointer(name);
if (tcd->m_name)
{
@@ -124,10 +118,10 @@ const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali
tcd->m_needsFeedback = m_needsFeedback;
tcd->m_overrideNumSolverIterations = m_overrideNumSolverIterations;
tcd->m_breakingImpulseThreshold = m_breakingImpulseThreshold;
tcd->m_isEnabled = m_isEnabled? 1: 0;
tcd->m_userConstraintId =m_userConstraintId;
tcd->m_userConstraintType =m_userConstraintType;
tcd->m_isEnabled = m_isEnabled ? 1 : 0;
tcd->m_userConstraintId = m_userConstraintId;
tcd->m_userConstraintType = m_userConstraintType;
tcd->m_appliedImpulse = m_appliedImpulse;
tcd->m_dbgDrawSize = m_dbgDrawSize;
@@ -135,10 +129,10 @@ const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali
tcd->m_disableCollisionsBetweenLinkedBodies = false;
int i;
for (i=0;i<m_rbA.getNumConstraintRefs();i++)
for (i = 0; i < m_rbA.getNumConstraintRefs(); i++)
if (m_rbA.getConstraintRef(i) == this)
tcd->m_disableCollisionsBetweenLinkedBodies = true;
for (i=0;i<m_rbB.getNumConstraintRefs();i++)
for (i = 0; i < m_rbB.getNumConstraintRefs(); i++)
if (m_rbB.getConstraintRef(i) == this)
tcd->m_disableCollisionsBetweenLinkedBodies = true;
@@ -147,17 +141,16 @@ const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali
btRigidBody& btTypedConstraint::getFixedBody()
{
static btRigidBody s_fixed(0, 0,0);
s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
static btRigidBody s_fixed(0, 0, 0);
s_fixed.setMassProps(btScalar(0.), btVector3(btScalar(0.), btScalar(0.), btScalar(0.)));
return s_fixed;
}
void btAngularLimit::set(btScalar low, btScalar high, btScalar _softness, btScalar _biasFactor, btScalar _relaxationFactor)
{
m_halfRange = (high - low) / 2.0f;
m_center = btNormalizeAngle(low + m_halfRange);
m_softness = _softness;
m_softness = _softness;
m_biasFactor = _biasFactor;
m_relaxationFactor = _relaxationFactor;
}
@@ -174,7 +167,7 @@ void btAngularLimit::test(const btScalar angle)
if (deviation < -m_halfRange)
{
m_solveLimit = true;
m_correction = - (deviation + m_halfRange);
m_correction = -(deviation + m_halfRange);
m_sign = +1.0f;
}
else if (deviation > m_halfRange)
@@ -186,7 +179,6 @@ void btAngularLimit::test(const btScalar angle)
}
}
btScalar btAngularLimit::getError() const
{
return m_correction * m_sign;

227
src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
View File

@@ -16,26 +16,24 @@ subject to the following restrictions:
#ifndef BT_TYPED_CONSTRAINT_H
#define BT_TYPED_CONSTRAINT_H
#include "LinearMath/btScalar.h"
#include "btSolverConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#ifdef BT_USE_DOUBLE_PRECISION
#define btTypedConstraintData2 btTypedConstraintDoubleData
#define btTypedConstraintDataName "btTypedConstraintDoubleData"
#define btTypedConstraintData2 btTypedConstraintDoubleData
#define btTypedConstraintDataName "btTypedConstraintDoubleData"
#else
#define btTypedConstraintData2 btTypedConstraintFloatData
#define btTypedConstraintDataName "btTypedConstraintFloatData"
#endif //BT_USE_DOUBLE_PRECISION
#define btTypedConstraintData2 btTypedConstraintFloatData
#define btTypedConstraintDataName "btTypedConstraintFloatData"
#endif //BT_USE_DOUBLE_PRECISION
class btSerializer;
//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
enum btTypedConstraintType
{
POINT2POINT_CONSTRAINT_TYPE=3,
POINT2POINT_CONSTRAINT_TYPE = 3,
HINGE_CONSTRAINT_TYPE,
CONETWIST_CONSTRAINT_TYPE,
D6_CONSTRAINT_TYPE,
@@ -48,91 +46,88 @@ enum btTypedConstraintType
MAX_CONSTRAINT_TYPE
};
enum btConstraintParams
{
BT_CONSTRAINT_ERP=1,
BT_CONSTRAINT_ERP = 1,
BT_CONSTRAINT_STOP_ERP,
BT_CONSTRAINT_CFM,
BT_CONSTRAINT_STOP_CFM
};
#if 1
#define btAssertConstrParams(_par) btAssert(_par)
#define btAssertConstrParams(_par) btAssert(_par)
#else
#define btAssertConstrParams(_par)
#define btAssertConstrParams(_par)
#endif
ATTRIBUTE_ALIGNED16(struct) btJointFeedback
ATTRIBUTE_ALIGNED16(struct)
btJointFeedback
{
BT_DECLARE_ALIGNED_ALLOCATOR();
btVector3 m_appliedForceBodyA;
btVector3 m_appliedTorqueBodyA;
btVector3 m_appliedForceBodyB;
btVector3 m_appliedTorqueBodyB;
btVector3 m_appliedForceBodyA;
btVector3 m_appliedTorqueBodyA;
btVector3 m_appliedForceBodyB;
btVector3 m_appliedTorqueBodyB;
};
///TypedConstraint is the baseclass for Bullet constraints and vehicles
ATTRIBUTE_ALIGNED16(class) btTypedConstraint : public btTypedObject
ATTRIBUTE_ALIGNED16(class)
btTypedConstraint : public btTypedObject
{
int m_userConstraintType;
int m_userConstraintType;
union
{
int m_userConstraintId;
union {
int m_userConstraintId;
void* m_userConstraintPtr;
};
btScalar m_breakingImpulseThreshold;
bool m_isEnabled;
bool m_needsFeedback;
int m_overrideNumSolverIterations;
btScalar m_breakingImpulseThreshold;
bool m_isEnabled;
bool m_needsFeedback;
int m_overrideNumSolverIterations;
btTypedConstraint& operator=(btTypedConstraint& other)
btTypedConstraint& operator=(btTypedConstraint& other)
{
btAssert(0);
(void) other;
(void)other;
return *this;
}
protected:
btRigidBody& m_rbA;
btRigidBody& m_rbB;
btScalar m_appliedImpulse;
btScalar m_dbgDrawSize;
btJointFeedback* m_jointFeedback;
btRigidBody& m_rbA;
btRigidBody& m_rbB;
btScalar m_appliedImpulse;
btScalar m_dbgDrawSize;
btJointFeedback* m_jointFeedback;
///internal method used by the constraint solver, don't use them directly
btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
virtual ~btTypedConstraint() {};
btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
virtual ~btTypedConstraint(){};
btTypedConstraint(btTypedConstraintType type, btRigidBody & rbA);
btTypedConstraint(btTypedConstraintType type, btRigidBody & rbA, btRigidBody & rbB);
struct btConstraintInfo1 {
int m_numConstraintRows,nub;
struct btConstraintInfo1
{
int m_numConstraintRows, nub;
};
static btRigidBody& getFixedBody();
struct btConstraintInfo2 {
struct btConstraintInfo2
{
// integrator parameters: frames per second (1/stepsize), default error
// reduction parameter (0..1).
btScalar fps,erp;
btScalar fps, erp;
// for the first and second body, pointers to two (linear and angular)
// n*3 jacobian sub matrices, stored by rows. these matrices will have
// been initialized to 0 on entry. if the second body is zero then the
// J2xx pointers may be 0.
btScalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis;
btScalar *m_J1linearAxis, *m_J1angularAxis, *m_J2linearAxis, *m_J2angularAxis;
// elements to jump from one row to the next in J's
int rowskip;
@@ -140,19 +135,19 @@ public:
// right hand sides of the equation J*v = c + cfm * lambda. cfm is the
// "constraint force mixing" vector. c is set to zero on entry, cfm is
// set to a constant value (typically very small or zero) value on entry.
btScalar *m_constraintError,*cfm;
btScalar *m_constraintError, *cfm;
// lo and hi limits for variables (set to -/+ infinity on entry).
btScalar *m_lowerLimit,*m_upperLimit;
btScalar *m_lowerLimit, *m_upperLimit;
// number of solver iterations
int m_numIterations;
//damping of the velocity
btScalar m_damping;
btScalar m_damping;
};
int getOverrideNumSolverIterations() const
int getOverrideNumSolverIterations() const
{
return m_overrideNumSolverIterations;
}
@@ -165,60 +160,57 @@ public:
}
///internal method used by the constraint solver, don't use them directly
virtual void buildJacobian() {};
virtual void buildJacobian(){};
///internal method used by the constraint solver, don't use them directly
virtual void setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep)
virtual void setupSolverConstraint(btConstraintArray & ca, int solverBodyA, int solverBodyB, btScalar timeStep)
{
(void)ca;
(void)solverBodyA;
(void)solverBodyB;
(void)timeStep;
(void)ca;
(void)solverBodyA;
(void)solverBodyB;
(void)timeStep;
}
///internal method used by the constraint solver, don't use them directly
virtual void getInfo1 (btConstraintInfo1* info)=0;
///internal method used by the constraint solver, don't use them directly
virtual void getInfo2 (btConstraintInfo2* info)=0;
virtual void getInfo1(btConstraintInfo1 * info) = 0;
///internal method used by the constraint solver, don't use them directly
void internalSetAppliedImpulse(btScalar appliedImpulse)
virtual void getInfo2(btConstraintInfo2 * info) = 0;
///internal method used by the constraint solver, don't use them directly
void internalSetAppliedImpulse(btScalar appliedImpulse)
{
m_appliedImpulse = appliedImpulse;
}
///internal method used by the constraint solver, don't use them directly
btScalar internalGetAppliedImpulse()
btScalar internalGetAppliedImpulse()
{
return m_appliedImpulse;
}
btScalar getBreakingImpulseThreshold() const
btScalar getBreakingImpulseThreshold() const
{
return m_breakingImpulseThreshold;
return m_breakingImpulseThreshold;
}
void setBreakingImpulseThreshold(btScalar threshold)
void setBreakingImpulseThreshold(btScalar threshold)
{
m_breakingImpulseThreshold = threshold;
}
bool isEnabled() const
bool isEnabled() const
{
return m_isEnabled;
}
void setEnabled(bool enabled)
void setEnabled(bool enabled)
{
m_isEnabled=enabled;
m_isEnabled = enabled;
}
///internal method used by the constraint solver, don't use them directly
virtual void solveConstraintObsolete(btSolverBody& /*bodyA*/,btSolverBody& /*bodyB*/,btScalar /*timeStep*/) {};
virtual void solveConstraintObsolete(btSolverBody& /*bodyA*/, btSolverBody& /*bodyB*/, btScalar /*timeStep*/){};
const btRigidBody& getRigidBodyA() const
{
return m_rbA;
@@ -228,7 +220,7 @@ public:
return m_rbB;
}
btRigidBody& getRigidBodyA()
btRigidBody& getRigidBodyA()
{
return m_rbA;
}
@@ -239,15 +231,15 @@ public:
int getUserConstraintType() const
{
return m_userConstraintType ;
return m_userConstraintType;
}
void setUserConstraintType(int userConstraintType)
void setUserConstraintType(int userConstraintType)
{
m_userConstraintType = userConstraintType;
};
void setUserConstraintId(int uid)
void setUserConstraintId(int uid)
{
m_userConstraintId = uid;
}
@@ -257,17 +249,17 @@ public:
return m_userConstraintId;
}
void setUserConstraintPtr(void* ptr)
void setUserConstraintPtr(void* ptr)
{
m_userConstraintPtr = ptr;
}
void* getUserConstraintPtr()
void* getUserConstraintPtr()
{
return m_userConstraintPtr;
}
void setJointFeedback(btJointFeedback* jointFeedback)
void setJointFeedback(btJointFeedback * jointFeedback)
{
m_jointFeedback = jointFeedback;
}
@@ -282,37 +274,36 @@ public:
return m_jointFeedback;
}
int getUid() const
{
return m_userConstraintId;
}
return m_userConstraintId;
}
bool needsFeedback() const
bool needsFeedback() const
{
return m_needsFeedback;
}
///enableFeedback will allow to read the applied linear and angular impulse
///use getAppliedImpulse, getAppliedLinearImpulse and getAppliedAngularImpulse to read feedback information
void enableFeedback(bool needsFeedback)
void enableFeedback(bool needsFeedback)
{
m_needsFeedback = needsFeedback;
}
///getAppliedImpulse is an estimated total applied impulse.
///getAppliedImpulse is an estimated total applied impulse.
///This feedback could be used to determine breaking constraints or playing sounds.
btScalar getAppliedImpulse() const
btScalar getAppliedImpulse() const
{
btAssert(m_needsFeedback);
return m_appliedImpulse;
}
btTypedConstraintType getConstraintType () const
btTypedConstraintType getConstraintType() const
{
return btTypedConstraintType(m_objectType);
}
void setDbgDrawSize(btScalar dbgDrawSize)
{
m_dbgDrawSize = dbgDrawSize;
@@ -322,35 +313,34 @@ public:
return m_dbgDrawSize;
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///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) = 0;
virtual void setParam(int num, btScalar value, int axis = -1) = 0;
///return the local value of parameter
virtual btScalar getParam(int num, int axis = -1) const = 0;
virtual int calculateSerializeBufferSize() const;
virtual btScalar getParam(int num, int axis = -1) const = 0;
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
};
// returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits
// returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits
// all arguments should be normalized angles (i.e. in range [-SIMD_PI, SIMD_PI])
SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
{
if(angleLowerLimitInRadians >= angleUpperLimitInRadians)
if (angleLowerLimitInRadians >= angleUpperLimitInRadians)
{
return angleInRadians;
}
else if(angleInRadians < angleLowerLimitInRadians)
else if (angleInRadians < angleLowerLimitInRadians)
{
btScalar diffLo = btFabs(btNormalizeAngle(angleLowerLimitInRadians - angleInRadians));
btScalar diffHi = btFabs(btNormalizeAngle(angleUpperLimitInRadians - angleInRadians));
return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI);
}
else if(angleInRadians > angleUpperLimitInRadians)
else if (angleInRadians > angleUpperLimitInRadians)
{
btScalar diffHi = btFabs(btNormalizeAngle(angleInRadians - angleUpperLimitInRadians));
btScalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians));
@@ -362,6 +352,8 @@ SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScal
}
}
// clang-format off
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btTypedConstraintFloatData
{
@@ -385,6 +377,8 @@ struct btTypedConstraintFloatData
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
#define BT_BACKWARDS_COMPATIBLE_SERIALIZATION
@@ -436,18 +430,17 @@ struct btTypedConstraintDoubleData
};
// clang-format on
SIMD_FORCE_INLINE int btTypedConstraint::calculateSerializeBufferSize() const
SIMD_FORCE_INLINE int btTypedConstraint::calculateSerializeBufferSize() const
{
return sizeof(btTypedConstraintData2);
}
class btAngularLimit
{
private:
btScalar
btScalar
m_center,
m_halfRange,
m_softness,
@@ -462,15 +455,16 @@ private:
public:
/// Default constructor initializes limit as inactive, allowing free constraint movement
btAngularLimit()
:m_center(0.0f),
m_halfRange(-1.0f),
m_softness(0.9f),
m_biasFactor(0.3f),
m_relaxationFactor(1.0f),
m_correction(0.0f),
m_sign(0.0f),
m_solveLimit(false)
{}
: m_center(0.0f),
m_halfRange(-1.0f),
m_softness(0.9f),
m_biasFactor(0.3f),
m_relaxationFactor(1.0f),
m_correction(0.0f),
m_sign(0.0f),
m_solveLimit(false)
{
}
/// Sets all limit's parameters.
/// When low > high limit becomes inactive.
@@ -499,13 +493,13 @@ public:
return m_relaxationFactor;
}
/// Returns correction value evaluated when test() was invoked
/// Returns correction value evaluated when test() was invoked
inline btScalar getCorrection() const
{
return m_correction;
}
/// Returns sign value evaluated when test() was invoked
/// Returns sign value evaluated when test() was invoked
inline btScalar getSign() const
{
return m_sign;
@@ -533,9 +527,6 @@ public:
btScalar getLow() const;
btScalar getHigh() const;
};
#endif //BT_TYPED_CONSTRAINT_H
#endif //BT_TYPED_CONSTRAINT_H

43
src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp
View File

@@ -13,43 +13,38 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btUniversalConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btTransformUtil.h"
#define UNIV_EPS btScalar(0.01f)
// constructor
// anchor, axis1 and axis2 are in world coordinate system
// axis1 must be orthogonal to axis2
btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2)
: btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true),
m_anchor(anchor),
m_axis1(axis1),
m_axis2(axis2)
: btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true),
m_anchor(anchor),
m_axis1(axis1),
m_axis2(axis2)
{
// build frame basis
// 6DOF constraint uses Euler angles and to define limits
// it is assumed that rotational order is :
// Z - first, allowed limits are (-PI,PI);
// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number
// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number
// used to prevent constraint from instability on poles;
// new position of X, allowed limits are (-PI,PI);
// So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
// Build the frame in world coordinate system first
btVector3 zAxis = m_axis1.normalize();
btVector3 yAxis = m_axis2.normalize();
btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
btTransform frameInW;
frameInW.setIdentity();
frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
xAxis[1], yAxis[1], zAxis[1],
xAxis[2], yAxis[2], zAxis[2]);
frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
xAxis[1], yAxis[1], zAxis[1],
xAxis[2], yAxis[2], zAxis[2]);
frameInW.setOrigin(anchor);
// now get constraint frame in local coordinate systems
m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW;
@@ -58,30 +53,28 @@ btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB,
setLinearLowerLimit(btVector3(0., 0., 0.));
setLinearUpperLimit(btVector3(0., 0., 0.));
setAngularLowerLimit(btVector3(0.f, -SIMD_HALF_PI + UNIV_EPS, -SIMD_PI + UNIV_EPS));
setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI - UNIV_EPS, SIMD_PI - UNIV_EPS));
setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI - UNIV_EPS, SIMD_PI - UNIV_EPS));
}
void btUniversalConstraint::setAxis(const btVector3& axis1,const btVector3& axis2)
void btUniversalConstraint::setAxis(const btVector3& axis1, const btVector3& axis2)
{
m_axis1 = axis1;
m_axis2 = axis2;
m_axis1 = axis1;
m_axis2 = axis2;
btVector3 zAxis = axis1.normalized();
btVector3 yAxis = axis2.normalized();
btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
btTransform frameInW;
frameInW.setIdentity();
frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
xAxis[1], yAxis[1], zAxis[1],
xAxis[2], yAxis[2], zAxis[2]);
frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
xAxis[1], yAxis[1], zAxis[1],
xAxis[2], yAxis[2], zAxis[2]);
frameInW.setOrigin(m_anchor);
// now get constraint frame in local coordinate systems
m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
calculateTransforms();
calculateTransforms();
}

30
src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h
View File

@@ -16,35 +16,32 @@ subject to the following restrictions:
#ifndef BT_UNIVERSAL_CONSTRAINT_H
#define BT_UNIVERSAL_CONSTRAINT_H
#include "LinearMath/btVector3.h"
#include "btTypedConstraint.h"
#include "btGeneric6DofConstraint.h"
/// Constraint similar to ODE Universal Joint
/// has 2 rotatioonal degrees of freedom, similar to Euler rotations around Z (axis 1)
/// and Y (axis 2)
/// Description from ODE manual :
/// "Given axis 1 on body 1, and axis 2 on body 2 that is perpendicular to axis 1, it keeps them perpendicular.
/// Description from ODE manual :
/// "Given axis 1 on body 1, and axis 2 on body 2 that is perpendicular to axis 1, it keeps them perpendicular.
/// In other words, rotation of the two bodies about the direction perpendicular to the two axes will be equal."
ATTRIBUTE_ALIGNED16(class) btUniversalConstraint : public btGeneric6DofConstraint
ATTRIBUTE_ALIGNED16(class)
btUniversalConstraint : public btGeneric6DofConstraint
{
protected:
btVector3 m_anchor;
btVector3 m_axis1;
btVector3 m_axis2;
btVector3 m_anchor;
btVector3 m_axis1;
btVector3 m_axis2;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
// constructor
// anchor, axis1 and axis2 are in world coordinate system
// axis1 must be orthogonal to axis2
btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2);
btUniversalConstraint(btRigidBody & rbA, btRigidBody & rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2);
// access
const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); }
const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); }
@@ -56,10 +53,7 @@ public:
void setUpperLimit(btScalar ang1max, btScalar ang2max) { setAngularUpperLimit(btVector3(0.f, ang1max, ang2max)); }
void setLowerLimit(btScalar ang1min, btScalar ang2min) { setAngularLowerLimit(btVector3(0.f, ang1min, ang2min)); }
void setAxis( const btVector3& axis1, const btVector3& axis2);
void setAxis(const btVector3& axis1, const btVector3& axis2);
};
#endif // BT_UNIVERSAL_CONSTRAINT_H
#endif // BT_UNIVERSAL_CONSTRAINT_H

11
src/BulletDynamics/Dynamics/btActionInterface.h
View File

@@ -26,21 +26,16 @@ class btCollisionWorld;
class btActionInterface
{
protected:
static btRigidBody& getFixedBody();
public:
public:
virtual ~btActionInterface()
{
}
virtual void updateAction( btCollisionWorld* collisionWorld, btScalar deltaTimeStep)=0;
virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTimeStep) = 0;
virtual void debugDraw(btIDebugDraw* debugDrawer) = 0;
};
#endif //_BT_ACTION_INTERFACE_H
#endif //_BT_ACTION_INTERFACE_H

1005
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
View File

File diff suppressed because it is too large Load Diff

161
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
View File

@@ -13,7 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_DISCRETE_DYNAMICS_WORLD_H
#define BT_DISCRETE_DYNAMICS_WORLD_H
@@ -32,159 +31,153 @@ struct InplaceSolverIslandCallback;
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btThreads.h"
///btDiscreteDynamicsWorld provides discrete rigid body simulation
///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController
ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorld : public btDynamicsWorld
ATTRIBUTE_ALIGNED16(class)
btDiscreteDynamicsWorld : public btDynamicsWorld
{
protected:
btAlignedObjectArray<btTypedConstraint*> m_sortedConstraints;
InplaceSolverIslandCallback* m_solverIslandCallback;
btAlignedObjectArray<btTypedConstraint*> m_sortedConstraints;
InplaceSolverIslandCallback* m_solverIslandCallback;
btConstraintSolver* m_constraintSolver;
btConstraintSolver* m_constraintSolver;
btSimulationIslandManager* m_islandManager;
btSimulationIslandManager* m_islandManager;
btAlignedObjectArray<btTypedConstraint*> m_constraints;
btAlignedObjectArray<btRigidBody*> m_nonStaticRigidBodies;
btVector3 m_gravity;
btVector3 m_gravity;
//for variable timesteps
btScalar m_localTime;
btScalar m_fixedTimeStep;
btScalar m_localTime;
btScalar m_fixedTimeStep;
//for variable timesteps
bool m_ownsIslandManager;
bool m_ownsConstraintSolver;
bool m_synchronizeAllMotionStates;
bool m_applySpeculativeContactRestitution;
bool m_ownsIslandManager;
bool m_ownsConstraintSolver;
bool m_synchronizeAllMotionStates;
bool m_applySpeculativeContactRestitution;
btAlignedObjectArray<btActionInterface*> m_actions;
int m_profileTimings;
btAlignedObjectArray<btActionInterface*> m_actions;
bool m_latencyMotionStateInterpolation;
int m_profileTimings;
btAlignedObjectArray<btPersistentManifold*> m_predictiveManifolds;
btSpinMutex m_predictiveManifoldsMutex; // used to synchronize threads creating predictive contacts
bool m_latencyMotionStateInterpolation;
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();
btAlignedObjectArray<btPersistentManifold*> m_predictiveManifolds;
btSpinMutex m_predictiveManifoldsMutex; // used to synchronize threads creating predictive contacts
virtual void solveConstraints(btContactSolverInfo& solverInfo);
virtual void updateActivationState(btScalar timeStep);
virtual void predictUnconstraintMotion(btScalar timeStep);
void updateActions(btScalar timeStep);
void integrateTransformsInternal(btRigidBody * *bodies, int numBodies, btScalar timeStep); // can be called in parallel
virtual void integrateTransforms(btScalar timeStep);
void startProfiling(btScalar timeStep);
virtual void calculateSimulationIslands();
virtual void internalSingleStepSimulation( btScalar timeStep);
virtual void solveConstraints(btContactSolverInfo & solverInfo);
void releasePredictiveContacts();
void createPredictiveContactsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep ); // can be called in parallel
virtual void createPredictiveContacts(btScalar timeStep);
virtual void updateActivationState(btScalar timeStep);
virtual void saveKinematicState(btScalar timeStep);
void updateActions(btScalar timeStep);
void serializeRigidBodies(btSerializer* serializer);
void startProfiling(btScalar timeStep);
void serializeDynamicsWorldInfo(btSerializer* serializer);
virtual void internalSingleStepSimulation(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);
void serializeRigidBodies(btSerializer * serializer);
void serializeDynamicsWorldInfo(btSerializer * serializer);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
///this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those
btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
btDiscreteDynamicsWorld(btDispatcher * dispatcher, btBroadphaseInterface * pairCache, btConstraintSolver * constraintSolver, btCollisionConfiguration * collisionConfiguration);
virtual ~btDiscreteDynamicsWorld();
///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's
virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.));
virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.));
virtual void synchronizeMotionStates();
virtual void synchronizeMotionStates();
///this can be useful to synchronize a single rigid body -> graphics object
void synchronizeSingleMotionState(btRigidBody* body);
void synchronizeSingleMotionState(btRigidBody * body);
virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false);
virtual void addConstraint(btTypedConstraint * constraint, bool disableCollisionsBetweenLinkedBodies = false);
virtual void removeConstraint(btTypedConstraint* constraint);
virtual void removeConstraint(btTypedConstraint * constraint);
virtual void addAction(btActionInterface*);
virtual void addAction(btActionInterface*);
virtual void removeAction(btActionInterface*);
btSimulationIslandManager* getSimulationIslandManager()
virtual void removeAction(btActionInterface*);
btSimulationIslandManager* getSimulationIslandManager()
{
return m_islandManager;
}
const btSimulationIslandManager* getSimulationIslandManager() const
const btSimulationIslandManager* getSimulationIslandManager() const
{
return m_islandManager;
}
btCollisionWorld* getCollisionWorld()
btCollisionWorld* getCollisionWorld()
{
return this;
}
virtual void setGravity(const btVector3& gravity);
virtual void setGravity(const btVector3& gravity);
virtual btVector3 getGravity () const;
virtual btVector3 getGravity() const;
virtual void addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup=btBroadphaseProxy::StaticFilter, int collisionFilterMask=btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
virtual void addCollisionObject(btCollisionObject * collisionObject, int collisionFilterGroup = btBroadphaseProxy::StaticFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
virtual void addRigidBody(btRigidBody* body);
virtual void addRigidBody(btRigidBody * body);
virtual void addRigidBody(btRigidBody* body, int group, int mask);
virtual void addRigidBody(btRigidBody * body, int group, int mask);
virtual void removeRigidBody(btRigidBody* body);
virtual void removeRigidBody(btRigidBody * body);
///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
virtual void removeCollisionObject(btCollisionObject* collisionObject);
virtual void removeCollisionObject(btCollisionObject * collisionObject);
virtual void debugDrawConstraint(btTypedConstraint * constraint);
virtual void debugDrawConstraint(btTypedConstraint* constraint);
virtual void debugDrawWorld();
virtual void debugDrawWorld();
virtual void setConstraintSolver(btConstraintSolver* solver);
virtual void setConstraintSolver(btConstraintSolver * solver);
virtual btConstraintSolver* getConstraintSolver();
virtual int getNumConstraints() const;
virtual btTypedConstraint* getConstraint(int index) ;
virtual int getNumConstraints() const;
virtual btTypedConstraint* getConstraint(int index);
virtual const btTypedConstraint* getConstraint(int index) const;
virtual btDynamicsWorldType getWorldType() const
virtual btDynamicsWorldType getWorldType() const
{
return BT_DISCRETE_DYNAMICS_WORLD;
}
///the forces on each rigidbody is accumulating together with gravity. clear this after each timestep.
virtual void clearForces();
virtual void clearForces();
///apply gravity, call this once per timestep
virtual void applyGravity();
virtual void applyGravity();
virtual void setNumTasks(int numTasks)
virtual void setNumTasks(int numTasks)
{
(void) numTasks;
(void)numTasks;
}
///obsolete, use updateActions instead
@@ -194,15 +187,15 @@ public:
}
///obsolete, use addAction instead
virtual void addVehicle(btActionInterface* vehicle);
virtual void addVehicle(btActionInterface * vehicle);
///obsolete, use removeAction instead
virtual void removeVehicle(btActionInterface* vehicle);
virtual void removeVehicle(btActionInterface * vehicle);
///obsolete, use addAction instead
virtual void addCharacter(btActionInterface* character);
virtual void addCharacter(btActionInterface * character);
///obsolete, use removeAction instead
virtual void removeCharacter(btActionInterface* character);
virtual void removeCharacter(btActionInterface * character);
void setSynchronizeAllMotionStates(bool synchronizeAll)
void setSynchronizeAllMotionStates(bool synchronizeAll)
{
m_synchronizeAllMotionStates = synchronizeAll;
}
@@ -215,18 +208,18 @@ public:
{
m_applySpeculativeContactRestitution = enable;
}
bool getApplySpeculativeContactRestitution() const
{
return m_applySpeculativeContactRestitution;
}
///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (see Bullet/Demos/SerializeDemo)
virtual void serialize(btSerializer* serializer);
virtual void serialize(btSerializer * serializer);
///Interpolate motion state between previous and current transform, instead of current and next transform.
///This can relieve discontinuities in the rendering, due to penetrations
void setLatencyMotionStateInterpolation(bool latencyInterpolation )
void setLatencyMotionStateInterpolation(bool latencyInterpolation)
{
m_latencyMotionStateInterpolation = latencyInterpolation;
}
@@ -236,4 +229,4 @@ public:
}
};
#endif //BT_DISCRETE_DYNAMICS_WORLD_H
#endif //BT_DISCRETE_DYNAMICS_WORLD_H

289
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp
View File

@@ -13,7 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btDiscreteDynamicsWorldMt.h"
//collision detection
@@ -38,148 +37,139 @@ subject to the following restrictions:
#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"
///
/// btConstraintSolverPoolMt
///
btConstraintSolverPoolMt::ThreadSolver* btConstraintSolverPoolMt::getAndLockThreadSolver()
{
int i = 0;
int i = 0;
#if BT_THREADSAFE
i = btGetCurrentThreadIndex() % m_solvers.size();
#endif // #if BT_THREADSAFE
while ( true )
{
ThreadSolver& solver = m_solvers[ i ];
if ( solver.mutex.tryLock() )
{
return &solver;
}
// failed, try the next one
i = ( i + 1 ) % m_solvers.size();
}
return NULL;
i = btGetCurrentThreadIndex() % m_solvers.size();
#endif // #if BT_THREADSAFE
while (true)
{
ThreadSolver& solver = m_solvers[i];
if (solver.mutex.tryLock())
{
return &solver;
}
// failed, try the next one
i = (i + 1) % m_solvers.size();
}
return NULL;
}
void btConstraintSolverPoolMt::init( btConstraintSolver** solvers, int numSolvers )
void btConstraintSolverPoolMt::init(btConstraintSolver** solvers, int numSolvers)
{
m_solverType = BT_SEQUENTIAL_IMPULSE_SOLVER;
m_solvers.resize( numSolvers );
for ( int i = 0; i < numSolvers; ++i )
{
m_solvers[ i ].solver = solvers[ i ];
}
if ( numSolvers > 0 )
{
m_solverType = solvers[ 0 ]->getSolverType();
}
m_solverType = BT_SEQUENTIAL_IMPULSE_SOLVER;
m_solvers.resize(numSolvers);
for (int i = 0; i < numSolvers; ++i)
{
m_solvers[i].solver = solvers[i];
}
if (numSolvers > 0)
{
m_solverType = solvers[0]->getSolverType();
}
}
// create the solvers for me
btConstraintSolverPoolMt::btConstraintSolverPoolMt( int numSolvers )
btConstraintSolverPoolMt::btConstraintSolverPoolMt(int numSolvers)
{
btAlignedObjectArray<btConstraintSolver*> solvers;
solvers.reserve( numSolvers );
for ( int i = 0; i < numSolvers; ++i )
{
btConstraintSolver* solver = new btSequentialImpulseConstraintSolver();
solvers.push_back( solver );
}
init( &solvers[ 0 ], numSolvers );
btAlignedObjectArray<btConstraintSolver*> solvers;
solvers.reserve(numSolvers);
for (int i = 0; i < numSolvers; ++i)
{
btConstraintSolver* solver = new btSequentialImpulseConstraintSolver();
solvers.push_back(solver);
}
init(&solvers[0], numSolvers);
}
// pass in fully constructed solvers (destructor will delete them)
btConstraintSolverPoolMt::btConstraintSolverPoolMt( btConstraintSolver** solvers, int numSolvers )
btConstraintSolverPoolMt::btConstraintSolverPoolMt(btConstraintSolver** solvers, int numSolvers)
{
init( solvers, numSolvers );
init(solvers, numSolvers);
}
btConstraintSolverPoolMt::~btConstraintSolverPoolMt()
{
// delete all solvers
for ( int i = 0; i < m_solvers.size(); ++i )
{
ThreadSolver& solver = m_solvers[ i ];
delete solver.solver;
solver.solver = NULL;
}
// delete all solvers
for (int i = 0; i < m_solvers.size(); ++i)
{
ThreadSolver& solver = m_solvers[i];
delete solver.solver;
solver.solver = NULL;
}
}
///solve a group of constraints
btScalar btConstraintSolverPoolMt::solveGroup( btCollisionObject** bodies,
int numBodies,
btPersistentManifold** manifolds,
int numManifolds,
btTypedConstraint** constraints,
int numConstraints,
const btContactSolverInfo& info,
btIDebugDraw* debugDrawer,
btDispatcher* dispatcher
)
btScalar btConstraintSolverPoolMt::solveGroup(btCollisionObject** bodies,
int numBodies,
btPersistentManifold** manifolds,
int numManifolds,
btTypedConstraint** constraints,
int numConstraints,
const btContactSolverInfo& info,
btIDebugDraw* debugDrawer,
btDispatcher* dispatcher)
{
ThreadSolver* ts = getAndLockThreadSolver();
ts->solver->solveGroup( bodies, numBodies, manifolds, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher );
ts->mutex.unlock();
return 0.0f;
ThreadSolver* ts = getAndLockThreadSolver();
ts->solver->solveGroup(bodies, numBodies, manifolds, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher);
ts->mutex.unlock();
return 0.0f;
}
void btConstraintSolverPoolMt::reset()
{
for ( int i = 0; i < m_solvers.size(); ++i )
{
ThreadSolver& solver = m_solvers[ i ];
solver.mutex.lock();
solver.solver->reset();
solver.mutex.unlock();
}
for (int i = 0; i < m_solvers.size(); ++i)
{
ThreadSolver& solver = m_solvers[i];
solver.mutex.lock();
solver.solver->reset();
solver.mutex.unlock();
}
}
///
/// btDiscreteDynamicsWorldMt
///
btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,
btBroadphaseInterface* pairCache,
btConstraintSolverPoolMt* constraintSolver,
btConstraintSolver* constraintSolverMt,
btCollisionConfiguration* collisionConfiguration
)
: btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration)
btBroadphaseInterface* pairCache,
btConstraintSolverPoolMt* constraintSolver,
btConstraintSolver* constraintSolverMt,
btCollisionConfiguration* collisionConfiguration)
: btDiscreteDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration)
{
if (m_ownsIslandManager)
{
m_islandManager->~btSimulationIslandManager();
btAlignedFree( m_islandManager);
btAlignedFree(m_islandManager);
}
{
void* mem = btAlignedAlloc(sizeof(btSimulationIslandManagerMt),16);
void* mem = btAlignedAlloc(sizeof(btSimulationIslandManagerMt), 16);
btSimulationIslandManagerMt* im = new (mem) btSimulationIslandManagerMt();
im->setMinimumSolverBatchSize( m_solverInfo.m_minimumSolverBatchSize );
m_islandManager = im;
im->setMinimumSolverBatchSize(m_solverInfo.m_minimumSolverBatchSize);
m_islandManager = im;
}
m_constraintSolverMt = constraintSolverMt;
m_constraintSolverMt = constraintSolverMt;
}
btDiscreteDynamicsWorldMt::~btDiscreteDynamicsWorldMt()
{
}
void btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo)
{
BT_PROFILE("solveConstraints");
@@ -187,92 +177,87 @@ void btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo
m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
/// solve all the constraints for this island
btSimulationIslandManagerMt* im = static_cast<btSimulationIslandManagerMt*>(m_islandManager);
btSimulationIslandManagerMt::SolverParams solverParams;
solverParams.m_solverPool = m_constraintSolver;
solverParams.m_solverMt = m_constraintSolverMt;
solverParams.m_solverInfo = &solverInfo;
solverParams.m_debugDrawer = m_debugDrawer;
solverParams.m_dispatcher = getCollisionWorld()->getDispatcher();
im->buildAndProcessIslands( getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, solverParams );
btSimulationIslandManagerMt* im = static_cast<btSimulationIslandManagerMt*>(m_islandManager);
btSimulationIslandManagerMt::SolverParams solverParams;
solverParams.m_solverPool = m_constraintSolver;
solverParams.m_solverMt = m_constraintSolverMt;
solverParams.m_solverInfo = &solverInfo;
solverParams.m_debugDrawer = m_debugDrawer;
solverParams.m_dispatcher = getCollisionWorld()->getDispatcher();
im->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, solverParams);
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
}
struct UpdaterUnconstrainedMotion : public btIParallelForBody
{
btScalar timeStep;
btRigidBody** rigidBodies;
btScalar timeStep;
btRigidBody** rigidBodies;
void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
{
for ( int i = iBegin; i < iEnd; ++i )
{
btRigidBody* body = rigidBodies[ i ];
if ( !body->isStaticOrKinematicObject() )
{
//don't integrate/update velocities here, it happens in the constraint solver
body->applyDamping( timeStep );
body->predictIntegratedTransform( timeStep, body->getInterpolationWorldTransform() );
}
}
}
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
{
for (int i = iBegin; i < iEnd; ++i)
{
btRigidBody* body = rigidBodies[i];
if (!body->isStaticOrKinematicObject())
{
//don't integrate/update velocities here, it happens in the constraint solver
body->applyDamping(timeStep);
body->predictIntegratedTransform(timeStep, body->getInterpolationWorldTransform());
}
}
}
};
void btDiscreteDynamicsWorldMt::predictUnconstraintMotion( btScalar timeStep )
void btDiscreteDynamicsWorldMt::predictUnconstraintMotion(btScalar timeStep)
{
BT_PROFILE( "predictUnconstraintMotion" );
if ( m_nonStaticRigidBodies.size() > 0 )
{
UpdaterUnconstrainedMotion update;
update.timeStep = timeStep;
update.rigidBodies = &m_nonStaticRigidBodies[ 0 ];
int grainSize = 50; // num of iterations per task for task scheduler
btParallelFor( 0, m_nonStaticRigidBodies.size(), grainSize, update );
}
BT_PROFILE("predictUnconstraintMotion");
if (m_nonStaticRigidBodies.size() > 0)
{
UpdaterUnconstrainedMotion update;
update.timeStep = timeStep;
update.rigidBodies = &m_nonStaticRigidBodies[0];
int grainSize = 50; // num of iterations per task for task scheduler
btParallelFor(0, m_nonStaticRigidBodies.size(), grainSize, update);
}
}
void btDiscreteDynamicsWorldMt::createPredictiveContacts( btScalar timeStep )
void btDiscreteDynamicsWorldMt::createPredictiveContacts(btScalar timeStep)
{
BT_PROFILE( "createPredictiveContacts" );
releasePredictiveContacts();
if ( m_nonStaticRigidBodies.size() > 0 )
{
UpdaterCreatePredictiveContacts update;
update.world = this;
update.timeStep = timeStep;
update.rigidBodies = &m_nonStaticRigidBodies[ 0 ];
int grainSize = 50; // num of iterations per task for task scheduler
btParallelFor( 0, m_nonStaticRigidBodies.size(), grainSize, update );
}
BT_PROFILE("createPredictiveContacts");
releasePredictiveContacts();
if (m_nonStaticRigidBodies.size() > 0)
{
UpdaterCreatePredictiveContacts update;
update.world = this;
update.timeStep = timeStep;
update.rigidBodies = &m_nonStaticRigidBodies[0];
int grainSize = 50; // num of iterations per task for task scheduler
btParallelFor(0, m_nonStaticRigidBodies.size(), grainSize, update);
}
}
void btDiscreteDynamicsWorldMt::integrateTransforms( btScalar timeStep )
void btDiscreteDynamicsWorldMt::integrateTransforms(btScalar timeStep)
{
BT_PROFILE( "integrateTransforms" );
if ( m_nonStaticRigidBodies.size() > 0 )
{
UpdaterIntegrateTransforms update;
update.world = this;
update.timeStep = timeStep;
update.rigidBodies = &m_nonStaticRigidBodies[ 0 ];
int grainSize = 50; // num of iterations per task for task scheduler
btParallelFor( 0, m_nonStaticRigidBodies.size(), grainSize, update );
}
BT_PROFILE("integrateTransforms");
if (m_nonStaticRigidBodies.size() > 0)
{
UpdaterIntegrateTransforms update;
update.world = this;
update.timeStep = timeStep;
update.rigidBodies = &m_nonStaticRigidBodies[0];
int grainSize = 50; // num of iterations per task for task scheduler
btParallelFor(0, m_nonStaticRigidBodies.size(), grainSize, update);
}
}
int btDiscreteDynamicsWorldMt::stepSimulation( btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep )
int btDiscreteDynamicsWorldMt::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep)
{
int numSubSteps = btDiscreteDynamicsWorld::stepSimulation(timeStep, maxSubSteps, fixedTimeStep);
if (btITaskScheduler* scheduler = btGetTaskScheduler())
{
// tell Bullet's threads to sleep, so other threads can run
scheduler->sleepWorkerThreadsHint();
}
return numSubSteps;
int numSubSteps = btDiscreteDynamicsWorld::stepSimulation(timeStep, maxSubSteps, fixedTimeStep);
if (btITaskScheduler* scheduler = btGetTaskScheduler())
{
// tell Bullet's threads to sleep, so other threads can run
scheduler->sleepWorkerThreadsHint();
}
return numSubSteps;
}

129
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h
View File

@@ -13,7 +13,6 @@ subject to the following restrictions:
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
@@ -21,7 +20,6 @@ subject to the following restrictions:
#include "btSimulationIslandManagerMt.h"
#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
///
/// btConstraintSolverPoolMt - masquerades as a constraint solver, but really it is a threadsafe pool of them.
///
@@ -34,46 +32,43 @@ subject to the following restrictions:
class btConstraintSolverPoolMt : public btConstraintSolver
{
public:
// create the solvers for me
explicit btConstraintSolverPoolMt( int numSolvers );
// create the solvers for me
explicit btConstraintSolverPoolMt(int numSolvers);
// pass in fully constructed solvers (destructor will delete them)
btConstraintSolverPoolMt( btConstraintSolver** solvers, int numSolvers );
// pass in fully constructed solvers (destructor will delete them)
btConstraintSolverPoolMt(btConstraintSolver** solvers, int numSolvers);
virtual ~btConstraintSolverPoolMt();
virtual ~btConstraintSolverPoolMt();
///solve a group of constraints
virtual btScalar solveGroup( btCollisionObject** bodies,
int numBodies,
btPersistentManifold** manifolds,
int numManifolds,
btTypedConstraint** constraints,
int numConstraints,
const btContactSolverInfo& info,
btIDebugDraw* debugDrawer,
btDispatcher* dispatcher
) BT_OVERRIDE;
///solve a group of constraints
virtual btScalar solveGroup(btCollisionObject** bodies,
int numBodies,
btPersistentManifold** manifolds,
int numManifolds,
btTypedConstraint** constraints,
int numConstraints,
const btContactSolverInfo& info,
btIDebugDraw* debugDrawer,
btDispatcher* dispatcher) BT_OVERRIDE;
virtual void reset() BT_OVERRIDE;
virtual btConstraintSolverType getSolverType() const BT_OVERRIDE { return m_solverType; }
virtual void reset() BT_OVERRIDE;
virtual btConstraintSolverType getSolverType() const BT_OVERRIDE { return m_solverType; }
private:
const static size_t kCacheLineSize = 128;
struct ThreadSolver
{
btConstraintSolver* solver;
btSpinMutex mutex;
char _cachelinePadding[ kCacheLineSize - sizeof( btSpinMutex ) - sizeof( void* ) ]; // keep mutexes from sharing a cache line
};
btAlignedObjectArray<ThreadSolver> m_solvers;
btConstraintSolverType m_solverType;
const static size_t kCacheLineSize = 128;
struct ThreadSolver
{
btConstraintSolver* solver;
btSpinMutex mutex;
char _cachelinePadding[kCacheLineSize - sizeof(btSpinMutex) - sizeof(void*)]; // keep mutexes from sharing a cache line
};
btAlignedObjectArray<ThreadSolver> m_solvers;
btConstraintSolverType m_solverType;
ThreadSolver* getAndLockThreadSolver();
void init( btConstraintSolver** solvers, int numSolvers );
ThreadSolver* getAndLockThreadSolver();
void init(btConstraintSolver** solvers, int numSolvers);
};
///
/// btDiscreteDynamicsWorldMt -- a version of DiscreteDynamicsWorld with some minor changes to support
/// solving simulation islands on multiple threads.
@@ -84,53 +79,53 @@ private:
/// - integrateTransforms
/// - createPredictiveContacts
///
ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorldMt : public btDiscreteDynamicsWorld
ATTRIBUTE_ALIGNED16(class)
btDiscreteDynamicsWorldMt : public btDiscreteDynamicsWorld
{
protected:
btConstraintSolver* m_constraintSolverMt;
btConstraintSolver* m_constraintSolverMt;
virtual void solveConstraints(btContactSolverInfo& solverInfo) BT_OVERRIDE;
virtual void solveConstraints(btContactSolverInfo & solverInfo) BT_OVERRIDE;
virtual void predictUnconstraintMotion( btScalar timeStep ) BT_OVERRIDE;
virtual void predictUnconstraintMotion(btScalar timeStep) BT_OVERRIDE;
struct UpdaterCreatePredictiveContacts : public btIParallelForBody
{
btScalar timeStep;
btRigidBody** rigidBodies;
btDiscreteDynamicsWorldMt* world;
struct UpdaterCreatePredictiveContacts : public btIParallelForBody
{
btScalar timeStep;
btRigidBody** rigidBodies;
btDiscreteDynamicsWorldMt* world;
void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
{
world->createPredictiveContactsInternal( &rigidBodies[ iBegin ], iEnd - iBegin, timeStep );
}
};
virtual void createPredictiveContacts( btScalar timeStep ) BT_OVERRIDE;
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
{
world->createPredictiveContactsInternal(&rigidBodies[iBegin], iEnd - iBegin, timeStep);
}
};
virtual void createPredictiveContacts(btScalar timeStep) BT_OVERRIDE;
struct UpdaterIntegrateTransforms : public btIParallelForBody
{
btScalar timeStep;
btRigidBody** rigidBodies;
btDiscreteDynamicsWorldMt* world;
struct UpdaterIntegrateTransforms : public btIParallelForBody
{
btScalar timeStep;
btRigidBody** rigidBodies;
btDiscreteDynamicsWorldMt* world;
void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
{
world->integrateTransformsInternal( &rigidBodies[ iBegin ], iEnd - iBegin, timeStep );
}
};
virtual void integrateTransforms( btScalar timeStep ) BT_OVERRIDE;
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
{
world->integrateTransformsInternal(&rigidBodies[iBegin], iEnd - iBegin, timeStep);
}
};
virtual void integrateTransforms(btScalar timeStep) BT_OVERRIDE;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,
btBroadphaseInterface* pairCache,
btConstraintSolverPoolMt* constraintSolver, // Note this should be a solver-pool for multi-threading
btConstraintSolver* constraintSolverMt, // single multi-threaded solver for large islands (or NULL)
btCollisionConfiguration* collisionConfiguration
);
btDiscreteDynamicsWorldMt(btDispatcher * dispatcher,
btBroadphaseInterface * pairCache,
btConstraintSolverPoolMt * constraintSolver, // Note this should be a solver-pool for multi-threading
btConstraintSolver * constraintSolverMt, // single multi-threaded solver for large islands (or NULL)
btCollisionConfiguration * collisionConfiguration);
virtual ~btDiscreteDynamicsWorldMt();
virtual int stepSimulation( btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep ) BT_OVERRIDE;
virtual int stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep) BT_OVERRIDE;
};
#endif //BT_DISCRETE_DYNAMICS_WORLD_H
#endif //BT_DISCRETE_DYNAMICS_WORLD_H

224
src/BulletDynamics/Dynamics/btDynamicsWorld.h
View File

@@ -24,150 +24,150 @@ class btActionInterface;
class btConstraintSolver;
class btDynamicsWorld;
/// Type for the callback for each tick
typedef void (*btInternalTickCallback)(btDynamicsWorld *world, btScalar timeStep);
typedef void (*btInternalTickCallback)(btDynamicsWorld* world, btScalar timeStep);
enum btDynamicsWorldType
{
BT_SIMPLE_DYNAMICS_WORLD=1,
BT_DISCRETE_DYNAMICS_WORLD=2,
BT_CONTINUOUS_DYNAMICS_WORLD=3,
BT_SOFT_RIGID_DYNAMICS_WORLD=4,
BT_GPU_DYNAMICS_WORLD=5,
BT_SOFT_MULTIBODY_DYNAMICS_WORLD=6
BT_SIMPLE_DYNAMICS_WORLD = 1,
BT_DISCRETE_DYNAMICS_WORLD = 2,
BT_CONTINUOUS_DYNAMICS_WORLD = 3,
BT_SOFT_RIGID_DYNAMICS_WORLD = 4,
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.
class btDynamicsWorld : public btCollisionWorld
{
protected:
btInternalTickCallback m_internalTickCallback;
btInternalTickCallback m_internalPreTickCallback;
void* m_worldUserInfo;
btInternalTickCallback m_internalTickCallback;
btInternalTickCallback m_internalPreTickCallback;
void* m_worldUserInfo;
btContactSolverInfo m_solverInfo;
btContactSolverInfo m_solverInfo;
public:
btDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* broadphase, btCollisionConfiguration* collisionConfiguration)
: btCollisionWorld(dispatcher, broadphase, collisionConfiguration), m_internalTickCallback(0), m_internalPreTickCallback(0), m_worldUserInfo(0)
{
}
btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration)
:btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0),m_internalPreTickCallback(0), m_worldUserInfo(0)
virtual ~btDynamicsWorld()
{
}
///stepSimulation proceeds the simulation over 'timeStep', units in preferably in seconds.
///By default, Bullet will subdivide the timestep in constant substeps of each 'fixedTimeStep'.
///in order to keep the simulation real-time, the maximum number of substeps can be clamped to 'maxSubSteps'.
///You can disable subdividing the timestep/substepping by passing maxSubSteps=0 as second argument to stepSimulation, but in that case you have to keep the timeStep constant.
virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.)) = 0;
virtual void debugDrawWorld() = 0;
virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies = false)
{
(void)constraint;
(void)disableCollisionsBetweenLinkedBodies;
}
virtual void removeConstraint(btTypedConstraint* constraint) { (void)constraint; }
virtual void addAction(btActionInterface* action) = 0;
virtual void removeAction(btActionInterface* action) = 0;
//once a rigidbody is added to the dynamics world, it will get this gravity assigned
//existing rigidbodies in the world get gravity assigned too, during this method
virtual void setGravity(const btVector3& gravity) = 0;
virtual btVector3 getGravity() const = 0;
virtual void synchronizeMotionStates() = 0;
virtual void addRigidBody(btRigidBody* body) = 0;
virtual void addRigidBody(btRigidBody* body, int group, int mask) = 0;
virtual void removeRigidBody(btRigidBody* body) = 0;
virtual void setConstraintSolver(btConstraintSolver* solver) = 0;
virtual btConstraintSolver* getConstraintSolver() = 0;
virtual int getNumConstraints() const { return 0; }
virtual btTypedConstraint* getConstraint(int index)
{
(void)index;
return 0;
}
virtual const btTypedConstraint* getConstraint(int index) const
{
(void)index;
return 0;
}
virtual btDynamicsWorldType getWorldType() const = 0;
virtual void clearForces() = 0;
/// Set the callback for when an internal tick (simulation substep) happens, optional user info
void setInternalTickCallback(btInternalTickCallback cb, void* worldUserInfo = 0, bool isPreTick = false)
{
if (isPreTick)
{
m_internalPreTickCallback = cb;
}
virtual ~btDynamicsWorld()
else
{
m_internalTickCallback = cb;
}
///stepSimulation proceeds the simulation over 'timeStep', units in preferably in seconds.
///By default, Bullet will subdivide the timestep in constant substeps of each 'fixedTimeStep'.
///in order to keep the simulation real-time, the maximum number of substeps can be clamped to 'maxSubSteps'.
///You can disable subdividing the timestep/substepping by passing maxSubSteps=0 as second argument to stepSimulation, but in that case you have to keep the timeStep constant.
virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0;
virtual void debugDrawWorld() = 0;
virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false)
{
(void)constraint; (void)disableCollisionsBetweenLinkedBodies;
}
m_worldUserInfo = worldUserInfo;
}
virtual void removeConstraint(btTypedConstraint* constraint) {(void)constraint;}
void setWorldUserInfo(void* worldUserInfo)
{
m_worldUserInfo = worldUserInfo;
}
virtual void addAction(btActionInterface* action) = 0;
void* getWorldUserInfo() const
{
return m_worldUserInfo;
}
virtual void removeAction(btActionInterface* action) = 0;
//once a rigidbody is added to the dynamics world, it will get this gravity assigned
//existing rigidbodies in the world get gravity assigned too, during this method
virtual void setGravity(const btVector3& gravity) = 0;
virtual btVector3 getGravity () const = 0;
virtual void synchronizeMotionStates() = 0;
virtual void addRigidBody(btRigidBody* body) = 0;
virtual void addRigidBody(btRigidBody* body, int group, int mask) = 0;
virtual void removeRigidBody(btRigidBody* body) = 0;
virtual void setConstraintSolver(btConstraintSolver* solver) = 0;
virtual btConstraintSolver* getConstraintSolver() = 0;
virtual int getNumConstraints() const { return 0; }
virtual btTypedConstraint* getConstraint(int index) { (void)index; return 0; }
virtual const btTypedConstraint* getConstraint(int index) const { (void)index; return 0; }
virtual btDynamicsWorldType getWorldType() const=0;
virtual void clearForces() = 0;
/// Set the callback for when an internal tick (simulation substep) happens, optional user info
void setInternalTickCallback(btInternalTickCallback cb, void* worldUserInfo=0,bool isPreTick=false)
{
if (isPreTick)
{
m_internalPreTickCallback = cb;
} else
{
m_internalTickCallback = cb;
}
m_worldUserInfo = worldUserInfo;
}
void setWorldUserInfo(void* worldUserInfo)
{
m_worldUserInfo = worldUserInfo;
}
void* getWorldUserInfo() const
{
return m_worldUserInfo;
}
btContactSolverInfo& getSolverInfo()
{
return m_solverInfo;
}
const btContactSolverInfo& getSolverInfo() const
{
return m_solverInfo;
}
///obsolete, use addAction instead.
virtual void addVehicle(btActionInterface* vehicle) {(void)vehicle;}
///obsolete, use removeAction instead
virtual void removeVehicle(btActionInterface* vehicle) {(void)vehicle;}
///obsolete, use addAction instead.
virtual void addCharacter(btActionInterface* character) {(void)character;}
///obsolete, use removeAction instead
virtual void removeCharacter(btActionInterface* character) {(void)character;}
btContactSolverInfo& getSolverInfo()
{
return m_solverInfo;
}
const btContactSolverInfo& getSolverInfo() const
{
return m_solverInfo;
}
///obsolete, use addAction instead.
virtual void addVehicle(btActionInterface* vehicle) { (void)vehicle; }
///obsolete, use removeAction instead
virtual void removeVehicle(btActionInterface* vehicle) { (void)vehicle; }
///obsolete, use addAction instead.
virtual void addCharacter(btActionInterface* character) { (void)character; }
///obsolete, use removeAction instead
virtual void removeCharacter(btActionInterface* character) { (void)character; }
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btDynamicsWorldDoubleData
{
btContactSolverInfoDoubleData m_solverInfo;
btVector3DoubleData m_gravity;
btContactSolverInfoDoubleData m_solverInfo;
btVector3DoubleData m_gravity;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btDynamicsWorldFloatData
{
btContactSolverInfoFloatData m_solverInfo;
btVector3FloatData m_gravity;
btContactSolverInfoFloatData m_solverInfo;
btVector3FloatData m_gravity;
};
#endif //BT_DYNAMICS_WORLD_H
#endif //BT_DYNAMICS_WORLD_H

263
src/BulletDynamics/Dynamics/btRigidBody.cpp
View File

@@ -22,36 +22,34 @@ subject to the following restrictions:
#include "LinearMath/btSerializer.h"
//'temporarily' global variables
btScalar gDeactivationTime = btScalar(2.);
bool gDisableDeactivation = false;
btScalar gDeactivationTime = btScalar(2.);
bool gDisableDeactivation = false;
static int uniqueId = 0;
btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
{
setupRigidBody(constructionInfo);
}
btRigidBody::btRigidBody(btScalar mass, btMotionState *motionState, btCollisionShape *collisionShape, const btVector3 &localInertia)
btRigidBody::btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia)
{
btRigidBodyConstructionInfo cinfo(mass,motionState,collisionShape,localInertia);
btRigidBodyConstructionInfo cinfo(mass, motionState, collisionShape, localInertia);
setupRigidBody(cinfo);
}
void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
{
m_internalType=CO_RIGID_BODY;
m_internalType = CO_RIGID_BODY;
m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
m_angularFactor.setValue(1,1,1);
m_linearFactor.setValue(1,1,1);
m_angularVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
m_angularFactor.setValue(1, 1, 1);
m_linearFactor.setValue(1, 1, 1);
m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping);
setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping);
m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold;
m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold;
@@ -67,48 +65,44 @@ void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo&
if (m_optionalMotionState)
{
m_optionalMotionState->getWorldTransform(m_worldTransform);
} else
}
else
{
m_worldTransform = constructionInfo.m_startWorldTransform;
}
m_interpolationWorldTransform = m_worldTransform;
m_interpolationLinearVelocity.setValue(0,0,0);
m_interpolationAngularVelocity.setValue(0,0,0);
m_interpolationLinearVelocity.setValue(0, 0, 0);
m_interpolationAngularVelocity.setValue(0, 0, 0);
//moved to btCollisionObject
m_friction = constructionInfo.m_friction;
m_rollingFriction = constructionInfo.m_rollingFriction;
m_spinningFriction = constructionInfo.m_spinningFriction;
m_spinningFriction = constructionInfo.m_spinningFriction;
m_restitution = constructionInfo.m_restitution;
setCollisionShape( constructionInfo.m_collisionShape );
setCollisionShape(constructionInfo.m_collisionShape);
m_debugBodyId = uniqueId++;
setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia);
updateInertiaTensor();
m_rigidbodyFlags = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY;
m_deltaLinearVelocity.setZero();
m_deltaAngularVelocity.setZero();
m_invMass = m_inverseMass*m_linearFactor;
m_invMass = m_inverseMass * m_linearFactor;
m_pushVelocity.setZero();
m_turnVelocity.setZero();
}
void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform)
void btRigidBody::predictIntegratedTransform(btScalar timeStep, btTransform& predictedTransform)
{
btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
btTransformUtil::integrateTransform(m_worldTransform, m_linearVelocity, m_angularVelocity, timeStep, predictedTransform);
}
void btRigidBody::saveKinematicState(btScalar timeStep)
void btRigidBody::saveKinematicState(btScalar timeStep)
{
//todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
if (timeStep != btScalar(0.))
@@ -116,25 +110,22 @@ void btRigidBody::saveKinematicState(btScalar timeStep)
//if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
if (getMotionState())
getMotionState()->getWorldTransform(m_worldTransform);
btVector3 linVel,angVel;
btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,timeStep,m_linearVelocity,m_angularVelocity);
btVector3 linVel, angVel;
btTransformUtil::calculateVelocity(m_interpolationWorldTransform, m_worldTransform, timeStep, m_linearVelocity, m_angularVelocity);
m_interpolationLinearVelocity = m_linearVelocity;
m_interpolationAngularVelocity = m_angularVelocity;
m_interpolationWorldTransform = m_worldTransform;
//printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ());
}
}
void btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const
void btRigidBody::getAabb(btVector3& aabbMin, btVector3& aabbMax) const
{
getCollisionShape()->getAabb(m_worldTransform,aabbMin,aabbMax);
getCollisionShape()->getAabb(m_worldTransform, aabbMin, aabbMax);
}
void btRigidBody::setGravity(const btVector3& acceleration)
void btRigidBody::setGravity(const btVector3& acceleration)
{
if (m_inverseMass != btScalar(0.0))
{
@@ -143,22 +134,14 @@ void btRigidBody::setGravity(const btVector3& acceleration)
m_gravity_acceleration = acceleration;
}
void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
{
m_linearDamping = btClamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
m_angularDamping = btClamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
}
///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping
void btRigidBody::applyDamping(btScalar timeStep)
void btRigidBody::applyDamping(btScalar timeStep)
{
//On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
//todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway
@@ -168,8 +151,8 @@ void btRigidBody::applyDamping(btScalar timeStep)
m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
#else
m_linearVelocity *= btPow(btScalar(1)-m_linearDamping, timeStep);
m_angularVelocity *= btPow(btScalar(1)-m_angularDamping, timeStep);
m_linearVelocity *= btPow(btScalar(1) - m_linearDamping, timeStep);
m_angularVelocity *= btPow(btScalar(1) - m_angularDamping, timeStep);
#endif
if (m_additionalDamping)
@@ -182,7 +165,6 @@ void btRigidBody::applyDamping(btScalar timeStep)
m_angularVelocity *= m_additionalDampingFactor;
m_linearVelocity *= m_additionalDampingFactor;
}
btScalar speed = m_linearVelocity.length();
if (speed < m_linearDamping)
@@ -191,10 +173,11 @@ void btRigidBody::applyDamping(btScalar timeStep)
if (speed > dampVel)
{
btVector3 dir = m_linearVelocity.normalized();
m_linearVelocity -= dir * dampVel;
} else
m_linearVelocity -= dir * dampVel;
}
else
{
m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
m_linearVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
}
}
@@ -205,30 +188,28 @@ void btRigidBody::applyDamping(btScalar timeStep)
if (angSpeed > angDampVel)
{
btVector3 dir = m_angularVelocity.normalized();
m_angularVelocity -= dir * angDampVel;
} else
m_angularVelocity -= dir * angDampVel;
}
else
{
m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
m_angularVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
}
}
}
}
void btRigidBody::applyGravity()
{
if (isStaticOrKinematicObject())
return;
applyCentralForce(m_gravity);
applyCentralForce(m_gravity);
}
void btRigidBody::proceedToTransform(const btTransform& newTrans)
{
setCenterOfMassTransform( newTrans );
setCenterOfMassTransform(newTrans);
}
void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
{
@@ -236,7 +217,8 @@ void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
{
m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT;
m_inverseMass = btScalar(0.);
} else
}
else
{
m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
m_inverseMass = btScalar(1.0) / mass;
@@ -244,50 +226,45 @@ void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
//Fg = m * a
m_gravity = mass * m_gravity_acceleration;
m_invInertiaLocal.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));
m_invMass = m_linearFactor*m_inverseMass;
m_invInertiaLocal.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));
m_invMass = m_linearFactor * m_inverseMass;
}
void btRigidBody::updateInertiaTensor()
void btRigidBody::updateInertiaTensor()
{
m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
}
btVector3 btRigidBody::getLocalInertia() const
{
btVector3 inertiaLocal;
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));
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 btMatrix3x3& I)
{
const btVector3 w2 = I*w1 + w1.cross(I*w1)*dt - (T*dt + I*w0);
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)
const btMatrix3x3& I)
{
btMatrix3x3 w1x, Iw1x;
const btVector3 Iwi = (I*w1);
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;
const btMatrix3x3 dfw1 = I + (w1x * I - Iw1x) * dt;
return dfw1;
}
@@ -295,58 +272,55 @@ btVector3 btRigidBody::computeGyroscopicForceExplicit(btScalar maxGyroscopicForc
{
btVector3 inertiaLocal = getLocalInertia();
btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose();
btVector3 tmp = inertiaTensorWorld*getAngularVelocity();
btVector3 tmp = inertiaTensorWorld * getAngularVelocity();
btVector3 gf = getAngularVelocity().cross(tmp);
btScalar l2 = gf.length2();
if (l2>maxGyroscopicForce*maxGyroscopicForce)
if (l2 > maxGyroscopicForce * maxGyroscopicForce)
{
gf *= btScalar(1.)/btSqrt(l2)*maxGyroscopicForce;
gf *= btScalar(1.) / btSqrt(l2) * maxGyroscopicForce;
}
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;
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;
btVector3 om = Ib * omegab;
btMatrix3x3 skew1;
om.getSkewSymmetricMatrix(&skew1[0],&skew1[1],&skew1[2]);
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;
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);
omegab = omegab - omega_div; //Solve33(J, f);
// Back to world coordinates
btVector3 omega2 = quatRotate(q,omegab);
btVector3 gf = omega2-omega1;
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.
@@ -361,7 +335,7 @@ btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) con
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
// f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0
// df/dw' = I + 1xIw'*step + w'xI*step
btVector3 w1 = w0;
@@ -383,8 +357,7 @@ btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) con
return gf;
}
void btRigidBody::integrateVelocities(btScalar step)
void btRigidBody::integrateVelocities(btScalar step)
{
if (isStaticOrKinematicObject())
return;
@@ -393,30 +366,28 @@ void btRigidBody::integrateVelocities(btScalar step)
m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
#define MAX_ANGVEL SIMD_HALF_PI
/// clamp angular velocity. collision calculations will fail on higher angular velocities
/// clamp angular velocity. collision calculations will fail on higher angular velocities
btScalar angvel = m_angularVelocity.length();
if (angvel*step > MAX_ANGVEL)
if (angvel * step > MAX_ANGVEL)
{
m_angularVelocity *= (MAX_ANGVEL/step) /angvel;
m_angularVelocity *= (MAX_ANGVEL / step) / angvel;
}
}
btQuaternion btRigidBody::getOrientation() const
{
btQuaternion orn;
m_worldTransform.getBasis().getRotation(orn);
return orn;
btQuaternion orn;
m_worldTransform.getBasis().getRotation(orn);
return orn;
}
void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
{
if (isKinematicObject())
{
m_interpolationWorldTransform = m_worldTransform;
} else
}
else
{
m_interpolationWorldTransform = xform;
}
@@ -426,10 +397,6 @@ void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
updateInertiaTensor();
}
void btRigidBody::addConstraintRef(btTypedConstraint* c)
{
///disable collision with the 'other' body
@@ -450,39 +417,39 @@ void btRigidBody::addConstraintRef(btTypedConstraint* c)
{
colObjB->setIgnoreCollisionCheck(colObjA, true);
}
}
}
}
void btRigidBody::removeConstraintRef(btTypedConstraint* c)
{
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);
}
}
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
int btRigidBody::calculateSerializeBufferSize() const
{
int sz = sizeof(btRigidBodyData);
return sz;
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const
///fills the dataBuffer and returns the struct name (and 0 on failure)
const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const
{
btRigidBodyData* rbd = (btRigidBodyData*) dataBuffer;
btRigidBodyData* rbd = (btRigidBodyData*)dataBuffer;
btCollisionObject::serialize(&rbd->m_collisionObjectData, serializer);
@@ -504,7 +471,7 @@ const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* seriali
rbd->m_additionalLinearDampingThresholdSqr = m_additionalLinearDampingThresholdSqr;
rbd->m_additionalAngularDampingThresholdSqr = m_additionalAngularDampingThresholdSqr;
rbd->m_additionalAngularDampingFactor = m_additionalAngularDampingFactor;
rbd->m_linearSleepingThreshold=m_linearSleepingThreshold;
rbd->m_linearSleepingThreshold = m_linearSleepingThreshold;
rbd->m_angularSleepingThreshold = m_angularSleepingThreshold;
// Fill padding with zeros to appease msan.
@@ -515,13 +482,9 @@ const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* seriali
return btRigidBodyDataName;
}
void btRigidBody::serializeSingleObject(class btSerializer* serializer) const
{
btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(),1);
btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(), 1);
const char* structType = serialize(chunk->m_oldPtr, serializer);
serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,(void*)this);
serializer->finalizeChunk(chunk, structType, BT_RIGIDBODY_CODE, (void*)this);
}

502
src/BulletDynamics/Dynamics/btRigidBody.h
View File

@@ -25,209 +25,195 @@ class btCollisionShape;
class btMotionState;
class btTypedConstraint;
extern btScalar gDeactivationTime;
extern bool gDisableDeactivation;
#ifdef BT_USE_DOUBLE_PRECISION
#define btRigidBodyData btRigidBodyDoubleData
#define btRigidBodyDataName "btRigidBodyDoubleData"
#define btRigidBodyData btRigidBodyDoubleData
#define btRigidBodyDataName "btRigidBodyDoubleData"
#else
#define btRigidBodyData btRigidBodyFloatData
#define btRigidBodyDataName "btRigidBodyFloatData"
#endif //BT_USE_DOUBLE_PRECISION
#define btRigidBodyData btRigidBodyFloatData
#define btRigidBodyDataName "btRigidBodyFloatData"
#endif //BT_USE_DOUBLE_PRECISION
enum btRigidBodyFlags
enum btRigidBodyFlags
{
BT_DISABLE_WORLD_GRAVITY = 1,
///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_GYROSCOPIC_FORCE_IMPLICIT_WORLD = 4,
BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY = 8,
BT_ENABLE_GYROPSCOPIC_FORCE = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY,
};
///The btRigidBody is the main class for rigid body objects. It is derived from btCollisionObject, so it keeps a pointer to a btCollisionShape.
///It is recommended for performance and memory use to share btCollisionShape objects whenever possible.
///There are 3 types of rigid bodies:
///There are 3 types of rigid bodies:
///- A) Dynamic rigid bodies, with positive mass. Motion is controlled by rigid body dynamics.
///- B) Fixed objects with zero mass. They are not moving (basically collision objects)
///- C) Kinematic objects, which are objects without mass, but the user can move them. There is one-way interaction, and Bullet calculates a velocity based on the timestep and previous and current world transform.
///Bullet automatically deactivates dynamic rigid bodies, when the velocity is below a threshold for a given time.
///Deactivated (sleeping) rigid bodies don't take any processing time, except a minor broadphase collision detection impact (to allow active objects to activate/wake up sleeping objects)
class btRigidBody : public btCollisionObject
class btRigidBody : public btCollisionObject
{
btMatrix3x3 m_invInertiaTensorWorld;
btVector3 m_linearVelocity;
btVector3 m_angularVelocity;
btScalar m_inverseMass;
btVector3 m_linearFactor;
btMatrix3x3 m_invInertiaTensorWorld;
btVector3 m_linearVelocity;
btVector3 m_angularVelocity;
btScalar m_inverseMass;
btVector3 m_linearFactor;
btVector3 m_gravity;
btVector3 m_gravity_acceleration;
btVector3 m_invInertiaLocal;
btVector3 m_totalForce;
btVector3 m_totalTorque;
btVector3 m_gravity;
btVector3 m_gravity_acceleration;
btVector3 m_invInertiaLocal;
btVector3 m_totalForce;
btVector3 m_totalTorque;
btScalar m_linearDamping;
btScalar m_angularDamping;
btScalar m_linearDamping;
btScalar m_angularDamping;
bool m_additionalDamping;
btScalar m_additionalDampingFactor;
btScalar m_additionalLinearDampingThresholdSqr;
btScalar m_additionalAngularDampingThresholdSqr;
btScalar m_additionalAngularDampingFactor;
bool m_additionalDamping;
btScalar m_additionalDampingFactor;
btScalar m_additionalLinearDampingThresholdSqr;
btScalar m_additionalAngularDampingThresholdSqr;
btScalar m_additionalAngularDampingFactor;
btScalar m_linearSleepingThreshold;
btScalar m_angularSleepingThreshold;
btScalar m_linearSleepingThreshold;
btScalar m_angularSleepingThreshold;
//m_optionalMotionState allows to automatic synchronize the world transform for active objects
btMotionState* m_optionalMotionState;
btMotionState* m_optionalMotionState;
//keep track of typed constraints referencing this rigid body, to disable collision between linked bodies
btAlignedObjectArray<btTypedConstraint*> m_constraintRefs;
int m_rigidbodyFlags;
int m_debugBodyId;
int m_rigidbodyFlags;
int m_debugBodyId;
protected:
ATTRIBUTE_ALIGNED16(btVector3 m_deltaLinearVelocity);
btVector3 m_deltaAngularVelocity;
btVector3 m_angularFactor;
btVector3 m_invMass;
btVector3 m_pushVelocity;
btVector3 m_turnVelocity;
ATTRIBUTE_ALIGNED16(btVector3 m_deltaLinearVelocity);
btVector3 m_deltaAngularVelocity;
btVector3 m_angularFactor;
btVector3 m_invMass;
btVector3 m_pushVelocity;
btVector3 m_turnVelocity;
public:
///The btRigidBodyConstructionInfo structure provides information to create a rigid body. Setting mass to zero creates a fixed (non-dynamic) rigid body.
///For dynamic objects, you can use the collision shape to approximate the local inertia tensor, otherwise use the zero vector (default argument)
///You can use the motion state to synchronize the world transform between physics and graphics objects.
///You can use the motion state to synchronize the world transform between physics and graphics objects.
///And if the motion state is provided, the rigid body will initialize its initial world transform from the motion state,
///m_startWorldTransform is only used when you don't provide a motion state.
struct btRigidBodyConstructionInfo
struct btRigidBodyConstructionInfo
{
btScalar m_mass;
btScalar m_mass;
///When a motionState is provided, the rigid body will initialize its world transform from the motion state
///In this case, m_startWorldTransform is ignored.
btMotionState* m_motionState;
btTransform m_startWorldTransform;
btMotionState* m_motionState;
btTransform m_startWorldTransform;
btCollisionShape* m_collisionShape;
btVector3 m_localInertia;
btScalar m_linearDamping;
btScalar m_angularDamping;
btCollisionShape* m_collisionShape;
btVector3 m_localInertia;
btScalar m_linearDamping;
btScalar m_angularDamping;
///best simulation results when friction is non-zero
btScalar m_friction;
btScalar m_friction;
///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;
btScalar m_rollingFriction;
btScalar m_spinningFriction; //torsional friction around contact normal
btScalar m_linearSleepingThreshold;
btScalar m_angularSleepingThreshold;
///best simulation results using zero restitution.
btScalar m_restitution;
btScalar m_linearSleepingThreshold;
btScalar m_angularSleepingThreshold;
//Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
//Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
bool m_additionalDamping;
btScalar m_additionalDampingFactor;
btScalar m_additionalLinearDampingThresholdSqr;
btScalar m_additionalAngularDampingThresholdSqr;
btScalar m_additionalAngularDampingFactor;
bool m_additionalDamping;
btScalar m_additionalDampingFactor;
btScalar m_additionalLinearDampingThresholdSqr;
btScalar m_additionalAngularDampingThresholdSqr;
btScalar m_additionalAngularDampingFactor;
btRigidBodyConstructionInfo( btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)):
m_mass(mass),
m_motionState(motionState),
m_collisionShape(collisionShape),
m_localInertia(localInertia),
m_linearDamping(btScalar(0.)),
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)),
m_additionalDamping(false),
m_additionalDampingFactor(btScalar(0.005)),
m_additionalLinearDampingThresholdSqr(btScalar(0.01)),
m_additionalAngularDampingThresholdSqr(btScalar(0.01)),
m_additionalAngularDampingFactor(btScalar(0.01))
btRigidBodyConstructionInfo(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia = btVector3(0, 0, 0)) : m_mass(mass),
m_motionState(motionState),
m_collisionShape(collisionShape),
m_localInertia(localInertia),
m_linearDamping(btScalar(0.)),
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)),
m_additionalDamping(false),
m_additionalDampingFactor(btScalar(0.005)),
m_additionalLinearDampingThresholdSqr(btScalar(0.01)),
m_additionalAngularDampingThresholdSqr(btScalar(0.01)),
m_additionalAngularDampingFactor(btScalar(0.01))
{
m_startWorldTransform.setIdentity();
}
};
///btRigidBody constructor using construction info
btRigidBody( const btRigidBodyConstructionInfo& constructionInfo);
btRigidBody(const btRigidBodyConstructionInfo& constructionInfo);
///btRigidBody constructor for backwards compatibility.
///btRigidBody constructor for backwards compatibility.
///To specify friction (etc) during rigid body construction, please use the other constructor (using btRigidBodyConstructionInfo)
btRigidBody( btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0));
btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia = btVector3(0, 0, 0));
virtual ~btRigidBody()
{
//No constraints should point to this rigidbody
//Remove constraints from the dynamics world before you delete the related rigidbodies.
btAssert(m_constraintRefs.size()==0);
}
{
//No constraints should point to this rigidbody
//Remove constraints from the dynamics world before you delete the related rigidbodies.
btAssert(m_constraintRefs.size() == 0);
}
protected:
///setupRigidBody is only used internally by the constructor
void setupRigidBody(const btRigidBodyConstructionInfo& constructionInfo);
void setupRigidBody(const btRigidBodyConstructionInfo& constructionInfo);
public:
void proceedToTransform(const btTransform& newTrans);
void proceedToTransform(const btTransform& newTrans);
///to keep collision detection and dynamics separate we don't store a rigidbody pointer
///but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast
static const btRigidBody* upcast(const btCollisionObject* colObj)
static const btRigidBody* upcast(const btCollisionObject* colObj)
{
if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY)
if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY)
return (const btRigidBody*)colObj;
return 0;
}
static btRigidBody* upcast(btCollisionObject* colObj)
static btRigidBody* upcast(btCollisionObject* colObj)
{
if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY)
if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY)
return (btRigidBody*)colObj;
return 0;
}
/// continuous collision detection needs prediction
void predictIntegratedTransform(btScalar step, btTransform& predictedTransform) ;
void saveKinematicState(btScalar step);
void applyGravity();
void setGravity(const btVector3& acceleration);
const btVector3& getGravity() const
/// continuous collision detection needs prediction
void predictIntegratedTransform(btScalar step, btTransform& predictedTransform);
void saveKinematicState(btScalar step);
void applyGravity();
void setGravity(const btVector3& acceleration);
const btVector3& getGravity() const
{
return m_gravity_acceleration;
}
void setDamping(btScalar lin_damping, btScalar ang_damping);
void setDamping(btScalar lin_damping, btScalar ang_damping);
btScalar getLinearDamping() const
{
@@ -249,18 +235,20 @@ public:
return m_angularSleepingThreshold;
}
void applyDamping(btScalar timeStep);
void applyDamping(btScalar timeStep);
SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const {
SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const
{
return m_collisionShape;
}
SIMD_FORCE_INLINE btCollisionShape* getCollisionShape() {
return m_collisionShape;
SIMD_FORCE_INLINE btCollisionShape* getCollisionShape()
{
return m_collisionShape;
}
void setMassProps(btScalar mass, const btVector3& inertia);
void setMassProps(btScalar mass, const btVector3& inertia);
const btVector3& getLinearFactor() const
{
return m_linearFactor;
@@ -268,20 +256,21 @@ public:
void setLinearFactor(const btVector3& linearFactor)
{
m_linearFactor = linearFactor;
m_invMass = m_linearFactor*m_inverseMass;
m_invMass = m_linearFactor * m_inverseMass;
}
btScalar getInvMass() const { return m_inverseMass; }
const btMatrix3x3& getInvInertiaTensorWorld() const {
return m_invInertiaTensorWorld;
}
void integrateVelocities(btScalar step);
void setCenterOfMassTransform(const btTransform& xform);
void applyCentralForce(const btVector3& force)
btScalar getInvMass() const { return m_inverseMass; }
const btMatrix3x3& getInvInertiaTensorWorld() const
{
m_totalForce += force*m_linearFactor;
return m_invInertiaTensorWorld;
}
void integrateVelocities(btScalar step);
void setCenterOfMassTransform(const btTransform& xform);
void applyCentralForce(const btVector3& force)
{
m_totalForce += force * m_linearFactor;
}
const btVector3& getTotalForce() const
@@ -293,90 +282,93 @@ public:
{
return m_totalTorque;
};
const btVector3& getInvInertiaDiagLocal() const
{
return m_invInertiaLocal;
};
void setInvInertiaDiagLocal(const btVector3& diagInvInertia)
void setInvInertiaDiagLocal(const btVector3& diagInvInertia)
{
m_invInertiaLocal = diagInvInertia;
}
void setSleepingThresholds(btScalar linear,btScalar angular)
void setSleepingThresholds(btScalar linear, btScalar angular)
{
m_linearSleepingThreshold = linear;
m_angularSleepingThreshold = angular;
}
void applyTorque(const btVector3& torque)
void applyTorque(const btVector3& torque)
{
m_totalTorque += torque*m_angularFactor;
m_totalTorque += torque * m_angularFactor;
}
void applyForce(const btVector3& force, const btVector3& rel_pos)
void applyForce(const btVector3& force, const btVector3& rel_pos)
{
applyCentralForce(force);
applyTorque(rel_pos.cross(force*m_linearFactor));
applyTorque(rel_pos.cross(force * m_linearFactor));
}
void applyCentralImpulse(const btVector3& impulse)
{
m_linearVelocity += impulse *m_linearFactor * m_inverseMass;
m_linearVelocity += impulse * m_linearFactor * m_inverseMass;
}
void applyTorqueImpulse(const btVector3& torque)
void applyTorqueImpulse(const btVector3& torque)
{
m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
}
void applyImpulse(const btVector3& impulse, const btVector3& rel_pos)
void applyImpulse(const btVector3& impulse, const btVector3& rel_pos)
{
if (m_inverseMass != btScalar(0.))
{
applyCentralImpulse(impulse);
if (m_angularFactor)
{
applyTorqueImpulse(rel_pos.cross(impulse*m_linearFactor));
applyTorqueImpulse(rel_pos.cross(impulse * m_linearFactor));
}
}
}
void clearForces()
void clearForces()
{
m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
}
void updateInertiaTensor();
const btVector3& getCenterOfMassPosition() const {
return m_worldTransform.getOrigin();
void updateInertiaTensor();
const btVector3& getCenterOfMassPosition() const
{
return m_worldTransform.getOrigin();
}
btQuaternion getOrientation() const;
const btTransform& getCenterOfMassTransform() const {
return m_worldTransform;
const btTransform& getCenterOfMassTransform() const
{
return m_worldTransform;
}
const btVector3& getLinearVelocity() const {
return m_linearVelocity;
const btVector3& getLinearVelocity() const
{
return m_linearVelocity;
}
const btVector3& getAngularVelocity() const {
return m_angularVelocity;
const btVector3& getAngularVelocity() const
{
return m_angularVelocity;
}
inline void setLinearVelocity(const btVector3& lin_vel)
{
{
m_updateRevision++;
m_linearVelocity = lin_vel;
m_linearVelocity = lin_vel;
}
inline void setAngularVelocity(const btVector3& ang_vel)
{
inline void setAngularVelocity(const btVector3& ang_vel)
{
m_updateRevision++;
m_angularVelocity = ang_vel;
m_angularVelocity = ang_vel;
}
btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const
@@ -388,18 +380,13 @@ public:
// return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep;
}
void translate(const btVector3& v)
void translate(const btVector3& v)
{
m_worldTransform.getOrigin() += v;
m_worldTransform.getOrigin() += v;
}
void getAabb(btVector3& aabbMin,btVector3& aabbMax) const;
void getAabb(btVector3& aabbMin, btVector3& aabbMax) const;
SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btVector3& pos, const btVector3& normal) const
{
btVector3 r0 = pos - getCenterOfMassPosition();
@@ -409,7 +396,6 @@ public:
btVector3 vec = (c0 * getInvInertiaTensorWorld()).cross(r0);
return m_inverseMass + normal.dot(vec);
}
SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis) const
@@ -418,26 +404,25 @@ public:
return axis.dot(vec);
}
SIMD_FORCE_INLINE void updateDeactivation(btScalar timeStep)
SIMD_FORCE_INLINE void updateDeactivation(btScalar timeStep)
{
if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
return;
if ((getLinearVelocity().length2() < m_linearSleepingThreshold*m_linearSleepingThreshold) &&
(getAngularVelocity().length2() < m_angularSleepingThreshold*m_angularSleepingThreshold))
if ((getLinearVelocity().length2() < m_linearSleepingThreshold * m_linearSleepingThreshold) &&
(getAngularVelocity().length2() < m_angularSleepingThreshold * m_angularSleepingThreshold))
{
m_deactivationTime += timeStep;
} else
}
else
{
m_deactivationTime=btScalar(0.);
m_deactivationTime = btScalar(0.);
setActivationState(0);
}
}
SIMD_FORCE_INLINE bool wantsSleeping()
SIMD_FORCE_INLINE bool wantsSleeping()
{
if (getActivationState() == DISABLE_DEACTIVATION)
return false;
@@ -445,41 +430,39 @@ public:
if (gDisableDeactivation || (gDeactivationTime == btScalar(0.)))
return false;
if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION))
if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION))
return true;
if (m_deactivationTime> gDeactivationTime)
if (m_deactivationTime > gDeactivationTime)
{
return true;
}
return false;
}
const btBroadphaseProxy* getBroadphaseProxy() const
const btBroadphaseProxy* getBroadphaseProxy() const
{
return m_broadphaseHandle;
}
btBroadphaseProxy* getBroadphaseProxy()
btBroadphaseProxy* getBroadphaseProxy()
{
return m_broadphaseHandle;
}
void setNewBroadphaseProxy(btBroadphaseProxy* broadphaseProxy)
void setNewBroadphaseProxy(btBroadphaseProxy* broadphaseProxy)
{
m_broadphaseHandle = broadphaseProxy;
}
//btMotionState allows to automatic synchronize the world transform for active objects
btMotionState* getMotionState()
btMotionState* getMotionState()
{
return m_optionalMotionState;
}
const btMotionState* getMotionState() const
const btMotionState* getMotionState() const
{
return m_optionalMotionState;
}
void setMotionState(btMotionState* motionState)
void setMotionState(btMotionState* motionState)
{
m_optionalMotionState = motionState;
if (m_optionalMotionState)
@@ -487,27 +470,27 @@ public:
}
//for experimental overriding of friction/contact solver func
int m_contactSolverType;
int m_frictionSolverType;
int m_contactSolverType;
int m_frictionSolverType;
void setAngularFactor(const btVector3& angFac)
void setAngularFactor(const btVector3& angFac)
{
m_updateRevision++;
m_angularFactor = angFac;
}
void setAngularFactor(btScalar angFac)
void setAngularFactor(btScalar angFac)
{
m_updateRevision++;
m_angularFactor.setValue(angFac,angFac,angFac);
m_angularFactor.setValue(angFac, angFac, angFac);
}
const btVector3& getAngularFactor() const
const btVector3& getAngularFactor() const
{
return m_angularFactor;
}
//is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase?
bool isInWorld() const
bool isInWorld() const
{
return (getBroadphaseProxy() != 0);
}
@@ -525,7 +508,7 @@ public:
return m_constraintRefs.size();
}
void setFlags(int flags)
void setFlags(int flags)
{
m_rigidbodyFlags = flags;
}
@@ -535,12 +518,9 @@ public:
return m_rigidbodyFlags;
}
///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;
@@ -550,70 +530,66 @@ public:
///////////////////////////////////////////////
virtual int calculateSerializeBufferSize() const;
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;
virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
virtual void serializeSingleObject(class btSerializer* serializer) const;
};
//@todo add m_optionalMotionState and m_constraintRefs to btRigidBodyData
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btRigidBodyFloatData
struct btRigidBodyFloatData
{
btCollisionObjectFloatData m_collisionObjectData;
btMatrix3x3FloatData m_invInertiaTensorWorld;
btVector3FloatData m_linearVelocity;
btVector3FloatData m_angularVelocity;
btVector3FloatData m_angularFactor;
btVector3FloatData m_linearFactor;
btVector3FloatData m_gravity;
btVector3FloatData m_gravity_acceleration;
btVector3FloatData m_invInertiaLocal;
btVector3FloatData m_totalForce;
btVector3FloatData m_totalTorque;
float m_inverseMass;
float m_linearDamping;
float m_angularDamping;
float m_additionalDampingFactor;
float m_additionalLinearDampingThresholdSqr;
float m_additionalAngularDampingThresholdSqr;
float m_additionalAngularDampingFactor;
float m_linearSleepingThreshold;
float m_angularSleepingThreshold;
int m_additionalDamping;
btCollisionObjectFloatData m_collisionObjectData;
btMatrix3x3FloatData m_invInertiaTensorWorld;
btVector3FloatData m_linearVelocity;
btVector3FloatData m_angularVelocity;
btVector3FloatData m_angularFactor;
btVector3FloatData m_linearFactor;
btVector3FloatData m_gravity;
btVector3FloatData m_gravity_acceleration;
btVector3FloatData m_invInertiaLocal;
btVector3FloatData m_totalForce;
btVector3FloatData m_totalTorque;
float m_inverseMass;
float m_linearDamping;
float m_angularDamping;
float m_additionalDampingFactor;
float m_additionalLinearDampingThresholdSqr;
float m_additionalAngularDampingThresholdSqr;
float m_additionalAngularDampingFactor;
float m_linearSleepingThreshold;
float m_angularSleepingThreshold;
int m_additionalDamping;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btRigidBodyDoubleData
struct btRigidBodyDoubleData
{
btCollisionObjectDoubleData m_collisionObjectData;
btMatrix3x3DoubleData m_invInertiaTensorWorld;
btVector3DoubleData m_linearVelocity;
btVector3DoubleData m_angularVelocity;
btVector3DoubleData m_angularFactor;
btVector3DoubleData m_linearFactor;
btVector3DoubleData m_gravity;
btVector3DoubleData m_gravity_acceleration;
btVector3DoubleData m_invInertiaLocal;
btVector3DoubleData m_totalForce;
btVector3DoubleData m_totalTorque;
double m_inverseMass;
double m_linearDamping;
double m_angularDamping;
double m_additionalDampingFactor;
double m_additionalLinearDampingThresholdSqr;
double m_additionalAngularDampingThresholdSqr;
double m_additionalAngularDampingFactor;
double m_linearSleepingThreshold;
double m_angularSleepingThreshold;
int m_additionalDamping;
char m_padding[4];
btCollisionObjectDoubleData m_collisionObjectData;
btMatrix3x3DoubleData m_invInertiaTensorWorld;
btVector3DoubleData m_linearVelocity;
btVector3DoubleData m_angularVelocity;
btVector3DoubleData m_angularFactor;
btVector3DoubleData m_linearFactor;
btVector3DoubleData m_gravity;
btVector3DoubleData m_gravity_acceleration;
btVector3DoubleData m_invInertiaLocal;
btVector3DoubleData m_totalForce;
btVector3DoubleData m_totalTorque;
double m_inverseMass;
double m_linearDamping;
double m_angularDamping;
double m_additionalDampingFactor;
double m_additionalLinearDampingThresholdSqr;
double m_additionalAngularDampingThresholdSqr;
double m_additionalAngularDampingFactor;
double m_linearSleepingThreshold;
double m_angularSleepingThreshold;
int m_additionalDamping;
char m_padding[4];
};
#endif //BT_RIGIDBODY_H
#endif //BT_RIGIDBODY_H

116
src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
View File

@@ -21,47 +21,40 @@ subject to the following restrictions:
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
/*
Make sure this dummy function never changes so that it
can be used by probes that are checking whether the
library is actually installed.
*/
extern "C"
extern "C"
{
void btBulletDynamicsProbe ();
void btBulletDynamicsProbe () {}
void btBulletDynamicsProbe();
void btBulletDynamicsProbe() {}
}
btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration),
m_constraintSolver(constraintSolver),
m_ownsConstraintSolver(false),
m_gravity(0,0,-10)
btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
: btDynamicsWorld(dispatcher, pairCache, collisionConfiguration),
m_constraintSolver(constraintSolver),
m_ownsConstraintSolver(false),
m_gravity(0, 0, -10)
{
}
btSimpleDynamicsWorld::~btSimpleDynamicsWorld()
{
if (m_ownsConstraintSolver)
btAlignedFree( m_constraintSolver);
btAlignedFree(m_constraintSolver);
}
int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
int btSimpleDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep)
{
(void)fixedTimeStep;
(void)maxSubSteps;
///apply gravity, predict motion
predictUnconstraintMotion(timeStep);
btDispatcherInfo& dispatchInfo = getDispatchInfo();
btDispatcherInfo& dispatchInfo = getDispatchInfo();
dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0;
dispatchInfo.m_debugDraw = getDebugDrawer();
@@ -74,17 +67,17 @@ int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b
if (numManifolds)
{
btPersistentManifold** manifoldPtr = ((btCollisionDispatcher*)m_dispatcher1)->getInternalManifoldPointer();
btContactSolverInfo infoGlobal;
infoGlobal.m_timeStep = timeStep;
m_constraintSolver->prepareSolve(0,numManifolds);
m_constraintSolver->solveGroup(&getCollisionObjectArray()[0],getNumCollisionObjects(),manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_dispatcher1);
m_constraintSolver->allSolved(infoGlobal,m_debugDrawer);
m_constraintSolver->prepareSolve(0, numManifolds);
m_constraintSolver->solveGroup(&getCollisionObjectArray()[0], getNumCollisionObjects(), manifoldPtr, numManifolds, 0, 0, infoGlobal, m_debugDrawer, m_dispatcher1);
m_constraintSolver->allSolved(infoGlobal, m_debugDrawer);
}
///integrate transforms
integrateTransforms(timeStep);
updateAabbs();
synchronizeMotionStates();
@@ -92,29 +85,27 @@ int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b
clearForces();
return 1;
}
void btSimpleDynamicsWorld::clearForces()
void btSimpleDynamicsWorld::clearForces()
{
///@todo: iterate over awake simulation islands!
for ( int i=0;i<m_collisionObjects.size();i++)
for (int i = 0; i < m_collisionObjects.size(); i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
btRigidBody* body = btRigidBody::upcast(colObj);
if (body)
{
body->clearForces();
}
}
}
}
void btSimpleDynamicsWorld::setGravity(const btVector3& gravity)
void btSimpleDynamicsWorld::setGravity(const btVector3& gravity)
{
m_gravity = gravity;
for ( int i=0;i<m_collisionObjects.size();i++)
for (int i = 0; i < m_collisionObjects.size(); i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
btRigidBody* body = btRigidBody::upcast(colObj);
@@ -125,17 +116,17 @@ void btSimpleDynamicsWorld::setGravity(const btVector3& gravity)
}
}
btVector3 btSimpleDynamicsWorld::getGravity () const
btVector3 btSimpleDynamicsWorld::getGravity() const
{
return m_gravity;
}
void btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body)
void btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body)
{
btCollisionWorld::removeCollisionObject(body);
}
void btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
void btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
{
btRigidBody* body = btRigidBody::upcast(collisionObject);
if (body)
@@ -144,8 +135,7 @@ void btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionOb
btCollisionWorld::removeCollisionObject(collisionObject);
}
void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)
void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)
{
body->setGravity(m_gravity);
@@ -155,37 +145,32 @@ void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)
}
}
void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mask)
void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mask)
{
body->setGravity(m_gravity);
if (body->getCollisionShape())
{
addCollisionObject(body,group,mask);
addCollisionObject(body, group, mask);
}
}
void btSimpleDynamicsWorld::debugDrawWorld()
void btSimpleDynamicsWorld::debugDrawWorld()
{
}
void btSimpleDynamicsWorld::addAction(btActionInterface* action)
{
}
void btSimpleDynamicsWorld::removeAction(btActionInterface* action)
void btSimpleDynamicsWorld::addAction(btActionInterface* action)
{
}
void btSimpleDynamicsWorld::removeAction(btActionInterface* action)
{
}
void btSimpleDynamicsWorld::updateAabbs()
void btSimpleDynamicsWorld::updateAabbs()
{
btTransform predictedTrans;
for ( int i=0;i<m_collisionObjects.size();i++)
for (int i = 0; i < m_collisionObjects.size(); i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
btRigidBody* body = btRigidBody::upcast(colObj);
@@ -193,19 +178,19 @@ void btSimpleDynamicsWorld::updateAabbs()
{
if (body->isActive() && (!body->isStaticObject()))
{
btVector3 minAabb,maxAabb;
colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
btVector3 minAabb, maxAabb;
colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb, maxAabb);
btBroadphaseInterface* bp = getBroadphase();
bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
bp->setAabb(body->getBroadphaseHandle(), minAabb, maxAabb, m_dispatcher1);
}
}
}
}
void btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep)
void btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep)
{
btTransform predictedTrans;
for ( int i=0;i<m_collisionObjects.size();i++)
for (int i = 0; i < m_collisionObjects.size(); i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
btRigidBody* body = btRigidBody::upcast(colObj);
@@ -214,17 +199,15 @@ void btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep)
if (body->isActive() && (!body->isStaticObject()))
{
body->predictIntegratedTransform(timeStep, predictedTrans);
body->proceedToTransform( predictedTrans);
body->proceedToTransform(predictedTrans);
}
}
}
}
void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
{
for ( int i=0;i<m_collisionObjects.size();i++)
for (int i = 0; i < m_collisionObjects.size(); i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
btRigidBody* body = btRigidBody::upcast(colObj);
@@ -235,20 +218,19 @@ void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
if (body->isActive())
{
body->applyGravity();
body->integrateVelocities( timeStep);
body->integrateVelocities(timeStep);
body->applyDamping(timeStep);
body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
body->predictIntegratedTransform(timeStep, body->getInterpolationWorldTransform());
}
}
}
}
}
void btSimpleDynamicsWorld::synchronizeMotionStates()
void btSimpleDynamicsWorld::synchronizeMotionStates()
{
///@todo: iterate over awake simulation islands!
for ( int i=0;i<m_collisionObjects.size();i++)
for (int i = 0; i < m_collisionObjects.size(); i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
btRigidBody* body = btRigidBody::upcast(colObj);
@@ -260,11 +242,9 @@ void btSimpleDynamicsWorld::synchronizeMotionStates()
}
}
}
}
void btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
void btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
{
if (m_ownsConstraintSolver)
{

59
src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
View File

@@ -27,63 +27,58 @@ class btConstraintSolver;
class btSimpleDynamicsWorld : public btDynamicsWorld
{
protected:
btConstraintSolver* m_constraintSolver;
btConstraintSolver* m_constraintSolver;
bool m_ownsConstraintSolver;
bool m_ownsConstraintSolver;
void predictUnconstraintMotion(btScalar timeStep);
void integrateTransforms(btScalar timeStep);
btVector3 m_gravity;
void predictUnconstraintMotion(btScalar timeStep);
void integrateTransforms(btScalar timeStep);
btVector3 m_gravity;
public:
///this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver
btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
btSimpleDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration);
virtual ~btSimpleDynamicsWorld();
///maxSubSteps/fixedTimeStep for interpolation is currently ignored for btSimpleDynamicsWorld, use btDiscreteDynamicsWorld instead
virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.));
virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.));
virtual void setGravity(const btVector3& gravity);
virtual void setGravity(const btVector3& gravity);
virtual btVector3 getGravity () const;
virtual btVector3 getGravity() const;
virtual void addRigidBody(btRigidBody* body);
virtual void addRigidBody(btRigidBody* body);
virtual void addRigidBody(btRigidBody* body, int group, int mask);
virtual void addRigidBody(btRigidBody* body, int group, int mask);
virtual void removeRigidBody(btRigidBody* body);
virtual void removeRigidBody(btRigidBody* body);
virtual void debugDrawWorld();
virtual void addAction(btActionInterface* action);
virtual void debugDrawWorld();
virtual void removeAction(btActionInterface* action);
virtual void addAction(btActionInterface* action);
virtual void removeAction(btActionInterface* action);
///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
virtual void removeCollisionObject(btCollisionObject* collisionObject);
virtual void updateAabbs();
virtual void removeCollisionObject(btCollisionObject* collisionObject);
virtual void synchronizeMotionStates();
virtual void updateAabbs();
virtual void setConstraintSolver(btConstraintSolver* solver);
virtual void synchronizeMotionStates();
virtual void setConstraintSolver(btConstraintSolver* solver);
virtual btConstraintSolver* getConstraintSolver();
virtual btDynamicsWorldType getWorldType() const
virtual btDynamicsWorldType getWorldType() const
{
return BT_SIMPLE_DYNAMICS_WORLD;
}
virtual void clearForces();
virtual void clearForces();
};
#endif //BT_SIMPLE_DYNAMICS_WORLD_H
#endif //BT_SIMPLE_DYNAMICS_WORLD_H

991
src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp
View File

File diff suppressed because it is too large Load Diff

128
src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h
View File

@@ -35,80 +35,78 @@ class btIDebugDraw;
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;
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 SolverParams
{
btConstraintSolver* m_solverPool;
btConstraintSolver* m_solverMt;
btContactSolverInfo* m_solverInfo;
btIDebugDraw* m_debugDrawer;
btDispatcher* m_dispatcher;
};
static void solveIsland(btConstraintSolver* solver, Island& island, const SolverParams& solverParams);
void append(const Island& other); // add bodies, manifolds, constraints to my own
};
struct SolverParams
{
btConstraintSolver* m_solverPool;
btConstraintSolver* m_solverMt;
btContactSolverInfo* m_solverInfo;
btIDebugDraw* m_debugDrawer;
btDispatcher* m_dispatcher;
};
static void solveIsland(btConstraintSolver* solver, Island& island, const SolverParams& solverParams);
typedef void (*IslandDispatchFunc)(btAlignedObjectArray<Island*>* islands, const SolverParams& solverParams);
static void serialIslandDispatch(btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams);
static void parallelIslandDispatch(btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams);
typedef void( *IslandDispatchFunc ) ( btAlignedObjectArray<Island*>* islands, const SolverParams& solverParams );
static void serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams );
static void parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams );
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;
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();
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,
const SolverParams& solverParams
);
virtual void buildAndProcessIslands(btDispatcher* dispatcher,
btCollisionWorld* collisionWorld,
btAlignedObjectArray<btTypedConstraint*>& constraints,
const SolverParams& solverParams);
virtual void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld);
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;
}
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
#endif //BT_SIMULATION_ISLAND_MANAGER_H

1985
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

File diff suppressed because it is too large Load Diff

863
src/BulletDynamics/Featherstone/btMultiBody.h
View File

File diff suppressed because it is too large Load Diff

670
src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
View File

@@ -1,32 +1,29 @@
#include "btMultiBodyConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
#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)
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)
if (m_bodyA)
{
m_jacSizeA = (6 + m_bodyA->getNumDofs());
}
if(m_bodyB)
if (m_bodyB)
{
m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs();
}
@@ -38,7 +35,7 @@ void btMultiBodyConstraint::allocateJacobiansMultiDof()
{
updateJacobianSizes();
m_posOffset = ((1 + m_jacSizeBoth)*m_numRows);
m_posOffset = ((1 + m_jacSizeBoth) * m_numRows);
m_data.resize((2 + m_jacSizeBoth) * m_numRows);
}
@@ -46,298 +43,307 @@ btMultiBodyConstraint::~btMultiBodyConstraint()
{
}
void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
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;
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)
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) {
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 = constraintNormalAng.dot(solverConstraint.m_angularComponentA);
}
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) {
}
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 = constraintNormalAng.dot(-solverConstraint.m_angularComponentB);
}
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)
{
}
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->getLinearVelocity().dot(solverConstraint.m_contactNormal1);
rel_vel += rb0->getAngularVelocity().dot(solverConstraint.m_relpos1CrossNormal);
}
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)
{
}
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->getLinearVelocity().dot(solverConstraint.m_contactNormal2);
rel_vel += rb1->getAngularVelocity().dot(solverConstraint.m_relpos2CrossNormal);
}
solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
}
///warm starting (or zero if disabled)
/*
}
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;
@@ -369,38 +375,35 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
}
} 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;
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;
{
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;
}
// 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
@@ -409,11 +412,10 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
}
*/
solverConstraint.m_cfm = 0.f;
solverConstraint.m_lowerLimit = lowerLimit;
solverConstraint.m_upperLimit = upperLimit;
}
return rel_vel;
solverConstraint.m_cfm = 0.f;
solverConstraint.m_lowerLimit = lowerLimit;
solverConstraint.m_upperLimit = upperLimit;
}
return rel_vel;
}

155
src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
View File

@@ -27,66 +27,62 @@ struct btSolverInfo;
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;
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;
};
ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraint
ATTRIBUTE_ALIGNED16(class)
btMultiBodyConstraint
{
protected:
btMultiBody* m_bodyA;
btMultiBody* m_bodyB;
int m_linkA;
int m_linkB;
btMultiBody* m_bodyA;
btMultiBody* m_bodyB;
int m_linkA;
int m_linkB;
int m_numRows;
int m_jacSizeA;
int m_jacSizeBoth;
int m_posOffset;
int m_numRows;
int m_jacSizeA;
int m_jacSizeBoth;
int m_posOffset;
bool m_isUnilateral;
int m_numDofsFinalized;
btScalar m_maxAppliedImpulse;
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);
// 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;
btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint & solverConstraint,
btMultiBodyJacobianData & data,
btScalar * jacOrgA, btScalar * jacOrgB,
const btVector3& constraintNormalAng,
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);
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:
BT_DECLARE_ALIGNED_ALLOCATOR();
btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral);
btMultiBodyConstraint(btMultiBody * bodyA, btMultiBody * bodyB, int linkA, int linkB, int numRows, bool isUnilateral);
virtual ~btMultiBodyConstraint();
void updateJacobianSizes();
@@ -94,27 +90,27 @@ public:
//many constraints have setFrameInB/setPivotInB. Will use 'getConstraintType' later.
virtual void setFrameInB(const btMatrix3x3& frameInB) {}
virtual void setPivotInB(const btVector3& pivotInB){}
virtual void setPivotInB(const btVector3& pivotInB) {}
virtual void finalizeMultiDof()=0;
virtual void finalizeMultiDof() = 0;
virtual int getIslandIdA() const =0;
virtual int getIslandIdB() const =0;
virtual int getIslandIdA() const = 0;
virtual int getIslandIdB() const = 0;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)=0;
virtual void createConstraintRows(btMultiBodyConstraintArray & constraintRows,
btMultiBodyJacobianData & data,
const btContactSolverInfo& infoGlobal) = 0;
int getNumRows() const
int getNumRows() const
{
return m_numRows;
}
btMultiBody* getMultiBodyA()
btMultiBody* getMultiBodyA()
{
return m_bodyA;
}
btMultiBody* getMultiBodyB()
btMultiBody* getMultiBodyB()
{
return m_bodyB;
}
@@ -127,77 +123,72 @@ public:
{
return m_linkB;
}
void internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
void internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
{
btAssert(dof>=0);
btAssert(dof >= 0);
btAssert(dof < getNumRows());
m_data[dof] = appliedImpulse;
}
btScalar getAppliedImpulse(int dof)
btScalar getAppliedImpulse(int dof)
{
btAssert(dof>=0);
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
// 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)
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)
// 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
const btScalar* jacobianA(int row) const
{
return &m_data[m_numRows + (row * m_jacSizeBoth)];
}
btScalar* jacobianB(int row)
btScalar* jacobianB(int row)
{
return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
}
const btScalar* jacobianB(int row) const
const btScalar* jacobianB(int row) const
{
return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
}
btScalar getMaxAppliedImpulse() const
btScalar getMaxAppliedImpulse() const
{
return m_maxAppliedImpulse;
}
void setMaxAppliedImpulse(btScalar maxImp)
void setMaxAppliedImpulse(btScalar maxImp)
{
m_maxAppliedImpulse = maxImp;
}
virtual void debugDraw(class btIDebugDraw* drawer)=0;
virtual void debugDraw(class btIDebugDraw * drawer) = 0;
virtual void setGearRatio(btScalar ratio) {}
virtual void setGearAuxLink(int gearAuxLink) {}
virtual void setRelativePositionTarget(btScalar relPosTarget){}
virtual void setErp(btScalar erp){}
virtual void setRelativePositionTarget(btScalar relPosTarget) {}
virtual void setErp(btScalar erp) {}
};
#endif //BT_MULTIBODY_CONSTRAINT_H
#endif //BT_MULTIBODY_CONSTRAINT_H

1381
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

File diff suppressed because it is too large Load Diff

101
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
View File

@@ -25,80 +25,71 @@ class btMultiBody;
#include "btMultiBodyConstraint.h"
ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver
ATTRIBUTE_ALIGNED16(class)
btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver
{
protected:
btMultiBodyConstraintArray m_multiBodyNonContactConstraints;
btMultiBodyConstraintArray m_multiBodyNonContactConstraints;
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints;
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints;
btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints;
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints;
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints;
btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints;
btMultiBodyJacobianData m_data;
btMultiBodyJacobianData m_data;
//temp storage for multi body constraints for a specific island/group called by 'solveGroup'
btMultiBodyConstraint** m_tmpMultiBodyConstraints;
int m_tmpNumMultiBodyConstraints;
btMultiBodyConstraint** m_tmpMultiBodyConstraints;
int m_tmpNumMultiBodyConstraints;
btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
//solve 2 friction directions and clamp against the implicit friction cone
btScalar resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1, const btMultiBodySolverConstraint& cB);
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);
void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
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);
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);
void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow,
btScalar* jacA,btScalar* jacB,
btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
const btContactSolverInfo& infoGlobal);
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 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 setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint & constraintRow,
btScalar * jacA, btScalar * jacB,
btScalar penetration, btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
const btContactSolverInfo& infoGlobal);
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);
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);
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);
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
#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H

806
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

File diff suppressed because it is too large Load Diff

65
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
View File

@@ -33,8 +33,8 @@ protected:
btAlignedObjectArray<btMultiBody*> m_multiBodies;
btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
btAlignedObjectArray<btMultiBodyConstraint*> m_sortedMultiBodyConstraints;
btMultiBodyConstraintSolver* m_multiBodyConstraintSolver;
MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback;
btMultiBodyConstraintSolver* m_multiBodyConstraintSolver;
MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback;
//cached data to avoid memory allocations
btAlignedObjectArray<btQuaternion> m_scratch_world_to_local;
@@ -45,72 +45,69 @@ protected:
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);
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);
btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
virtual ~btMultiBodyDynamicsWorld();
virtual ~btMultiBodyDynamicsWorld ();
virtual void addMultiBody(btMultiBody* body, int group = btBroadphaseProxy::DefaultFilter, int mask = btBroadphaseProxy::AllFilter);
virtual void addMultiBody(btMultiBody* body, int group= btBroadphaseProxy::DefaultFilter, int mask=btBroadphaseProxy::AllFilter);
virtual void removeMultiBody(btMultiBody* body);
virtual void removeMultiBody(btMultiBody* body);
virtual int getNumMultibodies() const
virtual int getNumMultibodies() const
{
return m_multiBodies.size();
}
btMultiBody* getMultiBody(int mbIndex)
btMultiBody* getMultiBody(int mbIndex)
{
return m_multiBodies[mbIndex];
}
const btMultiBody* getMultiBody(int mbIndex) const
const btMultiBody* getMultiBody(int mbIndex) const
{
return m_multiBodies[mbIndex];
}
virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint);
virtual void addMultiBodyConstraint(btMultiBodyConstraint* constraint);
virtual int getNumMultiBodyConstraints() const
virtual int getNumMultiBodyConstraints() const
{
return m_multiBodyConstraints.size();
return m_multiBodyConstraints.size();
}
virtual btMultiBodyConstraint* getMultiBodyConstraint( int constraintIndex)
virtual btMultiBodyConstraint* getMultiBodyConstraint(int constraintIndex)
{
return m_multiBodyConstraints[constraintIndex];
return m_multiBodyConstraints[constraintIndex];
}
virtual const btMultiBodyConstraint* getMultiBodyConstraint( int constraintIndex) const
virtual const btMultiBodyConstraint* getMultiBodyConstraint(int constraintIndex) const
{
return m_multiBodyConstraints[constraintIndex];
return m_multiBodyConstraints[constraintIndex];
}
virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
virtual void removeMultiBodyConstraint(btMultiBodyConstraint* constraint);
virtual void integrateTransforms(btScalar timeStep);
virtual void integrateTransforms(btScalar timeStep);
virtual void debugDrawWorld();
virtual void debugDrawWorld();
virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);
void forwardKinematics();
virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);
void forwardKinematics();
virtual void clearForces();
virtual void clearMultiBodyConstraintForces();
virtual void clearMultiBodyForces();
virtual void applyGravity();
virtual void serialize(btSerializer* serializer);
virtual void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver);
virtual void setConstraintSolver(btConstraintSolver* solver);
virtual void serialize(btSerializer* serializer);
virtual void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver);
virtual void setConstraintSolver(btConstraintSolver* solver);
};
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H

184
src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp
View File

@@ -24,27 +24,27 @@ subject to the following restrictions:
#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)
: 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
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)
: 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
m_data.resize(BTMBFIXEDCONSTRAINT_DIM); //at least store the applied impulses
}
void btMultiBodyFixedConstraint::finalizeMultiDof()
@@ -57,7 +57,6 @@ btMultiBodyFixedConstraint::~btMultiBodyFixedConstraint()
{
}
int btMultiBodyFixedConstraint::getIslandIdA() const
{
if (m_rigidBodyA)
@@ -103,82 +102,83 @@ int btMultiBodyFixedConstraint::getIslandIdB() const
void btMultiBodyFixedConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
{
int numDim = BTMBFIXEDCONSTRAINT_DIM;
for (int i=0;i<numDim;i++)
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
);
}
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);
}
}
}

79
src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h
View File

@@ -23,16 +23,14 @@ subject to the following restrictions:
class btMultiBodyFixedConstraint : public btMultiBodyConstraint
{
protected:
btRigidBody* m_rigidBodyA;
btRigidBody* m_rigidBodyB;
btVector3 m_pivotInA;
btVector3 m_pivotInB;
btMatrix3x3 m_frameInA;
btMatrix3x3 m_frameInB;
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);
@@ -44,18 +42,18 @@ public:
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
const btVector3& getPivotInA() const
{
return m_pivotInA;
}
void setPivotInA(const btVector3& pivotInA)
{
m_pivotInA = pivotInA;
}
const btVector3& getPivotInA() const
{
return m_pivotInA;
}
void setPivotInA(const btVector3& pivotInA)
{
m_pivotInA = pivotInA;
}
const btVector3& getPivotInB() const
{
@@ -66,29 +64,28 @@ public:
{
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;
}
virtual void setFrameInB(const btMatrix3x3& frameInB)
{
m_frameInB = frameInB;
}
const btMatrix3x3& getFrameInA() const
{
return m_frameInA;
}
void setFrameInA(const btMatrix3x3& frameInA)
{
m_frameInA = frameInA;
}
const btMatrix3x3& getFrameInB() const
{
return m_frameInB;
}
virtual void setFrameInB(const btMatrix3x3& frameInB)
{
m_frameInB = frameInB;
}
virtual void debugDraw(class btIDebugDraw* drawer);
};
#endif //BT_MULTIBODY_FIXED_CONSTRAINT_H
#endif //BT_MULTIBODY_FIXED_CONSTRAINT_H

86
src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp
View File

@@ -21,20 +21,18 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
btMultiBodyGearConstraint::btMultiBodyGearConstraint(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,1,false),
m_gearRatio(1),
m_gearAuxLink(-1),
m_erp(0),
m_relativePositionTarget(0)
: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, 1, false),
m_gearRatio(1),
m_gearAuxLink(-1),
m_erp(0),
m_relativePositionTarget(0)
{
}
void btMultiBodyGearConstraint::finalizeMultiDof()
{
allocateJacobiansMultiDof();
m_numDofsFinalized = m_jacSizeBoth;
}
@@ -42,7 +40,6 @@ btMultiBodyGearConstraint::~btMultiBodyGearConstraint()
{
}
int btMultiBodyGearConstraint::getIslandIdA() const
{
if (m_bodyA)
@@ -81,27 +78,25 @@ int btMultiBodyGearConstraint::getIslandIdB() const
return -1;
}
void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)
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.
// 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();
finalizeMultiDof();
}
//don't crash
if (m_numDofsFinalized != m_jacSizeBoth)
return;
if (m_maxAppliedImpulse==0.f)
if (m_maxAppliedImpulse == 0.f)
return;
// note: we rely on the fact that data.m_jacobians are
// always initialized to zero by the Constraint ctor
int linkDoF = 0;
@@ -114,67 +109,66 @@ void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray&
btScalar posError = 0;
const btVector3 dummy(0, 0, 0);
btScalar kp = 1;
btScalar kd = 1;
int numRows = getNumRows();
for (int row=0;row<numRows;row++)
for (int row = 0; row < numRows; row++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
int dof = 0;
btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
int dof = 0;
btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
btScalar auxVel = 0;
if (m_gearAuxLink>=0)
if (m_gearAuxLink >= 0)
{
auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof];
}
currentVelocity += auxVel;
if (m_erp!=0)
if (m_erp != 0)
{
btScalar currentPositionA = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
if (m_gearAuxLink >= 0)
{
currentPositionA -= m_bodyA->getJointPosMultiDof(m_gearAuxLink)[dof];
}
btScalar currentPositionB = m_gearRatio*m_bodyA->getJointPosMultiDof(m_linkB)[dof];
btScalar diff = currentPositionB+currentPositionA;
btScalar currentPositionB = m_gearRatio * m_bodyA->getJointPosMultiDof(m_linkB)[dof];
btScalar diff = currentPositionB + currentPositionA;
btScalar desiredPositionDiff = this->m_relativePositionTarget;
posError = -m_erp*(desiredPositionDiff - diff);
posError = -m_erp * (desiredPositionDiff - diff);
}
btScalar desiredRelativeVelocity = auxVel;
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,desiredRelativeVelocity);
btScalar desiredRelativeVelocity = auxVel;
fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, -m_maxAppliedImpulse, m_maxAppliedImpulse, false, 1, false, desiredRelativeVelocity);
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));
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;
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;
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();
constraintRow.m_relpos2CrossNormal.setZero();
break;
}
default:
@@ -182,10 +176,6 @@ void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray&
btAssert(0);
}
};
}
}
}

88
src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h
View File

@@ -23,20 +23,18 @@ subject to the following restrictions:
class btMultiBodyGearConstraint : public btMultiBodyConstraint
{
protected:
btRigidBody* m_rigidBodyA;
btRigidBody* m_rigidBodyB;
btVector3 m_pivotInA;
btVector3 m_pivotInB;
btMatrix3x3 m_frameInA;
btMatrix3x3 m_frameInB;
btScalar m_gearRatio;
int m_gearAuxLink;
btScalar m_erp;
btScalar m_relativePositionTarget;
btRigidBody* m_rigidBodyA;
btRigidBody* m_rigidBodyB;
btVector3 m_pivotInA;
btVector3 m_pivotInB;
btMatrix3x3 m_frameInA;
btMatrix3x3 m_frameInB;
btScalar m_gearRatio;
int m_gearAuxLink;
btScalar m_erp;
btScalar m_relativePositionTarget;
public:
//btMultiBodyGearConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
@@ -48,18 +46,18 @@ public:
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
const btVector3& getPivotInA() const
{
return m_pivotInA;
}
void setPivotInA(const btVector3& pivotInA)
{
m_pivotInA = pivotInA;
}
const btVector3& getPivotInA() const
{
return m_pivotInA;
}
void setPivotInA(const btVector3& pivotInA)
{
m_pivotInA = pivotInA;
}
const btVector3& getPivotInB() const
{
@@ -70,32 +68,32 @@ public:
{
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;
}
virtual void setFrameInB(const btMatrix3x3& frameInB)
{
m_frameInB = frameInB;
}
const btMatrix3x3& getFrameInA() const
{
return m_frameInA;
}
void setFrameInA(const btMatrix3x3& frameInA)
{
m_frameInA = frameInA;
}
const btMatrix3x3& getFrameInB() const
{
return m_frameInB;
}
virtual void setFrameInB(const btMatrix3x3& frameInB)
{
m_frameInB = frameInB;
}
virtual void debugDraw(class btIDebugDraw* drawer)
{
//todo(erwincoumans)
}
virtual void setGearRatio(btScalar gearRatio)
{
m_gearRatio = gearRatio;
@@ -114,4 +112,4 @@ public:
}
};
#endif //BT_MULTIBODY_GEAR_CONSTRAINT_H
#endif //BT_MULTIBODY_GEAR_CONSTRAINT_H

6
src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h
View File

@@ -12,8 +12,6 @@ subject to the following restrictions:
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
@@ -21,7 +19,7 @@ subject to the following restrictions:
struct btMultiBodyJointFeedback
{
btSpatialForceVector m_reactionForces;
btSpatialForceVector m_reactionForces;
};
#endif //BT_MULTIBODY_JOINT_FEEDBACK_H
#endif //BT_MULTIBODY_JOINT_FEEDBACK_H

94
src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
View File

@@ -20,21 +20,18 @@ subject to the following restrictions:
#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)
: 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
// initialize them here
allocateJacobiansMultiDof();
@@ -53,10 +50,8 @@ btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
{
}
int btMultiBodyJointLimitConstraint::getIslandIdA() const
{
if (m_bodyA)
{
if (m_linkA < 0)
@@ -93,72 +88,69 @@ int btMultiBodyJointLimitConstraint::getIslandIdB() const
return -1;
}
void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)
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.
// 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();
finalizeMultiDof();
}
// row 0: the lower bound
setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); //multidof: this is joint-type dependent
// 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));
// row 1: the upper bound
setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
for (int row=0;row<getNumRows();row++)
for (int row = 0; row < getNumRows(); row++)
{
btScalar penetration = getPosition(row);
//todo: consider adding some safety threshold here
if (penetration>0)
if (penetration > 0)
{
continue;
}
btScalar direction = row? -1 : 1;
btScalar direction = row ? -1 : 1;
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = row;
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 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);
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));
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;
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;
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:
@@ -166,36 +158,35 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
btAssert(0);
}
};
}
{
btScalar positionalError = 0.f;
btScalar velocityError = - rel_vel;// * damping;
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)
if (penetration > 0)
{
positionalError = 0;
velocityError = -penetration / infoGlobal.m_timeStep;
} else
}
else
{
positionalError = -penetration * erp/infoGlobal.m_timeStep;
positionalError = -penetration * erp / infoGlobal.m_timeStep;
}
btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
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_rhs = penetrationImpulse + velocityImpulse;
constraintRow.m_rhsPenetration = 0.f;
} else
}
else
{
//split position and velocity into rhs and m_rhsPenetration
constraintRow.m_rhs = velocityImpulse;
@@ -203,9 +194,4 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
}
}
}
}

13
src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h
View File

@@ -22,11 +22,10 @@ struct btSolverInfo;
class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint
{
protected:
btScalar m_lowerBound;
btScalar m_upperBound;
btScalar m_lowerBound;
btScalar m_upperBound;
public:
btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper);
virtual ~btMultiBodyJointLimitConstraint();
@@ -36,15 +35,13 @@ public:
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
virtual void debugDraw(class btIDebugDraw* drawer)
{
//todo(erwincoumans)
}
};
#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H

101
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
View File

@@ -20,22 +20,18 @@ subject to the following restrictions:
#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)
: 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
// initialize them here
}
void btMultiBodyJointMotor::finalizeMultiDof()
@@ -55,18 +51,17 @@ void btMultiBodyJointMotor::finalizeMultiDof()
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)
: 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()
{
@@ -108,76 +103,74 @@ int btMultiBodyJointMotor::getIslandIdB() const
return -1;
}
void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)
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.
// 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();
finalizeMultiDof();
}
//don't crash
if (m_numDofsFinalized != m_jacSizeBoth)
return;
if (m_maxAppliedImpulse==0.f)
if (m_maxAppliedImpulse == 0.f)
return;
const btScalar posError = 0;
const btVector3 dummy(0, 0, 0);
for (int row=0;row<getNumRows();row++)
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)
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;
rhs = m_rhsClamp;
}
if (rhs<-m_rhsClamp)
if (rhs < -m_rhsClamp)
{
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);
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));
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;
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;
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:
@@ -185,10 +178,6 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
btAssert(0);
}
};
}
}
}

46
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
View File

@@ -24,41 +24,38 @@ 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
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 void finalizeMultiDof();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
virtual void setVelocityTarget(btScalar velTarget, btScalar kd = 1.f)
{
m_desiredVelocity = velTarget;
m_kd = kd;
}
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 setPositionTarget(btScalar posTarget, btScalar kp = 1.f)
{
m_desiredPosition = posTarget;
m_kp = kp;
}
virtual void setErp(btScalar erp)
{
m_erp = erp;
@@ -77,5 +74,4 @@ public:
}
};
#endif //BT_MULTIBODY_JOINT_MOTOR_H
#endif //BT_MULTIBODY_JOINT_MOTOR_H

189
src/BulletDynamics/Featherstone/btMultiBodyLink.h
View File

@@ -20,7 +20,7 @@ subject to the following restrictions:
#include "LinearMath/btVector3.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
enum btMultiBodyLinkFlags
enum btMultiBodyLinkFlags
{
BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1,
BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION = 2,
@@ -36,7 +36,6 @@ enum btMultiBodyLinkFlags
//namespace {
#include "LinearMath/btSpatialAlgebra.h"
//}
@@ -45,27 +44,26 @@ enum btMultiBodyLinkFlags
// Link struct
//
struct btMultibodyLink
struct btMultibodyLink
{
BT_DECLARE_ALIGNED_ALLOCATOR();
btScalar m_mass; // mass of link
btVector3 m_inertiaLocal; // inertia of link (local frame; diagonal)
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.
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.
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.
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;
btVector3 m_eVector;
btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;
@@ -79,13 +77,11 @@ struct btMultibodyLink
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 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)
@@ -93,143 +89,141 @@ struct btMultibodyLink
// 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
// 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)
void setAxisBottom(int dof, const btVector3 &axis)
{
m_axes[dof].m_topVec.setValue(x, y, z);
m_axes[dof].m_bottomVec = axis;
}
void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
{
m_axes[dof].m_bottomVec.setValue(x, y, z);
void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
{
m_axes[dof].m_topVec.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; }
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.
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_appliedForce; // In WORLD frame
btVector3 m_appliedTorque; // In WORLD frame
btVector3 m_appliedConstraintForce; // In WORLD frame
btVector3 m_appliedConstraintTorque; // 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
//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;
class btMultiBodyLinkCollider *m_collider;
int m_flags;
int m_dofCount, m_posVarCount; //redundant but handy
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
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.
btScalar m_jointLowerLimit; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
btScalar m_jointUpperLimit; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
btScalar m_jointMaxForce; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
btScalar m_jointMaxVelocity; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
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.
btScalar m_jointLowerLimit; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
btScalar m_jointUpperLimit; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
btScalar m_jointMaxForce; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
btScalar m_jointMaxVelocity;//todo: implement this internally. It is unused for now, it is set by a URDF loader.
// 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_jointLowerLimit(0),
m_jointUpperLimit(0),
m_jointMaxForce(0),
m_jointMaxVelocity(0)
: 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_jointLowerLimit(0),
m_jointUpperLimit(0),
m_jointMaxForce(0),
m_jointMaxVelocity(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_appliedForce.setValue(0, 0, 0);
m_appliedTorque.setValue(0, 0, 0);
m_appliedConstraintForce.setValue(0,0,0);
m_appliedConstraintTorque.setValue(0,0,0);
//
m_appliedConstraintForce.setValue(0, 0, 0);
m_appliedConstraintTorque.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_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
// routine to update m_cachedRotParentToThis and m_cachedRVector
void updateCacheMultiDof(btScalar *pq = 0)
{
btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
switch(m_jointType)
switch (m_jointType)
{
case eRevolute:
{
m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
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);
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);
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);
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);
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
break;
}
@@ -242,5 +236,4 @@ btVector3 m_appliedConstraintForce; // In WORLD frame
}
};
#endif //BT_MULTIBODY_LINK_H
#endif //BT_MULTIBODY_LINK_H

61
src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h
View File

@@ -29,21 +29,18 @@ subject to the following restrictions:
#define btMultiBodyLinkColliderDataName "btMultiBodyLinkColliderFloatData"
#endif
class btMultiBodyLinkCollider : public btCollisionObject
{
//protected:
//protected:
public:
btMultiBody* m_multiBody;
int m_link;
btMultiBodyLinkCollider (btMultiBody* multiBody,int link)
:m_multiBody(multiBody),
m_link(link)
btMultiBodyLinkCollider(btMultiBody* multiBody, int link)
: m_multiBody(multiBody),
m_link(link)
{
m_checkCollideWith = true;
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()))
@@ -59,18 +56,18 @@ public:
}
static btMultiBodyLinkCollider* upcast(btCollisionObject* colObj)
{
if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
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)
if (colObj->getInternalType() & btCollisionObject::CO_FEATHERSTONE_LINK)
return (btMultiBodyLinkCollider*)colObj;
return 0;
}
virtual bool checkCollideWithOverride(const btCollisionObject* co) const
virtual bool checkCollideWithOverride(const btCollisionObject* co) const
{
const btMultiBodyLinkCollider* other = btMultiBodyLinkCollider::upcast(co);
if (!other)
@@ -81,47 +78,46 @@ public:
return false;
//check if 'link' has collision disabled
if (m_link>=0)
if (m_link >= 0)
{
const btMultibodyLink& link = m_multiBody->getLink(this->m_link);
if (link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION)
if (link.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION)
{
int parent_of_this = m_link;
while (1)
{
if (parent_of_this==-1)
if (parent_of_this == -1)
break;
parent_of_this = m_multiBody->getLink(parent_of_this).m_parent;
if (parent_of_this==other->m_link)
if (parent_of_this == other->m_link)
{
return false;
}
}
}
else if (link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION)
else if (link.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION)
{
if ( link.m_parent == other->m_link)
if (link.m_parent == other->m_link)
return false;
}
}
if (other->m_link>=0)
if (other->m_link >= 0)
{
const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link);
if (otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION)
if (otherLink.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION)
{
int parent_of_other = other->m_link;
while (1)
{
if (parent_of_other==-1)
if (parent_of_other == -1)
break;
parent_of_other = m_multiBody->getLink(parent_of_other).m_parent;
if (parent_of_other==this->m_link)
if (parent_of_other == this->m_link)
return false;
}
}
else if (otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION)
else if (otherLink.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION)
{
if (otherLink.m_parent == this->m_link)
return false;
@@ -130,13 +126,13 @@ public:
return true;
}
virtual int calculateSerializeBufferSize() const;
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;
virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
};
// clang-format off
struct btMultiBodyLinkColliderFloatData
{
@@ -154,16 +150,18 @@ struct btMultiBodyLinkColliderDoubleData
char m_padding[4];
};
SIMD_FORCE_INLINE int btMultiBodyLinkCollider::calculateSerializeBufferSize() const
// clang-format on
SIMD_FORCE_INLINE int btMultiBodyLinkCollider::calculateSerializeBufferSize() const
{
return sizeof(btMultiBodyLinkColliderData);
}
SIMD_FORCE_INLINE const char* btMultiBodyLinkCollider::serialize(void* dataBuffer, class btSerializer* serializer) const
SIMD_FORCE_INLINE const char* btMultiBodyLinkCollider::serialize(void* dataBuffer, class btSerializer* serializer) const
{
btMultiBodyLinkColliderData* dataOut = (btMultiBodyLinkColliderData*)dataBuffer;
btCollisionObject::serialize(&dataOut->m_colObjData,serializer);
btCollisionObject::serialize(&dataOut->m_colObjData, serializer);
dataOut->m_link = this->m_link;
dataOut->m_multiBody = (btMultiBodyData*)serializer->getUniquePointer(m_multiBody);
@@ -173,5 +171,4 @@ SIMD_FORCE_INLINE const char* btMultiBodyLinkCollider::serialize(void* dataBuffe
return btMultiBodyLinkColliderDataName;
}
#endif //BT_FEATHERSTONE_LINK_COLLIDER_H
#endif //BT_FEATHERSTONE_LINK_COLLIDER_H

113
src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
View File

@@ -21,29 +21,29 @@ subject to the following restrictions:
#include "LinearMath/btIDebugDraw.h"
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
#define BTMBP2PCONSTRAINT_DIM 3
#define BTMBP2PCONSTRAINT_DIM 3
#else
#define BTMBP2PCONSTRAINT_DIM 6
#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)
: 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
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)
: 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
m_data.resize(BTMBP2PCONSTRAINT_DIM); //at least store the applied impulses
}
void btMultiBodyPoint2Point::finalizeMultiDof()
@@ -56,7 +56,6 @@ btMultiBodyPoint2Point::~btMultiBodyPoint2Point()
{
}
int btMultiBodyPoint2Point::getIslandIdA() const
{
if (m_rigidBodyA)
@@ -73,7 +72,7 @@ int btMultiBodyPoint2Point::getIslandIdA() const
else
{
if (m_bodyA->getLink(m_linkA).m_collider)
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
}
}
return -1;
@@ -100,48 +99,43 @@ int btMultiBodyPoint2Point::getIslandIdB() const
return -1;
}
void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)
{
// int i=1;
int numDim = BTMBP2PCONSTRAINT_DIM;
for (int i=0;i<numDim;i++)
// 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);
//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);
btVector3 contactNormalOnB(0, 0, 0);
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
contactNormalOnB[i] = -1;
#else
contactNormalOnB[i%3] = -1;
contactNormalOnB[i % 3] = -1;
#endif
// Convert local points back to world
// 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
pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
}
else
{
if (m_bodyA)
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
@@ -150,44 +144,41 @@ int numDim = BTMBP2PCONSTRAINT_DIM;
if (m_rigidBodyB)
{
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
} else
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;
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
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;
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
);
dummy, dummy, dummy, //sucks but let it be this way "for the time being"
posError,
infoGlobal,
-m_maxAppliedImpulse, m_maxAppliedImpulse);
#endif
}
}

30
src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h
View File

@@ -22,22 +22,20 @@ subject to the following restrictions:
//#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
ATTRIBUTE_ALIGNED16(class) btMultiBodyPoint2Point : public btMultiBodyConstraint
ATTRIBUTE_ALIGNED16(class)
btMultiBodyPoint2Point : public btMultiBodyConstraint
{
protected:
btRigidBody* m_rigidBodyA;
btRigidBody* m_rigidBodyB;
btVector3 m_pivotInA;
btVector3 m_pivotInB;
btRigidBody* m_rigidBodyA;
btRigidBody* m_rigidBodyB;
btVector3 m_pivotInA;
btVector3 m_pivotInB;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
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);
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();
@@ -46,9 +44,9 @@ public:
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
virtual void createConstraintRows(btMultiBodyConstraintArray & constraintRows,
btMultiBodyJacobianData & data,
const btContactSolverInfo& infoGlobal);
const btVector3& getPivotInB() const
{
@@ -60,9 +58,7 @@ public:
m_pivotInB = pivotInB;
}
virtual void debugDraw(class btIDebugDraw* drawer);
virtual void debugDraw(class btIDebugDraw * drawer);
};
#endif //BT_MULTIBODY_POINT2POINT_H
#endif //BT_MULTIBODY_POINT2POINT_H

221
src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp
View File

@@ -25,29 +25,29 @@ subject to the following restrictions:
#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)
: 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
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)
: 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
m_data.resize(BTMBSLIDERCONSTRAINT_DIM); //at least store the applied impulses
}
void btMultiBodySliderConstraint::finalizeMultiDof()
@@ -60,7 +60,6 @@ btMultiBodySliderConstraint::~btMultiBodySliderConstraint()
{
}
int btMultiBodySliderConstraint::getIslandIdA() const
{
if (m_rigidBodyA)
@@ -105,98 +104,100 @@ int btMultiBodySliderConstraint::getIslandIdB() const
}
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++)
// Convert local points back to world
btVector3 pivotAworld = m_pivotInA;
btMatrix3x3 frameAworld = m_frameInA;
btVector3 jointAxis = m_jointAxis;
if (m_rigidBodyA)
{
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
);
}
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);
}
}
}

101
src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h
View File

@@ -23,17 +23,15 @@ subject to the following restrictions:
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;
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);
@@ -45,18 +43,18 @@ public:
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
const btVector3& getPivotInA() const
{
return m_pivotInA;
}
void setPivotInA(const btVector3& pivotInA)
{
m_pivotInA = pivotInA;
}
const btVector3& getPivotInA() const
{
return m_pivotInA;
}
void setPivotInA(const btVector3& pivotInA)
{
m_pivotInA = pivotInA;
}
const btVector3& getPivotInB() const
{
@@ -67,39 +65,38 @@ public:
{
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;
}
virtual void setFrameInB(const btMatrix3x3& frameInB)
{
m_frameInB = frameInB;
}
const btVector3& getJointAxis() const
{
return m_jointAxis;
}
void setJointAxis(const btVector3& jointAxis)
{
m_jointAxis = jointAxis;
}
const btMatrix3x3& getFrameInA() const
{
return m_frameInA;
}
void setFrameInA(const btMatrix3x3& frameInA)
{
m_frameInA = frameInA;
}
const btMatrix3x3& getFrameInB() const
{
return m_frameInB;
}
virtual 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
#endif //BT_MULTIBODY_SLIDER_CONSTRAINT_H

82
src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h
View File

@@ -25,66 +25,66 @@ class btMultiBodyConstraint;
#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
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
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)
{
void* m_originalContactPoint;
btScalar m_unusedPadding4;
}
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_overrideNumSolverIterations;
int m_frictionIndex;
int m_solverBodyIdA;
btMultiBody* m_multiBodyA;
int m_linkA;
int m_linkA;
int m_solverBodyIdB;
btMultiBody* m_multiBodyB;
int m_linkB;
int m_linkB;
//for writing back applied impulses
btMultiBodyConstraint* m_orgConstraint;
btMultiBodyConstraint* m_orgConstraint;
int m_orgDofIndex;
enum btSolverConstraintType
enum btSolverConstraintType
{
BT_SOLVER_CONTACT_1D = 0,
BT_SOLVER_FRICTION_1D
};
};
typedef btAlignedObjectArray<btMultiBodySolverConstraint> btMultiBodyConstraintArray;
typedef btAlignedObjectArray<btMultiBodySolverConstraint> btMultiBodyConstraintArray;
#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_H
#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_H

3496
src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp
View File

File diff suppressed because it is too large Load Diff

12
src/BulletDynamics/MLCPSolvers/btDantzigLCP.h
View File

@@ -41,7 +41,6 @@ to be implemented. the first `nub' variables are assumed to have findex < 0.
*/
#ifndef _BT_LCP_H_
#define _BT_LCP_H_
@@ -49,7 +48,6 @@ to be implemented. the first `nub' variables are assumed to have findex < 0.
#include <stdio.h>
#include <assert.h>
#include "LinearMath/btScalar.h"
#include "LinearMath/btAlignedObjectArray.h"
@@ -62,16 +60,14 @@ struct btDantzigScratchMemory
btAlignedObjectArray<btScalar> delta_x;
btAlignedObjectArray<btScalar> Dell;
btAlignedObjectArray<btScalar> ell;
btAlignedObjectArray<btScalar*> Arows;
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);
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_
#endif //_BT_LCP_H_

32
src/BulletDynamics/MLCPSolvers/btDantzigSolver.h
View File

@@ -20,30 +20,28 @@ subject to the following restrictions:
#include "btMLCPSolverInterface.h"
#include "btDantzigLCP.h"
class btDantzigSolver : public btMLCPSolverInterface
{
protected:
btScalar m_acceptableUpperLimitSolution;
btAlignedObjectArray<char> m_tempBuffer;
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;
btAlignedObjectArray<int> m_dependencies;
btDantzigScratchMemory m_scratchMemory;
public:
public:
btDantzigSolver()
:m_acceptableUpperLimitSolution(btScalar(1000))
: 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)
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();
@@ -52,14 +50,12 @@ public:
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.resize(n * n);
for (int i = 0; i < n * n; i++)
{
m_A[i] = Aptr[i];
}
m_b.resize(n);
@@ -67,7 +63,7 @@ public:
m_lo.resize(n);
m_hi.resize(n);
m_dependencies.resize(n);
for (int i=0;i<n;i++)
for (int i = 0; i < n; i++)
{
m_lo[i] = lo[i];
m_hi[i] = hi[i];
@@ -76,13 +72,12 @@ public:
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);
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++)
// printf("numAllocas = %d\n",numAllocas);
for (int i = 0; i < n; i++)
{
volatile btScalar xx = m_x[i];
if (xx != m_x[i])
@@ -98,15 +93,14 @@ public:
}
}
for (int i=0;i<n;i++)
for (int i = 0; i < n; i++)
{
x[i] = m_x[i];
}
}
return result;
}
};
#endif //BT_DANTZIG_SOLVER_H
#endif //BT_DANTZIG_SOLVER_H

468
src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp
View File

@@ -19,64 +19,60 @@ subject to the following restrictions:
//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
#endif //BT_DEBUG_OSTREAM
btScalar btMachEps()
{
static bool calculated=false;
static bool calculated = false;
static btScalar machEps = btScalar(1.);
if (!calculated)
{
do {
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;
} while ((btScalar)(1.0 + (machEps / btScalar(2.0))) != btScalar(1.0));
// printf( "\nCalculated Machine epsilon: %G\n", machEps );
calculated = true;
}
return machEps;
}
btScalar btEpsRoot() {
btScalar btEpsRoot()
{
static btScalar epsroot = 0.;
static bool alreadyCalculated = false;
if (!alreadyCalculated) {
if (!alreadyCalculated)
{
epsroot = btSqrt(btMachEps());
alreadyCalculated = true;
}
return epsroot;
}
btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/)
btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/)
{
steps = 0;
steps = 0;
int dim = m_q.size();
int dim = m_q.size();
#ifdef BT_DEBUG_OSTREAM
if(DEBUGLEVEL >= 1) {
cout << "Dimension = " << dim << endl;
}
#endif //BT_DEBUG_OSTREAM
if (DEBUGLEVEL >= 1)
{
cout << "Dimension = " << dim << endl;
}
#endif //BT_DEBUG_OSTREAM
btVectorXu solutionVector(2 * dim);
solutionVector.setZero();
//, INIT, 0.);
//, INIT, 0.);
btMatrixXu ident(dim, dim);
ident.setIdentity();
@@ -85,287 +81,289 @@ btScalar btEpsRoot() {
#endif
btMatrixXu mNeg = m_M.negative();
btMatrixXu A(dim, 2 * dim + 2);
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, 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);
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
#endif //BT_DEBUG_OSTREAM
// btVectorXu q_;
// q_ >> A(0, 2 * dim + 1, dim - 1, 2 * dim + 1);
// 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);
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++)
for (int i = 0; i < dim; i++)
{
btScalar v =A(i,2*dim+1);
if (v<minValue)
btScalar v = A(i, 2 * dim + 1);
if (v < minValue)
{
minValue=v;
minValue = v;
pivotRowIndex = i;
}
if (v<0)
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
// 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)
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
// 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...
}
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);
/*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
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;
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;
/*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;
/*the column becomes part of the basis*/
basis[pivotRowIndex] = pivotColIndexOld;
pivotRowIndex = findLexicographicMinimum(A, pivotColIndex);
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;
}
}
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 (DEBUGLEVEL >= 1)
{
cout << "Number of loops: " << steps << endl;
cout << "Number of maximal loops: " << maxloops << endl;
}
#endif //BT_DEBUG_OSTREAM
if(!validBasis(basis)) {
info = -1;
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
if (DEBUGLEVEL >= 1)
cerr << "Lemke-Algorithm ended with Ray-Termination (no valid solution)." << endl;
#endif //BT_DEBUG_OSTREAM
return solutionVector;
}
}
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++)
if (DEBUGLEVEL >= 2)
{
solutionVector[basis[i]] = A(i,2*dim+1);//q_[i];
// 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;
info = 0;
return solutionVector;
}
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;
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;
// if (DEBUGLEVEL) {
// cout << "Rows(" << row << ") = " << Rows[row] << endl;
// }
#endif
}
}
#endif
}
}
for (int i = 0; i < Rows.size(); i++)
{
if (Rows[i].nrm2() > 0.) {
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];
}
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;
}
}
}
//=Rows[j] - Rows[i]
if (! LexicographicPositive(test))
break;
}
}
}
if (j == Rows.size())
{
RowIndex += i;
break;
}
}
}
if (j == Rows.size())
{
RowIndex += i;
break;
}
}
}
return RowIndex;
}
return RowIndex;
}
bool btLemkeAlgorithm::LexicographicPositive(const btVectorXu & v)
bool btLemkeAlgorithm::LexicographicPositive(const btVectorXu& v)
{
int i = 0;
// if (DEBUGLEVEL)
// cout << "v " << v << endl;
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;
while (i < v.size() - 1 && fabs(v[i]) < btMachEps())
i++;
if (v[i] > 0)
return true;
return false;
}
return false;
}
void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis)
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++)
for (int i = 0; i < A.rows(); i++)
{
if (i != pivotRowIndex)
{
for (int j = 0; j < A.cols(); j++)
if (i != pivotRowIndex)
{
if (j != pivotColumnIndex)
{
btScalar v = A(i, j);
v += A(pivotRowIndex, j) * A(i, pivotColumnIndex) * a;
A.setElem(i, j, v);
}
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++)
#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);
}
A.mulElem(pivotRowIndex, i, -a);
}
#ifdef BT_DEBUG_OSTREAM
cout << A << std::endl;
#endif //#ifdef BT_DEBUG_OSTREAM
}
#endif //#ifdef BT_DEBUG_OSTREAM
bool btLemkeAlgorithm::greaterZero(const btVectorXu & vector)
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;
}
}
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;
}
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;
}

82
src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h
View File

@@ -19,90 +19,84 @@ subject to the following restrictions:
//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
#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_);
}
btLemkeAlgorithm(const btMatrixXu& M_, const btVectorXu& q_, const int & DEBUGLEVEL_ = 0) :
DEBUGLEVEL(DEBUGLEVEL_)
{
setSystem(M_, q_);
}
/* GETTER / SETTER */
/**
/* GETTER / SETTER */
/**
* \brief return info of solution process
*/
int getInfo() {
return info;
}
int getInfo()
{
return info;
}
/**
/**
* \brief get the number of steps until the solution was found
*/
int getSteps(void) {
return steps;
}
int getSteps(void)
{
return steps;
}
/**
/**
* \brief set system with Matrix M and vector q
*/
void setSystem(const btMatrixXu & M_, const btVectorXu & 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);
btVectorXu solve(unsigned int maxloops = 0);
virtual ~btLemkeAlgorithm() {
}
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);
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;
btMatrixXu m_M;
btVectorXu m_q;
/**
/**
* \brief number of steps until the Lemke algorithm found a solution
*/
unsigned int steps;
unsigned int steps;
/**
/**
* \brief define level of debug output
*/
int DEBUGLEVEL;
int DEBUGLEVEL;
/**
/**
* \brief did the algorithm find a solution
*
* -1 : not successful
* 0 : successful
*/
int info;
int info;
};
#endif /* BT_NUMERICS_LEMKE_ALGORITHM_H_ */

504
src/BulletDynamics/MLCPSolvers/btLemkeSolver.h
View File

@@ -17,334 +17,322 @@ subject to the following restrictions:
#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<6B>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;
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)
: 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)
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;
BT_PROFILE("btLemkeSolver::solveMLCP");
int n = A.rows();
if (0==n)
return true;
bool fail = false;
bool fail = false;
btVectorXu solution(n);
btVectorXu q1;
q1.resize(n);
for (int row=0;row<n;row++)
{
q1[row] = -b[row];
}
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;
// cout << "A" << endl;
// cout << A << endl;
/////////////////////////////////////
//slow matrix inversion, replace with LU decomposition
btMatrixXu A1;
btMatrixXu B(n,n);
btMatrixXu B(n, n);
{
BT_PROFILE("inverse(slow)");
A1.resize(A.rows(),A.cols());
for (int row=0;row<A.rows();row++)
A1.resize(A.rows(), A.cols());
for (int row = 0; row < A.rows(); row++)
{
for (int col=0;col<A.cols();col++)
for (int col = 0; col < A.cols(); col++)
{
A1.setElem(row,col,A(row,col));
A1.setElem(row, col, A(row, col));
}
}
btMatrixXu matrix;
matrix.resize(n,2*n);
for (int row=0;row<n;row++)
matrix.resize(n, 2 * n);
for (int row = 0; row < n; row++)
{
for (int col=0;col<n;col++)
for (int col = 0; col < n; col++)
{
matrix.setElem(row,col,A1(row,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 ratio, a;
int i, j, k;
for (i = 0; i < n; i++)
{
for (j = n; j < 2 * n; j++)
{
btScalar v = matrix(i,i);
if (btFuzzyZero(v))
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)
{
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));
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))
for (i = 0; i < n; i++)
{
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++)
a = matrix(i, i);
if (btFuzzyZero(a))
{
B.setElem(row,col,matrix(row,n+col));
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 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++)
btMatrixXu M(n * 2, n * 2);
for (int row = 0; row < n; row++)
{
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)
b1.setElem(row, 0, -b[row]);
for (int col = 0; col < n; col++)
{
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]);
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);
}
}
}
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++)
btMatrixXu Bb1 = B * b1;
// q = [ (-B*b1 - lo)' (hi + B*b1)' ]'
btVectorXu qq;
qq.resize(n * 2);
for (int row = 0; row < n; row++)
{
x[i]=0.f;
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;
}
return !fail;
} else
{
else
{
int dimension = A.rows();
if (0==dimension)
return true;
// printf("================ solving using Lemke/Newton/Fixpoint\n");
if (0 == dimension)
return true;
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)
// printf("================ solving using Lemke/Newton/Fixpoint\n");
btVectorXu q;
q.resize(dimension);
for (int row = 0; row < dimension; row++)
{
x.setZero();
return false;
q[row] = -b[row];
}
//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)
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++)
{
if (x[i]> errorValueMax)
x[i] = solution[i + dimension];
volatile btScalar check = x[i];
if (x[i] != check)
{
fail = true;
errorIndexMax = i;
errorValueMax = x[i];
x.setZero();
return false;
}
////printf("x[i] = %f,",x[i]);
}
if (x[i]<-m_maxValue)
{
if (x[i]<errorValueMin)
//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)
{
errorIndexMin = i;
errorValueMin = x[i];
fail = true;
//printf("x[i] = %f,",x[i]);
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++)
if (fail)
{
x[i]=0.f;
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 !fail;
return true;
}
return true;
}
};
#endif //BT_LEMKE_SOLVER_H
#endif //BT_LEMKE_SOLVER_H

395
src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
View File

@@ -19,10 +19,9 @@ subject to the following restrictions:
#include "LinearMath/btQuickprof.h"
#include "btSolveProjectedGaussSeidel.h"
btMLCPSolver::btMLCPSolver( btMLCPSolverInterface* solver)
:m_solver(solver),
m_fallback(0)
btMLCPSolver::btMLCPSolver(btMLCPSolverInterface* solver)
: m_solver(solver),
m_fallback(0)
{
}
@@ -33,67 +32,65 @@ 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)
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);
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 numFrictionPerContact = m_tmpSolverContactConstraintPool.size() == m_tmpSolverContactFrictionConstraintPool.size() ? 1 : 2;
// int numBodies = m_tmpSolverBodyPool.size();
// 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());
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++)
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;
int firstContactConstraintOffset = dindex;
if (interleaveContactAndFriction)
{
for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
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]);
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;
m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact + 1]);
m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
}
}
} else
}
else
{
for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
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++)
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;
m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex + firstContactConstraintOffset;
}
}
if (!m_allConstraintPtrArray.size())
{
m_A.resize(0,0);
m_A.resize(0, 0);
m_b.resize(0);
m_x.resize(0);
m_lo.resize(0);
@@ -102,7 +99,6 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
}
}
if (gUseMatrixMultiply)
{
BT_PROFILE("createMLCP");
@@ -121,7 +117,7 @@ bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
{
bool result = true;
if (m_A.rows()==0)
if (m_A.rows() == 0)
return true;
//if using split impulse, we solve 2 separate (M)LCPs
@@ -129,28 +125,26 @@ bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
{
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 );
// 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(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 );
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 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;
@@ -163,36 +157,36 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
m_bSplit.resize(numConstraintRows);
m_b.setZero();
m_bSplit.setZero();
for (int i=0;i<numConstraintRows ;i++)
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;
m_b[i] = rhs / jacDiag;
m_bSplit[i] = rhsPenetration / jacDiag;
}
}
}
// btScalar* w = 0;
// int nub = 0;
// 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++)
for (int i = 0; i < numConstraintRows; i++)
{
if (0)//m_limitDependencies[i]>=0)
if (0) //m_limitDependencies[i]>=0)
{
m_lo[i] = -BT_INFINITY;
m_hi[i] = BT_INFINITY;
} else
}
else
{
m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
@@ -201,48 +195,48 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
}
//
int m=m_allConstraintPtrArray.size();
int m = m_allConstraintPtrArray.size();
int numBodies = m_tmpSolverBodyPool.size();
btAlignedObjectArray<int> bodyJointNodeArray;
{
BT_PROFILE("bodyJointNodeArray.resize");
bodyJointNodeArray.resize(numBodies,-1);
bodyJointNodeArray.resize(numBodies, -1);
}
btAlignedObjectArray<btJointNode> jointNodeArray;
{
BT_PROFILE("jointNodeArray.reserve");
jointNodeArray.reserve(2*m_allConstraintPtrArray.size());
jointNodeArray.reserve(2 * m_allConstraintPtrArray.size());
}
btMatrixXu& J3 = m_scratchJ3;
btMatrixXu& J3 = m_scratchJ3;
{
BT_PROFILE("J3.resize");
J3.resize(2*m,8);
J3.resize(2 * m, 8);
}
btMatrixXu& JinvM3 = m_scratchJInvM3;
btMatrixXu& JinvM3 = m_scratchJInvM3;
{
BT_PROFILE("JinvM3.resize/setZero");
JinvM3.resize(2*m,8);
JinvM3.resize(2 * m, 8);
JinvM3.setZero();
J3.setZero();
}
int cur=0;
int cur = 0;
int rowOffset = 0;
btAlignedObjectArray<int>& ofs = m_scratchOfs;
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 c = 0;
int numRows = 0;
for (int i=0;i<m_allConstraintPtrArray.size();i+=numRows,c++)
for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
{
ofs[c] = rowOffset;
int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
@@ -250,14 +244,14 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
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 ;
numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
if (orgBodyA)
{
{
int slotA=-1;
int slotA = -1;
//find free jointNode slot for sbA
slotA =jointNodeArray.size();
jointNodeArray.expand();//NonInitializing();
slotA = jointNodeArray.size();
jointNodeArray.expand(); //NonInitializing();
int prevSlot = bodyJointNodeArray[sbA];
bodyJointNodeArray[sbA] = slotA;
jointNodeArray[slotA].nextJointNodeIndex = prevSlot;
@@ -265,35 +259,35 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
jointNodeArray[slotA].constraintRowIndex = i;
jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1;
}
for (int row=0;row<numRows;row++,cur++)
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();
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++)
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, 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);
J3.setElem(cur, 3, 0);
JinvM3.setElem(cur, 3, 0);
J3.setElem(cur, 7, 0);
JinvM3.setElem(cur, 7, 0);
}
} else
}
else
{
cur += numRows;
}
if (orgBodyB)
{
{
int slotB=-1;
int slotB = -1;
//find free jointNode slot for sbA
slotB =jointNodeArray.size();
jointNodeArray.expand();//NonInitializing();
slotB = jointNodeArray.size();
jointNodeArray.expand(); //NonInitializing();
int prevSlot = bodyJointNodeArray[sbB];
bodyJointNodeArray[sbB] = slotB;
jointNodeArray[slotB].nextJointNodeIndex = prevSlot;
@@ -302,78 +296,74 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
jointNodeArray[slotB].constraintRowIndex = i;
}
for (int row=0;row<numRows;row++,cur++)
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();
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++)
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, 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);
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;
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);
m_A.resize(n, n);
}
{
BT_PROFILE("m_A.setZero");
m_A.setZero();
}
int c=0;
int c = 0;
{
int numRows = 0;
BT_PROFILE("Compute A");
for (int i=0;i<m_allConstraintPtrArray.size();i+= numRows,c++)
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;
// 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__;
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)
while (startJointNodeA >= 0)
{
int j0 = jointNodeArray[startJointNodeA].jointIndex;
int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
if (j0<c)
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;
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]);
m_A.multiplyAdd2_p8r(JinvMrow,
Jptr + 2 * 8 * (size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]);
}
startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
}
@@ -381,17 +371,17 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
{
int startJointNodeB = bodyJointNodeArray[sbB];
while (startJointNodeB>=0)
while (startJointNodeB >= 0)
{
int j1 = jointNodeArray[startJointNodeB].jointIndex;
int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
if (j1<c)
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]);
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;
}
@@ -402,27 +392,25 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
BT_PROFILE("compute diagonal");
// compute diagonal blocks of m_A
int row__ = 0;
int row__ = 0;
int numJointRows = m_allConstraintPtrArray.size();
int jj=0;
for (;row__<numJointRows;)
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* 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 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)
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__);
m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)infom, Jrow + 8 * (size_t)infom, infom, infom, row__, row__);
}
row__ += infom;
jj++;
@@ -433,12 +421,12 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
if (1)
{
// add cfm to the diagonal of m_A
for ( int i=0; i<m_A.rows(); ++i)
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_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 ");
@@ -450,21 +438,21 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
m_x.resize(numConstraintRows);
m_xSplit.resize(numConstraintRows);
if (infoGlobal.m_solverMode&SOLVER_USE_WARMSTARTING)
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
for (int i=0;i<m_allConstraintPtrArray.size();i++)
for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
{
const btSolverConstraint& c = *m_allConstraintPtrArray[i];
m_x[i]=c.m_appliedImpulse;
m_x[i] = c.m_appliedImpulse;
m_xSplit[i] = c.m_appliedPushImpulse;
}
} else
}
else
{
m_x.setZero();
m_xSplit.setZero();
}
}
}
void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
@@ -475,120 +463,116 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
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++)
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;
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;
m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration / m_allConstraintPtrArray[i]->m_jacDiagABInv;
}
}
btMatrixXu& Minv = m_scratchMInv;
Minv.resize(6*numBodies,6*numBodies);
btMatrixXu& Minv = m_scratchMInv;
Minv.resize(6 * numBodies, 6 * numBodies);
Minv.setZero();
for (int i=0;i<numBodies;i++)
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]);
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);
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);
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++)
{
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]);
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]);
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;
btMatrixXu& J_transpose = m_scratchJTranspose;
J_transpose = J.transpose();
btMatrixXu& tmp = m_scratchTmp;
{
{
BT_PROFILE("J*Minv");
tmp = J*Minv;
tmp = J * Minv;
}
{
BT_PROFILE("J*tmp");
m_A = tmp*J_transpose;
m_A = tmp * J_transpose;
}
}
if (1)
{
// add cfm to the diagonal of m_A
for ( int i=0; i<m_A.rows(); ++i)
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_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();
// m_x.setZero();
for (int i=0;i<m_allConstraintPtrArray.size();i++)
for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
{
const btSolverConstraint& c = *m_allConstraintPtrArray[i];
m_x[i]=c.m_appliedImpulse;
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)
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());
// printf("m_A(%d,%d)\n", m_A.rows(),m_A.cols());
result = solveMLCP(infoGlobal);
}
@@ -596,44 +580,41 @@ btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bod
if (result)
{
BT_PROFILE("process MLCP results");
for (int i=0;i<m_allConstraintPtrArray.size();i++)
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;
// 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;
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);
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);
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);
// printf("m_fallback = %d\n",m_fallback);
m_fallback++;
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
}
return 0.f;
}

40
src/BulletDynamics/MLCPSolvers/btMLCPSolver.h
View File

@@ -23,15 +23,13 @@ subject to the following restrictions:
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;
@@ -39,24 +37,23 @@ protected:
btVectorXu m_xSplit2;
btAlignedObjectArray<int> m_limitDependencies;
btAlignedObjectArray<btSolverConstraint*> m_allConstraintPtrArray;
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);
/// 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);
@@ -65,8 +62,7 @@ protected:
virtual bool solveMLCP(const btContactSolverInfo& infoGlobal);
public:
btMLCPSolver( btMLCPSolverInterface* solver);
btMLCPSolver(btMLCPSolverInterface* solver);
virtual ~btMLCPSolver();
void setMLCPSolver(btMLCPSolverInterface* solver)
@@ -83,12 +79,10 @@ public:
m_fallback = num;
}
virtual btConstraintSolverType getSolverType() const
virtual btConstraintSolverType getSolverType() const
{
return BT_MLCP_SOLVER;
}
};
#endif //BT_MLCP_SOLVER_H
#endif //BT_MLCP_SOLVER_H

4
src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h
View File

@@ -27,7 +27,7 @@ public:
}
//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;
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
#endif //BT_MLCP_SOLVER_INTERFACE_H

61
src/BulletDynamics/MLCPSolvers/btPATHSolver.h
View File

@@ -14,38 +14,35 @@ subject to the following restrictions:
*/
///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" {
extern "C"
{
#include "PATH/SimpleLCP.h"
#include "PATH/License.h"
#include "PATH/Error_Interface.h"
};
void __stdcall MyError(Void *data, Char *msg)
void __stdcall MyError(Void *data, Char *msg)
{
printf("Path Error: %s\n",msg);
printf("Path Error: %s\n", msg);
}
void __stdcall MyWarning(Void *data, Char *msg)
void __stdcall MyWarning(Void *data, Char *msg)
{
printf("Path Warning: %s\n",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");
@@ -55,17 +52,15 @@ public:
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)
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)
if (0 == numVariables)
return true;
/* - variables - the number of variables in the problem
/* - 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
@@ -78,16 +73,16 @@ public:
btAlignedObjectArray<int> rowIndices;
btAlignedObjectArray<int> colIndices;
for (int i=0;i<A.rows();i++)
for (int i = 0; i < A.rows(); i++)
{
for (int j=0;j<A.cols();j++)
for (int j = 0; j < A.cols(); j++)
{
if (A(i,j)!=0.f)
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));
rowIndices.push_back(i + 1);
colIndices.push_back(j + 1);
values.push_back(A(i, j));
}
}
}
@@ -97,19 +92,18 @@ public:
btAlignedObjectArray<double> rhs;
btAlignedObjectArray<double> upperBounds;
btAlignedObjectArray<double> lowerBounds;
for (int i=0;i<numVariables;i++)
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]);
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[] = {
static const char *gReturnMsgs[] = {
"Invalid return",
"MCP_Solved: The problem was solved",
"MCP_NoProgress: A stationary point was found",
@@ -122,16 +116,16 @@ public:
"MCP_Infeasible: Problem has no solution",
"MCP_Error: An error occurred within the code",
"MCP_LicenseError: License could not be found",
"MCP_OK"
};
"MCP_OK"};
printf("ERROR: The PATH MCP solver failed: %s\n", gReturnMsgs[(unsigned int)status]);// << std::endl;
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
}
else
{
for (int i=0;i<numVariables;i++)
for (int i = 0; i < numVariables; i++)
{
x[i] = zResult[i];
//check for #NAN
@@ -139,13 +133,10 @@ public:
return false;
}
return true;
}
}
};
#endif //BT_USE_PATH
#endif //BT_USE_PATH
#endif //BT_PATH_SOLVER_H
#endif //BT_PATH_SOLVER_H

63
src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h
View File

@@ -17,25 +17,22 @@ subject to the following restrictions:
#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)
: 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)
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;
@@ -46,65 +43,65 @@ public:
btAssert(A.rows() == b.rows());
int i, j, numRows = A.rows();
btScalar delta;
for (int k = 0; k <numIterations; k++)
for (int k = 0; k < numIterations; k++)
{
m_leastSquaresResidual = 0.f;
for (i = 0; i <numRows; i++)
for (i = 0; i < numRows; i++)
{
delta = 0.0f;
if (useSparsity)
{
for (int h=0;h<A.m_rowNonZeroElements1[i].size();h++)
for (int h = 0; h < A.m_rowNonZeroElements1[i].size(); h++)
{
int j = A.m_rowNonZeroElements1[i][h];
if (j != i)//skip main diagonal
if (j != i) //skip main diagonal
{
delta += A(i,j) * x[j];
delta += A(i, j) * x[j];
}
}
} else
}
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];
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 aDiag = A(i, i);
btScalar xOld = x[i];
x [i] = (b [i] - delta) / aDiag;
x[i] = (b[i] - delta) / aDiag;
btScalar s = 1.f;
if (limitDependency[i]>=0)
if (limitDependency[i] >= 0)
{
s = x[limitDependency[i]];
if (s<0)
s=1;
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;
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;
m_leastSquaresResidual += diff * diff;
}
btScalar eps = m_leastSquaresResidualThreshold;
if ((m_leastSquaresResidual < eps) || (k >=(numIterations-1)))
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);
printf("totalLenSqr = %f at iteration #%d\n", m_leastSquaresResidual, k);
#endif
break;
}
}
return true;
}
};
#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H

550
src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
View File

@@ -24,17 +24,16 @@
#define ROLLING_INFLUENCE_FIX
btRigidBody& btActionInterface::getFixedBody()
{
static btRigidBody s_fixed(0, 0,0);
s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
static btRigidBody s_fixed(0, 0, 0);
s_fixed.setMassProps(btScalar(0.), btVector3(btScalar(0.), btScalar(0.), btScalar(0.)));
return s_fixed;
}
btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster )
:m_vehicleRaycaster(raycaster),
m_pitchControl(btScalar(0.))
btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning, btRigidBody* chassis, btVehicleRaycaster* raycaster)
: m_vehicleRaycaster(raycaster),
m_pitchControl(btScalar(0.))
{
m_chassisBody = chassis;
m_indexRightAxis = 0;
@@ -43,28 +42,22 @@ m_pitchControl(btScalar(0.))
defaultInit(tuning);
}
void btRaycastVehicle::defaultInit(const btVehicleTuning& tuning)
{
(void)tuning;
m_currentVehicleSpeedKmHour = btScalar(0.);
m_steeringValue = btScalar(0.);
}
btRaycastVehicle::~btRaycastVehicle()
{
}
//
// basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed
//
btWheelInfo& btRaycastVehicle::addWheel( const btVector3& connectionPointCS, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel)
btWheelInfo& btRaycastVehicle::addWheel(const btVector3& connectionPointCS, const btVector3& wheelDirectionCS0, const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius, const btVehicleTuning& tuning, bool isFrontWheel)
{
btWheelInfoConstructionInfo ci;
ci.m_chassisConnectionCS = connectionPointCS;
@@ -80,83 +73,76 @@ btWheelInfo& btRaycastVehicle::addWheel( const btVector3& connectionPointCS, con
ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm;
ci.m_maxSuspensionForce = tuning.m_maxSuspensionForce;
m_wheelInfo.push_back( btWheelInfo(ci));
btWheelInfo& wheel = m_wheelInfo[getNumWheels()-1];
updateWheelTransformsWS( wheel , false );
updateWheelTransform(getNumWheels()-1,false);
m_wheelInfo.push_back(btWheelInfo(ci));
btWheelInfo& wheel = m_wheelInfo[getNumWheels() - 1];
updateWheelTransformsWS(wheel, false);
updateWheelTransform(getNumWheels() - 1, false);
return wheel;
}
const btTransform& btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const
const btTransform& btRaycastVehicle::getWheelTransformWS(int wheelIndex) const
{
btAssert(wheelIndex < getNumWheels());
const btWheelInfo& wheel = m_wheelInfo[wheelIndex];
return wheel.m_worldTransform;
}
void btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedTransform)
void btRaycastVehicle::updateWheelTransform(int wheelIndex, bool interpolatedTransform)
{
btWheelInfo& wheel = m_wheelInfo[ wheelIndex ];
updateWheelTransformsWS(wheel,interpolatedTransform);
btWheelInfo& wheel = m_wheelInfo[wheelIndex];
updateWheelTransformsWS(wheel, interpolatedTransform);
btVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS;
const btVector3& right = wheel.m_raycastInfo.m_wheelAxleWS;
btVector3 fwd = up.cross(right);
fwd = fwd.normalize();
// up = right.cross(fwd);
// up.normalize();
// up = right.cross(fwd);
// up.normalize();
//rotate around steering over de wheelAxleWS
btScalar steering = wheel.m_steering;
btQuaternion steeringOrn(up,steering);//wheel.m_steering);
btQuaternion steeringOrn(up, steering); //wheel.m_steering);
btMatrix3x3 steeringMat(steeringOrn);
btQuaternion rotatingOrn(right,-wheel.m_rotation);
btQuaternion rotatingOrn(right, -wheel.m_rotation);
btMatrix3x3 rotatingMat(rotatingOrn);
btMatrix3x3 basis2;
basis2[0][m_indexRightAxis] = -right[0];
basis2[1][m_indexRightAxis] = -right[1];
basis2[2][m_indexRightAxis] = -right[2];
btMatrix3x3 basis2;
basis2[0][m_indexRightAxis] = -right[0];
basis2[1][m_indexRightAxis] = -right[1];
basis2[2][m_indexRightAxis] = -right[2];
basis2[0][m_indexUpAxis] = up[0];
basis2[1][m_indexUpAxis] = up[1];
basis2[2][m_indexUpAxis] = up[2];
basis2[0][m_indexUpAxis] = up[0];
basis2[1][m_indexUpAxis] = up[1];
basis2[2][m_indexUpAxis] = up[2];
basis2[0][m_indexForwardAxis] = fwd[0];
basis2[1][m_indexForwardAxis] = fwd[1];
basis2[2][m_indexForwardAxis] = fwd[2];
basis2[0][m_indexForwardAxis] = fwd[0];
basis2[1][m_indexForwardAxis] = fwd[1];
basis2[2][m_indexForwardAxis] = fwd[2];
wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2);
wheel.m_worldTransform.setOrigin(
wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength
);
wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength);
}
void btRaycastVehicle::resetSuspension()
{
int i;
for (i=0;i<m_wheelInfo.size(); i++)
for (i = 0; i < m_wheelInfo.size(); i++)
{
btWheelInfo& wheel = m_wheelInfo[i];
wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
wheel.m_suspensionRelativeVelocity = btScalar(0.0);
wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS;
//wheel_info.setContactFriction(btScalar(0.0));
wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
btWheelInfo& wheel = m_wheelInfo[i];
wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
wheel.m_suspensionRelativeVelocity = btScalar(0.0);
wheel.m_raycastInfo.m_contactNormalWS = -wheel.m_raycastInfo.m_wheelDirectionWS;
//wheel_info.setContactFriction(btScalar(0.0));
wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
}
}
void btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform)
void btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel, bool interpolatedTransform)
{
wheel.m_raycastInfo.m_isInContact = false;
@@ -166,19 +152,18 @@ void btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel , bool interpo
getRigidBody()->getMotionState()->getWorldTransform(chassisTrans);
}
wheel.m_raycastInfo.m_hardPointWS = chassisTrans( wheel.m_chassisConnectionPointCS );
wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() * wheel.m_wheelDirectionCS ;
wheel.m_raycastInfo.m_hardPointWS = chassisTrans(wheel.m_chassisConnectionPointCS);
wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() * wheel.m_wheelDirectionCS;
wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS;
}
btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
{
updateWheelTransformsWS( wheel,false);
updateWheelTransformsWS(wheel, false);
btScalar depth = -1;
btScalar raylen = wheel.getSuspensionRestLength()+wheel.m_wheelsRadius;
btScalar raylen = wheel.getSuspensionRestLength() + wheel.m_wheelsRadius;
btVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
const btVector3& source = wheel.m_raycastInfo.m_hardPointWS;
@@ -186,12 +171,12 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
const btVector3& target = wheel.m_raycastInfo.m_contactPointWS;
btScalar param = btScalar(0.);
btVehicleRaycaster::btVehicleRaycasterResult rayResults;
btVehicleRaycaster::btVehicleRaycasterResult rayResults;
btAssert(m_vehicleRaycaster);
void* object = m_vehicleRaycaster->castRay(source,target,rayResults);
void* object = m_vehicleRaycaster->castRay(source, target, rayResults);
wheel.m_raycastInfo.m_groundObject = 0;
@@ -199,19 +184,18 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
{
param = rayResults.m_distFraction;
depth = raylen * rayResults.m_distFraction;
wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld;
wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld;
wheel.m_raycastInfo.m_isInContact = true;
wheel.m_raycastInfo.m_groundObject = &getFixedBody();///@todo for driving on dynamic/movable objects!;
wheel.m_raycastInfo.m_groundObject = &getFixedBody(); ///@todo for driving on dynamic/movable objects!;
//wheel.m_raycastInfo.m_groundObject = object;
btScalar hitDistance = param*raylen;
btScalar hitDistance = param * raylen;
wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius;
//clamp on max suspension travel
btScalar minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm*btScalar(0.01);
btScalar maxSuspensionLength = wheel.getSuspensionRestLength()+ wheel.m_maxSuspensionTravelCm*btScalar(0.01);
btScalar minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm * btScalar(0.01);
btScalar maxSuspensionLength = wheel.getSuspensionRestLength() + wheel.m_maxSuspensionTravelCm * btScalar(0.01);
if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength)
{
wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength;
@@ -223,16 +207,16 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld;
btScalar denominator= wheel.m_raycastInfo.m_contactNormalWS.dot( wheel.m_raycastInfo.m_wheelDirectionWS );
btScalar denominator = wheel.m_raycastInfo.m_contactNormalWS.dot(wheel.m_raycastInfo.m_wheelDirectionWS);
btVector3 chassis_velocity_at_contactPoint;
btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS-getRigidBody()->getCenterOfMassPosition();
btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition();
chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos);
btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );
btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot(chassis_velocity_at_contactPoint);
if ( denominator >= btScalar(-0.1))
if (denominator >= btScalar(-0.1))
{
wheel.m_suspensionRelativeVelocity = btScalar(0.0);
wheel.m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1);
@@ -243,20 +227,19 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
wheel.m_suspensionRelativeVelocity = projVel * inv;
wheel.m_clippedInvContactDotSuspension = inv;
}
} else
}
else
{
//put wheel info as in rest position
wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
wheel.m_suspensionRelativeVelocity = btScalar(0.0);
wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS;
wheel.m_raycastInfo.m_contactNormalWS = -wheel.m_raycastInfo.m_wheelDirectionWS;
wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
}
return depth;
}
const btTransform& btRaycastVehicle::getChassisWorldTransform() const
{
/*if (getRigidBody()->getMotionState())
@@ -267,26 +250,23 @@ const btTransform& btRaycastVehicle::getChassisWorldTransform() const
}
*/
return getRigidBody()->getCenterOfMassTransform();
}
void btRaycastVehicle::updateVehicle( btScalar step )
void btRaycastVehicle::updateVehicle(btScalar step)
{
{
for (int i=0;i<getNumWheels();i++)
for (int i = 0; i < getNumWheels(); i++)
{
updateWheelTransform(i,false);
updateWheelTransform(i, false);
}
}
m_currentVehicleSpeedKmHour = btScalar(3.6) * getRigidBody()->getLinearVelocity().length();
const btTransform& chassisTrans = getChassisWorldTransform();
btVector3 forwardW (
btVector3 forwardW(
chassisTrans.getBasis()[0][m_indexForwardAxis],
chassisTrans.getBasis()[1][m_indexForwardAxis],
chassisTrans.getBasis()[2][m_indexForwardAxis]);
@@ -299,52 +279,47 @@ void btRaycastVehicle::updateVehicle( btScalar step )
//
// simulate suspension
//
int i=0;
for (i=0;i<m_wheelInfo.size();i++)
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);
for (i=0;i<m_wheelInfo.size();i++)
for (i = 0; i < m_wheelInfo.size(); i++)
{
//apply suspension force
btWheelInfo& wheel = m_wheelInfo[i];
btScalar suspensionForce = wheel.m_wheelsSuspensionForce;
if (suspensionForce > wheel.m_maxSuspensionForce)
{
suspensionForce = wheel.m_maxSuspensionForce;
}
btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition();
getRigidBody()->applyImpulse(impulse, relpos);
}
updateFriction( step);
updateFriction(step);
for (i=0;i<m_wheelInfo.size();i++)
for (i = 0; i < m_wheelInfo.size(); i++)
{
btWheelInfo& wheel = m_wheelInfo[i];
btVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition();
btVector3 vel = getRigidBody()->getVelocityInLocalPoint( relpos );
btVector3 vel = getRigidBody()->getVelocityInLocalPoint(relpos);
if (wheel.m_raycastInfo.m_isInContact)
{
const btTransform& chassisWorldTransform = getChassisWorldTransform();
const btTransform& chassisWorldTransform = getChassisWorldTransform();
btVector3 fwd (
btVector3 fwd(
chassisWorldTransform.getBasis()[0][m_indexForwardAxis],
chassisWorldTransform.getBasis()[1][m_indexForwardAxis],
chassisWorldTransform.getBasis()[2][m_indexForwardAxis]);
@@ -353,99 +328,88 @@ void btRaycastVehicle::updateVehicle( btScalar step )
fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;
btScalar proj2 = fwd.dot(vel);
wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius);
wheel.m_rotation += wheel.m_deltaRotation;
} else
}
else
{
wheel.m_rotation += wheel.m_deltaRotation;
}
wheel.m_deltaRotation *= btScalar(0.99);//damping of rotation when not in contact
wheel.m_deltaRotation *= btScalar(0.99); //damping of rotation when not in contact
}
}
void btRaycastVehicle::setSteeringValue(btScalar steering,int wheel)
void btRaycastVehicle::setSteeringValue(btScalar steering, int wheel)
{
btAssert(wheel>=0 && wheel < getNumWheels());
btAssert(wheel >= 0 && wheel < getNumWheels());
btWheelInfo& wheelInfo = getWheelInfo(wheel);
wheelInfo.m_steering = steering;
}
btScalar btRaycastVehicle::getSteeringValue(int wheel) const
btScalar btRaycastVehicle::getSteeringValue(int wheel) const
{
return getWheelInfo(wheel).m_steering;
}
void btRaycastVehicle::applyEngineForce(btScalar force, int wheel)
void btRaycastVehicle::applyEngineForce(btScalar force, int wheel)
{
btAssert(wheel>=0 && wheel < getNumWheels());
btAssert(wheel >= 0 && wheel < getNumWheels());
btWheelInfo& wheelInfo = getWheelInfo(wheel);
wheelInfo.m_engineForce = force;
}
const btWheelInfo& btRaycastVehicle::getWheelInfo(int index) const
const btWheelInfo& btRaycastVehicle::getWheelInfo(int index) const
{
btAssert((index >= 0) && (index < getNumWheels()));
btAssert((index >= 0) && (index < getNumWheels()));
return m_wheelInfo[index];
}
btWheelInfo& btRaycastVehicle::getWheelInfo(int index)
btWheelInfo& btRaycastVehicle::getWheelInfo(int index)
{
btAssert((index >= 0) && (index < getNumWheels()));
btAssert((index >= 0) && (index < getNumWheels()));
return m_wheelInfo[index];
}
void btRaycastVehicle::setBrake(btScalar brake,int wheelIndex)
void btRaycastVehicle::setBrake(btScalar brake, int wheelIndex)
{
btAssert((wheelIndex >= 0) && (wheelIndex < getNumWheels()));
btAssert((wheelIndex >= 0) && (wheelIndex < getNumWheels()));
getWheelInfo(wheelIndex).m_brake = brake;
}
void btRaycastVehicle::updateSuspension(btScalar deltaTime)
void btRaycastVehicle::updateSuspension(btScalar deltaTime)
{
(void)deltaTime;
btScalar chassisMass = btScalar(1.) / m_chassisBody->getInvMass();
for (int w_it=0; w_it<getNumWheels(); w_it++)
for (int w_it = 0; w_it < getNumWheels(); w_it++)
{
btWheelInfo &wheel_info = m_wheelInfo[w_it];
if ( wheel_info.m_raycastInfo.m_isInContact )
btWheelInfo& wheel_info = m_wheelInfo[w_it];
if (wheel_info.m_raycastInfo.m_isInContact)
{
btScalar force;
// Spring
{
btScalar susp_length = wheel_info.getSuspensionRestLength();
btScalar current_length = wheel_info.m_raycastInfo.m_suspensionLength;
btScalar susp_length = wheel_info.getSuspensionRestLength();
btScalar current_length = wheel_info.m_raycastInfo.m_suspensionLength;
btScalar length_diff = (susp_length - current_length);
force = wheel_info.m_suspensionStiffness
* length_diff * wheel_info.m_clippedInvContactDotSuspension;
force = wheel_info.m_suspensionStiffness * length_diff * wheel_info.m_clippedInvContactDotSuspension;
}
// Damper
{
btScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity;
{
btScalar susp_damping;
if ( projected_rel_vel < btScalar(0.0) )
btScalar susp_damping;
if (projected_rel_vel < btScalar(0.0))
{
susp_damping = wheel_info.m_wheelsDampingCompression;
}
@@ -469,50 +433,43 @@ void btRaycastVehicle::updateSuspension(btScalar deltaTime)
wheel_info.m_wheelsSuspensionForce = btScalar(0.0);
}
}
}
struct btWheelContactPoint
{
btRigidBody* m_body0;
btRigidBody* m_body1;
btVector3 m_frictionPositionWorld;
btVector3 m_frictionDirectionWorld;
btScalar m_jacDiagABInv;
btScalar m_maxImpulse;
btVector3 m_frictionPositionWorld;
btVector3 m_frictionDirectionWorld;
btScalar m_jacDiagABInv;
btScalar m_maxImpulse;
btWheelContactPoint(btRigidBody* body0,btRigidBody* body1,const btVector3& frictionPosWorld,const btVector3& frictionDirectionWorld, btScalar maxImpulse)
:m_body0(body0),
m_body1(body1),
m_frictionPositionWorld(frictionPosWorld),
m_frictionDirectionWorld(frictionDirectionWorld),
m_maxImpulse(maxImpulse)
btWheelContactPoint(btRigidBody* body0, btRigidBody* body1, const btVector3& frictionPosWorld, const btVector3& frictionDirectionWorld, btScalar maxImpulse)
: m_body0(body0),
m_body1(body1),
m_frictionPositionWorld(frictionPosWorld),
m_frictionDirectionWorld(frictionDirectionWorld),
m_maxImpulse(maxImpulse)
{
btScalar denom0 = body0->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld);
btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld);
btScalar relaxation = 1.f;
m_jacDiagABInv = relaxation/(denom0+denom1);
btScalar denom0 = body0->computeImpulseDenominator(frictionPosWorld, frictionDirectionWorld);
btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld, frictionDirectionWorld);
btScalar relaxation = 1.f;
m_jacDiagABInv = relaxation / (denom0 + denom1);
}
};
btScalar calcRollingFriction(btWheelContactPoint& contactPoint, int numWheelsOnGround);
btScalar calcRollingFriction(btWheelContactPoint& contactPoint, int numWheelsOnGround)
{
btScalar j1=0.f;
btScalar j1 = 0.f;
const btVector3& contactPosWorld = contactPoint.m_frictionPositionWorld;
btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition();
btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition();
btVector3 rel_pos2 = contactPosWorld - contactPoint.m_body1->getCenterOfMassPosition();
btScalar maxImpulse = contactPoint.m_maxImpulse;
btScalar maxImpulse = contactPoint.m_maxImpulse;
btVector3 vel1 = contactPoint.m_body0->getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = contactPoint.m_body1->getVelocityInLocalPoint(rel_pos2);
btVector3 vel = vel1 - vel2;
@@ -520,253 +477,225 @@ btScalar calcRollingFriction(btWheelContactPoint& contactPoint, int numWheelsOnG
btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel);
// calculate j that moves us to zero relative velocity
j1 = -vrel * contactPoint.m_jacDiagABInv/btScalar(numWheelsOnGround);
j1 = -vrel * contactPoint.m_jacDiagABInv / btScalar(numWheelsOnGround);
btSetMin(j1, maxImpulse);
btSetMax(j1, -maxImpulse);
return j1;
}
btScalar sideFrictionStiffness2 = btScalar(1.0);
void btRaycastVehicle::updateFriction(btScalar timeStep)
void btRaycastVehicle::updateFriction(btScalar timeStep)
{
//calculate the impulse, so that the wheels don't move sidewards
int numWheel = getNumWheels();
if (!numWheel)
return;
//calculate the impulse, so that the wheels don't move sidewards
int numWheel = getNumWheels();
if (!numWheel)
return;
m_forwardWS.resize(numWheel);
m_axle.resize(numWheel);
m_forwardImpulse.resize(numWheel);
m_sideImpulse.resize(numWheel);
m_forwardWS.resize(numWheel);
m_axle.resize(numWheel);
m_forwardImpulse.resize(numWheel);
m_sideImpulse.resize(numWheel);
int numWheelsOnGround = 0;
int numWheelsOnGround = 0;
//collapse all those loops into one!
for (int i=0;i<getNumWheels();i++)
//collapse all those loops into one!
for (int i = 0; i < getNumWheels(); i++)
{
btWheelInfo& wheelInfo = m_wheelInfo[i];
class btRigidBody* groundObject = (class btRigidBody*)wheelInfo.m_raycastInfo.m_groundObject;
if (groundObject)
numWheelsOnGround++;
m_sideImpulse[i] = btScalar(0.);
m_forwardImpulse[i] = btScalar(0.);
}
{
for (int i = 0; i < getNumWheels(); i++)
{
btWheelInfo& wheelInfo = m_wheelInfo[i];
class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
class btRigidBody* groundObject = (class btRigidBody*)wheelInfo.m_raycastInfo.m_groundObject;
if (groundObject)
numWheelsOnGround++;
m_sideImpulse[i] = btScalar(0.);
m_forwardImpulse[i] = btScalar(0.);
}
{
for (int i=0;i<getNumWheels();i++)
{
const btTransform& wheelTrans = getWheelTransformWS(i);
btWheelInfo& wheelInfo = m_wheelInfo[i];
class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
m_axle[i] = -btVector3(
wheelBasis0[0][m_indexRightAxis],
wheelBasis0[1][m_indexRightAxis],
wheelBasis0[2][m_indexRightAxis]);
if (groundObject)
{
const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
btScalar proj = m_axle[i].dot(surfNormalWS);
m_axle[i] -= surfNormalWS * proj;
m_axle[i] = m_axle[i].normalize();
const btTransform& wheelTrans = getWheelTransformWS( i );
m_forwardWS[i] = surfNormalWS.cross(m_axle[i]);
m_forwardWS[i].normalize();
btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
m_axle[i] = -btVector3(
wheelBasis0[0][m_indexRightAxis],
wheelBasis0[1][m_indexRightAxis],
wheelBasis0[2][m_indexRightAxis]);
const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
btScalar proj = m_axle[i].dot(surfNormalWS);
m_axle[i] -= surfNormalWS * proj;
m_axle[i] = m_axle[i].normalize();
m_forwardWS[i] = surfNormalWS.cross(m_axle[i]);
m_forwardWS[i].normalize();
resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
*groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
btScalar(0.), m_axle[i],m_sideImpulse[i],timeStep);
m_sideImpulse[i] *= sideFrictionStiffness2;
}
resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
*groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
btScalar(0.), m_axle[i], m_sideImpulse[i], timeStep);
m_sideImpulse[i] *= sideFrictionStiffness2;
}
}
}
btScalar sideFactor = btScalar(1.);
btScalar fwdFactor = 0.5;
bool sliding = false;
{
for (int wheel =0;wheel <getNumWheels();wheel++)
for (int wheel = 0; wheel < getNumWheels(); wheel++)
{
btWheelInfo& wheelInfo = m_wheelInfo[wheel];
class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
class btRigidBody* groundObject = (class btRigidBody*)wheelInfo.m_raycastInfo.m_groundObject;
btScalar rollingFriction = 0.f;
btScalar rollingFriction = 0.f;
if (groundObject)
{
if (wheelInfo.m_engineForce != 0.f)
{
rollingFriction = wheelInfo.m_engineForce* timeStep;
} else
rollingFriction = wheelInfo.m_engineForce * timeStep;
}
else
{
btScalar defaultRollingFrictionImpulse = 0.f;
btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse);
btWheelContactPoint contactPt(m_chassisBody, groundObject, wheelInfo.m_raycastInfo.m_contactPointWS, m_forwardWS[wheel], maxImpulse);
btAssert(numWheelsOnGround > 0);
rollingFriction = calcRollingFriction(contactPt, numWheelsOnGround);
}
}
//switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
m_forwardImpulse[wheel] = btScalar(0.);
m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
m_wheelInfo[wheel].m_skidInfo = btScalar(1.);
if (groundObject)
{
m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
m_wheelInfo[wheel].m_skidInfo = btScalar(1.);
btScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
btScalar maximpSide = maximp;
btScalar maximpSquared = maximp * maximpSide;
m_forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep;
m_forwardImpulse[wheel] = rollingFriction; //wheelInfo.m_engineForce* timeStep;
btScalar x = (m_forwardImpulse[wheel] ) * fwdFactor;
btScalar y = (m_sideImpulse[wheel] ) * sideFactor;
btScalar impulseSquared = (x*x + y*y);
btScalar x = (m_forwardImpulse[wheel]) * fwdFactor;
btScalar y = (m_sideImpulse[wheel]) * sideFactor;
btScalar impulseSquared = (x * x + y * y);
if (impulseSquared > maximpSquared)
{
sliding = true;
btScalar factor = maximp / btSqrt(impulseSquared);
m_wheelInfo[wheel].m_skidInfo *= factor;
}
}
}
}
}
if (sliding)
if (sliding)
{
for (int wheel = 0; wheel < getNumWheels(); wheel++)
{
for (int wheel = 0;wheel < getNumWheels(); wheel++)
if (m_sideImpulse[wheel] != btScalar(0.))
{
if (m_sideImpulse[wheel] != btScalar(0.))
if (m_wheelInfo[wheel].m_skidInfo < btScalar(1.))
{
if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.))
{
m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
}
m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
}
}
}
}
// apply the impulses
// apply the impulses
{
for (int wheel = 0; wheel < getNumWheels(); wheel++)
{
for (int wheel = 0;wheel<getNumWheels() ; wheel++)
btWheelInfo& wheelInfo = m_wheelInfo[wheel];
btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
m_chassisBody->getCenterOfMassPosition();
if (m_forwardImpulse[wheel] != btScalar(0.))
{
btWheelInfo& wheelInfo = m_wheelInfo[wheel];
m_chassisBody->applyImpulse(m_forwardWS[wheel] * (m_forwardImpulse[wheel]), rel_pos);
}
if (m_sideImpulse[wheel] != btScalar(0.))
{
class btRigidBody* groundObject = (class btRigidBody*)m_wheelInfo[wheel].m_raycastInfo.m_groundObject;
btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
m_chassisBody->getCenterOfMassPosition();
btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS -
groundObject->getCenterOfMassPosition();
if (m_forwardImpulse[wheel] != btScalar(0.))
{
m_chassisBody->applyImpulse(m_forwardWS[wheel]*(m_forwardImpulse[wheel]),rel_pos);
}
if (m_sideImpulse[wheel] != btScalar(0.))
{
class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject;
btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS -
groundObject->getCenterOfMassPosition();
btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
#if defined ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT.
btVector3 vChassisWorldUp = getRigidBody()->getCenterOfMassTransform().getBasis().getColumn(m_indexUpAxis);
rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f-wheelInfo.m_rollInfluence));
#if defined ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT.
btVector3 vChassisWorldUp = getRigidBody()->getCenterOfMassTransform().getBasis().getColumn(m_indexUpAxis);
rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f - wheelInfo.m_rollInfluence));
#else
rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
#endif
m_chassisBody->applyImpulse(sideImp,rel_pos);
m_chassisBody->applyImpulse(sideImp, rel_pos);
//apply friction impulse on the ground
groundObject->applyImpulse(-sideImp,rel_pos2);
}
//apply friction impulse on the ground
groundObject->applyImpulse(-sideImp, rel_pos2);
}
}
}
}
void btRaycastVehicle::debugDraw(btIDebugDraw* debugDrawer)
void btRaycastVehicle::debugDraw(btIDebugDraw* debugDrawer)
{
for (int v=0;v<this->getNumWheels();v++)
for (int v = 0; v < this->getNumWheels(); v++)
{
btVector3 wheelColor(0,1,1);
btVector3 wheelColor(0, 1, 1);
if (getWheelInfo(v).m_raycastInfo.m_isInContact)
{
wheelColor.setValue(0,0,1);
} else
wheelColor.setValue(0, 0, 1);
}
else
{
wheelColor.setValue(1,0,1);
wheelColor.setValue(1, 0, 1);
}
btVector3 wheelPosWS = getWheelInfo(v).m_worldTransform.getOrigin();
btVector3 axle = btVector3(
btVector3 axle = btVector3(
getWheelInfo(v).m_worldTransform.getBasis()[0][getRightAxis()],
getWheelInfo(v).m_worldTransform.getBasis()[1][getRightAxis()],
getWheelInfo(v).m_worldTransform.getBasis()[2][getRightAxis()]);
//debug wheels (cylinders)
debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
debugDrawer->drawLine(wheelPosWS,getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
debugDrawer->drawLine(wheelPosWS, wheelPosWS + axle, wheelColor);
debugDrawer->drawLine(wheelPosWS, getWheelInfo(v).m_raycastInfo.m_contactPointWS, wheelColor);
}
}
void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result)
void* btDefaultVehicleRaycaster::castRay(const btVector3& from, const btVector3& to, btVehicleRaycasterResult& result)
{
// RayResultCallback& resultCallback;
// RayResultCallback& resultCallback;
btCollisionWorld::ClosestRayResultCallback rayCallback(from,to);
btCollisionWorld::ClosestRayResultCallback rayCallback(from, to);
m_dynamicsWorld->rayTest(from, to, rayCallback);
if (rayCallback.hasHit())
{
const btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject);
if (body && body->hasContactResponse())
if (body && body->hasContactResponse())
{
result.m_hitPointInWorld = rayCallback.m_hitPointWorld;
result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld;
@@ -777,4 +706,3 @@ void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3&
}
return 0;
}

156
src/BulletDynamics/Vehicle/btRaycastVehicle.h
View File

@@ -24,122 +24,111 @@ class btDynamicsWorld;
///rayCast vehicle, very special constraint that turn a rigidbody into a vehicle.
class btRaycastVehicle : public btActionInterface
{
btAlignedObjectArray<btVector3> m_forwardWS;
btAlignedObjectArray<btVector3> m_axle;
btAlignedObjectArray<btScalar> m_forwardImpulse;
btAlignedObjectArray<btScalar> m_sideImpulse;
btAlignedObjectArray<btVector3> m_forwardWS;
btAlignedObjectArray<btVector3> m_axle;
btAlignedObjectArray<btScalar> m_forwardImpulse;
btAlignedObjectArray<btScalar> m_sideImpulse;
///backwards compatibility
int m_userConstraintType;
int m_userConstraintId;
///backwards compatibility
int m_userConstraintType;
int m_userConstraintId;
public:
class btVehicleTuning
{
public:
btVehicleTuning()
: m_suspensionStiffness(btScalar(5.88)),
m_suspensionCompression(btScalar(0.83)),
m_suspensionDamping(btScalar(0.88)),
m_maxSuspensionTravelCm(btScalar(500.)),
m_frictionSlip(btScalar(10.5)),
m_maxSuspensionForce(btScalar(6000.))
{
public:
}
btScalar m_suspensionStiffness;
btScalar m_suspensionCompression;
btScalar m_suspensionDamping;
btScalar m_maxSuspensionTravelCm;
btScalar m_frictionSlip;
btScalar m_maxSuspensionForce;
};
btVehicleTuning()
:m_suspensionStiffness(btScalar(5.88)),
m_suspensionCompression(btScalar(0.83)),
m_suspensionDamping(btScalar(0.88)),
m_maxSuspensionTravelCm(btScalar(500.)),
m_frictionSlip(btScalar(10.5)),
m_maxSuspensionForce(btScalar(6000.))
{
}
btScalar m_suspensionStiffness;
btScalar m_suspensionCompression;
btScalar m_suspensionDamping;
btScalar m_maxSuspensionTravelCm;
btScalar m_frictionSlip;
btScalar m_maxSuspensionForce;
};
private:
btVehicleRaycaster* m_vehicleRaycaster;
btScalar m_pitchControl;
btScalar m_steeringValue;
btVehicleRaycaster* m_vehicleRaycaster;
btScalar m_pitchControl;
btScalar m_steeringValue;
btScalar m_currentVehicleSpeedKmHour;
btRigidBody* m_chassisBody;
int m_indexRightAxis;
int m_indexUpAxis;
int m_indexForwardAxis;
int m_indexForwardAxis;
void defaultInit(const btVehicleTuning& tuning);
public:
//constructor to create a car from an existing rigidbody
btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster );
virtual ~btRaycastVehicle() ;
btRaycastVehicle(const btVehicleTuning& tuning, btRigidBody* chassis, btVehicleRaycaster* raycaster);
virtual ~btRaycastVehicle();
///btActionInterface interface
virtual void updateAction( btCollisionWorld* collisionWorld, btScalar step)
virtual void updateAction(btCollisionWorld* collisionWorld, btScalar step)
{
(void) collisionWorld;
(void)collisionWorld;
updateVehicle(step);
}
///btActionInterface interface
void debugDraw(btIDebugDraw* debugDrawer);
void debugDraw(btIDebugDraw* debugDrawer);
const btTransform& getChassisWorldTransform() const;
btScalar rayCast(btWheelInfo& wheel);
virtual void updateVehicle(btScalar step);
void resetSuspension();
btScalar getSteeringValue(int wheel) const;
btScalar getSteeringValue(int wheel) const;
void setSteeringValue(btScalar steering,int wheel);
void setSteeringValue(btScalar steering, int wheel);
void applyEngineForce(btScalar force, int wheel);
void applyEngineForce(btScalar force, int wheel);
const btTransform& getWheelTransformWS(int wheelIndex) const;
const btTransform& getWheelTransformWS( int wheelIndex ) const;
void updateWheelTransform(int wheelIndex, bool interpolatedTransform = true);
void updateWheelTransform( int wheelIndex, bool interpolatedTransform = true );
// void setRaycastWheelInfo( int wheelIndex , bool isInContact, const btVector3& hitPoint, const btVector3& hitNormal,btScalar depth);
// void setRaycastWheelInfo( int wheelIndex , bool isInContact, const btVector3& hitPoint, const btVector3& hitNormal,btScalar depth);
btWheelInfo& addWheel( const btVector3& connectionPointCS0, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS,btScalar suspensionRestLength,btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel);
btWheelInfo& addWheel(const btVector3& connectionPointCS0, const btVector3& wheelDirectionCS0, const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius, const btVehicleTuning& tuning, bool isFrontWheel);
inline int getNumWheels() const {
return int (m_wheelInfo.size());
inline int getNumWheels() const
{
return int(m_wheelInfo.size());
}
btAlignedObjectArray<btWheelInfo> m_wheelInfo;
btAlignedObjectArray<btWheelInfo> m_wheelInfo;
const btWheelInfo& getWheelInfo(int index) const;
const btWheelInfo& getWheelInfo(int index) const;
btWheelInfo& getWheelInfo(int index);
btWheelInfo& getWheelInfo(int index);
void updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform = true);
void updateWheelTransformsWS(btWheelInfo& wheel, bool interpolatedTransform = true);
void setBrake(btScalar brake,int wheelIndex);
void setBrake(btScalar brake, int wheelIndex);
void setPitchControl(btScalar pitch)
void setPitchControl(btScalar pitch)
{
m_pitchControl = pitch;
}
void updateSuspension(btScalar deltaTime);
virtual void updateFriction(btScalar timeStep);
void updateSuspension(btScalar deltaTime);
virtual void updateFriction(btScalar timeStep);
inline btRigidBody* getRigidBody()
{
@@ -151,7 +140,7 @@ public:
return m_chassisBody;
}
inline int getRightAxis() const
inline int getRightAxis() const
{
return m_indexRightAxis;
}
@@ -165,46 +154,44 @@ public:
return m_indexForwardAxis;
}
///Worldspace forward vector
btVector3 getForwardVector() const
{
const btTransform& chassisTrans = getChassisWorldTransform();
const btTransform& chassisTrans = getChassisWorldTransform();
btVector3 forwardW (
chassisTrans.getBasis()[0][m_indexForwardAxis],
chassisTrans.getBasis()[1][m_indexForwardAxis],
chassisTrans.getBasis()[2][m_indexForwardAxis]);
btVector3 forwardW(
chassisTrans.getBasis()[0][m_indexForwardAxis],
chassisTrans.getBasis()[1][m_indexForwardAxis],
chassisTrans.getBasis()[2][m_indexForwardAxis]);
return forwardW;
}
///Velocity of vehicle (positive if velocity vector has same direction as foward vector)
btScalar getCurrentSpeedKmHour() const
btScalar getCurrentSpeedKmHour() const
{
return m_currentVehicleSpeedKmHour;
}
virtual void setCoordinateSystem(int rightIndex,int upIndex,int forwardIndex)
virtual void setCoordinateSystem(int rightIndex, int upIndex, int forwardIndex)
{
m_indexRightAxis = rightIndex;
m_indexUpAxis = upIndex;
m_indexForwardAxis = forwardIndex;
}
///backwards compatibility
int getUserConstraintType() const
{
return m_userConstraintType ;
return m_userConstraintType;
}
void setUserConstraintType(int userConstraintType)
void setUserConstraintType(int userConstraintType)
{
m_userConstraintType = userConstraintType;
};
void setUserConstraintId(int uid)
void setUserConstraintId(int uid)
{
m_userConstraintId = uid;
}
@@ -213,22 +200,19 @@ public:
{
return m_userConstraintId;
}
};
class btDefaultVehicleRaycaster : public btVehicleRaycaster
{
btDynamicsWorld* m_dynamicsWorld;
btDynamicsWorld* m_dynamicsWorld;
public:
btDefaultVehicleRaycaster(btDynamicsWorld* world)
:m_dynamicsWorld(world)
: m_dynamicsWorld(world)
{
}
virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result);
virtual void* castRay(const btVector3& from, const btVector3& to, btVehicleRaycasterResult& result);
};
#endif //BT_RAYCASTVEHICLE_H
#endif //BT_RAYCASTVEHICLE_H

20
src/BulletDynamics/Vehicle/btVehicleRaycaster.h
View File

@@ -16,20 +16,18 @@
/// btVehicleRaycaster is provides interface for between vehicle simulation and raycasting
struct btVehicleRaycaster
{
virtual ~btVehicleRaycaster()
{
}
virtual ~btVehicleRaycaster()
{
}
struct btVehicleRaycasterResult
{
btVehicleRaycasterResult() :m_distFraction(btScalar(-1.)){};
btVector3 m_hitPointInWorld;
btVector3 m_hitNormalInWorld;
btScalar m_distFraction;
btVehicleRaycasterResult() : m_distFraction(btScalar(-1.)){};
btVector3 m_hitPointInWorld;
btVector3 m_hitNormalInWorld;
btScalar m_distFraction;
};
virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result) = 0;
virtual void* castRay(const btVector3& from, const btVector3& to, btVehicleRaycasterResult& result) = 0;
};
#endif //BT_VEHICLE_RAYCASTER_H
#endif //BT_VEHICLE_RAYCASTER_H

21
src/BulletDynamics/Vehicle/btWheelInfo.cpp
View File

@@ -9,30 +9,26 @@
* It is provided "as is" without express or implied warranty.
*/
#include "btWheelInfo.h"
#include "BulletDynamics/Dynamics/btRigidBody.h" // for pointvelocity
#include "BulletDynamics/Dynamics/btRigidBody.h" // for pointvelocity
btScalar btWheelInfo::getSuspensionRestLength() const
{
return m_suspensionRestLength1;
}
void btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo)
void btWheelInfo::updateWheel(const btRigidBody& chassis, RaycastInfo& raycastInfo)
{
(void)raycastInfo;
if (m_raycastInfo.m_isInContact)
{
btScalar project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS );
btVector3 chassis_velocity_at_contactPoint;
btScalar project = m_raycastInfo.m_contactNormalWS.dot(m_raycastInfo.m_wheelDirectionWS);
btVector3 chassis_velocity_at_contactPoint;
btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition();
chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos );
btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );
if ( project >= btScalar(-0.1))
chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint(relpos);
btScalar projVel = m_raycastInfo.m_contactNormalWS.dot(chassis_velocity_at_contactPoint);
if (project >= btScalar(-0.1))
{
m_suspensionRelativeVelocity = btScalar(0.0);
m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1);
@@ -43,10 +39,9 @@ void btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInf
m_suspensionRelativeVelocity = projVel * inv;
m_clippedInvContactDotSuspension = inv;
}
}
else // Not in contact : position wheel in a nice (rest length) position
else // Not in contact : position wheel in a nice (rest length) position
{
m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength();
m_suspensionRelativeVelocity = btScalar(0.0);

103
src/BulletDynamics/Vehicle/btWheelInfo.h
View File

@@ -18,20 +18,19 @@ class btRigidBody;
struct btWheelInfoConstructionInfo
{
btVector3 m_chassisConnectionCS;
btVector3 m_wheelDirectionCS;
btVector3 m_wheelAxleCS;
btScalar m_suspensionRestLength;
btScalar m_maxSuspensionTravelCm;
btScalar m_wheelRadius;
btScalar m_suspensionStiffness;
btScalar m_wheelsDampingCompression;
btScalar m_wheelsDampingRelaxation;
btScalar m_frictionSlip;
btScalar m_maxSuspensionForce;
btVector3 m_chassisConnectionCS;
btVector3 m_wheelDirectionCS;
btVector3 m_wheelAxleCS;
btScalar m_suspensionRestLength;
btScalar m_maxSuspensionTravelCm;
btScalar m_wheelRadius;
btScalar m_suspensionStiffness;
btScalar m_wheelsDampingCompression;
btScalar m_wheelsDampingRelaxation;
btScalar m_frictionSlip;
btScalar m_maxSuspensionForce;
bool m_bIsFrontWheel;
};
/// btWheelInfo contains information per wheel about friction and suspension.
@@ -40,51 +39,50 @@ struct btWheelInfo
struct RaycastInfo
{
//set by raycaster
btVector3 m_contactNormalWS;//contactnormal
btVector3 m_contactPointWS;//raycast hitpoint
btScalar m_suspensionLength;
btVector3 m_hardPointWS;//raycast starting point
btVector3 m_wheelDirectionWS; //direction in worldspace
btVector3 m_wheelAxleWS; // axle in worldspace
bool m_isInContact;
void* m_groundObject; //could be general void* ptr
btVector3 m_contactNormalWS; //contactnormal
btVector3 m_contactPointWS; //raycast hitpoint
btScalar m_suspensionLength;
btVector3 m_hardPointWS; //raycast starting point
btVector3 m_wheelDirectionWS; //direction in worldspace
btVector3 m_wheelAxleWS; // axle in worldspace
bool m_isInContact;
void* m_groundObject; //could be general void* ptr
};
RaycastInfo m_raycastInfo;
RaycastInfo m_raycastInfo;
btTransform m_worldTransform;
btVector3 m_chassisConnectionPointCS; //const
btVector3 m_wheelDirectionCS;//const
btVector3 m_wheelAxleCS; // const or modified by steering
btScalar m_suspensionRestLength1;//const
btScalar m_maxSuspensionTravelCm;
btTransform m_worldTransform;
btVector3 m_chassisConnectionPointCS; //const
btVector3 m_wheelDirectionCS; //const
btVector3 m_wheelAxleCS; // const or modified by steering
btScalar m_suspensionRestLength1; //const
btScalar m_maxSuspensionTravelCm;
btScalar getSuspensionRestLength() const;
btScalar m_wheelsRadius;//const
btScalar m_suspensionStiffness;//const
btScalar m_wheelsDampingCompression;//const
btScalar m_wheelsDampingRelaxation;//const
btScalar m_frictionSlip;
btScalar m_steering;
btScalar m_rotation;
btScalar m_deltaRotation;
btScalar m_rollInfluence;
btScalar m_maxSuspensionForce;
btScalar m_wheelsRadius; //const
btScalar m_suspensionStiffness; //const
btScalar m_wheelsDampingCompression; //const
btScalar m_wheelsDampingRelaxation; //const
btScalar m_frictionSlip;
btScalar m_steering;
btScalar m_rotation;
btScalar m_deltaRotation;
btScalar m_rollInfluence;
btScalar m_maxSuspensionForce;
btScalar m_engineForce;
btScalar m_engineForce;
btScalar m_brake;
btScalar m_brake;
bool m_bIsFrontWheel;
void* m_clientInfo;//can be used to store pointer to sync transforms...
void* m_clientInfo; //can be used to store pointer to sync transforms...
btWheelInfo() {}
btWheelInfo(btWheelInfoConstructionInfo& ci)
{
m_suspensionRestLength1 = ci.m_suspensionRestLength;
m_maxSuspensionTravelCm = ci.m_maxSuspensionTravelCm;
@@ -104,18 +102,15 @@ struct btWheelInfo
m_rollInfluence = btScalar(0.1);
m_bIsFrontWheel = ci.m_bIsFrontWheel;
m_maxSuspensionForce = ci.m_maxSuspensionForce;
}
void updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo);
void updateWheel(const btRigidBody& chassis, RaycastInfo& raycastInfo);
btScalar m_clippedInvContactDotSuspension;
btScalar m_suspensionRelativeVelocity;
btScalar m_clippedInvContactDotSuspension;
btScalar m_suspensionRelativeVelocity;
//calculated by suspension
btScalar m_wheelsSuspensionForce;
btScalar m_skidInfo;
btScalar m_wheelsSuspensionForce;
btScalar m_skidInfo;
};
#endif //BT_WHEEL_INFO_H
#endif //BT_WHEEL_INFO_H