/*
* Add a face to face list if the face is correct.
*/
public int AddFace(Face face)
{
int [] vertices = face.vertexIndices;
Vertex vertex1 = GetVertex(vertices[0]);
Vertex vertex2 = GetVertex(vertices[1]);
Vertex vertex3 = GetVertex(vertices[2]);
Vect3 normal = Vect3.Cross(vertex1.coordinate - vertex2.coordinate,
vertex1.coordinate - vertex3.coordinate).GetNormalized();
if (normal != face.normal.GetNormalized())
{
throw new InvalidFaceException("Normal does not match vertices of face.");
}
_faces.Add(face);
vertex1.AddFaceIndex(_faces.Count - 1);
vertex2.AddFaceIndex(_faces.Count - 1);
vertex3.AddFaceIndex(_faces.Count - 1);
return(_faces.Count - 1);
}
/* Calculates the smallest angle in degrees between two lines defined by vectors. */
public double AngleTo(Vect3 other)
{
if (this == Zero)
{
throw new ZeroVectorException("Tried to calculate the angle from the Zero Vector.");
}
if (other == Zero)
{
throw new ZeroVectorException("Tried to calculate the angle to the Zero Vector.");
}
if (this == other)
{
return(0);
}
Vect3 v1 = this.GetNormalized();
Vect3 v2 = other.GetNormalized();
double angle = Math.Acos(Dot(v1, v2)) * (180 / Math.PI);
return(angle);
}
public Component Transform(Matrix4x4 transform)
{
Component componentCopy = this.DeepCopy();
Vect3 firstCoordinate = componentCopy.vertices[0].coordinate * transform;
componentCopy._lowestXValue = firstCoordinate.x;
componentCopy._highestXValue = firstCoordinate.x;
componentCopy._lowestYValue = firstCoordinate.y;
componentCopy._highestYValue = firstCoordinate.y;
componentCopy._lowestZValue = firstCoordinate.z;
componentCopy._highestZValue = firstCoordinate.z;
foreach (Vertex vertex in componentCopy.vertices)
{
vertex.coordinate = vertex.coordinate * transform;
componentCopy.UpdateBoxConstraints(vertex.coordinate);
}
Matrix4x4 tiTransform = new Matrix4x4();
if (Matrix4x4.Invert(transform, out tiTransform))
{
tiTransform = Matrix4x4.Transpose(tiTransform);
}
else
{
throw new InvalidTransformException("Invalid transformation matrix.");
}
foreach (Face face in componentCopy.faces)
{
face.normal = face.normal * tiTransform;
}
return(componentCopy);
}
/*
* Calculates Line/Face intersections using the Möller-Trumbore intersection algorithm.
*
* https://en.wikipedia.org/wiki/Möller–Trumbore_intersection_algorithm
*/
public static bool rayIntersectsTriangle(Vect3 rayOrigin,
Vect3 rayVector,
Vect3 vertex0,
Vect3 vertex1,
Vect3 vertex2)
{
double EPSILON = 1e-6;
Vect3 edge1 = vertex1 - vertex0;
Vect3 edge2 = vertex2 - vertex0;
double a, f, u, v;
Vect3 h = Vect3.Cross(rayVector, edge2);
a = Vect3.Dot(edge1, h);
if (a > -EPSILON && a < EPSILON)
{
return(false); // This ray is parallel to this triangle.
}
f = 1.0 / a;
Vect3 s = rayOrigin - vertex0;
u = f * Vect3.Dot(s, h);
if (u < 0.0 || u > 1.0)
{
return(false);
}
Vect3 q = Vect3.Cross(s, edge1);
v = f * Vect3.Dot(rayVector, q);
if (v < 0.0 || u + v > 1.0)
{
return(false);
}
double t = f * Vect3.Dot(edge2, q);
return(t > EPSILON);
}
public Plane(Vect3 offset, Vect3 normal)
{
this.offset = offset;
this.normal = normal;
}
/* Help functions for faces */
/*
* Create a new face from list of 3 vertices index and add it to faces list.
*/
public int CreateFace(Vect3 normal, List <int> vertices)
{
return(CreateFace(normal, vertices.ToArray()));
}
/* Double initializer */
public Vertex(double x, double y, double z)
{
coordinate = new Vect3(x, y, z);
_faceIndices = new List <int>();
this.faceIndices = _faceIndices.AsReadOnly();
}
public VoxelGrid(Component component, Vect3 plane1Normal = null, Vect3 plane2Normal = null, Vect3 plane3Normal = null)
{
orientedBbox = new OrientedBBox(component, plane1Normal, plane2Normal, plane3Normal);
componentOBBs[component.index] = orientedBbox;
SetupVoxelGrid();
this.CreateShellFromFaces(component);
}
/*
* Fills internal volume of voxels by finding points in the voxel grid captured
* by a mesh. Relies on the original meshes to perform odd/even polygon capture calculation.
*
* https://en.wikipedia.org/wiki/Point_in_polygon
*/
public void FillInternalVolume(Component component)
{
OrientedBBox componentOBB = componentOBBs[component.index];
List <Vect3> points = new List <Vect3>();
foreach (var vertex in component.vertices)
{
points.Add(vertex.coordinate);
}
VoxelSpan componentVoxelSpan = new VoxelSpan(points, this, BorderOffset);
for (int x = componentVoxelSpan.minX; x < componentVoxelSpan.maxX; x++)
{
for (int z = componentVoxelSpan.minZ; z < componentVoxelSpan.maxZ; z++)
{
for (int y = componentVoxelSpan.minY; y < componentVoxelSpan.maxY; y++)
{
if (!coordinateGrid[x][y][z] && coordinateGrid[x][Math.Max(0, y - 1)][z])
{
Vect3 globalPos = voxelStartCoordinate +
orientedBbox.localX * x * resolution +
orientedBbox.localY * y * resolution +
orientedBbox.localZ * z * resolution;
if (!componentOBB.ContainsGlobalCoordinate(globalPos, OrientedBBoxExpansionFactor))
{
continue;
}
int intersects = 0;
foreach (Face face in component.faces)
{
Vect3 triA = component.GetCoordinate(face.vertexIndices[0]);
Vect3 triB = component.GetCoordinate(face.vertexIndices[1]);
Vect3 triC = component.GetCoordinate(face.vertexIndices[2]);
// if (intersect_triangle(globalPos, Vect3.Up, triA, triB, triC))
if (rayIntersectsTriangle(globalPos, Vect3.Up, triA, triB, triC))
{
intersects++;
}
}
if ((intersects % 2) != 0)
{
int rise = 0;
while (!coordinateGrid[x][y + rise][z])
{
coordinateGrid[x][y + rise][z] = true;
rise++;
}
}
else if (intersects == 0)
{
// If there is no intersecting surface above whatsoever, this column cannot
// contain points that are inside a polygon.
break;
}
}
}
}
}
}
/* Returns the middle point in between two vect3. */
public static Vect3 MiddlePoint(Vect3 v1, Vect3 v2)
{
return((v1 + v2) / 2d);
}
public static double FindShortestDistanceLinePoint(Vect3 lineStart, Vect3 lineEnd, Vect3 point)
{
Vect3 p = FindClosestLinePoint(lineStart, lineEnd, point);
return((point - p).Length());
}
/* Static Functions */
/* Calculates dot product (scalar) of two vectors. */
public static double Dot(Vect3 v1, Vect3 v2)
{
return((v1.x * v2.x) + (v1.y * v2.y) + (v1.z * v2.z));
}
public bool Equals(Vect3 other)
{
return(this == other);
}
/**
* Generates a NodeGraph from a skeletonized voxelgrid, by traversing neighboring
* voxels and creating nodes connected by edges to maintain the topology of the voxelgrid.
*/
private static NodeGraph CreateNodeGraph(DistanceGrid distanceGrid, VoxelGrid voxelGrid)
{
bool[][][] visitedGrid = new bool[distanceGrid.xBound + 1][][];
for (int i = 0; i < visitedGrid.Length; i++)
{
visitedGrid[i] = new bool[distanceGrid.yBound + 1][];
for (int j = 0; j < visitedGrid[i].Length; j++)
{
visitedGrid[i][j] = new bool[distanceGrid.zBound + 1];
}
}
NodeGraph nodeGraph = new NodeGraph();
Queue <(Point3, Point3)> startPoints = GetStartPoints(distanceGrid);
Point3 initialPoint = startPoints.Peek().Item1;
nodeGraph.AddNode(voxelGrid.LowestPositionAtCoordinate(initialPoint));
visitedGrid[initialPoint.x][initialPoint.y][initialPoint.z] = true;
//Breadth-first traversing of the Voxels
while (startPoints.Count > 0)
{
(Point3 startPoint, Point3 currentPoint) = startPoints.Dequeue();
//Special case for when connecting to an allready visited node
if (visitedGrid[currentPoint.x][currentPoint.y][currentPoint.z])
{
Vect3 pos1 = voxelGrid.LowestPositionAtCoordinate(startPoint);
Vect3 pos2 = voxelGrid.LowestPositionAtCoordinate(currentPoint);
Node startNode = nodeGraph.GetNode(pos1);
Node currentNode = nodeGraph.GetNode(pos2);
if (nodeGraph.GetNode(pos1) == null || nodeGraph.GetNode(pos2) == null)
{
continue;
}
bool shouldLink = true;
foreach (var neighbor in currentNode.neighbors)
{
if (neighbor.nodeIndex == startNode.index)
{
shouldLink = false;
}
else
{
Node neighborNode = nodeGraph.GetNode(neighbor.nodeIndex);
foreach (var nextNeighbor in neighborNode.neighbors)
{
if (nextNeighbor.nodeIndex == startNode.index)
{
shouldLink = false;
}
}
}
if (!shouldLink)
{
break;
}
}
if (shouldLink)
{
nodeGraph.LinkNodes(pos1, pos2);
}
else
{
continue;
}
}
Point3 cameFrom = startPoint;
List <Point3> pathPoints = new List <Point3>()
{
startPoint
};
Vect3 startCoordinate = voxelGrid.LowestPositionAtCoordinate(startPoint);
Vect3 currentCoordinate = voxelGrid.LowestPositionAtCoordinate(currentPoint);
List <Vect3> intermediateCoords = new List <Vect3>();
List <Point3> neighbors = new List <Point3>();
bool distanceWasValid = false;
//Traversing untill the Nodeplacement would make the Edge to far away from the voxels it represents,
//an already visited node is reached, an intersection is found or it's a dead end.
while (DistancesAreValid(startCoordinate, currentCoordinate, intermediateCoords, voxelGrid.resolution))
{
// Node we found was visited
if (visitedGrid[currentPoint.x][currentPoint.y][currentPoint.z])
{
// Ensures nothing happens after we break
distanceWasValid = true;
break;
}
neighbors = distanceGrid.Get26AdjacentNeighbors(currentPoint);
pathPoints.Add(currentPoint);
if (neighbors.Count != 2)
{
visitedGrid[currentPoint.x][currentPoint.y][currentPoint.z] = true;
distanceWasValid = true;
break;
}
// Only one new neighbor
intermediateCoords.Add(voxelGrid.LowestPositionAtCoordinate(currentPoint));
Point3 temp = currentPoint;
currentPoint = neighbors[0] == cameFrom ? neighbors[1] : neighbors[0];
currentCoordinate = voxelGrid.LowestPositionAtCoordinate(currentPoint);
cameFrom = temp;
visitedGrid[cameFrom.x][cameFrom.y][cameFrom.z] = true;
}
double maxDistanceValue = 0;
foreach (var point in pathPoints)
{
maxDistanceValue += distanceGrid.grid[point.x][point.y][point.z] * voxelGrid.resolution * SquareRootOfThree; // Distance is 26-distance from edge
// double distanceValue = distanceGrid.grid[point.x][point.y][point.z]*voxelGrid.resolution*2;
// maxDistanceValue = distanceValue > maxDistanceValue ? distanceValue : maxDistanceValue;
}
maxDistanceValue /= (double)pathPoints.Count;
double minWidth = maxDistanceValue;
double minHeight = maxDistanceValue;
if (!distanceWasValid)
{
nodeGraph.AddNode(intermediateCoords[intermediateCoords.Count - 1], minWidth, minHeight, nodeGraph.GetNode(startCoordinate).index);
visitedGrid[cameFrom.x][cameFrom.y][cameFrom.z] = true;
if (!visitedGrid[currentPoint.x][currentPoint.y][currentPoint.z])
{
startPoints.Enqueue((cameFrom, currentPoint));
}
}
else
{
nodeGraph.AddNode(currentCoordinate, minWidth, minHeight, nodeGraph.GetNode(startCoordinate).index);
foreach (var neighbor in neighbors)
{
if (neighbor == cameFrom ||
visitedGrid[neighbor.x][neighbor.y][neighbor.z])
{
continue;
}
startPoints.Enqueue((currentPoint, neighbor));
}
}
}
return(nodeGraph);
}
private void SetupVoxelGrid(Structure structure = null)
{
ComponentWiseResolutionDivider = DefaultComponentWiseResolutionDivider;
double rightLength;
double heightLength;
double depthLength;
if (resolution == 0)
{
if (structure != null)
{
double shortestSide = 0;
foreach (var component in structure.components)
{
OrientedBBox componentOBB = new OrientedBBox(component);
rightLength = (componentOBB.localMaxX - componentOBB.localOrigin).Length();
heightLength = (componentOBB.localMaxY - componentOBB.localOrigin).Length();
depthLength = (componentOBB.localMaxZ - componentOBB.localOrigin).Length();
shortestSide += Math.Min(rightLength, Math.Min(heightLength, depthLength));
componentOBBs[component.index] = componentOBB;
}
shortestSide /= structure.components.Count;
resolution = shortestSide / ComponentWiseResolutionDivider;
}
else
{
rightLength = (orientedBbox.localMaxX - orientedBbox.localOrigin).Length();
heightLength = (orientedBbox.localMaxY - orientedBbox.localOrigin).Length();
depthLength = (orientedBbox.localMaxZ - orientedBbox.localOrigin).Length();
double shortestSide = Math.Min(rightLength, Math.Min(heightLength, depthLength));
resolution = shortestSide / ComponentWiseResolutionDivider;
}
}
halfResolution = resolution / 2d;
rightLength = (orientedBbox.localMaxX - orientedBbox.localOrigin).Length();
heightLength = (orientedBbox.localMaxY - orientedBbox.localOrigin).Length();
depthLength = (orientedBbox.localMaxZ - orientedBbox.localOrigin).Length();
xBound = Convert.ToInt32(Math.Round(rightLength / resolution)) + 4 + 1;
yBound = Convert.ToInt32(Math.Round(heightLength / resolution)) + 4 + 1;
zBound = Convert.ToInt32(Math.Round(depthLength / resolution)) + 4 + 1;
voxelStartCoordinate = orientedBbox.localOrigin - (orientedBbox.localX * 2d * resolution +
orientedBbox.localY * 2d * resolution +
orientedBbox.localZ * 2d * resolution);
// Include offset to move start position to the first cube center.
voxelMiddleStartCoordinate = orientedBbox.localOrigin - (orientedBbox.localX * 1.5d * resolution +
orientedBbox.localY * 1.5d * resolution +
orientedBbox.localZ * 1.5d * resolution);
coordinateGrid = new bool[xBound][][];
for (int i = 0; i < coordinateGrid.Length; i++)
{
coordinateGrid[i] = new bool[yBound][];
for (int j = 0; j < coordinateGrid[i].Length; j++)
{
coordinateGrid[i][j] = new bool[zBound];
}
}
// Resolution offsets are based around the Voxel center. This allows faster intersection calculations.
resolutionOffsets = new List <Vect3>
{
new Vect3(-halfResolution, -halfResolution, -halfResolution),
new Vect3(-halfResolution, -halfResolution, halfResolution),
new Vect3(-halfResolution, halfResolution, -halfResolution),
new Vect3(-halfResolution, halfResolution, halfResolution),
new Vect3(halfResolution, -halfResolution, -halfResolution),
new Vect3(halfResolution, -halfResolution, halfResolution),
new Vect3(halfResolution, halfResolution, -halfResolution),
new Vect3(halfResolution, halfResolution, halfResolution)
};
}
/*
* Fills VoxelGrid from component faces using a digital differential analyzer method.
*/
public void CreateShellFromFaces(Component component)
{
foreach (Face face in component.faces)
{
List <Vect3> trianglePoints = ConvertFaceToLocal(face, component);
Vect3 v1 = trianglePoints[0] / resolution;
Vect3 v2 = trianglePoints[1] / resolution;
Vect3 v3 = trianglePoints[2] / resolution;
// v2->v3 should be shortest triangle side
double v2v1length = (v2 - v1).Length();
double v3v2length = (v3 - v2).Length();
if (v3v2length > v2v1length)
{
Vect3 temp = v1;
v1 = v3;
v3 = temp;
}
double v3v1length = (v3 - v1).Length();
if (v3v2length > v3v1length)
{
Vect3 temp = v2;
v2 = v1;
v1 = temp;
}
Vect3 lp1 = v1;
Vect3 lp2 = v1;
Vect3 d2 = v2 - v1;
Vect3 d3 = v3 - v1;
double d2step = d2.Length();
double d3step = d3.Length();
double outerstep = d2step > d3step ? d2step : d3step;
outerstep *= 2;
d2 /= outerstep;
d3 /= outerstep;
int xi, yi, zi;
xi = Convert.ToInt32(lp1.x);
yi = Convert.ToInt32(lp1.y);
zi = Convert.ToInt32(lp1.z);
coordinateGrid[xi][yi][zi] = true;
lp1 += d2;
lp2 += d3;
int k = 0;
while (k <= outerstep)
{
Vect3 d = lp2 - lp1;
double step, x, y, z;
int i;
step = d.Length();
step *= 2;
if (step != 0)
{
d /= step;
}
x = lp1.x;
y = lp1.y;
z = lp1.z;
i = 0;
yi = Convert.ToInt32(y);
zi = Convert.ToInt32(z);
while (i <= step)
{
xi = Convert.ToInt32(x);
coordinateGrid[xi][yi][zi] = true;
yi = Convert.ToInt32(y);
coordinateGrid[xi][yi][zi] = true;
zi = Convert.ToInt32(z);
coordinateGrid[xi][yi][zi] = true;
x += d.x;
y += d.y;
z += d.z;
i += 1;
}
lp1 += d2;
lp2 += d3;
k += 1;
}
}
}
public static Matrix4x4 Translation(Vect3 v)
{
return(Translation(v.x, v.y, v.z));
}
public Vect3(Vect3 other)
{
x = other.x;
y = other.y;
z = other.z;
}
public double GetEdgeWidth(Vect3 coordinate1, Vect3 coordinate2)
{
return(GetEdgeWidth(GetEdge(coordinate1, coordinate2).index));
}
public VoxelGrid(Structure structure, Vect3 plane1Normal = null, Vect3 plane2Normal = null, Vect3 plane3Normal = null)
{
orientedBbox = new OrientedBBox(structure, plane1Normal, plane2Normal, plane3Normal);
SetupVoxelGrid(structure);
foreach (Component component in structure.components)
{
CreateShellFromFaces(component);
}
}
/* Coordinate initializer */
public Vertex(Vect3 _coordinate)
{
this.coordinate = _coordinate;
this._faceIndices = new List <int>();
this.faceIndices = _faceIndices.AsReadOnly();
}
