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 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 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 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 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 float GetSpherePenetration(CollisionObject boxObj, ref Vector3 v3PointOnBox, ref Vector3 v3PointOnSphere, ref Vector3 v3SphereCenter, float fRadius, ref Vector3 aabbMin, ref Vector3 aabbMax) { Vector3[] bounds = new Vector3[2]; bounds[0] = aabbMin; bounds[1] = aabbMax; Vector3 p0, tmp, prel, normal; Vector3[] n = new Vector3[6]; float fSep = -10000000.0f; float fSepThis; // set p0 and normal to a default value to shup up GCC p0 = Vector3.Zero; normal = Vector3.Zero; n[0] = new Vector3(-1,0,0); n[1] = new Vector3(0,-1,0); n[2] = new Vector3(0,0,-1); n[3] = new Vector3(1,0,0); n[4] = new Vector3(0,1,0); n[5] = new Vector3(0,0,1); Matrix m44T = boxObj.GetWorldTransform(); // convert point in local space prel = MathUtil.InverseTransform(ref m44T,ref v3SphereCenter); /////////// for (int i=0;i<6;i++) { int j = i<3 ? 0:1; if ( (fSepThis = (Vector3.Dot((prel-bounds[j]),n[i]))-fRadius) > 0f) { return 1f; } if ( fSepThis > fSep ) { p0 = bounds[j]; normal = n[i]; fSep = fSepThis; } } v3PointOnBox = prel - normal*(Vector3.Dot(normal,(prel-p0))); v3PointOnSphere = v3PointOnBox + normal*fSep; // transform back in world space tmp = Vector3.Transform(v3PointOnBox,m44T ); v3PointOnBox = tmp; tmp = Vector3.Transform(v3PointOnSphere,m44T); v3PointOnSphere = tmp; normal = (v3PointOnBox-v3PointOnSphere); normal.Normalize(); return fSep; }
public float GetSphereDistance(CollisionObject boxObj,ref Vector3 v3PointOnBox, ref Vector3 v3PointOnSphere, ref Vector3 v3SphereCenter, float fRadius ) { float margins; Vector3[] bounds = new Vector3[2]; BoxShape boxShape= (BoxShape)boxObj.GetCollisionShape(); bounds[0] = -boxShape.GetHalfExtentsWithoutMargin(); bounds[1] = boxShape.GetHalfExtentsWithoutMargin(); margins = boxShape.Margin;//also add sphereShape margin? Matrix m44T = boxObj.GetWorldTransform(); Vector3[] boundsVec = new Vector3[2]; float fPenetration; boundsVec[0] = bounds[0]; boundsVec[1] = bounds[1]; Vector3 marginsVec = new Vector3( margins, margins, margins ); // add margins bounds[0] += marginsVec; bounds[1] -= marginsVec; ///////////////////////////////////////////////// Vector3 tmp, prel, normal, v3P; Vector3[] n = new Vector3[6]; float fSep = 10000000.0f; float fSepThis; n[0] = new Vector3(-1,0,0); n[1] = new Vector3(0,-1,0); n[2] = new Vector3(0,0,-1); n[3] = new Vector3(1,0,0); n[4] = new Vector3(0,1,0); n[5] = new Vector3(0,0,1); // convert point in local space prel = MathUtil.InverseTransform(ref m44T,ref v3SphereCenter); bool bFound = false; v3P = prel; for (int i=0;i<6;i++) { int j = i<3? 0:1; fSepThis = Vector3.Dot((v3P-bounds[j]),n[i]); if ( fSepThis != 0f) { v3P = v3P - n[i]*fSepThis; bFound = true; } } // if ( bFound ) { bounds[0] = boundsVec[0]; bounds[1] = boundsVec[1]; normal = (prel - v3P); normal.Normalize(); v3PointOnBox = v3P + normal*margins; v3PointOnSphere = prel - normal*fRadius; if ( (Vector3.Dot((v3PointOnSphere - v3PointOnBox),normal)) > 0f ) { return 1f; } // transform back in world space tmp = Vector3.Transform(v3PointOnBox,m44T); v3PointOnBox = tmp; tmp = Vector3.Transform(v3PointOnSphere,m44T); v3PointOnSphere = tmp; float fSeps2 = (v3PointOnBox-v3PointOnSphere).LengthSquared(); //if this fails, fallback into deeper penetration case, below if (fSeps2 > MathUtil.SIMD_EPSILON) { fSep = (float)-System.Math.Sqrt(fSeps2); normal = (v3PointOnBox-v3PointOnSphere); normal *= 1f/fSep; } return fSep; } ////////////////////////////////////////////////// // Deep penetration case fPenetration = GetSpherePenetration(boxObj, ref v3PointOnBox, ref v3PointOnSphere, ref v3SphereCenter, fRadius, ref bounds[0], ref bounds[1]); bounds[0] = boundsVec[0]; bounds[1] = boundsVec[1]; if ( fPenetration <= 0f) { return (fPenetration-margins); } else { return 1f; } }
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 void UpdateSingleAabb(CollisionObject colObj) { Vector3 minAabb = new Vector3(); Vector3 maxAabb = new Vector3(); Matrix wt = colObj.GetWorldTransform(); colObj.GetCollisionShape().GetAabb(ref wt, ref minAabb,ref maxAabb); //need to increase the aabb for contact thresholds Vector3 contactThreshold = new Vector3(BulletGlobals.gContactBreakingThreshold, BulletGlobals.gContactBreakingThreshold, BulletGlobals.gContactBreakingThreshold); minAabb -= contactThreshold; maxAabb += contactThreshold; if (BulletGlobals.g_streamWriter != null && debugCollisionWorld) { MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "updateSingleAabbMin", minAabb); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "updateSingleAabbMax", maxAabb); } IBroadphaseInterface bp = (IBroadphaseInterface)m_broadphasePairCache; //moving objects should be moderately sized, probably something wrong if not if ( colObj.IsStaticObject() || ((maxAabb-minAabb).LengthSquared() < 1e12f)) { bp.SetAabb(colObj.GetBroadphaseHandle(),ref minAabb,ref maxAabb, m_dispatcher1); } else { //something went wrong, investigate //this assert is unwanted in 3D modelers (danger of loosing work) colObj.SetActivationState(ActivationState.DISABLE_SIMULATION); //static bool reportMe = true; bool reportMe = true; if (reportMe && m_debugDrawer != null) { reportMe = false; m_debugDrawer.ReportErrorWarning("Overflow in AABB, object removed from simulation"); m_debugDrawer.ReportErrorWarning("If you can reproduce this, please email [email protected]\n"); m_debugDrawer.ReportErrorWarning("Please include above information, your Platform, version of OS.\n"); m_debugDrawer.ReportErrorWarning("Thanks.\n"); } } }
public virtual void AddCollisionObject(CollisionObject collisionObject, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask) { //check that the object isn't already added //btAssert( m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size()); Debug.Assert(collisionObject != null); m_collisionObjects.Add(collisionObject); //calculate new AABB Matrix trans = collisionObject.GetWorldTransform(); Vector3 minAabb = new Vector3(); Vector3 maxAabb= new Vector3(); collisionObject.GetCollisionShape().GetAabb(ref trans,ref minAabb,ref maxAabb); BroadphaseNativeTypes type = collisionObject.GetCollisionShape().ShapeType; collisionObject.SetBroadphaseHandle( GetBroadphase().CreateProxy( ref minAabb, ref maxAabb, type, collisionObject, collisionFilterGroup, collisionFilterMask, m_dispatcher1,0 )) ; }
protected void SetupContactConstraint(ref SolverConstraint solverConstraint, CollisionObject colObj0, CollisionObject colObj1, ManifoldPoint cp, ContactSolverInfo infoGlobal, ref Vector3 vel, ref float rel_vel, ref float relaxation, ref Vector3 rel_pos1, ref Vector3 rel_pos2) { RigidBody rb0 = RigidBody.Upcast(colObj0); RigidBody rb1 = RigidBody.Upcast(colObj1); Vector3 pos1 = cp.GetPositionWorldOnA(); Vector3 pos2 = cp.GetPositionWorldOnB(); rel_pos1 = pos1 - colObj0.GetWorldTransform().Translation; rel_pos2 = pos2 - colObj1.GetWorldTransform().Translation; relaxation = 1f; Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.m_normalWorldOnB); solverConstraint.m_angularComponentA = rb0 != null ? Vector3.TransformNormal(torqueAxis0, rb0.GetInvInertiaTensorWorld()) * rb0.GetAngularFactor() : Vector3.Zero; Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.GetNormalWorldOnB()); solverConstraint.m_angularComponentB = rb1 != null ? Vector3.TransformNormal(-torqueAxis1, rb1.GetInvInertiaTensorWorld()) * rb1.GetAngularFactor() : Vector3.Zero; { #if COMPUTE_IMPULSE_DENOM float denom0 = rb0.computeImpulseDenominator(pos1,cp.m_normalWorldOnB); float denom1 = rb1.computeImpulseDenominator(pos2,cp.m_normalWorldOnB); #else Vector3 vec; float denom0 = 0f; float denom1 = 0f; if (rb0 != null) { vec = Vector3.Cross((solverConstraint.m_angularComponentA), rel_pos1); denom0 = rb0.GetInvMass() + Vector3.Dot(cp.GetNormalWorldOnB(), vec); } if (rb1 != null) { vec = Vector3.Cross((-solverConstraint.m_angularComponentB), rel_pos2); denom1 = rb1.GetInvMass() + Vector3.Dot(cp.GetNormalWorldOnB(), vec); } #endif //COMPUTE_IMPULSE_DENOM float denom = relaxation / (denom0 + denom1); MathUtil.SanityCheckFloat(denom); solverConstraint.m_jacDiagABInv = denom; } solverConstraint.m_contactNormal = cp.m_normalWorldOnB; solverConstraint.m_relpos1CrossNormal = Vector3.Cross(rel_pos1, cp.m_normalWorldOnB); solverConstraint.m_relpos2CrossNormal = Vector3.Cross(rel_pos2, -cp.m_normalWorldOnB); Vector3 vel1 = rb0 != null ? rb0.GetVelocityInLocalPoint(ref rel_pos1) : Vector3.Zero; Vector3 vel2 = rb1 != null ? rb1.GetVelocityInLocalPoint(ref rel_pos2) : Vector3.Zero; vel = vel1 - vel2; rel_vel = Vector3.Dot(cp.GetNormalWorldOnB(), vel); float penetration = cp.GetDistance() + infoGlobal.m_linearSlop; solverConstraint.m_friction = cp.GetCombinedFriction(); float restitution = 0f; if (cp.GetLifeTime() > infoGlobal.m_restingContactRestitutionThreshold) { restitution = 0f; } else { restitution = RestitutionCurve(rel_vel, cp.GetCombinedResitution()); if (restitution <= 0f) { restitution = 0f; } } ///warm starting (or zero if disabled) if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_WARMSTARTING)) { solverConstraint.m_appliedImpulse = cp.GetAppliedImpulse() * infoGlobal.m_warmstartingFactor; if (rb0 != null) { Vector3 contactNormalTemp = solverConstraint.m_contactNormal; Vector3.Multiply(ref contactNormalTemp, rb0.GetInvMass(), out contactNormalTemp); rb0.InternalApplyImpulse(ref contactNormalTemp, ref solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse); } if (rb1 != null) { Vector3 contactNormalTemp = solverConstraint.m_contactNormal; Vector3.Multiply(ref contactNormalTemp, rb1.GetInvMass(), out contactNormalTemp); Vector3 negAngular = -solverConstraint.m_angularComponentB; rb1.InternalApplyImpulse(ref contactNormalTemp, ref negAngular, -solverConstraint.m_appliedImpulse); } } else { solverConstraint.m_appliedImpulse = 0f; } solverConstraint.m_appliedPushImpulse = 0f; { float rel_vel2 = 0f; float vel1Dotn = Vector3.Dot(solverConstraint.m_contactNormal, (rb0 != null ? rb0.GetLinearVelocity() : Vector3.Zero)) + Vector3.Dot(solverConstraint.m_relpos1CrossNormal, (rb0 != null ? rb0.GetAngularVelocity() : Vector3.Zero)); float vel2Dotn = -Vector3.Dot(solverConstraint.m_contactNormal, (rb1 != null ? rb1.GetLinearVelocity() : Vector3.Zero)) + Vector3.Dot(solverConstraint.m_relpos2CrossNormal, (rb1 != null ? rb1.GetAngularVelocity() : Vector3.Zero)); rel_vel2 = vel1Dotn + vel2Dotn; float positionalError = 0f; positionalError = -penetration * infoGlobal.m_erp / infoGlobal.m_timeStep; float velocityError = restitution - rel_vel2;// * damping; float penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; float velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) { //combine position and velocity into rhs solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; solverConstraint.m_rhsPenetration = 0f; } else { //split position and velocity into rhs and m_rhsPenetration solverConstraint.m_rhs = velocityImpulse; solverConstraint.m_rhsPenetration = penetrationImpulse; } solverConstraint.m_cfm = 0f; solverConstraint.m_lowerLimit = 0; solverConstraint.m_upperLimit = 1e10f; } }
private static void ApplyAnisotropicFriction(CollisionObject colObj, ref Vector3 frictionDirection) { if (colObj != null && colObj.HasAnisotropicFriction()) { // transform to local coordinates Vector3 loc_lateral = MathUtil.TransposeTransformNormal(frictionDirection, colObj.GetWorldTransform()); Vector3 friction_scaling = colObj.GetAnisotropicFriction(); //apply anisotropic friction loc_lateral *= friction_scaling; // ... and transform it back to global coordinates frictionDirection = Vector3.TransformNormal(loc_lateral, colObj.GetWorldTransform()); } }