public static bool ApproxEquals(NuGenVec3D a, NuGenVec3D b)
{
return
Math.Abs(a._x[0] - b._x[0]) < NuGenVector.TINY_DOUBLE &&
Math.Abs(a._x[1] - b._x[1]) < NuGenVector.TINY_DOUBLE &&
Math.Abs(a._x[2] - b._x[2]) < NuGenVector.TINY_DOUBLE;
}
public static bool ApproxEquals(NuGenVec3D a, NuGenVec3D b)
{
return
(Math.Abs(a._x[0] - b._x[0]) < NuGenVector.TINY_DOUBLE &&
Math.Abs(a._x[1] - b._x[1]) < NuGenVector.TINY_DOUBLE &&
Math.Abs(a._x[2] - b._x[2]) < NuGenVector.TINY_DOUBLE);
}
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 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 static NuGenVec3D Cross(NuGenVec3D a, NuGenVec3D b)
{
return(new NuGenVec3D(
a.y * b.z - a.z * b.y,
a.z * b.x - a.x * b.z,
a.x * b.y - a.y * b.x
));
}
public static NuGenVec3D Max(NuGenVec3D a, NuGenVec3D b)
{
return(new NuGenVec3D(
Math.Max(a._x[0], b._x[0]),
Math.Max(a._x[1], b._x[1]),
Math.Max(a._x[2], b._x[2])
));
}
public override bool Equals(object obj)
{
NuGenVec3D x = (NuGenVec3D)obj;
return(
_x[0] == x._x[0] &&
_x[1] == x._x[1] &&
_x[2] == x._x[2]
);
}
public static NuGenVec3D Cross(NuGenVec3D a, NuGenVec3D b)
{
return new NuGenVec3D(
a.y * b.z - a.z * b.y,
a.z * b.x - a.x * b.z,
a.x * b.y - a.y * b.x
);
}
public NuGenRot3D(double radians, NuGenVec3D axis)
{
axis.Normalize();
r = Math.Cos(radians/2.0);
v = axis.Normalized * Math.Sin(radians/2.0);
}
public NuGenRay3D(NuGenPnt3D p, NuGenVec3D v)
{
this.p = p;
this.v = v;
}
public bool Intersect(NuGenRay3F r)
{
bool inside = true;
int [] quadrant = new int[3];
int i;
int whichPlane;
float [] max = new float[3];
float [] candidatePlane = new float[3];
NuGenVec3D coord = new NuGenVec3D(0,0,0);
for (i = 0; i < 3; i++)
{
if (r.Point[i] < lower[i])
{
quadrant[i] = 1;
candidatePlane[i] = lower[i];
inside = false;
}
else if (r.Point[i] > upper[i])
{
quadrant[i] = 0;
candidatePlane[i] = upper[i];
inside = false;
}
else
{
quadrant[i] = 2;
}
}
if (inside) return true;
for (i = 0; i < 3; i++)
{
if (quadrant[i] != 2 && r.Direction[i] != 0.0)
{
max[i] = (candidatePlane[i]-r.Point[i]) / r.Direction[i];
}
else
{
max[i] = -1.0f;
}
}
whichPlane = 0;
for (i = 1; i < 3; i++)
{
if (max[whichPlane] < max[i]) whichPlane = i;
}
if (max[whichPlane] < 0.0) return false;
for (i = 0; i < 3; i++)
{
if (whichPlane != i)
{
coord[i] = r.Point[i] + max[whichPlane] * r.Direction[i];
if (coord[i] < lower[i] || coord[i] > upper[i])
{
return false;
}
}
else
{
coord[i] = candidatePlane[i];
}
}
return true;
}
public static double Dot(NuGenVec3D u, NuGenVec3D v)
{
return(u[0] * v[0] + u[1] * v[1] + u[2] * v[2]);
}
public NuGenShift3D(NuGenVec3D shift)
{
v = shift;
}
public NuGenRot3D(double radians, NuGenVec3D axis)
{
axis.Normalize();
r = Math.Cos(radians / 2.0);
v = axis.Normalized * Math.Sin(radians / 2.0);
}
public NuGenShift3D(double x, double y, double z)
{
v = new NuGenVec3D(x,y,z);
}
public NuGenScale3D(double x, double y, double z)
{
v = new NuGenVec3D(x, y, z);
}
public static double Dot(NuGenVec3D u, NuGenVec3D v)
{
return u[0] * v[0] + u[1] * v[1] + u[2] * v[2];
}
public NuGenScale3D(NuGenVec3D shift)
{
v = shift;
}
public static NuGenVec3D Max(NuGenVec3D a, NuGenVec3D b)
{
return new NuGenVec3D(
Math.Max(a._x[0], b._x[0]),
Math.Max(a._x[1], b._x[1]),
Math.Max(a._x[2], b._x[2])
);
}
public NuGenShift3D(NuGenVec3D shift)
{
v = shift;
}
public bool Intersect(NuGenRay3D r)
{
bool inside = true;
int [] quadrant = new int[3];
int i;
int whichPlane;
double [] maxT = new double[3];
double [] candidatePlane = new double[3];
NuGenVec3D coord = new NuGenVec3D(0, 0, 0);
for (i = 0; i < 3; i++)
{
if (r.Point[i] < lower[i])
{
quadrant[i] = 1;
candidatePlane[i] = lower[i];
inside = false;
}
else if (r.Point[i] > upper[i])
{
quadrant[i] = 0;
candidatePlane[i] = upper[i];
inside = false;
}
else
{
quadrant[i] = 2;
}
}
if (inside)
{
return(true);
}
for (i = 0; i < 3; i++)
{
if (quadrant[i] != 2 && r.Direction[i] != 0.0)
{
maxT[i] = (candidatePlane[i] - r.Point[i]) / r.Direction[i];
}
else
{
maxT[i] = -1.0;
}
}
whichPlane = 0;
for (i = 1; i < 3; i++)
{
if (maxT[whichPlane] < maxT[i])
{
whichPlane = i;
}
}
if (maxT[whichPlane] < 0.0)
{
return(false);
}
for (i = 0; i < 3; i++)
{
if (whichPlane != i)
{
coord[i] = r.Point[i] + maxT[whichPlane] * r.Direction[i];
if (coord[i] < lower[i] || coord[i] > upper[i])
{
return(false);
}
}
else
{
coord[i] = candidatePlane[i];
}
}
return(true);
}
public NuGenRay3D(NuGenPnt3D p, NuGenVec3D v)
{
this.p = p;
this.v = v;
}
public NuGenShift3D(double x, double y, double z)
{
v = new NuGenVec3D(x, y, z);
}