//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 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); }
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; }
//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; }
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); } } } }