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 void NearCallback(BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo) { CollisionObject colObj0 = (CollisionObject)collisionPair.m_pProxy0.GetClientObject(); CollisionObject colObj1 = (CollisionObject)collisionPair.m_pProxy1.GetClientObject(); if (dispatcher.NeedsCollision(colObj0,colObj1)) { //dispatcher will keep algorithms persistent in the collision pair if (collisionPair.m_algorithm == null) { collisionPair.m_algorithm = dispatcher.FindAlgorithm(colObj0,colObj1,null); } if (collisionPair.m_algorithm != null) { ManifoldResult contactPointResult = new ManifoldResult(colObj0,colObj1); if (dispatchInfo.GetDispatchFunc() == DispatchFunc.DISPATCH_DISCRETE) { //discrete collision detection query collisionPair.m_algorithm.ProcessCollision(colObj0,colObj1,dispatchInfo,contactPointResult); } else { //continuous collision detection query, time of impact (toi) float toi = collisionPair.m_algorithm.CalculateTimeOfImpact(colObj0,colObj1,dispatchInfo,contactPointResult); if (dispatchInfo.GetTimeOfImpact() > toi) { dispatchInfo.SetTimeOfImpact(toi); } } } } }
public PerturbedContactResult(ManifoldResult originalResult,ref Matrix transformA,ref Matrix transformB,ref Matrix unPerturbedTransform,bool perturbA,IDebugDraw debugDrawer) { m_originalManifoldResult = originalResult; m_transformA = transformA; m_transformB = transformB; m_perturbA = perturbA; m_unPerturbedTransform = unPerturbedTransform; m_debugDrawer = debugDrawer; }
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) { 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 CompoundLeafCallback (CollisionObject compoundObj,CollisionObject otherObj,IDispatcher dispatcher,DispatcherInfo dispatchInfo,ManifoldResult resultOut,IList<CollisionAlgorithm> childCollisionAlgorithms,PersistentManifold sharedManifold) { m_compoundColObj = compoundObj; m_otherObj = otherObj; m_dispatcher = dispatcher; m_dispatchInfo = dispatchInfo; m_resultOut = resultOut; m_childCollisionAlgorithms = childCollisionAlgorithms; m_sharedManifold = sharedManifold; }
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 void SetTimeStepAndCounters(float collisionMarginTriangle, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { m_dispatchInfoPtr = dispatchInfo; m_collisionMarginTriangle = collisionMarginTriangle; m_resultOut = resultOut; //recalc aabbs //Matrix convexInTriangleSpace = MathUtil.bulletMatrixMultiply(Matrix.Invert(m_triBody.getWorldTransform()) , m_convexBody.getWorldTransform()); Matrix convexInTriangleSpace = MathUtil.InverseTimes(m_triBody.GetWorldTransform(), m_convexBody.GetWorldTransform()); CollisionShape convexShape = m_convexBody.GetCollisionShape(); convexShape.GetAabb(ref convexInTriangleSpace,ref m_aabbMin,ref m_aabbMax); float extraMargin = collisionMarginTriangle; Vector3 extra = new Vector3(extraMargin,extraMargin,extraMargin); m_aabbMax += extra; m_aabbMin -= extra; }
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) { 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) { 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(); } }
// Work in progress to copy redo the box detector to remove un-necessary allocations #if true public virtual void GetClosestPoints(ClosestPointInput input, ManifoldResult output, IDebugDraw debugDraw, bool swapResults) { Matrix transformA = input.m_transformA; Matrix transformB = input.m_transformB; if (BulletGlobals.g_streamWriter != null && debugBoxBox) { MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "BoxBox:GCP:transformA", transformA); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "BoxBox:GCP:transformB", transformB); } int skip = 0; Object contact = null; Matrix rotateA = Matrix.Identity; rotateA.Backward = transformA.Backward; rotateA.Right = transformA.Right; rotateA.Up = transformA.Up; Matrix rotateB = Matrix.Identity; rotateB.Backward = transformB.Backward; rotateB.Right = transformB.Right; rotateB.Up = transformB.Up; IndexedVector3 normal = new IndexedVector3(); float depth = 0f; int return_code = -1; int maxc = 4; IndexedVector3 translationA = new IndexedVector3(transformA.Translation); IndexedVector3 translationB = new IndexedVector3(transformB.Translation); Vector3 debugExtents = new Vector3(2f, 2f, 2f); IndexedVector3 box1Margin = new IndexedVector3(2f * m_box1.GetHalfExtentsWithMargin()); IndexedVector3 box2Margin = new IndexedVector3(2f * m_box2.GetHalfExtentsWithMargin()); //Vector3 box1Margin = 2f * debugExtents; //Vector3 box2Margin = 2f * debugExtents; rotateA = Matrix.Transpose(rotateA); rotateB = Matrix.Transpose(rotateB); float[] temp1 = s_temp1; float[] temp2 = s_temp2; temp1[0] = rotateA.M11; temp1[1] = rotateA.M12; temp1[2] = rotateA.M13; temp1[4] = rotateA.M21; temp1[5] = rotateA.M22; temp1[6] = rotateA.M23; temp1[8] = rotateA.M31; temp1[9] = rotateA.M32; temp1[10] = rotateA.M33; temp2[0] = rotateB.M11; temp2[1] = rotateB.M12; temp2[2] = rotateB.M13; temp2[4] = rotateB.M21; temp2[5] = rotateB.M22; temp2[6] = rotateB.M23; temp2[8] = rotateB.M31; temp2[9] = rotateB.M32; temp2[10] = rotateB.M33; DBoxBox2(ref translationA, temp1, ref box1Margin, ref translationB, temp2, ref box2Margin, ref normal, ref depth, ref return_code, maxc, contact, skip, output); }
public override void ProcessCollision (CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut) { }
public override float CalculateTimeOfImpact(CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut) { ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold ///body0.m_worldTransform, float resultFraction = 1.0f; float squareMot0 = (body0.GetInterpolationWorldTransform().Translation - body0.GetWorldTransform().Translation).LengthSquared(); float squareMot1 = (body1.GetInterpolationWorldTransform().Translation - body1.GetWorldTransform().Translation).LengthSquared(); if (squareMot0 < body0.GetCcdSquareMotionThreshold() && squareMot1 < body1.GetCcdSquareMotionThreshold()) { return resultFraction; } //An adhoc way of testing the Continuous Collision Detection algorithms //One object is approximated as a sphere, to simplify things //Starting in penetration should report no time of impact //For proper CCD, better accuracy and handling of 'allowed' penetration should be added //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies) /// Convex0 against sphere for Convex1 { ConvexShape convex0 = (ConvexShape)(body0.GetCollisionShape()); SphereShape sphere1 = new SphereShape(body1.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation CastResult result = new CastResult(); VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver(); //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); ///Simplification, one object is simplified as a sphere GjkConvexCast ccd1 = new GjkConvexCast( convex0 ,sphere1,voronoiSimplex); //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(),body0.GetInterpolationWorldTransform(), body1.GetWorldTransform(),body1.GetInterpolationWorldTransform(),result)) { //store result.m_fraction in both bodies if (body0.GetHitFraction()> result.m_fraction) { body0.SetHitFraction( result.m_fraction ); } if (body1.GetHitFraction() > result.m_fraction) { body1.SetHitFraction( result.m_fraction); } if (resultFraction > result.m_fraction) { resultFraction = result.m_fraction; } } } /// Sphere (for convex0) against Convex1 { ConvexShape convex1 = (ConvexShape)(body1.GetCollisionShape()); SphereShape sphere0 = new SphereShape(body0.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation CastResult result = new CastResult(); VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver(); //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); ///Simplification, one object is simplified as a sphere GjkConvexCast ccd1 = new GjkConvexCast(sphere0,convex1,voronoiSimplex); //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(),body0.GetInterpolationWorldTransform(), body1.GetWorldTransform(),body1.GetInterpolationWorldTransform(),result)) { //store result.m_fraction in both bodies if (body0.GetHitFraction() > result.m_fraction) { body0.SetHitFraction( result.m_fraction); } if (body1.GetHitFraction() > result.m_fraction) { body1.SetHitFraction( result.m_fraction); } if (resultFraction > result.m_fraction) { resultFraction = result.m_fraction; } } } return resultFraction; }
// Find edge normal of max separation on A - return if separating axis is found // Find edge normal of max separation on B - return if separation axis is found // Choose reference edge as min(minA, minB) // Find incident edge // Clip // The normal points from 1 to 2 void B2CollidePolygons(ref ManifoldResult manifold, Box2dShape polyA, Matrix xfA, Box2dShape polyB, Matrix xfB) { B2CollidePolygons(ref manifold, polyA, ref xfA, polyB, ref xfB); }
void B2CollidePolygons(ref ManifoldResult manifold, Box2dShape polyA, ref Matrix xfA, Box2dShape polyB, ref Matrix xfB) { int edgeA = 0; float separationA = FindMaxSeparation(ref edgeA, polyA, ref xfA, polyB, ref xfB); if (separationA > 0.0f) { return; } int edgeB = 0; float separationB = FindMaxSeparation(ref edgeB, polyB, ref xfB, polyA, ref xfA); if (separationB > 0.0f) { return; } Box2dShape poly1; // reference poly Box2dShape poly2; // incident poly Matrix xf1, xf2; int edge1; // reference edge bool flip; const float k_relativeTol = 0.98f; const float k_absoluteTol = 0.001f; // TODO_ERIN use "radius" of poly for absolute tolerance. if (separationB > k_relativeTol * separationA + k_absoluteTol) { poly1 = polyB; poly2 = polyA; xf1 = xfB; xf2 = xfA; edge1 = edgeB; flip = true; } else { poly1 = polyA; poly2 = polyB; xf1 = xfA; xf2 = xfB; edge1 = edgeA; flip = false; } ClipVertex[] incidentEdge = new ClipVertex[2]; FindIncidentEdge(incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2); int count1 = poly1.GetVertexCount(); Vector3[] vertices1 = poly1.GetVertices(); Vector3 v11 = vertices1[edge1]; Vector3 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0]; Vector3 dv = v12 - v11; Vector3 sideNormal = Vector3.TransformNormal( v12 - v11,xf1); sideNormal.Normalize(); Vector3 frontNormal = CrossS(ref sideNormal, 1.0f); v11 = Vector3.Transform(v11,xf1); v12 = Vector3.Transform(v12,xf1); float frontOffset = Vector3.Dot(frontNormal, v11); float sideOffset1 = -Vector3.Dot(sideNormal, v11); float sideOffset2 = Vector3.Dot(sideNormal, v12); // Clip incident edge against extruded edge1 side edges. ClipVertex[] clipPoints1 = new ClipVertex[2]; clipPoints1[0].v = Vector3.Zero; clipPoints1[1].v = Vector3.Zero; ClipVertex[] clipPoints2 = new ClipVertex[2]; clipPoints2[0].v = Vector3.Zero; clipPoints2[1].v = Vector3.Zero; int np; // Clip to box side 1 np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1); if (np < 2) { return; } // Clip to negative box side 1 np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2); if (np < 2) { return; } // Now clipPoints2 contains the clipped points. Vector3 manifoldNormal = flip ? -frontNormal : frontNormal; int pointCount = 0; for (int i = 0; i < b2_maxManifoldPoints; ++i) { float separation = Vector3.Dot(frontNormal, clipPoints2[i].v) - frontOffset; if (separation <= 0.0f) { //b2ManifoldPoint* cp = manifold.points + pointCount; //float separation = separation; //cp.localPoint1 = b2MulT(xfA, clipPoints2[i].v); //cp.localPoint2 = b2MulT(xfB, clipPoints2[i].v); manifold.AddContactPoint(-manifoldNormal,clipPoints2[i].v,separation); // cp.id = clipPoints2[i].id; // cp.id.features.flip = flip; ++pointCount; } } // manifold.pointCount = pointCount;} }
public virtual void CollideSingleContact(ref Quaternion perturbeRot, CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut) { CollisionObject convexObj = m_isSwapped? body1 : body0; CollisionObject planeObj = m_isSwapped? body0: body1; ConvexShape convexShape = (ConvexShape) convexObj.GetCollisionShape(); StaticPlaneShape planeShape = (StaticPlaneShape) planeObj.GetCollisionShape(); bool hasCollision = false; Vector3 planeNormal = planeShape.GetPlaneNormal(); float planeConstant = planeShape.GetPlaneConstant(); Matrix convexWorldTransform = convexObj.GetWorldTransform(); Matrix convexInPlaneTrans = Matrix.Identity; convexInPlaneTrans = MathUtil.BulletMatrixMultiply(Matrix.Invert(planeObj.GetWorldTransform()), convexWorldTransform); //now perturbe the convex-world transform // MAN - CHECKTHIS Matrix rotMatrix = Matrix.CreateFromQuaternion(perturbeRot); convexWorldTransform = MathUtil.BulletMatrixMultiplyBasis(convexWorldTransform,rotMatrix); Matrix planeInConvex = Matrix.Identity; planeInConvex= MathUtil.BulletMatrixMultiply(Matrix.Invert(convexWorldTransform),planeObj.GetWorldTransform()); Vector3 tmp = Vector3.TransformNormal(-planeNormal,planeInConvex); Vector3 vtx = convexShape.LocalGetSupportingVertex(ref tmp); Vector3 vtxInPlane = Vector3.Transform(vtx,convexInPlaneTrans); float distance = (Vector3.Dot(planeNormal,vtxInPlane) - planeConstant); Vector3 vtxInPlaneProjected = vtxInPlane - (distance*planeNormal); Vector3 vtxInPlaneWorld = Vector3.Transform(vtxInPlaneProjected,planeObj.GetWorldTransform()); hasCollision = distance < m_manifoldPtr.GetContactBreakingThreshold(); resultOut.SetPersistentManifold(m_manifoldPtr); if (hasCollision) { /// report a contact. internally this will be kept persistent, and contact reduction is done Vector3 normalOnSurfaceB = Vector3.TransformNormal(planeNormal,planeObj.GetWorldTransform()); Vector3 pOnB = vtxInPlaneWorld; resultOut.AddContactPoint(ref normalOnSurfaceB,ref pOnB,distance); } }
public override float CalculateTimeOfImpact(CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut) { resultOut = null; CollisionObject colObj = m_isSwapped? body1 : body0; CollisionObject otherObj = m_isSwapped? body0 : body1; Debug.Assert(colObj.GetCollisionShape().IsCompound()); CompoundShape compoundShape = (CompoundShape)(colObj.GetCollisionShape()); //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals //given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means: //determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1 //then use each overlapping node AABB against Tree0 //and vise versa. float hitFraction = 1f; int numChildren = m_childCollisionAlgorithms.Count; for (int i=0;i<numChildren;i++) { //temporarily exchange parent btCollisionShape with childShape, and recurse CollisionShape childShape = compoundShape.GetChildShape(i); //backup Matrix orgTrans = colObj.GetWorldTransform(); Matrix childTrans = compoundShape.GetChildTransform(i); Matrix newChildWorldTrans = MathUtil.BulletMatrixMultiply(ref orgTrans,ref childTrans); colObj.SetWorldTransform(ref newChildWorldTrans); CollisionShape tmpShape = colObj.GetCollisionShape(); colObj.InternalSetTemporaryCollisionShape( childShape ); float frac = m_childCollisionAlgorithms[i].CalculateTimeOfImpact(colObj,otherObj,dispatchInfo, resultOut); if (frac<hitFraction) { hitFraction = frac; } //revert back colObj.InternalSetTemporaryCollisionShape( tmpShape); colObj.SetWorldTransform(ref orgTrans); } return hitFraction; }
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 float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { CollisionObject convexbody = m_isSwapped ? bodyB : bodyA; CollisionObject triBody = m_isSwapped ? bodyA : bodyB; //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 1; } //Matrix triInv = MathHelper.InvertMatrix(triBody.getWorldTransform()); Matrix triInv = Matrix.Invert(triBody.GetWorldTransform()); Matrix convexFromLocal = triInv * convexbody.GetWorldTransform(); Matrix convexToLocal = triInv * 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 = 1f; //is this available? LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(ref convexFromLocal, ref convexToLocal, convexbody.GetCcdSweptSphereRadius(), curHitFraction); raycastCallback.m_hitFraction = convexbody.GetHitFraction(); CollisionObject concavebody = triBody; ConcaveShape triangleMesh = concavebody.GetCollisionShape() as ConcaveShape; if (triangleMesh != null) { triangleMesh.ProcessAllTriangles(raycastCallback, ref rayAabbMin, ref rayAabbMax); } if (raycastCallback.m_hitFraction < convexbody.GetHitFraction()) { convexbody.SetHitFraction(raycastCallback.m_hitFraction); return raycastCallback.m_hitFraction; } } return 1; }
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 float CalculateTimeOfImpact(CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut) { resultOut = new ManifoldResult(); //not yet return 1f; }
//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(); } } }