432 lines
14 KiB
C#
432 lines
14 KiB
C#
#region License
|
|
/*
|
|
MIT License
|
|
Copyright © 2006 The Mono.Xna Team
|
|
|
|
All rights reserved.
|
|
|
|
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.
|
|
*/
|
|
#endregion License
|
|
|
|
using System;
|
|
using System.ComponentModel;
|
|
|
|
namespace MonoXnaCompactMaths
|
|
{
|
|
[Serializable]
|
|
//[TypeConverter(typeof(QuaternionConverter))]
|
|
public struct Quaternion : IEquatable<Quaternion>
|
|
{
|
|
public float X;
|
|
public float Y;
|
|
public float Z;
|
|
public float W;
|
|
static Quaternion identity = new Quaternion(0, 0, 0, 1);
|
|
|
|
|
|
public Quaternion(float x, float y, float z, float w)
|
|
{
|
|
this.X = x;
|
|
this.Y = y;
|
|
this.Z = z;
|
|
this.W = w;
|
|
}
|
|
|
|
|
|
public Quaternion(Vector3 vectorPart, float scalarPart)
|
|
{
|
|
this.X = vectorPart.X;
|
|
this.Y = vectorPart.Y;
|
|
this.Z = vectorPart.Z;
|
|
this.W = scalarPart;
|
|
}
|
|
|
|
public static Quaternion Identity
|
|
{
|
|
get{ return identity; }
|
|
}
|
|
|
|
|
|
public static Quaternion Add(Quaternion quaternion1, Quaternion quaternion2)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static void Add(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static Quaternion CreateFromAxisAngle(Vector3 axis, float angle)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static void CreateFromAxisAngle(ref Vector3 axis, float angle, out Quaternion result)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static Quaternion CreateFromRotationMatrix(Matrix matrix)
|
|
{
|
|
float Omega2 = matrix.M44;
|
|
if (!isAprox(Omega2, 1f))
|
|
{
|
|
//"Normalize" the Rotation matrix. Norma = M44 = Omega2
|
|
matrix = matrix / Omega2;
|
|
}
|
|
//Deducted from: public static Matrix CreateFromQuaternion(Quaternion quaternion)
|
|
float lambda1pos, lambda2pos, lambda3pos, lambda1neg, lambda2neg, lambda3neg;
|
|
lambda1pos = (1f - matrix.M11 + matrix.M23 + matrix.M32) / 2f;
|
|
lambda2pos = (1f - matrix.M22 + matrix.M13 + matrix.M31) / 2f;
|
|
lambda3pos = (1f - matrix.M33 + matrix.M12 + matrix.M21) / 2f;
|
|
lambda1neg = (1f - matrix.M11 - matrix.M23 - matrix.M32) / 2f;
|
|
lambda2neg = (1f - matrix.M22 - matrix.M13 - matrix.M31) / 2f;
|
|
lambda3neg = (1f - matrix.M33 - matrix.M12 - matrix.M21) / 2f;
|
|
|
|
//lambadIS = (qJ + s*qK)^2
|
|
//q0 = w | q1 = x | q2 = y, q3 = z
|
|
//Every value of qI (I=1,2,3) has 4 possible values cause the sqrt
|
|
float[] x = new float[4]; float[] y = new float[4]; float[] z = new float[4];
|
|
float[] sig1 = {1f, 1f, -1f, -1f};
|
|
float[] sig2 = {1f, -1f, 1f, -1f};
|
|
for (int i = 0; i < 4; i++)
|
|
{
|
|
x[i] = (sig1[i] * (float)Math.Sqrt(lambda1pos) + sig2[i] * (float)Math.Sqrt(lambda1neg)) / 2f;
|
|
y[i] = (sig1[i] * (float)Math.Sqrt(lambda2pos) + sig2[i] * (float)Math.Sqrt(lambda2neg)) / 2f;
|
|
z[i] = (sig1[i] * (float)Math.Sqrt(lambda3pos) + sig2[i] * (float)Math.Sqrt(lambda3neg)) / 2f;
|
|
}
|
|
|
|
//Only a set of x, y, z are the corrects values. So it requires testing
|
|
int li_i=0, li_j=0, li_k=0;
|
|
bool lb_testL1P, lb_testL2P, lb_testL3P, lb_testL1N, lb_testL2N, lb_testL3N;
|
|
bool lb_superLambda = false;
|
|
while((li_i<4)&&(!lb_superLambda))
|
|
{
|
|
while ((li_j < 4) && (!lb_superLambda))
|
|
{
|
|
while ((li_k < 4) && (!lb_superLambda))
|
|
{
|
|
lb_testL1P = isAprox((float)(
|
|
Math.Pow((double)(y[li_j] + z[li_k]), 2.0)), lambda1pos);
|
|
lb_testL2P = isAprox((float)(
|
|
Math.Pow((double)(x[li_i] + z[li_k]), 2.0)), lambda2pos);
|
|
lb_testL3P = isAprox((float)(
|
|
Math.Pow((double)(x[li_i] + y[li_j]), 2.0)), lambda3pos);
|
|
lb_testL1N = isAprox((float)(
|
|
Math.Pow((double)(y[li_j] - z[li_k]), 2.0)), lambda1neg);
|
|
lb_testL2N = isAprox((float)(
|
|
Math.Pow((double)(x[li_i] - z[li_k]), 2.0)), lambda2neg);
|
|
lb_testL3N = isAprox((float)(
|
|
Math.Pow((double)(x[li_i] - y[li_j]), 2.0)), lambda3neg);
|
|
|
|
lb_superLambda = (lb_testL1P && lb_testL2P && lb_testL3P
|
|
&& lb_testL1N && lb_testL2N && lb_testL3N);
|
|
|
|
if (!lb_superLambda) li_k++;
|
|
}
|
|
if (!lb_superLambda) li_j++;
|
|
}
|
|
if (!lb_superLambda) li_i++;
|
|
}
|
|
|
|
Quaternion q = new Quaternion();
|
|
|
|
if (lb_superLambda)
|
|
{
|
|
q.X = x[li_i]; q.Y = y[li_j]; q.Z = z[li_k];
|
|
q.W = (matrix.M12 - 2f * q.X * q.Y) / (2f * q.Z);
|
|
|
|
if (!isAprox(Omega2, 1f))
|
|
{
|
|
if (Omega2 < 0) throw new Exception("Quaternion.CreateFromRotationMatrix: Omega2 is negative!");
|
|
q = q * (float)Math.Sqrt(Omega2);//2 possibles values (+/-). For now only 1.
|
|
}
|
|
}
|
|
else
|
|
{
|
|
q = Quaternion.identity;
|
|
}
|
|
|
|
return q;
|
|
}
|
|
private static float floatError = 0.000001f;
|
|
private static bool isAprox(float test, float realValue)
|
|
{
|
|
return (((realValue * (1f - floatError)) <= test) && (test <= (realValue * (1f + floatError))));
|
|
}
|
|
|
|
|
|
public static void CreateFromRotationMatrix(ref Matrix matrix, out Quaternion result)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static Quaternion Divide(Quaternion quaternion1, Quaternion quaternion2)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static void Divide(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static float Dot(Quaternion quaternion1, Quaternion quaternion2)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static void Dot(ref Quaternion quaternion1, ref Quaternion quaternion2, out float result)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public override bool Equals(object obj)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public bool Equals(Quaternion other)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public override int GetHashCode()
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static Quaternion Inverse(Quaternion quaternion)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static void Inverse(ref Quaternion quaternion, out Quaternion result)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public float Length()
|
|
{
|
|
//---
|
|
return (float)Math.Sqrt(Math.Pow(this.W, 2.0) + Math.Pow(this.X, 2.0) + Math.Pow(this.Y, 2.0) + Math.Pow(this.Z, 2.0));
|
|
//---
|
|
//throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public float LengthSquared()
|
|
{
|
|
//---
|
|
return (float)(Math.Pow(this.W, 2.0) + Math.Pow(this.X, 2.0) + Math.Pow(this.Y, 2.0) + Math.Pow(this.Z, 2.0));
|
|
//---
|
|
//throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static Quaternion Lerp(Quaternion quaternion1, Quaternion quaternion2, float amount)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static void Lerp(ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static Quaternion Slerp(Quaternion quaternion1, Quaternion quaternion2, float amount)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static void Slerp(ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static Quaternion Subtract(Quaternion quaternion1, Quaternion quaternion2)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static void Subtract(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static Quaternion Multiply(Quaternion quaternion1, Quaternion quaternion2)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static Quaternion Multiply(Quaternion quaternion1, float scaleFactor)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static void Multiply(ref Quaternion quaternion1, float scaleFactor, out Quaternion result)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static void Multiply(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static Quaternion Negate(Quaternion quaternion)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static void Negate(ref Quaternion quaternion, out Quaternion result)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public void Normalize()
|
|
{
|
|
//---
|
|
this = Normalize(this);
|
|
//---
|
|
//throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static Quaternion Normalize(Quaternion quaternion)
|
|
{
|
|
//---
|
|
return quaternion / quaternion.Length();
|
|
//---
|
|
//throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static void Normalize(ref Quaternion quaternion, out Quaternion result)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static Quaternion operator +(Quaternion quaternion1, Quaternion quaternion2)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static Quaternion operator /(Quaternion quaternion1, Quaternion quaternion2)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
public static Quaternion operator /(Quaternion quaternion, float factor)
|
|
{
|
|
quaternion.W /= factor;
|
|
quaternion.X /= factor;
|
|
quaternion.Y /= factor;
|
|
quaternion.Z /= factor;
|
|
return quaternion;
|
|
}
|
|
|
|
public static bool operator ==(Quaternion quaternion1, Quaternion quaternion2)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static bool operator !=(Quaternion quaternion1, Quaternion quaternion2)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static Quaternion operator *(Quaternion quaternion1, Quaternion quaternion2)
|
|
{
|
|
//---
|
|
//Grassmann product
|
|
Quaternion quaternionProduct = new Quaternion();
|
|
|
|
quaternionProduct.W = quaternion1.W * quaternion2.W - quaternion1.X * quaternion2.X - quaternion1.Y * quaternion2.Y - quaternion1.Z * quaternion2.Z;
|
|
quaternionProduct.X = quaternion1.W * quaternion2.X + quaternion1.X * quaternion2.W + quaternion1.Y * quaternion2.Z - quaternion1.Z * quaternion2.Y;
|
|
quaternionProduct.Y = quaternion1.W * quaternion2.Y - quaternion1.X * quaternion2.Z + quaternion1.Y * quaternion2.W + quaternion1.Z * quaternion2.X;
|
|
quaternionProduct.Z = quaternion1.W * quaternion2.Z + quaternion1.X * quaternion2.Y - quaternion1.Y * quaternion2.X + quaternion1.Z * quaternion2.W;
|
|
return quaternionProduct;
|
|
//---
|
|
//throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static Quaternion operator *(Quaternion quaternion1, float scaleFactor)
|
|
{
|
|
return new Quaternion(quaternion1.X / scaleFactor, quaternion1.Y / scaleFactor,
|
|
quaternion1.Z / scaleFactor, quaternion1.W / scaleFactor);
|
|
}
|
|
|
|
|
|
public static Quaternion operator -(Quaternion quaternion1, Quaternion quaternion2)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public static Quaternion operator -(Quaternion quaternion)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
|
|
public override string ToString()
|
|
{
|
|
return "(" + this.X + ", " + this.Y + ", " + this.Z + ", " + this.W + ")";
|
|
}
|
|
|
|
private static void Conjugate(ref Quaternion quaternion, out Quaternion result)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
}
|
|
}
|