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();
}
void getOrientation( out btQuaternion result )
{
m_worldTransform.m_basis.getRotation( out result );
}
public void setMotorTarget( ref btQuaternion qAinB, double dt )
{
// convert target from body to constraint space
btMatrix3x3 tmp;
m_rbBFrame.m_basis.inverse( out tmp );
btQuaternion tmpq;
m_rbBFrame.getRotation( out tmpq );
tmpq.inverse( out tmpq );
btQuaternion tmpq2;
tmpq.Mult( ref qAinB, out tmpq2 );
m_rbAFrame.getRotation( out tmpq );
btQuaternion qConstraint;// = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation();
tmpq2.Mult( ref tmpq, out qConstraint );
qConstraint.normalize();
// extract "pure" hinge component
btVector3 vNoHinge; btQuaternion.quatRotate( ref qConstraint, ref vHinge, out vNoHinge ); vNoHinge.normalize();
btQuaternion qNoHinge; btQuaternion.shortestArcQuat( ref vHinge, ref vNoHinge, out qNoHinge );
qNoHinge.inverse( out tmpq );
btQuaternion qHinge;// = qNoHinge.inverse() * qConstraint;
tmpq.Mult( ref qConstraint, out qHinge );
qHinge.normalize();
// compute angular target, clamped to limits
double targetAngle = qHinge.getAngle();
if( targetAngle > btScalar.SIMD_PI ) // long way around. flip quat and recalculate.
{
qHinge.inverse( out qHinge );
//qHinge = -( qHinge );
targetAngle = qHinge.getAngle();
}
if( qHinge.z < 0 )
targetAngle = -targetAngle;
setMotorTarget( targetAngle, dt );
}
//
// 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();
}
}
internal override void processCollision( btCollisionObjectWrapper body0Wrap
, ref btTransform body0Transform
, btCollisionObjectWrapper body1Wrap
, ref btTransform body1Transform
, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
{
//(void)dispatchInfo;
if( m_manifoldPtr == null )
return;
btCollisionObjectWrapper convexObjWrap = m_swapped ? body1Wrap : body0Wrap;
btCollisionObjectWrapper planeObjWrap = m_swapped ? body0Wrap : body1Wrap;
btConvexShape convexShape = (btConvexShape)convexObjWrap.m_shape;
btStaticPlaneShape planeShape = (btStaticPlaneShape)planeObjWrap.getCollisionShape();
//btVector3 planeNormal;
//planeShape.getPlaneNormal().Copy( out planeNormal );
double planeConstant = planeShape.getPlaneConstant();
if( m_swapped )
collideSingleContact( false, ref btQuaternion.Identity, convexObjWrap
, ref body1Transform
, planeObjWrap
, ref body0Transform
, dispatchInfo, resultOut
, ref planeShape.m_planeNormal, planeConstant );
else
collideSingleContact( false, ref btQuaternion.Identity, convexObjWrap
, ref body0Transform
, planeObjWrap
, ref body1Transform
, dispatchInfo, resultOut
, ref planeShape.m_planeNormal, planeConstant );
/*
btCollisionObjectWrapper convexObjWrap = m_swapped ? body1Wrap : body0Wrap;
btCollisionObjectWrapper planeObjWrap = m_swapped ? body0Wrap : body1Wrap;
btConvexShape convexShape = (btConvexShape)convexObjWrap.m_shape;
btStaticPlaneShape planeShape = (btStaticPlaneShape)planeObjWrap.m_shape;
bool hasCollision = false;
btVector3 planeNormal; planeShape.m_planeNormal.Copy( out planeNormal );
double planeConstant = planeShape.getPlaneConstant();
btTransform planeInConvex;
convexObjWrap.getWorldTransform().inverseTimes( planeObjWrap.getWorldTransform(), out planeInConvex );
btTransform convexInPlaneTrans;
planeObjWrap.getWorldTransform().inverseTimes( convexObjWrap.getWorldTransform(), out convexInPlaneTrans );
btVector3 invPlaneNormal;
planeNormal.Invert( out invPlaneNormal );
btVector3 tmp;
planeInConvex.getBasis().Apply( planeNormal, out tmp );
btVector3 vtx; convexShape.localGetSupportingVertex( ref tmp, out vtx );
btVector3 vtxInPlane; convexInPlaneTrans.Apply( ref vtx, out vtxInPlane );
double distance = ( planeNormal.dot( ref vtxInPlane ) - planeConstant );
btVector3 vtxInPlaneProjected; vtxInPlane.AddScale( planeNormal, -distance, out vtxInPlaneProjected );
btVector3 vtxInPlaneWorld; planeObjWrap.getWorldTransform().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 = planeObjWrap.getWorldTransform().getBasis() * planeNormal;
btVector3 pOnB = vtxInPlaneWorld;
resultOut.addContactPoint( ref normalOnSurfaceB, ref pOnB, distance );
}
*/
//the perturbation algorithm doesn't work well with implicit surfaces such as spheres, cylinder and cones:
//they keep on rolling forever because of the additional off-center contact points
//so only enable the feature for polyhedral shapes (btBoxShape, btConvexHullShape etc)
if( convexShape.isPolyhedral() && resultOut.m_manifoldPtr.m_cachedPoints < m_minimumPointsPerturbationThreshold )
{
btVector3 v0, v1;
btVector3.btPlaneSpace1( ref planeShape.m_planeNormal, out v0, out v1 );
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
double angleLimit = 0.125f * btScalar.SIMD_PI;
double perturbeAngle;
double radius = convexShape.getAngularMotionDisc();
perturbeAngle = btPersistentManifold.gContactBreakingThreshold / radius;
if( perturbeAngle > angleLimit )
perturbeAngle = angleLimit;
btQuaternion perturbeRot = new btQuaternion( ref v0, perturbeAngle );
double interval = btScalar.SIMD_2_PI / (double)( m_numPerturbationIterations );
for( int i = 0; i < m_numPerturbationIterations; i++ )
{
double iterationAngle = i * interval;
btQuaternion rotq = new btQuaternion( ref planeShape.m_planeNormal, iterationAngle );
btQuaternion rotqInv;
rotq.inverse( out rotqInv );
btQuaternion tmpq, tmpq2;
rotqInv.Mult( ref perturbeRot, out tmpq );
tmpq.Mult( ref rotq, out tmpq2 );
if( m_swapped )
collideSingleContact( true, ref tmpq2
, convexObjWrap, ref body1Transform
, planeObjWrap, ref body0Transform
, dispatchInfo
, resultOut, ref planeShape.m_planeNormal, planeConstant );
else
collideSingleContact( true, ref tmpq2
, convexObjWrap, ref body0Transform
, planeObjWrap, ref body1Transform, dispatchInfo
, resultOut, ref planeShape.m_planeNormal, planeConstant );
}
}
if( m_ownManifold )
{
if( m_manifoldPtr.m_cachedPoints != 0 )
{
resultOut.refreshContactPoints();
}
}
}
public void getMotorTarget( out btQuaternion result ) { result = m_qTarget; }
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 );
}
}
void setMotorTargetInConstraintSpace( ref btQuaternion q )
{
m_qTarget = q;
// clamp motor target to within limits
{
double softness = 1;//m_limitSoftness;
// split into twist and cone
btVector3 vTwisted; btQuaternion.quatRotate( ref m_qTarget, ref vTwist, out vTwisted );
btQuaternion qTargetCone; btQuaternion.shortestArcQuat( ref vTwist, ref vTwisted, out qTargetCone ); qTargetCone.normalize();
qTargetCone.inverse( out qTargetCone );
btQuaternion qTargetTwist; qTargetCone.Mult( ref m_qTarget, out qTargetTwist ); qTargetTwist.normalize();
// clamp cone
if( m_swingSpan1 >= (double)( 0.05f ) && m_swingSpan2 >= (double)( 0.05f ) )
{
double swingAngle, swingLimit; btVector3 swingAxis;
computeConeLimitInfo( ref qTargetCone, out swingAngle, out swingAxis, out swingLimit );
if( Math.Abs( swingAngle ) > btScalar.SIMD_EPSILON )
{
if( swingAngle > swingLimit * softness )
swingAngle = swingLimit * softness;
else if( swingAngle < -swingLimit * softness )
swingAngle = -swingLimit * softness;
qTargetCone = new btQuaternion( ref swingAxis, swingAngle );
}
}
// clamp twist
if( m_twistSpan >= (double)( 0.05f ) )
{
double twistAngle; btVector3 twistAxis;
computeTwistLimitInfo( ref qTargetTwist, out twistAngle, out twistAxis );
if( Math.Abs( twistAngle ) > btScalar.SIMD_EPSILON )
{
// eddy todo: limitSoftness used here???
if( twistAngle > m_twistSpan * softness )
twistAngle = m_twistSpan * softness;
else if( twistAngle < -m_twistSpan * softness )
twistAngle = -m_twistSpan * softness;
qTargetTwist = new btQuaternion( ref twistAxis, twistAngle );
}
}
qTargetCone.Mult( ref qTargetTwist, out m_qTarget );
}
}
// setMotorTarget:
// q: the desired rotation of bodyA wrt bodyB.
// note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability)
// note: don't forget to enableMotor()
public void setMotorTarget( ref btQuaternion q )
{
//btTransform trACur = m_rbA.m_worldTransform;
//btTransform trBCur = m_rbB.m_worldTransform;
// btTransform trABCur = trBCur.inverse() * trACur;
// btQuaternion qABCur = trABCur.getRotation();
// btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);
//btQuaternion qConstraintCur = trConstraintCur.getRotation();
btQuaternion tmp, tmp2, tmp3;
m_rbBFrame.getRotation( out tmp );
tmp.inverse( out tmp );
tmp.Mult( ref q, out tmp3 );
m_rbAFrame.getRotation( out tmp2 );
btQuaternion qConstraint;// = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation();
tmp3.Mult( ref tmp2, out qConstraint );
setMotorTargetInConstraintSpace( ref qConstraint );
}
// given a twist rotation in constraint space, (pre: cone must already be removed)
// this method computes its corresponding angle and axis.
void computeTwistLimitInfo( ref btQuaternion qTwist,
out double twistAngle, // out
out btVector3 vTwistAxis ) // out
{
btQuaternion qMinTwist = qTwist;
twistAngle = qTwist.getAngle();
if( twistAngle > btScalar.SIMD_PI ) // long way around. flip quat and recalculate.
{
qTwist.inverse( out qMinTwist );
twistAngle = qMinTwist.getAngle();
}
if( twistAngle < 0 )
{
// this should never happen
#if false
Debug.Assert(false);
#endif
}
vTwistAxis = new btVector3( qMinTwist.x, qMinTwist.y, qMinTwist.z );
if( twistAngle > btScalar.SIMD_EPSILON )
vTwistAxis.normalize();
}
internal void GetPointForAngle( double fAngleInRadians, double fLength, out btVector3 result )
{
// compute x/y in ellipse using cone angle (0 . 2*PI along surface of cone)
double xEllipse = btScalar.btCos( fAngleInRadians );
double yEllipse = btScalar.btSin( fAngleInRadians );
// Use the slope of the vector (using x/yEllipse) and find the length
// of the line that intersects the ellipse:
// x^2 y^2
// --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
// a^2 b^2
// Do the math and it should be clear.
double swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
if( Math.Abs( xEllipse ) > btScalar.SIMD_EPSILON )
{
double surfaceSlope2 = ( yEllipse * yEllipse ) / ( xEllipse * xEllipse );
double norm = 1 / ( m_swingSpan2 * m_swingSpan2 );
norm += surfaceSlope2 / ( m_swingSpan1 * m_swingSpan1 );
double swingLimit2 = ( 1 + surfaceSlope2 ) / norm;
swingLimit = btScalar.btSqrt( swingLimit2 );
}
// convert into point in constraint space:
// note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively
btVector3 vSwingAxis = new btVector3( 0, xEllipse, -yEllipse );
btQuaternion qSwing = new btQuaternion( ref vSwingAxis, swingLimit );
btVector3 vPointInConstraintSpace = new btVector3( fLength, 0, 0 );
btQuaternion.quatRotate( ref qSwing, ref vPointInConstraintSpace, out result );
}
// given a cone rotation in constraint space, (pre: twist must already be removed)
// this method computes its corresponding swing angle and axis.
// more interestingly, it computes the cone/swing limit (angle) for this cone "pose".
void computeConeLimitInfo( ref btQuaternion qCone,
out double swingAngle, // out
out btVector3 vSwingAxis, // out
out double swingLimit ) // out
{
swingAngle = qCone.getAngle();
if( swingAngle > btScalar.SIMD_EPSILON )
{
vSwingAxis = new btVector3( qCone.x, qCone.y, qCone.z );
vSwingAxis.normalize();
#if false
// non-zero twist?! this should never happen.
Debug.Assert(Math.Abs(vSwingAxis.x) <= btScalar.SIMD_EPSILON));
#endif
// Compute limit for given swing. tricky:
// Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
// (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)
// For starters, compute the direction from center to surface of ellipse.
// This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis.
// (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.)
double xEllipse = vSwingAxis.y;
double yEllipse = -vSwingAxis.z;
// Now, we use the slope of the vector (using x/yEllipse) and find the length
// of the line that intersects the ellipse:
// x^2 y^2
// --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
// a^2 b^2
// Do the math and it should be clear.
swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
if( Math.Abs( xEllipse ) > btScalar.SIMD_EPSILON )
{
double surfaceSlope2 = ( yEllipse * yEllipse ) / ( xEllipse * xEllipse );
double norm = 1 / ( m_swingSpan2 * m_swingSpan2 );
norm += surfaceSlope2 / ( m_swingSpan1 * m_swingSpan1 );
double swingLimit2 = ( 1 + surfaceSlope2 ) / norm;
swingLimit = btScalar.btSqrt( swingLimit2 );
}
// test!
/*swingLimit = m_swingSpan2;
if (Math.Abs(vSwingAxis.z) > btScalar.SIMD_EPSILON)
{
double mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2;
double sinphi = m_swingSpan2 / sqrt(mag_2);
double phi = asin(sinphi);
double theta = atan2(Math.Abs(vSwingAxis.y),Math.Abs(vSwingAxis.z));
double alpha = 3.14159f - theta - phi;
double sinalpha = sin(alpha);
swingLimit = m_swingSpan1 * sinphi/sinalpha;
}*/
}
else //if( swingAngle < 0 )
{
vSwingAxis = btVector3.xAxis;
swingLimit = 0;
// this should never happen!
#if false
Debug.Assert(false);
#endif
}
}
