static Vector3 ApproximateIntersection(Shape shape, Vector3 p0, Vector3 p1) { // https://en.wikipedia.org/wiki/False_position_method (aka Regula falsi). // The implementation is inspired by the Wikipedia's page. float d0 = shape.ComputeDistance(p0); float d1 = shape.ComputeDistance(p1); Vector3 p = Vector3.zero; int i = 0; int side = 0; const int maxIterations = 16; const float positionTolerance = 0.0001f; float t0 = 0f; float t1 = 1f; float t = 0.5f; Vector3 delta = p1 - p0; for (i = 0; i < maxIterations; ++i) { t = (t0 * d1 - t1 * d0) / (d1 - d0); // More stable than p0 - (p1 - p0) * d0 / (d1 - d0); more float precise says Wikipedia p = p0 + t * delta; if (Mathf.Abs(t1 - t0) < positionTolerance * Mathf.Abs(t1 + t0)) break; float d = shape.ComputeDistance(p); if (d * d1 > 0f) { // d and d1 have same sign, copy p to p1 t1 = t; d1 = d; if (side == -1) d0 *= 0.5f; side = -1; } else if (d * d0 > 0f) { // d and d0 have same sign, copy p to p0 t0 = t; d0 = d; if (side == +1) d1 *= 0.5f; side = +1; } else { // d * d_ very small (looks like zero) break; } } //Debug.Log("Iterations: " + i); return p; }
static OctreeNode ConstructLeaf(Shape shape, OctreeNode leaf) { if (leaf == null) { return null; } int corners = 0; for (int i = 0; i < 8; ++i) { Vector3 cornerPos = leaf.min + CHILD_MIN_OFFSETS[i] * leaf.size; float density = shape.ComputeDistance(cornerPos); int material = density < 0 ? MATERIAL_SOLID : MATERIAL_AIR; corners |= material << i; } if (corners == 0 || corners == 255) { // voxel is full inside or outside the volume return null; } // otherwise the voxel contains the surface, so find the edge intersections const int MAX_CROSSINGS = 6; int nbEdges = 0; Vector3 normalSum = Vector3.zero; QefData qefData = new QefData(); for (int i = 0; i < 12 && nbEdges < MAX_CROSSINGS; ++i) { int c1 = edgevmap[i, 0]; int c2 = edgevmap[i, 1]; int m1 = (corners >> c1) & 1; int m2 = (corners >> c2) & 1; if (m1 == m2) { // no crossing on this edge continue; } Vector3 p1 = leaf.min + CHILD_MIN_OFFSETS[c1] * leaf.size; Vector3 p2 = leaf.min + CHILD_MIN_OFFSETS[c2] * leaf.size; Vector3 p = ApproximateIntersection(shape, p1, p2); Vector3 n = shape.ComputeNormal(p); qefData.Add(p, n); normalSum += n; ++nbEdges; } Vector3 qefPosition; QefSolver.Solve(out qefPosition, qefData, QEF_ERROR, QEF_SWEEPS, QEF_ERROR); OctreeNodeInfo nodeInfo = new OctreeNodeInfo(); nodeInfo.position = qefPosition; nodeInfo.qefData = qefData; Vector3 min = leaf.min; Vector3 max = leaf.min + new Vector3(leaf.size, leaf.size, leaf.size); //nodeInfo.position = Vector3.Min(max, Vector3.Max(min, qefData.MassPoint)); if (nodeInfo.position.x < min.x || nodeInfo.position.x > max.x || nodeInfo.position.y < min.y || nodeInfo.position.y > max.y || nodeInfo.position.z < min.z || nodeInfo.position.z > max.z) { int x = 0; ++x; //nodeInfo.position = qefData.MassPoint; } { float tolerance = 1.0f; if (qefPosition.x - min.x < -tolerance || qefPosition.y - min.y < -tolerance || qefPosition.z - min.z < -tolerance || qefPosition.x - max.x > tolerance || qefPosition.y - max.y > tolerance || qefPosition.z - max.z > tolerance) { int x = 0; ++x; } } nodeInfo.averageNormal = Vector3.Normalize(normalSum); nodeInfo.corners = corners; leaf.type = OctreeNode.Type.Leaf; leaf.info = nodeInfo; return leaf; }