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 ComputeClosestPoints(ref IndexedMatrix transA, ref IndexedMatrix transB, PointCollector pointCollector)
{
if (m_convexB1 != null)
{
m_simplexSolver.Reset();
GjkPairDetector gjk = new GjkPairDetector(m_convexA, m_convexB1, m_convexA.GetShapeType(), m_convexB1.GetShapeType(), m_convexA.GetMargin(), m_convexB1.GetMargin(), m_simplexSolver, m_penetrationDepthSolver);
ClosestPointInput input = ClosestPointInput.Default();
input.m_transformA = transA;
input.m_transformB = transB;
gjk.GetClosestPoints(ref input, pointCollector, null);
}
else
{
//convex versus plane
ConvexShape convexShape = m_convexA;
StaticPlaneShape planeShape = m_planeShape;
bool hasCollision = false;
IndexedVector3 planeNormal = planeShape.GetPlaneNormal();
float planeConstant = planeShape.GetPlaneConstant();
IndexedMatrix convexWorldTransform = transA;
IndexedMatrix convexInPlaneTrans = transB.Inverse() * convexWorldTransform;
IndexedMatrix planeInConvex = convexWorldTransform.Inverse() * transB;
IndexedVector3 vtx = convexShape.LocalGetSupportingVertex(planeInConvex._basis * -planeNormal);
IndexedVector3 vtxInPlane = convexInPlaneTrans * vtx;
float distance = IndexedVector3.Dot(planeNormal, vtxInPlane) - planeConstant;
IndexedVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
IndexedVector3 vtxInPlaneWorld = transB * vtxInPlaneProjected;
IndexedVector3 normalOnSurfaceB = transB._basis * planeNormal;
pointCollector.AddContactPoint(
ref normalOnSurfaceB,
ref vtxInPlaneWorld,
distance);
}
}
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 virtual bool CalcTimeOfImpact(ref IndexedMatrix fromA, ref IndexedMatrix toA, ref IndexedMatrix fromB, ref IndexedMatrix toB, CastResult result)
{
m_simplexSolver.Reset();
/// compute linear velocity for this interval, to interpolate
//assume no rotation/angular velocity, assert here?
IndexedVector3 linVelA, linVelB;
linVelA = toA._origin - fromA._origin;
linVelB = toB._origin - fromB._origin;
float radius = 0.001f;
float lambda = 0f;
IndexedVector3 v = new IndexedVector3(1, 0, 0);
int maxIter = MAX_ITERATIONS;
IndexedVector3 n = IndexedVector3.Zero;
bool hasResult = false;
IndexedVector3 c;
IndexedVector3 r = (linVelA - linVelB);
float lastLambda = lambda;
//float epsilon = float(0.001);
int numIter = 0;
//first solution, using GJK
IndexedMatrix identityTrans = IndexedMatrix.Identity;
// result.drawCoordSystem(sphereTr);
PointCollector pointCollector = new PointCollector();
using (GjkPairDetector gjk = BulletGlobals.GjkPairDetectorPool.Get())
{
gjk.Initialize(m_convexA, m_convexB, m_simplexSolver, null);//m_penetrationDepthSolver);
ClosestPointInput input = ClosestPointInput.Default();
//we don't use margins during CCD
// gjk.setIgnoreMargin(true);
input.m_transformA = fromA;
input.m_transformB = fromB;
gjk.GetClosestPoints(ref input, pointCollector, null, false);
hasResult = pointCollector.m_hasResult;
c = pointCollector.m_pointInWorld;
if (hasResult)
{
float dist = pointCollector.m_distance;
n = pointCollector.m_normalOnBInWorld;
//not close enough
while (dist > radius)
{
numIter++;
if (numIter > maxIter)
{
return(false); //todo: report a failure
}
float dLambda = 0f;
float projectedLinearVelocity = IndexedVector3.Dot(r, n);
dLambda = dist / (projectedLinearVelocity);
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);
//break;
}
lastLambda = lambda;
//interpolate to next lambda
result.DebugDraw(lambda);
input.m_transformA._origin = MathUtil.Interpolate3(fromA._origin, toA._origin, lambda);
input.m_transformB._origin = MathUtil.Interpolate3(fromB._origin, toB._origin, lambda);
gjk.GetClosestPoints(ref input, pointCollector, null, false);
if (pointCollector.m_hasResult)
{
if (pointCollector.m_distance < 0f)
{
result.m_fraction = lastLambda;
n = pointCollector.m_normalOnBInWorld;
result.m_normal = n;
result.m_hitPoint = pointCollector.m_pointInWorld;
return(true);
}
c = pointCollector.m_pointInWorld;
n = pointCollector.m_normalOnBInWorld;
dist = pointCollector.m_distance;
}
else
{
//??
return(false);
}
}
//is n normalized?
//don't report time of impact for motion away from the contact normal (or causes minor penetration)
if (IndexedVector3.Dot(n, r) >= -result.m_allowedPenetration)
{
return(false);
}
result.m_fraction = lambda;
result.m_normal = n;
result.m_hitPoint = c;
return(true);
}
}
return(false);
}
public bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref IndexedMatrix transA, ref IndexedMatrix transB,
ref IndexedVector3 v, ref IndexedVector3 pa, ref IndexedVector3 pb, IDebugDraw debugDraw)
{
bool check2d = convexA.IsConvex2d() && convexB.IsConvex2d();
float minProj = float.MaxValue;
IndexedVector3 minNorm = IndexedVector3.Zero;
IndexedVector3 minA = IndexedVector3.Zero, minB = IndexedVector3.Zero;
IndexedVector3 seperatingAxisInA, seperatingAxisInB;
IndexedVector3 pInA, qInB, pWorld, qWorld, w;
#if USE_BATCHED_SUPPORT
IndexedVector4[] supportVerticesABatch = new IndexedVector4[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
IndexedVector4[] supportVerticesBBatch = new IndexedVector4[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
IndexedVector3[] seperatingAxisInABatch = new IndexedVector3[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
IndexedVector3[] seperatingAxisInBBatch = new IndexedVector3[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
int numSampleDirections = NUM_UNITSPHERE_POINTS;
for (int i = 0; i < numSampleDirections; i++)
{
IndexedVector3 norm = sPenetrationDirections[i];
IndexedVector3 negNorm = -norm;
IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[i], ref negNorm, ref transA._basis);
IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[i], ref norm, ref transB._basis);
//seperatingAxisInABatch[i] = (-norm) * transA._basis;
//seperatingAxisInBBatch[i] = norm * transB._basis;
}
{
int numPDA = convexA.GetNumPreferredPenetrationDirections();
if (numPDA > 0)
{
for (int i = 0; i < numPDA; i++)
{
IndexedVector3 norm;
convexA.GetPreferredPenetrationDirection(i, out norm);
IndexedBasisMatrix.Multiply(ref norm, ref transA._basis, ref norm);
sPenetrationDirections[numSampleDirections] = norm;
IndexedVector3 negNorm = -norm;
IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[numSampleDirections], ref negNorm, ref transA._basis);
IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[numSampleDirections], ref norm, ref transB._basis);
numSampleDirections++;
}
}
}
{
int numPDB = convexB.GetNumPreferredPenetrationDirections();
if (numPDB > 0)
{
for (int i = 0; i < numPDB; i++)
{
IndexedVector3 norm;
convexB.GetPreferredPenetrationDirection(i, out norm);
IndexedBasisMatrix.Multiply(ref norm, ref transB._basis, ref norm);
sPenetrationDirections[numSampleDirections] = norm;
IndexedVector3 negNorm = -norm;
IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[numSampleDirections], ref negNorm, ref transA._basis);
IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[numSampleDirections], ref norm, ref transB._basis);
numSampleDirections++;
}
}
}
convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch, numSampleDirections);
convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch, numSampleDirections);
for (int i = 0; i < numSampleDirections; i++)
{
IndexedVector3 norm = sPenetrationDirections[i];
if (check2d)
{
// shouldn't this be Y ?
norm.Z = 0;
}
if (norm.LengthSquared() > 0.01f)
{
seperatingAxisInA = seperatingAxisInABatch[i];
seperatingAxisInB = seperatingAxisInBBatch[i];
pInA = new IndexedVector3(supportVerticesABatch[i].X, supportVerticesABatch[i].Y, supportVerticesABatch[i].Z);
qInB = new IndexedVector3(supportVerticesBBatch[i].X, supportVerticesBBatch[i].Y, supportVerticesBBatch[i].Z);
IndexedMatrix.Multiply(out pWorld, ref transA, ref pInA);
IndexedMatrix.Multiply(out qWorld, ref transB, ref qInB);
if (check2d)
{
// shouldn't this be Y ?
pWorld.Z = 0f;
qWorld.Z = 0f;
}
IndexedVector3.Subtract(out w, ref qWorld, ref pWorld);
float delta = IndexedVector3.Dot(ref norm, ref 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++)
{
IndexedVector3 norm;
convexA.GetPreferredPenetrationDirection(i, out norm);
norm = IndexedVector3.TransformNormal(norm, transA);
sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
{
int numPDB = convexB.GetNumPreferredPenetrationDirections();
if (numPDB > 0)
{
for (int i = 0; i < numPDB; i++)
{
IndexedVector3 norm = IndexedVector3.Zero;
convexB.GetPreferredPenetrationDirection(i, out norm);
norm = IndexedVector3.TransformNormal(norm, transB);
sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
for (int i = 0; i < numSampleDirections; i++)
{
IndexedVector3 norm = sPenetrationDirections[i];
if (check2d)
{
norm.Z = 0f;
}
if (norm.LengthSquared() > 0.01f)
{
seperatingAxisInA = IndexedVector3.TransformNormal(-norm, transA);
seperatingAxisInB = IndexedVector3.TransformNormal(norm, transB);
pInA = convexA.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA);
qInB = convexB.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB);
pWorld = IndexedVector3.Transform(pInA, transA);
qWorld = IndexedVector3.Transform(qInB, transB);
if (check2d)
{
pWorld.Z = 0.0f;
qWorld.Z = 0.0f;
}
w = qWorld - pWorld;
float delta = IndexedVector3.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)
{
IndexedVector3 color = new IndexedVector3(0, 1, 0);
debugDraw.drawLine(minA, minB, color);
color = new IndexedVector3(1, 1, 1);
IndexedVector3 vec = minB - minA;
float prj2 = IndexedVector3.Dot(minNorm, vec);
debugDraw.drawLine(minA, minA + (minNorm * minProj), color);
}
#endif //DEBUG_DRAW
GjkPairDetector gjkdet = BulletGlobals.GjkPairDetectorPool.Get();
gjkdet.Initialize(convexA, convexB, simplexSolver, null);
float offsetDist = minProj;
IndexedVector3 offset = minNorm * offsetDist;
ClosestPointInput input = ClosestPointInput.Default();
IndexedVector3 newOrg = transA._origin + offset;
IndexedMatrix displacedTrans = transA;
displacedTrans._origin = newOrg;
input.m_transformA = displacedTrans;
input.m_transformB = transB;
input.m_maximumDistanceSquared = float.MaxValue;
MinkowskiIntermediateResult res = new MinkowskiIntermediateResult();
gjkdet.SetCachedSeperatingAxis(-minNorm);
gjkdet.GetClosestPoints(ref 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)
{
IndexedVector3 color = new IndexedVector3(1, 0, 0);
debugDraw.drawLine(pa, pb, color);
}
#endif//DEBUG_DRAW
}
BulletGlobals.GjkPairDetectorPool.Free(gjkdet);
return(res.m_hasResult);
}