mono
/
Mono.Simd.Math
зеркало из https://github.com/mono/Mono.Simd.Math.git
1
0
Форкнуть 0
Код Задачи Пакеты Проекты Релизы Вики Активность

* Filled in initial implementations for Matrix4f, Quaternion4f, and Vector4f helpers. 

* Added Utils class, adapted from Mono.Xna's MathHelper.
* Added stub files for Vector2f and Vector2d.

svn path=/trunk/Mono.Simd.Math/; revision=125036
This commit is contained in:
John Hurliman 2009-01-30 00:52:11 +00:00
Родитель 437c086ce8
Коммит db4eff14a0
14 изменённых файлов: 1750 добавлений и 26 удалений
Показать статистику Скачать Patch файл Скачать Diff файл Показать все файлы Свернуть все файлы

9
Changelog
Просмотреть файл

@ -1,3 +1,12 @@
2009-01-29 John Hurliman <john.hurliman@gmail.com>
* Filled in initial implementations for Matrix4f, Quaternion4f, and
Vector4f helpers.
* Added Utils class, adapted from Mono.Xna's MathHelper.
* Added stub files for Vector2f and Vector2d.
2009-01-25 Jerry Maine <crashfourit.gmail.com>
* Initial file import. Released under MIT/X11 License.

7
Matrix3d.cs
Просмотреть файл

@ -1,9 +1,10 @@
// Matrix3d.cs
//
// Author:
// Authors:
// Jerry Maine (crashfourit@gmail.com)
// John Hurliman (john.hurliman@intel.com)
//
// (C) Jerry Maine
// (C) Jerry Maine & Intel Corporation
//
// Permission is hereby granted, free of charge, to any person obtaining
// a copy of this software and associated documentation files (the
@ -23,4 +24,4 @@
// LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
// WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
//
//

5
Matrix3f.cs
Просмотреть файл

@ -1,9 +1,10 @@
// Matrix3f.cs
//
// Author:
// Authors:
// Jerry Maine (crashfourit@gmail.com)
// John Hurliman (john.hurliman@intel.com)
//
// (C) Jerry Maine
// (C) Jerry Maine & Intel Corporation
//
// Permission is hereby granted, free of charge, to any person obtaining
// a copy of this software and associated documentation files (the

5
Matrix4d.cs
Просмотреть файл

@ -1,9 +1,10 @@
// Matrix4d.cs
//
// Author:
// Authors:
// Jerry Maine (crashfourit@gmail.com)
// John Hurliman (john.hurliman@intel.com)
//
// (C) Jerry Maine
// (C) Jerry Maine & Intel Corporation
//
// Permission is hereby granted, free of charge, to any person obtaining
// a copy of this software and associated documentation files (the

651
Matrix4f.cs
Просмотреть файл

@ -1,9 +1,10 @@
// Matrix4f.cs
//
// Author:
// Authors:
// Jerry Maine (crashfourit@gmail.com)
// John Hurliman (john.hurliman@intel.com)
//
// (C) Jerry Maine
// (C) Jerry Maine & Intel Corporation
//
// Permission is hereby granted, free of charge, to any person obtaining
// a copy of this software and associated documentation files (the
@ -23,4 +24,648 @@
// LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
// WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
//
//
using System;
using System.Runtime.InteropServices;
namespace Mono.Simd.Math {
[Serializable]
[StructLayout(LayoutKind.Explicit, Pack = 0, Size = 64)]
public struct Matrix4f {
[FieldOffset(0)]
private Vector4f R0;
[FieldOffset(16)]
private Vector4f R1;
[FieldOffset(32)]
private Vector4f R2;
[FieldOffset(48)]
private Vector4f R3;
#region Properties
public float M11 { get { return R0.X; } set { R0.X = value; } }
public float M12 { get { return R0.Y; } set { R0.Y = value; } }
public float M13 { get { return R0.Z; } set { R0.Z = value; } }
public float M14 { get { return R0.W; } set { R0.W = value; } }
public float M21 { get { return R1.X; } set { R1.X = value; } }
public float M22 { get { return R1.Y; } set { R1.Y = value; } }
public float M23 { get { return R1.Z; } set { R1.Z = value; } }
public float M24 { get { return R1.W; } set { R1.W = value; } }
public float M31 { get { return R2.X; } set { R2.X = value; } }
public float M32 { get { return R2.Y; } set { R2.Y = value; } }
public float M33 { get { return R2.Z; } set { R2.Z = value; } }
public float M34 { get { return R2.W; } set { R2.W = value; } }
public float M41 { get { return R3.X; } set { R3.X = value; } }
public float M42 { get { return R3.Y; } set { R3.Y = value; } }
public float M43 { get { return R3.Z; } set { R3.Z = value; } }
public float M44 { get { return R3.W; } set { R3.W = value; } }
#endregion Properties
#region Constructors
public Matrix4f(
float m11, float m12, float m13, float m14,
float m21, float m22, float m23, float m24,
float m31, float m32, float m33, float m34,
float m41, float m42, float m43, float m44)
{
R0 = new Vector4f(m11, m12, m13, m14);
R1 = new Vector4f(m21, m22, m23, m24);
R2 = new Vector4f(m31, m32, m33, m34);
R3 = new Vector4f(m41, m42, m43, m44);
}
public Matrix4f(Vector4f r0, Vector4f r1, Vector4f r2, Vector4f r3)
{
R0 = r0;
R1 = r1;
R2 = r2;
R3 = r3;
}
public Matrix4f(Matrix4f m)
{
R0 = m.R0;
R1 = m.R1;
R2 = m.R2;
R3 = m.R3;
}
#endregion Constructors
#region Public Methods
public float Determinant()
{
return
R0.W * R1.Z * R2.Y * R3.X - R0.Z * R1.W * R2.Y * R3.X - R0.W * R1.Y * R2.Z * R3.X + R0.Y * R1.W * R2.Z * R3.X +
R0.Z * R1.Y * R2.W * R3.X - R0.Y * R1.Z * R2.W * R3.X - R0.W * R1.Z * R2.X * R3.Y + R0.Z * R1.W * R2.X * R3.Y +
R0.W * R1.X * R2.Z * R3.Y - R0.X * R1.W * R2.Z * R3.Y - R0.Z * R1.X * R2.W * R3.Y + R0.X * R1.Z * R2.W * R3.Y +
R0.W * R1.Y * R2.X * R3.Z - R0.Y * R1.W * R2.X * R3.Z - R0.W * R1.X * R2.Y * R3.Z + R0.X * R1.W * R2.Y * R3.Z +
R0.Y * R1.X * R2.W * R3.Z - R0.X * R1.Y * R2.W * R3.Z - R0.Z * R1.Y * R2.X * R3.W + R0.Y * R1.Z * R2.X * R3.W +
R0.Z * R1.X * R2.Y * R3.W - R0.X * R1.Z * R2.Y * R3.W - R0.Y * R1.X * R2.Z * R3.W + R0.X * R1.Y * R2.Z * R3.W;
}
public float Trace()
{
return R0.X + R1.Y + R2.Z + R3.W;
}
public void GetEulerAngles(out float roll, out float pitch, out float yaw)
{
double angleX, angleY, angleZ;
double cx, cy, cz; // cosines
double sx, sz; // sines
angleY = System.Math.Asin(Utils.Clamp(R0.Z, -1f, 1f));
cy = System.Math.Cos(angleY);
if (System.Math.Abs(cy) > 0.005f)
{
// No gimbal lock
cx = R2.Z / cy;
sx = (-R1.Z) / cy;
angleX = (float)System.Math.Atan2(sx, cx);
cz = R0.X / cy;
sz = (-R0.Y) / cy;
angleZ = (float)System.Math.Atan2(sz, cz);
}
else
{
// Gimbal lock
angleX = 0;
cz = R1.Y;
sz = R1.X;
angleZ = System.Math.Atan2(sz, cz);
}
// Return only positive angles in [0,360]
if (angleX < 0) angleX += 360d;
if (angleY < 0) angleY += 360d;
if (angleZ < 0) angleZ += 360d;
roll = (float)angleX;
pitch = (float)angleY;
yaw = (float)angleZ;
}
public Quaternionf GetQuaternion()
{
Quaternionf quat = new Quaternionf();
float trace = Trace() + 1f;
if (trace > Single.Epsilon)
{
float s = 0.5f / (float)System.Math.Sqrt(trace);
quat.X = (R2.Y - R1.Z) * s;
quat.Y = (R0.Z - R2.X) * s;
quat.Z = (R1.X - R0.Y) * s;
quat.W = 0.25f / s;
}
else
{
if (R0.X > R1.Y && R0.X > R2.Z)
{
float s = 2.0f * (float)System.Math.Sqrt(1.0f + R0.X - R1.Y - R2.Z);
quat.X = 0.25f * s;
quat.Y = (R0.Y + R1.X) / s;
quat.Z = (R0.Z + R2.X) / s;
quat.W = (R1.Z - R2.Y) / s;
}
else if (R1.Y > R2.Z)
{
float s = 2.0f * (float)System.Math.Sqrt(1.0f + R1.Y - R0.X - R2.Z);
quat.X = (R0.Y + R1.X) / s;
quat.Y = 0.25f * s;
quat.Z = (R1.Z + R2.Y) / s;
quat.W = (R0.Z - R2.X) / s;
}
else
{
float s = 2.0f * (float)System.Math.Sqrt(1.0f + R2.Z - R0.X - R1.Y);
quat.X = (R0.Z + R2.X) / s;
quat.Y = (R1.Z + R2.Y) / s;
quat.Z = 0.25f * s;
quat.W = (R0.Y - R1.X) / s;
}
}
return quat;
}
public void Negate()
{
R0 *= Vector4f.MinusOne;
R1 *= Vector4f.MinusOne;
R2 *= Vector4f.MinusOne;
R3 *= Vector4f.MinusOne;
}
public Matrix4f Transform(ref Quaternionf rotation)
{
float x2 = rotation.X + rotation.X;
float y2 = rotation.Y + rotation.Y;
float z2 = rotation.Z + rotation.Z;
float a = (1f - rotation.Y * y2) - rotation.Z * z2;
float b = rotation.X * y2 - rotation.W * z2;
float c = rotation.X * z2 + rotation.W * y2;
float d = rotation.X * y2 + rotation.W * z2;
float e = (1f - rotation.X * x2) - rotation.Z * z2;
float f = rotation.Y * z2 - rotation.W * x2;
float g = rotation.X * z2 - rotation.W * y2;
float h = rotation.Y * z2 + rotation.W * x2;
float i = (1f - rotation.X * x2) - rotation.Y * y2;
return new Matrix4f(
new Vector4f(
((R0.X * a) + (R0.Y * b)) + (R0.Z * c),
((R0.X * d) + (R0.Y * e)) + (R0.Z * f),
((R0.X * g) + (R0.Y * h)) + (R0.Z * i),
R0.W),
new Vector4f(
((R1.X * a) + (R1.Y * b)) + (R1.Z * c),
((R1.X * d) + (R1.Y * e)) + (R1.Z * f),
((R1.X * g) + (R1.Y * h)) + (R1.Z * i),
R1.W),
new Vector4f(
((R2.X * a) + (R2.Y * b)) + (R2.Z * c),
((R2.X * d) + (R2.Y * e)) + (R2.Z * f),
((R2.X * g) + (R2.Y * h)) + (R2.Z * i),
R2.W),
new Vector4f(
((R3.X * a) + (R3.Y * b)) + (R3.Z * c),
((R3.X * d) + (R3.Y * e)) + (R3.Z * f),
((R3.X * g) + (R3.Y * h)) + (R3.Z * i),
R3.W));
}
public Matrix4f Transpose()
{
return new Matrix4f(
new Vector4f(R0.X, R1.X, R2.X, R3.X),
new Vector4f(R0.Y, R1.Y, R2.Y, R3.Y),
new Vector4f(R0.Z, R1.Z, R2.Z, R3.Z),
new Vector4f(R0.W, R1.W, R2.W, R3.W));
}
public void FromAxisAngle(Vector3f axis, float angle)
{
float x = axis.X;
float y = axis.Y;
float z = axis.Z;
float sin = (float)System.Math.Sin(angle);
float cos = (float)System.Math.Cos(angle);
float xx = x * x;
float yy = y * y;
float zz = z * z;
float xy = x * y;
float xz = x * z;
float yz = y * z;
R0.X = xx + (cos * (1f - xx));
R0.Y = (xy - (cos * xy)) + (sin * z);
R0.Z = (xz - (cos * xz)) - (sin * y);
R0.W = 0f;
R1.X = (xy - (cos * xy)) - (sin * z);
R1.Y = yy + (cos * (1f - yy));
R1.Z = (yz - (cos * yz)) + (sin * x);
R1.W = 0f;
R2.X = (xz - (cos * xz)) + (sin * y);
R2.Y = (yz - (cos * yz)) - (sin * x);
R2.Z = zz + (cos * (1f - zz));
R2.W = 0f;
R3.X = R3.Y = R3.Z = 0f;
R3.W = 1f;
}
public void FromEulers(float roll, float pitch, float yaw)
{
float a, b, c, d, e, f;
float ad, bd;
a = (float)System.Math.Cos(roll);
b = (float)System.Math.Sin(roll);
c = (float)System.Math.Cos(pitch);
d = (float)System.Math.Sin(pitch);
e = (float)System.Math.Cos(yaw);
f = (float)System.Math.Sin(yaw);
ad = a * d;
bd = b * d;
R0.X = c * e;
R0.Y = -c * f;
R0.Z = d;
R0.W = 0f;
R1.X = bd * e + a * f;
R1.Y = -bd * f + a * e;
R1.Z = -b * c;
R1.W = 0f;
R2.X = -ad * e + b * f;
R2.Y = ad * f + b * e;
R2.Z = a * c;
R2.W = 0f;
R3.X = R3.Y = R3.Z = 0f;
R3.W = 1f;
}
public void FromQuaternion(Quaternionf quaternion)
{
Matrix4f m1 = new Matrix4f(
quaternion.W, quaternion.Z, -quaternion.Y, quaternion.X,
-quaternion.Z, quaternion.W, quaternion.X, quaternion.Y,
quaternion.Y, -quaternion.X, quaternion.W, quaternion.Z,
-quaternion.X, -quaternion.Y, -quaternion.Z, quaternion.W);
Matrix4f m2 = new Matrix4f(
quaternion.W, quaternion.Z, -quaternion.Y, -quaternion.X,
-quaternion.Z, quaternion.W, quaternion.X, -quaternion.Y,
quaternion.Y, -quaternion.X, quaternion.W, -quaternion.Z,
quaternion.X, quaternion.Y, quaternion.Z, quaternion.W);
this = m1 * m2;
}
public void FromRotationX(float radians)
{
float cos = (float)System.Math.Cos(radians);
float sin = (float)System.Math.Sin(radians);
R0 = new Vector4f(1f, 0f, 0f, 0f);
R1 = new Vector4f(0f, cos, sin, 0f);
R2 = new Vector4f(0f, -sin, cos, 0f);
R3 = new Vector4f(0f, 0f, 0f, 1f);
}
public void FromRotationY(float radians)
{
float cos = (float)System.Math.Cos(radians);
float sin = (float)System.Math.Sin(radians);
R0 = new Vector4f(cos, 0f, -sin, 0f);
R1 = new Vector4f(0f, 1f, 0f, 0f);
R2 = new Vector4f(sin, 0f, cos, 0f);
R3 = new Vector4f(0f, 0f, 0f, 1f);
}
public void FromRotationZ(float radians)
{
float cos = (float)System.Math.Cos(radians);
float sin = (float)System.Math.Sin(radians);
R0 = new Vector4f(cos, sin, 0f, 0f);
R1 = new Vector4f(-sin, cos, 0f, 0f);
R2 = new Vector4f(0f, 0f, 1f, 0f);
R3 = new Vector4f(0f, 0f, 0f, 1f);
}
public void FromScale(Vector3f scale)
{
R0 = new Vector4f(scale.X, 0f, 0f, 0f);
R1 = new Vector4f(0f, scale.Y, 0f, 0f);
R2 = new Vector4f(0f, 0f, scale.Z, 0f);
R3 = new Vector4f(0f, 0f, 0f, 1f);
}
public void FromTranslation(Vector3f position)
{
R0 = new Vector4f(1f, 0f, 0f, 0f);
R1 = new Vector4f(0f, 1f, 0f, 0f);
R2 = new Vector4f(0f, 0f, 1f, 0f);
R3 = new Vector4f(position.X, position.Y, position.Z, 1f);
}
public void FromWorld(Vector3f position, Vector3f forward, Vector3f up)
{
// Normalize forward vector
forward.Normalize();
// Calculate right vector
Vector3f right = forward.Cross(ref up);
right.Normalize();
// Recalculate up vector
up = right.Cross(ref forward);
up.Normalize();
R0 = new Vector4f(right.X, right.Y, right.Z, 0.0f);
R1 = new Vector4f(up.X, up.Y, up.Z, 0.0f);
R2 = new Vector4f(-forward.X, -forward.Y, -forward.Z, 0.0f);
R3 = new Vector4f(position.X, position.Y, position.Z, 1.0f);
}
#endregion Public Methods
#region Overrides
public override int GetHashCode()
{
return
R0.X.GetHashCode() ^ R0.Y.GetHashCode() ^ R0.Z.GetHashCode() ^ R0.W.GetHashCode() ^
R1.X.GetHashCode() ^ R1.Y.GetHashCode() ^ R1.Z.GetHashCode() ^ R1.W.GetHashCode() ^
R2.X.GetHashCode() ^ R2.Y.GetHashCode() ^ R2.Z.GetHashCode() ^ R2.W.GetHashCode() ^
R3.X.GetHashCode() ^ R3.Y.GetHashCode() ^ R3.Z.GetHashCode() ^ R3.W.GetHashCode();
}
/// <summary>
/// Get a formatted string representation of the vector
/// </summary>
/// <returns>A string representation of the vector</returns>
public override string ToString()
{
return string.Format(
"|{0}, {1}, {2}, {3}|\n" +
"|{4}, {5}, {6}, {7}|\n" +
"|{8}, {9}, {10}, {11}|\n" +
"|{12}, {13}, {14}, {15}|",
R0.X, R0.Y, R0.Z, R0.W,
R1.X, R1.Y, R1.Z, R1.W,
R2.X, R2.Y, R2.Z, R2.W,
R3.X, R3.Y, R3.Z, R3.W);
}
#endregion Overrides
#region Operators
public static bool operator ==(Matrix4f left, Matrix4f right)
{
Vector4f result =
left.R0.CompareNotEqual(right.R0) +
left.R1.CompareNotEqual(right.R1) +
left.R2.CompareNotEqual(right.R2) +
left.R3.CompareNotEqual(right.R3);
return result.X != 0f || result.Y != 0f || result.Y != 0f || result.Z != 0f;
}
public static bool operator !=(Matrix4f left, Matrix4f right)
{
Vector4f result =
left.R0.CompareEqual(right.R0) +
left.R1.CompareEqual(right.R1) +
left.R2.CompareEqual(right.R2) +
left.R3.CompareEqual(right.R3);
return result.X == 0f || result.Y == 0f || result.Z == 0f || result.W == 0f;
}
public static Matrix4f operator +(Matrix4f left, Matrix4f right)
{
return new Matrix4f(
left.R0 + right.R0,
left.R1 + right.R1,
left.R2 + right.R2,
left.R3 + right.R3);
}
public static Matrix4f operator -(Matrix4f matrix)
{
matrix.Negate();
return matrix;
}
public static Matrix4f operator -(Matrix4f left, Matrix4f right)
{
return new Matrix4f(
left.R0 - right.R0,
left.R1 - right.R1,
left.R2 - right.R2,
left.R3 - right.R3);
}
public static Matrix4f operator *(Matrix4f left, Matrix4f right)
{
Vector4f t1, t2;
Vector4f out0, out1, out2, out3;
t1 = (left.R0.Shuffle(ShuffleSel.ExpandX) * right.R0) + (left.R0.Shuffle(ShuffleSel.ExpandY) * right.R1);
t2 = (left.R0.Shuffle(ShuffleSel.ExpandZ) * right.R2) + (left.R0.Shuffle(ShuffleSel.ExpandW) * right.R3);
out0 = t1 + t2;
t1 = (left.R1.Shuffle(ShuffleSel.ExpandX) * right.R0) + (left.R1.Shuffle(ShuffleSel.ExpandY) * right.R1);
t2 = (left.R1.Shuffle(ShuffleSel.ExpandZ) * right.R2) + (left.R1.Shuffle(ShuffleSel.ExpandW) * right.R3);
out1 = t1 + t2;
t1 = (left.R2.Shuffle(ShuffleSel.ExpandX) * right.R0) + (left.R2.Shuffle(ShuffleSel.ExpandY) * right.R1);
t2 = (left.R2.Shuffle(ShuffleSel.ExpandZ) * right.R2) + (left.R2.Shuffle(ShuffleSel.ExpandW) * right.R3);
out2 = t1 + t2;
t1 = (left.R3.Shuffle(ShuffleSel.ExpandX) * right.R0) + (left.R3.Shuffle(ShuffleSel.ExpandY) * right.R1);
t2 = (left.R3.Shuffle(ShuffleSel.ExpandZ) * right.R2) + (left.R3.Shuffle(ShuffleSel.ExpandW) * right.R3);
out3 = t1 + t2;
return new Matrix4f(out0, out1, out2, out3);
}
public static Matrix4f operator *(Matrix4f left, float scaleFactor)
{
Vector4f v = new Vector4f(scaleFactor);
return new Matrix4f(
left.R0 * v,
left.R1 * v,
left.R2 * v,
left.R3 * v);
}
public static Matrix4f operator /(Matrix4f left, Matrix4f right)
{
return new Matrix4f(
left.R0 / right.R0,
left.R1 / right.R1,
left.R2 / right.R2,
left.R3 / right.R3);
}
public static Matrix4f operator /(Matrix4f matrix, float divider)
{
float oodivider = 1f / divider;
Vector4f v = new Vector4f(oodivider);
return new Matrix4f(
matrix.R0 * v,
matrix.R1 * v,
matrix.R2 * v,
matrix.R3 * v);
}
[System.Runtime.CompilerServices.IndexerName("Component")]
public Vector4f this[int row]
{
get
{
switch (row)
{
case 0:
return R0;
case 1:
return R1;
case 2:
return R2;
case 3:
return R3;
default:
throw new IndexOutOfRangeException("row");
}
}
set
{
switch (row)
{
case 0:
R0 = value;
break;
case 1:
R1 = value;
break;
case 2:
R2 = value;
break;
case 3:
R3 = value;
break;
default:
throw new IndexOutOfRangeException("row");
}
}
}
[System.Runtime.CompilerServices.IndexerName("Component")]
public float this[int row, int column]
{
get
{
switch (row)
{
case 0:
switch (column)
{
case 0:
return R0.X;
case 1:
return R0.Y;
case 2:
return R0.Z;
case 3:
return R0.W;
default:
throw new IndexOutOfRangeException("column");
}
case 1:
switch (column)
{
case 0:
return R1.X;
case 1:
return R1.Y;
case 2:
return R1.Z;
case 3:
return R1.W;
default:
throw new IndexOutOfRangeException("column");
}
case 2:
switch (column)
{
case 0:
return R2.X;
case 1:
return R2.Y;
case 2:
return R2.Z;
case 3:
return R2.W;
default:
throw new IndexOutOfRangeException("column");
}
case 3:
switch (column)
{
case 0:
return R3.X;
case 1:
return R3.Y;
case 2:
return R3.Z;
case 3:
return R3.W;
default:
throw new IndexOutOfRangeException("column");
}
default:
throw new IndexOutOfRangeException("row");
}
}
}
#endregion Operators
public static readonly Matrix4f Zero = new Matrix4f();
public static readonly Matrix4f Identity = new Matrix4f(
1f, 0f, 0f, 0f,
0f, 1f, 0f, 0f,
0f, 0f, 1f, 0f,
0f, 0f, 0f, 1f);
}
}

5
Quaterniond.cs
Просмотреть файл

@ -1,9 +1,10 @@
// Quaterniond.cs
//
// Author:
// Authors:
// Jerry Maine (crashfourit@gmail.com)
// John Hurliman (john.hurliman@intel.com)
//
// (C) Jerry Maine
// (C) Jerry Maine & Intel Corporation
//
// Permission is hereby granted, free of charge, to any person obtaining
// a copy of this software and associated documentation files (the

436
Quaternionf.cs
Просмотреть файл

@ -1,9 +1,10 @@
// Quaternionf.cs
//
// Author:
// Authors:
// Jerry Maine (crashfourit@gmail.com)
// John Hurliman (john.hurliman@intel.com)
//
// (C) Jerry Maine
// (C) Jerry Maine & Intel Corporation
//
// Permission is hereby granted, free of charge, to any person obtaining
// a copy of this software and associated documentation files (the
@ -23,4 +24,433 @@
// LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
// WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
//
//
using System;
using System.Runtime.InteropServices;
namespace Mono.Simd.Math
{
[Serializable]
[StructLayout(LayoutKind.Explicit, Pack = 0, Size = 16)]
public struct Quaternionf
{
[FieldOffset(0)]
public Vector4f Vector;
#region Properties
public float X { get { return Vector.X; } set { Vector.X = value; } }
public float Y { get { return Vector.Y; } set { Vector.Y = value; } }
public float Z { get { return Vector.Z; } set { Vector.Z = value; } }
public float W { get { return Vector.W; } set { Vector.W = value; } }
#endregion Properties
#region Constructors
public Quaternionf(float x, float y, float z, float w)
{
Vector = new Vector4f(x, y, z, w);
}
public Quaternionf(Vector4f vector)
{
Vector = vector;
}
public Quaternionf(Vector3f vector, float scalar)
{
Vector = new Vector4f(vector.X, vector.Y, vector.Z, scalar);
}
public Quaternionf(Quaternionf quaternion)
{
Vector = quaternion.Vector;
}
#endregion Constructors
#region Public Methods
public void Negate()
{
Vector *= Vector4f.MinusOne;
}
public void Normalize()
{
Vector4f factor = Vector * Vector;
factor = factor.HorizontalAdd(factor);
factor = factor.HorizontalAdd(factor);
factor = factor.InvSqrt();
Vector *= factor;
}
public float Length()
{
Vector4f length = Vector * Vector;
length = length.HorizontalAdd(length);
length = length.HorizontalAdd(length);
return length.Sqrt().X;
}
public float LengthSquared()
{
Vector4f length = Vector * Vector;
length = length.HorizontalAdd(length);
return length.HorizontalAdd(length).X;
}
/// <summary>
/// Convert this quaternion to euler angles
/// </summary>
/// <param name="roll">X euler angle</param>
/// <param name="pitch">Y euler angle</param>
/// <param name="yaw">Z euler angle</param>
public void GetEulerAngles(out float roll, out float pitch, out float yaw)
{
Vector4f sq = Vector * Vector;
// Unit will be a correction factor if the quaternion is not normalized
Vector4f sqSum = sq.HorizontalAdd(sq);
sqSum = sq.HorizontalAdd(sq);
float unit = sqSum.X;
double test = X * Y + Z * W;
if (test > 0.499f * unit)
{
// Singularity at north pole
yaw = 2f * (float)System.Math.Atan2(X, W);
pitch = Utils.Pi / 2f;
roll = 0f;
}
else if (test < -0.499f * unit)
{
// Singularity at south pole
yaw = -2f * (float)System.Math.Atan2(X, W);
pitch = -Utils.Pi / 2f;
roll = 0f;
}
else
{
yaw = (float)System.Math.Atan2(2f * Y * W - 2f * X * Z, sq.X - sq.Y - sq.Z + sq.W);
pitch = (float)System.Math.Asin(2f * test / unit);
roll = (float)System.Math.Atan2(2f * X * W - 2f * Y * Z, -sq.X + sq.Y - sq.Z + sq.W);
}
}
/// <summary>
/// Convert this quaternion to an angle around an axis
/// </summary>
/// <param name="axis">Unit vector describing the axis</param>
/// <param name="angle">Angle around the axis, in radians</param>
public void GetAxisAngle(out Vector3f axis, out float angle)
{
Vector4f abs = Vector & (Vector4f)new Vector4i(0x7fffffff);
abs = abs.HorizontalAdd(abs);
abs = abs.HorizontalAdd(abs);
float scale = abs.X;
if (scale < Single.Epsilon || W > 1.0f || W < -1.0f)
{
angle = 0.0f;
axis.X = 0.0f;
axis.Y = 1.0f;
axis.Z = 0.0f;
}
else
{
angle = 2.0f * (float)System.Math.Acos(W);
float ooscale = 1f / scale;
axis.X = X * ooscale;
axis.Y = Y * ooscale;
axis.Z = Z * ooscale;
}
}
/// <summary>
/// Sets this quaternion to the conjugate (spatial inverse) of itself
/// </summary>
public void Conjugate()
{
Vector.X = -X;
Vector.Y = -Y;
Vector.Z = -Z;
}
/// <summary>
/// Conjugates and renormalizes the quaternion
/// </summary>
public void Invert()
{
float norm = LengthSquared();
if (norm == 0f)
{
Vector = Vector4f.Zero;
}
else
{
Conjugate();
Vector4f r = new Vector4f(norm);
r = r.Reciprocal();
Vector *= r;
}
}
/// <summary>
/// Set this quaternion from a normalized axis and an angle of rotation
/// around that axis
/// </summary>
/// <param name="axis">Axis of rotation</param>
/// <param name="angle">Angle of rotation</param>
public void FromAxisAngle(Vector3f axis, float angle)
{
angle *= 0.5f;
float c = (float)System.Math.Cos(angle);
float s = (float)System.Math.Sin(angle);
Vector.X = axis.X * s;
Vector.Y = axis.Y * s;
Vector.Z = axis.Z * s;
Vector.W = c;
Vector.Normalize();
}
/// <summary>
/// Set this quaternion from roll, pitch, and yaw euler angles in
/// radians
/// </summary>
/// <param name="roll">X angle in radians</param>
/// <param name="pitch">Y angle in radians</param>
/// <param name="yaw">Z angle in radians</param>
public void FromEulers(float roll, float pitch, float yaw)
{
double atCos = System.Math.Cos(roll / 2f);
double atSin = System.Math.Sin(roll / 2f);
double leftCos = System.Math.Cos(pitch / 2f);
double leftSin = System.Math.Sin(pitch / 2f);
double upCos = System.Math.Cos(yaw / 2f);
double upSin = System.Math.Sin(yaw / 2f);
double atLeftCos = atCos * leftCos;
double atLeftSin = atSin * leftSin;
Vector.X = (float)(atSin * leftCos * upCos + atCos * leftSin * upSin);
Vector.Y = (float)(atCos * leftSin * upCos - atSin * leftCos * upSin);
Vector.Z = (float)(atLeftCos * upSin + atLeftSin * upCos);
Vector.W = (float)(atLeftCos * upCos - atLeftSin * upSin);
}
public void FromRotationMatrix(Matrix4f m)
{
float trace = m.Trace();
if (trace > Single.Epsilon)
{
float s = (float)System.Math.Sqrt(trace + 1f);
Vector.W = s * 0.5f;
s = 0.5f / s;
Vector.X = (m.M23 - m.M32) * s;
Vector.Y = (m.M31 - m.M13) * s;
Vector.Z = (m.M12 - m.M21) * s;
}
else
{
if (m.M11 > m.M22 && m.M11 > m.M33)
{
float s = (float)System.Math.Sqrt(1f + m.M11 - m.M22 - m.M33);
Vector.X = 0.5f * s;
s = 0.5f / s;
Vector.Y = (m.M12 + m.M21) * s;
Vector.Z = (m.M13 + m.M31) * s;
Vector.W = (m.M23 - m.M32) * s;
}
else if (m.M22 > m.M33)
{
float s = (float)System.Math.Sqrt(1f + m.M22 - m.M11 - m.M33);
Vector.Y = 0.5f * s;
s = 0.5f / s;
Vector.X = (m.M21 + m.M12) * s;
Vector.Z = (m.M32 + m.M23) * s;
Vector.W = (m.M31 - m.M13) * s;
}
else
{
float s = (float)System.Math.Sqrt(1f + m.M33 - m.M11 - m.M22);
Vector.Z = 0.5f * s;
s = 0.5f / s;
Vector.X = (m.M31 + m.M13) * s;
Vector.Y = (m.M32 + m.M23) * s;
Vector.W = (m.M12 - m.M21) * s;
}
}
}
public float Dot(Quaternionf quaternion)
{
return
(Vector.X * quaternion.Vector.X) +
(Vector.Y * quaternion.Vector.Y) +
(Vector.Z * quaternion.Vector.Z) +
(Vector.W * quaternion.Vector.W);
}
/// <summary>
/// Spherical linear interpolation between this quaternion and another
/// </summary>
/// <param name="quaternion"></param>
/// <param name="amount"></param>
public void Slerp(Quaternionf quaternion, float amount)
{
float angle = Dot(quaternion);
Quaternionf q1 = this;
if (angle < 0f)
{
q1.Negate();
angle *= -1f;
}
float scale;
float invscale;
if ((angle + 1f) > 0.05f)
{
if ((1f - angle) >= 0.05f)
{
// slerp
float theta = (float)System.Math.Acos(angle);
float invsintheta = 1f / (float)System.Math.Sin(theta);
scale = (float)System.Math.Sin(theta * (1f - amount)) * invsintheta;
invscale = (float)System.Math.Sin(theta * amount) * invsintheta;
}
else
{
// lerp
scale = 1f - amount;
invscale = amount;
}
}
else
{
quaternion.X = -q1.Y;
quaternion.Y = q1.X;
quaternion.Z = -q1.W;
quaternion.W = q1.Z;
scale = (float)System.Math.Sin(Utils.Pi * (0.5f - amount));
invscale = (float)System.Math.Sin(Utils.Pi * amount);
}
this = (q1 * scale) + (quaternion * invscale);
}
public bool ApproxEquals(Quaternionf quaternion, float tolerance)
{
Vector4f diff = Vector - quaternion.Vector;
return (diff.Length() <= tolerance);
}
public bool IsFinite()
{
return Utils.IsFinite(Vector.X) && Utils.IsFinite(Vector.Y) &&
Utils.IsFinite(Vector.Z) && Utils.IsFinite(Vector.W);
}
#endregion Public Methods
#region Overrides
public override int GetHashCode()
{
return Vector.X.GetHashCode() ^ Vector.Y.GetHashCode() ^
Vector.Z.GetHashCode() ^ Vector.W.GetHashCode();
}
public override string ToString()
{
return "<" + X + ", " + Y + ", " + Z + ", " + W + ">";
}
#endregion Overrides
#region Operators
public static bool operator ==(Quaternionf value1, Quaternionf value2)
{
return value1.X == value2.X && value1.Y == value2.Y && value1.Z == value2.Z && value1.W == value2.W;
}
public static bool operator !=(Quaternionf value1, Quaternionf value2)
{
return value1.X != value2.X || value1.Y != value2.Y || value1.Z != value2.Z || value1.W != value2.W;
}
public static Quaternionf operator +(Quaternionf value1, Quaternionf value2)
{
return new Quaternionf(
value1.X + value2.X,
value1.Y + value2.Y,
value1.Z + value2.Z,
value1.W + value2.W);
}
public static Quaternionf operator -(Quaternionf value)
{
value.Negate();
return value;
}
public static Quaternionf operator -(Quaternionf value1, Quaternionf value2)
{
return new Quaternionf(
value1.X - value2.X,
value1.Y - value2.Y,
value1.Z - value2.Z,
value1.W - value2.W);
}
public static Quaternionf operator *(Quaternionf q1, Quaternionf q2)
{
return new Quaternionf(
(q1.W * q2.X) + (q1.X * q2.W) + (q1.Y * q2.Z) - (q1.Z * q2.Y),
(q1.W * q2.Y) - (q1.X * q2.Z) + (q1.Y * q2.W) + (q1.Z * q2.X),
(q1.W * q2.Z) + (q1.X * q2.Y) - (q1.Y * q2.X) + (q1.Z * q2.W),
(q1.W * q2.W) - (q1.X * q2.X) - (q1.Y * q2.Y) - (q1.Z * q2.Z));
}
public static Quaternionf operator *(Quaternionf value, float scaleFactor)
{
Vector4f scale = new Vector4f(scaleFactor);
value.Vector *= scale;
return value;
}
public static Quaternionf operator /(Quaternionf q1, Quaternionf q2)
{
float x = q1.X;
float y = q1.Y;
float z = q1.Z;
float w = q1.W;
float q2lensq = q2.LengthSquared(); //num14
float ooq2lensq = 1f / q2lensq;
float x2 = -q2.X * ooq2lensq;
float y2 = -q2.Y * ooq2lensq;
float z2 = -q2.Z * ooq2lensq;
float w2 = q2.W * ooq2lensq;
return new Quaternionf(
((x * w2) + (x2 * w)) + (y * z2) - (z * y2),
((y * w2) + (y2 * w)) + (z * x2) - (x * z2),
((z * w2) + (z2 * w)) + (x * y2) - (y * x2),
(w * w2) - ((x * x2) + (y * y2)) + (z * z2));
}
#endregion Operators
}
}

150
Utils.cs Normal file
Просмотреть файл

@ -0,0 +1,150 @@
// Utils.cs
//
// Authors:
// The Mono.Xna Team (http://code.google.com/p/monoxna/)
//
// (C) The Mono.Xna Team
//
// Permission is hereby granted, free of charge, to any person obtaining
// a copy of this software and associated documentation files (the
// "Software"), to deal in the Software without restriction, including
// without limitation the rights to use, copy, modify, merge, publish,
// distribute, sublicense, and/or sell copies of the Software, and to
// permit persons to whom the Software is furnished to do so, subject to
// the following conditions:
//
// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
// LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
// WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
//
using System;
namespace Mono.Simd.Math
{
public static class Utils
{
public const float E = (float)System.Math.E;
public const float Log10E = 0.4342945f;
public const float Log2E = 1.442695f;
public const float Pi = (float)System.Math.PI;
public const float PiOver2 = (float)(System.Math.PI / 2.0);
public const float PiOver4 = (float)(System.Math.PI / 4.0);
public const float TwoPi = (float)(System.Math.PI * 2.0);
public static float Barycentric(float value1, float value2, float value3, float amount1, float amount2)
{
return value1 + (value2 - value1) * amount1 + (value3 - value1) * amount2;
}
public static float CatmullRom(float value1, float value2, float value3, float value4, float amount)
{
// Using formula from http://www.mvps.org/directx/articles/catmull/
// Internally using doubles not to lose precission
double amountSquared = amount * amount;
double amountCubed = amountSquared * amount;
return (float)(0.5 * (2.0 * value2 +
(value3 - value1) * amount +
(2.0 * value1 - 5.0 * value2 + 4.0 * value3 - value4) * amountSquared +
(3.0 * value2 - value1 - 3.0 * value3 + value4) * amountCubed));
}
/// <summary>
/// Clamp a given value between a range
/// </summary>
/// <param name="value">Value to clamp</param>
/// <param name="min">Minimum allowable value</param>
/// <param name="max">Maximum allowable value</param>
/// <returns>A value inclusively between lower and upper</returns>
public static float Clamp(float value, float min, float max)
{
// First we check to see if we're greater than the max
value = (value > max) ? max : value;
// Then we check to see if we're less than the min.
value = (value < min) ? min : value;
// There's no check to see if min > max.
return value;
}
public static float Distance(float value1, float value2)
{
return System.Math.Abs(value1 - value2);
}
public static float Hermite(float value1, float tangent1, float value2, float tangent2, float amount)
{
// All transformed to double not to lose precission
// Otherwise, for high numbers of param:amount the result is NaN instead of Infinity
double v1 = value1, v2 = value2, t1 = tangent1, t2 = tangent2, s = amount, result;
double sCubed = s * s * s;
double sSquared = s * s;
if (amount == 0f)
result = value1;
else if (amount == 1f)
result = value2;
else
result = (2d * v1 - 2d * v2 + t2 + t1) * sCubed +
(3d * v2 - 3d * v1 - 2d * t1 - t2) * sSquared +
t1 * s + v1;
return (float)result;
}
/// <summary>
/// Test if a single precision float is a finite number
/// </summary>
public static bool IsFinite(float value)
{
return !(Single.IsNaN(value) || Single.IsInfinity(value));
}
public static float Lerp(float value1, float value2, float amount)
{
return value1 + (value2 - value1) * amount;
}
public static float Max(float value1, float value2)
{
return System.Math.Max(value1, value2);
}
public static float Min(float value1, float value2)
{
return System.Math.Min(value1, value2);
}
public static float SmoothStep(float value1, float value2, float amount)
{
// It is expected that 0 < amount < 1
// If amount < 0, return value1
// If amount > 1, return value2
float result = Utils.Clamp(amount, 0f, 1f);
return Utils.Hermite(value1, 0f, value2, 0f, result);
}
public static float ToDegrees(float radians)
{
// This method uses double precission internally,
// though it returns single float
// Factor = 180 / pi
return (float)(radians * 57.295779513082320876798154814105);
}
public static float ToRadians(float degrees)
{
// This method uses double precission internally,
// though it returns single float
// Factor = pi / 180
return (float)(degrees * 0.017453292519943295769236907684886);
}
}
}

27
Vector2d.cs Normal file
Просмотреть файл

@ -0,0 +1,27 @@
// Vector2d.cs
//
// Authors:
// Jerry Maine (crashfourit@gmail.com)
// John Hurliman (john.hurliman@intel.com)
//
// (C) Jerry Maine & Intel Corporation
//
// Permission is hereby granted, free of charge, to any person obtaining
// a copy of this software and associated documentation files (the
// "Software"), to deal in the Software without restriction, including
// without limitation the rights to use, copy, modify, merge, publish,
// distribute, sublicense, and/or sell copies of the Software, and to
// permit persons to whom the Software is furnished to do so, subject to
// the following conditions:
//
// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
// LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
// WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
//

27
Vector2f.cs Normal file
Просмотреть файл

@ -0,0 +1,27 @@
// Vector2f.cs
//
// Authors:
// Jerry Maine (crashfourit@gmail.com)
// John Hurliman (john.hurliman@intel.com)
//
// (C) Jerry Maine & Intel Corporation
//
// Permission is hereby granted, free of charge, to any person obtaining
// a copy of this software and associated documentation files (the
// "Software"), to deal in the Software without restriction, including
// without limitation the rights to use, copy, modify, merge, publish,
// distribute, sublicense, and/or sell copies of the Software, and to
// permit persons to whom the Software is furnished to do so, subject to
// the following conditions:
//
// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
// LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
// WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
//

5
Vector3d.cs
Просмотреть файл

@ -1,9 +1,10 @@
// Vector3d.cs
//
// Author:
// Authors:
// Jerry Maine (crashfourit@gmail.com)
// John Hurliman (john.hurliman@intel.com)
//
// (C) Jerry Maine
// (C) Jerry Maine & Intel Corporation
//
// Permission is hereby granted, free of charge, to any person obtaining
// a copy of this software and associated documentation files (the

389
Vector3f.cs
Просмотреть файл

@ -1,9 +1,10 @@
// Vector3f.cs
// Vector3fNoSimd.cs
//
// Author:
// Authors:
// Jerry Maine (crashfourit@gmail.com)
// John Hurliman (john.hurliman@intel.com)
//
// (C) Jerry Maine
// (C) Jerry Maine & Intel Corporation
//
// Permission is hereby granted, free of charge, to any person obtaining
// a copy of this software and associated documentation files (the
@ -23,4 +24,384 @@
// LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
// WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
//
//
using System;
using System.Runtime.InteropServices;
namespace Mono.Simd.Math
{
[Serializable]
[StructLayout(LayoutKind.Explicit, Pack = 0, Size = 12)]
public struct Vector3f
{
[FieldOffset(0)]
public float X;
[FieldOffset(4)]
public float Y;
[FieldOffset(8)]
public float Z;
#region Properties
#endregion Properties
#region Constructors
public Vector3f(float x, float y, float z)
{
X = x;
Y = y;
Z = z;
}
public Vector3f(float value)
{
X = value;
Y = value;
Z = value;
}
public Vector3f(Vector3f vector)
{
X = vector.X;
Y = vector.Y;
Z = vector.Z;
}
#endregion Constructors
#region Public Methods
public void Negate()
{
X = -X;
Y = -Y;
Z = -Z;
}
public void Normalize()
{
const float MAG_THRESHOLD = 0.0000001f;
float factor = (float)System.Math.Sqrt(X * X + Y * Y + Z * Z);
if (factor > MAG_THRESHOLD)
{
factor = 1f / factor;
X *= factor;
Y *= factor;
Z *= factor;
}
else
{
X = 0f;
Y = 0f;
Z = 0f;
}
}
public float Length()
{
return (float)System.Math.Sqrt(X * X + Y * Y + Z * Z);
}
public float LengthSquared()
{
return X * X + Y * Y + Z * Z;
}
public bool ApproxEquals(Vector3f vec, float tolerance)
{
Vector3f diff = this - vec;
return (diff.Length() <= tolerance);
}
public bool IsFinite()
{
return (Utils.IsFinite(X) && Utils.IsFinite(Y) && Utils.IsFinite(Z));
}
public Vector3f Clamp(ref Vector3f min, ref Vector3f max)
{
return new Vector3f(
Utils.Clamp(X, min.X, max.X),
Utils.Clamp(Y, min.Y, max.Y),
Utils.Clamp(Z, min.Z, max.Z));
}
public Vector3f Cross(ref Vector3f vector)
{
return new Vector3f(
Y * vector.Z - vector.Y * Z,
Z * vector.X - vector.Z * X,
X * vector.Y - vector.X * Y);
}
public float Distance(ref Vector3f vector)
{
return (float)System.Math.Sqrt(DistanceSquared(ref vector));
}
public float DistanceSquared(ref Vector3f vector)
{
return
(X - vector.X) * (X - vector.X) +
(Y - vector.Y) * (Y - vector.Y) +
(Z - vector.Z) * (Z - vector.Z);
}
public float Dot(ref Vector3f vector)
{
return X * vector.X + Y * vector.Y + Z * vector.Z;
}
public Vector3f Lerp(ref Vector3f vector, float amount)
{
return new Vector3f(
Utils.Lerp(X, vector.X, amount),
Utils.Lerp(Y, vector.Y, amount),
Utils.Lerp(Z, vector.Z, amount));
}
public Vector3f Max(ref Vector3f vector)
{
return new Vector3f(
System.Math.Max(X, vector.X),
System.Math.Max(Y, vector.Y),
System.Math.Max(Z, vector.Z));
}
public Vector3f Min(ref Vector3f vector)
{
return new Vector3f(
System.Math.Min(X, vector.X),
System.Math.Min(Y, vector.Y),
System.Math.Min(Z, vector.Z));
}
/// <summary>
/// Calculate the rotation between this normalized directional vector
/// and a normalized target vector
/// </summary>
/// <param name="target">Target vector</param>
public Quaternionf RotationBetween(ref Vector3f target)
{
float dotProduct = Dot(ref target);
Vector3f crossProduct = Cross(ref target);
float magProduct = Length() * target.Length();
double angle = System.Math.Acos(dotProduct / magProduct);
crossProduct.Normalize();
float s = (float)System.Math.Sin(angle / 2d);
return new Quaternionf(
crossProduct.X * s,
crossProduct.Y * s,
crossProduct.Z * s,
(float)System.Math.Cos(angle / 2d));
}
/// <summary>
/// Interpolates between this vector and another using a cubic equation
/// </summary>
/// <param name="target">Target vector</param>
/// <param name="amount">Normalized amount to interpolate</param>
/// <returns>A vector in between this vector and the target</returns>
public Vector3f SmoothStep(ref Vector3f target, float amount)
{
return new Vector3f(
Utils.SmoothStep(X, target.X, amount),
Utils.SmoothStep(Y, target.Y, amount),
Utils.SmoothStep(Z, target.Z, amount));
}
public Vector3f Transform(ref Matrix4f matrix)
{
return new Vector3f(
(X * matrix.M11) + (Y * matrix.M21) + (Z * matrix.M31) + matrix.M41,
(X * matrix.M12) + (Y * matrix.M22) + (Z * matrix.M32) + matrix.M42,
(X * matrix.M13) + (Y * matrix.M23) + (Z * matrix.M33) + matrix.M43);
}
public Vector3f TransformNormal(ref Matrix4f matrix)
{
return new Vector3f(
(X * matrix.M11) + (Y * matrix.M21) + (Z * matrix.M31),
(X * matrix.M12) + (Y * matrix.M22) + (Z * matrix.M32),
(X * matrix.M13) + (Y * matrix.M23) + (Z * matrix.M33));
}
#endregion Public Methods
#region Overrides
public override int GetHashCode()
{
return X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode();
}
public override string ToString()
{
return "<" + X + ", " + Y + ", " + Z + ">";
}
[System.Runtime.CompilerServices.IndexerName ("Component")]
public float this[int index]
{
get
{
switch (index)
{
case 0:
return X;
case 1:
return Y;
case 2:
return Z;
default:
throw new IndexOutOfRangeException("index");
}
}
set
{
switch (index)
{
case 0:
X = value;
break;
case 1:
Y = value;
break;
case 2:
Z = value;
break;
default:
throw new IndexOutOfRangeException("index");
}
}
}
#endregion Overrides
#region Operators
public static bool operator ==(Vector3f value1, Vector3f value2)
{
return value1.X == value2.X && value1.Y == value2.Y && value1.Z == value2.Z;
}
public static bool operator !=(Vector3f value1, Vector3f value2)
{
return value1.X != value2.X || value1.Y != value2.Y || value1.Z != value2.Z;
}
public static Vector3f operator +(Vector3f value1, Vector3f value2)
{
return new Vector3f(
value1.X + value2.X,
value1.Y + value2.Y,
value1.Z + value2.Z);
}
public static Vector3f operator -(Vector3f value)
{
return value * -1f;
}
public static Vector3f operator -(Vector3f value1, Vector3f value2)
{
return new Vector3f(
value1.X - value2.X,
value1.Y - value2.Y,
value1.Z - value2.Z);
}
public static Vector3f operator *(Vector3f value1, Vector3f value2)
{
return new Vector3f(
value1.X * value2.X,
value1.Y * value2.Y,
value1.Z * value2.Z);
}
public static Vector3f operator *(Vector3f value, float scaleFactor)
{
return new Vector3f(
value.X * scaleFactor,
value.Y * scaleFactor,
value.Z * scaleFactor);
}
public static Vector3f operator *(Vector3f vec, Quaternionf rot)
{
Vector3f result;
result.X =
rot.W * rot.W * vec.X +
2f * rot.Y * rot.W * vec.Z -
2f * rot.Z * rot.W * vec.Y +
rot.X * rot.X * vec.X +
2f * rot.Y * rot.X * vec.Y +
2f * rot.Z * rot.X * vec.Z -
rot.Z * rot.Z * vec.X -
rot.Y * rot.Y * vec.X;
result.Y =
2f * rot.X * rot.Y * vec.X +
rot.Y * rot.Y * vec.Y +
2f * rot.Z * rot.Y * vec.Z +
2f * rot.W * rot.Z * vec.X -
rot.Z * rot.Z * vec.Y +
rot.W * rot.W * vec.Y -
2f * rot.X * rot.W * vec.Z -
rot.X * rot.X * vec.Y;
result.Z =
2f * rot.X * rot.Z * vec.X +
2f * rot.Y * rot.Z * vec.Y +
rot.Z * rot.Z * vec.Z -
2f * rot.W * rot.Y * vec.X -
rot.Y * rot.Y * vec.Z +
2f * rot.W * rot.X * vec.Y -
rot.X * rot.X * vec.Z +
rot.W * rot.W * vec.Z;
return result;
}
public static Vector3f operator *(Vector3f vector, Matrix4f matrix)
{
return vector.Transform(ref matrix);
}
public static Vector3f operator /(Vector3f value1, Vector3f value2)
{
return new Vector3f(
value1.X / value2.X,
value1.Y / value2.Y,
value1.Z / value2.Z);
}
public static Vector3f operator /(Vector3f value, float divider)
{
float oodivider = 1f / divider;
return new Vector3f(
value.X * oodivider,
value.Y * oodivider,
value.Z * oodivider);
}
#endregion Operators
/// <summary>A vector with a value of 0,0,0</summary>
public readonly static Vector3f Zero = new Vector3f();
/// <summary>A vector with a value of 1,1,1</summary>
public readonly static Vector3f One = new Vector3f(1f, 1f, 1f);
/// <summary>A vector with a value of -1,-1,-1</summary>
public readonly static Vector3f MinusOne = new Vector3f(-1f, -1f, -1f);
/// <summary>A unit vector facing forward (X axis), value 1,0,0</summary>
public readonly static Vector3f UnitX = new Vector3f(1f, 0f, 0f);
/// <summary>A unit vector facing left (Y axis), value 0,1,0</summary>
public readonly static Vector3f UnitY = new Vector3f(0f, 1f, 0f);
/// <summary>A unit vector facing up (Z axis), value 0,0,1</summary>
public readonly static Vector3f UnitZ = new Vector3f(0f, 0f, 1f);
}
}

5
Vector4d.cs
Просмотреть файл

@ -1,9 +1,10 @@
// Vector4d.cs
//
// Author:
// Authors:
// Jerry Maine (crashfourit@gmail.com)
// John Hurliman (john.hurliman@intel.com)
//
// (C) Jerry Maine
// (C) Jerry Maine & Intel Corporation
//
// Permission is hereby granted, free of charge, to any person obtaining
// a copy of this software and associated documentation files (the

55
Vector4f.cs
Просмотреть файл

@ -1,9 +1,10 @@
// Vector4f.cs
//
// Author:
// Authors:
// Jerry Maine (crashfourit@gmail.com)
// John Hurliman (john.hurliman@intel.com)
//
// (C) Jerry Maine
// (C) Jerry Maine & Intel Corporation
//
// Permission is hereby granted, free of charge, to any person obtaining
// a copy of this software and associated documentation files (the
@ -23,4 +24,52 @@
// LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
// WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
//
//
using System;
namespace Mono.Simd.Math
{
public static class Vector4fExtensions
{
public static void Negate(this Vector4f vec)
{
vec *= Vector4f.MinusOne;
}
public static void Normalize(this Vector4f vec)
{
Vector4f factor = vec * vec;
factor = factor.HorizontalAdd(factor);
factor = factor.HorizontalAdd(factor);
factor = factor.InvSqrt();
vec *= factor;
}
public static float Length(this Vector4f vec)
{
Vector4f length = vec * vec;
length = length.HorizontalAdd(length);
length = length.HorizontalAdd(length);
return length.Sqrt().X;
}
public static float LengthSquared(this Vector4f vec)
{
Vector4f length = vec * vec;
length = length.HorizontalAdd(length);
return length.HorizontalAdd(length).X;
}
public static bool ApproxEquals(this Vector4f vec, Vector4f vector, float tolerance)
{
Vector4f diff = vec - vector;
return (diff.Length() <= tolerance);
}
public static bool IsFinite(this Vector4f vec)
{
return (Utils.IsFinite(vec.X) && Utils.IsFinite(vec.Y) && Utils.IsFinite(vec.Z) && Utils.IsFinite(vec.W));
}
}
}