Minor constraint refactoring, to allow SPU-side processing for PLAYSTATION 3 (added non-virtual methods)
Also comment-out some code for __SPU__ to reduce code size Added btContactConstraint (only used on PS3 SPU right now, better to use btPersistentManifold directly for contact constraints) Improved readblend utility library (see also usage in http://gamekit.googlecode.com with Irrlicht) Fix for btConvexConvexAlgorithm, potential division by zero Thanks linzner http://code.google.com/p/bullet/issues/detail?id=260
This commit is contained in:
erwin.coumans
@@ -26,7 +26,7 @@ subject to the following restrictions:
|
||||
#define HINGE_USE_OBSOLETE_SOLVER false
|
||||
|
||||
|
||||
|
||||
#ifndef __SPU__
|
||||
|
||||
btHingeConstraint::btHingeConstraint()
|
||||
: btTypedConstraint (HINGE_CONSTRAINT_TYPE),
|
||||
@@ -249,7 +249,7 @@ void btHingeConstraint::buildJacobian()
|
||||
m_accLimitImpulse = btScalar(0.);
|
||||
|
||||
// test angular limit
|
||||
testLimit();
|
||||
testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
|
||||
|
||||
//Compute K = J*W*J' for hinge axis
|
||||
btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
|
||||
@@ -259,6 +259,166 @@ void btHingeConstraint::buildJacobian()
|
||||
}
|
||||
}
|
||||
|
||||
void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
|
||||
{
|
||||
|
||||
///for backwards compatibility during the transition to 'getInfo/getInfo2'
|
||||
if (m_useSolveConstraintObsolete)
|
||||
{
|
||||
|
||||
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
|
||||
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
|
||||
|
||||
btScalar tau = btScalar(0.3);
|
||||
|
||||
//linear part
|
||||
if (!m_angularOnly)
|
||||
{
|
||||
btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
|
||||
|
||||
btVector3 vel1,vel2;
|
||||
bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
|
||||
bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
const btVector3& normal = m_jac[i].m_linearJointAxis;
|
||||
btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
|
||||
|
||||
btScalar rel_vel;
|
||||
rel_vel = normal.dot(vel);
|
||||
//positional error (zeroth order error)
|
||||
btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
|
||||
btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
|
||||
m_appliedImpulse += impulse;
|
||||
btVector3 impulse_vector = normal * impulse;
|
||||
btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
|
||||
btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
|
||||
bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
|
||||
bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
{
|
||||
///solve angular part
|
||||
|
||||
// get axes in world space
|
||||
btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
|
||||
btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2);
|
||||
|
||||
btVector3 angVelA;
|
||||
bodyA.getAngularVelocity(angVelA);
|
||||
btVector3 angVelB;
|
||||
bodyB.getAngularVelocity(angVelB);
|
||||
|
||||
btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
|
||||
btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
|
||||
|
||||
btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
|
||||
btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
|
||||
btVector3 velrelOrthog = angAorthog-angBorthog;
|
||||
{
|
||||
|
||||
|
||||
//solve orthogonal angular velocity correction
|
||||
//btScalar relaxation = btScalar(1.);
|
||||
btScalar len = velrelOrthog.length();
|
||||
if (len > btScalar(0.00001))
|
||||
{
|
||||
btVector3 normal = velrelOrthog.normalized();
|
||||
btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
|
||||
getRigidBodyB().computeAngularImpulseDenominator(normal);
|
||||
// scale for mass and relaxation
|
||||
//velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
|
||||
|
||||
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom));
|
||||
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom));
|
||||
|
||||
}
|
||||
|
||||
//solve angular positional correction
|
||||
btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/timeStep);
|
||||
btScalar len2 = angularError.length();
|
||||
if (len2>btScalar(0.00001))
|
||||
{
|
||||
btVector3 normal2 = angularError.normalized();
|
||||
btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
|
||||
getRigidBodyB().computeAngularImpulseDenominator(normal2);
|
||||
//angularError *= (btScalar(1.)/denom2) * relaxation;
|
||||
|
||||
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2));
|
||||
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2));
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// solve limit
|
||||
if (m_solveLimit)
|
||||
{
|
||||
btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign;
|
||||
|
||||
btScalar impulseMag = amplitude * m_kHinge;
|
||||
|
||||
// Clamp the accumulated impulse
|
||||
btScalar temp = m_accLimitImpulse;
|
||||
m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
|
||||
impulseMag = m_accLimitImpulse - temp;
|
||||
|
||||
|
||||
|
||||
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign);
|
||||
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign));
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
//apply motor
|
||||
if (m_enableAngularMotor)
|
||||
{
|
||||
//todo: add limits too
|
||||
btVector3 angularLimit(0,0,0);
|
||||
|
||||
btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
|
||||
btScalar projRelVel = velrel.dot(axisA);
|
||||
|
||||
btScalar desiredMotorVel = m_motorTargetVelocity;
|
||||
btScalar motor_relvel = desiredMotorVel - projRelVel;
|
||||
|
||||
btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
|
||||
|
||||
// accumulated impulse clipping:
|
||||
btScalar fMaxImpulse = m_maxMotorImpulse;
|
||||
btScalar newAccImpulse = m_accMotorImpulse + unclippedMotorImpulse;
|
||||
btScalar clippedMotorImpulse = unclippedMotorImpulse;
|
||||
if (newAccImpulse > fMaxImpulse)
|
||||
{
|
||||
newAccImpulse = fMaxImpulse;
|
||||
clippedMotorImpulse = newAccImpulse - m_accMotorImpulse;
|
||||
}
|
||||
else if (newAccImpulse < -fMaxImpulse)
|
||||
{
|
||||
newAccImpulse = -fMaxImpulse;
|
||||
clippedMotorImpulse = newAccImpulse - m_accMotorImpulse;
|
||||
}
|
||||
m_accMotorImpulse += clippedMotorImpulse;
|
||||
|
||||
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse);
|
||||
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
#endif //__SPU__
|
||||
|
||||
|
||||
void btHingeConstraint::getInfo1(btConstraintInfo1* info)
|
||||
@@ -272,52 +432,77 @@ void btHingeConstraint::getInfo1(btConstraintInfo1* info)
|
||||
{
|
||||
info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
|
||||
info->nub = 1;
|
||||
//always add the row, to avoid computation (data is not available yet)
|
||||
//prepare constraint
|
||||
testLimit();
|
||||
testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
|
||||
if(getSolveLimit() || getEnableAngularMotor())
|
||||
{
|
||||
info->m_numConstraintRows++; // limit 3rd anguar as well
|
||||
info->nub--;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
|
||||
{
|
||||
if (m_useSolveConstraintObsolete)
|
||||
{
|
||||
info->m_numConstraintRows = 0;
|
||||
info->nub = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
//always add the 'limit' row, to avoid computation (data is not available yet)
|
||||
info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular
|
||||
info->nub = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
|
||||
{
|
||||
getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
|
||||
}
|
||||
|
||||
void btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
|
||||
{
|
||||
btAssert(!m_useSolveConstraintObsolete);
|
||||
int i, s = info->rowskip;
|
||||
int i, skip = info->rowskip;
|
||||
// transforms in world space
|
||||
btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
|
||||
btTransform trB = m_rbB.getCenterOfMassTransform()*m_rbBFrame;
|
||||
btTransform trA = transA*m_rbAFrame;
|
||||
btTransform trB = transB*m_rbBFrame;
|
||||
// pivot point
|
||||
btVector3 pivotAInW = trA.getOrigin();
|
||||
btVector3 pivotBInW = trB.getOrigin();
|
||||
|
||||
// linear (all fixed)
|
||||
info->m_J1linearAxis[0] = 1;
|
||||
info->m_J1linearAxis[s + 1] = 1;
|
||||
info->m_J1linearAxis[2 * s + 2] = 1;
|
||||
btVector3 a1 = pivotAInW - m_rbA.getCenterOfMassTransform().getOrigin();
|
||||
info->m_J1linearAxis[skip + 1] = 1;
|
||||
info->m_J1linearAxis[2 * skip + 2] = 1;
|
||||
|
||||
|
||||
|
||||
|
||||
btVector3 a1 = pivotAInW - transA.getOrigin();
|
||||
{
|
||||
btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
|
||||
btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + s);
|
||||
btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * s);
|
||||
btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip);
|
||||
btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip);
|
||||
btVector3 a1neg = -a1;
|
||||
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
|
||||
}
|
||||
btVector3 a2 = pivotBInW - m_rbB.getCenterOfMassTransform().getOrigin();
|
||||
btVector3 a2 = pivotBInW - transB.getOrigin();
|
||||
{
|
||||
btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
|
||||
btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + s);
|
||||
btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * s);
|
||||
btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip);
|
||||
btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip);
|
||||
a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
|
||||
}
|
||||
// linear RHS
|
||||
btScalar k = info->fps * info->erp;
|
||||
for(i = 0; i < 3; i++)
|
||||
{
|
||||
info->m_constraintError[i * s] = k * (pivotBInW[i] - pivotAInW[i]);
|
||||
info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]);
|
||||
}
|
||||
// make rotations around X and Y equal
|
||||
// the hinge axis should be the only unconstrained
|
||||
@@ -436,8 +621,8 @@ void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
|
||||
btScalar bounce = m_relaxationFactor;
|
||||
if(bounce > btScalar(0.0))
|
||||
{
|
||||
btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
|
||||
vel -= m_rbB.getAngularVelocity().dot(ax1);
|
||||
btScalar vel = angVelA.dot(ax1);
|
||||
vel -= angVelB.dot(ax1);
|
||||
// only apply bounce if the velocity is incoming, and if the
|
||||
// resulting c[] exceeds what we already have.
|
||||
if(limit == 1)
|
||||
@@ -470,163 +655,6 @@ void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
|
||||
|
||||
|
||||
|
||||
void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
|
||||
{
|
||||
|
||||
///for backwards compatibility during the transition to 'getInfo/getInfo2'
|
||||
if (m_useSolveConstraintObsolete)
|
||||
{
|
||||
|
||||
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
|
||||
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
|
||||
|
||||
btScalar tau = btScalar(0.3);
|
||||
|
||||
//linear part
|
||||
if (!m_angularOnly)
|
||||
{
|
||||
btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
|
||||
|
||||
btVector3 vel1,vel2;
|
||||
bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
|
||||
bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
const btVector3& normal = m_jac[i].m_linearJointAxis;
|
||||
btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
|
||||
|
||||
btScalar rel_vel;
|
||||
rel_vel = normal.dot(vel);
|
||||
//positional error (zeroth order error)
|
||||
btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
|
||||
btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
|
||||
m_appliedImpulse += impulse;
|
||||
btVector3 impulse_vector = normal * impulse;
|
||||
btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
|
||||
btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
|
||||
bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
|
||||
bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
{
|
||||
///solve angular part
|
||||
|
||||
// get axes in world space
|
||||
btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
|
||||
btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2);
|
||||
|
||||
btVector3 angVelA;
|
||||
bodyA.getAngularVelocity(angVelA);
|
||||
btVector3 angVelB;
|
||||
bodyB.getAngularVelocity(angVelB);
|
||||
|
||||
btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
|
||||
btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
|
||||
|
||||
btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
|
||||
btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
|
||||
btVector3 velrelOrthog = angAorthog-angBorthog;
|
||||
{
|
||||
|
||||
|
||||
//solve orthogonal angular velocity correction
|
||||
btScalar relaxation = btScalar(1.);
|
||||
btScalar len = velrelOrthog.length();
|
||||
if (len > btScalar(0.00001))
|
||||
{
|
||||
btVector3 normal = velrelOrthog.normalized();
|
||||
btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
|
||||
getRigidBodyB().computeAngularImpulseDenominator(normal);
|
||||
// scale for mass and relaxation
|
||||
//velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
|
||||
|
||||
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom));
|
||||
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom));
|
||||
|
||||
}
|
||||
|
||||
//solve angular positional correction
|
||||
btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/timeStep);
|
||||
btScalar len2 = angularError.length();
|
||||
if (len2>btScalar(0.00001))
|
||||
{
|
||||
btVector3 normal2 = angularError.normalized();
|
||||
btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
|
||||
getRigidBodyB().computeAngularImpulseDenominator(normal2);
|
||||
//angularError *= (btScalar(1.)/denom2) * relaxation;
|
||||
|
||||
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2));
|
||||
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2));
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// solve limit
|
||||
if (m_solveLimit)
|
||||
{
|
||||
btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign;
|
||||
|
||||
btScalar impulseMag = amplitude * m_kHinge;
|
||||
|
||||
// Clamp the accumulated impulse
|
||||
btScalar temp = m_accLimitImpulse;
|
||||
m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
|
||||
impulseMag = m_accLimitImpulse - temp;
|
||||
|
||||
|
||||
|
||||
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign);
|
||||
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign));
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
//apply motor
|
||||
if (m_enableAngularMotor)
|
||||
{
|
||||
//todo: add limits too
|
||||
btVector3 angularLimit(0,0,0);
|
||||
|
||||
btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
|
||||
btScalar projRelVel = velrel.dot(axisA);
|
||||
|
||||
btScalar desiredMotorVel = m_motorTargetVelocity;
|
||||
btScalar motor_relvel = desiredMotorVel - projRelVel;
|
||||
|
||||
btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
|
||||
|
||||
// accumulated impulse clipping:
|
||||
btScalar fMaxImpulse = m_maxMotorImpulse;
|
||||
btScalar newAccImpulse = m_accMotorImpulse + unclippedMotorImpulse;
|
||||
btScalar clippedMotorImpulse = unclippedMotorImpulse;
|
||||
if (newAccImpulse > fMaxImpulse)
|
||||
{
|
||||
newAccImpulse = fMaxImpulse;
|
||||
clippedMotorImpulse = newAccImpulse - m_accMotorImpulse;
|
||||
}
|
||||
else if (newAccImpulse < -fMaxImpulse)
|
||||
{
|
||||
newAccImpulse = -fMaxImpulse;
|
||||
clippedMotorImpulse = newAccImpulse - m_accMotorImpulse;
|
||||
}
|
||||
m_accMotorImpulse += clippedMotorImpulse;
|
||||
|
||||
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse);
|
||||
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -637,12 +665,16 @@ void btHingeConstraint::updateRHS(btScalar timeStep)
|
||||
}
|
||||
|
||||
|
||||
|
||||
btScalar btHingeConstraint::getHingeAngle()
|
||||
{
|
||||
const btVector3 refAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
|
||||
const btVector3 refAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
|
||||
const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
|
||||
return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
|
||||
}
|
||||
|
||||
btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB)
|
||||
{
|
||||
const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
|
||||
const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
|
||||
const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1);
|
||||
btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
|
||||
return m_referenceSign * angle;
|
||||
}
|
||||
@@ -676,10 +708,10 @@ void btHingeConstraint::testLimit()
|
||||
#else
|
||||
|
||||
|
||||
void btHingeConstraint::testLimit()
|
||||
void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB)
|
||||
{
|
||||
// Compute limit information
|
||||
m_hingeAngle = getHingeAngle();
|
||||
m_hingeAngle = getHingeAngle(transA,transB);
|
||||
m_correction = btScalar(0.);
|
||||
m_limitSign = btScalar(0.);
|
||||
m_solveLimit = false;
|
||||
@@ -741,7 +773,7 @@ void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
|
||||
}
|
||||
|
||||
// compute angular velocity
|
||||
btScalar curAngle = getHingeAngle();
|
||||
btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
|
||||
btScalar dAngle = targetAngle - curAngle;
|
||||
m_motorTargetVelocity = dAngle / dt;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user