context: /*
  * Farseer Physics Engine based on Box2D.XNA port:
  * Copyright (c) 2011 Ian Qvist
  * 
  * Original source Box2D:
  * Copyright (c) 2006-2011 Erin Catto http://www.box2d.org 
  * 
  * This software is provided 'as-is', without any express or implied 
  * warranty.  In no event will the authors be held liable for any damages 
  * arising from the use of this software. 
  * Permission is granted to anyone to use this software for any purpose, 
  * including commercial applications, and to alter it and redistribute it 
  * freely, subject to the following restrictions: 
  * 1. The origin of this software must not be misrepresented; you must not 
  * claim that you wrote the original software. If you use this software 
  * in a product, an acknowledgment in the product documentation would be 
  * appreciated but is not required. 
  * 2. Altered source versions must be plainly marked as such, and must not be 
  * misrepresented as being the original software. 
  * 3. This notice may not be removed or altered from any source distribution. 
  */
  
  using System;
  using System.Diagnostics;
  using FarseerPhysics.Common;
  using Microsoft.Xna.Framework;
  
  namespace FarseerPhysics.Dynamics.Joints
  {
      // Linear constraint (point-to-line)
      // d = p2 - p1 = x2 + r2 - x1 - r1
      // C = dot(perp, d)
      // Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1))
      //      = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2)
      // J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)]
      //
      // Angular constraint
      // C = a2 - a1 + a_initial
      // Cdot = w2 - w1
      // J = [0 0 -1 0 0 1]
      //
      // K = J * invM * JT
      //
      // J = [-a -s1 a s2]
      //     [0  -1  0  1]
      // a = perp
      // s1 = cross(d + r1, a) = cross(p2 - x1, a)
      // s2 = cross(r2, a) = cross(p2 - x2, a)
      // Motor/Limit linear constraint
      // C = dot(ax1, d)
      // Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
      // J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
      // Block Solver
      // We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even
      // when the mass has poor distribution (leading to large torques about the joint anchor points).
      //
      // The Jacobian has 3 rows:
      // J = [-uT -s1 uT s2] // linear
      //     [0   -1   0  1] // angular
      //     [-vT -a1 vT a2] // limit
      //
      // u = perp
      // v = axis
      // s1 = cross(d + r1, u), s2 = cross(r2, u)
      // a1 = cross(d + r1, v), a2 = cross(r2, v)
      // M * (v2 - v1) = JT * df
      // J * v2 = bias
      //
      // v2 = v1 + invM * JT * df
      // J * (v1 + invM * JT * df) = bias
      // K * df = bias - J * v1 = -Cdot
      // K = J * invM * JT
      // Cdot = J * v1 - bias
      //
      // Now solve for f2.
      // df = f2 - f1
      // K * (f2 - f1) = -Cdot
      // f2 = invK * (-Cdot) + f1
      //
      // Clamp accumulated limit impulse.
      // lower: f2(3) = max(f2(3), 0)
      // upper: f2(3) = min(f2(3), 0)
      //
      // Solve for correct f2(1:2)
      // K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1
      //                       = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3)
      // K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2)
      // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
      //
      // Now compute impulse to be applied:
      // df = f2 - f1
  
      /// <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 FSPrismaticJoint : FarseerJoint
      {
          public FVector2 LocalAnchorA;
          public FVector2 LocalAnchorB;
          private FVector2 _localXAxisA;
          private FVector2 _localYAxisA;
          private float m_referenceAngle;
          private FVector3 _impulse;
          private float _motorImpulse;
          private float _lowerTranslation;
          private float _upperTranslation;
          private float _maxMotorForce;
          private float _motorSpeed;
          private bool _enableLimit;
          private bool _enableMotor;
          private LimitState _limitState;
  
          // Solver temp
          private int m_indexA;
          private int m_indexB;
          private FVector2 m_localCenterA;
          private FVector2 m_localCenterB;
          private float m_invMassA;
          private float m_invMassB;
          private float m_invIA;
          private float m_invIB;
          private FVector2 m_axis, m_perp;
          private float m_s1, m_s2;
          private float m_a1, m_a2;
          private Mat33 m_K;
          private float m_motorMass;
  
          internal FSPrismaticJoint()
          {
              JointType = JointType.Prismatic;
          }
  
          /// <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="bodyA">The first body.</param>
          /// <param name="bodyB">The second body.</param>
          /// <param name="localAnchorA">The first body anchor.</param>
          /// <param name="localAnchorB">The second body anchor.</param>
          /// <param name="axis">The axis.</param>
          public FSPrismaticJoint(FSBody bodyA, FSBody bodyB, FVector2 localAnchorA, FVector2 localAnchorB, FVector2 axis)
              : base(bodyA, bodyB)
          {
              JointType = JointType.Prismatic;
  
              LocalAnchorA = localAnchorA;
              LocalAnchorB = localAnchorB;
  
              _localXAxisA = BodyA.GetLocalVector(axis);
              _localXAxisA.Normalize();
              _localYAxisA = MathUtils.Cross(1.0f, _localXAxisA);
              m_referenceAngle = BodyB.Rotation - BodyA.Rotation;
  
              _limitState = LimitState.Inactive;
          }
  
          public override FVector2 WorldAnchorA
          {
              get { return BodyA.GetWorldPoint(LocalAnchorA); }
          }
  
          public override FVector2 WorldAnchorB
          {
              get { return BodyB.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
              {
                  FVector2 d = BodyB.GetWorldPoint(LocalAnchorB) - BodyA.GetWorldPoint(LocalAnchorA);
                  FVector2 axis = BodyA.GetWorldVector(ref _localXAxisA);
  
                  return FVector2.Dot(d, axis);
              }
          }
  
          /// <summary>
          /// Get the current joint translation speed, usually in meters per second.
          /// </summary>
          /// <value></value>
          public float JointSpeed
          {
              get
              {
                  Transform xf1, xf2;
                  BodyA.GetTransform(out xf1);
                  BodyB.GetTransform(out xf2);
  
                  FVector2 r1 = MathUtils.Mul(ref xf1.q, LocalAnchorA - BodyA.LocalCenter);
                  FVector2 r2 = MathUtils.Mul(ref xf2.q, LocalAnchorB - BodyB.LocalCenter);
                  FVector2 p1 = BodyA.Sweep.C + r1;
                  FVector2 p2 = BodyB.Sweep.C + r2;
                  FVector2 d = p2 - p1;
                  FVector2 axis = BodyA.GetWorldVector(ref _localXAxisA);
  
                  FVector2 v1 = BodyA.LinearVelocityInternal;
                  FVector2 v2 = BodyB.LinearVelocityInternal;
                  float w1 = BodyA.AngularVelocityInternal;
                  float w2 = BodyB.AngularVelocityInternal;
  
                  float speed = FVector2.Dot(d, MathUtils.Cross(w1, axis)) +
                                FVector2.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 || BodyB.FixedRotation == false,
                               "Warning: limits does currently not work with fixed rotation");
  
                  if (value != _enableLimit)
                  {
                      WakeBodies();
                      _enableLimit = value;
                      _impulse.Z = 0;
                  }
              }
          }
  
          /// <summary>
          /// Get the lower joint limit, usually in meters.
          /// </summary>
          /// <value></value>
          public float LowerLimit
          {
              get { return _lowerTranslation; }
              set
              {
                  if (value != _lowerTranslation)
                  {
                      WakeBodies();
                      _lowerTranslation = value;
                      _impulse.Z = 0.0f;
                  }
              }
          }
  
          /// <summary>
          /// Get the upper joint limit, usually in meters.
          /// </summary>
          /// <value></value>
          public float UpperLimit
          {
              get { return _upperTranslation; }
              set
              {
                  if (value != _upperTranslation)
                  {
                      WakeBodies();
                      _upperTranslation = value;
                      _impulse.Z = 0.0f;
                  }
              }
          }
  
          /// <summary>
          /// Set the joint limits, usually in meters.
          /// </summary>
          /// <param name="lower"></param>
          /// <param name="upper"></param>
          public void SetLimits(float lower, float upper)
          {
              if (upper != _upperTranslation || lower != _lowerTranslation)
              {
                  WakeBodies();
                  _upperTranslation = upper;
                  _lowerTranslation = lower;
                  _impulse.Z = 0.0f;
              }
          }
  
          /// <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
          {
              get { return _maxMotorForce; }
              set
              {
                  WakeBodies();
                  _maxMotorForce = value;
              }
          }
  
          /// <summary>
          /// Get the current motor impulse, usually in N.
          /// </summary>
          /// <value></value>
          public float MotorImpulse
          {
              get { return _motorImpulse; }
              set { _motorImpulse = value; }
          }
  
          public float GetMotorForce(float inv_dt)
          {
              return inv_dt * _motorImpulse;
          }
  
          public FVector2 LocalXAxisA
          {
              get { return _localXAxisA; }
              set
              {
                  _localXAxisA = BodyA.GetLocalVector(value);
                  _localYAxisA = MathUtils.Cross(1.0f, _localXAxisA);
              }
          }
  
          public float ReferenceAngle
          {
              get { return m_referenceAngle; }
              set { m_referenceAngle = value; }
          }
  
          public override FVector2 GetReactionForce(float inv_dt)
          {
              return inv_dt * (_impulse.X * m_perp + (_motorImpulse + _impulse.Z) * m_axis);
          }
  
          public override float GetReactionTorque(float inv_dt)
          {
              return inv_dt * _impulse.Y;
          }
  
          internal override void InitVelocityConstraints(ref SolverData data)
          {
              m_indexA = BodyA.IslandIndex;
              m_indexB = BodyB.IslandIndex;
              m_localCenterA = BodyA.Sweep.LocalCenter;
              m_localCenterB = BodyB.Sweep.LocalCenter;
              m_invMassA = BodyA.InvMass;
              m_invMassB = BodyB.InvMass;
              m_invIA = BodyA.InvI;
              m_invIB = BodyB.InvI;
  
              FVector2 cA = data.positions[m_indexA].c;
              float aA = data.positions[m_indexA].a;
              FVector2 vA = data.velocities[m_indexA].v;
              float wA = data.velocities[m_indexA].w;
  
              FVector2 cB = data.positions[m_indexB].c;
              float aB = data.positions[m_indexB].a;
              FVector2 vB = data.velocities[m_indexB].v;
              float wB = data.velocities[m_indexB].w;
  
              Rot qA = new Rot(aA), qB = new Rot(aB);
  
              // Compute the effective masses.
              FVector2 rA = MathUtils.Mul(qA, LocalAnchorA - m_localCenterA);
              FVector2 rB = MathUtils.Mul(qB, LocalAnchorB - m_localCenterB);
              FVector2 d = (cB - cA) + rB - rA;
  
              float mA = m_invMassA, mB = m_invMassB;
              float iA = m_invIA, iB = m_invIB;
  
              // Compute motor Jacobian and effective mass.
              {
                  m_axis = MathUtils.Mul(qA, LocalXAxisA);
                  m_a1 = MathUtils.Cross(d + rA, m_axis);
                  m_a2 = MathUtils.Cross(rB, m_axis);
  
                  m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
                  if (m_motorMass > 0.0f)
                  {
                      m_motorMass = 1.0f / m_motorMass;
                  }
              }
  
              // Prismatic constraint.
              {
                  m_perp = MathUtils.Mul(qA, _localYAxisA);
  
                  m_s1 = MathUtils.Cross(d + rA, m_perp);
                  m_s2 = MathUtils.Cross(rB, m_perp);
  
                  float k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
                  float k12 = iA * m_s1 + iB * m_s2;
                  float k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2;
                  float k22 = iA + iB;
                  if (k22 == 0.0f)
                  {
                      // For bodies with fixed rotation.
                      k22 = 1.0f;
                  }
                  float k23 = iA * m_a1 + iB * m_a2;
                  float k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
  
                  m_K.ex = new FVector3(k11, k12, k13);
                  m_K.ey = new FVector3(k12, k22, k23);
                  m_K.ez = new FVector3(k13, k23, k33);
              }
  
              // Compute motor and limit terms.
              if (_enableLimit)
              {
                  float jointTranslation = FVector2.Dot(m_axis, d);
                  if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * FSSettings.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)
              {
                  _motorImpulse = 0.0f;
              }
  
              if (FSSettings.EnableWarmstarting)
              {
                  // Account for variable time step.
                  _impulse *= data.step.dtRatio;
                  _motorImpulse *= data.step.dtRatio;
  
                  FVector2 P = _impulse.X * m_perp + (_motorImpulse + _impulse.Z) * m_axis;
                  float LA = _impulse.X * m_s1 + _impulse.Y + (_motorImpulse + _impulse.Z) * m_a1;
                  float LB = _impulse.X * m_s2 + _impulse.Y + (_motorImpulse + _impulse.Z) * m_a2;
  
                  vA -= mA * P;
                  wA -= iA * LA;
  
                  vB += mB * P;
                  wB += iB * LB;
  
              }
              else
              {
                  _impulse = FVector3.Zero;
                  _motorImpulse = 0.0f;
              }
  
              data.velocities[m_indexA].v = vA;
              data.velocities[m_indexA].w = wA;
              data.velocities[m_indexB].v = vB;
              data.velocities[m_indexB].w = wB;
          }
  
          internal override void SolveVelocityConstraints(ref SolverData data)
          {
              FVector2 vA = data.velocities[m_indexA].v;
              float wA = data.velocities[m_indexA].w;
              FVector2 vB = data.velocities[m_indexB].v;
              float wB = data.velocities[m_indexB].w;
  
              float mA = m_invMassA, mB = m_invMassB;
              float iA = m_invIA, iB = m_invIB;
  
              // Solve linear motor constraint.
              if (_enableMotor && _limitState != LimitState.Equal)
              {
                  float Cdot = FVector2.Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;
                  float impulse = m_motorMass * (_motorSpeed - Cdot);
                  float oldImpulse = _motorImpulse;
                  float maxImpulse = data.step.dt * _maxMotorForce;
                  _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
                  impulse = _motorImpulse - oldImpulse;
  
                  FVector2 P = impulse * m_axis;
                  float LA = impulse * m_a1;
                  float LB = impulse * m_a2;
  
                  vA -= mA * P;
                  wA -= iA * LA;
  
                  vB += mB * P;
                  wB += iB * LB;
              }
  
              FVector2 Cdot1 = new FVector2();
              Cdot1.X = FVector2.Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA;
              Cdot1.Y = wB - wA;
  
              if (_enableLimit && _limitState != LimitState.Inactive)
              {
                  // Solve prismatic and limit constraint in block form.
                  float Cdot2;
                  Cdot2 = FVector2.Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;
                  FVector3 Cdot = new FVector3(Cdot1.X, Cdot1.Y, Cdot2);
  
                  FVector3 f1 = _impulse;
                  FVector3 df = m_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);
                  }
  
                  // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
                  FVector2 b = -Cdot1 - (_impulse.Z - f1.Z) * new FVector2(m_K.ez.X, m_K.ez.Y);
  
gt:                 FVector2 f2r = m_K.Solve22(b) + new FVector2(f1.X, f1.Y);