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 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 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 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 }
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 ManifoldResult output, 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; 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; btVector3 temp = pointOnB + positionOffset; output.addContactPoint( ref normalInB, ref temp, distance); } }
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); } }