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);
}
}
}
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;
}
}