public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (_manifold == null)
return;
SphereShape sphereA = bodyA.CollisionShape as SphereShape;
SphereShape sphereB = bodyB.CollisionShape as SphereShape;
Vector3 diff = bodyA.WorldTransform.Translation - bodyB.WorldTransform.Translation;
float len = diff.Length();
float radiusA = sphereA.Radius;
float radiusB = sphereB.Radius;
//if distance positive, don't generate a new contact
if (len > (radiusA + radiusB))
return;
//distance (negative means penetration)
float dist = len - (radiusA + radiusB);
Vector3 normalOnSurfaceB = diff / len;
//point on A (worldspace)
Vector3 posA = bodyA.WorldTransform.Translation - radiusA * normalOnSurfaceB;
//point on B (worldspace)
Vector3 posB = bodyB.WorldTransform.Translation + radiusB * normalOnSurfaceB;
// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.SetPersistentManifold(_manifold);
resultOut.AddContactPoint(normalOnSurfaceB, posB, dist);
}
public ManifoldResult(CollisionObject bodyA, CollisionObject bodyB)
{
_bodyA = bodyA;
_bodyB = bodyB;
_rootTransA = bodyA.WorldTransform;
_rootTransB = bodyB.WorldTransform;
}
public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
CollisionObject convexBody = _isSwapped ? bodyB : bodyA;
CollisionObject triBody = _isSwapped ? bodyA : bodyB;
if (triBody.CollisionShape.IsConcave)
{
CollisionObject triOb = triBody;
ConcaveShape concaveShape = triOb.CollisionShape as ConcaveShape;
if (convexBody.CollisionShape.IsConvex)
{
float collisionMarginTriangle = concaveShape.Margin;
resultOut.SetPersistentManifold(_convexTriangleCallback.Manifold);
_convexTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut);
//Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
//m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr);
_convexTriangleCallback.Manifold.SetBodies(convexBody, triBody);
concaveShape.ProcessAllTriangles(_convexTriangleCallback, _convexTriangleCallback.AabbMin, _convexTriangleCallback.AabbMax);
}
}
}
public override void ProcessCollision(
CollisionObject bodyA,
CollisionObject bodyB,
DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//Begin
CollisionObject collisionObject = _isSwapped ? bodyB : bodyB;
CollisionObject otherObject = _isSwapped ? bodyA : bodyB;
//Debug.Assert(collisionObject.getCollisionShape().isCompound());
BulletDebug.Assert(collisionObject.CollisionShape.IsCompound);
CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape;
int childrenNumber = _childCollisionAlgorithms.Count;
for (int i = 0; i < childrenNumber; i++)
{
CompoundShape childShape = compoundShape.GetChildShape(i) as CompoundShape;
Matrix orgTransform = collisionObject.WorldTransform;
CollisionShape orgShape = collisionObject.CollisionShape;
Matrix childTransform = compoundShape.GetChildTransform(i);
Matrix newChildWorld = orgTransform * childTransform;
collisionObject.WorldTransform = newChildWorld;
collisionObject.CollisionShape = childShape;
_childCollisionAlgorithms[i].ProcessCollision(collisionObject, otherObject, dispatchInfo, resultOut);
collisionObject.CollisionShape = orgShape;
collisionObject.WorldTransform = orgTransform;
}
}
public CompoundCollisionAlgorithm(
CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo,
CollisionObject bodyA,
CollisionObject bodyB, bool isSwapped)
: base(collisionAlgorithmConstructionInfo)
{
//Begin
_isSwapped = isSwapped;
CollisionObject collisionObject = isSwapped ? bodyB : bodyA;
CollisionObject otherObject = isSwapped ? bodyA : bodyB;
BulletDebug.Assert(collisionObject.CollisionShape.IsCompound);
CompoundShape compoundShape = collisionObject.CollisionShape as CompoundShape;
int childrenNumber = compoundShape.ChildShapeCount;
int index = 0;
_childCollisionAlgorithms = new List<CollisionAlgorithm>(childrenNumber);
for (index = 0; index < childrenNumber; index++)
{
CollisionShape childShape = compoundShape.GetChildShape(index);
CollisionShape orgShape = collisionObject.CollisionShape;
collisionObject.CollisionShape = childShape;
_childCollisionAlgorithms[index] = collisionAlgorithmConstructionInfo.Dispatcher.FindAlgorithm(collisionObject, otherObject);
collisionObject.CollisionShape = orgShape;
}
}
public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (_manifold == null)
{
//swapped?
_manifold = Dispatcher.GetNewManifold(bodyA, bodyB);
_ownManifold = true;
}
resultOut.SetPersistentManifold(_manifold);
ConvexShape min0 = bodyA.CollisionShape as ConvexShape;
ConvexShape min1 = bodyB.CollisionShape as ConvexShape;
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
//TODO: if (dispatchInfo.m_useContinuous)
_gjkPairDetector.setMinkowskiA(min0);
_gjkPairDetector.setMinkowskiB(min1);
input.MaximumDistanceSquared = min0.Margin + min1.Margin + PersistentManifold.ContactBreakingThreshold;
input.MaximumDistanceSquared *= input.MaximumDistanceSquared;
// input.m_maximumDistanceSquared = 1e30f;
input.TransformA = bodyA.WorldTransform;
input.TransformB = bodyB.WorldTransform;
_gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.DebugDraw);
}
public ConvexConvexAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
: base(collisionAlgorithmConstructionInfo)
{
_gjkPairDetector = new GjkPairDetector(null, null, simplexSolver, penetrationDepthSolver);
_ownManifold = false;
_manifold = manifold;
_lowLevelOfDetail = false;
}
public BridgeTriangleRaycastCallback(Vector3 from, Vector3 to,
CollisionWorld.RayResultCallback resultCallback, CollisionObject collisionObject, TriangleMeshShape triangleMesh)
: base(from, to)
{
_resultCallback = resultCallback;
_collisionObject = collisionObject;
_triangleMesh = triangleMesh;
}
public ConvexTriangleCallback(IDispatcher dispatcher, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped)
{
_dispatcher = dispatcher;
_dispatchInfo = null;
_convexBody = isSwapped ? bodyB : bodyA;
_triBody = isSwapped ? bodyA : bodyB;
// create the manifold from the dispatcher 'manifold pool'
_manifold = _dispatcher.GetNewManifold(_convexBody, _triBody);
ClearCache();
}
public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
CollisionObject convexbody = _isSwapped ? bodyB : bodyA;
CollisionObject triBody = _isSwapped ? bodyA : bodyB;
//quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
//only perform CCD above a certain threshold, this prevents blocking on the long run
//because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
float squareMot0 = (convexbody.InterpolationWorldTransform.Translation - convexbody.WorldTransform.Translation).LengthSquared();
if (squareMot0 < convexbody.CcdSquareMotionThreshold)
{
return 1;
}
Matrix triInv = MathHelper.InvertMatrix(triBody.WorldTransform);
Matrix convexFromLocal = triInv * convexbody.WorldTransform;
Matrix convexToLocal = triInv * convexbody.InterpolationWorldTransform;
if (triBody.CollisionShape.IsConcave)
{
Vector3 rayAabbMin = convexFromLocal.Translation;
MathHelper.SetMin(ref rayAabbMin, convexToLocal.Translation);
Vector3 rayAabbMax = convexFromLocal.Translation;
MathHelper.SetMax(ref rayAabbMax, convexToLocal.Translation);
float ccdRadius0 = convexbody.CcdSweptSphereRadius;
rayAabbMin -= new Vector3(ccdRadius0, ccdRadius0, ccdRadius0);
rayAabbMax += new Vector3(ccdRadius0, ccdRadius0, ccdRadius0);
float curHitFraction = 1f; //is this available?
LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(convexFromLocal, convexToLocal,
convexbody.CcdSweptSphereRadius, curHitFraction);
raycastCallback.HitFraction = convexbody.HitFraction;
CollisionObject concavebody = triBody;
ConcaveShape triangleMesh = concavebody.CollisionShape as ConcaveShape;
if (triangleMesh != null)
{
triangleMesh.ProcessAllTriangles(raycastCallback, rayAabbMin, rayAabbMax);
}
if (raycastCallback.HitFraction < convexbody.HitFraction)
{
convexbody.HitFraction = raycastCallback.HitFraction;
return raycastCallback.HitFraction;
}
}
return 1;
}
public SphereSphereCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
: base(collisionAlgorithmConstructionInfo)
{
_ownManifold = false;
_manifold = manifold;
if (_manifold == null)
{
_manifold = Dispatcher.GetNewManifold(bodyA, bodyB);
_ownManifold = true;
}
}
public SphereBoxCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject collisionObjectA, CollisionObject collisionObjectB, bool isSwapped)
: base(collisionAlgorithmConstructionInfo)
{
_ownManifold = false;
_manifold = manifold;
_isSwapped = isSwapped;
CollisionObject sphereObject = _isSwapped ? collisionObjectB : collisionObjectA;
CollisionObject boxObject = _isSwapped ? collisionObjectA : collisionObjectB;
if (_manifold == null && Dispatcher.NeedsCollision(sphereObject, boxObject))
{
_manifold = Dispatcher.GetNewManifold(sphereObject, boxObject);
_ownManifold = true;
}
}
public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (_manifold == null)
return;
SphereShape sphere = bodyA.CollisionShape as SphereShape;
TriangleShape triangle = bodyB.CollisionShape as TriangleShape;
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.SetPersistentManifold(_manifold);
SphereTriangleDetector detector = new SphereTriangleDetector(sphere, triangle);
DiscreteCollisionDetectorInterface.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
input.MaximumDistanceSquared = 1e30f;//todo: tighter bounds
input.TransformA = bodyA.WorldTransform;
input.TransformB = bodyB.WorldTransform;
detector.GetClosestPoints(input, resultOut, null);
}
public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
CollisionObject collisionObject = _isSwapped ? bodyB : bodyA;
CollisionObject otherObject = _isSwapped ? bodyA : bodyB;
BulletDebug.Assert(collisionObject.CollisionShape.IsCompound);
CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape;
float hitFraction = 1.0f;
for (int i = 0; i < _childCollisionAlgorithms.Count; i++)
{
CollisionShape childShape = compoundShape.GetChildShape(i);
Matrix orgTransform = collisionObject.WorldTransform;
CollisionShape orgShape = collisionObject.CollisionShape;
Matrix childTransform = compoundShape.GetChildTransform(i);
Matrix newChildWorld = orgTransform * childTransform;
collisionObject.WorldTransform = newChildWorld;
collisionObject.CollisionShape = childShape;
float frac = _childCollisionAlgorithms[i].CalculateTimeOfImpact(
collisionObject, otherObject, dispatchInfo, resultOut
);
if (frac < hitFraction)
{
hitFraction = frac;
}
collisionObject.CollisionShape = orgShape;
collisionObject.WorldTransform = orgTransform;
}
return hitFraction;
}
public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB, PersistentManifold sharedManifold)
{
CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo = new CollisionAlgorithmConstructionInfo();
collisionAlgorithmConstructionInfo.Dispatcher = this;
collisionAlgorithmConstructionInfo.Manifold = sharedManifold;
CollisionAlgorithm collisionAlgorithm = _doubleDispatch[(int)bodyA.CollisionShape.ShapeType, (int)bodyB.CollisionShape.ShapeType].CreateCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB);
return collisionAlgorithm;
}
public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB)
{
return FindAlgorithm(bodyA, bodyB, null);
}
public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
{
return new EmptyAlgorithm(collisionAlgorithmConstructionInfo);
}
public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
return 1f;
}
public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { }
public override bool NeedsCollision(CollisionObject bodyA, CollisionObject bodyB)
{
RigidBody rb;
BulletXCharacter bxcA = null;
BulletXPrim bxpA = null;
Type t = bodyA.GetType();
if (t == typeof (RigidBody))
{
rb = (RigidBody) bodyA;
relatedScene._characters.TryGetValue(rb, out bxcA);
relatedScene._prims.TryGetValue(rb, out bxpA);
}
// String nameA;
// if (bxcA != null)
// nameA = bxcA._name;
// else if (bxpA != null)
// nameA = bxpA._name;
// else
// nameA = "null";
BulletXCharacter bxcB = null;
BulletXPrim bxpB = null;
t = bodyB.GetType();
if (t == typeof (RigidBody))
{
rb = (RigidBody) bodyB;
relatedScene._characters.TryGetValue(rb, out bxcB);
relatedScene._prims.TryGetValue(rb, out bxpB);
}
// String nameB;
// if (bxcB != null)
// nameB = bxcB._name;
// else if (bxpB != null)
// nameB = bxpB._name;
// else
// nameB = "null";
bool needsCollision;// = base.NeedsCollision(bodyA, bodyB);
int c1 = 3;
int c2 = 3;
////////////////////////////////////////////////////////
//BulletX Mesh Collisions
//added by Jed zhu
//data: May 07,2005
////////////////////////////////////////////////////////
#region BulletXMeshCollisions Fields
if (bxcA != null && bxpB != null)
c1 = Collision(bxcA, bxpB);
if (bxpA != null && bxcB != null)
c2 = Collision(bxcB, bxpA);
if (c1 < 2)
needsCollision = (c1 > 0) ? true : false;
else if (c2 < 2)
needsCollision = (c2 > 0) ? true : false;
else
needsCollision = base.NeedsCollision(bodyA, bodyB);
#endregion
//m_log.DebugFormat("[BulletX]: A collision was detected between {0} and {1} --> {2}", nameA, nameB,
//needsCollision);
return needsCollision;
}
public float GetSphereDistance(CollisionObject boxObject, out Vector3 pointOnBox, out Vector3 pointOnSphere, Vector3 sphereCenter, float radius)
{
pointOnBox = new Vector3();
pointOnSphere = new Vector3();
float margins;
Vector3[] bounds = new Vector3[2];
BoxShape boxShape = boxObject.CollisionShape as BoxShape;
bounds[0] = -boxShape.HalfExtents;
bounds[1] = boxShape.HalfExtents;
margins = boxShape.Margin; //also add sphereShape margin?
Matrix m44T = boxObject.WorldTransform;
Vector3[] boundsVec = new Vector3[2];
float penetration;
boundsVec[0] = bounds[0];
boundsVec[1] = bounds[1];
Vector3 marginsVec = new Vector3(margins, margins, margins);
// add margins
bounds[0] += marginsVec;
bounds[1] -= marginsVec;
/////////////////////////////////////////////////
Vector3 tmp, prel, normal, v3P;
Vector3[] n = new Vector3[6];
float sep = 10000000.0f, sepThis;
n[0] = new Vector3(-1.0f, 0.0f, 0.0f);
n[1] = new Vector3(0.0f, -1.0f, 0.0f);
n[2] = new Vector3(0.0f, 0.0f, -1.0f);
n[3] = new Vector3(1.0f, 0.0f, 0.0f);
n[4] = new Vector3(0.0f, 1.0f, 0.0f);
n[5] = new Vector3(0.0f, 0.0f, 1.0f);
// convert point in local space
prel = MathHelper.InvXForm(m44T, sphereCenter);
bool found = false;
v3P = prel;
for (int i = 0; i < 6; i++)
{
int j = i < 3 ? 0 : 1;
if ((sepThis = (Vector3.Dot(v3P - bounds[j], n[i]))) > 0.0f)
{
v3P = v3P - n[i] * sepThis;
found = true;
}
}
//
if (found)
{
bounds[0] = boundsVec[0];
bounds[1] = boundsVec[1];
normal = Vector3.Normalize(prel - v3P);
pointOnBox = v3P + normal * margins;
pointOnSphere = prel - normal * radius;
if ((Vector3.Dot(pointOnSphere - pointOnBox, normal)) > 0.0f)
{
return 1.0f;
}
// transform back in world space
tmp = MathHelper.MatrixToVector(m44T, pointOnBox);
pointOnBox = tmp;
tmp = MathHelper.MatrixToVector(m44T, pointOnSphere);
pointOnSphere = tmp;
float seps2 = (pointOnBox - pointOnSphere).LengthSquared();
//if this fails, fallback into deeper penetration case, below
if (seps2 > MathHelper.Epsilon)
{
sep = -(float)Math.Sqrt(seps2);
normal = (pointOnBox - pointOnSphere);
normal *= 1f / sep;
}
return sep;
}
//////////////////////////////////////////////////
// Deep penetration case
penetration = GetSpherePenetration(boxObject, ref pointOnBox, ref pointOnSphere, sphereCenter, radius, bounds[0], bounds[1]);
bounds[0] = boundsVec[0];
bounds[1] = boundsVec[1];
if (penetration <= 0.0f)
return (penetration - margins);
else
return 1.0f;
}
public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
{
if (!IsSwapped)
return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, false);
else
return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, true);
}
public override float CalculateTimeOfImpact(CollisionObject collisionObjectA, CollisionObject collisionObjectB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//not yet
return 1;
}
public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (_manifold == null)
return;
CollisionObject sphereObject = _isSwapped ? bodyB : bodyA;
CollisionObject boxObject = _isSwapped ? bodyA : bodyB;
SphereShape sphereA = sphereObject.CollisionShape as SphereShape;
Vector3 pOnBox, pOnSphere;
Vector3 sphereCenter = sphereObject.WorldTransform.Translation;
float radius = sphereA.Radius;
float dist = GetSphereDistance(boxObject, out pOnBox, out pOnSphere, sphereCenter, radius);
if (dist < MathHelper.Epsilon)
{
Vector3 normalOnSurfaceB = Vector3.Normalize(pOnBox - pOnSphere);
// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.SetPersistentManifold(_manifold);
resultOut.AddContactPoint(normalOnSurfaceB, pOnBox, dist);
}
}
public override float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
//Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
//col0->m_worldTransform,
float resultFraction = 1f;
float squareMotA = (colA.InterpolationWorldTransform.Translation - colA.WorldTransform.Translation).LengthSquared();
float squareMotB = (colB.InterpolationWorldTransform.Translation - colB.WorldTransform.Translation).LengthSquared();
if (squareMotA < colA.CcdSquareMotionThreshold &&
squareMotB < colB.CcdSquareMotionThreshold)
return resultFraction;
if (DisableCcd)
return 1f;
//An adhoc way of testing the Continuous Collision Detection algorithms
//One object is approximated as a sphere, to simplify things
//Starting in penetration should report no time of impact
//For proper CCD, better accuracy and handling of 'allowed' penetration should be added
//also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
// Convex0 against sphere for Convex1
{
ConvexShape convexA = colA.CollisionShape as ConvexShape;
SphereShape sphereB = new SphereShape(colB.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = new CastResult();
VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
//Simplification, one object is simplified as a sphere
GjkConvexCast ccdB = new GjkConvexCast(convexA, sphereB, voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform,
colB.WorldTransform, colB.InterpolationWorldTransform, result))
{
//store result.m_fraction in both bodies
if (colA.HitFraction > result.Fraction)
colA.HitFraction = result.Fraction;
if (colB.HitFraction > result.Fraction)
colB.HitFraction = result.Fraction;
if (resultFraction > result.Fraction)
resultFraction = result.Fraction;
}
}
// Sphere (for convex0) against Convex1
{
ConvexShape convexB = colB.CollisionShape as ConvexShape;
SphereShape sphereA = new SphereShape(colA.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = new CastResult();
VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
GjkConvexCast ccdB = new GjkConvexCast(sphereA, convexB, voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform,
colB.WorldTransform, colB.InterpolationWorldTransform, result))
{
//store result.m_fraction in both bodies
if (colA.HitFraction > result.Fraction)
colA.HitFraction = result.Fraction;
if (colB.HitFraction > result.Fraction)
colB.HitFraction = result.Fraction;
if (resultFraction > result.Fraction)
resultFraction = result.Fraction;
}
}
return resultFraction;
}
public virtual CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1)
{
return null;
}
/*public CollisionAlgorithm internalFindAlgorithm(CollisionObject body0, CollisionObject body1)
{
return internalFindAlgorithm(body0, body1, null);
}
public CollisionAlgorithm internalFindAlgorithm(CollisionObject body0, CollisionObject body1, PersistentManifold sharedManifold)
{
m_count++;
CollisionAlgorithmConstructionInfo ci = new CollisionAlgorithmConstructionInfo();
ci.m_dispatcher = this;
if (body0.getCollisionShape().isConvex() && body1.getCollisionShape().isConvex())
{
return new ConvexConvexAlgorithm(sharedManifold, ci, body0, body1);
}
if (body0.getCollisionShape().isConvex() && body1.getCollisionShape().isConcave())
{
return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, false);
}
if (body1.getCollisionShape().isConvex() && body0.getCollisionShape().isConcave())
{
return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, true);
}
if (body0.getCollisionShape().isCompound())
{
return new CompoundCollisionAlgorithm(ci, body0, body1, false);
}
else
{
if (body1.getCollisionShape().isCompound())
{
return new CompoundCollisionAlgorithm(ci, body0, body1, true);
}
}
//failed to find an algorithm
return new EmptyAlgorithm(ci);
}*/
public virtual bool NeedsCollision(CollisionObject bodyA, CollisionObject bodyB)
{
if (bodyA == null || bodyB == null)
throw new BulletException();
bool needsCollision = true;
//broadphase filtering already deals with this
/*if ((body0.isStaticObject() || body0.isKinematicObject()) &&
(body1.isStaticObject() || body1.isKinematicObject()))
{
printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n");
}*/
if ((!bodyA.IsActive) && (!bodyB.IsActive))
needsCollision = false;
return needsCollision;
}
public virtual bool NeedsResponse(CollisionObject bodyA, CollisionObject bodyB)
{
//here you can do filtering
bool hasResponse = bodyA.HasContactResponse && bodyB.HasContactResponse;
hasResponse = hasResponse && (!bodyA.IsStaticOrKinematicObject || !bodyB.IsStaticOrKinematicObject);
return hasResponse;
}
public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
{
return new SphereTriangleCollisionAlgorithm(collisionAlgorithmConstructionInfo.Manifold, collisionAlgorithmConstructionInfo, bodyA, bodyB, IsSwapped);
}
public float GetSpherePenetration(CollisionObject boxObject, ref Vector3 pointOnBox, ref Vector3 pointOnSphere, Vector3 sphereCenter, float radius, Vector3 aabbMin, Vector3 aabbMax)
{
Vector3[] bounds = new Vector3[2];
bounds[0] = aabbMin;
bounds[1] = aabbMax;
Vector3 p0 = new Vector3(), tmp, prel, normal = new Vector3();
Vector3[] n = new Vector3[6];
float sep = -10000000.0f, sepThis;
n[0] = new Vector3(-1.0f, 0.0f, 0.0f);
n[1] = new Vector3(0.0f, -1.0f, 0.0f);
n[2] = new Vector3(0.0f, 0.0f, -1.0f);
n[3] = new Vector3(1.0f, 0.0f, 0.0f);
n[4] = new Vector3(0.0f, 1.0f, 0.0f);
n[5] = new Vector3(0.0f, 0.0f, 1.0f);
Matrix m44T = boxObject.WorldTransform;
// convert point in local space
prel = MathHelper.InvXForm(m44T, sphereCenter);
///////////
for (int i = 0; i < 6; i++)
{
int j = i < 3 ? 0 : 1;
if ((sepThis = (Vector3.Dot(prel - bounds[j], n[i])) - radius) > 0.0f) return 1.0f;
if (sepThis > sep)
{
p0 = bounds[j];
normal = n[i];
sep = sepThis;
}
}
pointOnBox = prel - normal * (Vector3.Dot(normal, (prel - p0)));
pointOnSphere = pointOnBox + normal * sep;
// transform back in world space
tmp = MathHelper.MatrixToVector(m44T, pointOnBox);
pointOnBox = tmp;
tmp = MathHelper.MatrixToVector(m44T, pointOnSphere);
pointOnSphere = tmp;
normal = Vector3.Normalize(pointOnBox - pointOnSphere);
return sep;
}
public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB)
{
return(FindAlgorithm(bodyA, bodyB, null));
}
