Java Mat22.mulTransToOut方法代码示例

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 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包/类
/**
 * @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 final void findIncidentEdge(final ClipVertex[] c, final PolygonShape poly1, final Transform xf1, int edge1,
		final PolygonShape poly2, final Transform xf2) {
	int count1 = poly1.m_vertexCount;
	final Vec2[] normals1 = poly1.m_normals;
	
	int count2 = poly2.m_vertexCount;
	final Vec2[] vertices2 = poly2.m_vertices;
	final Vec2[] normals2 = poly2.m_normals;
	
	assert (0 <= edge1 && edge1 < count1);
	
	// Get the normal of the reference edge in poly2's frame.
	Mat22.mulToOut(xf1.R, normals1[edge1], normal1); // temporary
	// b2Vec2 normal1 = b2MulT(xf2.R, b2Mul(xf1.R, normals1[edge1]));
	Mat22.mulTransToOut(xf2.R, normal1, normal1);
	
	// Find the incident edge on poly2.
	int index = 0;
	float minDot = Float.MAX_VALUE;
	for (int i = 0; i < count2; ++i) {
		float dot = Vec2.dot(normal1, normals2[i]);
		if (dot < minDot) {
			minDot = dot;
			index = i;
		}
	}
	
	// Build the clip vertices for the incident edge.
	int i1 = index;
	int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
	
	Transform.mulToOut(xf2, vertices2[i1], c[0].v); // = Mul(xf2, vertices2[i1]);
	c[0].id.features.referenceEdge = edge1;
	c[0].id.features.incidentEdge = i1;
	c[0].id.features.incidentVertex = 0;
	
	Transform.mulToOut(xf2, vertices2[i2], c[1].v); // = Mul(xf2, vertices2[i2]);
	c[1].id.features.referenceEdge = edge1;
	c[1].id.features.incidentEdge = i2;
	c[1].id.features.incidentVertex = 1;
}
 

import org.jbox2d.common.Mat22; //导入方法依赖的package包/类
public final void getLocalVectorToOut(Vec2 worldVector, Vec2 out) {
	Mat22.mulTransToOut(m_xf.R, worldVector, out);
}