private void PreallocateChildAlgorithms(CollisionObject body0, CollisionObject body1)
{
CollisionObject colObj = m_isSwapped ? body1 : body0;
CollisionObject otherObj = m_isSwapped ? body0 : body1;
Debug.Assert(colObj.GetCollisionShape().IsCompound());
CompoundShape compoundShape = (CompoundShape)(colObj.GetCollisionShape());
int numChildren = compoundShape.GetNumChildShapes();
int i;
//m_childCollisionAlgorithms.resize(numChildren);
m_childCollisionAlgorithms.Clear();
for (i = 0; i < numChildren; i++)
{
if (compoundShape.GetDynamicAabbTree() != null)
{
m_childCollisionAlgorithms.Add(null);
}
else
{
CollisionShape tmpShape = colObj.GetCollisionShape();
CollisionShape childShape = compoundShape.GetChildShape(i);
colObj.InternalSetTemporaryCollisionShape(childShape);
CollisionAlgorithm ca = m_dispatcher.FindAlgorithm(colObj, otherObj, m_sharedManifold);
m_childCollisionAlgorithms.Add(ca);
colObj.InternalSetTemporaryCollisionShape(tmpShape);
}
}
}
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 virtual void Initialize(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, bool isSwapped)
{
base.Initialize(ci, body0, body1);
m_isSwapped = isSwapped;
m_sharedManifold = ci.GetManifold();
m_ownsManifold = false;
CollisionObject colObj = m_isSwapped ? body1 : body0;
Debug.Assert(colObj.GetCollisionShape().IsCompound());
CompoundShape compoundShape = (CompoundShape)(colObj.GetCollisionShape());
m_compoundShapeRevision = compoundShape.GetUpdateRevision();
PreallocateChildAlgorithms(body0, body1);
}
public override float CalculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
resultOut = null;
CollisionObject colObj = m_isSwapped ? body1 : body0;
CollisionObject otherObj = m_isSwapped ? body0 : body1;
Debug.Assert(colObj.GetCollisionShape().IsCompound());
CompoundShape compoundShape = (CompoundShape)(colObj.GetCollisionShape());
//We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
//If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
//given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means:
//determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1
//then use each overlapping node AABB against Tree0
//and vise versa.
float hitFraction = 1f;
int numChildren = m_childCollisionAlgorithms.Count;
IndexedMatrix orgTrans;
float frac;
for (int i = 0; i < numChildren; i++)
{
//temporarily exchange parent btCollisionShape with childShape, and recurse
CollisionShape childShape = compoundShape.GetChildShape(i);
//backup
orgTrans = colObj.GetWorldTransform();
IndexedMatrix childTrans = compoundShape.GetChildTransform(i);
IndexedMatrix newChildWorldTrans = orgTrans * childTrans;
colObj.SetWorldTransform(ref newChildWorldTrans);
CollisionShape tmpShape = colObj.GetCollisionShape();
colObj.InternalSetTemporaryCollisionShape(childShape);
frac = m_childCollisionAlgorithms[i].CalculateTimeOfImpact(colObj, otherObj, dispatchInfo, resultOut);
if (frac < hitFraction)
{
hitFraction = frac;
}
//revert back
colObj.InternalSetTemporaryCollisionShape(tmpShape);
colObj.SetWorldTransform(ref orgTrans);
}
return(hitFraction);
}
protected void ConvexVsConvexCollision(CollisionObject body0,
CollisionObject body1,
CollisionShape shape0,
CollisionShape shape1)
{
CollisionShape tmpShape0 = body0.GetCollisionShape();
CollisionShape tmpShape1 = body1.GetCollisionShape();
body0.InternalSetTemporaryCollisionShape(shape0);
body1.InternalSetTemporaryCollisionShape(shape1);
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::ConvexVsConvex");
}
m_resultOut.SetShapeIdentifiersA(m_part0, m_triface0);
m_resultOut.SetShapeIdentifiersB(m_part1, m_triface1);
CheckConvexAlgorithm(body0, body1);
m_convex_algorithm.ProcessCollision(body0, body1, m_dispatchInfo, m_resultOut);
body0.InternalSetTemporaryCollisionShape(tmpShape0);
body1.InternalSetTemporaryCollisionShape(tmpShape1);
}
//public override void processCollision (CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut)
//{
// CollisionObject convexBody = m_isSwapped ? body1 : body0;
// CollisionObject triBody = m_isSwapped ? body0 : body1;
// if (triBody.getCollisionShape().isConcave())
// {
// CollisionObject triOb = triBody;
// ConcaveShape concaveShape = (ConcaveShape)(triOb.getCollisionShape());
// if (convexBody.getCollisionShape().isConvex())
// {
// float collisionMarginTriangle = concaveShape.getMargin();
// resultOut.setPersistentManifold(m_convexTriangleCallback.m_manifoldPtr);
// m_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_convexTriangleCallback.m_manifoldPtr);
// m_convexTriangleCallback.m_manifoldPtr.setBodies(convexBody, triBody);
// IndexedVector3 min = m_convexTriangleCallback.getAabbMin();
// IndexedVector3 max = m_convexTriangleCallback.getAabbMax();
// concaveShape.processAllTriangles(m_convexTriangleCallback, ref min,ref max );
// resultOut.refreshContactPoints();
// }
// }
//}
//public override float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
//{
// CollisionObject convexbody = m_isSwapped ? body1 : body0;
// CollisionObject triBody = m_isSwapped ? body0 : body1;
// //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.getInterpolationWorldTransform()._origin - convexbody.getWorldTransform()._origin).LengthSquared();
// if (squareMot0 < convexbody.getCcdSquareMotionThreshold())
// {
// return 1f;
// }
// //const IndexedVector3& from = convexbody.m_worldTransform._origin;
// //IndexedVector3 to = convexbody.m_interpolationWorldTransform._origin;
// //todo: only do if the motion exceeds the 'radius'
// //IndexedMatrix triInv = IndexedMatrix.Invert(triBody.getWorldTransform());
// //IndexedMatrix convexFromLocal = MathUtil.bulletMatrixMultiply(triInv , convexbody.getWorldTransform());
// //IndexedMatrix convexToLocal = MathUtil.bulletMatrixMultiply(triInv , convexbody.getInterpolationWorldTransform());
// IndexedMatrix triInv = IndexedMatrix.Invert(triBody.getWorldTransform());
// IndexedMatrix convexFromLocal = MathUtil.inverseTimes(triBody.getWorldTransform(), convexbody.getWorldTransform());
// IndexedMatrix convexToLocal = MathUtil.inverseTimes(triBody.getWorldTransform(), convexbody.getInterpolationWorldTransform());
// if (triBody.getCollisionShape().isConcave())
// {
// IndexedVector3 rayAabbMin = convexFromLocal._origin;
// MathUtil.vectorMin(convexToLocal._origin, ref rayAabbMin);
// IndexedVector3 rayAabbMax = convexFromLocal._origin;
// MathUtil.vectorMax(convexToLocal._origin,ref rayAabbMax);
// float ccdRadius0 = convexbody.getCcdSweptSphereRadius();
// rayAabbMin -= new IndexedVector3(ccdRadius0,ccdRadius0,ccdRadius0);
// rayAabbMax += new IndexedVector3(ccdRadius0,ccdRadius0,ccdRadius0);
// float curHitFraction = 1.0f; //is this available?
// LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(ref convexFromLocal, ref convexToLocal,
// convexbody.getCcdSweptSphereRadius(),curHitFraction);
// raycastCallback.m_hitFraction = convexbody.getHitFraction();
// CollisionObject concavebody = triBody;
// ConcaveShape triangleMesh = (ConcaveShape) concavebody.getCollisionShape();
// if (triangleMesh != null)
// {
// triangleMesh.processAllTriangles(raycastCallback,ref rayAabbMin,ref rayAabbMax);
// }
// if (raycastCallback.m_hitFraction < convexbody.getHitFraction())
// {
// convexbody.setHitFraction( raycastCallback.m_hitFraction);
// float result = raycastCallback.m_hitFraction;
// raycastCallback.cleanup();
// return result;
// }
// raycastCallback.cleanup();
// }
// return 1f;
//}
public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//fixme
CollisionObject convexBody = m_isSwapped ? bodyB : bodyA;
CollisionObject triBody = m_isSwapped ? bodyA : bodyB;
if (triBody.GetCollisionShape().IsConcave())
{
CollisionObject triOb = triBody;
ConcaveShape concaveShape = triOb.GetCollisionShape() as ConcaveShape;
if (convexBody.GetCollisionShape().IsConvex())
{
float collisionMarginTriangle = concaveShape.GetMargin();
resultOut.SetPersistentManifold(m_convexTriangleCallback.m_manifoldPtr);
m_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);
m_convexTriangleCallback.m_manifoldPtr.SetBodies(convexBody, triBody);
IndexedVector3 min = m_convexTriangleCallback.GetAabbMin();
IndexedVector3 max = m_convexTriangleCallback.GetAabbMax();
concaveShape.ProcessAllTriangles(m_convexTriangleCallback, ref min, ref max);
resultOut.RefreshContactPoints();
}
}
}
protected void ShapeVsShapeCollision(
CollisionObject body0,
CollisionObject body1,
CollisionShape shape0,
CollisionShape shape1)
{
CollisionShape tmpShape0 = body0.GetCollisionShape();
CollisionShape tmpShape1 = body1.GetCollisionShape();
body0.InternalSetTemporaryCollisionShape(shape0);
body1.InternalSetTemporaryCollisionShape(shape1);
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::ShapeVsShape");
}
{
CollisionAlgorithm algor = NewAlgorithm(body0, body1);
// post : checkManifold is called
m_resultOut.SetShapeIdentifiersA(m_part0, m_triface0);
m_resultOut.SetShapeIdentifiersB(m_part1, m_triface1);
algor.ProcessCollision(body0, body1, m_dispatchInfo, m_resultOut);
m_dispatcher.FreeCollisionAlgorithm(algor);
}
body0.InternalSetTemporaryCollisionShape(tmpShape0);
body1.InternalSetTemporaryCollisionShape(tmpShape1);
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//resultOut = new ManifoldResult();
if (m_manifoldPtr == null)
{
return;
}
CollisionObject sphereObj = m_swapped ? body1 : body0;
CollisionObject triObj = m_swapped ? body0 : body1;
SphereShape sphere = sphereObj.GetCollisionShape() as SphereShape;
TriangleShape triangle = triObj.GetCollisionShape() as TriangleShape;
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.SetPersistentManifold(m_manifoldPtr);
using (SphereTriangleDetector detector = BulletGlobals.SphereTriangleDetectorPool.Get())
{
detector.Initialize(sphere, triangle, m_manifoldPtr.GetContactBreakingThreshold());
ClosestPointInput input = ClosestPointInput.Default();
input.m_maximumDistanceSquared = float.MaxValue;
sphereObj.GetWorldTransform(out input.m_transformA);
triObj.GetWorldTransform(out input.m_transformB);
bool swapResults = m_swapped;
detector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), swapResults);
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
//swapped?
m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
m_ownManifold = true;
}
resultOut.SetPersistentManifold(m_manifoldPtr);
//comment-out next line to test multi-contact generation
//resultOut.getPersistentManifold().clearManifold();
ConvexShape min0 = body0.GetCollisionShape() as ConvexShape;
ConvexShape min1 = body1.GetCollisionShape() as ConvexShape;
IndexedVector3 normalOnB = IndexedVector3.Zero;
IndexedVector3 pointOnBWorld = IndexedVector3.Zero;
{
ClosestPointInput input = ClosestPointInput.Default();
using (GjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get())
{
gjkPairDetector.Initialize(min0, min1, m_simplexSolver, m_pdSolver);
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.SetMinkowskiA(min0);
gjkPairDetector.SetMinkowskiB(min1);
{
input.m_maximumDistanceSquared = min0.GetMargin() + min1.GetMargin() + m_manifoldPtr.GetContactBreakingThreshold();
input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
}
input.m_transformA = body0.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
gjkPairDetector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), false);
#if DEBUG
if (BulletGlobals.g_streamWriter != null)
{
BulletGlobals.g_streamWriter.WriteLine("c2dc2d processCollision");
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transformA", input.m_transformA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transformB", input.m_transformB);
}
#endif
}
//BulletGlobals.GjkPairDetectorPool.Free(gjkPairDetector);
//btVector3 v0,v1;
//btVector3 sepNormalWorldSpace;
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
public virtual PersistentManifold GetNewManifold(CollisionObject b0, CollisionObject b1)
{
gNumManifold++;
CollisionObject body0 = b0;
CollisionObject body1 = b1;
//optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance)
float contactBreakingThreshold = ((m_dispatcherFlags & DispatcherFlags.CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) > 0) ?
Math.Min(body0.GetCollisionShape().GetContactBreakingThreshold(BulletGlobals.gContactBreakingThreshold), body1.GetCollisionShape().GetContactBreakingThreshold(BulletGlobals.gContactBreakingThreshold))
: BulletGlobals.gContactBreakingThreshold;
float contactProcessingThreshold = Math.Min(body0.GetContactProcessingThreshold(), body1.GetContactProcessingThreshold());
// nothing in our pool so create a new one and return it.
// need a way to flush the pool ideally
PersistentManifold manifold = BulletGlobals.PersistentManifoldPool.Get();
manifold.Initialise(body0, body1, 0, contactBreakingThreshold, contactProcessingThreshold);
manifold.m_index1a = m_manifoldsPtr.Count;
m_manifoldsPtr.Add(manifold);
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDispatcher)
{
BulletGlobals.g_streamWriter.WriteLine("GetNewManifold[{0}][{1}]", manifold.m_index1a, m_manifoldsPtr.Count);
}
#endif
return(manifold);
}
public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
CollisionObject convexbody = m_isSwapped ? bodyB : bodyA;
CollisionObject triBody = m_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.GetInterpolationWorldTransform()._origin - convexbody.GetWorldTransform()._origin).LengthSquared();
if (squareMot0 < convexbody.GetCcdSquareMotionThreshold())
{
return(1);
}
//IndexedMatrix triInv = MathHelper.InvertMatrix(triBody.getWorldTransform());
IndexedMatrix triInv = triBody.GetWorldTransform().Inverse();
IndexedMatrix convexFromLocal = triInv * convexbody.GetWorldTransform();
IndexedMatrix convexToLocal = triInv * convexbody.GetInterpolationWorldTransform();
if (triBody.GetCollisionShape().IsConcave())
{
IndexedVector3 rayAabbMin = convexFromLocal._origin;
MathUtil.VectorMin(convexToLocal._origin, ref rayAabbMin);
IndexedVector3 rayAabbMax = convexFromLocal._origin;
MathUtil.VectorMax(convexToLocal._origin, ref rayAabbMax);
IndexedVector3 ccdRadius0 = new IndexedVector3(convexbody.GetCcdSweptSphereRadius());
rayAabbMin -= ccdRadius0;
rayAabbMax += ccdRadius0;
float curHitFraction = 1f; //is this available?
using (LocalTriangleSphereCastCallback raycastCallback = BulletGlobals.LocalTriangleSphereCastCallbackPool.Get())
{
raycastCallback.Initialize(ref convexFromLocal, ref convexToLocal,
convexbody.GetCcdSweptSphereRadius(), curHitFraction);
raycastCallback.m_hitFraction = convexbody.GetHitFraction();
CollisionObject concavebody = triBody;
ConcaveShape triangleMesh = concavebody.GetCollisionShape() as ConcaveShape;
if (triangleMesh != null)
{
triangleMesh.ProcessAllTriangles(raycastCallback, ref rayAabbMin, ref rayAabbMax);
}
if (raycastCallback.m_hitFraction < convexbody.GetHitFraction())
{
convexbody.SetHitFraction(raycastCallback.m_hitFraction);
return(raycastCallback.m_hitFraction);
}
}
}
return(1);
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
//swapped?
m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
m_ownManifold = true;
}
resultOut.SetPersistentManifold(m_manifoldPtr);
//comment-out next line to test multi-contact generation
//resultOut.getPersistentManifold().clearManifold();
ConvexShape min0 = body0.GetCollisionShape() as ConvexShape;
ConvexShape min1 = body1.GetCollisionShape() as ConvexShape;
IndexedVector3 normalOnB = IndexedVector3.Zero;
IndexedVector3 pointOnBWorld = IndexedVector3.Zero;
{
ClosestPointInput input = ClosestPointInput.Default();
using (GjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get())
{
gjkPairDetector.Initialize(min0, min1, m_simplexSolver, m_pdSolver);
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.SetMinkowskiA(min0);
gjkPairDetector.SetMinkowskiB(min1);
{
input.m_maximumDistanceSquared = min0.GetMargin() + min1.GetMargin() + m_manifoldPtr.GetContactBreakingThreshold();
input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
}
input.m_transformA = body0.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
gjkPairDetector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), false);
#if DEBUG
if (BulletGlobals.g_streamWriter != null)
{
BulletGlobals.g_streamWriter.WriteLine("c2dc2d processCollision");
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transformA", input.m_transformA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transformB", input.m_transformB);
}
#endif
}
//BulletGlobals.GjkPairDetectorPool.Free(gjkPairDetector);
//btVector3 v0,v1;
//btVector3 sepNormalWorldSpace;
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
public void SetTimeStepAndCounters(float collisionMarginTriangle, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
m_dispatchInfoPtr = dispatchInfo;
m_collisionMarginTriangle = collisionMarginTriangle;
m_resultOut = resultOut;
//recalc aabbs
//IndexedMatrix convexInTriangleSpace = MathUtil.bulletMatrixMultiply(IndexedMatrix.Invert(m_triBody.getWorldTransform()) , m_convexBody.getWorldTransform());
IndexedMatrix convexInTriangleSpace = m_triBody.GetWorldTransform().Inverse() * m_convexBody.GetWorldTransform();
CollisionShape convexShape = m_convexBody.GetCollisionShape();
convexShape.GetAabb(ref convexInTriangleSpace, out m_aabbMin, out m_aabbMax);
float extraMargin = collisionMarginTriangle;
IndexedVector3 extra = new IndexedVector3(extraMargin);
m_aabbMax += extra;
m_aabbMin -= extra;
}
public virtual void ProcessTriangle(IndexedVector3[] triangle, int partId, int triangleIndex)
{
//aabb filter is already applied!
CollisionAlgorithmConstructionInfo ci = new CollisionAlgorithmConstructionInfo();
ci.SetDispatcher(m_dispatcher);
CollisionObject ob = m_triBody as CollisionObject;
///debug drawing of the overlapping triangles
///
#if false
if (m_dispatchInfoPtr != null && m_dispatchInfoPtr.getDebugDraw() != null && ((m_dispatchInfoPtr.getDebugDraw().GetDebugMode() & DebugDrawModes.DBG_DrawWireframe) > 0))
{
IndexedVector3 color = new IndexedVector3(1, 1, 0);
IndexedMatrix tr = ob.GetWorldTransform();
IndexedVector3[] transformedTriangles = new IndexedVector3[3];
IndexedVector3.Transform(triangle, ref tr, transformedTriangles);
m_dispatchInfoPtr.getDebugDraw().DrawLine(ref transformedTriangles[0], ref transformedTriangles[1], ref color);
m_dispatchInfoPtr.getDebugDraw().DrawLine(ref transformedTriangles[1], ref transformedTriangles[2], ref color);
m_dispatchInfoPtr.getDebugDraw().DrawLine(ref transformedTriangles[2], ref transformedTriangles[0], ref color);
}
#endif
if (m_convexBody.GetCollisionShape().IsConvex())
{
using (TriangleShape tm = BulletGlobals.TriangleShapePool.Get())
{
tm.Initialize(ref triangle[0], ref triangle[1], ref triangle[2]);
tm.SetMargin(m_collisionMarginTriangle);
CollisionShape tmpShape = ob.GetCollisionShape();
ob.InternalSetTemporaryCollisionShape(tm);
CollisionAlgorithm colAlgo = ci.GetDispatcher().FindAlgorithm(m_convexBody, m_triBody, m_manifoldPtr);
///this should use the btDispatcher, so the actual registered algorithm is used
// btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody);
if (m_resultOut.GetBody0Internal() == m_triBody)
{
m_resultOut.SetShapeIdentifiersA(partId, triangleIndex);
}
else
{
m_resultOut.SetShapeIdentifiersB(partId, triangleIndex);
}
colAlgo.ProcessCollision(m_convexBody, m_triBody, m_dispatchInfoPtr, m_resultOut);
ci.GetDispatcher().FreeCollisionAlgorithm(colAlgo);
colAlgo = null;
ob.InternalSetTemporaryCollisionShape(tmpShape);
}
}
}
public CollisionAlgorithm FindAlgorithm(CollisionObject body0, CollisionObject body1, PersistentManifold sharedManifold)
{
CollisionAlgorithmConstructionInfo ci = new CollisionAlgorithmConstructionInfo(this, -1);
ci.SetManifold(sharedManifold);
int index1 = (int)body0.GetCollisionShape().GetShapeType();
int index2 = (int)body1.GetCollisionShape().GetShapeType();
return(m_doubleDispatch[index1, index2].CreateCollisionAlgorithm(ci, body0, body1));
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
return;
}
resultOut.SetPersistentManifold(m_manifoldPtr);
SphereShape sphere0 = body0.GetCollisionShape() as SphereShape;
SphereShape sphere1 = body1.GetCollisionShape() as SphereShape;
IndexedVector3 diff = body0.GetWorldTransform()._origin - body1.GetWorldTransform()._origin;
float len = diff.Length();
float radius0 = sphere0.GetRadius();
float radius1 = sphere1.GetRadius();
#if CLEAR_MANIFOLD
m_manifoldPtr.clearManifold(); //don't do this, it disables warmstarting
#endif
///iff distance positive, don't generate a new contact
if (len > (radius0 + radius1))
{
#if !CLEAR_MANIFOLD
resultOut.RefreshContactPoints();
#endif //CLEAR_MANIFOLD
return;
}
///distance (negative means penetration)
float dist = len - (radius0 + radius1);
IndexedVector3 normalOnSurfaceB = new IndexedVector3(1, 0, 0);
if (len > MathUtil.SIMD_EPSILON)
{
normalOnSurfaceB = diff / len;
}
///point on A (worldspace)
///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
///point on B (worldspace)
IndexedVector3 pos1 = body1.GetWorldTransform()._origin + radius1 * normalOnSurfaceB;
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.AddContactPoint(ref normalOnSurfaceB, ref pos1, dist);
#if !CLEAR_MANIFOLD
resultOut.RefreshContactPoints();
#endif //CLEAR_MANIFOLD
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
return;
}
resultOut.SetPersistentManifold(m_manifoldPtr);
SphereShape sphere0 = body0.GetCollisionShape() as SphereShape;
SphereShape sphere1 = body1.GetCollisionShape() as SphereShape;
IndexedVector3 diff = body0.GetWorldTransform()._origin - body1.GetWorldTransform()._origin;
float len = diff.Length();
float radius0 = sphere0.GetRadius();
float radius1 = sphere1.GetRadius();
#if CLEAR_MANIFOLD
m_manifoldPtr.clearManifold(); //don't do this, it disables warmstarting
#endif
///iff distance positive, don't generate a new contact
if (len > (radius0 + radius1))
{
#if !CLEAR_MANIFOLD
resultOut.RefreshContactPoints();
#endif //CLEAR_MANIFOLD
return;
}
///distance (negative means penetration)
float dist = len - (radius0 + radius1);
IndexedVector3 normalOnSurfaceB = new IndexedVector3(1, 0, 0);
if (len > MathUtil.SIMD_EPSILON)
{
normalOnSurfaceB = diff / len;
}
///point on A (worldspace)
///btVector3 pos0 = col0.getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
///point on B (worldspace)
IndexedVector3 pos1 = body1.GetWorldTransform()._origin + radius1 * normalOnSurfaceB;
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.AddContactPoint(ref normalOnSurfaceB, ref pos1, dist);
#if !CLEAR_MANIFOLD
resultOut.RefreshContactPoints();
#endif //CLEAR_MANIFOLD
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
ClearCache();
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::ProcessCollision");
}
m_resultOut = resultOut;
m_dispatchInfo = dispatchInfo;
GImpactShapeInterface gimpactshape0;
GImpactShapeInterface gimpactshape1;
if (body0.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.GIMPACT_SHAPE_PROXYTYPE)
{
gimpactshape0 = body0.GetCollisionShape() as GImpactShapeInterface;
if (body1.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.GIMPACT_SHAPE_PROXYTYPE)
{
gimpactshape1 = body1.GetCollisionShape() as GImpactShapeInterface;
GImpactVsGImpact(body0, body1, gimpactshape0, gimpactshape1);
}
else
{
GImpactVsShape(body0, body1, gimpactshape0, body1.GetCollisionShape(), false);
}
}
else if (body1.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.GIMPACT_SHAPE_PROXYTYPE)
{
gimpactshape1 = body1.GetCollisionShape() as GImpactShapeInterface;
GImpactVsShape(body1, body0, gimpactshape1, body0.GetCollisionShape(), true);
}
}
public virtual void CollideSingleContact(ref IndexedQuaternion perturbeRot, CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
CollisionObject convexObj = m_isSwapped ? body1 : body0;
CollisionObject planeObj = m_isSwapped ? body0 : body1;
ConvexShape convexShape = convexObj.GetCollisionShape() as ConvexShape;
StaticPlaneShape planeShape = planeObj.GetCollisionShape() as StaticPlaneShape;
bool hasCollision = false;
IndexedVector3 planeNormal = planeShape.GetPlaneNormal();
float planeConstant = planeShape.GetPlaneConstant();
IndexedMatrix convexWorldTransform = convexObj.GetWorldTransform();
IndexedMatrix convexInPlaneTrans = planeObj.GetWorldTransform().Inverse() * convexWorldTransform;
//now perturbe the convex-world transform
convexWorldTransform._basis *= new IndexedBasisMatrix(ref perturbeRot);
IndexedMatrix planeInConvex = convexWorldTransform.Inverse() * planeObj.GetWorldTransform();;
IndexedVector3 vtx = convexShape.LocalGetSupportingVertex(planeInConvex._basis * -planeNormal);
IndexedVector3 vtxInPlane = vtxInPlane = convexInPlaneTrans * vtx;
float distance = (IndexedVector3.Dot(planeNormal, vtxInPlane) - planeConstant);
IndexedVector3 vtxInPlaneProjected = vtxInPlane - (distance * planeNormal);
IndexedVector3 vtxInPlaneWorld = planeObj.GetWorldTransform() * vtxInPlaneProjected;
hasCollision = distance < m_manifoldPtr.GetContactBreakingThreshold();
resultOut.SetPersistentManifold(m_manifoldPtr);
if (hasCollision)
{
/// report a contact. internally this will be kept persistent, and contact reduction is done
IndexedVector3 normalOnSurfaceB = planeObj.GetWorldTransform()._basis *planeNormal;
IndexedVector3 pOnB = vtxInPlaneWorld;
resultOut.AddContactPoint(ref normalOnSurfaceB, ref pOnB, distance);
}
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
return;
}
CollisionObject col0 = body0;
CollisionObject col1 = body1;
//resultOut = new ManifoldResult(body0, body1);
BoxShape box0 = col0.GetCollisionShape() as BoxShape;
BoxShape box1 = col1.GetCollisionShape() as BoxShape;
//if (((String)col0.getUserPointer()).Contains("Box") &&
// ((String)col1.getUserPointer()).Contains("Box") )
//{
// int ibreak = 0;
//}
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.SetPersistentManifold(m_manifoldPtr);
#if !USE_PERSISTENT_CONTACTS
m_manifoldPtr.ClearManifold();
#endif //USE_PERSISTENT_CONTACTS
ClosestPointInput input = ClosestPointInput.Default();
input.m_maximumDistanceSquared = float.MaxValue;
input.m_transformA = body0.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
BoxBoxDetector.GetClosestPoints(box0, box1, ref input, resultOut, dispatchInfo.getDebugDraw(), false);
#if USE_PERSISTENT_CONTACTS
// refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
#endif //USE_PERSISTENT_CONTACTS
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
return;
}
CollisionObject col0 = body0;
CollisionObject col1 = body1;
Box2dShape box0 = (Box2dShape)col0.GetCollisionShape();
Box2dShape box1 = (Box2dShape)col1.GetCollisionShape();
resultOut.SetPersistentManifold(m_manifoldPtr);
B2CollidePolygons(ref resultOut, box0, col0.GetWorldTransform(), box1, col1.GetWorldTransform());
// refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//(void)dispatchInfo;
//(void)resultOut;
if (m_manifoldPtr == null)
{
return;
}
CollisionObject sphereObj = m_isSwapped ? body1 : body0;
CollisionObject boxObj = m_isSwapped ? body0 : body1;
IndexedVector3 pOnBox = new IndexedVector3();;
IndexedVector3 normalOnSurfaceB = new IndexedVector3();
float penetrationDepth = 0f;
IndexedVector3 sphereCenter = sphereObj.GetWorldTransform()._origin;
SphereShape sphere0 = sphereObj.GetCollisionShape() as SphereShape;
float radius = sphere0.GetRadius();
float maxContactDistance = m_manifoldPtr.GetContactBreakingThreshold();
resultOut.SetPersistentManifold(m_manifoldPtr);
if (GetSphereDistance(boxObj, ref pOnBox, ref normalOnSurfaceB, ref penetrationDepth, sphereCenter, radius, maxContactDistance))
{
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.AddContactPoint(normalOnSurfaceB, pOnBox, penetrationDepth);
}
if (m_ownManifold)
{
if (m_manifoldPtr.GetNumContacts() > 0)
{
resultOut.RefreshContactPoints();
}
}
}
public virtual void AddCollisionObject(CollisionObject collisionObject, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask)
{
//check that the object isn't already added
//btAssert( m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size());
Debug.Assert(collisionObject != null);
//Debug.Assert(!m_collisionObjects.Contains(collisionObject));
if (m_collisionObjects.Contains(collisionObject))
{
return;
}
m_collisionObjects.Add(collisionObject);
//calculate new AABB
IndexedMatrix trans = collisionObject.GetWorldTransform();
IndexedVector3 minAabb;
IndexedVector3 maxAabb;
collisionObject.GetCollisionShape().GetAabb(ref trans, out minAabb, out maxAabb);
BroadphaseNativeTypes type = collisionObject.GetCollisionShape().GetShapeType();
collisionObject.SetBroadphaseHandle(GetBroadphase().CreateProxy(
ref minAabb,
ref maxAabb,
type,
collisionObject,
collisionFilterGroup,
collisionFilterMask,
m_dispatcher1, 0
));
}
public void UpdateSingleAabb(CollisionObject colObj)
{
IndexedVector3 minAabb;
IndexedVector3 maxAabb;
IndexedMatrix wt = colObj.GetWorldTransform();
colObj.GetCollisionShape().GetAabb(ref wt, out minAabb, out maxAabb);
//need to increase the aabb for contact thresholds
IndexedVector3 contactThreshold = new IndexedVector3(BulletGlobals.gContactBreakingThreshold);
minAabb -= contactThreshold;
maxAabb += contactThreshold;
if (GetDispatchInfo().m_useContinuous && colObj.GetInternalType() == CollisionObjectTypes.CO_RIGID_BODY && !colObj.IsStaticOrKinematicObject())
{
IndexedVector3 minAabb2,maxAabb2;
colObj.GetCollisionShape().GetAabb(colObj.GetInterpolationWorldTransform(),out minAabb2 ,out maxAabb2);
minAabb2 -= contactThreshold;
maxAabb2 += contactThreshold;
MathUtil.VectorMin(ref minAabb2,ref minAabb);
MathUtil.VectorMax(ref maxAabb2, ref maxAabb);
}
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugCollisionWorld)
{
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "updateSingleAabbMin", minAabb);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "updateSingleAabbMax", maxAabb);
}
IBroadphaseInterface bp = m_broadphasePairCache as IBroadphaseInterface;
//moving objects should be moderately sized, probably something wrong if not
if (colObj.IsStaticObject() || ((maxAabb - minAabb).LengthSquared() < 1e12f))
{
bp.SetAabb(colObj.GetBroadphaseHandle(), ref minAabb, ref maxAabb, m_dispatcher1);
}
else
{
//something went wrong, investigate
//this assert is unwanted in 3D modelers (danger of loosing work)
colObj.SetActivationState(ActivationState.DISABLE_SIMULATION);
//static bool reportMe = true;
bool reportMe = true;
if (reportMe && m_debugDrawer != null)
{
reportMe = false;
m_debugDrawer.ReportErrorWarning("Overflow in AABB, object removed from simulation");
m_debugDrawer.ReportErrorWarning("If you can reproduce this, please email [email protected]\n");
m_debugDrawer.ReportErrorWarning("Please include above information, your Platform, version of OS.\n");
m_debugDrawer.ReportErrorWarning("Thanks.\n");
}
}
}
/// objectQuerySingle performs a collision detection query and calls the resultCallback. It is used internally by rayTest.
public static void ObjectQuerySingle(ConvexShape castShape, ref IndexedMatrix convexFromTrans, ref IndexedMatrix convexToTrans,
CollisionObject collisionObject, CollisionShape collisionShape,
ref IndexedMatrix colObjWorldTransform,
ConvexResultCallback resultCallback, float allowedPenetration)
{
if (collisionShape.IsConvex())
{
BulletGlobals.StartProfile("convexSweepConvex");
CastResult castResult = BulletGlobals.CastResultPool.Get();
castResult.m_allowedPenetration = allowedPenetration;
castResult.m_fraction = resultCallback.m_closestHitFraction;//float(1.);//??
ConvexShape convexShape = collisionShape as ConvexShape;
VoronoiSimplexSolver simplexSolver = BulletGlobals.VoronoiSimplexSolverPool.Get();
GjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver = BulletGlobals.GjkEpaPenetrationDepthSolverPool.Get();
ContinuousConvexCollision convexCaster1 = BulletGlobals.ContinuousConvexCollisionPool.Get();
convexCaster1.Initialize(castShape, convexShape, simplexSolver, gjkEpaPenetrationSolver);
//btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
//btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
IConvexCast castPtr = convexCaster1;
if (castPtr.CalcTimeOfImpact(ref convexFromTrans, ref convexToTrans, ref colObjWorldTransform, ref colObjWorldTransform, castResult))
{
//add hit
if (castResult.m_normal.LengthSquared() > 0.0001f)
{
if (castResult.m_fraction < resultCallback.m_closestHitFraction)
{
castResult.m_normal.Normalize();
LocalConvexResult localConvexResult = new LocalConvexResult
(
collisionObject,
//null, // updated to allow different ctor on struct
ref castResult.m_normal,
ref castResult.m_hitPoint,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.AddSingleResult(ref localConvexResult, normalInWorldSpace);
}
}
}
BulletGlobals.ContinuousConvexCollisionPool.Free(convexCaster1);
BulletGlobals.GjkEpaPenetrationDepthSolverPool.Free(gjkEpaPenetrationSolver);
BulletGlobals.VoronoiSimplexSolverPool.Free(simplexSolver);
castResult.Cleanup();
BulletGlobals.StopProfile();
}
else
{
if (collisionShape.IsConcave())
{
if (collisionShape.GetShapeType() == BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE)
{
BulletGlobals.StartProfile("convexSweepbtBvhTriangleMesh");
BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape)collisionShape;
IndexedMatrix worldTocollisionObject = colObjWorldTransform.Inverse();
IndexedVector3 convexFromLocal = worldTocollisionObject * convexFromTrans._origin;
IndexedVector3 convexToLocal = worldTocollisionObject * convexToTrans._origin;
// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
IndexedMatrix rotationXform = new IndexedMatrix(worldTocollisionObject._basis * convexToTrans._basis,new IndexedVector3(0));
using (BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get())
{
tccb.Initialize(castShape, ref convexFromTrans, ref convexToTrans, resultCallback, collisionObject, triangleMesh, ref colObjWorldTransform);
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
tccb.m_allowedPenetration = allowedPenetration;
IndexedVector3 boxMinLocal;
IndexedVector3 boxMaxLocal;
castShape.GetAabb(ref rotationXform, out boxMinLocal, out boxMaxLocal);
triangleMesh.PerformConvexCast(tccb, ref convexFromLocal, ref convexToLocal, ref boxMinLocal, ref boxMaxLocal);
}
BulletGlobals.StopProfile();
}
else
{
if (collisionShape.GetShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE)
{
CastResult castResult = BulletGlobals.CastResultPool.Get();
castResult.m_allowedPenetration = allowedPenetration;
castResult.m_fraction = resultCallback.m_closestHitFraction;
StaticPlaneShape planeShape = collisionShape as StaticPlaneShape;
ContinuousConvexCollision convexCaster1 = new ContinuousConvexCollision(castShape, planeShape);
if (convexCaster1.CalcTimeOfImpact(ref convexFromTrans, ref convexToTrans, ref colObjWorldTransform, ref colObjWorldTransform, castResult))
{
//add hit
if (castResult.m_normal.LengthSquared() > 0.0001f)
{
if (castResult.m_fraction < resultCallback.m_closestHitFraction)
{
castResult.m_normal.Normalize();
LocalConvexResult localConvexResult = new LocalConvexResult
(
collisionObject,
//null, // updated to allow different ctor on struct
ref castResult.m_normal,
ref castResult.m_hitPoint,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.AddSingleResult(ref localConvexResult, normalInWorldSpace);
}
}
}
castResult.Cleanup();
}
else
{
BulletGlobals.StartProfile("convexSweepConcave");
ConcaveShape concaveShape = (ConcaveShape)collisionShape;
IndexedMatrix worldTocollisionObject = colObjWorldTransform.Inverse();
IndexedVector3 convexFromLocal = worldTocollisionObject * convexFromTrans._origin;
IndexedVector3 convexToLocal = worldTocollisionObject * convexToTrans._origin;
// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
IndexedMatrix rotationXform = new IndexedMatrix(worldTocollisionObject._basis * convexToTrans._basis, new IndexedVector3(0));
using (BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get())
{
tccb.Initialize(castShape, ref convexFromTrans, ref convexToTrans, resultCallback, collisionObject, concaveShape, ref colObjWorldTransform);
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
tccb.m_allowedPenetration = allowedPenetration;
IndexedVector3 boxMinLocal;
IndexedVector3 boxMaxLocal;
castShape.GetAabb(ref rotationXform, out boxMinLocal, out boxMaxLocal);
IndexedVector3 rayAabbMinLocal = convexFromLocal;
MathUtil.VectorMin(ref convexToLocal, ref rayAabbMinLocal);
//rayAabbMinLocal.setMin(convexToLocal);
IndexedVector3 rayAabbMaxLocal = convexFromLocal;
//rayAabbMaxLocal.setMax(convexToLocal);
MathUtil.VectorMax(ref convexToLocal, ref rayAabbMaxLocal);
rayAabbMinLocal += boxMinLocal;
rayAabbMaxLocal += boxMaxLocal;
concaveShape.ProcessAllTriangles(tccb, ref rayAabbMinLocal, ref rayAabbMaxLocal);
BulletGlobals.StopProfile();
}
}
}
}
else
{
///@todo : use AABB tree or other BVH acceleration structure!
if (collisionShape.IsCompound())
{
BulletGlobals.StartProfile("convexSweepCompound");
CompoundShape compoundShape = (CompoundShape)collisionShape;
for (int i = 0; i < compoundShape.GetNumChildShapes(); i++)
{
IndexedMatrix childTrans = compoundShape.GetChildTransform(i);
CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
IndexedMatrix childWorldTrans = colObjWorldTransform * childTrans;
// replace collision shape so that callback can determine the triangle
CollisionShape saveCollisionShape = collisionObject.GetCollisionShape();
collisionObject.InternalSetTemporaryCollisionShape(childCollisionShape);
LocalInfoAdder my_cb = new LocalInfoAdder(i, resultCallback);
my_cb.m_closestHitFraction = resultCallback.m_closestHitFraction;
ObjectQuerySingle(castShape, ref convexFromTrans, ref convexToTrans,
collisionObject,
childCollisionShape,
ref childWorldTrans,
my_cb, allowedPenetration);
// restore
collisionObject.InternalSetTemporaryCollisionShape(saveCollisionShape);
}
BulletGlobals.StopProfile();
}
}
}
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
ClearCache();
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::ProcessCollision");
}
m_resultOut = resultOut;
m_dispatchInfo = dispatchInfo;
GImpactShapeInterface gimpactshape0;
GImpactShapeInterface gimpactshape1;
if (body0.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.GIMPACT_SHAPE_PROXYTYPE)
{
gimpactshape0 = body0.GetCollisionShape() as GImpactShapeInterface;
if (body1.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.GIMPACT_SHAPE_PROXYTYPE)
{
gimpactshape1 = body1.GetCollisionShape() as GImpactShapeInterface;
GImpactVsGImpact(body0, body1, gimpactshape0, gimpactshape1);
}
else
{
GImpactVsShape(body0, body1, gimpactshape0, body1.GetCollisionShape(), false);
}
}
else if (body1.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.GIMPACT_SHAPE_PROXYTYPE)
{
gimpactshape1 = body1.GetCollisionShape() as GImpactShapeInterface;
GImpactVsShape(body1, body0, gimpactshape1, body0.GetCollisionShape(), true);
}
}
protected void ConvexVsConvexCollision(CollisionObject body0,
CollisionObject body1,
CollisionShape shape0,
CollisionShape shape1)
{
CollisionShape tmpShape0 = body0.GetCollisionShape();
CollisionShape tmpShape1 = body1.GetCollisionShape();
body0.InternalSetTemporaryCollisionShape(shape0);
body1.InternalSetTemporaryCollisionShape(shape1);
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::ConvexVsConvex");
}
m_resultOut.SetShapeIdentifiersA(m_part0, m_triface0);
m_resultOut.SetShapeIdentifiersB(m_part1, m_triface1);
CheckConvexAlgorithm(body0, body1);
m_convex_algorithm.ProcessCollision(body0, body1, m_dispatchInfo, m_resultOut);
body0.InternalSetTemporaryCollisionShape(tmpShape0);
body1.InternalSetTemporaryCollisionShape(tmpShape1);
}
protected void ShapeVsShapeCollision(
CollisionObject body0,
CollisionObject body1,
CollisionShape shape0,
CollisionShape shape1)
{
CollisionShape tmpShape0 = body0.GetCollisionShape();
CollisionShape tmpShape1 = body1.GetCollisionShape();
body0.InternalSetTemporaryCollisionShape(shape0);
body1.InternalSetTemporaryCollisionShape(shape1);
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::ShapeVsShape");
}
{
CollisionAlgorithm algor = NewAlgorithm(body0, body1);
// post : checkManifold is called
m_resultOut.SetShapeIdentifiersA(m_part0, m_triface0);
m_resultOut.SetShapeIdentifiersB(m_part1, m_triface1);
algor.ProcessCollision(body0, body1, m_dispatchInfo, m_resultOut);
m_dispatcher.FreeCollisionAlgorithm(algor);
}
body0.InternalSetTemporaryCollisionShape(tmpShape0);
body1.InternalSetTemporaryCollisionShape(tmpShape1);
}
public CollisionAlgorithm FindAlgorithm(CollisionObject body0, CollisionObject body1, PersistentManifold sharedManifold)
{
CollisionAlgorithmConstructionInfo ci = new CollisionAlgorithmConstructionInfo(this, -1);
ci.SetManifold(sharedManifold);
int index1 = (int)body0.GetCollisionShape().GetShapeType();
int index2 = (int)body1.GetCollisionShape().GetShapeType();
return m_doubleDispatch[index1, index2].CreateCollisionAlgorithm(ci, body0, body1);
}
public override float CalculateTimeOfImpact(CollisionObject body0, CollisionObject body1, 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
///body0.m_worldTransform,
float resultFraction = 1.0f;
float squareMot0 = (body0.GetInterpolationWorldTransform()._origin - body0.GetWorldTransform()._origin).LengthSquared();
float squareMot1 = (body1.GetInterpolationWorldTransform()._origin - body1.GetWorldTransform()._origin).LengthSquared();
if (squareMot0 < body0.GetCcdSquareMotionThreshold() &&
squareMot1 < body1.GetCcdSquareMotionThreshold())
{
return(resultFraction);
}
//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 convex0 = body0.GetCollisionShape() as ConvexShape;
SphereShape sphere1 = BulletGlobals.SphereShapePool.Get();
sphere1.Initialize(body1.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = BulletGlobals.CastResultPool.Get();
VoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
using (GjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get())
{
ccd1.Initialize(convex0, sphere1, voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(), body0.GetInterpolationWorldTransform(),
body1.GetWorldTransform(), body1.GetInterpolationWorldTransform(), result))
{
//store result.m_fraction in both bodies
if (body0.GetHitFraction() > result.m_fraction)
{
body0.SetHitFraction(result.m_fraction);
}
if (body1.GetHitFraction() > result.m_fraction)
{
body1.SetHitFraction(result.m_fraction);
}
if (resultFraction > result.m_fraction)
{
resultFraction = result.m_fraction;
}
}
BulletGlobals.VoronoiSimplexSolverPool.Free(voronoiSimplex);
BulletGlobals.SphereShapePool.Free(sphere1);
result.Cleanup();
}
}
/// Sphere (for convex0) against Convex1
{
ConvexShape convex1 = body1.GetCollisionShape() as ConvexShape;
SphereShape sphere0 = BulletGlobals.SphereShapePool.Get();
sphere0.Initialize(body0.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = BulletGlobals.CastResultPool.Get();
VoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
using (GjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get())
{
ccd1.Initialize(sphere0, convex1, voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(), body0.GetInterpolationWorldTransform(),
body1.GetWorldTransform(), body1.GetInterpolationWorldTransform(), result))
{
//store result.m_fraction in both bodies
if (body0.GetHitFraction() > result.m_fraction)
{
body0.SetHitFraction(result.m_fraction);
}
if (body1.GetHitFraction() > result.m_fraction)
{
body1.SetHitFraction(result.m_fraction);
}
if (resultFraction > result.m_fraction)
{
resultFraction = result.m_fraction;
}
}
BulletGlobals.VoronoiSimplexSolverPool.Free(voronoiSimplex);
BulletGlobals.SphereShapePool.Free(sphere0);
result.Cleanup();
}
}
return(resultFraction);
}
public void ProcessChildShape(CollisionShape childShape, int index)
{
Debug.Assert(index >= 0);
CompoundShape compoundShape = (CompoundShape)(m_compoundColObj.GetCollisionShape());
Debug.Assert(index < compoundShape.GetNumChildShapes());
//backup
IndexedMatrix orgTrans = m_compoundColObj.GetWorldTransform();
IndexedMatrix orgInterpolationTrans = m_compoundColObj.GetInterpolationWorldTransform();
IndexedMatrix childTrans = compoundShape.GetChildTransform(index);
IndexedMatrix newChildWorldTrans = orgTrans * childTrans;
//perform an AABB check first
IndexedVector3 aabbMin0;
IndexedVector3 aabbMax0;
IndexedVector3 aabbMin1;
IndexedVector3 aabbMax1;
childShape.GetAabb(ref newChildWorldTrans, out aabbMin0, out aabbMax0);
m_otherObj.GetCollisionShape().GetAabb(m_otherObj.GetWorldTransform(), out aabbMin1, out aabbMax1);
if (AabbUtil2.TestAabbAgainstAabb2(ref aabbMin0, ref aabbMax0, ref aabbMin1, ref aabbMax1))
{
m_compoundColObj.SetWorldTransform(ref newChildWorldTrans);
m_compoundColObj.SetInterpolationWorldTransform(ref newChildWorldTrans);
//the contactpoint is still projected back using the original inverted worldtrans
CollisionShape tmpShape = m_compoundColObj.GetCollisionShape();
m_compoundColObj.InternalSetTemporaryCollisionShape(childShape);
if (m_childCollisionAlgorithms[index] == null)
{
m_childCollisionAlgorithms[index] = m_dispatcher.FindAlgorithm(m_compoundColObj, m_otherObj, m_sharedManifold);
if (m_childCollisionAlgorithms[index] == m_parent)
{
int ibreak = 0;
}
}
///detect swapping case
if (m_resultOut.GetBody0Internal() == m_compoundColObj)
{
m_resultOut.SetShapeIdentifiersA(-1, index);
}
else
{
m_resultOut.SetShapeIdentifiersB(-1, index);
}
m_childCollisionAlgorithms[index].ProcessCollision(m_compoundColObj, m_otherObj, m_dispatchInfo, m_resultOut);
if (m_dispatchInfo.getDebugDraw() != null && (((m_dispatchInfo.getDebugDraw().GetDebugMode() & DebugDrawModes.DBG_DrawAabb)) != 0))
{
IndexedVector3 worldAabbMin = IndexedVector3.Zero, worldAabbMax = IndexedVector3.Zero;
m_dispatchInfo.getDebugDraw().DrawAabb(aabbMin0, aabbMax0, new IndexedVector3(1, 1, 1));
m_dispatchInfo.getDebugDraw().DrawAabb(aabbMin1, aabbMax1, new IndexedVector3(1, 1, 1));
}
//revert back transform
m_compoundColObj.InternalSetTemporaryCollisionShape(tmpShape);
m_compoundColObj.SetWorldTransform(ref orgTrans);
m_compoundColObj.SetInterpolationWorldTransform(ref orgInterpolationTrans);
}
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
return;
}
CollisionObject convexObj = m_isSwapped ? body1 : body0;
CollisionObject planeObj = m_isSwapped ? body0 : body1;
ConvexShape convexShape = convexObj.GetCollisionShape() as ConvexShape;
StaticPlaneShape planeShape = planeObj.GetCollisionShape() as StaticPlaneShape;
bool hasCollision = false;
IndexedVector3 planeNormal = planeShape.GetPlaneNormal();
float planeConstant = planeShape.GetPlaneConstant();
IndexedMatrix planeInConvex;
planeInConvex = convexObj.GetWorldTransform().Inverse() * planeObj.GetWorldTransform();
IndexedMatrix convexInPlaneTrans;
convexInPlaneTrans = planeObj.GetWorldTransform().Inverse() * convexObj.GetWorldTransform();
IndexedVector3 vtx = convexShape.LocalGetSupportingVertex(planeInConvex._basis * -planeNormal);
IndexedVector3 vtxInPlane = convexInPlaneTrans * vtx;
float distance = (planeNormal.Dot(vtxInPlane) - planeConstant);
IndexedVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
IndexedVector3 vtxInPlaneWorld = planeObj.GetWorldTransform() * vtxInPlaneProjected;
hasCollision = distance < m_manifoldPtr.GetContactBreakingThreshold();
resultOut.SetPersistentManifold(m_manifoldPtr);
if (hasCollision)
{
/// report a contact. internally this will be kept persistent, and contact reduction is done
IndexedVector3 normalOnSurfaceB = planeObj.GetWorldTransform()._basis *planeNormal;
IndexedVector3 pOnB = vtxInPlaneWorld;
resultOut.AddContactPoint(normalOnSurfaceB, pOnB, distance);
}
////first perform a collision query with the non-perturbated collision objects
//{
// IndexedQuaternion rotq = IndexedQuaternion.Identity;
// CollideSingleContact(ref rotq, body0, body1, dispatchInfo, resultOut);
//}
if (convexShape.IsPolyhedral() && resultOut.GetPersistentManifold().GetNumContacts() < m_minimumPointsPerturbationThreshold)
{
IndexedVector3 v0;
IndexedVector3 v1;
TransformUtil.PlaneSpace1(ref planeNormal, out v0, out v1);
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
float angleLimit = 0.125f * MathUtil.SIMD_PI;
float perturbeAngle;
float radius = convexShape.GetAngularMotionDisc();
perturbeAngle = BulletGlobals.gContactBreakingThreshold / radius;
if (perturbeAngle > angleLimit)
{
perturbeAngle = angleLimit;
}
IndexedQuaternion perturbeRot = new IndexedQuaternion(v0, perturbeAngle);
for (int i = 0; i < m_numPerturbationIterations; i++)
{
float iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations);
IndexedQuaternion rotq = new IndexedQuaternion(planeNormal, iterationAngle);
rotq = IndexedQuaternion.Inverse(rotq) * perturbeRot * rotq;
CollideSingleContact(ref rotq, body0, body1, dispatchInfo, resultOut);
}
}
if (m_ownManifold)
{
if (m_manifoldPtr.GetNumContacts() > 0)
{
resultOut.RefreshContactPoints();
}
}
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
//swapped?
m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
m_ownManifold = true;
}
//resultOut = new ManifoldResult();
resultOut.SetPersistentManifold(m_manifoldPtr);
//comment-out next line to test multi-contact generation
//resultOut.GetPersistentManifold().ClearManifold();
ConvexShape min0 = body0.GetCollisionShape() as ConvexShape;
ConvexShape min1 = body1.GetCollisionShape() as ConvexShape;
IndexedVector3 normalOnB;
IndexedVector3 pointOnBWorld;
#if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
if ((min0.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE))
{
CapsuleShape capsuleA = min0 as CapsuleShape;
CapsuleShape capsuleB = min1 as CapsuleShape;
//IndexedVector3 localScalingA = capsuleA.GetLocalScaling();
//IndexedVector3 localScalingB = capsuleB.GetLocalScaling();
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
float dist = CapsuleCapsuleDistance(out normalOnB, out pointOnBWorld, capsuleA.GetHalfHeight(), capsuleA.GetRadius(),
capsuleB.GetHalfHeight(), capsuleB.GetRadius(), capsuleA.GetUpAxis(), capsuleB.GetUpAxis(),
body0.GetWorldTransform(), body1.GetWorldTransform(), threshold);
if (dist < threshold)
{
Debug.Assert(normalOnB.LengthSquared() >= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON));
resultOut.AddContactPoint(ref normalOnB, ref pointOnBWorld, dist);
}
resultOut.RefreshContactPoints();
return;
}
#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
m_sepDistance.updateSeparatingDistance(body0.getWorldTransform(), body1.getWorldTransform());
}
if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance() <= 0.f)
#endif //USE_SEPDISTANCE_UTIL2
{
ClosestPointInput input = ClosestPointInput.Default();
using (GjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get())
{
gjkPairDetector.Initialize(min0, min1, m_simplexSolver, m_pdSolver);
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.SetMinkowskiA(min0);
gjkPairDetector.SetMinkowskiB(min1);
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
input.m_maximumDistanceSquared = float.MaxValue;
}
else
#endif //USE_SEPDISTANCE_UTIL2
{
input.m_maximumDistanceSquared = min0.GetMargin() + min1.GetMargin() + m_manifoldPtr.GetContactBreakingThreshold();
input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
}
//input.m_stackAlloc = dispatchInfo.m_stackAllocator;
input.m_transformA = body0.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
if (min0.IsPolyhedral() && min1.IsPolyhedral())
{
DummyResult dummy = new DummyResult();
PolyhedralConvexShape polyhedronA = min0 as PolyhedralConvexShape;
PolyhedralConvexShape polyhedronB = min1 as PolyhedralConvexShape;
if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetConvexPolyhedron() != null)
{
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
float minDist = float.MinValue;
IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0);
bool foundSepAxis = true;
if (dispatchInfo.m_enableSatConvex)
{
foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis(
polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(),
out sepNormalWorldSpace);
}
else
{
#if ZERO_MARGIN
gjkPairDetector.SetIgnoreMargin(true);
gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.m_debugDraw);
#else
gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw);
#endif
float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared();
if (l2 > MathUtil.SIMD_EPSILON)
{
sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2);
//minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin();
#if ZERO_MARGIN
foundSepAxis = true; //gjkPairDetector.getCachedSeparatingDistance()<0.f;
#else
foundSepAxis = gjkPairDetector.GetCachedSeparatingDistance() < (min0.GetMargin() + min1.GetMargin());
#endif
}
}
if (foundSepAxis)
{
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
PolyhedralContactClipping.ClipHullAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(), minDist - threshold, threshold, resultOut);
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
return;
}
else
{
//we can also deal with convex versus triangle (without connectivity data)
if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetShapeType() == BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE)
{
m_vertices.Clear();
TriangleShape tri = polyhedronB as TriangleShape;
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[0]);
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[1]);
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[2]);
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0);;
float minDist = float.MinValue;
float maxDist = threshold;
bool foundSepAxis = false;
if (false)
{
polyhedronB.InitializePolyhedralFeatures();
foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis(
polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(),
out sepNormalWorldSpace);
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
}
else
{
#if ZERO_MARGIN
gjkPairDetector.SetIgnoreMargin(true);
gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.m_debugDraw);
#else
gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw);
#endif//ZERO_MARGIN
float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared();
if (l2 > MathUtil.SIMD_EPSILON)
{
sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2);
//minDist = gjkPairDetector.getCachedSeparatingDistance();
//maxDist = threshold;
minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin();
foundSepAxis = true;
}
}
if (foundSepAxis)
{
PolyhedralContactClipping.ClipFaceAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(),
body0.GetWorldTransform(), m_vertices, minDist - threshold, maxDist, resultOut);
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
return;
}
}
}
gjkPairDetector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), false);
#if USE_SEPDISTANCE_UTIL2
float sepDist = 0.f;
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
sepDist = gjkPairDetector.getCachedSeparatingDistance();
if (sepDist > MathUtil.SIMD_EPSILON)
{
sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
//now perturbe directions to get multiple contact points
}
}
#endif //USE_SEPDISTANCE_UTIL2
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
//perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
if (m_numPerturbationIterations > 0 && resultOut.GetPersistentManifold().GetNumContacts() < m_minimumPointsPerturbationThreshold)
{
IndexedVector3 v0, v1;
IndexedVector3 sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis();
sepNormalWorldSpace.Normalize();
TransformUtil.PlaneSpace1(ref sepNormalWorldSpace, out v0, out v1);
bool perturbeA = true;
const float angleLimit = 0.125f * MathUtil.SIMD_PI;
float perturbeAngle;
float radiusA = min0.GetAngularMotionDisc();
float radiusB = min1.GetAngularMotionDisc();
if (radiusA < radiusB)
{
perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusA;
perturbeA = true;
}
else
{
perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusB;
perturbeA = false;
}
if (perturbeAngle > angleLimit)
{
perturbeAngle = angleLimit;
}
IndexedMatrix unPerturbedTransform;
if (perturbeA)
{
unPerturbedTransform = input.m_transformA;
}
else
{
unPerturbedTransform = input.m_transformB;
}
for (int i = 0; i < m_numPerturbationIterations; i++)
{
if (v0.LengthSquared() > MathUtil.SIMD_EPSILON)
{
IndexedQuaternion perturbeRot = new IndexedQuaternion(v0, perturbeAngle);
float iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations);
IndexedQuaternion rotq = new IndexedQuaternion(sepNormalWorldSpace, iterationAngle);
if (perturbeA)
{
input.m_transformA._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body0.GetWorldTransform()._basis);
input.m_transformB = body1.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformA, 10.0f);
#endif //DEBUG_CONTACTS
}
else
{
input.m_transformA = body0.GetWorldTransform();
input.m_transformB._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body1.GetWorldTransform()._basis);
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformB, 10.0f);
#endif
}
PerturbedContactResult perturbedResultOut = new PerturbedContactResult(resultOut, ref input.m_transformA, ref input.m_transformB, ref unPerturbedTransform, perturbeA, dispatchInfo.getDebugDraw());
gjkPairDetector.GetClosestPoints(ref input, perturbedResultOut, dispatchInfo.getDebugDraw(), false);
}
}
}
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist > MathUtil.SIMD_EPSILON))
{
m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(), sepDist, body0.getWorldTransform(), body1.getWorldTransform());
}
#endif //USE_SEPDISTANCE_UTIL2
}
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
public bool GetSphereDistance(CollisionObject boxObj, ref IndexedVector3 pointOnBox, ref IndexedVector3 normal, ref float penetrationDepth, IndexedVector3 sphereCenter, float fRadius, float maxContactDistance)
{
BoxShape boxShape = boxObj.GetCollisionShape() as BoxShape;
IndexedVector3 boxHalfExtent = boxShape.GetHalfExtentsWithoutMargin();
float boxMargin = boxShape.GetMargin();
penetrationDepth = 1.0f;
// convert the sphere position to the box's local space
IndexedMatrix m44T = boxObj.GetWorldTransform();
IndexedVector3 sphereRelPos = m44T.InvXform(sphereCenter);
// Determine the closest point to the sphere center in the box
IndexedVector3 closestPoint = sphereRelPos;
closestPoint.X = (Math.Min(boxHalfExtent.X, closestPoint.X));
closestPoint.X = (Math.Max(-boxHalfExtent.X, closestPoint.X));
closestPoint.Y = (Math.Min(boxHalfExtent.Y, closestPoint.Y));
closestPoint.Y = (Math.Max(-boxHalfExtent.Y, closestPoint.Y));
closestPoint.Z = (Math.Min(boxHalfExtent.Z, closestPoint.Z));
closestPoint.Z = (Math.Max(-boxHalfExtent.Z, closestPoint.Z));
float intersectionDist = fRadius + boxMargin;
float contactDist = intersectionDist + maxContactDistance;
normal = sphereRelPos - closestPoint;
//if there is no penetration, we are done
float dist2 = normal.LengthSquared();
if (dist2 > contactDist * contactDist)
{
return(false);
}
float distance;
//special case if the sphere center is inside the box
if (dist2 == 0.0f)
{
distance = -GetSpherePenetration(ref boxHalfExtent, ref sphereRelPos, ref closestPoint, ref normal);
}
else //compute the penetration details
{
distance = normal.Length();
normal /= distance;
}
pointOnBox = closestPoint + normal * boxMargin;
// v3PointOnSphere = sphereRelPos - (normal * fRadius);
penetrationDepth = distance - intersectionDist;
// transform back in world space
IndexedVector3 tmp = m44T * pointOnBox;
pointOnBox = tmp;
// tmp = m44T(v3PointOnSphere);
// v3PointOnSphere = tmp;
tmp = m44T._basis * normal;
normal = tmp;
return(true);
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
//swapped?
m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
m_ownManifold = true;
}
//resultOut = new ManifoldResult();
resultOut.SetPersistentManifold(m_manifoldPtr);
//comment-out next line to test multi-contact generation
//resultOut.GetPersistentManifold().ClearManifold();
ConvexShape min0 = body0.GetCollisionShape() as ConvexShape;
ConvexShape min1 = body1.GetCollisionShape() as ConvexShape;
IndexedVector3 normalOnB;
IndexedVector3 pointOnBWorld;
#if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
if ((min0.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE))
{
CapsuleShape capsuleA = min0 as CapsuleShape;
CapsuleShape capsuleB = min1 as CapsuleShape;
//IndexedVector3 localScalingA = capsuleA.GetLocalScaling();
//IndexedVector3 localScalingB = capsuleB.GetLocalScaling();
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
float dist = CapsuleCapsuleDistance(out normalOnB, out pointOnBWorld, capsuleA.GetHalfHeight(), capsuleA.GetRadius(),
capsuleB.GetHalfHeight(), capsuleB.GetRadius(), capsuleA.GetUpAxis(), capsuleB.GetUpAxis(),
body0.GetWorldTransform(), body1.GetWorldTransform(), threshold);
if (dist < threshold)
{
Debug.Assert(normalOnB.LengthSquared() >= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON));
resultOut.AddContactPoint(ref normalOnB, ref pointOnBWorld, dist);
}
resultOut.RefreshContactPoints();
return;
}
#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
m_sepDistance.updateSeparatingDistance(body0.getWorldTransform(),body1.getWorldTransform());
}
if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
#endif //USE_SEPDISTANCE_UTIL2
{
ClosestPointInput input = ClosestPointInput.Default();
using (GjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get())
{
gjkPairDetector.Initialize(min0, min1, m_simplexSolver, m_pdSolver);
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.SetMinkowskiA(min0);
gjkPairDetector.SetMinkowskiB(min1);
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
input.m_maximumDistanceSquared = float.MaxValue;
}
else
#endif //USE_SEPDISTANCE_UTIL2
{
input.m_maximumDistanceSquared = min0.GetMargin() + min1.GetMargin() + m_manifoldPtr.GetContactBreakingThreshold();
input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
}
//input.m_stackAlloc = dispatchInfo.m_stackAllocator;
input.m_transformA = body0.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
if (min0.IsPolyhedral() && min1.IsPolyhedral())
{
DummyResult dummy = new DummyResult();
PolyhedralConvexShape polyhedronA = min0 as PolyhedralConvexShape;
PolyhedralConvexShape polyhedronB = min1 as PolyhedralConvexShape;
if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetConvexPolyhedron() != null)
{
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
float minDist = float.MinValue;
IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0);
bool foundSepAxis = true;
if (dispatchInfo.m_enableSatConvex)
{
foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis(
polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(),
out sepNormalWorldSpace);
}
else
{
#if ZERO_MARGIN
gjkPairDetector.SetIgnoreMargin(true);
gjkPairDetector.GetClosestPoints(input,resultOut,dispatchInfo.m_debugDraw);
#else
gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw);
#endif
float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared();
if (l2 > MathUtil.SIMD_EPSILON)
{
sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2);
//minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin();
#if ZERO_MARGIN
foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f;
#else
foundSepAxis = gjkPairDetector.GetCachedSeparatingDistance() < (min0.GetMargin() + min1.GetMargin());
#endif
}
}
if (foundSepAxis)
{
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
PolyhedralContactClipping.ClipHullAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(), minDist - threshold, threshold, resultOut);
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
return;
}
else
{
//we can also deal with convex versus triangle (without connectivity data)
if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetShapeType() == BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE)
{
m_vertices.Clear();
TriangleShape tri = polyhedronB as TriangleShape;
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[0]);
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[1]);
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[2]);
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0); ;
float minDist = float.MinValue;
float maxDist = threshold;
bool foundSepAxis = false;
if (false)
{
polyhedronB.InitializePolyhedralFeatures();
foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis(
polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(),
out sepNormalWorldSpace);
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
}
else
{
#if ZERO_MARGIN
gjkPairDetector.SetIgnoreMargin(true);
gjkPairDetector.GetClosestPoints(input,resultOut,dispatchInfo.m_debugDraw);
#else
gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw);
#endif//ZERO_MARGIN
float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared();
if (l2 > MathUtil.SIMD_EPSILON)
{
sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2);
//minDist = gjkPairDetector.getCachedSeparatingDistance();
//maxDist = threshold;
minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin();
foundSepAxis = true;
}
}
if (foundSepAxis)
{
PolyhedralContactClipping.ClipFaceAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(),
body0.GetWorldTransform(), m_vertices, minDist - threshold, maxDist, resultOut);
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
return;
}
}
}
gjkPairDetector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), false);
#if USE_SEPDISTANCE_UTIL2
float sepDist = 0.f;
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
sepDist = gjkPairDetector.getCachedSeparatingDistance();
if (sepDist>MathUtil.SIMD_EPSILON)
{
sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
//now perturbe directions to get multiple contact points
}
}
#endif //USE_SEPDISTANCE_UTIL2
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
//perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
if (m_numPerturbationIterations > 0 && resultOut.GetPersistentManifold().GetNumContacts() < m_minimumPointsPerturbationThreshold)
{
IndexedVector3 v0, v1;
IndexedVector3 sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis();
sepNormalWorldSpace.Normalize();
TransformUtil.PlaneSpace1(ref sepNormalWorldSpace, out v0, out v1);
bool perturbeA = true;
const float angleLimit = 0.125f * MathUtil.SIMD_PI;
float perturbeAngle;
float radiusA = min0.GetAngularMotionDisc();
float radiusB = min1.GetAngularMotionDisc();
if (radiusA < radiusB)
{
perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusA;
perturbeA = true;
}
else
{
perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusB;
perturbeA = false;
}
if (perturbeAngle > angleLimit)
{
perturbeAngle = angleLimit;
}
IndexedMatrix unPerturbedTransform;
if (perturbeA)
{
unPerturbedTransform = input.m_transformA;
}
else
{
unPerturbedTransform = input.m_transformB;
}
for (int i = 0; i < m_numPerturbationIterations; i++)
{
if (v0.LengthSquared() > MathUtil.SIMD_EPSILON)
{
IndexedQuaternion perturbeRot = new IndexedQuaternion(v0, perturbeAngle);
float iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations);
IndexedQuaternion rotq = new IndexedQuaternion(sepNormalWorldSpace, iterationAngle);
if (perturbeA)
{
input.m_transformA._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body0.GetWorldTransform()._basis);
input.m_transformB = body1.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformA, 10.0f);
#endif //DEBUG_CONTACTS
}
else
{
input.m_transformA = body0.GetWorldTransform();
input.m_transformB._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body1.GetWorldTransform()._basis);
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformB, 10.0f);
#endif
}
PerturbedContactResult perturbedResultOut = new PerturbedContactResult(resultOut, ref input.m_transformA, ref input.m_transformB, ref unPerturbedTransform, perturbeA, dispatchInfo.getDebugDraw());
gjkPairDetector.GetClosestPoints(ref input, perturbedResultOut, dispatchInfo.getDebugDraw(), false);
}
}
}
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist > MathUtil.SIMD_EPSILON))
{
m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0.getWorldTransform(),body1.getWorldTransform());
}
#endif //USE_SEPDISTANCE_UTIL2
}
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//resultOut = null;
CollisionObject colObj = m_isSwapped ? body1 : body0;
CollisionObject otherObj = m_isSwapped ? body0 : body1;
Debug.Assert(colObj.GetCollisionShape().IsCompound());
CompoundShape compoundShape = (CompoundShape)(colObj.GetCollisionShape());
///btCompoundShape might have changed:
////make sure the internal child collision algorithm caches are still valid
if (compoundShape.GetUpdateRevision() != m_compoundShapeRevision)
{
///clear and update all
RemoveChildAlgorithms();
PreallocateChildAlgorithms(body0, body1);
}
Dbvt tree = compoundShape.GetDynamicAabbTree();
//use a dynamic aabb tree to cull potential child-overlaps
using (CompoundLeafCallback callback = BulletGlobals.CompoundLeafCallbackPool.Get())
{
callback.Initialize(colObj, otherObj, m_dispatcher, dispatchInfo, resultOut, this, m_childCollisionAlgorithms, m_sharedManifold);
///we need to refresh all contact manifolds
///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
{
m_manifoldArray.Clear();
for (int i = 0; i < m_childCollisionAlgorithms.Count; i++)
{
if (m_childCollisionAlgorithms[i] != null)
{
m_childCollisionAlgorithms[i].GetAllContactManifolds(m_manifoldArray);
for (int m = 0; m < m_manifoldArray.Count; m++)
{
if (m_manifoldArray[m].GetNumContacts() > 0)
{
resultOut.SetPersistentManifold(m_manifoldArray[m]);
resultOut.RefreshContactPoints();
resultOut.SetPersistentManifold(null);//??necessary?
}
}
m_manifoldArray.Clear();
}
}
}
if (tree != null)
{
IndexedVector3 localAabbMin;
IndexedVector3 localAabbMax;
IndexedMatrix otherInCompoundSpace;
//otherInCompoundSpace = MathUtil.BulletMatrixMultiply(colObj.GetWorldTransform(),otherObj.GetWorldTransform());
otherInCompoundSpace = colObj.GetWorldTransform().Inverse() * otherObj.GetWorldTransform();
otherObj.GetCollisionShape().GetAabb(ref otherInCompoundSpace, out localAabbMin, out localAabbMax);
DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax);
//process all children, that overlap with the given AABB bounds
Dbvt.CollideTV(tree.m_root, ref bounds, callback, tree.CollideTVStack, ref tree.CollideTVCount);
}
else
{
//iterate over all children, perform an AABB check inside ProcessChildShape
int numChildren = m_childCollisionAlgorithms.Count;
for (int i = 0; i < numChildren; i++)
{
callback.ProcessChildShape(compoundShape.GetChildShape(i), i);
}
}
{
//iterate over all children, perform an AABB check inside ProcessChildShape
int numChildren = m_childCollisionAlgorithms.Count;
m_manifoldArray.Clear();
CollisionShape childShape = null;
IndexedMatrix orgTrans;
IndexedMatrix orgInterpolationTrans;
IndexedMatrix newChildWorldTrans;
for (int i = 0; i < numChildren; i++)
{
if (m_childCollisionAlgorithms[i] != null)
{
childShape = compoundShape.GetChildShape(i);
//if not longer overlapping, remove the algorithm
orgTrans = colObj.GetWorldTransform();
orgInterpolationTrans = colObj.GetInterpolationWorldTransform();
IndexedMatrix childTrans = compoundShape.GetChildTransform(i);
newChildWorldTrans = orgTrans * childTrans;
//perform an AABB check first
IndexedVector3 aabbMin0;
IndexedVector3 aabbMax0;
IndexedVector3 aabbMin1;
IndexedVector3 aabbMax1;
childShape.GetAabb(ref newChildWorldTrans, out aabbMin0, out aabbMax0);
otherObj.GetCollisionShape().GetAabb(otherObj.GetWorldTransform(), out aabbMin1, out aabbMax1);
if (!AabbUtil2.TestAabbAgainstAabb2(ref aabbMin0, ref aabbMax0, ref aabbMin1, ref aabbMax1))
{
m_dispatcher.FreeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
m_childCollisionAlgorithms[i] = null;
}
}
}
}
}
}
public override float CalculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//(void)resultOut;
//(void)dispatchInfo;
///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
///body0.m_worldTransform,
float resultFraction = 1.0f;
float squareMot0 = (body0.GetInterpolationWorldTransform()._origin - body0.GetWorldTransform()._origin).LengthSquared();
float squareMot1 = (body1.GetInterpolationWorldTransform()._origin - body1.GetWorldTransform()._origin).LengthSquared();
if (squareMot0 < body0.GetCcdSquareMotionThreshold() &&
squareMot1 < body1.GetCcdSquareMotionThreshold())
{
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 convex0 = body0.GetCollisionShape() as ConvexShape;
SphereShape sphere1 = BulletGlobals.SphereShapePool.Get();
sphere1.Initialize(body1.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = BulletGlobals.CastResultPool.Get();
VoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
using (GjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get())
{
ccd1.Initialize(convex0, sphere1, voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(), body0.GetInterpolationWorldTransform(),
body1.GetWorldTransform(), body1.GetInterpolationWorldTransform(), result))
{
//store result.m_fraction in both bodies
if (body0.GetHitFraction() > result.m_fraction)
{
body0.SetHitFraction(result.m_fraction);
}
if (body1.GetHitFraction() > result.m_fraction)
{
body1.SetHitFraction(result.m_fraction);
}
if (resultFraction > result.m_fraction)
{
resultFraction = result.m_fraction;
}
}
BulletGlobals.VoronoiSimplexSolverPool.Free(voronoiSimplex);
BulletGlobals.SphereShapePool.Free(sphere1);
result.Cleanup();
}
}
/// Sphere (for convex0) against Convex1
{
ConvexShape convex1 = body1.GetCollisionShape() as ConvexShape;
SphereShape sphere0 = BulletGlobals.SphereShapePool.Get();
sphere0.Initialize(body0.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = BulletGlobals.CastResultPool.Get();
VoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
using (GjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get())
{
ccd1.Initialize(sphere0, convex1, voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(), body0.GetInterpolationWorldTransform(),
body1.GetWorldTransform(), body1.GetInterpolationWorldTransform(), result))
{
//store result.m_fraction in both bodies
if (body0.GetHitFraction() > result.m_fraction)
{
body0.SetHitFraction(result.m_fraction);
}
if (body1.GetHitFraction() > result.m_fraction)
{
body1.SetHitFraction(result.m_fraction);
}
if (resultFraction > result.m_fraction)
{
resultFraction = result.m_fraction;
}
}
BulletGlobals.VoronoiSimplexSolverPool.Free(voronoiSimplex);
BulletGlobals.SphereShapePool.Free(sphere0);
result.Cleanup();
}
}
return resultFraction;
}
public bool GetSphereDistance(CollisionObject boxObj, ref IndexedVector3 pointOnBox, ref IndexedVector3 normal, ref float penetrationDepth, IndexedVector3 sphereCenter, float fRadius, float maxContactDistance)
{
BoxShape boxShape = boxObj.GetCollisionShape() as BoxShape;
IndexedVector3 boxHalfExtent = boxShape.GetHalfExtentsWithoutMargin();
float boxMargin = boxShape.GetMargin();
penetrationDepth = 1.0f;
// convert the sphere position to the box's local space
IndexedMatrix m44T = boxObj.GetWorldTransform();
IndexedVector3 sphereRelPos = m44T.InvXform(sphereCenter);
// Determine the closest point to the sphere center in the box
IndexedVector3 closestPoint = sphereRelPos;
closestPoint.X = (Math.Min(boxHalfExtent.X, closestPoint.X));
closestPoint.X = (Math.Max(-boxHalfExtent.X, closestPoint.X));
closestPoint.Y = (Math.Min(boxHalfExtent.Y, closestPoint.Y));
closestPoint.Y = (Math.Max(-boxHalfExtent.Y, closestPoint.Y));
closestPoint.Z = (Math.Min(boxHalfExtent.Z, closestPoint.Z));
closestPoint.Z = (Math.Max(-boxHalfExtent.Z, closestPoint.Z));
float intersectionDist = fRadius + boxMargin;
float contactDist = intersectionDist + maxContactDistance;
normal = sphereRelPos - closestPoint;
//if there is no penetration, we are done
float dist2 = normal.LengthSquared();
if (dist2 > contactDist * contactDist)
{
return false;
}
float distance;
//special case if the sphere center is inside the box
if (dist2 == 0.0f)
{
distance = -GetSpherePenetration(ref boxHalfExtent, ref sphereRelPos, ref closestPoint, ref normal);
}
else //compute the penetration details
{
distance = normal.Length();
normal /= distance;
}
pointOnBox = closestPoint + normal * boxMargin;
// v3PointOnSphere = sphereRelPos - (normal * fRadius);
penetrationDepth = distance - intersectionDist;
// transform back in world space
IndexedVector3 tmp = m44T * pointOnBox;
pointOnBox = tmp;
// tmp = m44T(v3PointOnSphere);
// v3PointOnSphere = tmp;
tmp = m44T._basis * normal;
normal = tmp;
return true;
}