internal void debugDrawConstraint( btTypedConstraint constraint )
{
bool drawFrames = ( getDebugDrawer().getDebugMode() & btIDebugDraw.DebugDrawModes.DBG_DrawConstraints ) != 0;
bool drawLimits = ( getDebugDrawer().getDebugMode() & btIDebugDraw.DebugDrawModes.DBG_DrawConstraintLimits ) != 0;
double dbgDrawSize = constraint.getDbgDrawSize();
if( dbgDrawSize <= (double)( 0 ) )
{
return;
}
switch( constraint.getConstraintType() )
{
case btObjectTypes.POINT2POINT_CONSTRAINT_TYPE:
{
btPoint2PointConstraint p2pC = (btPoint2PointConstraint)constraint;
btTransform tr = btTransform.Identity;
btVector3 pivot;
p2pC.getPivotInA( out pivot );
btVector3 tmp;
p2pC.getRigidBodyA().m_worldTransform.Apply( ref pivot, out tmp );
tr.setOrigin( ref tmp );
getDebugDrawer().drawTransform( ref tr, dbgDrawSize );
// that ideally should draw the same frame
p2pC.getPivotInB( out pivot );
p2pC.getRigidBodyB().m_worldTransform.Apply( ref pivot, out tmp );
tr.setOrigin( ref tmp );
if( drawFrames ) getDebugDrawer().drawTransform( ref tr, dbgDrawSize );
}
break;
case btObjectTypes.HINGE_CONSTRAINT_TYPE:
{
btHingeConstraint pHinge = (btHingeConstraint)constraint;
btTransform tr; pHinge.getRigidBodyA().m_worldTransform.Apply( ref pHinge.m_rbAFrame, out tr );
if( drawFrames ) getDebugDrawer().drawTransform( ref tr, dbgDrawSize );
pHinge.getRigidBodyB().m_worldTransform.Apply( ref pHinge.m_rbBFrame, out tr );
if( drawFrames ) getDebugDrawer().drawTransform( ref tr, dbgDrawSize );
double minAng = pHinge.getLowerLimit();
double maxAng = pHinge.getUpperLimit();
if( minAng == maxAng )
{
break;
}
bool drawSect = true;
if( !pHinge.hasLimit() )
{
minAng = (double)( 0 );
maxAng = btScalar.SIMD_2_PI;
drawSect = false;
}
if( drawLimits )
{
btVector3 center = tr.m_origin;
btVector3 normal; tr.m_basis.getColumn( 2, out normal );
btVector3 axis; tr.m_basis.getColumn( 0, out axis );
getDebugDrawer().drawArc( ref tr.m_origin, ref normal, ref axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, ref btVector3.Zero, drawSect );
}
}
break;
case btObjectTypes.CONETWIST_CONSTRAINT_TYPE:
{
btConeTwistConstraint pCT = (btConeTwistConstraint)constraint;
btTransform tr; pCT.getRigidBodyA().m_worldTransform.Apply( ref pCT.m_rbAFrame, out tr );
if( drawFrames ) getDebugDrawer().drawTransform( ref tr, dbgDrawSize );
pCT.m_rbB.m_worldTransform.Apply( ref pCT.m_rbBFrame, out tr );
if( drawFrames ) getDebugDrawer().drawTransform( ref tr, dbgDrawSize );
if( drawLimits )
{
//double length = (double)(5);
double length = dbgDrawSize;
int nSegments = 8 * 4;
double fAngleInRadians = btScalar.SIMD_2_PI * (double)( nSegments - 1 ) / (double)( nSegments );
btVector3 tmp;
btVector3 pPrev; pCT.GetPointForAngle( fAngleInRadians, length, out tmp );
tr.Apply( ref tmp, out pPrev );
for( int i = 0; i < nSegments; i++ )
{
fAngleInRadians = btScalar.SIMD_2_PI * (double)i / (double)( nSegments );
btVector3 pCur; pCT.GetPointForAngle( fAngleInRadians, length, out pCur );
//btVector3 tmp;
tr.Apply( ref pCur, out tmp );
getDebugDrawer().drawLine( ref pPrev, ref tmp, ref btVector3.Zero );
if( i % ( nSegments / 8 ) == 0 )
getDebugDrawer().drawLine( ref tr.m_origin, ref pCur, ref btVector3.Zero );
pPrev = pCur;
}
double tws = pCT.getTwistSpan();
double twa = pCT.getTwistAngle();
bool useFrameB = ( pCT.getRigidBodyB().getInvMass() > (double)( 0 ) );
if( useFrameB )
{
pCT.getRigidBodyB().m_worldTransform.Apply( ref pCT.m_rbBFrame, out tr );
}
else
{
pCT.getRigidBodyA().m_worldTransform.Apply( ref pCT.m_rbBFrame, out tr );
}
btVector3 pivot = tr.m_origin;
btVector3 normal; tr.m_basis.getColumn( 0, out normal );
btVector3 axis1; tr.m_basis.getColumn( 1, out axis1 );
getDebugDrawer().drawArc( ref pivot, ref normal, ref axis1, dbgDrawSize, dbgDrawSize, -twa - tws, -twa + tws, ref btVector3.Zero, true );
}
}
break;
case btObjectTypes.D6_SPRING_CONSTRAINT_TYPE:
case btObjectTypes.D6_CONSTRAINT_TYPE:
{
btGeneric6DofConstraint p6DOF = (btGeneric6DofConstraint)constraint;
btTransform tr;// = p6DOF.getCalculatedTransformA();
if( drawFrames ) getDebugDrawer().drawTransform( ref p6DOF.m_calculatedTransformA, dbgDrawSize );
//tr = p6DOF.getCalculatedTransformB();
if( drawFrames ) getDebugDrawer().drawTransform( ref p6DOF.m_calculatedTransformB, dbgDrawSize );
if( drawLimits )
{
tr = p6DOF.m_calculatedTransformA;
btVector3 center = p6DOF.m_calculatedTransformB.m_origin;
btVector3 up = tr.m_basis.getColumn( 2 );
btVector3 axis = tr.m_basis.getColumn( 0 );
double minTh = p6DOF.getRotationalLimitMotor( 1 ).m_loLimit;
double maxTh = p6DOF.getRotationalLimitMotor( 1 ).m_hiLimit;
double minPs = p6DOF.getRotationalLimitMotor( 2 ).m_loLimit;
double maxPs = p6DOF.getRotationalLimitMotor( 2 ).m_hiLimit;
getDebugDrawer().drawSpherePatch( ref center, ref up, ref axis, dbgDrawSize * (double)( .9f ), minTh, maxTh, minPs, maxPs, ref btVector3.Zero );
axis = tr.m_basis.getColumn( 1 );
double ay = p6DOF.getAngle( 1 );
double az = p6DOF.getAngle( 2 );
double cy = btScalar.btCos( ay );
double sy = btScalar.btSin( ay );
double cz = btScalar.btCos( az );
double sz = btScalar.btSin( az );
btVector3 ref_point;
ref_point.x = cy * cz * axis[0] + cy * sz * axis[1] - sy * axis[2];
ref_point.y = -sz * axis[0] + cz * axis[1];
ref_point.z = cz * sy * axis[0] + sz * sy * axis[1] + cy * axis[2];
ref_point.w = 0;
tr = p6DOF.m_calculatedTransformB;
btVector3 normal; tr.m_basis.getColumn( 0, out normal );
normal.Invert( out normal );
double minFi = p6DOF.getRotationalLimitMotor( 0 ).m_loLimit;
double maxFi = p6DOF.getRotationalLimitMotor( 0 ).m_hiLimit;
if( minFi > maxFi )
{
getDebugDrawer().drawArc( ref center, ref normal, ref ref_point, dbgDrawSize, dbgDrawSize, -btScalar.SIMD_PI, btScalar.SIMD_PI, ref btVector3.Zero, false );
}
else if( minFi < maxFi )
{
getDebugDrawer().drawArc( ref center, ref normal, ref ref_point, dbgDrawSize, dbgDrawSize, minFi, maxFi, ref btVector3.Zero, true );
}
tr = p6DOF.m_calculatedTransformA;
btVector3 bbMin = p6DOF.getTranslationalLimitMotor().m_lowerLimit;
btVector3 bbMax = p6DOF.getTranslationalLimitMotor().m_upperLimit;
getDebugDrawer().drawBox( ref bbMin, ref bbMax, ref tr, ref btVector3.Zero );
}
}
break;
///note: the code for D6_SPRING_2_CONSTRAINT_TYPE is identical to D6_CONSTRAINT_TYPE, the D6_CONSTRAINT_TYPE+D6_SPRING_CONSTRAINT_TYPE will likely become obsolete/deprecated at some stage
case btObjectTypes.D6_SPRING_2_CONSTRAINT_TYPE:
{
{
btGeneric6DofSpring2Constraint p6DOF = (btGeneric6DofSpring2Constraint)constraint;
//btTransform tr;// = p6DOF.getCalculatedTransformA();
if( drawFrames ) getDebugDrawer().drawTransform( ref p6DOF.m_calculatedTransformA, dbgDrawSize );
//tr = p6DOF.getCalculatedTransformB();
if( drawFrames ) getDebugDrawer().drawTransform( ref p6DOF.m_calculatedTransformB, dbgDrawSize );
if( drawLimits )
{
//tr = p6DOF.getCalculatedTransformA();
btVector3 center = p6DOF.m_calculatedTransformB.m_origin;
btVector3 up = p6DOF.m_calculatedTransformA.m_basis.getColumn( 2 );
btVector3 axis = p6DOF.m_calculatedTransformA.m_basis.getColumn( 0 );
double minTh = p6DOF.getRotationalLimitMotor( 1 ).m_loLimit;
double maxTh = p6DOF.getRotationalLimitMotor( 1 ).m_hiLimit;
double minPs = p6DOF.getRotationalLimitMotor( 2 ).m_loLimit;
double maxPs = p6DOF.getRotationalLimitMotor( 2 ).m_hiLimit;
getDebugDrawer().drawSpherePatch( ref center, ref up, ref axis, dbgDrawSize * (double)( .9f ), minTh, maxTh, minPs, maxPs, ref btVector3.Zero );
axis = p6DOF.m_calculatedTransformA.m_basis.getColumn( 1 );
double ay = p6DOF.getAngle( 1 );
double az = p6DOF.getAngle( 2 );
double cy = btScalar.btCos( ay );
double sy = btScalar.btSin( ay );
double cz = btScalar.btCos( az );
double sz = btScalar.btSin( az );
btVector3 ref_point;
ref_point.x = cy * cz * axis[0] + cy * sz * axis[1] - sy * axis[2];
ref_point.y = -sz * axis[0] + cz * axis[1];
ref_point.z = cz * sy * axis[0] + sz * sy * axis[1] + cy * axis[2];
ref_point.w = 0;
//tr = p6DOF.getCalculatedTransformB();
btVector3 normal; p6DOF.m_calculatedTransformB.m_basis.getColumn( 0, out normal );
normal.Invert( out normal );
double minFi = p6DOF.getRotationalLimitMotor( 0 ).m_loLimit;
double maxFi = p6DOF.getRotationalLimitMotor( 0 ).m_hiLimit;
if( minFi > maxFi )
{
getDebugDrawer().drawArc( ref center, ref normal, ref ref_point, dbgDrawSize, dbgDrawSize, -btScalar.SIMD_PI, btScalar.SIMD_PI, ref btVector3.Zero, false );
}
else if( minFi < maxFi )
{
getDebugDrawer().drawArc( ref center, ref normal, ref ref_point, dbgDrawSize, dbgDrawSize, minFi, maxFi, ref btVector3.Zero, true );
}
//tr = p6DOF.getCalculatedTransformA();
btVector3 bbMin = p6DOF.getTranslationalLimitMotor().m_lowerLimit;
btVector3 bbMax = p6DOF.getTranslationalLimitMotor().m_upperLimit;
getDebugDrawer().drawBox( ref bbMin, ref bbMax, ref p6DOF.m_calculatedTransformA, ref btVector3.Zero );
}
}
break;
}
case btObjectTypes.SLIDER_CONSTRAINT_TYPE:
{
btSliderConstraint pSlider = (btSliderConstraint)constraint;
//btTransform tr = pSlider.m_calculatedTransformA;
if( drawFrames ) getDebugDrawer().drawTransform( ref pSlider.m_calculatedTransformA, dbgDrawSize );
//tr = pSlider.getCalculatedTransformB();
if( drawFrames ) getDebugDrawer().drawTransform( ref pSlider.m_calculatedTransformB, dbgDrawSize );
if( drawLimits )
{
btTransform tr = pSlider.getUseLinearReferenceFrameA()
? pSlider.m_calculatedTransformA
: pSlider.m_calculatedTransformB;
btVector3 tmp = new btVector3( pSlider.getLowerLinLimit(), 0, 0 );
btVector3 li_min; tr.Apply( ref tmp, out li_min );
tmp = new btVector3( pSlider.getUpperLinLimit(), 0, 0 );
btVector3 li_max; tr.Apply( ref tmp, out li_max );
getDebugDrawer().drawLine( ref li_min, ref li_max, ref btVector3.Zero );
btVector3 normal = tr.m_basis.getColumn( 0 );
btVector3 axis = tr.m_basis.getColumn( 1 );
double a_min = pSlider.getLowerAngLimit();
double a_max = pSlider.getUpperAngLimit();
btVector3 center = pSlider.m_calculatedTransformB.m_origin;
getDebugDrawer().drawArc( ref center, ref normal, ref axis, dbgDrawSize, dbgDrawSize, a_min, a_max, ref btVector3.Zero, true );
}
}
break;
default:
break;
}
return;
}
