/// <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;
// recalculate points in world coordinates
Vector3 impulse;
Vector3.Transform(ref _bodyPoint, ref a.World.Combined, out _worldOffset);
Vector3.Subtract(ref _worldPoint, ref _worldOffset, out impulse);
Vector3.Subtract(ref _worldOffset, ref a.World.Position, out _worldOffset);
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 = a.MassWorld.EffectiveMass(ref _worldOffset, ref n);
Vector3.Multiply(ref impulse, mass * this.Manager.PositionCorrectionFactor, out impulse);
// apply impulse
a.ApplyFlashImpulse(ref impulse, ref _worldOffset);
return(false);
}
/// <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);
}
private bool SolveLinearPosition(LimitState state, float pos, float minPos, float maxPos, ref Vector3 axis, ref Vector3 accImpulse, float mass)
{
RigidBody a = this.BodyA, b = this.BodyB;
float error = 0f;
switch (state)
{
case LimitState.Inactive:
return(true);
case LimitState.Locked:
if (pos < minPos)
{
error = minPos - pos;
}
else if (pos > maxPos)
{
error = maxPos - pos;
}
else
{
error = 0f;
}
break;
case LimitState.Min:
error = minPos - pos;
break;
case LimitState.Max:
error = maxPos - pos;
break;
}
if (Math.Abs(error) <= this.Manager.LinearErrorTolerance)
{
return(true);
}
Vector3 impulse, oldImpulse = accImpulse;
Vector3.Multiply(ref axis, error * mass * this.Manager.PositionCorrectionFactor, out impulse);
Vector3.Add(ref accImpulse, ref impulse, out accImpulse);
float d;
Vector3.Dot(ref axis, ref accImpulse, out d);
if (state == LimitState.Min && d < 0f || state == LimitState.Max && d > 0f)
{
accImpulse = Vector3.Zero;
}
Vector3.Subtract(ref accImpulse, ref oldImpulse, out impulse);
a.ApplyFlashImpulse(ref impulse, ref _anchorA);
Vector3.Negate(ref impulse, out impulse);
b.ApplyFlashImpulse(ref impulse, ref _anchorB);
return(false);
}
/// <summary>
/// Solve the constraint for position by applying flash impulses that modify each bodies' position.
/// </summary>
/// <returns>Returns a value indicating whether the constraint has been satisfied.</returns>
public override bool ProcessPosition()
{
if (this.IsCollisionSuppressed)
{
return(true);
}
RigidBody a = BodyA, b = BodyB;
bool satisfied = true;
for (int i = (_count > 2 && _count < _points.Length ? _count : _count - 1); i >= 0; 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.Subtract(ref pb, ref pa, out pa);
Vector3.Dot(ref _normal, ref pa, out depth);
if (Math.Abs(depth) > 2f * this.Manager.LinearErrorTolerance)
{
satisfied = false;
}
else if (depth <= this.Manager.LinearErrorTolerance)
{
continue;
}
float impulseMag = Math.Max(depth - this.Manager.LinearErrorTolerance, 0f)
* this.Manager.PositionCorrectionFactor * p.NormalMass;
float oldImpulseMag = p.PositionImpulse;
p.PositionImpulse = MathHelper.Max(oldImpulseMag + impulseMag, 0f);
impulseMag = p.PositionImpulse - oldImpulseMag;
Vector3 impulse;
Vector3.Multiply(ref _normal, impulseMag, out impulse);
a.ApplyFlashImpulse(ref impulse, ref p.OffsetA);
Vector3.Negate(ref impulse, out impulse);
b.ApplyFlashImpulse(ref impulse, ref p.OffsetB);
}
return(satisfied);
}
/// <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);
}