/// <summary>
/// Creates a rotation matrix from axis/angle rotation.
/// </summary>
/// <param name="axis">Axis around which the rotation is performed.</param>
/// <param name="angle">Amount of rotation.</param>
/// <returns>Rotation matrix that can rotate an object around the specified axis for the specified amount.</returns>
public static Matrix3 FromAxisAngle(Vector3 axis, Degree angle)
{
Matrix3 mat;
float cos = MathEx.Cos(angle);
float sin = MathEx.Sin(angle);
float oneMinusCos = 1.0f - cos;
float x2 = axis.x * axis.x;
float y2 = axis.y * axis.y;
float z2 = axis.z * axis.z;
float xym = axis.x * axis.y * oneMinusCos;
float xzm = axis.x * axis.z * oneMinusCos;
float yzm = axis.y * axis.z * oneMinusCos;
float xSin = axis.x * sin;
float ySin = axis.y * sin;
float zSin = axis.z * sin;
mat.m00 = x2 * oneMinusCos + cos;
mat.m01 = xym - zSin;
mat.m02 = xzm + ySin;
mat.m10 = xym + zSin;
mat.m11 = y2 * oneMinusCos + cos;
mat.m12 = yzm - xSin;
mat.m20 = xzm - ySin;
mat.m21 = yzm + xSin;
mat.m22 = z2 * oneMinusCos + cos;
return(mat);
}
/// <summary>
/// Creates a rotation matrix from the provided euler angle (pitch/yaw/roll) rotation. Angles are applied in YXZ
/// order.
/// </summary>
/// <param name="xAngle">Pitch angle of rotation.</param>
/// <param name="yAngle">Yar angle of rotation.</param>
/// <param name="zAngle">Roll angle of rotation.</param>
/// <returns>Rotation matrix that can rotate an object to the specified angles.</returns>
public static Matrix3 FromEuler(Radian xAngle, Radian yAngle, Radian zAngle)
{
Matrix3 m = new Matrix3();
float cx = MathEx.Cos(xAngle);
float sx = MathEx.Sin(xAngle);
float cy = MathEx.Cos(yAngle);
float sy = MathEx.Sin(yAngle);
float cz = MathEx.Cos(zAngle);
float sz = MathEx.Sin(zAngle);
m[0, 0] = cy * cz + sx * sy * sz;
m[0, 1] = cz * sx * sy - cy * sz;
m[0, 2] = cx * sy;
m[1, 0] = cx * sz;
m[1, 1] = cx * cz;
m[1, 2] = -sx;
m[2, 0] = -cz * sy + cy * sx * sz;
m[2, 1] = cy * cz * sx + sy * sz;
m[2, 2] = cx * cy;
return(m);
}
/// <summary>
/// Creates a rotation matrix from the provided euler angle (pitch/yaw/roll) rotation.
/// </summary>
/// <param name="xAngle">Pitch angle of rotation.</param>
/// <param name="yAngle">Yar angle of rotation.</param>
/// <param name="zAngle">Roll angle of rotation.</param>
/// <param name="order">The order in which rotations will be applied. Different rotations can be created depending
/// on the order.</param>
/// <returns>Rotation matrix that can rotate an object to the specified angles.</returns>
public static Matrix3 FromEuler(Radian xAngle, Radian yAngle, Radian zAngle, EulerAngleOrder order)
{
EulerAngleOrderData l = EA_LOOKUP[(int)order];
Matrix3[] mats = new Matrix3[3];
float cx = MathEx.Cos(xAngle);
float sx = MathEx.Sin(xAngle);
mats[0] = new Matrix3(
1.0f, 0.0f, 0.0f,
0.0f, cx, -sx,
0.0f, sx, cx);
float cy = MathEx.Cos(yAngle);
float sy = MathEx.Sin(yAngle);
mats[1] = new Matrix3(
cy, 0.0f, sy,
0.0f, 1.0f, 0.0f,
-sy, 0.0f, cy);
float cz = MathEx.Cos(zAngle);
float sz = MathEx.Sin(zAngle);
mats[2] = new Matrix3(
cz, -sz, 0.0f,
sz, cz, 0.0f,
0.0f, 0.0f, 1.0f);
return(mats[l.a] * (mats[l.b] * mats[l.c]));
}
/// <summary>
/// Creates a quaternion from the provided euler angle (pitch/yaw/roll) rotation.
/// </summary>
/// <param name="xAngle">Pitch angle of rotation.</param>
/// <param name="yAngle">Yar angle of rotation.</param>
/// <param name="zAngle">Roll angle of rotation.</param>
/// <param name="order">The order in which rotations will be applied. Different rotations can be created depending
/// on the order.</param>
/// <returns>Quaternion that can rotate an object to the specified angles.</returns>
public static Quaternion FromEuler(Degree xAngle, Degree yAngle, Degree zAngle,
EulerAngleOrder order = EulerAngleOrder.YXZ)
{
EulerAngleOrderData l = EA_LOOKUP[(int)order];
Radian halfXAngle = xAngle * 0.5f;
Radian halfYAngle = yAngle * 0.5f;
Radian halfZAngle = zAngle * 0.5f;
float cx = MathEx.Cos(halfXAngle);
float sx = MathEx.Sin(halfXAngle);
float cy = MathEx.Cos(halfYAngle);
float sy = MathEx.Sin(halfYAngle);
float cz = MathEx.Cos(halfZAngle);
float sz = MathEx.Sin(halfZAngle);
Quaternion[] quats = new Quaternion[3];
quats[0] = new Quaternion(sx, 0.0f, 0.0f, cx);
quats[1] = new Quaternion(0.0f, sy, 0.0f, cy);
quats[2] = new Quaternion(0.0f, 0.0f, sz, cz);
return((quats[l.a] * quats[l.b]) * quats[l.c]);
}
/// <summary>
/// Performs spherical interpolation between two quaternions. Spherical interpolation neatly interpolates between
/// two rotations without modifying the size of the vector it is applied to (unlike linear interpolation).
/// </summary>
/// <param name="from">Start quaternion.</param>
/// <param name="to">End quaternion.</param>
/// <param name="t">Interpolation factor in range [0, 1] that determines how much to interpolate between
/// <paramref name="from"/> and <paramref name="to"/>.</param>
/// <param name="shortestPath">Should the interpolation be performed between the shortest or longest path between
/// the two quaternions.</param>
/// <returns>Interpolated quaternion representing a rotation between <paramref name="from"/> and
/// <paramref name="to"/>.</returns>
public static Quaternion Slerp(Quaternion from, Quaternion to, float t, bool shortestPath = true)
{
float dot = Dot(from, to);
Quaternion quat;
if (dot < 0.0f && shortestPath)
{
dot = -dot;
quat = -to;
}
else
{
quat = to;
}
if (MathEx.Abs(dot) < (1 - epsilon))
{
float sin = MathEx.Sqrt(1 - (dot * dot));
Radian angle = MathEx.Atan2(sin, dot);
float invSin = 1.0f / sin;
float a = MathEx.Sin((1.0f - t) * angle) * invSin;
float b = MathEx.Sin(t * angle) * invSin;
return(a * from + b * quat);
}
else
{
Quaternion ret = (1.0f - t) * from + t * quat;
ret.Normalize();
return(ret);
}
}
/// <summary>
/// Creates a quaternion from axis/angle rotation.
/// </summary>
/// <param name="axis">Axis around which the rotation is performed.</param>
/// <param name="angle">Amount of rotation.</param>
/// <returns>Quaternion that rotates an object around the specified axis for the specified amount.</returns>
public static Quaternion FromAxisAngle(Vector3 axis, Degree angle)
{
Quaternion quat;
float halfAngle = (float)(0.5f * angle.Radians);
float sin = (float)MathEx.Sin(halfAngle);
quat.w = (float)MathEx.Cos(halfAngle);
quat.x = sin * axis.x;
quat.y = sin * axis.y;
quat.z = sin * axis.z;
return(quat);
}