diff --git a/Source/OpenTK/Math/Functions.cs b/Source/OpenTK/Math/Functions.cs index 259d0eb8..68eb42a7 100644 --- a/Source/OpenTK/Math/Functions.cs +++ b/Source/OpenTK/Math/Functions.cs @@ -1,5 +1,6 @@ #region --- License --- /* Copyright (c) 2006, 2007 Stefanos Apostolopoulos + * Contributions by Andy Gill. * See license.txt for license info */ #endregion @@ -38,6 +39,28 @@ namespace OpenTK.Math return x; } } + + /// + /// Convert degrees to radians + /// + /// An angle in degrees + /// The angle expressed in radians + public static float DegreesToRadians(float degrees) + { + const float degToRad = (float)System.Math.PI / 180.0f; + return degrees * degToRad; + } + + /// + /// Convert radians to degrees + /// + /// An angle in radians + /// The angle expressed in degrees + public static float RadiansToDegrees(float radians) + { + const float radToDeg = 180.0f / (float)System.Math.PI; + return radians * radToDeg; + } } #if false diff --git a/Source/OpenTK/Math/Matrix4.cs b/Source/OpenTK/Math/Matrix4.cs new file mode 100644 index 00000000..9c1cfdc0 --- /dev/null +++ b/Source/OpenTK/Math/Matrix4.cs @@ -0,0 +1,523 @@ +#region --- License --- +/* Copyright (c) 2006, 2007 the OpenTK team + * Implemented by Andy Gill + * See license.txt for license info + */ +#endregion + +using System; +using System.Collections.Generic; +using System.Text; +using System.Runtime.InteropServices; + +namespace OpenTK.Math +{ + /// + /// Represents a 4x4 Matrix + /// + [StructLayout(LayoutKind.Sequential)] + public struct Matrix4 + { + #region Fields + + /// + /// Top row of the matrix + /// + public Vector4 Row0; + /// + /// 2nd row of the matrix + /// + public Vector4 Row1; + /// + /// 3rd row of the matrix + /// + public Vector4 Row2; + /// + /// Bottom row of the matrix + /// + public Vector4 Row3; + + /// + /// The identity matrix + /// + public static Matrix4 Identity = new Matrix4(Vector4.UnitX, Vector4.UnitY, Vector4.UnitZ, Vector4.UnitW); + + #endregion + + #region Constructors + + /// + /// Construct a new matrix from 4 vectors representing each row + /// + /// Top row of the matrix + /// 2nd row of the matrix + /// 3rd row of the matrix + /// Bottom row of the matrix + public Matrix4(Vector4 row0, Vector4 row1, Vector4 row2, Vector4 row3) + { + Row0 = row0; + Row1 = row1; + Row2 = row2; + Row3 = row3; + } + + #endregion + + #region Functions + + #region public void Invert() + + public void Invert() + { + this = Matrix4.Invert(this); + } + + #endregion + + #region public void Transpose() + + public void Transpose() + { + this = Matrix4.Transpose(this); + } + + #endregion + + #endregion + + #region Properties + + /// + /// The determinant of this matrix + /// + public float Determinant + { + get + { + return Row0.X * Row1.Y * Row2.Z * Row3.W - Row0.X * Row1.Y * Row2.W * Row3.Z + Row0.X * Row1.Z * Row2.W * Row3.Y - Row0.X * Row1.Z * Row2.Y * Row3.W + + Row0.X * Row1.W * Row2.Y * Row3.Z - Row0.X * Row1.W * Row2.Z * Row3.Y - Row0.Y * Row1.Z * Row2.W * Row3.X + Row0.Y * Row1.Z * Row2.X * Row3.W + - Row0.Y * Row1.W * Row2.X * Row3.Z + Row0.Y * Row1.W * Row2.Z * Row3.X - Row0.Y * Row1.X * Row2.Z * Row3.W + Row0.Y * Row1.X * Row2.W * Row3.Z + + Row0.Z * Row1.W * Row2.X * Row3.Y - Row0.Z * Row1.W * Row2.Y * Row3.X + Row0.Z * Row1.X * Row2.Y * Row3.W - Row0.Z * Row1.X * Row2.W * Row3.Y + + Row0.Z * Row1.Y * Row2.W * Row3.X - Row0.Z * Row1.Y * Row2.X * Row3.W - Row0.W * Row1.X * Row2.Y * Row3.Z + Row0.W * Row1.X * Row2.Z * Row3.Y + - Row0.W * Row1.Y * Row2.Z * Row3.X + Row0.W * Row1.Y * Row2.X * Row3.Z - Row0.W * Row1.Z * Row2.X * Row3.Y + Row0.W * Row1.Z * Row2.Y * Row3.X; + } + } + + /// + /// The first column of this matrix + /// + public Vector4 Column0 + { + get { return new Vector4(Row0.X, Row1.X, Row2.X, Row3.X); } + } + + /// + /// The second column of this matrix + /// + public Vector4 Column1 + { + get { return new Vector4(Row0.Y, Row1.Y, Row2.Y, Row3.Y); } + } + + /// + /// The third column of this matrix + /// + public Vector4 Column2 + { + get { return new Vector4(Row0.Z, Row1.Z, Row2.Z, Row3.Z); } + } + + /// + /// The fourth column of this matrix + /// + public Vector4 Column3 + { + get { return new Vector4(Row0.W, Row1.W, Row2.W, Row3.W); } + } + + #endregion + + #region Operator overloads + + /// + /// Matrix multiplication + /// + /// left-hand operand + /// right-hand operand + /// A new Matrix44 which holds the result of the multiplication + public static Matrix4 operator *(Matrix4 left, Matrix4 right) + { + return Matrix4.Mult(left, right); + } + + [CLSCompliant(false)] + unsafe public static explicit operator float*(Matrix4 mat) + { + return &mat.Row0.X; + } + + public static explicit operator IntPtr(Matrix4 mat) + { + unsafe + { + return (IntPtr)(&mat.Row0.X); + } + } + + #endregion + + #region Static functions + + #region Scale Functions + + /// + /// Build a scaling matrix + /// + /// Single scale factor for x,y and z axes + /// A scaling matrix + public static Matrix4 Scale(float scale) + { + return Scale(scale, scale, scale); + } + + /// + /// Build a scaling matrix + /// + /// Scale factors for x,y and z axes + /// A scaling matrix + public static Matrix4 Scale(Vector3 scale) + { + return Scale(scale.X, scale.Y, scale.Z); + } + + /// + /// Build a scaling matrix + /// + /// Scale factor for x-axis + /// Scale factor for y-axis + /// Scale factor for z-axis + /// A scaling matrix + public static Matrix4 Scale(float x, float y, float z) + { + Matrix4 result; + result.Row0 = Vector4.UnitX * x; + result.Row1 = Vector4.UnitY * y; + result.Row2 = Vector4.UnitZ * z; + result.Row3 = Vector4.UnitW; + return result; + } + + #endregion + + #region Translation Functions + + /// + /// Build a translation matrix with the given translation + /// + /// The vector to translate along + /// A Translation matrix + public static Matrix4 Translation(Vector3 trans) + { + return Translation(trans.X, trans.Y, trans.Z); + } + + /// + /// Build a translation matrix with the given translation + /// + /// X translation + /// Y translation + /// Z translation + /// A Translation matrix + public static Matrix4 Translation(float x, float y, float z) + { + Matrix4 result = Identity; + result.Row3 = new Vector4(x, y, z, 1.0f); + return result; + } + + #endregion + + #region Rotation Functions + + /// + /// Build a rotation matrix that rotates about the x-axis + /// + /// angle in radians to rotate counter-clockwise around the x-axis + /// A rotation matrix + public static Matrix4 RotateX(float angle) + { + float cos = (float)System.Math.Cos(angle); + float sin = (float)System.Math.Sin(angle); + + Matrix4 result; + result.Row0 = Vector4.UnitX; + result.Row1 = new Vector4(0.0f, cos, sin, 0.0f); + result.Row2 = new Vector4(0.0f, -sin, cos, 0.0f); + result.Row3 = Vector4.UnitW; + return result; + } + + /// + /// Build a rotation matrix that rotates about the y-axis + /// + /// angle in radians to rotate counter-clockwise around the y-axis + /// A rotation matrix + public static Matrix4 RotateY(float angle) + { + float cos = (float)System.Math.Cos(angle); + float sin = (float)System.Math.Sin(angle); + + Matrix4 result; + result.Row0 = new Vector4(cos, 0.0f, -sin, 0.0f); + result.Row1 = Vector4.UnitY; + result.Row2 = new Vector4(sin, 0.0f, cos, 0.0f); + result.Row3 = Vector4.UnitW; + return result; + } + + /// + /// Build a rotation matrix that rotates about the z-axis + /// + /// angle in radians to rotate counter-clockwise around the z-axis + /// A rotation matrix + public static Matrix4 RotateZ(float angle) + { + float cos = (float)System.Math.Cos(angle); + float sin = (float)System.Math.Sin(angle); + + Matrix4 result; + result.Row0 = new Vector4(cos, sin, 0.0f, 0.0f); + result.Row1 = new Vector4(-sin, cos, 0.0f, 0.0f); + result.Row2 = Vector4.UnitZ; + result.Row3 = Vector4.UnitW; + return result; + } + + /// + /// Build a rotation matrix to rotate about the given axis + /// + /// the axis to rotate about + /// angle in radians to rotate counter-clockwise (looking in the direction of the given axis) + /// A rotation matrix + public static Matrix4 Rotate(Vector3 axis, float angle) + { + float cos = (float)System.Math.Cos(-angle); + float sin = (float)System.Math.Sin(-angle); + float t = 1.0f - cos; + + axis.Normalize(); + + Matrix4 result; + result.Row0 = new Vector4(t * axis.X * axis.X + cos, t * axis.X * axis.Y - sin * axis.Z, t * axis.X * axis.Z + sin * axis.Y, 0.0f); + result.Row1 = new Vector4(t * axis.X * axis.Y + sin * axis.Z, t * axis.Y * axis.Y + cos, t * axis.Y * axis.Z - sin * axis.X, 0.0f); + result.Row2 = new Vector4(t * axis.X * axis.Z - sin * axis.Y, t * axis.Y * axis.Z + sin * axis.X, t * axis.Z * axis.Z + cos, 0.0f); + result.Row3 = Vector4.UnitW; + return result; + } + + /// + /// Build a rotation matrix from a quaternion + /// + /// the quaternion + /// A rotation matrix + public static Matrix4 Rotate(Quaternion q) + { + Vector3 axis; + float angle; + q.ToAxisAngle(out axis, out angle); + return Rotate(axis, angle); + } + + #endregion + + #region Multiply Functions + + /// + /// Post multiply this matrix by another matrix + /// + /// The matrix to multiply + /// A new Matrix44 that is the result of the multiplication + public static Matrix4 Mult(Matrix4 left, Matrix4 right) + { + Vector4 col0 = right.Column0; + Vector4 col1 = right.Column1; + Vector4 col2 = right.Column2; + Vector4 col3 = right.Column3; + + left.Row0 = new Vector4(Vector4.Dot(left.Row0, col0), Vector4.Dot(left.Row0, col1), Vector4.Dot(left.Row0, col2), Vector4.Dot(left.Row0, col3)); + left.Row1 = new Vector4(Vector4.Dot(left.Row1, col0), Vector4.Dot(left.Row1, col1), Vector4.Dot(left.Row1, col2), Vector4.Dot(left.Row1, col3)); + left.Row2 = new Vector4(Vector4.Dot(left.Row2, col0), Vector4.Dot(left.Row2, col1), Vector4.Dot(left.Row2, col2), Vector4.Dot(left.Row2, col3)); + left.Row3 = new Vector4(Vector4.Dot(left.Row3, col0), Vector4.Dot(left.Row3, col1), Vector4.Dot(left.Row3, col2), Vector4.Dot(left.Row3, col3)); + return left; + } + + public static void Mult(ref Matrix4 left, ref Matrix4 right, out Matrix4 result) + { + Vector4 col0 = right.Column0; + Vector4 col1 = right.Column1; + Vector4 col2 = right.Column2; + Vector4 col3 = right.Column3; + + result.Row0 = new Vector4(Vector4.Dot(left.Row0, col0), Vector4.Dot(left.Row0, col1), Vector4.Dot(left.Row0, col2), Vector4.Dot(left.Row0, col3)); + result.Row1 = new Vector4(Vector4.Dot(left.Row1, col0), Vector4.Dot(left.Row1, col1), Vector4.Dot(left.Row1, col2), Vector4.Dot(left.Row1, col3)); + result.Row2 = new Vector4(Vector4.Dot(left.Row2, col0), Vector4.Dot(left.Row2, col1), Vector4.Dot(left.Row2, col2), Vector4.Dot(left.Row2, col3)); + result.Row3 = new Vector4(Vector4.Dot(left.Row3, col0), Vector4.Dot(left.Row3, col1), Vector4.Dot(left.Row3, col2), Vector4.Dot(left.Row3, col3)); + } + + #endregion + + #region Invert Functions + + /// + /// Calculate the inverse of the given matrix + /// + /// The matrix to invert + /// The inverse of the given matrix if it has one, or the input if it is singular + public static Matrix4 Invert(Matrix4 mat) + { + int[] colIdx = { 0, 0, 0, 0 }; + int[] rowIdx = { 0, 0, 0, 0 }; + int[] pivotIdx = { -1, -1, -1, -1 }; + + // convert the matrix to an array for easy looping + float[,] inverse = {{mat.Row0.X, mat.Row0.Y, mat.Row0.Z, mat.Row0.W}, + {mat.Row1.X, mat.Row1.Y, mat.Row1.Z, mat.Row1.W}, + {mat.Row2.X, mat.Row2.Y, mat.Row2.Z, mat.Row2.W}, + {mat.Row3.X, mat.Row3.Y, mat.Row3.Z, mat.Row3.W} }; + int icol = 0; + int irow = 0; + for (int i = 0; i < 4; i++) + { + // Find the largest pivot value + float maxPivot = 0.0f; + for (int j = 0; j < 4; j++) + { + if (pivotIdx[j] != 0) + { + for (int k = 0; k < 4; ++k) + { + if (pivotIdx[k] == -1) + { + float absVal = System.Math.Abs(inverse[j, k]); + if (absVal > maxPivot) + { + maxPivot = absVal; + irow = j; + icol = k; + } + } + else if (pivotIdx[k] > 0) + { + return mat; + } + } + } + } + + ++(pivotIdx[icol]); + + // Swap rows over so pivot is on diagonal + if (irow != icol) + { + for (int k = 0; k < 4; ++k) + { + float f = inverse[irow, k]; + inverse[irow, k] = inverse[icol, k]; + inverse[icol, k] = f; + } + } + + rowIdx[i] = irow; + colIdx[i] = icol; + + float pivot = inverse[icol, icol]; + // check for singular matrix + if (pivot == 0.0f) + { + return mat; + } + + // Scale row so it has a unit diagonal + float oneOverPivot = 1.0f / pivot; + inverse[icol, icol] = 1.0f; + for (int k = 0; k < 4; ++k) + inverse[icol, k] *= oneOverPivot; + + // Do elimination of non-diagonal elements + for (int j = 0; j < 4; ++j) + { + // check this isn't on the diagonal + if (icol != j) + { + float f = inverse[j, icol]; + inverse[j, icol] = 0.0f; + for (int k = 0; k < 4; ++k) + inverse[j, k] -= inverse[icol, k] * f; + } + } + } + + for (int j = 3; j >= 0; --j) + { + int ir = rowIdx[j]; + int ic = colIdx[j]; + for (int k = 0; k < 4; ++k) + { + float f = inverse[k, ir]; + inverse[k, ir] = inverse[k, ic]; + inverse[k, ic] = f; + } + } + + mat.Row0 = new Vector4(inverse[0, 0], inverse[0, 1], inverse[0, 2], inverse[0, 3]); + mat.Row1 = new Vector4(inverse[1, 0], inverse[1, 1], inverse[1, 2], inverse[1, 3]); + mat.Row2 = new Vector4(inverse[2, 0], inverse[2, 1], inverse[2, 2], inverse[2, 3]); + mat.Row3 = new Vector4(inverse[3, 0], inverse[3, 1], inverse[3, 2], inverse[3, 3]); + return mat; + } + + #endregion + + #region Transpose + + /// + /// Calculate the transpose of the given matrix + /// + /// The matrix to transpose + /// The transpose of the given matrix + public static Matrix4 Transpose(Matrix4 mat) + { + return new Matrix4(mat.Column0, mat.Column1, mat.Column2, mat.Column3); + } + + + /// + /// Calculate the transpose of the given matrix + /// + /// The matrix to transpose + public static void Transpose(ref Matrix4 mat, out Matrix4 result) + { + result.Row0 = mat.Column0; + result.Row1 = mat.Column1; + result.Row2 = mat.Column2; + result.Row3 = mat.Column3; + } + + #endregion + + #endregion + + #region public override string ToString() + + /// + /// Returns a System.String that represents the current Matrix44. + /// + /// + public override string ToString() + { + return String.Format("{0}\n{1}\n{2}\n{3}", Row0, Row1, Row2, Row3); + } + + #endregion + } +} diff --git a/Source/OpenTK/Math/Quaternion.cs b/Source/OpenTK/Math/Quaternion.cs new file mode 100644 index 00000000..510d3984 --- /dev/null +++ b/Source/OpenTK/Math/Quaternion.cs @@ -0,0 +1,458 @@ +#region --- License --- +/* Copyright (c) 2006, 2007 the OpenTK team + * Implemented by Andy Gill + * See license.txt for license info + */ +#endregion + +using System; +using System.Collections.Generic; +using System.Text; +using System.Runtime.InteropServices; + +namespace OpenTK.Math +{ + /// + /// Represents a Quaternion + /// + [StructLayout(LayoutKind.Sequential)] + public struct Quaternion + { + #region Fields + + /// + /// The vector part of the quaternion + /// + public Vector3 XYZ; + /// + /// The w component of the quaternion + /// + public float W; + + public static Quaternion Identity = new Quaternion(0, 0, 0, 1); + + #endregion + + #region Constructors + + /// + /// Construct a new Quaternion from vector and w components + /// + /// The vector part + /// The w part + public Quaternion(Vector3 v, float w) + { + XYZ = v; + W = w; + } + + /// + /// Construct a new Quaternion + /// + /// The x component + /// The y component + /// The z component + /// The w component + public Quaternion(float x, float y, float z, float w) + { + XYZ = new Vector3(x, y, z); + W = w; + } + + #endregion + + #region Functions + + #region pubilc void ToAxisAngle(out Vector3 axis, out float angle) + + /// + /// Convert the current quaternion to axis angle representation + /// + /// The resultant axis + /// The resultant angle + public void ToAxisAngle(out Vector3 axis, out float angle) + { + Quaternion q = this; + if (q.W > 1.0f) + q.Normalize(); + + angle = 2.0f * (float)System.Math.Acos(q.W); + float den = (float)System.Math.Sqrt(1.0 - q.W * q.W); + axis = q.XYZ; + if (den > 0.0001f) + { + axis = q.XYZ / den; + } + } + + #endregion + + #region public float Length + + /// + /// Gets the length (magnitude) of the quaternion. + /// + /// + public float Length + { + get + { + return (float)System.Math.Sqrt(W * W + XYZ.LengthSquared); + } + } + + #endregion + + #region public float LengthSquared + + /// + /// Gets the square of the quaternion length (magnitude). + /// + public float LengthSquared + { + get + { + return W * W + XYZ.LengthSquared; + } + } + + #endregion + + #region public void Normalize() + + /// + /// Scales the Quaternion to unit length. + /// + public void Normalize() + { + float scale = 1.0f / this.Length; + XYZ *= scale; + W *= scale; + } + + #endregion + + #region public void Conjugate() + + /// + /// Convert this quaternion to its conjugate + /// + public void Conjugate() + { + XYZ = -XYZ; + } + + #endregion + + #endregion + + #region Operator overloads + + public static Quaternion operator +(Quaternion left, Quaternion right) + { + left.XYZ += right.XYZ; + left.W += right.W; + return left; + } + + public static Quaternion operator -(Quaternion left, Quaternion right) + { + left.XYZ -= right.XYZ; + left.W -= right.W; + return left; + } + + public static Quaternion operator *(Quaternion left, Quaternion right) + { + float w = left.W * right.W - Vector3.Dot(left.XYZ, right.XYZ); + left.XYZ = right.W * left.XYZ + left.W * right.XYZ + Vector3.Cross(left.XYZ, right.XYZ); + left.W = w; + return left; + } + + [CLSCompliant(false)] + unsafe public static explicit operator float*(Quaternion q) + { + return &q.XYZ.X; + } + + public static explicit operator IntPtr(Quaternion q) + { + unsafe + { + return (IntPtr)(&q.XYZ.X); + } + } + + #endregion + + #region Static functions + + #region Add + + /// + /// Add two quaternions + /// + /// The first operand + /// The second operand + /// The result of the addition + public static Quaternion Add(Quaternion left, Quaternion right) + { + left.XYZ += right.XYZ; + left.W += right.W; + return left; + } + + /// + /// Add two quaternions + /// + /// The first operand + /// The second operand + /// The result of the addition + public static void Add(ref Quaternion left, ref Quaternion right, out Quaternion result) + { + result.XYZ = left.XYZ + right.XYZ; + result.W = left.W + right.W; + } + + #endregion + + #region Sub + + public static Quaternion Sub(Quaternion left, Quaternion right) + { + left.XYZ -= right.XYZ; + left.W -= right.W; + return left; + } + + public static void Sub(ref Quaternion left, ref Quaternion right, out Quaternion result) + { + result.XYZ = left.XYZ - right.XYZ; + result.W = left.W - right.W; + } + + #endregion + + #region Mult + + public static Quaternion Mult(Quaternion left, Quaternion right) + { + float w = left.W * right.W - Vector3.Dot(left.XYZ, right.XYZ); + left.XYZ = right.W * left.XYZ + left.W * right.XYZ + Vector3.Cross(left.XYZ, right.XYZ); + left.W = w; + return left; + } + + public static void Mult(ref Quaternion left, ref Quaternion right, out Quaternion result) + { + result.W = left.W * right.W - Vector3.Dot(left.XYZ, right.XYZ); + result.XYZ = right.W * left.XYZ + left.W * right.XYZ + Vector3.Cross(left.XYZ, right.XYZ); + } + + #endregion + + #region Conjugate + + /// + /// Get the conjugate of the given quaternion + /// + /// The quaternion + /// The conjugate of the given quaternion + public static Quaternion Conjugate(Quaternion q) + { + q.XYZ = -q.XYZ; + return q; + } + + /// + /// Get the conjugate of the given quaternion + /// + /// The quaternion + /// The conjugate of the given quaternion + public static void Conjugate(ref Quaternion q, out Quaternion result) + { + result.XYZ = -q.XYZ; + result.W = q.W; + } + + #endregion + + #region Invert + + /// + /// Get the inverse of the given quaternion + /// + /// The quaternion to invert + /// The inverse of the given quaternion + public static Quaternion Invert(Quaternion q) + { + float lengthSq = q.LengthSquared; + if (lengthSq != 0.0) + { + float i = 1.0f / lengthSq; + q.XYZ *= -i; + q.W *= i; + } + return q; + } + + /// + /// Get the inverse of the given quaternion + /// + /// The quaternion to invert + /// The inverse of the given quaternion + public static void Invert(ref Quaternion q, out Quaternion result) + { + float lengthSq = q.LengthSquared; + if (lengthSq != 0.0) + { + float i = 1.0f / lengthSq; + result.XYZ = q.XYZ * -i; + result.W = q.W * i; + } + else + { + result = q; + } + } + + #endregion + + #region Normalize + + /// + /// Scale the given quaternion to unit length + /// + /// The quaternion to normalize + /// The normalized quaternion + public static Quaternion Normalize(Quaternion q) + { + float scale = 1.0f / q.Length; + q.XYZ *= scale; + q.W *= scale; + return q; + } + + /// + /// Scale the given quaternion to unit length + /// + /// The quaternion to normalize + /// The normalized quaternion + public static void Normalize(ref Quaternion q, out Quaternion result) + { + float scale = 1.0f / q.Length; + result.XYZ = q.XYZ * scale; + result.W = q.W * scale; + } + + #endregion + + #region FromAxisAngle + + /// + /// Build a quaternion from the given axis and angle + /// + /// The axis to rotate about + /// The rotation angle in radians + /// + public static Quaternion FromAxisAngle(Vector3 axis, float angle) + { + if (axis.LengthSquared == 0.0f) + return Identity; + + Quaternion result = Identity; + + angle *= 0.5f; + axis.Normalize(); + result.XYZ = axis * (float)System.Math.Sin(angle); + result.W = (float)System.Math.Cos(angle); + + return Normalize(result); + } + + #endregion + + #region Slerp + + /// + /// Do Spherical linear interpolation between two quaternions + /// + /// The first quaternion + /// The second quaternion + /// The blend factor + /// A smooth blend between the given quaternions + public static Quaternion Slerp(Quaternion q1, Quaternion q2, float blend) + { + // if either input is zero, return the other. + if (q1.LengthSquared == 0.0f) + { + if (q2.LengthSquared == 0.0f) + { + return Identity; + } + return q2; + } + else if (q2.LengthSquared == 0.0f) + { + return q1; + } + + + float cosHalfAngle = q1.W * q2.W + Vector3.Dot(q1.XYZ, q2.XYZ); + + if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f) + { + // angle = 0.0f, so just return one input. + return q1; + } + else if (cosHalfAngle < 0.0f) + { + q2.XYZ = -q2.XYZ; + q2.W = -q2.W; + cosHalfAngle = -cosHalfAngle; + } + + float blendA; + float blendB; + if (cosHalfAngle < 0.99f) + { + // do proper slerp for big angles + float halfAngle = (float)System.Math.Acos(cosHalfAngle); + float sinHalfAngle = (float)System.Math.Sin(halfAngle); + float oneOverSinHalfAngle = 1.0f / sinHalfAngle; + blendA = (float)System.Math.Sin(halfAngle * (1.0f - blend)) * oneOverSinHalfAngle; + blendB = (float)System.Math.Sin(halfAngle * blend) * oneOverSinHalfAngle; + } + else + { + // do lerp if angle is really small. + blendA = 1.0f - blend; + blendB = blend; + } + + Quaternion result = new Quaternion(blendA * q1.XYZ + blendB * q2.XYZ, blendA * q1.W + blendB * q2.W); + if (result.LengthSquared > 0.0f) + return Normalize(result); + else + return Identity; + } + + #endregion + + #endregion + + #region public override string ToString() + + /// + /// Returns a System.String that represents the current Quaternion. + /// + /// + public override string ToString() + { + return String.Format("V: {0}, W: {1}", XYZ, W); + } + + #endregion + } +} diff --git a/Source/OpenTK/Math/Vector2.cs b/Source/OpenTK/Math/Vector2.cs index d7944c5d..7f0596f7 100644 --- a/Source/OpenTK/Math/Vector2.cs +++ b/Source/OpenTK/Math/Vector2.cs @@ -1,5 +1,6 @@ #region --- License --- /* Copyright (c) 2006, 2007 Stefanos Apostolopoulos + * Contributions by Andy Gill. * See license.txt for license info */ #endregion @@ -32,6 +33,10 @@ namespace OpenTK.Math /// public float Y; + public static Vector2 UnitX = new Vector2(1, 0); + public static Vector2 UnitY = new Vector2(0, 1); + public static Vector2 Zero = new Vector2(0, 0); + #endregion #region Constructors @@ -43,8 +48,8 @@ namespace OpenTK.Math /// The y coordinate of the net Vector2. public Vector2(float x, float y) { - X = x; - Y = y; + X = x; + Y = y; } /// @@ -53,8 +58,8 @@ namespace OpenTK.Math /// The Vector2 to copy components from. public Vector2(Vector2 v) { - X = v.X; - Y = v.Y; + X = v.X; + Y = v.Y; } /// @@ -63,8 +68,8 @@ namespace OpenTK.Math /// The Vector3 to copy components from. Z is discarded. public Vector2(Vector3 v) { - X = v.X; - Y = v.Y; + X = v.X; + Y = v.Y; } /// @@ -73,60 +78,14 @@ namespace OpenTK.Math /// The Vector4 to copy components from. Z and W are discarded. public Vector2(Vector4 v) { - X = v.X; - Y = v.Y; + X = v.X; + Y = v.Y; } #endregion #region Functions - #region public Vector2 Add(Vector2 right) - - /// - /// Adds the given Vector2 to the current Vector2. - /// - /// The right operand of the addition. - /// The current Vector2, modified by the operation. - public Vector2 Add(Vector2 right) - { - this.X = X + right.X; - this.Y = Y + right.Y; - return this; - } - - #endregion - - #region public Vector2 Sub(Vector2 right) - - /// - /// Subtracts the given Vector2 from the current Vector2. - /// - /// The right operand of the subtraction. - /// The current Vector2, modified by the operation. - public Vector2 Sub(Vector2 right) - { - this.X = X - right.X; - this.Y = Y - right.Y; - return this; - } - - #endregion - - #region public float Dot(Vector2 right) - - /// - /// Computes the dot product between the current Vector2 and the given Vector2. - /// - /// The right operand of the dot product. - /// A float containing the result of the operation. - public float Dot(Vector2 right) - { - return X * right.X + Y * right.Y; - } - - #endregion - #region public float Length /// @@ -136,10 +95,10 @@ namespace OpenTK.Math /// public float Length { - get - { - return (float)System.Math.Sqrt(X * X + Y * Y); - } + get + { + return (float)System.Math.Sqrt(X * X + Y * Y); + } } #endregion @@ -158,10 +117,10 @@ namespace OpenTK.Math /// public float LengthFast { - get - { - return 1.0f / OpenTK.Math.Functions.InverseSqrtFast(X * X + Y * Y); - } + get + { + return 1.0f / OpenTK.Math.Functions.InverseSqrtFast(X * X + Y * Y); + } } #endregion @@ -179,59 +138,53 @@ namespace OpenTK.Math /// public float LengthSquared { - get - { - return X * X + Y * Y; - } + get + { + return X * X + Y * Y; + } } #endregion - #region public Vector2 Normalize() + #region public void Normalize() /// /// Scales the Vector2 to unit length. /// - /// The normalized version of the current vector. - public Vector2 Normalize() + public void Normalize() { - float scale = 1.0f / this.Length; - X *= scale; - Y *= scale; - return this; + float scale = 1.0f / this.Length; + X *= scale; + Y *= scale; } #endregion - #region public Vector2 NormalizeFast() + #region public void NormalizeFast() /// /// Scales the Vector2 to approximately unit length. /// - /// The normalized version of the current vector. - public Vector2 NormalizeFast() + public void NormalizeFast() { - float scale = Functions.InverseSqrtFast(X * X + Y * Y); - X *= scale; - Y *= scale; - return this; + float scale = Functions.InverseSqrtFast(X * X + Y * Y); + X *= scale; + Y *= scale; } #endregion - #region public Vector2 Scale(float sx, float sy) + #region public void Scale(float sx, float sy) /// /// Scales the current Vector2 by the given amounts. /// /// The scale of the X component. /// The scale of the Y component. - /// The current Vector2, scaled. - public Vector2 Scale(float sx, float sy) + public void Scale(float sx, float sy) { - this.X = X * sx; - this.Y = Y * sy; - return this; + this.X = X * sx; + this.Y = Y * sy; } #endregion @@ -240,35 +193,388 @@ namespace OpenTK.Math #region Operator overloads - public static Vector2 operator +(Vector2 left, Vector2 right) - { - return new Vector2(left).Add(right); - } + public static Vector2 operator +(Vector2 left, Vector2 right) + { + left.X += right.X; + left.Y += right.Y; + return left; + } - public static Vector2 operator -(Vector2 left, Vector2 right) - { - return new Vector2(left).Sub(right); - } + public static Vector2 operator -(Vector2 left, Vector2 right) + { + left.X -= right.X; + left.Y -= right.Y; + return left; + } - [CLSCompliant(false)] - unsafe public static explicit operator float*(Vector2 v) - { - return &v.X; - } + public static Vector2 operator -(Vector2 vec) + { + vec.X = -vec.X; + vec.Y = -vec.Y; + return vec; + } - public static explicit operator IntPtr(Vector2 v) - { - unsafe - { - return (IntPtr)(&v.X); - } - } + public static Vector2 operator *(Vector2 vec, float f) + { + vec.X *= f; + vec.Y *= f; + return vec; + } + + public static Vector2 operator *(float f, Vector2 vec) + { + vec.X *= f; + vec.Y *= f; + return vec; + } + + public static Vector2 operator /(Vector2 vec, float f) + { + float mult = 1.0f / f; + vec.X *= mult; + vec.Y *= mult; + return vec; + } + + [CLSCompliant(false)] + unsafe public static explicit operator float*(Vector2 v) + { + return &v.X; + } + + public static explicit operator IntPtr(Vector2 v) + { + unsafe + { + return (IntPtr)(&v.X); + } + } #endregion - #region public override string ToString() + #region Static functions - /// + #region Add + + /// + /// Add two Vectors + /// + /// First operand + /// Second operand + /// Result of addition + public static Vector2 Add(Vector2 a, Vector2 b) + { + a.X += b.X; + a.Y += b.Y; + return a; + } + + /// + /// Add two Vectors + /// + /// First operand + /// Second operand + /// Result of addition + public static void Add(ref Vector2 a, ref Vector2 b, out Vector2 result) + { + result.X = a.X + b.X; + result.Y = a.Y + b.Y; + } + + #endregion + + #region Sub + + /// + /// Subtract one Vector from another + /// + /// First operand + /// Second operand + /// Result of subtraction + public static Vector2 Sub(Vector2 a, Vector2 b) + { + a.X -= b.X; + a.Y -= b.Y; + return a; + } + + /// + /// Subtract one Vector from another + /// + /// First operand + /// Second operand + /// Result of subtraction + public static void Sub(ref Vector2 a, ref Vector2 b, out Vector2 result) + { + result.X = a.X - b.X; + result.Y = a.Y - b.Y; + } + + #endregion + + #region Mult + + /// + /// Multiply a vector and a scalar + /// + /// Vector operand + /// Scalar operand + /// Result of the multiplication + public static Vector2 Mult(Vector2 a, float f) + { + a.X *= f; + a.Y *= f; + return a; + } + + /// + /// Multiply a vector and a scalar + /// + /// Vector operand + /// Scalar operand + /// Result of the multiplication + public static void Mult(ref Vector2 a, float f, out Vector2 result) + { + result.X = a.X * f; + result.Y = a.Y * f; + } + + #endregion + + #region Div + + /// + /// Divide a vector by a scalar + /// + /// Vector operand + /// Scalar operand + /// Result of the division + public static Vector2 Div(Vector2 a, float f) + { + float mult = 1.0f / f; + a.X *= mult; + a.Y *= mult; + return a; + } + + /// + /// Divide a vector by a scalar + /// + /// Vector operand + /// Scalar operand + /// Result of the division + public static void Div(ref Vector2 a, float f, out Vector2 result) + { + float mult = 1.0f / f; + result.X = a.X * mult; + result.Y = a.Y * mult; + } + + #endregion + + #region Min + + /// + /// Calculate the component-wise minimum of two vectors + /// + /// First operand + /// Second operand + /// The component-wise minimum + public static Vector2 Min(Vector2 a, Vector2 b) + { + a.X = a.X < b.X ? a.X : b.X; + a.Y = a.Y < b.Y ? a.Y : b.Y; + return a; + } + + /// + /// Calculate the component-wise minimum of two vectors + /// + /// First operand + /// Second operand + /// The component-wise minimum + public static void Min(ref Vector2 a, ref Vector2 b, out Vector2 result) + { + result.X = a.X < b.X ? a.X : b.X; + result.Y = a.Y < b.Y ? a.Y : b.Y; + } + + #endregion + + #region Max + + /// + /// Calculate the component-wise maximum of two vectors + /// + /// First operand + /// Second operand + /// The component-wise maximum + public static Vector2 Max(Vector2 a, Vector2 b) + { + a.X = a.X > b.X ? a.X : b.X; + a.Y = a.Y > b.Y ? a.Y : b.Y; + return a; + } + + /// + /// Calculate the component-wise maximum of two vectors + /// + /// First operand + /// Second operand + /// The component-wise maximum + public static void Max(ref Vector2 a, ref Vector2 b, out Vector2 result) + { + result.X = a.X > b.X ? a.X : b.X; + result.Y = a.Y > b.Y ? a.Y : b.Y; + } + + #endregion + + #region Clamp + + /// + /// Clamp a vector to the given minimum and maximum vectors + /// + /// Input vector + /// Minimum vector + /// Maximum vector + /// The clamped vector + public static Vector2 Clamp(Vector2 vec, Vector2 min, Vector2 max) + { + vec.X = vec.X < min.X ? min.X : vec.X > max.X ? max.X : vec.X; + vec.Y = vec.Y < min.Y ? min.Y : vec.Y > max.Y ? max.Y : vec.Y; + return vec; + } + + /// + /// Clamp a vector to the given minimum and maximum vectors + /// + /// Input vector + /// Minimum vector + /// Maximum vector + /// The clamped vector + public static void Clamp(ref Vector2 vec, ref Vector2 min, ref Vector2 max, out Vector2 result) + { + result.X = vec.X < min.X ? min.X : vec.X > max.X ? max.X : vec.X; + result.Y = vec.Y < min.Y ? min.Y : vec.Y > max.Y ? max.Y : vec.Y; + } + + #endregion + + #region Normalize + + /// + /// Scale a vector to unit length + /// + /// The input vector + /// The normalized vector + public static Vector2 Normalize(Vector2 vec) + { + float scale = 1.0f / vec.Length; + vec.X *= scale; + vec.Y *= scale; + return vec; + } + + /// + /// Scale a vector to unit length + /// + /// The input vector + /// The normalized vector + public static void Normalize(ref Vector2 vec, out Vector2 result) + { + float scale = 1.0f / vec.Length; + result.X = vec.X * scale; + result.Y = vec.Y * scale; + } + + #endregion + + #region NormalizeFast + + /// + /// Scale a vector to approximately unit length + /// + /// The input vector + /// The normalized vector + public static Vector2 NormalizeFast(Vector2 vec) + { + float scale = Functions.InverseSqrtFast(vec.X * vec.X + vec.Y * vec.Y); + vec.X *= scale; + vec.Y *= scale; + return vec; + } + + /// + /// Scale a vector to approximately unit length + /// + /// The input vector + /// The normalized vector + public static void NormalizeFast(ref Vector2 vec, out Vector2 result) + { + float scale = Functions.InverseSqrtFast(vec.X * vec.X + vec.Y * vec.Y); + result.X = vec.X * scale; + result.Y = vec.Y * scale; + } + + #endregion + + #region Dot + + /// + /// Caclulate the dot (scalar) product of two vectors + /// + /// First operand + /// Second operand + /// The dot product of the two inputs + public static float Dot(Vector2 left, Vector2 right) + { + return left.X * right.X + left.Y * right.Y; + } + + #endregion + + #region Lerp + + /// + /// Returns a new Vector that is the linear blend of the 2 given Vectors + /// + /// First input vector + /// Second input vector + /// The blend factor + /// a when blend=0, b when blend=1, and a linear combination otherwise + public static Vector2 Lerp(Vector2 a, Vector2 b, float blend) + { + a.X = blend * (b.X - a.X) + a.X; + a.Y = blend * (b.Y - a.Y) + a.Y; + return a; + } + + #endregion + + #region Barycentric + + /// + /// Interpolate 3 Vectors using Barycentric coordinates + /// + /// First input Vector + /// Second input Vector + /// Third input Vector + /// First Barycentric Coordinate + /// Second Barycentric Coordinate + /// a when u=v=0, b when u=1,v=0, c when u=0,v=1, and a linear combination of a,b,c otherwise + public static Vector2 BaryCentric(Vector2 a, Vector2 b, Vector2 c, float u, float v) + { + return a + u * (b - a) + v * (c - a); + } + + #endregion + + #endregion + + #region public override string ToString() + + /// /// Returns a System.String that represents the current Vector2. /// /// diff --git a/Source/OpenTK/Math/Vector3.cs b/Source/OpenTK/Math/Vector3.cs index ccc891f9..a25feaa9 100644 --- a/Source/OpenTK/Math/Vector3.cs +++ b/Source/OpenTK/Math/Vector3.cs @@ -1,5 +1,6 @@ #region --- License --- /* Copyright (c) 2006, 2007 Stefanos Apostolopoulos + * Contributions by Andy Gill. * See license.txt for license info */ #endregion @@ -11,45 +12,50 @@ using System.Runtime.InteropServices; namespace OpenTK.Math { - /// - /// Represents a three-dimensional vector. - /// - [StructLayout(LayoutKind.Sequential)] - public struct Vector3 - { - #region Fields + /// + /// Represents a three-dimensional vector. + /// + [StructLayout(LayoutKind.Sequential)] + public struct Vector3 + { + #region Fields - /// - /// The X component of the Vector3. - /// - public float X; + /// + /// The X component of the Vector3. + /// + public float X; - /// - /// The Y component of the Vector3. - /// - public float Y; + /// + /// The Y component of the Vector3. + /// + public float Y; - /// - /// The Z component of the Vector3. - /// - public float Z; + /// + /// The Z component of the Vector3. + /// + public float Z; + + public static Vector3 UnitX = new Vector3(1, 0, 0); + public static Vector3 UnitY = new Vector3(0, 1, 0); + public static Vector3 UnitZ = new Vector3(0, 0, 1); + public static Vector3 Zero = new Vector3(0, 0, 0); #endregion - #region Constructors + #region Constructors - /// - /// Constructs a new Vector3. - /// - /// The x component of the Vector3. - /// The y component of the Vector3. - /// The z component of the Vector3. - public Vector3(float x, float y, float z) - { - X = x; - Y = y; - Z = z; - } + /// + /// Constructs a new Vector3. + /// + /// The x component of the Vector3. + /// The y component of the Vector3. + /// The z component of the Vector3. + public Vector3(float x, float y, float z) + { + X = x; + Y = y; + Z = z; + } /// /// Constructs a new Vector3 from the given Vector2. @@ -88,75 +94,6 @@ namespace OpenTK.Math #region Functions - #region public Vector3 Add(Vector3 right) - - /// - /// Adds the given Vector3 to the current Vector3. - /// - /// The right operand of the addition. - /// The current Vector3, containing the result of the addition. - public Vector3 Add(Vector3 right) - { - X += right.X; - Y += right.Y; - Z += right.Z; - return this; - } - - #endregion - - #region public Vector3 Sub(Vector3 right) - - /// - /// Subtracts the given Vector3 from the current Vector3. - /// - /// The right operand of the subtraction. - /// A new Vector3 containing the result of the subtraction. - public Vector3 Sub(Vector3 right) - { - X -= right.X; - Y -= right.Y; - Z -= right.Z; - return this; - } - - #endregion - - #region public float Dot(Vector3 right) - - /// - /// Computes the dot product between the current Vector3 and the given Vector3. - /// - /// The right operand of the dot product. - /// A float containing the result of the dot product. - public float Dot(Vector3 right) - { - return X * right.X + Y * right.Y + Z * right.Z; - } - - #endregion - - #region public Vector3 Cross(Vector3 right) - - /// - /// Computes the cross product between the current and the given Vector3. The current Vector3 is set to the result of the computation. - /// - /// The right operand of the cross product - /// The current - public Vector3 Cross(Vector3 right) - { - float - x = Y * right.Z - Z * right.Y, - y = Z * right.X - X * right.Z, - z = X * right.Y - Y * right.X; - X = x; - Y = y; - Z = z; - return this; - } - - #endregion - #region public float Length /// @@ -222,14 +159,12 @@ namespace OpenTK.Math /// /// Scales the Vector3 to unit length. /// - /// The normalized version of the current vector. - public Vector3 Normalize() + public void Normalize() { float scale = 1.0f / this.Length; X *= scale; Y *= scale; Z *= scale; - return this; } #endregion @@ -239,14 +174,12 @@ namespace OpenTK.Math /// /// Scales the Vector3 to approximately unit length. /// - /// The normalized version of the current vector. - public Vector3 NormalizeFast() + public void NormalizeFast() { float scale = Functions.InverseSqrtFast(X * X + Y * Y + Z * Z); X *= scale; Y *= scale; Z *= scale; - return this; } #endregion @@ -259,13 +192,11 @@ namespace OpenTK.Math /// The scale of the X component. /// The scale of the Y component. /// The scale of the Z component. - /// The current Vector3, scaled. - public Vector3 Scale(float sx, float sy, float sz) + public void Scale(float sx, float sy, float sz) { this.X = X * sx; this.Y = Y * sy; this.Z = Z * sz; - return this; } #endregion @@ -276,14 +207,53 @@ namespace OpenTK.Math public static Vector3 operator +(Vector3 left, Vector3 right) { - return new Vector3(left.Add(right)); + left.X += right.X; + left.Y += right.Y; + left.Z += right.Z; + return left; } public static Vector3 operator -(Vector3 left, Vector3 right) { - return new Vector3(left.Sub(right)); + left.X -= right.X; + left.Y -= right.Y; + left.Z -= right.Z; + return left; } + public static Vector3 operator -(Vector3 vec) + { + vec.X = -vec.X; + vec.Y = -vec.Y; + vec.Z = -vec.Z; + return vec; + } + + public static Vector3 operator *(Vector3 vec, float f) + { + vec.X *= f; + vec.Y *= f; + vec.Z *= f; + return vec; + } + + public static Vector3 operator *(float f, Vector3 vec) + { + vec.X *= f; + vec.Y *= f; + vec.Z *= f; + return vec; + } + + public static Vector3 operator /(Vector3 vec, float f) + { + float mult = 1.0f / f; + vec.X *= mult; + vec.Y *= mult; + vec.Z *= mult; + return vec; + } + [CLSCompliant(false)] unsafe public static explicit operator float*(Vector3 v) { @@ -302,11 +272,477 @@ namespace OpenTK.Math #region Static functions - #endregion + #region Add - public override string ToString() + /// + /// Add two Vectors + /// + /// First operand + /// Second operand + /// Result of addition + public static Vector3 Add(Vector3 a, Vector3 b) + { + a.X += b.X; + a.Y += b.Y; + a.Z += b.Z; + return a; + } + + /// + /// Add two Vectors + /// + /// First operand + /// Second operand + /// Result of addition + public static void Add(ref Vector3 a, ref Vector3 b, out Vector3 result) + { + result.X = a.X + b.X; + result.Y = a.Y + b.Y; + result.Z = a.Z + b.Z; + } + + #endregion + + #region Sub + + /// + /// Subtract one Vector from another + /// + /// First operand + /// Second operand + /// Result of subtraction + public static Vector3 Sub(Vector3 a, Vector3 b) + { + a.X -= b.X; + a.Y -= b.Y; + a.Z -= b.Z; + return a; + } + + /// + /// Subtract one Vector from another + /// + /// First operand + /// Second operand + /// Result of subtraction + public static void Sub(ref Vector3 a, ref Vector3 b, out Vector3 result) + { + result.X = a.X - b.X; + result.Y = a.Y - b.Y; + result.Z = a.Z - b.Z; + } + + #endregion + + #region Mult + + /// + /// Multiply a vector and a scalar + /// + /// Vector operand + /// Scalar operand + /// Result of the multiplication + public static Vector3 Mult(Vector3 a, float f) + { + a.X *= f; + a.Y *= f; + a.Z *= f; + return a; + } + + /// + /// Multiply a vector and a scalar + /// + /// Vector operand + /// Scalar operand + /// Result of the multiplication + public static void Mult(ref Vector3 a, float f, out Vector3 result) + { + result.X = a.X * f; + result.Y = a.Y * f; + result.Z = a.Z * f; + } + + #endregion + + #region Div + + /// + /// Divide a vector by a scalar + /// + /// Vector operand + /// Scalar operand + /// Result of the division + public static Vector3 Div(Vector3 a, float f) + { + float mult = 1.0f / f; + a.X *= mult; + a.Y *= mult; + a.Z *= mult; + return a; + } + + /// + /// Divide a vector by a scalar + /// + /// Vector operand + /// Scalar operand + /// Result of the division + public static void Div(ref Vector3 a, float f, out Vector3 result) + { + float mult = 1.0f / f; + result.X = a.X * mult; + result.Y = a.Y * mult; + result.Z = a.Z * mult; + } + + #endregion + + #region Min + + /// + /// Calculate the component-wise minimum of two vectors + /// + /// First operand + /// Second operand + /// The component-wise minimum + public static Vector3 Min(Vector3 a, Vector3 b) + { + a.X = a.X < b.X ? a.X : b.X; + a.Y = a.Y < b.Y ? a.Y : b.Y; + a.Z = a.Z < b.Z ? a.Z : b.Z; + return a; + } + + /// + /// Calculate the component-wise minimum of two vectors + /// + /// First operand + /// Second operand + /// The component-wise minimum + public static void Min(ref Vector3 a, ref Vector3 b, out Vector3 result) + { + result.X = a.X < b.X ? a.X : b.X; + result.Y = a.Y < b.Y ? a.Y : b.Y; + result.Z = a.Z < b.Z ? a.Z : b.Z; + } + + #endregion + + #region Max + + /// + /// Calculate the component-wise maximum of two vectors + /// + /// First operand + /// Second operand + /// The component-wise maximum + public static Vector3 Max(Vector3 a, Vector3 b) + { + a.X = a.X > b.X ? a.X : b.X; + a.Y = a.Y > b.Y ? a.Y : b.Y; + a.Z = a.Z > b.Z ? a.Z : b.Z; + return a; + } + + /// + /// Calculate the component-wise maximum of two vectors + /// + /// First operand + /// Second operand + /// The component-wise maximum + public static void Max(ref Vector3 a, ref Vector3 b, out Vector3 result) + { + result.X = a.X > b.X ? a.X : b.X; + result.Y = a.Y > b.Y ? a.Y : b.Y; + result.Z = a.Z > b.Z ? a.Z : b.Z; + } + + #endregion + + #region Clamp + + /// + /// Clamp a vector to the given minimum and maximum vectors + /// + /// Input vector + /// Minimum vector + /// Maximum vector + /// The clamped vector + public static Vector3 Clamp(Vector3 vec, Vector3 min, Vector3 max) + { + vec.X = vec.X < min.X ? min.X : vec.X > max.X ? max.X : vec.X; + vec.Y = vec.Y < min.Y ? min.Y : vec.Y > max.Y ? max.Y : vec.Y; + vec.Z = vec.Z < min.Z ? min.Z : vec.Z > max.Z ? max.Z : vec.Z; + return vec; + } + + /// + /// Clamp a vector to the given minimum and maximum vectors + /// + /// Input vector + /// Minimum vector + /// Maximum vector + /// The clamped vector + public static void Clamp(ref Vector3 vec, ref Vector3 min, ref Vector3 max, out Vector3 result) + { + result.X = vec.X < min.X ? min.X : vec.X > max.X ? max.X : vec.X; + result.Y = vec.Y < min.Y ? min.Y : vec.Y > max.Y ? max.Y : vec.Y; + result.Z = vec.Z < min.Z ? min.Z : vec.Z > max.Z ? max.Z : vec.Z; + } + + #endregion + + #region Normalize + + /// + /// Scale a vector to unit length + /// + /// The input vector + /// The normalized vector + public static Vector3 Normalize(Vector3 vec) + { + float scale = 1.0f / vec.Length; + vec.X *= scale; + vec.Y *= scale; + vec.Z *= scale; + return vec; + } + + /// + /// Scale a vector to unit length + /// + /// The input vector + /// The normalized vector + public static void Normalize(ref Vector3 vec, out Vector3 result) + { + float scale = 1.0f / vec.Length; + result.X = vec.X * scale; + result.Y = vec.Y * scale; + result.Z = vec.Z * scale; + } + + #endregion + + #region NormalizeFast + + /// + /// Scale a vector to approximately unit length + /// + /// The input vector + /// The normalized vector + public static Vector3 NormalizeFast(Vector3 vec) + { + float scale = Functions.InverseSqrtFast(vec.X * vec.X + vec.Y * vec.Y + vec.Z * vec.Z); + vec.X *= scale; + vec.Y *= scale; + vec.Z *= scale; + return vec; + } + + /// + /// Scale a vector to approximately unit length + /// + /// The input vector + /// The normalized vector + public static void NormalizeFast(ref Vector3 vec, out Vector3 result) + { + float scale = Functions.InverseSqrtFast(vec.X * vec.X + vec.Y * vec.Y + vec.Z * vec.Z); + result.X = vec.X * scale; + result.Y = vec.Y * scale; + result.Z = vec.Z * scale; + } + + #endregion + + #region Dot + + /// + /// Caclulate the dot (scalar) product of two vectors + /// + /// First operand + /// Second operand + /// The dot product of the two inputs + public static float Dot(Vector3 left, Vector3 right) + { + return left.X * right.X + left.Y * right.Y + left.Z * right.Z; + } + + #endregion + + #region Cross + + /// + /// Caclulate the cross (vector) product of two vectors + /// + /// First operand + /// Second operand + /// The cross product of the two inputs + public static Vector3 Cross(Vector3 left, Vector3 right) + { + float + x = left.Y * right.Z - left.Z * right.Y, + y = left.Z * right.X - left.X * right.Z, + z = left.X * right.Y - left.Y * right.X; + left.X = x; + left.Y = y; + left.Z = z; + return left; + } + + /// + /// Caclulate the cross (vector) product of two vectors + /// + /// First operand + /// Second operand + /// The cross product of the two inputs + /// The cross product of the two inputs + public static void Cross(ref Vector3 left, ref Vector3 right, out Vector3 result) + { + result.X = left.Y * right.Z - left.Z * right.Y; + result.Y = left.Z * right.X - left.X * right.Z; + result.Z = left.X * right.Y - left.Y * right.X; + } + + #endregion + + #region Lerp + + /// + /// Returns a new Vector that is the linear blend of the 2 given Vectors + /// + /// First input vector + /// Second input vector + /// The blend factor + /// a when blend=0, b when blend=1, and a linear combination otherwise + public static Vector3 Lerp(Vector3 a, Vector3 b, float blend) + { + a.X = blend * (b.X - a.X) + a.X; + a.Y = blend * (b.Y - a.Y) + a.Y; + a.Z = blend * (b.Z - a.Z) + a.Z; + return a; + } + + #endregion + + #region Barycentric + + /// + /// Interpolate 3 Vectors using Barycentric coordinates + /// + /// First input Vector + /// Second input Vector + /// Third input Vector + /// First Barycentric Coordinate + /// Second Barycentric Coordinate + /// a when u=v=0, b when u=1,v=0, c when u=0,v=1, and a linear combination of a,b,c otherwise + public static Vector3 BaryCentric(Vector3 a, Vector3 b, Vector3 c, float u, float v) + { + return a + u * (b - a) + v * (c - a); + } + + #endregion + + #region Transform + + /// + /// Transform a direction vector by the given Matrix + /// Assumes the matrix has a bottom row of (0,0,0,1), that is the translation part is ignored. + /// + /// The vector to transform + /// The desired transformation + /// The transformed vector + public static Vector3 TransformVector(Vector3 vec, Matrix4 mat) + { + Vector3 v; + v.X = Vector3.Dot(vec, new Vector3(mat.Column0)); + v.Y = Vector3.Dot(vec, new Vector3(mat.Column1)); + v.Z = Vector3.Dot(vec, new Vector3(mat.Column2)); + return v; + } + + /// + /// Transform a Normal by the given Matrix + /// + /// + /// This calculates the inverse of the given matrix, use TransformNormalInverse if you + /// already have the inverse to avoid this extra calculation + /// + /// The normal to transform + /// The desired transformation + /// The transformed normal + public static Vector3 TransformNormal(Vector3 norm, Matrix4 mat) + { + mat.Invert(); + return TransformNormalInverse(norm, mat); + } + + /// + /// Transform a Normal by the (transpose of the) given Matrix + /// + /// + /// This version doesn't calculate the inverse matrix. + /// Use this version if you already have the inverse of the desired transform to hand + /// + /// The normal to transform + /// The inverse of the desired transformation + /// The transformed normal + public static Vector3 TransformNormalInverse(Vector3 norm, Matrix4 invMat) + { + Vector3 n; + n.X = Vector3.Dot(norm, new Vector3(invMat.Row0)); + n.Y = Vector3.Dot(norm, new Vector3(invMat.Row1)); + n.Z = Vector3.Dot(norm, new Vector3(invMat.Row2)); + return n; + } + + /// + /// Transform a Position by the given Matrix + /// + /// The position to transform + /// The desired transformation + /// The transformed position + public static Vector3 TransformPosition(Vector3 pos, Matrix4 mat) + { + Vector3 p; + p.X = Vector3.Dot(pos, new Vector3(mat.Column0)) + mat.Row3.X; + p.Y = Vector3.Dot(pos, new Vector3(mat.Column1)) + mat.Row3.Y; + p.Z = Vector3.Dot(pos, new Vector3(mat.Column2)) + mat.Row3.Z; + return p; + } + + /// + /// Transform a Vector by the given Matrix + /// + /// The vector to transform + /// The desired transformation + /// The transformed vector + public static Vector4 Transform(Vector3 vec, Matrix4 mat) + { + Vector4 v4 = new Vector4(vec.X, vec.Y, vec.Z, 1.0f); + Vector4 result; + result.X = Vector4.Dot(v4, mat.Column0); + result.Y = Vector4.Dot(v4, mat.Column1); + result.Z = Vector4.Dot(v4, mat.Column2); + result.W = Vector4.Dot(v4, mat.Column3); + return result; + } + + #endregion + + #endregion + + #region public override string ToString() + + /// + /// Returns a System.String that represents the current Vector3. + /// + /// + public override string ToString() { return String.Format("({0}, {1}, {2})", X, Y, Z); - } - } + } + + #endregion + } } diff --git a/Source/OpenTK/Math/Vector4.cs b/Source/OpenTK/Math/Vector4.cs index 56f51205..24660a39 100644 --- a/Source/OpenTK/Math/Vector4.cs +++ b/Source/OpenTK/Math/Vector4.cs @@ -1,5 +1,6 @@ #region --- License --- /* Copyright (c) 2006, 2007 Stefanos Apostolopoulos + * Contributions by Andy Gill. * See license.txt for license info */ #endregion @@ -39,6 +40,12 @@ namespace OpenTK.Math /// public float W; + public static Vector4 UnitX = new Vector4(1, 0, 0, 0); + public static Vector4 UnitY = new Vector4(0, 1, 0, 0); + public static Vector4 UnitZ = new Vector4(0, 0, 1, 0); + public static Vector4 UnitW = new Vector4(0, 0, 0, 1); + public static Vector4 Zero = new Vector4(0, 0, 0, 0); + #endregion #region Constructors @@ -98,56 +105,6 @@ namespace OpenTK.Math #region Functions - #region public Vector4 Add(Vector4 right) - - /// - /// Adds the given Vector4 to the current Vector4. W-coordinate remains unaffected. - /// - /// The right operand of the addition. - /// A new Vector4 containing the result of the addition. - public Vector4 Add(Vector4 right) - { - X += right.X; - Y += right.Y; - Z += right.Z; - W += right.W; - return this; - } - - #endregion - - #region public Vector4 Sub(Vector4 right) - - /// - /// Subtracts the given Vector4 from the current Vector4. - /// - /// The right operand of the subtraction. - /// A new Vector4 containing the result of the subtraction. - public Vector4 Sub(Vector4 right) - { - X -= right.X; - Y -= right.Y; - Z -= right.Z; - W -= right.W; - return this; - } - - #endregion - - #region public float Dot(Vector4 right) - - /// - /// Computes the dot product between the current Vector4 and the given Vector4. - /// - /// The right operand of the dot product. - /// A float containing the result of the dot product. - public float Dot(Vector4 right) - { - return X * right.X + Y * right.Y + Z * right.Z + W * right.W; - } - - #endregion - #region public float Length /// @@ -208,43 +165,39 @@ namespace OpenTK.Math #endregion - #region public Vector4 Normalize() + #region public void Normalize() /// /// Scales the Vector4 to unit length. /// - /// The normalized version of the current vector. - public Vector4 Normalize() + public void Normalize() { float scale = 1.0f / this.Length; X *= scale; Y *= scale; Z *= scale; W *= scale; - return this; } #endregion - #region public Vector4 NormalizeFast() + #region public void NormalizeFast() /// /// Scales the Vector4 to approximately unit length. /// - /// The normalized version of the current vector. - public Vector4 NormalizeFast() + public void NormalizeFast() { - float scale = Functions.InverseSqrtFast(X * X + Y * Y + Z * Z); + float scale = Functions.InverseSqrtFast(X * X + Y * Y + Z * Z + W * W); X *= scale; Y *= scale; Z *= scale; W *= scale; - return this; } #endregion - #region public Vector4 Scale(float sx, float sy, float sz, float sw) + #region public void Scale(float sx, float sy, float sz, float sw) /// /// Scales the current Vector4 by the given amounts. @@ -253,11 +206,13 @@ namespace OpenTK.Math /// The scale of the Y component. /// The scale of the Z component. /// The scale of the Z component. - /// The current Vector4, scaled. - public Vector4 Scale(float sx, float sy, float sz, float sw) + public void Scale(float sx, float sy, float sz, float sw) { - return new Vector4(X * sx, Y * sy, Z * sz, W * sw); - } + this.X = X * sx; + this.Y = Y * sy; + this.Z = Z * sz; + this.W = W * sw; + } #endregion @@ -267,14 +222,59 @@ namespace OpenTK.Math public static Vector4 operator +(Vector4 left, Vector4 right) { - return left.Add(right); + left.X += right.X; + left.Y += right.Y; + left.Z += right.Z; + left.W += right.W; + return left; } public static Vector4 operator -(Vector4 left, Vector4 right) { - return left.Sub(right); + left.X -= right.X; + left.Y -= right.Y; + left.Z -= right.Z; + left.W -= right.W; + return left; } + public static Vector4 operator -(Vector4 vec) + { + vec.X = -vec.X; + vec.Y = -vec.Y; + vec.Z = -vec.Z; + vec.W = -vec.W; + return vec; + } + + public static Vector4 operator *(Vector4 vec, float f) + { + vec.X *= f; + vec.Y *= f; + vec.Z *= f; + vec.W *= f; + return vec; + } + + public static Vector4 operator *(float f, Vector4 vec) + { + vec.X *= f; + vec.Y *= f; + vec.Z *= f; + vec.W *= f; + return vec; + } + + public static Vector4 operator /(Vector4 vec, float f) + { + float mult = 1.0f / f; + vec.X *= mult; + vec.Y *= mult; + vec.Z *= mult; + vec.W *= mult; + return vec; + } + [CLSCompliant(false)] unsafe public static explicit operator float*(Vector4 v) { @@ -289,12 +289,397 @@ namespace OpenTK.Math } } - #endregion - public override string ToString() + #region Static functions + + #region Add + + /// + /// Add two Vectors + /// + /// First operand + /// Second operand + /// Result of addition + public static Vector4 Add(Vector4 a, Vector4 b) + { + a.X += b.X; + a.Y += b.Y; + a.Z += b.Z; + a.W += b.W; + return a; + } + + /// + /// Add two Vectors + /// + /// First operand + /// Second operand + /// Result of addition + public static void Add(ref Vector4 a, ref Vector4 b, out Vector4 result) + { + result.X = a.X + b.X; + result.Y = a.Y + b.Y; + result.Z = a.Z + b.Z; + result.W = a.W + b.W; + } + + #endregion + + #region Sub + + /// + /// Subtract one Vector from another + /// + /// First operand + /// Second operand + /// Result of subtraction + public static Vector4 Sub(Vector4 a, Vector4 b) + { + a.X -= b.X; + a.Y -= b.Y; + a.Z -= b.Z; + a.W -= b.W; + return a; + } + + /// + /// Subtract one Vector from another + /// + /// First operand + /// Second operand + /// Result of subtraction + public static void Sub(ref Vector4 a, ref Vector4 b, out Vector4 result) + { + result.X = a.X - b.X; + result.Y = a.Y - b.Y; + result.Z = a.Z - b.Z; + result.W = a.W - b.W; + } + + #endregion + + #region Mult + + /// + /// Multiply a vector and a scalar + /// + /// Vector operand + /// Scalar operand + /// Result of the multiplication + public static Vector4 Mult(Vector4 a, float f) + { + a.X *= f; + a.Y *= f; + a.Z *= f; + a.W *= f; + return a; + } + + /// + /// Multiply a vector and a scalar + /// + /// Vector operand + /// Scalar operand + /// Result of the multiplication + public static void Mult(ref Vector4 a, float f, out Vector4 result) + { + result.X = a.X * f; + result.Y = a.Y * f; + result.Z = a.Z * f; + result.W = a.W * f; + } + + #endregion + + #region Div + + /// + /// Divide a vector by a scalar + /// + /// Vector operand + /// Scalar operand + /// Result of the division + public static Vector4 Div(Vector4 a, float f) + { + float mult = 1.0f / f; + a.X *= mult; + a.Y *= mult; + a.Z *= mult; + a.W *= mult; + return a; + } + + /// + /// Divide a vector by a scalar + /// + /// Vector operand + /// Scalar operand + /// Result of the division + public static void Div(ref Vector4 a, float f, out Vector4 result) + { + float mult = 1.0f / f; + result.X = a.X * mult; + result.Y = a.Y * mult; + result.Z = a.Z * mult; + result.W = a.W * mult; + } + + #endregion + + #region Min + + /// + /// Calculate the component-wise minimum of two vectors + /// + /// First operand + /// Second operand + /// The component-wise minimum + public static Vector4 Min(Vector4 a, Vector4 b) + { + a.X = a.X < b.X ? a.X : b.X; + a.Y = a.Y < b.Y ? a.Y : b.Y; + a.Z = a.Z < b.Z ? a.Z : b.Z; + a.W = a.W < b.W ? a.W : b.W; + return a; + } + + /// + /// Calculate the component-wise minimum of two vectors + /// + /// First operand + /// Second operand + /// The component-wise minimum + public static void Min(ref Vector4 a, ref Vector4 b, out Vector4 result) + { + result.X = a.X < b.X ? a.X : b.X; + result.Y = a.Y < b.Y ? a.Y : b.Y; + result.Z = a.Z < b.Z ? a.Z : b.Z; + result.W = a.W < b.W ? a.W : b.W; + } + + #endregion + + #region Max + + /// + /// Calculate the component-wise maximum of two vectors + /// + /// First operand + /// Second operand + /// The component-wise maximum + public static Vector4 Max(Vector4 a, Vector4 b) + { + a.X = a.X > b.X ? a.X : b.X; + a.Y = a.Y > b.Y ? a.Y : b.Y; + a.Z = a.Z > b.Z ? a.Z : b.Z; + a.W = a.W > b.W ? a.W : b.W; + return a; + } + + /// + /// Calculate the component-wise maximum of two vectors + /// + /// First operand + /// Second operand + /// The component-wise maximum + public static void Max(ref Vector4 a, ref Vector4 b, out Vector4 result) + { + result.X = a.X > b.X ? a.X : b.X; + result.Y = a.Y > b.Y ? a.Y : b.Y; + result.Z = a.Z > b.Z ? a.Z : b.Z; + result.W = a.W > b.W ? a.W : b.W; + } + + #endregion + + #region Clamp + + /// + /// Clamp a vector to the given minimum and maximum vectors + /// + /// Input vector + /// Minimum vector + /// Maximum vector + /// The clamped vector + public static Vector4 Clamp(Vector4 vec, Vector4 min, Vector4 max) + { + vec.X = vec.X < min.X ? min.X : vec.X > max.X ? max.X : vec.X; + vec.Y = vec.Y < min.Y ? min.Y : vec.Y > max.Y ? max.Y : vec.Y; + vec.Z = vec.X < min.Z ? min.Z : vec.Z > max.Z ? max.Z : vec.Z; + vec.W = vec.Y < min.W ? min.W : vec.W > max.W ? max.W : vec.W; + return vec; + } + + /// + /// Clamp a vector to the given minimum and maximum vectors + /// + /// Input vector + /// Minimum vector + /// Maximum vector + /// The clamped vector + public static void Clamp(ref Vector4 vec, ref Vector4 min, ref Vector4 max, out Vector4 result) + { + result.X = vec.X < min.X ? min.X : vec.X > max.X ? max.X : vec.X; + result.Y = vec.Y < min.Y ? min.Y : vec.Y > max.Y ? max.Y : vec.Y; + result.Z = vec.X < min.Z ? min.Z : vec.Z > max.Z ? max.Z : vec.Z; + result.W = vec.Y < min.W ? min.W : vec.W > max.W ? max.W : vec.W; + } + + #endregion + + #region Normalize + + /// + /// Scale a vector to unit length + /// + /// The input vector + /// The normalized vector + public static Vector4 Normalize(Vector4 vec) + { + float scale = 1.0f / vec.Length; + vec.X *= scale; + vec.Y *= scale; + vec.Z *= scale; + vec.W *= scale; + return vec; + } + + /// + /// Scale a vector to unit length + /// + /// The input vector + /// The normalized vector + public static void Normalize(ref Vector4 vec, out Vector4 result) + { + float scale = 1.0f / vec.Length; + result.X = vec.X * scale; + result.Y = vec.Y * scale; + result.Z = vec.Z * scale; + result.W = vec.W * scale; + } + + #endregion + + #region NormalizeFast + + /// + /// Scale a vector to approximately unit length + /// + /// The input vector + /// The normalized vector + public static Vector4 NormalizeFast(Vector4 vec) + { + float scale = Functions.InverseSqrtFast(vec.X * vec.X + vec.Y * vec.Y + vec.Z * vec.Z + vec.W * vec.W); + vec.X *= scale; + vec.Y *= scale; + vec.Z *= scale; + vec.W *= scale; + return vec; + } + + /// + /// Scale a vector to approximately unit length + /// + /// The input vector + /// The normalized vector + public static void NormalizeFast(ref Vector4 vec, out Vector4 result) + { + float scale = Functions.InverseSqrtFast(vec.X * vec.X + vec.Y * vec.Y + vec.Z * vec.Z + vec.W * vec.W); + result.X = vec.X * scale; + result.Y = vec.Y * scale; + result.Z = vec.Z * scale; + result.W = vec.W * scale; + } + + #endregion + + #region Dot + + /// + /// Caclulate the dot product of two vectors + /// + /// First operand + /// Second operand + /// The dot product of the two inputs + public static float Dot(Vector4 left, Vector4 right) + { + return left.X * right.X + left.Y * right.Y + left.Z * right.Z + left.W * right.W; + } + + #endregion + + #region Lerp + + /// + /// Returns a new Vector that is the linear blend of the 2 given Vectors + /// + /// First input vector + /// Second input vector + /// The blend factor + /// a when blend=0, b when blend=1, and a linear combination otherwise + public static Vector4 Lerp(Vector4 a, Vector4 b, float blend) + { + a.X = blend * (b.X - a.X) + a.X; + a.Y = blend * (b.Y - a.Y) + a.Y; + a.Z = blend * (b.Z - a.Z) + a.Z; + a.W = blend * (b.W - a.W) + a.W; + return a; + } + + #endregion + + #region Barycentric + + /// + /// Interpolate 3 Vectors using Barycentric coordinates + /// + /// First input Vector + /// Second input Vector + /// Third input Vector + /// First Barycentric Coordinate + /// Second Barycentric Coordinate + /// a when u=v=0, b when u=1,v=0, c when u=0,v=1, and a linear combination of a,b,c otherwise + public static Vector4 BaryCentric(Vector4 a, Vector4 b, Vector4 c, float u, float v) + { + return a + u * (b - a) + v * (c - a); + } + + #endregion + + #region Transform + + /// + /// Transform a Vector by the given Matrix + /// + /// The vector to transform + /// The desired transformation + /// The transformed vector + public static Vector4 Transform(Vector4 vec, Matrix4 mat) + { + Vector4 result; + result.X = Vector4.Dot(vec, mat.Column0); + result.Y = Vector4.Dot(vec, mat.Column1); + result.Z = Vector4.Dot(vec, mat.Column2); + result.W = Vector4.Dot(vec, mat.Column3); + return result; + } + + #endregion + + #endregion + + #region public override string ToString() + + /// + /// Returns a System.String that represents the current Vector4. + /// + /// + public override string ToString() { return String.Format("({0}, {1}, {2}, {3})", X, Y, Z, W); - } - } + } + + #endregion + } }