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