public Matrix44 <T> SetAxisAngle(Vector3 <T> axis, T angle, T unit)
{
Vector3 <T> vunit = axis.Normalized(unit);
Numeric <T> sine = (GenericMath.Sin(angle, unit));
Numeric <T> cosine = (GenericMath.Cos(angle, unit));
Numeric <T> scalarUnit = unit;
x[0][0] = (Numeric <T>)vunit[0] * vunit[0] * (scalarUnit - cosine) + cosine;
x[0][1] = (Numeric <T>)vunit[0] * vunit[1] * (scalarUnit - cosine) + vunit[2] * sine;
x[0][2] = (Numeric <T>)vunit[0] * vunit[2] * (scalarUnit - cosine) - vunit[1] * sine;
x[0][3] = Numeric <T> .Zero();
x[1][0] = (Numeric <T>)vunit[0] * vunit[1] * (scalarUnit - cosine) - vunit[2] * sine;
x[1][1] = (Numeric <T>)vunit[1] * vunit[1] * (scalarUnit - cosine) + cosine;
x[1][2] = (Numeric <T>)vunit[1] * vunit[2] * (scalarUnit - cosine) + vunit[0] * sine;
x[1][3] = Numeric <T> .Zero();
x[2][0] = (Numeric <T>)vunit[0] * vunit[2] * (scalarUnit - cosine) + vunit[1] * sine;
x[2][1] = (Numeric <T>)vunit[1] * vunit[2] * (scalarUnit - cosine) - vunit[0] * sine;
x[2][2] = (Numeric <T>)vunit[2] * vunit[2] * (scalarUnit - cosine) + cosine;
x[2][3] = Numeric <T> .Zero();
x[3][0] = Numeric <T> .Zero();
x[3][1] = Numeric <T> .Zero();
x[3][2] = Numeric <T> .Zero();
x[3][3] = unit;
return(this);
}
/// <summary>
/// Set matrix to scale by s
/// </summary>
/// <param name="s">The uniform factor</param>
/// <returns></returns>
public Matrix44 <T> SetScale(T s, T unit)
{
x[0][0] = s;
x[0][1] = Numeric <T> .Zero();
x[0][2] = Numeric <T> .Zero();
x[0][3] = Numeric <T> .Zero();
x[1][0] = Numeric <T> .Zero();
x[1][1] = s;
x[1][2] = Numeric <T> .Zero();
x[1][3] = Numeric <T> .Zero();
x[2][0] = Numeric <T> .Zero();
x[2][1] = Numeric <T> .Zero();
x[2][2] = s;
x[2][3] = Numeric <T> .Zero();
x[3][0] = Numeric <T> .Zero();
x[3][1] = Numeric <T> .Zero();
x[3][2] = Numeric <T> .Zero();
x[3][3] = unit;
return(this);
}
public Matrix44(Matrix33 <T> r, Vector3 <T> t, T unit)
{
x = new Numeric <T> [4][];
for (int i = 0; i < 4; i++)
{
x[i] = new Numeric <T> [4];
}
x[0][0] = r[0][0];
x[0][1] = r[0][1];
x[0][2] = r[0][2];
x[0][3] = Numeric <T> .Zero();
x[1][0] = r[1][0];
x[1][1] = r[1][1];
x[1][2] = r[1][2];
x[1][3] = Numeric <T> .Zero();
x[2][0] = r[2][0];
x[2][1] = r[2][1];
x[2][2] = r[2][2];
x[2][3] = Numeric <T> .Zero();
x[3][0] = t[0];
x[3][1] = t[1];
x[3][2] = t[2];
x[3][3] = unit;
}
//public IntersectionResult<Vector3d<T>> Intersect(Line3<T> line)
//{
// Numeric<T> d = normal ^ line.Dir;
// if (d.Equals(Numeric<T>.Zero()))
// return new IntersectionResult<Vector3d<T>>(false);
// T t = -((normal ^ line.From) - distance) / d;
// Vector3d<T> point = line.GetPoint(t);
// return new IntersectionResult<Vector3d<T>>(point, true);
//}
//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.From) - distance) / d;
// return new IntersectionResult<T>(t, true);
//}
public void MultiplyByMatrix(Matrix4d4 <T> m, T unit)
{
Vector3d <T> dir1 = new Vector3d <T>(unit, Numeric <T> .Zero(), Numeric <T> .Zero()) % normal;
Numeric <T> dir1Len = dir1 ^ dir1;
Vector3d <T> tmp = new Vector3d <T>(Numeric <T> .Zero(), unit, Numeric <T> .Zero()) % normal;
Numeric <T> tmpLen = tmp ^ tmp;
if (tmpLen > dir1Len)
{
dir1 = tmp;
dir1Len = tmpLen;
}
tmp = new Vector3d <T>(Numeric <T> .Zero(), Numeric <T> .Zero(), unit) % normal;
tmpLen = tmp ^ tmp;
if (tmpLen > dir1Len)
{
dir1 = tmp;
}
Vector3d <T> dir2 = dir1 % normal;
Vector3d <T> point = distance * normal;
this = new Plane3 <T>(point * m, (point + dir2) * m, (point + dir1) * m, unit);
}
public void MakeIdentity(T unit)
{
x[0][0] = unit;
x[0][1] = Numeric <T> .Zero();
x[0][2] = Numeric <T> .Zero();
x[0][3] = Numeric <T> .Zero();
x[1][0] = Numeric <T> .Zero();
x[1][1] = unit;
x[1][2] = Numeric <T> .Zero();
x[1][3] = Numeric <T> .Zero();
x[2][0] = Numeric <T> .Zero();
x[2][1] = Numeric <T> .Zero();
x[2][2] = unit;
x[2][3] = Numeric <T> .Zero();
x[3][0] = Numeric <T> .Zero();
x[3][1] = Numeric <T> .Zero();
x[3][2] = Numeric <T> .Zero();
x[3][3] = unit;
}
public Matrix44 <T> SetShear(Shear6 <T> h, T unit)
{
x[0][0] = unit;
x[0][1] = h.YX;
x[0][2] = h.ZX;
x[0][3] = Numeric <T> .Zero();
x[1][0] = h.XY;
x[1][1] = unit;
x[1][2] = h.ZY;
x[1][3] = Numeric <T> .Zero();
x[2][0] = h.XZ;
x[2][1] = h.YZ;
x[2][2] = unit;
x[2][3] = Numeric <T> .Zero();
x[3][0] = Numeric <T> .Zero();
x[3][1] = Numeric <T> .Zero();
x[3][2] = Numeric <T> .Zero();
x[3][3] = unit;
return(this);
}
static public Plane3 <T> MultiplyByMatrix(Plane3 <T> plane, Matrix4d4 <T> m, T unit)
{
Vector3d <T> dir1 = new Vector3d <T>(unit, Numeric <T> .Zero(), Numeric <T> .Zero()) % plane.normal;
Numeric <T> dir1Len = dir1 ^ dir1;
Vector3d <T> tmp = new Vector3d <T>(Numeric <T> .Zero(), unit, Numeric <T> .Zero()) % plane.normal;
Numeric <T> tmpLen = tmp ^ tmp;
if (tmpLen > dir1Len)
{
dir1 = tmp;
dir1Len = tmpLen;
}
tmp = new Vector3d <T>(Numeric <T> .Zero(), Numeric <T> .Zero(), unit) % plane.normal;
tmpLen = tmp ^ tmp;
if (tmpLen > dir1Len)
{
dir1 = tmp;
}
Vector3d <T> dir2 = dir1 % plane.normal;
Vector3d <T> point = plane.distance * plane.normal;
return(new Plane3 <T>(point * m, (point + dir2) * m, (point + dir1) * m, unit));
}
public Matrix44 <T> SetTranslation(Vector3 <T> t, T unit)
{
x[0][0] = unit;
x[0][1] = Numeric <T> .Zero();
x[0][2] = Numeric <T> .Zero();
x[0][3] = Numeric <T> .Zero();
x[1][0] = Numeric <T> .Zero();
x[1][1] = unit;
x[1][2] = Numeric <T> .Zero();
x[1][3] = Numeric <T> .Zero();
x[2][0] = Numeric <T> .Zero();
x[2][1] = Numeric <T> .Zero();
x[2][2] = unit;
x[2][3] = Numeric <T> .Zero();
x[3][0] = t[0];
x[3][1] = t[1];
x[3][2] = t[2];
x[3][3] = unit;
return(this);
}
public Matrix44 <T> SetShear(Vector3 <T> h, T unit)
{
x[0][0] = unit;
x[0][1] = Numeric <T> .Zero();
x[0][2] = Numeric <T> .Zero();
x[0][3] = Numeric <T> .Zero();
x[1][0] = h[0];
x[1][1] = unit;
x[1][2] = Numeric <T> .Zero();
x[1][3] = Numeric <T> .Zero();
x[2][0] = h[1];
x[2][1] = h[2];
x[2][2] = unit;
x[2][3] = Numeric <T> .Zero();
x[3][0] = Numeric <T> .Zero();
x[3][1] = Numeric <T> .Zero();
x[3][2] = Numeric <T> .Zero();
x[3][3] = unit;
return(this);
}
/// <summary>
/// Set matrix to scale by s
/// </summary>
public Matrix44 <T> SetScale(Vector3 <T> s, T unit)
{
x[0][0] = s[0];
x[0][1] = Numeric <T> .Zero();
x[0][2] = Numeric <T> .Zero();
x[0][3] = Numeric <T> .Zero();
x[1][0] = Numeric <T> .Zero();
x[1][1] = s[1];
x[1][2] = Numeric <T> .Zero();
x[1][3] = Numeric <T> .Zero();
x[2][0] = Numeric <T> .Zero();
x[2][1] = Numeric <T> .Zero();
x[2][2] = s[2];
x[2][3] = Numeric <T> .Zero();
x[3][0] = Numeric <T> .Zero();
x[3][1] = Numeric <T> .Zero();
x[3][2] = Numeric <T> .Zero();
x[3][3] = unit;
return(this);
}
/// <summary>
/// Matrix multiplication
/// </summary>
/// <param name="v"></param>
/// <returns></returns>
public static Matrix44 <T> operator *(Matrix44 <T> v1, Matrix44 <T> v2)
{
Matrix44 <T> tmp = new Matrix44 <T>(Numeric <T> .Zero());
Matrix44 <T> .Multiply(v1, v2, ref tmp);
return(tmp);
}
public static Matrix33 <T> Identity(T unit)
{
Matrix33 <T> m = new Matrix33 <T>(Numeric <T> .Zero());
m.MakeIdentity(unit);
return(m);
}
public Shear6(T XY, T XZ, T YZ)
{
xy = XY;
xz = XZ;
yz = YZ;
yx = Numeric <T> .Zero();
zx = Numeric <T> .Zero();
zy = Numeric <T> .Zero();
}
public Shear6(Vector3 <T> v)
{
xy = v.X;
xz = v.Y;
yz = v.Z;
yx = Numeric <T> .Zero();
zx = Numeric <T> .Zero();
zy = Numeric <T> .Zero();
}
/// <summary>
/// Multiply x by n
/// </summary>
/// <typeparam name="T"></typeparam>
/// <param name="x"></param>
/// <param name="identity"></param>
/// <param name="n"></param>
/// <returns></returns>
static public T Multiply <T>(T x, int n)
where T : IEquatable <T>
{
Numeric <T> sum = Numeric <T> .Zero();
for (int i = 0; i < n; i++)
{
sum = sum + x;
}
return(sum);
}
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));
}
/// <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 Matrix44 <V> Select <V>(Func <T, V> transf) where V : IEquatable <V>
{
Matrix44 <V> transfMatrix = new Matrix44 <V>(Numeric <V> .Zero());
for (int i = 0; i < 4; ++i)
{
for (int j = 0; j < 4; ++j)
{
transfMatrix[i][j] = transf(x[i][j]);
}
}
return(transfMatrix);
}
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));
}
/// <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 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);
}
static public T Cos <T>(T x, T multiplicativeIdentity, int limit = 8)
where T : IEquatable <T>
{
Numeric <T> num = x;
Numeric <T> sum = Numeric <T> .Zero();
for (int i = 0; i < limit; i++)
{
float coef = Convert.ToSingle(Math.Pow(-1, i) / Factorial(2 * i, 1));
Numeric <T> xi = Power((T)num, multiplicativeIdentity, 2 * i);
sum = sum + xi * coef;
}
return(sum);
}
static public T Sin <T>(T x, T multiplicativeIdentity, int limit = 8)
where T : IEquatable <T>
{
Numeric <T> num = x;
Numeric <T> sum = Numeric <T> .Zero();
for (int i = 0; i < limit; i++)
{
double coef = Math.Pow(-1, i) / (double)Factorial(2 * i + 1, 1);
Numeric <T> xi = Power((T)num, multiplicativeIdentity, 2 * i + 1);
sum = sum + xi * coef;
}
return(sum);
}
/// <summary>
/// Matrix multiplication
/// </summary>
/// <param name="v"></param>
/// <returns></returns>
public static Matrix33 <T> operator *(Matrix33 <T> v2, Matrix33 <T> v)
{
Matrix33 <T> tmp = new Matrix33 <T>(Numeric <T> .Zero());
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
for (int k = 0; k < 3; k++)
{
tmp.x[i][j] += v2.x[i][k] * v.x[k][j];
}
}
}
return(tmp);
}
public Matrix33 <T> SetShear(T xy, T unit)
{
x[0][0] = unit;
x[0][1] = Numeric <T> .Zero();
x[0][2] = Numeric <T> .Zero();
x[1][0] = xy;
x[1][1] = unit;
x[1][2] = Numeric <T> .Zero();
x[2][0] = Numeric <T> .Zero();
x[2][1] = Numeric <T> .Zero();
x[2][2] = unit;
return(this);
}
static public T Factorial <T>(T x, T additiveIdentity)
where T : IEquatable <T>
{
if (x == Numeric <T> .Zero())
{
return(additiveIdentity);
}
Numeric <T> num = x;
Numeric <T> pow = additiveIdentity;
while (num != (Numeric <T>)additiveIdentity)
{
pow = pow * num;
num = num - additiveIdentity;
}
return(pow);
}
SetScale(T s, T unit)
{
x[0][0] = s;
x[0][1] = Numeric <T> .Zero();
x[0][2] = Numeric <T> .Zero();
x[1][0] = Numeric <T> .Zero();
x[1][1] = s;
x[1][2] = Numeric <T> .Zero();
x[2][0] = Numeric <T> .Zero();
x[2][1] = Numeric <T> .Zero();
x[2][2] = unit;
return(this);
}
public Matrix44 <T> SetEulerAngles(Vector3 <T> r, T unit)
{
Numeric <T> cosRZ, sinRZ, cosRY, sinRY, cosRX, sinRX;
T rx = r.X;
T ry = r.Y;
T rz = r.Z;
cosRZ = (GenericMath.Cos(rz, unit));
cosRY = (GenericMath.Cos(ry, unit));
cosRX = (GenericMath.Cos(rx, unit));
sinRZ = (GenericMath.Sin(rz, unit));
sinRY = (GenericMath.Sin(ry, unit));
sinRX = (GenericMath.Sin(rx, unit));
x[0][0] = cosRZ * cosRY;
x[0][1] = sinRZ * cosRY;
x[0][2] = -sinRY;
x[0][3] = Numeric <T> .Zero();
x[1][0] = -sinRZ * cosRX + cosRZ * sinRY * sinRX;
x[1][1] = cosRZ * cosRX + sinRZ * sinRY * sinRX;
x[1][2] = cosRY * sinRX;
x[1][3] = Numeric <T> .Zero();
x[2][0] = sinRZ * sinRX + cosRZ * sinRY * cosRX;
x[2][1] = -cosRZ * sinRX + sinRZ * sinRY * cosRX;
x[2][2] = cosRY * cosRX;
x[2][3] = Numeric <T> .Zero();
x[3][0] = Numeric <T> .Zero();
x[3][1] = Numeric <T> .Zero();
x[3][2] = Numeric <T> .Zero();
x[3][3] = unit;
return(this);
}
/// <summary>
/// Set matrix to rotation by angle
/// </summary>
/// <param name="angle">angle of rotation in radians</param>
/// <returns></returns>
public Matrix33 <T> SetRotation(T angle, T unit)
{
Numeric <T> cos_r, sin_r;
cos_r = GenericMath.Cos(angle, unit);
sin_r = GenericMath.Sin(angle, unit);
x[0][0] = cos_r;
x[0][1] = sin_r;
x[0][2] = Numeric <T> .Zero();
x[1][0] = -sin_r;
x[1][1] = cos_r;
x[1][2] = Numeric <T> .Zero();
x[2][0] = Numeric <T> .Zero();
x[2][1] = Numeric <T> .Zero();
x[2][2] = unit;
return(this);
}
public Vec3 <T> ClosestPointTo(Line3 <T> line, Func <double, T> fromDouble = null, Func <T, double> magnitude = null)
{
if (fromDouble == null)
{
fromDouble = Numeric <T> .FromDouble;
}
if (magnitude == null)
{
magnitude = Numeric <T> .ToDouble;
}
// Assumes the lines are normalized
Vec3 <T> posLpos = pos - line.pos;
Numeric <T> c = dir ^ posLpos;
Numeric <T> a = line.dir ^ dir;
Numeric <T> f = line.dir ^ posLpos;
Numeric <T> num = c - a * f;
Numeric <T> denom = a * a - fromDouble(1.0);
Numeric <T> absDenom = ((denom >= Numeric <T> .Zero()) ? denom : -denom);
if (absDenom < fromDouble(1.0))
{
Numeric <T> absNum = ((num >= Numeric <T> .Zero()) ? num : -num);
if (magnitude(absNum) >= magnitude(absDenom) * double.MaxValue)
{
return(pos);
}
}
return(pos + dir * (num / denom));
}
