// Static function to find the intersection of two planes
// If they are parallel, returns false and outputs an invalid line struct
// Otherwise, returns true and outputs the line of intersection
public static bool Intersect(Plane a, Plane b, out Line result)
{
Vec3 cross = Vec3.Cross(a.normal, b.normal);
double magsq = cross.ComputeMagnitudeSquared();
if (magsq == 0)
{
// failure! planes did not intersect, or planes were equal
result = new Line { direction = Vec3.Zero, origin = Vec3.Zero }; // not a valid line!
return false;
}
double invmag = 1.0 / Math.Sqrt(magsq);
Vec3 line_direction = cross * invmag;
// using plane a to find intersection (also could try b?)
Vec3 in_a_toward_edge = Vec3.Normalize(Vec3.Cross(a.normal, line_direction));
Vec3 point_in_a = a.normal * a.offset;
double dist = b.PointDistance(point_in_a);
// seems this number could be either the positive or negative of what we want...
double unsigned_r = dist * invmag;
Vec3 positive = point_in_a + in_a_toward_edge * unsigned_r;
Vec3 negative = point_in_a - in_a_toward_edge * unsigned_r;
// figure out which one is actually at the intersection (or closest to it)
double positive_check = new Vec2 { x = a.PointDistance(positive), y = b.PointDistance(positive) }.ComputeMagnitudeSquared();
double negative_check = new Vec2 { x = a.PointDistance(negative), y = b.PointDistance(negative) }.ComputeMagnitudeSquared();
// and use that one as a point on the line (for the out value)
Vec3 point_on_line;
if (positive_check < negative_check)
point_on_line = positive;
else
point_on_line = negative;
// success! planes intersectedx
result = new Line { origin = point_on_line, direction = line_direction };
return true;
}
// 2D yes-or-no intersection test for a pair of line segments
public static bool LineSegmentIntersection2D(Vec2 a_begin, Vec2 a_end, Vec2 b_begin, Vec2 b_end)
{
Vec2 a_dir = a_end - a_begin;
Vec2 b_dir = b_end - b_begin;
Vec2 a_normal = new Vec2 { x = a_dir.y, y = -a_dir.x };
a_normal /= a_normal.ComputeMagnitude();
Vec2 b_normal = new Vec2 { x = b_dir.y, y = -b_dir.x };
b_normal /= b_normal.ComputeMagnitude();
double a_offset = Vec2.Dot(a_normal, a_begin);
double b_offset = Vec2.Dot(b_normal, b_begin);
double a_dir_dot = Vec2.Dot(a_dir, b_normal);
double a_origin_dot = Vec2.Dot(a_begin, b_normal);
double a_hit = (b_offset - a_origin_dot) / a_dir_dot;
double b_dir_dot = Vec2.Dot(b_dir, a_normal);
double b_origin_dot = Vec2.Dot(b_begin, a_normal);
double b_hit = (a_offset - b_origin_dot) / b_dir_dot;
Vec2 a_intersect = a_begin + a_dir * a_hit;
Vec2 b_intersect = b_begin + b_dir * b_hit;
return (a_hit >= 0 && a_hit <= 1 && b_hit >= 0 && b_hit <= 1);
}
public static BasicModelVert Interpolate(BasicModelVert[] verts, double[] weights)
{
Vec3 position = Vec3.Zero;
Vec3 normal = Vec3.Zero;
Vec2 uv = Vec2.Zero;
for (int i = 0; i < weights.Length; i++)
{
position += verts[i].position * weights[i];
normal += verts[i].normal * weights[i];
uv += verts[i].uv * weights[i];
}
normal /= normal.ComputeMagnitude();
return new BasicModelVert { position = position, normal = normal, uv = uv };
}
// Function to determine whether a 2D line intersects the triangle with vertices at (0,0), (1,0), and (0,1); returns true if they intersect, false otherwise
// The output variables "enter" and "exit" are the number of times the direction vector must be repeated (starting at the line's origin) to reach the line's intersections with the triangle
// If there is no interesection, both will be zero
public static bool LineIntersectIJTriangle(Vec2 line_origin, Vec2 line_direction, out double enter, out double exit)
{
double[] edge_hits = new double[] { -line_origin.x / line_direction.x, -line_origin.y / line_direction.y, (1.0 - line_origin.x - line_origin.y) / (line_direction.x + line_direction.y) };
List<double> hits = new List<double>();
for (int i = 0; i < 3; i++)
{
double t = edge_hits[i];
if (t >= 0 || t < 0) // check that it's a real number
{
Vec2 pos = line_origin + line_direction * t;
switch (i)
{
case 0: // x is zero, check y
if (pos.y >= 0 && pos.y <= 1)
hits.Add(t);
break;
case 1: // y is zero, check x
if (pos.x >= 0 && pos.x <= 1)
hits.Add(t);
break;
case 2: // x + y is one, check x and y
default:
if (pos.x >= 0 && pos.y >= 0)
hits.Add(t);
break;
}
}
}
if (hits.Count > 0)
{
double min = hits[0], max = min;
for (int i = 1; i < hits.Count; i++)
{
min = Math.Min(min, hits[i]);
max = Math.Max(max, hits[i]);
}
enter = min;
exit = max;
return true;
}
else
{
enter = exit = 0.0;
return false;
}
}
// Returns a Vec2 representing the PoI in the triangle's coordinate system
// A value of (0,0) corresponds to the 1st vertex, (1,0) corresponds to the 2nd vertex, (0,1) corresponds to the 3rd vertex
public static Vec2 VectorToTriangleCoords(Vec3[] tri_verts, Vec3 tri_normal, Vec3 point_of_interest)
{
Vec3 relative = point_of_interest - tri_verts[0];
Vec3 b_minus_a = tri_verts[1] - tri_verts[0];
Vec3 c_minus_a = tri_verts[2] - tri_verts[0];
Vec3 P = Vec3.Cross(c_minus_a, tri_normal), Q = Vec3.Cross(b_minus_a, tri_normal);
double div_x = 1.0 / Vec3.Dot(P, b_minus_a);
double div_y = 1.0 / Vec3.Dot(Q, c_minus_a);
Vec2 result = new Vec2 { x = Vec3.Dot(P, relative) * div_x, y = Vec3.Dot(Q, relative) * div_y };
return result;
}
// Finds the intersection of two triangles
// If the triangles are coplanar or on parallel planes, returns null
// If there is an intersection, returns the A and B triangles' IJ coordinates of the endpoints of the intersecting line segment
// The first indexer is which triangle, and the second is which of the two endpoints
public static Vec2[,] TriangleTriangleIntersection(Vec3[] a_verts, Vec3[] b_verts)
{
Plane a_plane = Plane.FromTriangleVertices(a_verts[0], a_verts[1], a_verts[2]);
Plane b_plane = Plane.FromTriangleVertices(b_verts[0], b_verts[1], b_verts[2]);
Line line;
if (!Plane.Intersect(a_plane, b_plane, out line))
return null;
Vec2 a_line_origin = VectorToTriangleCoords(a_verts, a_plane.normal, line.origin);
Vec2 a_line_direction = VectorToTriangleCoords(a_verts, a_plane.normal, line.origin + line.direction) - a_line_origin;
Vec2 b_line_origin = VectorToTriangleCoords(b_verts, b_plane.normal, line.origin);
Vec2 b_line_direction = VectorToTriangleCoords(b_verts, b_plane.normal, line.origin + line.direction) - b_line_origin;
double a_dot = Vec3.Dot(line.direction, a_plane.normal), b_dot = Vec3.Dot(line.direction, b_plane.normal);
double a_min, a_max;
if (!LineIntersectIJTriangle(a_line_origin, a_line_direction, out a_min, out a_max))
return null;
double b_min, b_max;
if (!LineIntersectIJTriangle(b_line_origin, b_line_direction, out b_min, out b_max))
return null;
if (a_max < b_min || b_max < a_min)
return null;
double min = Math.Max(a_min, b_min), max = Math.Min(a_max, b_max);
Vec2[,] result = new Vec2[2, 2];
result[0, 0] = a_line_origin + a_line_direction * min;
result[0, 1] = a_line_origin + a_line_direction * max;
result[1, 0] = b_line_origin + b_line_direction * min;
result[1, 1] = b_line_origin + b_line_direction * max;
return result;
}
public static void IntersectTest(int a_tri, ModelInput a_obj, int b_tri, ModelInput b_obj, out Vec2[,] intersections, out Vec3[] a_verts, out Vec3[] b_verts)
{
a_verts = new Vec3[] { a_obj.verts[a_obj.t_v[a_tri, 0]], a_obj.verts[a_obj.t_v[a_tri, 1]], a_obj.verts[a_obj.t_v[a_tri, 2]] };
b_verts = new Vec3[] { b_obj.verts[b_obj.t_v[b_tri, 0]], b_obj.verts[b_obj.t_v[b_tri, 1]], b_obj.verts[b_obj.t_v[b_tri, 2]] };
intersections = Util.TriangleTriangleIntersection(a_verts, b_verts);
}
public static double Dot(ref Vec2 a, ref Vec2 b)
{
return a.x * b.x + a.y * b.y;
}
// Finds the square of the distance between two points
public static double DistanceSquared(Vec2 a, Vec2 b)
{
double dx = a.x - b.x, dy = a.y - b.y;
return dx * dx + dy * dy;
}
// Finds the distance between two points
public static double Distance(Vec2 a, Vec2 b)
{
double dx = a.x - b.x, dy = a.y - b.y;
return Math.Sqrt(dx * dx + dy * dy);
}