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();
}
/// This maximum should not be necessary. It allows for untested/degenerate cases in production code.
/// You don't want your game ever to lock-up.
void computeClosestPoints( ref btTransform transA, ref btTransform transB, btPointCollector pointCollector)
{
if( m_convexB1 != null)
{
m_simplexSolver.reset();
btGjkPairDetector gjk = BulletGlobals.GjkPairDetectorPool.Get();
gjk.Initialize( m_convexA, m_convexB1, m_convexA.getShapeType(), m_convexB1.getShapeType(), m_convexA.getMargin(), m_convexB1.getMargin(), m_simplexSolver, m_penetrationDepthSolver);
btGjkPairDetector.ClosestPointInput input = new btDiscreteCollisionDetectorInterface.ClosestPointInput();
input.m_transformA = transA;
input.m_transformB = transB;
gjk.getClosestPoints( input, pointCollector, null );
BulletGlobals.GjkPairDetectorPool.Free( gjk );
}
else
{
//convex versus plane
btConvexShape convexShape = m_convexA;
btStaticPlaneShape planeShape = m_planeShape;
btVector3 planeNormal; planeShape.getPlaneNormal( out planeNormal );
double planeConstant = planeShape.getPlaneConstant();
//btTransform convexWorldTransform = transA;
btTransform convexInPlaneTrans;
btTransform tmpInv;
transB.inverse( out tmpInv );
tmpInv.Apply( ref transA, out convexInPlaneTrans );
btTransform planeInConvex;
convexInPlaneTrans.inverse( out tmpInv );
tmpInv.Apply( ref transB, out planeInConvex );
//planeInConvex = convexWorldTransform.inverse() * transB;
btVector3 tmp;
planeInConvex.m_basis.Mult( ref planeNormal, out tmp );
tmp.Invert( out tmp );
btVector3 vtx; convexShape.localGetSupportingVertex( ref tmp, out vtx );
btVector3 vtxInPlane; convexInPlaneTrans.Apply( ref vtx, out vtxInPlane );
double distance = ( planeNormal.dot( vtxInPlane ) - planeConstant );
btVector3 vtxInPlaneProjected;// = vtxInPlane - distance * planeNormal;
vtxInPlane.SubScale( ref planeNormal, distance, out vtxInPlaneProjected );
btVector3 vtxInPlaneWorld; transB.Apply(ref vtxInPlaneProjected, out vtxInPlaneWorld );
btVector3 normalOnSurfaceB; transB.m_basis.Apply( ref planeNormal, out normalOnSurfaceB );
pointCollector.addContactPoint(
ref normalOnSurfaceB,
ref vtxInPlaneWorld,
distance );
}
}
public static void objectQuerySingleInternal( btConvexShape castShape, ref btTransform convexFromTrans, ref btTransform convexToTrans
, btCollisionShape collisionShape//btCollisionObjectWrapper colObjWrap
, btCollisionObject collisionObject
, ref btTransform colObjTransform
, ConvexResultCallback resultCallback, double allowedPenetration )
{
//btCollisionShape collisionShape = colObjWrap.getCollisionShape();
//btTransform colObjWorldTransform = colObjWrap.m_worldTransform;
if( collisionShape.isConvex() )
{
//CProfileSample sample = new CProfileSample("convexSweepConvex");
btConvexCast.CastResult castResult = new btConvexCast.CastResult();
castResult.m_allowedPenetration = allowedPenetration;
castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar.BT_ONE;//??
btConvexShape convexShape = (btConvexShape)collisionShape;
btVoronoiSimplexSolver simplexSolver = BulletGlobals.VoronoiSimplexSolverPool.Get();
btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver = BulletGlobals.GjkEpaPenetrationDepthSolverPool.Get();
// new btGjkEpaPenetrationDepthSolver();
btContinuousConvexCollision convexCaster1 = BulletGlobals.ContinuousConvexCollisionPool.Get();
convexCaster1.Initialize( castShape, convexShape, simplexSolver, gjkEpaPenetrationSolver );
//btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
//btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
btConvexCast castPtr = convexCaster1;
if( castPtr.calcTimeOfImpact( ref convexFromTrans, ref convexToTrans, ref colObjTransform, ref colObjTransform, castResult ) )
{
//add hit
if( castResult.m_normal.length2() > (double)( 0.0001 ) )
{
if( castResult.m_fraction < resultCallback.m_closestHitFraction )
{
castResult.m_normal.normalize();
LocalConvexResult localConvexResult =
new LocalConvexResult
(
collisionObject,
null,
ref castResult.m_normal,
ref castResult.m_hitPoint,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.addSingleResult( ref localConvexResult, normalInWorldSpace );
}
}
}
BulletGlobals.GjkEpaPenetrationDepthSolverPool.Free( gjkEpaPenetrationSolver );
BulletGlobals.VoronoiSimplexSolverPool.Free( simplexSolver );
BulletGlobals.ContinuousConvexCollisionPool.Free( convexCaster1 );
}
else
{
if( collisionShape.isConcave() )
{
if( collisionShape.getShapeType() == BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE )
{
//CProfileSample sample = new CProfileSample("convexSweepbtBvhTriangleMesh");
btConcaveShape triangleMesh = (btConcaveShape)collisionShape;
btTransform worldTocollisionObject; colObjTransform.inverse( out worldTocollisionObject );
btVector3 convexFromLocal; worldTocollisionObject.Apply( ref convexFromTrans.m_origin, out convexFromLocal );
btVector3 convexToLocal; worldTocollisionObject.Apply( ref convexToTrans.m_origin, out convexToLocal );
// rotation of box in local mesh space = MeshRotation^-1 ConvexToRotation
btTransform rotationXform; worldTocollisionObject.m_basis.Apply( ref convexToTrans.m_basis, out rotationXform.m_basis );
rotationXform.m_origin = btVector3.Zero;
//ConvexCast::CastResult
BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get();
tccb.Initialize( castShape, ref convexFromTrans, ref convexToTrans
, resultCallback, collisionObject
, triangleMesh, ref colObjTransform );
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
tccb.m_allowedPenetration = allowedPenetration;
btVector3 boxMinLocal, boxMaxLocal;
castShape.getAabb( ref rotationXform, out boxMinLocal, out boxMaxLocal );
triangleMesh.performConvexcast( tccb, ref convexFromLocal, ref convexToLocal, ref boxMinLocal, ref boxMaxLocal );
BulletGlobals.BridgeTriangleConvexcastCallbackPool.Free( tccb );
}
else
{
if( collisionShape.getShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE )
{
btConvexCast.CastResult castResult = BulletGlobals.CastResultPool.Get();
castResult.m_allowedPenetration = allowedPenetration;
castResult.m_fraction = resultCallback.m_closestHitFraction;
btStaticPlaneShape planeShape = (btStaticPlaneShape)collisionShape;
btContinuousConvexCollision convexCaster1 = BulletGlobals.ContinuousConvexCollisionPool.Get();
convexCaster1.Initialize( castShape, planeShape );
btConvexCast castPtr = convexCaster1;
if( castPtr.calcTimeOfImpact(ref convexFromTrans, ref convexToTrans, ref colObjTransform, ref colObjTransform, castResult ) )
{
//add hit
if( castResult.m_normal.length2() > (double)( 0.0001 ) )
{
if( castResult.m_fraction < resultCallback.m_closestHitFraction )
{
castResult.m_normal.normalize();
LocalConvexResult localConvexResult = new LocalConvexResult
(
collisionObject,
null,
ref castResult.m_normal,
ref castResult.m_hitPoint,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.addSingleResult( ref localConvexResult, normalInWorldSpace );
}
}
}
}
else
{
//CProfileSample sample = new CProfileSample("convexSweepConcave");
btConcaveShape concaveShape = (btConcaveShape)collisionShape;
btTransform worldTocollisionObject; colObjTransform.inverse( out worldTocollisionObject );
btVector3 convexFromLocal; worldTocollisionObject.Apply( ref convexFromTrans.m_origin, out convexFromLocal );
btVector3 convexToLocal; worldTocollisionObject.Apply( ref convexToTrans.m_origin, out convexToLocal );
// rotation of box in local mesh space = MeshRotation^-1 ConvexToRotation
btTransform rotationXform; worldTocollisionObject.m_basis.Apply( ref convexToTrans.m_basis, out rotationXform.m_basis );
rotationXform.m_origin = btVector3.Zero;
BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get();
tccb.Initialize( castShape, ref convexFromTrans, ref convexToTrans, resultCallback, collisionObject
, concaveShape, ref colObjTransform );
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
tccb.m_allowedPenetration = allowedPenetration;
btVector3 boxMinLocal, boxMaxLocal;
castShape.getAabb( ref rotationXform, out boxMinLocal, out boxMaxLocal );
btVector3 rayAabbMinLocal = convexFromLocal;
rayAabbMinLocal.setMin( ref convexToLocal );
btVector3 rayAabbMaxLocal = convexFromLocal;
rayAabbMaxLocal.setMax( ref convexToLocal );
rayAabbMinLocal += boxMinLocal;
rayAabbMaxLocal += boxMaxLocal;
concaveShape.processAllTriangles( tccb, ref rayAabbMinLocal, ref rayAabbMaxLocal );
BulletGlobals.BridgeTriangleConvexcastCallbackPool.Free( tccb );
}
}
}
else
{
///@todo : use AABB tree or other BVH acceleration structure!
if( collisionShape.isCompound() )
{
CProfileSample sample = new CProfileSample( "convexSweepCompound" );
btCompoundShape compoundShape = (btCompoundShape)( collisionShape );
int i = 0;
for( i = 0; i < compoundShape.getNumChildShapes(); i++ )
{
//btTransform childTrans = compoundShape.getChildTransform( i );
btCollisionShape childCollisionShape = compoundShape.getChildShape( i );
btTransform childWorldTrans; colObjTransform.Apply( ref compoundShape.m_children.InternalArray[i].m_transform
, out childWorldTrans );
LocalInfoAdder my_cb = new LocalInfoAdder( i, resultCallback );
//btCollisionObjectWrapper tmpObj = BulletGlobals.CollisionObjectWrapperPool.Get();
//tmpObj.Initialize( colObjWrap, childCollisionShape, colObjWrap.m_collisionObject, ref childWorldTrans, -1, i );
objectQuerySingleInternal( castShape, ref convexFromTrans, ref convexToTrans
, childCollisionShape
, collisionObject
, ref childWorldTrans
, my_cb, allowedPenetration );
//BulletGlobals.CollisionObjectWrapperPool.Free( tmpObj );
}
}
}
}
}
public void setTimeStepAndCounters( double collisionMarginTriangle
, btDispatcherInfo dispatchInfo, btCollisionObjectWrapper convexBodyWrap
, ref btTransform convexTransform
, btCollisionObjectWrapper triBodyWrap
, ref btTransform triBodyTransform
, btManifoldResult resultOut )
{
m_convexBodyWrap = convexBodyWrap;
m_triBodyWrap = triBodyWrap;
m_dispatchInfoPtr = dispatchInfo;
m_collisionMarginTriangle = collisionMarginTriangle;
m_resultOut = resultOut;
//recalc aabbs
btTransform convexInTriangleSpace;
btTransform invTrans;
triBodyTransform.inverse( out invTrans );
invTrans.Apply( ref convexTransform, out convexInTriangleSpace );
btCollisionShape convexShape = m_convexBodyWrap.getCollisionShape();
//CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody.m_collisionShape);
convexShape.getAabb( ref convexInTriangleSpace, out m_aabbMin, out m_aabbMax );
double extraMargin = collisionMarginTriangle;
btVector3 extra = new btVector3( extraMargin );
m_aabbMax += extra;
m_aabbMin -= extra;
}
