/***************************************************************************
* Copyright (C) 2007 by Abderrahman Taha *
* taha_ab@yahoo.fr *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 2 of the License, or *
* (at your option) any later version. *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with this program; if not, write to the *
* Free Software Foundation, Inc., *
* 51 Franklin Street, Fifth Floor,Boston, MA 02110-1301 USA *
***************************************************************************/
#include "Matrix4D.h"
/** Create a new unit matrix */
Matrix4D::Matrix4D() {
pi = 3.14159265;
xx = 1.0f;
yy = 1.0f;
zz = 1.0f;
ww = 1.0f;
ko = 1.0f;
xy = xz = xw = xo = 0;
yx = yz = yw = yo = 0;
zx = zy = zw = zo = 0;
wx = wy = wz = wo = 0;
kx = ky = kz = kw = 0;
}
void Matrix4D::mult(Matrix4D rhs) {
double lxx = xx * rhs.xx + yx * rhs.xy + zx * rhs.xz + wx * rhs.xw;
double lxy = xy * rhs.xx + yy * rhs.xy + zy * rhs.xz + wy * rhs.xw;
double lxz = xz * rhs.xx + yz * rhs.xy + zz * rhs.xz + wz * rhs.xw;
double lxw = xw * rhs.xx + yw * rhs.xy + zw * rhs.xz + ww * rhs.xw;
double lxo = xo * rhs.xx + yo * rhs.xy + zo * rhs.xz + wo * rhs.xw + rhs.xo;
double lyx = xx * rhs.yx + yx * rhs.yy + zx * rhs.yz + wx * rhs.yw;
double lyy = xy * rhs.yx + yy * rhs.yy + zy * rhs.yz + wy * rhs.yw;
double lyz = xz * rhs.yx + yz * rhs.yy + zz * rhs.yz + wz * rhs.yw;
double lyw = xw * rhs.yx + yw * rhs.yy + zw * rhs.yz + ww * rhs.yw;
double lyo = xo * rhs.yx + yo * rhs.yy + zo * rhs.yz + wo * rhs.yw + rhs.yo;
double lzx = xx * rhs.zx + yx * rhs.zy + zx * rhs.zz + wx * rhs.zw;
double lzy = xy * rhs.zx + yy * rhs.zy + zy * rhs.zz + wy * rhs.zw;
double lzz = xz * rhs.zx + yz * rhs.zy + zz * rhs.zz + wz * rhs.zw;
double lzw = xw * rhs.zx + yw * rhs.zy + zw * rhs.zz + ww * rhs.zw;
double lzo = xo * rhs.zx + yo * rhs.zy + zo * rhs.zz + wo * rhs.zw + rhs.zo;
double lwx = xx * rhs.wx + yx * rhs.wy + zx * rhs.wz + wx * rhs.ww;
double lwy = xy * rhs.wx + yy * rhs.wy + zy * rhs.wz + wy * rhs.ww;
double lwz = xz * rhs.wx + yz * rhs.wy + zz * rhs.wz + wz * rhs.ww;
double lww = xw * rhs.wx + yw * rhs.wy + zw * rhs.wz + ww * rhs.ww;
double lwo = xo * rhs.wx + yo * rhs.wy + zo * rhs.wz + wo * rhs.ww + rhs.wo;
xx = lxx;
xy = lxy;
xz = lxz;
xw = lxw;
xo = lxo;
yx = lyx;
yy = lyy;
yz = lyz;
yw = lyw;
yo = lyo;
zx = lzx;
zy = lzy;
zz = lzz;
zw = lzw;
zo = lzo;
wx = lwx;
wy = lwy;
wz = lwz;
ww = lww;
wo = lwo;
}
/** rotate theta degrees about the yz plan */
void Matrix4D::yzrot(double theta) {
theta *= (pi / 180);
double ct = cos(theta);
double st = sin(theta);
double Nyx = (double) (yx * ct + zx * st);
double Nyy = (double) (yy * ct + zy * st);
double Nyz = (double) (yz * ct + zz * st);
double Nyw = (double) (yw * ct + zw * st);
double Nyo = (double) (yo * ct + zo * st);
double Nzx = (double) (zx * ct - yx * st);
double Nzy = (double) (zy * ct - yy * st);
double Nzz = (double) (zz * ct - yz * st);
double Nzw = (double) (zw * ct - yw * st);
double Nzo = (double) (zo * ct - yo * st);
yo = Nyo;
yx = Nyx;
yy = Nyy;
yz = Nyz;
yw = Nyw;
zo = Nzo;
zx = Nzx;
zy = Nzy;
zz = Nzz;
zw = Nzw;
}
/** rotate theta degrees about the xz plan */
void Matrix4D::xzrot(double theta) {
theta *= (pi / 180);
double ct = cos(theta);
double st = sin(theta);
double Nxx = (double) (xx * ct + zx * st);
double Nxy = (double) (xy * ct + zy * st);
double Nxz = (double) (xz * ct + zz * st);
double Nxw = (double) (xw * ct + zw * st);
double Nxo = (double) (xo * ct + zo * st);
double Nzx = (double) (zx * ct - xx * st);
double Nzy = (double) (zy * ct - xy * st);
double Nzz = (double) (zz * ct - xz * st);
double Nzw = (double) (zw * ct - xw * st);
double Nzo = (double) (zo * ct - xo * st);
xo = Nxo;
xx = Nxx;
xy = Nxy;
xz = Nxz;
xw = Nxw;
zo = Nzo;
zx = Nzx;
zy = Nzy;
zz = Nzz;
zw = Nzw;
}
/** rotate theta degrees about the xy plan */
void Matrix4D::xyrot(double theta) {
theta *= (pi / 180);
double ct = cos(theta);
double st = sin(theta);
double Nyx = (double) (yx * ct + xx * st);
double Nyy = (double) (yy * ct + xy * st);
double Nyz = (double) (yz * ct + xz * st);
double Nyw = (double) (yw * ct + xw * st);
double Nyo = (double) (yo * ct + xo * st);
double Nxx = (double) (xx * ct - yx * st);
double Nxy = (double) (xy * ct - yy * st);
double Nxz = (double) (xz * ct - yz * st);
double Nxw = (double) (xw * ct - yw * st);
double Nxo = (double) (xo * ct - yo * st);
yo = Nyo;
yx = Nyx;
yy = Nyy;
yz = Nyz;
yw = Nyw;
xo = Nxo;
xx = Nxx;
xy = Nxy;
xz = Nxz;
xw = Nxw;
}
/** rotate theta degrees about the xw plan */
void Matrix4D::xwrot(double theta) {
theta *= (pi / 180);
double ct = cos(theta);
double st = sin(theta);
double Nwx = (double) (wx * ct + xx * st);
double Nwy = (double) (wy * ct + xy * st);
double Nwz = (double) (wz * ct + xz * st);
double Nww = (double) (ww * ct + xw * st);
double Nwo = (double) (wo * ct + xo * st);
double Nxx = (double) (xx * ct - wx * st);
double Nxy = (double) (xy * ct - wy * st);
double Nxz = (double) (xz * ct - wz * st);
double Nxw = (double) (xw * ct - ww * st);
double Nxo = (double) (xo * ct - wo * st);
wo = Nwo;
wx = Nwx;
wy = Nwy;
wz = Nwz;
ww = Nww;
xo = Nxo;
xx = Nxx;
xy = Nxy;
xz = Nxz;
xw = Nxw;
}
/** rotate theta degrees about the yw plan */
void Matrix4D::ywrot(double theta) {
theta *= (pi / 180);
double ct = cos(theta);
double st = sin(theta);
double Nwx = (double) (wx * ct + yx * st);
double Nwy = (double) (wy * ct + yy * st);
double Nwz = (double) (wz * ct + yz * st);
double Nww = (double) (ww * ct + yw * st);
double Nwo = (double) (wo * ct + yo * st);
double Nyx = (double) (yx * ct - wx * st);
double Nyy = (double) (yy * ct - wy * st);
double Nyz = (double) (yz * ct - wz * st);
double Nyw = (double) (yw * ct - ww * st);
double Nyo = (double) (yo * ct - wo * st);
wo = Nwo;
wx = Nwx;
wy = Nwy;
wz = Nwz;
ww = Nww;
yo = Nyo;
yx = Nyx;
yy = Nyy;
yz = Nyz;
yw = Nyw;
}
/** rotate theta degrees about the yw plan */
void Matrix4D::zwrot(double theta) {
theta *= (pi / 180);
double ct = cos(theta);
double st = sin(theta);
double Nwx = (double) (wx * ct + zx * st);
double Nwy = (double) (wy * ct + zy * st);
double Nwz = (double) (wz * ct + zz * st);
double Nww = (double) (ww * ct + zw * st);
double Nwo = (double) (wo * ct + zo * st);
double Nzx = (double) (zx * ct - wx * st);
double Nzy = (double) (zy * ct - wy * st);
double Nzz = (double) (zz * ct - wz * st);
double Nzw = (double) (zw * ct - ww * st);
double Nzo = (double) (zo * ct - wo * st);
wo = Nwo;
wx = Nwx;
wy = Nwy;
wz = Nwz;
ww = Nww;
zo = Nzo;
zx = Nzx;
zy = Nzy;
zz = Nzz;
zw = Nzw;
}
/** Multiply this matrix by a second: M = M*R */
/** Reinitialize to the unit matrix */
void Matrix4D::unit() {
xo = 0;
xx = 1;
xy = 0;
xz = 0;
xw = 0;
yo = 0;
yx = 0;
yy = 1;
yz = 0;
yw = 0;
zo = 0;
zx = 0;
zy = 0;
zz = 1;
zw = 0;
wo = 0;
wx = 0;
wy = 0;
wz = 0;
ww = 1;
ko = 1;
kx = 0;
ky = 0;
kz = 0;
kw = 0;
};
/** Translate the origin */
void Matrix4D::translate(double x, double y, double z, double w) {
xo += x;
yo += y;
zo += z;
wo += w;
}
/** Scale by f in all dimensions */
void Matrix4D::scale(double f) {
xx *= f;
xy *= f;
xz *= f;
xo *= f;
yx *= f;
yy *= f;
yz *= f;
yo *= f;
zx *= f;
zy *= f;
zz *= f;
zo *= f;
}
/** Scale along each axis independently */
void Matrix4D::scale_2(double xf, double yf, double zf) {
xx *= xf;
xy *= xf;
xz *= xf;
xo *= xf;
yx *= yf;
yy *= yf;
yz *= yf;
yo *= yf;
zx *= zf;
zy *= zf;
zz *= zf;
zo *= zf;
}
syntax highlighted by Code2HTML, v. 0.9.1