/// <summary>
/// Transforms a postion using a matrix.
/// </summary>
/// <param name="v">Position to transform.</param>
/// <param name="matrix">Transform to apply to the vector.</param>
/// <returns>Transformed vector.</returns>
public static fix3 TransformPoint(fix3 v, fix4x4 matrix)
{
fix4 result;
Transform(v, matrix, out result);
return(new fix3(result.x / result.w, result.y / result.w, result.z / result.w));
}
/// <summary>
/// Transforms a vector using a matrix.
/// </summary>
/// <param name="v">Vector to transform.</param>
/// <param name="matrix">Transform to apply to the vector.</param>
/// <param name="result">Transformed vector.</param>
public static void Transform(fix3 v, fix4x4 matrix, out fix4 result)
{
result.x = v.x * matrix.M11 + v.y * matrix.M12 + v.z * matrix.M13 + matrix.M14;
result.y = v.x * matrix.M21 + v.y * matrix.M22 + v.z * matrix.M23 + matrix.M24;
result.z = v.x * matrix.M31 + v.y * matrix.M32 + v.z * matrix.M33 + matrix.M34;
result.w = v.x * matrix.M41 + v.y * matrix.M42 + v.z * matrix.M43 + matrix.M44;
}
/// <summary>
/// Transforms a vector using a matrix.
/// </summary>
/// <param name="v">Vector to transform.</param>
/// <param name="matrix">Transform to apply to the vector.</param>
/// <returns>Transformed vector.</returns>
public static fix4 Transform(fix3 v, fix4x4 matrix)
{
fix4 toReturn;
Transform(v, matrix, out toReturn);
return(toReturn);
}
/// <summary>
/// Creates a matrix representing the given axis aligned scale.
/// </summary>
/// <param name="scale">Scale to be represented by the matrix.</param>
/// <returns>FixMatrix representing the given scale.</returns>
public static fix4x4 CreateScale(fix3 scale)
{
fix4x4 scaleMatrix;
CreateScale(scale, out scaleMatrix);
return(scaleMatrix);
}
/// <summary>
/// Creates a matrix representing the given axis and angle rotation.
/// </summary>
/// <param name="axis">Axis around which to rotate.</param>
/// <param name="angle">Angle to rotate around the axis.</param>
/// <param name="result">FixMatrix created from the axis and angle.</param>
public static void CreateFromAxisAngle(fix3 axis, fix angle, out fix4x4 result)
{
fix xx = axis.x * axis.x;
fix yy = axis.y * axis.y;
fix zz = axis.z * axis.z;
fix xy = axis.x * axis.y;
fix xz = axis.x * axis.z;
fix yz = axis.y * axis.z;
fix sinAngle = fix.Sin(angle);
fix oneMinusCosAngle = F64.C1 - fix.Cos(angle);
result.M11 = F64.C1 + oneMinusCosAngle * (xx - F64.C1);
result.M12 = -axis.z * sinAngle + oneMinusCosAngle * xy;
result.M13 = axis.y * sinAngle + oneMinusCosAngle * xz;
result.M14 = F64.C0;
result.M21 = axis.z * sinAngle + oneMinusCosAngle * xy;
result.M22 = F64.C1 + oneMinusCosAngle * (yy - F64.C1);
result.M23 = -axis.x * sinAngle + oneMinusCosAngle * yz;
result.M24 = F64.C0;
result.M31 = -axis.y * sinAngle + oneMinusCosAngle * xz;
result.M32 = axis.x * sinAngle + oneMinusCosAngle * yz;
result.M33 = F64.C1 + oneMinusCosAngle * (zz - F64.C1);
result.M34 = F64.C0;
result.M41 = F64.C0;
result.M42 = F64.C0;
result.M43 = F64.C0;
result.M44 = F64.C1;
}
/// <summary>
/// Creates a world matrix pointing from a position to a target with the given up vector.
/// </summary>
/// <param name="position">Position of the transform.</param>
/// <param name="forward">Forward direction of the transformation.</param>
/// <param name="upVector">Up vector which is crossed against the forward vector to compute the transform's basis.</param>
/// <returns>World matrix.</returns>
public static fix4x4 CreateWorldRH(fix3 position, fix3 forward, fix3 upVector)
{
fix4x4 lookat;
CreateWorldRH(position, forward, upVector, out lookat);
return(lookat);
}
/// <summary>
/// Creates a matrix representing a translation.
/// </summary>
/// <param name="translation">Translation to be represented by the matrix.</param>
/// <returns>FixMatrix representing the given translation.</returns>
public static fix4x4 CreateTranslation(fix3 translation)
{
fix4x4 translationMatrix;
CreateTranslation(translation, out translationMatrix);
return(translationMatrix);
}
/// <summary>
/// Creates a world matrix pointing from a position to a target with the given up vector.
/// </summary>
/// <param name="position">Position of the transform.</param>
/// <param name="forward">Forward direction of the transformation.</param>
/// <param name="upVector">Up vector which is crossed against the forward vector to compute the transform's basis.</param>
/// <param name="worldMatrix">World matrix.</param>
public static void CreateWorldRH(fix3 position, fix3 forward, fix3 upVector, out fix4x4 worldMatrix)
{
fix3 z;
fix length = forward.length;
fix3.Divide(forward, -length, out z);
fix3 x;
fix3.Cross(upVector, z, out x);
x.Normalize();
fix3 y;
fix3.Cross(z, x, out y);
worldMatrix.M11 = x.x;
worldMatrix.M21 = x.y;
worldMatrix.M31 = x.z;
worldMatrix.M41 = F64.C0;
worldMatrix.M12 = y.x;
worldMatrix.M22 = y.y;
worldMatrix.M32 = y.z;
worldMatrix.M42 = F64.C0;
worldMatrix.M13 = z.x;
worldMatrix.M23 = z.y;
worldMatrix.M33 = z.z;
worldMatrix.M43 = F64.C0;
worldMatrix.M14 = position.x;
worldMatrix.M24 = position.y;
worldMatrix.M34 = position.z;
worldMatrix.M44 = F64.C1;
}
/// <summary>
/// Creates a transform matrix with the given positon, rotation and scale
/// </summary>
public static fix4x4 CreateTRS(fix3 position, fixQuaternion rotation, fix3 scale)
{
fix4x4 result;
CreateTRS(position, rotation, scale, out result);
return(result);
}
/// <summary>
/// Creates a view matrix pointing in a direction with a given up vector.
/// </summary>
/// <param name="position">Position of the camera.</param>
/// <param name="forward">Forward direction of the camera.</param>
/// <param name="upVector">Up vector of the camera.</param>
/// <param name="viewMatrix">Look at matrix.</param>
public static void CreateViewRH(fix3 position, fix3 forward, fix3 upVector, out fix4x4 viewMatrix)
{
fix3 z;
fix length = forward.length;
fix3.Divide(forward, -length, out z);
fix3 x;
fix3.Cross(upVector, z, out x);
x.Normalize();
fix3 y;
fix3.Cross(z, x, out y);
viewMatrix.M11 = x.x;
viewMatrix.M21 = y.x;
viewMatrix.M31 = z.x;
viewMatrix.M41 = F64.C0;
viewMatrix.M12 = x.y;
viewMatrix.M22 = y.y;
viewMatrix.M32 = z.y;
viewMatrix.M42 = F64.C0;
viewMatrix.M13 = x.z;
viewMatrix.M23 = y.z;
viewMatrix.M33 = z.z;
viewMatrix.M43 = F64.C0;
fix3.Dot(x, position, out viewMatrix.M14);
fix3.Dot(y, position, out viewMatrix.M24);
fix3.Dot(z, position, out viewMatrix.M34);
viewMatrix.M14 = -viewMatrix.M14;
viewMatrix.M24 = -viewMatrix.M24;
viewMatrix.M34 = -viewMatrix.M34;
viewMatrix.M44 = F64.C1;
}
/// <summary>
/// Transforms a vector using the transpose of a matrix.
/// </summary>
/// <param name="v">Vector to transform.</param>
/// <param name="matrix">Transform to tranpose and apply to the vector.</param>
/// <param name="result">Transformed vector.</param>
public static void TransformTranspose(fix3 v, fix4x4 matrix, out fix4 result)
{
result.x = v.x * matrix.M11 + v.y * matrix.M21 + v.z * matrix.M31 + matrix.M41;
result.y = v.x * matrix.M12 + v.y * matrix.M22 + v.z * matrix.M32 + matrix.M42;
result.z = v.x * matrix.M13 + v.y * matrix.M23 + v.z * matrix.M33 + matrix.M43;
result.w = v.x * matrix.M14 + v.y * matrix.M24 + v.z * matrix.M34 + matrix.M44;
}
/// <summary>
/// Transforms a vector using the transpose of a matrix.
/// </summary>
/// <param name="v">Vector to transform.</param>
/// <param name="matrix">Transform to tranpose and apply to the vector.</param>
/// <returns>Transformed vector.</returns>
public static fix3 TransformNormalTranspose(fix3 v, fix4x4 matrix)
{
fix3 toReturn;
TransformNormalTranspose(v, matrix, out toReturn);
return(toReturn);
}
/// <summary>
/// Computes the axis angle representation of a normalized quaternion.
/// </summary>
/// <param name="q">FixQuaternion to be converted.</param>
/// <param name="axis">Axis represented by the quaternion.</param>
/// <param name="angle">Angle around the axis represented by the quaternion.</param>
public static void GetAxisAngleFromQuaternion(fixQuaternion q, out fix3 axis, out fix angle)
{
#if !WINDOWS
axis = new fix3();
#endif
fix qw = q.w;
if (qw > F64.C0)
{
axis.x = q.x;
axis.y = q.y;
axis.z = q.z;
}
else
{
axis.x = -q.x;
axis.y = -q.y;
axis.z = -q.z;
qw = -qw;
}
fix lengthSquared = axis.lengthSquared;
if (lengthSquared > fix.Epsilon)
{
fix3.Divide(axis, fix.Sqrt(lengthSquared), out axis);
angle = F64.C2 * fix.Acos(fixMath.Clamp(qw, -1, F64.C1));
}
else
{
axis = fix3.up;
angle = F64.C0;
}
}
/// <summary>
/// Creates a matrix representing a rotation of a given angle around a given axis.
/// </summary>
/// <param name="axis">Axis around which to rotate.</param>
/// <param name="angle">Amount to rotate.</param>
/// <returns>FixMatrix representing the rotation.</returns>
public static fix3x3 CreateFromAxisAngle(fix3 axis, fix angle)
{
fix3x3 toReturn;
CreateFromAxisAngle(axis, angle, out toReturn);
return(toReturn);
}
/// <summary>
/// Constructs a new 3d vector.
/// </summary>
/// <param name="x">X component of the vector.</param>
/// <param name="yzw">Y, Z, and W components of the vector.</param>
public fix4(fix x, fix3 yzw)
{
this.x = x;
this.y = yzw.x;
this.z = yzw.y;
this.w = yzw.z;
}
/// <summary>
/// Transforms the vector using a quaternion.
/// </summary>
/// <param name="v">Vector to transform.</param>
/// <param name="rotation">Rotation to apply to the vector.</param>
/// <param name="result">Transformed vector.</param>
public static void Transform(fix3 v, fixQuaternion rotation, out fix3 result)
{
//This operation is an optimized-down version of v' = q * v * q^-1.
//The expanded form would be to treat v as an 'axis only' quaternion
//and perform standard quaternion multiplication. Assuming q is normalized,
//q^-1 can be replaced by a conjugation.
fix x2 = rotation.x + rotation.x;
fix y2 = rotation.y + rotation.y;
fix z2 = rotation.z + rotation.z;
fix xx2 = rotation.x * x2;
fix xy2 = rotation.x * y2;
fix xz2 = rotation.x * z2;
fix yy2 = rotation.y * y2;
fix yz2 = rotation.y * z2;
fix zz2 = rotation.z * z2;
fix wx2 = rotation.w * x2;
fix wy2 = rotation.w * y2;
fix wz2 = rotation.w * z2;
//Defer the component setting since they're used in computation.
fix transformedX = v.x * (F64.C1 - yy2 - zz2) + v.y * (xy2 - wz2) + v.z * (xz2 + wy2);
fix transformedY = v.x * (xy2 + wz2) + v.y * (F64.C1 - xx2 - zz2) + v.z * (yz2 - wx2);
fix transformedZ = v.x * (xz2 - wy2) + v.y * (yz2 + wx2) + v.z * (F64.C1 - xx2 - yy2);
result.x = transformedX;
result.y = transformedY;
result.z = transformedZ;
}
/// <summary>
/// Transforms the vector using a quaternion.
/// </summary>
/// <param name="v">Vector to transform.</param>
/// <param name="rotation">Rotation to apply to the vector.</param>
/// <returns>Transformed vector.</returns>
public static fix3 Transform(fix3 v, fixQuaternion rotation)
{
fix3 toReturn;
Transform(v, rotation, out toReturn);
return(toReturn);
}
/// <summary>
/// Constructs a quaternion from yaw, pitch, and roll.
/// </summary>
/// <param name="yaw">Yaw of the rotation.</param>
/// <param name="pitch">Pitch of the rotation.</param>
/// <param name="roll">Roll of the rotation.</param>
/// <returns>FixQuaternion representing the yaw, pitch, and roll.</returns>
public static fixQuaternion FromEuler(fix3 euler)
{
fixQuaternion toReturn;
FromEuler(euler.x, euler.y, euler.z, out toReturn);
return(toReturn);
}
/// <summary>
/// Constructs a new 3d vector.
/// </summary>
/// <param name="xyz">X, Y, and Z components of the vector.</param>
/// <param name="w">W component of the vector.</param>
public fix4(fix3 xyz, fix w)
{
this.x = xyz.x;
this.y = xyz.y;
this.z = xyz.z;
this.w = w;
}
/// <summary>
/// Creates a view matrix pointing from a position to a target with the given up vector.
/// </summary>
/// <param name="position">Position of the camera.</param>
/// <param name="target">Target of the camera.</param>
/// <param name="upVector">Up vector of the camera.</param>
/// <param name="viewMatrix">Look at matrix.</param>
public static void CreateLookAtRH(fix3 position, fix3 target, fix3 upVector, out fix4x4 viewMatrix)
{
fix3 forward;
fix3.Subtract(target, position, out forward);
CreateViewRH(position, forward, upVector, out viewMatrix);
}
/// <summary>
/// Transforms the vector by the matrix.
/// </summary>
/// <param name="v">FixVector2 to transform. Considered to be a column vector for purposes of multiplication.</param>
/// <param name="matrix">FixMatrix to use as the transformation.</param>
/// <param name="result">Column vector product of the transformation.</param>
public static void Transform(ref fix3 v, ref fix2x3 matrix, out fix2 result)
{
#if !WINDOWS
result = new fix2();
#endif
result.x = matrix.M11 * v.x + matrix.M12 * v.y + matrix.M13 * v.z;
result.y = matrix.M21 * v.x + matrix.M22 * v.y + matrix.M23 * v.z;
}
/// <summary>
/// Transforms the vector by the matrix.
/// </summary>
/// <param name="v">FixVector2 to transform. Considered to be a row vector for purposes of multiplication.</param>
/// <param name="matrix">FixMatrix to use as the transformation.</param>
/// <param name="result">Row vector product of the transformation.</param>
public static void Transform(ref fix3 v, ref fix3x2 matrix, out fix2 result)
{
#if !WINDOWS
result = new fix2();
#endif
result.x = v.x * matrix.M11 + v.y * matrix.M21 + v.z * matrix.M31;
result.y = v.x * matrix.M12 + v.y * matrix.M22 + v.z * matrix.M32;
}
/// <summary>
/// Constructs a non-uniform scaling matrix.
/// </summary>
/// <param name="scale">Values defining the axis scales.</param>
/// <returns>Scaling matrix.</returns>
public static fix3x3 CreateScale(fix3 scale)
{
var matrix = new fix3x3 {
M11 = scale.x, M22 = scale.y, M33 = scale.z
};
return(matrix);
}
/// <summary>
/// Creates a view matrix pointing from a position to a target with the given up vector.
/// </summary>
/// <param name="position">Position of the camera.</param>
/// <param name="target">Target of the camera.</param>
/// <param name="upVector">Up vector of the camera.</param>
/// <returns>Look at matrix.</returns>
public static fix4x4 CreateLookAtRH(fix3 position, fix3 target, fix3 upVector)
{
fix4x4 lookAt;
fix3 forward;
fix3.Subtract(target, position, out forward);
CreateViewRH(position, forward, upVector, out lookAt);
return(lookAt);
}
/// <summary>
/// Transforms the vector by the matrix.
/// </summary>
/// <param name="v">FixVector2 to transform. Considered to be a column vector for purposes of multiplication.</param>
/// <param name="matrix">FixMatrix to use as the transformation.</param>
/// <param name="result">Column vector product of the transformation.</param>
public static void Transform(ref fix2 v, ref fix3x2 matrix, out fix3 result)
{
#if !WINDOWS
result = new fix3();
#endif
result.x = matrix.M11 * v.x + matrix.M12 * v.y;
result.y = matrix.M21 * v.x + matrix.M22 * v.y;
result.z = matrix.M31 * v.x + matrix.M32 * v.y;
}
/// <summary>
/// Transforms the vector by the matrix.
/// </summary>
/// <param name="v">FixVector2 to transform. Considered to be a row vector for purposes of multiplication.</param>
/// <param name="matrix">FixMatrix to use as the transformation.</param>
/// <param name="result">Row vector product of the transformation.</param>
public static void Transform(ref fix2 v, ref fix2x3 matrix, out fix3 result)
{
#if !WINDOWS
result = new fix3();
#endif
result.x = v.x * matrix.M11 + v.y * matrix.M21;
result.y = v.x * matrix.M12 + v.y * matrix.M22;
result.z = v.x * matrix.M13 + v.y * matrix.M23;
}
/// <summary>
/// Transforms a vector using a matrix.
/// </summary>
/// <param name="v">Vector to transform.</param>
/// <param name="matrix">Transform to apply to the vector.</param>
/// <param name="result">Transformed vector.</param>
public static void Transform(fix3 v, fix4x4 matrix, out fix3 result)
{
fix vX = v.x;
fix vY = v.y;
fix vZ = v.z;
result.x = vX * matrix.M11 + vY * matrix.M12 + vZ * matrix.M13 + matrix.M14;
result.y = vX * matrix.M21 + vY * matrix.M22 + vZ * matrix.M23 + matrix.M24;
result.z = vX * matrix.M31 + vY * matrix.M32 + vZ * matrix.M33 + matrix.M34;
}
/// <summary>
/// Creates a quaternion from an axis and angle.
/// </summary>
/// <param name="axis">Axis of rotation.</param>
/// <param name="angle">Angle to rotate around the axis.</param>
/// <param name="q">FixQuaternion representing the axis and angle rotation.</param>
public static void CreateFromAxisAngle(fix3 axis, fix angle, out fixQuaternion q)
{
fix halfAngle = angle * F64.C0p5;
fix s = fix.Sin(halfAngle);
q.x = axis.x * s;
q.y = axis.y * s;
q.z = axis.z * s;
q.w = fix.Cos(halfAngle);
}
public static fix3 clampLength(fix3 v, fix min, fix max)
{
fix l = length(v);
if (l <= global::fix.Epsilon)
{
return(new fix3(min, 0, 0));
}
return(clamp(l, min, max) * (v / l));
}
/// <summary>
/// Transforms a vector using the transpose of a matrix.
/// </summary>
/// <param name="v">Vector to transform.</param>
/// <param name="matrix">Transform to tranpose and apply to the vector.</param>
/// <param name="result">Transformed vector.</param>
public static void TransformNormalTranspose(fix3 v, fix4x4 matrix, out fix3 result)
{
fix vX = v.x;
fix vY = v.y;
fix vZ = v.z;
result.x = vX * matrix.M11 + vY * matrix.M21 + vZ * matrix.M31;
result.y = vX * matrix.M12 + vY * matrix.M22 + vZ * matrix.M32;
result.z = vX * matrix.M13 + vY * matrix.M23 + vZ * matrix.M33;
}
