public void toAt(Vector cam, Vector obj) {
if (cam == null || obj == null) throw new NullPointerException();
dir.set(0, 0, 0);
dir.set(obj);
dir.sub(cam);
dir.mult(-1f);
dir.norm();
dir.get(dirArray);
right.set(0, 0, 0);
right.cross(worldUp, dir);
right.norm();
right.get(rightArray);
up.set(0, 0, 0);
up.cross(dir, right);
up.norm();
up.get(upArray);
set(
rightArray[0],
rightArray[1],
rightArray[2],
upArray[0],
upArray[1],
upArray[2],
dirArray[0],
dirArray[1],
dirArray[2]);
}
public static Matrix lookAtLH(Vector eye, Vector target, Vector up) {
Matrix res = new Matrix();
Vector zaxis = Vector.sub(target, eye).normalize();
if (zaxis.length() == 0) zaxis.Z = 1;
Vector xaxis = Vector.cross(up, zaxis).normalize();
if (xaxis.length() == 0) {
zaxis.X += 0.000001;
xaxis = Vector.cross(up, zaxis).normalize();
}
Vector yaxis = Vector.cross(zaxis, xaxis);
res.m[0] = xaxis.X;
res.m[1] = xaxis.Y;
res.m[2] = xaxis.Z;
res.m[3] = -Vector.dot(xaxis, eye);
res.m[4] = yaxis.X;
res.m[5] = yaxis.Y;
res.m[6] = yaxis.Z;
res.m[7] = -Vector.dot(yaxis, eye);
res.m[8] = zaxis.X;
res.m[9] = zaxis.Y;
res.m[10] = zaxis.Z;
res.m[11] = -Vector.dot(zaxis, eye);
// already set during initialization of matrix
/*
* res.m[12] = 0; res.m[13] = 0; res.m[14] = 0; res.m[15] = 1;
*/
return res;
}
public void testCrossProduct() {
Matrix result = test.cross(test);
assertEquals("row size", test.size(), result.size()[0]);
assertEquals("col size", test.size(), result.size()[1]);
for (int row = 0; row < result.size()[0]; row++) {
for (int col = 0; col < result.size()[1]; col++) {
assertEquals(
"cross[" + row + "][" + col + ']',
test.getQuick(row) * test.getQuick(col),
result.getQuick(row, col));
}
}
}
// 0: disjoint, 1: intersection, 2: coincide
int intersect3D(Plane p) {
Line inter; // intersection line
Vector u = n.cross(p.n);
double ax = Math.abs(u.x);
double ay = Math.abs(u.y);
double az = Math.abs(u.z);
if (ax + ay + ax < EPS) { // parallelv
Vector w = p.v.minus(v);
if (n.dot(w) == 0) // p.v lies in this plane
return 2; // concide
else return 0; // disjoint
}
int maxc; // max coordinate
if (ax > ay)
if (ax > az) maxc = 1;
else maxc = 3;
else if (ay > az) maxc = 2;
else maxc = 3;
// get a point on the intersect line
double x = 0, y = 0, z = 0; // intersect point coordinates
double d1, d2; // constants in the 2 plane equations
d1 = -n.dot(p.v);
d2 = -p.n.dot(p.v);
switch (maxc) { // intersection with maxc (x, y, z)
case 1:
x = 0;
y = (d2 * n.z - d1 * p.n.z) / u.x;
z = (d1 * p.n.y - d2 * n.y) / u.x;
break;
case 2:
x = (d1 * p.n.z - d2 * n.z) / u.y;
y = 0;
z = (d2 * n.x - d1 * p.n.x) / u.y;
break;
case 3:
x = (d2 * n.y - d1 * p.n.y) / u.z;
y = (d1 * p.n.x - d2 * n.x) / u.z;
z = 0;
}
Point iP = new Point(x, y, z);
inter = new Line(iP, iP.plus(u));
return 1;
}
/** Gets the speed of the wheel based on the linear and rotational movements */
public double getSpeed(Vector velocity, Vector rotation) {
Vector wheel_velocity = velocity.add(rotation.cross(position));
return direction.dot(wheel_velocity) * speed_ratio;
}
