Matrix3d3(Numeric <T> a, Numeric <T> b, Numeric <T> c, Numeric <T> d, Numeric <T> e, Numeric <T> f, Numeric <T> g, Numeric <T> h, Numeric <T> i)
{
x = new Numeric <T> [3][];
for (int k = 0; k < 3; k++)
{
x[k] = new Numeric <T> [3];
}
x[0][0] = a;
x[0][1] = b;
x[0][2] = c;
x[1][0] = d;
x[1][1] = e;
x[1][2] = f;
x[2][0] = g;
x[2][1] = h;
x[2][2] = i;
}
Inverse(T unit)
{
int i, j, k;
Matrix3d3 <T> s = Matrix3d3 <T> .Identity(unit);
Matrix3d3 <T> t = new Matrix3d3 <T>(this);
// Forward elimination
for (i = 0; i < 2; i++)
{
int pivot = i;
Numeric <T> pivotsize = (t[i][i]);
if (pivotsize < Numeric <T> .Zero())
{
pivotsize = -pivotsize;
}
for (j = i + 1; j < 3; j++)
{
Numeric <T> tmp = (t[j][i]);
if (tmp < Numeric <T> .Zero())
{
tmp = -tmp;
}
if (tmp > pivotsize)
{
pivot = j;
pivotsize = tmp;
}
}
if (pivotsize.Equals(Numeric <T> .Zero()))
{
throw new Exception("Cannot invert singular matrix.");
}
if (pivot != i)
{
for (j = 0; j < 3; j++)
{
T tmp;
tmp = t[i][j];
t[i][j] = t[pivot][j];
t[pivot][j] = tmp;
tmp = s[i][j];
s[i][j] = s[pivot][j];
s[pivot][j] = tmp;
}
}
for (j = i + 1; j < 3; j++)
{
T f = t[j][i] / t[i][i];
for (k = 0; k < 3; k++)
{
t[j][k] -= f * t[i][k];
s[j][k] -= f * s[i][k];
}
}
}
// Backward substitution
for (i = 2; i >= 0; --i)
{
Numeric <T> f;
if ((f = t[i][i]).Equals(Numeric <T> .Zero()))
{
throw new Exception("Cannot invert singular matrix.");
}
for (j = 0; j < 3; j++)
{
t[i][j] /= f;
s[i][j] /= f;
}
for (j = 0; j < i; j++)
{
f = t[j][i];
for (k = 0; k < 3; k++)
{
t[j][k] -= f * t[i][k];
s[j][k] -= f * s[i][k];
}
}
}
return(s);
}
public MinStruct(Numeric <T> x1, Numeric <T> y1, Numeric <T> z1, Numeric <T> x2, Numeric <T> y2, Numeric <T> z2)
{
X = x2;
if (x1 < x2)
{
X = x1;
}
Y = y2;
if (y1 < y2)
{
Y = y1;
}
Z = z2;
if (z1 < z2)
{
Z = z1;
}
}
public MaxStruct(Numeric <T> x1, Numeric <T> y1, Numeric <T> z1, Numeric <T> x2, Numeric <T> y2, Numeric <T> z2)
{
X = x2;
if (x1 > x2)
{
X = x1;
}
Y = y2;
if (y1 > y2)
{
Y = y1;
}
Z = z2;
if (z1 > z2)
{
Z = z1;
}
}
private static bool Equals(ref Numeric <T> x, ref Numeric <T> y)
{
return(EqualityComparer <T> .Default.Equals(x.Value, y.Value));
}
public BoundingBox(Numeric <T> x1, Numeric <T> y1, Numeric <T> z1, Numeric <T> x2, Numeric <T> y2, Numeric <T> z2)
{
this.X1 = x1;
this.Y1 = y1;
this.Z1 = z1;
this.X2 = x2;
this.Y2 = y2;
this.Z2 = z2;
Min = new MinStruct <Numeric <T> >(x1, y1, z1, x2, y2, z2);
Max = new MaxStruct <Numeric <T> >(x1, y1, z1, x2, y2, z2);
}
/// <summary>
/// Constructor (x y) = (a b)
/// </summary>
/// <param name="a"></param>
public Vector2(T a, T b)
{
x = a;
y = b;
}
/// <summary>
/// Constructor (x y) = (a b)
/// </summary>
/// <param name="a"></param>
public Vec2(T a, T b)
{
x = a;
y = b;
}
public T DistanceTo(Line3 <T> line)
{
Numeric <T> d = (dir % line.dir) ^ (line.pos - pos);
return((d >= Numeric <T> .Zero()) ? d : -d);
}
/// <summary>
/// Constructor x = y = a
/// </summary>
/// <param name="a"></param>
public Vector2(T a)
{
x = y = a;
}
public Plane3(Vector3 <T> p, Vector3 <T> n, T unit)
{
normal = n;
normal.Normalize(unit);
distance = normal ^ p;
}
public Plane3(Vector3 <T> n, T d, T unit)
{
normal = n;
normal.Normalize(unit);
distance = d;
}
public Plane3(Vector3 <T> point1, Vector3 <T> point2, Vector3 <T> point3, T unit)
{
normal = (point2 - point1) % (point3 - point1);
normal.Normalize(unit);
distance = normal ^ point1;
}
/// <summary>
/// Constructor x = y = z = a
/// </summary>
/// <param name="a"></param>
public Vec3(T a)
{
x = y = z = a;
}
public bool Equals(Numeric <T> other)
{
return(Equals(this, other));
}
/// <summary>
/// Constructor (x y z) = (a b c)
/// </summary>
/// <param name="a"></param>
public Vec3(T a, T b, T c)
{
x = a;
y = b;
z = c;
}
/// <summary>
/// Constructor x = y = a
/// </summary>
/// <param name="a"></param>
public Vec2(T a)
{
x = y = a;
}