How to use org.jbox2d.common

public final static Transform mulTrans(final Transform A, final Transform B) {
 Transform C = new Transform();
 Rot.mulTransUnsafe(A.q, B.q, C.q);
 pool.set(B.p).subLocal(A.p);
 Rot.mulTransUnsafe(A.q, pool, C.p);
 return C;
}

 protected final void advance(float t) {
  // Advance to the new safe time. This doesn't sync the broad-phase.
  m_sweep.advance(t);
  m_sweep.c.set(m_sweep.c0);
  m_sweep.a = m_sweep.a0;
  m_xf.q.set(m_sweep.a);
  // m_xf.position = m_sweep.c - Mul(m_xf.R, m_sweep.localCenter);
  Rot.mulToOutUnsafe(m_xf.q, m_sweep.localCenter, m_xf.p);
  m_xf.p.mulLocal(-1).addLocal(m_sweep.c);
 }
}

public final static Transform mul(final Transform A, final Transform B) {
 Transform C = new Transform();
 Rot.mulUnsafe(A.q, B.q, C.q);
 Rot.mulToOutUnsafe(A.q, B.p, C.p);
 C.p.addLocal(A.p);
 return C;
}

public final static void mulTransToOut(final Transform A, final Transform B, final Transform out) {
 assert (out != A);
 Rot.mulTrans(A.q, B.q, out.q);
 pool.set(B.p).subLocal(A.p);
 Rot.mulTrans(A.q, pool, out.p);
}

public final static void mulTransToOutUnsafe(final Transform A, final Transform B,
  final Transform out) {
 assert (out != A);
 assert (out != B);
 Rot.mulTransUnsafe(A.q, B.q, out.q);
 pool.set(B.p).subLocal(A.p);
 Rot.mulTransUnsafe(A.q, pool, out.p);
}

 /**
  * takes the screen coordinates and returns the world coordinates.
  * 
  * @param screenX
  * @param screenY
  */
 public Vec2 getScreenToWorld(float screenX, float screenY) {
  Vec2 screen = new Vec2(screenX, screenY);
  viewportTransform.getScreenToWorld(screen, screen);
  return screen;
 }
}

/**
 * takes the screen coordinates and puts the corresponding world coordinates in argWorld.
 * 
 * @param screenX
 * @param screenY
 * @param argWorld
 */
public void getScreenToWorldToOut(float screenX, float screenY, Vec2 argWorld) {
 argWorld.set(screenX, screenY);
 viewportTransform.getScreenToWorld(argWorld, argWorld);
}

/**
 * takes the world coordinate (argWorld) and returns the screen coordinates.
 * 
 * @param argWorld
 */
public Vec2 getWorldToScreen(Vec2 argWorld) {
 Vec2 screen = new Vec2();
 viewportTransform.getWorldToScreen(argWorld, screen);
 return screen;
}

/**
 * Set this based on the position and angle.
 * 
 * @param p
 * @param angle
 */
public final void set(Vec2 p, float angle) {
 this.p.set(p);
 q.set(angle);
}

/**
 * Takes the world coordinates and puts the corresponding screen coordinates in argScreen.
 * 
 * @param worldX
 * @param worldY
 * @param argScreen
 */
public void getWorldToScreenToOut(float worldX, float worldY, Vec2 argScreen) {
 argScreen.set(worldX, worldY);
 viewportTransform.getWorldToScreen(argScreen, argScreen);
}

public final static Mat22 createRotationalTransform(float angle) {
 Mat22 mat = new Mat22();
 final float c = MathUtils.cos(angle);
 final float s = MathUtils.sin(angle);
 mat.ex.x = c;
 mat.ey.x = -s;
 mat.ex.y = s;
 mat.ey.y = c;
 return mat;
}

/**
 * Solve A * x = b, where b is a column vector. This is more efficient than computing the inverse
 * in one-shot cases.
 * 
 * @param b
 * @return
 */
public final Vec2 solve22(Vec2 b) {
 Vec2 x = new Vec2();
 solve22ToOut(b, x);
 return x;
}

/** The default constructor. */
public Transform() {
 p = new Vec2();
 q = new Rot();
}

public void set(OBBViewportTransform vpt) {
 box.center.set(vpt.box.center);
 box.extents.set(vpt.box.extents);
 box.R.set(vpt.box.R);
 yFlip = vpt.yFlip;
}

/** Set this to the identity transform. */
public final void setIdentity() {
 p.setZero();
 q.setIdentity();
}

/**
 * Solve A * x = b, where b is a column vector. This is more efficient than computing the inverse
 * in one-shot cases.
 * 
 * @param b
 * @return
 */
public final Vec3 solve33(Vec3 b) {
 Vec3 x = new Vec3();
 solve33ToOut(b, x);
 return x;
}

/**
 * Return the matrix composed of the absolute values of all elements. djm: fixed double allocation
 * 
 * @return Absolute value matrix
 */
public final Mat22 abs() {
 return new Mat22(MathUtils.abs(ex.x), MathUtils.abs(ey.x), MathUtils.abs(ex.y),
   MathUtils.abs(ey.y));
}

/** Initialize as a copy of another transform. */
public Transform(final Transform xf) {
 p = xf.p.clone();
 q = xf.q.clone();
}

/**
 * takes the screen coordinates (argScreen) and returns the world coordinates
 * 
 * @param argScreen
 */
public Vec2 getScreenToWorld(Vec2 argScreen) {
 Vec2 world = new Vec2();
 viewportTransform.getScreenToWorld(argScreen, world);
 return world;
}

/**
 * Takes the world coordinates and returns the screen coordinates.
 * 
 * @param worldX
 * @param worldY
 */
public Vec2 getWorldToScreen(float worldX, float worldY) {
 Vec2 argScreen = new Vec2(worldX, worldY);
 viewportTransform.getWorldToScreen(argScreen, argScreen);
 return argScreen;
}