void B2CollidePolygons(ref ManifoldResult manifold,
Box2dShape polyA, ref Matrix xfA,
Box2dShape polyB, ref Matrix xfB)
{
int edgeA = 0;
float separationA = FindMaxSeparation(ref edgeA, polyA, ref xfA, polyB, ref xfB);
if (separationA > 0.0f)
{
return;
}
int edgeB = 0;
float separationB = FindMaxSeparation(ref edgeB, polyB, ref xfB, polyA, ref xfA);
if (separationB > 0.0f)
{
return;
}
Box2dShape poly1; // reference poly
Box2dShape poly2; // incident poly
Matrix xf1, xf2;
int edge1; // reference edge
bool flip;
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
// TODO_ERIN use "radius" of poly for absolute tolerance.
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
flip = true;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
flip = false;
}
ClipVertex[] incidentEdge = new ClipVertex[2];
FindIncidentEdge(incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2);
int count1 = poly1.GetVertexCount();
Vector3[] vertices1 = poly1.GetVertices();
Vector3 v11 = vertices1[edge1];
Vector3 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0];
Vector3 dv = v12 - v11;
Vector3 sideNormal = Vector3.TransformNormal( v12 - v11,xf1);
sideNormal.Normalize();
Vector3 frontNormal = CrossS(ref sideNormal, 1.0f);
v11 = Vector3.Transform(v11,xf1);
v12 = Vector3.Transform(v12,xf1);
float frontOffset = Vector3.Dot(frontNormal, v11);
float sideOffset1 = -Vector3.Dot(sideNormal, v11);
float sideOffset2 = Vector3.Dot(sideNormal, v12);
// Clip incident edge against extruded edge1 side edges.
ClipVertex[] clipPoints1 = new ClipVertex[2];
clipPoints1[0].v = Vector3.Zero;
clipPoints1[1].v = Vector3.Zero;
ClipVertex[] clipPoints2 = new ClipVertex[2];
clipPoints2[0].v = Vector3.Zero;
clipPoints2[1].v = Vector3.Zero;
int np;
// Clip to box side 1
np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
Vector3 manifoldNormal = flip ? -frontNormal : frontNormal;
int pointCount = 0;
for (int i = 0; i < b2_maxManifoldPoints; ++i)
{
float separation = Vector3.Dot(frontNormal, clipPoints2[i].v) - frontOffset;
if (separation <= 0.0f)
{
//b2ManifoldPoint* cp = manifold.points + pointCount;
//float separation = separation;
//cp.localPoint1 = b2MulT(xfA, clipPoints2[i].v);
//cp.localPoint2 = b2MulT(xfB, clipPoints2[i].v);
manifold.AddContactPoint(-manifoldNormal,clipPoints2[i].v,separation);
// cp.id = clipPoints2[i].id;
// cp.id.features.flip = flip;
++pointCount;
}
}
// manifold.pointCount = pointCount;}
}
public override void ProcessCollision (CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
return;
}
resultOut.SetPersistentManifold(m_manifoldPtr);
SphereShape sphere0 = (SphereShape)body0.GetCollisionShape();
SphereShape sphere1 = (SphereShape)body1.GetCollisionShape();
Vector3 diff = body0.GetWorldTransform().Translation - body1.GetWorldTransform().Translation;
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);
Vector3 normalOnSurfaceB = new Vector3(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)
Vector3 pos1 = body1.GetWorldTransform().Translation + 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)
{
//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();
}
}
public override void ProcessCollision (CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut)
{
//(void)dispatchInfo;
//(void)resultOut;
if (m_manifoldPtr == null)
{
resultOut = null;
return;
}
CollisionObject sphereObj = m_isSwapped? body1 : body0;
CollisionObject boxObj = m_isSwapped? body0 : body1;
SphereShape sphere0 = (SphereShape)sphereObj.GetCollisionShape();
//Vector3 normalOnSurfaceB;
Vector3 pOnBox = Vector3.Zero, pOnSphere = Vector3.Zero;
Vector3 sphereCenter = sphereObj.GetWorldTransform().Translation;
float radius = sphere0.GetRadius();
float dist = GetSphereDistance(boxObj,ref pOnBox,ref pOnSphere,ref sphereCenter,radius);
resultOut = new ManifoldResult();
resultOut.SetPersistentManifold(m_manifoldPtr);
if (dist < MathUtil.SIMD_EPSILON)
{
Vector3 normalOnSurfaceB = (pOnBox - pOnSphere);
normalOnSurfaceB.Normalize();
/// 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.GetNumContacts() > 0)
{
resultOut.RefreshContactPoints();
}
}
}
public virtual void CollideSingleContact(ref Quaternion perturbeRot, CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut)
{
CollisionObject convexObj = m_isSwapped? body1 : body0;
CollisionObject planeObj = m_isSwapped? body0: body1;
ConvexShape convexShape = (ConvexShape) convexObj.GetCollisionShape();
StaticPlaneShape planeShape = (StaticPlaneShape) planeObj.GetCollisionShape();
bool hasCollision = false;
Vector3 planeNormal = planeShape.GetPlaneNormal();
float planeConstant = planeShape.GetPlaneConstant();
Matrix convexWorldTransform = convexObj.GetWorldTransform();
Matrix convexInPlaneTrans = Matrix.Identity;
convexInPlaneTrans = MathUtil.BulletMatrixMultiply(Matrix.Invert(planeObj.GetWorldTransform()), convexWorldTransform);
//now perturbe the convex-world transform
// MAN - CHECKTHIS
Matrix rotMatrix = Matrix.CreateFromQuaternion(perturbeRot);
convexWorldTransform = MathUtil.BulletMatrixMultiplyBasis(convexWorldTransform,rotMatrix);
Matrix planeInConvex = Matrix.Identity;
planeInConvex= MathUtil.BulletMatrixMultiply(Matrix.Invert(convexWorldTransform),planeObj.GetWorldTransform());
Vector3 tmp = Vector3.TransformNormal(-planeNormal,planeInConvex);
Vector3 vtx = convexShape.LocalGetSupportingVertex(ref tmp);
Vector3 vtxInPlane = Vector3.Transform(vtx,convexInPlaneTrans);
float distance = (Vector3.Dot(planeNormal,vtxInPlane) - planeConstant);
Vector3 vtxInPlaneProjected = vtxInPlane - (distance*planeNormal);
Vector3 vtxInPlaneWorld = Vector3.Transform(vtxInPlaneProjected,planeObj.GetWorldTransform());
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
Vector3 normalOnSurfaceB = Vector3.TransformNormal(planeNormal,planeObj.GetWorldTransform());
Vector3 pOnB = vtxInPlaneWorld;
resultOut.AddContactPoint(ref normalOnSurfaceB,ref pOnB,distance);
}
}