public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { //resultOut = new ManifoldResult(); if (m_manifoldPtr == null) { return; } CollisionObject sphereObj = m_swapped ? body1 : body0; CollisionObject triObj = m_swapped ? body0 : body1; SphereShape sphere = sphereObj.GetCollisionShape() as SphereShape; TriangleShape triangle = triObj.GetCollisionShape() as TriangleShape; /// report a contact. internally this will be kept persistent, and contact reduction is done resultOut.SetPersistentManifold(m_manifoldPtr); using (SphereTriangleDetector detector = BulletGlobals.SphereTriangleDetectorPool.Get()) { detector.Initialize(sphere, triangle, m_manifoldPtr.GetContactBreakingThreshold()); ClosestPointInput input = ClosestPointInput.Default(); input.m_maximumDistanceSquared = float.MaxValue; sphereObj.GetWorldTransform(out input.m_transformA); triObj.GetWorldTransform(out input.m_transformB); bool swapResults = m_swapped; detector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), swapResults); if (m_ownManifold) { resultOut.RefreshContactPoints(); } } }
public void GetClosestPoints(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw, bool swapResults) { IndexedMatrix transformA = input.m_transformA; IndexedMatrix transformB = input.m_transformB; IndexedVector3 point, normal; float timeOfImpact = 1f; float depth = 0f; // output.m_distance = float(1e30); //move sphere into triangle space IndexedMatrix sphereInTr = transformB.InverseTimes(ref transformA); IndexedVector3 temp = sphereInTr._origin; if (Collide(ref temp, out point, out normal, ref depth, ref timeOfImpact, m_contactBreakingThreshold)) { if (swapResults) { IndexedVector3 normalOnB = transformB._basis * normal; IndexedVector3 normalOnA = -normalOnB; IndexedVector3 pointOnA = transformB * point + normalOnB * depth; output.AddContactPoint(ref normalOnA, ref pointOnA, depth); } else { IndexedVector3 p = transformB._basis * normal; IndexedVector3 p2 = transformB * point; output.AddContactPoint(ref p, ref p2, depth); } } }
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.SetPersistentManifold(m_manifoldPtr); //comment-out next line to test multi-contact generation //resultOut.getPersistentManifold().clearManifold(); ConvexShape min0 = body0.GetCollisionShape() as ConvexShape; ConvexShape min1 = body1.GetCollisionShape() as ConvexShape; IndexedVector3 normalOnB = IndexedVector3.Zero; IndexedVector3 pointOnBWorld = IndexedVector3.Zero; { ClosestPointInput input = ClosestPointInput.Default(); using (GjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get()) { gjkPairDetector.Initialize(min0, min1, m_simplexSolver, m_pdSolver); //TODO: if (dispatchInfo.m_useContinuous) gjkPairDetector.SetMinkowskiA(min0); gjkPairDetector.SetMinkowskiB(min1); { input.m_maximumDistanceSquared = min0.GetMargin() + min1.GetMargin() + m_manifoldPtr.GetContactBreakingThreshold(); input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared; } input.m_transformA = body0.GetWorldTransform(); input.m_transformB = body1.GetWorldTransform(); gjkPairDetector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), false); #if DEBUG if (BulletGlobals.g_streamWriter != null) { BulletGlobals.g_streamWriter.WriteLine("c2dc2d processCollision"); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transformA", input.m_transformA); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transformB", input.m_transformB); } #endif } //BulletGlobals.GjkPairDetectorPool.Free(gjkPairDetector); //btVector3 v0,v1; //btVector3 sepNormalWorldSpace; } if (m_ownManifold) { resultOut.RefreshContactPoints(); } }
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { if (m_manifoldPtr == null) { return; } CollisionObject col0 = body0; CollisionObject col1 = body1; //resultOut = new ManifoldResult(body0, body1); BoxShape box0 = col0.GetCollisionShape() as BoxShape; BoxShape box1 = col1.GetCollisionShape() as BoxShape; //if (((String)col0.getUserPointer()).Contains("Box") && // ((String)col1.getUserPointer()).Contains("Box") ) //{ // int ibreak = 0; //} /// report a contact. internally this will be kept persistent, and contact reduction is done resultOut.SetPersistentManifold(m_manifoldPtr); #if !USE_PERSISTENT_CONTACTS m_manifoldPtr.ClearManifold(); #endif //USE_PERSISTENT_CONTACTS ClosestPointInput input = ClosestPointInput.Default(); input.m_maximumDistanceSquared = float.MaxValue; input.m_transformA = body0.GetWorldTransform(); input.m_transformB = body1.GetWorldTransform(); BoxBoxDetector.GetClosestPoints(box0, box1, ref input, resultOut, dispatchInfo.getDebugDraw(), false); #if USE_PERSISTENT_CONTACTS // refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added if (m_ownManifold) { resultOut.RefreshContactPoints(); } #endif //USE_PERSISTENT_CONTACTS }
public void ComputeClosestPoints(ref IndexedMatrix transA, ref IndexedMatrix transB, PointCollector pointCollector) { if (m_convexB1 != null) { m_simplexSolver.Reset(); GjkPairDetector gjk = new GjkPairDetector(m_convexA, m_convexB1, m_convexA.GetShapeType(), m_convexB1.GetShapeType(), m_convexA.GetMargin(), m_convexB1.GetMargin(), m_simplexSolver, m_penetrationDepthSolver); ClosestPointInput input = ClosestPointInput.Default(); input.m_transformA = transA; input.m_transformB = transB; gjk.GetClosestPoints(ref input, pointCollector, null); } else { //convex versus plane ConvexShape convexShape = m_convexA; StaticPlaneShape planeShape = m_planeShape; bool hasCollision = false; IndexedVector3 planeNormal = planeShape.GetPlaneNormal(); float planeConstant = planeShape.GetPlaneConstant(); IndexedMatrix convexWorldTransform = transA; IndexedMatrix convexInPlaneTrans = transB.Inverse() * convexWorldTransform; IndexedMatrix planeInConvex = convexWorldTransform.Inverse() * transB; IndexedVector3 vtx = convexShape.LocalGetSupportingVertex(planeInConvex._basis * -planeNormal); IndexedVector3 vtxInPlane = convexInPlaneTrans * vtx; float distance = IndexedVector3.Dot(planeNormal, vtxInPlane) - planeConstant; IndexedVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal; IndexedVector3 vtxInPlaneWorld = transB * vtxInPlaneProjected; IndexedVector3 normalOnSurfaceB = transB._basis * planeNormal; pointCollector.AddContactPoint( ref normalOnSurfaceB, ref vtxInPlaneWorld, distance); } }
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 = body0.GetCollisionShape() as ConvexShape; ConvexShape min1 = body1.GetCollisionShape() as ConvexShape; IndexedVector3 normalOnB; IndexedVector3 pointOnBWorld; #if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER if ((min0.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE)) { CapsuleShape capsuleA = min0 as CapsuleShape; CapsuleShape capsuleB = min1 as CapsuleShape; //IndexedVector3 localScalingA = capsuleA.GetLocalScaling(); //IndexedVector3 localScalingB = capsuleB.GetLocalScaling(); float threshold = m_manifoldPtr.GetContactBreakingThreshold(); float dist = CapsuleCapsuleDistance(out normalOnB, out 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 = ClosestPointInput.Default(); using (GjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get()) { gjkPairDetector.Initialize(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.GetMargin() + min1.GetMargin() + 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(); if (min0.IsPolyhedral() && min1.IsPolyhedral()) { DummyResult dummy = new DummyResult(); PolyhedralConvexShape polyhedronA = min0 as PolyhedralConvexShape; PolyhedralConvexShape polyhedronB = min1 as PolyhedralConvexShape; if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetConvexPolyhedron() != null) { float threshold = m_manifoldPtr.GetContactBreakingThreshold(); float minDist = float.MinValue; IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0); bool foundSepAxis = true; if (dispatchInfo.m_enableSatConvex) { foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis( polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(), body0.GetWorldTransform(), body1.GetWorldTransform(), out sepNormalWorldSpace); } else { #if ZERO_MARGIN gjkPairDetector.SetIgnoreMargin(true); gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.m_debugDraw); #else gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw); #endif float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared(); if (l2 > MathUtil.SIMD_EPSILON) { sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2); //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance(); minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin(); #if ZERO_MARGIN foundSepAxis = true; //gjkPairDetector.getCachedSeparatingDistance()<0.f; #else foundSepAxis = gjkPairDetector.GetCachedSeparatingDistance() < (min0.GetMargin() + min1.GetMargin()); #endif } } if (foundSepAxis) { // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); PolyhedralContactClipping.ClipHullAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(), body0.GetWorldTransform(), body1.GetWorldTransform(), minDist - threshold, threshold, resultOut); } if (m_ownManifold) { resultOut.RefreshContactPoints(); } return; } else { //we can also deal with convex versus triangle (without connectivity data) if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetShapeType() == BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE) { m_vertices.Clear(); TriangleShape tri = polyhedronB as TriangleShape; m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[0]); m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[1]); m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[2]); float threshold = m_manifoldPtr.GetContactBreakingThreshold(); IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0);; float minDist = float.MinValue; float maxDist = threshold; bool foundSepAxis = false; if (false) { polyhedronB.InitializePolyhedralFeatures(); foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis( polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(), body0.GetWorldTransform(), body1.GetWorldTransform(), out sepNormalWorldSpace); // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); } else { #if ZERO_MARGIN gjkPairDetector.SetIgnoreMargin(true); gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.m_debugDraw); #else gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw); #endif//ZERO_MARGIN float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared(); if (l2 > MathUtil.SIMD_EPSILON) { sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2); //minDist = gjkPairDetector.getCachedSeparatingDistance(); //maxDist = threshold; minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin(); foundSepAxis = true; } } if (foundSepAxis) { PolyhedralContactClipping.ClipFaceAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(), body0.GetWorldTransform(), m_vertices, minDist - threshold, maxDist, resultOut); } if (m_ownManifold) { resultOut.RefreshContactPoints(); } return; } } } gjkPairDetector.GetClosestPoints(ref 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) { IndexedVector3 v0, v1; IndexedVector3 sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis(); sepNormalWorldSpace.Normalize(); TransformUtil.PlaneSpace1(ref sepNormalWorldSpace, out v0, out 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; } IndexedMatrix unPerturbedTransform; 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) { IndexedQuaternion perturbeRot = new IndexedQuaternion(v0, perturbeAngle); float iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations); IndexedQuaternion rotq = new IndexedQuaternion(sepNormalWorldSpace, iterationAngle); if (perturbeA) { input.m_transformA._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body0.GetWorldTransform()._basis); input.m_transformB = body1.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._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body1.GetWorldTransform()._basis); #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(ref 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 virtual void GetClosestPoints(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw) { GetClosestPoints(ref input, output, debugDraw, false); }
public void GetClosestPointsNonVirtual(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw) { m_cachedSeparatingDistance = 0f; float distance = 0f; IndexedVector3 normalInB = IndexedVector3.Zero; IndexedVector3 pointOnA = IndexedVector3.Zero, pointOnB = IndexedVector3.Zero; IndexedMatrix localTransA = input.m_transformA; IndexedMatrix localTransB = input.m_transformB; IndexedVector3 positionOffset = (localTransA._origin + localTransB._origin) * .5f; IndexedVector3.Subtract(out localTransA._origin, ref localTransA._origin, ref positionOffset); IndexedVector3.Subtract(out localTransB._origin, ref localTransB._origin, ref positionOffset); //localTransB._origin -= positionOffset; bool check2d = m_minkowskiA.IsConvex2d() && m_minkowskiB.IsConvex2d(); float marginA = m_marginA; float marginB = m_marginB; #if TEST_NON_VIRTUAL float marginAv = m_minkowskiA.getMarginNonVirtual(); float marginBv = m_minkowskiB.getMarginNonVirtual(); Debug.Assert(marginA == marginAv); Debug.Assert(marginB == marginBv); #endif //TEST_NON_VIRTUAL gNumGjkChecks++; #if DEBUG_SPU_COLLISION_DETECTION spu_printf("inside gjk\n"); #endif //for CCD we don't use margins if (m_ignoreMargin) { marginA = 0f; marginB = 0f; #if DEBUG_SPU_COLLISION_DETECTION spu_printf("ignoring margin\n"); #endif } m_curIter = 0; int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN? m_cachedSeparatingAxis = new IndexedVector3(0, 1, 0); bool isValid = false; bool checkSimplex = false; bool checkPenetration = true; m_degenerateSimplex = 0; m_lastUsedMethod = -1; #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector) { MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjk::getClosestPointsNonVirtual transA", localTransA); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjk::getClosestPointsNonVirtual transB", localTransB); } #endif { float squaredDistance = MathUtil.BT_LARGE_FLOAT; float delta = 0f; float margin = marginA + marginB; m_simplexSolver.Reset(); int count = 0; for (; ;) //while (true) { count++; IndexedVector3 seperatingAxisInA = (-m_cachedSeparatingAxis) * input.m_transformA._basis; IndexedVector3 seperatingAxisInB = m_cachedSeparatingAxis * input.m_transformB._basis; IndexedVector3 pInA = m_minkowskiA.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA); IndexedVector3 qInB = m_minkowskiB.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB); IndexedVector3 pWorld = localTransA * pInA; IndexedVector3 qWorld = localTransB * qInB; if (check2d) { pWorld.Z = 0.0f; qWorld.Z = 0.0f; } IndexedVector3 w = new IndexedVector3(pWorld.X - qWorld.X, pWorld.Y - qWorld.Y, pWorld.Z - qWorld.Z); delta = IndexedVector3.Dot(ref m_cachedSeparatingAxis, ref w); #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector) { MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "m_cachedSeparatingAxis", m_cachedSeparatingAxis); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "w", w); BulletGlobals.g_streamWriter.WriteLine(String.Format("simplex num vertices [{0}]", m_simplexSolver.NumVertices())); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisA", seperatingAxisInA); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisB", seperatingAxisInB); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pInA", pInA); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qInB", qInB); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransA", localTransA); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransB", localTransB); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pWorld", pWorld); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qWorld", qWorld); } #endif // potential exit, they don't overlap if ((delta > 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(ref 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(ref w, ref pWorld, ref qWorld); //calculate the closest point to the origin (update vector v) IndexedVector3 newCachedSeparatingAxis; if (!m_simplexSolver.Closest(out newCachedSeparatingAxis)) { m_degenerateSimplex = 3; checkSimplex = true; break; } if (newCachedSeparatingAxis.LengthSquared() < REL_ERROR2) { m_cachedSeparatingAxis = newCachedSeparatingAxis; m_degenerateSimplex = 6; checkSimplex = true; break; } float previousSquaredDistance = squaredDistance; squaredDistance = newCachedSeparatingAxis.LengthSquared(); #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector) { MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisA", seperatingAxisInA); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisB", seperatingAxisInB); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pInA", pInA); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qInB", qInB); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pWorld", pWorld); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qWorld", qWorld); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "newSeperatingAxis", newCachedSeparatingAxis); BulletGlobals.g_streamWriter.WriteLine(String.Format("f0[{0:0.00000000}] f1[{1:0.00000000}] checkSimplex[{2}] degen[{3}]", f0, f1, checkSimplex, m_degenerateSimplex)); } #endif #if false ///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo if (squaredDistance > previousSquaredDistance) { m_degenerateSimplex = 7; squaredDistance = previousSquaredDistance; checkSimplex = false; break; } #endif // //redundant m_simplexSolver->compute_points(pointOnA, pointOnB); //are we getting any closer ? if (previousSquaredDistance - squaredDistance <= MathUtil.SIMD_EPSILON * previousSquaredDistance) { //m_simplexSolver.BackupClosest(ref m_cachedSeparatingAxis); checkSimplex = true; m_degenerateSimplex = 12; break; } m_cachedSeparatingAxis = newCachedSeparatingAxis; //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.BackupClosest(ref m_cachedSeparatingAxis); m_degenerateSimplex = 13; break; } } if (checkSimplex) { m_simplexSolver.ComputePoints(out pointOnA, out pointOnB); normalInB = m_cachedSeparatingAxis; float lenSqr = m_cachedSeparatingAxis.LengthSquared(); //valid normal if (lenSqr < 0.0001f) { m_degenerateSimplex = 5; } if (lenSqr > MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON) { //float rlen = 1 / normalInB.Length(); float rlen = 1.0f / (float)Math.Sqrt((float)lenSqr); //normalInB.Normalize(); normalInB *= rlen; float s = (float)Math.Sqrt((float)squaredDistance); Debug.Assert(s > 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 && 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 ref if (m_penetrationDepthSolver != null) { // Penetration depth case. IndexedVector3 tmpPointOnA = IndexedVector3.Zero, tmpPointOnB = IndexedVector3.Zero; gNumDeepPenetrationChecks++; m_cachedSeparatingAxis = IndexedVector3.Zero; bool isValid2 = m_penetrationDepthSolver.CalcPenDepth( m_simplexSolver, m_minkowskiA, m_minkowskiB, ref localTransA, ref localTransB, ref m_cachedSeparatingAxis, ref tmpPointOnA, ref tmpPointOnB, debugDraw ); #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector) { BulletGlobals.g_streamWriter.WriteLine("calcPenDepthResult"); BulletGlobals.g_streamWriter.WriteLine("lastMethodUsed : " + m_lastUsedMethod); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransA", localTransA); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransB", localTransB); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxis", m_cachedSeparatingAxis); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "tmpA", tmpPointOnA); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "tmpB", tmpPointOnB); } #endif if (isValid2) { IndexedVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA; float lenSqr = tmpNormalInB.LengthSquared(); if (lenSqr <= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON)) { tmpNormalInB = m_cachedSeparatingAxis; lenSqr = m_cachedSeparatingAxis.LengthSquared(); } if (lenSqr > (MathUtil.SIMD_EPSILON * MathUtil.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; //FIXME! check2d THIS m_lastUsedMethod = 3; } else { m_lastUsedMethod = 8; } } else { //isValid = false; 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.LengthSquared() > 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 DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector) { BulletGlobals.g_streamWriter.WriteLine("valid [{0}] distance[{1:0000.00000000}][{2:0000.00000000}] maxDistSq[{3:0000.00000000}]", isValid, distance, distance * distance, input.m_maximumDistanceSquared); } #endif if (isValid && ((distance < 0) || (distance * distance < input.m_maximumDistanceSquared))) { m_cachedSeparatingAxis = normalInB; m_cachedSeparatingDistance = distance; IndexedVector3 temp = pointOnB + positionOffset; output.AddContactPoint( ref normalInB, ref temp, distance); } }
public virtual void GetClosestPoints(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw, bool swapResults) { GetClosestPointsNonVirtual(ref input, output, debugDraw); }
public virtual bool CalcTimeOfImpact(ref IndexedMatrix fromA, ref IndexedMatrix toA, ref IndexedMatrix fromB, ref IndexedMatrix toB, CastResult result) { m_simplexSolver.Reset(); /// compute linear velocity for this interval, to interpolate //assume no rotation/angular velocity, assert here? IndexedVector3 linVelA, linVelB; linVelA = toA._origin - fromA._origin; linVelB = toB._origin - fromB._origin; float radius = 0.001f; float lambda = 0f; IndexedVector3 v = new IndexedVector3(1, 0, 0); int maxIter = MAX_ITERATIONS; IndexedVector3 n = IndexedVector3.Zero; bool hasResult = false; IndexedVector3 c; IndexedVector3 r = (linVelA - linVelB); float lastLambda = lambda; //float epsilon = float(0.001); int numIter = 0; //first solution, using GJK IndexedMatrix identityTrans = IndexedMatrix.Identity; // result.drawCoordSystem(sphereTr); PointCollector pointCollector = new PointCollector(); using (GjkPairDetector gjk = BulletGlobals.GjkPairDetectorPool.Get()) { gjk.Initialize(m_convexA, m_convexB, m_simplexSolver, null);//m_penetrationDepthSolver); ClosestPointInput input = ClosestPointInput.Default(); //we don't use margins during CCD // gjk.setIgnoreMargin(true); input.m_transformA = fromA; input.m_transformB = fromB; gjk.GetClosestPoints(ref input, pointCollector, null, false); hasResult = pointCollector.m_hasResult; c = pointCollector.m_pointInWorld; if (hasResult) { float dist = pointCollector.m_distance; n = pointCollector.m_normalOnBInWorld; //not close enough while (dist > radius) { numIter++; if (numIter > maxIter) { return(false); //todo: report a failure } float dLambda = 0f; float projectedLinearVelocity = IndexedVector3.Dot(r, n); dLambda = dist / (projectedLinearVelocity); lambda = lambda - dLambda; if (lambda > 1f || lambda < 0f) { return(false); } //todo: next check with relative epsilon if (lambda <= lastLambda) { return(false); //n.setValue(0,0,0); //break; } lastLambda = lambda; //interpolate to next lambda result.DebugDraw(lambda); input.m_transformA._origin = MathUtil.Interpolate3(fromA._origin, toA._origin, lambda); input.m_transformB._origin = MathUtil.Interpolate3(fromB._origin, toB._origin, lambda); gjk.GetClosestPoints(ref input, pointCollector, null, false); if (pointCollector.m_hasResult) { if (pointCollector.m_distance < 0f) { result.m_fraction = lastLambda; n = pointCollector.m_normalOnBInWorld; result.m_normal = n; result.m_hitPoint = pointCollector.m_pointInWorld; return(true); } c = pointCollector.m_pointInWorld; n = pointCollector.m_normalOnBInWorld; dist = pointCollector.m_distance; } else { //?? return(false); } } //is n normalized? //don't report time of impact for motion away from the contact normal (or causes minor penetration) if (IndexedVector3.Dot(n, r) >= -result.m_allowedPenetration) { return(false); } result.m_fraction = lambda; result.m_normal = n; result.m_hitPoint = c; return(true); } } return(false); }
public bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedVector3 v, ref IndexedVector3 pa, ref IndexedVector3 pb, IDebugDraw debugDraw) { bool check2d = convexA.IsConvex2d() && convexB.IsConvex2d(); float minProj = float.MaxValue; IndexedVector3 minNorm = IndexedVector3.Zero; IndexedVector3 minA = IndexedVector3.Zero, minB = IndexedVector3.Zero; IndexedVector3 seperatingAxisInA, seperatingAxisInB; IndexedVector3 pInA, qInB, pWorld, qWorld, w; #if USE_BATCHED_SUPPORT IndexedVector4[] supportVerticesABatch = new IndexedVector4[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; IndexedVector4[] supportVerticesBBatch = new IndexedVector4[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; IndexedVector3[] seperatingAxisInABatch = new IndexedVector3[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; IndexedVector3[] seperatingAxisInBBatch = new IndexedVector3[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; int numSampleDirections = NUM_UNITSPHERE_POINTS; for (int i = 0; i < numSampleDirections; i++) { IndexedVector3 norm = sPenetrationDirections[i]; IndexedVector3 negNorm = -norm; IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[i], ref negNorm, ref transA._basis); IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[i], ref norm, ref transB._basis); //seperatingAxisInABatch[i] = (-norm) * transA._basis; //seperatingAxisInBBatch[i] = norm * transB._basis; } { int numPDA = convexA.GetNumPreferredPenetrationDirections(); if (numPDA > 0) { for (int i = 0; i < numPDA; i++) { IndexedVector3 norm; convexA.GetPreferredPenetrationDirection(i, out norm); IndexedBasisMatrix.Multiply(ref norm, ref transA._basis, ref norm); sPenetrationDirections[numSampleDirections] = norm; IndexedVector3 negNorm = -norm; IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[numSampleDirections], ref negNorm, ref transA._basis); IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[numSampleDirections], ref norm, ref transB._basis); numSampleDirections++; } } } { int numPDB = convexB.GetNumPreferredPenetrationDirections(); if (numPDB > 0) { for (int i = 0; i < numPDB; i++) { IndexedVector3 norm; convexB.GetPreferredPenetrationDirection(i, out norm); IndexedBasisMatrix.Multiply(ref norm, ref transB._basis, ref norm); sPenetrationDirections[numSampleDirections] = norm; IndexedVector3 negNorm = -norm; IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[numSampleDirections], ref negNorm, ref transA._basis); IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[numSampleDirections], ref norm, ref transB._basis); numSampleDirections++; } } } convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch, numSampleDirections); convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch, numSampleDirections); for (int i = 0; i < numSampleDirections; i++) { IndexedVector3 norm = sPenetrationDirections[i]; if (check2d) { // shouldn't this be Y ? norm.Z = 0; } if (norm.LengthSquared() > 0.01f) { seperatingAxisInA = seperatingAxisInABatch[i]; seperatingAxisInB = seperatingAxisInBBatch[i]; pInA = new IndexedVector3(supportVerticesABatch[i].X, supportVerticesABatch[i].Y, supportVerticesABatch[i].Z); qInB = new IndexedVector3(supportVerticesBBatch[i].X, supportVerticesBBatch[i].Y, supportVerticesBBatch[i].Z); IndexedMatrix.Multiply(out pWorld, ref transA, ref pInA); IndexedMatrix.Multiply(out qWorld, ref transB, ref qInB); if (check2d) { // shouldn't this be Y ? pWorld.Z = 0f; qWorld.Z = 0f; } IndexedVector3.Subtract(out w, ref qWorld, ref pWorld); float delta = IndexedVector3.Dot(ref norm, ref w); //find smallest delta if (delta < minProj) { minProj = delta; minNorm = norm; minA = pWorld; minB = qWorld; } } } #else int numSampleDirections = NUM_UNITSPHERE_POINTS; { int numPDA = convexA.GetNumPreferredPenetrationDirections(); if (numPDA > 0) { for (int i = 0; i < numPDA; i++) { IndexedVector3 norm; convexA.GetPreferredPenetrationDirection(i, out norm); norm = IndexedVector3.TransformNormal(norm, transA); sPenetrationDirections[numSampleDirections] = norm; numSampleDirections++; } } } { int numPDB = convexB.GetNumPreferredPenetrationDirections(); if (numPDB > 0) { for (int i = 0; i < numPDB; i++) { IndexedVector3 norm = IndexedVector3.Zero; convexB.GetPreferredPenetrationDirection(i, out norm); norm = IndexedVector3.TransformNormal(norm, transB); sPenetrationDirections[numSampleDirections] = norm; numSampleDirections++; } } } for (int i = 0; i < numSampleDirections; i++) { IndexedVector3 norm = sPenetrationDirections[i]; if (check2d) { norm.Z = 0f; } if (norm.LengthSquared() > 0.01f) { seperatingAxisInA = IndexedVector3.TransformNormal(-norm, transA); seperatingAxisInB = IndexedVector3.TransformNormal(norm, transB); pInA = convexA.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA); qInB = convexB.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB); pWorld = IndexedVector3.Transform(pInA, transA); qWorld = IndexedVector3.Transform(qInB, transB); if (check2d) { pWorld.Z = 0.0f; qWorld.Z = 0.0f; } w = qWorld - pWorld; float delta = IndexedVector3.Dot(norm, w); //find smallest delta if (delta < minProj) { minProj = delta; minNorm = norm; minA = pWorld; minB = qWorld; } } } #endif //USE_BATCHED_SUPPORT //add the margins minA += minNorm * convexA.GetMarginNonVirtual(); minB -= minNorm * convexB.GetMarginNonVirtual(); //no penetration if (minProj < 0f) { return(false); } float extraSeparation = 0.5f;///scale dependent minProj += extraSeparation + (convexA.GetMarginNonVirtual() + convexB.GetMarginNonVirtual()); #if DEBUG_DRAW if (debugDraw) { IndexedVector3 color = new IndexedVector3(0, 1, 0); debugDraw.drawLine(minA, minB, color); color = new IndexedVector3(1, 1, 1); IndexedVector3 vec = minB - minA; float prj2 = IndexedVector3.Dot(minNorm, vec); debugDraw.drawLine(minA, minA + (minNorm * minProj), color); } #endif //DEBUG_DRAW GjkPairDetector gjkdet = BulletGlobals.GjkPairDetectorPool.Get(); gjkdet.Initialize(convexA, convexB, simplexSolver, null); float offsetDist = minProj; IndexedVector3 offset = minNorm * offsetDist; ClosestPointInput input = ClosestPointInput.Default(); IndexedVector3 newOrg = transA._origin + offset; IndexedMatrix displacedTrans = transA; displacedTrans._origin = newOrg; input.m_transformA = displacedTrans; input.m_transformB = transB; input.m_maximumDistanceSquared = float.MaxValue; MinkowskiIntermediateResult res = new MinkowskiIntermediateResult(); gjkdet.SetCachedSeperatingAxis(-minNorm); gjkdet.GetClosestPoints(ref input, res, debugDraw, false); float correctedMinNorm = minProj - res.m_depth; //the penetration depth is over-estimated, relax it float penetration_relaxation = 1f; minNorm *= penetration_relaxation; if (res.m_hasResult) { pa = res.m_pointInWorld - minNorm * correctedMinNorm; pb = res.m_pointInWorld; v = minNorm; #if DEBUG_DRAW if (debugDraw != null) { IndexedVector3 color = new IndexedVector3(1, 0, 0); debugDraw.drawLine(pa, pb, color); } #endif//DEBUG_DRAW } BulletGlobals.GjkPairDetectorPool.Free(gjkdet); return(res.m_hasResult); }
//public BoxBoxDetector(BoxShape box1, BoxShape box2) //{ // m_box1 = box1; // m_box2 = box2; //} // Work in progress to copy redo the box detector to remove un-necessary allocations public static void GetClosestPoints(BoxShape box1, BoxShape box2, ref ClosestPointInput input, ManifoldResult output, IDebugDraw debugDraw, bool swapResults) { IndexedMatrix transformA = input.m_transformA; IndexedMatrix transformB = input.m_transformB; #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugBoxBoxDetector) { MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "BoxBox:GCP:transformA", transformA); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "BoxBox:GCP:transformB", transformB); } #endif int skip = 0; Object contact = null; IndexedVector3 normal = new IndexedVector3(); float depth = 0f; int return_code = -1; int maxc = 4; IndexedVector3 translationA = new IndexedVector3(transformA._origin); IndexedVector3 translationB = new IndexedVector3(transformB._origin); //IndexedVector3 debugExtents = new IndexedVector3(2f, 2f, 2f); IndexedVector3 box1Margin = new IndexedVector3(2f * box1.GetHalfExtentsWithMargin()); IndexedVector3 box2Margin = new IndexedVector3(2f * box2.GetHalfExtentsWithMargin()); //IndexedVector3 box1Margin = 2f * debugExtents; //IndexedVector3 box2Margin = 2f * debugExtents; IndexedBasisMatrix rotateA = transformA._basis.Transpose(); IndexedBasisMatrix rotateB = transformB._basis.Transpose(); for (int j = 0; j < 3; j++) { s_temp1[0 + 4 * j] = transformA._basis[j].X; s_temp2[0 + 4 * j] = transformB._basis[j].X; s_temp1[1 + 4 * j] = transformA._basis[j].Y; s_temp2[1 + 4 * j] = transformB._basis[j].Y; s_temp1[2 + 4 * j] = transformA._basis[j].Z; s_temp2[2 + 4 * j] = transformB._basis[j].Z; } //s_temp1[0] = rotateA._Row0.X; //s_temp1[1] = rotateA._Row0.Y; //s_temp1[2] = rotateA._Row0.Z; //s_temp1[4] = rotateA._Row1.X; //s_temp1[5] = rotateA._Row1.Y; //s_temp1[6] = rotateA._Row1.Z; //s_temp1[8] = rotateA._Row2.X; //s_temp1[9] = rotateA._Row2.X; //s_temp1[10] = rotateA._Row2.X; //s_temp2[0] = rotateB._Row0.X; //s_temp2[1] = rotateB._Row0.Y; //s_temp2[2] = rotateB._Row0.Z; //s_temp2[4] = rotateB._Row1.X; //s_temp2[5] = rotateB._Row1.Y; //s_temp2[6] = rotateB._Row1.Z; //s_temp2[8] = rotateB._Row2.X; //s_temp2[9] = rotateB._Row2.Y; //s_temp2[10] = rotateB._Row2.Z; DBoxBox2(ref translationA, s_temp1, ref box1Margin, ref translationB, s_temp2, ref box2Margin, ref normal, ref depth, ref return_code, maxc, contact, skip, output); }