private void PreProcessCollisionAccumulated(CollisionInfo collision, float dt) { Body body0 = collision.SkinInfo.Skin0.Owner; Body body1 = collision.SkinInfo.Skin1.Owner; // make as not satisfied collision.Satisfied = false; // always calc the following Vector3 N = collision.DirToBody0; float timescale = numPenetrationRelaxtionTimesteps * dt; for (int pos = 0; pos < collision.NumCollPts; ++pos) { CollPointInfo ptInfo = collision.PointInfo[pos]; // some things we only calculate if there are bodies, and they are // movable if (body0.Immovable) ptInfo.Denominator = 0.0f; else { #region INLINE: ptInfo.Denominator = body0.InvMass + Vector3.Dot(N, Vector3.Cross(body0.WorldInvInertia * (Vector3.Cross(ptInfo.R0, N)), ptInfo.R0)); float num0 = ptInfo.Info.R0.Y * N.Z - ptInfo.Info.R0.Z * N.Y; float num1 = ptInfo.Info.R0.Z * N.X - ptInfo.Info.R0.X * N.Z; float num2 = ptInfo.Info.R0.X * N.Y - ptInfo.Info.R0.Y * N.X; float num3 = (((num0 * body0.worldInvInertia.M11) + (num1 * body0.worldInvInertia.M21)) + (num2 * body0.worldInvInertia.M31)); float num4 = (((num0 * body0.worldInvInertia.M12) + (num1 * body0.worldInvInertia.M22)) + (num2 * body0.worldInvInertia.M32)); float num5 = (((num0 * body0.worldInvInertia.M13) + (num1 * body0.worldInvInertia.M23)) + (num2 * body0.worldInvInertia.M33)); num0 = num4 * ptInfo.Info.R0.Z - num5 * ptInfo.Info.R0.Y; num1 = num5 * ptInfo.Info.R0.X - num3 * ptInfo.Info.R0.Z; num2 = num3 * ptInfo.Info.R0.Y - num4 * ptInfo.Info.R0.X; ptInfo.Denominator = body0.InverseMass + ((num0 * N.X) + (num1 * N.Y) + (num2 * N.Z)); #endregion } if ((body1 != null) && !body1.Immovable) { #region INLINE: ptInfo.Denominator += body1.InvMass + Vector3.Dot(N, Vector3.Cross(body1.WorldInvInertia * (Vector3.Cross(ptInfo.R1, N)), ptInf1.R0)); float num0 = ptInfo.Info.R1.Y * N.Z - ptInfo.Info.R1.Z * N.Y; float num1 = ptInfo.Info.R1.Z * N.X - ptInfo.Info.R1.X * N.Z; float num2 = ptInfo.Info.R1.X * N.Y - ptInfo.Info.R1.Y * N.X; float num3 = (((num0 * body1.worldInvInertia.M11) + (num1 * body1.worldInvInertia.M21)) + (num2 * body1.worldInvInertia.M31)); float num4 = (((num0 * body1.worldInvInertia.M12) + (num1 * body1.worldInvInertia.M22)) + (num2 * body1.worldInvInertia.M32)); float num5 = (((num0 * body1.worldInvInertia.M13) + (num1 * body1.worldInvInertia.M23)) + (num2 * body1.worldInvInertia.M33)); num0 = num4 * ptInfo.Info.R1.Z - num5 * ptInfo.Info.R1.Y; num1 = num5 * ptInfo.Info.R1.X - num3 * ptInfo.Info.R1.Z; num2 = num3 * ptInfo.Info.R1.Y - num4 * ptInfo.Info.R1.X; ptInfo.Denominator += body1.InverseMass + ((num0 * N.X) + (num1 * N.Y) + (num2 * N.Z)); #endregion } if (ptInfo.Denominator < JiggleMath.Epsilon) ptInfo.Denominator = JiggleMath.Epsilon; // calculate the world position Vector3.Add(ref body0.oldTransform.Position, ref ptInfo.Info.R0, out ptInfo.Position); //ptInfo.Position = body0.OldPosition + ptInfo.R0; // per-point penetetration resolution if (ptInfo.Info.InitialPenetration > allowedPenetration) { ptInfo.MinSeparationVel = (ptInfo.Info.InitialPenetration - allowedPenetration) / timescale; } else { float approachScale = -0.1f * (ptInfo.Info.InitialPenetration - allowedPenetration) / (JiggleMath.Epsilon + allowedPenetration); approachScale = MathHelper.Clamp(approachScale, JiggleMath.Epsilon, 1.0f); ptInfo.MinSeparationVel = approachScale * (ptInfo.Info.InitialPenetration - allowedPenetration) / MathHelper.Max(dt, JiggleMath.Epsilon); } ptInfo.AccumulatedNormalImpulse = 0.0f; ptInfo.AccumulatedNormalImpulseAux = 0.0f; ptInfo.AccumulatedFrictionImpulse = Vector3.Zero; /// todo take this value from config or derive from the geometry (but don't reference the body in the cache as it /// may be deleted) float minDist = 0.2f; float bestDistSq = minDist * minDist; Contact.BodyPair bp = new Contact.BodyPair(body0, body1); int count = catchedContacts.Count; for (int i = 0; i < count; i++) { if (!(bp.BodyA == catchedContacts[i].Pair.BodyA && bp.BodyB == catchedContacts[i].Pair.BodyB)) continue; //float distSq = (catchedContacts[i].Pair.BodyA == collision.SkinInfo.Skin0.Owner) ? // Distance.PointPointDistanceSq(catchedContacts[i].Pair.RA, ptInfo.R0) : // Distance.PointPointDistanceSq(catchedContacts[i].Pair.RA, ptInfo.R1); float distSq; if (catchedContacts[i].Pair.BodyA == collision.SkinInfo.Skin0.Owner) { float num3 = catchedContacts[i].Pair.RA.X - ptInfo.Info.R0.X; float num2 = catchedContacts[i].Pair.RA.Y - ptInfo.Info.R0.Y; float num0 = catchedContacts[i].Pair.RA.Z - ptInfo.Info.R0.Z; distSq = ((num3 * num3) + (num2 * num2)) + (num0 * num0); } //Distance.PointPointDistanceSq(ref catchedContacts[i].Pair.RA, ref ptInfo.R0, out distSq); else { //Distance.PointPointDistanceSq(ref catchedContacts[i].Pair.RA, ref ptInfo.R1, out distSq); float num3 = catchedContacts[i].Pair.RA.X - ptInfo.Info.R1.X; float num2 = catchedContacts[i].Pair.RA.Y - ptInfo.Info.R1.Y; float num0 = catchedContacts[i].Pair.RA.Z - ptInfo.Info.R1.Z; distSq = ((num3 * num3) + (num2 * num2)) + (num0 * num0); } if (distSq < bestDistSq) { bestDistSq = distSq; ptInfo.AccumulatedNormalImpulse = catchedContacts[i].Impulse.NormalImpulse; ptInfo.AccumulatedNormalImpulseAux = catchedContacts[i].Impulse.NormalImpulseAux; ptInfo.AccumulatedFrictionImpulse = catchedContacts[i].Impulse.FrictionImpulse; if (catchedContacts[i].Pair.BodyA != collision.SkinInfo.Skin0.Owner) ptInfo.AccumulatedFrictionImpulse *= -1; } } //float oldScale = 1.0f; //ptInfo.AccumulatedNormalImpulse *= oldScale; //ptInfo.AccumulatedFrictionImpulse *= oldScale; //ptInfo.AccumulatedNormalImpulseAux *= oldScale; if (ptInfo.AccumulatedNormalImpulse != 0.0f) { //Vector3 impulse = N * ptInfo.AccumulatedNormalImpulse; Vector3 impulse; Vector3.Multiply(ref N, ptInfo.AccumulatedNormalImpulse, out impulse); //impulse += ptInfo.AccumulatedFrictionImpulse; Vector3.Add(ref impulse, ref ptInfo.AccumulatedFrictionImpulse, out impulse); body0.ApplyBodyWorldImpulse(ref impulse, ref ptInfo.Info.R0); if (body1 != null) body1.ApplyNegativeBodyWorldImpulse(ref impulse, ref ptInfo.Info.R1); } if (ptInfo.AccumulatedNormalImpulseAux != 0.0f) { //Vector3 impulse = N * ptInfo.AccumulatedNormalImpulseAux; Vector3 impulse; Vector3.Multiply(ref N, ptInfo.AccumulatedNormalImpulseAux, out impulse); body0.ApplyBodyWorldImpulseAux(ref impulse,ref ptInfo.Info.R0); if (body1 != null) body1.ApplyNegativeBodyWorldImpulseAux(ref impulse,ref ptInfo.Info.R1); } } }
/// <summary> /// Return this info to the pool. /// </summary> /// <param name="info"></param> public static void FreeCollisionInfo(CollisionInfo info) { info.Destroy(); freeInfos.Push(info); }
private bool ProcessCollisionsForShock(CollisionInfo collision, float dt) { collision.Satisfied = true; Vector3 N = collision.DirToBody0; // Changed here. N.X = N.Y = 0.0f; N.X = N.Z = 0.0f; JiggleMath.NormalizeSafe(ref N); int iterations = 5; int pos; float timescale = penetrationShockRelaxtionTimestep * dt; for (pos = 0; pos < collision.NumCollPts; ++pos) { CollPointInfo ptInfo = collision.PointInfo[pos]; } // since this is shock, body 0 OR body1 can be immovable. Also, if // immovable make the constraint against a non-moving object Body body0 = collision.SkinInfo.Skin0.Owner; Body body1 = collision.SkinInfo.Skin1.Owner; if (body0.Immovable) body0 = null; if ((body1 != null) && body1.Immovable) body1 = null; if (body0 == null && body1 == null) return false; for (int iteration = 0; iteration < iterations; ++iteration) { for (pos = 0; pos < collision.NumCollPts; ++pos) { CollPointInfo ptInfo = collision.PointInfo[pos]; float normalVel = 0.0f; if (body0 != null) normalVel = Vector3.Dot(body0.GetVelocity(ptInfo.Info.R0), N) + Vector3.Dot(body0.GetVelocityAux(ptInfo.Info.R0), N); if (body1 != null) normalVel -= Vector3.Dot(body1.GetVelocity(ptInfo.Info.R1), N) + Vector3.Dot(body1.GetVelocityAux(ptInfo.Info.R1), N); float finalNormalVel = (ptInfo.Info.InitialPenetration - allowedPenetration) / timescale; if (finalNormalVel < 0.0f) continue; float impulse = (finalNormalVel - normalVel) / ptInfo.Denominator; float orig = ptInfo.AccumulatedNormalImpulseAux; ptInfo.AccumulatedNormalImpulseAux = System.Math.Max(ptInfo.AccumulatedNormalImpulseAux + impulse, 0.0f); Vector3 actualImpulse = (ptInfo.AccumulatedNormalImpulseAux - orig) * N; if (body0 != null) body0.ApplyBodyWorldImpulseAux(ref actualImpulse,ref ptInfo.Info.R0); if (body1 != null) body1.ApplyNegativeBodyWorldImpulseAux(ref actualImpulse,ref ptInfo.Info.R1); } } if (body0 != null) body0.SetConstraintsAndCollisionsUnsatisfied(); if (body1 != null) body1.SetConstraintsAndCollisionsUnsatisfied(); return true; }
private void PreProcessCollision(CollisionInfo collision, float dt) { Body body0 = collision.SkinInfo.Skin0.Owner; Body body1 = collision.SkinInfo.Skin1.Owner; // make as not satisfied collision.Satisfied = false; //always calc the following Vector3 N = collision.DirToBody0; float timescale = numPenetrationRelaxtionTimesteps * dt; for (int pos = 0; pos < collision.NumCollPts; ++pos) { CollPointInfo ptInfo = collision.PointInfo[pos]; // some things we only calculate if there are bodies, and they are // movable if (body0.Immovable) ptInfo.Denominator = 0.0f; else { #region INLINE: ptInfo.Denominator = body0.InvMass + Vector3.Dot(N, Vector3.Cross(body0.WorldInvInertia * (Vector3.Cross(ptInfo.R0, N)), ptInfo.R0)); float num0 = (ptInfo.Info.R0.Y * N.Z) - (ptInfo.Info.R0.Z * N.Y); float num1 = (ptInfo.Info.R0.Z * N.X) - (ptInfo.Info.R0.X * N.Z); float num2 = (ptInfo.Info.R0.X * N.Y) - (ptInfo.Info.R0.Y * N.X); float num3 = (((num0 * body0.worldInvInertia.M11) + (num1 * body0.worldInvInertia.M21)) + (num2 * body0.worldInvInertia.M31)); float num4 = (((num0 * body0.worldInvInertia.M12) + (num1 * body0.worldInvInertia.M22)) + (num2 * body0.worldInvInertia.M32)); float num5 = (((num0 * body0.worldInvInertia.M13) + (num1 * body0.worldInvInertia.M23)) + (num2 * body0.worldInvInertia.M33)); num0 = (num4 * ptInfo.Info.R0.Z) - (num5 * ptInfo.Info.R0.Y); num1 = (num5 * ptInfo.Info.R0.X) - (num3 * ptInfo.Info.R0.Z); num2 = (num3 * ptInfo.Info.R0.Y) - (num4 * ptInfo.Info.R0.X); ptInfo.Denominator = body0.InverseMass + ((num0 * N.X) + (num1 * N.Y) + (num2 * N.Z)); #endregion } if ((body1 != null) && !body1.Immovable) { #region INLINE: ptInfo.Denominator += body1.InvMass + Vector3.Dot(N, Vector3.Cross(body1.WorldInvInertia * (Vector3.Cross(ptInfo.R1, N)), ptInf1.R0)); float num0 = (ptInfo.Info.R1.Y * N.Z) - (ptInfo.Info.R1.Z * N.Y); float num1 = (ptInfo.Info.R1.Z * N.X) - (ptInfo.Info.R1.X * N.Z); float num2 = (ptInfo.Info.R1.X * N.Y) - (ptInfo.Info.R1.Y * N.X); float num3 = (((num0 * body1.worldInvInertia.M11) + (num1 * body1.worldInvInertia.M21)) + (num2 * body1.worldInvInertia.M31)); float num4 = (((num0 * body1.worldInvInertia.M12) + (num1 * body1.worldInvInertia.M22)) + (num2 * body1.worldInvInertia.M32)); float num5 = (((num0 * body1.worldInvInertia.M13) + (num1 * body1.worldInvInertia.M23)) + (num2 * body1.worldInvInertia.M33)); num0 = (num4 * ptInfo.Info.R1.Z) - (num5 * ptInfo.Info.R1.Y); num1 = (num5 * ptInfo.Info.R1.X) - (num3 * ptInfo.Info.R1.Z); num2 = (num3 * ptInfo.Info.R1.Y) - (num4 * ptInfo.Info.R1.X); ptInfo.Denominator += body1.InverseMass + ((num0 * N.X) + (num1 * N.Y) + (num2 * N.Z)); #endregion } if (ptInfo.Denominator < JiggleMath.Epsilon) ptInfo.Denominator = JiggleMath.Epsilon; // calculate the world position //Vector3.Add(ref body0.oldTransform.Position, ref ptInfo.R0, out ptInfo.Position); //ptInfo.Position = body0.OldPosition + ptInfo.R0; Vector3.Add(ref body0.oldTransform.Position, ref ptInfo.Info.R0, out ptInfo.Position); // per-point penetration resolution if (ptInfo.Info.InitialPenetration > allowedPenetration) { ptInfo.MinSeparationVel = (ptInfo.Info.InitialPenetration - allowedPenetration) / timescale; } else { float approachScale = -0.1f * (ptInfo.Info.InitialPenetration - allowedPenetration) / (JiggleMath.Epsilon + allowedPenetration); approachScale = MathHelper.Clamp(approachScale, JiggleMath.Epsilon, 1.0f); ptInfo.MinSeparationVel = approachScale * (ptInfo.Info.InitialPenetration - allowedPenetration) / MathHelper.Max(dt, JiggleMath.Epsilon); } if (ptInfo.MinSeparationVel > maxVelMag) ptInfo.MinSeparationVel = maxVelMag; } }
private bool ProcessCollisionFast(CollisionInfo collision, float dt, bool firstContact) { collision.Satisfied = true; Body body0 = collision.SkinInfo.Skin0.Owner; Body body1 = collision.SkinInfo.Skin1.Owner; Vector3 N = collision.DirToBody0; bool gotOne = false; for (int pos = collision.NumCollPts; pos-- != 0; ) { CollPointInfo ptInfo = collision.PointInfo[pos]; float normalVel; if (body1 != null) { //normalVel = Vector3.Dot(body0.GetVelocity(ptInfo.R0) - body1.GetVelocity(ptInfo.R1), collision.DirToBody0); Vector3 v0, v1; body0.GetVelocity(ref ptInfo.Info.R0, out v0); body1.GetVelocity(ref ptInfo.Info.R1, out v1); Vector3.Subtract(ref v0, ref v1, out v0); normalVel = Vector3.Dot(v0, N); } else { Vector3 v0; body0.GetVelocity(ref ptInfo.Info.R0, out v0); normalVel = Vector3.Dot(v0, N); } if (normalVel > ptInfo.MinSeparationVel) continue; float finalNormalVel = -collision.MatPairProperties.Restitution * normalVel; if (finalNormalVel < minVelForProcessing) { // could be zero elasticity in collision, or could be zero // elasticity in contact - don't care. relax towards 0 // penetration finalNormalVel = ptInfo.MinSeparationVel; } float deltaVel = finalNormalVel - normalVel; if (deltaVel < minVelForProcessing) continue; float normalImpulse = deltaVel / ptInfo.Denominator; // prepare our return value gotOne = true; Vector3 impulse = normalImpulse * N; body0.ApplyBodyWorldImpulse(ref impulse, ref ptInfo.Info.R0); if (body1 != null) body1.ApplyNegativeBodyWorldImpulse(ref impulse, ref ptInfo.Info.R1); // recalculate the velocity since it's changed. Vector3 vrNew = body0.GetVelocity(ptInfo.Info.R0); if (body1 != null) vrNew -= body1.GetVelocity(ptInfo.Info.R1); Vector3 tangentVel = vrNew - Vector3.Dot(vrNew, N) * N; float tangentSpeed = tangentVel.Length(); if (tangentSpeed > minVelForProcessing) { Vector3 T = -tangentVel / tangentSpeed; // calculate the "inelastic collision" to zeor the relative vel float denominator = 0.0f; if (!body0.Immovable) { //denominator = body0.InvMass + // Vector3.Dot(T, Vector3.Cross(Vector3.TransformCoordinate(Vector3.Cross(ptInfo.R0, T), body0.WorldInvInertia), ptInfo.R0)); Vector3 v1; float f2; Vector3.Cross(ref ptInfo.Info.R0, ref T, out v1); Vector3.TransformCoordinate(ref v1, ref body0.worldInvInertia, out v1); Vector3.Cross(ref v1, ref ptInfo.Info.R0, out v1); f2 = Vector3.Dot(T, v1); denominator = body0.InverseMass + f2; } if ((body1 != null) && (!body1.Immovable)) { //denominator += body1.InvMass + // Vector3.Dot(T, Vector3.Cross(Vector3.TransformCoordinate(Vector3.Cross(ptInfo.R1, T), body1.WorldInvInertia), ptInfo.R1)); Vector3 v1; float f2; Vector3.Cross(ref ptInfo.Info.R1, ref T, out v1); Vector3.TransformCoordinate(ref v1, ref body1.worldInvInertia, out v1); Vector3.Cross(ref v1, ref ptInfo.Info.R1, out v1); f2 = Vector3.Dot(T, v1); denominator += body1.InverseMass + f2; } if (denominator > JiggleMath.Epsilon) { float impulseToReverse = tangentSpeed / denominator; T *= impulseToReverse; body0.ApplyBodyWorldImpulse(ref T, ref ptInfo.Info.R0); if (body1 != null) body1.ApplyNegativeBodyWorldImpulse(ref T, ref ptInfo.Info.R1); } } // end of friction } if (gotOne) { body0.SetConstraintsAndCollisionsUnsatisfied(); if (body1 != null) body1.SetConstraintsAndCollisionsUnsatisfied(); } return gotOne; }
private unsafe bool ProcessCollisionCombined(CollisionInfo collision, float dt, bool firstContact) { collision.Satisfied = true; Body body0 = collision.SkinInfo.Skin0.Owner; Body body1 = collision.SkinInfo.Skin1.Owner; Vector3 N = collision.DirToBody0; // the individual impulses in the same order as // collision->mPointInfo - for friction float totalImpulse = 0.0f; int pos; Vector3 avPos = Vector3.Zero; float avMinSeparationVel = 0.0f; // the fastest possible way to allocate short living arrays of primitive types float* impulses = stackalloc float[CollisionInfo.MaxCollisionPoints]; for (pos = collision.NumCollPts; pos-- != 0; ) { CollPointInfo ptInfo = collision.PointInfo[pos]; impulses[pos] = 0.0f; float normalVel; if (body1 != null) normalVel = Vector3.Dot(body0.GetVelocity(ptInfo.Info.R0) - body1.GetVelocity(ptInfo.Info.R1), N); else normalVel = Vector3.Dot(body0.GetVelocity(ptInfo.Info.R0), N); if (normalVel > ptInfo.MinSeparationVel) continue; float finalNormalVel = -collision.MatPairProperties.Restitution * normalVel; if (finalNormalVel < minVelForProcessing) { // could be zero elasticity in collision, or could be zero // elasticity in contact - don't care. relax towards 0 // penetration finalNormalVel = ptInfo.MinSeparationVel; } float deltaVel = finalNormalVel - normalVel; if (deltaVel < minVelForProcessing) continue; float normalImpulse = deltaVel / ptInfo.Denominator; impulses[pos] = normalImpulse; totalImpulse += normalImpulse; avPos = avPos + normalImpulse * ptInfo.Position; avMinSeparationVel += ptInfo.MinSeparationVel * normalImpulse; } if (totalImpulse <= JiggleMath.Epsilon) return false; float scale = 1.0f / totalImpulse; // apply all these impulses (as well as subsequently applying an // impulse at an averaged position) for (pos = collision.NumCollPts; pos-- != 0; ) { if (impulses[pos] > JiggleMath.Epsilon) { CollPointInfo ptInfo = collision.PointInfo[pos]; float sc = impulses[pos] * scale; Vector3 impulse; Vector3.Multiply(ref N, impulses[pos] * sc, out impulse); body0.ApplyBodyWorldImpulse(ref impulse, ref ptInfo.Info.R0); if (body1 != null) body1.ApplyNegativeBodyWorldImpulse(ref impulse, ref ptInfo.Info.R1); } } Vector3.Multiply(ref avPos, scale, out avPos); avMinSeparationVel *= scale; // now calculate the single impulse to be applied at avPos Vector3 R0, R1 = Vector3.Zero; R0 = avPos - body0.Position; Vector3 Vr = body0.GetVelocity(R0); if (body1 != null) { R1 = avPos - body1.Position; Vr -= body1.GetVelocity(R1); } float normalVel2 = Vector3.Dot(Vr, N); float normalImpulse2 = 0.0f; if (normalVel2 < avMinSeparationVel) { // coefficient of restitution float finalNormalVel = -collision.MatPairProperties.Restitution * normalVel2; if (finalNormalVel < minVelForProcessing) { // must be a contact - could be zero elasticity in collision, or // could be zero elasticity in contact - don't care. relax // towards 0 penetration finalNormalVel = avMinSeparationVel; } float deltaVel = finalNormalVel - normalVel2; if (deltaVel > minVelForProcessing) { float denominator = 0.0f; if (!body0.Immovable) denominator = body0.InverseMass + Vector3.Dot(N, Vector3.Cross(Vector3.TransformCoordinate(Vector3.Cross(R0, N), body0.WorldInvInertia), R0)); if ((body1 != null) && (!body1.Immovable)) denominator += body1.InverseMass + Vector3.Dot(N, Vector3.Cross(Vector3.TransformCoordinate(Vector3.Cross(R1, N), body1.WorldInvInertia), R1)); if (denominator < JiggleMath.Epsilon) denominator = JiggleMath.Epsilon; normalImpulse2 = deltaVel / denominator; Vector3 impulse = normalImpulse2 * N; body0.ApplyWorldImpulse(impulse, avPos); if (body1 != null) body1.ApplyNegativeWorldImpulse(impulse, avPos); } } // Now do friction point by point for (pos = collision.NumCollPts; pos-- != 0; ) { // For friction, work out the impulse in the opposite direction to // the tangential velocity that would be required to bring this // point to a halt. Apply the minimum of this impulse magnitude, // and the one obtained from the normal impulse. This prevents // reversing the velocity direction. // // However, recalculate the velocity since it's changed. CollPointInfo ptInfo = collision.PointInfo[pos]; Vector3 vrNew = (body1 != null) ? (body0.GetVelocity(ptInfo.Info.R0) - body1.GetVelocity(ptInfo.Info.R1)) : (body0.GetVelocity(ptInfo.Info.R0)); Vector3 T = vrNew - Vector3.Dot(vrNew, N) * N; float tangentSpeed = T.Length(); if (tangentSpeed > minVelForProcessing) { T /= -tangentSpeed; float sc = impulses[pos] * scale; float ptNormalImpulse = sc * (normalImpulse2 + impulses[pos]); // calculate an "inelastic collision" to zero the relative vel float denominator = 0.0f; if (!body0.Immovable) { denominator = body0.InverseMass + Vector3.Dot(T, Vector3.Cross(Vector3.TransformCoordinate(Vector3.Cross(ptInfo.Info.R0, T), body0.WorldInvInertia), ptInfo.Info.R0)); } if ((body1 != null) && (!body1.Immovable)) { denominator += body1.InverseMass + Vector3.Dot(T, Vector3.Cross(Vector3.TransformCoordinate(Vector3.Cross(ptInfo.Info.R1, T), body1.WorldInvInertia), ptInfo.Info.R1)); } if (denominator > JiggleMath.Epsilon) { float impulseToReverse = tangentSpeed / denominator; float impulseFromNormalImpulse = collision.MatPairProperties.StaticFriction * ptNormalImpulse; float frictionImpulse; if (impulseToReverse < impulseFromNormalImpulse) frictionImpulse = impulseToReverse; else frictionImpulse = collision.MatPairProperties.DynamicFriction * ptNormalImpulse; T *= frictionImpulse; body0.ApplyBodyWorldImpulse(T, ptInfo.Info.R0); if (body1 != null) body1.ApplyNegativeBodyWorldImpulse(ref T, ref ptInfo.Info.R1); } } } // end of friction body0.SetConstraintsAndCollisionsUnsatisfied(); if (body1 != null) body1.SetConstraintsAndCollisionsUnsatisfied(); return true; }
private bool ProcessCollisionAccumulated(CollisionInfo collision, float dt, bool firstContact) { collision.Satisfied = true; Body body0 = collision.SkinInfo.Skin0.Owner; Body body1 = collision.SkinInfo.Skin1.Owner; Vector3 N = collision.DirToBody0; bool gotOne = false; for (int pos = collision.NumCollPts; pos-- != 0; ) { CollPointInfo ptInfo = collision.PointInfo[pos]; float normalImpulse; // first the real impulse { float normalVel; if (body1 != null) { Vector3 v0, v1; body0.GetVelocity(ref ptInfo.Info.R0, out v0); body1.GetVelocity(ref ptInfo.Info.R1, out v1); Vector3.Subtract(ref v0, ref v1, out v0); normalVel = Vector3.Dot(v0, N); //normalVel = Vector3.Dot(v0, N); } else { Vector3 v0; body0.GetVelocity(ref ptInfo.Info.R0, out v0); //normalVel = Vector3.Dot(v0, N); normalVel = Vector3.Dot(v0, N); } // result in zero... float deltaVel = -normalVel; // ...except that the impulse reduction to achieve the desired separation must be done // here - not with aux - because aux would suck objects together if (ptInfo.MinSeparationVel < 0.0f) deltaVel += ptInfo.MinSeparationVel; if (System.Math.Abs(deltaVel) > minVelForProcessing) { normalImpulse = deltaVel / ptInfo.Denominator; float origAccumulatedNormalImpulse = ptInfo.AccumulatedNormalImpulse; ptInfo.AccumulatedNormalImpulse = MathHelper.Max(ptInfo.AccumulatedNormalImpulse + normalImpulse, 0.0f); float actualImpulse = ptInfo.AccumulatedNormalImpulse - origAccumulatedNormalImpulse; //Vector3 impulse = Vector3.Multiply(N, actualImpulse); Vector3 impulse; Vector3.Multiply(ref N, actualImpulse, out impulse); body0.ApplyBodyWorldImpulse(ref impulse, ref ptInfo.Info.R0); if (body1 != null) body1.ApplyNegativeBodyWorldImpulse(ref impulse, ref ptInfo.Info.R1); // prepare our return value gotOne = true; } } // now the correction impulse bool doCorrection = true; if (doCorrection) { float normalVel; if (body1 != null) { Vector3 v0, v1; body0.GetVelocityAux(ref ptInfo.Info.R0, out v0); body1.GetVelocityAux(ref ptInfo.Info.R1, out v1); Vector3.Subtract(ref v0, ref v1, out v0); normalVel = Vector3.Dot(v0, N); //normalVel = Vector3.Dot(v0, N); //normalVel = Vector3.Dot(body0.GetVelocityAux(ptInfo.R0) - body1.GetVelocityAux(ptInfo.R1), collision.DirToBody0); } else { Vector3 v0; body0.GetVelocityAux(ref ptInfo.Info.R0, out v0); //normalVel = Vector3.Dot(v0, N); normalVel = Vector3.Dot(v0, N); } float deltaVel = -normalVel; // only try to separate objects if (ptInfo.MinSeparationVel > 0.0f) deltaVel += ptInfo.MinSeparationVel; if (System.Math.Abs(deltaVel) > minVelForProcessing) { normalImpulse = deltaVel / ptInfo.Denominator; float origAccumulatedNormalImpulse = ptInfo.AccumulatedNormalImpulseAux; ptInfo.AccumulatedNormalImpulseAux = System.Math.Max(ptInfo.AccumulatedNormalImpulseAux + normalImpulse, 0.0f); float actualImpulse = ptInfo.AccumulatedNormalImpulseAux - origAccumulatedNormalImpulse; //Vector3 impulse = actualImpulse * collision.DirToBody0; //Vector3 impulse = Vector3.Multiply(N, actualImpulse); Vector3 impulse; Vector3.Multiply(ref N, actualImpulse, out impulse); body0.ApplyBodyWorldImpulseAux(ref impulse, ref ptInfo.Info.R0); if (body1 != null) body1.ApplyNegativeBodyWorldImpulseAux(ref impulse,ref ptInfo.Info.R1); //prepare our return value gotOne = true; } } // For friction, work out the impulse in the opposite direction to // the tangential velocity that would be required to bring this // point to a halt. Apply the minimum of this impulse magnitude, // and the one obtained from the normal impulse. This prevents // reversing the velocity direction. // // recalculate the velocity since it's changed. if (ptInfo.AccumulatedNormalImpulse > 0.0f) { Vector3 vrNew = body0.GetVelocity(ptInfo.Info.R0); if (body1 != null) { Vector3 pt1Vel; body1.GetVelocity(ref ptInfo.Info.R1, out pt1Vel); Vector3.Subtract(ref vrNew, ref pt1Vel, out vrNew); //vrNew = Vector3.Subtract(vrNew, body1.GetVelocity(ptInfo.R1)); } //vrNew -= body1.GetVelocity(ptInfo.R1); //Vector3 tangentVel = vrNew - Vector3.Dot(vrNew, N) * N; Vector3 tangentVel; float f1; f1 = Vector3.Dot(vrNew, N); Vector3.Multiply(ref N, f1, out tangentVel); Vector3.Subtract(ref vrNew, ref tangentVel, out tangentVel); float tangentSpeed = tangentVel.Length(); if (tangentSpeed > minVelForProcessing) { //Vector3 T = -tangentVel / tangentSpeed; Vector3 T; Vector3.Divide(ref tangentVel, -tangentSpeed, out T); // calculate an "inelastic collision" to zeor the relative vel float denominator = 0.0f; if (!body0.Immovable) { #region INLINE: denominator = body0.InvMass + Vector3.Dot(T, Vector3.Cross(body0.WorldInvInertia * (Vector3.Cross(ptInfo.R0, T)), ptInfo.R0)); float num0 = ptInfo.Info.R0.Y * T.Z - ptInfo.Info.R0.Z * T.Y; float num1 = ptInfo.Info.R0.Z * T.X - ptInfo.Info.R0.X * T.Z; float num2 = ptInfo.Info.R0.X * T.Y - ptInfo.Info.R0.Y * T.X; float num3 = (((num0 * body0.worldInvInertia.M11) + (num1 * body0.worldInvInertia.M21)) + (num2 * body0.worldInvInertia.M31)); float num4 = (((num0 * body0.worldInvInertia.M12) + (num1 * body0.worldInvInertia.M22)) + (num2 * body0.worldInvInertia.M32)); float num5 = (((num0 * body0.worldInvInertia.M13) + (num1 * body0.worldInvInertia.M23)) + (num2 * body0.worldInvInertia.M33)); num0 = num4 * ptInfo.Info.R0.Z - num5 * ptInfo.Info.R0.Y; num1 = num5 * ptInfo.Info.R0.X - num3 * ptInfo.Info.R0.Z; num2 = num3 * ptInfo.Info.R0.Y - num4 * ptInfo.Info.R0.X; denominator = body0.InverseMass + ((num0 * T.X) + (num1 * T.Y) + (num2 * T.Z)); #endregion } if ((body1 != null) && (!body1.Immovable)) { #region INLINE: denominator += body1.InvMass + Vector3.Dot(T, Vector3.Cross(body1.WorldInvInertia * (Vector3.Cross(ptInfo.R1, T)), ptInfo.R1)); float num0 = ptInfo.Info.R1.Y * T.Z - ptInfo.Info.R1.Z * T.Y; float num1 = ptInfo.Info.R1.Z * T.X - ptInfo.Info.R1.X * T.Z; float num2 = ptInfo.Info.R1.X * T.Y - ptInfo.Info.R1.Y * T.X; float num3 = (((num0 * body1.worldInvInertia.M11) + (num1 * body1.worldInvInertia.M21)) + (num2 * body1.worldInvInertia.M31)); float num4 = (((num0 * body1.worldInvInertia.M12) + (num1 * body1.worldInvInertia.M22)) + (num2 * body1.worldInvInertia.M32)); float num5 = (((num0 * body1.worldInvInertia.M13) + (num1 * body1.worldInvInertia.M23)) + (num2 * body1.worldInvInertia.M33)); num0 = num4 * ptInfo.Info.R1.Z - num5 * ptInfo.Info.R1.Y; num1 = num5 * ptInfo.Info.R1.X - num3 * ptInfo.Info.R1.Z; num2 = num3 * ptInfo.Info.R1.Y - num4 * ptInfo.Info.R1.X; denominator += body1.InverseMass + ((num0 * T.X) + (num1 * T.Y) + (num2 * T.Z)); #endregion } if (denominator > JiggleMath.Epsilon) { float impulseToReverse = tangentSpeed / denominator; //Vector3 frictionImpulseVec = T * impulseToReverse; Vector3 frictionImpulseVec; Vector3.Multiply(ref T, impulseToReverse, out frictionImpulseVec); Vector3 origAccumulatedFrictionImpulse = ptInfo.AccumulatedFrictionImpulse; //ptInfo.AccumulatedFrictionImpulse += frictionImpulseVec; Vector3.Add(ref ptInfo.AccumulatedFrictionImpulse, ref frictionImpulseVec, out ptInfo.AccumulatedFrictionImpulse); float AFIMag = ptInfo.AccumulatedFrictionImpulse.Length(); float maxAllowedAFIMAg = collision.MatPairProperties.StaticFriction * ptInfo.AccumulatedNormalImpulse; if (AFIMag > JiggleMath.Epsilon && AFIMag > maxAllowedAFIMAg) { //ptInfo.AccumulatedFrictionImpulse *= maxAllowedAFIMAg / AFIMag; Vector3.Multiply(ref ptInfo.AccumulatedFrictionImpulse, maxAllowedAFIMAg / AFIMag, out ptInfo.AccumulatedFrictionImpulse); } //Vector3 actualFrictionImpulse = ptInfo.AccumulatedFrictionImpulse - origAccumulatedFrictionImpulse; Vector3 actualFrictionImpulse; Vector3.Subtract(ref ptInfo.AccumulatedFrictionImpulse, ref origAccumulatedFrictionImpulse, out actualFrictionImpulse); body0.ApplyBodyWorldImpulse(ref actualFrictionImpulse, ref ptInfo.Info.R0); if (body1 != null) body1.ApplyNegativeBodyWorldImpulse(ref actualFrictionImpulse, ref ptInfo.Info.R1); } } // end of friction } } if (gotOne) { body0.SetConstraintsAndCollisionsUnsatisfied(); if (body1 != null) body1.SetConstraintsAndCollisionsUnsatisfied(); } return gotOne; }
private bool ProcessCollision(CollisionInfo collision, float dt, bool firstContact) { collision.Satisfied = true; Body body0 = collision.SkinInfo.Skin0.Owner; Body body1 = collision.SkinInfo.Skin1.Owner; Vector3 N = collision.DirToBody0; bool gotOne = false; for (int pos = 0; pos < collision.NumCollPts; ++pos) { CollPointInfo ptInfo = collision.PointInfo[pos]; float normalVel; if (body1 != null) { Vector3 v0, v1; body0.GetVelocity(ref ptInfo.Info.R0, out v0); body1.GetVelocity(ref ptInfo.Info.R1, out v1); Vector3.Subtract(ref v0, ref v1, out v0); normalVel = Vector3.Dot(v0, N); } else { Vector3 v0; body0.GetVelocity(ref ptInfo.Info.R0, out v0); normalVel = Vector3.Dot(v0, N); } if (normalVel > ptInfo.MinSeparationVel) continue; float finalNormalVel = -collision.MatPairProperties.Restitution * normalVel; if (finalNormalVel < minVelForProcessing) { // could be zero elasticity in collision, or could be zero // elasticity in contact - don't care. relax towards 0 // penetration finalNormalVel = ptInfo.MinSeparationVel; } float deltaVel = finalNormalVel - normalVel; if (deltaVel <= minVelForProcessing) continue; if (ptInfo.Denominator < JiggleMath.Epsilon) { ptInfo.Denominator = JiggleMath.Epsilon; } float normalImpulse = deltaVel / ptInfo.Denominator; // prepare our return value gotOne = true; Vector3 impulse; Vector3.Multiply(ref N, normalImpulse, out impulse); body0.ApplyBodyWorldImpulse(ref impulse, ref ptInfo.Info.R0); if (body1 != null) body1.ApplyNegativeBodyWorldImpulse(ref impulse, ref ptInfo.Info.R1); // For friction, work out the impulse in the opposite direction to // the tangential velocity that would be required to bring this // point to a halt. Apply the minimum of this impulse magnitude, // and the one obtained from the normal impulse. This prevents // reversing the velocity direction. // // recalculate the velocity since it's changed. Vector3 vrNew; body0.GetVelocity(ref ptInfo.Info.R0, out vrNew); if (body1 != null) { Vector3 v1; body1.GetVelocity(ref ptInfo.Info.R1, out v1); Vector3.Subtract(ref vrNew, ref v1, out vrNew); //vrNew -= body1.GetVelocity(ptInfo.R1); } //Vector3 tangentVel = vrNew - Vector3.Dot(vrNew, N) * N; Vector3 tangentVel; float f1; f1 = Vector3.Dot(vrNew, N); Vector3.Multiply(ref N, f1, out tangentVel); Vector3.Subtract(ref vrNew, ref tangentVel, out tangentVel); float tangentSpeed = tangentVel.Length(); if (tangentSpeed > minVelForProcessing) { Vector3 T = -tangentVel / tangentSpeed; // calculate an "inelastic collision" to zero the relative vel float denominator = 0.0f; if (!body0.Immovable) { #region INLINE: denominator = body0.InvMass + Vector3.Dot(T, Vector3.Cross(body0.WorldInvInertia * (Vector3.Cross(ptInfo.R0, T)), ptInfo.R0)); float num0 = (ptInfo.Info.R0.Y * T.Z) - (ptInfo.Info.R0.Z * T.Y); float num1 = (ptInfo.Info.R0.Z * T.X) - (ptInfo.Info.R0.X * T.Z); float num2 = (ptInfo.Info.R0.X * T.Y) - (ptInfo.Info.R0.Y * T.X); float num3 = (((num0 * body0.worldInvInertia.M11) + (num1 * body0.worldInvInertia.M21)) + (num2 * body0.worldInvInertia.M31)); float num4 = (((num0 * body0.worldInvInertia.M12) + (num1 * body0.worldInvInertia.M22)) + (num2 * body0.worldInvInertia.M32)); float num5 = (((num0 * body0.worldInvInertia.M13) + (num1 * body0.worldInvInertia.M23)) + (num2 * body0.worldInvInertia.M33)); num0 = (num4 * ptInfo.Info.R0.Z) - (num5 * ptInfo.Info.R0.Y); num1 = (num5 * ptInfo.Info.R0.X) - (num3 * ptInfo.Info.R0.Z); num2 = (num3 * ptInfo.Info.R0.Y) - (num4 * ptInfo.Info.R0.X); denominator = body0.InverseMass + ((num0 * T.X) + (num1 * T.Y) + (num2 * T.Z)); #endregion } if ((body1 != null) && (!body1.Immovable)) { #region INLINE: denominator += body1.InvMass + Vector3.Dot(T, Vector3.Cross(body1.WorldInvInertia * (Vector3.Cross(ptInfo.R1, T)), ptInfo.R1)); float num0 = (ptInfo.Info.R1.Y * T.Z) - (ptInfo.Info.R1.Z * T.Y); float num1 = (ptInfo.Info.R1.Z * T.X) - (ptInfo.Info.R1.X * T.Z); float num2 = (ptInfo.Info.R1.X * T.Y) - (ptInfo.Info.R1.Y * T.X); float num3 = (((num0 * body1.worldInvInertia.M11) + (num1 * body1.worldInvInertia.M21)) + (num2 * body1.worldInvInertia.M31)); float num4 = (((num0 * body1.worldInvInertia.M12) + (num1 * body1.worldInvInertia.M22)) + (num2 * body1.worldInvInertia.M32)); float num5 = (((num0 * body1.worldInvInertia.M13) + (num1 * body1.worldInvInertia.M23)) + (num2 * body1.worldInvInertia.M33)); num0 = (num4 * ptInfo.Info.R1.Z) - (num5 * ptInfo.Info.R1.Y); num1 = (num5 * ptInfo.Info.R1.X) - (num3 * ptInfo.Info.R1.Z); num2 = (num3 * ptInfo.Info.R1.Y) - (num4 * ptInfo.Info.R1.X); denominator += body1.InverseMass + ((num0 * T.X) + (num1 * T.Y) + (num2 * T.Z)); #endregion } if (denominator > JiggleMath.Epsilon) { float impulseToReserve = tangentSpeed / denominator; float impulseFromNormalImpulse = collision.MatPairProperties.StaticFriction * normalImpulse; float frictionImpulse; if (impulseToReserve < impulseFromNormalImpulse) frictionImpulse = impulseToReserve; else frictionImpulse = collision.MatPairProperties.DynamicFriction * normalImpulse; T *= frictionImpulse; body0.ApplyBodyWorldImpulse(ref T, ref ptInfo.Info.R0); if (body1 != null) body1.ApplyNegativeBodyWorldImpulse(ref T, ref ptInfo.Info.R1); } } // end of friction } if (gotOne) { body0.SetConstraintsAndCollisionsUnsatisfied(); if (body1 != null) body1.SetConstraintsAndCollisionsUnsatisfied(); } return gotOne; }
private void PreProcessCollisionFast(CollisionInfo collision, float dt) { Body body0 = collision.SkinInfo.Skin0.Owner; Body body1 = collision.SkinInfo.Skin1.Owner; // make as not satisfied collision.Satisfied = false; // always calc the following Vector3 N = collision.DirToBody0; float timescale = numPenetrationRelaxtionTimesteps * dt; const int keep = 3; if (collision.NumCollPts > keep) { Array.Sort( collision.PointInfo, MoreCollPtPenetration); collision.NumCollPts = keep; } for (int pos = 0; pos < collision.NumCollPts; ++pos) { CollPointInfo ptInfo = collision.PointInfo[pos]; // some things we only calculate if there are bodies, and they are // movable if (body0.Immovable) ptInfo.Denominator = 0.0f; else { Vector3 cross; float res; Vector3.Cross(ref ptInfo.Info.R0, ref N, out cross); Vector3.TransformCoordinate(ref cross, ref body0.worldInvInertia, out cross); Vector3.Cross(ref cross, ref ptInfo.Info.R0, out cross); res = Vector3.Dot(N, cross); ptInfo.Denominator = body0.InverseMass + res; } if ((body1 != null) && !body1.Immovable) { //ptInfo.Denominator += body1.InvMass + // Vector3.Dot(N, Vector3.Cross(Vector3.TransformCoordinate(Vector3.Cross(ptInfo.R1, N), body1.WorldInvInertia), ptInfo.R1)); Vector3 cross; float res; Vector3.Cross(ref ptInfo.Info.R1, ref N, out cross); Vector3.TransformCoordinate(ref cross, ref body1.worldInvInertia, out cross); Vector3.Cross(ref cross, ref ptInfo.Info.R1, out cross); res = Vector3.Dot(N, cross); ptInfo.Denominator += body1.InverseMass + res; } if (ptInfo.Denominator < JiggleMath.Epsilon) ptInfo.Denominator = JiggleMath.Epsilon; // calculate the world position Vector3.Add(ref body0.oldTransform.Position, ref ptInfo.Info.R0, out ptInfo.Position); // per-point penetration resolution if (ptInfo.Info.InitialPenetration > allowedPenetration) { ptInfo.MinSeparationVel = (ptInfo.Info.InitialPenetration - allowedPenetration) / timescale; } else { float approachScale = -0.1f * (ptInfo.Info.InitialPenetration - allowedPenetration) / (JiggleMath.Epsilon + allowedPenetration); approachScale = MathHelper.Clamp(approachScale, JiggleMath.Epsilon, 1.0f); ptInfo.MinSeparationVel = approachScale * (ptInfo.Info.InitialPenetration - allowedPenetration) / MathHelper.Max(dt, JiggleMath.Epsilon); } if (ptInfo.MinSeparationVel > maxVelMag) ptInfo.MinSeparationVel = maxVelMag; } }