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