public override void processCollision(CollisionObject col0, CollisionObject col1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
return;
resultOut.PersistentManifold = m_manifoldPtr;
SphereShape sphere0 = (SphereShape)col0.CollisionShape;
SphereShape sphere1 = (SphereShape)col1.CollisionShape;
btVector3 diff = col0.WorldTransform.Origin - col1.WorldTransform.Origin;
float len = diff.Length;
float radius0 = sphere0.Radius;
float radius1 = sphere1.Radius;
#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);
btVector3 normalOnSurfaceB = new btVector3(1, 0, 0);
if (len > BulletGlobal.SIMD_EPSILON)
{
//normalOnSurfaceB = diff / len;
btVector3.Divide(ref diff, len, out normalOnSurfaceB);
}
///point on A (worldspace)
///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
///point on B (worldspace)
btVector3 pos1;// = col1.WorldTransform.Origin + radius1 * normalOnSurfaceB;
{
btVector3 temp;
btVector3.Multiply(ref normalOnSurfaceB, radius1, out temp);
btVector3.Add(ref col1.WorldTransform.Origin, ref temp, out pos1);
}
/// 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 void addContactPoint(btVector3 normalOnBInWorld,btVector3 pointInWorld,float orgDepth,ref ManifoldResult originalManifoldResult)
{
btVector3 endPt,startPt;
float newDepth;
//btVector3 newNormal;
if (m_perturbA)
{
btVector3 endPtOrg;// = pointInWorld + normalOnBInWorld*orgDepth;
{
btVector3 temp;
btVector3.Multiply(ref normalOnBInWorld, orgDepth, out temp);
btVector3.Add(ref pointInWorld, ref temp, out endPtOrg);
}
endPt = btVector3.Transform(endPtOrg, m_unPerturbedTransform * m_transformA.inverse());
#region newDepth = (endPt - pointInWorld).dot(normalOnBInWorld);
{
btVector3 temp;
btVector3.Subtract(ref endPt, ref pointInWorld, out temp);
newDepth = temp.dot(ref normalOnBInWorld);
}
#endregion
#region startPt = endPt + normalOnBInWorld * newDepth;
{
btVector3 temp;
btVector3.Multiply(ref normalOnBInWorld, newDepth, out temp);
btVector3.Multiply(ref endPt, ref temp, out startPt);
}
#endregion
}
else
{
#region endPt = pointInWorld + normalOnBInWorld*orgDepth;
{
btVector3 temp;
btVector3.Multiply(ref normalOnBInWorld, orgDepth, out temp);
btVector3.Add(ref pointInWorld, ref temp, out endPt);
}
#endregion
startPt = btVector3.Transform(pointInWorld, m_unPerturbedTransform * m_transformB.inverse());
#region newDepth = (endPt - startPt).dot(normalOnBInWorld);
{
btVector3 temp;
btVector3.Subtract(ref endPt, ref startPt, out temp);
newDepth = temp.dot(ref normalOnBInWorld);
}
#endregion
}
originalManifoldResult.addContactPoint(ref normalOnBInWorld,ref startPt,newDepth);
}
public override void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
return;
CollisionObject convexObj = m_isSwapped ? body1 : body0;
CollisionObject planeObj = m_isSwapped ? body0 : body1;
ConvexShape convexShape = (ConvexShape)convexObj.CollisionShape;
StaticPlaneShape planeShape = (StaticPlaneShape)planeObj.CollisionShape;
btVector3 planeNormal = planeShape.PlaneNormal;
//const btScalar& planeConstant = planeShape->getPlaneConstant();
//first perform a collision query with the non-perturbated collision objects
{
btQuaternion rotq = new btQuaternion(0, 0, 0, 1);
collideSingleContact(rotq, body0, body1, dispatchInfo, ref resultOut);
}
if (resultOut.PersistentManifold.NumContacts < m_minimumPointsPerturbationThreshold)
{
btVector3 v0, v1;
btVector3.PlaneSpace1(ref planeNormal, out v0, out v1);
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
float angleLimit = 0.125f * BulletGlobal.SIMD_PI;
float perturbeAngle;
float radius = convexShape.getAngularMotionDisc();
perturbeAngle = PersistentManifold.gContactBreakingThreshold / radius;
if (perturbeAngle > angleLimit)
perturbeAngle = angleLimit;
btQuaternion perturbeRot = new btQuaternion(v0, perturbeAngle);
for (int i = 0; i < m_numPerturbationIterations; i++)
{
float iterationAngle = (float)i * (BulletGlobal.SIMD_2_PI / (float)m_numPerturbationIterations);
btQuaternion rotq = new btQuaternion(planeNormal, iterationAngle);
collideSingleContact(rotq.inverse() * perturbeRot * rotq, body0, body1, dispatchInfo, ref resultOut);
}
}
if (m_ownManifold)
{
if (m_manifoldPtr.NumContacts != 0)
{
resultOut.refreshContactPoints();
}
}
}
public void getClosestPoints(ref ClosestPointInput input, ref ManifoldResult output, IDebugDraw debugDraw)
{
//float* R1 = stackalloc float[12];
//float* R2 = stackalloc float[12];
StackPtr<float> R1 = StackPtr<float>.Allocate(12);
StackPtr<float> R2 = StackPtr<float>.Allocate(12);
try
{
for (int j = 0; j < 3; j++)
{
R1[0 + 4 * j] = input.m_transformA.Basis[j].X;
R2[0 + 4 * j] = input.m_transformB.Basis[j].X;
R1[1 + 4 * j] = input.m_transformA.Basis[j].Y;
R2[1 + 4 * j] = input.m_transformB.Basis[j].Y;
R1[2 + 4 * j] = input.m_transformA.Basis[j].Z;
R2[2 + 4 * j] = input.m_transformB.Basis[j].Z;
}
btVector3 normal;
float depth;
int return_code;
int maxc = 4;
dBoxBox2(input.m_transformA.Origin,
R1,
m_box1.HalfExtentsWithMargin * 2f,
input.m_transformB.Origin,
R2,
m_box2.HalfExtentsWithMargin * 2f,
out normal, out depth, out return_code,
maxc,
ref output
);
}
finally
{
R1.Dispose();
R2.Dispose();
}
}
public override void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
return;
CollisionObject sphereObj = m_isSwapped ? body1 : body0;
CollisionObject boxObj = m_isSwapped ? body0 : body1;
SphereShape sphere0 = (SphereShape)sphereObj.CollisionShape;
//btVector3 normalOnSurfaceB;
btVector3 pOnBox=btVector3.Zero, pOnSphere=btVector3.Zero;
btVector3 sphereCenter = sphereObj.WorldTransform.Origin;
float radius = sphere0.Radius;
float dist = getSphereDistance(boxObj, ref pOnBox, ref pOnSphere, sphereCenter, radius);
resultOut.PersistentManifold = m_manifoldPtr;
if (dist < BulletGlobal.SIMD_EPSILON)
{
btVector3 normalOnSurfaceB;// = (pOnBox - pOnSphere).normalize();
(pOnBox - pOnSphere).normalize(out normalOnSurfaceB);
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.addContactPoint(ref normalOnSurfaceB, ref pOnBox, dist);
}
if (m_ownManifold)
{
if (m_manifoldPtr.NumContacts != 0)
{
resultOut.refreshContactPoints();
}
}
}
public abstract float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut);
public abstract void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut);
public override float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut)
{
//not yet
return 1f;
}
public override float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut)
{
throw new NotImplementedException();
}
public override void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut)
{
throw new NotImplementedException();
}
public override void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
//swapped?
m_manifoldPtr = m_dispatcher.getNewManifold(body0, body1);
m_ownManifold = true;
}
resultOut.PersistentManifold = m_manifoldPtr;
//comment-out next line to test multi-contact generation
//resultOut->getPersistentManifold()->clearManifold();
ConvexShape min0 = (ConvexShape)(body0.CollisionShape);
ConvexShape min1 = (ConvexShape)(body1.CollisionShape);
btVector3 normalOnB;
btVector3 pointOnBWorld;
if ((min0.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE))
{
CapsuleShape capsuleA = (CapsuleShape)min0;
CapsuleShape capsuleB = (CapsuleShape)min1;
btVector3 localScalingA = capsuleA.LocalScaling;
btVector3 localScalingB = capsuleB.LocalScaling;
float threshold = m_manifoldPtr.ContactBreakingThreshold;
float dist = capsuleCapsuleDistance(out normalOnB, out pointOnBWorld, capsuleA.HalfHeight, capsuleA.Radius,
capsuleB.HalfHeight, capsuleB.Radius, capsuleA.UpAxis, capsuleB.UpAxis,
body0.WorldTransform, body1.WorldTransform, threshold);
if (dist < threshold)
{
Debug.Assert(normalOnB.Length2 >= (BulletGlobal.SIMD_EPSILON * BulletGlobal.SIMD_EPSILON));
resultOut.addContactPoint(ref normalOnB, ref pointOnBWorld, dist);
}
resultOut.refreshContactPoints();
return;
}
#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;
GjkPairDetector gjkPairDetector = new GjkPairDetector(min0, min1, m_simplexSolver, m_pdSolver);
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.MinkowskiA = min0;
gjkPairDetector.MinkowskiB = min1;
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
} else
#endif //USE_SEPDISTANCE_UTIL2
{
input.m_maximumDistanceSquared = min0.Margin + min1.Margin + m_manifoldPtr.ContactBreakingThreshold;
input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
}
//input.m_stackAlloc = dispatchInfo.m_stackAllocator;
input.m_transformA = body0.WorldTransform;
input.m_transformB = body1.WorldTransform;
gjkPairDetector.getClosestPoints(ref input, ref resultOut, dispatchInfo.m_debugDraw);
#if USE_SEPDISTANCE_UTIL2
btScalar sepDist = 0.f;
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
sepDist = gjkPairDetector.getCachedSeparatingDistance();
if (sepDist>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.PersistentManifold.NumContacts < m_minimumPointsPerturbationThreshold)
{
int i;
btVector3 v0, v1;
btVector3 sepNormalWorldSpace;
//sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
gjkPairDetector.getCachedSeparatingAxis().normalized(out sepNormalWorldSpace);
btVector3.PlaneSpace1(ref sepNormalWorldSpace, out v0, out v1);
bool perturbeA = true;
float angleLimit = 0.125f * BulletGlobal.SIMD_PI;
float perturbeAngle;
float radiusA = min0.getAngularMotionDisc();
float radiusB = min1.getAngularMotionDisc();
if (radiusA < radiusB)
{
perturbeAngle = PersistentManifold.gContactBreakingThreshold / radiusA;
perturbeA = true;
}
else
{
perturbeAngle = PersistentManifold.gContactBreakingThreshold / radiusB;
perturbeA = false;
}
if (perturbeAngle > angleLimit)
perturbeAngle = angleLimit;
btTransform unPerturbedTransform;
if (perturbeA)
{
unPerturbedTransform = input.m_transformA;
}
else
{
unPerturbedTransform = input.m_transformB;
}
for (i = 0; i < m_numPerturbationIterations; i++)
{
if (v0.Length2 > BulletGlobal.SIMD_EPSILON)
{
btQuaternion perturbeRot = new btQuaternion(v0, perturbeAngle);
float iterationAngle = i * (BulletGlobal.SIMD_2_PI / m_numPerturbationIterations);
btQuaternion rotq = new btQuaternion(sepNormalWorldSpace, iterationAngle);
if (perturbeA)
{
#region input.m_transformA.Basis = new btMatrix3x3(rotq.inverse() * perturbeRot * rotq) * body0.WorldTransform.Basis;
{
btMatrix3x3 temp = new btMatrix3x3(rotq.inverse() * perturbeRot * rotq);
btMatrix3x3.Multiply(ref temp, ref body0.WorldTransform.Basis, out input.m_transformA.Basis);
}
#endregion
input.m_transformB = body1.WorldTransform;
#if DEBUG
dispatchInfo.m_debugDraw.drawTransform(ref input.m_transformA, 10.0f);
#endif //DEBUG_CONTACTS
}
else
{
input.m_transformA = body0.WorldTransform;
#region input.m_transformB.Basis = new btMatrix3x3(rotq.inverse() * perturbeRot * rotq) * body1.WorldTransform.Basis;
{
btMatrix3x3 temp = new btMatrix3x3(rotq.inverse() * perturbeRot * rotq);
btMatrix3x3.Multiply(ref temp, ref body1.WorldTransform.Basis, out input.m_transformB.Basis);
}
#endregion
#if DEBUG
dispatchInfo.m_debugDraw.drawTransform(ref input.m_transformB, 10.0f);
#endif
}
PerturbedContactResult perturbedResultOut = new PerturbedContactResult(input.m_transformA, input.m_transformB, unPerturbedTransform, perturbeA, dispatchInfo.m_debugDraw);
gjkPairDetector.getClosestPoints(ref input, ref perturbedResultOut, ref resultOut, dispatchInfo.m_debugDraw);
}
}
}
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
{
m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
}
#endif //USE_SEPDISTANCE_UTIL2
}
if (m_ownManifold)
{
resultOut.refreshContactPoints();
}
}
public override float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut)
{
throw new NotImplementedException();
#if false//未移植
///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
///col0->m_worldTransform,
float resultFraction = btScalar(1.);
float squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
float squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
squareMot1 < col1->getCcdSquareMotionThreshold())
return resultFraction;
if (disableCcd)
return btScalar(1.);
//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 = static_cast<btConvexShape*>(col0->getCollisionShape());
SphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
btConvexCast::CastResult result;
btVoronoiSimplexSolver voronoiSimplex;
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
{
//store result.m_fraction in both bodies
if (col0->getHitFraction()> result.m_fraction)
col0->setHitFraction( result.m_fraction );
if (col1->getHitFraction() > result.m_fraction)
col1->setHitFraction( result.m_fraction);
if (resultFraction > result.m_fraction)
resultFraction = result.m_fraction;
}
}
/// Sphere (for convex0) against Convex1
{
btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
btConvexCast::CastResult result;
btVoronoiSimplexSolver voronoiSimplex;
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
{
//store result.m_fraction in both bodies
if (col0->getHitFraction() > result.m_fraction)
col0->setHitFraction( result.m_fraction);
if (col1->getHitFraction() > result.m_fraction)
col1->setHitFraction( result.m_fraction);
if (resultFraction > result.m_fraction)
resultFraction = result.m_fraction;
}
}
return resultFraction;
#endif
}
static unsafe int dBoxBox2(btVector3 p1, float[] R1safe, btVector3 side1, btVector3 p2, float[] R2safe, btVector3 side2,
out btVector3 normal, out float depth, out int return_code, int maxc, ref ManifoldResult output)
{
fixed (float* R1 = &R1safe[0], R2 = &R2safe[0])
{
float fudge_factor = 1.05f;
btVector3 p, pp, normalC = new btVector3(0f, 0f, 0f);
float* normalR = null;
//float* A = stackalloc float[3], B = stackalloc float[3];
StackPtr<float> A = StackPtr<float>.Allocate(3);
StackPtr<float> B = StackPtr<float>.Allocate(3);
try
{
float R11, R12, R13, R21, R22, R23, R31, R32, R33,
Q11, Q12, Q13, Q21, Q22, Q23, Q31, Q32, Q33, s, s2, l;
int i, j, code;
bool invert_normal;
normal = btVector3.Zero;
depth = 0;
return_code = -1;
// get vector from centers of box 1 to box 2, relative to box 1
p = p2 - p1;
pp = new btVector3(dDOT41((R1), ref p), dDOT41((R1 + 1), ref p), dDOT41((R1 + 2), ref p));
//dMULTIPLYOP1_331 (pp,=,R1,p); // get pp = p relative to body 1
// get side lengths / 2
A[0] = side1.X * 0.5f;
A[1] = side1.Y * 0.5f;
A[2] = side1.Z * 0.5f;
B[0] = side2.X * 0.5f;
B[1] = side2.Y * 0.5f;
B[2] = side2.Z * 0.5f;
// Rij is R1'*R2, i.e. the relative rotation between R1 and R2
R11 = dDOT44(R1 + 0, R2 + 0); R12 = dDOT44(R1 + 0, R2 + 1); R13 = dDOT44(R1 + 0, R2 + 2);
R21 = dDOT44(R1 + 1, R2 + 0); R22 = dDOT44(R1 + 1, R2 + 1); R23 = dDOT44(R1 + 1, R2 + 2);
R31 = dDOT44(R1 + 2, R2 + 0); R32 = dDOT44(R1 + 2, R2 + 1); R33 = dDOT44(R1 + 2, R2 + 2);
Q11 = (float)Math.Abs(R11); Q12 = (float)Math.Abs(R12); Q13 = (float)Math.Abs(R13);
Q21 = (float)Math.Abs(R21); Q22 = (float)Math.Abs(R22); Q23 = (float)Math.Abs(R23);
Q31 = (float)Math.Abs(R31); Q32 = (float)Math.Abs(R32); Q33 = (float)Math.Abs(R33);
// for all 15 possible separating axes:
// * see if the axis separates the boxes. if so, return 0.
// * find the depth of the penetration along the separating axis (s2)
// * if this is the largest depth so far, record it.
// the normal vector will be set to the separating axis with the smallest
// depth. note: normalR is set to point to a column of R1 or R2 if that is
// the smallest depth normal so far. otherwise normalR is 0 and normalC is
// set to a vector relative to body 1. invert_normal is 1 if the sign of
// the normal should be flipped.
{
//TSTマクロ有効域1
/*TST(expr1,expr2,norm,cc)
{
s2 = (float)Math.Abs(expr1) - (expr2);
if (s2 > 0) return 0;
if (s2 > s) {
s = s2;
normalR = norm;
invert_normal = ((expr1) < 0);
code = (cc);
}
}*/
s = float.MinValue;
invert_normal = false;
code = 0;
// separating axis = u1,u2,u3
//TST(pp[0], (A[0] + B[0] * Q11 + B[1] * Q12 + B[2] * Q13), R1 + 0, 1);
{
s2 = (float)Math.Abs(pp.X) - ((A[0] + B[0] * Q11 + B[1] * Q12 + B[2] * Q13));
if (s2 > 0) return 0;
if (s2 > s)
{
s = s2;
normalR = R1;
invert_normal = ((pp.X) < 0);
code = (1);
}
}
//TST(pp[1], (A[1] + B[0] * Q21 + B[1] * Q22 + B[2] * Q23), R1 + 1, 2);
{
s2 = (float)Math.Abs(pp.Y) - ((A[1] + B[0] * Q21 + B[1] * Q22 + B[2] * Q23));
if (s2 > 0) return 0;
if (s2 > s)
{
s = s2;
normalR = R1 + 1;
invert_normal = ((pp.Y) < 0);
code = (2);
}
}
//TST(pp[2], (A[2] + B[0] * Q31 + B[1] * Q32 + B[2] * Q33), R1 + 2, 3);
{
s2 = (float)Math.Abs(pp.Z) - ((A[2] + B[0] * Q31 + B[1] * Q32 + B[2] * Q33));
if (s2 > 0) return 0;
if (s2 > s)
{
s = s2;
normalR = R1 + 2;
invert_normal = ((pp.Z) < 0);
code = (3);
}
}
// separating axis = v1,v2,v3
//TST(dDOT41(R2 + 0, ref p), (A[0] * Q11 + A[1] * Q21 + A[2] * Q31 + B[0]), R2 + 0, 4);
{
s2 = (float)Math.Abs(dDOT41(R2 + 0, ref p)) - ((A[0] * Q11 + A[1] * Q21 + A[2] * Q31 + B[0]));
if (s2 > 0) return 0;
if (s2 > s)
{
s = s2;
normalR = R2;
invert_normal = ((dDOT41(R2 + 0, ref p)) < 0);
code = (4);
}
}
//TST(dDOT41(R2 + 1, ref p), (A[0] * Q12 + A[1] * Q22 + A[2] * Q32 + B[1]), R2 + 1, 5);
{
s2 = (float)Math.Abs(dDOT41(R2 + 1, ref p)) - ((A[0] * Q12 + A[1] * Q22 + A[2] * Q32 + B[1]));
if (s2 > 0) return 0;
if (s2 > s)
{
s = s2;
normalR = R2 + 1;
invert_normal = ((dDOT41(R2 + 1, ref p)) < 0);
code = (5);
}
}
//TST(dDOT41(R2 + 2, ref p), (A[0] * Q13 + A[1] * Q23 + A[2] * Q33 + B[2]), R2 + 2, 6);
{
s2 = (float)Math.Abs(dDOT41(R2 + 2, ref p)) - ((A[0] * Q13 + A[1] * Q23 + A[2] * Q33 + B[2]));
if (s2 > 0) return 0;
if (s2 > s)
{
s = s2;
normalR = R2 + 2;
invert_normal = ((dDOT41(R2 + 2, ref p)) < 0);
code = (6);
}
}
// note: cross product axes need to be scaled when s is computed.
// normal (n1,n2,n3) is relative to box 1.
}
{
/*TST(expr1,expr2,n1,n2,n3,cc)
{
s2 = (float)Math.Abs(expr1) - (expr2);
if (s2 > SIMD_EPSILON) return 0;
l = (float)Math.Sqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3));
if (l > SIMD_EPSILON) {
s2 /= l;
if (s2*fudge_factor > s) {
s = s2;
normalR = null;
normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l;
invert_normal = ((expr1) < 0);
code = (cc);
}
}
}*/
// separating axis = u1 x (v1,v2,v3)
//TST(pp[2] * R21 - pp[1] * R31, (A[1] * Q31 + A[2] * Q21 + B[1] * Q13 + B[2] * Q12), 0, -R31, R21, 7);
{
s2 = (float)Math.Abs(pp.Z * R21 - pp.Y * R31) - ((A[1] * Q31 + A[2] * Q21 + B[1] * Q13 + B[2] * Q12));
if (s2 > BulletGlobal.SIMD_EPSILON) return 0;
l = (float)Math.Sqrt((0) * (0) + (-R31) * (-R31) + (R21) * (R21));
if (l > BulletGlobal.SIMD_EPSILON)
{
s2 /= l;
if (s2 * fudge_factor > s)
{
s = s2;
normalR = null;
normalC.X = (0) / l; normalC.Y = (-R31) / l; normalC.Z = (R21) / l;
invert_normal = ((pp.Z * R21 - pp.Y * R31) < 0);
code = (7);
}
}
}
//TST(pp[2] * R22 - pp[1] * R32, (A[1] * Q32 + A[2] * Q22 + B[0] * Q13 + B[2] * Q11), 0, -R32, R22, 8);
{
s2 = (float)Math.Abs(pp.Z * R22 - pp.Y * R32) - ((A[1] * Q32 + A[2] * Q22 + B[0] * Q13 + B[2] * Q11));
if (s2 > BulletGlobal.SIMD_EPSILON) return 0;
l = (float)Math.Sqrt((0) * (0) + (-R32) * (-R32) + (R22) * (R22));
if (l > BulletGlobal.SIMD_EPSILON)
{
s2 /= l;
if (s2 * fudge_factor > s)
{
s = s2;
normalR = null;
normalC.X = (0) / l; normalC.Y = (-R32) / l; normalC.Z = (R22) / l;
invert_normal = ((pp.Z * R22 - pp.Y * R32) < 0);
code = (8);
}
}
}
//TST(pp[2] * R23 - pp[1] * R33, (A[1] * Q33 + A[2] * Q23 + B[0] * Q12 + B[1] * Q11), 0, -R33, R23, 9);
{
s2 = (float)Math.Abs(pp.Z * R23 - pp.Y * R33) - ((A[1] * Q33 + A[2] * Q23 + B[0] * Q12 + B[1] * Q11));
if (s2 > BulletGlobal.SIMD_EPSILON) return 0;
l = (float)Math.Sqrt((0) * (0) + (-R33) * (-R33) + (R23) * (R23));
if (l > BulletGlobal.SIMD_EPSILON)
{
s2 /= l;
if (s2 * fudge_factor > s)
{
s = s2;
normalR = null;
normalC.X = (0) / l; normalC.Y = (-R33) / l; normalC.Z = (R23) / l;
invert_normal = ((pp.Z * R23 - pp.Y * R33) < 0);
code = (9);
}
}
}
// separating axis = u2 x (v1,v2,v3)
//TST(pp[0] * R31 - pp[2] * R11, (A[0] * Q31 + A[2] * Q11 + B[1] * Q23 + B[2] * Q22), R31, 0, -R11, 10);
{
s2 = (float)Math.Abs(pp.X * R31 - pp.Z * R11) - ((A[0] * Q31 + A[2] * Q11 + B[1] * Q23 + B[2] * Q22));
if (s2 > BulletGlobal.SIMD_EPSILON) return 0;
l = (float)Math.Sqrt((R31) * (R31) + (0) * (0) + (-R11) * (-R11));
if (l > BulletGlobal.SIMD_EPSILON)
{
s2 /= l;
if (s2 * fudge_factor > s)
{
s = s2;
normalR = null;
normalC.X = (R31) / l; normalC.Y = (0) / l; normalC.Z = (-R11) / l;
invert_normal = ((pp.X * R31 - pp.Z * R11) < 0);
code = (10);
}
}
}
//TST(pp[0] * R32 - pp[2] * R12, (A[0] * Q32 + A[2] * Q12 + B[0] * Q23 + B[2] * Q21), R32, 0, -R12, 11);
{
s2 = (float)Math.Abs(pp.X * R32 - pp.Z * R12) - ((A[0] * Q32 + A[2] * Q12 + B[0] * Q23 + B[2] * Q21));
if (s2 > BulletGlobal.SIMD_EPSILON) return 0;
l = (float)Math.Sqrt((R32) * (R32) + (0) * (0) + (-R12) * (-R12));
if (l > BulletGlobal.SIMD_EPSILON)
{
s2 /= l;
if (s2 * fudge_factor > s)
{
s = s2;
normalR = null;
normalC.X = (R32) / l; normalC.Y = (0) / l; normalC.Z = (-R12) / l;
invert_normal = ((pp.X * R32 - pp.Z * R12) < 0);
code = (11);
}
}
}
//TST(pp[0] * R33 - pp[2] * R13, (A[0] * Q33 + A[2] * Q13 + B[0] * Q22 + B[1] * Q21), R33, 0, -R13, 12);
{
s2 = (float)Math.Abs(pp.X * R33 - pp.Z * R13) - ((A[0] * Q33 + A[2] * Q13 + B[0] * Q22 + B[1] * Q21));
if (s2 > BulletGlobal.SIMD_EPSILON) return 0;
l = (float)Math.Sqrt((R33) * (R33) + (0) * (0) + (-R13) * (-R13));
if (l > BulletGlobal.SIMD_EPSILON)
{
s2 /= l;
if (s2 * fudge_factor > s)
{
s = s2;
normalR = null;
normalC.X = (R33) / l; normalC.Y = (0) / l; normalC.Z = (-R13) / l;
invert_normal = ((pp.X * R33 - pp.Z * R13) < 0);
code = (12);
}
}
}
// separating axis = u3 x (v1,v2,v3)
//TST(pp[1] * R11 - pp[0] * R21, (A[0] * Q21 + A[1] * Q11 + B[1] * Q33 + B[2] * Q32), -R21, R11, 0, 13);
{
s2 = (float)Math.Abs(pp.Y * R11 - pp.X * R21) - ((A[0] * Q21 + A[1] * Q11 + B[1] * Q33 + B[2] * Q32));
if (s2 > BulletGlobal.SIMD_EPSILON) return 0;
l = (float)Math.Sqrt((-R21) * (-R21) + (R11) * (R11) + (0) * (0));
if (l > BulletGlobal.SIMD_EPSILON)
{
s2 /= l;
if (s2 * fudge_factor > s)
{
s = s2;
normalR = null;
normalC.X = (-R21) / l; normalC.Y = (R11) / l; normalC.Z = (0) / l;
invert_normal = ((pp.Y * R11 - pp.X * R21) < 0);
code = (13);
}
}
}
//TST(pp[1] * R12 - pp[0] * R22, (A[0] * Q22 + A[1] * Q12 + B[0] * Q33 + B[2] * Q31), -R22, R12, 0, 14);
{
s2 = (float)Math.Abs(pp.Y * R12 - pp.X * R22) - ((A[0] * Q22 + A[1] * Q12 + B[0] * Q33 + B[2] * Q31));
if (s2 > BulletGlobal.SIMD_EPSILON) return 0;
l = (float)Math.Sqrt((-R22) * (-R22) + (R12) * (R12) + (0) * (0));
if (l > BulletGlobal.SIMD_EPSILON)
{
s2 /= l;
if (s2 * fudge_factor > s)
{
s = s2;
normalR = null;
normalC.X = (-R22) / l; normalC.Y = (R12) / l; normalC.Z = (0) / l;
invert_normal = ((pp.Y * R12 - pp.X * R22) < 0);
code = (14);
}
}
}
//TST(pp[1] * R13 - pp[0] * R23, (A[0] * Q23 + A[1] * Q13 + B[0] * Q32 + B[1] * Q31), -R23, R13, 0, 15);
{
s2 = (float)Math.Abs(pp.Y * R13 - pp.X * R23) - ((A[0] * Q23 + A[1] * Q13 + B[0] * Q32 + B[1] * Q31));
if (s2 > BulletGlobal.SIMD_EPSILON) return 0;
l = (float)Math.Sqrt((-R23) * (-R23) + (R13) * (R13) + (0) * (0));
if (l > BulletGlobal.SIMD_EPSILON)
{
s2 /= l;
if (s2 * fudge_factor > s)
{
s = s2;
normalR = null;
normalC.X = (-R23) / l; normalC.Y = (R13) / l; normalC.Z = (0) / l;
invert_normal = ((pp.Y * R13 - pp.X * R23) < 0);
code = (15);
}
}
}
}
if (code == 0) return 0;
// if we get to this point, the boxes interpenetrate. compute the normal
// in global coordinates.
if (normalR != null)
{
normal.X = normalR[0];
normal.Y = normalR[4];
normal.Z = normalR[8];
}
else
{
/*
//#define dMULTIPLYOP0_331(A,op,B,C) \
{ \
(A)[0] op dDOT((B),(C)); \
(A)[1] op dDOT((B+4),(C)); \
(A)[2] op dDOT((B+8),(C)); \
} */
//dMULTIPLY0_331 (normal,R1,normalC);
(normal).X = dDOT((R1), ref normalC);
(normal).Y = dDOT((R1 + 4), ref normalC);
(normal).Z = dDOT((R1 + 8), ref normalC);
}
if (invert_normal)
{
/*normal[0] = -normal[0];
normal[1] = -normal[1];
normal[2] = -normal[2];*/
normal = -normal;
}
depth = -s;
// compute contact point(s)
if (code > 6)
{
// an edge from box 1 touches an edge from box 2.
// find a point pa on the intersecting edge of box 1
btVector3 pa = btVector3.Zero;
float sign;
for (i = 0; i < 3; i++) pa[i] = p1[i];
for (j = 0; j < 3; j++)
{
sign = (dDOT14(ref normal, (R1 + j)) > 0) ? 1.0f : -1.0f;
for (i = 0; i < 3; i++) pa[i] += sign * A[j] * R1[i * 4 + j];
}
// find a point pb on the intersecting edge of box 2
btVector3 pb = btVector3.Zero;
for (i = 0; i < 3; i++) pb[i] = p2[i];
for (j = 0; j < 3; j++)
{
sign = (dDOT14(ref normal, R2 + j) > 0) ? -1.0f : 1.0f;
for (i = 0; i < 3; i++) pb[i] += sign * B[j] * R2[i * 4 + j];
}
float alpha, beta;
btVector3 ua = btVector3.Zero, ub = btVector3.Zero;
for (i = 0; i < 3; i++) ua[i] = R1[((code) - 7) / 3 + i * 4];
for (i = 0; i < 3; i++) ub[i] = R2[((code) - 7) % 3 + i * 4];
dLineClosestApproach(ref pa, ref ua, ref pb, ref ub, &alpha, &beta);
for (i = 0; i < 3; i++) pa[i] += ua[i] * alpha;
for (i = 0; i < 3; i++) pb[i] += ub[i] * beta;
{
//contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]);
//contact[0].depth = *depth;
#if USE_CENTER_POINT
btVector3 pointInWorld;
for (i=0; i<3; i++)
pointInWorld[i] = (pa[i]+pb[i])*0.5f;
output.addContactPoint(-normal,pointInWorld,-depth);
#else
btVector3 temp = -normal;
output.addContactPoint(ref temp, ref pb, -depth);
normal = -temp;
#endif //
return_code = code;
}
return 1;
}
// okay, we have a face-something intersection (because the separating
// axis is perpendicular to a face). define face 'a' to be the reference
// face (i.e. the normal vector is perpendicular to this) and face 'b' to be
// the incident face (the closest face of the other box).
{
float* Ra, Rb;
float[] Sa, Sb;
//float* pa=stackalloc float[3], pb=stackalloc float[3];
StackPtr<float> pa = StackPtr<float>.Allocate(3);
StackPtr<float> pb = StackPtr<float>.Allocate(3);
try
{
if (code <= 3)
{
Ra = R1;
Rb = R2;
//pa = p1;
pa[0] = p1.X;
pa[1] = p1.Y;
pa[2] = p1.Z;
//pb = p2;
pb[0] = p2.X;
pb[1] = p2.Y;
pb[2] = p2.Z;
Sa = A;
Sb = B;
}
else
{
Ra = R2;
Rb = R1;
//pa = p2;
pa[0] = p2.X;
pa[1] = p2.Y;
pa[2] = p2.Z;
//pb = p1;
pb[0] = p1.X;
pb[1] = p1.Y;
pb[2] = p1.Z;
Sa = B;
Sb = A;
}
// nr = normal vector of reference face dotted with axes of incident box.
// anr = absolute values of nr.
btVector3 normal2, nr, anr;
if (code <= 3)
{
normal2 = normal;// new btVector3(normal[0], normal[1], normal[2]);
}
else
{
normal2 = -normal;// new btVector3(-normal[0], -normal[1], -normal[2]);
}
//dMULTIPLY1_331(nr, Rb, normal2);
nr = new btVector3(dDOT41((Rb), ref (normal2)), dDOT41((Rb + 1), ref (normal2)), dDOT41((Rb + 2), ref (normal2)));
anr = new btVector3((float)Math.Abs(nr.X), (float)Math.Abs(nr.Y), (float)Math.Abs(nr.Z));
// find the largest compontent of anr: this corresponds to the normal
// for the indident face. the other axis numbers of the indicent face
// are stored in a1,a2.
int lanr, a1, a2;
if (anr.Y > anr.X)
{
if (anr.Y > anr.Z)
{
a1 = 0;
lanr = 1;
a2 = 2;
}
else
{
a1 = 0;
a2 = 1;
lanr = 2;
}
}
else
{
if (anr.X > anr.Z)
{
lanr = 0;
a1 = 1;
a2 = 2;
}
else
{
a1 = 0;
a2 = 1;
lanr = 2;
}
}
// compute center point of incident face, in reference-face coordinates
btVector3 center = btVector3.Zero;
if (nr[lanr] < 0)
{
for (i = 0; i < 3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i * 4 + lanr];
}
else
{
for (i = 0; i < 3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i * 4 + lanr];
}
// find the normal and non-normal axis numbers of the reference box
int codeN, code1, code2;
if (code <= 3) codeN = code - 1; else codeN = code - 4;
if (codeN == 0)
{
code1 = 1;
code2 = 2;
}
else if (codeN == 1)
{
code1 = 0;
code2 = 2;
}
else
{
code1 = 0;
code2 = 1;
}
// find the four corners of the incident face, in reference-face coordinates
//float* quad = stackalloc float[8]; // 2D coordinate of incident face (x,y pairs)
StackPtr<float> quad = StackPtr<float>.Allocate(8);
try
{
float c1, c2, m11, m12, m21, m22;
c1 = dDOT14(ref center, Ra + code1);
c2 = dDOT14(ref center, Ra + code2);
// optimize this? - we have already computed this data above, but it is not
// stored in an easy-to-index format. for now it's quicker just to recompute
// the four dot products.
m11 = dDOT44(Ra + code1, Rb + a1);
m12 = dDOT44(Ra + code1, Rb + a2);
m21 = dDOT44(Ra + code2, Rb + a1);
m22 = dDOT44(Ra + code2, Rb + a2);
{
float k1 = m11 * Sb[a1];
float k2 = m21 * Sb[a1];
float k3 = m12 * Sb[a2];
float k4 = m22 * Sb[a2];
quad[0] = c1 - k1 - k3;
quad[1] = c2 - k2 - k4;
quad[2] = c1 - k1 + k3;
quad[3] = c2 - k2 + k4;
quad[4] = c1 + k1 + k3;
quad[5] = c2 + k2 + k4;
quad[6] = c1 + k1 - k3;
quad[7] = c2 + k2 - k4;
}
// find the size of the reference face
//float* rect = stackalloc float[2];
StackPtr<float> rect = StackPtr<float>.Allocate(2);
try
{
rect[0] = Sa[code1];
rect[1] = Sa[code2];
// intersect the incident and reference faces
//float* ret = stackalloc float[16];
StackPtr<float> ret = StackPtr<float>.Allocate(16);
try
{
int n = intersectRectQuad2(rect, quad, ret);
if (n < 1) return 0; // this should never happen
// convert the intersection points into reference-face coordinates,
// and compute the contact position and depth for each point. only keep
// those points that have a positive (penetrating) depth. delete points in
// the 'ret' array as necessary so that 'point' and 'ret' correspond.
//float* point = stackalloc float[3 * 8]; // penetrating contact points
//float* dep = stackalloc float[8]; // depths for those points
StackPtr<float> pointSafe = StackPtr<float>.Allocate(3 * 8);
StackPtr<float> dep = StackPtr<float>.Allocate(8);
try
{
fixed (float* point = &pointSafe.Array[0])
{
float det1 = 1f / (m11 * m22 - m12 * m21);
m11 *= det1;
m12 *= det1;
m21 *= det1;
m22 *= det1;
int cnum = 0; // number of penetrating contact points found
for (j = 0; j < n; j++)
{
float k1 = m22 * (ret[j * 2] - c1) - m12 * (ret[j * 2 + 1] - c2);
float k2 = -m21 * (ret[j * 2] - c1) + m11 * (ret[j * 2 + 1] - c2);
for (i = 0; i < 3; i++) point[cnum * 3 + i] =
center[i] + k1 * Rb[i * 4 + a1] + k2 * Rb[i * 4 + a2];
dep[cnum] = Sa[codeN] - dDOT(ref normal2, point + cnum * 3);
if (dep[cnum] >= 0)
{
ret[cnum * 2] = ret[j * 2];
ret[cnum * 2 + 1] = ret[j * 2 + 1];
cnum++;
}
}
if (cnum < 1) return 0; // this should never happen
// we can't generate more contacts than we actually have
if (maxc > cnum) maxc = cnum;
if (maxc < 1) maxc = 1;
if (cnum <= maxc)
{
if (code < 4)
{
// we have less contacts than we need, so we use them all
for (j = 0; j < cnum; j++)
{
btVector3 pointInWorld = btVector3.Zero;
for (i = 0; i < 3; i++)
pointInWorld[i] = point[j * 3 + i] + pa[i];
btVector3 temp = -normal;
output.addContactPoint(ref temp, ref pointInWorld, -dep[j]);
normal = -temp;
}
}
else
{
// we have less contacts than we need, so we use them all
for (j = 0; j < cnum; j++)
{
btVector3 pointInWorld = btVector3.Zero;
for (i = 0; i < 3; i++)
pointInWorld[i] = point[j * 3 + i] + pa[i] - normal[i] * dep[j];
//pointInWorld[i] = point[j*3+i] + pa[i];
btVector3 temp = -normal;
output.addContactPoint(ref temp, ref pointInWorld, -dep[j]);
normal = -temp;
}
}
}
else
{
// we have more contacts than are wanted, some of them must be culled.
// find the deepest point, it is always the first contact.
int i1 = 0;
float maxdepth = dep[0];
for (i = 1; i < cnum; i++)
{
if (dep[i] > maxdepth)
{
maxdepth = dep[i];
i1 = i;
}
}
//int* iret = stackalloc int[8];
StackPtr<int> iret = StackPtr<int>.Allocate(8);
try
{
cullPoints2(cnum, ret, maxc, i1, iret);
for (j = 0; j < maxc; j++)
{
// dContactGeom *con = CONTACT(contact,skip*j);
// for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
// con->depth = dep[iret[j]];
btVector3 posInWorld = btVector3.Zero;
for (i = 0; i < 3; i++)
posInWorld[i] = point[iret[j] * 3 + i] + pa[i];
btVector3 temp = -normal;
if (code < 4)
{
output.addContactPoint(ref temp, ref posInWorld, -dep[iret[j]]);
}
else
{
btVector3 temp2 = posInWorld - normal * dep[iret[j]];
output.addContactPoint(ref temp, ref temp2, -dep[iret[j]]);
}
normal = -temp;
}
cnum = maxc;
}
finally
{
iret.Dispose();
}
}
return_code = code;
return cnum;
}
}
finally
{
pointSafe.Dispose();
dep.Dispose();
}
}
finally
{
ret.Dispose();
}
}
finally
{
rect.Dispose();
}
}
finally
{
quad.Dispose();
}
}
finally
{
pa.Dispose();
pb.Dispose();
}
}
}
finally
{
A.Dispose();
B.Dispose();
}
}
}
public void getClosestPoints(ref ClosestPointInput input, ref PerturbedContactResult output, ref ManifoldResult originalManifoldResult, IDebugDraw debugDraw)
{
m_cachedSeparatingDistance = 0f;
float distance = 0f;
btVector3 normalInB = new btVector3(0f, 0f, 0f);
btVector3 pointOnA, pointOnB = btVector3.Zero;
btTransform localTransA = input.m_transformA;
btTransform localTransB = input.m_transformB;
btVector3 positionOffset;// = (localTransA.Origin + localTransB.Origin) * 0.5f;
{
btVector3 temp;
btVector3.Add(ref localTransA.Origin, ref localTransB.Origin, out temp);
btVector3.Multiply(ref temp, 0.5f, out positionOffset);
}
localTransA.Origin -= positionOffset;
localTransB.Origin -= positionOffset;
bool check2d = m_minkowskiA.isConvex2d && m_minkowskiB.isConvex2d;
float marginA = m_marginA;
float marginB = m_marginB;
//gNumGjkChecks++;
//for CCD we don't use margins
if (m_ignoreMargin)
{
marginA = 0f;
marginB = 0f;
}
m_curIter = 0;
int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
m_cachedSeparatingAxis.setValue(0, 1, 0);
bool isValid = false;
bool checkSimplex = false;
bool checkPenetration = true;
m_degenerateSimplex = 0;
m_lastUsedMethod = -1;
{
float squaredDistance = BulletGlobal.BT_LARGE_FLOAT;
float delta = 0f;
float margin = marginA + marginB;
m_simplexSolver.reset();
for (; ; )
//while (true)
{
btVector3 seperatingAxisInA;// = (-m_cachedSeparatingAxis) * input.m_transformA.Basis;
{
btVector3 temp = -m_cachedSeparatingAxis;
btMatrix3x3.Multiply(ref temp, ref input.m_transformA.Basis, out seperatingAxisInA);
}
btVector3 seperatingAxisInB;// = m_cachedSeparatingAxis * input.m_transformB.Basis;
btMatrix3x3.Multiply(ref m_cachedSeparatingAxis, ref input.m_transformB.Basis, out seperatingAxisInB);
btVector3 pInA;// = m_minkowskiA.localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
m_minkowskiA.localGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA, out pInA);
btVector3 qInB;// = m_minkowskiB.localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
m_minkowskiB.localGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB, out qInB);
btVector3 pWorld = btVector3.Transform(pInA, localTransA);
btVector3 qWorld = btVector3.Transform(qInB, localTransB);
if (check2d)
{
pWorld.Z = 0f;
qWorld.Z = 0f;
}
btVector3 w = pWorld - qWorld;
delta = m_cachedSeparatingAxis.dot(w);
// potential exit, they don't overlap
if ((delta > 0.0f) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
{
m_degenerateSimplex = 10;
checkSimplex = true;
//checkPenetration = false;
break;
}
//exit 0: the new point is already in the simplex, or we didn't come any closer
if (m_simplexSolver.inSimplex(w))
{
m_degenerateSimplex = 1;
checkSimplex = true;
break;
}
// are we getting any closer ?
float f0 = squaredDistance - delta;
float f1 = squaredDistance * REL_ERROR2;
if (f0 <= f1)
{
if (f0 <= 0f)
{
m_degenerateSimplex = 2;
}
else
{
m_degenerateSimplex = 11;
}
checkSimplex = true;
break;
}
//add current vertex to simplex
m_simplexSolver.addVertex(w, pWorld, qWorld);
btVector3 newCachedSeparatingAxis;
//calculate the closest point to the origin (update vector v)
if (!m_simplexSolver.closest(out newCachedSeparatingAxis))
{
m_degenerateSimplex = 3;
checkSimplex = true;
break;
}
if (newCachedSeparatingAxis.Length2 < REL_ERROR2)
{
m_cachedSeparatingAxis = newCachedSeparatingAxis;
m_degenerateSimplex = 6;
checkSimplex = true;
break;
}
float previousSquaredDistance = squaredDistance;
squaredDistance = newCachedSeparatingAxis.Length2;
m_cachedSeparatingAxis = newCachedSeparatingAxis;
//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
//are we getting any closer ?
if (previousSquaredDistance - squaredDistance <= BulletGlobal.SIMD_EPSILON * previousSquaredDistance)
{
m_simplexSolver.backup_closest(ref m_cachedSeparatingAxis);
checkSimplex = true;
m_degenerateSimplex = 12;
break;
}
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
if (m_curIter++ > gGjkMaxIter)
{
/*#if defined(DEBUG) || defined (_DEBUG) || defined (DEBUG_SPU_COLLISION_DETECTION)
printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
m_cachedSeparatingAxis.getX(),
m_cachedSeparatingAxis.getY(),
m_cachedSeparatingAxis.getZ(),
squaredDistance,
m_minkowskiA->getShapeType(),
m_minkowskiB->getShapeType());
#endif */
break;
}
bool check = (!m_simplexSolver.fullSimplex);
//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
if (!check)
{
//do we need this backup_closest here ?
m_simplexSolver.backup_closest(ref m_cachedSeparatingAxis);
m_degenerateSimplex = 13;
break;
}
}
if (checkSimplex)
{
m_simplexSolver.compute_points(out pointOnA, out pointOnB);
normalInB = pointOnA - pointOnB;
float lenSqr = m_cachedSeparatingAxis.Length2;
//valid normal
if (lenSqr < 0.0001)
{
m_degenerateSimplex = 5;
}
if (lenSqr > BulletGlobal.SIMD_EPSILON * BulletGlobal.SIMD_EPSILON)
{
float rlen = 1f / (float)Math.Sqrt(lenSqr);
normalInB *= rlen; //normalize
float s = (float)Math.Sqrt(squaredDistance);
Debug.Assert(s > 0.0f);
pointOnA -= m_cachedSeparatingAxis * (marginA / s);
pointOnB += m_cachedSeparatingAxis * (marginB / s);
distance = ((1f / rlen) - margin);
isValid = true;
m_lastUsedMethod = 1;
}
else
{
m_lastUsedMethod = 2;
}
}
bool catchDegeneratePenetrationCase =
(m_catchDegeneracies != 0 && m_penetrationDepthSolver != null && m_degenerateSimplex != 0 && ((distance + margin) < 0.01));
//if (checkPenetration && !isValid)
if (checkPenetration && (!isValid || catchDegeneratePenetrationCase))
{
//penetration case
//if there is no way to handle penetrations, bail out
if (m_penetrationDepthSolver != null)
{
// Penetration depth case.
btVector3 tmpPointOnA, tmpPointOnB;
//gNumDeepPenetrationChecks++;
m_cachedSeparatingAxis.setZero();
bool isValid2 = m_penetrationDepthSolver.calcPenDepth(
m_simplexSolver,
m_minkowskiA, m_minkowskiB,
localTransA, localTransB,
ref m_cachedSeparatingAxis, out tmpPointOnA, out tmpPointOnB,
debugDraw//, input.m_stackAlloc
);
if (isValid2)
{
btVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
float lenSqr = tmpNormalInB.Length2;
if (lenSqr <= (BulletGlobal.SIMD_EPSILON * BulletGlobal.SIMD_EPSILON))
{
tmpNormalInB = m_cachedSeparatingAxis;
lenSqr = m_cachedSeparatingAxis.Length2;
}
if (lenSqr > (BulletGlobal.SIMD_EPSILON * BulletGlobal.SIMD_EPSILON))
{
tmpNormalInB /= (float)Math.Sqrt(lenSqr);
float distance2 = -(tmpPointOnA - tmpPointOnB).Length;
//only replace valid penetrations when the result is deeper (check)
if (!isValid || (distance2 < distance))
{
distance = distance2;
pointOnA = tmpPointOnA;
pointOnB = tmpPointOnB;
normalInB = tmpNormalInB;
isValid = true;
m_lastUsedMethod = 3;
}
else
{
m_lastUsedMethod = 8;
}
}
else
{
m_lastUsedMethod = 9;
}
}
else
{
///this is another degenerate case, where the initial GJK calculation reports a degenerate case
///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
///reports a valid positive distance. Use the results of the second GJK instead of failing.
///thanks to Jacob.Langford for the reproduction case
///http://code.google.com/p/bullet/issues/detail?id=250
if (m_cachedSeparatingAxis.Length2 > 0f)
{
float distance2 = (tmpPointOnA - tmpPointOnB).Length - margin;
//only replace valid distances when the distance is less
if (!isValid || (distance2 < distance))
{
distance = distance2;
pointOnA = tmpPointOnA;
pointOnB = tmpPointOnB;
pointOnA -= m_cachedSeparatingAxis * marginA;
pointOnB += m_cachedSeparatingAxis * marginB;
normalInB = m_cachedSeparatingAxis;
normalInB.normalize();
isValid = true;
m_lastUsedMethod = 6;
}
else
{
m_lastUsedMethod = 5;
}
}
}
}
}
}
if (isValid && ((distance < 0) || (distance * distance < input.m_maximumDistanceSquared)))
{
m_cachedSeparatingAxis = normalInB;
m_cachedSeparatingDistance = distance;
output.addContactPoint(
normalInB,
pointOnB + positionOffset,
distance, ref originalManifoldResult);
}
}
public void collideSingleContact(btQuaternion perturbeRot, CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut)
{
CollisionObject convexObj = m_isSwapped ? body1 : body0;
CollisionObject planeObj = m_isSwapped ? body0 : body1;
ConvexShape convexShape = (ConvexShape)convexObj.CollisionShape;
StaticPlaneShape planeShape = (StaticPlaneShape)planeObj.CollisionShape;
bool hasCollision = false;
btVector3 planeNormal = planeShape.PlaneNormal;
float planeConstant = planeShape.PlaneConstant;
btTransform convexWorldTransform = convexObj.WorldTransform;
btTransform convexInPlaneTrans;
convexInPlaneTrans = planeObj.WorldTransform.inverse() * convexWorldTransform;
//now perturbe the convex-world transform
#region convexWorldTransform.Basis *= new btMatrix3x3(perturbeRot);
{
btMatrix3x3 temp1 = convexWorldTransform.Basis, temp2 = new btMatrix3x3(perturbeRot);
btMatrix3x3.Multiply(ref temp1, ref temp2, out convexWorldTransform.Basis);
}
#endregion
btTransform planeInConvex;
planeInConvex = convexWorldTransform.inverse() * planeObj.WorldTransform;
#region btVector3 vtx = convexShape.localGetSupportingVertex(planeInConvex.Basis * -planeNormal);
btVector3 vtx;
{
btVector3 temp, temp2;
temp2 = -planeNormal;
btMatrix3x3.Multiply(ref planeInConvex.Basis, ref temp2, out temp);
//vtx = convexShape.localGetSupportingVertex(temp);
convexShape.localGetSupportingVertex(ref temp, out vtx);
}
#endregion
btVector3 vtxInPlane = convexInPlaneTrans * vtx;
float distance = (planeNormal.dot(vtxInPlane) - planeConstant);
btVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
btVector3 vtxInPlaneWorld = planeObj.WorldTransform * vtxInPlaneProjected;
hasCollision = distance < m_manifoldPtr.ContactBreakingThreshold;
resultOut.PersistentManifold = m_manifoldPtr;
if (hasCollision)
{
/// report a contact. internally this will be kept persistent, and contact reduction is done
btVector3 normalOnSurfaceB;// = planeObj.WorldTransform.Basis * planeNormal;
btMatrix3x3.Multiply(ref planeObj.WorldTransform.Basis, ref planeNormal, out normalOnSurfaceB);
btVector3 pOnB = vtxInPlaneWorld;
resultOut.addContactPoint(ref normalOnSurfaceB, ref pOnB, distance);
}
}
//by default, Bullet will use this near callback
static void DefaultNearCallback(BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo)
{
CollisionObject colObj0 = (CollisionObject)collisionPair.m_pProxy0.m_clientObject;
CollisionObject colObj1 = (CollisionObject)collisionPair.m_pProxy1.m_clientObject;
if (dispatcher.needsCollision(colObj0,colObj1))
{
//dispatcher will keep algorithms persistent in the collision pair
if (collisionPair.m_algorithm==null)
{
collisionPair.m_algorithm = dispatcher.findAlgorithm(colObj0,colObj1);
}
if (collisionPair.m_algorithm!=null)
{
ManifoldResult contactPointResult=new ManifoldResult(colObj0,colObj1);
if (dispatchInfo.m_dispatchFunc == DispatchFunc.DISPATCH_DISCRETE)
{
//discrete collision detection query
collisionPair.m_algorithm.processCollision(colObj0,colObj1,dispatchInfo,ref contactPointResult);
} else
{
//continuous collision detection query, time of impact (toi)
float toi = collisionPair.m_algorithm.calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,ref contactPointResult);
if (dispatchInfo.m_timeOfImpact > toi)
dispatchInfo.m_timeOfImpact = toi;
}
}
}
}
