content: /*
   * FXGL - JavaFX Game Library. The MIT License (MIT).
   * Copyright (c) AlmasB (almaslvl@gmail.com).
   * See LICENSE for details.
   */
  
  package com.almasb.fxgl.physics.box2d.dynamics.joints;
  
  import com.almasb.fxgl.core.math.FXGLMath;
  import com.almasb.fxgl.core.math.Vec2;
  import com.almasb.fxgl.physics.box2d.common.JBoxSettings;
  import com.almasb.fxgl.physics.box2d.common.Rotation;
  import com.almasb.fxgl.physics.box2d.dynamics.SolverData;
  import com.almasb.fxgl.physics.box2d.pooling.IWorldPool;
  
  /**
   * A rope joint enforces a maximum distance between two points on two bodies. It has no other
   * effect. Warning: if you attempt to change the maximum length during the simulation you will get
   * some non-physical behavior. A model that would allow you to dynamically modify the length would
   * have some sponginess, so I chose not to implement it that way. See DistanceJoint if you want to
   * dynamically control length.
   *
   * @author Daniel Murphy
   */
  public final class RopeJoint extends Joint {
      // Solver shared
      private final Vec2 m_localAnchorA = new Vec2();
      private final Vec2 m_localAnchorB = new Vec2();
      private float m_maxLength;
      private float m_length = 0f;
      private float m_impulse = 0f;
  
      // Solver temp
      private int m_indexA;
      private int m_indexB;
      private final Vec2 m_u = new Vec2();
      private final Vec2 m_rA = new Vec2();
      private final Vec2 m_rB = new Vec2();
      private final Vec2 m_localCenterA = new Vec2();
      private final Vec2 m_localCenterB = new Vec2();
      private float m_invMassA;
      private float m_invMassB;
      private float m_invIA;
      private float m_invIB;
      private float m_mass = 0f;
      private LimitState m_state = LimitState.INACTIVE;
  
      RopeJoint(IWorldPool worldPool, RopeJointDef def) {
          super(worldPool, def);
          m_localAnchorA.set(def.localAnchorA);
          m_localAnchorB.set(def.localAnchorB);
  
          m_maxLength = def.maxLength;
      }
  
      @Override
      public void initVelocityConstraints(final SolverData data) {
          m_indexA = m_bodyA.m_islandIndex;
          m_indexB = m_bodyB.m_islandIndex;
          m_localCenterA.set(m_bodyA.m_sweep.localCenter);
          m_localCenterB.set(m_bodyB.m_sweep.localCenter);
          m_invMassA = m_bodyA.m_invMass;
          m_invMassB = m_bodyB.m_invMass;
          m_invIA = m_bodyA.m_invI;
          m_invIB = m_bodyB.m_invI;
  
          Vec2 cA = data.positions[m_indexA].c;
          float aA = data.positions[m_indexA].a;
          Vec2 vA = data.velocities[m_indexA].v;
          float wA = data.velocities[m_indexA].w;
  
          Vec2 cB = data.positions[m_indexB].c;
          float aB = data.positions[m_indexB].a;
          Vec2 vB = data.velocities[m_indexB].v;
          float wB = data.velocities[m_indexB].w;
  
          final Rotation qA = pool.popRot();
          final Rotation qB = pool.popRot();
          final Vec2 temp = pool.popVec2();
  
          qA.set(aA);
          qB.set(aB);
  
          // Compute the effective masses.
          Rotation.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), m_rA);
          Rotation.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), m_rB);
  
          m_u.set(cB).addLocal(m_rB).subLocal(cA).subLocal(m_rA);
  
          m_length = m_u.length();
  
          float C = m_length - m_maxLength;
          if (C > 0.0f) {
              m_state = LimitState.AT_UPPER;
          } else {
              m_state = LimitState.INACTIVE;
          }
  
          pool.pushRot(2);
          pool.pushVec2(1);
  
          if (m_length > JBoxSettings.linearSlop) {
              m_u.mulLocal(1.0f / m_length);
          } else {
              m_u.setZero();
              m_mass = 0.0f;
              m_impulse = 0.0f;
              return;
          }
  
          // Compute effective mass.
          float crA = Vec2.cross(m_rA, m_u);
          float crB = Vec2.cross(m_rB, m_u);
          float invMass = m_invMassA + m_invIA * crA * crA + m_invMassB + m_invIB * crB * crB;
  
          m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
  
          if (data.step.warmStarting) {
              // Scale the impulse to support a variable time step.
              m_impulse *= data.step.dtRatio;
  
              float Px = m_impulse * m_u.x;
              float Py = m_impulse * m_u.y;
              vA.x -= m_invMassA * Px;
              vA.y -= m_invMassA * Py;
              wA -= m_invIA * (m_rA.x * Py - m_rA.y * Px);
  
              vB.x += m_invMassB * Px;
              vB.y += m_invMassB * Py;
              wB += m_invIB * (m_rB.x * Py - m_rB.y * Px);
          } else {
              m_impulse = 0.0f;
          }
  
          data.velocities[m_indexA].w = wA;
          data.velocities[m_indexB].w = wB;
      }
  
      @Override
      public void solveVelocityConstraints(final SolverData data) {
          Vec2 vA = data.velocities[m_indexA].v;
          float wA = data.velocities[m_indexA].w;
          Vec2 vB = data.velocities[m_indexB].v;
          float wB = data.velocities[m_indexB].w;
  
          // Cdot = dot(u, v + cross(w, r))
          Vec2 vpA = pool.popVec2();
          Vec2 vpB = pool.popVec2();
          Vec2 temp = pool.popVec2();
  
          Vec2.crossToOutUnsafe(wA, m_rA, vpA);
          vpA.addLocal(vA);
          Vec2.crossToOutUnsafe(wB, m_rB, vpB);
          vpB.addLocal(vB);
  
          float C = m_length - m_maxLength;
          float Cdot = Vec2.dot(m_u, temp.set(vpB).subLocal(vpA));
  
          // Predictive constraint.
          if (C < 0.0f) {
              Cdot += data.step.inv_dt * C;
          }
  
          float impulse = -m_mass * Cdot;
          float oldImpulse = m_impulse;
          m_impulse = Math.min(0.0f, m_impulse + impulse);
          impulse = m_impulse - oldImpulse;
  
          float Px = impulse * m_u.x;
          float Py = impulse * m_u.y;
          vA.x -= m_invMassA * Px;
          vA.y -= m_invMassA * Py;
          wA -= m_invIA * (m_rA.x * Py - m_rA.y * Px);
          vB.x += m_invMassB * Px;
          vB.y += m_invMassB * Py;
          wB += m_invIB * (m_rB.x * Py - m_rB.y * Px);
  
          pool.pushVec2(3);
  
          data.velocities[m_indexA].w = wA;
          data.velocities[m_indexB].w = wB;
      }
  
      @Override
      public boolean solvePositionConstraints(final SolverData data) {
          Vec2 cA = data.positions[m_indexA].c;
          float aA = data.positions[m_indexA].a;
          Vec2 cB = data.positions[m_indexB].c;
          float aB = data.positions[m_indexB].a;
  
          final Rotation qA = pool.popRot();
          final Rotation qB = pool.popRot();
          final Vec2 u = pool.popVec2();
          final Vec2 rA = pool.popVec2();
          final Vec2 rB = pool.popVec2();
          final Vec2 temp = pool.popVec2();
  
          qA.set(aA);
          qB.set(aB);
  
          // Compute the effective masses.
          Rotation.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA);
          Rotation.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB);
          u.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);
  
          float length = u.getLengthAndNormalize();
          float C = length - m_maxLength;
  
          C = FXGLMath.clamp(C, 0.0f, JBoxSettings.maxLinearCorrection);
  
          float impulse = -m_mass * C;
          float Px = impulse * u.x;
          float Py = impulse * u.y;
  
          cA.x -= m_invMassA * Px;
          cA.y -= m_invMassA * Py;
          aA -= m_invIA * (rA.x * Py - rA.y * Px);
          cB.x += m_invMassB * Px;
          cB.y += m_invMassB * Py;
          aB += m_invIB * (rB.x * Py - rB.y * Px);
  
          pool.pushRot(2);
          pool.pushVec2(4);
  
          data.positions[m_indexA].a = aA;
          data.positions[m_indexB].a = aB;
  
          return length - m_maxLength < JBoxSettings.linearSlop;
      }
  
      @Override
      public void getAnchorA(Vec2 argOut) {
          m_bodyA.getWorldPointToOut(m_localAnchorA, argOut);
      }
  
      @Override
      public void getAnchorB(Vec2 argOut) {
          m_bodyB.getWorldPointToOut(m_localAnchorB, argOut);
      }
  
      @Override
      public void getReactionForce(float inv_dt, Vec2 argOut) {
          argOut.set(m_u).mulLocal(inv_dt).mulLocal(m_impulse);
      }
  
      @Override
      public float getReactionTorque(float inv_dt) {
          return 0f;
      }
  
      public Vec2 getLocalAnchorA() {
          return m_localAnchorA;
      }
  
      public Vec2 getLocalAnchorB() {
          return m_localAnchorB;
      }
  
      public float getMaxLength() {
          return m_maxLength;
      }
  
      public void setMaxLength(float maxLength) {
          this.m_maxLength = maxLength;
      }
  
      public LimitState getLimitState() {
          return m_state;
      }
  }
  