import org.jbox2d.common.Mat22; //导入依赖的package包/类
public DefaultWorldPool(int argSize, int argContainerSize) {
  vecs = new OrderedStack<Vec2>(argSize, argContainerSize) {
    protected Vec2 newInstance() { return new Vec2(); }
  };
  vec3s = new OrderedStack<Vec3>(argSize, argContainerSize) {
    protected Vec3 newInstance() { return new Vec3(); }
  };
  mats = new OrderedStack<Mat22>(argSize, argContainerSize) {
    protected Mat22 newInstance() { return new Mat22(); }
  };
  aabbs = new OrderedStack<AABB>(argSize, argContainerSize) {
    protected AABB newInstance() { return new AABB(); }
  };
  rots = new OrderedStack<Rot>(argSize, argContainerSize) {
    protected Rot newInstance() { return new Rot(); }
  };
  mat33s = new OrderedStack<Mat33>(argSize, argContainerSize) {
    protected Mat33 newInstance() { return new Mat33(); }
  };

  dist = new Distance();
  collision = new Collision(this);
  toi = new TimeOfImpact(this);
}
 

import org.jbox2d.common.Mat22; //导入依赖的package包/类
/**
 * Build vertices to represent an oriented box.
 * 
 * @param hx
 *            the half-width.
 * @param hy
 *            the half-height.
 * @param center
 *            the center of the box in local coordinates.
 * @param angle
 *            the rotation of the box in local coordinates.
 */
public final void setAsBox(final float hx, final float hy, final Vec2 center, final float angle) {
	m_vertexCount = 4;
	m_vertices[0].set(-hx, -hy);
	m_vertices[1].set(hx, -hy);
	m_vertices[2].set(hx, hy);
	m_vertices[3].set(-hx, hy);
	m_normals[0].set(0.0f, -1.0f);
	m_normals[1].set(1.0f, 0.0f);
	m_normals[2].set(0.0f, 1.0f);
	m_normals[3].set(-1.0f, 0.0f);
	m_centroid.set(center);
	
	final Transform xf = poolt1;
	xf.position.set(center);
	xf.R.set(angle);
	
	// Transform vertices and normals.
	for (int i = 0; i < m_vertexCount; ++i) {
		Transform.mulToOut(xf, m_vertices[i], m_vertices[i]);
		Mat22.mulToOut(xf.R, m_normals[i], m_normals[i]);
	}
}
 

import org.jbox2d.common.Mat22; //导入依赖的package包/类
public void transformViewport(GL2 gl, Vec2 center) {
  Vec2 e = viewportTransform.getExtents();
  Vec2 vc = viewportTransform.getCenter();
  Mat22 vt = viewportTransform.getMat22Representation();

  int f = viewportTransform.isYFlip() ? -1 : 1;
  mat[0] = vt.ex.x;
  mat[4] = vt.ey.x;
  // mat[8] = 0;
  mat[12] = e.x;
  mat[1] = f * vt.ex.y;
  mat[5] = f * vt.ey.y;
  // mat[9] = 0;
  mat[13] = e.y;
  // mat[2] = 0;
  // mat[6] = 0;
  // mat[10] = 1;
  // mat[14] = 0;
  // mat[3] = 0;
  // mat[7] = 0;
  // mat[11] = 0;
  // mat[15] = 1;

  gl.glMultMatrixf(mat, 0);
  gl.glTranslatef(center.x - vc.x, center.y - vc.y, 0);
}
 

import org.jbox2d.common.Mat22; //导入依赖的package包/类
public void testMat22Unsafes() {
  Vec2 v1 = new Vec2(10, -1.3f);
  Mat22 m1 = new Mat22(1, 34, -3, 3);
  Mat22 m2 = new Mat22(2, -1, 4.1f, -4);
  Vec2 vo = new Vec2();
  Mat22 mo = new Mat22();

  Mat22.mulToOutUnsafe(m1, m2, mo);
  assertEquals(Mat22.mul(m1, m2), mo);

  Mat22.mulToOutUnsafe(m1, v1, vo);
  assertEquals(Mat22.mul(m1, v1), vo);

  Mat22.mulTransToOutUnsafe(m1, m2, mo);
  assertEquals(Mat22.mulTrans(m1, m2), mo);

  Mat22.mulTransToOutUnsafe(m1, v1, vo);
  assertEquals(Mat22.mulTrans(m1, v1), vo);
}
 

import org.jbox2d.common.Mat22; //导入依赖的package包/类
/**
 * Zooms the camera to a point on the screen. The zoom amount is given on camera initialization.
 */
public void zoomToPoint(Vec2 screenPosition, ZoomType zoomType) {
  Mat22 zoom;
  switch (zoomType) {
    case ZOOM_IN:
      zoom = upScale;
      break;
    case ZOOM_OUT:
      zoom = downScale;
      break;
    default:
      Preconditions.checkArgument(false, "Zoom type invalid");
      return;
  }

  transform.getScreenToWorld(screenPosition, oldCenter);
  transform.mulByTransform(zoom);
  transform.getScreenToWorld(screenPosition, newCenter);

  Vec2 transformedMove = oldCenter.subLocal(newCenter);
  // set, just in case bad impl by someone
  if (!transform.isYFlip()) {
    transformedMove.y = -transformedMove.y;
  }
  transform.setCenter(transform.getCenter().addLocal(transformedMove));
}
 

import org.jbox2d.common.Mat22; //导入依赖的package包/类
public void transformViewport(GL2 gl, Vec2 center) {
    Vec2 e = viewportTransform.getExtents();
    Vec2 vc = viewportTransform.getCenter();
    Mat22 vt = viewportTransform.getMat22Representation();
    
    int f = viewportTransform.isYFlip() ? -1 : 1;
    mat[0] = vt.ex.x;
    mat[4] = vt.ey.x;
    // mat[8] = 0;
    mat[12] = e.x;
    mat[1] = f * vt.ex.y;
    mat[5] = f * vt.ey.y;
    // mat[9] = 0;
    mat[13] = e.y;
    // mat[2] = 0;
    // mat[6] = 0;
    // mat[10] = 1;
    // mat[14] = 0;
    // mat[3] = 0;
    // mat[7] = 0;
    // mat[11] = 0;
    // mat[15] = 1;

    gl.glMultMatrixf(mat, 0);
    gl.glTranslatef(center.x - vc.x, center.y - vc.y, 0);
}
 

import org.jbox2d.common.Mat22; //导入依赖的package包/类
/**
 * Build vertices to represent an oriented box.
 * 
 * @param hx
 *            the half-width.
 * @param hy
 *            the half-height.
 * @param center
 *            the center of the box in local coordinates.
 * @param angle
 *            the rotation of the box in local coordinates.
 */
public final void setAsBox(final float hx, final float hy,
		final Vec2 center, final float angle) {
	m_vertexCount = 4;
	m_vertices[0].set(-hx, -hy);
	m_vertices[1].set(hx, -hy);
	m_vertices[2].set(hx, hy);
	m_vertices[3].set(-hx, hy);
	m_normals[0].set(0.0f, -1.0f);
	m_normals[1].set(1.0f, 0.0f);
	m_normals[2].set(0.0f, 1.0f);
	m_normals[3].set(-1.0f, 0.0f);
	m_centroid.set(center);

	final Transform xf = poolt1;
	xf.position.set(center);
	xf.R.set(angle);

	// Transform vertices and normals.
	for (int i = 0; i < m_vertexCount; ++i) {
		Transform.mulToOut(xf, m_vertices[i], m_vertices[i]);
		Mat22.mulToOut(xf.R, m_normals[i], m_normals[i]);
	}
}
 

import org.jbox2d.common.Mat22; //导入依赖的package包/类
@Override
  public void solveVelocityConstraints(final SolverData data) {

    Vec2 vB = data.velocities[m_indexB].v;
    float wB = data.velocities[m_indexB].w;

    // Cdot = v + cross(w, r)
    final Vec2 Cdot = pool.popVec2();
    Vec2.crossToOutUnsafe(wB, m_rB, Cdot);
    Cdot.addLocal(vB);

    final Vec2 impulse = pool.popVec2();
    final Vec2 temp = pool.popVec2();

    temp.set(m_impulse).mulLocal(m_gamma).addLocal(m_C).addLocal(Cdot).negateLocal();
    Mat22.mulToOutUnsafe(m_mass, temp, impulse);

    Vec2 oldImpulse = temp;
    oldImpulse.set(m_impulse);
    m_impulse.addLocal(impulse);
    float maxImpulse = data.step.dt * m_maxForce;
    if (m_impulse.lengthSquared() > maxImpulse * maxImpulse) {
      m_impulse.mulLocal(maxImpulse / m_impulse.length());
    }
    impulse.set(m_impulse).subLocal(oldImpulse);

    vB.x += m_invMassB * impulse.x;
    vB.y += m_invMassB * impulse.y;
    wB += m_invIB * Vec2.cross(m_rB, impulse);

//    data.velocities[m_indexB].v.set(vB);
    data.velocities[m_indexB].w = wB;
    
    pool.pushVec2(3);
  }
 

import org.jbox2d.common.Mat22; //导入依赖的package包/类
/**
 * Find the separation between poly1 and poly2 for a given edge normal on poly1.
 * 
 * @param poly1
 * @param xf1
 * @param edge1
 * @param poly2
 * @param xf2
 */
public final float edgeSeparation(final PolygonShape poly1, final Transform xf1, final int edge1,
		final PolygonShape poly2, final Transform xf2) {
	
	int count1 = poly1.m_vertexCount;
	final Vec2[] vertices1 = poly1.m_vertices;
	final Vec2[] normals1 = poly1.m_normals;
	
	int count2 = poly2.m_vertexCount;
	final Vec2[] vertices2 = poly2.m_vertices;
	
	assert (0 <= edge1 && edge1 < count1);
	
	// Convert normal from poly1's frame into poly2's frame.
	// Vec2 normal1World = Mul(xf1.R, normals1[edge1]);
	Mat22.mulToOut(xf1.R, normals1[edge1], normal1World);
	// Vec2 normal1 = MulT(xf2.R, normal1World);
	Mat22.mulTransToOut(xf2.R, normal1World, normal1);
	
	// Find support vertex on poly2 for -normal.
	int index = 0;
	float minDot = Float.MAX_VALUE;
	
	for (int i = 0; i < count2; ++i) {
		float dot = Vec2.dot(vertices2[i], normal1);
		if (dot < minDot) {
			minDot = dot;
			index = i;
		}
	}
	
	// Vec2 v1 = Mul(xf1, vertices1[edge1]);
	// Vec2 v2 = Mul(xf2, vertices2[index]);
	Transform.mulToOut(xf1, vertices1[edge1], v1);
	Transform.mulToOut(xf2, vertices2[index], v2);
	
	float separation = Vec2.dot(v2.subLocal(v1), normal1World);
	return separation;
}
 

import org.jbox2d.common.Mat22; //导入依赖的package包/类
/**
 * @see Shape#testPoint(Transform, Vec2)
 */
@Override
public final boolean testPoint(final Transform transform, final Vec2 p) {
	final Vec2 center = pool1;
	Mat22.mulToOut(transform.R, m_p, center);
	center.addLocal(transform.position);
	
	final Vec2 d = center.subLocal(p).negateLocal();
	return Vec2.dot(d, d) <= m_radius * m_radius;
}
 

import org.jbox2d.common.Mat22; //导入依赖的package包/类
/**
 * @see Shape#raycast(org.jbox2d.collision.RayCastOutput,
 *      org.jbox2d.collision.RayCastInput, org.jbox2d.common.Transform, int)
 */
@Override
public final boolean raycast(RayCastOutput argOutput, RayCastInput argInput, Transform argTransform) {
	
	final Vec2 position = pool1;
	final Vec2 s = pool2;
	final Vec2 r = pool3;
	
	Mat22.mulToOut(argTransform.R, m_p, position);
	position.addLocal(argTransform.position);
	s.set(argInput.p1).subLocal(position);
	final float b = Vec2.dot(s, s) - m_radius * m_radius;
	
	// Solve quadratic equation.
	r.set(argInput.p2).subLocal(argInput.p1);
	final float c = Vec2.dot(s, r);
	final float rr = Vec2.dot(r, r);
	final float sigma = c * c - rr * b;
	
	// Check for negative discriminant and short segment.
	if (sigma < 0.0f || rr < Settings.EPSILON) {
		return false;
	}
	
	// Find the point of intersection of the line with the circle.
	float a = -(c + MathUtils.sqrt(sigma));
	
	// Is the intersection point on the segment?
	if (0.0f <= a && a <= argInput.maxFraction * rr) {
		a /= rr;
		argOutput.fraction = a;
		argOutput.normal.set(r).mulLocal(a);
		argOutput.normal.addLocal(s);
		argOutput.normal.normalize();
		return true;
	}
	
	return false;
}
 

import org.jbox2d.common.Mat22; //导入依赖的package包/类
/**
 * @see org.jbox2d.collision.shapes.Shape#computeAABB(org.jbox2d.collision.AABB,
 *      org.jbox2d.common.Transform, int)
 */
@Override
public final void computeAABB(final AABB argAabb, final Transform argTransform) {
	final Vec2 p = pool1;
	Mat22.mulToOut(argTransform.R, m_p, p);
	p.addLocal(argTransform.position);
	
	argAabb.lowerBound.x = p.x - m_radius;
	argAabb.lowerBound.y = p.y - m_radius;
	argAabb.upperBound.x = p.x + m_radius;
	argAabb.upperBound.y = p.y + m_radius;
}
 

import org.jbox2d.common.Mat22; //导入依赖的package包/类
/**
 * @see Shape#testPoint(Transform, Vec2)
 */
@Override
public final boolean testPoint(final Transform xf, final Vec2 p) {
	
	final Vec2 pLocal = pool1;
	
	pLocal.set(p).subLocal(xf.position);
	Mat22.mulTransToOut(xf.R, pLocal, pLocal);
	
	if (m_debug) {
		System.out.println("--testPoint debug--");
		System.out.println("Vertices: ");
		for (int i = 0; i < m_vertexCount; ++i) {
			System.out.println(m_vertices[i]);
		}
		System.out.println("pLocal: " + pLocal);
	}
	
	final Vec2 temp = pool2;
	
	for (int i = 0; i < m_vertexCount; ++i) {
		temp.set(pLocal).subLocal(m_vertices[i]);
		final float dot = Vec2.dot(m_normals[i], temp);
		if (dot > 0.0f) {
			return false;
		}
	}
	
	return true;
}
 

import org.jbox2d.common.Mat22; //导入依赖的package包/类
public ContactConstraint() {
    points = new ContactConstraintPoint[Settings.maxManifoldPoints];
    for (int i = 0; i < Settings.maxManifoldPoints; i++) {
        points[i] = new ContactConstraintPoint();
    }
    pointCount = 0;
    localNormal = new Vec2();
    localPoint = new Vec2();
    normal = new Vec2();
    normalMass = new Mat22();
    K = new Mat22();

}
 

import org.jbox2d.common.Mat22; //导入依赖的package包/类
@Override
public void solveVelocityConstraints(final TimeStep step) {
	final Body b1 = m_bodyA;
	final Body b2 = m_bodyB;
	
	final Vec2 r1 = pool.popVec2();
	final Vec2 r2 = pool.popVec2();
	
	r1.set(m_localAnchor1).subLocal(b1.getLocalCenter());
	r2.set(m_localAnchor2).subLocal(b2.getLocalCenter());
	Mat22.mulToOut(b1.getTransform().R, r1, r1);
	Mat22.mulToOut(b2.getTransform().R, r2, r2);
	
	final Vec2 v1 = pool.popVec2();
	final Vec2 v2 = pool.popVec2();
	
	// Cdot = dot(u, v + cross(w, r))
	Vec2.crossToOut(b1.m_angularVelocity, r1, v1);
	Vec2.crossToOut(b2.m_angularVelocity, r2, v2);
	v1.set(b1.m_linearVelocity).addLocal(b1.m_linearVelocity);
	v2.set(b2.m_linearVelocity).addLocal(b2.m_linearVelocity);
	
	float Cdot = Vec2.dot(m_u, v2.subLocal(v1));
	
	float impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse);
	m_impulse += impulse;
	
	float Px = impulse * m_u.x;
	float Py = impulse * m_u.y;
	b1.m_linearVelocity.x -= b1.m_invMass * Px;
	b1.m_linearVelocity.y -= b1.m_invMass * Py;
	b1.m_angularVelocity -= b1.m_invI * (r1.x * Py - r1.y * Px);// b2Cross(r1, P);
	b2.m_linearVelocity.x += b2.m_invMass * Px;
	b2.m_linearVelocity.y += b2.m_invMass * Py;
	b2.m_angularVelocity += b2.m_invI * (r2.x * Py - r2.y * Px);// b2Cross(r2, P);
	
	pool.pushVec2(4);
}
 

import org.jbox2d.common.Mat22; //导入依赖的package包/类
@Override
public boolean solvePositionConstraints(float baumgarte) {
	if (m_frequencyHz > 0.0f) {
		return true;
	}
	
	final Body b1 = m_bodyA;
	final Body b2 = m_bodyB;
	
	final Vec2 r1 = pool.popVec2();
	final Vec2 r2 = pool.popVec2();
	final Vec2 d = pool.popVec2();
	
	r1.set(m_localAnchor1).subLocal(b1.getLocalCenter());
	r2.set(m_localAnchor2).subLocal(b2.getLocalCenter());
	Mat22.mulToOut(b1.getTransform().R, r1, r1);
	Mat22.mulToOut(b2.getTransform().R, r2, r2);
	
	d.x = b2.m_sweep.c.x + r2.x - b1.m_sweep.c.x - r1.x;
	d.y = b2.m_sweep.c.y + r2.y - b1.m_sweep.c.y - r1.y;
	
	float length = d.normalize();
	float C = length - m_length;
	C = MathUtils.clamp(C, -Settings.maxLinearCorrection, Settings.maxLinearCorrection);
	
	float impulse = -m_mass * C;
	m_u.set(d);
	float Px = impulse * m_u.x;
	float Py = impulse * m_u.y;
	
	b1.m_sweep.c.x -= b1.m_invMass * Px;
	b1.m_sweep.c.y -= b1.m_invMass * Py;
	b1.m_sweep.a -= b1.m_invI * (r1.x * Py - r1.y * Px);// b2Cross(r1, P);
	
	b2.m_sweep.c.x += b2.m_invMass * Px;
	b2.m_sweep.c.y += b2.m_invMass * Py;
	b2.m_sweep.a += b2.m_invI * (r2.x * Py - r2.y * Px);// b2Cross(r2, P);
	
	b1.synchronizeTransform();
	b2.synchronizeTransform();
	
	pool.pushVec2(3);
	
	return MathUtils.abs(C) < Settings.linearSlop;
}
 

import org.jbox2d.common.Mat22; //导入依赖的package包/类
/**
 * @see org.jbox2d.dynamics.joints.Joint#getReactionTorque(float)
 */
@Override
public float getReactionTorque(float inv_dt) {
	
	final Vec2 r = pool.popVec2();
	final Vec2 p = pool.popVec2();
	
	r.set(m_localAnchor2).subLocal(m_bodyB.getLocalCenter());
	Mat22.mulToOut(m_bodyB.getTransform().R, r, r);
	p.set(m_J.linearB).mulLocal(m_impulse);
	float L = m_impulse * m_J.angularB - Vec2.cross(r, p);
	
	pool.pushVec2(2);
	return inv_dt * L;
}
 

import org.jbox2d.common.Mat22; //导入依赖的package包/类
@Override
public void solveVelocityConstraints(TimeStep step) {
	Body b = m_bodyB;

	Vec2 r = pool.popVec2();

	r.set(m_localAnchor).subLocal(b.getLocalCenter());
	Mat22.mulToOut(b.getTransform().R, r, r);
	
	// Cdot = v + cross(w, r)
	Vec2 Cdot = pool.popVec2();
	Vec2.crossToOut(b.m_angularVelocity, r, Cdot);
	Cdot.addLocal(b.m_linearVelocity);
	
	Vec2 impulse = pool.popVec2();
	Vec2 temp = pool.popVec2();
	
	//Mul(m_mass, -(Cdot + m_beta * m_C + m_gamma * m_impulse));
	impulse.set(m_C).mulLocal(m_beta);
	temp.set(m_impulse).mulLocal(m_gamma);
	temp.addLocal(impulse).addLocal(Cdot).mulLocal(-1);
	Mat22.mulToOut(m_mass, temp, impulse);

	Vec2 oldImpulse = temp;
	oldImpulse.set(m_impulse);
	m_impulse.addLocal(impulse);
	float maxImpulse = step.dt * m_maxForce;
	if (m_impulse.lengthSquared() > maxImpulse * maxImpulse){
		m_impulse.mulLocal(maxImpulse / m_impulse.length());
	}
	impulse.set(m_impulse).subLocal(oldImpulse);

	// pooling
	oldImpulse.set(impulse).mulLocal(b.m_invMass);
	b.m_linearVelocity.addLocal(oldImpulse);
	b.m_angularVelocity += b.m_invI * Vec2.cross(r, impulse);
	
	pool.pushVec2(4);
}
 

import org.jbox2d.common.Mat22; //导入依赖的package包/类
public float getJointSpeed() {
	Body b1 = m_bodyA;
	Body b2 = m_bodyB;
	
	final Vec2 r1 = pool.popVec2();
	final Vec2 r2 = pool.popVec2();
	final Vec2 p1 = pool.popVec2();
	final Vec2 p2 = pool.popVec2();
	
	r1.set(m_localAnchor1).subLocal(b1.getLocalCenter());
	r2.set(m_localAnchor2).subLocal(b2.getLocalCenter());
	Mat22.mulToOut(b1.getTransform().R, r1, r1);
	Mat22.mulToOut(b2.getTransform().R, r2, r2);
	
	p1.set(b1.m_sweep.c).addLocal(r1);
	p2.set(b2.m_sweep.c).addLocal(r2);
	p2.subLocal(p1);
	
	final Vec2 axis = pool.popVec2();
	b1.getWorldPointToOut(m_localXAxis1, axis);
	
	final Vec2 v1 = b1.m_linearVelocity;
	final Vec2 v2 = b2.m_linearVelocity;
	float w1 = b1.m_angularVelocity;
	float w2 = b2.m_angularVelocity;
	
	final Vec2 temp1 = pool.popVec2();
	final Vec2 temp2 = pool.popVec2();
	
	Vec2.crossToOut(w1, r1, temp1);
	Vec2.crossToOut(w2, r2, temp2);
	temp2.addLocal(v2).subLocal(v1).subLocal(temp1);
	float s2 = Vec2.dot(axis, temp2);
	
	Vec2.crossToOut(w1, axis, temp1);
	float speed = Vec2.dot(p2, temp1) + s2;
	
	pool.pushVec2(7);
	return speed;
}
 

import org.jbox2d.common.Mat22; //导入依赖的package包/类
/**
 * @param argWorldPool
 * @param def
 */
public FrictionJoint(IWorldPool argWorldPool, FrictionJointDef def) {
	super(argWorldPool, def);
	m_localAnchorA = new Vec2(def.localAnchorA);
	m_localAnchorB = new Vec2(def.localAnchorB);
	
	m_linearImpulse = new Vec2();
	m_angularImpulse = 0.0f;
	
	m_maxForce = def.maxForce;
	m_maxTorque = def.maxTorque;
	
	m_linearMass = new Mat22();
}