public static PlaneIntersectType PlaneTest(ref IndexedVector4 p, ref IndexedVector3 v)
{
float planetestepsilon = 0.0001f;
float a = IndexedVector3.Dot(v, p.ToVector3()) + p.W;
PlaneIntersectType flag = (a > planetestepsilon) ? PlaneIntersectType.OVER : ((a < -planetestepsilon) ? PlaneIntersectType.UNDER : PlaneIntersectType.COPLANAR);
return flag;
}
public static float DistanceBetweenLines(ref IndexedVector3 ustart, ref IndexedVector3 udir, ref IndexedVector3 vstart, ref IndexedVector3 vdir, ref IndexedVector3? upoint, ref IndexedVector3? vpoint)
{
IndexedVector3 cp = IndexedVector3.Cross(udir, vdir);
cp.Normalize();
float distu = -IndexedVector3.Dot(cp, ustart);
float distv = -IndexedVector3.Dot(cp, vstart);
float dist = (float)Math.Abs(distu - distv);
if (upoint.HasValue)
{
IndexedVector4 plane = new IndexedVector4(IndexedVector3.Cross(vdir, cp).Normalized(),0);
plane.W = -IndexedVector3.Dot(plane.ToVector3(), vstart);
IndexedVector3 a = ustart + udir;
upoint = PlaneLineIntersection(ref plane, ref ustart, ref a);
}
if (vpoint.HasValue)
{
IndexedVector4 plane = new IndexedVector4(IndexedVector3.Cross(udir, cp).Normalized(), 0);
plane.W = -IndexedVector3.Dot(plane.ToVector3(), ustart);
IndexedVector3 a = vstart + vdir;
vpoint = PlaneLineIntersection(ref plane, ref vstart, ref a);
}
return dist;
}
public static IndexedVector3 PlaneLineIntersection(ref IndexedVector4 plane, ref IndexedVector3 p0, ref IndexedVector3 p1)
{
// returns the point where the line p0-p1 intersects the IndexedVector4 n&
IndexedVector3 dif = p1 - p0;
float dn = IndexedVector3.Dot(plane.ToVector3(), dif);
float t = -(plane.W + IndexedVector3.Dot(plane.ToVector3(), p0)) / dn;
return p0 + (dif * t);
}
public static IndexedVector3 PlaneProject(ref IndexedVector4 plane, ref IndexedVector3 point)
{
return point - plane.ToVector3() * (IndexedVector3.Dot(point, plane.ToVector3()) + plane.W);
}
public static IndexedVector3 ThreePlaneIntersection(IndexedVector4 p0, IndexedVector4 p1, IndexedVector4 p2)
{
IndexedVector3 N1 = p0.ToVector3();
IndexedVector3 N2 = p1.ToVector3();
IndexedVector3 N3 = p2.ToVector3();
IndexedVector3 n2n3 = IndexedVector3.Cross(N2, N3);
IndexedVector3 n3n1 = IndexedVector3.Cross(N3, N1);
IndexedVector3 n1n2 = IndexedVector3.Cross(N1, N2);
float quotient = IndexedVector3.Dot(N1, n2n3);
Debug.Assert(Math.Abs(quotient) > 0.000001f);
quotient = -1.0f / quotient;
n2n3 *= p0.W;
n3n1 *= p1.W;
n1n2 *= p2.W;
IndexedVector3 potentialVertex = n2n3;
potentialVertex += n3n1;
potentialVertex += n1n2;
potentialVertex *= quotient;
IndexedVector3 result = potentialVertex;
return result;
}
