|
import java.io.PrintStream;
|
|
class LA
|
{
|
|
static cVector newVector(double x, double y, double z)
|
{
|
cVector temp = new cVector();
|
temp.x = x;
|
temp.y = y;
|
temp.z = z;
|
return temp;
|
}
|
|
static void setVector(cVector v, double x, double y, double z)
|
{
|
v.x = x;
|
v.y = y;
|
v.z = z;
|
}
|
|
static String toPOVRay(cVector v)
|
{
|
StringBuffer buf = new StringBuffer(50);
|
toPOVRay(v, buf);
|
return buf.toString();
|
}
|
|
static void toPOVRay(cVector v, StringBuffer buf)
|
{
|
buf.append('<');
|
buf.append(v.x);
|
buf.append(',');
|
buf.append(v.y);
|
buf.append(',');
|
buf.append(v.z);
|
buf.append('>');
|
}
|
|
static boolean vecEqual(cVector a, cVector b)
|
{
|
return a.x == b.x && a.y == b.y && a.z == b.z;
|
}
|
|
static double distance(cVector a, cVector b)
|
{
|
return Math.sqrt(distance2(a, b));
|
}
|
|
static double distance2(cVector a, cVector b)
|
{
|
double x = b.x - a.x;
|
double y = b.y - a.y;
|
double z = b.z - a.z;
|
|
return x * x + y * y + z * z;
|
}
|
|
static void vecNegate(cVector v)
|
{
|
v.x *= -1;
|
v.y *= -1;
|
v.z *= -1;
|
}
|
|
static void vecScale(cVector v, double s)
|
{
|
v.x *= s;
|
v.y *= s;
|
v.z *= s;
|
}
|
|
static double vecDot(cVector a, cVector b)
|
{
|
return a.x * b.x + a.y * b.y + a.z * b.z;
|
}
|
|
static double vecLen(cVector v)
|
{
|
return (double) Math.sqrt(vecDot(v, v));
|
}
|
|
static double vecLen2(cVector v)
|
{
|
return vecDot(v, v);
|
}
|
|
static void vecCopy(cVector a, cVector b)
|
{
|
Grafreed.Assert (a != null);
|
assert (b != null);
|
|
b.x = a.x;
|
b.y = a.y;
|
b.z = a.z;
|
}
|
|
static void vecAdd(cVector a, cVector b, cVector c)
|
{
|
c.x = a.x + b.x;
|
c.y = a.y + b.y;
|
c.z = a.z + b.z;
|
}
|
|
static void vecSub(cVector a, cVector b, cVector c)
|
{
|
c.x = a.x - b.x;
|
c.y = a.y - b.y;
|
c.z = a.z - b.z;
|
}
|
|
static void vecCross(cVector a, cVector b, cVector c)
|
{
|
c.x = a.y * b.z - a.z * b.y;
|
c.y = a.z * b.x - a.x * b.z;
|
c.z = a.x * b.y - a.y * b.x;
|
}
|
|
static boolean vecNormalize(cVector v)
|
{
|
double len = v.x * v.x + v.y * v.y + v.z * v.z; // vecLen(v);
|
|
if (len <= 0) // 0.0001)
|
{
|
return false;
|
}
|
|
len = Math.sqrt(len);
|
|
//vecScale(v, 1 / Math.sqrt(len));
|
v.x /= len;
|
v.y /= len;
|
v.z /= len;
|
return true;
|
}
|
|
static cVector xformPos(cVector v, double mat[][])
|
{
|
new Exception().printStackTrace();
|
cVector temp = new cVector();
|
xformPos(v, mat, temp);
|
return temp;
|
}
|
|
static void xformPos(cVector in, double mat[][], cVector out)
|
{
|
//assert (mat != null);
|
if (mat == null)
|
{
|
out.set(in);
|
return;
|
}
|
|
assert (in != null);
|
for (int i = 0; i < 3; i++)
|
{
|
xfpTemp.set(i, in.x * mat[0][i] + in.y * mat[1][i] + in.z * mat[2][i] + mat[3][i]);
|
}
|
|
vecCopy(xfpTemp, out);
|
}
|
|
static cVector xformDir(cVector v, double mat[][])
|
{
|
new Exception().printStackTrace();
|
System.exit(0);
|
cVector temp = new cVector();
|
xformDir(v, mat, temp);
|
return temp;
|
}
|
|
static void xformDir(cVector in, double mat[][], cVector out)
|
{
|
//xfdTemp.x = xfdTemp.y = xfdTemp.z = 0;
|
//xformPos(xfdTemp, mat, xfdTemp);
|
//xformPos(in, mat, out);
|
//vecSub(out, xfdTemp, out);
|
if (mat == null)
|
{
|
out.set(in);
|
return;
|
}
|
|
for (int i = 0; i < 3; i++)
|
{
|
xfpTemp.set(i, in.x * mat[0][i] + in.y * mat[1][i] + in.z * mat[2][i]);
|
}
|
|
vecCopy(xfpTemp, out);
|
}
|
|
static double[][] newMatrix()
|
{
|
double temp[][] = new double[4][4];
|
matIdentity(temp);
|
return temp;
|
}
|
|
static void matPrint(double mat[][])
|
{
|
for (int i = 0; i < 4; i++)
|
{
|
for (int j = 0; j < 4; j++)
|
{
|
System.out.print(mat[i][j]);
|
if (j < 3)
|
{
|
System.out.print(", ");
|
}
|
}
|
System.out.println();
|
}
|
}
|
|
static void matIdentity(double mat[][])
|
{
|
// matIdentity(mat, false);
|
int s = 4;
|
|
for (int i = 0; i < s; i++)
|
{
|
double[] mati = mat[i];
|
for (int j = 0; j < s; j++)
|
{
|
mati[j] = i != j ? 0 : 1;
|
}
|
}
|
}
|
|
static void matIdentity(double mat[][], boolean rotationonly)
|
{
|
int s = 4;
|
|
if (rotationonly)
|
{
|
s = 3;
|
|
for (int i = 0; i < s; i++)
|
{
|
double[] mati = mat[i];
|
for (int j = 0; j < s; j++)
|
{
|
mati[j] = i != j ? 0 : 1;
|
}
|
}
|
}
|
else
|
{
|
mat[3][0] = 0;
|
mat[3][1] = 0;
|
mat[3][2] = 0;
|
}
|
}
|
|
static boolean isIdentity(double mat[][])
|
{
|
if (mat == null)
|
return true;
|
|
//new Exception().printStackTrace();
|
//LA.matPrint(mat);
|
for (int i = 0; i < 4; i++)
|
{
|
double[] mati = mat[i];
|
for (int j = 0; j < 4; j++)
|
{
|
if (mati[j] != (i != j ? 0 : 1))
|
{
|
return false;
|
}
|
}
|
}
|
|
return true;
|
}
|
|
static boolean isIdentityEps(double mat[][])
|
{
|
if (mat == null)
|
return true;
|
|
//new Exception().printStackTrace();
|
//LA.matPrint(mat);
|
for (int i = 0; i < 4; i++)
|
{
|
double[] mati = mat[i];
|
for (int j = 0; j < 4; j++)
|
{
|
if ((mati[j] - ((i != j) ? 0 : 1)) > 0.00001)
|
{
|
return false;
|
}
|
}
|
}
|
|
return true;
|
}
|
|
static void matCopy(double src[][], double dst[][])
|
{
|
for (int i = 0; i < 4; i++)
|
{
|
double[] dsti = dst[i];
|
double[] srci = src[i];
|
for (int j = 0; j < 4; j++)
|
{
|
dsti[j] = srci[j];
|
}
|
|
}
|
|
// Last row should always be 0 0 0 1
|
Grafreed.Assert(Math.abs(src[0][3]) <= 1E-15);
|
Grafreed.Assert(Math.abs(src[1][3]) <= 1E-15);
|
Grafreed.Assert(Math.abs(src[2][3]) <= 1E-15);
|
Grafreed.Assert(Math.abs(src[3][3] - 1) <= 1E-15);
|
Grafreed.Assert(Math.abs(dst[0][3]) <= 1E-15);
|
Grafreed.Assert(Math.abs(dst[1][3]) <= 1E-15);
|
Grafreed.Assert(Math.abs(dst[2][3]) <= 1E-15);
|
Grafreed.Assert(Math.abs(dst[3][3] - 1) <= 1E-15);
|
}
|
|
static double toRadians(double degrees)
|
{
|
return (degrees * Math.PI) / 180;
|
}
|
|
static void matConcat(double left[][], double right[][], double product[][])
|
{
|
//double tmp[][] = left;
|
//left = right;
|
//right = tmp;
|
|
for (int j = 0; j < 4; j++)
|
{
|
double[] rightj = right[j];
|
double[] concat = concatTemp[j];
|
for (int i = 0; i < 4; i++)
|
{
|
concat[i] = 0;
|
for (int k = 0; k < 4; k++)
|
{
|
concat[i] += left[k][i] * rightj[k];
|
}
|
}
|
}
|
|
matCopy(concatTemp, product);
|
}
|
|
static void matTranslate(double mat[][], double dx, double dy, double dz)
|
{
|
matIdentity(xlateTemp);
|
xlateTemp[3][0] = dx;
|
xlateTemp[3][1] = dy;
|
xlateTemp[3][2] = dz;
|
//xlateTemp[0][3] = dx;
|
//xlateTemp[1][3] = dy;
|
//xlateTemp[2][3] = dz;
|
matConcat(xlateTemp, mat, mat);
|
}
|
|
static void matHomogene(double mat[][], double dx, double dy, double dz)
|
{
|
matIdentity(xlateTemp);
|
xlateTemp[0][3] = dx;
|
xlateTemp[1][3] = dy;
|
xlateTemp[2][3] = dz;
|
//xlateTemp[0][3] = dx;
|
//xlateTemp[1][3] = dy;
|
//xlateTemp[2][3] = dz;
|
matConcat(xlateTemp, mat, mat);
|
}
|
|
static void matTranslateInv(double mat[][], double dx, double dy, double dz)
|
{
|
matIdentity(xlateTemp);
|
xlateTemp[3][0] = dx;
|
xlateTemp[3][1] = dy;
|
xlateTemp[3][2] = dz;
|
//xlateTemp[0][3] = dx;
|
//xlateTemp[1][3] = dy;
|
//xlateTemp[2][3] = dz;
|
matConcat(mat, xlateTemp, mat);
|
}
|
|
static void matScale(double mat[][], double sx, double sy, double sz)
|
{
|
matIdentity(xlateTemp);
|
xlateTemp[0][0] = sx;
|
xlateTemp[1][1] = sy;
|
xlateTemp[2][2] = sz;
|
matConcat(xlateTemp, mat, mat);
|
//matConcat(mat, xlateTemp, mat);
|
}
|
|
static void matXRotate(double mat[][], double radians)
|
{
|
matIdentity(rotTemp);
|
rotTemp[1][1] = rotTemp[2][2] = (double) Math.cos(radians);
|
rotTemp[2][1] = -(rotTemp[1][2] = (double) Math.sin(radians));
|
// if (CameraPane.LOCALTRANSFORM)
|
matConcat(rotTemp, mat, mat);
|
// else
|
// matConcat(mat, rotTemp, mat);
|
}
|
|
static void matYRotate(double mat[][], double radians)
|
{
|
matIdentity(rotTemp);
|
rotTemp[0][0] = rotTemp[2][2] = (double) Math.cos(-radians);
|
rotTemp[2][0] = -(rotTemp[0][2] = (double) Math.sin(-radians));
|
// if (CameraPane.LOCALTRANSFORM)
|
matConcat(rotTemp, mat, mat);
|
// else
|
// matConcat(mat, rotTemp, mat);
|
}
|
|
static void matZRotate(double mat[][], double radians)
|
{
|
matIdentity(rotTemp);
|
rotTemp[0][0] = rotTemp[1][1] = (double) Math.cos(radians);
|
rotTemp[1][0] = -(rotTemp[0][1] = (double) Math.sin(radians));
|
// if (CameraPane.LOCALTRANSFORM)
|
matConcat(rotTemp, mat, mat);
|
// else
|
// matConcat(mat, rotTemp, mat);
|
}
|
|
// Project A onto B
|
static void matRotate(double mat[][], cVector pA, cVector pB)
|
{
|
matIdentity(mat);
|
|
double x, y, z, w;
|
|
/*
|
if (pA == null || pB == null)
|
{
|
x = y = z = 0.0D;
|
w = 1.0D;
|
return;
|
}
|
*/
|
cVector lA = cStatic.point1; // new cVector(pA);
|
lA.set(pA);
|
cVector lB = cStatic.point2; // new cVector(pB);
|
lB.set(pB);
|
if (lA.length2() > 0.0F)
|
{
|
lA.normalize();
|
}
|
if (lB.length2() > 0.0F)
|
{
|
lB.normalize();
|
}
|
double lDot = lA.dot(lB);
|
cVector lCP = cStatic.point3; // new cVector();
|
lCP.cross(lA, lB);
|
if (lCP.length2() > 0.0F)
|
{
|
lCP.normalize();
|
} else if (lDot < 0.0D)
|
{
|
lCP.cross(lA, cVector.Y);
|
if (lCP.length2() > 0.0F)
|
{
|
lCP.normalize();
|
} else
|
{
|
lCP.cross(lA, cVector.X);
|
lCP.normalize();
|
}
|
}
|
double lTmp = (lDot + 1.0D) / 2D;
|
double lCos = Math.sqrt(lTmp);
|
double lSin = Math.sqrt(1.0D - lTmp);
|
x = lCP.x * lSin;
|
y = lCP.y * lSin;
|
z = lCP.z * lSin;
|
w = lCos;
|
|
/*
|
| 2 2 |
|
| 1 - 2Y - 2Z 2XY - 2ZW 2XZ + 2YW |
|
| |
|
| 2 2 |
|
M = | 2XY + 2ZW 1 - 2X - 2Z 2YZ - 2XW |
|
| |
|
| 2 2 |
|
| 2XZ - 2YW 2YZ + 2XW 1 - 2X - 2Y |
|
| |
|
*/
|
mat[0][0] = 1 - 2 * (y*y + z*z);
|
mat[1][1] = 1 - 2 * (x*x + z*z);
|
mat[2][2] = 1 - 2 * (x*x + y*y);
|
mat[0][1] = 2 * (x*y - z*w);
|
mat[0][2] = 2 * (x*z + y*w);
|
mat[1][0] = 2 * (x*y + z*w);
|
mat[1][2] = 2 * (y*z - x*w);
|
mat[2][0] = 2 * (x*z - y*w);
|
mat[2][1] = 2 * (y*z + x*w);
|
}
|
|
static void matTranspose(double input[][], double output[][])
|
{
|
matCopy(input, output);
|
|
for (int i = 1; i < 4; i++)
|
{
|
for (int j = 0; j < i; j++)
|
{
|
double t = output[i][j];
|
output[i][j] = output[j][i];
|
output[j][i] = t;
|
}
|
}
|
}
|
|
static void matInvert(double input[][], double output[][])
|
{
|
int irow = 0;
|
int icol = 0;
|
matCopy(input, output);
|
for (int i = 0; i < 4; i++)
|
{
|
ipiv[i] = -1;
|
}
|
|
for (int i = 0; i < 4; i++)
|
{
|
double big = 0;
|
for (int j = 0; j < 4; j++)
|
{
|
if (ipiv[j] != 0)
|
{
|
double[] cj = output[j];
|
for (int k = 0; k < 4; k++)
|
{
|
if (ipiv[k] == -1)
|
{
|
double piv = Math.abs(cj[k]);
|
if (piv >= big)
|
{
|
big = piv;
|
irow = j;
|
icol = k;
|
}
|
} else if (ipiv[k] > 0)
|
{
|
System.out.println("singular matrix");
|
//new Exception().printStackTrace();
|
return;
|
}
|
}
|
|
}
|
}
|
|
ipiv[icol]++;
|
double[] col = output[icol];
|
if (irow != icol)
|
{
|
double[] row = output[irow];
|
for (int j = 0; j < 4; j++)
|
{
|
double dum = row[j];
|
row[j] = col[j];
|
col[j] = dum;
|
}
|
|
}
|
|
indxr[i] = irow;
|
indxc[i] = icol;
|
/*
|
if (Math.abs(output[icol][icol]) < 1E-20)
|
{
|
System.out.println("singular matrix II");
|
new Exception().printStackTrace();
|
return;
|
}
|
*/
|
double pivinv = 1 / col[icol];
|
col[icol] = 1;
|
for (int j = 0; j < 4; j++)
|
{
|
col[j] *= pivinv;
|
}
|
|
for (int j = 0; j < 4; j++)
|
{
|
if (j != icol)
|
{
|
double[] cj = output[j];
|
double dum = cj[icol];
|
cj[icol] = 0;
|
for (int k = 0; k < 4; k++)
|
{
|
cj[k] -= col[k] * dum;
|
}
|
}
|
}
|
}
|
|
for (int i = 3; i >= 0; i--)
|
{
|
if (indxr[i] != indxc[i])
|
{
|
for (int j = 0; j < 4; j++)
|
{
|
double[] cj = output[j];
|
double dum = cj[indxr[i]];
|
cj[indxr[i]] = cj[indxc[i]];
|
cj[indxc[i]] = dum;
|
}
|
}
|
}
|
}
|
|
private static cVector xfpTemp = new cVector();
|
private static cVector xfdTemp = new cVector();
|
private static double concatTemp[][] = new double[4][4];
|
private static double xlateTemp[][] = new double[4][4];
|
private static double rotTemp[][] = new double[4][4];
|
private static int ipiv[] = new int[4];
|
private static int indxr[] = new int[4];
|
private static int indxc[] = new int[4];
|
|
static double[][] Identity = new double[4][4];
|
|
static int SIZE = 0; // 65536*64;
|
|
static double[] costable = new double[SIZE];
|
static double[] sintable = new double[SIZE];
|
|
static double PI2 = 2 * Math.PI;
|
|
static
|
{
|
for (int i=SIZE; --i>=0;)
|
{
|
costable[i] = Math.cos(PI2 * i/SIZE);
|
sintable[i] = Math.sin(PI2 * i*i/SIZE/SIZE);
|
}
|
|
LA.matIdentity(Identity);
|
}
|
|
static double cos(double x0)
|
{
|
if (true)
|
return Math.cos(x0);
|
|
double x = x0;
|
|
while (x < 0)
|
{
|
x += PI2;
|
}
|
|
while (x > PI2)
|
{
|
x -= PI2;
|
}
|
|
x /= PI2;
|
|
int modi = (int)(x*(SIZE-1));
|
|
// double real = Math.cos(x0);
|
// double test = costable[modi];
|
//
|
// if (Math.abs(real - test) > 0.000001)
|
// x = x;
|
|
return costable[modi];
|
}
|
|
static double sin(double x0)
|
{
|
if (true)
|
return Math.sin(x0);
|
|
double x = x0;
|
|
while (x < 0)
|
{
|
x += PI2;
|
}
|
|
while (x > PI2)
|
{
|
x -= PI2;
|
}
|
|
x /= PI2;
|
|
x = Math.sqrt(x);
|
|
int modi = (int)(x*(SIZE-1));
|
|
// double real = Math.sin(x0);
|
// double test = sintable[modi];
|
//
|
// if (Math.abs(real - test) > 0.000001)
|
// x = x;
|
|
return sintable[modi];
|
}
|
}
|
