/// <summary>
/// Solves the constraint for velocity by applying impulses.
/// </summary>
public override void ProcessVelocity()
{
if (this.IsCollisionSuppressed)
{
return;
}
RigidBody a = BodyA, b = BodyB;
for (int i = 0; i < _count; i++)
{
var p = _points[i];
float normalMag;
Vector3 va, vb;
float oldImpulseMag, impulseMag;
Vector3 impulse = Vector3.Zero;
// step 1: collision impulse
a.GetVelocityAtPoint(ref p.OffsetA, out va);
b.GetVelocityAtPoint(ref p.OffsetB, out vb);
va.X -= vb.X; va.Y -= vb.Y; va.Z -= vb.Z; //Vector3.Subtract(ref va, ref vb, out va);
normalMag = _normal.X * va.X + _normal.Y * va.Y + _normal.Z * va.Z; //Vector3.Dot(ref normal, ref va, out normalMag);
impulseMag = (p.Target - normalMag) * p.NormalMass;
if (Math.Abs(impulseMag) >= Constants.Epsilon)
{
oldImpulseMag = p.Impulse;
p.Impulse = Math.Max(p.Impulse + impulseMag, 0f);
impulseMag = p.Impulse - oldImpulseMag;
impulse.X = _normal.X * impulseMag; impulse.Y = _normal.Y * impulseMag; impulse.Z = _normal.Z * impulseMag; // Vector3.Multiply(ref normal, reactImpulse, out impulse);
a.ApplyImpulse(ref impulse, ref p.OffsetA);
impulse.X = -impulse.X; impulse.Y = -impulse.Y; impulse.Z = -impulse.Z; // Vector3.Negate(ref impulse, out impulse);
b.ApplyImpulse(ref impulse, ref p.OffsetB);
}
// step 2: friction impulse
if (p.TangentMass >= Constants.Epsilon)
{
float tangentMag;
a.GetVelocityAtPoint(ref p.OffsetA, out va);
b.GetVelocityAtPoint(ref p.OffsetB, out vb);
va.X -= vb.X; va.Y -= vb.Y; va.Z -= vb.Z; // Vector3.Subtract(ref va, ref vb, out va);
tangentMag = p.Tangent.X * va.X + p.Tangent.Y * va.Y + p.Tangent.Z * va.Z; // Vector3.Dot(ref p.Tangent, ref va, out tangentMag);
float maxFriction = _friction * p.Impulse;
float tangentImpulse = -tangentMag * p.TangentMass;
oldImpulseMag = p.TangentImpulse;
p.TangentImpulse = MathHelper.Clamp(p.TangentImpulse + tangentImpulse, -maxFriction, maxFriction);
impulseMag = p.TangentImpulse - oldImpulseMag;
impulse.X = p.Tangent.X * impulseMag; impulse.Y = p.Tangent.Y * impulseMag; impulse.Z = p.Tangent.Z * impulseMag; // Vector3.Multiply(ref p.Tangent, tangentImpulse, out impulse);
a.ApplyImpulse(ref impulse, ref p.OffsetA);
impulse.X = -impulse.X; impulse.Y = -impulse.Y; impulse.Z = -impulse.Z; // Vector3.Negate(ref impulse, out impulse);
b.ApplyImpulse(ref impulse, ref p.OffsetB);
}
_points[i] = p;
}
}
/// <summary>
/// Prepares the constraint for iterative processing in the current frame.
/// </summary>
public override void PreProcess()
{
RigidBody a = BodyA, b = BodyB;
// get offsets and mass in world coordinates
Vector3.Transform(ref _bodyPointA, ref a.World.Combined, out _worldOffsetA);
Vector3.Subtract(ref _worldOffsetA, ref a.World.Position, out _worldOffsetA);
Vector3.Transform(ref _bodyPointB, ref b.World.Combined, out _worldOffsetB);
Vector3.Subtract(ref _worldOffsetB, ref b.World.Position, out _worldOffsetB);
MassProperties.EffectiveMassMatrix(ref a.MassWorld, ref b.MassWorld, ref _worldOffsetA, ref _worldOffsetB, out _mass);
if (this.Manager.IsSolverWarmStarted)
{
Vector3 impulse;
Vector3.Multiply(ref _impulse, this.Manager.TimeStep, out impulse);
b.ApplyImpulse(ref impulse, ref _worldOffsetB);
Vector3.Negate(ref impulse, out impulse);
a.ApplyImpulse(ref impulse, ref _worldOffsetA);
}
else
{
_impulse = Vector3.Zero;
}
}
private void SolveLinearVelocity(LimitState state, ref Vector3 vel, ref Vector3 axis, ref float accImpulse, float mass)
{
RigidBody a = this.BodyA, b = this.BodyB;
if (state == LimitState.Inactive)
{
return;
}
float impulse, oldImpulse = accImpulse;
Vector3.Dot(ref axis, ref vel, out impulse);
impulse *= -mass;
// clamp the impulse
accImpulse += impulse;
if (state == LimitState.Min && accImpulse < 0f || state == LimitState.Max && accImpulse > 0f)
{
accImpulse = 0f;
}
impulse = accImpulse - oldImpulse;
Vector3 i;
Vector3.Multiply(ref axis, impulse, out i);
a.ApplyImpulse(ref i, ref _anchorA);
Vector3.Negate(ref i, out i);
b.ApplyImpulse(ref i, ref _anchorB);
}
/// <summary>
/// Solve the constraint for velocity by applying impulses.
/// </summary>
public override void ProcessVelocity()
{
RigidBody a = this.BodyA;
Vector3 impulse;
a.GetVelocityAtPoint(ref _worldOffset, out impulse);
Vector3.Negate(ref impulse, out impulse);
Vector3.Add(ref impulse, ref _normal, out impulse);
Vector3.Transform(ref impulse, ref _mass, out impulse);
// clamp the accumulated impulse against the max force
Vector3.Multiply(ref impulse, this.Manager.TimeStepInverse, out impulse);
Vector3 oldImpulse = _impulse;
Vector3.Add(ref _impulse, ref impulse, out _impulse);
if (_impulse.LengthSquared() > _maxForce * _maxForce)
{
Vector3.Multiply(ref _impulse, _maxForce / _impulse.Length(), out _impulse);
}
Vector3.Subtract(ref _impulse, ref oldImpulse, out impulse);
Vector3.Multiply(ref impulse, this.Manager.TimeStep, out impulse);
a.ApplyImpulse(ref impulse, ref _worldOffset);
}
/// <summary>
/// Prepares the constraint for iterative processing in the current frame.
/// </summary>
public override void PreProcess()
{
RigidBody a = BodyA, b = BodyB;
// get world points and normal
Vector3.Transform(ref _bodyPointA, ref a.World.Combined, out _worldOffsetA);
Vector3.Transform(ref _bodyPointB, ref b.World.Combined, out _worldOffsetB);
Vector3.Subtract(ref _worldOffsetA, ref _worldOffsetB, out _normal);
Vector3.Subtract(ref _worldOffsetA, ref a.World.Position, out _worldOffsetA);
Vector3.Subtract(ref _worldOffsetB, ref b.World.Position, out _worldOffsetB);
_distance = _normal.Length();
if (_distance < Constants.Epsilon)
{
_normal = Vector3.Zero;
}
else
{
Vector3.Divide(ref _normal, _distance, out _normal);
}
_mass = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _worldOffsetA, ref _worldOffsetB, ref _normal);
// determine the constraint behavior for this frame
var prevState = _state;
if (Math.Abs(_maxDistance - _minDistance) < 2f * this.Manager.LinearErrorTolerance)
{
_state = LimitState.Equal;
}
else if (_distance <= _minDistance)
{
_state = LimitState.Min;
}
else if (_distance >= _maxDistance)
{
_state = LimitState.Max;
}
else
{
_state = LimitState.Between;
}
if (this.Manager.IsSolverWarmStarted && prevState == _state)
{
Vector3 impulse;
Vector3.Multiply(ref _impulse, this.Manager.TimeStep, out impulse);
b.ApplyImpulse(ref impulse, ref _worldOffsetB);
Vector3.Negate(ref impulse, out impulse);
a.ApplyImpulse(ref impulse, ref _worldOffsetA);
}
else
{
_impulse = Vector3.Zero;
}
_pImpulse = Vector3.Zero;
}
private void DoLinearWarmStart(LimitState prevState, LimitState state, ref float accImpulse, ref Vector3 axis)
{
RigidBody a = this.BodyA, b = this.BodyB;
if (prevState == state)
{
Vector3 i;
Vector3.Multiply(ref axis, accImpulse, out i);
a.ApplyImpulse(ref i, ref _anchorA);
Vector3.Negate(ref i, out i);
b.ApplyImpulse(ref i, ref _anchorB);
}
else
{
accImpulse = 0f;
}
}
/// <summary>
/// Solve the constraint for velocity by applying impulses.
/// </summary>
public override void ProcessVelocity()
{
RigidBody a = BodyA, b = BodyB;
// calculate impulse required to halt motion
Vector3 impulse;
a.GetVelocityAtPoint(ref _worldOffset, out impulse);
Vector3.Negate(ref impulse, out impulse);
Vector3.Transform(ref impulse, ref _mass, out impulse);
// add to accumulated impulse for warm starting (no clamping)
Vector3.Multiply(ref impulse, this.Manager.TimeStepInverse, out impulse);
Vector3.Add(ref _impulse, ref impulse, out _impulse);
// apply impulse
Vector3.Multiply(ref impulse, this.Manager.TimeStep, out impulse);
a.ApplyImpulse(ref impulse, ref _worldOffset);
}
/// <summary>
/// Prepares the constraint for iterative processing in the current frame.
/// </summary>
public override void PreProcess()
{
RigidBody a = BodyA;
// get points and mass in world coordinates
Vector3.Transform(ref _bodyPoint, ref a.World.Combined, out _worldOffset);
Vector3.Subtract(ref _worldOffset, ref a.World.Position, out _worldOffset);
a.MassWorld.InverseMassMatrix(ref _worldOffset, out _mass);
Matrix.Invert(ref _mass, out _mass);
if (this.Manager.IsSolverWarmStarted)
{
Vector3 impulse;
Vector3.Multiply(ref _impulse, this.Manager.TimeStep, out impulse);
a.ApplyImpulse(ref impulse, ref _worldOffset);
}
else
{
_impulse = Vector3.Zero;
}
}
/// <summary>
/// Solve the constraint for velocity by applying impulses.
/// </summary>
public override void ProcessVelocity()
{
RigidBody a = BodyA, b = BodyB;
// calculate impulse required to halt motion at point
Vector3 va, vb, impulse;
a.GetVelocityAtPoint(ref _worldOffsetA, out va);
b.GetVelocityAtPoint(ref _worldOffsetB, out vb);
Vector3.Subtract(ref va, ref vb, out impulse);
Vector3.Transform(ref impulse, ref _mass, out impulse);
// save accumulated impulse for warm starting (no clamping)
Vector3.Multiply(ref impulse, this.Manager.TimeStepInverse, out impulse);
Vector3.Add(ref _impulse, ref impulse, out _impulse);
// apply impulse
Vector3.Multiply(ref impulse, this.Manager.TimeStep, out impulse);
b.ApplyImpulse(ref impulse, ref _worldOffsetB);
Vector3.Negate(ref impulse, out impulse);
a.ApplyImpulse(ref impulse, ref _worldOffsetA);
}
/// <summary>
/// Solve the constraint for velocity by applying impulses.
/// </summary>
public override void ProcessVelocity()
{
RigidBody a = BodyA, b = BodyB;
if (_state == LimitState.Between)
{
return;
}
// calculate impulse required to zero velocity along normal
float impulseMag;
Vector3 va, vb, impulse;
a.GetVelocityAtPoint(ref _worldOffsetA, out va);
b.GetVelocityAtPoint(ref _worldOffsetB, out vb);
Vector3.Subtract(ref va, ref vb, out va);
Vector3.Dot(ref va, ref _normal, out impulseMag);
Vector3.Multiply(ref _normal, impulseMag * _mass * this.Manager.TimeStepInverse, out impulse);
// clamp the impulse appropriately for the current state
float d;
Vector3 oldImpulse = _impulse;
Vector3.Add(ref _impulse, ref impulse, out _impulse);
Vector3.Dot(ref _impulse, ref _normal, out d);
if (_state == LimitState.Min && d > 0f || _state == LimitState.Max && d < 0f)
{
_impulse = Vector3.Zero;
}
Vector3.Subtract(ref _impulse, ref oldImpulse, out impulse);
// apply impulse
Vector3.Multiply(ref impulse, this.Manager.TimeStep, out impulse);
b.ApplyImpulse(ref impulse, ref _worldOffsetB);
Vector3.Negate(ref impulse, out impulse);
a.ApplyImpulse(ref impulse, ref _worldOffsetA);
}
/// <summary>
/// Prepare the contact constraint for iterative processing within a single frame. Computes the desired target velocities
/// to attempt to prevent inter-penetration.
/// </summary>
public override void PreProcess()
{
if (this.IsCollisionSuppressed)
{
return;
}
RigidBody a = BodyA, b = BodyB;
var cached = b.Manager.ContactCache.Get(a, b);
bool isWarmStarted = cached != null && cached.Count == _count;
// calculate relative force applied during this frame
Vector3 va = Vector3.Zero, vb = Vector3.Zero;
float forceMag;
if (a.IsMovable)
{
Vector3.Multiply(ref a.Force, this.Manager.TimeStep * a.Mass.MassInverse, out va);
}
if (b.IsMovable)
{
Vector3.Multiply(ref b.Force, this.Manager.TimeStep * b.Mass.MassInverse, out vb);
}
Vector3.Add(ref va, ref vb, out va);
Vector3.Dot(ref _normal, ref va, out forceMag);
forceMag = MathHelper.Min(forceMag, 0f);
for (int i = 0; i < _count; i++)
{
var p = _points[i];
// calculate movement along normal and tangent
float normalDelta;
a.GetVelocityAtPoint(ref p.OffsetA, out va);
b.GetVelocityAtPoint(ref p.OffsetB, out vb);
Vector3.Subtract(ref va, ref vb, out va);
Vector3.Dot(ref _normal, ref va, out normalDelta);
float tangentDelta;
Vector3.Multiply(ref _normal, normalDelta, out p.Tangent);
Vector3.Subtract(ref va, ref p.Tangent, out p.Tangent);
Vector3.Dot(ref p.Tangent, ref va, out tangentDelta);
if (p.Tangent.LengthSquared() >= Constants.Epsilon)
{
p.Tangent.Normalize();
Vector3.Negate(ref p.Tangent, out p.Tangent);
}
else
{
p.Tangent = Vector3.Zero;
}
// calculate effective mass along tangent and normal
p.NormalMass = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref p.OffsetA, ref p.OffsetB, ref _normal);
p.TangentMass = p.Tangent != Vector3.Zero ?
MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref p.OffsetA, ref p.OffsetB, ref p.Tangent) : 0f;
// calculate target velocity
float restitution = Math.Max(_restitution * -(normalDelta - forceMag), 0f);
float penetration = (p.Depth - this.Manager.LinearErrorTolerance);
if (restitution < this.Manager.MinRestitution)
{
if (penetration > 0f)
{
p.Target = penetration * this.Manager.PenetrationBias;
}
else
{
float scale = MathHelper.Clamp(-0.1f * penetration / this.Manager.LinearErrorTolerance, Constants.Epsilon, 1f);
p.Target = scale * (p.Depth - this.Manager.LinearErrorTolerance) * this.Manager.TimeStepInverse;
}
}
else
{
p.Target = Math.Max(Math.Max(penetration * this.Manager.PenetrationBias, 0f), restitution);
}
p.Impulse = 0f;
p.TangentImpulse = 0f;
p.PositionImpulse = 0f;
if (isWarmStarted)
{
Vector3 impulse, tangentImpulse;
// find the best cached point
var bestPoint = new CachedContactPoint();
float bestDistance = float.MaxValue;
for (int j = 0; j < cached.Points.Length; j++)
{
float d1, d2;
Vector3.DistanceSquared(ref cached.Points[j].OffsetA, ref p.OffsetA, out d1);
Vector3.DistanceSquared(ref cached.Points[j].OffsetB, ref p.OffsetB, out d2);
if (d1 + d2 < bestDistance)
{
bestDistance = d1 + d2;
bestPoint = cached.Points[j];
}
}
p.Impulse = bestPoint.NormalImpulse;
float tangentImpulseMag = MathHelper.Clamp(-tangentDelta * p.TangentMass, 0f, _friction * p.Impulse);
p.TangentImpulse = tangentImpulseMag * p.NormalMass;
if (Math.Abs(p.Impulse) >= Constants.Epsilon)
{
Vector3.Multiply(ref _normal, p.Impulse, out impulse);
Vector3.Multiply(ref p.Tangent, p.TangentImpulse, out tangentImpulse);
Vector3.Add(ref impulse, ref tangentImpulse, out impulse);
a.ApplyImpulse(ref impulse, ref p.OffsetA);
Vector3.Negate(ref impulse, out impulse);
b.ApplyImpulse(ref impulse, ref p.OffsetB);
}
}
_points[i] = p;
}
// calculate an averaged contact point for help with stabilization during position correction
if (_count > 2 && _count < _points.Length)
{
var ap = _points[_count];
ap.Depth = float.MaxValue;
for (int i = 0; i < _count; i++)
{
var p = _points[i];
float depth;
Vector3 pa, pb;
Vector3.Add(ref a.World.Position, ref p.OffsetA, out pa);
Vector3.Add(ref b.World.Position, ref p.OffsetB, out pb);
Vector3.Add(ref ap.OffsetA, ref pa, out ap.OffsetA);
Vector3.Add(ref ap.OffsetB, ref pb, out ap.OffsetB);
Vector3.Subtract(ref pb, ref pa, out pa);
Vector3.Dot(ref _normal, ref pa, out depth);
if (depth < ap.Depth)
{
ap.Depth = depth;
}
}
Vector3.Divide(ref ap.OffsetA, _count, out ap.OffsetA);
Vector3.Divide(ref ap.OffsetB, _count, out ap.OffsetB);
ap.NormalMass = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref ap.OffsetA, ref ap.OffsetB, ref _normal);
ap.PositionImpulse = 0f;
_points[_count] = ap;
}
}