public static Vector3m PlaneNormal(Vector3m v0, Vector3m v1, Vector3m v2)
{
Vector3m a = v1 - v0;
Vector3m b = v2 - v0;
return(a.Cross(b));
}
public bool RaySegmentIntersection(out Vector3m intersection, out Rational distanceSquared, Vector3m linePoint1, Vector3m lineVec1, Vector3m linePoint3, Vector3m linePoint4, Vector3m direction)
{
var lineVec2 = linePoint4 - linePoint3;
Vector3m lineVec3 = linePoint3 - linePoint1;
Vector3m crossVec1and2 = lineVec1.Cross(lineVec2);
Vector3m crossVec3and2 = lineVec3.Cross(lineVec2);
var res = Misc.PointLineDistance(linePoint3, linePoint4, linePoint1);
if (res == 0) // line and ray are collinear
{
var p = linePoint1 + lineVec1;
var res2 = Misc.PointLineDistance(linePoint3, linePoint4, p);
if (res2 == 0)
{
var s = linePoint3 - linePoint1;
if (s.X == direction.X && s.Y == direction.Y && s.Z == direction.Z)
{
intersection = linePoint3;
distanceSquared = s.LengthSquared();
return(true);
}
}
}
//is coplanar, and not parallel
if (/*planarFactor == 0.0f && */ crossVec1and2.LengthSquared() > 0)
{
var s = crossVec3and2.Dot(crossVec1and2) / crossVec1and2.LengthSquared();
if (s >= 0)
{
intersection = linePoint1 + (lineVec1 * s);
distanceSquared = (lineVec1 * s).LengthSquared();
if ((intersection - linePoint3).LengthSquared() + (intersection - linePoint4).LengthSquared() <=
lineVec2.LengthSquared())
{
return(true);
}
}
}
intersection = Vector3m.Zero();
distanceSquared = 0;
return(false);
}