public override void ProcessCollision (CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut) { if (m_manifoldPtr == null) { return; } resultOut.SetPersistentManifold(m_manifoldPtr); SphereShape sphere0 = (SphereShape)body0.GetCollisionShape(); SphereShape sphere1 = (SphereShape)body1.GetCollisionShape(); Vector3 diff = body0.GetWorldTransform().Translation - body1.GetWorldTransform().Translation; float len = diff.Length(); float radius0 = sphere0.GetRadius(); float radius1 = sphere1.GetRadius(); #if CLEAR_MANIFOLD m_manifoldPtr.clearManifold(); //don't do this, it disables warmstarting #endif ///iff distance positive, don't generate a new contact if ( len > (radius0+radius1)) { #if !CLEAR_MANIFOLD resultOut.RefreshContactPoints(); #endif //CLEAR_MANIFOLD return; } ///distance (negative means penetration) float dist = len - (radius0+radius1); Vector3 normalOnSurfaceB = new Vector3(1,0,0); if (len > MathUtil.SIMD_EPSILON) { normalOnSurfaceB = diff / len; } ///point on A (worldspace) ///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB; ///point on B (worldspace) Vector3 pos1 = body1.GetWorldTransform().Translation + radius1* normalOnSurfaceB; /// report a contact. internally this will be kept persistent, and contact reduction is done resultOut.AddContactPoint(ref normalOnSurfaceB,ref pos1,dist); #if !CLEAR_MANIFOLD resultOut.RefreshContactPoints(); #endif //CLEAR_MANIFOLD }
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { if (m_manifoldPtr == null) { return; } CollisionObject col0 = body0; CollisionObject col1 = body1; Box2dShape box0 = (Box2dShape)col0.GetCollisionShape(); Box2dShape box1 = (Box2dShape)col1.GetCollisionShape(); resultOut.SetPersistentManifold(m_manifoldPtr); B2CollidePolygons(ref resultOut, box0, col0.GetWorldTransform(), box1, col1.GetWorldTransform()); // refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added if (m_ownManifold) { resultOut.RefreshContactPoints(); } }
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 = (SphereShape)sphereObj.GetCollisionShape(); TriangleShape triangle = (TriangleShape)triObj.GetCollisionShape(); /// report a contact. internally this will be kept persistent, and contact reduction is done resultOut.SetPersistentManifold(m_manifoldPtr); SphereTriangleDetector detector = new SphereTriangleDetector(sphere,triangle, m_manifoldPtr.GetContactBreakingThreshold()); ClosestPointInput input = new ClosestPointInput(); input.m_maximumDistanceSquared = float.MaxValue; input.m_transformA = sphereObj.GetWorldTransform(); input.m_transformB = triObj.GetWorldTransform(); bool swapResults = m_swapped; detector.GetClosestPoints(input,resultOut,dispatchInfo.getDebugDraw(),swapResults); if (m_ownManifold) { resultOut.RefreshContactPoints(); } }
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 = (ConvexShape)(body0.GetCollisionShape()); ConvexShape min1 = (ConvexShape)(body1.GetCollisionShape()); Vector3 normalOnB = Vector3.Zero; Vector3 pointOnBWorld = Vector3.Zero; { ClosestPointInput input = new ClosestPointInput(); GjkPairDetector gjkPairDetector = new GjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver); //TODO: if (dispatchInfo.m_useContinuous) gjkPairDetector.SetMinkowskiA(min0); gjkPairDetector.SetMinkowskiB(min1); { input.m_maximumDistanceSquared = min0.Margin + min1.Margin + m_manifoldPtr.GetContactBreakingThreshold(); input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; } input.m_transformA = body0.GetWorldTransform(); input.m_transformB = body1.GetWorldTransform(); gjkPairDetector.GetClosestPoints(input,resultOut,dispatchInfo.getDebugDraw(),false); 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); } //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 = (BoxShape)col0.GetCollisionShape(); BoxShape box1 = (BoxShape)col1.GetCollisionShape(); //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 = new ClosestPointInput(); input.m_maximumDistanceSquared = float.MaxValue; input.m_transformA = body0.GetWorldTransform(); input.m_transformB = body1.GetWorldTransform(); BoxBoxDetector detector = new BoxBoxDetector(box0,box1); detector.GetClosestPoints(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 override void ProcessCollision (CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut) { //(void)dispatchInfo; //(void)resultOut; if (m_manifoldPtr == null) { resultOut = null; return; } CollisionObject sphereObj = m_isSwapped? body1 : body0; CollisionObject boxObj = m_isSwapped? body0 : body1; SphereShape sphere0 = (SphereShape)sphereObj.GetCollisionShape(); //Vector3 normalOnSurfaceB; Vector3 pOnBox = Vector3.Zero, pOnSphere = Vector3.Zero; Vector3 sphereCenter = sphereObj.GetWorldTransform().Translation; float radius = sphere0.GetRadius(); float dist = GetSphereDistance(boxObj,ref pOnBox,ref pOnSphere,ref sphereCenter,radius); resultOut = new ManifoldResult(); resultOut.SetPersistentManifold(m_manifoldPtr); if (dist < MathUtil.SIMD_EPSILON) { Vector3 normalOnSurfaceB = (pOnBox - pOnSphere); normalOnSurfaceB.Normalize(); /// report a contact. internally this will be kept persistent, and contact reduction is done resultOut.AddContactPoint(ref normalOnSurfaceB,ref pOnBox,dist); } if (m_ownManifold) { if (m_manifoldPtr.GetNumContacts() > 0) { resultOut.RefreshContactPoints(); } } }
public override void ProcessCollision(CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut) { if (m_manifoldPtr == null) { //swapped? m_manifoldPtr = m_dispatcher.GetNewManifold(body0,body1); m_ownManifold = true; } //resultOut = new ManifoldResult(); resultOut.SetPersistentManifold(m_manifoldPtr); //comment-out next line to test multi-contact generation //resultOut.getPersistentManifold().clearManifold(); ConvexShape min0 = (ConvexShape)(body0.GetCollisionShape()); ConvexShape min1 = (ConvexShape)(body1.GetCollisionShape()); Vector3 normalOnB = Vector3.Up; Vector3 pointOnBWorld = Vector3.Zero; #if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER if ((min0.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE)) { CapsuleShape capsuleA = (CapsuleShape) min0; CapsuleShape capsuleB = (CapsuleShape) min1; Vector3 localScalingA = capsuleA.GetLocalScaling(); Vector3 localScalingB = capsuleB.GetLocalScaling(); float threshold = m_manifoldPtr.GetContactBreakingThreshold(); float dist = CapsuleCapsuleDistance(ref normalOnB,ref pointOnBWorld,capsuleA.getHalfHeight(),capsuleA.getRadius(), capsuleB.getHalfHeight(),capsuleB.getRadius(),capsuleA.GetUpAxis(),capsuleB.GetUpAxis(), body0.GetWorldTransform(),body1.GetWorldTransform(),threshold); if (dist<threshold) { Debug.Assert(normalOnB.LengthSquared() >= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON)); resultOut.AddContactPoint(ref normalOnB,ref pointOnBWorld,dist); } resultOut.RefreshContactPoints(); return; } #endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER #if USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { m_sepDistance.updateSeparatingDistance(body0.getWorldTransform(),body1.getWorldTransform()); } if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f) #endif //USE_SEPDISTANCE_UTIL2 { ClosestPointInput input = new ClosestPointInput(); GjkPairDetector gjkPairDetector = new GjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver); //TODO: if (dispatchInfo.m_useContinuous) gjkPairDetector.SetMinkowskiA(min0); gjkPairDetector.SetMinkowskiB(min1); #if USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { input.m_maximumDistanceSquared = float.MaxValue; } else #endif //USE_SEPDISTANCE_UTIL2 { input.m_maximumDistanceSquared = min0.Margin + min1.Margin + m_manifoldPtr.GetContactBreakingThreshold(); input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; } //input.m_stackAlloc = dispatchInfo.m_stackAllocator; input.m_transformA = body0.GetWorldTransform(); input.m_transformB = body1.GetWorldTransform(); gjkPairDetector.GetClosestPoints(input,resultOut,dispatchInfo.getDebugDraw(),false); #if USE_SEPDISTANCE_UTIL2 float sepDist = 0.f; if (dispatchInfo.m_useConvexConservativeDistanceUtil) { sepDist = gjkPairDetector.getCachedSeparatingDistance(); if (sepDist>MathUtil.SIMD_EPSILON) { sepDist += dispatchInfo.m_convexConservativeDistanceThreshold; //now perturbe directions to get multiple contact points } } #endif //USE_SEPDISTANCE_UTIL2 //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points if (m_numPerturbationIterations > 0 && resultOut.GetPersistentManifold().GetNumContacts() < m_minimumPointsPerturbationThreshold) { Vector3 v0 = Vector3.Zero, v1 = Vector3.Zero; Vector3 sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis(); sepNormalWorldSpace.Normalize(); TransformUtil.PlaneSpace1(ref sepNormalWorldSpace, ref v0, ref v1); bool perturbeA = true; const float angleLimit = 0.125f * MathUtil.SIMD_PI; float perturbeAngle; float radiusA = min0.GetAngularMotionDisc(); float radiusB = min1.GetAngularMotionDisc(); if (radiusA < radiusB) { perturbeAngle = BulletGlobals.gContactBreakingThreshold /radiusA; perturbeA = true; } else { perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusB; perturbeA = false; } if (perturbeAngle > angleLimit) { perturbeAngle = angleLimit; } Matrix unPerturbedTransform = Matrix.Identity; if (perturbeA) { unPerturbedTransform = input.m_transformA; } else { unPerturbedTransform = input.m_transformB; } for (int i=0;i<m_numPerturbationIterations;i++) { if (v0.LengthSquared() > MathUtil.SIMD_EPSILON) { Quaternion perturbeRot = Quaternion.CreateFromAxisAngle(v0, perturbeAngle); float iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations); Quaternion rotq = Quaternion.CreateFromAxisAngle(sepNormalWorldSpace, iterationAngle); if (perturbeA) { //input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0.getWorldTransform().getBasis()); Quaternion temp = MathUtil.QuaternionMultiply(MathUtil.QuaternionInverse(ref rotq),MathUtil.QuaternionMultiply(perturbeRot,rotq)); input.m_transformA = MathUtil.BulletMatrixMultiplyBasis(Matrix.CreateFromQuaternion(temp),body0.GetWorldTransform()); input.m_transformB = body1.GetWorldTransform(); #if DEBUG_CONTACTS dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformA,10.0f); #endif //DEBUG_CONTACTS } else { input.m_transformA = body0.GetWorldTransform(); //input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1.getWorldTransform().getBasis()); Quaternion temp = MathUtil.QuaternionMultiply(MathUtil.QuaternionInverse(ref rotq),MathUtil.QuaternionMultiply(perturbeRot,rotq)); input.m_transformB = MathUtil.BulletMatrixMultiplyBasis(Matrix.CreateFromQuaternion(temp),body1.GetWorldTransform()); #if DEBUG_CONTACTS dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformB,10.0f); #endif } PerturbedContactResult perturbedResultOut = new PerturbedContactResult(resultOut, ref input.m_transformA, ref input.m_transformB, ref unPerturbedTransform, perturbeA, dispatchInfo.getDebugDraw()); gjkPairDetector.GetClosestPoints(input, perturbedResultOut, dispatchInfo.getDebugDraw(), false); } } } #if USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist > MathUtil.SIMD_EPSILON)) { m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0.getWorldTransform(),body1.getWorldTransform()); } #endif //USE_SEPDISTANCE_UTIL2 } if (m_ownManifold) { resultOut.RefreshContactPoints(); } }
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { //resultOut = null; CollisionObject colObj = m_isSwapped? body1 : body0; CollisionObject otherObj = m_isSwapped? body0 : body1; System.Diagnostics.Debug.Assert(colObj.GetCollisionShape().IsCompound()); CompoundShape compoundShape = (CompoundShape)(colObj.GetCollisionShape()); ///btCompoundShape might have changed: ////make sure the internal child collision algorithm caches are still valid if (compoundShape.GetUpdateRevision() != m_compoundShapeRevision) { ///clear and update all RemoveChildAlgorithms(); PreallocateChildAlgorithms(body0,body1); } Dbvt tree = compoundShape.GetDynamicAabbTree(); //use a dynamic aabb tree to cull potential child-overlaps CompoundLeafCallback callback = new CompoundLeafCallback(colObj,otherObj,m_dispatcher,dispatchInfo,resultOut,m_childCollisionAlgorithms,m_sharedManifold); ///we need to refresh all contact manifolds ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm { IList<PersistentManifold> manifoldArray = new List<PersistentManifold>(); for (int i=0;i<m_childCollisionAlgorithms.Count;i++) { if (m_childCollisionAlgorithms[i] != null) { m_childCollisionAlgorithms[i].GetAllContactManifolds(manifoldArray); for (int m=0;m<manifoldArray.Count;m++) { if (manifoldArray[m].GetNumContacts() > 0) { resultOut.SetPersistentManifold(manifoldArray[m]); resultOut.RefreshContactPoints(); resultOut.SetPersistentManifold(null);//??necessary? } } manifoldArray.Clear(); } } } if (tree != null) { Vector3 localAabbMin = new Vector3(); Vector3 localAabbMax = new Vector3(); Matrix otherInCompoundSpace = Matrix.Identity; //otherInCompoundSpace = MathUtil.BulletMatrixMultiply(colObj.GetWorldTransform(),otherObj.GetWorldTransform()); otherInCompoundSpace = MathUtil.InverseTimes(colObj.GetWorldTransform(), otherObj.GetWorldTransform()); otherObj.GetCollisionShape().GetAabb(ref otherInCompoundSpace,ref localAabbMin,ref localAabbMax); DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax); //process all children, that overlap with the given AABB bounds Dbvt.CollideTV(tree.m_root,ref bounds,callback); } else { //iterate over all children, perform an AABB check inside ProcessChildShape int numChildren = m_childCollisionAlgorithms.Count; for (int i=0;i<numChildren;i++) { callback.ProcessChildShape(compoundShape.GetChildShape(i),i); } } { //iterate over all children, perform an AABB check inside ProcessChildShape int numChildren = m_childCollisionAlgorithms.Count; IList<PersistentManifold> manifoldArray = new List<PersistentManifold>(); for (int i=0;i<numChildren;i++) { if (m_childCollisionAlgorithms[i] != null) { CollisionShape childShape = compoundShape.GetChildShape(i); //if not longer overlapping, remove the algorithm Matrix orgTrans = colObj.GetWorldTransform(); Matrix orgInterpolationTrans = colObj.GetInterpolationWorldTransform(); Matrix childTrans = compoundShape.GetChildTransform(i); Matrix newChildWorldTrans = MathUtil.BulletMatrixMultiply(ref orgTrans, ref childTrans); //perform an AABB check first Vector3 aabbMin0 = new Vector3(); Vector3 aabbMax0 = new Vector3(); Vector3 aabbMin1 = new Vector3(); Vector3 aabbMax1 = new Vector3(); childShape.GetAabb(ref newChildWorldTrans,ref aabbMin0,ref aabbMax0); otherObj.GetCollisionShape().GetAabb(otherObj.GetWorldTransform(),ref aabbMin1,ref aabbMax1); if (!AabbUtil2.TestAabbAgainstAabb2(ref aabbMin0,ref aabbMax0,ref aabbMin1,ref aabbMax1)) { m_dispatcher.FreeCollisionAlgorithm(m_childCollisionAlgorithms[i]); m_childCollisionAlgorithms[i] = null; } } } } }
public override void ProcessCollision (CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut) { if (m_manifoldPtr == null) { return; } CollisionObject convexObj = m_isSwapped? body1 : body0; CollisionObject planeObj = m_isSwapped? body0: body1; ConvexShape convexShape = (ConvexShape) convexObj.GetCollisionShape(); StaticPlaneShape planeShape = (StaticPlaneShape) planeObj.GetCollisionShape(); //bool hasCollision = false; Vector3 planeNormal = planeShape.GetPlaneNormal(); //float planeConstant = planeShape.getPlaneConstant(); //first perform a collision query with the non-perturbated collision objects { Quaternion rotq = Quaternion.Identity; CollideSingleContact(ref rotq,body0,body1,dispatchInfo,resultOut); } if (resultOut.GetPersistentManifold().GetNumContacts()<m_minimumPointsPerturbationThreshold) { Vector3 v0 = Vector3.Zero; Vector3 v1 = Vector3.Zero; TransformUtil.PlaneSpace1(ref planeNormal,ref v0,ref v1); //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects float angleLimit = 0.125f * MathUtil.SIMD_PI; float perturbeAngle; float radius = convexShape.GetAngularMotionDisc(); perturbeAngle = BulletGlobals.gContactBreakingThreshold / radius; if ( perturbeAngle > angleLimit ) { perturbeAngle = angleLimit; } Quaternion perturbeRot = Quaternion.CreateFromAxisAngle(v0,perturbeAngle); for (int i=0;i<m_numPerturbationIterations;i++) { float iterationAngle = i*(MathUtil.SIMD_2_PI/(float)m_numPerturbationIterations); Quaternion rotq = Quaternion.CreateFromAxisAngle(planeNormal,iterationAngle); rotq = MathUtil.QuaternionMultiply(Quaternion.Inverse(rotq),MathUtil.QuaternionMultiply(perturbeRot,rotq)); CollideSingleContact(ref rotq,body0,body1,dispatchInfo,resultOut); } } if (m_ownManifold) { if (m_manifoldPtr.GetNumContacts() > 0) { resultOut.RefreshContactPoints(); } } }
//public override void processCollision (CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut) //{ // CollisionObject convexBody = m_isSwapped ? body1 : body0; // CollisionObject triBody = m_isSwapped ? body0 : body1; // if (triBody.getCollisionShape().isConcave()) // { // CollisionObject triOb = triBody; // ConcaveShape concaveShape = (ConcaveShape)(triOb.getCollisionShape()); // if (convexBody.getCollisionShape().isConvex()) // { // float collisionMarginTriangle = concaveShape.getMargin(); // resultOut.setPersistentManifold(m_convexTriangleCallback.m_manifoldPtr); // m_convexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut); // //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here. // //m_dispatcher.clearManifold(m_convexTriangleCallback.m_manifoldPtr); // m_convexTriangleCallback.m_manifoldPtr.setBodies(convexBody, triBody); // Vector3 min = m_convexTriangleCallback.getAabbMin(); // Vector3 max = m_convexTriangleCallback.getAabbMax(); // concaveShape.processAllTriangles(m_convexTriangleCallback, ref min,ref max ); // resultOut.refreshContactPoints(); // } // } //} //public override float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) //{ // CollisionObject convexbody = m_isSwapped ? body1 : body0; // CollisionObject triBody = m_isSwapped ? body0 : body1; // //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast) // //only perform CCD above a certain threshold, this prevents blocking on the long run // //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame... // float squareMot0 = (convexbody.getInterpolationWorldTransform().Translation - convexbody.getWorldTransform().Translation).LengthSquared(); // if (squareMot0 < convexbody.getCcdSquareMotionThreshold()) // { // return 1f; // } // //const Vector3& from = convexbody.m_worldTransform.Translation; // //Vector3 to = convexbody.m_interpolationWorldTransform.Translation; // //todo: only do if the motion exceeds the 'radius' // //Matrix triInv = Matrix.Invert(triBody.getWorldTransform()); // //Matrix convexFromLocal = MathUtil.bulletMatrixMultiply(triInv , convexbody.getWorldTransform()); // //Matrix convexToLocal = MathUtil.bulletMatrixMultiply(triInv , convexbody.getInterpolationWorldTransform()); // Matrix triInv = Matrix.Invert(triBody.getWorldTransform()); // Matrix convexFromLocal = MathUtil.inverseTimes(triBody.getWorldTransform(), convexbody.getWorldTransform()); // Matrix convexToLocal = MathUtil.inverseTimes(triBody.getWorldTransform(), convexbody.getInterpolationWorldTransform()); // if (triBody.getCollisionShape().isConcave()) // { // Vector3 rayAabbMin = convexFromLocal.Translation; // MathUtil.vectorMin(convexToLocal.Translation, ref rayAabbMin); // Vector3 rayAabbMax = convexFromLocal.Translation; // MathUtil.vectorMax(convexToLocal.Translation,ref rayAabbMax); // float ccdRadius0 = convexbody.getCcdSweptSphereRadius(); // rayAabbMin -= new Vector3(ccdRadius0,ccdRadius0,ccdRadius0); // rayAabbMax += new Vector3(ccdRadius0,ccdRadius0,ccdRadius0); // float curHitFraction = 1.0f; //is this available? // LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(ref convexFromLocal, ref convexToLocal, // convexbody.getCcdSweptSphereRadius(),curHitFraction); // raycastCallback.m_hitFraction = convexbody.getHitFraction(); // CollisionObject concavebody = triBody; // ConcaveShape triangleMesh = (ConcaveShape) concavebody.getCollisionShape(); // if (triangleMesh != null) // { // triangleMesh.processAllTriangles(raycastCallback,ref rayAabbMin,ref rayAabbMax); // } // if (raycastCallback.m_hitFraction < convexbody.getHitFraction()) // { // convexbody.setHitFraction( raycastCallback.m_hitFraction); // float result = raycastCallback.m_hitFraction; // raycastCallback.cleanup(); // return result; // } // raycastCallback.cleanup(); // } // return 1f; //} public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { //fixme CollisionObject convexBody = m_isSwapped ? bodyB : bodyA; CollisionObject triBody = m_isSwapped ? bodyA : bodyB; if (triBody.GetCollisionShape().IsConcave()) { CollisionObject triOb = triBody; ConcaveShape concaveShape = triOb.GetCollisionShape() as ConcaveShape; if (convexBody.GetCollisionShape().IsConvex()) { float collisionMarginTriangle = concaveShape.Margin; resultOut.SetPersistentManifold(m_convexTriangleCallback.m_manifoldPtr); m_convexTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut); //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here. //m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr); m_convexTriangleCallback.m_manifoldPtr.SetBodies(convexBody, triBody); Vector3 min = m_convexTriangleCallback.GetAabbMin(); Vector3 max = m_convexTriangleCallback.GetAabbMax(); concaveShape.ProcessAllTriangles(m_convexTriangleCallback, ref min,ref max); resultOut.RefreshContactPoints(); } } }