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); } }