Get rid of btSolverBody and use btRigidBody directly. btSolverBody didn't improve performance after all, due to random-access
Tweak the BenchmarkDemo a bit: 1) disable deactivation in graphical mode 2) add some settings that increase performance in the BenchmarkDemo2 (1000 stack) from 35ms to 15ms on this quad core (at the cost of a bit of quality)
This commit is contained in:
erwin.coumans
@@ -107,79 +107,9 @@ btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& fram
|
||||
|
||||
|
||||
|
||||
void btSliderConstraint::buildJacobian()
|
||||
{
|
||||
if (!m_useSolveConstraintObsolete)
|
||||
{
|
||||
return;
|
||||
}
|
||||
if(m_useLinearReferenceFrameA)
|
||||
{
|
||||
buildJacobianInt(m_rbA, m_rbB, m_frameInA, m_frameInB);
|
||||
}
|
||||
else
|
||||
{
|
||||
buildJacobianInt(m_rbB, m_rbA, m_frameInB, m_frameInA);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
|
||||
{
|
||||
#ifndef __SPU__
|
||||
//calculate transforms
|
||||
m_calculatedTransformA = rbA.getCenterOfMassTransform() * frameInA;
|
||||
m_calculatedTransformB = rbB.getCenterOfMassTransform() * frameInB;
|
||||
m_realPivotAInW = m_calculatedTransformA.getOrigin();
|
||||
m_realPivotBInW = m_calculatedTransformB.getOrigin();
|
||||
m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
|
||||
m_delta = m_realPivotBInW - m_realPivotAInW;
|
||||
m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
|
||||
m_relPosA = m_projPivotInW - rbA.getCenterOfMassPosition();
|
||||
m_relPosB = m_realPivotBInW - rbB.getCenterOfMassPosition();
|
||||
btVector3 normalWorld;
|
||||
int i;
|
||||
//linear part
|
||||
for(i = 0; i < 3; i++)
|
||||
{
|
||||
normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
|
||||
new (&m_jacLin[i]) btJacobianEntry(
|
||||
rbA.getCenterOfMassTransform().getBasis().transpose(),
|
||||
rbB.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_relPosA,
|
||||
m_relPosB,
|
||||
normalWorld,
|
||||
rbA.getInvInertiaDiagLocal(),
|
||||
rbA.getInvMass(),
|
||||
rbB.getInvInertiaDiagLocal(),
|
||||
rbB.getInvMass()
|
||||
);
|
||||
m_jacLinDiagABInv[i] = btScalar(1.) / m_jacLin[i].getDiagonal();
|
||||
m_depth[i] = m_delta.dot(normalWorld);
|
||||
}
|
||||
testLinLimits();
|
||||
// angular part
|
||||
for(i = 0; i < 3; i++)
|
||||
{
|
||||
normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
|
||||
new (&m_jacAng[i]) btJacobianEntry(
|
||||
normalWorld,
|
||||
rbA.getCenterOfMassTransform().getBasis().transpose(),
|
||||
rbB.getCenterOfMassTransform().getBasis().transpose(),
|
||||
rbA.getInvInertiaDiagLocal(),
|
||||
rbB.getInvInertiaDiagLocal()
|
||||
);
|
||||
}
|
||||
testAngLimits();
|
||||
btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0);
|
||||
m_kAngle = btScalar(1.0 )/ (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA));
|
||||
// clear accumulator for motors
|
||||
m_accumulatedLinMotorImpulse = btScalar(0.0);
|
||||
m_accumulatedAngMotorImpulse = btScalar(0.0);
|
||||
#endif //__SPU__
|
||||
}
|
||||
|
||||
void btSliderConstraint::getInfo1(btConstraintInfo1* info)
|
||||
{
|
||||
if (m_useSolveConstraintObsolete)
|
||||
@@ -222,210 +152,6 @@ void btSliderConstraint::getInfo2(btConstraintInfo2* info)
|
||||
|
||||
|
||||
|
||||
void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
|
||||
{
|
||||
if (m_useSolveConstraintObsolete)
|
||||
{
|
||||
m_timeStep = timeStep;
|
||||
if(m_useLinearReferenceFrameA)
|
||||
{
|
||||
solveConstraintInt(m_rbA,bodyA, m_rbB,bodyB);
|
||||
}
|
||||
else
|
||||
{
|
||||
solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB)
|
||||
{
|
||||
#ifndef __SPU__
|
||||
int i;
|
||||
// linear
|
||||
btVector3 velA;
|
||||
bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA);
|
||||
btVector3 velB;
|
||||
bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB);
|
||||
btVector3 vel = velA - velB;
|
||||
for(i = 0; i < 3; i++)
|
||||
{
|
||||
const btVector3& normal = m_jacLin[i].m_linearJointAxis;
|
||||
btScalar rel_vel = normal.dot(vel);
|
||||
// calculate positional error
|
||||
btScalar depth = m_depth[i];
|
||||
// get parameters
|
||||
btScalar softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin);
|
||||
btScalar restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin);
|
||||
btScalar damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin);
|
||||
// calcutate and apply impulse
|
||||
btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i];
|
||||
btVector3 impulse_vector = normal * normalImpulse;
|
||||
|
||||
//rbA.applyImpulse( impulse_vector, m_relPosA);
|
||||
//rbB.applyImpulse(-impulse_vector, m_relPosB);
|
||||
{
|
||||
btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
|
||||
btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
|
||||
bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
|
||||
bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
|
||||
}
|
||||
|
||||
|
||||
|
||||
if(m_poweredLinMotor && (!i))
|
||||
{ // apply linear motor
|
||||
if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce)
|
||||
{
|
||||
btScalar desiredMotorVel = m_targetLinMotorVelocity;
|
||||
btScalar motor_relvel = desiredMotorVel + rel_vel;
|
||||
normalImpulse = -motor_relvel * m_jacLinDiagABInv[i];
|
||||
// clamp accumulated impulse
|
||||
btScalar new_acc = m_accumulatedLinMotorImpulse + btFabs(normalImpulse);
|
||||
if(new_acc > m_maxLinMotorForce)
|
||||
{
|
||||
new_acc = m_maxLinMotorForce;
|
||||
}
|
||||
btScalar del = new_acc - m_accumulatedLinMotorImpulse;
|
||||
if(normalImpulse < btScalar(0.0))
|
||||
{
|
||||
normalImpulse = -del;
|
||||
}
|
||||
else
|
||||
{
|
||||
normalImpulse = del;
|
||||
}
|
||||
m_accumulatedLinMotorImpulse = new_acc;
|
||||
// apply clamped impulse
|
||||
impulse_vector = normal * normalImpulse;
|
||||
//rbA.applyImpulse( impulse_vector, m_relPosA);
|
||||
//rbB.applyImpulse(-impulse_vector, m_relPosB);
|
||||
|
||||
{
|
||||
btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
|
||||
btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
|
||||
bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
|
||||
bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
// angular
|
||||
// get axes in world space
|
||||
btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0);
|
||||
btVector3 axisB = m_calculatedTransformB.getBasis().getColumn(0);
|
||||
|
||||
btVector3 angVelA;
|
||||
bodyA.getAngularVelocity(angVelA);
|
||||
btVector3 angVelB;
|
||||
bodyB.getAngularVelocity(angVelB);
|
||||
|
||||
btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
|
||||
btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB);
|
||||
|
||||
btVector3 angAorthog = angVelA - angVelAroundAxisA;
|
||||
btVector3 angBorthog = angVelB - angVelAroundAxisB;
|
||||
btVector3 velrelOrthog = angAorthog-angBorthog;
|
||||
//solve orthogonal angular velocity correction
|
||||
btScalar len = velrelOrthog.length();
|
||||
btScalar orthorImpulseMag = 0.f;
|
||||
|
||||
if (len > btScalar(0.00001))
|
||||
{
|
||||
btVector3 normal = velrelOrthog.normalized();
|
||||
btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal);
|
||||
//velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
|
||||
orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
|
||||
}
|
||||
//solve angular positional correction
|
||||
btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep);
|
||||
btVector3 angularAxis = angularError;
|
||||
btScalar angularImpulseMag = 0;
|
||||
|
||||
btScalar len2 = angularError.length();
|
||||
if (len2>btScalar(0.00001))
|
||||
{
|
||||
btVector3 normal2 = angularError.normalized();
|
||||
btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2);
|
||||
angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
|
||||
angularError *= angularImpulseMag;
|
||||
}
|
||||
// apply impulse
|
||||
//rbA.applyTorqueImpulse(-velrelOrthog+angularError);
|
||||
//rbB.applyTorqueImpulse(velrelOrthog-angularError);
|
||||
|
||||
bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*velrelOrthog,-orthorImpulseMag);
|
||||
bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*velrelOrthog,orthorImpulseMag);
|
||||
bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*angularAxis,angularImpulseMag);
|
||||
bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*angularAxis,-angularImpulseMag);
|
||||
|
||||
|
||||
btScalar impulseMag;
|
||||
//solve angular limits
|
||||
if(m_solveAngLim)
|
||||
{
|
||||
impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / m_timeStep;
|
||||
impulseMag *= m_kAngle * m_softnessLimAng;
|
||||
}
|
||||
else
|
||||
{
|
||||
impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / m_timeStep;
|
||||
impulseMag *= m_kAngle * m_softnessDirAng;
|
||||
}
|
||||
btVector3 impulse = axisA * impulseMag;
|
||||
//rbA.applyTorqueImpulse(impulse);
|
||||
//rbB.applyTorqueImpulse(-impulse);
|
||||
|
||||
bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,impulseMag);
|
||||
bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-impulseMag);
|
||||
|
||||
|
||||
|
||||
//apply angular motor
|
||||
if(m_poweredAngMotor)
|
||||
{
|
||||
if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce)
|
||||
{
|
||||
btVector3 velrel = angVelAroundAxisA - angVelAroundAxisB;
|
||||
btScalar projRelVel = velrel.dot(axisA);
|
||||
|
||||
btScalar desiredMotorVel = m_targetAngMotorVelocity;
|
||||
btScalar motor_relvel = desiredMotorVel - projRelVel;
|
||||
|
||||
btScalar angImpulse = m_kAngle * motor_relvel;
|
||||
// clamp accumulated impulse
|
||||
btScalar new_acc = m_accumulatedAngMotorImpulse + btFabs(angImpulse);
|
||||
if(new_acc > m_maxAngMotorForce)
|
||||
{
|
||||
new_acc = m_maxAngMotorForce;
|
||||
}
|
||||
btScalar del = new_acc - m_accumulatedAngMotorImpulse;
|
||||
if(angImpulse < btScalar(0.0))
|
||||
{
|
||||
angImpulse = -del;
|
||||
}
|
||||
else
|
||||
{
|
||||
angImpulse = del;
|
||||
}
|
||||
m_accumulatedAngMotorImpulse = new_acc;
|
||||
// apply clamped impulse
|
||||
btVector3 motorImp = angImpulse * axisA;
|
||||
//rbA.applyTorqueImpulse(motorImp);
|
||||
//rbB.applyTorqueImpulse(-motorImp);
|
||||
|
||||
bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,angImpulse);
|
||||
bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse);
|
||||
}
|
||||
}
|
||||
#endif //__SPU__
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user