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 rotation by angle
/// </summary>
/// <param name="angle">angle of rotation in radians</param>
/// <returns></returns>
public Matrix44 <T> Rotate(Vector3 <T> angle, T unit)
{
Numeric <T> cosRZ, sinRZ, cosRY, sinRY, cosRX, sinRX;
Numeric <T> m00, m01, m02;
Numeric <T> m10, m11, m12;
Numeric <T> m20, m21, m22;
T rx = angle.X;
T ry = angle.Y;
T rz = angle.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));
m00 = cosRZ * cosRY;
m01 = sinRZ * cosRY;
m02 = -sinRY;
m10 = -sinRZ * cosRX + cosRZ * sinRY * sinRX;
m11 = cosRZ * cosRX + sinRZ * sinRY * sinRX;
m12 = cosRY * sinRX;
m20 = -sinRZ * -sinRX + cosRZ * sinRY * cosRX;
m21 = cosRZ * -sinRX + sinRZ * sinRY * cosRX;
m22 = cosRY * cosRX;
Matrix44 <T> P = new Matrix44 <T>(this);
x[0][0] = P[0][0] * m00 + P[1][0] * m01 + P[2][0] * m02;
x[0][1] = P[0][1] * m00 + P[1][1] * m01 + P[2][1] * m02;
x[0][2] = P[0][2] * m00 + P[1][2] * m01 + P[2][2] * m02;
x[0][3] = P[0][3] * m00 + P[1][3] * m01 + P[2][3] * m02;
x[1][0] = P[0][0] * m10 + P[1][0] * m11 + P[2][0] * m12;
x[1][1] = P[0][1] * m10 + P[1][1] * m11 + P[2][1] * m12;
x[1][2] = P[0][2] * m10 + P[1][2] * m11 + P[2][2] * m12;
x[1][3] = P[0][3] * m10 + P[1][3] * m11 + P[2][3] * m12;
x[2][0] = P[0][0] * m20 + P[1][0] * m21 + P[2][0] * m22;
x[2][1] = P[0][1] * m20 + P[1][1] * m21 + P[2][1] * m22;
x[2][2] = P[0][2] * m20 + P[1][2] * m21 + P[2][2] * m22;
x[2][3] = P[0][3] * m20 + P[1][3] * m21 + P[2][3] * m22;
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 T DistanceTo(Vector3 <T> other, T unit)
{
T dx = this.x - other.x;
T dy = this.y - other.y;
T dz = this.z - other.z;
Numeric <T> vx = GenericMath.Abs(dx);
vx = GenericMath.Power <T>(vx, unit, 2);
Numeric <T> vy = GenericMath.Abs(dy);
vy = GenericMath.Power <T>(vy, unit, 2);
Numeric <T> vz = GenericMath.Abs(dz);
vz = GenericMath.Power <T>(vz, unit, 2);
return(GenericMath.Sqrt <T>(vx + vy + vz, unit));
//(Math.Sqrt(Math.Pow(Math.Abs(x1 - x2), 2) + Math.Pow(Math.Abs(y1 - y2), 2) + Math.Pow(Math.Abs(z1 - z2), 2)));
}
/// <summary>
/// Length of the vector
/// </summary>
/// <returns></returns>
public T Length(T unit)
{
return(GenericMath.Sqrt <T>(x * x + y * y + z * z, unit));
}