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 static void rayTestSingleInternal( ref btTransform rayFromTrans, ref btTransform rayToTrans,
btCollisionShape collisionShape,
btCollisionObject collisionObject,
ref btTransform useTransform,
RayResultCallback resultCallback )
{
btSphereShape pointShape = BulletGlobals.SphereShapePool.Get();
pointShape.Initialize( btScalar.BT_ZERO );
pointShape.setMargin( 0f );
btConvexShape castShape = pointShape;
//btCollisionShape collisionShape = collisionObjectWrap.getCollisionShape();
btTransform colObjWorldTransform = useTransform;// collisionObjectWrap.m_worldTransform;
if( collisionShape.isConvex() )
{
// CProfileSample sample = new CProfileSample("rayTestConvex");
btConvexCast.CastResult castResult = new btConvexCast.CastResult();
castResult.m_fraction = resultCallback.m_closestHitFraction;
btConvexShape convexShape = (btConvexShape)collisionShape;
btVoronoiSimplexSolver simplexSolver = BulletGlobals.VoronoiSimplexSolverPool.Get();
//new btVoronoiSimplexSolver();
btSubsimplexConvexCast subSimplexConvexCaster = BulletGlobals.SubSimplexConvexCastPool.Get();
subSimplexConvexCaster.Initialize( castShape, convexShape, simplexSolver );
btGjkConvexCast gjkConvexCaster = BulletGlobals.GjkConvexCastPool.Get();
gjkConvexCaster.Initialize( castShape, convexShape, simplexSolver );
BulletGlobals.SubSimplexConvexCastPool.Free( subSimplexConvexCaster );
BulletGlobals.VoronoiSimplexSolverPool.Free( simplexSolver );
//btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
btConvexCast convexCasterPtr = null;
//use kF_UseSubSimplexConvexCastRaytest by default
if( ( resultCallback.m_flags & (uint)btTriangleRaycastCallback.EFlags.kF_UseGjkConvexCastRaytest ) != 0 )
convexCasterPtr = gjkConvexCaster;
else
convexCasterPtr = subSimplexConvexCaster;
btConvexCast convexCaster = convexCasterPtr;
if( convexCaster.calcTimeOfImpact( ref rayFromTrans, ref rayToTrans
, ref useTransform/*collisionObjectWrap.m_worldTransform*/
, ref useTransform/*collisionObjectWrap.m_worldTransform*/
, castResult ) )
{
//add hit
if( castResult.m_normal.length2() > (double)( 0.0001 ) )
{
if( castResult.m_fraction < resultCallback.m_closestHitFraction )
{
//todo: figure out what this is about. When is rayFromTest.getBasis() not identity?
#if USE_SUBSIMPLEX_CONVEX_CAST
//rotate normal into worldspace
castResult.m_normal = rayFromTrans.getBasis() castResult.m_normal;
#endif //USE_SUBSIMPLEX_CONVEX_CAST
castResult.m_normal.normalize();
LocalRayResult localRayResult = new LocalRayResult
(
collisionObject,
null,
castResult.m_normal,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.addSingleResult( localRayResult, normalInWorldSpace );
}
}
}
BulletGlobals.GjkConvexCastPool.Free( gjkConvexCaster );
}
else
{
if( collisionShape.isConcave() )
{
btTransform worldTocollisionObject; colObjWorldTransform.inverse( out worldTocollisionObject );
btVector3 tmp;
rayFromTrans.getOrigin( out tmp );
btVector3 rayFromLocal; worldTocollisionObject.Apply( ref tmp, out rayFromLocal );
rayToTrans.getOrigin( out tmp );
btVector3 rayToLocal; worldTocollisionObject.Apply( ref tmp, out rayToLocal );
// CProfileSample sample = new CProfileSample("rayTestConcave");
#if SUPPORT_TRIANGLE_MESH
if( collisionShape.getShapeType() == BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE )
{
///optimized version for btBvhTriangleMeshShape
btBvhTriangleMeshShape triangleMesh = (btBvhTriangleMeshShape)collisionShape;
BridgeTriangleRaycastCallback rcb( rayFromLocal, rayToLocal,&resultCallback, collisionObjectWrap.getCollisionObject(), triangleMesh, colObjWorldTransform);
rcb.m_hitFraction = resultCallback.m_closestHitFraction;
triangleMesh.performRaycast( &rcb, rayFromLocal, rayToLocal );
}
else
#endif
{
//generic (slower) case
btConcaveShape concaveShape = (btConcaveShape)collisionShape;
//ConvexCast::CastResult
BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback( ref rayFromLocal, ref rayToLocal, resultCallback
, collisionObject, concaveShape, ref colObjWorldTransform );
rcb.m_hitFraction = resultCallback.m_closestHitFraction;
btVector3 rayAabbMinLocal = rayFromLocal;
rayAabbMinLocal.setMin( ref rayToLocal );
btVector3 rayAabbMaxLocal = rayFromLocal;
rayAabbMaxLocal.setMax( ref rayToLocal );
concaveShape.processAllTriangles( rcb, ref rayAabbMinLocal, ref rayAabbMaxLocal );
}
}
else
{
// CProfileSample sample = new CProfileSample("rayTestCompound");
if( collisionShape.isCompound() )
{
btCompoundShape compoundShape = (btCompoundShape)( collisionShape );
btDbvt dbvt = compoundShape.getDynamicAabbTree();
RayTester rayCB = new RayTester(
collisionObject,
compoundShape,
ref colObjWorldTransform,
ref rayFromTrans,
ref rayToTrans,
resultCallback );
#if !DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
if( dbvt != null )
{
btTransform tmp;
colObjWorldTransform.inverseTimes( ref rayFromTrans, out tmp );
btVector3 localRayFrom; tmp.getOrigin( out localRayFrom );
colObjWorldTransform.inverseTimes( ref rayToTrans, out tmp );
btVector3 localRayTo; tmp.getOrigin( out localRayTo );
btDbvt.rayTest( dbvt.m_root, ref localRayFrom, ref localRayTo, rayCB );
}
else
#endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
{
for( int i = 0, n = compoundShape.getNumChildShapes(); i < n; ++i )
{
rayCB.Process( i );
}
}
}
}
}
BulletGlobals.SphereShapePool.Free( pointShape );
}