internal btRotationalLimitMotor2( btRotationalLimitMotor2 limot )
{
m_loLimit = limot.m_loLimit;
m_hiLimit = limot.m_hiLimit;
m_bounce = limot.m_bounce;
m_stopERP = limot.m_stopERP;
m_stopCFM = limot.m_stopCFM;
m_motorERP = limot.m_motorERP;
m_motorCFM = limot.m_motorCFM;
m_enableMotor = limot.m_enableMotor;
m_targetVelocity = limot.m_targetVelocity;
m_maxMotorForce = limot.m_maxMotorForce;
m_servoMotor = limot.m_servoMotor;
m_servoTarget = limot.m_servoTarget;
m_enableSpring = limot.m_enableSpring;
m_springStiffness = limot.m_springStiffness;
m_springStiffnessLimited = limot.m_springStiffnessLimited;
m_springDamping = limot.m_springDamping;
m_springDampingLimited = limot.m_springDampingLimited;
m_equilibriumPoint = limot.m_equilibriumPoint;
m_currentLimitError = limot.m_currentLimitError;
m_currentLimitErrorHi = limot.m_currentLimitErrorHi;
m_currentPosition = limot.m_currentPosition;
m_currentLimit = limot.m_currentLimit;
}
void calculateJacobi( btRotationalLimitMotor2 limot, ref btTransform transA, ref btTransform transB, btConstraintInfo2 info, int row, ref btVector3 ax1, bool rotational, bool rotAllowed )
{
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 );
}
if( !rotational )
{
btVector3 tmpA, tmpB, relA, relB;
// get vector from bodyB to frameB in WCS
m_calculatedTransformB.m_origin.Sub( ref transB.m_origin, out relB );
// same for bodyA
m_calculatedTransformA.m_origin.Sub( ref transA.m_origin, out relA ) ;
relA.cross( ref ax1, out tmpA );
relB.cross( ref ax1, out tmpB );
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 );
}
}
int get_limit_motor_info2(
btRotationalLimitMotor2 limot,
ref btTransform transA, ref btTransform transB, ref btVector3 linVelA, ref btVector3 linVelB, ref btVector3 angVelA, ref btVector3 angVelB,
btConstraintInfo2 info, int row, ref btVector3 ax1, bool rotational, bool rotAllowed = false )
{
int count = 0;
//int srow = row * info.rowskip;
if( limot.m_currentLimit == 4 )
{
double vel = rotational ? angVelA.dot( ax1 ) - angVelB.dot( ax1 ) : linVelA.dot( ax1 ) - linVelB.dot( ax1 );
calculateJacobi( limot, ref transA, ref transB, info, row, ref ax1, rotational, rotAllowed );
info.m_solverConstraints[row].m_rhs = info.fps * limot.m_stopERP * limot.m_currentLimitError * ( rotational ? -1 : 1 );
if( rotational )
{
if( info.m_solverConstraints[row].m_rhs - vel * limot.m_stopERP > 0 )
{
double bounceerror = -limot.m_bounce * vel;
if( bounceerror > info.m_solverConstraints[row].m_rhs ) info.m_solverConstraints[row].m_rhs = bounceerror;
}
}
else
{
if( info.m_solverConstraints[row].m_rhs - vel * limot.m_stopERP < 0 )
{
double bounceerror = -limot.m_bounce * vel;
if( bounceerror < info.m_solverConstraints[row].m_rhs ) info.m_solverConstraints[row].m_rhs = bounceerror;
}
}
info.m_solverConstraints[row].m_lowerLimit = rotational ? 0 : btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[row].m_upperLimit = rotational ? btScalar.BT_MAX_FLOAT : 0;
info.m_solverConstraints[row].m_cfm = limot.m_stopCFM;
row ++;
++count;
calculateJacobi( limot, ref transA, ref transB, info, row, ref ax1, rotational, rotAllowed );
info.m_solverConstraints[row].m_rhs = info.fps * limot.m_stopERP * limot.m_currentLimitErrorHi * ( rotational ? -1 : 1 );
if( rotational )
{
if( info.m_solverConstraints[row].m_rhs - vel * limot.m_stopERP < 0 )
{
double bounceerror = -limot.m_bounce * vel;
if( bounceerror < info.m_solverConstraints[row].m_rhs ) info.m_solverConstraints[row].m_rhs = bounceerror;
}
}
else
{
if( info.m_solverConstraints[row].m_rhs - vel * limot.m_stopERP > 0 )
{
double bounceerror = -limot.m_bounce * vel;
if( bounceerror > info.m_solverConstraints[row].m_rhs ) info.m_solverConstraints[row].m_rhs = bounceerror;
}
}
info.m_solverConstraints[row].m_lowerLimit = rotational ? btScalar.BT_MIN_FLOAT : 0;
info.m_solverConstraints[row].m_upperLimit = rotational ? 0 : btScalar.BT_MAX_FLOAT;
info.m_solverConstraints[row].m_cfm = limot.m_stopCFM;
row ++;
++count;
}
else
if( limot.m_currentLimit == 3 )
{
calculateJacobi( limot, ref transA, ref transB, info, row, ref ax1, rotational, rotAllowed );
info.m_solverConstraints[row].m_rhs = info.fps * limot.m_stopERP * limot.m_currentLimitError * ( rotational ? -1 : 1 );
info.m_solverConstraints[row].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[row].m_upperLimit = btScalar.BT_MAX_FLOAT;
info.m_solverConstraints[row].m_cfm = limot.m_stopCFM;
row++;
++count;
}
if( limot.m_enableMotor && !limot.m_servoMotor )
{
calculateJacobi( limot, ref transA, ref transB, info, row, ref ax1, rotational, rotAllowed );
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_motorERP );
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;
info.m_solverConstraints[row].m_cfm = limot.m_motorCFM;
row++;
++count;
}
if( limot.m_enableMotor && limot.m_servoMotor )
{
double error = limot.m_currentPosition - limot.m_servoTarget;
calculateJacobi( limot, ref transA, ref transB, info, row, ref ax1, rotational, rotAllowed );
double targetvelocity = error < 0 ? -limot.m_targetVelocity : limot.m_targetVelocity;
double tag_vel = -targetvelocity;
double mot_fact;
if( error != 0 )
{
double lowLimit;
double hiLimit;
if( limot.m_loLimit > limot.m_hiLimit )
{
lowLimit = error > 0 ? limot.m_servoTarget : btScalar.BT_MIN_FLOAT;
hiLimit = error < 0 ? limot.m_servoTarget : btScalar.BT_MAX_FLOAT;
}
else
{
lowLimit = error > 0 && limot.m_servoTarget > limot.m_loLimit ? limot.m_servoTarget : limot.m_loLimit;
hiLimit = error < 0 && limot.m_servoTarget < limot.m_hiLimit ? limot.m_servoTarget : limot.m_hiLimit;
}
mot_fact = getMotorFactor( limot.m_currentPosition, lowLimit, hiLimit, tag_vel, info.fps * limot.m_motorERP );
}
else
{
mot_fact = 0;
}
info.m_solverConstraints[row].m_rhs = mot_fact * targetvelocity * ( rotational ? -1 : 1 );
info.m_solverConstraints[row].m_lowerLimit = -limot.m_maxMotorForce;
info.m_solverConstraints[row].m_upperLimit = limot.m_maxMotorForce;
info.m_solverConstraints[row].m_cfm = limot.m_motorCFM;
row++;
//srow += info.rowskip;
++count;
}
if( limot.m_enableSpring )
{
double error = limot.m_currentPosition - limot.m_equilibriumPoint;
calculateJacobi( limot, ref transA, ref transB, info, row, ref ax1, rotational, rotAllowed );
//double cfm = 1.0 / ((1.0/info.fps)*limot.m_springStiffness+ limot.m_springDamping);
//if(cfm > 0.99999)
// cfm = 0.99999;
//double erp = (1.0/info.fps)*limot.m_springStiffness / ((1.0/info.fps)*limot.m_springStiffness + limot.m_springDamping);
//info.m_constraintError[srow] = info.fps * erp * error * (rotational ? -1.0 : 1.0);
//info.m_lowerLimit[srow] = -SIMD_INFINITY;
//info.m_upperLimit[srow] = SIMD_INFINITY;
double dt = btScalar.BT_ONE / info.fps;
double kd = limot.m_springDamping;
double ks = limot.m_springStiffness;
double vel = rotational ? angVelA.dot( ax1 ) - angVelB.dot( ax1 ) : linVelA.dot( ax1 ) - linVelB.dot( ax1 );
// double erp = 0.1;
double cfm = btScalar.BT_ZERO;
double mA = btScalar.BT_ONE / m_rbA.getInvMass();
double mB = btScalar.BT_ONE / m_rbB.getInvMass();
double m = mA > mB ? mB : mA;
double angularfreq = btScalar.btSqrt( ks / m );
//limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency)
if( limot.m_springStiffnessLimited && 0.25 < angularfreq * dt )
{
ks = btScalar.BT_ONE / dt / dt / (double)( 16.0 ) * m;
}
//avoid damping that would blow up the spring
if( limot.m_springDampingLimited && kd * dt > m )
{
kd = m / dt;
}
double fs = ks * error * dt;
double fd = -kd * ( vel ) * ( rotational ? -1 : 1 ) * dt;
double f = ( fs + fd );
info.m_solverConstraints[row].m_rhs = ( vel + f * ( rotational ? -1 : 1 ) );
double minf = f < fd ? f : fd;
double maxf = f < fd ? fd : f;
if( !rotational )
{
info.m_solverConstraints[row].m_lowerLimit = minf > 0 ? 0 : minf;
info.m_solverConstraints[row].m_upperLimit = maxf < 0 ? 0 : maxf;
}
else
{
info.m_solverConstraints[row].m_lowerLimit = -maxf > 0 ? 0 : -maxf;
info.m_solverConstraints[row].m_upperLimit = -minf < 0 ? 0 : -minf;
}
info.m_solverConstraints[row].m_cfm = cfm;
row++;
++count;
}
return count;
}
int setLinearLimits( btConstraintInfo2 info, int row, ref btTransform transA, ref btTransform transB, ref btVector3 linVelA, ref btVector3 linVelB, ref btVector3 angVelA, ref btVector3 angVelB )
{
//solve linear limits
btRotationalLimitMotor2 limot = new btRotationalLimitMotor2();
for( int i = 0; i < 3; i++ )
{
if( m_linearLimits.m_currentLimit[i] != 0 || m_linearLimits.m_enableMotor[i] || m_linearLimits.m_enableSpring[i] )
{ // re-use rotational motor code
limot.m_bounce = m_linearLimits.m_bounce[i];
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_currentLimitErrorHi = m_linearLimits.m_currentLimitErrorHi[i];
limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
limot.m_servoMotor = m_linearLimits.m_servoMotor[i];
limot.m_servoTarget = m_linearLimits.m_servoTarget[i];
limot.m_enableSpring = m_linearLimits.m_enableSpring[i];
limot.m_springStiffness = m_linearLimits.m_springStiffness[i];
limot.m_springStiffnessLimited = m_linearLimits.m_springStiffnessLimited[i];
limot.m_springDamping = m_linearLimits.m_springDamping[i];
limot.m_springDampingLimited = m_linearLimits.m_springDampingLimited[i];
limot.m_equilibriumPoint = m_linearLimits.m_equilibriumPoint[i];
limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
btVector3 axis = m_calculatedTransformA.m_basis.getColumn( i );
//int flags = m_flags >> ( i * BT_6DOF_FLAGS_AXIS_SHIFT2 );
limot.m_stopCFM = ( m_flags & bt6DofFlagsIndexed.BT_6DOF_FLAGS_CFM_STOP2[i] ) != 0 ? m_linearLimits.m_stopCFM[i] : info.m_solverConstraints[0].m_cfm;
limot.m_stopERP = ( m_flags & bt6DofFlagsIndexed.BT_6DOF_FLAGS_ERP_STOP2[i] ) != 0 ? m_linearLimits.m_stopERP[i] : info.erp;
limot.m_motorCFM = ( m_flags & bt6DofFlagsIndexed.BT_6DOF_FLAGS_CFM_MOTO2[i] ) != 0 ? m_linearLimits.m_motorCFM[i] : info.m_solverConstraints[0].m_cfm;
limot.m_motorERP = ( m_flags & bt6DofFlagsIndexed.BT_6DOF_FLAGS_ERP_MOTO2[i] ) != 0 ? m_linearLimits.m_motorERP[i] : info.erp;
//rotAllowed is a bit of a magic from the original 6dof. The calculation of it here is something that imitates the original behavior as much as possible.
int indx1 = ( i + 1 ) % 3;
int indx2 = ( i + 2 ) % 3;
bool rotAllowed = true; // rotations around orthos to current axis (it is used only when one of the body is static)
bool indx1Violated = m_angularLimits[indx1].m_currentLimit == 1 ||
m_angularLimits[indx1].m_currentLimit == 2 ||
( m_angularLimits[indx1].m_currentLimit == 3 && ( m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ) ||
( m_angularLimits[indx1].m_currentLimit == 4 && ( m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) );
bool indx2Violated = m_angularLimits[indx2].m_currentLimit == 1 ||
m_angularLimits[indx2].m_currentLimit == 2 ||
( m_angularLimits[indx2].m_currentLimit == 3 && ( m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ) ||
( m_angularLimits[indx2].m_currentLimit == 4 && ( m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) );
if( indx1Violated && indx2Violated )
{
rotAllowed = false;
}
row += get_limit_motor_info2( limot, ref transA, ref transB
, ref linVelA, ref linVelB
, ref angVelA, ref angVelB
, info, row, ref axis, false, rotAllowed );
}
}
return row;
}