public double distance(vector3d v)
{
double dx = x - v.x;
double dy = y - v.y;
double dz = z - v.z;
return(Math.Sqrt(dx * dx + dy * dy + dz * dz));
}
public double distanceSquared(vector3d v)
{
double dx = x - v.x;
double dy = y - v.y;
double dz = z - v.z;
return(dx * dx + dy * dy + dz * dz);
}
public void cross(vector3d v1, vector3d v2)
{
double tmpx = v1.y * v2.z - v1.z * v2.y;
double tmpy = v1.z * v2.x - v1.x * v2.z;
double tmpz = v1.x * v2.y - v1.y * v2.x;
x = tmpx;
y = tmpy;
z = tmpz;
}
public void computeNormal(vector3d normal)
{
HalfEdge he1 = he0.next;
HalfEdge he2 = he1.next;
point3d p0 = he0.head().pnt;
point3d p2 = he1.head().pnt;
double d2x = p2.x - p0.x;
double d2y = p2.y - p0.y;
double d2z = p2.z - p0.z;
normal.setZero();
numVerts = 2;
while (he2 != he0)
{
double d1x = d2x;
double d1y = d2y;
double d1z = d2z;
p2 = he2.head().pnt;
d2x = p2.x - p0.x;
d2y = p2.y - p0.y;
d2z = p2.z - p0.z;
normal.x += d1y * d2z - d1z * d2y;
normal.y += d1z * d2x - d1x * d2z;
normal.z += d1x * d2y - d1y * d2x;
he1 = he2;
he2 = he2.next;
numVerts++;
}
area = normal.norm();
normal.scale(1 / area);
}
public void computeNormal(vector3d normal, double minArea)
{
computeNormal(normal);
if (area < minArea)
{
// make the normal more robust by removing
// components parallel to the longest edge
HalfEdge hedgeMax = null;
double lenSqrMax = 0;
HalfEdge hedge = he0;
do
{
double lenSqr = hedge.lengthSquared();
if (lenSqr > lenSqrMax)
{
hedgeMax = hedge;
lenSqrMax = lenSqr;
}
hedge = hedge.next;
}while (hedge != he0);
point3d p2 = hedgeMax.head().pnt;
point3d p1 = hedgeMax.tail().pnt;
double lenMax = Math.Sqrt(lenSqrMax);
double ux = (p2.x - p1.x) / lenMax;
double uy = (p2.y - p1.y) / lenMax;
double uz = (p2.z - p1.z) / lenMax;
double dot = normal.x * ux + normal.y * uy + normal.z * uz;
normal.x -= dot * ux;
normal.y -= dot * uy;
normal.z -= dot * uz;
normal.normalize();
}
}
public void set(vector3d v1)
{
x = v1.x;
y = v1.y;
z = v1.z;
}
private void createInitialSimplex()
{
double max = 0;
int imax = 0;
for (int i = 0; i < 3; i++)
{
double diff = maxVtxs[i].pnt.get(i) - minVtxs[i].pnt.get(i);
if (diff > max)
{
max = diff;
imax = i;
}
}
if (max <= tolerance)
{
throw new Exception("Input points appear to be coincident");
}
Vertex[] vtx = new Vertex[4];
vtx[0] = maxVtxs[imax];
vtx[1] = minVtxs[imax];
vector3d u01 = new vector3d();
vector3d diff02 = new vector3d();
vector3d nrml = new vector3d();
vector3d xprod = new vector3d();
double maxSqr = 0;
u01.sub(vtx[1].pnt, vtx[0].pnt);
u01.normalize();
for (int i = 0; i < numPoints; i++)
{
diff02.sub(pointBuffer[i].pnt, vtx[0].pnt);
xprod.cross(u01, diff02);
double lenSqr = xprod.normSquared();
if (lenSqr > maxSqr &&
pointBuffer[i] != vtx[0] && // paranoid
pointBuffer[i] != vtx[1])
{
maxSqr = lenSqr;
vtx[2] = pointBuffer[i];
nrml.set(xprod);
}
}
if (Math.Sqrt(maxSqr) <= 100 * tolerance)
{
throw new Exception("Input points appear to be colinear");
}
nrml.normalize();
double maxDist = 0;
double d0 = vtx[2].pnt.dot(nrml);
for (int i = 0; i < numPoints; i++)
{
double dist = Math.Abs(pointBuffer[i].pnt.dot(nrml) - d0);
if (dist > maxDist &&
pointBuffer[i] != vtx[0] && // paranoid
pointBuffer[i] != vtx[1] &&
pointBuffer[i] != vtx[2])
{
maxDist = dist;
vtx[3] = pointBuffer[i];
}
}
if (Math.Abs(maxDist) <= 100 * tolerance)
{
throw new Exception("Input points appear to be coplanar");
}
if (debug)
{
Print("initial vertices:");
Print(vtx[0].index + ": " + vtx[0].pnt);
Print(vtx[1].index + ": " + vtx[1].pnt);
Print(vtx[2].index + ": " + vtx[2].pnt);
Print(vtx[3].index + ": " + vtx[3].pnt);
}
Face[] tris = new Face[4];
if (vtx[3].pnt.dot(nrml) - d0 < 0)
{
tris[0] = Face.createTriangle(vtx[0], vtx[1], vtx[2]);
tris[1] = Face.createTriangle(vtx[3], vtx[1], vtx[0]);
tris[2] = Face.createTriangle(vtx[3], vtx[2], vtx[1]);
tris[3] = Face.createTriangle(vtx[3], vtx[0], vtx[2]);
for (int i = 0; i < 3; i++)
{
int k = (i + 1) % 3;
tris[i + 1].getEdge(1).setOpposite(tris[k + 1].getEdge(0));
tris[i + 1].getEdge(2).setOpposite(tris[0].getEdge(k));
}
}
else
{
tris[0] = Face.createTriangle(vtx[0], vtx[2], vtx[1]);
tris[1] = Face.createTriangle(vtx[3], vtx[0], vtx[1]);
tris[2] = Face.createTriangle(vtx[3], vtx[1], vtx[2]);
tris[3] = Face.createTriangle(vtx[3], vtx[2], vtx[0]);
for (int i = 0; i < 3; i++)
{
int k = (i + 1) % 3;
tris[i + 1].getEdge(0).setOpposite(tris[k + 1].getEdge(1));
tris[i + 1].getEdge(2).setOpposite(tris[0].getEdge((3 - i) % 3));
}
}
for (int i = 0; i < 4; i++)
{
faces.Add(tris[i]);
}
for (int i = 0; i < numPoints; i++)
{
Vertex v = pointBuffer[i];
if (v == vtx[0] || v == vtx[1] || v == vtx[2] || v == vtx[3])
{
continue;
}
maxDist = tolerance;
Face maxFace = null;
for (int k = 0; k < 4; k++)
{
double dist = tris[k].distanceToPlane(v.pnt);
if (dist > maxDist)
{
maxFace = tris[k];
maxDist = dist;
}
}
if (maxFace != null)
{
addPointToFace(v, maxFace);
}
}
}
public Face()
{
normal = new vector3d();
centroid = new point3d();
mark = VISIBLE;
}
public vector3d(vector3d v)
{
set(v);
}
private void computeMaxAndMin()
{
vector3d max = new vector3d();
vector3d min = new vector3d();
for (int i = 0; i < 3; i++)
{
maxVtxs[i] = minVtxs[i] = pointBuffer[0];
}
max.set(pointBuffer[0].pnt);
min.set(pointBuffer[0].pnt);
for (int i = 1; i < numPoints; i++)
{
point3d pnt = pointBuffer[i].pnt;
if (pnt.x > max.x)
{
max.x = pnt.x;
maxVtxs[0] = pointBuffer[i];
}
else if (pnt.x < min.x)
{
min.x = pnt.x;
minVtxs[0] = pointBuffer[i];
}
if (pnt.y > max.y)
{
max.y = pnt.y;
maxVtxs[1] = pointBuffer[i];
}
else if (pnt.y < min.y)
{
min.y = pnt.y;
minVtxs[1] = pointBuffer[i];
}
if (pnt.z > max.z)
{
max.z = pnt.z;
maxVtxs[2] = pointBuffer[i];
}
else if (pnt.z < min.z)
{
min.z = pnt.z;
minVtxs[2] = pointBuffer[i];
}
}
// this epsilon formula comes from QuickHull, and I'm
// not about to quibble.
charLength = Math.Max(max.x - min.x, max.y - min.y);
charLength = Math.Max(max.z - min.z, charLength);
if (explicitTolerance == AUTOMATIC_TOLERANCE)
{
tolerance =
3 * DOUBLE_PREC * (Math.Max(Math.Abs(max.x), Math.Abs(min.x)) +
Math.Max(Math.Abs(max.y), Math.Abs(min.y)) +
Math.Max(Math.Abs(max.z), Math.Abs(min.z)));
}
else
{
tolerance = explicitTolerance;
}
}
public double dot(vector3d v1)
{
return(x * v1.x + y * v1.y + z * v1.z);
}
public void add(vector3d v1)
{
x += v1.x;
y += v1.y;
z += v1.z;
}
public void add(vector3d v1, vector3d v2)
{
x = v1.x + v2.x;
y = v1.y + v2.y;
z = v1.z + v2.z;
}
public void sub(vector3d v1, vector3d v2)
{
x = v1.x - v2.x;
y = v1.y - v2.y;
z = v1.z - v2.z;
}
public void scale(double s, vector3d v1)
{
x = s * v1.x;
y = s * v1.y;
z = s * v1.z;
}
public point3d(vector3d v)
{
set(v);
}
public void sub(vector3d v1)
{
x -= v1.x;
y -= v1.y;
z -= v1.z;
}
