GitBucket
Newer
package org.ntlab.radishforandroidstudio.java3d;
public class BoundingPolytope extends Bounds {
Vector4d[] planes;
double[] mag; // magnitude of plane vector
double[] pDotN; // point on plane dotted with normal
Point3d[] verts; // vertices of polytope
int nVerts; // number of verts in polytope
Point3d centroid = new Point3d(); // centroid of polytope
Point3d boxVerts[];
boolean allocBoxVerts = false;
/**
* Constructs a BoundingPolytope using the specified planes.
*
* @param planes
* a set of planes defining the polytope.
* @exception IllegalArgumentException
* if the length of the specified array of planes is less
* than 4.
*/
public BoundingPolytope(Vector4d[] planes) {
boundId = BOUNDING_POLYTOPE;
int i;
double invMag;
this.planes = new Vector4d[planes.length];
mag = new double[planes.length];
pDotN = new double[planes.length];
for (i = 0; i < planes.length; i++) {
// normalize the plane normals
mag[i] = Math.sqrt(planes[i].x * planes[i].x + planes[i].y
* planes[i].y + planes[i].z * planes[i].z);
invMag = 1.0 / mag[i];
this.planes[i] = new Vector4d(planes[i].x * invMag, planes[i].y
* invMag, planes[i].z * invMag, planes[i].w * invMag);
}
computeAllVerts(); // XXXX: lazy evaluate
}
/**
* Constructs a BoundingPolytope and initializes it to a set of 6 planes
* that defines a cube such that -1 <= x,y,z <= 1. The values of the planes
* are as follows:
* <ul>
* planes[0] : x <= 1 (1,0,0,-1)<br>
* planes[1] : -x <= 1 (-1,0,0,-1)<br>
* planes[2] : y <= 1 (0,1,0,-1)<br>
* planes[3] : -y <= 1 (0,-1,0,-1)<br>
* planes[4] : z <= 1 (0,0,1,-1)<br>
* planes[5] : -z <= 1 (0,0,-1,-1)<br>
* </ul>
*/
public BoundingPolytope() {
boundId = BOUNDING_POLYTOPE;
planes = new Vector4d[6];
mag = new double[planes.length];
pDotN = new double[planes.length];
planes[0] = new Vector4d(1.0, 0.0, 0.0, -1.0);
planes[1] = new Vector4d(-1.0, 0.0, 0.0, -1.0);
planes[2] = new Vector4d(0.0, 1.0, 0.0, -1.0);
planes[3] = new Vector4d(0.0, -1.0, 0.0, -1.0);
planes[4] = new Vector4d(0.0, 0.0, 1.0, -1.0);
planes[5] = new Vector4d(0.0, 0.0, -1.0, -1.0);
mag[0] = 1.0;
mag[1] = 1.0;
mag[2] = 1.0;
mag[3] = 1.0;
mag[4] = 1.0;
mag[5] = 1.0;
computeAllVerts(); // XXXX: lazy evaluate
}
public void setPlanes(Vector4d[] planes) {
int i;
double invMag;
this.planes = new Vector4d[planes.length];
pDotN = new double[planes.length];
mag = new double[planes.length];
if (planes.length <= 0) {
computeAllVerts(); // XXXX: lazy evaluate
return;
}
for (i = 0; i < planes.length; i++) {
// normalize the plane normals
mag[i] = Math.sqrt(planes[i].x * planes[i].x + planes[i].y
* planes[i].y + planes[i].z * planes[i].z);
invMag = 1.0 / mag[i];
this.planes[i] = new Vector4d(planes[i].x * invMag, planes[i].y
* invMag, planes[i].z * invMag, planes[i].w * invMag);
}
computeAllVerts(); // XXXX: lazy evaluate
}
/**
* Returns the equations of the bounding planes for this bounding polytope.
* The equations are copied into the specified array. The array must be
* large enough to hold all of the vectors. The individual array elements
* must be allocated by the caller.
*
* @param planes
* an array Vector4d to receive the bounding planes
*/
public void getPlanes(Vector4d[] planes) {
int i;
for (i = 0; i < planes.length; i++) {
planes[i].x = this.planes[i].x * mag[i];
planes[i].y = this.planes[i].y * mag[i];
planes[i].z = this.planes[i].z * mag[i];
planes[i].w = this.planes[i].w * mag[i];
}
}
public int getNumPlanes() {
return planes.length;
}
/**
* Sets the planes for this BoundingPolytope by keeping its current
* number and position of planes and computing new planes positions
* to enclose the given bounds object.
* @param boundsObject another bounds object
*/
public void set(Bounds boundsObject) {
int i,k;
double dis;
// no polytope exists yet so initialize one using the boundsObject
if( boundsObject == null ) {
boundsIsEmpty = true;
boundsIsInfinite = false;
computeAllVerts(); // XXXX: lazy evaluate
}else if( boundsObject.boundId == BOUNDING_SPHERE ) {
BoundingSphere sphere = (BoundingSphere)boundsObject;
if( boundsIsEmpty) {
initEmptyPolytope(); // no ptope exist so must initialize to default
computeAllVerts();
}
for(i=0;i<planes.length;i++) { // D = -(N dot C + radius)
planes[i].w = -(sphere.center.x*planes[i].x +
sphere.center.y*planes[i].y +
sphere.center.z*planes[i].z + sphere.radius);
}
boundsIsEmpty = boundsObject.boundsIsEmpty;
boundsIsInfinite = boundsObject.boundsIsInfinite;
computeAllVerts(); // XXXX: lazy evaluate
} else if( boundsObject.boundId == BOUNDING_BOX){
BoundingBox box = (BoundingBox)boundsObject;
double ux,uy,uz,lx,ly,lz,newD;
if( boundsIsEmpty) {
initEmptyPolytope(); // no ptope exist so must initialize to default
computeAllVerts();
}
for(i=0;i<planes.length;i++) {
ux = box.upper.x*planes[i].x;
uy = box.upper.y*planes[i].y;
uz = box.upper.z*planes[i].z;
lx = box.lower.x*planes[i].x;
ly = box.lower.y*planes[i].y;
lz = box.lower.z*planes[i].z;
planes[i].w = -(ux + uy + uz ); // initalize plane to upper vert
if( (newD = ux + uy + lz ) + planes[i].w > 0.0) planes[i].w = -newD;
if( (newD = ux + ly + uz ) + planes[i].w > 0.0) planes[i].w = -newD;
if( (newD = ux + ly + lz ) + planes[i].w > 0.0) planes[i].w = -newD;
if( (newD = lx + uy + uz ) + planes[i].w > 0.0) planes[i].w = -newD;
if( (newD = lx + uy + lz ) + planes[i].w > 0.0) planes[i].w = -newD;
if( (newD = lx + ly + uz ) + planes[i].w > 0.0) planes[i].w = -newD;
if( (newD = lx + ly + lz ) + planes[i].w > 0.0) planes[i].w = -newD;
}
boundsIsEmpty = boundsObject.boundsIsEmpty;
boundsIsInfinite = boundsObject.boundsIsInfinite;
computeAllVerts(); // XXXX: lazy evaluate
} else if(boundsObject.boundId == BOUNDING_POLYTOPE) {
BoundingPolytope polytope = (BoundingPolytope)boundsObject;
if( planes.length != polytope.planes.length) {
planes = new Vector4d[polytope.planes.length];
for(k=0;k<polytope.planes.length;k++) planes[k] = new Vector4d();
mag = new double[polytope.planes.length];
pDotN = new double[polytope.planes.length];
}
for(i=0;i<polytope.planes.length;i++) {
planes[i].x = polytope.planes[i].x;
planes[i].y = polytope.planes[i].y;
planes[i].z = polytope.planes[i].z;
planes[i].w = polytope.planes[i].w;
mag[i] = polytope.mag[i];
}
nVerts = polytope.nVerts;
verts = new Point3d[nVerts];
for (k=0; k<nVerts; k++) {
verts[k] = new Point3d(polytope.verts[k]);
}
boundsIsEmpty = boundsObject.boundsIsEmpty;
boundsIsInfinite = boundsObject.boundsIsInfinite;
} else {
}
}
/**
* Creates a copy of a polytope.
*
* @return a new BoundingPolytope
*/
@Override
public Object clone() {
return new BoundingPolytope(planes);
}
/**
* Combines this bounding polytope with a bounding object so that the
* resulting bounding polytope encloses the original bounding polytope and
* the given bounds object.
*
* @param boundsObject
* another bounds object
*/
@Override
public void combine(Bounds boundsObject) {
BoundingSphere sphere;
if((boundsObject == null) || (boundsObject.boundsIsEmpty)
|| (boundsIsInfinite))
return;
if((boundsIsEmpty) || (boundsObject.boundsIsInfinite)) {
this.set(boundsObject);
return;
}
boundsIsEmpty = boundsObject.boundsIsEmpty;
boundsIsInfinite = boundsObject.boundsIsInfinite;
if( boundsObject.boundId == BOUNDING_SPHERE ) {
sphere = (BoundingSphere)boundsObject;
int i;
double dis;
for(i = 0; i < planes.length; i++){
dis = sphere.radius+ sphere.center.x*planes[i].x +
sphere.center.y*planes[i].y + sphere.center.z *
planes[i].z + planes[i].w;
if( dis > 0.0 ) {
planes[i].w += -dis;
}
}
} else if( boundsObject instanceof BoundingBox){
BoundingBox b = (BoundingBox)boundsObject;
if( !allocBoxVerts){
boxVerts = new Point3d[8];
for(int j=0;j<8;j++)boxVerts[j] = new Point3d();
allocBoxVerts = true;
}
boxVerts[0].set(b.lower.x, b.lower.y, b.lower.z );
boxVerts[1].set(b.lower.x, b.upper.y, b.lower.z );
boxVerts[2].set(b.upper.x, b.lower.y, b.lower.z );
boxVerts[3].set(b.upper.x, b.upper.y, b.lower.z );
boxVerts[4].set(b.lower.x, b.lower.y, b.upper.z );
boxVerts[5].set(b.lower.x, b.upper.y, b.upper.z );
boxVerts[6].set(b.upper.x, b.lower.y, b.upper.z );
boxVerts[7].set(b.upper.x, b.upper.y, b.upper.z );
this.combine(boxVerts);
} else if(boundsObject.boundId == BOUNDING_POLYTOPE) {
BoundingPolytope polytope = (BoundingPolytope)boundsObject;
this.combine(polytope.verts);
} else {
}
computeAllVerts();
}
/**
* Combines this bounding polytope with a point.
* @param point a 3d point in space
*/
public void combine(Point3d point) {
int i;
double dis;
if(boundsIsInfinite) {
return;
}
if( boundsIsEmpty ){
planes = new Vector4d[6];
mag = new double[planes.length];
pDotN = new double[planes.length];
nVerts = 1;
verts = new Point3d[nVerts];
verts[0] = new Point3d( point.x, point.y, point.z);
for(i=0;i<planes.length;i++) {
pDotN[i] = 0.0;
}
planes[0] = new Vector4d( 1.0, 0.0, 0.0, -point.x );
planes[1] = new Vector4d(-1.0, 0.0, 0.0, point.x );
planes[2] = new Vector4d( 0.0, 1.0, 0.0, -point.y );
planes[3] = new Vector4d( 0.0,-1.0, 0.0, point.y );
planes[4] = new Vector4d( 0.0, 0.0, 1.0, -point.z );
planes[5] = new Vector4d( 0.0, 0.0,-1.0, point.z );
mag[0] = 1.0;
mag[1] = 1.0;
mag[2] = 1.0;
mag[3] = 1.0;
mag[4] = 1.0;
mag[5] = 1.0;
centroid.x = point.x;
centroid.y = point.y;
centroid.z = point.z;
boundsIsEmpty = false;
boundsIsInfinite = false;
} else {
for(i = 0; i < planes.length; i++){
dis = point.x*planes[i].x + point.y*planes[i].y + point.z*
planes[i].z + planes[i].w;
if( dis > 0.0 ) {
planes[i].w += -dis;
}
}
computeAllVerts();
}
}
/**
* Combines this bounding polytope with an array of points.
* @param points an array of 3d points in space
*/
public void combine(Point3d[] points) {
int i,j;
double dis;
if( boundsIsInfinite) {
return;
}
if( boundsIsEmpty ){
planes = new Vector4d[6];
mag = new double[planes.length];
pDotN = new double[planes.length];
nVerts = points.length;
verts = new Point3d[nVerts];
verts[0] = new Point3d( points[0].x, points[0].y, points[0].z);
for(i=0;i<planes.length;i++) {
pDotN[i] = 0.0;
}
planes[0] = new Vector4d( 1.0, 0.0, 0.0, -points[0].x );
planes[1] = new Vector4d(-1.0, 0.0, 0.0, points[0].x );
planes[2] = new Vector4d( 0.0, 1.0, 0.0, -points[0].y );
planes[3] = new Vector4d( 0.0,-1.0, 0.0, points[0].y );
planes[4] = new Vector4d( 0.0, 0.0, 1.0, -points[0].z );
planes[5] = new Vector4d( 0.0, 0.0,-1.0, points[0].z );
mag[0] = 1.0;
mag[1] = 1.0;
mag[2] = 1.0;
mag[3] = 1.0;
mag[4] = 1.0;
mag[5] = 1.0;
centroid.x = points[0].x;
centroid.y = points[0].y;
centroid.z = points[0].z;
boundsIsEmpty = false;
boundsIsInfinite = false;
}
for(j = 0; j < points.length; j++){
for(i = 0; i < planes.length; i++){
dis = points[j].x*planes[i].x + points[j].y*planes[i].y +
points[j].z*planes[i].z + planes[i].w;
if( dis > 0.0 ) {
planes[i].w += -dis;
}
}
}
computeAllVerts();
}
/**
* Transforms this bounding polytope by the given transformation matrix.
*
* @param matrix
* a transformation matrix
*/
@Override
public void transform(Transform3D matrix) {
int i;
double invMag;
Transform3D invTrans = new Transform3D(matrix);
invTrans.invert();
invTrans.transpose();
for (i = 0; i < planes.length; i++) {
planes[i].x = planes[i].x * mag[i];
planes[i].y = planes[i].y * mag[i];
planes[i].z = planes[i].z * mag[i];
planes[i].w = planes[i].w * mag[i];
invTrans.transform(planes[i]);
}
for (i = 0; i < planes.length; i++) {
// normalize the plane normals
mag[i] = Math.sqrt(planes[i].x * planes[i].x + planes[i].y
* planes[i].y + planes[i].z * planes[i].z);
invMag = 1.0 / mag[i];
this.planes[i] = new Vector4d(planes[i].x * invMag, planes[i].y
* invMag, planes[i].z * invMag, planes[i].w * invMag);
}
for (i = 0; i < verts.length; i++) {
matrix.transform(verts[i]);
}
}
/**
* Test for intersection with another bounds object.
* @param boundsObject another bounds object
* @return true or false indicating if an intersection occured
*/
public boolean intersect(Bounds boundsObject) {
if( boundsObject == null ) {
return false;
}
if( boundsIsEmpty || boundsObject.boundsIsEmpty ) {
return false;
}
if( boundsIsInfinite || boundsObject.boundsIsInfinite ) {
return true;
}
if( boundsObject.boundId == BOUNDING_SPHERE ) {
return intersect_ptope_sphere( this, (BoundingSphere)boundsObject);
} else if( boundsObject.boundId == BOUNDING_BOX){
return intersect_ptope_abox( this, (BoundingBox)boundsObject);
} else if(boundsObject.boundId == BOUNDING_POLYTOPE) {
return intersect_ptope_ptope( this, (BoundingPolytope)boundsObject);
} else {
return false;
}
}
private void computeVertex(int a, int b, int c) {
double det, x, y, z;
det = planes[a].x * planes[b].y * planes[c].z + planes[a].y
* planes[b].z * planes[c].x + planes[a].z * planes[b].x
* planes[c].y - planes[a].z * planes[b].y * planes[c].x
- planes[a].y * planes[b].x * planes[c].z - planes[a].x
* planes[b].z * planes[c].y;
// System.err.println("\n det="+det);
if (det * det < EPSILON) {
// System.err.println("parallel planes="+a+" "+b+" "+c);
return; // two planes are parallel
}
det = 1.0 / det;
x = (planes[b].y * planes[c].z - planes[b].z * planes[c].y) * pDotN[a];
y = (planes[b].z * planes[c].x - planes[b].x * planes[c].z) * pDotN[a];
z = (planes[b].x * planes[c].y - planes[b].y * planes[c].x) * pDotN[a];
x += (planes[c].y * planes[a].z - planes[c].z * planes[a].y) * pDotN[b];
y += (planes[c].z * planes[a].x - planes[c].x * planes[a].z) * pDotN[b];
z += (planes[c].x * planes[a].y - planes[c].y * planes[a].x) * pDotN[b];
x += (planes[a].y * planes[b].z - planes[a].z * planes[b].y) * pDotN[c];
y += (planes[a].z * planes[b].x - planes[a].x * planes[b].z) * pDotN[c];
z += (planes[a].x * planes[b].y - planes[a].y * planes[b].x) * pDotN[c];
x = x * det;
y = y * det;
z = z * det;
if (pointInPolytope(x, y, z)) {
if (nVerts >= verts.length) {
Point3d newVerts[] = new Point3d[nVerts << 1];
for (int i = 0; i < nVerts; i++) {
newVerts[i] = verts[i];
}
verts = newVerts;
}
verts[nVerts++] = new Point3d(x, y, z);
}
}
private void computeAllVerts() {
int i, a, b, c;
double x, y, z;
nVerts = 0;
verts = new Point3d[planes.length * planes.length];
for (i = 0; i < planes.length; i++) {
pDotN[i] = -planes[i].x * planes[i].w * planes[i].x - planes[i].y
* planes[i].w * planes[i].y - planes[i].z * planes[i].w
* planes[i].z;
}
for (a = 0; a < planes.length - 2; a++) {
for (b = a + 1; b < planes.length - 1; b++) {
for (c = b + 1; c < planes.length; c++) {
computeVertex(a, b, c);
}
}
}
// XXXX: correctly compute centroid
x = y = z = 0.0;
Point3d newVerts[] = new Point3d[nVerts];
for (i = 0; i < nVerts; i++) {
x += verts[i].x;
y += verts[i].y;
z += verts[i].z;
// copy the verts into an array of the correct size
newVerts[i] = verts[i];
}
this.verts = newVerts; // copy the verts into an array of the correct
// size
centroid.x = x / nVerts;
centroid.y = y / nVerts;
centroid.z = z / nVerts;
}
private boolean pointInPolytope(double x, double y, double z) {
for (int i = 0; i < planes.length; i++) {
if ((x * planes[i].x + y * planes[i].y + z * planes[i].z + planes[i].w) > EPSILON) {
return false;
}
}
return true;
}
private void checkBoundsIsEmpty() {
boundsIsEmpty = (planes.length < 4);
}
private void initEmptyPolytope() {
planes = new Vector4d[6];
pDotN = new double[6];
mag = new double[6];
verts = new Point3d[planes.length*planes.length];
nVerts = 0;
planes[0] = new Vector4d( 1.0, 0.0, 0.0, -1.0 );
planes[1] = new Vector4d(-1.0, 0.0, 0.0, -1.0 );
planes[2] = new Vector4d( 0.0, 1.0, 0.0, -1.0 );
planes[3] = new Vector4d( 0.0,-1.0, 0.0, -1.0 );
planes[4] = new Vector4d( 0.0, 0.0, 1.0, -1.0 );
planes[5] = new Vector4d( 0.0, 0.0,-1.0, -1.0 );
mag[0] = 1.0;
mag[1] = 1.0;
mag[2] = 1.0;
mag[3] = 1.0;
mag[4] = 1.0;
mag[5] = 1.0;
checkBoundsIsEmpty();
}
}