XForm.java

/*
 * JBox2D - A Java Port of Erin Catto's Box2D
 * 
 * JBox2D homepage: http://jbox2d.sourceforge.net/
 * Box2D homepage: http://www.box2d.org
 * 
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
 * arising from the use of this software.
 * 
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */

package org.jbox2d.common;

/**
 * A transform contains translation and rotation. It is used to represent
 * the position and orientation of rigid frames.
 */
public class XForm {
	/** The translation caused by the transform */
	public Vec2 position;

	/** A matrix representing a rotation */
	public Mat22 R;

	// The identity transform 
	public static XForm identity;

	static{
		XForm.identity = new XForm();
		XForm.identity.setIdentity();
	}

	/** The default constructor. */
	public XForm() {
		position = new Vec2();
		R = new Mat22();
	}

	/** Initialize as a copy of another transform. */
	public XForm(final XForm xf) {
		position = xf.position.clone();
		R = xf.R.clone();
	}

	/** Initialize using a position vector and a rotation matrix. */
	public XForm(final Vec2 _position, final Mat22 _R){
		position = _position.clone();
		R = _R.clone();
	}

	/** Set this to equal another transform. */
	public final XForm set(final XForm xf) {
		position.set(xf.position);
		R.set(xf.R);
		return this;
	}

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

	public final static Vec2 mul(final XForm T, final Vec2 v){
		return new Vec2(T.position.x + T.R.col1.x * v.x + T.R.col2.x * v.y,
		                T.position.y + T.R.col1.y * v.x + T.R.col2.y * v.y);
	}

	/* djm added */
	public final static void mulToOut(final XForm T, final Vec2 v, final Vec2 out){
		final float tempy = T.position.y + T.R.col1.y * v.x + T.R.col2.y * v.y;
		out.x = T.position.x + T.R.col1.x * v.x + T.R.col2.x * v.y;
		out.y = tempy;
	}

	public final static Vec2 mulTrans(final XForm T, final Vec2 v){
		final float v1x = v.x-T.position.x;
		final float v1y = v.y-T.position.y;
		final Vec2 b = T.R.col1;
		final Vec2 b1 = T.R.col2;
		return new Vec2((v1x * b.x + v1y * b.y), (v1x * b1.x + v1y * b1.y));
		//return T.R.mulT(v.sub(T.position));
	}

	public final static void mulTransToOut(final XForm T, final Vec2 v, final Vec2 out){
		final float v1x = v.x-T.position.x;
		final float v1y = v.y-T.position.y;
		final Vec2 b = T.R.col1;
		final Vec2 b1 = T.R.col2;
		final float tempy = v1x * b1.x + v1y * b1.y;
		out.x = v1x * b.x + v1y * b.y;
		out.y = tempy;
	}

	@Override
	public final String toString() {
		String s = "XForm:\n";
		s += "Position: "+position + "\n";
		s += "R: \n"+R+"\n";
		return s;
	}
}