public void Dispose()
{
CameraSphere.Dispose();
ConvexResultCallback.Dispose();
this.StandardCleanup();
}
public void Dispose()
{
CameraSphere.Dispose();
ConvexResultCallback.Dispose();
Character.Dispose();
this.StandardCleanup();
_ghostPairCallback.Dispose();
}
public void ConvexSweepTest(ConvexShape castShape, Matrix convexFromWorld,
Matrix convexToWorld, ConvexResultCallback resultCallback, double allowedCcdPenetration = 0)
{
btGhostObject_convexSweepTest(Native, castShape.Native, ref convexFromWorld,
ref convexToWorld, resultCallback.Native, allowedCcdPenetration);
}
public void ConvexSweepTest(ConvexShape castShape, Matrix convexFromWorld, Matrix convexToWorld, ConvexResultCallback resultCallback, float allowedCcdPenetration)
{
btGhostObject_convexSweepTest2(_native, castShape._native, ref convexFromWorld, ref convexToWorld, resultCallback._native, allowedCcdPenetration);
}
public void ConvexSweepTest(ConvexShape castShape, Matrix convexFromWorld, Matrix convexToWorld, ConvexResultCallback resultCallback)
{
btGhostObject_convexSweepTest(_native, castShape._native, ref convexFromWorld, ref convexToWorld, resultCallback._native);
}
public void ConvexSweepTest(ConvexShape castShape, ref IndexedMatrix convexFromWorld, ref IndexedMatrix convexToWorld, ConvexResultCallback resultCallback, float allowedCcdPenetration)
{
IndexedMatrix convexFromTrans = convexFromWorld;
IndexedMatrix convexToTrans = convexToWorld;
IndexedVector3 castShapeAabbMin;
IndexedVector3 castShapeAabbMax;
/* Compute AABB that encompasses angular movement */
IndexedVector3 linVel, angVel;
TransformUtil.CalculateVelocity(ref convexFromTrans, ref convexToTrans, 1.0f, out linVel, out angVel);
// FIXME MAN check this - should be a get/set rotation call, basis copy like this may break with scale?
IndexedMatrix R = IndexedMatrix.Identity;
R.SetRotation(convexFromTrans.GetRotation());
castShape.CalculateTemporalAabb(ref R, ref linVel, ref angVel, 1.0f, out castShapeAabbMin, out castShapeAabbMax);
/// go over all objects, and if the ray intersects their aabb + cast shape aabb,
// do a ray-shape query using convexCaster (CCD)
for (int i = 0; i < m_overlappingObjects.Count; i++)
{
CollisionObject collisionObject = m_overlappingObjects[i];
//only perform raycast if filterMask matches
if (resultCallback.NeedsCollision(collisionObject.GetBroadphaseHandle()))
{
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
IndexedVector3 collisionObjectAabbMin;
IndexedVector3 collisionObjectAabbMax;
IndexedMatrix t = collisionObject.GetWorldTransform();
collisionObject.GetCollisionShape().GetAabb(ref t, out collisionObjectAabbMin, out collisionObjectAabbMax);
AabbUtil2.AabbExpand(ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref castShapeAabbMin, ref castShapeAabbMax);
float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing
IndexedVector3 hitNormal;
if (AabbUtil2.RayAabb(convexFromWorld._origin, convexToWorld._origin, ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref hitLambda, out hitNormal))
{
IndexedMatrix wt = collisionObject.GetWorldTransform();
CollisionWorld.ObjectQuerySingle(castShape, ref convexFromTrans, ref convexToTrans,
collisionObject,
collisionObject.GetCollisionShape(),
ref wt,
resultCallback,
allowedCcdPenetration);
}
}
}
}
public unsafe static void ConvexSweepTest(this CollisionWorld obj, ConvexShape castShape, ref OpenTK.Matrix4 from, ref OpenTK.Matrix4 to, ConvexResultCallback resultCallback)
{
fixed(OpenTK.Matrix4 *fromPtr = &from)
{
fixed(OpenTK.Matrix4 *toPtr = &to)
{
obj.ConvexSweepTest(castShape, ref *(BulletSharp.Math.Matrix *)fromPtr, ref *(BulletSharp.Math.Matrix *)toPtr, resultCallback);
}
}
}
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 );
}
/// 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
}
internal LocalInfoAdder( int i, ConvexResultCallback user )
{
m_userCallback = ( user ); m_i = ( i );
m_closestHitFraction = m_userCallback.m_closestHitFraction;
}
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 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 static global::System.Runtime.InteropServices.HandleRef getCPtr(ConvexResultCallback obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
public LocalInfoAdder (int i, ConvexResultCallback user)
{
m_userCallback = user;
m_i = i;
}
public unsafe static void ConvexSweepTest(this GhostObject obj, ConvexShape castShape, ref OpenTK.Matrix4 convexFromWorld, ref OpenTK.Matrix4 convexToWorld, ConvexResultCallback resultCallback, float allowedCcdPenetration)
{
fixed(OpenTK.Matrix4 *convexFromWorldPtr = &convexFromWorld)
{
fixed(OpenTK.Matrix4 *convexToWorldPtr = &convexToWorld)
{
obj.ConvexSweepTest(castShape, ref *(BulletSharp.Math.Matrix *)convexFromWorldPtr, ref *(BulletSharp.Math.Matrix *)convexToWorldPtr, resultCallback, allowedCcdPenetration);
}
}
}
