/// <summary>
/// get a normalized vector (x y z) / Length( (x y z) )
/// </summary>
/// <returns></returns>
public Vector3 <T> Normalized(T unit)
{
T l = Length(unit);
Numeric <T> numL = l;
if (numL.Equals(Numeric <T> .Zero()))
{
return(new Vector3 <T>(Numeric <T> .Zero()));
}
return(new Vector3 <T>(x / l, y / l, z / l));
}
public IntersectionResult <T> IntersectT(Line3 <T> line)
{
Numeric <T> d = normal ^ line.Dir;
if (d.Equals(Numeric <T> .Zero()))
{
return(new IntersectionResult <T>(false));
}
T t = -((normal ^ line.Pos) - distance) / d;
return(new IntersectionResult <T>(t, true));
}
public Vector3d <T> IntersectT(Line3 <T> line)
{
Numeric <T> d = normal ^ line.Dir;
if (d.Equals(Numeric <T> .Zero()))
{
return(Vector3d <T> .Zero());
}
T t = -((normal ^ line.From) - distance) / d;
Vector3d <T> point = line.GetPoint(t);
return(point);
}
/// <summary>
/// Normalize the vector (x y) / Length( (x y) )
/// </summary>
/// <returns></returns>
public Vector2 <T> Normalize(T unit)
{
T l = Length(unit);
Numeric <T> numL = l;
if (!numL.Equals(Numeric <T> .Zero()))
{
x /= l;
y /= l;
}
return(this);
}
public IntersectionResult <Vec3 <T> > Intersect(Line3 <T> line)
{
Numeric <T> d = normal ^ line.Dir;
if (d.Equals(Numeric <T> .Zero()))
{
return(new IntersectionResult <Vec3 <T> >(false));
}
T t = -((normal ^ line.Pos) - distance) / d;
Vec3 <T> point = line.GetPoint(t);
return(new IntersectionResult <Vec3 <T> >(point, true));
}
Inverse(T unit)
{
int i, j, k;
Matrix33 <T> s = Matrix33 <T> .Identity(unit);
Matrix33 <T> t = new Matrix33 <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 MatrixNotInvertibleException("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 MatrixNotInvertibleException("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);
}