public NuGenRot3D(NuGenVec3D from, NuGenVec3D to)
{
from.Normalize();
to.Normalize();
double cost =
NuGenVec3D.Dot(from, to) /
Math.Sqrt(NuGenVec3D.Dot(from, to) * NuGenVec3D.Dot(to, to));
if (cost > 0.99999)
{
r = 1;
v = new NuGenVec3D(0, 0, 0);
}
else if (cost < -0.99999)
{
NuGenVec3D frm = from.Normalized;
v = NuGenVec3D.Cross(frm, NuGenVec3D.UnitX);
if (v.Length < 0.00001)
{
v = NuGenVec3D.Cross(frm, NuGenVec3D.UnitY);
}
r = 0;
v.Normalize();
}
else
{
r = Math.Sqrt(0.5 * (1.0 + cost));
v = NuGenVec3D.Cross(from, to);
v *= Math.Sqrt((0.5 * (1.0 - cost)) / NuGenVec3D.Dot(v, v));
}
}
public bool Intersect(
NuGenRay3D ray, ref double t, ref double u, ref double v, ref NuGenVec3D normal
)
{
NuGenVec3D e1 = p1 - p0;
NuGenVec3D e2 = p2 - p0;
NuGenVec3D p = NuGenVec3D.Cross(ray.Direction, e2);
double a = NuGenVec3D.Dot(e1, p);
if (a > -NuGenVector.TINY_DOUBLE && a < NuGenVector.TINY_DOUBLE)
{
return(false);
}
double f = 1.0 / a;
NuGenVec3D s = ray.Point - p0;
u = f * NuGenVec3D.Dot(s, p);
if (u < 0.0 || u > 1.0)
{
return(false);
}
NuGenVec3D q = NuGenVec3D.Cross(s, e1);
v = f * NuGenVec3D.Dot(ray.Direction, q);
if (v < 0.0 || (u + v) > 1.0)
{
return(false);
}
t = f * NuGenVec3D.Dot(e2, q);
normal = NuGenVec3D.Cross(e1, e2); normal.Normalize();
return(true);
}