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);
btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
return m_referenceSign * angle;
}
void btGeneric6DofSpring2Constraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
{
m_calculatedTransformA = transA * m_frameInA;
m_calculatedTransformB = transB * m_frameInB;
calculateLinearInfo();
calculateAngleInfo();
btScalar miA = getRigidBodyA().getInvMass();
btScalar miB = getRigidBodyB().getInvMass();
m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
btScalar miS = miA + miB;
if(miS > btScalar(0.f))
{
m_factA = miB / miS;
}
else
{
m_factA = btScalar(0.5f);
}
m_factB = btScalar(1.0f) - m_factA;
}
void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
{
btAssert(!m_useSolveConstraintObsolete);
int i, s = info->rowskip;
// transforms in world space
btTransform trA = transA*m_rbAFrame;
btTransform trB = transB*m_rbBFrame;
// pivot point
btVector3 pivotAInW = trA.getOrigin();
btVector3 pivotBInW = trB.getOrigin();
#if 1
// difference between frames in WCS
btVector3 ofs = trB.getOrigin() - trA.getOrigin();
// now get weight factors depending on masses
btScalar miA = getRigidBodyA().getInvMass();
btScalar miB = getRigidBodyB().getInvMass();
bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
btScalar miS = miA + miB;
btScalar factA, factB;
if(miS > btScalar(0.f))
{
factA = miB / miS;
}
else
{
factA = btScalar(0.5f);
}
factB = btScalar(1.0f) - factA;
// get the desired direction of hinge axis
// as weighted sum of Z-orthos of frameA and frameB in WCS
btVector3 ax1A = trA.getBasis().getColumn(2);
btVector3 ax1B = trB.getBasis().getColumn(2);
btVector3 ax1 = ax1A * factA + ax1B * factB;
ax1.normalize();
// fill first 3 rows
// we want: velA + wA x relA == velB + wB x relB
btTransform bodyA_trans = transA;
btTransform bodyB_trans = transB;
int s0 = 0;
int s1 = s;
int s2 = s * 2;
int nrow = 2; // last filled row
btVector3 tmpA, tmpB, relA, relB, p, q;
// get vector from bodyB to frameB in WCS
relB = trB.getOrigin() - bodyB_trans.getOrigin();
// get its projection to hinge axis
btVector3 projB = ax1 * relB.dot(ax1);
// get vector directed from bodyB to hinge axis (and orthogonal to it)
btVector3 orthoB = relB - projB;
// same for bodyA
relA = trA.getOrigin() - bodyA_trans.getOrigin();
btVector3 projA = ax1 * relA.dot(ax1);
btVector3 orthoA = relA - projA;
btVector3 totalDist = projA - projB;
// get offset vectors relA and relB
relA = orthoA + totalDist * factA;
relB = orthoB - totalDist * factB;
// now choose average ortho to hinge axis
p = orthoB * factA + orthoA * factB;
btScalar len2 = p.length2();
if(len2 > SIMD_EPSILON)
{
p /= btSqrt(len2);
}
else
{
p = trA.getBasis().getColumn(1);
}
// make one more ortho
q = ax1.cross(p);
// fill three rows
tmpA = relA.cross(p);
tmpB = relB.cross(p);
for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i];
for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i];
tmpA = relA.cross(q);
tmpB = relB.cross(q);
if(hasStaticBody && getSolveLimit())
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation if angular limit is hit
tmpB *= factB;
tmpA *= factA;
}
for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i];
for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i];
tmpA = relA.cross(ax1);
tmpB = relB.cross(ax1);
if(hasStaticBody)
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation
tmpB *= factB;
tmpA *= factA;
}
for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i];
for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i];
for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i];
// compute three elements of right hand side
btScalar k = info->fps * info->erp;
btScalar rhs = k * p.dot(ofs);
info->m_constraintError[s0] = rhs;
rhs = k * q.dot(ofs);
info->m_constraintError[s1] = rhs;
rhs = k * ax1.dot(ofs);
info->m_constraintError[s2] = rhs;
// the hinge axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the hinge axis should be equal. thus the constraint equations are
// p*w1 - p*w2 = 0
// q*w1 - q*w2 = 0
// where p and q are unit vectors normal to the hinge axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
int s3 = 3 * s;
int s4 = 4 * s;
info->m_J1angularAxis[s3 + 0] = p[0];
info->m_J1angularAxis[s3 + 1] = p[1];
info->m_J1angularAxis[s3 + 2] = p[2];
info->m_J1angularAxis[s4 + 0] = q[0];
info->m_J1angularAxis[s4 + 1] = q[1];
info->m_J1angularAxis[s4 + 2] = q[2];
info->m_J2angularAxis[s3 + 0] = -p[0];
info->m_J2angularAxis[s3 + 1] = -p[1];
info->m_J2angularAxis[s3 + 2] = -p[2];
info->m_J2angularAxis[s4 + 0] = -q[0];
info->m_J2angularAxis[s4 + 1] = -q[1];
info->m_J2angularAxis[s4 + 2] = -q[2];
// compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the hinge back into alignment.
// if ax1A,ax1B are the unit length hinge axes as computed from bodyA and
// bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
// if "theta" is the angle between ax1 and ax2, we need an angular velocity
// along u to cover angle erp*theta in one step :
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~= sin(theta), so
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
k = info->fps * info->erp;
btVector3 u = ax1A.cross(ax1B);
info->m_constraintError[s3] = k * u.dot(p);
info->m_constraintError[s4] = k * u.dot(q);
#endif
// check angular limits
nrow = 4; // last filled row
int srow;
btScalar limit_err = btScalar(0.0);
int limit = 0;
if(getSolveLimit())
{
limit_err = m_correction * m_referenceSign;
limit = (limit_err > btScalar(0.0)) ? 1 : 2;
}
// if the hinge has joint limits or motor, add in the extra row
int powered = 0;
if(getEnableAngularMotor())
{
powered = 1;
}
if(limit || powered)
{
nrow++;
srow = nrow * info->rowskip;
info->m_J1angularAxis[srow+0] = ax1[0];
info->m_J1angularAxis[srow+1] = ax1[1];
info->m_J1angularAxis[srow+2] = ax1[2];
info->m_J2angularAxis[srow+0] = -ax1[0];
info->m_J2angularAxis[srow+1] = -ax1[1];
info->m_J2angularAxis[srow+2] = -ax1[2];
btScalar lostop = getLowerLimit();
btScalar histop = getUpperLimit();
if(limit && (lostop == histop))
{ // the joint motor is ineffective
powered = 0;
}
info->m_constraintError[srow] = btScalar(0.0f);
btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
if(powered)
{
if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
{
info->cfm[srow] = m_normalCFM;
}
btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
info->m_lowerLimit[srow] = - m_maxMotorImpulse;
info->m_upperLimit[srow] = m_maxMotorImpulse;
}
if(limit)
{
k = info->fps * currERP;
info->m_constraintError[srow] += k * limit_err;
if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
{
info->cfm[srow] = m_stopCFM;
}
if(lostop == histop)
{
// limited low and high simultaneously
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else if(limit == 1)
{ // low limit
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else
{ // high limit
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = 0;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
btScalar bounce = m_relaxationFactor;
if(bounce > btScalar(0.0))
{
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)
{ // low limit
if(vel < 0)
{
btScalar newc = -bounce * vel;
if(newc > info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if(vel > 0)
{
btScalar newc = -bounce * vel;
if(newc < info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
}
info->m_constraintError[srow] *= m_biasFactor;
} // if(limit)
} // if angular limit or powered
}
void btHingeConstraint::buildJacobian()
{
if (m_useSolveConstraintObsolete)
{
m_appliedImpulse = btScalar(0.);
m_accMotorImpulse = btScalar(0.);
if (!m_angularOnly)
{
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
btVector3 relPos = pivotBInW - pivotAInW;
btVector3 normal[3];
if (relPos.length2() > SIMD_EPSILON)
{
normal[0] = relPos.normalized();
}
else
{
normal[0].setValue(btScalar(1.0),0,0);
}
btPlaneSpace1(normal[0], normal[1], normal[2]);
for (int i=0;i<3;i++)
{
new (&m_jac[i]) btJacobianEntry(
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
pivotAInW - m_rbA.getCenterOfMassPosition(),
pivotBInW - m_rbB.getCenterOfMassPosition(),
normal[i],
m_rbA.getInvInertiaDiagLocal(),
m_rbA.getInvMass(),
m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvMass());
}
}
//calculate two perpendicular jointAxis, orthogonal to hingeAxis
//these two jointAxis require equal angular velocities for both bodies
//this is unused for now, it's a todo
btVector3 jointAxis0local;
btVector3 jointAxis1local;
btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
new (&m_jacAng[0]) btJacobianEntry(jointAxis0,
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
m_rbA.getInvInertiaDiagLocal(),
m_rbB.getInvInertiaDiagLocal());
new (&m_jacAng[1]) btJacobianEntry(jointAxis1,
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
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());
// clear accumulator
m_accLimitImpulse = btScalar(0.);
// test angular limit
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);
m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
getRigidBodyB().computeAngularImpulseDenominator(axisA));
}
}
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 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);
}
}
}
}
void btConeTwistConstraint::calcAngleInfo()
{
m_swingCorrection = btScalar(0.);
m_twistLimitSign = btScalar(0.);
m_solveTwistLimit = false;
m_solveSwingLimit = false;
btVector3 b1Axis1,b1Axis2,b1Axis3;
btVector3 b2Axis1,b2Axis2;
b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
btScalar swx=btScalar(0.),swy = btScalar(0.);
btScalar thresh = btScalar(10.);
btScalar fact;
// Get Frame into world space
if (m_swingSpan1 >= btScalar(0.05f))
{
b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
swx = b2Axis1.dot(b1Axis1);
swy = b2Axis1.dot(b1Axis2);
swing1 = btAtan2Fast(swy, swx);
fact = (swy*swy + swx*swx) * thresh * thresh;
fact = fact / (fact + btScalar(1.0));
swing1 *= fact;
}
if (m_swingSpan2 >= btScalar(0.05f))
{
b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);
swx = b2Axis1.dot(b1Axis1);
swy = b2Axis1.dot(b1Axis3);
swing2 = btAtan2Fast(swy, swx);
fact = (swy*swy + swx*swx) * thresh * thresh;
fact = fact / (fact + btScalar(1.0));
swing2 *= fact;
}
btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);
btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);
btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq;
if (EllipseAngle > 1.0f)
{
m_swingCorrection = EllipseAngle-1.0f;
m_solveSwingLimit = true;
// Calculate necessary axis & factors
m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
m_swingAxis.normalize();
btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
m_swingAxis *= swingAxisSign;
}
// Twist limits
if (m_twistSpan >= btScalar(0.))
{
btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
btVector3 TwistRef = quatRotate(rotationArc,b2Axis2);
btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
m_twistAngle = twist;
// btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.);
if (twist <= -m_twistSpan*lockedFreeFactor)
{
m_twistCorrection = -(twist + m_twistSpan);
m_solveTwistLimit = true;
m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
m_twistAxis.normalize();
m_twistAxis *= -1.0f;
}
else if (twist > m_twistSpan*lockedFreeFactor)
{
m_twistCorrection = (twist - m_twistSpan);
m_solveTwistLimit = true;
m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
m_twistAxis.normalize();
}
}
}
void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
{
#ifndef __SPU__
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;
bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
btVector3 vel2;
bodyB.internalGetVelocityInLocalPointObsolete(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 ftorqueAxis1 = rel_pos1.cross(normal);
btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
}
}
// apply motor
if (m_bMotorEnabled)
{
// compute current and predicted transforms
btTransform trACur = m_rbA.getCenterOfMassTransform();
btTransform trBCur = m_rbB.getCenterOfMassTransform();
btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA);
btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB);
btTransform trAPred; trAPred.setIdentity();
btVector3 zerovec(0,0,0);
btTransformUtil::integrateTransform(
trACur, zerovec, omegaA, timeStep, trAPred);
btTransform trBPred; trBPred.setIdentity();
btTransformUtil::integrateTransform(
trBCur, zerovec, omegaB, timeStep, trBPred);
// compute desired transforms in world
btTransform trPose(m_qTarget);
btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse();
btTransform trADes = trBPred * trABDes;
btTransform trBDes = trAPred * trABDes.inverse();
// compute desired omegas in world
btVector3 omegaADes, omegaBDes;
btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes);
btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes);
// compute delta omegas
btVector3 dOmegaA = omegaADes - omegaA;
btVector3 dOmegaB = omegaBDes - omegaB;
// compute weighted avg axis of dOmega (weighting based on inertias)
btVector3 axisA, axisB;
btScalar kAxisAInv = 0, kAxisBInv = 0;
if (dOmegaA.length2() > SIMD_EPSILON)
{
axisA = dOmegaA.normalized();
kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA);
}
if (dOmegaB.length2() > SIMD_EPSILON)
{
axisB = dOmegaB.normalized();
kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB);
}
btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;
static bool bDoTorque = true;
if (bDoTorque && avgAxis.length2() > SIMD_EPSILON)
{
avgAxis.normalize();
kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis);
kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis);
btScalar kInvCombined = kAxisAInv + kAxisBInv;
btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) /
(kInvCombined * kInvCombined);
if (m_maxMotorImpulse >= 0)
{
btScalar fMaxImpulse = m_maxMotorImpulse;
if (m_bNormalizedMotorStrength)
fMaxImpulse = fMaxImpulse/kAxisAInv;
btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse;
btScalar newUnclampedMag = newUnclampedAccImpulse.length();
if (newUnclampedMag > fMaxImpulse)
{
newUnclampedAccImpulse.normalize();
newUnclampedAccImpulse *= fMaxImpulse;
impulse = newUnclampedAccImpulse - m_accMotorImpulse;
}
m_accMotorImpulse += impulse;
}
btScalar impulseMag = impulse.length();
btVector3 impulseAxis = impulse / impulseMag;
bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
}
}
else if (m_damping > SIMD_EPSILON) // no motor: do a little damping
{
btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA);
btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB);
btVector3 relVel = angVelB - angVelA;
if (relVel.length2() > SIMD_EPSILON)
{
btVector3 relVelAxis = relVel.normalized();
btScalar m_kDamping = btScalar(1.) /
(getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) +
getRigidBodyB().computeAngularImpulseDenominator(relVelAxis));
btVector3 impulse = m_damping * m_kDamping * relVel;
btScalar impulseMag = impulse.length();
btVector3 impulseAxis = impulse / impulseMag;
bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
}
}
// joint limits
{
///solve angular part
btVector3 angVelA;
bodyA.internalGetAngularVelocity(angVelA);
btVector3 angVelB;
bodyB.internalGetAngularVelocity(angVelB);
// solve swing limit
if (m_solveSwingLimit)
{
btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep;
btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis);
if (relSwingVel > 0)
amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor;
btScalar impulseMag = amplitude * m_kSwing;
// Clamp the accumulated impulse
btScalar temp = m_accSwingLimitImpulse;
m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
impulseMag = m_accSwingLimitImpulse - temp;
btVector3 impulse = m_swingAxis * impulseMag;
// don't let cone response affect twist
// (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit)
{
btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA;
btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple;
impulse = impulseNoTwistCouple;
}
impulseMag = impulse.length();
btVector3 noTwistSwingAxis = impulse / impulseMag;
bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
}
// solve twist limit
if (m_solveTwistLimit)
{
btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep;
btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis );
if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important)
amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor;
btScalar impulseMag = amplitude * m_kTwist;
// Clamp the accumulated impulse
btScalar temp = m_accTwistLimitImpulse;
m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
impulseMag = m_accTwistLimitImpulse - temp;
// btVector3 impulse = m_twistAxis * impulseMag;
bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
}
}
}
#else
btAssert(0);
#endif //__SPU__
}
void btConeTwistConstraint::buildJacobian()
{
m_appliedImpulse = btScalar(0.);
//set bias, sign, clear accumulator
m_swingCorrection = btScalar(0.);
m_twistLimitSign = btScalar(0.);
m_solveTwistLimit = false;
m_solveSwingLimit = false;
m_accTwistLimitImpulse = btScalar(0.);
m_accSwingLimitImpulse = btScalar(0.);
if (!m_angularOnly)
{
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
btVector3 relPos = pivotBInW - pivotAInW;
btVector3 normal[3];
if (relPos.length2() > SIMD_EPSILON)
{
normal[0] = relPos.normalized();
}
else
{
normal[0].setValue(btScalar(1.0),0,0);
}
btPlaneSpace1(normal[0], normal[1], normal[2]);
for (int i=0;i<3;i++)
{
new (&m_jac[i]) btJacobianEntry(
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
pivotAInW - m_rbA.getCenterOfMassPosition(),
pivotBInW - m_rbB.getCenterOfMassPosition(),
normal[i],
m_rbA.getInvInertiaDiagLocal(),
m_rbA.getInvMass(),
m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvMass());
}
}
btVector3 b1Axis1,b1Axis2,b1Axis3;
btVector3 b2Axis1,b2Axis2;
b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
// Get Frame into world space
if (m_swingSpan1 >= btScalar(0.05f))
{
b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
}
if (m_swingSpan2 >= btScalar(0.05f))
{
b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);
swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
}
btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);
btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);
btScalar EllipseAngle = btFabs(swing1)* RMaxAngle1Sq + btFabs(swing2) * RMaxAngle2Sq;
if (EllipseAngle > 1.0f)
{
m_swingCorrection = EllipseAngle-1.0f;
m_solveSwingLimit = true;
// Calculate necessary axis & factors
m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
m_swingAxis.normalize();
btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
m_swingAxis *= swingAxisSign;
m_kSwing = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
}
// Twist limits
if (m_twistSpan >= btScalar(0.))
{
btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
btVector3 TwistRef = quatRotate(rotationArc,b2Axis2);
btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
if (twist <= -m_twistSpan*lockedFreeFactor)
{
m_twistCorrection = -(twist + m_twistSpan);
m_solveTwistLimit = true;
m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
m_twistAxis.normalize();
m_twistAxis *= -1.0f;
m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
} else
if (twist > m_twistSpan*lockedFreeFactor)
{
m_twistCorrection = (twist - m_twistSpan);
m_solveTwistLimit = true;
m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
m_twistAxis.normalize();
m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
}
}
}
void btConeTwistConstraint::solveConstraint(btScalar timeStep)
{
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
btScalar tau = btScalar(0.3);
btScalar damping = btScalar(1.);
//linear part
if (!m_angularOnly)
{
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;
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;
m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
}
}
{
///solve angular part
const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
// solve swing limit
if (m_solveSwingLimit)
{
btScalar amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(btScalar(1.)/timeStep)*m_biasFactor);
btScalar impulseMag = amplitude * m_kSwing;
// Clamp the accumulated impulse
btScalar temp = m_accSwingLimitImpulse;
m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
impulseMag = m_accSwingLimitImpulse - temp;
btVector3 impulse = m_swingAxis * impulseMag;
m_rbA.applyTorqueImpulse(impulse);
m_rbB.applyTorqueImpulse(-impulse);
}
// solve twist limit
if (m_solveTwistLimit)
{
btScalar amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(btScalar(1.)/timeStep)*m_biasFactor );
btScalar impulseMag = amplitude * m_kTwist;
// Clamp the accumulated impulse
btScalar temp = m_accTwistLimitImpulse;
m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
impulseMag = m_accTwistLimitImpulse - temp;
btVector3 impulse = m_twistAxis * impulseMag;
m_rbA.applyTorqueImpulse(impulse);
m_rbB.applyTorqueImpulse(-impulse);
}
}
}
bool BConstraint::isThis(btRigidBody *body_1, btRigidBody *body_2) const
{
if(&getRigidBodyA()==body_1 && &getRigidBodyB()==body_2)
return true;
return false;
}
//--------------------------------------------------------------
ofVec3f ofxBulletJoint::getPositionA() const {
return ofGetVec3fPosFromRigidBody( getRigidBodyA() );
}
void btConeTwistConstraint::calcAngleInfo2()
{
m_swingCorrection = btScalar(0.);
m_twistLimitSign = btScalar(0.);
m_solveTwistLimit = false;
m_solveSwingLimit = false;
// compute rotation of A wrt B (in constraint space)
if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
{ // it is assumed that setMotorTarget() was alredy called
// and motor target m_qTarget is within constraint limits
// TODO : split rotation to pure swing and pure twist
// compute desired transforms in world
btTransform trPose(m_qTarget);
btTransform trA = getRigidBodyA().getCenterOfMassTransform() * m_rbAFrame;
btTransform trB = getRigidBodyB().getCenterOfMassTransform() * m_rbBFrame;
btTransform trDeltaAB = trB * trPose * trA.inverse();
btQuaternion qDeltaAB = trDeltaAB.getRotation();
btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
m_swingAxis = swingAxis;
m_swingAxis.normalize();
m_swingCorrection = qDeltaAB.getAngle();
if(!btFuzzyZero(m_swingCorrection))
{
m_solveSwingLimit = true;
}
return;
}
{
// compute rotation of A wrt B (in constraint space)
btQuaternion qA = getRigidBodyA().getCenterOfMassTransform().getRotation() * m_rbAFrame.getRotation();
btQuaternion qB = getRigidBodyB().getCenterOfMassTransform().getRotation() * m_rbBFrame.getRotation();
btQuaternion qAB = qB.inverse() * qA;
// split rotation into cone and twist
// (all this is done from B's perspective. Maybe I should be averaging axes...)
btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
{
btScalar swingAngle, swingLimit = 0; btVector3 swingAxis;
computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit);
if (swingAngle > swingLimit * m_limitSoftness)
{
m_solveSwingLimit = true;
// compute limit ratio: 0->1, where
// 0 == beginning of soft limit
// 1 == hard/real limit
m_swingLimitRatio = 1.f;
if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON)
{
m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/
(swingLimit - swingLimit * m_limitSoftness);
}
// swing correction tries to get back to soft limit
m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);
// adjustment of swing axis (based on ellipse normal)
adjustSwingAxisToUseEllipseNormal(swingAxis);
// Calculate necessary axis & factors
m_swingAxis = quatRotate(qB, -swingAxis);
m_twistAxisA.setValue(0,0,0);
m_kSwing = btScalar(1.) /
(getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
}
}
else
{
// you haven't set any limits;
// or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
// anyway, we have either hinge or fixed joint
btVector3 ivA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
btVector3 jvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
btVector3 kvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
btVector3 ivB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(0);
btVector3 target;
btScalar x = ivB.dot(ivA);
btScalar y = ivB.dot(jvA);
btScalar z = ivB.dot(kvA);
if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
{ // fixed. We'll need to add one more row to constraint
if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
{
m_solveSwingLimit = true;
m_swingAxis = -ivB.cross(ivA);
}
}
else
{
if(m_swingSpan1 < m_fixThresh)
{ // hinge around Y axis
if(!(btFuzzyZero(y)))
{
m_solveSwingLimit = true;
if(m_swingSpan2 >= m_fixThresh)
{
y = btScalar(0.f);
btScalar span2 = btAtan2(z, x);
if(span2 > m_swingSpan2)
{
x = btCos(m_swingSpan2);
z = btSin(m_swingSpan2);
}
else if(span2 < -m_swingSpan2)
{
x = btCos(m_swingSpan2);
z = -btSin(m_swingSpan2);
}
}
}
}
else
{ // hinge around Z axis
if(!btFuzzyZero(z))
{
m_solveSwingLimit = true;
if(m_swingSpan1 >= m_fixThresh)
{
z = btScalar(0.f);
btScalar span1 = btAtan2(y, x);
if(span1 > m_swingSpan1)
{
x = btCos(m_swingSpan1);
y = btSin(m_swingSpan1);
}
else if(span1 < -m_swingSpan1)
{
x = btCos(m_swingSpan1);
y = -btSin(m_swingSpan1);
}
}
}
}
target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0];
target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1];
target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2];
target.normalize();
m_swingAxis = -ivB.cross(target);
m_swingCorrection = m_swingAxis.length();
m_swingAxis.normalize();
}
}
if (m_twistSpan >= btScalar(0.f))
{
btVector3 twistAxis;
computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);
if (m_twistAngle > m_twistSpan*m_limitSoftness)
{
m_solveTwistLimit = true;
m_twistLimitRatio = 1.f;
if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON)
{
m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/
(m_twistSpan - m_twistSpan * m_limitSoftness);
}
// twist correction tries to get back to soft limit
m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);
m_twistAxis = quatRotate(qB, -twistAxis);
m_kTwist = btScalar(1.) /
(getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
}
if (m_solveSwingLimit)
m_twistAxisA = quatRotate(qA, -twistAxis);
}
else
{
m_twistAngle = btScalar(0.f);
}
}
}
void btHingeConstraint::solveConstraint(btScalar timeStep)
{
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 = m_rbA.getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
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;
m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
}
}
{
///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);
const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
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;
}
//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;
}
m_rbA.applyTorqueImpulse(-velrelOrthog+angularError);
m_rbB.applyTorqueImpulse(velrelOrthog-angularError);
// 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;
btVector3 impulse = axisA * impulseMag * m_limitSign;
m_rbA.applyTorqueImpulse(impulse);
m_rbB.applyTorqueImpulse(-impulse);
}
}
//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;;
//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);
}
}
}
