protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) { float maxImpulse = 0; Vector3 color = new Vector3(0, 1, 0); if (cp.Distance <= 0) { if (iter == 0) { if (debugDraw != null) { debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color); } } ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; float impulse = cpd.ContactSolverFunc( bodyA, bodyB, cp, info); if (maxImpulse < impulse) { maxImpulse = impulse; } } return(maxImpulse); }
public override void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep) { //apply gravity, predict motion PredictUnconstraintMotion(timeStep); DispatcherInfo dispatchInfo = new DispatcherInfo(); dispatchInfo.TimeStep = timeStep; dispatchInfo.StepCount = 0; dispatchInfo.DebugDraw = DebugDrawer; //perform collision detection PerformDiscreteCollisionDetection(); //solve contact constraints int numManifolds = Dispatcher.ManifoldCount; if (numManifolds != 0) { List <PersistentManifold> manifolds = (Dispatcher as CollisionDispatcher).Manifolds; //int numManifolds = m_dispatcher1.GetNumManifolds(); ContactSolverInfo infoGlobal = new ContactSolverInfo(); infoGlobal.TimeStep = timeStep; _constraintSolver.SolveGroup(new List <CollisionObject>(), manifolds, manifolds.Count, new List <TypedConstraint>(), infoGlobal, _debugDrawer); } //integrate transforms IntegrateTransforms(timeStep); UpdateAabbs(); SynchronizeMotionStates(); }
public static float ResolveSingleFrictionEmpty( RigidBody bodyA, RigidBody bodyB, ManifoldPoint contactPoint, ContactSolverInfo solverInfo) { return(0); }
//velocity + friction //response between two dynamic objects with friction public virtual float ResolveSingleCollisionCombinedCacheFriendly( SolverBody bodyA, SolverBody bodyB, SolverConstraint contactConstraint, ContactSolverInfo solverInfo) { float normalImpulse = 0; if (contactConstraint.Penetration < 0) { return(0); } float relVel; float velADotn = Vector3.Dot(contactConstraint.ContactNormal, bodyA.LinearVelocity) + Vector3.Dot(contactConstraint.RelPosACrossNormal, bodyA.AngularVelocity); float velBDotn = Vector3.Dot(contactConstraint.ContactNormal, bodyB.LinearVelocity) + Vector3.Dot(contactConstraint.RelPosBCrossNormal, bodyB.AngularVelocity); relVel = velADotn - velBDotn; float positionalError = contactConstraint.Penetration; float velocityError = contactConstraint.Restitution - relVel; // * damping; float penetrationImpulse = positionalError * contactConstraint.JacDiagABInv; float velocityImpulse = velocityError * contactConstraint.JacDiagABInv; normalImpulse = penetrationImpulse + velocityImpulse; // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse float oldNormalImpulse = contactConstraint.AppliedImpulse; float sum = oldNormalImpulse + normalImpulse; contactConstraint.AppliedImpulse = 0 > sum ? 0 : sum; float oldVelocityImpulse = contactConstraint.AppliedVelocityImpulse; float velocitySum = oldVelocityImpulse + velocityImpulse; contactConstraint.AppliedVelocityImpulse = 0 > velocitySum ? 0 : velocitySum; normalImpulse = contactConstraint.AppliedImpulse - oldNormalImpulse; if (bodyA.InvMass != 0) { bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, contactConstraint.AngularComponentA, normalImpulse); } if (bodyB.InvMass != 0) { bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, contactConstraint.AngularComponentB, -normalImpulse); } return(normalImpulse); }
public InplaceSolverIslandCallback( ContactSolverInfo solverInfo, IConstraintSolver solver, List <TypedConstraint> sortedConstraints, IDebugDraw debugDrawer) { _solverInfo = solverInfo; _solver = solver; _sortedConstraints = sortedConstraints; _debugDrawer = debugDrawer; }
public virtual float ResolveSingleFrictionCacheFriendly( SolverBody bodyA, SolverBody bodyB, SolverConstraint contactConstraint, ContactSolverInfo solverInfo, float appliedNormalImpulse) { float combinedFriction = contactConstraint.Friction; float limit = appliedNormalImpulse * combinedFriction; if (appliedNormalImpulse > 0) //friction { float j1; { float relVel; float velADotn = Vector3.Dot(contactConstraint.ContactNormal, bodyA.LinearVelocity) + Vector3.Dot(contactConstraint.RelPosACrossNormal, bodyA.AngularVelocity); float velBDotn = Vector3.Dot(contactConstraint.ContactNormal, bodyB.LinearVelocity) + Vector3.Dot(contactConstraint.RelPosBCrossNormal, bodyB.AngularVelocity); relVel = velADotn - velBDotn; // calculate j that moves us to zero relative velocity j1 = -relVel * contactConstraint.JacDiagABInv; float oldTangentImpulse = contactConstraint.AppliedImpulse; contactConstraint.AppliedImpulse = oldTangentImpulse + j1; float test = contactConstraint.AppliedImpulse; MathHelper.SetMin(ref test, limit); MathHelper.SetMax(ref test, -limit); contactConstraint.AppliedImpulse = test; j1 = contactConstraint.AppliedImpulse - oldTangentImpulse; } if (bodyA.InvMass != 0) { bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, contactConstraint.AngularComponentA, j1); } if (bodyB.InvMass != 0) { bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, contactConstraint.AngularComponentB, -j1); } } return(0); }
//protected void SolveNonContactConstraints(ContactSolverInfo solverInfo) //{ // //constraint preparation: building jacobians // for (int i = 0; i < _constraints.Count; i++) // { // TypedConstraint constraint = _constraints[i]; // constraint.BuildJacobian(); // } // //solve the regular non-contact constraints (point 2 point, hinge, generic d6) // for (int g = 0; g < solverInfo.IterationsCount; g++) // { // for (int i = 0; i < _constraints.Count; i++) // { // TypedConstraint constraint = _constraints[i]; // constraint.SolveConstraint(solverInfo.TimeStep); // } // } //} //protected void SolveContactConstraints(ContactSolverInfo solverInfo) //{ // InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, _debugDrawer); // // solve all the contact points and contact friction // _islandManager.BuildAndProcessIslands(Dispatcher, CollisionObjects, solverCallback); //} protected void SolveConstraints(ContactSolverInfo solverInfo) { //sorted version of all btTypedConstraint, based on islandId List <TypedConstraint> sortedConstraints = new List <TypedConstraint>(ConstraintsCount); for (int i = 0; i < ConstraintsCount; i++) { sortedConstraints.Add(_constraints[i]); } sortedConstraints.Sort(new Comparison <TypedConstraint>(TypedConstraint.SortConstraintOnIslandPredicate)); List <TypedConstraint> constraintsPtr = ConstraintsCount != 0 ? sortedConstraints : new List <TypedConstraint>(); InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, constraintsPtr, _debugDrawer); // solve all the constraints for this island _islandManager.BuildAndProcessIslands(CollisionWorld.Dispatcher, CollisionWorld.CollisionObjects, solverCallback); }
//velocity + friction //response between two dynamic objects with friction public virtual float ResolveSingleCollisionCombinedCacheFriendly( SolverBody bodyA, SolverBody bodyB, SolverConstraint contactConstraint, ContactSolverInfo solverInfo) { float normalImpulse = 0; if (contactConstraint.Penetration < 0) return 0; float relVel; float velADotn = Vector3.Dot(contactConstraint.ContactNormal,bodyA.LinearVelocity) + Vector3.Dot(contactConstraint.RelPosACrossNormal,bodyA.AngularVelocity); float velBDotn = Vector3.Dot(contactConstraint.ContactNormal,bodyB.LinearVelocity) + Vector3.Dot(contactConstraint.RelPosBCrossNormal,bodyB.AngularVelocity); relVel = velADotn - velBDotn; float positionalError = contactConstraint.Penetration; float velocityError = contactConstraint.Restitution - relVel;// * damping; float penetrationImpulse = positionalError * contactConstraint.JacDiagABInv; float velocityImpulse = velocityError * contactConstraint.JacDiagABInv; normalImpulse = penetrationImpulse + velocityImpulse; // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse float oldNormalImpulse = contactConstraint.AppliedImpulse; float sum = oldNormalImpulse + normalImpulse; contactConstraint.AppliedImpulse = 0 > sum ? 0 : sum; float oldVelocityImpulse = contactConstraint.AppliedVelocityImpulse; float velocitySum = oldVelocityImpulse + velocityImpulse; contactConstraint.AppliedVelocityImpulse = 0 > velocitySum ? 0 : velocitySum; normalImpulse = contactConstraint.AppliedImpulse - oldNormalImpulse; if (bodyA.InvMass != 0) { bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, contactConstraint.AngularComponentA, normalImpulse); } if (bodyB.InvMass != 0) { bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, contactConstraint.AngularComponentB, -normalImpulse); } return normalImpulse; }
public virtual float ResolveSingleFrictionCacheFriendly( SolverBody bodyA, SolverBody bodyB, SolverConstraint contactConstraint, ContactSolverInfo solverInfo, float appliedNormalImpulse) { float combinedFriction = contactConstraint.Friction; float limit = appliedNormalImpulse * combinedFriction; if (appliedNormalImpulse > 0) //friction { float j1; { float relVel; float velADotn = Vector3.Dot(contactConstraint.ContactNormal, bodyA.LinearVelocity) + Vector3.Dot(contactConstraint.RelPosACrossNormal, bodyA.AngularVelocity); float velBDotn = Vector3.Dot(contactConstraint.ContactNormal, bodyB.LinearVelocity) + Vector3.Dot(contactConstraint.RelPosBCrossNormal, bodyB.AngularVelocity); relVel = velADotn - velBDotn; // calculate j that moves us to zero relative velocity j1 = -relVel * contactConstraint.JacDiagABInv; float oldTangentImpulse = contactConstraint.AppliedImpulse; contactConstraint.AppliedImpulse = oldTangentImpulse + j1; float test = contactConstraint.AppliedImpulse; MathHelper.SetMin(ref test, limit); MathHelper.SetMax(ref test, -limit); contactConstraint.AppliedImpulse = test; j1 = contactConstraint.AppliedImpulse - oldTangentImpulse; } if (bodyA.InvMass != 0) { bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, contactConstraint.AngularComponentA, j1); } if (bodyB.InvMass != 0) { bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, contactConstraint.AngularComponentB, -j1); } } return 0; }
protected float SolveFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) { Vector3 color = new Vector3(0, 1, 0); if (cp.Distance <= 0) { ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; cpd.FrictionSolverFunc( bodyA, bodyB, cp, info); } return 0; }
protected void PrepareConstraints(PersistentManifold manifold, ContactSolverInfo info) { RigidBody body0 = manifold.BodyA as RigidBody; RigidBody body1 = manifold.BodyB as RigidBody; //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop { manifold.RefreshContactPoints(body0.CenterOfMassTransform, body1.CenterOfMassTransform); int numpoints = manifold.ContactsCount; _totalContactPoints += numpoints; Vector3 color = new Vector3(0, 1, 0); for (int i = 0; i < numpoints; i++) { ManifoldPoint cp = manifold.GetContactPoint(i); if (cp.Distance <= 0) { Vector3 pos1 = cp.PositionWorldOnA; Vector3 pos2 = cp.PositionWorldOnB; Vector3 rel_pos1 = pos1 - body0.CenterOfMassPosition; Vector3 rel_pos2 = pos2 - body1.CenterOfMassPosition; //this jacobian entry is re-used for all iterations JacobianEntry jac = new JacobianEntry(MatrixOperations.Transpose(body0.CenterOfMassTransform), MatrixOperations.Transpose(body1.CenterOfMassTransform), rel_pos1, rel_pos2, cp.NormalWorldOnB, body0.InvInertiaDiagLocal, body0.InverseMass, body1.InvInertiaDiagLocal, body1.InverseMass); float jacDiagAB = jac.Diagonal; ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; if (cpd != null) { //might be invalid cpd.PersistentLifeTime++; if (cpd.PersistentLifeTime != cp.LifeTime) { //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime()); cpd = new ConstraintPersistentData(); cpd.PersistentLifeTime = cp.LifeTime; } } else { cpd = new ConstraintPersistentData(); _totalCpd++; //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd); cp.UserPersistentData = cpd; cpd.PersistentLifeTime = cp.LifeTime; //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime()); } if (cpd == null) throw new BulletException(); cpd.JacDiagABInv = 1f / jacDiagAB; //Dependent on Rigidbody A and B types, fetch the contact/friction response func //perhaps do a similar thing for friction/restutution combiner funcs... cpd.FrictionSolverFunc = _frictionDispatch[(int)body0.FrictionSolverType, (int)body1.FrictionSolverType]; cpd.ContactSolverFunc = _contactDispatch[(int)body0.ContactSolverType, (int)body1.ContactSolverType]; Vector3 vel1 = body0.GetVelocityInLocalPoint(rel_pos1); Vector3 vel2 = body1.GetVelocityInLocalPoint(rel_pos2); Vector3 vel = vel1 - vel2; float rel_vel; rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel); float combinedRestitution = cp.CombinedRestitution; cpd.Penetration = cp.Distance; cpd.Friction = cp.CombinedFriction; cpd.Restitution = RestitutionCurve(rel_vel, combinedRestitution); if (cpd.Restitution < 0f) { cpd.Restitution = 0.0f; }; //restitution and penetration work in same direction so //rel_vel float penVel = -cpd.Penetration / info.TimeStep; if (cpd.Restitution > penVel) { cpd.Penetration = 0; } float relaxation = info.Damping; if ((_solverMode & SolverMode.UseWarmstarting) != 0) { cpd.AppliedImpulse *= relaxation; } else { cpd.AppliedImpulse = 0f; } //for friction cpd.PreviousAppliedImpulse = cpd.AppliedImpulse; //re-calculate friction direction every frame, todo: check if this is really needed Vector3 fwta = cpd.FrictionWorldTangentialA; Vector3 fwtb = cpd.FrictionWorldTangentialB; MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref fwta, ref fwtb); cpd.FrictionWorldTangentialA = fwta; cpd.FrictionWorldTangentialB = fwtb; cpd.AccumulatedTangentImpulseA = 0; cpd.AccumulatedTangentImpulseB = 0; float denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialA); float denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialA); float denom = relaxation / (denom0 + denom1); cpd.JacDiagABInvTangentA = denom; denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialB); denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialB); denom = relaxation / (denom0 + denom1); cpd.JacDiagABInvTangentB = denom; Vector3 totalImpulse = cp.NormalWorldOnB * cpd.AppliedImpulse; { Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); cpd.AngularComponentA = Vector3.TransformNormal(torqueAxis0, body0.InvInertiaTensorWorld); Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); cpd.AngularComponentB = Vector3.TransformNormal(torqueAxis1, body1.InvInertiaTensorWorld); } { Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialA); cpd.FrictionAngularComponent0A = Vector3.TransformNormal(ftorqueAxis0, body0.InvInertiaTensorWorld); } { Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialB); cpd.FrictionAngularComponent1A = Vector3.TransformNormal(ftorqueAxis1, body0.InvInertiaTensorWorld); } { Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialA); cpd.FrictionAngularComponent0B = Vector3.TransformNormal(ftorqueAxis0, body1.InvInertiaTensorWorld); } { Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialB); cpd.FrictionAngularComponent1B = Vector3.TransformNormal(ftorqueAxis1, body1.InvInertiaTensorWorld); } //apply previous frames impulse on both bodies body0.ApplyImpulse(totalImpulse, rel_pos1); body1.ApplyImpulse(-totalImpulse, rel_pos2); } } } }
protected float SolveFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) { Vector3 color = new Vector3(0, 1, 0); if (cp.Distance <= 0) { ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; cpd.FrictionSolverFunc( bodyA, bodyB, cp, info); } return(0); }
protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter) { return(Solve(bodyA, bodyB, cp, info, iter, null)); }
public static float ResolveSingleFrictionOriginal( RigidBody bodyA, RigidBody bodyB, ManifoldPoint contactPoint, ContactSolverInfo solverInfo) { Vector3 posA = contactPoint.PositionWorldOnA; Vector3 posB = contactPoint.PositionWorldOnB; Vector3 relPosA = posA - bodyA.CenterOfMassPosition; Vector3 relPosB = posB - bodyB.CenterOfMassPosition; ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; if (cpd == null) { throw new BulletException(); } float combinedFriction = cpd.Friction; float limit = cpd.AppliedImpulse * combinedFriction; //if (contactPoint.m_appliedImpulse>0.f) //friction { //apply friction in the 2 tangential directions { // 1st tangent Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); Vector3 vel = velA - velB; float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel); // calculate j that moves us to zero relative velocity float j = -vrel * cpd.JacDiagABInvTangentA; float total = cpd.AccumulatedTangentImpulseA + j; if (limit < total) { total = limit; } if (total < -limit) { total = -limit; } j = total - cpd.AccumulatedTangentImpulseA; cpd.AccumulatedTangentImpulseA = total; bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialA, relPosA); bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialA, relPosB); } { // 2nd tangent Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); Vector3 vel = velA - velB; float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel); // calculate j that moves us to zero relative velocity float j = -vrel * cpd.JacDiagABInvTangentB; float total = cpd.AccumulatedTangentImpulseB + j; if (limit < total) { total = limit; } if (total < -limit) { total = -limit; } j = total - cpd.AccumulatedTangentImpulseB; cpd.AccumulatedTangentImpulseB = total; bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialB, relPosA); bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialB, relPosB); } } return(cpd.AppliedImpulse); }
protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) { float maxImpulse = 0; Vector3 color = new Vector3(0, 1, 0); if (cp.Distance <= 0) { if (iter == 0) if(debugDraw != null) debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color); ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; float impulse = cpd.ContactSolverFunc( bodyA, bodyB, cp, info); if (maxImpulse < impulse) maxImpulse = impulse; } return maxImpulse; }
public static float ResolveSingleFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint contactPoint, ContactSolverInfo solverInfo) { Vector3 pos1 = contactPoint.PositionWorldOnA; Vector3 pos2 = contactPoint.PositionWorldOnB; Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition; Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition; ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; if (cpd == null) throw new BulletException(); float combinedFriction = cpd.Friction; float limit = cpd.AppliedImpulse * combinedFriction; //friction if (cpd.AppliedImpulse > 0) { //apply friction in the 2 tangential directions // 1st tangent Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1); Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2); Vector3 vel = vel1 - vel2; float j1, j2; { float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel); // calculate j that moves us to zero relative velocity j1 = -vrel * cpd.JacDiagABInvTangentA; float oldTangentImpulse = cpd.AccumulatedTangentImpulseA; cpd.AccumulatedTangentImpulseA = oldTangentImpulse + j1; float atia = cpd.AccumulatedTangentImpulseA; MathHelper.SetMin(ref atia, limit); MathHelper.SetMax(ref atia, -limit); cpd.AccumulatedTangentImpulseA = atia; j1 = cpd.AccumulatedTangentImpulseA - oldTangentImpulse; } { // 2nd tangent float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel); // calculate j that moves us to zero relative velocity j2 = -vrel * cpd.JacDiagABInvTangentB; float oldTangentImpulse = cpd.AccumulatedTangentImpulseB; cpd.AccumulatedTangentImpulseB = oldTangentImpulse + j2; float atib = cpd.AccumulatedTangentImpulseB; MathHelper.SetMin(ref atib, limit); MathHelper.SetMax(ref atib, -limit); cpd.AccumulatedTangentImpulseB = atib; j2 = cpd.AccumulatedTangentImpulseB - oldTangentImpulse; } if (bodyA.InverseMass != 0) { bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyA.InverseMass, cpd.FrictionAngularComponent0A, j1); bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyA.InverseMass, cpd.FrictionAngularComponent1A, j2); } if (bodyB.InverseMass != 0) { bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyB.InverseMass, cpd.FrictionAngularComponent0B, -j1); bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyB.InverseMass, cpd.FrictionAngularComponent1B, -j2); } } return cpd.AppliedImpulse; }
/// <summary> /// contact constraint resolution: /// calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint /// positive distance = separation, negative distance = penetration /// </summary> /// <param name="body1"></param> /// <param name="body2"></param> /// <param name="contactPoint"></param> /// <param name="info"></param> /// <returns></returns> public static float ResolveSingleCollision(RigidBody bodyA, RigidBody bodyB, ManifoldPoint contactPoint, ContactSolverInfo solverInfo) { Vector3 pos1 = contactPoint.PositionWorldOnA; Vector3 pos2 = contactPoint.PositionWorldOnB; // printf("distance=%f\n",distance); Vector3 normal = contactPoint.NormalWorldOnB; Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition; Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition; Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1); Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2); Vector3 vel = vel1 - vel2; float rel_vel; rel_vel = Vector3.Dot(normal, vel); float Kfps = 1f / solverInfo.TimeStep; //float damping = solverInfo.m_damping; float Kerp = solverInfo.Erp; float Kcor = Kerp * Kfps; //printf("dist=%f\n",distance); ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; if (cpd == null) { throw new BulletException(); } float distance = cpd.Penetration; //contactPoint.getDistance(); //distance = 0.f; float positionalError = Kcor * -distance; //jacDiagABInv; float velocityError = cpd.Restitution - rel_vel; // * damping; float penetrationImpulse = positionalError * cpd.JacDiagABInv; float velocityImpulse = velocityError * cpd.JacDiagABInv; float normalImpulse = penetrationImpulse + velocityImpulse; // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse float oldNormalImpulse = cpd.AppliedImpulse; float sum = oldNormalImpulse + normalImpulse; cpd.AppliedImpulse = 0f > sum ? 0f : sum; normalImpulse = cpd.AppliedImpulse - oldNormalImpulse; if (bodyA.InverseMass != 0) { bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse); } if (bodyB.InverseMass != 0) { bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse); } /*body1.applyImpulse(normal * (normalImpulse), rel_pos1); * body2.applyImpulse(-normal * (normalImpulse), rel_pos2);*/ return(normalImpulse); }
/// <summary> /// contact constraint resolution: /// calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint /// positive distance = separation, negative distance = penetration /// </summary> /// <param name="body1"></param> /// <param name="body2"></param> /// <param name="contactPoint"></param> /// <param name="info"></param> /// <returns></returns> public static float ResolveSingleCollision(RigidBody bodyA, RigidBody bodyB, ManifoldPoint contactPoint, ContactSolverInfo solverInfo) { Vector3 pos1 = contactPoint.PositionWorldOnA; Vector3 pos2 = contactPoint.PositionWorldOnB; // printf("distance=%f\n",distance); Vector3 normal = contactPoint.NormalWorldOnB; Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition; Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition; Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1); Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2); Vector3 vel = vel1 - vel2; float rel_vel; rel_vel = Vector3.Dot(normal, vel); float Kfps = 1f / solverInfo.TimeStep; //float damping = solverInfo.m_damping; float Kerp = solverInfo.Erp; float Kcor = Kerp * Kfps; //printf("dist=%f\n",distance); ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; if (cpd == null) throw new BulletException(); float distance = cpd.Penetration;//contactPoint.getDistance(); //distance = 0.f; float positionalError = Kcor * -distance; //jacDiagABInv; float velocityError = cpd.Restitution - rel_vel;// * damping; float penetrationImpulse = positionalError * cpd.JacDiagABInv; float velocityImpulse = velocityError * cpd.JacDiagABInv; float normalImpulse = penetrationImpulse + velocityImpulse; // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse float oldNormalImpulse = cpd.AppliedImpulse; float sum = oldNormalImpulse + normalImpulse; cpd.AppliedImpulse = 0f > sum ? 0f : sum; normalImpulse = cpd.AppliedImpulse - oldNormalImpulse; if (bodyA.InverseMass != 0) { bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse); } if (bodyB.InverseMass != 0) { bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse); } /*body1.applyImpulse(normal * (normalImpulse), rel_pos1); body2.applyImpulse(-normal * (normalImpulse), rel_pos2);*/ return normalImpulse; }
public virtual float SolveGroup(List <CollisionObject> bodies, List <PersistentManifold> manifolds, int numManifolds, List <TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer) { if ((_solverMode & SolverMode.CacheFriendly) != SolverMode.None) { return(SolveGroupCacheFriendly(bodies, manifolds, numManifolds, constraints, infoGlobal, debugDrawer)); } ContactSolverInfo info = infoGlobal; int totalPoints = 0; int numiter = infoGlobal.IterationsCount; for (int j = 0; j < manifolds.Count; j++) { PersistentManifold manifold = manifolds[j]; PrepareConstraints(manifold, info); for (int p = 0; p < manifolds[j].ContactsCount; p++) { _order[totalPoints].ManifoldIndex = j; _order[totalPoints].PointIndex = p; totalPoints++; } } for (int j = 0; j < constraints.Count; j++) { constraints[j].BuildJacobian(); } //should traverse the contacts random order... int iteration; for (iteration = 0; iteration < numiter; iteration++) { int j; if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None) { if ((iteration & 7) == 0) { for (j = 0; j < totalPoints; ++j) { OrderIndex tmp = _order[j]; int swapi = RandInt2(j + 1); _order[j] = _order[swapi]; _order[swapi] = tmp; } } } for (j = 0; j < constraints.Count; j++) { constraints[j].SolveConstraint(info.TimeStep); } for (j = 0; j < totalPoints; j++) { PersistentManifold manifold = manifolds[_order[j].ManifoldIndex]; Solve((RigidBody)manifold.BodyA, (RigidBody)manifold.BodyB, manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer); } for (j = 0; j < totalPoints; j++) { PersistentManifold manifold = manifolds[_order[j].ManifoldIndex]; SolveFriction((RigidBody)manifold.BodyA, (RigidBody)manifold.BodyB, manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer); } } return(0); }
public virtual float SolveGroupCacheFriendly(List <CollisionObject> bodies, List <PersistentManifold> manifolds, int numManifolds, List <TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer) { if (constraints.Count + numManifolds == 0) { return(0); } for (int i = 0; i < numManifolds; i++) { PersistentManifold manifold = manifolds[i]; RigidBody rbA = (RigidBody)manifold.BodyA; RigidBody rbB = (RigidBody)manifold.BodyB; manifold.RefreshContactPoints(rbA.CenterOfMassTransform, rbB.CenterOfMassTransform); } int minReservation = manifolds.Count * 2; _tmpSolverBodyPool = new List <SolverBody>(minReservation); for (int i = 0; i < bodies.Count; i++) { RigidBody rb = RigidBody.Upcast(bodies[i]); if (rb != null && rb.IslandTag >= 0) { BulletDebug.Assert(rb.CompanionID < 0); int solverBodyId = _tmpSolverBodyPool.Count; SolverBody solverBody; InitSolverBody(out solverBody, rb); _tmpSolverBodyPool.Add(solverBody); rb.CompanionID = solverBodyId; } } _tmpSolverConstraintPool = new List <SolverConstraint>(minReservation); _tmpSolverFrictionConstraintPool = new List <SolverConstraint>(minReservation); for (int i = 0; i < numManifolds; i++) { PersistentManifold manifold = manifolds[i]; RigidBody rb0 = (RigidBody)manifold.BodyA; RigidBody rb1 = (RigidBody)manifold.BodyB; int solverBodyIdA = -1; int solverBodyIdB = -1; //if (i == 89) // System.Diagnostics.Debugger.Break(); if (manifold.ContactsCount != 0) { if (rb0.IslandTag >= 0) { solverBodyIdA = rb0.CompanionID; } else { //create a static body solverBodyIdA = _tmpSolverBodyPool.Count; SolverBody solverBody; InitSolverBody(out solverBody, rb0); _tmpSolverBodyPool.Add(solverBody); } if (rb1.IslandTag >= 0) { solverBodyIdB = rb1.CompanionID; } else { //create a static body solverBodyIdB = _tmpSolverBodyPool.Count; SolverBody solverBody; InitSolverBody(out solverBody, rb1); _tmpSolverBodyPool.Add(solverBody); } } if (solverBodyIdB == -1 || solverBodyIdA == -1) { System.Diagnostics.Debug.WriteLine(string.Format("We're in ass ! {0}", i)); } for (int j = 0; j < manifold.ContactsCount; j++) { ManifoldPoint cp = manifold.GetContactPoint(j); int frictionIndex = _tmpSolverConstraintPool.Count; if (cp.Distance <= 0) { Vector3 pos1 = cp.PositionWorldOnA; Vector3 pos2 = cp.PositionWorldOnB; Vector3 rel_pos1 = pos1 - rb0.CenterOfMassPosition; Vector3 rel_pos2 = pos2 - rb1.CenterOfMassPosition; float relaxation = 1; { SolverConstraint solverConstraint = new SolverConstraint(); _tmpSolverConstraintPool.Add(solverConstraint); solverConstraint.SolverBodyIdA = solverBodyIdA; solverConstraint.SolverBodyIdB = solverBodyIdB; solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Contact; //can be optimized, the cross products are already calculated float denom0 = rb0.ComputeImpulseDenominator(pos1, cp.NormalWorldOnB); float denom1 = rb1.ComputeImpulseDenominator(pos2, cp.NormalWorldOnB); float denom = relaxation / (denom0 + denom1); solverConstraint.JacDiagABInv = denom; solverConstraint.ContactNormal = cp.NormalWorldOnB; solverConstraint.RelPosACrossNormal = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); solverConstraint.RelPosBCrossNormal = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); Vector3 vel1 = rb0.GetVelocityInLocalPoint(rel_pos1); Vector3 vel2 = rb1.GetVelocityInLocalPoint(rel_pos2); Vector3 vel = vel1 - vel2; float rel_vel; rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel); solverConstraint.Penetration = cp.Distance; //btScalar(infoGlobal.m_numIterations); solverConstraint.Friction = cp.CombinedFriction; float rest = RestitutionCurve(rel_vel, cp.CombinedRestitution); if (rest <= 0) { rest = 0; } float penVel = -solverConstraint.Penetration / infoGlobal.TimeStep; if (rest > penVel) { rest = 0; } solverConstraint.Restitution = rest; solverConstraint.Penetration *= -(infoGlobal.Erp / infoGlobal.TimeStep); solverConstraint.AppliedImpulse = 0f; solverConstraint.AppliedVelocityImpulse = 0f; #warning Check to see if we need Vector3.Transform Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); solverConstraint.AngularComponentA = Vector3.TransformNormal(torqueAxis0, rb0.InvInertiaTensorWorld); Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); solverConstraint.AngularComponentB = Vector3.TransformNormal(torqueAxis1, rb1.InvInertiaTensorWorld); } //create 2 '1d axis' constraints for 2 tangential friction directions //re-calculate friction direction every frame, todo: check if this is really needed Vector3 frictionTangential0a = new Vector3(), frictionTangential1b = new Vector3(); MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref frictionTangential0a, ref frictionTangential1b); { SolverConstraint solverConstraint = new SolverConstraint(); _tmpSolverFrictionConstraintPool.Add(solverConstraint); solverConstraint.ContactNormal = frictionTangential0a; solverConstraint.SolverBodyIdA = solverBodyIdA; solverConstraint.SolverBodyIdB = solverBodyIdB; solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction; solverConstraint.FrictionIndex = frictionIndex; solverConstraint.Friction = cp.CombinedFriction; solverConstraint.AppliedImpulse = 0; solverConstraint.AppliedVelocityImpulse = 0; float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal); float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal); float denom = relaxation / (denom0 + denom1); solverConstraint.JacDiagABInv = denom; { Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal); solverConstraint.RelPosACrossNormal = ftorqueAxis0; solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis0, rb0.InvInertiaTensorWorld); } { Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal); solverConstraint.RelPosBCrossNormal = ftorqueAxis0; solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis0, rb1.InvInertiaTensorWorld); } } { SolverConstraint solverConstraint = new SolverConstraint(); _tmpSolverFrictionConstraintPool.Add(solverConstraint); solverConstraint.ContactNormal = frictionTangential1b; solverConstraint.SolverBodyIdA = solverBodyIdA; solverConstraint.SolverBodyIdB = solverBodyIdB; solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction; solverConstraint.FrictionIndex = frictionIndex; solverConstraint.Friction = cp.CombinedFriction; solverConstraint.AppliedImpulse = 0; solverConstraint.AppliedVelocityImpulse = 0; float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal); float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal); float denom = relaxation / (denom0 + denom1); solverConstraint.JacDiagABInv = denom; { Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal); solverConstraint.RelPosACrossNormal = ftorqueAxis1; solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis1, rb0.InvInertiaTensorWorld); } { Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal); solverConstraint.RelPosBCrossNormal = ftorqueAxis1; solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis1, rb1.InvInertiaTensorWorld); } } } } } ContactSolverInfo info = infoGlobal; { for (int j = 0; j < constraints.Count; j++) { TypedConstraint constraint = constraints[j]; constraint.BuildJacobian(); } } int numConstraintPool = _tmpSolverConstraintPool.Count; int numFrictionPool = _tmpSolverFrictionConstraintPool.Count; //todo: use stack allocator for such temporarily memory, same for solver bodies/constraints List <int> gOrderTmpConstraintPool = new List <int>(numConstraintPool); List <int> gOrderFrictionConstraintPool = new List <int>(numFrictionPool); { for (int i = 0; i < numConstraintPool; i++) { gOrderTmpConstraintPool.Add(i); } for (int i = 0; i < numFrictionPool; i++) { gOrderFrictionConstraintPool.Add(i); } } //should traverse the contacts random order... int iteration; { for (iteration = 0; iteration < info.IterationsCount; iteration++) { int j; if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None) { if ((iteration & 7) == 0) { for (j = 0; j < numConstraintPool; ++j) { int tmp = gOrderTmpConstraintPool[j]; int swapi = RandInt2(j + 1); gOrderTmpConstraintPool[j] = gOrderTmpConstraintPool[swapi]; gOrderTmpConstraintPool[swapi] = tmp; } for (j = 0; j < numFrictionPool; ++j) { int tmp = gOrderFrictionConstraintPool[j]; int swapi = RandInt2(j + 1); gOrderFrictionConstraintPool[j] = gOrderFrictionConstraintPool[swapi]; gOrderFrictionConstraintPool[swapi] = tmp; } } } for (j = 0; j < constraints.Count; j++) { TypedConstraint constraint = constraints[j]; //todo: use solver bodies, so we don't need to copy from/to btRigidBody if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0)) { _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].WriteBackVelocity(); } if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0)) { _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].WriteBackVelocity(); } constraint.SolveConstraint(info.TimeStep); if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0)) { _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].ReadVelocity(); } if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0)) { _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].ReadVelocity(); } } { int numPoolConstraints = _tmpSolverConstraintPool.Count; for (j = 0; j < numPoolConstraints; j++) { SolverConstraint solveManifold = _tmpSolverConstraintPool[gOrderTmpConstraintPool[j]]; ResolveSingleCollisionCombinedCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA], _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info); } } { int numFrictionPoolConstraints = _tmpSolverFrictionConstraintPool.Count; for (j = 0; j < numFrictionPoolConstraints; j++) { SolverConstraint solveManifold = _tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]]; float appliedNormalImpulse = _tmpSolverConstraintPool[solveManifold.FrictionIndex].AppliedImpulse; ResolveSingleFrictionCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA], _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info, appliedNormalImpulse); } } } } for (int i = 0; i < _tmpSolverBodyPool.Count; i++) { _tmpSolverBodyPool[i].WriteBackVelocity(); } _tmpSolverBodyPool.Clear(); _tmpSolverConstraintPool.Clear(); _tmpSolverFrictionConstraintPool.Clear(); return(0); }
public virtual float SolveGroupCacheFriendly(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer) { if (constraints.Count + numManifolds == 0) { return 0; } for (int i = 0; i < numManifolds; i++) { PersistentManifold manifold = manifolds[i]; RigidBody rbA = (RigidBody)manifold.BodyA; RigidBody rbB = (RigidBody)manifold.BodyB; manifold.RefreshContactPoints(rbA.CenterOfMassTransform, rbB.CenterOfMassTransform); } int minReservation = manifolds.Count * 2; _tmpSolverBodyPool = new List<SolverBody>(minReservation); for (int i = 0; i < bodies.Count; i++) { RigidBody rb = RigidBody.Upcast(bodies[i]); if (rb != null && rb.IslandTag >= 0) { BulletDebug.Assert(rb.CompanionID < 0); int solverBodyId = _tmpSolverBodyPool.Count; SolverBody solverBody; InitSolverBody(out solverBody, rb); _tmpSolverBodyPool.Add(solverBody); rb.CompanionID = solverBodyId; } } _tmpSolverConstraintPool = new List<SolverConstraint>(minReservation); _tmpSolverFrictionConstraintPool = new List<SolverConstraint>(minReservation); for (int i = 0; i < numManifolds; i++) { PersistentManifold manifold = manifolds[i]; RigidBody rb0 = (RigidBody)manifold.BodyA; RigidBody rb1 = (RigidBody)manifold.BodyB; int solverBodyIdA = -1; int solverBodyIdB = -1; //if (i == 89) // System.Diagnostics.Debugger.Break(); if (manifold.ContactsCount != 0) { if (rb0.IslandTag >= 0) { solverBodyIdA = rb0.CompanionID; } else { //create a static body solverBodyIdA = _tmpSolverBodyPool.Count; SolverBody solverBody; InitSolverBody(out solverBody, rb0); _tmpSolverBodyPool.Add(solverBody); } if (rb1.IslandTag >= 0) { solverBodyIdB = rb1.CompanionID; } else { //create a static body solverBodyIdB = _tmpSolverBodyPool.Count; SolverBody solverBody; InitSolverBody(out solverBody, rb1); _tmpSolverBodyPool.Add(solverBody); } } if (solverBodyIdB == -1 || solverBodyIdA == -1) System.Diagnostics.Debug.WriteLine(string.Format("We're in ass ! {0}", i)); for (int j = 0; j < manifold.ContactsCount; j++) { ManifoldPoint cp = manifold.GetContactPoint(j); int frictionIndex = _tmpSolverConstraintPool.Count; if (cp.Distance <= 0) { Vector3 pos1 = cp.PositionWorldOnA; Vector3 pos2 = cp.PositionWorldOnB; Vector3 rel_pos1 = pos1 - rb0.CenterOfMassPosition; Vector3 rel_pos2 = pos2 - rb1.CenterOfMassPosition; float relaxation = 1; { SolverConstraint solverConstraint = new SolverConstraint(); _tmpSolverConstraintPool.Add(solverConstraint); solverConstraint.SolverBodyIdA = solverBodyIdA; solverConstraint.SolverBodyIdB = solverBodyIdB; solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Contact; //can be optimized, the cross products are already calculated float denom0 = rb0.ComputeImpulseDenominator(pos1, cp.NormalWorldOnB); float denom1 = rb1.ComputeImpulseDenominator(pos2, cp.NormalWorldOnB); float denom = relaxation / (denom0 + denom1); solverConstraint.JacDiagABInv = denom; solverConstraint.ContactNormal = cp.NormalWorldOnB; solverConstraint.RelPosACrossNormal = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); solverConstraint.RelPosBCrossNormal = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); Vector3 vel1 = rb0.GetVelocityInLocalPoint(rel_pos1); Vector3 vel2 = rb1.GetVelocityInLocalPoint(rel_pos2); Vector3 vel = vel1 - vel2; float rel_vel; rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel); solverConstraint.Penetration = cp.Distance;//btScalar(infoGlobal.m_numIterations); solverConstraint.Friction = cp.CombinedFriction; float rest = RestitutionCurve(rel_vel, cp.CombinedRestitution); if (rest <= 0) { rest = 0; } float penVel = -solverConstraint.Penetration / infoGlobal.TimeStep; if (rest > penVel) { rest = 0; } solverConstraint.Restitution = rest; solverConstraint.Penetration *= -(infoGlobal.Erp / infoGlobal.TimeStep); solverConstraint.AppliedImpulse = 0f; solverConstraint.AppliedVelocityImpulse = 0f; #warning Check to see if we need Vector3.Transform Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); solverConstraint.AngularComponentA = Vector3.TransformNormal(torqueAxis0, rb0.InvInertiaTensorWorld); Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); solverConstraint.AngularComponentB = Vector3.TransformNormal(torqueAxis1, rb1.InvInertiaTensorWorld); } //create 2 '1d axis' constraints for 2 tangential friction directions //re-calculate friction direction every frame, todo: check if this is really needed Vector3 frictionTangential0a = new Vector3(), frictionTangential1b = new Vector3(); MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref frictionTangential0a, ref frictionTangential1b); { SolverConstraint solverConstraint = new SolverConstraint(); _tmpSolverFrictionConstraintPool.Add(solverConstraint); solverConstraint.ContactNormal = frictionTangential0a; solverConstraint.SolverBodyIdA = solverBodyIdA; solverConstraint.SolverBodyIdB = solverBodyIdB; solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction; solverConstraint.FrictionIndex = frictionIndex; solverConstraint.Friction = cp.CombinedFriction; solverConstraint.AppliedImpulse = 0; solverConstraint.AppliedVelocityImpulse = 0; float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal); float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal); float denom = relaxation / (denom0 + denom1); solverConstraint.JacDiagABInv = denom; { Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal); solverConstraint.RelPosACrossNormal = ftorqueAxis0; solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis0, rb0.InvInertiaTensorWorld); } { Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal); solverConstraint.RelPosBCrossNormal = ftorqueAxis0; solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis0, rb1.InvInertiaTensorWorld); } } { SolverConstraint solverConstraint = new SolverConstraint(); _tmpSolverFrictionConstraintPool.Add(solverConstraint); solverConstraint.ContactNormal = frictionTangential1b; solverConstraint.SolverBodyIdA = solverBodyIdA; solverConstraint.SolverBodyIdB = solverBodyIdB; solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction; solverConstraint.FrictionIndex = frictionIndex; solverConstraint.Friction = cp.CombinedFriction; solverConstraint.AppliedImpulse = 0; solverConstraint.AppliedVelocityImpulse = 0; float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal); float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal); float denom = relaxation / (denom0 + denom1); solverConstraint.JacDiagABInv = denom; { Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal); solverConstraint.RelPosACrossNormal = ftorqueAxis1; solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis1, rb0.InvInertiaTensorWorld); } { Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal); solverConstraint.RelPosBCrossNormal = ftorqueAxis1; solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis1, rb1.InvInertiaTensorWorld); } } } } } ContactSolverInfo info = infoGlobal; { for (int j = 0; j < constraints.Count; j++) { TypedConstraint constraint = constraints[j]; constraint.BuildJacobian(); } } int numConstraintPool = _tmpSolverConstraintPool.Count; int numFrictionPool = _tmpSolverFrictionConstraintPool.Count; //todo: use stack allocator for such temporarily memory, same for solver bodies/constraints List<int> gOrderTmpConstraintPool = new List<int>(numConstraintPool); List<int> gOrderFrictionConstraintPool = new List<int>(numFrictionPool); { for (int i = 0; i < numConstraintPool; i++) { gOrderTmpConstraintPool.Add(i); } for (int i = 0; i < numFrictionPool; i++) { gOrderFrictionConstraintPool.Add(i); } } //should traverse the contacts random order... int iteration; { for (iteration = 0; iteration < info.IterationsCount; iteration++) { int j; if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None) { if ((iteration & 7) == 0) { for (j = 0; j < numConstraintPool; ++j) { int tmp = gOrderTmpConstraintPool[j]; int swapi = RandInt2(j + 1); gOrderTmpConstraintPool[j] = gOrderTmpConstraintPool[swapi]; gOrderTmpConstraintPool[swapi] = tmp; } for (j = 0; j < numFrictionPool; ++j) { int tmp = gOrderFrictionConstraintPool[j]; int swapi = RandInt2(j + 1); gOrderFrictionConstraintPool[j] = gOrderFrictionConstraintPool[swapi]; gOrderFrictionConstraintPool[swapi] = tmp; } } } for (j = 0; j < constraints.Count; j++) { TypedConstraint constraint = constraints[j]; //todo: use solver bodies, so we don't need to copy from/to btRigidBody if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0)) { _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].WriteBackVelocity(); } if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0)) { _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].WriteBackVelocity(); } constraint.SolveConstraint(info.TimeStep); if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0)) { _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].ReadVelocity(); } if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0)) { _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].ReadVelocity(); } } { int numPoolConstraints = _tmpSolverConstraintPool.Count; for (j = 0; j < numPoolConstraints; j++) { SolverConstraint solveManifold = _tmpSolverConstraintPool[gOrderTmpConstraintPool[j]]; ResolveSingleCollisionCombinedCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA], _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info); } } { int numFrictionPoolConstraints = _tmpSolverFrictionConstraintPool.Count; for (j = 0; j < numFrictionPoolConstraints; j++) { SolverConstraint solveManifold = _tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]]; float appliedNormalImpulse = _tmpSolverConstraintPool[solveManifold.FrictionIndex].AppliedImpulse; ResolveSingleFrictionCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA], _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info, appliedNormalImpulse); } } } } for (int i = 0; i < _tmpSolverBodyPool.Count; i++) { _tmpSolverBodyPool[i].WriteBackVelocity(); } _tmpSolverBodyPool.Clear(); _tmpSolverConstraintPool.Clear(); _tmpSolverFrictionConstraintPool.Clear(); return 0; }
protected float SolveCombinedContactFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) { float maxImpulse = 0; Vector3 color = new Vector3(0, 1, 0); if (cp.Distance <= 0) { if (iter == 0) { if (debugDraw != null) { debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color); } } float impulse = ContactConstraint.ResolveSingleCollisionCombined( bodyA, bodyB, cp, info); if (maxImpulse < impulse) { maxImpulse = impulse; } } return(maxImpulse); }
public virtual float SolveGroup(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer) { if ((_solverMode & SolverMode.CacheFriendly) != SolverMode.None) { return SolveGroupCacheFriendly(bodies, manifolds, numManifolds, constraints, infoGlobal, debugDrawer); } ContactSolverInfo info = infoGlobal; int totalPoints = 0; int numiter = infoGlobal.IterationsCount; for (int j = 0; j < manifolds.Count; j++) { PersistentManifold manifold = manifolds[j]; PrepareConstraints(manifold, info); for (int p = 0; p < manifolds[j].ContactsCount; p++) { _order[totalPoints].ManifoldIndex = j; _order[totalPoints].PointIndex = p; totalPoints++; } } for (int j = 0; j < constraints.Count; j++) { constraints[j].BuildJacobian(); } //should traverse the contacts random order... int iteration; for (iteration = 0; iteration < numiter; iteration++) { int j; if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None) { if ((iteration & 7) == 0) { for (j = 0; j < totalPoints; ++j) { OrderIndex tmp = _order[j]; int swapi = RandInt2(j + 1); _order[j] = _order[swapi]; _order[swapi] = tmp; } } } for (j = 0; j < constraints.Count; j++) { constraints[j].SolveConstraint(info.TimeStep); } for (j = 0; j < totalPoints; j++) { PersistentManifold manifold = manifolds[_order[j].ManifoldIndex]; Solve((RigidBody)manifold.BodyA, (RigidBody)manifold.BodyB, manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer); } for (j = 0; j < totalPoints; j++) { PersistentManifold manifold = manifolds[_order[j].ManifoldIndex]; SolveFriction((RigidBody)manifold.BodyA, (RigidBody)manifold.BodyB, manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer); } } return 0; }
//protected void SolveNonContactConstraints(ContactSolverInfo solverInfo) //{ // //constraint preparation: building jacobians // for (int i = 0; i < _constraints.Count; i++) // { // TypedConstraint constraint = _constraints[i]; // constraint.BuildJacobian(); // } // //solve the regular non-contact constraints (point 2 point, hinge, generic d6) // for (int g = 0; g < solverInfo.IterationsCount; g++) // { // for (int i = 0; i < _constraints.Count; i++) // { // TypedConstraint constraint = _constraints[i]; // constraint.SolveConstraint(solverInfo.TimeStep); // } // } //} //protected void SolveContactConstraints(ContactSolverInfo solverInfo) //{ // InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, _debugDrawer); // // solve all the contact points and contact friction // _islandManager.BuildAndProcessIslands(Dispatcher, CollisionObjects, solverCallback); //} protected void SolveConstraints(ContactSolverInfo solverInfo) { //sorted version of all btTypedConstraint, based on islandId List<TypedConstraint> sortedConstraints = new List<TypedConstraint>(ConstraintsCount); for (int i = 0; i < ConstraintsCount; i++) { sortedConstraints.Add(_constraints[i]); } sortedConstraints.Sort(new Comparison<TypedConstraint>(TypedConstraint.SortConstraintOnIslandPredicate)); List<TypedConstraint> constraintsPtr = ConstraintsCount != 0 ? sortedConstraints : new List<TypedConstraint>(); InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, constraintsPtr, _debugDrawer); // solve all the constraints for this island _islandManager.BuildAndProcessIslands(CollisionWorld.Dispatcher, CollisionWorld.CollisionObjects, solverCallback); }
public static float ResolveSingleFrictionOriginal( RigidBody bodyA, RigidBody bodyB, ManifoldPoint contactPoint, ContactSolverInfo solverInfo) { Vector3 posA = contactPoint.PositionWorldOnA; Vector3 posB = contactPoint.PositionWorldOnB; Vector3 relPosA = posA - bodyA.CenterOfMassPosition; Vector3 relPosB = posB - bodyB.CenterOfMassPosition; ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; if (cpd == null) throw new BulletException(); float combinedFriction = cpd.Friction; float limit = cpd.AppliedImpulse * combinedFriction; //if (contactPoint.m_appliedImpulse>0.f) //friction { //apply friction in the 2 tangential directions { // 1st tangent Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); Vector3 vel = velA - velB; float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel); // calculate j that moves us to zero relative velocity float j = -vrel * cpd.JacDiagABInvTangentA; float total = cpd.AccumulatedTangentImpulseA + j; if (limit < total) total = limit; if (total < -limit) total = -limit; j = total - cpd.AccumulatedTangentImpulseA; cpd.AccumulatedTangentImpulseA = total; bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialA, relPosA); bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialA, relPosB); } { // 2nd tangent Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); Vector3 vel = velA - velB; float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel); // calculate j that moves us to zero relative velocity float j = -vrel * cpd.JacDiagABInvTangentB; float total = cpd.AccumulatedTangentImpulseB + j; if (limit < total) total = limit; if (total < -limit) total = -limit; j = total - cpd.AccumulatedTangentImpulseB; cpd.AccumulatedTangentImpulseB = total; bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialB, relPosA); bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialB, relPosB); } } return cpd.AppliedImpulse; }
protected void PrepareConstraints(PersistentManifold manifold, ContactSolverInfo info) { RigidBody body0 = manifold.BodyA as RigidBody; RigidBody body1 = manifold.BodyB as RigidBody; //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop { manifold.RefreshContactPoints(body0.CenterOfMassTransform, body1.CenterOfMassTransform); int numpoints = manifold.ContactsCount; _totalContactPoints += numpoints; Vector3 color = new Vector3(0, 1, 0); for (int i = 0; i < numpoints; i++) { ManifoldPoint cp = manifold.GetContactPoint(i); if (cp.Distance <= 0) { Vector3 pos1 = cp.PositionWorldOnA; Vector3 pos2 = cp.PositionWorldOnB; Vector3 rel_pos1 = pos1 - body0.CenterOfMassPosition; Vector3 rel_pos2 = pos2 - body1.CenterOfMassPosition; //this jacobian entry is re-used for all iterations JacobianEntry jac = new JacobianEntry(MatrixOperations.Transpose(body0.CenterOfMassTransform), MatrixOperations.Transpose(body1.CenterOfMassTransform), rel_pos1, rel_pos2, cp.NormalWorldOnB, body0.InvInertiaDiagLocal, body0.InverseMass, body1.InvInertiaDiagLocal, body1.InverseMass); float jacDiagAB = jac.Diagonal; ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; if (cpd != null) { //might be invalid cpd.PersistentLifeTime++; if (cpd.PersistentLifeTime != cp.LifeTime) { //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime()); cpd = new ConstraintPersistentData(); cpd.PersistentLifeTime = cp.LifeTime; } } else { cpd = new ConstraintPersistentData(); _totalCpd++; //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd); cp.UserPersistentData = cpd; cpd.PersistentLifeTime = cp.LifeTime; //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime()); } if (cpd == null) { throw new BulletException(); } cpd.JacDiagABInv = 1f / jacDiagAB; //Dependent on Rigidbody A and B types, fetch the contact/friction response func //perhaps do a similar thing for friction/restutution combiner funcs... cpd.FrictionSolverFunc = _frictionDispatch[(int)body0.FrictionSolverType, (int)body1.FrictionSolverType]; cpd.ContactSolverFunc = _contactDispatch[(int)body0.ContactSolverType, (int)body1.ContactSolverType]; Vector3 vel1 = body0.GetVelocityInLocalPoint(rel_pos1); Vector3 vel2 = body1.GetVelocityInLocalPoint(rel_pos2); Vector3 vel = vel1 - vel2; float rel_vel; rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel); float combinedRestitution = cp.CombinedRestitution; cpd.Penetration = cp.Distance; cpd.Friction = cp.CombinedFriction; cpd.Restitution = RestitutionCurve(rel_vel, combinedRestitution); if (cpd.Restitution < 0f) { cpd.Restitution = 0.0f; } ; //restitution and penetration work in same direction so //rel_vel float penVel = -cpd.Penetration / info.TimeStep; if (cpd.Restitution > penVel) { cpd.Penetration = 0; } float relaxation = info.Damping; if ((_solverMode & SolverMode.UseWarmstarting) != 0) { cpd.AppliedImpulse *= relaxation; } else { cpd.AppliedImpulse = 0f; } //for friction cpd.PreviousAppliedImpulse = cpd.AppliedImpulse; //re-calculate friction direction every frame, todo: check if this is really needed Vector3 fwta = cpd.FrictionWorldTangentialA; Vector3 fwtb = cpd.FrictionWorldTangentialB; MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref fwta, ref fwtb); cpd.FrictionWorldTangentialA = fwta; cpd.FrictionWorldTangentialB = fwtb; cpd.AccumulatedTangentImpulseA = 0; cpd.AccumulatedTangentImpulseB = 0; float denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialA); float denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialA); float denom = relaxation / (denom0 + denom1); cpd.JacDiagABInvTangentA = denom; denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialB); denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialB); denom = relaxation / (denom0 + denom1); cpd.JacDiagABInvTangentB = denom; Vector3 totalImpulse = cp.NormalWorldOnB * cpd.AppliedImpulse; { Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); cpd.AngularComponentA = Vector3.TransformNormal(torqueAxis0, body0.InvInertiaTensorWorld); Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); cpd.AngularComponentB = Vector3.TransformNormal(torqueAxis1, body1.InvInertiaTensorWorld); } { Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialA); cpd.FrictionAngularComponent0A = Vector3.TransformNormal(ftorqueAxis0, body0.InvInertiaTensorWorld); } { Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialB); cpd.FrictionAngularComponent1A = Vector3.TransformNormal(ftorqueAxis1, body0.InvInertiaTensorWorld); } { Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialA); cpd.FrictionAngularComponent0B = Vector3.TransformNormal(ftorqueAxis0, body1.InvInertiaTensorWorld); } { Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialB); cpd.FrictionAngularComponent1B = Vector3.TransformNormal(ftorqueAxis1, body1.InvInertiaTensorWorld); } //apply previous frames impulse on both bodies body0.ApplyImpulse(totalImpulse, rel_pos1); body1.ApplyImpulse(-totalImpulse, rel_pos2); } } } }
public static float ResolveSingleFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint contactPoint, ContactSolverInfo solverInfo) { Vector3 pos1 = contactPoint.PositionWorldOnA; Vector3 pos2 = contactPoint.PositionWorldOnB; Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition; Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition; ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; if (cpd == null) { throw new BulletException(); } float combinedFriction = cpd.Friction; float limit = cpd.AppliedImpulse * combinedFriction; //friction if (cpd.AppliedImpulse > 0) { //apply friction in the 2 tangential directions // 1st tangent Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1); Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2); Vector3 vel = vel1 - vel2; float j1, j2; { float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel); // calculate j that moves us to zero relative velocity j1 = -vrel * cpd.JacDiagABInvTangentA; float oldTangentImpulse = cpd.AccumulatedTangentImpulseA; cpd.AccumulatedTangentImpulseA = oldTangentImpulse + j1; float atia = cpd.AccumulatedTangentImpulseA; MathHelper.SetMin(ref atia, limit); MathHelper.SetMax(ref atia, -limit); cpd.AccumulatedTangentImpulseA = atia; j1 = cpd.AccumulatedTangentImpulseA - oldTangentImpulse; } { // 2nd tangent float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel); // calculate j that moves us to zero relative velocity j2 = -vrel * cpd.JacDiagABInvTangentB; float oldTangentImpulse = cpd.AccumulatedTangentImpulseB; cpd.AccumulatedTangentImpulseB = oldTangentImpulse + j2; float atib = cpd.AccumulatedTangentImpulseB; MathHelper.SetMin(ref atib, limit); MathHelper.SetMax(ref atib, -limit); cpd.AccumulatedTangentImpulseB = atib; j2 = cpd.AccumulatedTangentImpulseB - oldTangentImpulse; } if (bodyA.InverseMass != 0) { bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyA.InverseMass, cpd.FrictionAngularComponent0A, j1); bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyA.InverseMass, cpd.FrictionAngularComponent1A, j2); } if (bodyB.InverseMass != 0) { bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyB.InverseMass, cpd.FrictionAngularComponent0B, -j1); bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyB.InverseMass, cpd.FrictionAngularComponent1B, -j2); } } return(cpd.AppliedImpulse); }
//velocity + friction //response between two dynamic objects with friction public static float ResolveSingleCollisionCombined( RigidBody bodyA, RigidBody bodyB, ManifoldPoint contactPoint, ContactSolverInfo solverInfo) { Vector3 posA = contactPoint.PositionWorldOnA; Vector3 posB = contactPoint.PositionWorldOnB; Vector3 normal = contactPoint.NormalWorldOnB; Vector3 relPosA = posA - bodyA.CenterOfMassPosition; Vector3 relPosB = posB - bodyB.CenterOfMassPosition; Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); Vector3 vel = velA - velB; float relVel; relVel = Vector3.Dot(normal, vel); float Kfps = 1f / solverInfo.TimeStep; //float damping = solverInfo.m_damping; float Kerp = solverInfo.Erp; float Kcor = Kerp * Kfps; ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; if (cpd == null) throw new BulletException(); float distance = cpd.Penetration; float positionalError = Kcor * -distance; float velocityError = cpd.Restitution - relVel;// * damping; float penetrationImpulse = positionalError * cpd.JacDiagABInv; float velocityImpulse = velocityError * cpd.JacDiagABInv; float normalImpulse = penetrationImpulse + velocityImpulse; // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse float oldNormalImpulse = cpd.AppliedImpulse; float sum = oldNormalImpulse + normalImpulse; cpd.AppliedImpulse = 0 > sum ? 0 : sum; normalImpulse = cpd.AppliedImpulse - oldNormalImpulse; if (bodyA.InverseMass != 0) { bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse); } if (bodyB.InverseMass != 0) { bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse); } { //friction Vector3 vel12 = bodyA.GetVelocityInLocalPoint(relPosA); Vector3 vel22 = bodyB.GetVelocityInLocalPoint(relPosB); Vector3 vel3 = vel12 - vel22; relVel = Vector3.Dot(normal, vel3); Vector3 latVel = vel3 - normal * relVel; float lat_rel_vel = latVel.Length(); float combinedFriction = cpd.Friction; if (cpd.AppliedImpulse > 0) if (lat_rel_vel > float.Epsilon) { latVel /= lat_rel_vel; Vector3 temp1 = Vector3.TransformNormal(Vector3.Cross(relPosA, latVel), bodyA.InvInertiaTensorWorld); Vector3 temp2 = Vector3.TransformNormal(Vector3.Cross(relPosB, latVel), bodyB.InvInertiaTensorWorld); float friction_impulse = lat_rel_vel / (bodyA.InverseMass + bodyB.InverseMass + Vector3.Dot(latVel, Vector3.Cross(temp1, relPosA) + Vector3.Cross(temp2, relPosB))); float normal_impulse = cpd.AppliedImpulse * combinedFriction; MathHelper.SetMin(ref friction_impulse, normal_impulse); MathHelper.SetMin(ref friction_impulse, -normal_impulse); bodyA.ApplyImpulse(latVel * -friction_impulse, relPosA); bodyB.ApplyImpulse(latVel * friction_impulse, relPosB); } } return normalImpulse; }
//velocity + friction //response between two dynamic objects with friction public static float ResolveSingleCollisionCombined( RigidBody bodyA, RigidBody bodyB, ManifoldPoint contactPoint, ContactSolverInfo solverInfo) { Vector3 posA = contactPoint.PositionWorldOnA; Vector3 posB = contactPoint.PositionWorldOnB; Vector3 normal = contactPoint.NormalWorldOnB; Vector3 relPosA = posA - bodyA.CenterOfMassPosition; Vector3 relPosB = posB - bodyB.CenterOfMassPosition; Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); Vector3 vel = velA - velB; float relVel; relVel = Vector3.Dot(normal, vel); float Kfps = 1f / solverInfo.TimeStep; //float damping = solverInfo.m_damping; float Kerp = solverInfo.Erp; float Kcor = Kerp * Kfps; ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; if (cpd == null) { throw new BulletException(); } float distance = cpd.Penetration; float positionalError = Kcor * -distance; float velocityError = cpd.Restitution - relVel; // * damping; float penetrationImpulse = positionalError * cpd.JacDiagABInv; float velocityImpulse = velocityError * cpd.JacDiagABInv; float normalImpulse = penetrationImpulse + velocityImpulse; // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse float oldNormalImpulse = cpd.AppliedImpulse; float sum = oldNormalImpulse + normalImpulse; cpd.AppliedImpulse = 0 > sum ? 0 : sum; normalImpulse = cpd.AppliedImpulse - oldNormalImpulse; if (bodyA.InverseMass != 0) { bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse); } if (bodyB.InverseMass != 0) { bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse); } { //friction Vector3 vel12 = bodyA.GetVelocityInLocalPoint(relPosA); Vector3 vel22 = bodyB.GetVelocityInLocalPoint(relPosB); Vector3 vel3 = vel12 - vel22; relVel = Vector3.Dot(normal, vel3); Vector3 latVel = vel3 - normal * relVel; float lat_rel_vel = latVel.Length(); float combinedFriction = cpd.Friction; if (cpd.AppliedImpulse > 0) { if (lat_rel_vel > float.Epsilon) { latVel /= lat_rel_vel; Vector3 temp1 = Vector3.TransformNormal(Vector3.Cross(relPosA, latVel), bodyA.InvInertiaTensorWorld); Vector3 temp2 = Vector3.TransformNormal(Vector3.Cross(relPosB, latVel), bodyB.InvInertiaTensorWorld); float friction_impulse = lat_rel_vel / (bodyA.InverseMass + bodyB.InverseMass + Vector3.Dot(latVel, Vector3.Cross(temp1, relPosA) + Vector3.Cross(temp2, relPosB))); float normal_impulse = cpd.AppliedImpulse * combinedFriction; MathHelper.SetMin(ref friction_impulse, normal_impulse); MathHelper.SetMin(ref friction_impulse, -normal_impulse); bodyA.ApplyImpulse(latVel * -friction_impulse, relPosA); bodyB.ApplyImpulse(latVel * friction_impulse, relPosB); } } } return(normalImpulse); }
public static float ResolveSingleFrictionEmpty( RigidBody bodyA, RigidBody bodyB, ManifoldPoint contactPoint, ContactSolverInfo solverInfo) { return 0; }
public override void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep) { //apply gravity, predict motion PredictUnconstraintMotion(timeStep); DispatcherInfo dispatchInfo = new DispatcherInfo(); dispatchInfo.TimeStep = timeStep; dispatchInfo.StepCount = 0; dispatchInfo.DebugDraw = DebugDrawer; //perform collision detection PerformDiscreteCollisionDetection(); //solve contact constraints int numManifolds = Dispatcher.ManifoldCount; if (numManifolds != 0) { List<PersistentManifold> manifolds = (Dispatcher as CollisionDispatcher).Manifolds; //int numManifolds = m_dispatcher1.GetNumManifolds(); ContactSolverInfo infoGlobal = new ContactSolverInfo(); infoGlobal.TimeStep = timeStep; _constraintSolver.SolveGroup(new List<CollisionObject>(), manifolds, manifolds.Count, new List<TypedConstraint>(), infoGlobal, _debugDrawer); } //integrate transforms IntegrateTransforms(timeStep); UpdateAabbs(); SynchronizeMotionStates(); }
protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter) { return Solve(bodyA, bodyB, cp, info, iter, null); }
public InplaceSolverIslandCallback( ContactSolverInfo solverInfo, IConstraintSolver solver, List<TypedConstraint> sortedConstraints, IDebugDraw debugDrawer) { _solverInfo = solverInfo; _solver = solver; _sortedConstraints = sortedConstraints; _debugDrawer = debugDrawer; }
protected float SolveCombinedContactFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) { float maxImpulse = 0; Vector3 color = new Vector3(0, 1, 0); if (cp.Distance <= 0) { if (iter == 0) if (debugDraw != null) debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color); float impulse = ContactConstraint.ResolveSingleCollisionCombined( bodyA, bodyB, cp, info); if (maxImpulse < impulse) maxImpulse = impulse; } return maxImpulse; }