void
dxJointHinge::getInfo1( dxJoint::Info1 *info )
{
info->nub = 5;
// see if joint is powered
if ( limot.fmax > 0 )
info->m = 6; // powered hinge needs an extra constraint row
else info->m = 5;
// if proper joint limits are specified
// see if we're at a joint limit.
if ( limot.lostop <= limot.histop )
{
dReal angle = getHingeAngle( node[0].body,
node[1].body,
axis1, qrel );
// From angle, update cumulative_angle, which does not wrap.
// Assume this is called only once per time step.
cumulative_angle =
dShortestAngularDistanceUpdate(cumulative_angle,angle);
if ( limot.testRotationalLimit( cumulative_angle ) )
info->m = 6;
}
// joint damping
if ( use_damping )
info->m = 6;
}
void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB)
{
// Compute limit information
m_hingeAngle = getHingeAngle(transA,transB);
m_correction = btScalar(0.);
m_limitSign = btScalar(0.);
m_solveLimit = false;
if (m_lowerLimit <= m_upperLimit)
{
m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, m_lowerLimit, m_upperLimit);
if (m_hingeAngle <= m_lowerLimit)
{
m_correction = (m_lowerLimit - m_hingeAngle);
m_limitSign = 1.0f;
m_solveLimit = true;
}
else if (m_hingeAngle >= m_upperLimit)
{
m_correction = m_upperLimit - m_hingeAngle;
m_limitSign = -1.0f;
m_solveLimit = true;
}
}
return;
}
void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB)
{
// Compute limit information
m_hingeAngle = getHingeAngle(transA,transB);
m_limit.test(m_hingeAngle);
return;
}
void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
{
m_limit.fit(targetAngle);
// compute angular velocity
btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
btScalar dAngle = targetAngle - curAngle;
m_motorTargetVelocity = dAngle / dt;
}
void btHingeAccumulatedAngleConstraint::getInfo1(btConstraintInfo1* info)
{
//update m_accumulatedAngle
btScalar curHingeAngle = getHingeAngle();
m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,curHingeAngle);
btHingeConstraint::getInfo1(info);
}
void
dxJointScrew::getInfo1( dxJoint::Info1 *info )
{
info->nub = 5;
info->m = 5;
// if proper joint limits are specified
// see if we're at a joint limit for the rotational hinge
if ( limot.lostop <= limot.histop )
{
dReal angle = getHingeAngle( node[0].body,
node[1].body,
axis1, qrel );
// from angle, update cumulative_angle, which does not wrap
cumulative_angle =
dShortestAngularDistanceUpdate(cumulative_angle,angle);
// printf("angle: %f lo[%f] hi[%f]\n", cumulative_angle,
// limot.lostop, limot.histop);
if ( limot.testRotationalLimit( cumulative_angle ) )
info->m = 6;
// FIXME: reset does not reset joint angle if more than 2pi
// printf("cumulative angle: %f lo: %f hi: %f test: %f\n",
// cumulative_angle, limot.lostop, limot.histop,
// limot.testRotationalLimit( cumulative_angle ));
}
/* uncommnet to enforce slider joint limit
// see if we're at a joint limit for the slider
limot.limit = 0;
if ( ( limot.lostop > -dInfinity || limot.histop < dInfinity ) &&
limot.lostop <= limot.histop )
{
// measure joint position
dReal pos = dJointGetScrewPosition ( this );
// printf("pos: %f lo[%f] hi[%f]\n", pos, limot.lostop, limot.histop);
if ( pos <= limot.lostop )
{
limot.limit = 1;
limot.limit_err = pos - limot.lostop;
info->m = 6;
}
else if ( pos >= limot.histop )
{
limot.limit = 2;
limot.limit_err = pos - limot.histop;
info->m = 6;
}
}
*/
}
void
dxJointGearbox::getInfo2( dxJoint::Info2* info )
{
dVector3 globalAxis1, globalAxis2;
dBodyVectorToWorld(node[0].body, axis1[0], axis1[1], axis1[2], globalAxis1);
dBodyVectorToWorld(node[1].body, axis2[0], axis2[1], axis2[2], globalAxis2);
double ang1 = getHingeAngle(refBody1,node[0].body,globalAxis1,qrel1);
double ang2 = getHingeAngle(refBody2,node[1].body,globalAxis2,qrel2);
// printf("a1(%f) a10(%f) a2(%f) a20(%f)\n",
// ang1, cumulative_angle1, ang2, cumulative_angle2);
cumulative_angle1 = dShortestAngularDistanceUpdate(cumulative_angle1,ang1);
cumulative_angle2 = dShortestAngularDistanceUpdate(cumulative_angle2,ang2);
double err = dShortestAngularDistance(
cumulative_angle1, -ratio * cumulative_angle2);
// FIXME: error calculation is not amenable to reset of poses,
// cumulative angles might snap to wrong angular value.
// printf("a1(%f) a1f(%f) a2(%f) a2f(%f) e(%f)\n",
// ang1, cumulative_angle1, ang2, cumulative_angle2, err);
info->J1a[0] = globalAxis1[0];
info->J1a[1] = globalAxis1[1];
info->J1a[2] = globalAxis1[2];
info->J2a[0] = ratio * globalAxis2[0];
info->J2a[1] = ratio * globalAxis2[1];
info->J2a[2] = ratio * globalAxis2[2];
dReal k = info->fps * info->erp;
info->c[0] = -k * err;
// dVector3 d;
// dAddScaledVectors3(d, node[0].body->avel, node[1].body->avel,
// 1.0, ratio);
// printf("d: %f\n", dCalcVectorDot3(globalAxis1, d));
}
void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
{
if (m_lowerLimit < m_upperLimit)
{
if (targetAngle < m_lowerLimit)
targetAngle = m_lowerLimit;
else if (targetAngle > m_upperLimit)
targetAngle = m_upperLimit;
}
// compute angular velocity
btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
btScalar dAngle = targetAngle - curAngle;
m_motorTargetVelocity = dAngle / dt;
}
void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
{
#ifdef _BT_USE_CENTER_LIMIT_
m_limit.fit(targetAngle);
#else
if (m_lowerLimit < m_upperLimit)
{
if (targetAngle < m_lowerLimit)
targetAngle = m_lowerLimit;
else if (targetAngle > m_upperLimit)
targetAngle = m_upperLimit;
}
#endif
// compute angular _velocity
btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
btScalar dAngle = targetAngle - curAngle;
m_motorTargetVelocity = dAngle / dt;
}
dReal dJointGetPRAngle( dJointID j )
{
dxJointPR* joint = ( dxJointPR* )j;
dAASSERT( joint );
checktype( joint, PR );
if ( joint->node[0].body )
{
dReal ang = getHingeAngle( joint->node[0].body,
joint->node[1].body,
joint->axisR1,
joint->qrel );
if ( joint->flags & dJOINT_REVERSE )
return -ang;
else
return ang;
}
else return 0;
}
dReal dJointGetHingeAngle( dJointID j )
{
dxJointHinge* joint = ( dxJointHinge* )j;
dAASSERT( joint );
checktype( joint, Hinge );
if ( joint->node[0].body )
{
dReal ang = getHingeAngle( joint->node[0].body,
joint->node[1].body,
joint->axis1,
joint->qrel );
// from angle, update cumulative_angle, which does not wrap
joint->cumulative_angle = joint->cumulative_angle + shortest_angular_distance(joint->cumulative_angle,ang);
if ( joint->flags & dJOINT_REVERSE )
return -joint->cumulative_angle;
else
return joint->cumulative_angle;
}
else return 0;
}
void btHingeConstraint::testLimit()
{
// Compute limit information
m_hingeAngle = getHingeAngle();
m_correction = btScalar(0.);
m_limitSign = btScalar(0.);
m_solveLimit = false;
if (m_lowerLimit <= m_upperLimit)
{
if (m_hingeAngle <= m_lowerLimit)
{
m_correction = (m_lowerLimit - m_hingeAngle);
m_limitSign = 1.0f;
m_solveLimit = true;
}
else if (m_hingeAngle >= m_upperLimit)
{
m_correction = m_upperLimit - m_hingeAngle;
m_limitSign = -1.0f;
m_solveLimit = true;
}
}
return;
}
void
dxJointPR::getInfo1( dxJoint::Info1 *info )
{
info->nub = 4;
info->m = 4;
// see if we're at a joint limit.
limotP.limit = 0;
if (( limotP.lostop > -dInfinity || limotP.histop < dInfinity ) &&
limotP.lostop <= limotP.histop )
{
// measure joint position
dReal pos = dJointGetPRPosition( this );
limotP.testRotationalLimit( pos ); // N.B. The function is ill named
}
// powered needs an extra constraint row
if ( limotP.limit || limotP.fmax > 0 ) info->m++;
// see if we're at a joint limit.
limotR.limit = 0;
if (( limotR.lostop >= -M_PI || limotR.histop <= M_PI ) &&
limotR.lostop <= limotR.histop )
{
dReal angle = getHingeAngle( node[0].body,
node[1].body,
axisR1, qrel );
limotR.testRotationalLimit( angle );
}
// powered morit or at limits needs an extra constraint row
if ( limotR.limit || limotR.fmax > 0 ) info->m++;
}
dReal dJointGetHingeAngle( dJointID j )
{
dxJointHinge* joint = ( dxJointHinge* )j;
dAASSERT( joint );
checktype( joint, Hinge );
if ( joint->node[0].body )
{
dReal ang = getHingeAngle( joint->node[0].body,
joint->node[1].body,
joint->axis1,
joint->qrel );
// from angle, update cumulative_angle, which does not wrap
// dJointGetHingeAngle is static, so do not overwrite
// joint->cumulative_angle by updated joint angle.
// Simply calculate the current angle and return it.
dReal joint_angle =
dShortestAngularDistanceUpdate(joint->cumulative_angle,ang);
if ( joint->flags & dJOINT_REVERSE )
return -joint_angle;
else
return joint_angle;
}
else return 0;
}
btScalar btHingeConstraint::getHingeAngle()
{
return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
}
btScalar btHingeAccumulatedAngleConstraint::getAccumulatedHingeAngle()
{
btScalar hingeAngle = getHingeAngle();
m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,hingeAngle);
return m_accumulatedAngle;
}
void btHingeConstraint::buildJacobian()
{
m_appliedImpulse = 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);
getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
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());
// Compute limit information
btScalar hingeAngle = getHingeAngle();
//set bias, sign, clear accumulator
m_correction = btScalar(0.);
m_limitSign = btScalar(0.);
m_solveLimit = false;
m_accLimitImpulse = btScalar(0.);
// if (m_lowerLimit < m_upperLimit)
if (m_lowerLimit <= m_upperLimit)
{
// if (hingeAngle <= m_lowerLimit*m_limitSoftness)
if (hingeAngle <= m_lowerLimit)
{
m_correction = (m_lowerLimit - hingeAngle);
m_limitSign = 1.0f;
m_solveLimit = true;
}
// else if (hingeAngle >= m_upperLimit*m_limitSoftness)
else if (hingeAngle >= m_upperLimit)
{
m_correction = m_upperLimit - hingeAngle;
m_limitSign = -1.0f;
m_solveLimit = true;
}
}
//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));
}
