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