public override bool Apply(float dt) { Satisfied = true; if (body == null) { return(false); } var frac = 0.001f; if (frame == ReferenceFrame.Body) { if (doVel) { Vector3.TransformNormal(ref vel, ref body.Transform.Orientation, out var velBodyFrame); Vector3.Multiply(ref body.TransformRate.Velocity, 1.0f - frac, out var v1); Vector3.Multiply(ref velBodyFrame, frac, out body.TransformRate.Velocity); Vector3.Add(ref body.TransformRate.Velocity, ref v1, out body.TransformRate.Velocity); } if (doAngVel) { Vector3.TransformNormal(ref angVel, ref body.Transform.Orientation, out var angVelBodyFrame); Vector3.Multiply(ref body.TransformRate.AngularVelocity, 1.0f - frac, out var v1); Vector3.Multiply(ref angVelBodyFrame, frac, out body.TransformRate.AngularVelocity); Vector3.Add(ref body.TransformRate.AngularVelocity, ref v1, out body.TransformRate.AngularVelocity); } } else { if (doVel) { Vector3.Multiply(ref body.TransformRate.Velocity, 1.0f - frac, out body.TransformRate.Velocity); Vector3.Multiply(ref vel, frac, out var v1); Vector3.Add(ref body.TransformRate.Velocity, ref v1, out body.TransformRate.Velocity); } if (doAngVel) { Vector3.Multiply(ref body.TransformRate.AngularVelocity, 1.0f - frac, out body.TransformRate.AngularVelocity); Vector3.Multiply(ref angVel, frac, out var v1); Vector3.Add(ref body.TransformRate.AngularVelocity, ref v1, out body.TransformRate.AngularVelocity); } } body.SetConstraintsAndCollisionsUnsatisfied(); Satisfied = true; return(true); }
public override bool Apply(float dt) { this.Satisfied = true; bool body0FrozenPre = !body0.IsActive; bool body1FrozenPre = !body1.IsActive; // if (body0FrozenPre && body1FrozenPre) // return false; #region REFERENCE: Vector3 currentVel0 = (body0.Velocity + Vector3.Cross(body0.AngVel, R0)); Vector3 currentVel0; Vector3.Cross(ref body0.transformRate.AngularVelocity, ref R0, out currentVel0); Vector3.Add(ref currentVel0, ref body0.transformRate.Velocity, out currentVel0); #endregion #region REFERENCE: Vector3 currentVel1 = (body1.Velocity + Vector3.Cross(body1.AngVel, R1)); Vector3 currentVel1; Vector3.Cross(ref body1.transformRate.AngularVelocity, ref R1, out currentVel1); Vector3.Add(ref currentVel1, ref body1.transformRate.Velocity, out currentVel1); #endregion // add a "correction" based on the deviation of point 0 #region REFERENCE: Vector3 Vr = (vrExtra + currentVel0 - currentVel1); Vector3 Vr; Vector3.Add(ref vrExtra, ref currentVel0, out Vr); Vector3.Subtract(ref Vr, ref currentVel1, out Vr); #endregion float normalVel = Vr.Length(); if (normalVel < minVelForProcessing) { return(false); } // limit things if (normalVel > mMaxVelMag) { #region REFERENCE: Vr *= mMaxVelMag / normalVel; Vector3.Multiply(ref Vr, mMaxVelMag / normalVel, out Vr); #endregion normalVel = mMaxVelMag; } #region REFERENCE: Vector3 N = Vr / normalVel; Vector3 N; Vector3.Divide(ref Vr, normalVel, out N); #endregion float numerator = -normalVel; #region REFERENCE: float denominator = body0.InvMass + body1.InvMass + Vector3.Dot(N, Vector3.Cross(Vector3.Transform(Vector3.Cross(R0, N), body0.WorldInvInertia), R0)) + Vector3.Dot(N, Vector3.Cross(Vector3.Transform(Vector3.Cross(R1, N), body1.WorldInvInertia), R1)); Vector3 v1; float f1, f2; Vector3.Cross(ref R0, ref N, out v1); Vector3.Transform(ref v1, ref body0.worldInvInertia, out v1); Vector3.Cross(ref v1, ref R0, out v1); Vector3.Dot(ref N, ref v1, out f1); Vector3.Cross(ref R1, ref N, out v1); Vector3.Transform(ref v1, ref body1.worldInvInertia, out v1); Vector3.Cross(ref v1, ref R1, out v1); Vector3.Dot(ref N, ref v1, out f2); float denominator = body0.InverseMass + body1.InverseMass + f1 + f2; #endregion if (denominator < JiggleMath.Epsilon) { return(false); } #region REFERENCE: Vector3 normalImpulse = (numerator / denominator) * N; Vector3 normalImpulse; Vector3.Multiply(ref N, numerator / denominator, out normalImpulse); #endregion if (!body0.Immovable) { body0.ApplyWorldImpulse(normalImpulse, worldPos); } if (!body1.Immovable) { body1.ApplyWorldImpulse(-normalImpulse, worldPos); } body0.SetConstraintsAndCollisionsUnsatisfied(); body1.SetConstraintsAndCollisionsUnsatisfied(); this.Satisfied = true; return(true); }
public override bool Apply(float dt) { Satisfied = true; if (body == null) { return(false); } float frac = 0.5f; if (frame == ReferenceFrame.Body) // transfrom velocity to the body frame { if (doVel) { #region REFERENCE: Vector3 velBodyFrame = Vector3.Transform(vel, body.Orientation); Vector3 velBodyFrame; Vector3.Transform(ref vel, ref body.transform.Orientation, out velBodyFrame); #endregion #region REFERENCE: body.Velocity = (frac * velBodyFrame + (1.0f - frac) * body.Velocity); Vector3 v1; Vector3.Multiply(ref body.transformRate.Velocity, 1.0f - frac, out v1); Vector3.Multiply(ref velBodyFrame, frac, out body.transformRate.Velocity); Vector3.Add(ref body.transformRate.Velocity, ref v1, out body.transformRate.Velocity); #endregion } if (doAngVel) { #region REFERENCE: Vector3 angVelBodyFrame = Vector3.Transform(angVel, body.Orientation); Vector3 angVelBodyFrame; Vector3.Transform(ref angVel, ref body.transform.Orientation, out angVelBodyFrame); #endregion #region REFERENCE: body.AngVel = (frac * angVelBodyFrame + (1.0f - frac) * body.AngVel); Vector3 v1; Vector3.Multiply(ref body.transformRate.AngularVelocity, 1.0f - frac, out v1); Vector3.Multiply(ref angVelBodyFrame, frac, out body.transformRate.AngularVelocity); Vector3.Add(ref body.transformRate.AngularVelocity, ref v1, out body.transformRate.AngularVelocity); #endregion } } else // leave velocity in the world frame { if (doVel) { #region REFERENCE: body.Velocity = (frac * vel + (1.0f - frac) * body.Velocity); Vector3 v1; Vector3.Multiply(ref body.transformRate.Velocity, 1.0f - frac, out body.transformRate.Velocity); Vector3.Multiply(ref vel, frac, out v1); Vector3.Add(ref body.transformRate.Velocity, ref v1, out body.transformRate.Velocity); #endregion } if (doAngVel) { #region REFERENCE: body.AngVel = (frac * angVel + (1.0f - frac) * body.AngVel); Vector3 v1; Vector3.Multiply(ref body.transformRate.AngularVelocity, 1.0f - frac, out body.transformRate.AngularVelocity); Vector3.Multiply(ref angVel, frac, out v1); Vector3.Add(ref body.transformRate.AngularVelocity, ref v1, out body.transformRate.AngularVelocity); #endregion } } /// todo return false if we were already there... body.SetConstraintsAndCollisionsUnsatisfied(); Satisfied = true; return(true); }
public override bool Apply(float dt) { Satisfied = true; var body0FrozenPre = !body0.IsActive; var body1FrozenPre = !body1.IsActive; if (body0FrozenPre && body1FrozenPre) { return(false); } var currentVel0 = body0.AngularVelocity; Vector3.Cross(ref currentVel0, ref R0, out currentVel0); var body0TransformRate = body0.TransformRate; Vector3.Add(ref body0TransformRate.Velocity, ref currentVel0, out currentVel0); var currentVel1 = body1.AngularVelocity; Vector3.Cross(ref currentVel1, ref R1, out currentVel1); var body1TransformRate = body1.TransformRate; Vector3.Add(ref body1TransformRate.Velocity, ref currentVel1, out currentVel1); Vector3.Subtract(ref currentVel0, ref currentVel1, out var predRelPos0); Vector3.Multiply(ref predRelPos0, dt, out predRelPos0); Vector3.Add(ref predRelPos0, ref currentRelPos0, out predRelPos0); var clampedRelPos0 = predRelPos0; var clampedRelPos0Mag = clampedRelPos0.Length(); if (clampedRelPos0Mag <= JiggleMath.Epsilon) { return(false); } if (clampedRelPos0Mag > mMaxDistance) { Vector3.Multiply(ref clampedRelPos0, mMaxDistance / clampedRelPos0Mag, out clampedRelPos0); } Vector3.Subtract(ref clampedRelPos0, ref currentRelPos0, out var desiredRelVel0); Vector3.Divide(ref desiredRelVel0, MathHelper.Max(dt, JiggleMath.Epsilon), out desiredRelVel0); Vector3.Subtract(ref currentVel0, ref currentVel1, out var Vr); Vector3.Subtract(ref Vr, ref desiredRelVel0, out Vr); var normalVel = Vr.Length(); if (normalVel > maxVelMag) { Vector3.Multiply(ref Vr, maxVelMag / normalVel, out Vr); normalVel = maxVelMag; } else if (normalVel < minVelForProcessing) { return(false); } Vector3.Divide(ref Vr, normalVel, out var N); Vector3.Cross(ref R0, ref N, out var v1); Vector3.TransformNormal(ref v1, ref body0.worldInvInertia, out v1); Vector3.Cross(ref v1, ref R0, out v1); Vector3.Dot(ref N, ref v1, out var f1); Vector3.Cross(ref R1, ref N, out v1); Vector3.TransformNormal(ref v1, ref body1.worldInvInertia, out v1); Vector3.Cross(ref v1, ref R1, out v1); Vector3.Dot(ref N, ref v1, out var f2); var denominator = body0.InverseMass + body1.InverseMass + f1 + f2; if (denominator < JiggleMath.Epsilon) { return(false); } var normalImpulse = -normalVel / denominator; Vector3.Multiply(ref N, normalImpulse, out var imp); if (!body0.Immovable) { body0.ApplyWorldImpulse(ref imp, ref worldPos); } Vector3.Multiply(ref N, -normalImpulse, out imp); if (!body1.Immovable) { body1.ApplyWorldImpulse(ref imp, ref worldPos); } body0.SetConstraintsAndCollisionsUnsatisfied(); body1.SetConstraintsAndCollisionsUnsatisfied(); Satisfied = true; return(true); }
/// <summary> /// Apply /// </summary> /// <param name="dt"></param> /// <returns>bool</returns> public override bool Apply(float dt) { this.Satisfied = true; bool body0FrozenPre = !body0.IsActive; bool body1FrozenPre = !body1.IsActive; if (body0FrozenPre && body1FrozenPre) { return(false); } #region REFERENCE: Vector3 currentVel0 = body0.Velocity + Vector3.Cross(body0.AngVel, R0); Vector3 currentVel0 = body0.AngularVelocity; Vector3.Cross(ref currentVel0, ref R0, out currentVel0); Vector3.Add(ref body0.transformRate.Velocity, ref currentVel0, out currentVel0); #endregion #region REFERENCE: Vector3 currentVel1 = body1.Velocity + Vector3.Cross(body1.AngVel, R1); Vector3 currentVel1 = body1.AngularVelocity; Vector3.Cross(ref currentVel1, ref R1, out currentVel1); Vector3.Add(ref body1.transformRate.Velocity, ref currentVel1, out currentVel1); #endregion // predict a new location #region REFERENCE: Vector3 predRelPos0 = (currentRelPos0 + (currentVel0 - currentVel1) * dt); Vector3 predRelPos0; Vector3.Subtract(ref currentVel0, ref currentVel1, out predRelPos0); Vector3.Multiply(ref predRelPos0, dt, out predRelPos0); Vector3.Add(ref predRelPos0, ref currentRelPos0, out predRelPos0); #endregion // if the new position is out of range then clamp it Vector3 clampedRelPos0 = predRelPos0; float clampedRelPos0Mag = clampedRelPos0.Length; if (clampedRelPos0Mag <= JiggleMath.Epsilon) { return(false); } if (clampedRelPos0Mag > mMaxDistance) #region REFERENCE: clampedRelPos0 *= mMaxDistance / clampedRelPos0Mag; { Vector3.Multiply(ref clampedRelPos0, mMaxDistance / clampedRelPos0Mag, out clampedRelPos0); } #endregion // now claculate desired vel based on the current pos, new/clamped // pos and dt #region REFERENCE: Vector3 desiredRelVel0 = ((clampedRelPos0 - currentRelPos0) / System.Math.Max(dt, JiggleMath.Epsilon)); Vector3 desiredRelVel0; Vector3.Subtract(ref clampedRelPos0, ref currentRelPos0, out desiredRelVel0); Vector3.Divide(ref desiredRelVel0, OpenTKHelper.Max(dt, JiggleMath.Epsilon), out desiredRelVel0); #endregion // Vr is -ve the total velocity change #region REFERENCE: Vector3 Vr = (currentVel0 - currentVel1) - desiredRelVel0; Vector3 Vr; Vector3.Subtract(ref currentVel0, ref currentVel1, out Vr); Vector3.Subtract(ref Vr, ref desiredRelVel0, out Vr); #endregion float normalVel = Vr.Length; // limit it if (normalVel > maxVelMag) { #region REFERENCE: Vr *= (maxVelMag / normalVel); Vector3.Multiply(ref Vr, maxVelMag / normalVel, out Vr); #endregion normalVel = maxVelMag; } else if (normalVel < minVelForProcessing) { return(false); } #region REFERENCE: Vector3 N = Vr / normalVel; Vector3 N; Vector3.Divide(ref Vr, normalVel, out N); #endregion #region REFERENCE: float denominator = body0.InvMass + body1.InvMass + Vector3.Dot(N, Vector3.Cross(Vector3.Transform(Vector3.Cross(R0, N), body0.WorldInvInertia), R0)) + Vector3.Dot(N, Vector3.Cross(Vector3.Transform(Vector3.Cross(R1, N), body1.WorldInvInertia), R1)); Vector3 v1; float f1, f2; Vector3.Cross(ref R0, ref N, out v1); Vector3Extensions.TransformNormal(ref v1, ref body0.worldInvInertia, out v1); Vector3.Cross(ref v1, ref R0, out v1); Vector3.Dot(ref N, ref v1, out f1); Vector3.Cross(ref R1, ref N, out v1); Vector3Extensions.TransformNormal(ref v1, ref body1.worldInvInertia, out v1); Vector3.Cross(ref v1, ref R1, out v1); Vector3.Dot(ref N, ref v1, out f2); float denominator = body0.InverseMass + body1.InverseMass + f1 + f2; #endregion if (denominator < JiggleMath.Epsilon) { return(false); } float normalImpulse = -normalVel / denominator; #region REFERENCE: if (!body0.Immovable) body0.ApplyWorldImpulse(normalImpulse * N, worldPos); Vector3 imp; Vector3.Multiply(ref N, normalImpulse, out imp); if (!body0.Immovable) { body0.ApplyWorldImpulse(ref imp, ref worldPos); } #endregion #region REFERENCE: if (!body1.Immovable) body1.ApplyWorldImpulse(-normalImpulse * N, worldPos); Vector3.Multiply(ref N, -normalImpulse, out imp); if (!body1.Immovable) { body1.ApplyWorldImpulse(ref imp, ref worldPos); } #endregion body0.SetConstraintsAndCollisionsUnsatisfied(); body1.SetConstraintsAndCollisionsUnsatisfied(); this.Satisfied = true; return(true); }
public override bool Apply(float dt) { Satisfied = true; var body0FrozenPre = !body0.IsActive; var body1FrozenPre = !body1.IsActive; Vector3.Cross(ref body0.TransformRate.AngularVelocity, ref R0, out var currentVel0); Vector3.Add(ref currentVel0, ref body0.TransformRate.Velocity, out currentVel0); Vector3.Cross(ref body1.TransformRate.AngularVelocity, ref R1, out var currentVel1); Vector3.Add(ref currentVel1, ref body1.TransformRate.Velocity, out currentVel1); Vector3.Add(ref vrExtra, ref currentVel0, out var Vr); Vector3.Subtract(ref Vr, ref currentVel1, out Vr); var normalVel = Vr.Length(); if (normalVel < minVelForProcessing) { return(false); } if (normalVel > mMaxVelMag) { Vector3.Multiply(ref Vr, mMaxVelMag / normalVel, out Vr); normalVel = mMaxVelMag; } Vector3.Divide(ref Vr, normalVel, out var N); var numerator = -normalVel; Vector3.Cross(ref R0, ref N, out var v1); Vector3.TransformNormal(ref v1, ref body0.worldInvInertia, out v1); Vector3.Cross(ref v1, ref R0, out v1); Vector3.Dot(ref N, ref v1, out var f1); Vector3.Cross(ref R1, ref N, out v1); Vector3.TransformNormal(ref v1, ref body1.worldInvInertia, out v1); Vector3.Cross(ref v1, ref R1, out v1); Vector3.Dot(ref N, ref v1, out var f2); var denominator = body0.InverseMass + body1.InverseMass + f1 + f2; if (denominator < JiggleMath.Epsilon) { return(false); } Vector3.Multiply(ref N, numerator / denominator, out var normalImpulse); if (!body0.Immovable) { body0.ApplyWorldImpulse(normalImpulse, worldPos); } if (!body1.Immovable) { body1.ApplyWorldImpulse(-normalImpulse, worldPos); } body0.SetConstraintsAndCollisionsUnsatisfied(); body1.SetConstraintsAndCollisionsUnsatisfied(); Satisfied = true; return(true); }
/// <summary> /// Apply /// </summary> /// <param name="dt"></param> /// <returns>bool</returns> public override bool Apply(float dt) { Satisfied = true; // transform PointOnBody to the world space #region REFERENCE: Vector3 worldPos = body.Position + Vector3.Transform(pointOnBody, body.Orientation); Vector3 worldPos; Vector3Extensions.TransformNormal(ref pointOnBody, ref body.transform.Orientation, out worldPos); Vector3.Add(ref worldPos, ref body.transform.Position, out worldPos); #endregion #region REFERENCE: Vector3 R = worldPos - body.Position; Vector3 R; Vector3.Subtract(ref worldPos, ref body.transform.Position, out R); #endregion #region REFERENCE: Vector3 currentVel = body.Velocity + Vector3.Cross(body.AngVel, R); Vector3 currentVel; Vector3.Cross(ref body.transformRate.AngularVelocity, ref R, out currentVel); Vector3.Add(ref currentVel, ref body.transformRate.Velocity, out currentVel); #endregion // add an extra term to get us back to the original position Vector3 desiredVel; float allowedDeviation = 0.01f; float timescale = 4.0f * dt; #region REFERENCE: Vector3 deviation = worldPos - worldPosition; Vector3 deviation; Vector3.Subtract(ref worldPos, ref worldPosition, out deviation); #endregion float deviationDistance = deviation.Length; if (deviationDistance > allowedDeviation) { #region REFERENCE: Vector3 deviationDir = deviation / deviationDistance; Vector3 deviationDir; Vector3.Divide(ref deviation, deviationDistance, out deviationDir); #endregion #region REFERENCE: desiredVel = ((allowedDeviation - deviationDistance) / timescale) * deviationDir; Vector3.Multiply(ref deviationDir, (allowedDeviation - deviationDistance) / timescale, out desiredVel); #endregion } else { desiredVel = Vector3.Zero; } // stop velocities pushing us through geometry if (body.CollisionSkin != null) { List <CollisionInfo> collisions = body.CollisionSkin.Collisions; int num = collisions.Count; for (int i = 0; i < num; i++) { CollisionInfo collInfo = collisions[i]; if (collInfo.SkinInfo.Skin1.Owner == null) { Vector3 dir = collInfo.DirToBody0; #region float dot = Vector3.Dot(desiredVel, dir); float dot; Vector3.Dot(ref desiredVel, ref dir, out dot); #endregion if (dot < 0.0f) { desiredVel -= dot * dir; } } } } // need an impulse to take us from the current vel to the desired vel #region REFERENCE: Vector3 N = currentVel - desiredVel; Vector3 N; Vector3.Subtract(ref currentVel, ref desiredVel, out N); #endregion float normalVel = N.Length; if (normalVel < minVelForProcessing) { return(false); } #region REFERENCE: N /= normalVel; Vector3.Divide(ref N, normalVel, out N); #endregion #region REFERENCE: float denominator = body.InvMass + Vector3.Dot(N, Vector3.Cross(Vector3.Transform(Vector3.Cross(R, N), body.WorldInvInertia), R)); Vector3 v1; float f1; Vector3.Cross(ref R, ref N, out v1); Vector3Extensions.TransformNormal(ref v1, ref body.worldInvInertia, out v1); Vector3.Cross(ref v1, ref R, out v1); Vector3.Dot(ref N, ref v1, out f1); float denominator = body.InverseMass + f1; #endregion if (denominator < JiggleMath.Epsilon) { return(false); } float normalImpulse = -normalVel / denominator; body.ApplyWorldImpulse(normalImpulse * N, worldPos); body.SetConstraintsAndCollisionsUnsatisfied(); Satisfied = true; return(true); }
public override bool Apply(float dt) { Satisfied = true; Vector3.TransformNormal(ref pointOnBody, ref Body.Transform.Orientation, out var worldPos); Vector3.Add(ref worldPos, ref Body.Transform.Position, out worldPos); Vector3.Subtract(ref worldPos, ref Body.Transform.Position, out var R); Vector3.Cross(ref Body.TransformRate.AngularVelocity, ref R, out var currentVel); Vector3.Add(ref currentVel, ref Body.TransformRate.Velocity, out currentVel); Vector3 desiredVel; var allowedDeviation = 0.01f; var timescale = 4.0f * dt; Vector3.Subtract(ref worldPos, ref worldPosition, out var deviation); var deviationDistance = deviation.Length(); if (deviationDistance > allowedDeviation) { Vector3.Divide(ref deviation, deviationDistance, out var deviationDir); Vector3.Multiply(ref deviationDir, (allowedDeviation - deviationDistance) / timescale, out desiredVel); } else { desiredVel = Vector3.Zero; } if (Body.CollisionSkin != null) { var collisions = Body.CollisionSkin.Collisions; var num = collisions.Count; for (var i = 0; i < num; i++) { var collInfo = collisions[i]; if (collInfo.SkinInfo.Skin1.Owner == null) { var dir = collInfo.DirToBody0; Vector3.Dot(ref desiredVel, ref dir, out var dot); if (dot < 0.0f) { desiredVel -= dot * dir; } } } } Vector3.Subtract(ref currentVel, ref desiredVel, out var N); var normalVel = N.Length(); if (normalVel < minVelForProcessing) { return(false); } Vector3.Divide(ref N, normalVel, out N); Vector3.Cross(ref R, ref N, out var v1); Vector3.TransformNormal(ref v1, ref Body.worldInvInertia, out v1); Vector3.Cross(ref v1, ref R, out v1); Vector3.Dot(ref N, ref v1, out var f1); var denominator = Body.InverseMass + f1; if (denominator < JiggleMath.Epsilon) { return(false); } var normalImpulse = -normalVel / denominator; Body.ApplyWorldImpulse(normalImpulse * N, worldPos); Body.SetConstraintsAndCollisionsUnsatisfied(); Satisfied = true; return(true); }