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