using
System;
using
System.Diagnostics;
using
FarseerPhysics.Common;
using
Microsoft.Xna.Framework;
namespace
FarseerPhysics.Dynamics.Joints
{
/// <summary>
/// A prismatic joint. This joint provides one degree of freedom: translation
/// along an axis fixed in body1. Relative rotation is prevented. You can
/// use a joint limit to restrict the range of motion and a joint motor to
/// drive the motion or to model joint friction.
/// </summary>
public
class
FixedPrismaticJoint : Joint
{
private
Mat33 _K;
private
float
_a1, _a2;
private
Vector2 _axis;
private
bool
_enableLimit;
private
bool
_enableMotor;
private
Vector3 _impulse;
private
LimitState _limitState;
private
Vector2 _localXAxis1;
private
Vector2 _localYAxis1;
private
float
_lowerTranslation;
private
float
_maxMotorForce;
private
float
_motorMass;
private
float
_motorSpeed;
private
Vector2 _perp;
private
float
_refAngle;
private
float
_s1, _s2;
private
float
_upperTranslation;
/// <summary>
/// This requires defining a line of
/// motion using an axis and an anchor point. The definition uses local
/// anchor points and a local axis so that the initial configuration
/// can violate the constraint slightly. The joint translation is zero
/// when the local anchor points coincide in world space. Using local
/// anchors and a local axis helps when saving and loading a game.
/// </summary>
/// <param name="body">The body.</param>
/// <param name="worldAnchor">The anchor.</param>
/// <param name="axis">The axis.</param>
public
FixedPrismaticJoint(Body body, Vector2 worldAnchor, Vector2 axis)
:
base
(body)
{
JointType = JointType.FixedPrismatic;
BodyB = BodyA;
LocalAnchorA = worldAnchor;
LocalAnchorB = BodyB.GetLocalPoint(worldAnchor);
_localXAxis1 = axis;
_localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);
_refAngle = BodyB.Rotation;
_limitState = LimitState.Inactive;
}
public
Vector2 LocalAnchorA {
get
;
set
; }
public
Vector2 LocalAnchorB {
get
;
set
; }
public
override
Vector2 WorldAnchorA
{
get
{
return
LocalAnchorA; }
}
public
override
Vector2 WorldAnchorB
{
get
{
return
BodyA.GetWorldPoint(LocalAnchorB); }
set
{ Debug.Assert(
false
,
"You can't set the world anchor on this joint type."
); }
}
/// <summary>
/// Get the current joint translation, usually in meters.
/// </summary>
/// <value></value>
public
float
JointTranslation
{
get
{
Vector2 d = BodyB.GetWorldPoint(LocalAnchorB) - LocalAnchorA;
Vector2 axis = _localXAxis1;
return
Vector2.Dot(d, axis);
}
}
/// <summary>
/// Get the current joint translation speed, usually in meters per second.
/// </summary>
/// <value></value>
public
float
JointSpeed
{
get
{
Transform xf2;
BodyB.GetTransform(
out
xf2);
Vector2 r1 = LocalAnchorA;
Vector2 r2 = MathUtils.Multiply(
ref
xf2.R, LocalAnchorB - BodyB.LocalCenter);
Vector2 p1 = r1;
Vector2 p2 = BodyB.Sweep.C + r2;
Vector2 d = p2 - p1;
Vector2 axis = _localXAxis1;
Vector2 v1 = Vector2.Zero;
Vector2 v2 = BodyB.LinearVelocityInternal;
const
float
w1 = 0.0f;
float
w2 = BodyB.AngularVelocityInternal;
float
speed = Vector2.Dot(d, MathUtils.Cross(w1, axis)) +
Vector2.Dot(axis, v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1));
return
speed;
}
}
/// <summary>
/// Is the joint limit enabled?
/// </summary>
/// <value><c>true</c> if [limit enabled]; otherwise, <c>false</c>.</value>
public
bool
LimitEnabled
{
get
{
return
_enableLimit; }
set
{
Debug.Assert(BodyA.FixedRotation ==
false
,
"Warning: limits does currently not work with fixed rotation"
);
WakeBodies();
_enableLimit = value;
}
}
/// <summary>
/// Get the lower joint limit, usually in meters.
/// </summary>
/// <value></value>
public
float
LowerLimit
{
get
{
return
_lowerTranslation; }
set
{
WakeBodies();
_lowerTranslation = value;
}
}
/// <summary>
/// Get the upper joint limit, usually in meters.
/// </summary>
/// <value></value>
public
float
UpperLimit
{
get
{
return
_upperTranslation; }
set
{
WakeBodies();
_upperTranslation = value;
}
}
/// <summary>
/// Is the joint motor enabled?
/// </summary>
/// <value><c>true</c> if [motor enabled]; otherwise, <c>false</c>.</value>
public
bool
MotorEnabled
{
get
{
return
_enableMotor; }
set
{
WakeBodies();
_enableMotor = value;
}
}
/// <summary>
/// Set the motor speed, usually in meters per second.
/// </summary>
/// <value>The speed.</value>
public
float
MotorSpeed
{
set
{
WakeBodies();
_motorSpeed = value;
}
get
{
return
_motorSpeed; }
}
/// <summary>
/// Set the maximum motor force, usually in N.
/// </summary>
/// <value>The force.</value>
public
float
MaxMotorForce
{
set
{
WakeBodies();
_maxMotorForce = value;
}
}
/// <summary>
/// Get the current motor force, usually in N.
/// </summary>
/// <value></value>
public
float
MotorForce {
get
;
set
; }
public
Vector2 LocalXAxis1
{
get
{
return
_localXAxis1; }
set
{
_localXAxis1 = value;
_localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);
}
}
public
override
Vector2 GetReactionForce(
float
inv_dt)
{
return
inv_dt * (_impulse.X * _perp + (MotorForce + _impulse.Z) * _axis);
}
public
override
float
GetReactionTorque(
float
inv_dt)
{
return
inv_dt * _impulse.Y;
}
internal
override
void
InitVelocityConstraints(
ref
TimeStep step)
{
Body bB = BodyB;
LocalCenterA = Vector2.Zero;
LocalCenterB = bB.LocalCenter;
Transform xf2;
bB.GetTransform(
out
xf2);
Vector2 r1 = LocalAnchorA;
Vector2 r2 = MathUtils.Multiply(
ref
xf2.R, LocalAnchorB - LocalCenterB);
Vector2 d = bB.Sweep.C + r2 -
r1;
InvMassA = 0.0f;
InvIA = 0.0f;
InvMassB = bB.InvMass;
InvIB = bB.InvI;
{
_axis = _localXAxis1;
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
_motorMass = InvMassA + InvMassB + InvIA * _a1 * _a1 + InvIB * _a2 * _a2;
if
(_motorMass > Settings.Epsilon)
{
_motorMass = 1.0f / _motorMass;
}
}
{
_perp = _localYAxis1;
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
float
m1 = InvMassA, m2 = InvMassB;
float
i1 = InvIA, i2 = InvIB;
float
k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float
k12 = i1 * _s1 + i2 * _s2;
float
k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float
k22 = i1 + i2;
float
k23 = i1 * _a1 + i2 * _a2;
float
k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1 =
new
Vector3(k11, k12, k13);
_K.Col2 =
new
Vector3(k12, k22, k23);
_K.Col3 =
new
Vector3(k13, k23, k33);
}
if
(_enableLimit)
{
float
jointTranslation = Vector2.Dot(_axis, d);
if
(Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
_limitState = LimitState.Equal;
}
else
if
(jointTranslation <= _lowerTranslation)
{
if
(_limitState != LimitState.AtLower)
{
_limitState = LimitState.AtLower;
_impulse.Z = 0.0f;
}
}
else
if
(jointTranslation >= _upperTranslation)
{
if
(_limitState != LimitState.AtUpper)
{
_limitState = LimitState.AtUpper;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if
(_enableMotor ==
false
)
{
MotorForce = 0.0f;
}
if
(Settings.EnableWarmstarting)
{
_impulse *= step.dtRatio;
MotorForce *= step.dtRatio;
Vector2 P = _impulse.X * _perp + (MotorForce + _impulse.Z) * _axis;
float
L2 = _impulse.X * _s2 + _impulse.Y + (MotorForce + _impulse.Z) * _a2;
bB.LinearVelocityInternal += InvMassB * P;
bB.AngularVelocityInternal += InvIB * L2;
}
else
{
_impulse = Vector3.Zero;
MotorForce = 0.0f;
}
}
internal
override
void
SolveVelocityConstraints(
ref
TimeStep step)
{
Body bB = BodyB;
Vector2 v1 = Vector2.Zero;
float
w1 = 0.0f;
Vector2 v2 = bB.LinearVelocityInternal;
float
w2 = bB.AngularVelocityInternal;
if
(_enableMotor && _limitState != LimitState.Equal)
{
float
Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
float
impulse = _motorMass * (_motorSpeed - Cdot);
float
oldImpulse = MotorForce;
float
maxImpulse = step.dt * _maxMotorForce;
MotorForce = MathUtils.Clamp(MotorForce + impulse, -maxImpulse, maxImpulse);
impulse = MotorForce - oldImpulse;
Vector2 P = impulse * _axis;
float
L1 = impulse * _a1;
float
L2 = impulse * _a2;
v1 -= InvMassA * P;
w1 -= InvIA * L1;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
Vector2 Cdot1 =
new
Vector2(Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1, w2 - w1);
if
(_enableLimit && _limitState != LimitState.Inactive)
{
float
Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
Vector3 Cdot =
new
Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 f1 = _impulse;
Vector3 df = _K.Solve33(-Cdot);
_impulse += df;
if
(_limitState == LimitState.AtLower)
{
_impulse.Z = Math.Max(_impulse.Z, 0.0f);
}
else
if
(_limitState == LimitState.AtUpper)
{
_impulse.Z = Math.Min(_impulse.Z, 0.0f);
}
Vector2 b = -Cdot1 - (_impulse.Z - f1.Z) *
new
Vector2(_K.Col3.X, _K.Col3.Y);
Vector2 f2r = _K.Solve22(b) +
new
Vector2(f1.X, f1.Y);
_impulse.X = f2r.X;
_impulse.Y = f2r.Y;
df = _impulse - f1;
Vector2 P = df.X * _perp + df.Z * _axis;
float
L2 = df.X * _s2 + df.Y + df.Z * _a2;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
else
{
Vector2 df = _K.Solve22(-Cdot1);
_impulse.X += df.X;
_impulse.Y += df.Y;
Vector2 P = df.X * _perp;
float
L2 = df.X * _s2 + df.Y;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
bB.LinearVelocityInternal = v2;
bB.AngularVelocityInternal = w2;
}
internal
override
bool
SolvePositionConstraints()
{
Body b2 = BodyB;
Vector2 c1 = Vector2.Zero;
float
a1 = 0.0f;
Vector2 c2 = b2.Sweep.C;
float
a2 = b2.Sweep.A;
float
linearError = 0.0f;
bool
active =
false
;
float
C2 = 0.0f;
Mat22 R1 =
new
Mat22(a1);
Mat22 R2 =
new
Mat22(a2);
Vector2 r1 = MathUtils.Multiply(
ref
R1, LocalAnchorA - LocalCenterA);
Vector2 r2 = MathUtils.Multiply(
ref
R2, LocalAnchorB - LocalCenterB);
Vector2 d = c2 + r2 - c1 - r1;
if
(_enableLimit)
{
_axis = MathUtils.Multiply(
ref
R1, _localXAxis1);
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
float
translation = Vector2.Dot(_axis, d);
if
(Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = Math.Abs(translation);
active =
true
;
}
else
if
(translation <= _lowerTranslation)
{
C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop,
-Settings.MaxLinearCorrection, 0.0f);
linearError = _lowerTranslation - translation;
active =
true
;
}
else
if
(translation >= _upperTranslation)
{
C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f,
Settings.MaxLinearCorrection);
linearError = translation - _upperTranslation;
active =
true
;
}
}
_perp = MathUtils.Multiply(
ref
R1, _localYAxis1);
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
Vector3 impulse;
Vector2 C1 =
new
Vector2(Vector2.Dot(_perp, d), a2 - a1 - _refAngle);
linearError = Math.Max(linearError, Math.Abs(C1.X));
float
angularError = Math.Abs(C1.Y);
if
(active)
{
float
m1 = InvMassA, m2 = InvMassB;
float
i1 = InvIA, i2 = InvIB;
float
k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float
k12 = i1 * _s1 + i2 * _s2;
float
k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float
k22 = i1 + i2;
float
k23 = i1 * _a1 + i2 * _a2;
float
k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1 =
new
Vector3(k11, k12, k13);
_K.Col2 =
new
Vector3(k12, k22, k23);
_K.Col3 =
new
Vector3(k13, k23, k33);
Vector3 C =
new
Vector3(-C1.X, -C1.Y, -C2);
impulse = _K.Solve33(C);
}
else
{
float
m1 = InvMassA, m2 = InvMassB;
float
i1 = InvIA, i2 = InvIB;
float
k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float
k12 = i1 * _s1 + i2 * _s2;
float
k22 = i1 + i2;
_K.Col1 =
new
Vector3(k11, k12, 0.0f);
_K.Col2 =
new
Vector3(k12, k22, 0.0f);
Vector2 impulse1 = _K.Solve22(-C1);
impulse.X = impulse1.X;
impulse.Y = impulse1.Y;
impulse.Z = 0.0f;
}
Vector2 P = impulse.X * _perp + impulse.Z * _axis;
float
L2 = impulse.X * _s2 + impulse.Y + impulse.Z * _a2;
c2 += InvMassB * P;
a2 += InvIB * L2;
b2.Sweep.C = c2;
b2.Sweep.A = a2;
b2.SynchronizeTransform();
return
linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
}
}
}