public static bool ClosestPoints(Line line1, Line line2, out Vector3 line1ClosestPoint, out Vector3 line2ClosestPoint)
{
/*
float a = Vector3.Dot(line1.Direction, line1.Direction);
float b = Vector3.Dot(line1.Direction, line2.Direction);
float e = Vector3.Dot(line2.Direction, line2.Direction);
float d = a * e - b * b;
if (d != 0.0f)
{
var r = line1.Point - line2.Point;
float c = Vector3.Dot(line1.Direction, r);
float f = Vector3.Dot(line2.Direction, r);
float s = (b * f - c * e) / d;
float t = (a * f - c * b) / d;
line1ClosestPoint = line1.Point + line1.Direction * s;
line2ClosestPoint = line2.Point + line2.Direction * t;
return true;
}
else
{
//parrallel, can't compute
line1ClosestPoint = Vector3.zero;
line2ClosestPoint = Vector3.zero;
return false;
}
*/
//a and e are going to be 1, drop them out...
float b = Vector3.Dot(line1.Direction, line2.Direction);
float d = 1 - b * b;
if (d != 0.0f)
{
var r = line1.Point - line2.Point;
float c = Vector3.Dot(line1.Direction, r);
float f = Vector3.Dot(line2.Direction, r);
float s = (b * f - c) / d;
float t = (f - c * b) / d;
line1ClosestPoint = line1.Point + line1.Direction * s;
line2ClosestPoint = line2.Point + line2.Direction * t;
return true;
}
else
{
line1ClosestPoint = Vector3.zero;
line2ClosestPoint = Vector3.zero;
return false;
}
}
public static Vector3 ClosestPoint(Line line, Vector3 point)
{
if (line.Point == point) return point;
var v = (line.Point - point).normalized;
v = Vector3.Cross(line.Direction, v);
float b = Vector3.Dot(line.Direction, v);
float d = 1 - b * b;
var r = line.Point - point;
float c = Vector3.Dot(line.Direction, r);
float f = Vector3.Dot(v, r);
float s = (b * f - c) / d;
return line.Point + line.Direction * s;
}
/// <summary>
/// Find intersecting line of two planes
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <returns></returns>
public static Line IntersectionOfPlanes(Plane p1, Plane p2)
{
var line = new Line();
line.Direction = Vector3.Cross(p1.normal, p2.normal);
Vector3 ldir = Vector3.Cross(p2.normal, line.Direction);
float d = Vector3.Dot(p1.normal, ldir);
//if d is to close to 0, planes are parallel
if (Mathf.Abs(d) > 0.005f)
{
Vector3 p1Top2 = (p1.normal * p1.distance) - (p2.normal * p2.distance);
float t = Vector3.Dot(p1.normal, p1Top2) / d;
line.Point = (p2.normal * p2.distance) + t * ldir;
return line;
}
else
{
throw new System.Exception("both planes are parallel");
}
}
public static bool Intersection(Line line1, Line line2, out Vector3 point)
{
const float EPSILON = 0.0001f;
point = Vector3.zero;
var dir3 = line2.Point - line1.Point;
var pnorm1 = Vector3.Cross(line1.Direction, line2.Direction);
var pnorm2 = Vector3.Cross(dir3, line2.Direction);
if (Mathf.Abs(Vector3.Dot(dir3, pnorm1)) > EPSILON) return false; //lines aren't coplanar
float s = Vector3.Dot(pnorm2, pnorm1) / pnorm1.sqrMagnitude;
if (s >= 0 && s <= 1.0f)
{
point = line1.Point + (line1.Direction * s);
return true;
}
return false;
}
public static bool IntersectionOfPlanes(Plane p1, Plane p2, out Line line)
{
line = new Line();
line.Direction = Vector3.Cross(p1.normal, p2.normal);
Vector3 ldir = Vector3.Cross(p2.normal, line.Direction);
float d = Vector3.Dot(p1.normal, ldir);
//if d is to close to 0, planes are parallel
if (Mathf.Abs(d) > 0.005f)
{
Vector3 p1Top2 = (p1.normal * p1.distance) - (p2.normal * p2.distance);
float t = Vector3.Dot(p1.normal, p1Top2) / d;
line.Point = (p2.normal * p2.distance) + t * ldir;
return true;
}
else
{
line = new Line();
return false;
}
}
public static bool IntersectionOfLineAndPlane(Line line, Plane plane, out Vector3 point)
{
//calc dist...
float dotnum = Vector3.Dot((plane.normal * plane.distance) - line.Point, plane.normal);
float dotdenom = Vector3.Dot(line.Direction, plane.normal);
if (dotdenom != 0.0f)
{
float len = dotnum / dotdenom;
var v = line.Direction * len;
point = line.Point + v;
return true;
}
else
{
point = Vector3.zero;
return false;
}
}
/// <summary>
/// Find interesection location of a line and a plane
/// </summary>
/// <param name="line"></param>
/// <param name="plane"></param>
/// <returns></returns>
public static Vector3 IntersectionOfLineAndPlane(Line line, Plane plane)
{
//calc dist...
float dotnum = Vector3.Dot((plane.normal * plane.distance) - line.Point, plane.normal);
float dotdenom = Vector3.Dot(line.Direction, plane.normal);
if (dotdenom != 0.0f)
{
float len = dotnum / dotdenom;
var v = line.Direction * len;
return line.Point + v;
}
else
{
throw new System.Exception("line and plane are parallel");
}
}