/// <summary> /// Return this info to the pool. /// </summary> /// <param name="info"></param> public static void FreeCollisionInfo(CollisionInfo info) { info.Destroy(); freeInfos.Push(info); }
/// <summary> /// Return this info to the pool. /// </summary> /// <param name="info"></param> public static void FreeCollisionInfo(CollisionInfo info) { info.Destroy(); freeInfos.Push(info); }
/// <summary> /// PreProcessCollision /// </summary> /// <param name="collision"></param> /// <param name="dt"></param> 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 = OpenTKHelper.Clamp(approachScale, JiggleMath.Epsilon, 1.0f); ptInfo.MinSeparationVel = approachScale * (ptInfo.Info.InitialPenetration - allowedPenetration) / OpenTKHelper.Max(dt, JiggleMath.Epsilon); } if (ptInfo.MinSeparationVel > maxVelMag) ptInfo.MinSeparationVel = maxVelMag; } }
/// <summary> /// PreProcessCollisionAccumulated /// </summary> /// <param name="collision"></param> /// <param name="dt"></param> 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 = OpenTKHelper.Clamp(approachScale, JiggleMath.Epsilon, 1.0f); ptInfo.MinSeparationVel = approachScale * (ptInfo.Info.InitialPenetration - allowedPenetration) / OpenTKHelper.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> /// ProcessCollisionFast /// </summary> /// <param name="collision"></param> /// <param name="dt"></param> /// <param name="firstContact"></param> /// <returns>bool</returns> 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.Transform(Vector3.Cross(ptInfo.R0, T), body0.WorldInvInertia), ptInfo.R0)); Vector3 v1; float f2; Vector3.Cross(ref ptInfo.Info.R0, ref T, out v1); Vector3Extensions.TransformNormal(ref v1, ref body0.worldInvInertia, out v1); Vector3.Cross(ref v1, ref ptInfo.Info.R0, out v1); Vector3.Dot(ref T, ref v1, out f2); denominator = body0.InverseMass + f2; } if ((body1 != null) && (!body1.Immovable)) { //denominator += body1.InvMass + // Vector3.Dot(T, Vector3.Cross(Vector3.Transform(Vector3.Cross(ptInfo.R1, T), body1.WorldInvInertia), ptInfo.R1)); Vector3 v1; float f2; Vector3.Cross(ref ptInfo.Info.R1, ref T, out v1); Vector3Extensions.TransformNormal(ref v1, ref body1.worldInvInertia, out v1); Vector3.Cross(ref v1, ref ptInfo.Info.R1, out v1); Vector3.Dot(ref T, ref v1, out f2); 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; }
/// <summary> /// ProcessCollisionsForShock /// </summary> /// <param name="collision"></param> /// <param name="dt"></param> /// <returns></returns> 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; }
/// <summary> /// ProcessCollisionCombined /// </summary> /// <param name="collision"></param> /// <param name="dt"></param> /// <param name="firstContact"></param> /// <returns>bool</returns> 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(Vector3Extensions.TransformNormal(Vector3.Cross(R0, N), body0.WorldInvInertia), R0)); if ((body1 != null) && (!body1.Immovable)) denominator += body1.InverseMass + Vector3.Dot(N, Vector3.Cross(Vector3Extensions.TransformNormal(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(Vector3Extensions.TransformNormal(Vector3.Cross(ptInfo.Info.R0, T), body0.WorldInvInertia), ptInfo.Info.R0)); } if ((body1 != null) && (!body1.Immovable)) { denominator += body1.InverseMass + Vector3.Dot(T, Vector3.Cross(Vector3Extensions.TransformNormal(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; }
/// <summary> /// ProcessCollisionAccumulated /// </summary> /// <param name="collision"></param> /// <param name="dt"></param> /// <param name="firstContact"></param> /// <returns>bool</returns> 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); Vector3.Dot(ref v0, ref N, out normalVel); //normalVel = Vector3.Dot(v0, N); } else { Vector3 v0; body0.GetVelocity(ref ptInfo.Info.R0, out v0); //normalVel = Vector3.Dot(v0, N); Vector3.Dot(ref v0, ref N, out normalVel); } // 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 = OpenTKHelper.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); Vector3.Dot(ref v0, ref N, out normalVel); //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); Vector3.Dot(ref v0, ref N, out normalVel); } 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; Vector3.Dot(ref vrNew, ref N, out f1); 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; }
/// <summary> /// ProcessCollision /// </summary> /// <param name="collision"></param> /// <param name="dt"></param> /// <param name="firstContact"></param> /// <returns>bool</returns> 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); Vector3.Dot(ref v0, ref N, out normalVel); } else { Vector3 v0; body0.GetVelocity(ref ptInfo.Info.R0, out v0); Vector3.Dot(ref v0, ref N, out normalVel); } 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; Vector3.Dot(ref vrNew, ref N, out f1); 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; }
/// <summary> /// PreProcessCollisionFast /// </summary> /// <param name="collision"></param> /// <param name="dt"></param> 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); Vector3Extensions.TransformNormal(ref cross, ref body0.worldInvInertia, out cross); Vector3.Cross(ref cross, ref ptInfo.Info.R0, out cross); Vector3.Dot(ref N, ref cross, out res); ptInfo.Denominator = body0.InverseMass + res; } if ((body1 != null) && !body1.Immovable) { //ptInfo.Denominator += body1.InvMass + // Vector3.Dot(N, Vector3.Cross(Vector3.Transform(Vector3.Cross(ptInfo.R1, N), body1.WorldInvInertia), ptInfo.R1)); Vector3 cross; float res; Vector3.Cross(ref ptInfo.Info.R1, ref N, out cross); Vector3Extensions.TransformNormal(ref cross, ref body1.worldInvInertia, out cross); Vector3.Cross(ref cross, ref ptInfo.Info.R1, out cross); Vector3.Dot(ref N, ref cross, out res); 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 = OpenTKHelper.Clamp(approachScale, JiggleMath.Epsilon, 1.0f); ptInfo.MinSeparationVel = approachScale * (ptInfo.Info.InitialPenetration - allowedPenetration) / OpenTKHelper.Max(dt, JiggleMath.Epsilon); } if (ptInfo.MinSeparationVel > maxVelMag) ptInfo.MinSeparationVel = maxVelMag; } }
/// <summary> /// 指定の方向ベクトルについての衝突情報とその衝突法線ベクトルを探します。 /// </summary> /// <param name="direction">判定する方向ベクトル。</param> /// <param name="resultCollision">衝突情報 (JigLibX)。</param> /// <param name="resultNormal">衝突法線ベクトル。</param> void FindCollisionAndNormal(ref Vector3 direction, out CollisionInfo resultCollision, out Vector3? resultNormal) { resultCollision = null; resultNormal = null; if (direction.IsZero()) return; // 衝突リストから指定された検査方向ベクトルの逆ベクトルを // 衝突法線ベクトルとする CollisionSkin を探します。 // 逆ベクトルと法線ベクトルの一致性は、 // その内積結果が一定の範囲内である場合に一致とみなします。 var dir = -direction; dir.Normalize(); foreach (var collision in Collisions) { var normal = collision.DirToBody0; if (collision.SkinInfo.Skin1 == this) { Vector3.Negate(ref normal, out normal); } float nDotDir; Vector3.Dot(ref normal, ref dir, out nDotDir); if (0.7f < nDotDir) { resultCollision = collision; resultNormal = normal; break; } } }