/// <summary>
/// Solve the constraint for position.
/// </summary>
/// <returns>Returns a value indicating whether the constraint has been satisfied.</returns>
public override bool ProcessPosition()
{
RigidBody a = BodyA, b = BodyB;
// get offsets and distance in world coordinates
Vector3 impulse;
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 impulse);
Vector3.Subtract(ref _worldOffsetA, ref a.World.Position, out _worldOffsetA);
Vector3.Subtract(ref _worldOffsetB, ref b.World.Position, out _worldOffsetB);
float error = impulse.Length();
if (error <= this.Manager.LinearErrorTolerance)
{
return(true);
}
// need normalized direction to calculate effective mass
Vector3 n;
Vector3.Divide(ref impulse, error, out n);
float mass = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _worldOffsetA, ref _worldOffsetB, ref n);
Vector3.Multiply(ref impulse, mass * this.Manager.PositionCorrectionFactor, out impulse);
// apply impulse
b.ApplyFlashImpulse(ref impulse, ref _worldOffsetB);
Vector3.Negate(ref impulse, out impulse);
a.ApplyFlashImpulse(ref impulse, ref _worldOffsetA);
return(false);
}
/// <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 ComputeVitals()
{
RigidBody a = this.BodyA, b = this.BodyB;
// compute relative basis between the two objects in world position
Frame.Transform(ref _bodyBasisA, ref this.BodyA.World, out _worldBasisA);
Frame.Transform(ref _bodyBasisB, ref this.BodyB.World, out _worldBasisB);
Frame rel;
Frame.Subtract(ref _worldBasisA, ref _worldBasisB, out rel);
// compute current positions and angles
Vector3.Subtract(ref _worldBasisA.Origin, ref _worldBasisB.Origin, out _positions);
Matrix m;
_worldBasisB.ToMatrix(out m);
Matrix.Transpose(ref m, out m);
Vector3.Transform(ref _positions, ref m, out _positions);
rel.ComputeEulerAnglesXYZ(out _angles);
// borrowed from Bullet; construct the axes about which we actually restrict rotation
Vector3.Cross(ref _worldBasisB.Z, ref _worldBasisA.X, out _axisY);
Vector3.Cross(ref _axisY, ref _worldBasisB.Z, out _axisX);
Vector3.Cross(ref _worldBasisA.X, ref _axisY, out _axisZ);
_axisX.Normalize();
_axisY.Normalize();
_axisZ.Normalize();
// calculate effective inertia along each axis
Matrix inertia = new Matrix();
if (a.IsMovable)
{
Matrix.Add(ref inertia, ref a.MassWorld.InertiaInverse, out inertia);
}
if (b.IsMovable)
{
Matrix.Add(ref inertia, ref b.MassWorld.InertiaInverse, out inertia);
}
inertia.M44 = 1f;
Matrix.Invert(ref inertia, out inertia);
Vector3 d;
Vector3.Transform(ref _axisX, ref inertia, out d);
Vector3.Dot(ref d, ref _axisX, out _inertia.X);
Vector3.Transform(ref _axisY, ref inertia, out d);
Vector3.Dot(ref d, ref _axisY, out _inertia.Y);
Vector3.Transform(ref _axisZ, ref inertia, out d);
Vector3.Dot(ref d, ref _axisZ, out _inertia.Z);
// calculate effective mass along each axis of basis B
//float w = b.MassWorld.MassInverse / (a.MassWorld.MassInverse = b.MassWorld.MassInverse);
//Vector3.Multiply(ref _worldBasisA.Origin, 1f - w, out _anchorA);
//Vector3.Multiply(ref _worldBasisB.Origin, w, out _anchorB);
//Vector3.Add(ref _anchorA, ref _anchorB, out _anchorB);
//Vector3.Subtract(ref _anchorB, ref a.World.Position, out _anchorA);
//Vector3.Subtract(ref _anchorB, ref b.World.Position, out _anchorB);
Vector3.Subtract(ref _worldBasisA.Origin, ref a.World.Position, out _anchorA);
Vector3.Subtract(ref _worldBasisB.Origin, ref b.World.Position, out _anchorB);
_mass.X = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _anchorA, ref _anchorB, ref _worldBasisB.X);
_mass.Y = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _anchorA, ref _anchorB, ref _worldBasisB.Y);
_mass.Z = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _anchorA, ref _anchorB, ref _worldBasisB.Z);
}
/// <summary>
/// Solve the constraint for position.
/// </summary>
/// <returns>Returns a value indicating whether the constraint has been satisfied.</returns>
public override bool ProcessPosition()
{
RigidBody a = BodyA, b = BodyB;
if (_state == LimitState.Between)
{
return(true);
}
// recalculate vitals
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();
Vector3.Divide(ref _normal, _distance, out _normal);
_mass = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _worldOffsetA, ref _worldOffsetB, ref _normal);
// the error depends on the current limit state
float error = 0f;
switch (_state)
{
case LimitState.Equal:
if (_distance > _maxDistance)
{
error = _distance - _maxDistance;
}
else if (_distance < _minDistance)
{
error = _distance - _minDistance;
}
break;
case LimitState.Min:
error = MathHelper.Min(_distance - _minDistance, 0f);
break;
case LimitState.Max:
error = MathHelper.Max(_distance - _maxDistance, 0f);
break;
}
if (Math.Abs(error) <= this.Manager.LinearErrorTolerance)
{
return(true);
}
// clamp impulse
Vector3 impulse, oldImpulse = _pImpulse;
Vector3.Multiply(ref _normal, error * _mass * this.Manager.PositionCorrectionFactor, out impulse);
Vector3.Add(ref _pImpulse, ref impulse, out _pImpulse);
float d;
Vector3.Dot(ref _normal, ref _pImpulse, out d);
if (_state == LimitState.Min && d > 0f || _state == LimitState.Max && d < 0f)
{
_pImpulse = Vector3.Zero;
}
Vector3.Subtract(ref _pImpulse, ref oldImpulse, out impulse);
// apply impulse
b.ApplyFlashImpulse(ref impulse, ref _worldOffsetB);
Vector3.Negate(ref impulse, out impulse);
a.ApplyFlashImpulse(ref impulse, ref _worldOffsetA);
return(false);
}
/// <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;
}
}
