123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431 |
- #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();
- }
- }
- }
|