Java Mat22.mulToOut方法代码示例

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包/类
/**
 * 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包/类
/**
 * 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包/类
@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包/类
protected final void synchronizeFixtures() {
	final Transform xf1 = pxf;
	xf1.R.set(m_sweep.a0);
	// xf1.position = m_sweep.c0 - Mul(xf1.R, m_sweep.localCenter);
	Mat22.mulToOut(xf1.R, m_sweep.localCenter, xf1.position);
	xf1.position.mulLocal(-1).addLocal(m_sweep.c0);
	
	BroadPhase broadPhase = m_world.m_contactManager.m_broadPhase;
	for (Fixture f = m_fixtureList; f != null; f = f.m_next) {
		f.synchronize(broadPhase, xf1, m_xf);
	}
}
 

import org.jbox2d.common.Mat22; //导入方法依赖的package包/类
/**
 * @see Shape#raycast(org.jbox2d.structs.collision.RayCastOutput,
 *      org.jbox2d.structs.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包/类
@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.addLocal(b1.m_linearVelocity);
	v2.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包/类
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;
}