mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-16 06:30:05 +00:00
process contact and non-contact constraints inside the same iteration loop
added first draft for hingeConstraint motor
This commit is contained in:
ejcoumans
parent
a4541d2470
commit
6dff5a218e
@ -84,6 +84,8 @@ void ConstraintDemo::initPhysics()
|
||||
btConstraintSolver* constraintSolver = new btSequentialImpulseConstraintSolver();
|
||||
//ConstraintSolver* solver = new OdeConstraintSolver;
|
||||
m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver);
|
||||
|
||||
//m_dynamicsWorld->setGravity(btVector3(0,0,0));
|
||||
|
||||
m_dynamicsWorld->setDebugDrawer(&debugDrawer);
|
||||
|
||||
@ -92,16 +94,16 @@ void ConstraintDemo::initPhysics()
|
||||
trans.setIdentity();
|
||||
trans.setOrigin(btVector3(0,20,0));
|
||||
|
||||
float mass = 0.f;
|
||||
float mass = 1.f;
|
||||
//point to point constraint (ball socket)
|
||||
{
|
||||
btRigidBody* body0 = localCreateRigidBody( mass,trans,shape);
|
||||
trans.setOrigin(btVector3(2*CUBE_HALF_EXTENTS,20,0));
|
||||
|
||||
mass = 1.f;
|
||||
btRigidBody* body1 = localCreateRigidBody( mass,trans,shape);
|
||||
body1->setActivationState(DISABLE_DEACTIVATION);
|
||||
body1->setDamping(0.3,0.3);
|
||||
btRigidBody* body1 = 0;//localCreateRigidBody( mass,trans,shape);
|
||||
//body1->setActivationState(DISABLE_DEACTIVATION);
|
||||
//body1->setDamping(0.3,0.3);
|
||||
|
||||
btVector3 pivotInA(CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS);
|
||||
btVector3 axisInA(0,0,1);
|
||||
@ -112,7 +114,15 @@ void ConstraintDemo::initPhysics()
|
||||
body0->getCenterOfMassTransform().getBasis() * axisInA;
|
||||
|
||||
//btTypedConstraint* p2p = new btPoint2PointConstraint(*body0,*body1,pivotInA,pivotInB);
|
||||
btTypedConstraint* hinge = new btHingeConstraint(*body0,*body1,pivotInA,pivotInB,axisInA,axisInB);
|
||||
//btTypedConstraint* hinge = new btHingeConstraint(*body0,*body1,pivotInA,pivotInB,axisInA,axisInB);
|
||||
btHingeConstraint* hinge = new btHingeConstraint(*body0,pivotInA,axisInA);
|
||||
|
||||
//use zero targetVelocity and a small maxMotorImpulse to simulate joint friction
|
||||
//float targetVelocity = 0.f;
|
||||
//float maxMotorImpulse = 0.01;
|
||||
float targetVelocity = 1.f;
|
||||
float maxMotorImpulse = 1.0f;
|
||||
hinge->enableAngularMotor(true,targetVelocity,maxMotorImpulse);
|
||||
|
||||
m_dynamicsWorld->addConstraint(hinge);//p2p);
|
||||
|
||||
|
||||
@ -70,7 +70,7 @@ m_erp(0.4f)
|
||||
|
||||
|
||||
//iterative lcp and penalty method
|
||||
float OdeConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
|
||||
float OdeConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
|
||||
{
|
||||
m_CurBody = 0;
|
||||
m_CurJoint = 0;
|
||||
|
||||
@ -45,7 +45,7 @@ public:
|
||||
|
||||
virtual ~OdeConstraintSolver() {}
|
||||
|
||||
virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info,btIDebugDraw* debugDrawer = 0);
|
||||
virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info,btIDebugDraw* debugDrawer = 0);
|
||||
|
||||
///setConstraintForceMixing, the cfm adds some positive value to the main diagonal
|
||||
///This can improve convergence (make matrix positive semidefinite), but it can make the simulation look more 'springy'
|
||||
|
||||
@ -268,17 +268,44 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
|
||||
int startManifoldIndex = 0;
|
||||
int endManifoldIndex = 1;
|
||||
|
||||
for (startManifoldIndex=0;startManifoldIndex<numManifolds;startManifoldIndex = endManifoldIndex)
|
||||
int islandId;
|
||||
|
||||
|
||||
//update the sleeping state for bodies, if all are sleeping
|
||||
for (int startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
|
||||
{
|
||||
int islandId = getIslandId(islandmanifold[startManifoldIndex]);
|
||||
for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(islandmanifold[endManifoldIndex]));endManifoldIndex++)
|
||||
int islandId = getUnionFind().getElement(startIslandIndex).m_id;
|
||||
|
||||
for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
|
||||
{
|
||||
}
|
||||
/// Process the actual simulation, only if not sleeping/deactivated
|
||||
int numIslandManifolds = endManifoldIndex-startManifoldIndex;
|
||||
|
||||
//find the accompanying contact manifold for this islandId
|
||||
int numIslandManifolds = 0;
|
||||
btPersistentManifold** startManifold = 0;
|
||||
|
||||
if (startManifoldIndex<numManifolds)
|
||||
{
|
||||
int curIslandId = getIslandId(islandmanifold[startManifoldIndex]);
|
||||
if (curIslandId == islandId)
|
||||
{
|
||||
startManifold = &islandmanifold[startManifoldIndex];
|
||||
|
||||
for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(islandmanifold[endManifoldIndex]));endManifoldIndex++)
|
||||
{
|
||||
|
||||
}
|
||||
/// Process the actual simulation, only if not sleeping/deactivated
|
||||
numIslandManifolds = endManifoldIndex-startManifoldIndex;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
callback->ProcessIsland(startManifold,numIslandManifolds, islandId);
|
||||
|
||||
if (numIslandManifolds)
|
||||
{
|
||||
callback->ProcessIsland(&islandmanifold[startManifoldIndex],numIslandManifolds);
|
||||
startManifoldIndex = endManifoldIndex;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -49,7 +49,7 @@ public:
|
||||
{
|
||||
virtual ~IslandCallback() {};
|
||||
|
||||
virtual void ProcessIsland(class btPersistentManifold** manifolds,int numManifolds) = 0;
|
||||
virtual void ProcessIsland(class btPersistentManifold** manifolds,int numManifolds, int islandId) = 0;
|
||||
};
|
||||
|
||||
void buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback);
|
||||
|
||||
@ -18,7 +18,7 @@ subject to the following restrictions:
|
||||
|
||||
class btPersistentManifold;
|
||||
class btRigidBody;
|
||||
|
||||
class btTypedConstraint;
|
||||
struct btContactSolverInfo;
|
||||
struct btBroadphaseProxy;
|
||||
class btIDebugDraw;
|
||||
@ -31,7 +31,7 @@ public:
|
||||
|
||||
virtual ~btConstraintSolver() {}
|
||||
|
||||
virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info,class btIDebugDraw* debugDrawer = 0) = 0;
|
||||
virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer = 0) = 0;
|
||||
|
||||
};
|
||||
|
||||
|
||||
@ -289,7 +289,7 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep)
|
||||
btScalar loLimit = m_upperLimit[i+3] > -3.1415 ? m_lowerLimit[i+3] : -1e30f;
|
||||
btScalar hiLimit = m_upperLimit[i+3] < 3.1415 ? m_upperLimit[i+3] : 1e30f;
|
||||
|
||||
float projAngle = -2.*xyz[i];
|
||||
float projAngle = -2.f*xyz[i];
|
||||
|
||||
if (projAngle < loLimit)
|
||||
{
|
||||
|
||||
@ -19,7 +19,8 @@ subject to the following restrictions:
|
||||
#include "LinearMath/btTransformUtil.h"
|
||||
|
||||
|
||||
btHingeConstraint::btHingeConstraint()
|
||||
btHingeConstraint::btHingeConstraint():
|
||||
m_enableAngularMotor(false)
|
||||
{
|
||||
}
|
||||
|
||||
@ -28,7 +29,8 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
|
||||
:btTypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
|
||||
m_axisInA(axisInA),
|
||||
m_axisInB(-axisInB),
|
||||
m_angularOnly(false)
|
||||
m_angularOnly(false),
|
||||
m_enableAngularMotor(false)
|
||||
{
|
||||
|
||||
}
|
||||
@ -39,7 +41,8 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,
|
||||
m_axisInA(axisInA),
|
||||
//fixed axis in worldspace
|
||||
m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA),
|
||||
m_angularOnly(false)
|
||||
m_angularOnly(false),
|
||||
m_enableAngularMotor(false)
|
||||
{
|
||||
|
||||
}
|
||||
@ -73,11 +76,16 @@ void btHingeConstraint::buildJacobian()
|
||||
//these two jointAxis require equal angular velocities for both bodies
|
||||
|
||||
//this is unused for now, it's a todo
|
||||
btVector3 axisWorldA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
|
||||
btVector3 jointAxis0;
|
||||
btVector3 jointAxis1;
|
||||
btPlaneSpace1(axisWorldA,jointAxis0,jointAxis1);
|
||||
btVector3 jointAxis0local;
|
||||
btVector3 jointAxis1local;
|
||||
|
||||
btPlaneSpace1(m_axisInA,jointAxis0local,jointAxis1local);
|
||||
|
||||
getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
|
||||
btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
|
||||
btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
|
||||
btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
|
||||
|
||||
new (&m_jacAng[0]) btJacobianEntry(jointAxis0,
|
||||
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
|
||||
@ -90,107 +98,18 @@ void btHingeConstraint::buildJacobian()
|
||||
m_rbA.getInvInertiaDiagLocal(),
|
||||
m_rbB.getInvInertiaDiagLocal());
|
||||
|
||||
new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld,
|
||||
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbA.getInvInertiaDiagLocal(),
|
||||
m_rbB.getInvInertiaDiagLocal());
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
void btHingeConstraint::solveConstraint(btScalar timeStep)
|
||||
{
|
||||
//#define NEW_IMPLEMENTATION
|
||||
|
||||
#ifdef NEW_IMPLEMENTATION
|
||||
btScalar tau = 0.3f;
|
||||
btScalar damping = 1.f;
|
||||
|
||||
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
|
||||
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
|
||||
|
||||
// Dirk: Don't we need to update this after each applied impulse
|
||||
btVector3 angvelA; // = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
|
||||
btVector3 angvelB; // = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
|
||||
|
||||
|
||||
if (!m_angularOnly)
|
||||
{
|
||||
btVector3 normal(0,0,0);
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
normal[i] = 1;
|
||||
btScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
|
||||
|
||||
btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
|
||||
|
||||
btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
// Dirk: Get new angular velocity since it changed after applying an impulse
|
||||
angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
|
||||
angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
|
||||
|
||||
//velocity error (first order error)
|
||||
btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
|
||||
m_rbB.getLinearVelocity(),angvelB);
|
||||
|
||||
//positional error (zeroth order error)
|
||||
btScalar depth = -(pivotAInW - pivotBInW).dot(normal);
|
||||
|
||||
btScalar impulse = tau*depth/timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv;
|
||||
|
||||
btVector3 impulse_vector = normal * impulse;
|
||||
m_rbA.applyImpulse( impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
|
||||
m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
|
||||
|
||||
normal[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
///solve angular part
|
||||
|
||||
// get axes in world space
|
||||
btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
|
||||
btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
|
||||
|
||||
// constraint axes in world space
|
||||
btVector3 jointAxis0;
|
||||
btVector3 jointAxis1;
|
||||
btPlaneSpace1(axisA,jointAxis0,jointAxis1);
|
||||
|
||||
|
||||
// Dirk: Get new angular velocity since it changed after applying an impulse
|
||||
angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
|
||||
angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
|
||||
|
||||
btScalar jacDiagABInv0 = 1.f / m_jacAng[0].getDiagonal();
|
||||
btScalar rel_vel0 = m_jacAng[0].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
|
||||
m_rbB.getLinearVelocity(),angvelB);
|
||||
float tau1 = tau;//0.f;
|
||||
|
||||
btScalar impulse0 = (tau1 * axisB.dot(jointAxis1) / timeStep - damping * rel_vel0) * jacDiagABInv0;
|
||||
btVector3 angular_impulse0 = jointAxis0 * impulse0;
|
||||
|
||||
m_rbA.applyTorqueImpulse( angular_impulse0);
|
||||
m_rbB.applyTorqueImpulse(-angular_impulse0);
|
||||
|
||||
|
||||
|
||||
// Dirk: Get new angular velocity since it changed after applying an impulse
|
||||
angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
|
||||
angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
|
||||
|
||||
btScalar jacDiagABInv1 = 1.f / m_jacAng[1].getDiagonal();
|
||||
btScalar rel_vel1 = m_jacAng[1].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
|
||||
m_rbB.getLinearVelocity(),angvelB);;
|
||||
|
||||
btScalar impulse1 = -(tau1 * axisB.dot(jointAxis0) / timeStep + damping * rel_vel1) * jacDiagABInv1;
|
||||
btVector3 angular_impulse1 = jointAxis1 * impulse1;
|
||||
|
||||
m_rbA.applyTorqueImpulse( angular_impulse1);
|
||||
m_rbB.applyTorqueImpulse(-angular_impulse1);
|
||||
|
||||
#else
|
||||
|
||||
|
||||
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
|
||||
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
|
||||
@ -237,37 +156,68 @@ void btHingeConstraint::solveConstraint(btScalar timeStep)
|
||||
|
||||
const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
|
||||
const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
|
||||
btVector3 angA = angVelA - axisA * axisA.dot(angVelA);
|
||||
btVector3 angB = angVelB - axisB * axisB.dot(angVelB);
|
||||
btVector3 velrel = angA-angB;
|
||||
|
||||
//solve angular velocity correction
|
||||
float relaxation = 1.f;
|
||||
float len = velrel.length();
|
||||
if (len > 0.00001f)
|
||||
btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
|
||||
btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
|
||||
|
||||
btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
|
||||
btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
|
||||
btVector3 velrelOrthog = angAorthog-angBorthog;
|
||||
{
|
||||
btVector3 normal = velrel.normalized();
|
||||
float denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
|
||||
getRigidBodyB().computeAngularImpulseDenominator(normal);
|
||||
// scale for mass and relaxation
|
||||
velrel *= (1.f/denom) * 0.9;
|
||||
//solve orthogonal angular velocity correction
|
||||
float relaxation = 1.f;
|
||||
float len = velrelOrthog.length();
|
||||
if (len > 0.00001f)
|
||||
{
|
||||
btVector3 normal = velrelOrthog.normalized();
|
||||
float denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
|
||||
getRigidBodyB().computeAngularImpulseDenominator(normal);
|
||||
// scale for mass and relaxation
|
||||
//todo: expose this 0.9 factor to developer
|
||||
velrelOrthog *= (1.f/denom) * 0.9f;
|
||||
}
|
||||
|
||||
//solve angular positional correction
|
||||
btVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep);
|
||||
float len2 = angularError.length();
|
||||
if (len2>0.00001f)
|
||||
{
|
||||
btVector3 normal2 = angularError.normalized();
|
||||
float denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
|
||||
getRigidBodyB().computeAngularImpulseDenominator(normal2);
|
||||
angularError *= (1.f/denom2) * relaxation;
|
||||
}
|
||||
|
||||
m_rbA.applyTorqueImpulse(-velrelOrthog+angularError);
|
||||
m_rbB.applyTorqueImpulse(velrelOrthog-angularError);
|
||||
}
|
||||
|
||||
//solve angular positional correction
|
||||
btVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep);
|
||||
float len2 = angularError.length();
|
||||
if (len2>0.00001f)
|
||||
//apply motor
|
||||
if (m_enableAngularMotor)
|
||||
{
|
||||
btVector3 normal2 = angularError.normalized();
|
||||
float denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
|
||||
getRigidBodyB().computeAngularImpulseDenominator(normal2);
|
||||
angularError *= (1.f/denom2) * relaxation;
|
||||
}
|
||||
//todo: add limits too
|
||||
btVector3 angularLimit(0,0,0);
|
||||
|
||||
m_rbA.applyTorqueImpulse(-velrel+angularError);
|
||||
m_rbB.applyTorqueImpulse(velrel-angularError);
|
||||
btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
|
||||
btScalar projRelVel = velrel.dot(axisA);
|
||||
|
||||
btScalar desiredMotorVel = m_motorTargetVelocity;
|
||||
btScalar motor_relvel = desiredMotorVel - projRelVel;
|
||||
|
||||
float denom3 = getRigidBodyA().computeAngularImpulseDenominator(axisA) +
|
||||
getRigidBodyB().computeAngularImpulseDenominator(axisA);
|
||||
|
||||
btScalar unclippedMotorImpulse = (1.f/denom3) * motor_relvel;;
|
||||
//todo: should clip against accumulated impulse
|
||||
btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
|
||||
clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
|
||||
btVector3 motorImp = clippedMotorImpulse * axisA;
|
||||
|
||||
m_rbA.applyTorqueImpulse(motorImp+angularLimit);
|
||||
m_rbB.applyTorqueImpulse(-motorImp-angularLimit);
|
||||
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -29,14 +29,18 @@ class btRigidBody;
|
||||
class btHingeConstraint : public btTypedConstraint
|
||||
{
|
||||
btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
|
||||
btJacobianEntry m_jacAng[2]; //2 orthogonal angular constraints
|
||||
btJacobianEntry m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
|
||||
|
||||
btVector3 m_pivotInA;
|
||||
btVector3 m_pivotInB;
|
||||
btVector3 m_axisInA;
|
||||
btVector3 m_axisInB;
|
||||
|
||||
bool m_angularOnly;
|
||||
bool m_angularOnly;
|
||||
|
||||
float m_motorTargetVelocity;
|
||||
float m_maxMotorImpulse;
|
||||
bool m_enableAngularMotor;
|
||||
|
||||
public:
|
||||
|
||||
@ -66,7 +70,12 @@ public:
|
||||
m_angularOnly = angularOnly;
|
||||
}
|
||||
|
||||
|
||||
void enableAngularMotor(bool enableMotor,float targetVelocity,float maxMotorImpulse)
|
||||
{
|
||||
m_enableAngularMotor = enableMotor;
|
||||
m_motorTargetVelocity = targetVelocity;
|
||||
m_maxMotorImpulse = maxMotorImpulse;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
@ -23,6 +23,8 @@ subject to the following restrictions:
|
||||
#include "LinearMath/btIDebugDraw.h"
|
||||
#include "btJacobianEntry.h"
|
||||
#include "LinearMath/btMinMax.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
|
||||
|
||||
|
||||
#ifdef USE_PROFILE
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
@ -123,7 +125,7 @@ btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
|
||||
}
|
||||
|
||||
/// btSequentialImpulseConstraintSolver Sequentially applies impulses
|
||||
float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
|
||||
float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
|
||||
{
|
||||
|
||||
btContactSolverInfo info = infoGlobal;
|
||||
@ -151,6 +153,15 @@ float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** ma
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
int j;
|
||||
for (j=0;j<numConstraints;j++)
|
||||
{
|
||||
btTypedConstraint* constraint = constraints[j];
|
||||
constraint->buildJacobian();
|
||||
}
|
||||
}
|
||||
|
||||
//should traverse the contacts random order...
|
||||
int iteration;
|
||||
@ -171,6 +182,12 @@ float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** ma
|
||||
}
|
||||
}
|
||||
|
||||
for (j=0;j<numConstraints;j++)
|
||||
{
|
||||
btTypedConstraint* constraint = constraints[j];
|
||||
constraint->solveConstraint(info.m_timeStep);
|
||||
}
|
||||
|
||||
for (j=0;j<totalPoints;j++)
|
||||
{
|
||||
btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
|
||||
@ -197,7 +214,7 @@ float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** ma
|
||||
|
||||
|
||||
/// btSequentialImpulseConstraintSolver Sequentially applies impulses
|
||||
float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
|
||||
float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
|
||||
{
|
||||
|
||||
btContactSolverInfo info = infoGlobal;
|
||||
@ -222,6 +239,14 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man
|
||||
}
|
||||
}
|
||||
}
|
||||
{
|
||||
int j;
|
||||
for (j=0;j<numConstraints;j++)
|
||||
{
|
||||
btTypedConstraint* constraint = constraints[j];
|
||||
constraint->buildJacobian();
|
||||
}
|
||||
}
|
||||
|
||||
//should traverse the contacts random order...
|
||||
int iteration;
|
||||
@ -230,6 +255,12 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man
|
||||
{
|
||||
int j;
|
||||
|
||||
for (j=0;j<numConstraints;j++)
|
||||
{
|
||||
btTypedConstraint* constraint = constraints[j];
|
||||
constraint->solveConstraint(info.m_timeStep);
|
||||
}
|
||||
|
||||
for (j=0;j<numManifolds;j++)
|
||||
{
|
||||
btPersistentManifold* manifold = manifoldPtr[j];
|
||||
|
||||
@ -68,7 +68,7 @@ public:
|
||||
|
||||
virtual ~btSequentialImpulseConstraintSolver() {}
|
||||
|
||||
virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
|
||||
virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
|
||||
|
||||
void setSolverMode(int mode)
|
||||
{
|
||||
@ -88,7 +88,7 @@ public:
|
||||
|
||||
btSequentialImpulseConstraintSolver3();
|
||||
|
||||
virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
|
||||
virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
|
||||
|
||||
|
||||
};
|
||||
|
||||
@ -270,12 +270,9 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(float timeStep)
|
||||
|
||||
|
||||
|
||||
///solve non-contact constraints
|
||||
solveNoncontactConstraints(getSolverInfo());
|
||||
///solve contact and other joint constraints
|
||||
solveConstraints(getSolverInfo());
|
||||
|
||||
///solve contact constraints
|
||||
solveContactConstraints(getSolverInfo());
|
||||
|
||||
///CallbackTriggers();
|
||||
|
||||
///integrate transforms
|
||||
@ -401,79 +398,116 @@ void btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle)
|
||||
}
|
||||
}
|
||||
|
||||
inline int btGetConstraintIslandId(const btTypedConstraint* lhs)
|
||||
{
|
||||
int islandId;
|
||||
|
||||
const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
|
||||
const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
|
||||
islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
|
||||
return islandId;
|
||||
|
||||
void btDiscreteDynamicsWorld::solveContactConstraints(btContactSolverInfo& solverInfo)
|
||||
}
|
||||
|
||||
static bool btSortConstraintOnIslandPredicate(const btTypedConstraint* lhs, const btTypedConstraint* rhs)
|
||||
{
|
||||
int rIslandId0,lIslandId0;
|
||||
rIslandId0 = btGetConstraintIslandId(rhs);
|
||||
lIslandId0 = btGetConstraintIslandId(lhs);
|
||||
return lIslandId0 < rIslandId0;
|
||||
}
|
||||
|
||||
void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
|
||||
{
|
||||
|
||||
BEGIN_PROFILE("solveContactConstraints");
|
||||
BEGIN_PROFILE("solveConstraints");
|
||||
|
||||
struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
|
||||
{
|
||||
|
||||
btContactSolverInfo& m_solverInfo;
|
||||
btConstraintSolver* m_solver;
|
||||
btIDebugDraw* m_debugDrawer;
|
||||
btContactSolverInfo& m_solverInfo;
|
||||
btConstraintSolver* m_solver;
|
||||
btTypedConstraint** m_sortedConstraints;
|
||||
int m_numConstraints;
|
||||
btIDebugDraw* m_debugDrawer;
|
||||
|
||||
|
||||
|
||||
InplaceSolverIslandCallback(
|
||||
btContactSolverInfo& solverInfo,
|
||||
btConstraintSolver* solver,
|
||||
btTypedConstraint** sortedConstraints,
|
||||
int numConstraints,
|
||||
btIDebugDraw* debugDrawer)
|
||||
:m_solverInfo(solverInfo),
|
||||
m_solver(solver),
|
||||
m_sortedConstraints(sortedConstraints),
|
||||
m_numConstraints(numConstraints),
|
||||
m_debugDrawer(debugDrawer)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
virtual void ProcessIsland(btPersistentManifold** manifolds,int numManifolds)
|
||||
virtual void ProcessIsland(btPersistentManifold** manifolds,int numManifolds, int islandId)
|
||||
{
|
||||
m_solver->solveGroup( manifolds, numManifolds,m_solverInfo,m_debugDrawer);
|
||||
//also add all non-contact constraints/joints for this island
|
||||
btTypedConstraint** startConstraint = 0;
|
||||
int numCurConstraints = 0;
|
||||
int i;
|
||||
|
||||
//find the first constraint for this island
|
||||
for (i=0;i<m_numConstraints;i++)
|
||||
{
|
||||
if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
|
||||
{
|
||||
startConstraint = &m_sortedConstraints[i];
|
||||
break;
|
||||
}
|
||||
}
|
||||
//count the number of constraints in this island
|
||||
for (;i<m_numConstraints;i++)
|
||||
{
|
||||
if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
|
||||
{
|
||||
numCurConstraints++;
|
||||
}
|
||||
}
|
||||
|
||||
m_solver->solveGroup( manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
InplaceSolverIslandCallback solverCallback( solverInfo, m_constraintSolver, m_debugDrawer);
|
||||
|
||||
|
||||
/// solve all the contact points and contact friction
|
||||
|
||||
//sorted version of all btTypedConstraint, based on islandId
|
||||
std::vector<btTypedConstraint*> sortedConstraints;
|
||||
sortedConstraints.resize( m_constraints.size());
|
||||
int i;
|
||||
for (i=0;i<getNumConstraints();i++)
|
||||
{
|
||||
sortedConstraints[i] = m_constraints[i];
|
||||
}
|
||||
|
||||
std::sort(sortedConstraints.begin(),sortedConstraints.end(),btSortConstraintOnIslandPredicate);
|
||||
|
||||
btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0;
|
||||
|
||||
InplaceSolverIslandCallback solverCallback( solverInfo, m_constraintSolver, constraintsPtr,sortedConstraints.size(), m_debugDrawer);
|
||||
|
||||
|
||||
|
||||
/// solve all the constraints for this island
|
||||
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback);
|
||||
|
||||
END_PROFILE("solveContactConstraints");
|
||||
END_PROFILE("solveConstraints");
|
||||
|
||||
}
|
||||
|
||||
|
||||
void btDiscreteDynamicsWorld::solveNoncontactConstraints(btContactSolverInfo& solverInfo)
|
||||
{
|
||||
BEGIN_PROFILE("solveNoncontactConstraints");
|
||||
|
||||
int i;
|
||||
int numConstraints = int(m_constraints.size());
|
||||
|
||||
///constraint preparation: building jacobians
|
||||
for (i=0;i< numConstraints ; i++ )
|
||||
{
|
||||
btTypedConstraint* constraint = m_constraints[i];
|
||||
constraint->buildJacobian();
|
||||
}
|
||||
|
||||
//solve the regular non-contact constraints (point 2 point, hinge, generic d6)
|
||||
for (int g=0;g<solverInfo.m_numIterations;g++)
|
||||
{
|
||||
//
|
||||
// constraint solving
|
||||
//
|
||||
for (i=0;i< numConstraints ; i++ )
|
||||
{
|
||||
btTypedConstraint* constraint = m_constraints[i];
|
||||
constraint->solveConstraint( solverInfo.m_timeStep );
|
||||
}
|
||||
}
|
||||
|
||||
END_PROFILE("solveNoncontactConstraints");
|
||||
|
||||
}
|
||||
|
||||
void btDiscreteDynamicsWorld::calculateSimulationIslands()
|
||||
{
|
||||
|
||||
@ -66,10 +66,8 @@ protected:
|
||||
|
||||
void calculateSimulationIslands();
|
||||
|
||||
void solveNoncontactConstraints(btContactSolverInfo& solverInfo);
|
||||
|
||||
void solveContactConstraints(btContactSolverInfo& solverInfo);
|
||||
|
||||
void solveConstraints(btContactSolverInfo& solverInfo);
|
||||
|
||||
void updateActivationState(float timeStep);
|
||||
|
||||
void updateVehicles(float timeStep);
|
||||
|
||||
@ -63,7 +63,7 @@ int btSimpleDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, floa
|
||||
btContactSolverInfo infoGlobal;
|
||||
infoGlobal.m_timeStep = timeStep;
|
||||
|
||||
m_constraintSolver->solveGroup(manifoldPtr, numManifolds,infoGlobal,m_debugDrawer);
|
||||
m_constraintSolver->solveGroup(manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer);
|
||||
}
|
||||
|
||||
///integrate transforms
|
||||
@ -101,7 +101,10 @@ void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)
|
||||
{
|
||||
body->setGravity(m_gravity);
|
||||
|
||||
addCollisionObject(body);
|
||||
if (body->getCollisionShape())
|
||||
{
|
||||
addCollisionObject(body);
|
||||
}
|
||||
}
|
||||
|
||||
void btSimpleDynamicsWorld::updateAabbs()
|
||||
|
||||
@ -54,8 +54,10 @@ subject to the following restrictions:
|
||||
|
||||
typedef float btScalar;
|
||||
|
||||
#if defined (__sun) || defined (__sun__) || defined (__sparc) || defined (__APPLE__)
|
||||
//use double float precision operation on those platforms for Blender
|
||||
///older compilers (gcc 3.x) and Sun needs double versions of srqt etc.
|
||||
///exclude Apple Intel (it's assumed to be a Macbook or newer Intel Dual Core processor)
|
||||
#if defined (__sun) || defined (__sun__) || defined (__sparc) || (defined (__APPLE__) && ! defined (__i386__))
|
||||
//use slow double float precision operation on those platforms
|
||||
|
||||
SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrt(x); }
|
||||
SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); }
|
||||
|
||||
Loading…
Reference in New Issue
Block a user