package java3d;
public abstract class Bounds extends Object implements Cloneable {
static final double EPSILON = .000001;
static final boolean debug = false;
static final int BOUNDING_BOX = 0x1;
static final int BOUNDING_SPHERE = 0x2;
static final int BOUNDING_POLYTOPE = 0x4;
boolean boundsIsEmpty = false;
boolean boundsIsInfinite = false;
int boundId = 0;
public Bounds(){
}
// public Bounds clone() {
// return new Bounds();
// }
@Override
public abstract Object clone();
public abstract void combine(Bounds boundsObject);
public abstract void transform(Transform3D trans);
public abstract boolean intersect(Bounds boundsObject);
// //浸食確認メソッド。radius(半径)を利用した円の接触であると仮定してみるといいかも
// public boolean intersect(BoundingSphere bs) {
// // TODO Auto-generated method stub
// return false;
// }
private void test_point(Vector4d[] planes, Point3d new_point) {
for (int i = 0; i < planes.length; i++){
double dist = (new_point.x*planes[i].x + new_point.y*planes[i].y +
new_point.z*planes[i].z + planes[i].w ) ;
if (dist > EPSILON ){
System.err.println("new point is outside of" +
" plane["+i+"] dist = " + dist);
}
}
}
/**
* computes the closest point from the given point to a set of planes
* (polytope)
* @param g the point
* @param planes array of bounding planes
* @param new_point point on planes closest g
*/
boolean closest_point(Point3d g, Vector4d[] planes, Point3d new_point) {
double t, s, dist, w;
boolean converged, inside, firstPoint, firstInside;
int i, count;
double ab, ac, bc, ad, bd, cd, aa, bb, cc;
double b1, b2, b3, d1, d2, d3, y1, y2, y3;
double h11, h12, h13, h22, h23, h33;
double l12, l13, l23;
Point3d n = new Point3d();
Point3d p = new Point3d();
Vector3d delta = null;
// These are temporary until the solve code is working
/*
* The algorithm: We want to find the point "n", closest to "g", while
* still within the the polytope defined by "planes". We find the
* solution by minimizing the value for a "penalty function";
*
* f = distance(n,g)^2 + sum for each i: w(distance(n, planes[i]))
*
* Where "w" is a weighting which indicates how much more important it
* is to be close to the planes than it is to be close to "g".
*
* We minimize this function by taking it's derivitive, and then solving
* for the value of n when the derivitive equals 0.
*
* For the 1D case with a single plane (a,b,c,d), x = n.x and g = g.x,
* this looks like:
*
* f(x) = (x - g) ^ 2 + w(ax + d)^2 f'(x) = 2x -2g + 2waax + 2wad
*
* (note aa = a^2) setting f'(x) = 0 gives:
*
* (1 + waa)x = g - wad
*
* Note that the solution is just outside the plane [a, d]. With the
* correct choice of w, this should be inside of the EPSILON tolerance
* outside the planes.
*
* Extending to 3D gives the matrix solution:
*
* | (1 + waa) wab wac | H = | wab (1 + wbb) wbc | | wac wbc (1 + wcc) |
*
* b = [g.x - wad, g.y - wbd, g.z - wcd]
*
* H * n = b
*
* n = b * H.inverse()
*
* The implementation speeds this process up by recognizing that H is
* symmetric, so that it can be decomposed into three matrices:
*
* H = L * D * L.transpose()
*
* 1.0 0.0 0.0 d1 0.0 0.0 L = l12 1.0 0.0 D = 0.0 d2 0.0 l13 l23 1.0 0.0
* 0.0 d3
*
* n can then be derived by back-substitution, where the original
* problem is decomposed as:
*
* H * n = b L * D * L.transpose() * n = b L * D * y = b; L.transpose()
* * n = y
*
* We can then multiply out the terms of L * D and solve for y, and then
* use y to solve for n.
*/
w = 100.0 / EPSILON; // must be large enough to ensure that solution
// is within EPSILON of planes
count = 0;
p.set(g);
if (debug) {
System.err.println("closest_point():\nincoming g=" + " " + g.x
+ " " + g.y + " " + g.z);
}
converged = false;
firstPoint = true;
firstInside = false;
Vector4d pln;
while (!converged) {
if (debug) {
System.err.println("start: p=" + " " + p.x + " " + p.y + " "
+ p.z);
}
// test the current point against the planes, for each
// plane that is violated, add it's contribution to the
// penalty function
inside = true;
aa = 0.0;
bb = 0.0;
cc = 0.0;
ab = 0.0;
ac = 0.0;
bc = 0.0;
ad = 0.0;
bd = 0.0;
cd = 0.0;
for (i = 0; i < planes.length; i++) {
pln = planes[i];
dist = (p.x * pln.x + p.y * pln.y + p.z * pln.z + pln.w);
// if point is outside or within EPSILON of the boundary, add
// the plane to the penalty matrix. We do this even if the
// point is already inside the polytope to prevent numerical
// instablity in cases where the point is just outside the
// boundary of several planes of the polytope
if (dist > -EPSILON) {
aa = aa + pln.x * pln.x;
bb = bb + pln.y * pln.y;
cc = cc + pln.z * pln.z;
ab = ab + pln.x * pln.y;
ac = ac + pln.x * pln.z;
bc = bc + pln.y * pln.z;
ad = ad + pln.x * pln.w;
bd = bd + pln.y * pln.w;
cd = cd + pln.z * pln.w;
}
// If the point is inside if dist is <= EPSILON
if (dist > EPSILON) {
inside = false;
if (debug) {
System.err.println("point outside plane[" + i + "]=("
+ pln.x + "," + pln.y + ",\n\t" + pln.z + ","
+ pln.w + ")\ndist = " + dist);
}
}
}
// see if we are done
if (inside) {
if (debug) {
System.err.println("p is inside");
}
if (firstPoint) {
firstInside = true;
}
new_point.set(p);
converged = true;
} else { // solve for a closer point
firstPoint = false;
// this is the upper right corner of H, which is all we
// need to do the decomposition since the matrix is symetric
h11 = 1.0 + aa * w;
h12 = ab * w;
h13 = ac * w;
h22 = 1.0 + bb * w;
h23 = bc * w;
h33 = 1.0 + cc * w;
if (debug) {
System.err.println(" hessin= ");
System.err.println(h11 + " " + h12 + " " + h13);
System.err.println(" " + h22 + " " + h23);
System.err.println(" " + h33);
}
// these are the constant terms
b1 = g.x - w * ad;
b2 = g.y - w * bd;
b3 = g.z - w * cd;
if (debug) {
System.err.println(" b1,b2,b3 = " + b1 + " " + b2 + " "
+ b3);
}
// solve, d1, d2, d3 actually 1/dx, which is more useful
d1 = 1 / h11;
l12 = d1 * h12;
l13 = d1 * h13;
s = h22 - l12 * h12;
d2 = 1 / s;
t = h23 - h12 * l13;
l23 = d2 * t;
d3 = 1 / (h33 - h13 * l13 - t * l23);
if (debug) {
System.err.println(" l12,l13,l23 " + l12 + " " + l13 + " "
+ l23);
System.err.println(" d1,d2,d3 " + d1 + " " + d2 + " " + d3);
}
// we have L and D, now solve for y
y1 = d1 * b1;
y2 = d2 * (b2 - h12 * y1);
y3 = d3 * (b3 - h13 * y1 - t * y2);
if (debug) {
System.err.println(" y1,y2,y3 = " + y1 + " " + y2 + " "
+ y3);
}
// we have y, solve for n
n.z = y3;
n.y = (y2 - l23 * n.z);
n.x = (y1 - l13 * n.z - l12 * n.y);
if (debug) {
System.err.println("new point = " + n.x + " " + n.y + " "
+ n.z);
test_point(planes, n);
if (delta == null)
delta = new Vector3d();
delta.sub(n, p);
delta.normalize();
System.err.println("p->n direction: " + delta);
Matrix3d hMatrix = new Matrix3d();
// check using the the javax.vecmath routine
hMatrix.m00 = h11;
hMatrix.m01 = h12;
hMatrix.m02 = h13;
hMatrix.m10 = h12; // h21 = h12
hMatrix.m11 = h22;
hMatrix.m12 = h23;
hMatrix.m20 = h13; // h31 = h13
hMatrix.m21 = h23; // h32 = h22
hMatrix.m22 = h33;
hMatrix.invert();
Point3d check = new Point3d(b1, b2, b3);
hMatrix.transform(check);
System.err.println("check point = " + check.x + " "
+ check.y + " " + check.z);
}
// see if we have converged yet
dist = (p.x - n.x) * (p.x - n.x) + (p.y - n.y) * (p.y - n.y)
+ (p.z - n.z) * (p.z - n.z);
if (debug) {
System.err.println("p->n distance =" + dist);
}
if (dist < EPSILON) { // close enough
converged = true;
new_point.set(n);
} else {
p.set(n);
count++;
if (count > 4) { // watch for cycling between two minimums
new_point.set(n);
converged = true;
}
}
}
}
if (debug) {
System.err.println("returning pnt (" + new_point.x + " "
+ new_point.y + " " + new_point.z + ")");
if (firstInside)
System.err.println("input point inside polytope ");
}
return firstInside;
}
boolean intersect_ptope_sphere( BoundingPolytope polyTope,
BoundingSphere sphere) {
Point3d p = new Point3d();
boolean inside;
if (debug) {
System.err.println("ptope_sphere intersect sphere ="+sphere);
}
inside = closest_point( sphere.center, polyTope.planes, p );
if (debug) {
System.err.println("ptope sphere intersect point ="+p);
}
if (!inside){
// if distance between polytope and sphere center is greater than
// radius then no intersection
if (p.distanceSquared( sphere.center) >
sphere.radius*sphere.radius){
if (debug) {
System.err.println("ptope_sphere returns false");
}
return false;
} else {
if (debug) {
System.err.println("ptope_sphere returns true");
}
return true;
}
} else {
if (debug) {
System.err.println("ptope_sphere returns true");
}
return true;
}
}
boolean intersect_ptope_abox( BoundingPolytope polyTope, BoundingBox box) {
Vector4d planes[] = new Vector4d[6];
if (debug) {
System.err.println("ptope_abox, box = " + box);
}
planes[0] = new Vector4d( -1.0, 0.0, 0.0, box.lower.x);
planes[1] = new Vector4d( 1.0, 0.0, 0.0,-box.upper.x);
planes[2] = new Vector4d( 0.0,-1.0, 0.0, box.lower.y);
planes[3] = new Vector4d( 0.0, 1.0, 0.0,-box.upper.y);
planes[4] = new Vector4d( 0.0, 0.0,-1.0, box.lower.z);
planes[5] = new Vector4d( 0.0, 0.0, 1.0,-box.upper.z);
BoundingPolytope pbox = new BoundingPolytope( planes);
boolean result = intersect_ptope_ptope( polyTope, pbox );
if (debug) {
System.err.println("ptope_abox returns " + result);
}
return(result);
}
boolean intersect_ptope_ptope( BoundingPolytope poly1,
BoundingPolytope poly2) {
boolean intersect;
Point3d p = new Point3d();
Point3d g = new Point3d();
Point3d gnew = new Point3d();
Point3d pnew = new Point3d();
intersect = false;
p.x = 0.0;
p.y = 0.0;
p.z = 0.0;
// start from an arbitrary point on poly1
closest_point( p, poly1.planes, g);
// get the closest points on each polytope
if (debug) {
System.err.println("ptope_ptope: first g = "+g);
}
intersect = closest_point( g, poly2.planes, p);
if (intersect) {
return true;
}
if (debug) {
System.err.println("first p = "+p+"\n");
}
intersect = closest_point( p, poly1.planes, gnew);
if (debug) {
System.err.println("gnew = "+gnew+" intersect="+intersect);
}
// loop until the closest points on the two polytopes are not changing
double prevDist = p.distanceSquared(g);
double dist;
while( !intersect ) {
dist = p.distanceSquared(gnew);
if (dist < prevDist) {
g.set(gnew);
intersect = closest_point( g, poly2.planes, pnew );
if (debug) {
System.err.println("pnew = "+pnew+" intersect="+intersect);
}
} else {
g.set(gnew);
break;
}
prevDist = dist;
dist = pnew.distanceSquared(g);
if (dist < prevDist) {
p.set(pnew);
if( !intersect ) {
intersect = closest_point( p, poly1.planes, gnew );
if (debug) {
System.err.println("gnew = "+gnew+" intersect="+
intersect);
}
}
} else {
p.set(pnew);
break;
}
prevDist = dist;
}
if (debug) {
System.err.println("gnew="+" "+gnew.x+" "+gnew.y+" "+gnew.z);
System.err.println("pnew="+" "+pnew.x+" "+pnew.y+" "+pnew.z);
}
return intersect;
}
}
