public static void Run()
{
{
btTransform planeObjWorld = new btTransform( new btQuaternion( 0.2, 0.1, 0.3, 1 ), new btVector3( 3, 4, 5 ) );
btTransform convexWorldTransform = new btTransform( new btQuaternion( 0.2, 0.3, 0, 1 ), new btVector3( 3, 4, 5 ) );
btTransform convexInPlaneTrans;
btTransform tmp;
planeObjWorld.inverse( out tmp );
tmp.Apply( ref convexWorldTransform, out convexInPlaneTrans );
PrintTransform( ref convexInPlaneTrans );
planeObjWorld.inverseTimes( ref convexWorldTransform, out convexInPlaneTrans );
PrintTransform( ref convexInPlaneTrans );
//convexInPlaneTrans = planeObjWorld( convexWorldTransform;
//PrintTransform( &convexInPlaneTrans );
}
{
btTransform planeObjWorld = new btTransform( new btQuaternion( 0.2, 0.1, 0.3, 1 ), new btVector3( 5, 2, 1 ) );
btTransform convexWorldTransform = new btTransform( new btQuaternion( 0.2, 0.3, 0, 1 ), new btVector3( 3, 4, 5 ) );
btTransform convexInPlaneTrans;
btTransform tmp;
planeObjWorld.inverse( out tmp );
tmp.Apply( ref convexWorldTransform, out convexInPlaneTrans );
PrintTransform( ref convexInPlaneTrans );
planeObjWorld.inverseTimes( ref convexWorldTransform, out convexInPlaneTrans );
PrintTransform( ref convexInPlaneTrans );
//convexInPlaneTrans = planeObjWorld( convexWorldTransform;
//PrintTransform( &convexInPlaneTrans );
btQuaternion perturbeRot = new btQuaternion( 0.1, 0.5, 0.25, 0.8 );
perturbeRot.normalize();
btTransform planeObjWrapTrans = new btTransform( new btQuaternion( 0.2, 0.1, 0.3, 1 ), new btVector3( 4, 7, 2 ) );
planeObjWrapTrans.inverseTimes( ref convexWorldTransform, out convexInPlaneTrans );
//now perturbe the convex-world transform
btMatrix3x3 perturbeMat = new btMatrix3x3( ref perturbeRot );
btMatrix3x3 tmpPerturbe; convexWorldTransform.m_basis.Apply( ref perturbeMat, out tmpPerturbe );
convexWorldTransform.m_basis = tmpPerturbe;
btTransform planeInConvex;
convexWorldTransform.inverseTimes( ref planeObjWrapTrans, out planeInConvex );
PrintTransform( ref planeInConvex );
}
Console.Read();
}
bool matrixToEulerYZX( ref btMatrix3x3 mat, ref btVector3 xyz )
{
// rot = cy*cz sy*sx-cy*cx*sz cx*sy+cy*sz*sx
// sz cz*cx -cz*sx
// -cz*sy cy*sx+cx*sy*sz cy*cx-sy*sz*sx
double fi = btGetMatrixElem( ref mat, 3 );
if( fi < (double)( 1.0f ) )
{
if( fi > (double)( -1.0f ) )
{
xyz[0] = btScalar.btAtan2( -btGetMatrixElem( ref mat, 5 ), btGetMatrixElem( ref mat, 4 ) );
xyz[1] = btScalar.btAtan2( -btGetMatrixElem( ref mat, 6 ), btGetMatrixElem( ref mat, 0 ) );
xyz[2] = btScalar.btAsin( btGetMatrixElem( ref mat, 3 ) );
return true;
}
else
{
xyz[0] = (double)( 0.0 );
xyz[1] = -btScalar.btAtan2( btGetMatrixElem( ref mat, 7 ), btGetMatrixElem( ref mat, 8 ) );
xyz[2] = -btScalar.SIMD_HALF_PI;
return false;
}
}
else
{
xyz[0] = (double)( 0.0 );
xyz[1] = btScalar.btAtan2( btGetMatrixElem( ref mat, 7 ), btGetMatrixElem( ref mat, 8 ) );
xyz[2] = btScalar.SIMD_HALF_PI;
}
return false;
}
bool matrixToEulerYXZ( ref btMatrix3x3 mat, ref btVector3 xyz )
{
// rot = cy*cz+sy*sx*sz cz*sy*sx-cy*sz cx*sy
// cx*sz cx*cz -sx
// cy*sx*sz-cz*sy sy*sz+cy*cz*sx cy*cx
double fi = btGetMatrixElem( ref mat, 5 );
if( fi < (double)( 1.0f ) )
{
if( fi > (double)( -1.0f ) )
{
xyz[0] = btScalar.btAsin( -btGetMatrixElem( ref mat, 5 ) );
xyz[1] = btScalar.btAtan2( btGetMatrixElem( ref mat, 2 ), btGetMatrixElem( ref mat, 8 ) );
xyz[2] = btScalar.btAtan2( btGetMatrixElem( ref mat, 3 ), btGetMatrixElem( ref mat, 4 ) );
return true;
}
else
{
xyz[0] = btScalar.SIMD_HALF_PI;
xyz[1] = -btScalar.btAtan2( -btGetMatrixElem( ref mat, 1 ), btGetMatrixElem( ref mat, 0 ) );
xyz[2] = (double)( 0.0 );
return false;
}
}
else
{
xyz[0] = -btScalar.SIMD_HALF_PI;
xyz[1] = btScalar.btAtan2( -btGetMatrixElem( ref mat, 1 ), btGetMatrixElem( ref mat, 0 ) );
xyz[2] = 0.0;
}
return false;
}
bool matrixToEulerXZY( ref btMatrix3x3 mat, ref btVector3 xyz )
{
// rot = cy*cz -sz sy*cz
// cy*cx*sz+sx*sy cx*cz sy*cx*sz-cy*sx
// cy*sx*sz-cx*sy sx*cz sy*sx*sz+cx*cy
double fi = btGetMatrixElem( ref mat, 1 );
if( fi < (double)( 1.0f ) )
{
if( fi > (double)( -1.0f ) )
{
xyz[0] = btScalar.btAtan2( btGetMatrixElem( ref mat, 7 ), btGetMatrixElem( ref mat, 4 ) );
xyz[1] = btScalar.btAtan2( btGetMatrixElem( ref mat, 2 ), btGetMatrixElem( ref mat, 0 ) );
xyz[2] = btScalar.btAsin( -btGetMatrixElem( ref mat, 1 ) );
return true;
}
else
{
xyz[0] = -btScalar.btAtan2( -btGetMatrixElem( ref mat, 6 ), btGetMatrixElem( ref mat, 8 ) );
xyz[1] = (double)( 0.0 );
xyz[2] = btScalar.SIMD_HALF_PI;
return false;
}
}
else
{
xyz[0] = btScalar.btAtan2( -btGetMatrixElem( ref mat, 6 ), btGetMatrixElem( ref mat, 8 ) );
xyz[1] = 0.0;
xyz[2] = -btScalar.SIMD_HALF_PI;
}
return false;
}
// MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
static bool matrixToEulerXYZ( ref btMatrix3x3 mat, ref btVector3 xyz )
{
// rot = cy*cz -cy*sz sy
// cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
// -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
double fi = btGetMatrixElem( ref mat, 2 );
if( fi < (double)( 1.0f ) )
{
if( fi > (double)( -1.0f ) )
{
xyz[0] = btScalar.btAtan2( -btGetMatrixElem( ref mat, 5 ), btGetMatrixElem( ref mat, 8 ) );
xyz[1] = btScalar.btAsin( btGetMatrixElem( ref mat, 2 ) );
xyz[2] = btScalar.btAtan2( -btGetMatrixElem( ref mat, 1 ), btGetMatrixElem( ref mat, 0 ) );
return true;
}
else
{
// WARNING. Not unique. XA - ZA = -atan2(r10,r11)
xyz[0] = -btScalar.btAtan2( btGetMatrixElem( ref mat, 3 ), btGetMatrixElem( ref mat, 4 ) );
xyz[1] = -btScalar.SIMD_HALF_PI;
xyz[2] = (double)( 0.0 );
return false;
}
}
else
{
// WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
xyz[0] = btScalar.btAtan2( btGetMatrixElem( ref mat, 3 ), btGetMatrixElem( ref mat, 4 ) );
xyz[1] = btScalar.SIMD_HALF_PI;
xyz[2] = 0.0;
}
return false;
}
static double btGetMatrixElem( ref btMatrix3x3 mat, int index )
{
int i = index % 3;
int j = index / 3;
return btMatrix3x3.getValue( ref mat, i,j );
}
public static void btSetCrossMatrixMinus( btMatrix3x3 res, ref btVector3 a )
{
double a_0 = a.x, a_1 = a.y, a_2 = a.z;
res.setValue( 0, +a_2, -a_1,
-a_2, 0, +a_0,
+a_1, -a_0, 0 );
}
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++;
}
}
public void evalEulerEqn( ref btVector3 w1, ref btVector3 w0, ref btVector3 T, double dt,
ref btMatrix3x3 I, out btVector3 result )
{
btVector3 Iw0;
btVector3 dtT;
btVector3 Iw1;
btVector3 cross;
T.Mult( dt, out dtT );
I.Apply( ref w0, out Iw0 );
dtT.Add( ref Iw0, out Iw0 );
// Iw0 = ( T * dt + I * w0 )
I.Apply( ref w1, out Iw1 );
w1.cross( ref Iw1, out cross );
cross.Mult( dt, out cross );
// cross = w1.cross( I * w1 ) * dt
Iw1.Add( ref cross, out Iw1 );
// Iw1 = Iw1 + cross
Iw1.Sub( ref Iw0, out result );
//btVector3 w2; = I * w1 + w1.cross( I * w1 ) * dt - ( T * dt + I * w0 );
//return w2;
}
//
// Convex-Convex collision algorithm
//
internal override void processCollision( btCollisionObjectWrapper body0Wrap
, ref btTransform body0Transform
, btCollisionObjectWrapper body1Wrap
, ref btTransform body1Transform
, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
{
if( m_manifoldPtr == null )
{
//swapped?
m_manifoldPtr = m_dispatcher.getNewManifold( body0Wrap.m_collisionObject, body1Wrap.m_collisionObject );
m_ownManifold = true;
}
resultOut.setPersistentManifold( m_manifoldPtr );
//comment-out next line to test multi-contact generation
//resultOut.getPersistentManifold().clearManifold();
btConvexShape min0 = (btConvexShape)body0Wrap.getCollisionShape();
btConvexShape min1 = (btConvexShape)body1Wrap.getCollisionShape();
btVector3 normalOnB;
btVector3 pointOnBWorld;
#if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
if( ( min0.getShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE )
&& ( min1.getShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE ) )
{
btCapsuleShape capsuleA = (btCapsuleShape)min0;
btCapsuleShape capsuleB = (btCapsuleShape)min1;
// btVector3 localScalingA = capsuleA.getLocalScaling();
// btVector3 localScalingB = capsuleB.getLocalScaling();
double threshold = m_manifoldPtr.getContactBreakingThreshold();
double dist = capsuleCapsuleDistance( out normalOnB, out pointOnBWorld
, capsuleA.getHalfHeight(), capsuleA.getRadius()
, capsuleB.getHalfHeight(), capsuleB.getRadius()
, capsuleA.getUpAxis(), capsuleB.getUpAxis()
, ref body0Wrap.m_collisionObject.m_worldTransform, ref body1Wrap.m_collisionObject.m_worldTransform, threshold );
if( dist < threshold )
{
Debug.Assert( normalOnB.length2() >= ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) );
resultOut.addContactPoint( ref normalOnB, ref pointOnBWorld, dist );
}
resultOut.refreshContactPoints();
return;
}
#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
m_sepDistance.updateSeparatingDistance(body0.getWorldTransform(),body1.getWorldTransform());
}
if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0)
#endif //USE_SEPDISTANCE_UTIL2
{
btGjkPairDetector.ClosestPointInput input = BulletGlobals.ClosestPointInputPool.Get();
input.Initialize();
btGjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get();
gjkPairDetector.Initialize( min0, min1, m_simplexSolver, m_pdSolver );
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.setMinkowskiA( min0 );
gjkPairDetector.setMinkowskiB( min1 );
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
} else
#endif //USE_SEPDISTANCE_UTIL2
{
//if (dispatchInfo.m_convexMaxDistanceUseCPT)
//{
// input.m_maximumDistanceSquared = min0.getMargin() + min1.getMargin() + m_manifoldPtr.getContactProcessingThreshold();
//} else
//{
input.m_maximumDistanceSquared = min0.getMargin() + min1.getMargin() + m_manifoldPtr.getContactBreakingThreshold();
// }
input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
}
input.m_transformA = body0Transform;
input.m_transformB = body1Transform;
#if USE_SEPDISTANCE_UTIL2
double sepDist = 0;
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
sepDist = gjkPairDetector.getCachedSeparatingDistance();
if (sepDist>SIMD_EPSILON)
{
sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
//now perturbe directions to get multiple contact points
}
}
#endif //USE_SEPDISTANCE_UTIL2
if( min0.isPolyhedral() && min1.isPolyhedral() )
{
btDummyResult dummy = new btDummyResult();
///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it
double min0Margin = min0.getShapeType() == BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE ? 0 : min0.getMargin();
double min1Margin = min1.getShapeType() == BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE ? 0 : min1.getMargin();
btWithoutMarginResult withoutMargin = new btWithoutMarginResult( resultOut, min0Margin, min1Margin );
btPolyhedralConvexShape polyhedronA = (btPolyhedralConvexShape)min0;
btPolyhedralConvexShape polyhedronB = (btPolyhedralConvexShape)min1;
if( polyhedronA.getConvexPolyhedron() != null && polyhedronB.getConvexPolyhedron() != null )
{
double threshold = m_manifoldPtr.getContactBreakingThreshold();
double minDist = -1e30f;
btVector3 sepNormalWorldSpace;
bool foundSepAxis = true;
if( dispatchInfo.m_enableSatConvex )
{
foundSepAxis = btPolyhedralContactClipping.findSeparatingAxis(
polyhedronA.getConvexPolyhedron(), polyhedronB.getConvexPolyhedron(),
ref body0Wrap.m_collisionObject.m_worldTransform,
ref body1Wrap.m_collisionObject.m_worldTransform,
out sepNormalWorldSpace, resultOut );
}
else
{
#if ZERO_MARGIN
gjkPairDetector.setIgnoreMargin(true);
gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
#else
gjkPairDetector.getClosestPoints( input, withoutMargin, dispatchInfo.m_debugDraw );
//gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
#endif //ZERO_MARGIN
//double l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
//if (l2>SIMD_EPSILON)
{
sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;//gjkPairDetector.getCachedSeparatingAxis()*(1/l2);
//minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
minDist = withoutMargin.m_reportedDistance;//gjkPairDetector.getCachedSeparatingDistance()+min0.getMargin()+min1.getMargin();
#if ZERO_MARGIN
foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0;
#else
foundSepAxis = withoutMargin.m_foundResult && minDist < 0;//-(min0.getMargin()+min1.getMargin());
#endif
}
}
if( foundSepAxis )
{
// Console.WriteLine("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z);
btPolyhedralContactClipping.clipHullAgainstHull( ref sepNormalWorldSpace, polyhedronA.getConvexPolyhedron(), polyhedronB.getConvexPolyhedron(),
ref body0Wrap.m_collisionObject.m_worldTransform,
ref body1Wrap.m_collisionObject.m_worldTransform
, minDist - threshold, threshold, resultOut );
}
if( m_ownManifold )
{
resultOut.refreshContactPoints();
}
BulletGlobals.ClosestPointInputPool.Free( input );
BulletGlobals.GjkPairDetectorPool.Free( gjkPairDetector );
return;
}
else
{
//we can also deal with convex versus triangle (without connectivity data)
if( polyhedronA.getConvexPolyhedron() != null && polyhedronB.getShapeType() == BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE )
{
btVertexArray vertices = new btVertexArray();
btTriangleShape tri = (btTriangleShape)polyhedronB;
btVector3 tmp;
body1Transform.Apply( ref tri.m_vertices1, out tmp );
vertices.Add( ref tmp );
body1Transform.Apply( ref tri.m_vertices2, out tmp );
vertices.Add( ref tmp );
body1Transform.Apply( ref tri.m_vertices3, out tmp );
vertices.Add( ref tmp );
//tri.initializePolyhedralFeatures();
double threshold = m_manifoldPtr.getContactBreakingThreshold();
btVector3 sepNormalWorldSpace;
double minDist = -btScalar.BT_LARGE_FLOAT;
double maxDist = threshold;
bool foundSepAxis = false;
if( false )
{
polyhedronB.initializePolyhedralFeatures();
foundSepAxis = btPolyhedralContactClipping.findSeparatingAxis(
polyhedronA.getConvexPolyhedron(), polyhedronB.getConvexPolyhedron(),
ref body0Wrap.m_collisionObject.m_worldTransform,
ref body1Wrap.m_collisionObject.m_worldTransform,
out sepNormalWorldSpace, resultOut );
// Console.WriteLine("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z);
btPolyhedralContactClipping.clipFaceAgainstHull( ref sepNormalWorldSpace
, polyhedronA.getConvexPolyhedron(),
ref body0Wrap.m_collisionObject.m_worldTransform, vertices, minDist - threshold, maxDist, resultOut );
}
else
{
#if ZERO_MARGIN
gjkPairDetector.setIgnoreMargin(true);
gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
#else
gjkPairDetector.getClosestPoints( input, dummy, dispatchInfo.m_debugDraw );
#endif//ZERO_MARGIN
double l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
if( l2 > btScalar.SIMD_EPSILON )
{
gjkPairDetector.getCachedSeparatingAxis().Mult( ( 1 / l2 ), out sepNormalWorldSpace );
//minDist = gjkPairDetector.getCachedSeparatingDistance();
//maxDist = threshold;
minDist = gjkPairDetector.getCachedSeparatingDistance() - min0.getMargin() - min1.getMargin();
//foundSepAxis = true;
btPolyhedralContactClipping.clipFaceAgainstHull( ref sepNormalWorldSpace
, polyhedronA.getConvexPolyhedron(),
ref body0Wrap.m_collisionObject.m_worldTransform, vertices, minDist - threshold, maxDist, resultOut );
}
else
{
//sepNormalWorldSpace = btVector3.Zero;
}
}
if( m_ownManifold )
{
resultOut.refreshContactPoints();
}
BulletGlobals.ClosestPointInputPool.Free( input );
BulletGlobals.GjkPairDetectorPool.Free( gjkPairDetector );
return;
}
}
}
gjkPairDetector.getClosestPoints( input, resultOut, dispatchInfo.m_debugDraw );
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
//perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
if( m_numPerturbationIterations != 0
&& resultOut.m_manifoldPtr.m_cachedPoints < m_minimumPointsPerturbationThreshold )
{
int i;
btVector3 v0, v1;
btVector3 sepNormalWorldSpace;
double l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
if( l2 > btScalar.SIMD_EPSILON )
{
gjkPairDetector.getCachedSeparatingAxis().Mult( ( 1 / l2 ), out sepNormalWorldSpace );
btVector3.btPlaneSpace1( ref sepNormalWorldSpace, out v0, out v1 );
bool perturbeA = true;
double angleLimit = 0.125f * btScalar.SIMD_PI;
double perturbeAngle;
double radiusA = min0.getAngularMotionDisc();
double radiusB = min1.getAngularMotionDisc();
if( radiusA < radiusB )
{
perturbeAngle = btPersistentManifold.gContactBreakingThreshold / radiusA;
perturbeA = true;
}
else
{
perturbeAngle = btPersistentManifold.gContactBreakingThreshold / radiusB;
perturbeA = false;
}
if( perturbeAngle > angleLimit )
perturbeAngle = angleLimit;
btTransform unPerturbedTransform;
if( perturbeA )
{
unPerturbedTransform = input.m_transformA;
}
else
{
unPerturbedTransform = input.m_transformB;
}
for( i = 0; i < m_numPerturbationIterations; i++ )
{
if( v0.length2() > btScalar.SIMD_EPSILON )
{
btQuaternion perturbeRot = new btQuaternion( ref v0, perturbeAngle );
double iterationAngle = i * ( btScalar.SIMD_2_PI / (double)( m_numPerturbationIterations ) );
btQuaternion rotq = new btQuaternion( ref sepNormalWorldSpace, iterationAngle );
if( perturbeA )
{
btQuaternion tmpq;
btQuaternion tmpq2;
rotq.inverse( out tmpq );
btQuaternion.Mult( ref tmpq, ref perturbeRot, out tmpq2 );
btQuaternion.Mult( ref tmpq2, ref rotq, out tmpq );
btMatrix3x3 m = new btMatrix3x3( ref tmpq );
btMatrix3x3 m2; body0Transform.getBasis( out m2 );
btMatrix3x3 m3;
btMatrix3x3.Mult( ref m, ref m2, out m3 );
input.m_transformA.setBasis( ref m3 );
input.m_transformB = body1Transform;
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.drawTransform(input.m_transformA,10.0);
#endif //DEBUG_CONTACTS
}
else
{
btQuaternion tmpq;
btQuaternion tmpq2;
rotq.inverse( out tmpq );
btQuaternion.Mult( ref tmpq, ref perturbeRot, out tmpq2 );
btQuaternion.Mult( ref tmpq2, ref rotq, out tmpq );
btMatrix3x3 m = new btMatrix3x3( ref tmpq );
btMatrix3x3 m2; body1Transform.getBasis( out m2 );
btMatrix3x3 m3;
btMatrix3x3.Mult( ref m, ref m2, out m3 );
input.m_transformA = body0Transform;
input.m_transformB.setBasis( ref m3 );
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.drawTransform(input.m_transformB,10.0);
#endif
}
btPerturbedContactResult perturbedResultOut = BulletGlobals.PerturbedContactResultPool.Get();
if( perturbeA )
perturbedResultOut.Initialize( resultOut
, ref input.m_transformA, ref input.m_transformB
, ref input.m_transformA
, perturbeA
, dispatchInfo.m_debugDraw );
else
perturbedResultOut.Initialize( resultOut
, ref input.m_transformA, ref input.m_transformB
, ref input.m_transformB
, perturbeA
, dispatchInfo.m_debugDraw );
gjkPairDetector.getClosestPoints( input, perturbedResultOut, dispatchInfo.m_debugDraw );
BulletGlobals.PerturbedContactResultPool.Free( perturbedResultOut );
}
}
}
}
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
{
m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0.getWorldTransform(),body1.getWorldTransform());
}
#endif //USE_SEPDISTANCE_UTIL2
BulletGlobals.ClosestPointInputPool.Free( input );
BulletGlobals.GjkPairDetectorPool.Free( gjkPairDetector );
}
if( m_ownManifold )
{
resultOut.refreshContactPoints();
}
}
///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia
///and the center of mass to the current coordinate system. "masses" points to an array of masses of the children. The resulting transform
///"principal" has to be applied inversely to all children transforms in order for the local coordinate system of the compound
///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform
///of the collision object by the principal transform.
void calculatePrincipalAxisTransform( double[] masses, ref btTransform principal, ref btVector3 inertia )
{
int n = m_children.Count;
double totalMass = 0;
btVector3 center = btVector3.Zero;
int k;
for( k = 0; k < n; k++ )
{
Debug.Assert( masses[k] > 0 );
center.AddScale( ref m_children[k].m_transform.m_origin, masses[k], out center );
totalMass += masses[k];
}
Debug.Assert( totalMass > 0 );
center /= totalMass;
principal.setOrigin( ref center );
btMatrix3x3 tensor = new btMatrix3x3( 0, 0, 0, 0, 0, 0, 0, 0, 0);
for( k = 0; k < n; k++ )
{
btVector3 i;
m_children[k].m_childShape.calculateLocalInertia( masses[k], out i );
btTransform t = m_children[k].m_transform;
btVector3 o; t.m_origin.Sub(ref center, out o );
//compute inertia tensor in coordinate system of compound shape
btMatrix3x3 j; t.m_basis.transpose( out j);
j.m_el0 *= i.x;
j.m_el1 *= i.y;
j.m_el2 *= i.z;
btMatrix3x3 j2;
t.m_basis.Mult( ref j, out j2 );
//add inertia tensor
tensor.m_el0 += j2.m_el0;
tensor.m_el1 += j2.m_el1;
tensor.m_el2 += j2.m_el2;
//compute inertia tensor of pointmass at o
double o2 = o.length2();
j[0].setValue( o2, 0, 0 );
j[1].setValue( 0, o2, 0 );
j[2].setValue( 0, 0, o2 );
j.m_el0 += o * -o.x;
j.m_el1 += o * -o.y;
j.m_el2 += o * -o.z;
//add inertia tensor of pointmass
tensor.m_el0.AddScale( ref j.m_el0, masses[k], out tensor.m_el0 );
tensor.m_el1.AddScale( ref j.m_el1, masses[k], out tensor.m_el1 );
tensor.m_el2.AddScale( ref j.m_el2, masses[k], out tensor.m_el2 );
}
tensor.diagonalize( out principal.m_basis, (double)( 0.00001 ), 20 );
inertia.setValue( tensor[0][0], tensor[1][1], tensor[2][2] );
}
void collideSingleContact( bool usePertube, ref btQuaternion perturbeRot
, btCollisionObjectWrapper convexObjWrap
, ref btTransform convexTransform
, btCollisionObjectWrapper planeObjWrap
, ref btTransform planeTransform
, btDispatcherInfo dispatchInfo, btManifoldResult resultOut
, ref btVector3 planeNormal, double planeConstant
)
{
//btCollisionObjectWrapper convexObjWrap = m_swapped ? body1Wrap : body0Wrap;
//btCollisionObjectWrapper planeObjWrap = m_swapped ? body0Wrap : body1Wrap;
btConvexShape convexShape = (btConvexShape)convexObjWrap.getCollisionShape();
btStaticPlaneShape planeShape = (btStaticPlaneShape)planeObjWrap.getCollisionShape();
bool hasCollision = false;
//planeNormal = planeShape.getPlaneNormal().Copy( out planeNormal );
//double planeConstant = planeShape.getPlaneConstant();
btTransform convexWorldTransform = convexTransform;
//btTransform planeWorldTransform = planeObjWrap.m_worldTransform;
btTransform convexInPlaneTrans;
planeTransform.inverseTimes( ref convexWorldTransform, out convexInPlaneTrans );
if( usePertube )
{
//now perturbe the convex-world transform
btMatrix3x3 perturbeMat = new btMatrix3x3( ref perturbeRot );
btMatrix3x3 tmpPerturbe; convexWorldTransform.m_basis.Apply( ref perturbeMat, out tmpPerturbe );
convexWorldTransform.m_basis = tmpPerturbe;
//convexWorldTransform.getBasis() *= btMatrix3x3( perturbeRot );
}
btTransform planeInConvex;
convexTransform.inverseTimes( ref planeObjWrap.m_collisionObject.m_worldTransform, out planeInConvex );
btVector3 tmp, tmp2;
planeNormal.Invert( out tmp );
planeInConvex.m_basis.Apply( ref tmp, out tmp2 );
btVector3 vtx; convexShape.localGetSupportingVertex( ref tmp2, out vtx );
btVector3 vtxInPlane; convexInPlaneTrans.Apply( ref vtx, out vtxInPlane );
double distance = ( planeNormal.dot( ref vtxInPlane ) - planeConstant );
btVector3 vtxInPlaneProjected; vtxInPlane.AddScale( ref planeNormal, -distance, out vtxInPlaneProjected );
btVector3 vtxInPlaneWorld; planeTransform.Apply( ref vtxInPlaneProjected, out vtxInPlaneWorld );
hasCollision = distance < m_manifoldPtr.getContactBreakingThreshold();
resultOut.setPersistentManifold( m_manifoldPtr );
if( hasCollision )
{
/// report a contact. internally this will be kept persistent, and contact reduction is done
btVector3 normalOnSurfaceB; planeTransform.m_basis.Apply( ref planeNormal, out normalOnSurfaceB );
btScalar.Dbg( "Convex plane adds point " + normalOnSurfaceB + " " + vtxInPlaneWorld + " " + distance.ToString( "g17" ) );
resultOut.addContactPoint( ref normalOnSurfaceB, ref vtxInPlaneWorld, distance );
}
}
public void dot3( ref btMatrix3x3 m, out btVector3 result )
{
result.x = dot( ref m.m_el0 );
result.y = dot( ref m.m_el1 );
result.z = dot( ref m.m_el2 );
result.w = 0;
}
/*
void solveConstraintObsolete( btSolverBody bodyA, btSolverBody bodyB, double timeStep )
{
if( m_useSolveConstraintObsolete )
{
btVector3 pivotAInW = m_rbA.m_worldTransform * m_rbAFrame.m_origin;
btVector3 pivotBInW = m_rbB.m_worldTransform * m_rbBFrame.m_origin;
double tau = (double)( 0.3 );
//linear part
if( !m_angularOnly )
{
btVector3 rel_pos1 = pivotAInW - m_rbA.m_worldTransform.m_origin;
btVector3 rel_pos2 = pivotBInW - m_rbB.m_worldTransform.m_origin;
btVector3 vel1;
bodyA.internalGetVelocityInLocalPointObsolete( rel_pos1, vel1 );
btVector3 vel2;
bodyB.internalGetVelocityInLocalPointObsolete( rel_pos2, vel2 );
btVector3 vel = vel1 - vel2;
for( int i = 0; i < 3; i++ )
{
btIVector3 normal = m_jac[i].m_linearJointAxis;
double jacDiagABInv = btScalar.BT_ONE / m_jac[i].getDiagonal();
double rel_vel;
rel_vel = normal.dot( vel );
//positional error (zeroth order error)
double depth = -( pivotAInW - pivotBInW ).dot( normal ); //this is the error projected on the normal
double impulse = depth * tau / timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
m_appliedImpulse += impulse;
btVector3 ftorqueAxis1 = rel_pos1.cross( normal );
btVector3 ftorqueAxis2 = rel_pos2.cross( normal );
bodyA.internalApplyImpulse( normal * m_rbA.getInvMass(), m_rbA.m_invInertiaTensorWorld * ftorqueAxis1, impulse );
bodyB.internalApplyImpulse( normal * m_rbB.getInvMass(), m_rbB.m_invInertiaTensorWorld * ftorqueAxis2, -impulse );
}
}
// apply motor
if( m_bMotorEnabled )
{
// compute current and predicted transforms
btTransform trACur = m_rbA.m_worldTransform;
btTransform trBCur = m_rbB.m_worldTransform;
btVector3 omegaA; bodyA.internalGetAngularVelocity( omegaA );
btVector3 omegaB; bodyB.internalGetAngularVelocity( omegaB );
btTransform trAPred; trAPred.setIdentity();
btVector3 zerovec( 0, 0, 0);
btTransformUtil::integrateTransform(
trACur, zerovec, omegaA, timeStep, trAPred );
btTransform trBPred; trBPred.setIdentity();
btTransformUtil::integrateTransform(
trBCur, zerovec, omegaB, timeStep, trBPred );
// compute desired transforms in world
btTransform trPose( m_qTarget );
btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse();
btTransform trADes = trBPred * trABDes;
btTransform trBDes = trAPred * trABDes.inverse();
// compute desired omegas in world
btVector3 omegaADes, omegaBDes;
btTransformUtil::calculateVelocity( trACur, trADes, timeStep, zerovec, omegaADes );
btTransformUtil::calculateVelocity( trBCur, trBDes, timeStep, zerovec, omegaBDes );
// compute delta omegas
btVector3 dOmegaA = omegaADes - omegaA;
btVector3 dOmegaB = omegaBDes - omegaB;
// compute weighted avg axis of dOmega (weighting based on inertias)
btVector3 axisA, axisB;
double kAxisAInv = 0, kAxisBInv = 0;
if( dOmegaA.length2() > btScalar.SIMD_EPSILON )
{
axisA = dOmegaA.normalized();
kAxisAInv = m_rbA.computeAngularImpulseDenominator( axisA );
}
if( dOmegaB.length2() > btScalar.SIMD_EPSILON )
{
axisB = dOmegaB.normalized();
kAxisBInv = m_rbB.computeAngularImpulseDenominator( axisB );
}
btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;
static bool bDoTorque = true;
if( bDoTorque & avgAxis.length2() > btScalar.SIMD_EPSILON )
{
avgAxis.normalize();
kAxisAInv = m_rbA.computeAngularImpulseDenominator( avgAxis );
kAxisBInv = m_rbB.computeAngularImpulseDenominator( avgAxis );
double kInvCombined = kAxisAInv + kAxisBInv;
btVector3 impulse = ( kAxisAInv * dOmegaA - kAxisBInv * dOmegaB ) /
( kInvCombined * kInvCombined );
if( m_maxMotorImpulse >= 0 )
{
double fMaxImpulse = m_maxMotorImpulse;
if( m_bNormalizedMotorStrength )
fMaxImpulse = fMaxImpulse / kAxisAInv;
btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse;
double newUnclampedMag = newUnclampedAccImpulse.length();
if( newUnclampedMag > fMaxImpulse )
{
newUnclampedAccImpulse.normalize();
newUnclampedAccImpulse *= fMaxImpulse;
impulse = newUnclampedAccImpulse - m_accMotorImpulse;
}
m_accMotorImpulse += impulse;
}
double impulseMag = impulse.length();
btVector3 impulseAxis = impulse / impulseMag;
bodyA.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbA.m_invInertiaTensorWorld * impulseAxis, impulseMag );
bodyB.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbB.m_invInertiaTensorWorld * impulseAxis, -impulseMag );
}
}
else if( m_damping > btScalar.SIMD_EPSILON ) // no motor: do a little damping
{
btVector3 angVelA; bodyA.internalGetAngularVelocity( angVelA );
btVector3 angVelB; bodyB.internalGetAngularVelocity( angVelB );
btVector3 relVel = angVelB - angVelA;
if( relVel.length2() > btScalar.SIMD_EPSILON )
{
btVector3 relVelAxis = relVel.normalized();
double m_kDamping = btScalar.BT_ONE /
( m_rbA.computeAngularImpulseDenominator( relVelAxis ) +
m_rbB.computeAngularImpulseDenominator( relVelAxis ) );
btVector3 impulse = m_damping * m_kDamping * relVel;
double impulseMag = impulse.length();
btVector3 impulseAxis = impulse / impulseMag;
bodyA.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbA.m_invInertiaTensorWorld * impulseAxis, impulseMag );
bodyB.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbB.m_invInertiaTensorWorld * impulseAxis, -impulseMag );
}
}
// joint limits
{
///solve angular part
btVector3 angVelA;
bodyA.internalGetAngularVelocity( angVelA );
btVector3 angVelB;
bodyB.internalGetAngularVelocity( angVelB );
// solve swing limit
if( m_solveSwingLimit )
{
double amplitude = m_swingLimitRatio * m_swingCorrection * m_biasFactor / timeStep;
double relSwingVel = ( angVelB - angVelA ).dot( m_swingAxis );
if( relSwingVel > 0 )
amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor;
double impulseMag = amplitude * m_kSwing;
// Clamp the accumulated impulse
double temp = m_accSwingLimitImpulse;
m_accSwingLimitImpulse = btMax( m_accSwingLimitImpulse + impulseMag, (double)( 0.0 ) );
impulseMag = m_accSwingLimitImpulse - temp;
btVector3 impulse = m_swingAxis * impulseMag;
// don't let cone response affect twist
// (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit)
{
btVector3 impulseTwistCouple = impulse.dot( m_twistAxisA ) * m_twistAxisA;
btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple;
impulse = impulseNoTwistCouple;
}
impulseMag = impulse.length();
btVector3 noTwistSwingAxis = impulse / impulseMag;
bodyA.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbA.m_invInertiaTensorWorld * noTwistSwingAxis, impulseMag );
bodyB.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbB.m_invInertiaTensorWorld * noTwistSwingAxis, -impulseMag );
}
// solve twist limit
if( m_solveTwistLimit )
{
double amplitude = m_twistLimitRatio * m_twistCorrection * m_biasFactor / timeStep;
double relTwistVel = ( angVelB - angVelA ).dot( m_twistAxis );
if( relTwistVel > 0 ) // only damp when moving towards limit (m_twistAxis flipping is important)
amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor;
double impulseMag = amplitude * m_kTwist;
// Clamp the accumulated impulse
double temp = m_accTwistLimitImpulse;
m_accTwistLimitImpulse = btMax( m_accTwistLimitImpulse + impulseMag, (double)( 0.0 ) );
impulseMag = m_accTwistLimitImpulse - temp;
// btVector3 impulse = m_twistAxis * impulseMag;
bodyA.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbA.m_invInertiaTensorWorld * m_twistAxis, impulseMag );
bodyB.internalApplyImpulse( btVector3( 0, 0, 0 ), m_rbB.m_invInertiaTensorWorld * m_twistAxis, -impulseMag );
}
}
}
}
*/
/*
void calcAngleInfo()
{
m_swingCorrection = btScalar.BT_ZERO;
m_twistLimitSign = btScalar.BT_ZERO;
m_solveTwistLimit = false;
m_solveSwingLimit = false;
btVector3 b1Axis1 = btVector3.Zero,b1Axis2 = btVector3.Zero, b1Axis3 = btVector3.Zero;
btVector3 b2Axis1 = btVector3.Zero, b2Axis2 = btVector3.Zero;
b1Axis1 = m_rbA.m_worldTransform.m_basis * this.m_rbAFrame.m_basis.getColumn( 0 );
b2Axis1 = m_rbB.m_worldTransform.m_basis * this.m_rbBFrame.m_basis.getColumn( 0 );
double swing1 = btScalar.BT_ZERO, swing2 = btScalar.BT_ZERO;
double swx = btScalar.BT_ZERO, swy = btScalar.BT_ZERO;
double thresh = 10;
double fact;
// Get Frame into world space
if( m_swingSpan1 >= (double)( 0.05f ) )
{
b1Axis2 = m_rbA.m_worldTransform.m_basis * this.m_rbAFrame.m_basis.getColumn( 1 );
swx = b2Axis1.dot( b1Axis1 );
swy = b2Axis1.dot( b1Axis2 );
swing1 = btScalar.btAtan2Fast( swy, swx );
fact = ( swy * swy + swx * swx ) * thresh * thresh;
fact = fact / ( fact + (double)( 1.0 ) );
swing1 *= fact;
}
if( m_swingSpan2 >= (double)( 0.05f ) )
{
b1Axis3 = m_rbA.m_worldTransform.m_basis * this.m_rbAFrame.m_basis.getColumn( 2 );
swx = b2Axis1.dot( b1Axis1 );
swy = b2Axis1.dot( b1Axis3 );
swing2 = btScalar.btAtan2Fast( swy, swx );
fact = ( swy * swy + swx * swx ) * thresh * thresh;
fact = fact / ( fact + (double)( 1.0 ) );
swing2 *= fact;
}
double RMaxAngle1Sq = 1.0f / ( m_swingSpan1 * m_swingSpan1 );
double RMaxAngle2Sq = 1.0f / ( m_swingSpan2 * m_swingSpan2 );
double EllipseAngle = btScalar.btFabs( swing1 * swing1 ) * RMaxAngle1Sq + btScalar.btFabs( swing2 * swing2 ) * RMaxAngle2Sq;
if( EllipseAngle > 1.0f )
{
m_swingCorrection = EllipseAngle - 1.0f;
m_solveSwingLimit = true;
// Calculate necessary axis & factors
m_swingAxis = b2Axis1.cross( b1Axis2 * b2Axis1.dot( b1Axis2 ) + b1Axis3 * b2Axis1.dot( b1Axis3 ) );
m_swingAxis.normalize();
double swingAxisSign = ( b2Axis1.dot( b1Axis1 ) >= 0.0f ) ? 1.0f : -1.0f;
m_swingAxis *= swingAxisSign;
}
// Twist limits
if( m_twistSpan >= btScalar.BT_ZERO )
{
btVector3 b2Axis2 = m_rbB.m_worldTransform.m_basis * this.m_rbBFrame.m_basis.getColumn( 1 );
btQuaternion rotationArc = btQuaternion.shortestArcQuat( b2Axis1, b1Axis1 );
btVector3 TwistRef = btQuaternion.quatRotate( rotationArc, b2Axis2 );
double twist = btScalar.btAtan2Fast( TwistRef.dot( b1Axis3 ), TwistRef.dot( b1Axis2 ) );
m_twistAngle = twist;
// double lockedFreeFactor = (m_twistSpan > (double)(0.05f)) ? m_limitSoftness : btScalar.BT_ZERO;
double lockedFreeFactor = ( m_twistSpan > (double)( 0.05f ) ) ? (double)( 1.0f ) : btScalar.BT_ZERO;
if( twist <= -m_twistSpan * lockedFreeFactor )
{
m_twistCorrection = -( twist + m_twistSpan );
m_solveTwistLimit = true;
m_twistAxis = ( b2Axis1 + b1Axis1 ) * 0.5f;
m_twistAxis.normalize();
m_twistAxis *= -1.0f;
}
else if( twist > m_twistSpan * lockedFreeFactor )
{
m_twistCorrection = ( twist - m_twistSpan );
m_solveTwistLimit = true;
m_twistAxis = ( b2Axis1 + b1Axis1 ) * 0.5f;
m_twistAxis.normalize();
}
}
}
*/
void calcAngleInfo2( ref btTransform transA, ref btTransform transB, ref btMatrix3x3 invInertiaWorldA, ref btMatrix3x3 invInertiaWorldB )
{
m_swingCorrection = btScalar.BT_ZERO;
m_twistLimitSign = btScalar.BT_ZERO;
m_solveTwistLimit = false;
m_solveSwingLimit = false;
// compute rotation of A wrt B (in constraint space)
if( m_bMotorEnabled && ( !m_useSolveConstraintObsolete ) )
{ // it is assumed that setMotorTarget() was alredy called
// and motor target m_qTarget is within constraint limits
// TODO : split rotation to pure swing and pure twist
// compute desired transforms in world
btTransform trPose = new btTransform( ref m_qTarget );
btTransform trA; transA.Apply( ref m_rbAFrame, out trA );
btTransform trB; transB.Apply( ref m_rbBFrame, out trB );
btTransform tmp;
btTransform trAInv;
trA.inverse( out trAInv );
trB.Apply( ref trPose, out tmp );
btTransform trDeltaAB;// = trB * trPose * trA.inverse();
tmp.Apply( ref trAInv, out trDeltaAB );
btQuaternion qDeltaAB; trDeltaAB.getRotation( out qDeltaAB );
btVector3 swingAxis = new btVector3( qDeltaAB.x, qDeltaAB.y, qDeltaAB.z );
double swingAxisLen2 = swingAxis.length2();
if( btScalar.btFuzzyZero( swingAxisLen2 ) )
{
return;
}
m_swingAxis = swingAxis;
m_swingAxis.normalize();
m_swingCorrection = qDeltaAB.getAngle();
if( !btScalar.btFuzzyZero( m_swingCorrection ) )
{
m_solveSwingLimit = true;
}
return;
}
{
// compute rotation of A wrt B (in constraint space)
btQuaternion tmpA;
transA.getRotation( out tmpA );
btQuaternion tmpAFrame;
m_rbAFrame.getRotation( out tmpAFrame );
btQuaternion qA; tmpA.Mult( ref tmpAFrame, out qA );
transB.getRotation( out tmpA );
m_rbBFrame.getRotation( out tmpAFrame );
btQuaternion qB; tmpA.Mult( ref tmpAFrame, out qB );// transB.getRotation() * m_rbBFrame.getRotation();
btQuaternion qBInv;
qB.inverse( out qBInv );
btQuaternion qAB; qBInv.Mult( ref qA, out qAB );
// split rotation into cone and twist
// (all this is done from B's perspective. Maybe I should be averaging axes...)
btVector3 vConeNoTwist; btQuaternion.quatRotate( ref qAB, ref vTwist, out vConeNoTwist ); vConeNoTwist.normalize();
btQuaternion qABCone; btQuaternion.shortestArcQuat( ref vTwist, ref vConeNoTwist, out qABCone ); qABCone.normalize();
btQuaternion qABInv;
qABCone.inverse( out qABInv );
btQuaternion qABTwist; qABInv.Mult( ref qAB, out qABTwist ); qABTwist.normalize();
if( m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh )
{
double swingAngle, swingLimit = 0;
btVector3 swingAxis;
computeConeLimitInfo( ref qABCone, out swingAngle, out swingAxis, out swingLimit );
if( swingAngle > swingLimit * m_limitSoftness )
{
m_solveSwingLimit = true;
// compute limit ratio: 0.1, where
// 0 == beginning of soft limit
// 1 == hard/real limit
m_swingLimitRatio = 1;
if( swingAngle < swingLimit && m_limitSoftness < 1 - btScalar.SIMD_EPSILON )
{
m_swingLimitRatio = ( swingAngle - swingLimit * m_limitSoftness ) /
( swingLimit - swingLimit * m_limitSoftness );
}
// swing correction tries to get back to soft limit
m_swingCorrection = swingAngle - ( swingLimit * m_limitSoftness );
// adjustment of swing axis (based on ellipse normal)
adjustSwingAxisToUseEllipseNormal( ref swingAxis );
// Calculate necessary axis & factors
btVector3 swingAxisInv;
swingAxis.Invert( out swingAxisInv );
btQuaternion.quatRotate( ref qB, ref swingAxisInv, out m_swingAxis );
m_twistAxisA.setValue( 0, 0, 0 );
m_kSwing = btScalar.BT_ONE /
( computeAngularImpulseDenominator( ref m_swingAxis, invInertiaWorldA ) +
computeAngularImpulseDenominator( ref m_swingAxis, invInertiaWorldB ) );
}
}
else
{
// you haven't set any limits;
// or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
// anyway, we have either hinge or fixed joint
btVector3 ivA = transA.m_basis * m_rbAFrame.m_basis.getColumn( 0 );
btVector3 jvA = transA.m_basis * m_rbAFrame.m_basis.getColumn( 1 );
btVector3 kvA = transA.m_basis * m_rbAFrame.m_basis.getColumn( 2 );
btVector3 ivB = transB.m_basis * m_rbBFrame.m_basis.getColumn( 0 );
btVector3 target;
double x = ivB.dot( ivA );
double y = ivB.dot( jvA );
double z = ivB.dot( kvA );
if( ( m_swingSpan1 < m_fixThresh ) && ( m_swingSpan2 < m_fixThresh ) )
{ // fixed. We'll need to add one more row to constraint
if( ( !btScalar.btFuzzyZero( y ) ) || ( !( btScalar.btFuzzyZero( z ) ) ) )
{
m_solveSwingLimit = true;
m_swingAxis = -ivB.cross( ivA );
}
}
else
{
if( m_swingSpan1 < m_fixThresh )
{ // hinge around Y axis
// if(!(btFuzzyZero(y)))
if( ( !( btScalar.btFuzzyZero( x ) ) ) || ( !( btScalar.btFuzzyZero( z ) ) ) )
{
m_solveSwingLimit = true;
if( m_swingSpan2 >= m_fixThresh )
{
y = (double)( 0 );
double span2 = btScalar.btAtan2( z, x );
if( span2 > m_swingSpan2 )
{
x = btScalar.btCos( m_swingSpan2 );
z = btScalar.btSin( m_swingSpan2 );
}
else if( span2 < -m_swingSpan2 )
{
x = btScalar.btCos( m_swingSpan2 );
z = -btScalar.btSin( m_swingSpan2 );
}
}
}
}
else
{ // hinge around Z axis
// if(!btFuzzyZero(z))
if( ( !( btScalar.btFuzzyZero( x ) ) ) || ( !( btScalar.btFuzzyZero( y ) ) ) )
{
m_solveSwingLimit = true;
if( m_swingSpan1 >= m_fixThresh )
{
z = (double)( 0 );
double span1 = btScalar.btAtan2( y, x );
if( span1 > m_swingSpan1 )
{
x = btScalar.btCos( m_swingSpan1 );
y = btScalar.btSin( m_swingSpan1 );
}
else if( span1 < -m_swingSpan1 )
{
x = btScalar.btCos( m_swingSpan1 );
y = -btScalar.btSin( m_swingSpan1 );
}
}
}
}
target.x = x * ivA[0] + y * jvA[0] + z * kvA[0];
target.y = x * ivA[1] + y * jvA[1] + z * kvA[1];
target.z = x * ivA[2] + y * jvA[2] + z * kvA[2];
target.w = 0;
target.normalize();
m_swingAxis = -ivB.cross( target );
m_swingCorrection = m_swingAxis.length();
if( !btScalar.btFuzzyZero( m_swingCorrection ) )
m_swingAxis.normalize();
}
}
if( m_twistSpan >= (double)( 0 ) )
{
btVector3 twistAxis;
computeTwistLimitInfo( ref qABTwist, out m_twistAngle, out twistAxis );
twistAxis.Invert( out twistAxis );
if( m_twistAngle > m_twistSpan * m_limitSoftness )
{
m_solveTwistLimit = true;
m_twistLimitRatio = 1;
if( m_twistAngle < m_twistSpan && m_limitSoftness < 1 - btScalar.SIMD_EPSILON )
{
m_twistLimitRatio = ( m_twistAngle - m_twistSpan * m_limitSoftness ) /
( m_twistSpan - m_twistSpan * m_limitSoftness );
}
// twist correction tries to get back to soft limit
m_twistCorrection = m_twistAngle - ( m_twistSpan * m_limitSoftness );
btQuaternion.quatRotate( ref qB, ref twistAxis, out m_twistAxis );
m_kTwist = btScalar.BT_ONE /
( computeAngularImpulseDenominator( ref m_twistAxis, invInertiaWorldA ) +
computeAngularImpulseDenominator( ref m_twistAxis, invInertiaWorldB ) );
}
if( m_solveSwingLimit )
{
btQuaternion.quatRotate( ref qA, ref twistAxis, out m_twistAxisA );
}
}
else
{
m_twistAngle = (double)( 0 );
}
}
}
bool matrixToEulerZXY( ref btMatrix3x3 mat, ref btVector3 xyz )
{
// rot = cz*cy-sz*sx*sy -cx*sz cz*sy+cy*sz*sx
// cy*sz+cz*sx*sy cz*cx sz*sy-cz*xy*sx
// -cx*sy sx cx*cy
double fi = btGetMatrixElem( ref mat, 7 );
if( fi < (double)( 1.0f ) )
{
if( fi > (double)( -1.0f ) )
{
xyz[0] = btScalar.btAsin( btGetMatrixElem( ref mat, 7 ) );
xyz[1] = btScalar.btAtan2( -btGetMatrixElem( ref mat, 6 ), btGetMatrixElem( ref mat, 8 ) );
xyz[2] = btScalar.btAtan2( -btGetMatrixElem( ref mat, 1 ), btGetMatrixElem( ref mat, 4 ) );
return true;
}
else
{
xyz[0] = -btScalar.SIMD_HALF_PI;
xyz[1] = (double)( 0.0 );
xyz[2] = -btScalar.btAtan2( btGetMatrixElem( ref mat, 2 ), btGetMatrixElem( ref mat, 0 ) );
return false;
}
}
else
{
xyz[0] = btScalar.SIMD_HALF_PI;
xyz[1] = (double)( 0.0 );
xyz[2] = btScalar.btAtan2( btGetMatrixElem( ref mat, 2 ), btGetMatrixElem( ref mat, 0 ) );
}
return false;
}
public void evalEulerEqnDeriv( ref btVector3 w1, ref btVector3 w0, double dt,
ref btMatrix3x3 I, out btMatrix3x3 result )
{
btMatrix3x3 w1x, Iw1x;
btMatrix3x3 tmpm;
btMatrix3x3 tmp2;
btVector3 Iwi; I.Apply( ref w1, out Iwi );
w1.getSkewSymmetricMatrix( out w1x.m_el0, out w1x.m_el1, out w1x.m_el2 );
#if !DISABLE_ROW4
w1x.m_el3 = btVector3.wAxis;
#endif
Iwi.getSkewSymmetricMatrix( out Iw1x.m_el0, out Iw1x.m_el1, out Iw1x.m_el2 );
#if !DISABLE_ROW4
Iw1x.m_el3 = btVector3.wAxis;
#endif
I.Mult( ref w1x, out tmpm );
tmpm.Sub( ref Iw1x, out tmp2 );
tmp2.Mult( dt, out result );
//btMatrix3x3 dfw1 = I + ( w1x * I - Iw1x ) * dt;
//return dfw1;
}
bool matrixToEulerZYX( ref btMatrix3x3 mat, ref btVector3 xyz )
{
// rot = cz*cy cz*sy*sx-cx*sz sz*sx+cz*cx*sy
// cy*sz cz*cx+sz*sy*sx cx*sz*sy-cz*sx
// -sy cy*sx cy*cx
double fi = btGetMatrixElem( ref mat, 6 );
if( fi < (double)( 1.0f ) )
{
if( fi > (double)( -1.0f ) )
{
xyz[0] = btScalar.btAtan2( btGetMatrixElem( ref mat, 7 ), btGetMatrixElem( ref mat, 8 ) );
xyz[1] = btScalar.btAsin( -btGetMatrixElem( ref mat, 6 ) );
xyz[2] = btScalar.btAtan2( btGetMatrixElem( ref mat, 3 ), btGetMatrixElem( ref mat, 0 ) );
return true;
}
else
{
xyz[0] = (double)( 0.0 );
xyz[1] = btScalar.SIMD_HALF_PI;
xyz[2] = -btScalar.btAtan2( btGetMatrixElem( ref mat, 1 ), btGetMatrixElem( ref mat, 2 ) );
return false;
}
}
else
{
xyz[0] = (double)( 0.0 );
xyz[1] = -btScalar.SIMD_HALF_PI;
xyz[2] = btScalar.btAtan2( -btGetMatrixElem( ref mat, 1 ), -btGetMatrixElem( ref mat, 2 ) );
}
return false;
}
public double computeAngularImpulseDenominator( ref btVector3 axis, btMatrix3x3 invInertiaWorld )
{
btVector3 vec = axis * invInertiaWorld;
return axis.dot( vec );
}
