internal override void getInfo2( btConstraintInfo2 info )
{
getInfo2NonVirtual( info, ref m_rbA.m_worldTransform, ref m_rbB.m_worldTransform );
}
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(btConstraintInfo2 obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
virtual void getInfo2 (btConstraintInfo2* info);
void getInfo2NonVirtual( ref btConstraintInfo2 info, ref btTransform transA, ref btTransform transB, ref btMatrix3x3 invInertiaWorldA, ref btMatrix3x3 invInertiaWorldB )
{
calcAngleInfo2( ref transA, ref transB, ref invInertiaWorldA, ref invInertiaWorldB );
Debug.Assert( !m_useSolveConstraintObsolete );
// set jacobian
info.m_solverConstraints[0].m_contactNormal1 = btVector3.xAxis;
info.m_solverConstraints[1].m_contactNormal1 = btVector3.yAxis;
info.m_solverConstraints[2].m_contactNormal1 = btVector3.zAxis;
//info.m_J1linearAxis = 1;
//info.m_J1linearAxis[info.rowskip + 1] = 1;
//info.m_J1linearAxis[2 * info.rowskip + 2] = 1;
btVector3 a1; transA.m_basis.Apply( ref m_rbAFrame.m_origin, out a1 );
{
//btVector3* angular0 = (btVector3*)( info.m_J1angularAxis );
//btVector3* angular1 = (btVector3*)( info.m_J1angularAxis + info.rowskip );
//btVector3* angular2 = (btVector3*)( info.m_J1angularAxis + 2 * info.rowskip );
btVector3 a1neg; a1.Invert( out a1neg );
a1neg.getSkewSymmetricMatrix( out info.m_solverConstraints[0].m_contactNormal1
, out info.m_solverConstraints[1].m_contactNormal1
, out info.m_solverConstraints[2].m_contactNormal1 );
}
info.m_solverConstraints[0].m_contactNormal2 = -btVector3.xAxis;
info.m_solverConstraints[1].m_contactNormal2 = -btVector3.yAxis;
info.m_solverConstraints[2].m_contactNormal2 = -btVector3.zAxis;
//info.m_J2linearAxis[0] = -1;
//info.m_J2linearAxis[info.rowskip + 1] = -1;
//info.m_J2linearAxis[2 * info.rowskip + 2] = -1;
btVector3 a2; transB.m_basis.Apply( ref m_rbBFrame.m_origin, out a2 );
{
a2.getSkewSymmetricMatrix( out info.m_solverConstraints[0].m_contactNormal1
, out info.m_solverConstraints[1].m_contactNormal1
, out info.m_solverConstraints[2].m_contactNormal1 );
}
// set right hand side
double linERP = ( m_flags & btConeTwistFlags.BT_CONETWIST_FLAGS_LIN_ERP ) != 0 ? m_linERP : info.erp;
double k = info.fps * linERP;
int j;
for( j = 0; j < 3; j++ )
{
info.m_solverConstraints[j].m_rhs = k * ( a2[j] + transB.m_origin[j] - a1[j] - transA.m_origin[j] );
info.m_solverConstraints[j].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[j].m_upperLimit = btScalar.BT_MAX_FLOAT;
if( ( m_flags & btConeTwistFlags.BT_CONETWIST_FLAGS_LIN_CFM ) != 0 )
{
info.m_solverConstraints[j].m_cfm = m_linCFM;
}
}
int row = 3;
//int srow = row * info.rowskip;
btVector3 ax1;
// angular limits
if( m_solveSwingLimit )
{
//double* J1 = info.m_J1angularAxis;
//double* J2 = info.m_J2angularAxis;
if( ( m_swingSpan1 < m_fixThresh ) && ( m_swingSpan2 < m_fixThresh ) )
{
btTransform trA; transA.Apply( ref m_rbAFrame, out trA );
btVector3 p; trA.m_basis.getColumn( 1, out p );
btVector3 q; trA.m_basis.getColumn( 2, out q );
int row1 = row + 1;
//int srow1 = srow + info.rowskip;
info.m_solverConstraints[row].m_relpos1CrossNormal = p;
info.m_solverConstraints[row1].m_relpos1CrossNormal = q;
p.Invert( out info.m_solverConstraints[row].m_relpos2CrossNormal );
q.Invert( out info.m_solverConstraints[row1].m_relpos2CrossNormal );
double fact = info.fps * m_relaxationFactor;
info.m_solverConstraints[row].m_rhs = fact * m_swingAxis.dot( p );
info.m_solverConstraints[row1].m_rhs = fact * m_swingAxis.dot( q );
info.m_solverConstraints[row].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[row1].m_upperLimit = btScalar.BT_MAX_FLOAT;
info.m_solverConstraints[row].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[row1].m_upperLimit = btScalar.BT_MAX_FLOAT;
row = row1 + 1;
//srow = srow1 + info.rowskip;
}
else
{
ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor;
info.m_solverConstraints[row].m_relpos1CrossNormal = ax1;
ax1.Invert( out info.m_solverConstraints[row].m_relpos2CrossNormal );
k = info.fps * m_biasFactor;
info.m_solverConstraints[row].m_rhs = k * m_swingCorrection;
if( ( m_flags & btConeTwistFlags.BT_CONETWIST_FLAGS_ANG_CFM ) != 0 )
{
info.m_solverConstraints[row].m_cfm = m_angCFM;
}
// m_swingCorrection is always positive or 0
info.m_solverConstraints[row].m_lowerLimit = 0;
info.m_solverConstraints[row].m_upperLimit = ( m_bMotorEnabled && m_maxMotorImpulse >= 0.0f ) ? m_maxMotorImpulse : btScalar.BT_MAX_FLOAT;
//srow += info.rowskip;
row++;
}
}
if( m_solveTwistLimit )
{
ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor;
info.m_solverConstraints[row].m_relpos1CrossNormal = ax1;
ax1.Invert( out info.m_solverConstraints[row].m_relpos2CrossNormal );
k = info.fps * m_biasFactor;
info.m_solverConstraints[row].m_rhs = k * m_twistCorrection;
if( ( m_flags & btConeTwistFlags.BT_CONETWIST_FLAGS_ANG_CFM ) != 0 )
{
info.m_solverConstraints[row].m_cfm = m_angCFM;
}
if( m_twistSpan > 0.0f )
{
if( m_twistCorrection > 0.0f )
{
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;
}
}
else
{
info.m_solverConstraints[row].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[row].m_upperLimit = btScalar.BT_MAX_FLOAT;
}
//srow += info.rowskip;
row++;
}
}
internal override void getInfo2( btConstraintInfo2 info )
{
Debug.Assert( !m_useSolveConstraintObsolete );
if( m_useOffsetForConstraintFrame )
{ // for stability better to solve angular limits first
int row = setAngularLimits( ref info, 0, ref m_rbA.m_worldTransform, ref m_rbB.m_worldTransform
, ref m_rbA.m_linearVelocity, ref m_rbB.m_linearVelocity
, ref m_rbA.m_angularVelocity, ref m_rbB.m_angularVelocity );
setLinearLimits( ref info, row, ref m_rbA.m_worldTransform, ref m_rbB.m_worldTransform
, ref m_rbA.m_linearVelocity, ref m_rbB.m_linearVelocity
, ref m_rbA.m_angularVelocity, ref m_rbB.m_angularVelocity );
}
else
{ // leave old version for compatibility
int row = setLinearLimits( ref info, 0, ref m_rbA.m_worldTransform, ref m_rbB.m_worldTransform
, ref m_rbA.m_linearVelocity, ref m_rbB.m_linearVelocity
, ref m_rbA.m_angularVelocity, ref m_rbB.m_angularVelocity );
setAngularLimits( ref info, row, ref m_rbA.m_worldTransform, ref m_rbB.m_worldTransform
, ref m_rbA.m_linearVelocity, ref m_rbB.m_linearVelocity
, ref m_rbA.m_angularVelocity, ref m_rbB.m_angularVelocity );
}
}
int setAngularLimits( btConstraintInfo2 info, int row_offset, ref btTransform transA, ref btTransform transB, ref btVector3 linVelA, ref btVector3 linVelB, ref btVector3 angVelA, ref btVector3 angVelB )
{
int row = row_offset;
//order of rotational constraint rows
int[] cIdx = { 0, 1, 2 };
switch( m_rotateOrder )
{
case RotateOrder.RO_XYZ: cIdx[0] = 0; cIdx[1] = 1; cIdx[2] = 2; break;
case RotateOrder.RO_XZY: cIdx[0] = 0; cIdx[1] = 2; cIdx[2] = 1; break;
case RotateOrder.RO_YXZ: cIdx[0] = 1; cIdx[1] = 0; cIdx[2] = 2; break;
case RotateOrder.RO_YZX: cIdx[0] = 1; cIdx[1] = 2; cIdx[2] = 0; break;
case RotateOrder.RO_ZXY: cIdx[0] = 2; cIdx[1] = 0; cIdx[2] = 1; break;
case RotateOrder.RO_ZYX: cIdx[0] = 2; cIdx[1] = 1; cIdx[2] = 0; break;
default: Debug.Assert( false ); break;
}
for( int ii = 0; ii < 3; ii++ )
{
int i = cIdx[ii];
if( m_angularLimits[i].m_currentLimit != 0 || m_angularLimits[i].m_enableMotor || m_angularLimits[i].m_enableSpring )
{
btVector3 axis = getAxis( i );
//int flags = m_flags >> ( ( i + 3 ) * BT_6DOF_FLAGS_AXIS_SHIFT2 );
if( 0 == ( m_flags & bt6DofFlagsIndexed.BT_6DOF_FLAGS_ERP_STOP2[i] ) )
{
m_angularLimits[i].m_stopCFM = info.m_solverConstraints[0].m_cfm;
}
if( 0 == ( m_flags & bt6DofFlagsIndexed.BT_6DOF_FLAGS_ERP_STOP2[i] ) )
{
m_angularLimits[i].m_stopERP = info.erp;
}
if( 0 == ( m_flags & bt6DofFlagsIndexed.BT_6DOF_FLAGS_CFM_MOTO2[i] ) )
{
m_angularLimits[i].m_motorCFM = info.m_solverConstraints[0].m_cfm;
}
if( 0 == ( m_flags & bt6DofFlagsIndexed.BT_6DOF_FLAGS_ERP_MOTO2[i] ) )
{
m_angularLimits[i].m_motorERP = info.erp;
}
row += get_limit_motor_info2( m_angularLimits[i], ref transA, ref transB
, ref linVelA, ref linVelB, ref angVelA, ref angVelB
, info, row, ref axis, true );
}
}
return row;
}
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;
}
void getInfo2InternalUsingFrameOffset( ref btConstraintInfo2 info, ref btTransform transA, ref btTransform transB, ref btVector3 angVelA, ref btVector3 angVelB )
{
//Debug.Assert( !m_useSolveConstraintObsolete );
//int i;
// transforms in world space
btTransform trA; transA.Apply( ref m_rbAFrame, out trA );
btTransform trB; transB.Apply( ref m_rbBFrame, out trB );
// pivot point
// btVector3 pivotAInW = trA.getOrigin();
// btVector3 pivotBInW = trB.getOrigin();
#if true
// difference between frames in WCS
btVector3 ofs; trB.m_origin.Sub( ref trA.m_origin, out ofs );// getOrigin() - trA.getOrigin();
// now get weight factors depending on masses
double miA = getRigidBodyA().getInvMass();
double miB = getRigidBodyB().getInvMass();
bool hasStaticBody = ( miA < btScalar.SIMD_EPSILON ) || ( miB < btScalar.SIMD_EPSILON );
double miS = miA + miB;
double factA, factB;
if( miS > (double)( 0 ) )
{
factA = miB / miS;
}
else
{
factA = (double)( 0.5f );
}
factB = (double)( 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.m_basis.getColumn( 2, out ax1A );
btVector3 ax1B; trB.m_basis.getColumn( 2, out ax1B );
btVector3 tmp;
ax1A.Mult( factA, out tmp );
btVector3 ax1; tmp.AddScale( ref ax1B, factB, out ax1 );
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 nrow = 2; // last filled row
btVector3 tmpA, tmpB, relA, relB, p, q;
// get vector from bodyB to frameB in WCS
trB.m_origin.Sub( ref bodyB_trans.m_origin, out relB );
// get its projection to hinge axis
btVector3 projB; ax1.Mult( relB.dot( ref ax1 ), out projB );
// get vector directed from bodyB to hinge axis (and orthogonal to it)
btVector3 orthoB; relB.Sub( ref projB, out orthoB );
// same for bodyA
trA.m_origin.Sub( ref bodyA_trans.m_origin, out relA );
btVector3 projA; ax1.Mult( relA.dot( ref ax1 ), out projA );
btVector3 orthoA; relA.Sub( ref projA, out orthoA );
btVector3 totalDist; projA.Sub( ref projB, out totalDist );
// get offset vectors relA and relB
orthoA.AddScale( ref totalDist, factA, out relA );
orthoB.AddScale( ref totalDist, -factB, out relB );
// now choose average ortho to hinge axis
orthoB.Mult( factA, out tmp );
tmp.AddScale( ref orthoA, factB, out p );
double len2 = p.length2();
if( len2 > btScalar.SIMD_EPSILON )
{
p.normalize();
}
else
{
trA.m_basis.getColumn( 1, out p );
}
// make one more ortho
ax1.cross( ref p, out q );
// fill three rows
relA.cross( ref p, out tmpA );
relB.cross( ref p, out tmpB );
info.m_solverConstraints[0].m_relpos1CrossNormal = tmpA;
tmpB.Invert( out info.m_solverConstraints[0].m_relpos2CrossNormal ); // = -tmpB;
relA.cross( ref q, out tmpA );
relB.cross( ref q, out tmpB );
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.Mult( factB, out tmpB );
tmpA.Mult( factA, out tmpA );
}
info.m_solverConstraints[1].m_relpos1CrossNormal = tmpA;
tmpB.Invert( out info.m_solverConstraints[1].m_relpos2CrossNormal );
relA.cross( ref ax1, out tmpA );
relB.cross( ref ax1, out tmpB );
if( hasStaticBody )
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation
tmpB.Mult( factB, out tmpB );
tmpA.Mult( factA, out tmpA );
}
info.m_solverConstraints[2].m_relpos1CrossNormal = tmpA;
tmpB.Invert( out info.m_solverConstraints[2].m_relpos2CrossNormal );
double normalErp = ( ( m_flags & btHingeFlags.BT_HINGE_FLAGS_ERP_NORM ) != 0 ) ? m_normalERP : info.erp;
double k = info.fps * normalErp;
if( !m_angularOnly )
{
info.m_solverConstraints[0].m_contactNormal1 = p;
info.m_solverConstraints[1].m_contactNormal1 = q;
info.m_solverConstraints[2].m_contactNormal1 = ax1;
p.Invert( out info.m_solverConstraints[0].m_contactNormal2 );
q.Invert( out info.m_solverConstraints[1].m_contactNormal2 );
ax1.Invert( out info.m_solverConstraints[2].m_contactNormal2 );
// compute three elements of right hand side
double rhs = k * p.dot( ofs );
info.m_solverConstraints[0].m_rhs = rhs;
rhs = k * q.dot( ofs );
info.m_solverConstraints[1].m_rhs = rhs;
rhs = k * ax1.dot( ofs );
info.m_solverConstraints[2].m_rhs = 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_solverConstraints[3].m_relpos1CrossNormal = p;
info.m_solverConstraints[4].m_relpos1CrossNormal = q;
p.Invert( out info.m_solverConstraints[3].m_relpos2CrossNormal );
q.Invert( out info.m_solverConstraints[4].m_relpos2CrossNormal );
// 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 * normalErp;//??
btVector3 u; ax1A.cross( ref ax1B, out u );
info.m_solverConstraints[3].m_rhs = k * u.dot( p );
info.m_solverConstraints[4].m_rhs = k * u.dot( q );
#endif
// check angular limits
//int nrow = 4; // last filled row
//int srow;
double limit_err = (double)( 0.0 );
int limit = 0;
if( getSolveLimit() )
{
#if _BT_USE_CENTER_LIMIT_
limit_err = m_limit.getCorrection() * m_referenceSign;
#else
limit_err = m_correction * m_referenceSign;
#endif
limit = ( limit_err > (double)( 0.0 ) ) ? 1 : 2;
}
// if the hinge has joint limits or motor, add in the extra row
bool powered = false;
if( getEnableAngularMotor() )
{
powered = true;
}
if( limit != 0 || powered )
{
info.m_solverConstraints[5].m_relpos1CrossNormal = ax1;
ax1.Invert( out info.m_solverConstraints[5].m_relpos2CrossNormal );
double lostop = getLowerLimit();
double histop = getUpperLimit();
if( limit != 0 && ( lostop == histop ) )
{ // the joint motor is ineffective
powered = false;
}
info.m_solverConstraints[5].m_rhs = (double)( 0.0f );
double currERP = ( ( m_flags & btHingeFlags.BT_HINGE_FLAGS_ERP_STOP ) != 0 ) ? m_stopERP : normalErp;
if( powered )
{
if( ( m_flags & btHingeFlags.BT_HINGE_FLAGS_CFM_NORM ) != 0 )
{
info.m_solverConstraints[5].m_cfm = m_normalCFM;
}
double mot_fact = getMotorFactor( m_hingeAngle, lostop, histop, m_motorTargetVelocity, info.fps * currERP );
info.m_solverConstraints[5].m_rhs += mot_fact * m_motorTargetVelocity * m_referenceSign;
info.m_solverConstraints[5].m_lowerLimit = -m_maxMotorImpulse;
info.m_solverConstraints[5].m_upperLimit = m_maxMotorImpulse;
}
if( limit != 0 )
{
k = info.fps * currERP;
info.m_solverConstraints[5].m_rhs += k * limit_err;
if( ( m_flags & btHingeFlags.BT_HINGE_FLAGS_CFM_STOP ) != 0 )
{
info.m_solverConstraints[5].m_cfm = m_stopCFM;
}
if( lostop == histop )
{
// limited low and high simultaneously
info.m_solverConstraints[5].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[5].m_upperLimit = btScalar.BT_MAX_FLOAT;
}
else if( limit == 1 )
{ // low limit
info.m_solverConstraints[5].m_lowerLimit = 0;
info.m_solverConstraints[5].m_upperLimit = btScalar.BT_MAX_FLOAT;
}
else
{ // high limit
info.m_solverConstraints[5].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[5].m_upperLimit = 0;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
#if _BT_USE_CENTER_LIMIT_
double bounce = m_limit.getRelaxationFactor();
#else
double bounce = m_relaxationFactor;
#endif
if( bounce > (double)( 0.0 ) )
{
double 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 )
{
double newc = -bounce * vel;
if( newc > info.m_solverConstraints[5].m_rhs )
{
info.m_solverConstraints[5].m_rhs = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if( vel > 0 )
{
double newc = -bounce * vel;
if( newc < info.m_solverConstraints[5].m_rhs )
{
info.m_solverConstraints[5].m_rhs = newc;
}
}
}
}
#if _BT_USE_CENTER_LIMIT_
info.m_solverConstraints[5].m_rhs *= m_limit.getBiasFactor();
#else
info.m_solverConstraints[5].m_rhs *= m_biasFactor;
#endif
} // if(limit)
} // if angular limit or powered
}
void getInfo2NonVirtual( ref btConstraintInfo2 info
, ref btTransform transA, ref btTransform transB
, ref btVector3 linVelA, ref btVector3 linVelB
, ref btVector3 angVelA, ref btVector3 angVelB )
{
Debug.Assert( !m_useSolveConstraintObsolete );
//prepare constraint
calculateTransforms( ref transA, ref transB );
int i;
for( i = 0; i < 3; i++ )
{
testAngularLimitMotor( i );
}
if( m_useOffsetForConstraintFrame )
{ // for stability better to solve angular limits first
int row = setAngularLimits( ref info, 0
, ref transA, ref transB
, ref linVelA, ref linVelB
, ref angVelA, ref angVelB );
setLinearLimits( ref info, row
, ref transA, ref transB
, ref linVelA, ref linVelB
, ref angVelA, ref angVelB );
}
else
{ // leave old version for compatibility
int row = setLinearLimits( ref info, 0, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB );
setAngularLimits( ref info, row, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB );
}
}
internal void getInfo2NonVirtual( btConstraintInfo2 info, ref btTransform transA, ref btTransform transB, ref btVector3 linVelA, ref btVector3 linVelB, double rbAinvMass, double rbBinvMass )
{
//btITransform m_calculatedTransformB = getCalculatedTransformB();
//Debug.Assert( !m_useSolveConstraintObsolete );
//int i;//, s = info.rowskip;
double signFact = m_useLinearReferenceFrameA ? (double)( 1.0f ) : (double)( -1.0f );
// difference between frames in WCS
btVector3 ofs; m_calculatedTransformB.m_origin.Sub( ref m_calculatedTransformA.m_origin, out ofs );
// now get weight factors depending on masses
double miA = rbAinvMass;
double miB = rbBinvMass;
bool hasStaticBody = ( miA < btScalar.SIMD_EPSILON ) || ( miB < btScalar.SIMD_EPSILON );
double miS = miA + miB;
double factA, factB;
if( miS > (double)( 0 ) )
{
factA = miB / miS;
}
else
{
factA = (double)( 0.5f );
}
factB = (double)( 1.0f ) - factA;
btVector3 ax1, p, q;
btVector3 ax1A = m_calculatedTransformA.m_basis.getColumn( 0 );
btVector3 ax1B = m_calculatedTransformB.m_basis.getColumn( 0 );
if( m_useOffsetForConstraintFrame )
{
// get the desired direction of slider axis
// as weighted sum of X-orthos of frameA and frameB in WCS
ax1 = ax1A * factA + ax1B * factB;
ax1.normalize();
// construct two orthos to slider axis
btVector3.btPlaneSpace1( ref ax1, out p, out q );
}
else
{ // old way - use frameA
ax1 = m_calculatedTransformA.m_basis.getColumn( 0 );
// get 2 orthos to slider axis (Y, Z)
p = m_calculatedTransformA.m_basis.getColumn( 1 );
q = m_calculatedTransformA.m_basis.getColumn( 2 );
}
// make rotations around these orthos equal
// the slider axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the slider 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 slider axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
info.m_solverConstraints[0].m_relpos1CrossNormal = p;// m_J1angularAxis[0] = p[0];
info.m_solverConstraints[1].m_relpos1CrossNormal = q;// m_J1angularAxis[0] = p[0];
p.Invert( out info.m_solverConstraints[0].m_relpos2CrossNormal );
q.Invert( out info.m_solverConstraints[1].m_relpos2CrossNormal );
// compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the slider back into alignment.
// if ax1A,ax1B are the unit length slider 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.
// double k = info.fps * info.erp * getSoftnessOrthoAng();
double currERP = ( ( m_flags & btSliderFlags.BT_SLIDER_FLAGS_ERP_ORTANG ) != 0 ) ? m_softnessOrthoAng : m_softnessOrthoAng * info.erp;
double k = info.fps * currERP;
btVector3 u = ax1A.cross( ax1B );
info.m_solverConstraints[0].m_rhs = k * u.dot( p );
info.m_solverConstraints[1].m_rhs = k * u.dot( q );
if( ( m_flags & btSliderFlags.BT_SLIDER_FLAGS_CFM_ORTANG ) != 0 )
{
info.m_solverConstraints[0].m_cfm = m_cfmOrthoAng;
info.m_solverConstraints[1].m_cfm = m_cfmOrthoAng;
}
int nrow = 1; // last filled row
//int srow;
double limit_err;
int limit;
bool powered;
// next two rows.
// we want: velA + wA x relA == velB + wB x relB ... but this would
// result in three equations, so we project along two orthos to the slider axis
btTransform bodyA_trans = transA;
btTransform bodyB_trans = transB;
nrow++;
//int s2 = nrow * s;
nrow++;
//int s3 = nrow * s;
btVector3 tmpA = btVector3.Zero, tmpB = btVector3.Zero
, relA = btVector3.Zero, relB = btVector3.Zero, c = btVector3.Zero;
if( m_useOffsetForConstraintFrame )
{
// get vector from bodyB to frameB in WCS
relB = m_calculatedTransformB.m_origin - bodyB_trans.m_origin;
// get its projection to slider axis
btVector3 projB = ax1 * relB.dot( ax1 );
// get vector directed from bodyB to slider axis (and orthogonal to it)
btVector3 orthoB = relB - projB;
// same for bodyA
relA = m_calculatedTransformA.m_origin - bodyA_trans.m_origin;
btVector3 projA = ax1 * relA.dot( ax1 );
btVector3 orthoA = relA - projA;
// get desired offset between frames A and B along slider axis
double sliderOffs = m_linPos - m_depth[0];
// desired vector from projection of center of bodyA to projection of center of bodyB to slider axis
btVector3 totalDist = projA + ax1 * sliderOffs - projB;
// get offset vectors relA and relB
relA = orthoA + totalDist * factA;
relB = orthoB - totalDist * factB;
// now choose average ortho to slider axis
p = orthoB * factA + orthoA * factB;
double len2 = p.length2();
if( len2 > btScalar.SIMD_EPSILON )
{
p /= btScalar.btSqrt( len2 );
}
else
{
p = m_calculatedTransformA.m_basis.getColumn( 1 );
}
// make one more ortho
q = ax1.cross( p );
// fill two rows
tmpA = relA.cross( p );
tmpB = relB.cross( p );
info.m_solverConstraints[2].m_relpos1CrossNormal = tmpA;
tmpB.Invert( out info.m_solverConstraints[2].m_relpos2CrossNormal );
//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];
tmpA = relA.cross( q );
tmpB = relB.cross( q );
if( hasStaticBody && getSolveAngLimit() )
{ // 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;
}
info.m_solverConstraints[3].m_relpos1CrossNormal = tmpA;
tmpB.Invert( out info.m_solverConstraints[3].m_relpos2CrossNormal );
//for( i = 0; i < 3; i++ ) info.m_J1angularAxis[s3 + i] = tmpA[i];
//for( i = 0; i < 3; i++ ) info.m_J2angularAxis[s3 + i] = -tmpB[i];
info.m_solverConstraints[2].m_contactNormal1 = p;
info.m_solverConstraints[3].m_contactNormal1 = q;
p.Invert( out info.m_solverConstraints[2].m_contactNormal2 );
q.Invert( out info.m_solverConstraints[3].m_contactNormal2 );
//for( i = 0; i < 3; i++ ) info.m_J1linearAxis[s2 + i] = p[i];
//for( i = 0; i < 3; i++ ) info.m_J1linearAxis[s3 + i] = q[i];
//for( i = 0; i < 3; i++ ) info.m_J2linearAxis[s2 + i] = -p[i];
//for( i = 0; i < 3; i++ ) info.m_J2linearAxis[s3 + i] = -q[i];
}
else
{ // old way - maybe incorrect if bodies are not on the slider axis
// see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0
c = bodyB_trans.m_origin - bodyA_trans.m_origin;
btVector3 tmp = c.cross( p );
tmp.Mult( factA, out info.m_solverConstraints[2].m_relpos1CrossNormal );
//for( i = 0; i < 3; i++ ) info.m_J1angularAxis[s2 + i] = factA * tmp[i];
tmp.Mult( factB, out info.m_solverConstraints[2].m_relpos2CrossNormal );
//for( i = 0; i < 3; i++ ) info.m_J2angularAxis[s2 + i] = factB * tmp[i];
tmp = c.cross( q );
tmp.Mult( factA, out info.m_solverConstraints[3].m_relpos1CrossNormal );
//for( i = 0; i < 3; i++ ) info.m_J1angularAxis[s3 + i] = factA * tmp[i];
tmp.Mult( factB, out info.m_solverConstraints[3].m_relpos2CrossNormal );
//for( i = 0; i < 3; i++ ) info.m_J2angularAxis[s3 + i] = factB * tmp[i];
info.m_solverConstraints[2].m_contactNormal1 = p;
info.m_solverConstraints[3].m_contactNormal1 = q;
p.Invert( out info.m_solverConstraints[2].m_contactNormal2 );
q.Invert( out info.m_solverConstraints[3].m_contactNormal2 );
//for( i = 0; i < 3; i++ ) info.m_J1linearAxis[s2 + i] = p[i];
//for( i = 0; i < 3; i++ ) info.m_J1linearAxis[s3 + i] = q[i];
//for( i = 0; i < 3; i++ ) info.m_J2linearAxis[s2 + i] = -p[i];
//for( i = 0; i < 3; i++ ) info.m_J2linearAxis[s3 + i] = -q[i];
}
// compute two elements of right hand side
// k = info.fps * info.erp * getSoftnessOrthoLin();
currERP = ( ( m_flags & btSliderFlags.BT_SLIDER_FLAGS_ERP_ORTLIN ) != 0 ) ? m_softnessOrthoLin : m_softnessOrthoLin * info.erp;
k = info.fps * currERP;
double rhs = k * p.dot( ofs );
info.m_solverConstraints[2].m_rhs = rhs;
//info.m_constraintError[s2] = rhs;
rhs = k * q.dot( ofs );
info.m_solverConstraints[3].m_rhs = rhs;
//info.m_constraintError[s3] = rhs;
if( ( m_flags & btSliderFlags.BT_SLIDER_FLAGS_CFM_ORTLIN ) != 0 )
{
info.m_solverConstraints[2].m_cfm = m_cfmOrthoLin;
info.m_solverConstraints[3].m_cfm = m_cfmOrthoLin;
//info.cfm[s2] = m_cfmOrthoLin;
//info.cfm[s3] = m_cfmOrthoLin;
}
// check linear limits
limit_err = (double)( 0.0 );
limit = 0;
if( getSolveLinLimit() )
{
limit_err = getLinDepth() * signFact;
limit = ( limit_err > (double)( 0.0 ) ) ? 2 : 1;
}
powered = false;
if( getPoweredLinMotor() )
{
powered = true;
}
// if the slider has joint limits or motor, add in the extra row
if( limit != 0 || powered )
{
nrow++;
//srow = nrow * info.rowskip;
info.m_solverConstraints[4].m_contactNormal1 = ax1;
//info.m_J1linearAxis[srow + 0] = ax1[0];
//info.m_J1linearAxis[srow + 1] = ax1[1];
//info.m_J1linearAxis[srow + 2] = ax1[2];
ax1.Invert( out info.m_solverConstraints[4].m_contactNormal2 );
//info.m_J2linearAxis[srow + 0] = -ax1[0];
//info.m_J2linearAxis[srow + 1] = -ax1[1];
//info.m_J2linearAxis[srow + 2] = -ax1[2];
// linear torque decoupling step:
//
// we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
// do not create a torque couple. in other words, the points that the
// constraint force is applied at must lie along the same ax1 axis.
// a torque couple will result in limited slider-jointed free
// bodies from gaining angular momentum.
if( m_useOffsetForConstraintFrame )
{
// this is needed only when bodyA and bodyB are both dynamic.
if( !hasStaticBody )
{
tmpA = relA.cross( ax1 );
tmpB = relB.cross( ax1 );
info.m_solverConstraints[4].m_relpos1CrossNormal = tmpA;
//info.m_J1angularAxis[srow + 0] = tmpA[0];
//info.m_J1angularAxis[srow + 1] = tmpA[1];
//info.m_J1angularAxis[srow + 2] = tmpA[2];
tmpB.Invert( out info.m_solverConstraints[4].m_relpos2CrossNormal );
//info.m_J2angularAxis[srow + 0] = -tmpB[0];
//info.m_J2angularAxis[srow + 1] = -tmpB[1];
//info.m_J2angularAxis[srow + 2] = -tmpB[2];
}
}
else
{ // The old way. May be incorrect if bodies are not on the slider axis
btVector3 ltd; // Linear Torque Decoupling vector (a torque)
ltd = c.cross( ax1 );
ltd.Mult( factA, out info.m_solverConstraints[4].m_relpos1CrossNormal );
//info.m_J1angularAxis[srow + 0] = factA * ltd[0];
//info.m_J1angularAxis[srow + 1] = factA * ltd[1];
//info.m_J1angularAxis[srow + 2] = factA * ltd[2];
ltd.Mult( factB, out info.m_solverConstraints[4].m_relpos2CrossNormal );
//info.m_J2angularAxis[srow + 0] = factB * ltd[0];
//info.m_J2angularAxis[srow + 1] = factB * ltd[1];
//info.m_J2angularAxis[srow + 2] = factB * ltd[2];
}
// right-hand part
double lostop = getLowerLinLimit();
double histop = getUpperLinLimit();
if( ( limit != 0 ) && ( lostop == histop ) )
{ // the joint motor is ineffective
powered = false;
}
info.m_solverConstraints[4].m_rhs = 0;
info.m_solverConstraints[4].m_lowerLimit = 0;
info.m_solverConstraints[4].m_upperLimit = 0;
currERP = ( ( m_flags & btSliderFlags.BT_SLIDER_FLAGS_ERP_LIMLIN ) != 0 ) ? m_softnessLimLin : info.erp;
if( powered )
{
if( ( m_flags & btSliderFlags.BT_SLIDER_FLAGS_CFM_DIRLIN ) != 0 )
{
info.m_solverConstraints[4].m_cfm = m_cfmDirLin;
}
double tag_vel = getTargetLinMotorVelocity();
double mot_fact = getMotorFactor( m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info.fps * currERP );
info.m_solverConstraints[4].m_rhs -= signFact * mot_fact * getTargetLinMotorVelocity();
info.m_solverConstraints[4].m_lowerLimit += -getMaxLinMotorForce() / info.fps;
info.m_solverConstraints[4].m_upperLimit += getMaxLinMotorForce() / info.fps;
}
if( limit != 0 )
{
k = info.fps * currERP;
info.m_solverConstraints[4].m_rhs += k * limit_err;
if( ( m_flags & btSliderFlags.BT_SLIDER_FLAGS_CFM_LIMLIN ) != 0 )
{
info.m_solverConstraints[4].m_cfm = m_cfmLimLin;
}
if( lostop == histop )
{ // limited low and high simultaneously
info.m_solverConstraints[4].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[4].m_upperLimit = btScalar.BT_MAX_FLOAT;
}
else if( limit == 1 )
{ // low limit
info.m_solverConstraints[4].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[4].m_upperLimit = 0;
}
else
{ // high limit
info.m_solverConstraints[4].m_lowerLimit = 0;
info.m_solverConstraints[4].m_upperLimit = btScalar.BT_MAX_FLOAT;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
double bounce = btScalar.btFabs( (double)( 1.0 ) - getDampingLimLin() );
if( bounce > (double)( 0.0 ) )
{
double vel = linVelA.dot( ax1 );
vel -= linVelB.dot( ax1 );
vel *= signFact;
// 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 )
{
double newc = -bounce * vel;
if( newc > info.m_solverConstraints[nrow].m_rhs )
{
info.m_solverConstraints[nrow].m_rhs = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if( vel > 0 )
{
double newc = -bounce * vel;
if( newc < info.m_solverConstraints[nrow].m_rhs )
{
info.m_solverConstraints[nrow].m_rhs = newc;
}
}
}
}
info.m_solverConstraints[4].m_rhs *= getSoftnessLimLin();
} // if(limit)
} // if linear limit
// check angular limits
limit_err = (double)( 0.0 );
limit = 0;
if( getSolveAngLimit() )
{
limit_err = getAngDepth();
limit = ( limit_err > (double)( 0.0 ) ) ? 1 : 2;
}
// if the slider has joint limits, add in the extra row
powered = false;
if( getPoweredAngMotor() )
{
powered = true;
}
if( limit != 0 || powered )
{
nrow++;
//srow = nrow * info.rowskip;
info.m_solverConstraints[nrow].m_relpos1CrossNormal = ax1;
//info.m_J1angularAxis[srow + 0] = ax1[0];
//info.m_J1angularAxis[srow + 1] = ax1[1];
//info.m_J1angularAxis[srow + 2] = ax1[2];
ax1.Invert( out info.m_solverConstraints[nrow].m_relpos2CrossNormal );
//info.m_J2angularAxis[srow + 0] = -ax1[0];
//info.m_J2angularAxis[srow + 1] = -ax1[1];
//info.m_J2angularAxis[srow + 2] = -ax1[2];
double lostop = getLowerAngLimit();
double histop = getUpperAngLimit();
if( limit != 0 && ( lostop == histop ) )
{ // the joint motor is ineffective
powered = false;
}
currERP = ( m_flags & btSliderFlags.BT_SLIDER_FLAGS_ERP_LIMANG ) != 0 ? m_softnessLimAng : info.erp;
if( powered )
{
if( ( m_flags & btSliderFlags.BT_SLIDER_FLAGS_CFM_DIRANG ) != 0 )
{
info.m_solverConstraints[nrow].m_cfm = m_cfmDirAng;
}
double mot_fact = getMotorFactor( m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info.fps * currERP );
info.m_solverConstraints[nrow].m_rhs = mot_fact * getTargetAngMotorVelocity();
info.m_solverConstraints[nrow].m_lowerLimit = -getMaxAngMotorForce() / info.fps;
info.m_solverConstraints[nrow].m_upperLimit = getMaxAngMotorForce() / info.fps;
}
if( limit != 0 )
{
k = info.fps * currERP;
info.m_solverConstraints[nrow].m_rhs += k * limit_err;
if( ( m_flags & btSliderFlags.BT_SLIDER_FLAGS_CFM_LIMANG ) != 0 )
{
info.m_solverConstraints[nrow].m_cfm = m_cfmLimAng;
}
if( lostop == histop )
{
// limited low and high simultaneously
info.m_solverConstraints[nrow].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[nrow].m_upperLimit = btScalar.BT_MAX_FLOAT;
}
else if( limit == 1 )
{ // low limit
info.m_solverConstraints[nrow].m_lowerLimit = 0;
info.m_solverConstraints[nrow].m_upperLimit = btScalar.BT_MAX_FLOAT;
}
else
{ // high limit
info.m_solverConstraints[nrow].m_lowerLimit = btScalar.BT_MIN_FLOAT;
info.m_solverConstraints[nrow].m_upperLimit = 0;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
double bounce = btScalar.btFabs( (double)( 1.0 ) - getDampingLimAng() );
if( bounce > (double)( 0.0 ) )
{
double vel = m_rbA.getAngularVelocity().dot( ax1 );
vel -= m_rbB.getAngularVelocity().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 )
{
double newc = -bounce * vel;
if( newc > info.m_solverConstraints[nrow].m_rhs )
{
info.m_solverConstraints[nrow].m_rhs = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if( vel > 0 )
{
double newc = -bounce * vel;
if( newc < info.m_solverConstraints[nrow].m_rhs )
{
info.m_solverConstraints[nrow].m_rhs = newc;
}
}
}
}
info.m_solverConstraints[nrow].m_rhs *= getSoftnessLimAng();
} // if(limit)
} // if angular limit or powered
}
internal override void getInfo2( btConstraintInfo2 info )
{
//if( m_useOffsetForConstraintFrame )
{
getInfo2InternalUsingFrameOffset( ref info, ref m_rbA.m_worldTransform, ref m_rbB.m_worldTransform, ref m_rbA.m_angularVelocity, ref m_rbB.m_angularVelocity );
}
}
internal override void getInfo2( btConstraintInfo2 info )
{
getInfo2NonVirtual( info, ref m_rbA.m_worldTransform, ref m_rbB.m_worldTransform
, ref m_rbA.m_linearVelocity, ref m_rbB.m_linearVelocity
, m_rbA.getInvMass(), m_rbB.getInvMass() );
}
void getInfo2NonVirtual( btConstraintInfo2* info, ref btTransform transA, ref btTransform transB, ref btVector3 linVelA, ref btVector3 linVelB, double rbAinvMass, double rbBinvMass );
///internal method used by the constraint solver, don't use them directly internal abstract void getInfo2( btConstraintInfo2 info );
void getInfo2NonVirtual( btConstraintInfo2 info, ref btTransform body0_trans, ref btTransform body1_trans )
{
Debug.Assert( !m_useSolveConstraintObsolete );
//retrieve matrices
// anchor points in global coordinates with respect to body PORs.
// set jacobian
info.m_solverConstraints[0].m_contactNormal1 = btVector3.xAxis;
info.m_solverConstraints[1].m_contactNormal1 = btVector3.yAxis;
info.m_solverConstraints[2].m_contactNormal1 = btVector3.zAxis;
btVector3 a1; body0_trans.m_basis.Apply( ref m_pivotInA, out a1 ) ;
{
//btVector3* angular0 = (btVector3*)( info.m_J1angularAxis );
//btVector3* angular1 = (btVector3*)( info.m_J1angularAxis + info.rowskip );
//btVector3* angular2 = (btVector3*)( info.m_J1angularAxis + 2 * info.rowskip );
btVector3 a1neg;
a1.Invert( out a1neg );
a1neg.getSkewSymmetricMatrix( out info.m_solverConstraints[0].m_relpos1CrossNormal
, out info.m_solverConstraints[1].m_relpos1CrossNormal
, out info.m_solverConstraints[2].m_relpos1CrossNormal );
}
btVector3.xAxis.Invert( out info.m_solverConstraints[0].m_contactNormal2 );
btVector3.yAxis.Invert( out info.m_solverConstraints[1].m_contactNormal2 );
btVector3.zAxis.Invert( out info.m_solverConstraints[2].m_contactNormal2 );
//info.m_J2linearAxis[0] = -1;
//info.m_J2linearAxis[info.rowskip + 1] = -1;
//info.m_J2linearAxis[2 * info.rowskip + 2] = -1;
btVector3 a2; body1_trans.m_basis.Apply( ref m_pivotInB, out a2 );
//getBasis() * getPivotInB();
{
// btVector3 a2n = -a2;
//btVector3* angular0 = (btVector3*)( info.m_J2angularAxis );
//btVector3* angular1 = (btVector3*)( info.m_J2angularAxis + info.rowskip );
//btVector3* angular2 = (btVector3*)( info.m_J2angularAxis + 2 * info.rowskip );
a2.getSkewSymmetricMatrix( out info.m_solverConstraints[0].m_relpos2CrossNormal
, out info.m_solverConstraints[1].m_relpos2CrossNormal
, out info.m_solverConstraints[2].m_relpos2CrossNormal );
}
// set right hand side
double currERP = ( m_flags & btPoint2PointFlags.BT_P2P_FLAGS_ERP )!= 0 ? m_erp : info.erp;
double k = info.fps * currERP;
int j;
for( j = 0; j < 3; j++ )
{
info.m_solverConstraints[j].m_rhs = k * ( a2[j] + body1_trans.m_origin[j] - a1[j] - body0_trans.m_origin[j] );
//Console.WriteLine("info.m_constraintError[%d]=%f\n",j,info.m_constraintError[j]);
}
if( ( m_flags & btPoint2PointFlags.BT_P2P_FLAGS_CFM ) != 0 )
{
for( j = 0; j < 3; j++ )
{
info.m_solverConstraints[j].m_cfm = m_cfm;
}
}
double impulseClamp = m_setting.m_impulseClamp;//
for( j = 0; j < 3; j++ )
{
if( m_setting.m_impulseClamp > 0 )
{
info.m_solverConstraints[j]. m_lowerLimit = -impulseClamp;
info.m_solverConstraints[j].m_upperLimit = impulseClamp;
}
}
info.m_damping = m_setting.m_damping;
}
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;
}
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;
}
int setAngularLimits( ref btConstraintInfo2 info
, int row_offset, ref btTransform transA, ref btTransform transB
, ref btVector3 linVelA, ref btVector3 linVelB
, ref btVector3 angVelA, ref btVector3 angVelB )
{
btGeneric6DofConstraint d6constraint = this;
int row = row_offset;
//solve angular limits
for( int i = 0; i < 3; i++ )
{
if( d6constraint.getRotationalLimitMotor( i ).needApplyTorques() )
{
btVector3 axis = d6constraint.getAxis( i );
//int flags = m_flags >> ( ( i + 3 ) * BT_6DOF_FLAGS_AXIS_SHIFT );
if( ( m_flags & bt6DofFlagsIndexed.BT_6DOF_FLAGS_CFM_NORM[i] ) == 0 )
{
m_angularLimits[i].m_normalCFM = info.m_solverConstraints[0].m_cfm;
}
if( ( m_flags & bt6DofFlagsIndexed.BT_6DOF_FLAGS_CFM_STOP[i] ) == 0 )
{
m_angularLimits[i].m_stopCFM = info.m_solverConstraints[0].m_cfm;
}
if( ( m_flags & bt6DofFlagsIndexed.BT_6DOF_FLAGS_ERP_STOP[i] ) == 0 )
{
m_angularLimits[i].m_stopERP = info.erp;
}
row += get_limit_motor_info2( d6constraint.getRotationalLimitMotor( i ),
ref transA, ref transB
, ref linVelA, ref linVelB
, ref angVelA, ref angVelB
, ref info, row, ref axis, true );
}
}
return row;
}
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 );
}
}
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 override void getInfo2( btConstraintInfo2 info )
{
btTransform transA = m_rbA.m_worldTransform;
btTransform transB = m_rbB.m_worldTransform;
btVector3 linVelA = m_rbA.m_linearVelocity;
btVector3 linVelB = m_rbB.m_linearVelocity;
btVector3 angVelA = m_rbA.m_angularVelocity;
btVector3 angVelB = m_rbB.m_angularVelocity;
// for stability better to solve angular limits first
int row = setAngularLimits( info, 0, ref m_rbA.m_worldTransform, ref m_rbB.m_worldTransform
, ref m_rbA.m_linearVelocity, ref m_rbB.m_linearVelocity
, ref m_rbA.m_angularVelocity, ref m_rbB.m_angularVelocity );
setLinearLimits( info, row, ref m_rbA.m_worldTransform, ref m_rbB.m_worldTransform
, ref m_rbA.m_linearVelocity, ref m_rbB.m_linearVelocity
, ref m_rbA.m_angularVelocity, ref m_rbB.m_angularVelocity );
}
internal override void getInfo2( btConstraintInfo2 info )
{
getInfo2NonVirtual( ref info, ref m_rbA.m_worldTransform, ref m_rbB.m_worldTransform, ref m_rbA.m_invInertiaTensorWorld, ref m_rbB.m_invInertiaTensorWorld );
}