// this is a
internal override bool calcPenDepth( btSimplexSolverInterface simplexSolver,
btConvexShape pConvexA, btConvexShape pConvexB,
ref btTransform transformA, ref btTransform transformB,
ref btVector3 v, out btVector3 wWitnessOnA, out btVector3 wWitnessOnB,
btIDebugDraw debugDraw )
{
//(void)debugDraw;
//(void)v;
//(void)simplexSolver;
// double radialmargin(btScalar.BT_ZERO);
btVector3 guessVector; transformB.m_origin.Sub( ref transformA.m_origin, out guessVector );
btGjkEpaSolver2.sResults results;
if( btGjkEpaSolver2.Penetration( pConvexA, ref transformA,
pConvexB, ref transformB,
ref guessVector, out results ) )
{
// debugDraw.drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
//resultOut.addContactPoint(results.normal,results.witnesses[1],-results.depth);
wWitnessOnA = results.witness0;
wWitnessOnB = results.witness1;
v = results.normal;
return true;
}
else
{
if( btGjkEpaSolver2.Distance( pConvexA, ref transformA, pConvexB, ref transformB, ref guessVector, out results ) )
{
wWitnessOnA = results.witness0;
wWitnessOnB = results.witness1;
v = results.normal;
return false;
}
}
wWitnessOnA = results.witness0;
wWitnessOnB = results.witness1;
return false;
}
//
public static double SignedDistance( ref btVector3 position,
double margin,
btConvexShape shape0,
ref btTransform wtrs0,
sResults results )
{
tShape shape = new tShape();
using( btSphereShape shape1 = BulletGlobals.SphereShapePool.Get() )
{
shape1.Initialize( margin );
btTransform wtrs1 = new btTransform( ref btQuaternion.Zero, ref position );
Initialize( shape0, ref wtrs0, shape1, ref wtrs1, out results, shape, false );
GJK gjk = new GJK();
GJK.eStatus._ gjk_status = gjk.Evaluate( shape, ref btVector3.One );
if( gjk_status == GJK.eStatus._.Valid )
{
btVector3 w0 = btVector3.Zero;
btVector3 w1 = btVector3.Zero;
for( uint i = 0; i < gjk.m_simplex.rank; ++i )
{
double p = gjk.m_simplex.p[i];
btVector3 tmp;
shape.Support( ref gjk.m_simplex.c[i].d, 0, out tmp );
w0.AddScale( ref tmp, p, out w0 );
btVector3 tmp2;
gjk.m_simplex.c[i].d.Invert( out tmp2 );
shape.Support( ref tmp2, 1, out tmp );
w1.AddScale( ref tmp, p, out w1 );
}
wtrs0.Apply( ref w0, out results.witness0 );
wtrs0.Apply( ref w1, out results.witness1 );
btVector3 delta; results.witness1.Sub( ref results.witness0, out delta );
margin = shape0.getMarginNonVirtual() +
shape1.getMarginNonVirtual();
double length = delta.length();
delta.Div( length, out results.normal );
results.witness0.AddScale( ref results.normal, margin, out results.witness0 );
return ( length - margin );
}
else
{
if( gjk_status == GJK.eStatus._.Inside )
{
if( Penetration( shape0, ref wtrs0, shape1, ref wtrs1, ref gjk.m_ray, out results ) )
{
btVector3 delta; results.witness0.Sub(
ref results.witness1, out delta );
double length = delta.length();
if( length >= btScalar.SIMD_EPSILON )
delta.Div( length, out results.normal );
return ( -length );
}
}
}
}
return ( btScalar.SIMD_INFINITY );
}
public btContinuousConvexCollision( btConvexShape convexA, btStaticPlaneShape plane )
{
m_simplexSolver = ( null );
m_penetrationDepthSolver = ( null );
m_convexA = ( convexA ); m_convexB1 = ( null ); m_planeShape = ( plane );
}
internal void Initialize( btConvexShape convexA, btConvexShape convexB, btSimplexSolverInterface simplexSolver, btConvexPenetrationDepthSolver penetrationDepthSolver )
{
m_simplexSolver = ( simplexSolver );
m_penetrationDepthSolver = ( penetrationDepthSolver );
m_convexA = ( convexA ); m_convexB1 = ( convexB ); m_planeShape = null;
}
internal void Initialize( btConvexShape convexA, btStaticPlaneShape plane )
{
m_simplexSolver = ( null );
m_penetrationDepthSolver = ( null );
m_convexA = ( convexA ); m_convexB1 = ( null ); m_planeShape = ( plane );
}
internal void Initialize( btConvexShape convexShape, ref btTransform convexShapeFrom, ref btTransform convexShapeTo, ref btTransform triangleToWorld, double triangleCollisionMargin )
{
m_convexShape = convexShape;
m_convexShapeFrom = convexShapeFrom;
m_convexShapeTo = convexShapeTo;
m_triangleToWorld = triangleToWorld.T;
m_hitFraction = 1.0f;
m_triangleCollisionMargin = triangleCollisionMargin;
m_allowedPenetration = 0;
}
///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation. internal abstract bool calcPenDepth( btSimplexSolverInterface simplexSolver, btConvexShape convexA, btConvexShape convexB, ref btTransform transA, ref btTransform transB, ref btVector3 v, out btVector3 pa, out btVector3 pb, btIDebugDraw debugDraw );
//
// Api
//
//
//
public static bool Distance( btConvexShape shape0,
ref btTransform wtrs0,
btConvexShape shape1,
ref btTransform wtrs1,
ref btVector3 guess,
out sResults results )
{
tShape shape = new tShape();
results.witness0 =
results.witness1 = btVector3.Zero;
results.status = btGjkEpaSolver2.sResults.eStatus.Separated;
Initialize( shape0, ref wtrs0, shape1, ref wtrs1, out results, shape, false );
GJK gjk = new GJK();
GJK.eStatus._ gjk_status = gjk.Evaluate( shape, ref guess );
if( gjk_status == GJK.eStatus._.Valid )
{
btVector3 w0 = btVector3.Zero;
btVector3 w1 = btVector3.Zero;
for( uint i = 0; i < gjk.m_simplex.rank; ++i )
{
double p = gjk.m_simplex.p[i];
btVector3 tmp;
shape.Support( ref gjk.m_simplex.c[i].d, 0, out tmp );
w0.AddScale( ref tmp, p, out w0 );
gjk.m_simplex.c[i].d.Invert( out tmp );
shape.Support( ref tmp, 1, out tmp );
w1.AddScale( ref tmp, p, out w1 );
}
wtrs0.Apply( ref w0, out results.witness0 );
wtrs0.Apply( ref w1, out results.witness1 );
w0.Sub( ref w1, out results.normal );
results.distance = results.normal.length();
results.normal.Div( ( results.distance > GJK_MIN_DISTANCE ) ? results.distance : 1, out results.normal );
return ( true );
}
else
{
results.status = gjk_status == GJK.eStatus._.Inside
? sResults.eStatus.Penetrating
: sResults.eStatus.GJK_Failed;
return ( false );
}
}
public static void objectQuerySingle( btConvexShape castShape, ref btTransform convexFromTrans, ref btTransform convexToTrans,
btCollisionObject collisionObject,
btCollisionShape collisionShape,
ref btTransform colObjWorldTransform,
ConvexResultCallback resultCallback, double allowedPenetration )
{
//btCollisionObjectWrapper tmpOb = BulletGlobals.CollisionObjectWrapperPool.Get();
//tmpOb.Initialize( null, collisionShape, collisionObject, ref colObjWorldTransform, -1, -1 );
objectQuerySingleInternal( castShape, ref convexFromTrans, ref convexToTrans
, collisionShape, collisionObject
, ref colObjWorldTransform, resultCallback, allowedPenetration );
//BulletGlobals.CollisionObjectWrapperPool.Free( tmpOb );
}
internal void Initialize( btConvexShape castShape, ref btTransform from, ref btTransform to,
ConvexResultCallback resultCallback, btCollisionObject collisionObject
, btConcaveShape triangleMesh, ref btTransform triangleToWorld )
{
base.Initialize( castShape, ref from, ref to, ref triangleToWorld, triangleMesh.getMargin() );
m_resultCallback = ( resultCallback );
m_collisionObject = ( collisionObject );
m_triangleMesh = ( triangleMesh );
}
internal void Initialize( btConvexShape objectA, btConvexShape objectB, btSimplexSolverInterface simplexSolver
, btConvexPenetrationDepthSolver penetrationDepthSolver )
{
m_cachedSeparatingAxis = btVector3.yAxis;
m_penetrationDepthSolver = ( penetrationDepthSolver );
m_simplexSolver = ( simplexSolver );
m_minkowskiA = ( objectA );
m_minkowskiB = ( objectB );
m_shapeTypeA = ( objectA.getShapeType() );
m_shapeTypeB = ( objectB.getShapeType() );
m_marginA = ( objectA.getMargin() );
m_marginB = ( objectB.getMargin() );
m_ignoreMargin = ( false );
m_lastUsedMethod = ( -1 );
m_catchDegeneracies = ( 1 );
m_fixContactNormalDirection = ( 1 );
}
public void setMinkowskiB( btConvexShape minkB )
{
m_minkowskiB = minkB;
}
public void setMinkowskiA( btConvexShape minkA )
{
m_minkowskiA = minkA;
}
public virtual bool calcPenDepth( btSimplexSolverInterface simplexSolver,
btConvexShape convexA, btConvexShape convexB,
ref btTransform transA, ref btTransform transB,
ref btVector3 v, ref btVector3 pa, ref btVector3 pb,
btIDebugDraw debugDraw
)
{
//(void)v;
bool check2d = convexA.isConvex2d() && convexB.isConvex2d();
//just take fixed number of orientation, and sample the penetration depth in that direction
double minProj = btScalar.BT_LARGE_FLOAT;
btVector3 minNorm = btVector3.Zero;
btVector3 minA, minB;
btVector3 seperatingAxisInA, seperatingAxisInB;
btVector3 pInA, qInB, pWorld, qWorld, w;
#if USE_BATCHED_SUPPORT
btVector3[] supportVerticesABatch = new btVector3[NUM_UNITSPHERE_POINTS + btConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
btVector3[] supportVerticesBBatch = new btVector3[NUM_UNITSPHERE_POINTS + btConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
btVector3[] seperatingAxisInABatch = new btVector3[NUM_UNITSPHERE_POINTS + btConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
btVector3[] seperatingAxisInBBatch = new btVector3[NUM_UNITSPHERE_POINTS + btConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
int i;
int numSampleDirections = NUM_UNITSPHERE_POINTS;
for( i = 0; i < numSampleDirections; i++ )
{
btVector3 norm = getPenetrationDirections()[i];
seperatingAxisInABatch[i] = ( -norm ) * transA.getBasis();
seperatingAxisInBBatch[i] = norm * transB.getBasis();
}
{
int numPDA = convexA.getNumPreferredPenetrationDirections();
if( numPDA )
{
for( int i = 0; i < numPDA; i++ )
{
btVector3 norm;
convexA.getPreferredPenetrationDirection( i, norm );
norm = transA.getBasis() * norm;
getPenetrationDirections()[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = ( -norm ) * transA.getBasis();
seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
numSampleDirections++;
}
}
}
{
int numPDB = convexB.getNumPreferredPenetrationDirections();
if( numPDB )
{
for( int i = 0; i < numPDB; i++ )
{
btVector3 norm;
convexB.getPreferredPenetrationDirection( i, norm );
norm = transB.getBasis() * norm;
getPenetrationDirections()[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = ( -norm ) * transA.getBasis();
seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
numSampleDirections++;
}
}
}
convexA.batchedUnitVectorGetSupportingVertexWithoutMargin( seperatingAxisInABatch, supportVerticesABatch, numSampleDirections );
convexB.batchedUnitVectorGetSupportingVertexWithoutMargin( seperatingAxisInBBatch, supportVerticesBBatch, numSampleDirections );
for( i = 0; i < numSampleDirections; i++ )
{
btVector3 norm = getPenetrationDirections()[i];
if( check2d )
{
norm[2] = 0;
}
if( norm.length2() > 0.01 )
{
seperatingAxisInA = seperatingAxisInABatch[i];
seperatingAxisInB = seperatingAxisInBBatch[i];
pInA = supportVerticesABatch[i];
qInB = supportVerticesBBatch[i];
pWorld = transA( pInA );
qWorld = transB( qInB );
if( check2d )
{
pWorld[2] = 0;
qWorld[2] = 0;
}
w = qWorld - pWorld;
double delta = norm.dot( w );
//find smallest delta
if( delta < minProj )
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
}
#else
int numSampleDirections = NUM_UNITSPHERE_POINTS;
#if !__SPU__
{
int numPDA = convexA.getNumPreferredPenetrationDirections();
if (numPDA)
{
for (int i=0;i<numPDA;i++)
{
btVector3 norm;
convexA.getPreferredPenetrationDirection(i,norm);
norm = transA.getBasis() * norm;
getPenetrationDirections()[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
{
int numPDB = convexB.getNumPreferredPenetrationDirections();
if (numPDB)
{
for (int i=0;i<numPDB;i++)
{
btVector3 norm;
convexB.getPreferredPenetrationDirection(i,norm);
norm = transB.getBasis() * norm;
getPenetrationDirections()[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
#endif // __SPU__
for (int i=0;i<numSampleDirections;i++)
{
ref btVector3 norm = getPenetrationDirections()[i];
seperatingAxisInA = (-norm)* transA.getBasis();
seperatingAxisInB = norm* transB.getBasis();
pInA = convexA.localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
qInB = convexB.localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
pWorld = transA(pInA);
qWorld = transB(qInB);
w = qWorld - pWorld;
double delta = norm.dot(w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
//
public static bool SignedDistance( btConvexShape shape0,
ref btTransform wtrs0,
btConvexShape shape1,
ref btTransform wtrs1,
ref btVector3 guess,
out sResults results )
{
if( !Distance( shape0, ref wtrs0, shape1, ref wtrs1, ref guess, out results ) )
return ( Penetration( shape0, ref wtrs0, shape1, ref wtrs1, ref guess, out results, false ) );
else
{
return ( true );
}
}
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 );
}
}
}
}
}
//
static void Initialize( btConvexShape shape0, ref btTransform wtrs0,
btConvexShape shape1, ref btTransform wtrs1,
out btGjkEpaSolver2.sResults results,
tShape shape,
bool withmargins )
{
/* Results */
results.normal = btVector3.xAxis;
results.witness0 =
results.witness1 = btVector3.Zero;
results.status = btGjkEpaSolver2.sResults.eStatus.Separated;
results.distance = 0;
/* Shape */
shape.m_shape0 = shape0;
shape.m_shape1 = shape1;
wtrs1.m_basis.transposeTimes( ref wtrs0.m_basis, out shape.m_toshape1 );
wtrs0.inverseTimes( ref wtrs1, out shape.m_toshape0 );
shape.EnableMargin( withmargins );
}
internal void Initialize( btConvexShape castShape, ref btTransform convexFromTrans, ref btTransform convexToTrans, btCollisionWorld world, ConvexResultCallback resultCallback, double allowedPenetration )
{
m_convexFromTrans = ( convexFromTrans );
m_convexToTrans = ( convexToTrans );
m_world = ( world );
m_resultCallback = ( resultCallback );
m_allowedCcdPenetration = ( allowedPenetration );
m_castShape = ( castShape );
btVector3 unnormalizedRayDir; m_convexToTrans.m_origin.Sub( ref m_convexFromTrans.m_origin, out unnormalizedRayDir );
btVector3 rayDir; unnormalizedRayDir.normalized( out rayDir );
///what about division by zero? -. just set rayDirection[i] to INF/BT_LARGE_FLOAT
m_rayDirectionInverse[0] = rayDir[0] == btScalar.BT_ZERO ? btScalar.BT_LARGE_FLOAT : (double)( 1.0 ) / rayDir[0];
m_rayDirectionInverse[1] = rayDir[1] == btScalar.BT_ZERO ? btScalar.BT_LARGE_FLOAT : (double)( 1.0 ) / rayDir[1];
m_rayDirectionInverse[2] = rayDir[2] == btScalar.BT_ZERO ? btScalar.BT_LARGE_FLOAT : (double)( 1.0 ) / rayDir[2];
m_signs[0] = m_rayDirectionInverse[0] < 0.0 ? 1U : 0;
m_signs[1] = m_rayDirectionInverse[1] < 0.0 ? 1U : 0;
m_signs[2] = m_rayDirectionInverse[2] < 0.0 ? 1U : 0;
m_lambda_max = rayDir.dot( unnormalizedRayDir );
}
//
public static bool Penetration( btConvexShape shape0,
ref btTransform wtrs0,
btConvexShape shape1,
ref btTransform wtrs1,
ref btVector3 guess,
out sResults results,
bool usemargins = false )
{
tShape shape = new tShape();
Initialize( shape0, ref wtrs0, shape1, ref wtrs1, out results, shape, usemargins );
GJK gjk = new GJK();
btVector3 tmp;
guess.Invert( out tmp );
GJK.eStatus._ gjk_status = gjk.Evaluate( shape, ref tmp );
switch( gjk_status )
{
case GJK.eStatus._.Inside:
{
EPA epa = new EPA();
EPA.eStatus._ epa_status = epa.Evaluate( gjk, ref tmp );
if( epa_status != EPA.eStatus._.Failed )
{
btVector3 w0 = btVector3.Zero;
for( uint i = 0; i < epa.m_result.rank; ++i )
{
shape.Support( ref epa.m_result.c[i].d, 0, out tmp );
w0.AddScale( ref tmp, epa.m_result.p[i], out w0 );
}
results.status = sResults.eStatus.Penetrating;
wtrs0.Apply( ref w0, out results.witness0 );
w0.SubScale( ref epa.m_normal, epa.m_depth, out tmp );
wtrs0.Apply( ref tmp, out results.witness1 );
epa.m_normal.Invert( out results.normal );
results.distance = -epa.m_depth;
return ( true );
}
else results.status = sResults.eStatus.EPA_Failed;
}
break;
case GJK.eStatus._.Failed:
results.status = sResults.eStatus.GJK_Failed;
break;
default:
break;
}
return ( false );
}
/// convexTest performs a swept convex cast on all objects in the btCollisionWorld, and calls the resultCallback
/// This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback.
internal void convexSweepTest( btConvexShape castShape, ref btTransform convexFromWorld, ref btTransform convexToWorld, ConvexResultCallback resultCallback, double allowedCcdPenetration = btScalar.BT_ZERO )
{
CProfileSample sample = new CProfileSample( "convexSweepTest" );
/// use the broadphase to accelerate the search for objects, based on their aabb
/// and for each object with ray-aabb overlap, perform an exact ray test
/// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical
btTransform convexFromTrans, convexToTrans;
convexFromTrans = convexFromWorld;
convexToTrans = convexToWorld;
btVector3 castShapeAabbMin, castShapeAabbMax;
/* Compute AABB that encompasses angular movement */
{
btVector3 linVel, angVel;
btTransformUtil.calculateVelocity( ref convexFromWorld, ref convexToWorld, 1.0f, out linVel, out angVel );
btVector3 zeroLinVel = btVector3.Zero;
btTransform R = btTransform.Identity;
R.m_basis = convexFromWorld.m_basis;
castShape.calculateTemporalAabb( ref R, ref zeroLinVel, ref angVel, 1.0f, out castShapeAabbMin, out castShapeAabbMax );
}
#if !USE_BRUTEFORCE_RAYBROADPHASE
btSingleSweepCallback convexCB = BulletGlobals.SingleSweepCallbackPool.Get();
convexCB.Initialize( castShape, ref convexFromWorld, ref convexToWorld, this, resultCallback, allowedCcdPenetration );
m_broadphasePairCache.rayTest( ref convexFromTrans.m_origin, ref convexToTrans.m_origin, convexCB, ref castShapeAabbMin, ref castShapeAabbMax );
BulletGlobals.SingleSweepCallbackPool.Free( convexCB );
#else
/// go over all objects, and if the ray intersects their aabb + cast shape aabb,
// do a ray-shape query using convexCaster (CCD)
int i;
for( i = 0; i < m_collisionObjects.Count; i++ )
{
btCollisionObject collisionObject = m_collisionObjects[i];
//only perform raycast if filterMask matches
if( resultCallback.needsCollision( collisionObject.getBroadphaseHandle() ) )
{
//RigidcollisionObject collisionObject = ctrl.GetRigidcollisionObject();
btVector3 collisionObjectAabbMin, collisionObjectAabbMax;
collisionObject.getCollisionShape().getAabb( collisionObject.getWorldTransform(), collisionObjectAabbMin, collisionObjectAabbMax );
AabbExpand( collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax );
double hitLambda = (double)( 1.0 ); //could use resultCallback.m_closestHitFraction, but needs testing
btVector3 hitNormal;
if( btRayAabb( convexFromWorld.getOrigin(), convexToWorld.getOrigin(), collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal ) )
{
objectQuerySingle( castShape, convexFromTrans, convexToTrans,
collisionObject,
collisionObject.getCollisionShape(),
collisionObject.getWorldTransform(),
resultCallback,
allowedCcdPenetration );
}
}
}
#endif //USE_BRUTEFORCE_RAYBROADPHASE
}
public void Initialize(btConvexShape convexA, btConvexShape convexB, btSimplexSolverInterface simplexSolver)
{
m_simplexSolver = ( simplexSolver );
m_convexA = ( convexA );
m_convexB = ( convexB );
}
///////////////////////////
///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
public btConvex2dShape( btConvexShape convexChildShape )
{
m_childConvexShape = ( convexChildShape );
m_shapeType = BroadphaseNativeTypes.CONVEX_2D_SHAPE_PROXYTYPE;
}
