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 = (ConvexShape)(body0.GetCollisionShape());
ConvexShape min1 = (ConvexShape)(body1.GetCollisionShape());
Vector3 normalOnB = Vector3.Zero;
Vector3 pointOnBWorld = Vector3.Zero;
{
ClosestPointInput input = new ClosestPointInput();
GjkPairDetector gjkPairDetector = new GjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.SetMinkowskiA(min0);
gjkPairDetector.SetMinkowskiB(min1);
{
input.m_maximumDistanceSquared = min0.Margin + min1.Margin + m_manifoldPtr.GetContactBreakingThreshold();
input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
}
input.m_transformA = body0.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
gjkPairDetector.GetClosestPoints(input,resultOut,dispatchInfo.getDebugDraw(),false);
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);
}
//btVector3 v0,v1;
//btVector3 sepNormalWorldSpace;
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
public virtual bool CalcTimeOfImpact(ref Matrix fromA, ref Matrix toA, ref Matrix fromB, ref Matrix toB, CastResult result)
{
m_simplexSolver.Reset();
/// compute linear and angular velocity for this interval, to interpolate
Vector3 linVelA = Vector3.Zero, angVelA = Vector3.Zero, linVelB = Vector3.Zero, angVelB = Vector3.Zero;
TransformUtil.CalculateVelocity(ref fromA,ref toA,1f,ref linVelA,ref angVelA);
TransformUtil.CalculateVelocity(ref fromB,ref toB,1f,ref linVelB,ref angVelB);
float boundingRadiusA = m_convexA.GetAngularMotionDisc();
float boundingRadiusB = m_convexB.GetAngularMotionDisc();
float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA + angVelB.Length() * boundingRadiusB;
Vector3 relLinVel = (linVelB-linVelA);
float relLinVelocLength = relLinVel.Length();
if (MathUtil.FuzzyZero(relLinVelocLength + maxAngularProjectedVelocity))
{
return false;
}
float radius = 0.001f;
float lambda = 0f;
Vector3 v = new Vector3(1,0,0);
int maxIter = MAX_ITERATIONS;
Vector3 n = Vector3.Zero;
bool hasResult = false;
Vector3 c;
float lastLambda = lambda;
//btScalar epsilon = btScalar(0.001);
int numIter = 0;
//first solution, using GJK
Matrix identityTrans = Matrix.Identity;
SphereShape raySphere = new SphereShape(0f);
raySphere.Margin = 0f;
// result.drawCoordSystem(sphereTr);
PointCollector pointCollector1 = new PointCollector();
{
GjkPairDetector gjk = new GjkPairDetector(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
ClosestPointInput input = new ClosestPointInput();
//we don't use margins during CCD
// gjk.setIgnoreMargin(true);
input.m_transformA = fromA;
input.m_transformB = fromB;
gjk.GetClosestPoints(input,pointCollector1,null,false);
hasResult = pointCollector1.m_hasResult;
c = pointCollector1.m_pointInWorld;
}
if (hasResult)
{
float dist = pointCollector1.m_distance;
n = pointCollector1.m_normalOnBInWorld;
float projectedLinearVelocity = Vector3.Dot(relLinVel,n);
//not close enough
while (dist > radius)
{
if (result.m_debugDrawer != null)
{
Vector3 colour = new Vector3(1, 1, 1);
result.m_debugDrawer.DrawSphere(ref c, 0.2f, ref colour);
}
numIter++;
if (numIter > maxIter)
{
return false; //todo: report a failure
}
float dLambda = 0f;
projectedLinearVelocity = Vector3.Dot(relLinVel,n);
//calculate safe moving fraction from distance / (linear+rotational velocity)
//btScalar clippedDist = GEN_min(angularConservativeRadius,dist);
//btScalar clippedDist = dist;
//don't report time of impact for motion away from the contact normal (or causes minor penetration)
if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=MathUtil.SIMD_EPSILON)
{
return false;
}
dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
lambda = lambda + dLambda;
if (lambda > 1f || lambda < 0f)
{
return false;
}
//todo: next check with relative epsilon
if (lambda <= lastLambda)
{
return false;
//n.setValue(0,0,0);
}
lastLambda = lambda;
//interpolate to next lambda
Matrix interpolatedTransA = Matrix.Identity, interpolatedTransB = Matrix.Identity, relativeTrans = Matrix.Identity;
TransformUtil.IntegrateTransform(ref fromA,ref linVelA,ref angVelA,lambda,ref interpolatedTransA);
TransformUtil.IntegrateTransform(ref fromB,ref linVelB,ref angVelB,lambda,ref interpolatedTransB);
//relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
relativeTrans = MathUtil.InverseTimes(ref interpolatedTransB, ref interpolatedTransA);
if (result.m_debugDrawer != null)
{
result.m_debugDrawer.DrawSphere(interpolatedTransA.Translation, 0.2f, new Vector3(1, 0, 0));
}
result.DebugDraw( lambda );
PointCollector pointCollector = new PointCollector();
GjkPairDetector gjk = new GjkPairDetector(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
ClosestPointInput input = new ClosestPointInput();
input.m_transformA = interpolatedTransA;
input.m_transformB = interpolatedTransB;
gjk.GetClosestPoints(input,pointCollector,null,false);
if (pointCollector.m_hasResult)
{
if (pointCollector.m_distance < 0f)
{
//degenerate ?!
result.m_fraction = lastLambda;
n = pointCollector.m_normalOnBInWorld;
result.m_normal=n;//.setValue(1,1,1);// = n;
result.m_hitPoint = pointCollector.m_pointInWorld;
return true;
}
c = pointCollector.m_pointInWorld;
n = pointCollector.m_normalOnBInWorld;
dist = pointCollector.m_distance;
} else
{
//??
return false;
}
}
if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= result.m_allowedPenetration)//SIMD_EPSILON)
{
return false;
}
result.m_fraction = lambda;
result.m_normal = n;
result.m_hitPoint = c;
return true;
}
return false;
/*
//todo:
//if movement away from normal, discard result
btVector3 move = transBLocalTo.getOrigin() - transBLocalFrom.getOrigin();
if (result.m_fraction < btScalar(1.))
{
if (move.dot(result.m_normal) <= btScalar(0.))
{
}
}
*/
}
public bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref Matrix transA, ref Matrix transB,
ref Vector3 v, ref Vector3 pa, ref Vector3 pb, IDebugDraw debugDraw)
{
bool check2d = convexA.IsConvex2D() && convexB.IsConvex2D();
float minProj = float.MaxValue;
Vector3 minNorm = Vector3.Zero;
Vector3 minA = Vector3.Zero, minB = Vector3.Zero;
Vector3 seperatingAxisInA, seperatingAxisInB;
Vector3 pInA, qInB, pWorld, qWorld, w;
#if USE_BATCHED_SUPPORT
IList<Vector4> supportVerticesABatch = new ObjectArray<Vector4>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);
IList<Vector4> supportVerticesBBatch = new ObjectArray<Vector4>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);
IList<Vector3> seperatingAxisInABatch = new ObjectArray<Vector3>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);
IList<Vector3> seperatingAxisInBBatch = new ObjectArray<Vector3>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);
int numSampleDirections = NUM_UNITSPHERE_POINTS;
for (int i = 0; i < numSampleDirections; i++)
{
Vector3 norm = sPenetrationDirections[i];
seperatingAxisInABatch[i] = MathUtil.TransposeTransformNormal(-norm, transA);
seperatingAxisInBBatch[i] = MathUtil.TransposeTransformNormal(norm, transB);
}
{
int numPDA = convexA.GetNumPreferredPenetrationDirections();
if (numPDA > 0)
{
for (int i = 0; i < numPDA; i++)
{
Vector3 norm = Vector3.Up;
convexA.GetPreferredPenetrationDirection(i, ref norm);
norm = Vector3.TransformNormal(norm, transA);
sPenetrationDirections[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal(-norm, transA);
seperatingAxisInBBatch[numSampleDirections] = Vector3.Transform(norm, transB);
numSampleDirections++;
}
}
}
{
int numPDB = convexB.GetNumPreferredPenetrationDirections();
if (numPDB > 0)
{
for (int i = 0; i < numPDB; i++)
{
Vector3 norm = Vector3.Up;
convexB.GetPreferredPenetrationDirection(i, ref norm);
norm = Vector3.TransformNormal(norm, transB);
sPenetrationDirections[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal(-norm, transA);
seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transB);
numSampleDirections++;
}
}
}
convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch, numSampleDirections);
convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch, numSampleDirections);
for (int i = 0; i < numSampleDirections; i++)
{
Vector3 norm = sPenetrationDirections[i];
if (check2d)
{
// shouldn't this be Y ?
norm.Z = 0;
}
seperatingAxisInA = seperatingAxisInABatch[i];
seperatingAxisInB = seperatingAxisInBBatch[i];
pInA = new Vector3(supportVerticesABatch[i].X, supportVerticesABatch[i].Y, supportVerticesABatch[i].Z);
qInB = new Vector3(supportVerticesBBatch[i].X, supportVerticesBBatch[i].Y, supportVerticesBBatch[i].Z);
pWorld = Vector3.Transform(pInA, transA);
qWorld = Vector3.Transform(qInB, transB);
if (check2d)
{
// shouldn't this be Y ?
pWorld.Z = 0f;
qWorld.Z = 0f;
}
w = qWorld - pWorld;
float delta = Vector3.Dot(norm, w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
#else
int numSampleDirections = NUM_UNITSPHERE_POINTS;
{
int numPDA = convexA.getNumPreferredPenetrationDirections();
if (numPDA > 0)
{
for (int i=0;i<numPDA;i++)
{
Vector3 norm = Vector3.Zero;
convexA.getPreferredPenetrationDirection(i,ref norm);
norm = Vector3.TransformNormal(norm,transA);
sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
{
int numPDB = convexB.getNumPreferredPenetrationDirections();
if (numPDB > 0)
{
for (int i=0;i<numPDB;i++)
{
Vector3 norm = Vector3.Zero;
convexB.getPreferredPenetrationDirection(i,ref norm);
norm = Vector3.TransformNormal(norm,transB);
sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
for (int i=0;i<numSampleDirections;i++)
{
Vector3 norm = sPenetrationDirections[i];
if (check2d)
{
norm.Z = 0f;
}
if (norm.LengthSquared() > 0.01f)
{
seperatingAxisInA = Vector3.TransformNormal(-norm, transA);
seperatingAxisInB = Vector3.TransformNormal(norm, transB);
pInA = convexA.localGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA);
qInB = convexB.localGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB);
pWorld = Vector3.Transform(pInA, transA);
qWorld = Vector3.Transform(qInB, transB);
if (check2d)
{
pWorld.Z = 0.0f;
qWorld.Z = 0.0f;
}
w = qWorld - pWorld;
float delta = Vector3.Dot(norm, w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
}
#endif //USE_BATCHED_SUPPORT
//add the margins
minA += minNorm * convexA.GetMarginNonVirtual();
minB -= minNorm * convexB.GetMarginNonVirtual();
//no penetration
if (minProj < 0f)
{
return false;
}
float extraSeparation = 0.5f;///scale dependent
minProj += extraSeparation + (convexA.GetMarginNonVirtual() + convexB.GetMarginNonVirtual());
#if DEBUG_DRAW
if (debugDraw)
{
Vector3 color = new Vector3(0,1,0);
debugDraw.drawLine(minA,minB,color);
color = new Vector3(1,1,1);
Vector3 vec = minB-minA;
float prj2 = Vector3.Dot(minNorm,vec);
debugDraw.drawLine(minA,minA+(minNorm*minProj),color);
}
#endif //DEBUG_DRAW
GjkPairDetector gjkdet = new GjkPairDetector(convexA, convexB, simplexSolver, null);
float offsetDist = minProj;
Vector3 offset = minNorm * offsetDist;
ClosestPointInput input = new ClosestPointInput();
Vector3 newOrg = transA.Translation + offset;
Matrix displacedTrans = transA;
displacedTrans.Translation = newOrg;
input.m_transformA = displacedTrans;
input.m_transformB = transB;
input.m_maximumDistanceSquared = float.MaxValue;
MinkowskiIntermediateResult res = new MinkowskiIntermediateResult();
Vector3 temp = -minNorm;
gjkdet.SetCachedSeperatingAxis(-minNorm);
gjkdet.GetClosestPoints(input, res, debugDraw,false);
float correctedMinNorm = minProj - res.m_depth;
//the penetration depth is over-estimated, relax it
float penetration_relaxation = 1f;
minNorm *= penetration_relaxation;
if (res.m_hasResult)
{
pa = res.m_pointInWorld - minNorm * correctedMinNorm;
pb = res.m_pointInWorld;
v = minNorm;
#if DEBUG_DRAW
if (debugDraw != null)
{
Vector3 color = new Vector3(1,0,0);
debugDraw.drawLine(pa,pb,color);
}
#endif//DEBUG_DRAW
}
return res.m_hasResult;
}
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 = (ConvexShape)(body0.GetCollisionShape());
ConvexShape min1 = (ConvexShape)(body1.GetCollisionShape());
Vector3 normalOnB = Vector3.Up;
Vector3 pointOnBWorld = Vector3.Zero;
#if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
if ((min0.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE))
{
CapsuleShape capsuleA = (CapsuleShape) min0;
CapsuleShape capsuleB = (CapsuleShape) min1;
Vector3 localScalingA = capsuleA.GetLocalScaling();
Vector3 localScalingB = capsuleB.GetLocalScaling();
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
float dist = CapsuleCapsuleDistance(ref normalOnB,ref 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 = new ClosestPointInput();
GjkPairDetector gjkPairDetector = new GjkPairDetector(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.Margin + min1.Margin + 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();
gjkPairDetector.GetClosestPoints(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)
{
Vector3 v0 = Vector3.Zero, v1 = Vector3.Zero;
Vector3 sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis();
sepNormalWorldSpace.Normalize();
TransformUtil.PlaneSpace1(ref sepNormalWorldSpace, ref v0, ref 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;
}
Matrix unPerturbedTransform = Matrix.Identity;
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)
{
Quaternion perturbeRot = Quaternion.CreateFromAxisAngle(v0, perturbeAngle);
float iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations);
Quaternion rotq = Quaternion.CreateFromAxisAngle(sepNormalWorldSpace, iterationAngle);
if (perturbeA)
{
//input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0.getWorldTransform().getBasis());
Quaternion temp = MathUtil.QuaternionMultiply(MathUtil.QuaternionInverse(ref rotq),MathUtil.QuaternionMultiply(perturbeRot,rotq));
input.m_transformA = MathUtil.BulletMatrixMultiplyBasis(Matrix.CreateFromQuaternion(temp),body0.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.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1.getWorldTransform().getBasis());
Quaternion temp = MathUtil.QuaternionMultiply(MathUtil.QuaternionInverse(ref rotq),MathUtil.QuaternionMultiply(perturbeRot,rotq));
input.m_transformB = MathUtil.BulletMatrixMultiplyBasis(Matrix.CreateFromQuaternion(temp),body1.GetWorldTransform());
#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(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();
}
}