/// <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>
/// 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>
/// Apply forces to bodies.
/// </summary>
/// <param name="bodies">The list of bodies to which forces will be applied.</param>
public void Generate(IList <RigidBody> bodies)
{
Vector3 pa, pb, n;
Vector3.Transform(ref _bodyPointA, ref _bodyA.World.Combined, out pa);
Vector3.Transform(ref _bodyPointB, ref _bodyB.World.Combined, out pb);
Vector3.Subtract(ref pa, ref pb, out n);
Vector3.Subtract(ref pa, ref _bodyA.World.Position, out pa);
Vector3.Subtract(ref pb, ref _bodyB.World.Position, out pb);
float dist = n.Length();
Vector3.Divide(ref n, dist, out n);
float speed;
Vector3 va, vb;
_bodyA.GetVelocityAtPoint(ref pa, out va);
_bodyB.GetVelocityAtPoint(ref pb, out vb);
Vector3.Subtract(ref va, ref vb, out va);
Vector3.Dot(ref n, ref va, out speed);
dist -= _length;
Vector3.Multiply(ref n, dist * _k + speed * _c, out n);
_bodyB.ApplyForce(ref n, ref pb);
Vector3.Negate(ref n, out n);
_bodyA.ApplyForce(ref n, ref pa);
if (n.LengthSquared() >= Constants.Epsilon)
{
_bodyA.IsActive = true;
_bodyB.IsActive = true;
}
}
/// <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>
/// Solve the constraint for velocity by applying impulses.
/// </summary>
public override void ProcessVelocity()
{
RigidBody a = this.BodyA, b = this.BodyB;
// solve linear constraints
Vector3 va, vb;
a.GetVelocityAtPoint(ref _anchorA, out va);
b.GetVelocityAtPoint(ref _anchorB, out vb);
Vector3.Subtract(ref va, ref vb, out va);
this.SolveLinearVelocity(_statePosX, ref va, ref _worldBasisB.X, ref _impulsePos.X, _mass.X);
this.SolveLinearVelocity(_statePosY, ref va, ref _worldBasisB.Y, ref _impulsePos.Y, _mass.Y);
this.SolveLinearVelocity(_statePosZ, ref va, ref _worldBasisB.Z, ref _impulsePos.Z, _mass.Z);
// solve angular constraints
Vector3.Subtract(ref a.Velocity.Angular, ref b.Velocity.Angular, out va);
this.SolveAngularVelocity(_stateRotX, ref va, ref _axisX, ref _impulseRot.X, _inertia.X);
this.SolveAngularVelocity(_stateRotY, ref va, ref _axisY, ref _impulseRot.Y, _inertia.Y);
this.SolveAngularVelocity(_stateRotZ, ref va, ref _axisZ, ref _impulseRot.Z, _inertia.Z);
}
/// <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;
}
}