internal int get_limit_motor_info2(
btRotationalLimitMotor limot,
ref btTransform transA, ref btTransform transB, ref btVector3 linVelA, ref btVector3 linVelB, ref btVector3 angVelA, ref btVector3 angVelB,
ref btConstraintInfo2 info, int row, ref btVector3 ax1, bool rotational, bool rotAllowed = false )
{
//int srow = row * info.rowskip;
bool powered = limot.m_enableMotor;
int limit = limot.m_currentLimit;
if( powered || limit != 0 )
{ // if the joint is powered, or has joint limits, add in the extra row
if( rotational )
{
info.m_solverConstraints[row].m_relpos1CrossNormal = ax1;
ax1.Invert( out info.m_solverConstraints[row].m_relpos2CrossNormal );
}
else
{
info.m_solverConstraints[row].m_contactNormal1 = ax1;
ax1.Invert( out info.m_solverConstraints[row].m_contactNormal2 );
}
/*
double* J1 = rotational ? info.m_J1angularAxis : info.m_J1linearAxis;
double* J2 = rotational ? info.m_J2angularAxis : info.m_J2linearAxis;
J1[srow + 0] = ax1[0];
J1[srow + 1] = ax1[1];
J1[srow + 2] = ax1[2];
J2[srow + 0] = -ax1[0];
J2[srow + 1] = -ax1[1];
J2[srow + 2] = -ax1[2];
*/
if( ( !rotational ) )
{
if( m_useOffsetForConstraintFrame )
{
btVector3 tmpA, tmpB, relA, relB;
// get vector from bodyB to frameB in WCS
m_calculatedTransformB.m_origin.Sub( ref transB.m_origin, out relB );
// get its projection to constraint axis
btVector3 projB = ax1 * relB.dot( ax1 );
// get vector directed from bodyB to constraint axis (and orthogonal to it)
btVector3 orthoB = relB - projB;
// same for bodyA
m_calculatedTransformA.m_origin.Sub( ref transA.m_origin, out relA );
btVector3 projA = ax1 * relA.dot( ax1 );
btVector3 orthoA = relA - projA;
// get desired offset between frames A and B along constraint axis
double desiredOffs = limot.m_currentPosition - limot.m_currentLimitError;
// desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
btVector3 totalDist = projA + ax1 * desiredOffs - projB;
// get offset vectors relA and relB
relA = orthoA + totalDist * m_factA;
relB = orthoB - totalDist * m_factB;
tmpA = relA.cross( ax1 );
tmpB = relB.cross( ax1 );
if( m_hasStaticBody && ( !rotAllowed ) )
{
tmpA *= m_factA;
tmpB *= m_factB;
}
//int i;
info.m_solverConstraints[row].m_relpos1CrossNormal = tmpA;
tmpB.Invert( out info.m_solverConstraints[row].m_relpos2CrossNormal );
//for( i = 0; i < 3; i++ ) info.m_J1angularAxis[srow + i] = tmpA[i];
//for( i = 0; i < 3; i++ ) info.m_J2angularAxis[srow + i] = -tmpB[i];
}
else
{
btVector3 ltd; // Linear Torque Decoupling vector
btVector3 c = m_calculatedTransformB.m_origin - transA.m_origin;
ltd = c.cross( ax1 );
info.m_solverConstraints[row].m_relpos1CrossNormal = ltd;
//info.m_J1angularAxis[srow + 0] = ltd[0];
//info.m_J1angularAxis[srow + 1] = ltd[1];
//info.m_J1angularAxis[srow + 2] = ltd[2];
c = m_calculatedTransformB.m_origin - transB.m_origin;
ltd = -c.cross( ax1 );
info.m_solverConstraints[row].m_relpos2CrossNormal = ltd;
//info.m_J2angularAxis[srow + 0] = ltd[0];
//info.m_J2angularAxis[srow + 1] = ltd[1];
//info.m_J2angularAxis[srow + 2] = ltd[2];
}
}
// if we're limited low and high simultaneously, the joint motor is
// ineffective
if( limit != 0 && ( limot.m_loLimit == limot.m_hiLimit ) ) powered = false;
info.m_solverConstraints[row].m_rhs = (double)( 0 );
if( powered )
{
info.m_solverConstraints[row].m_cfm = limot.m_normalCFM;
if( limit == 0 )
{
double tag_vel = rotational ? limot.m_targetVelocity : -limot.m_targetVelocity;
double mot_fact = getMotorFactor( limot.m_currentPosition,
limot.m_loLimit,
limot.m_hiLimit,
tag_vel,
info.fps * limot.m_stopERP );
info.m_solverConstraints[row].m_rhs += mot_fact * limot.m_targetVelocity;
info.m_solverConstraints[row].m_lowerLimit = -limot.m_maxMotorForce;
info.m_solverConstraints[row].m_upperLimit = limot.m_maxMotorForce;
}
}
if( limit != 0 )
{
double k = info.fps * limot.m_stopERP;
if( !rotational )
{
info.m_solverConstraints[row].m_rhs += k * limot.m_currentLimitError;
}
else
{
info.m_solverConstraints[row].m_rhs += -k * limot.m_currentLimitError;
}
info.m_solverConstraints[row].m_cfm = limot.m_stopCFM;
if( limot.m_loLimit == limot.m_hiLimit )
{ // limited low and high simultaneously
info.m_solverConstraints[row].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[row].m_upperLimit = btScalar.BT_MAX_FLOAT;
}
else
{
if( limit == 1 )
{
info.m_solverConstraints[row].m_lowerLimit = 0;
info.m_solverConstraints[row].m_upperLimit = btScalar.BT_MAX_FLOAT;
}
else
{
info.m_solverConstraints[row].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[row].m_upperLimit = 0;
}
// deal with bounce
if( limot.m_bounce > 0 )
{
// calculate joint velocity
double vel;
if( rotational )
{
vel = angVelA.dot( ax1 );
//make sure that if no body . angVelB == zero vec
// if (body1)
vel -= angVelB.dot( ax1 );
}
else
{
vel = linVelA.dot( ax1 );
//make sure that if no body . angVelB == zero vec
// if (body1)
vel -= linVelB.dot( ax1 );
}
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if( limit == 1 )
{
if( vel < 0 )
{
double newc = -limot.m_bounce * vel;
if( newc > info.m_solverConstraints[row].m_rhs )
info.m_solverConstraints[row].m_rhs = newc;
}
}
else
{
if( vel > 0 )
{
double newc = -limot.m_bounce * vel;
if( newc < info.m_solverConstraints[row].m_rhs )
info.m_solverConstraints[row].m_rhs = newc;
}
}
}
}
}
return 1;
}
else return 0;
}
internal btRotationalLimitMotor( ref btRotationalLimitMotor limot )
{
m_targetVelocity = limot.m_targetVelocity;
m_maxMotorForce = limot.m_maxMotorForce;
m_limitSoftness = limot.m_limitSoftness;
m_loLimit = limot.m_loLimit;
m_hiLimit = limot.m_hiLimit;
m_normalCFM = limot.m_normalCFM;
m_stopERP = limot.m_stopERP;
m_stopCFM = limot.m_stopCFM;
m_bounce = limot.m_bounce;
m_currentLimit = limot.m_currentLimit;
m_currentLimitError = limot.m_currentLimitError;
m_enableMotor = limot.m_enableMotor;
}
int setLinearLimits( ref btConstraintInfo2 info, int row
, ref btTransform transA, ref btTransform transB
, ref btVector3 linVelA, ref btVector3 linVelB
, ref btVector3 angVelA, ref btVector3 angVelB )
{
// int row = 0;
//solve linear limits
btRotationalLimitMotor limot = new btRotationalLimitMotor();
for( int i = 0; i < 3; i++ )
{
if( m_linearLimits.needApplyForce( i ) )
{ // re-use rotational motor code
limot.m_bounce = btScalar.BT_ZERO;
limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
limot.m_damping = m_linearLimits.m_damping;
limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
limot.m_maxLimitForce = (double)( 0 );
limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
btVector3 axis = m_calculatedTransformA.m_basis.getColumn( i );
//bt6DofFlags flags = m_flags >> ( i * BT_6DOF_FLAGS_AXIS_SHIFT );
limot.m_normalCFM = ( m_flags & bt6DofFlagsIndexed.BT_6DOF_FLAGS_CFM_NORM[i] ) != 0 ? m_linearLimits.m_normalCFM[i] : info.m_solverConstraints[0].m_cfm;
limot.m_stopCFM = ( m_flags & bt6DofFlagsIndexed.BT_6DOF_FLAGS_CFM_STOP[i] ) != 0 ? m_linearLimits.m_stopCFM[i] : info.m_solverConstraints[0].m_cfm;
limot.m_stopERP = ( m_flags & bt6DofFlagsIndexed.BT_6DOF_FLAGS_ERP_STOP[i] ) != 0 ? m_linearLimits.m_stopERP[i] : info.erp;
if( m_useOffsetForConstraintFrame )
{
int indx1 = ( i + 1 ) % 3;
int indx2 = ( i + 2 ) % 3;
bool rotAllowed = true; // rotations around orthos to current axis
if( m_angularLimits[indx1].m_currentLimit != 0
&& m_angularLimits[indx2].m_currentLimit != 0 )
{
rotAllowed = false;
}
row += get_limit_motor_info2( limot, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB, ref info, row, ref axis, false, rotAllowed );
}
else
{
row += get_limit_motor_info2( limot, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB, ref info, row, ref axis, false );
}
}
}
return row;
}
