/// <summary>Creates a matrix that rotates around an arbitrary vector.</summary>
/// <param name="axis">The axis to rotate around.</param>
/// <param name="angle">The angle to rotate around the given axis, in radians.</param>
/// <returns>The rotation matrix.</returns>
public static Matrix3X3 <T> CreateFromAxisAngle <T>(Vector3D <T> axis, T angle)
where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T>
{
// a: angle
// x, y, z: unit vector for axis.
//
// Rotation matrix M can compute by using below equation.
//
// T T
// M = uu + (cos a)( I-uu ) + (sin a)S
//
// Where:
//
// u = ( x, y, z )
//
// [ 0 -z y ]
// S = [ z 0 -x ]
// [ -y x 0 ]
//
// [ 1 0 0 ]
// I = [ 0 1 0 ]
// [ 0 0 1 ]
//
//
// [ xx+cosa*(1-xx) yx-cosa*yx-sina*z zx-cosa*xz+sina*y ]
// M = [ xy-cosa*yx+sina*z yy+cosa(1-yy) yz-cosa*yz-sina*x ]
// [ zx-cosa*zx-sina*y zy-cosa*zy+sina*x zz+cosa*(1-zz) ]
//
T x = axis.X, y = axis.Y, z = axis.Z;
T sa = Scalar.Sin(angle), ca = Scalar.Cos(angle);
T xx = Scalar.Multiply(x, x), yy = Scalar.Multiply(y, y), zz = Scalar.Multiply(z, z);
T xy = Scalar.Multiply(x, y), xz = Scalar.Multiply(x, z), yz = Scalar.Multiply(y, z);
Matrix3X3 <T> result = Matrix3X3 <T> .Identity;
result.M11 = Scalar.Add(xx, Scalar.Multiply(ca, Scalar.Subtract(Scalar <T> .One, xx)));
result.M12 = Scalar.Add(Scalar.Subtract(xy, Scalar.Multiply(ca, xy)), Scalar.Multiply(sa, z));
result.M13 = Scalar.Subtract(Scalar.Subtract(xz, Scalar.Multiply(ca, xz)), Scalar.Multiply(sa, y));
result.M21 = Scalar.Subtract(Scalar.Subtract(xy, Scalar.Multiply(ca, xy)), Scalar.Multiply(sa, z));
result.M22 = Scalar.Add(yy, Scalar.Multiply(ca, Scalar.Subtract(Scalar <T> .One, yy)));
result.M23 = Scalar.Add(Scalar.Subtract(yz, Scalar.Multiply(ca, yz)), Scalar.Multiply(sa, x));
result.M31 = Scalar.Add(Scalar.Subtract(xz, Scalar.Multiply(ca, xz)), Scalar.Multiply(sa, y));
result.M32 = Scalar.Subtract(Scalar.Subtract(yz, Scalar.Multiply(ca, yz)), Scalar.Multiply(sa, x));
result.M33 = Scalar.Add(zz, Scalar.Multiply(ca, Scalar.Subtract(Scalar <T> .One, zz)));
return(result);
}
/// <summary>Creates a matrix for rotating points around the Z-axis.</summary>
/// <param name="radians">The amount, in radians, by which to rotate around the Z-axis.</param>
/// <returns>The rotation matrix.</returns>
public static Matrix3X3 <T> CreateRotationZ <T>(T radians)
where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T>
{
Matrix3X3 <T> result = Matrix3X3 <T> .Identity;
T c = Scalar.Cos(radians);
T s = Scalar.Sin(radians);
// [ c s 0 0 ]
// [ -s c 0 0 ]
// [ 0 0 1 0 ]
// [ 0 0 0 1 ]
result.M11 = c;
result.M12 = s;
result.M21 = Scalar.Negate(s);
result.M22 = c;
return(result);
}
/// <summary>Creates a matrix for rotating points around the Y-axis.</summary>
/// <param name="radians">The amount, in radians, by which to rotate around the Y-axis.</param>
/// <returns>The rotation matrix.</returns>
public static Matrix3X3 <T> CreateRotationY <T>(T radians)
where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T>
{
Matrix3X3 <T> result = Matrix3X3 <T> .Identity;
T c = Scalar.Cos(radians);
T s = Scalar.Sin(radians);
// [ c 0 -s 0 ]
// [ 0 1 0 0 ]
// [ s 0 c 0 ]
// [ 0 0 0 1 ]
result.M11 = c;
result.M13 = Scalar.Negate(s);
result.M31 = s;
result.M33 = c;
return(result);
}
/// <summary>Creates a rotation matrix using the given rotation in radians.</summary>
/// <param name="radians">The amount of rotation, in radians.</param>
/// <returns>A rotation matrix.</returns>
public static Matrix3X2 <T> CreateRotation <T>(T radians)
where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T>
{
radians = Scalar.IEEERemainder(radians, Scalar <T> .Tau);
T c, s;
if (Scalar.GreaterThan(radians, Scalar.As <float, T>(-RotationEpsilon)) && !Scalar.GreaterThanOrEqual(radians, Scalar.As <float, T>(RotationEpsilon)))
{
// Exact case for zero rotation.
c = Scalar <T> .One;
s = Scalar <T> .Zero;
}
else if (Scalar.GreaterThan(radians, Scalar.As <float, T>(
#if MATHF
MathF.PI
#else
((float)Math.PI)
#endif
/ 2 - RotationEpsilon)) && !Scalar.GreaterThanOrEqual(radians, Scalar.As <float, T>(
#if MATHF
MathF.PI
#else
((float)Math.PI)
#endif
/ 2 + RotationEpsilon)))
{
// Exact case for 90 degree rotation.
c = Scalar <T> .Zero;
s = Scalar <T> .One;
}
else if (!Scalar.GreaterThanOrEqual(radians, Scalar.As <float, T>(-
#if MATHF
MathF.PI
#else
((float)Math.PI)
#endif
+ RotationEpsilon)) || Scalar.GreaterThan(radians, Scalar.As <float, T>(
#if MATHF
MathF.PI
#else
((float)Math.PI)
#endif
- RotationEpsilon)))
{
// Exact case for 180 degree rotation.
c = Scalar <T> .MinusOne;
s = Scalar <T> .Zero;
}
else if (Scalar.GreaterThan(radians, Scalar.As <float, T>(-
#if MATHF
MathF.PI
#else
((float)Math.PI)
#endif
/ 2 - RotationEpsilon)) && !Scalar.GreaterThanOrEqual(radians, Scalar.As <float, T>(-
#if MATHF
MathF.PI
#else
((float)Math.PI)
#endif
/ 2 + RotationEpsilon)))
{
// Exact case for 270 degree rotation.
c = Scalar <T> .Zero;
s = Scalar <T> .MinusOne;
}
else
{
// Arbitrary rotation.
c = Scalar.Cos(radians);
s = Scalar.Sin(radians);
}
// [ c s ]
// [ -s c ]
// [ 0 0 ]
Matrix3X2 <T> result = Matrix3X2 <T> .Identity;
result.M11 = c;
result.M12 = s;
result.M21 = Scalar.Negate(s);
result.M22 = c;
return(result);
}
/// <summary>Creates a rotation matrix using the given rotation in radians and a center point.</summary>
/// <param name="radians">The amount of rotation, in radians.</param>
/// <param name="centerPoint">The center point.</param>
/// <returns>A rotation matrix.</returns>
public static Matrix3X2 <T> CreateRotation <T>(T radians, Vector2D <T> centerPoint)
where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T>
{
radians = Scalar.IEEERemainder(radians, Scalar <T> .Tau);
T c, s;
if (Scalar.GreaterThan(radians, Scalar.As <float, T>(-RotationEpsilon)) && !Scalar.GreaterThanOrEqual(radians, Scalar.As <float, T>(RotationEpsilon)))
{
// Exact case for zero rotation.
c = Scalar <T> .One;
s = Scalar <T> .Zero;
}
else if (Scalar.GreaterThan(radians, Scalar.As <float, T>(
#if MATHF
MathF.PI
#else
((float)Math.PI)
#endif
/ 2 - RotationEpsilon)) && !Scalar.GreaterThanOrEqual(radians, Scalar.As <float, T>(
#if MATHF
MathF.PI
#else
((float)Math.PI)
#endif
/ 2 + RotationEpsilon)))
{
// Exact case for 90 degree rotation.
c = Scalar <T> .Zero;
s = Scalar <T> .One;
}
else if (!Scalar.GreaterThanOrEqual(radians, Scalar.As <float, T>(-
#if MATHF
MathF.PI
#else
((float)Math.PI)
#endif
+ RotationEpsilon)) || Scalar.GreaterThan(radians, Scalar.As <float, T>(
#if MATHF
MathF.PI
#else
((float)Math.PI)
#endif
- RotationEpsilon)))
{
// Exact case for 180 degree rotation.
c = Scalar <T> .MinusOne;
s = Scalar <T> .Zero;
}
else if (Scalar.GreaterThan(radians, Scalar.As <float, T>(-
#if MATHF
MathF.PI
#else
((float)Math.PI)
#endif
/ 2 - RotationEpsilon)) && !Scalar.GreaterThanOrEqual(radians, Scalar.As <float, T>(-
#if MATHF
MathF.PI
#else
((float)Math.PI)
#endif
/ 2 + RotationEpsilon)))
{
// Exact case for 270 degree rotation.
c = Scalar <T> .Zero;
s = Scalar <T> .MinusOne;
}
else
{
// Arbitrary rotation.
c = Scalar.Cos(radians);
s = Scalar.Sin(radians);
}
T x = Scalar.Add(Scalar.Multiply(centerPoint.X, Scalar.Subtract(Scalar <T> .One, c)), Scalar.Multiply(centerPoint.Y, s));
T y = Scalar.Subtract(Scalar.Multiply(centerPoint.Y, Scalar.Subtract(Scalar <T> .One, c)), Scalar.Multiply(centerPoint.X, s));
// [ c s ]
// [ -s c ]
// [ x y ]
return(new(
new(c, s),
new(Scalar.Negate(s), c),
new(x, y)));
}
