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