|
import java.util.Vector;
|
|
class Simplex implements Comparable
|
{
|
|
static boolean SURFACE = true;
|
static boolean SOLID = false; //true;
|
static boolean BOUNDARY = false; // true;
|
static boolean SWAP = false;
|
static boolean includeabcd = true;
|
static int mincount = 1;
|
|
interface Solid
|
{
|
|
boolean inside(double x, double y, double z);
|
|
void push(Simplex simplex);
|
|
void push(Vector simplex);
|
|
void appendTriangle(Vertex p, Vertex q, Vertex r);
|
|
int MinDepth();
|
|
int MaxDepth();
|
|
double Tolerance();
|
//void DumpStack();
|
}
|
|
public Simplex()
|
{
|
outside = true;
|
}
|
|
public Simplex(Point a, Point b, Point c, Point d, int depth)
|
//double ax, double ay, double az,
|
// double bx, double by, double bz,
|
// double cx, double cy, double cz,
|
// double dx, double dy, double dz,
|
{
|
/*
|
this.ax = ax; this.ay = ay; this.az = az;
|
this.bx = bx; this.by = by; this.bz = bz;
|
this.cx = cx; this.cy = cy; this.cz = cz;
|
this.dx = dx; this.dy = dy; this.dz = dz;
|
*/
|
this.a = a;
|
this.b = b;
|
this.c = c;
|
this.d = d;
|
this.depth = (short) depth;
|
|
if (a == null)
|
{
|
return;
|
}
|
|
planes[0] = new Plane(a, b, c, d); // ax,ay,az, bx,by,bz, cx,cy,cz, dx,dy,dz);
|
planes[1] = new Plane(a, b, d, c); // ax,ay,az, bx,by,bz, dx,dy,dz, cx,cy,cz);
|
planes[2] = new Plane(a, c, d, b); // ax,ay,az, cx,cy,cz, dx,dy,dz, bx,by,bz);
|
planes[3] = new Plane(b, c, d, a); // bx,by,bz, cx,cy,cz, dx,dy,dz, ax,ay,az);
|
}
|
static Simplex tempSimplex = new Simplex(null, null, null, null, 0);
|
static Simplex parent; // debug
|
|
Simplex newSimplex(Point a, Point b, Point c, Point d, int depth)
|
{
|
parent = this;
|
|
tempSimplex.a = a;
|
tempSimplex.b = b;
|
tempSimplex.c = c;
|
tempSimplex.d = d;
|
tempSimplex.depth = (short) depth;
|
|
return tempSimplex;
|
}
|
static Vector simplexList = new Vector();
|
static Point testinside = new Point();
|
|
boolean IntersectionSampling()
|
{
|
boolean inside = false;
|
boolean outside = false;
|
|
inside |= a.inside;
|
inside |= b.inside;
|
inside |= c.inside;
|
inside |= d.inside;
|
|
outside |= !a.inside;
|
outside |= !b.inside;
|
outside |= !c.inside;
|
outside |= !d.inside;
|
|
if (inside && outside)
|
{
|
return true;
|
}
|
|
testinside.x = (a.x + b.x + c.x + d.x) / 4;
|
testinside.y = (a.y + b.y + c.y + d.y) / 4;
|
testinside.z = (a.z + b.z + c.z + d.z) / 4;
|
|
boolean i = solid.inside(testinside.x, testinside.y, testinside.z);
|
|
if ((inside != i) && (outside |= !i))
|
{
|
return true;
|
}
|
|
testinside.x = (a.x + b.x + c.x) / 3;
|
testinside.y = (a.y + b.y + c.y) / 3;
|
testinside.z = (a.z + b.z + c.z) / 3;
|
|
i = solid.inside(testinside.x, testinside.y, testinside.z);
|
|
if ((inside != i) && (outside |= !i))
|
{
|
return true;
|
}
|
|
testinside.x = (d.x + b.x + c.x) / 3;
|
testinside.y = (d.y + b.y + c.y) / 3;
|
testinside.z = (d.z + b.z + c.z) / 3;
|
|
i = solid.inside(testinside.x, testinside.y, testinside.z);
|
|
if ((inside != i) && (outside |= !i))
|
{
|
return true;
|
}
|
|
testinside.x = (a.x + d.x + c.x) / 3;
|
testinside.y = (a.y + d.y + c.y) / 3;
|
testinside.z = (a.z + d.z + c.z) / 3;
|
|
i = solid.inside(testinside.x, testinside.y, testinside.z);
|
|
if ((inside != i) && (outside |= !i))
|
{
|
return true;
|
}
|
|
testinside.x = (a.x + b.x + d.x) / 3;
|
testinside.y = (a.y + b.y + d.y) / 3;
|
testinside.z = (a.z + b.z + d.z) / 3;
|
|
i = solid.inside(testinside.x, testinside.y, testinside.z);
|
|
if ((inside != i) && (outside |= !i))
|
{
|
return true;
|
}
|
|
testinside.x = (a.x + b.x) / 2;
|
testinside.y = (a.y + b.y) / 2;
|
testinside.z = (a.z + b.z) / 2;
|
|
i = solid.inside(testinside.x, testinside.y, testinside.z);
|
|
if ((inside != i) && (outside |= !i))
|
{
|
return true;
|
}
|
|
testinside.x = (c.x + b.x) / 2;
|
testinside.y = (c.y + b.y) / 2;
|
testinside.z = (c.z + b.z) / 2;
|
|
i = solid.inside(testinside.x, testinside.y, testinside.z);
|
|
if ((inside != i) && (outside |= !i))
|
{
|
return true;
|
}
|
|
testinside.x = (d.x + b.x) / 2;
|
testinside.y = (d.y + b.y) / 2;
|
testinside.z = (d.z + b.z) / 2;
|
|
i = solid.inside(testinside.x, testinside.y, testinside.z);
|
|
if ((inside != i) && (outside |= !i))
|
{
|
return true;
|
}
|
|
testinside.x = (a.x + d.x) / 2;
|
testinside.y = (a.y + d.y) / 2;
|
testinside.z = (a.z + d.z) / 2;
|
|
i = solid.inside(testinside.x, testinside.y, testinside.z);
|
|
if ((inside != i) && (outside |= !i))
|
{
|
return true;
|
}
|
|
testinside.x = (a.x + c.x) / 2;
|
testinside.y = (a.y + c.y) / 2;
|
testinside.z = (a.z + c.z) / 2;
|
|
i = solid.inside(testinside.x, testinside.y, testinside.z);
|
|
if ((inside != i) && (outside |= !i))
|
{
|
return true;
|
}
|
|
testinside.x = (c.x + d.x) / 2;
|
testinside.y = (c.y + d.y) / 2;
|
testinside.z = (c.z + d.z) / 2;
|
|
i = solid.inside(testinside.x, testinside.y, testinside.z);
|
|
if ((inside != i) && (outside |= !i))
|
{
|
return true;
|
}
|
|
return false;
|
}
|
|
// True if at least one triangle is found
|
boolean subdivide(Solid solid)
|
{
|
assert children == null;
|
|
this.solid = solid;
|
|
if (solid == null)
|
{
|
return true;
|
}
|
|
assert depth <= solid.MaxDepth();
|
|
if (depth == solid.MaxDepth())
|
{
|
return true;
|
}
|
|
if (!postpone && depth >= solid.MinDepth() && !IntersectionSampling())
|
{
|
/**/
|
inside[0] = a.inside; // solid.inside(ax, ay, az);
|
inside[1] = b.inside; // solid.inside(bx, by, bz);
|
inside[2] = c.inside; // solid.inside(cx, cy, cz);
|
inside[3] = d.inside; // solid.inside(dx, dy, dz);
|
|
tryit = true;
|
|
if (inside[0] && inside[1] && inside[2] && inside[3])
|
{
|
tryit = false;
|
}
|
if (!inside[0] && !inside[1] && !inside[2] && !inside[3])
|
{
|
tryit = false;
|
}
|
|
if (!tryit || accept(false))
|
{
|
return true;
|
}
|
/*
|
boolean end = depth >= solid.MaxDepth();
|
if (tryit && accept(end)) // , ax,ay,az, bx,by,bz, cx,cy,cz, dx,dy,dz))
|
{
|
//Vector simplex = new Vector();
|
//for (int i=0; i<4; i++)
|
//{
|
//if (!faces[i].outside && faces[i].depth < depth)
|
//{
|
////faces[i].postpone = true;
|
//assert faces[i].children == null;
|
//simplex.add(faces[i]);
|
//}
|
//}
|
//solid.push(simplex);
|
return true;
|
}
|
if (!tryit) // && end)
|
{
|
//if (SOLID && inside[0])
|
//{
|
//appendSimplex();
|
//}
|
return false;
|
}
|
/**/
|
}
|
|
int count = 0;
|
|
//Vector simplexList = new Vector();
|
simplexList.clear();
|
|
for (int i = 0; i < 4; i++)
|
{
|
/*
|
if (!faces[i].outside && faces[i].depth < depth)
|
{
|
assert faces[i].children == null;
|
count += 1;
|
if (!faces[i].postpone)
|
{
|
faces[i].postpone = true;
|
simplex.add(faces[i]);
|
}
|
}
|
*/
|
if (!faces[i].outside)
|
{
|
parent = this;
|
touched = true;
|
faces[i].GetBigger(planes[i], simplexList);
|
touched = false;
|
parent = null;
|
}
|
}
|
|
if (/*count > 0) //*/simplexList.size() > 0)
|
{
|
//assert !postpone;
|
// System.out.println("MAJOR PROBLEM");
|
solid.push(this);
|
//if (!postpone)
|
{
|
solid.push(simplexList);
|
postpone = true;
|
}
|
} else
|
{
|
assert children == null;
|
|
children = new Vector(simplexList);
|
|
// Subdivide
|
double abx = (a.x + b.x) / 2;
|
double aby = (a.y + b.y) / 2;
|
double abz = (a.z + b.z) / 2;
|
double acx = (a.x + c.x) / 2;
|
double acy = (a.y + c.y) / 2;
|
double acz = (a.z + c.z) / 2;
|
double adx = (a.x + d.x) / 2;
|
double ady = (a.y + d.y) / 2;
|
double adz = (a.z + d.z) / 2;
|
double bcx = (b.x + c.x) / 2;
|
double bcy = (b.y + c.y) / 2;
|
double bcz = (b.z + c.z) / 2;
|
double cdx = (c.x + d.x) / 2;
|
double cdy = (c.y + d.y) / 2;
|
double cdz = (c.z + d.z) / 2;
|
double dbx = (d.x + b.x) / 2;
|
double dby = (d.y + b.y) / 2;
|
double dbz = (d.z + b.z) / 2;
|
|
Point ab = new Point(abx, aby, abz);
|
Point ac = new Point(acx, acy, acz);
|
Point ad = new Point(adx, ady, adz);
|
Point bc = new Point(bcx, bcy, bcz);
|
Point cd = new Point(cdx, cdy, cdz);
|
Point db = new Point(dbx, dby, dbz);
|
|
children.add(new Simplex(a, // ax,ay,az,
|
ab, // x,aby,abz,
|
ac, // x,acy,acz,
|
ad, // x,ady,adz,
|
depth + 1));
|
children.add(new Simplex(b, // x,by,bz,
|
ab, // x,aby,abz,
|
bc, // x,bcy,bcz,
|
db, // x,dby,dbz,
|
depth + 1));
|
children.add(new Simplex(c, // x,cy,cz,
|
ac, // x,acy,acz,
|
bc, // x,bcy,bcz,
|
cd, // x,cdy,cdz,
|
depth + 1));
|
children.add(new Simplex(d, // x,dy,dz,
|
ad, // x,ady,adz,
|
cd, // x,cdy,cdz,
|
db, // x,dby,dbz,
|
depth + 1));
|
|
v0.x = abx;
|
v0.y = aby;
|
v0.z = abz;
|
v1.x = cdx;
|
v1.y = cdy;
|
v1.z = cdz;
|
//LA.vecSub(v0,v1, v2);
|
double diag0 = LA.distance2(v0, v1);
|
|
v0.x = acx;
|
v0.y = acy;
|
v0.z = acz;
|
v1.x = dbx;
|
v1.y = dby;
|
v1.z = dbz;
|
//LA.vecSub(v0,v1, v2);
|
double diag1 = LA.distance2(v0, v1);
|
|
v0.x = adx;
|
v0.y = ady;
|
v0.z = adz;
|
v1.x = bcx;
|
v1.y = bcy;
|
v1.z = bcz;
|
//LA.vecSub(v0,v1, v2);
|
double diag2 = LA.distance2(v0, v1);
|
|
int which = 0;
|
double min = diag0;
|
|
if (min > diag1)
|
{
|
min = diag1;
|
which = 1;
|
}
|
|
if (min > diag2)
|
{
|
min = diag2;
|
which = 2;
|
}
|
|
// Central octahedron
|
switch (which) // depth % 3)
|
{
|
case 0:
|
children.add(new Simplex(ab, // x,aby,abz,
|
cd, // x,cdy,cdz,
|
bc, // x,bcy,bcz,
|
db, // x,dby,dbz,
|
depth + 1));
|
children.add(new Simplex(ab, // x,aby,abz,
|
cd, // x,cdy,cdz,
|
db, // x,dby,dbz,
|
ad, // x,ady,adz,
|
depth + 1));
|
children.add(new Simplex(ab, // x,aby,abz,
|
cd, // x,cdy,cdz,
|
ad, // x,ady,adz,
|
ac, // x,acy,acz,
|
depth + 1));
|
children.add(new Simplex(ab, // x,aby,abz,
|
cd, // x,cdy,cdz,
|
ac, // x,acy,acz,
|
bc, // x,bcy,bcz,
|
depth + 1));
|
break;
|
case 1:
|
children.add(new Simplex(ac, // x,acy,acz,
|
db, // x,dby,dbz,
|
ab, // x,aby,abz,
|
ad, // x,ady,adz,
|
depth + 1));
|
children.add(new Simplex(ac, // x,acy,acz,
|
db, // x,dby,dbz,
|
ad, // x,ady,adz,
|
cd, // x,cdy,cdz,
|
depth + 1));
|
children.add(new Simplex(ac, // x,acy,acz,
|
db, // x,dby,dbz,
|
cd, // x,cdy,cdz,
|
bc, // x,bcy,bcz,
|
depth + 1));
|
children.add(new Simplex(ac, // x,acy,acz,
|
db, // x,dby,dbz,
|
bc, // x,bcy,bcz,
|
ab, // x,aby,abz,
|
depth + 1));
|
break;
|
case 2:
|
children.add(new Simplex(ad, // x,ady,adz,
|
bc, // x,bcy,bcz,
|
ab, // x,aby,abz,
|
ac, // x,acy,acz,
|
depth + 1));
|
children.add(new Simplex(ad, // x,ady,adz,
|
bc, // x,bcy,bcz,
|
ac, // x,acy,acz,
|
cd, // x,cdy,cdz,
|
depth + 1));
|
children.add(new Simplex(ad, // x,ady,adz,
|
bc, // x,bcy,bcz,
|
cd, // x,cdy,cdz,
|
db, // x,dby,dbz,
|
depth + 1));
|
children.add(new Simplex(ad, // x,ady,adz,
|
bc, // x,bcy,bcz,
|
db, // x,dby,dbz,
|
ab, // x,aby,abz,
|
depth + 1));
|
break;
|
default:
|
}
|
|
LinkSubSimplex(children);
|
|
solid.push(children);
|
}
|
|
return false;
|
}
|
|
void GetBigger(Plane p, Vector simplex)
|
{
|
if (!touched && !outside)
|
{
|
if (depth < parent.depth) // || (depth == parent.depth && parent.tryit))
|
{
|
assert children == null;
|
|
if (!postpone)
|
{
|
postpone = true;
|
simplex.add(this);
|
}
|
}
|
|
touched = true;
|
for (int i = 0; i < 4; i++)
|
{
|
if (faces[i].outside || faces[i].touched)
|
{
|
continue;
|
}
|
|
int count = 0;
|
/**/
|
if (p.inplane(faces[i].a) &&
|
//!faces[i].a.equals(parent.a) &&
|
//!faces[i].a.equals(parent.b) &&
|
//!faces[i].a.equals(parent.c) &&
|
//!faces[i].a.equals(parent.d) &&
|
parent.Contains(faces[i].a, includeabcd))
|
{
|
count++;
|
}
|
if (p.inplane(faces[i].b) &&
|
//!faces[i].b.equals(parent.a) &&
|
//!faces[i].b.equals(parent.b) &&
|
//!faces[i].b.equals(parent.c) &&
|
//!faces[i].b.equals(parent.d) &&
|
parent.Contains(faces[i].b, includeabcd))
|
{
|
count++;
|
}
|
if (p.inplane(faces[i].c) &&
|
//!faces[i].c.equals(parent.a) &&
|
//!faces[i].c.equals(parent.b) &&
|
//!faces[i].c.equals(parent.c) &&
|
//!faces[i].c.equals(parent.d) &&
|
parent.Contains(faces[i].c, includeabcd))
|
{
|
count++;
|
}
|
if (p.inplane(faces[i].d) &&
|
//!faces[i].d.equals(parent.a) &&
|
//!faces[i].d.equals(parent.b) &&
|
//!faces[i].d.equals(parent.c) &&
|
//!faces[i].d.equals(parent.d) &&
|
parent.Contains(faces[i].d, includeabcd))
|
{
|
count++;
|
}
|
/**/
|
//if (p.inplane(faces[i].a)) count++;
|
//if (p.inplane(faces[i].b)) count++;
|
//if (p.inplane(faces[i].c)) count++;
|
//if (p.inplane(faces[i].d)) count++;
|
if (count > mincount)
|
{
|
faces[i].GetBigger(p, simplex);
|
}
|
}
|
touched = false;
|
}
|
}
|
|
static int LinkSimplex(Vector simplex)
|
{
|
int link = 0;
|
for (int i = 0; i < simplex.size(); i++)
|
{
|
for (int j = i + 1; j < simplex.size(); j++)
|
{
|
Simplex si = (Simplex) simplex.get(i);
|
Simplex sj = (Simplex) simplex.get(j);
|
|
assert si.depth == sj.depth;
|
|
for (int k = 0; k < 4; k++)
|
{
|
for (int l = 0; l < 4; l++)
|
{
|
if ((si.planes[k]).opposite(sj.planes[l]))
|
{
|
si.faces[k] = sj;
|
sj.faces[l] = si;
|
link += 1;
|
}
|
}
|
}
|
}
|
}
|
return link;
|
}
|
|
static void SetRoot(Vector simplex)
|
{
|
for (int i = 0; i < simplex.size(); i++)
|
{
|
Simplex si = (Simplex) simplex.get(i);
|
|
for (int k = 0; k < 4; k++)
|
{
|
if (si.faces[k] == null)
|
{
|
si.faces[k] = OUTSIDE;
|
}
|
}
|
}
|
}
|
static Point[] centers = new Point[3];
|
|
void ShareFace(/*Point center,*/Plane p) //, Vector edgecenters)
|
{
|
if (outside)
|
{
|
return;
|
}
|
|
//if (p.b == -1)
|
//System.out.println("plane = " + p);
|
|
//if (children == null)
|
//{
|
for (int i = 0; i < 4; i++)
|
{
|
if (planes[i].sameplane(p))
|
{
|
Point pa = null, pb = null, pc = null;
|
switch (i)
|
{
|
case 0:
|
pa = a;
|
pb = b;
|
pc = c;
|
break;
|
case 1:
|
pa = a;
|
pb = b;
|
pc = d;
|
break;
|
case 2:
|
pa = a;
|
pb = c;
|
pc = d;
|
break;
|
case 3:
|
pa = b;
|
pb = c;
|
pc = d;
|
break;
|
default:
|
assert false;
|
}
|
|
int count = 0;
|
|
for (int j = 0; j < edgecenters.size(); j++)
|
{
|
Point pt = (Point) edgecenters.get(j);
|
if (p.inplane(pt))
|
{
|
if (Contains(pt, false))
|
{
|
if (count >= 3)
|
{
|
assert count < 3;
|
}
|
centers[count++] = (Point) edgecenters.get(j);
|
}
|
}
|
}
|
|
//assert count <= 3 && edgecenters.size() - count <= 3;
|
LinkFace(pa, pb, pc, count);
|
|
break;
|
}
|
}
|
//}
|
//else
|
//{
|
//for (int i=0; i<children.size(); i++)
|
//{
|
//Simplex si = (Simplex) children.get(i);
|
|
//si.ShareFace(center, p, edgecenters);
|
//}
|
//}
|
}
|
|
void LinkFaces()
|
{
|
assert !outside;
|
|
assert children == null;
|
|
for (int i = 0; i < 4; i++)
|
{
|
Plane p = planes[i];
|
|
Point pa = null, pb = null, pc = null;
|
|
switch (i)
|
{
|
case 0:
|
pa = a;
|
pb = b;
|
pc = c;
|
break;
|
case 1:
|
pa = a;
|
pb = b;
|
pc = d;
|
break;
|
case 2:
|
pa = a;
|
pb = c;
|
pc = d;
|
break;
|
case 3:
|
pa = b;
|
pb = c;
|
pc = d;
|
break;
|
default:
|
assert false;
|
}
|
|
int count = 0;
|
|
for (int j = 0; j < edgecenters.size(); j++)
|
{
|
Point pt = (Point) edgecenters.get(j);
|
if (p.inplane(pt))
|
{
|
if (Contains(pt, false))
|
{
|
if (count >= 3)
|
{
|
//assert count < 3;
|
System.err.println("COUNT >= 3!!");
|
break;
|
}
|
centers[count++] = pt;
|
}
|
}
|
}
|
|
LinkFace(pa, pb, pc, count);
|
}
|
}
|
|
void LinkFace(Point pa, Point pb, Point pc, int count)
|
{
|
double max;
|
double dist;
|
int which;
|
|
switch (count)
|
{
|
case 0:
|
newSimplex(pa, pb, pc, center, -1).TestLeaf();
|
break;
|
case 1:
|
max = LA.distance2(centers[0], pa);
|
which = 0;
|
dist = LA.distance2(centers[0], pb);
|
if (max < dist)
|
{
|
which = 1;
|
max = dist;
|
}
|
dist = LA.distance2(centers[0], pc);
|
if (max < dist)
|
{
|
which = 2;
|
//max = dist;
|
}
|
|
switch (which)
|
{
|
case 0:
|
newSimplex(centers[0], pa, pb, center, -1).TestLeaf();
|
newSimplex(centers[0], pa, pc, center, -1).TestLeaf();
|
break;
|
case 1:
|
newSimplex(centers[0], pb, pc, center, -1).TestLeaf();
|
newSimplex(centers[0], pb, pa, center, -1).TestLeaf();
|
break;
|
case 2:
|
newSimplex(centers[0], pc, pa, center, -1).TestLeaf();
|
newSimplex(centers[0], pc, pb, center, -1).TestLeaf();
|
break;
|
default:
|
assert false;
|
}
|
break;
|
case 2:
|
max = LA.distance2(centers[0], pa);
|
which = 0;
|
dist = LA.distance2(centers[1], pa);
|
if (max < dist)
|
{
|
max = dist;
|
}
|
double min = max;
|
max = LA.distance2(centers[0], pb);
|
dist = LA.distance2(centers[1], pb);
|
if (max < dist)
|
{
|
max = dist;
|
}
|
|
if (min > max)
|
{
|
which = 1;
|
min = max;
|
}
|
|
max = LA.distance2(centers[0], pc);
|
dist = LA.distance2(centers[1], pc);
|
if (max < dist)
|
{
|
max = dist;
|
}
|
|
if (min > max)
|
{
|
which = 2;
|
//min = max;
|
}
|
|
switch (which)
|
{
|
case 0:
|
newSimplex(centers[0], centers[1], pa, center, -1).TestLeaf();
|
newSimplex(centers[0], centers[1], pb, center, -1).TestLeaf();
|
if (oppositeSide(centers[0], centers[1], pb, pc))
|
{
|
newSimplex(centers[1], pb, pc, center, -1).TestLeaf();
|
} else
|
{
|
newSimplex(centers[0], pb, pc, center, -1).TestLeaf();
|
}
|
break;
|
case 1:
|
newSimplex(centers[0], centers[1], pb, center, -1).TestLeaf();
|
newSimplex(centers[0], centers[1], pc, center, -1).TestLeaf();
|
if (oppositeSide(centers[0], centers[1], pc, pa))
|
{
|
newSimplex(centers[1], pc, pa, center, -1).TestLeaf();
|
} else
|
{
|
newSimplex(centers[0], pc, pa, center, -1).TestLeaf();
|
}
|
break;
|
case 2:
|
newSimplex(centers[0], centers[1], pc, center, -1).TestLeaf();
|
newSimplex(centers[0], centers[1], pa, center, -1).TestLeaf();
|
if (oppositeSide(centers[0], centers[1], pa, pb))
|
{
|
newSimplex(centers[1], pa, pb, center, -1).TestLeaf();
|
} else
|
{
|
newSimplex(centers[0], pa, pb, center, -1).TestLeaf();
|
}
|
break;
|
default:
|
assert false;
|
}
|
break;
|
case 3:
|
GetTriangle(pa); //, center);
|
GetTriangle(pb); //, center);
|
GetTriangle(pc); //, center);
|
newSimplex(centers[0], centers[1], centers[2], center, -1).TestLeaf();
|
break;
|
default:
|
assert false;
|
break;
|
}
|
}
|
|
void GetTriangle(Point p) // , Point center)
|
{
|
double max = LA.distance2(centers[0], p);
|
int which = 0;
|
double dist = LA.distance2(centers[1], p);
|
if (max < dist)
|
{
|
which = 1;
|
max = dist;
|
}
|
dist = LA.distance2(centers[2], p);
|
if (max < dist)
|
{
|
which = 2;
|
//max = dist;
|
}
|
|
switch (which)
|
{
|
case 0:
|
newSimplex(centers[1], centers[2], p, center, -1).TestLeaf();
|
break;
|
case 1:
|
newSimplex(centers[0], centers[2], p, center, -1).TestLeaf();
|
break;
|
case 2:
|
newSimplex(centers[0], centers[1], p, center, -1).TestLeaf();
|
break;
|
default:
|
assert false;
|
}
|
}
|
|
void GetEdgeCenters(Plane p, int level) // , Vector edgecenters)
|
{
|
if (level == 0)
|
{
|
return;
|
}
|
|
//System.out.println("Simplex = " + this);
|
if (!touched && !outside)
|
{
|
//Applet3D.tracein("GetEdgeCenters = " + this);
|
if (children != null)
|
{
|
for (int i = 0; i < children.size(); i++)
|
{
|
Simplex child = (Simplex) children.get(i);
|
|
Point pt = child.a;
|
if (p.inplane(pt)) // && !pt.equals(parent.a) && !pt.equals(parent.b) &&
|
//!pt.equals(parent.c) && !pt.equals(parent.d))
|
{
|
if (!edgecenters.contains(pt) && parent.Contains(pt, false))
|
{
|
edgecenters.add(pt);
|
}
|
}
|
|
//if (edgecenters.size() - prevsize == 3) return;
|
|
pt = child.b;
|
if (p.inplane(pt)) // && !pt.equals(parent.a) && !pt.equals(parent.b) &&
|
//!pt.equals(parent.c) && !pt.equals(parent.d))
|
{
|
if (!edgecenters.contains(pt) && parent.Contains(pt, false))
|
{
|
edgecenters.add(pt);
|
}
|
}
|
|
//if (edgecenters.size() - prevsize == 3) return;
|
|
pt = child.c;
|
if (p.inplane(pt)) // && !pt.equals(parent.a) && !pt.equals(parent.b) &&
|
//!pt.equals(parent.c) && !pt.equals(parent.d))
|
{
|
if (!edgecenters.contains(pt) && parent.Contains(pt, false))
|
{
|
edgecenters.add(pt);
|
}
|
}
|
|
//if (edgecenters.size() - prevsize == 3) return;
|
|
pt = child.d;
|
if (p.inplane(pt)) // && !pt.equals(parent.a) && !pt.equals(parent.b) &&
|
//!pt.equals(parent.c) && !pt.equals(parent.d))
|
{
|
if (!edgecenters.contains(pt) && parent.Contains(pt, false))
|
{
|
edgecenters.add(pt);
|
}
|
}
|
|
//if (edgecenters.size() - prevsize == 3) return;
|
}
|
}
|
|
touched = true;
|
for (int i = 0; i < 4; i++)
|
{
|
if (faces[i].outside || faces[i].touched)
|
{
|
continue;
|
}
|
|
//assert faces[i].depth <= parent.depth;
|
|
int count = 0;
|
/**/
|
if (p.inplane(faces[i].a) &&
|
//!faces[i].a.equals(parent.a) &&
|
//!faces[i].a.equals(parent.b) &&
|
//!faces[i].a.equals(parent.c) &&
|
//!faces[i].a.equals(parent.d) &&
|
parent.Contains(faces[i].a, includeabcd))
|
{
|
count++;
|
}
|
if (p.inplane(faces[i].b) &&
|
//!faces[i].b.equals(parent.a) &&
|
//!faces[i].b.equals(parent.b) &&
|
//!faces[i].b.equals(parent.c) &&
|
//!faces[i].b.equals(parent.d) &&
|
parent.Contains(faces[i].b, includeabcd))
|
{
|
count++;
|
}
|
if (p.inplane(faces[i].c) &&
|
//!faces[i].c.equals(parent.a) &&
|
//!faces[i].c.equals(parent.b) &&
|
//!faces[i].c.equals(parent.c) &&
|
//!faces[i].c.equals(parent.d) &&
|
parent.Contains(faces[i].c, includeabcd))
|
{
|
count++;
|
}
|
if (p.inplane(faces[i].d) &&
|
//!faces[i].d.equals(parent.a) &&
|
//!faces[i].d.equals(parent.b) &&
|
//!faces[i].d.equals(parent.c) &&
|
//!faces[i].d.equals(parent.d) &&
|
parent.Contains(faces[i].d, includeabcd))
|
{
|
count++;
|
}
|
/**/
|
//if (p.inplane(faces[i].a)) count++;
|
//if (p.inplane(faces[i].b)) count++;
|
//if (p.inplane(faces[i].c)) count++;
|
//if (p.inplane(faces[i].d)) count++;
|
if (count > mincount)
|
{
|
faces[i].GetEdgeCenters(p, level - 1);
|
}
|
|
if (edgecenters.size() - prevsize == 3)
|
{
|
//touched = false;
|
//return;
|
}
|
}
|
touched = false;
|
|
//Applet3D.traceout();
|
}
|
}
|
|
boolean Contains(Point p, boolean includeABCD)
|
{
|
assert !outside;
|
|
for (int i = 0; i < 4; i++)
|
{
|
if (planes[i].distance(p) > 0.000001)
|
{
|
return false;
|
}
|
}
|
|
if (includeABCD)
|
{
|
return true;
|
}
|
|
return !p.equals(a) && !p.equals(b) && !p.equals(c) && !p.equals(d);
|
}
|
static int DEPTH = 3;
|
static Point center = new Point();
|
static Vector edgecenters = new Vector();
|
boolean touched = false;
|
static int prevsize;
|
|
void GenerateTriangles()
|
{
|
//if (DEPTH != -1 && depth != DEPTH) //
|
//return;
|
|
if (children != null)
|
{
|
for (int i = 0; i < children.size(); i++)
|
{
|
Simplex si = (Simplex) children.get(i);
|
|
si.GenerateTriangles();
|
}
|
} else
|
{
|
/*
|
int samelevel = 4;
|
for (int i=0; i<4; i++)
|
{
|
//if (faces[i].outside)
|
// continue;
|
//if (faces[i].depth != depth)
|
//System.out.println("DEPTH = " + (faces[i].depth - depth));
|
if (faces[i].children != null) // faces[i].depth > depth) // watch for surface cracks
|
{
|
samelevel -= 1;
|
//break;
|
}
|
}
|
*/
|
|
if (true) // samelevel < 4)
|
{
|
//System.out.println("SAME LEVEL = " + samelevel);
|
center.x = (a.x + b.x + c.x + d.x) / 4;
|
center.y = (a.y + b.y + c.y + d.y) / 4;
|
center.z = (a.z + b.z + c.z + d.z) / 4;
|
center.inside = solid.inside(center.x, center.y, center.z);
|
|
//Vector edgecenters = new Vector();
|
edgecenters.clear();
|
|
for (int i = 0; i < 4; i++)
|
{
|
prevsize = edgecenters.size();
|
parent = this;
|
touched = true;
|
faces[i].GetEdgeCenters(planes[i], 1); // , edgecenters);
|
touched = false;
|
parent = null;
|
assert edgecenters.size() - prevsize <= 3;
|
}
|
|
/*
|
switch (samelevel)
|
{
|
case 3 : assert edgecenters.size() == 3; break;
|
case 2 : assert edgecenters.size() == 5; break;
|
case 1 :
|
case 0 : assert edgecenters.size() == 6;
|
}
|
*/
|
|
assert edgecenters.size() <= 6;
|
|
if (edgecenters.size() == 0)
|
{
|
parent = this;
|
TestLeaf();
|
parent = null;
|
} else
|
{
|
//SWAP = true;
|
/*
|
for (int i=0; i<4; i++)
|
{
|
faces[i].ShareFace(planes[i]); // center, edgecenters);
|
}
|
/**/
|
LinkFaces();
|
SWAP = false;
|
}
|
} else
|
{
|
parent = this;
|
TestLeaf();
|
parent = null;
|
}
|
}
|
}
|
|
void TestLeaf()
|
{
|
inside[0] = a.inside; // solid.inside(ax, ay, az);
|
inside[1] = b.inside; // solid.inside(bx, by, bz);
|
inside[2] = c.inside; // solid.inside(cx, cy, cz);
|
inside[3] = d.inside; // solid.inside(dx, dy, dz);
|
|
tryit = true;
|
|
if (inside[0] && inside[1] && inside[2] && inside[3])
|
{
|
tryit = false;
|
}
|
if (!inside[0] && !inside[1] && !inside[2] && !inside[3])
|
{
|
tryit = false;
|
}
|
|
if (!tryit)
|
{
|
if (SOLID && inside[0])
|
{
|
appendSimplex();
|
}
|
} else
|
{
|
boolean forced = true; // depth < solid.MaxDepth();
|
|
if (forced)
|
{
|
boolean acc = accept(forced);
|
|
assert acc;
|
}
|
}
|
}
|
|
void appendSimplex()
|
{
|
if (!locked)
|
{
|
locked = true;
|
pp.x = a.x;
|
pp.y = a.y;
|
pp.z = a.z;
|
qq.x = b.x;
|
qq.y = b.y;
|
qq.z = b.z;
|
rr.x = c.x;
|
rr.y = c.y;
|
rr.z = c.z;
|
ss.x = d.x;
|
ss.y = d.y;
|
ss.z = d.z;
|
double refx = (a.x + b.x + c.x + d.x) / 4;
|
double refy = (a.y + b.y + c.y + d.y) / 4;
|
double refz = (a.z + b.z + c.z + d.z) / 4;
|
appendTriangleRef(pp, qq, rr, refx, refy, refz, true);
|
appendTriangleRef(pp, qq, ss, refx, refy, refz, true);
|
appendTriangleRef(pp, rr, ss, refx, refy, refz, true);
|
appendTriangleRef(qq, rr, ss, refx, refy, refz, true);
|
locked = false;
|
}
|
}
|
|
static void GenerateTriangles(Vector simplex)
|
{
|
for (int i = 0; i < simplex.size(); i++)
|
{
|
Simplex si = (Simplex) simplex.get(i);
|
|
si.invariants();
|
si.GenerateTriangles();
|
}
|
}
|
static Simplex OUTSIDE = new Simplex();
|
|
void LinkSubSimplex(Vector simplex)
|
{
|
int links = LinkSimplex(simplex);
|
|
if (simplex.size() == 12)
|
{
|
assert links == 18;
|
}
|
|
if (simplex.size() == 8)
|
{
|
if (links != 8)
|
{
|
if (links == 0)
|
{
|
return;
|
}
|
|
System.out.println("links = " + links);
|
assert links == 8; // || links == 0;
|
}
|
}
|
|
//System.out.println("Children of " + this);
|
for (int i = 0; i < simplex.size(); i++)
|
{
|
Simplex si = (Simplex) simplex.get(i);
|
//System.out.println(" --> " + si);
|
for (int f = 0; f < 4; f++)
|
{
|
assert (faces[f] != null);
|
//continue;
|
|
LinkFace(si, f);
|
//assert false;
|
}
|
|
//si.invariants();
|
}
|
}
|
|
boolean LinkFace(Simplex si, int f)
|
{
|
for (int sf = 0; sf < 4; sf++)
|
{
|
if (si.planes[sf].equals(planes[f]))
|
{
|
assert si.faces[sf] == null;
|
if ((faces[f]).depth != depth)
|
{
|
//System.out.println((faces[f]) + " VS " + this);
|
}
|
Vector neighbors = faces[f].children;
|
if (neighbors == null)
|
{
|
assert (faces[f]).outside || (faces[f]).depth + 1 == si.depth;
|
si.faces[sf] = faces[f];
|
assert faces[f].outside || faces[f].faces[0] == this ||
|
faces[f].faces[1] == this ||
|
faces[f].faces[2] == this ||
|
faces[f].faces[3] == this;
|
return true;
|
} else
|
{
|
assert (faces[f]).outside || (faces[f]).depth + 1 == si.depth;
|
//assert neighbors.size() > 0;
|
for (int n = 0; n < neighbors.size(); n++)
|
{
|
Simplex sn = (Simplex) neighbors.get(n);
|
assert sn.depth == si.depth;
|
for (int j = 0; j < 4; j++)
|
{
|
if (!sn.faces[j].outside)
|
{
|
if (sn.planes[j].opposite(si.planes[sf]))
|
{
|
assert sn.faces[j] == this;
|
sn.faces[j] = si;
|
si.faces[sf] = sn;
|
return true;
|
}
|
}
|
}
|
}
|
}
|
}
|
}
|
|
return false;
|
}
|
|
boolean accept(boolean forced) //, double ax, double ay, double az,
|
//double bx, double by, double bz,
|
//double cx, double cy, double cz,
|
//double dx, double dy, double dz)
|
{
|
//cVector[] triangle = new cVector[4];
|
|
if (!forced && solid.Tolerance() == 0) // + 1E-5)
|
{
|
return false;
|
}
|
|
//Applet3D.tracein("acceptSimplex");
|
|
int i = 0;
|
|
interindex = 0;
|
|
if (inside[0] ^ inside[1])
|
{
|
triangle[i++] = intersect(a, b);
|
} // ax,ay,az,bx,by,bz, inside[0]);
|
if (inside[0] ^ inside[2])
|
{
|
triangle[i++] = intersect(a, c);
|
} // ax,ay,az,cx,cy,cz, inside[0]);
|
if (inside[0] ^ inside[3])
|
{
|
triangle[i++] = intersect(a, d);
|
} // ax,ay,az,dx,dy,dz, inside[0]);
|
if (inside[1] ^ inside[2])
|
{
|
triangle[i++] = intersect(b, c);
|
} // bx,by,bz,cx,cy,cz, inside[1]);
|
if (inside[1] ^ inside[3])
|
{
|
triangle[i++] = intersect(b, d);
|
} // bx,by,bz,dx,dy,dz, inside[1]);
|
if (inside[2] ^ inside[3])
|
{
|
triangle[i++] = intersect(c, d);
|
} // cx,cy,cz,dx,dy,dz, inside[2]);
|
|
if (i != 3 && i != 4)
|
//new Exception("Must have 3-4 intersections").printStackTrace();
|
{
|
System.out.println("Must have 3-4 intersections: " + i);
|
}
|
|
//if (i == 3) return maxdepth;
|
|
if (i == 4 && !forced)
|
{
|
//if (!forced)
|
//return false; // reject now?
|
|
/*
|
if (!coplanar(triangle[0], triangle[1], triangle[2], triangle[3]))
|
{
|
Applet3D.traceout();
|
return false;
|
}
|
*/
|
|
simplex[0] = a; // .x = a.x; simplex[0].y = a.y; simplex[0].z = a.z;
|
simplex[1] = b; // .x = b.x; simplex[1].y = b.y; simplex[1].z = b.z;
|
simplex[2] = c; // .x = c.x; simplex[2].y = c.y; simplex[2].z = c.z;
|
simplex[3] = d; // .x = d.x; simplex[3].y = d.y; simplex[3].z = d.z;
|
|
p0.x = 0;
|
p0.y = 0;
|
p0.z = 0;
|
p1.x = 0;
|
p1.y = 0;
|
p1.z = 0;
|
|
for (int l = 0; l < 4; l++)
|
{
|
if (inside[l])
|
{
|
p0.x += simplex[l].x / 2;
|
p0.y += simplex[l].y / 2;
|
p0.z += simplex[l].z / 2;
|
} else
|
{
|
p1.x += simplex[l].x / 2;
|
p1.y += simplex[l].y / 2;
|
p1.z += simplex[l].z / 2;
|
}
|
}
|
|
p0.inside = solid.inside(p0.x, p0.y, p0.z);
|
p1.inside = solid.inside(p1.x, p1.y, p1.z);
|
|
LA.vecSub(p0, p1, v2);
|
LA.vecNormalize(v2);
|
|
cVector inter = intersect(p0, p1); //v0.x, v0.y, v0.z,
|
//v1.x, v1.y, v1.z,
|
//true);
|
|
v0.x = (triangle[0].x + triangle[1].x + triangle[2].x + triangle[3].x) / 4;
|
v0.y = (triangle[0].y + triangle[1].y + triangle[2].y + triangle[3].y) / 4;
|
v0.z = (triangle[0].z + triangle[1].z + triangle[2].z + triangle[3].z) / 4;
|
|
/*
|
if (oppositeSide(triangle[0], triangle[1], triangle[2], triangle[3]))
|
{
|
v0.x = (triangle[1].x + triangle[2].x)/2;
|
v0.y = (triangle[1].y + triangle[2].y)/2;
|
v0.z = (triangle[1].z + triangle[2].z)/2;
|
}
|
else if (oppositeSide(triangle[1], triangle[0], triangle[2], triangle[3]))
|
{
|
v0.x = (triangle[0].x + triangle[2].x)/2;
|
v0.y = (triangle[0].y + triangle[2].y)/2;
|
v0.z = (triangle[0].z + triangle[2].z)/2;
|
}
|
else if (oppositeSide(triangle[2], triangle[0], triangle[1], triangle[3]))
|
{
|
v0.x = (triangle[0].x + triangle[1].x)/2;
|
v0.y = (triangle[0].y + triangle[1].y)/2;
|
v0.z = (triangle[0].z + triangle[1].z)/2;
|
}
|
else System.out.println("NO OTHER TRIANGLE");
|
/**/
|
|
//LA.vecSub(v0,v4, v3);
|
//System.out.println("length = " + LA.vecLen(v3));
|
|
LA.vecSub(inter, p0, v1);
|
|
//System.out.println("dot4 = " + Math.abs(LA.vecDot(v1,v2)));
|
|
if (Math.abs(LA.vecDot(v1, v2)) > solid.Tolerance()) // + 1E-5)
|
{
|
//Applet3D.traceout();
|
return false;
|
}
|
}
|
|
if (i == 3 && !forced)
|
{
|
//if (!forced)
|
//return false; // reject now
|
|
int count = 0;
|
|
for (int l = 0; l < 4; l++)
|
{
|
if (inside[l])
|
{
|
count++;
|
}
|
}
|
|
int which;
|
|
if (count == 3) // get false one
|
{
|
for (which = 0; which < 4; which++)
|
{
|
if (!inside[which])
|
{
|
break;
|
}
|
}
|
} else // get true one
|
{
|
if (count != 1)
|
{
|
System.out.println("COUNT != 1");
|
}
|
for (which = 0; which < 4; which++)
|
{
|
if (inside[which])
|
{
|
break;
|
}
|
}
|
}
|
|
simplex[0] = a; // .x = a.x; simplex[0].y = a.y; simplex[0].z = a.z;
|
simplex[1] = b; // .x = b.x; simplex[1].y = b.y; simplex[1].z = b.z;
|
simplex[2] = c; // .x = c.x; simplex[2].y = c.y; simplex[2].z = c.z;
|
simplex[3] = d; // .x = d.x; simplex[3].y = d.y; simplex[3].z = d.z;
|
|
cVector vert1 = simplex[(which + 1) % 4];
|
cVector vert2 = simplex[(which + 2) % 4];
|
cVector vert3 = simplex[(which + 3) % 4];
|
|
p2.x = (vert1.x + vert2.x + vert3.x) / 3;
|
p2.y = (vert1.y + vert2.y + vert3.y) / 3;
|
p2.z = (vert1.z + vert2.z + vert3.z) / 3;
|
p2.inside = solid.inside(p2.x, p2.y, p2.z);
|
if (p2.inside == inside[which])
|
{
|
//Applet3D.traceout();
|
return false;
|
}
|
|
cVector inter = intersect(simplex[which], // .x, simplex[which].y, simplex[which].z,
|
p2); // v2.x, v2.y, v2.z,
|
//inside[which]);
|
|
LA.vecSub(triangle[0], triangle[1], v0);
|
LA.vecSub(triangle[0], triangle[2], v1);
|
LA.vecCross(v0, v1, v2);
|
LA.vecNormalize(v2);
|
|
v0.x = (triangle[0].x + triangle[1].x + triangle[2].x) / 3;
|
v0.y = (triangle[0].y + triangle[1].y + triangle[2].y) / 3;
|
v0.z = (triangle[0].z + triangle[1].z + triangle[2].z) / 3;
|
|
LA.vecSub(inter, v0, v1);
|
|
//System.out.println("dot3 = " + Math.abs(LA.vecDot(v1,v2)));
|
|
if (Math.abs(LA.vecDot(v1, v2)) > solid.Tolerance()) // + 1E-5)
|
{
|
//Applet3D.traceout();
|
return false;
|
}
|
}
|
|
if (/*SURFACE &&*/forced)
|
{
|
parent = this;
|
|
appendTriangleRef(triangle[0], triangle[1], triangle[2], a); // .x,a.y,a.z, inside[0]);
|
|
Vertex v = (Vertex) interlist.get(interindex++);
|
|
if (SURFACE && SOLID && !BOUNDARY)
|
{
|
if (a.inside)
|
{
|
v.set(a);
|
solid.appendTriangle(triangle[0], triangle[0], v);
|
solid.appendTriangle(triangle[1], triangle[1], v);
|
solid.appendTriangle(triangle[2], triangle[2], v);
|
if (i == 4)
|
{
|
solid.appendTriangle(triangle[3], triangle[3], v);
|
}
|
}
|
|
if (b.inside)
|
{
|
v.set(b);
|
solid.appendTriangle(triangle[0], triangle[0], v);
|
solid.appendTriangle(triangle[1], triangle[1], v);
|
solid.appendTriangle(triangle[2], triangle[2], v);
|
if (i == 4)
|
{
|
solid.appendTriangle(triangle[3], triangle[3], v);
|
}
|
}
|
|
if (c.inside)
|
{
|
v.set(c);
|
solid.appendTriangle(triangle[0], triangle[0], v);
|
solid.appendTriangle(triangle[1], triangle[1], v);
|
solid.appendTriangle(triangle[2], triangle[2], v);
|
if (i == 4)
|
{
|
solid.appendTriangle(triangle[3], triangle[3], v);
|
}
|
}
|
|
if (d.inside)
|
{
|
v.set(d);
|
solid.appendTriangle(triangle[0], triangle[0], v);
|
solid.appendTriangle(triangle[1], triangle[1], v);
|
solid.appendTriangle(triangle[2], triangle[2], v);
|
if (i == 4)
|
{
|
solid.appendTriangle(triangle[3], triangle[3], v);
|
}
|
}
|
}
|
|
if (i == 4)
|
{
|
if (oppositeSide(triangle[0], triangle[1], triangle[2], triangle[3]))
|
{
|
appendTriangleRef(triangle[1], triangle[2], triangle[3], a); // .x,a.y,a.z, inside[0]);
|
} else if (oppositeSide(triangle[1], triangle[0], triangle[2], triangle[3]))
|
{
|
appendTriangleRef(triangle[0], triangle[2], triangle[3], a); // .x,a.y,a.z, inside[0]);
|
} else if (oppositeSide(triangle[2], triangle[0], triangle[1], triangle[3]))
|
{
|
appendTriangleRef(triangle[0], triangle[1], triangle[3], a); // .x,a.y,a.z, inside[0]);
|
} else
|
{
|
System.out.println("NO OTHER TRIANGLE 2 !!!");
|
}
|
}
|
|
parent = null;
|
}
|
|
//Applet3D.traceout();
|
return true;
|
}
|
static Plane plane = new Plane();
|
|
// return true if point and ref lie on both side of edge plane
|
boolean oppositeSide(cVector point, cVector edge1, cVector edge2, cVector ref)
|
{
|
/*
|
LA.vecSub(point,edge1, v0);
|
LA.vecSub(ref,edge1, v1);
|
LA.vecCross(v0,v1, v2);
|
LA.vecSub(edge2,edge1, v3);
|
LA.vecCross(v2,v3, v4);
|
return (LA.vecDot(v0, v4) < 0) ^ (LA.vecDot(v1, v4) < 0);
|
*/
|
|
LA.vecSub(point, ref, v0);
|
LA.vecSub(edge2, edge1, v1);
|
LA.vecCross(v0, v1, v2);
|
LA.vecCross(v1, v2, v3);
|
|
plane.set(v3, edge1);
|
|
return (plane.distance(point) < 0) ^ (plane.distance(ref) < 0);
|
}
|
|
boolean coplanar(cVector pa, cVector pb, cVector pc, cVector pd)
|
{
|
LA.vecSub(pa, pb, v0);
|
LA.vecNormalize(v0);
|
LA.vecSub(pa, pc, v1);
|
LA.vecNormalize(v1);
|
LA.vecCross(v0, v1, v2);
|
LA.vecNormalize(v2);
|
LA.vecSub(pa, pd, v3);
|
|
double dist = Math.abs(LA.vecDot(v2, v3));
|
|
//System.out.println("dist = " + dist);
|
return dist < solid.Tolerance();
|
}
|
static Vector interlist = new Vector();
|
static int interindex;
|
|
Vertex intersect(Point p, Point q) // double x1, double y1, double z1, double x2, double y2, double z2, boolean pIn)
|
{
|
double x1, y1, z1;
|
double x2, y2, z2;
|
|
boolean reversed = q.x < p.x;
|
|
if (q.x == p.x)
|
{
|
reversed = q.y < p.y;
|
if (q.y == p.y)
|
{
|
reversed = q.z < p.z;
|
}
|
}
|
|
//reversed = false;
|
|
if (reversed) // !p.inside) // !pIn)
|
{
|
/*
|
double temp = x1;
|
x1 = x2;
|
x2 = temp;
|
temp = y1;
|
y1 = y2;
|
y2 = temp;
|
temp = z1;
|
z1 = z2;
|
z2 = temp;
|
*/
|
x1 = q.x;
|
y1 = q.y;
|
z1 = q.z;
|
x2 = p.x;
|
y2 = p.y;
|
z2 = p.z;
|
} else
|
{
|
x1 = p.x;
|
y1 = p.y;
|
z1 = p.z;
|
x2 = q.x;
|
y2 = q.y;
|
z2 = q.z;
|
}
|
|
Vertex result = (Vertex) interlist.get(interindex++); // new cVector();
|
atom = null;
|
ClearSolid(((ImplicitTiler) solid).implicit);
|
//assert p.inside == solid.inside(p.x, p.y, p.z);
|
do
|
{
|
result.x = 0.5 * (x1 + x2);
|
result.y = 0.5 * (y1 + y2);
|
result.z = 0.5 * (z1 + z2);
|
|
if (//atom != null &&
|
Math.abs(x1 - x2) + Math.abs(y1 - y2) + Math.abs(z1 - z2) < 0.00001)
|
{
|
//assert(atom != null);
|
if (atom == null)
|
{
|
//System.out.println("NULL ATOM!");
|
} else
|
{
|
//System.out.println("Atom = " + atom);
|
Gradient(atom, result);
|
}
|
//System.out.println("Point#1 = " + p + "; Point#2 = " + q + "; RESULT = " + result);
|
return result;
|
}
|
|
boolean resins = solid.inside(result.x, result.y, result.z);
|
if (resins ^ !p.inside ^ reversed)
|
{
|
x1 = result.x;
|
y1 = result.y;
|
z1 = result.z;
|
} else
|
{
|
x2 = result.x;
|
y2 = result.y;
|
z2 = result.z;
|
}
|
} while (true);
|
}
|
|
static Object3D atom = null;
|
|
void ClearSolid(Object3D s)
|
{
|
s.depth = -1;
|
}
|
static cVector inPnt = new cVector();
|
|
void untransform(Object3D o, cVector v)
|
{
|
if (o.parent != null && o != ((ImplicitTiler) solid).implicit)
|
{
|
untransform(o.parent, v);
|
}
|
|
if (o.fromParent != null)
|
o.untransform(v.x, v.y, v.z, v);
|
}
|
|
void transformNormal(Object3D o, cVector n)
|
{
|
if (o.fromParent != null)
|
{
|
double x = n.x;
|
double y = n.y;
|
double z = n.z;
|
n.x = x * o.fromParent[0][0] + y * o.fromParent[0][1] + z * o.fromParent[0][2];
|
n.y = x * o.fromParent[1][0] + y * o.fromParent[1][1] + z * o.fromParent[1][2];
|
n.z = x * o.fromParent[2][0] + y * o.fromParent[2][1] + z * o.fromParent[2][2];
|
}
|
|
if (o.parent != null && o != ((ImplicitTiler) solid).implicit)
|
{
|
transformNormal(o.parent, n);
|
}
|
}
|
|
void Gradient(Object3D o, Vertex v)
|
{
|
//double[][] gt = o.GlobalTransform();
|
//LA.xformPos(v, gt, inPnt);
|
inPnt.set(v);
|
|
untransform(o, inPnt);
|
|
if (o instanceof Composite)
|
{
|
assert false;
|
} else if (o instanceof Sphere)
|
{
|
//Sphere s = (Sphere) o;
|
|
v.norm.set(inPnt);
|
} else if (o instanceof Cone)
|
{
|
Cone c = (Cone) o;
|
|
v.norm.set(inPnt);
|
v.norm.y = 0;
|
v.norm.normalize();
|
v.norm.y = -(c.apexRadius - c.baseRadius) / c.height;
|
} else if (o instanceof Superellipsoid)
|
{
|
Superellipsoid s = (Superellipsoid) o;
|
|
LA.vecScale(inPnt, 1 / Math.abs(s.radius));
|
int signs = 0;
|
for (int i = 0; i < 3; i++)
|
{
|
if (inPnt.get(i) < 0)
|
{
|
signs |= 1 << i;
|
inPnt.set(i, -inPnt.get(i));
|
}
|
}
|
|
double e2 = 2 / s.north;
|
double e1 = 2 / s.east;
|
double dx = Math.pow(inPnt.x, e2) + Math.pow(inPnt.z, e2);
|
dx = e1 / e2 * Math.pow(dx, e1 / e2 - 1);
|
double dz = dx * e2 * Math.pow(inPnt.z, e2 - 1);
|
dx *= e2 * Math.pow(inPnt.x, e2 - 1);
|
double dy = e1 * Math.pow(inPnt.y, e1 - 1);
|
|
if ((signs & 1) != 0)
|
{
|
dx = -dx;
|
}
|
if ((signs & 2) != 0)
|
{
|
dy = -dy;
|
}
|
if ((signs & 4) != 0)
|
{
|
dz = -dz;
|
}
|
|
v.norm.set(dx, dy, dz);
|
}
|
// sept 2013
|
else
|
{
|
o.bRep.gradient(v);
|
}
|
|
//v.norm.normalize();
|
|
transformNormal(o, v.norm);
|
|
v.norm.normalize();
|
}
|
|
void appendTriangleRef(Vertex p, Vertex q, Vertex r, Point ref)
|
{
|
//newSimplex(p,q,r,ref,0).appendSimplex();
|
appendTriangleRef(p, q, r, ref.x, ref.y, ref.z, ref.inside);
|
}
|
static boolean locked = false;
|
|
void appendTriangleRef(Vertex p, Vertex q, Vertex r, double refx, double refy, double refz, boolean insideref)
|
{
|
boolean isinsidebox = true; //(refx < 60 && refx > 30 &&
|
//refy < 60 && refy > 40 &&
|
//refz < 60 && refz < -30);
|
if (isinsidebox)
|
{
|
//if (!locked)
|
{
|
//System.out.println("SIMPLEX = " + parent);
|
//locked = true;
|
if (BOUNDARY)
|
{
|
parent.appendSimplex();
|
}
|
//locked = false;
|
//new Exception().printStackTrace();
|
}
|
} else
|
{
|
if (!locked)
|
{
|
return;
|
}
|
}
|
|
if (!SURFACE && !locked)
|
{
|
return;
|
}
|
|
if (SWAP ^ !isinsidebox) // (DEPTH != -1 && depth != DEPTH))
|
{
|
insideref = !insideref;
|
}
|
|
ref.x = refx;
|
ref.y = refy;
|
ref.z = refz;
|
|
LA.vecSub(p, q, v0);
|
LA.vecSub(p, r, v1);
|
LA.vecCross(v0, v1, v2);
|
|
LA.vecSub(p, ref, v1);
|
|
if ((LA.vecDot(v1, v2) < 0) ^ insideref)
|
{
|
solid.appendTriangle(p, q, r);
|
} else
|
{
|
solid.appendTriangle(p, r, q);
|
}
|
}
|
static Solid solid;
|
boolean tryit; // inside/outside status
|
Vector children = null;
|
Simplex[] faces = new Simplex[4]; // A,B,C,D;
|
Plane[] planes = new Plane[4]; // pA,pB,pC,pD;
|
boolean outside = false;
|
boolean postpone = false;
|
short depth;
|
//double ax, ay, az;
|
//double bx, by, bz;
|
//double cx, cy, cz;
|
//double dx, dy, dz;
|
Point a, b, c, d;
|
private static boolean inside8[] = new boolean[8];
|
private static boolean inside[] = new boolean[4];
|
private static Vertex pp = new Vertex(true);
|
private static Vertex qq = new Vertex(true);
|
private static Vertex rr = new Vertex(true);
|
private static Vertex ss = new Vertex(true);
|
private static cVector v0 = new cVector();
|
private static cVector v1 = new cVector();
|
private static cVector v2 = new cVector();
|
private static cVector v3 = new cVector();
|
private static cVector v4 = new cVector();
|
private static Point p0 = new Point();
|
private static Point p1 = new Point();
|
private static Point p2 = new Point();
|
private static cVector ref = new cVector();
|
static Vertex[] triangle = new Vertex[4];
|
static Point[] simplex = new Point[4];
|
|
static
|
{
|
for (int i = 0; i < 4; i++)
|
{
|
//simplex[i] = new Point();
|
}
|
|
for (int i = 0; i < 5; i++)
|
{
|
interlist.add(new Vertex(true));
|
}
|
}
|
|
static public class Point extends cVector
|
{
|
|
boolean inside;
|
|
public Point()
|
{
|
}
|
|
public Point(double x, double y, double z)
|
{
|
super(x, y, z);
|
|
inside = Simplex.solid != null && Simplex.solid.inside(x, y, z);
|
}
|
|
public boolean equals(Object o)
|
{
|
Point p = (Point) o;
|
|
return LA.distance2(this, p) < 0.0000001;
|
}
|
}
|
|
static public class Plane
|
{
|
|
float a, b, c, d;
|
float mx, my, mz;
|
float radius2;
|
|
Plane(cVector pa, cVector pb, cVector pc, cVector pd)
|
{
|
mx = (float) ((pa.x + pb.x + pc.x) / 3);
|
my = (float) ((pa.y + pb.y + pc.y) / 3);
|
mz = (float) ((pa.z + pb.z + pc.z) / 3);
|
|
radius2 = (float) ((pa.x - mx) * (pa.x - mx) +
|
(pa.y - my) * (pa.y - my) + (pa.z - mz) * (pa.z - mz));
|
|
float test = (float) ((pb.x - mx) * (pb.x - mx) +
|
(pb.y - my) * (pb.y - my) + (pb.z - mz) * (pb.z - mz));
|
if (radius2 < test)
|
{
|
radius2 = test;
|
}
|
|
test = (float) ((pc.x - mx) * (pc.x - mx) +
|
(pc.y - my) * (pc.y - my) + (pc.z - mz) * (pc.z - mz));
|
if (radius2 < test)
|
{
|
radius2 = test;
|
}
|
|
pp.x = pa.x;
|
pp.y = pa.y;
|
pp.z = pa.z;
|
qq.x = pb.x;
|
qq.y = pb.y;
|
qq.z = pb.z;
|
rr.x = pc.x;
|
rr.y = pc.y;
|
rr.z = pc.z;
|
|
LA.vecSub(pp, qq, v0);
|
LA.vecSub(pp, rr, v1);
|
LA.vecCross(v0, v1, v2);
|
LA.vecNormalize(v2);
|
|
a = (float) v2.x;
|
b = (float) v2.y;
|
c = (float) v2.z;
|
d = (float) -LA.vecDot(v2, pp);
|
|
if (a * pd.x + b * pd.y + c * pd.z + d > 0)
|
{
|
a = -a;
|
b = -b;
|
c = -c;
|
d = -d;
|
}
|
|
//assert inplane(new cVector(mx,my,mz));
|
}
|
|
Plane(cVector n, cVector p)
|
{
|
set(n, p);
|
}
|
|
Plane()
|
{
|
}
|
|
public void set(cVector n, cVector p)
|
{
|
LA.vecNormalize(n);
|
a = (float) n.x;
|
b = (float) n.y;
|
c = (float) n.z;
|
d = (float) -LA.vecDot(n, p);
|
}
|
|
public double distance(cVector p)
|
{
|
return a * p.x + b * p.y + c * p.z + d;
|
}
|
|
public boolean inplane(cVector p)
|
{
|
return Math.abs(distance(p)) < 0.001 &&
|
((p.x - mx) * (p.x - mx) + (p.y - my) * (p.y - my) + (p.z - mz) * (p.z - mz)) <= radius2;
|
}
|
|
public boolean opposite(Plane p)
|
{
|
if (Math.abs(mx - p.mx) + Math.abs(my - p.my) +
|
Math.abs(mz - p.mz) > 0.00001)
|
//if (mx != p.mx || my != p.my || mz != p.mz)
|
{
|
return false;
|
}
|
|
return Math.abs(a + p.a) + Math.abs(b + p.b) +
|
Math.abs(c + p.c) + Math.abs(d + p.d) < 0.00001;
|
}
|
|
public boolean sameplane(Plane p)
|
{
|
boolean res = Math.abs(a - p.a) + Math.abs(b - p.b) +
|
Math.abs(c - p.c) + Math.abs(d - p.d) < 0.00001;
|
|
return res || Math.abs(a + p.a) + Math.abs(b + p.b) +
|
Math.abs(c + p.c) + Math.abs(d + p.d) < 0.00001;
|
}
|
|
public boolean equals(Object o)
|
{
|
//System.out.println("Compare " + this + " with " + o);
|
|
Plane p = (Plane) o;
|
|
//if (mx != p.mx || my != p.my || mz != p.mz)
|
//return false;
|
|
boolean res = Math.abs(a - p.a) + Math.abs(b - p.b) +
|
Math.abs(c - p.c) + Math.abs(d - p.d) < 0.00001;
|
|
//System.out.println("Result = " + res);
|
|
return res;
|
}
|
|
public String toString()
|
{
|
return super.toString() + " (a=" + a + ", b=" + b + ", c=" + c + ", d=" + d + ")";
|
}
|
}
|
|
public String toString()
|
{
|
if (outside)
|
{
|
return "OUTSIDE";
|
}
|
|
return super.toString() + " (depth = " + depth + ", touched = " + touched + ")";
|
}
|
|
public int compareTo(Object o)
|
{
|
Simplex s = (Simplex) o;
|
|
if (depth == s.depth)
|
{
|
return 0;
|
}
|
|
if (s.depth < depth)
|
{
|
return 1;
|
} else
|
{
|
return -1;
|
}
|
}
|
|
public void invariants()
|
{
|
for (int l = 0; l < 4; l++)
|
{
|
if (faces[l] == null)
|
{
|
assert (faces[l] != null);
|
}
|
|
if ((faces[l]).outside)
|
{
|
continue;
|
}
|
|
//assert faces[l].children == null;
|
|
boolean integre = (faces[l]).depth == depth - 1 ||
|
(faces[l]).depth == depth ||
|
(faces[l]).depth == depth + 1;
|
|
if (!integre)
|
{
|
System.out.println("BUG with " + this + " (" + (faces[l]) + ")");
|
//ImplicitTiler.DumpStack();
|
//System.out.println("BUG with " + this + " (" + (faces[l]) + ")");
|
}
|
|
assert integre;
|
}
|
|
if (children == null)
|
{
|
return;
|
}
|
|
for (int i = 0; i < children.size(); i++)
|
{
|
Simplex si = (Simplex) children.get(i);
|
si.invariants();
|
}
|
}
|
}
|
