private void PreallocateChildAlgorithms(CollisionObject body0, CollisionObject body1) { CollisionObject colObj = m_isSwapped ? body1 : body0; CollisionObject otherObj = m_isSwapped ? body0 : body1; System.Diagnostics.Debug.Assert(colObj.GetCollisionShape().IsCompound()); CompoundShape compoundShape = (CompoundShape)(colObj.GetCollisionShape()); int numChildren = compoundShape.GetNumChildShapes(); int i; //m_childCollisionAlgorithms.resize(numChildren); m_childCollisionAlgorithms.Clear(); for (i = 0; i < numChildren; i++) { if (compoundShape.GetDynamicAabbTree() != null) { m_childCollisionAlgorithms.Add(null); } else { CollisionShape tmpShape = colObj.GetCollisionShape(); CollisionShape childShape = compoundShape.GetChildShape(i); colObj.InternalSetTemporaryCollisionShape(childShape); m_childCollisionAlgorithms.Add(m_dispatcher.FindAlgorithm(colObj, otherObj, m_sharedManifold)); colObj.InternalSetTemporaryCollisionShape(tmpShape); } } }
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 ConvexTriangleCallback(IDispatcher dispatcher, CollisionObject body0, CollisionObject body1, bool isSwapped) { m_dispatcher = dispatcher; m_convexBody = isSwapped ? body1 : body0; m_triBody = isSwapped ? body0 : body1; m_manifoldPtr = m_dispatcher.GetNewManifold(m_convexBody, m_triBody); ClearCache(); }
public BridgeTriangleConvexcastCallback(ConvexShape castShape, ref Matrix from,ref Matrix to, ConvexResultCallback resultCallback, CollisionObject collisionObject,TriangleMeshShape triangleMesh, ref Matrix triangleToWorld): base(castShape, ref from,ref to, ref triangleToWorld, triangleMesh.Margin) { m_resultCallback = resultCallback; m_collisionObject = collisionObject; m_triangleMesh = triangleMesh; }
public BoxBoxCollisionAlgorithm(PersistentManifold mf,CollisionAlgorithmConstructionInfo ci,CollisionObject body0,CollisionObject body1) : base(ci) { if (m_manifoldPtr == null && m_dispatcher.NeedsCollision(body0,body1)) { m_manifoldPtr = m_dispatcher.GetNewManifold(body0,body1); m_ownManifold = true; } }
public ClosestNotMeConvexResultCallback(CollisionObject me, ref Vector3 fromA, ref Vector3 toA, IOverlappingPairCache pairCache, IDispatcher dispatcher) : base(ref fromA, ref toA) { m_allowedPenetration = 0.0f; m_me = me; m_pairCache = pairCache; m_dispatcher = dispatcher; }
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 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 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 LocalRayResult(CollisionObject collisionObject, LocalShapeInfo localShapeInfo, ref Vector3 hitNormalLocal, float hitFraction) { m_collisionObject = collisionObject; m_localShapeInfo = localShapeInfo; m_hitNormalLocal = hitNormalLocal; m_hitFraction = hitFraction; }
public BridgeTriangleConcaveRaycastCallback(ref Vector3 from, ref Vector3 to, RayResultCallback resultCallback, CollisionObject collisionObject, ConcaveShape triangleMesh, ref Matrix colObjWorldTransform) : //@BP Mod base(ref from, ref to, resultCallback.m_flags) { m_resultCallback = resultCallback; m_collisionObject = collisionObject; m_triangleMesh = triangleMesh; m_colObjWorldTransform = colObjWorldTransform; }
public Convex2dConvex2dAlgorithm(PersistentManifold mf,CollisionAlgorithmConstructionInfo ci,CollisionObject body0,CollisionObject body1, ISimplexSolverInterface simplexSolver, IConvexPenetrationDepthSolver pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold) : base(ci,body0,body1) { m_simplexSolver = simplexSolver; m_pdSolver = pdSolver; m_ownManifold = false; m_manifoldPtr = mf; m_lowLevelOfDetail = false; m_numPerturbationIterations = numPerturbationIterations; m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold; }
public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0,CollisionObject body1) { if (!m_swapped) { return new ConvexPlaneCollisionAlgorithm(null,ci,body0,body1,false,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); } else { return new ConvexPlaneCollisionAlgorithm(null,ci,body0,body1,true,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); } }
public ManifoldResult(CollisionObject body0, CollisionObject body1) { if (body0 == null || body1 == null) { int ibreak = 0; } m_body0 = body0; m_body1 = body1; m_rootTransA = body0.GetWorldTransform(); m_rootTransB = body1.GetWorldTransform(); }
public SphereBoxCollisionAlgorithm(PersistentManifold mf,CollisionAlgorithmConstructionInfo ci,CollisionObject col0,CollisionObject col1, bool isSwapped) : base(ci,col0,col1) { CollisionObject sphereObj = m_isSwapped ? col1 : col0; CollisionObject boxObj = m_isSwapped ? col0 : col1; if (m_manifoldPtr == null && m_dispatcher.NeedsCollision(sphereObj, boxObj)) { m_manifoldPtr = m_dispatcher.GetNewManifold(sphereObj, boxObj); m_ownManifold = true; } }
public CompoundCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, bool isSwapped) : base(ci, body0, body1) { m_isSwapped = isSwapped; m_sharedManifold = ci.GetManifold(); m_ownsManifold = false; CollisionObject colObj = m_isSwapped ? body1 : body0; System.Diagnostics.Debug.Assert(colObj.GetCollisionShape().IsCompound()); CompoundShape compoundShape = (CompoundShape)(colObj.GetCollisionShape()); m_compoundShapeRevision = compoundShape.GetUpdateRevision(); PreallocateChildAlgorithms(body0, body1); }
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 LocalConvexResult(CollisionObject hitCollisionObject, LocalShapeInfo localShapeInfo, ref Vector3 hitNormalLocal, ref Vector3 hitPointLocal, float hitFraction ) { m_hitCollisionObject = hitCollisionObject; m_localShapeInfo = localShapeInfo; m_hitNormalLocal = hitNormalLocal; m_hitPointLocal = hitPointLocal; m_hitFraction = hitFraction; }
public ConvexConvexAlgorithm(PersistentManifold mf,CollisionAlgorithmConstructionInfo ci,CollisionObject body0,CollisionObject body1, ISimplexSolverInterface simplexSolver, IConvexPenetrationDepthSolver pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold) : base(ci,body0,body1) { m_simplexSolver = simplexSolver; m_pdSolver = pdSolver; m_ownManifold = false; m_manifoldPtr = mf; m_lowLevelOfDetail = false; #if USE_SEPDISTANCE_UTIL2 m_sepDistance ((static_cast<btConvexShape*>(body0.getCollisionShape())).getAngularMotionDisc(), (static_cast<btConvexShape*>(body1.getCollisionShape())).getAngularMotionDisc()), #endif m_numPerturbationIterations = numPerturbationIterations; m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold; }
public ConvexPlaneCollisionAlgorithm(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject col0,CollisionObject col1,bool isSwapped,int numPerturbationIterations,int minimumPointsPerturbationThreshold) : base(ci) { m_manifoldPtr = mf; m_ownManifold = false; m_isSwapped = isSwapped; m_numPerturbationIterations = numPerturbationIterations; m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold; CollisionObject convexObj = m_isSwapped? col1 : col0; CollisionObject planeObj = m_isSwapped? col0 : col1; if (m_manifoldPtr == null && m_dispatcher.NeedsCollision(convexObj,planeObj)) { m_manifoldPtr = m_dispatcher.GetNewManifold(convexObj,planeObj); m_ownManifold = true; } }
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(); } }
public override float CalculateTimeOfImpact(CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut) { resultOut = new ManifoldResult(); //not yet return 1f; }
public SphereTriangleCollisionAlgorithm(PersistentManifold mf,CollisionAlgorithmConstructionInfo ci,CollisionObject body0,CollisionObject body1,bool swapped) : base(ci,body0,body1) { m_ownManifold = false; m_manifoldPtr = mf; m_swapped = swapped; }
public ClosestNotMeConvexResultCallback(CollisionObject me, Vector3 fromA, Vector3 toA, IOverlappingPairCache pairCache, IDispatcher dispatcher) : this(me, ref fromA, ref toA, pairCache, dispatcher) { }
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; }
public BridgedManifoldResult(CollisionObject obj0,CollisionObject obj1,ContactResultCallback resultCallback) :base(obj0,obj1) { m_resultCallback = resultCallback; }
public abstract float AddSingleResult(ManifoldPoint cp, CollisionObject colObj0,int partId0,int index0,CollisionObject colObj1,int partId1,int index1);
public SphereSphereCollisionAlgorithm(PersistentManifold mf,CollisionAlgorithmConstructionInfo ci,CollisionObject body0,CollisionObject body1) : base(ci,body0,body1) { }