public void CreateZigZagHybridPath(ref List <Vec3> simplePolygon, out List <Vec3> result, float distanceBetweenPaths) { int idx1, idx2, distPoint; FindOptimalDirection(ref simplePolygon, out idx1, out idx2, out distPoint); List <Vec3> transformed; Vec3 transformDir = (simplePolygon[idx2] - simplePolygon[idx1]); transformDir.Normalize(); ExtractLeftChainAndOffset(ref simplePolygon, out transformed, idx1, distanceBetweenPaths, ref transformDir); CalculateInterriorZigZag(ref simplePolygon, out result, idx1, idx2, distPoint, distanceBetweenPaths); }
public void CalculateInterriorZigZag(ref List <Vec3> simplePolygon, out List <Vec3> points, int idx1, int idx2, int endIdx, float offset) { points = new List <Vec3>(); Vec3 direction = simplePolygon[idx2] - simplePolygon[idx1]; direction.Normalize(); int idx2Adder = idx2; int idx1Adder = idx1; int size = (int)simplePolygon.Count; Vec3 edge2; Vec3 edge2Norm; Vec3 parallel2; Vec3 normal2; Vec3 parallelNorm2; Vec3 normalNorm2; Vec3 newPoint2; ; List <Vec3> points1 = new List <Vec3>(); List <Vec3> points2 = new List <Vec3>(); newPoint2 = simplePolygon[(idx2Adder) % size]; float fraction = offset; while (idx2Adder != endIdx) { edge2 = simplePolygon[(idx2Adder + 1) % size] - simplePolygon[idx2Adder]; edge2Norm = edge2; edge2Norm.Normalize(); parallel2 = direction * edge2.dot(direction); normal2 = direction - parallel2; parallelNorm2 = parallel2; normalNorm2 = normal2; parallelNorm2.Normalize(); normalNorm2.Normalize(); if (parallelNorm2.dot(direction) > 0) { parallelNorm2 *= -1f; } float normal2Abs = normal2.Abs(); float numPoints = (normal2Abs - fraction) / offset; int fullPoints = (int)Math.Floor(numPoints); float remainingFraction = numPoints - (float)fullPoints; if (idx2Adder == idx2) { newPoint2 = newPoint2 + normalNorm2 * fraction + parallel2 * fraction + parallelNorm2 * offset; } else { newPoint2 = newPoint2 + normalNorm2 * fraction + parallel2 * fraction; } points2.Add(newPoint2); for (int i = 0; i < fullPoints; ++i) { newPoint2 = newPoint2 + normalNorm2 * offset + parallel2 * offset; points2.Add(newPoint2); } newPoint2 = newPoint2 + normalNorm2 * remainingFraction + parallel2 * remainingFraction; fraction = offset - remainingFraction; idx2Adder = (idx2Adder + 1) % size; } newPoint2 = simplePolygon[(idx1Adder) % size]; fraction = offset; while (idx1Adder != endIdx) { edge2 = simplePolygon[(idx1Adder - 1) % size] - simplePolygon[idx1Adder]; edge2Norm = edge2; edge2Norm.Normalize(); parallel2 = direction * edge2.dot(direction); normal2 = direction - parallel2; parallelNorm2 = parallel2; normalNorm2 = normal2; parallelNorm2.Normalize(); normalNorm2.Normalize(); if (parallelNorm2.dot(direction) < 0) { parallelNorm2 *= -1f; } float normal2Abs = normal2.Abs(); float numPoints = (normal2Abs - fraction) / offset; int fullPoints = (int)Math.Floor(numPoints); float remainingFraction = numPoints - (float)fullPoints; if (idx1Adder == idx1) { newPoint2 = newPoint2 + normalNorm2 * fraction + parallel2 * fraction + parallelNorm2 * offset; } else { newPoint2 = newPoint2 + normalNorm2 * fraction + parallel2 * fraction; } points1.Add(newPoint2); for (int i = 0; i < fullPoints; ++i) { newPoint2 = newPoint2 + normalNorm2 * offset + parallel2 * offset; points1.Add(newPoint2); } newPoint2 = newPoint2 + normalNorm2 * remainingFraction + parallel2 * remainingFraction; fraction = offset - remainingFraction; idx1Adder = (idx1Adder - 1) % size; } size = points1.Count > points2.Count ? points1.Count : points2.Count; for (int i = 0; i < size; ++i) { if (i % 2 == 0) { points.Add(points2[i]); points.Add(points1[i]); } else { points.Add(points1[i]); points.Add(points2[i]); } } }
public List <Vec3> GetAllVoxelOnPlane(Vec3 p1, Vec3 p2, Vec3 p3, bool getAll) { List <Vec3> result = new List <Vec3>(); Vec3 normal = (p2 - p1).Cross((p3 - p1)); float d = -p1.dot(normal); uint resolution = xResolution; Vec3 mainDirection = new Vec3(1, 0, 0); if (Math.Abs(normal.x) < Math.Abs(normal.y)) { if (Math.Abs(normal.z) < Math.Abs(normal.y)) { resolution = yResolution; mainDirection = new Vec3(0, 1, 0); } else { resolution = zResolution; mainDirection = new Vec3(0, 0, 1); } } else if (Math.Abs(normal.x) < Math.Abs(normal.z)) { resolution = zResolution; mainDirection = new Vec3(0, 0, 1); } Vec3 planeCoord1 = new Vec3(mainDirection.z, mainDirection.x, mainDirection.y); Vec3 planeCoord2 = new Vec3(mainDirection.y, mainDirection.z, mainDirection.x); float min = -d / mainDirection.dot(normal); float max = min; float tmp = -(d + (planeCoord1 * resolution).dot(normal)) / mainDirection.dot(normal); min = tmp < min ? tmp : min; max = tmp > max ? tmp : max; tmp = -(d + (planeCoord2 * resolution).dot(normal)) / mainDirection.dot(normal); min = tmp < min ? tmp : min; max = tmp > max ? tmp : max; tmp = -(d + (planeCoord1 * resolution + planeCoord2 * resolution).dot(normal)) / mainDirection.dot(normal); min = tmp < min ? tmp : min; max = tmp > max ? tmp : max; min = (float)Math.Floor(min); max = (float)Math.Ceiling(max); min = min < 0 ? 0 : min; max = max > resolution ? resolution : max; Vec3 maxHeightVector = mainDirection * max; if (max - min > 1) { Vec3 point1 = new Vec3(); Vec3 point2 = new Vec3(); Vec3 maxVector1 = planeCoord1 * resolution; Vec3 minVector1 = new Vec3(0, 0, 0); Vec3 minVector2 = new Vec3(); Vec3 maxVector2 = new Vec3(); Vec3 lineNormal = normal.Cross(mainDirection); lineNormal.Normalize(); float t; float dotTmp; Vec3 tmpVec; Vec3 tmpResult = new Vec3(); if (lineNormal.dot(planeCoord1) == 0) { tmpVec = planeCoord1; planeCoord1 = planeCoord2; planeCoord2 = tmpVec; maxVector1 = planeCoord1 * resolution; minVector1 = new Vec3(0, 0, 0); } for (Vec3 mainVec = mainDirection * min; mainVec != maxHeightVector; mainVec = mainVec + mainDirection) { if (normal.dot(planeCoord1) != 0) { point1 = planeCoord1 * -(mainVec.dot(normal) + d) / (normal.dot(planeCoord1)) + mainVec; point2 = planeCoord1 * -((mainVec + mainDirection).dot(normal) + d) / (normal.dot(planeCoord1)) + (mainVec + mainDirection); } else { point1 = planeCoord2 * -(mainVec.dot(normal) + d) / (normal.dot(planeCoord2)) + mainVec; point2 = planeCoord2 * -((mainVec + mainDirection).dot(normal) + d) / (normal.dot(planeCoord2)) + (mainVec + mainDirection); } for (Vec3 direction1 = minVector1; direction1 != maxVector1; direction1 += planeCoord1) { dotTmp = lineNormal.dot(planeCoord1); t = (direction1 + mainVec - point1).dot(planeCoord1) / dotTmp; minVector2 = point1 + lineNormal * t; t = (direction1 + mainVec + mainDirection - point2).dot(planeCoord1) / dotTmp; maxVector2 = point2 + lineNormal * t; dotTmp = maxVector2.dot(planeCoord2); if (dotTmp < minVector2.dot(planeCoord2)) { tmpVec = minVector2; minVector2 = maxVector2; maxVector2 = tmpVec; dotTmp = maxVector2.dot(planeCoord2); } if (dotTmp < 0) { continue; } if ((dotTmp + 1) > resolution) { maxVector2 = planeCoord1 * maxVector2.dot(planeCoord1) + planeCoord2 * resolution + mainVec; } else { maxVector2 = planeCoord1 * maxVector2.dot(planeCoord1) + planeCoord2 * (dotTmp + 1) + mainVec; } dotTmp = minVector2.dot(planeCoord2); if (dotTmp > resolution) { continue; } if (dotTmp < 0) { minVector2 = planeCoord1 * (minVector2.dot(planeCoord1)) + mainVec; } minVector2 = direction1 + mainVec + planeCoord2 * (float)Math.Floor(planeCoord2.dot(minVector2)); maxVector2 = direction1 + mainVec + planeCoord2 * (float)Math.Ceiling(planeCoord2.dot(maxVector2)); for (Vec3 direction2 = minVector2; direction2 != maxVector2; direction2 += planeCoord2) { tmpResult.x = (float)Math.Floor(direction2.x); tmpResult.y = (float)Math.Floor(direction2.y); tmpResult.z = (float)Math.Floor(direction2.z); if (grid[(int)tmpResult.x, (int)tmpResult.y, (int)tmpResult.z].set || getAll) { result.Add(tmpResult); } } } } } else { Vec3 pos; for (int i = 0; i < resolution; ++i) { for (int j = 0; j < resolution; ++j) { pos = planeCoord1 * i + planeCoord2 * j + maxHeightVector; if (grid[(int)pos.x, (int)pos.y, (int)pos.z].set || getAll) { result.Add(pos); } } } } return(result); }
private void CreateStart() { float lowestX = points[0].x, highestX = points[0].x; float lowestY = points[0].y, highestY = points[0].y; float lowestZ = points[0].z, highestZ = points[0].z; int[] lowest = new int[3]; int[] highest = new int[3]; int listSize = points.Count; Vec3 point; for (int i = 0; i < listSize; ++i) { point = points[i]; if (point.x < lowestX) { lowestX = point.x; lowest[0] = i; } if (point.x > highestX) { highestX = point.x; highest[0] = i; } if (point.y < lowestY) { lowestY = point.y; lowest[1] = i; } if (point.y > highestY) { highestY = point.y; highest[1] = i; } if (point.z < lowestZ) { lowestZ = point.z; lowest[2] = i; } if (point.z > highestZ) { highestZ = point.z; highest[2] = i; } } int greatestDistanceUp = lowest[0]; int greatestDistanceDown = highest[0]; Vec3 distanceVec = points[greatestDistanceDown] - points[greatestDistanceUp]; float maxDistance = distanceVec.dot(distanceVec); float tmpDistance = 0; for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { distanceVec = points[lowest[i]] - points[highest[j]]; tmpDistance = distanceVec.dot(distanceVec); if (tmpDistance > maxDistance) { greatestDistanceUp = lowest[i]; greatestDistanceDown = highest[j]; maxDistance = tmpDistance; } } } distanceVec = points[greatestDistanceDown] - points[greatestDistanceUp]; int distanceIdx = 0; Vec3 tmpVec; float newMaxDistance = 0; for (int i = 0; i < 3; ++i) { tmpVec = distanceVec.Cross(points[greatestDistanceUp] - points[lowest[i]]); tmpDistance = (tmpVec).dot(tmpVec) / maxDistance; if (tmpDistance > newMaxDistance) { distanceIdx = lowest[i]; newMaxDistance = tmpDistance; } } for (int i = 0; i < 3; ++i) { tmpVec = distanceVec.Cross(points[greatestDistanceUp] - points[highest[i]]); tmpDistance = (tmpVec).dot(tmpVec) / maxDistance; if (tmpDistance > newMaxDistance) { distanceIdx = highest[i]; newMaxDistance = tmpDistance; } } Vec3 normal = (distanceVec.Cross(points[greatestDistanceDown] - points[distanceIdx])); normal.Normalize(); newMaxDistance = 0; int distanceIdx2 = 0; for (int i = 0; i < 3; ++i) { tmpDistance = Math.Abs(normal.dot(points[distanceIdx] - points[lowest[i]])); if (tmpDistance > newMaxDistance) { distanceIdx2 = lowest[i]; newMaxDistance = tmpDistance; } } for (int i = 0; i < 3; ++i) { tmpDistance = Math.Abs(normal.dot(points[distanceIdx] - points[highest[i]])); if (tmpDistance > newMaxDistance) { distanceIdx2 = highest[i]; newMaxDistance = tmpDistance; } } if (normal.dot(points[distanceIdx2] - points[distanceIdx]) > 0) { //Face 0 HalfEdge newHalfEdge = new HalfEdge(); newHalfEdge.vertex = greatestDistanceUp; newHalfEdge.face = 0; newHalfEdge.opposite = 3; edges.Add(newHalfEdge); newHalfEdge.vertex = distanceIdx; newHalfEdge.face = 0; newHalfEdge.opposite = 6; edges.Add(newHalfEdge); newHalfEdge.vertex = greatestDistanceDown; newHalfEdge.face = 0; newHalfEdge.opposite = 9; edges.Add(newHalfEdge); //Face 1 newHalfEdge.vertex = greatestDistanceDown; newHalfEdge.face = 1; newHalfEdge.opposite = 0; edges.Add(newHalfEdge); newHalfEdge.vertex = distanceIdx2; newHalfEdge.face = 1; newHalfEdge.opposite = 11; edges.Add(newHalfEdge); newHalfEdge.vertex = greatestDistanceUp; newHalfEdge.face = 1; newHalfEdge.opposite = 7; edges.Add(newHalfEdge); //Face2 newHalfEdge.vertex = greatestDistanceUp; newHalfEdge.face = 2; newHalfEdge.opposite = 1; edges.Add(newHalfEdge); newHalfEdge.vertex = distanceIdx2; newHalfEdge.face = 2; newHalfEdge.opposite = 5; edges.Add(newHalfEdge); newHalfEdge.vertex = distanceIdx; newHalfEdge.face = 2; newHalfEdge.opposite = 10; edges.Add(newHalfEdge); //Face3 newHalfEdge.vertex = distanceIdx; newHalfEdge.face = 3; newHalfEdge.opposite = 2; edges.Add(newHalfEdge); newHalfEdge.vertex = distanceIdx2; newHalfEdge.face = 3; newHalfEdge.opposite = 8; edges.Add(newHalfEdge); newHalfEdge.vertex = greatestDistanceDown; newHalfEdge.face = 3; newHalfEdge.opposite = 4; edges.Add(newHalfEdge); } else { //Face 0 HalfEdge newHalfEdge = new HalfEdge(); newHalfEdge.vertex = distanceIdx; newHalfEdge.face = 0; newHalfEdge.opposite = 11; edges.Add(newHalfEdge); newHalfEdge.vertex = greatestDistanceUp; newHalfEdge.face = 0; newHalfEdge.opposite = 8; edges.Add(newHalfEdge); newHalfEdge.vertex = greatestDistanceDown; newHalfEdge.face = 0; newHalfEdge.opposite = 5; edges.Add(newHalfEdge); //Face 1 newHalfEdge.vertex = distanceIdx2; newHalfEdge.face = 1; newHalfEdge.opposite = 7; edges.Add(newHalfEdge); newHalfEdge.vertex = greatestDistanceDown; newHalfEdge.face = 1; newHalfEdge.opposite = 9; edges.Add(newHalfEdge); newHalfEdge.vertex = greatestDistanceUp; newHalfEdge.face = 1; newHalfEdge.opposite = 2; edges.Add(newHalfEdge); //Face2 newHalfEdge.vertex = distanceIdx2; newHalfEdge.face = 2; newHalfEdge.opposite = 10; edges.Add(newHalfEdge); newHalfEdge.vertex = greatestDistanceUp; newHalfEdge.face = 2; newHalfEdge.opposite = 3; edges.Add(newHalfEdge); newHalfEdge.vertex = distanceIdx; newHalfEdge.face = 2; newHalfEdge.opposite = 1; edges.Add(newHalfEdge); //Face3 newHalfEdge.vertex = distanceIdx2; newHalfEdge.face = 3; newHalfEdge.opposite = 4; edges.Add(newHalfEdge); newHalfEdge.vertex = distanceIdx; newHalfEdge.face = 3; newHalfEdge.opposite = 6; edges.Add(newHalfEdge); newHalfEdge.vertex = greatestDistanceDown; newHalfEdge.face = 3; newHalfEdge.opposite = 0; edges.Add(newHalfEdge); } Face newFace = new Face(); for (int i = 0; i < 4; ++i) { newFace.halfEdge = i * 3; newFace.normal = ((points[edges[i * 3 + 2].vertex] - points[edges[i * 3].vertex]).Cross(points[edges[i * 3 + 1].vertex] - points[edges[i * 3].vertex])); newFace.normal.Normalize(); newFace.pointList = -1; faces.Add(newFace); } }