/*
 * @(#)Matrix3D.java	1.4 98/03/18
 *
 * Copyright (c) 1995-1997 Sun Microsystems, Inc. All Rights Reserved.
 *
 * Sun grants you ("Licensee") a non-exclusive, royalty free, license to use,
 * modify and redistribute this software in source and binary code form,
 * provided that i) this copyright notice and license appear on all copies of
 * the software; and ii) Licensee does not utilize the software in a manner
 * which is disparaging to Sun.
 *
 * This software is provided "AS IS," without a warranty of any kind. ALL
 * EXPRESS OR IMPLIED CONDITIONS, REPRESENTATIONS AND WARRANTIES, INCLUDING ANY
 * IMPLIED WARRANTY OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE OR
 * NON-INFRINGEMENT, ARE HEREBY EXCLUDED. SUN AND ITS LICENSORS SHALL NOT BE
 * LIABLE FOR ANY DAMAGES SUFFERED BY LICENSEE AS A RESULT OF USING, MODIFYING
 * OR DISTRIBUTING THE SOFTWARE OR ITS DERIVATIVES. IN NO EVENT WILL SUN OR ITS
 * LICENSORS BE LIABLE FOR ANY LOST REVENUE, PROFIT OR DATA, OR FOR DIRECT,
 * INDIRECT, SPECIAL, CONSEQUENTIAL, INCIDENTAL OR PUNITIVE DAMAGES, HOWEVER
 * CAUSED AND REGARDLESS OF THE THEORY OF LIABILITY, ARISING OUT OF THE USE OF
 * OR INABILITY TO USE SOFTWARE, EVEN IF SUN HAS BEEN ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGES.
 *
 * This software is not designed or intended for use in on-line control of
 * aircraft, air traffic, aircraft navigation or aircraft communications; or in
 * the design, construction, operation or maintenance of any nuclear
 * facility. Licensee represents and warrants that it will not use or
 * redistribute the Software for such purposes.
 *
 * Modified by hqm@ai.mit.edu for IAppli fixed point math lib
 */

/** A fairly conventional 3D matrix object that can transform sets of
    3D points and perform a variety of manipulations on the transform */


package com.beartronics;

public class Matrix3D {
    public static final int ONE = 1<<16;
    int xx, xy, xz, xo;
    int yx, yy, yz, yo;
    int zx, zy, zz, zo;

    /** Create a new unit matrix */
    public Matrix3D () {
	xx = ONE;
	yy = ONE;
	zz = ONE;
    }
    /** Scale by f in all dimensions */
    public void scale(int f) {
	xx = FP.Mul(xx, f);
	xy = FP.Mul(xy, f);
	xz = FP.Mul(xz, f);
	xo = FP.Mul(xo, f);
	yx = FP.Mul(yx, f);
	yy = FP.Mul(yy, f);
	yz = FP.Mul(yz, f);
	yo = FP.Mul(yo, f);
	zx = FP.Mul(zx, f);
	zy = FP.Mul(zy, f);
	zz = FP.Mul(zz, f);
	zo = FP.Mul(zo, f);
    }
    /** Scale along each axis independently */
    public void scale(int xf, int yf, int zf) {
	xx = FP.Mul(xx, xf);
	xy = FP.Mul(xy, xf);
	xz = FP.Mul(xz, xf);
	xo = FP.Mul(xo, xf);
	yx = FP.Mul(yx, yf);
	yy = FP.Mul(yy, yf);
	yz = FP.Mul(yz, yf);
	yo = FP.Mul(yo, yf);
	zx = FP.Mul(zx, zf);
	zy = FP.Mul(zy, zf);
	zz = FP.Mul(zz, zf);
	zo = FP.Mul(zo, zf);
    }
    /** Translate the origin */
    public void translate(int x, int y, int z) {
	xo += x;
	yo += y;
	zo += z;
    }
    /** rotate theta degrees about the y axis */
    public void yrot(int theta) {
	theta = FP.Mul(FP.intToFP(theta), FP.PI_OVER_180);
	int ct = FP.Cos(theta);
	int st = FP.Sin(theta);

	int Nxx =  (FP.Mul(xx, ct) + FP.Mul(zx, st));
	int Nxy =  (FP.Mul(xy, ct) + FP.Mul(zy, st));
	int Nxz =  (FP.Mul(xz, ct) + FP.Mul(zz, st));
	int Nxo =  (FP.Mul(xo, ct) + FP.Mul(zo, st));
	
	int Nzx =  (FP.Mul(zx, ct) - FP.Mul(xx, st));
	int Nzy =  (FP.Mul(zy, ct) - FP.Mul(xy, st));
	int Nzz =  (FP.Mul(zz, ct) - FP.Mul(xz, st));
	int Nzo =  (FP.Mul(zo, ct) - FP.Mul(xo, st));
	
	xo = Nxo;
	xx = Nxx;
	xy = Nxy;
	xz = Nxz;
	zo = Nzo;
	zx = Nzx;
	zy = Nzy;
	zz = Nzz;
    }
    /** rotate theta degrees about the x axis */
    public void xrot(int theta) {
	theta = FP.Mul(FP.intToFP(theta), FP.PI_OVER_180);
	int ct = FP.Cos(theta);
	int st = FP.Sin(theta);

	int Nyx = (int) (FP.Mul(yx, ct) + FP.Mul(zx, st));
	int Nyy = (int) (FP.Mul(yy, ct) + FP.Mul(zy, st));
	int Nyz = (int) (FP.Mul(yz, ct) + FP.Mul(zz, st));
	int Nyo = (int) (FP.Mul(yo, ct) + FP.Mul(zo, st));
	
	int Nzx = (int) (FP.Mul(zx, ct) - FP.Mul(yx, st));
	int Nzy = (int) (FP.Mul(zy, ct) - FP.Mul(yy, st));
	int Nzz = (int) (FP.Mul(zz, ct) - FP.Mul(yz, st));
	int Nzo = (int) (FP.Mul(zo, ct) - FP.Mul(yo, st));
	
	yo = Nyo;
	yx = Nyx;
	yy = Nyy;
	yz = Nyz;
	zo = Nzo;
	zx = Nzx;
	zy = Nzy;
	zz = Nzz;
    }
    /** rotate theta degrees about the z axis */
    public void zrot(int theta) {
	theta = FP.Mul(FP.intToFP(theta), FP.PI_OVER_180);
	int ct = FP.Cos(theta);
	int st = FP.Sin(theta);

	int Nyx = (int) (FP.Mul(yx, ct) + FP.Mul(xx, st));
	int Nyy = (int) (FP.Mul(yy, ct) + FP.Mul(xy, st));
	int Nyz = (int) (FP.Mul(yz, ct) + FP.Mul(xz, st));
	int Nyo = (int) (FP.Mul(yo, ct) + FP.Mul(xo, st));
	
	int Nxx = (int) (FP.Mul(xx, ct) - FP.Mul(yx, st));
	int Nxy = (int) (FP.Mul(xy, ct) - FP.Mul(yy, st));
	int Nxz = (int) (FP.Mul(xz, ct) - FP.Mul(yz, st));
	int Nxo = (int) (FP.Mul(xo, ct) - FP.Mul(yo, st));
	
	yo = Nyo;
	yx = Nyx;
	yy = Nyy;
	yz = Nyz;
	xo = Nxo;
	xx = Nxx;
	xy = Nxy;
	xz = Nxz;
    }
    /** Multiply this matrix by a second: M = M*R */
    public void mult(Matrix3D rhs) {
	int lxx = FP.Mul(xx, rhs.xx) + FP.Mul(yx, rhs.xy) + FP.Mul(zx, rhs.xz);
	int lxy = FP.Mul(xy, rhs.xx) + FP.Mul(yy, rhs.xy) + FP.Mul(zy, rhs.xz);
	int lxz = FP.Mul(xz, rhs.xx) + FP.Mul(yz, rhs.xy) + FP.Mul(zz, rhs.xz);
	int lxo = FP.Mul(xo, rhs.xx) + FP.Mul(yo, rhs.xy) + FP.Mul(zo, rhs.xz) + rhs.xo;

	int lyx = FP.Mul(xx, rhs.yx) + FP.Mul(yx, rhs.yy) + FP.Mul(zx, rhs.yz);
	int lyy = FP.Mul(xy, rhs.yx) + FP.Mul(yy, rhs.yy) + FP.Mul(zy, rhs.yz);
	int lyz = FP.Mul(xz, rhs.yx) + FP.Mul(yz, rhs.yy) + FP.Mul(zz, rhs.yz);
	int lyo = FP.Mul(xo, rhs.yx) + FP.Mul(yo, rhs.yy) + FP.Mul(zo, rhs.yz) + rhs.yo;

	int lzx = FP.Mul(xx, rhs.zx) + FP.Mul(yx, rhs.zy) + FP.Mul(zx, rhs.zz);
	int lzy = FP.Mul(xy, rhs.zx) + FP.Mul(yy, rhs.zy) + FP.Mul(zy, rhs.zz);
	int lzz = FP.Mul(xz, rhs.zx) + FP.Mul(yz, rhs.zy) + FP.Mul(zz, rhs.zz);
	int lzo = FP.Mul(xo, rhs.zx) + FP.Mul(yo, rhs.zy) + FP.Mul(zo, rhs.zz) + rhs.zo;

	xx = lxx;
	xy = lxy;
	xz = lxz;
	xo = lxo;

	yx = lyx;
	yy = lyy;
	yz = lyz;
	yo = lyo;

	zx = lzx;
	zy = lzy;
	zz = lzz;
	zo = lzo;
    }

    /** Reinitialize to the unit matrix */
    public void unit() {
	xo = 0;
	xx = ONE;
	xy = 0;
	xz = 0;
	yo = 0;
	yx = 0;
	yy = ONE;
	yz = 0;
	zo = 0;
	zx = 0;
	zy = 0;
	zz = ONE;
    }
    /** Transform nvert points from v into tv.  v contains the input
        coordinates in inting point.  Three successive entries in
	the array constitute a point.  tv ends up holding the transformed
	points as integers; three successive entries per point */
    public void transform(int v[], int tv[], int nvert) {
	int lxx = xx, lxy = xy, lxz = xz, lxo = xo;
	int lyx = yx, lyy = yy, lyz = yz, lyo = yo;
	int lzx = zx, lzy = zy, lzz = zz, lzo = zo;
	for (int i = nvert * 3; (i -= 3) >= 0;) {
	    int x = v[i];
	    int y = v[i + 1];
	    int z = v[i + 2];
	    tv[i    ] = (int) (FP.Mul(x, lxx) + FP.Mul(y, lxy) + FP.Mul(z, lxz) + lxo);
	    tv[i + 1] = (int) (FP.Mul(x, lyx) + FP.Mul(y, lyy) + FP.Mul(z, lyz) + lyo);
	    tv[i + 2] = (int) (FP.Mul(x, lzx) + FP.Mul(y, lzy) + FP.Mul(z, lzz) + lzo);
	}
    }

    public String toString() {
	return ("[" + xo + "," + xx + "," + xy + "," + xz + ";"
		+ yo + "," + yx + "," + yy + "," + yz + ";"
		+ zo + "," + zx + "," + zy + "," + zz + "]");
    }
}