aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs')
-rw-r--r--libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs91
1 files changed, 88 insertions, 3 deletions
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs
index b4f1873..b6d9d34 100644
--- a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs
+++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs
@@ -90,7 +90,91 @@ namespace MonoXnaCompactMaths
90 90
91 public static Quaternion CreateFromRotationMatrix(Matrix matrix) 91 public static Quaternion CreateFromRotationMatrix(Matrix matrix)
92 { 92 {
93 throw new NotImplementedException(); 93 float Omega2 = matrix.M44;
94 if (!isAprox(Omega2, 1f))
95 {
96 //"Normalize" the Rotation matrix. Norma = M44 = Omega2
97 matrix = matrix / Omega2;
98 }
99 //Deducted from: public static Matrix CreateFromQuaternion(Quaternion quaternion)
100 float lambda1pos, lambda2pos, lambda3pos, lambda1neg, lambda2neg, lambda3neg;
101 lambda1pos = (1f - matrix.M11 + matrix.M23 + matrix.M32) / 2f;
102 lambda2pos = (1f - matrix.M22 + matrix.M13 + matrix.M31) / 2f;
103 lambda3pos = (1f - matrix.M33 + matrix.M12 + matrix.M21) / 2f;
104 lambda1neg = (1f - matrix.M11 - matrix.M23 - matrix.M32) / 2f;
105 lambda2neg = (1f - matrix.M22 - matrix.M13 - matrix.M31) / 2f;
106 lambda3neg = (1f - matrix.M33 - matrix.M12 - matrix.M21) / 2f;
107
108 //lambadIS = (qJ + s*qK)^2
109 //q0 = w | q1 = x | q2 = y, q3 = z
110 //Every value of qI (I=1,2,3) has 4 possible values cause the sqrt
111 float[] x = new float[4]; float[] y = new float[4]; float[] z = new float[4];
112 float[] sig1 = {1f, 1f, -1f, -1f};
113 float[] sig2 = {1f, -1f, 1f, -1f};
114 for (int i = 0; i < 4; i++)
115 {
116 x[i] = (sig1[i] * (float)Math.Sqrt(lambda1pos) + sig2[i] * (float)Math.Sqrt(lambda1neg)) / 2f;
117 y[i] = (sig1[i] * (float)Math.Sqrt(lambda2pos) + sig2[i] * (float)Math.Sqrt(lambda2neg)) / 2f;
118 z[i] = (sig1[i] * (float)Math.Sqrt(lambda3pos) + sig2[i] * (float)Math.Sqrt(lambda3neg)) / 2f;
119 }
120
121 //Only a set of x, y, z are the corrects values. So it requires testing
122 int li_i=0, li_j=0, li_k=0;
123 bool lb_testL1P, lb_testL2P, lb_testL3P, lb_testL1N, lb_testL2N, lb_testL3N;
124 bool lb_superLambda = false;
125 while((li_i<4)&&(!lb_superLambda))
126 {
127 while ((li_j < 4) && (!lb_superLambda))
128 {
129 while ((li_k < 4) && (!lb_superLambda))
130 {
131 lb_testL1P = isAprox((float)(
132 Math.Pow((double)(y[li_j] + z[li_k]), 2.0)), lambda1pos);
133 lb_testL2P = isAprox((float)(
134 Math.Pow((double)(x[li_i] + z[li_k]), 2.0)), lambda2pos);
135 lb_testL3P = isAprox((float)(
136 Math.Pow((double)(x[li_i] + y[li_j]), 2.0)), lambda3pos);
137 lb_testL1N = isAprox((float)(
138 Math.Pow((double)(y[li_j] - z[li_k]), 2.0)), lambda1neg);
139 lb_testL2N = isAprox((float)(
140 Math.Pow((double)(x[li_i] - z[li_k]), 2.0)), lambda2neg);
141 lb_testL3N = isAprox((float)(
142 Math.Pow((double)(x[li_i] - y[li_j]), 2.0)), lambda3neg);
143
144 lb_superLambda = (lb_testL1P && lb_testL2P && lb_testL3P
145 && lb_testL1N && lb_testL2N && lb_testL3N);
146
147 if (!lb_superLambda) li_k++;
148 }
149 if (!lb_superLambda) li_j++;
150 }
151 if (!lb_superLambda) li_i++;
152 }
153
154 Quaternion q = new Quaternion();
155
156 if (lb_superLambda)
157 {
158 q.X = x[li_i]; q.Y = y[li_j]; q.Z = z[li_k];
159 q.W = (matrix.M12 - 2f * q.X * q.Y) / (2f * q.Z);
160
161 if (!isAprox(Omega2, 1f))
162 {
163 if (Omega2 < 0) throw new Exception("Quaternion.CreateFromRotationMatrix: Omega2 is negative!");
164 q = q * (float)Math.Sqrt(Omega2);//2 possibles values (+/-). For now only 1.
165 }
166 }
167 else
168 {
169 q = Quaternion.identity;
170 }
171
172 return q;
173 }
174 private static float floatError = 0.000001f;
175 private static bool isAprox(float test, float realValue)
176 {
177 return (((realValue * (1f - floatError)) <= test) && (test <= (realValue * (1f + floatError))));
94 } 178 }
95 179
96 180
@@ -317,7 +401,8 @@ namespace MonoXnaCompactMaths
317 401
318 public static Quaternion operator *(Quaternion quaternion1, float scaleFactor) 402 public static Quaternion operator *(Quaternion quaternion1, float scaleFactor)
319 { 403 {
320 throw new NotImplementedException(); 404 return new Quaternion(quaternion1.X / scaleFactor, quaternion1.Y / scaleFactor,
405 quaternion1.Z / scaleFactor, quaternion1.W / scaleFactor);
321 } 406 }
322 407
323 408
@@ -335,7 +420,7 @@ namespace MonoXnaCompactMaths
335 420
336 public override string ToString() 421 public override string ToString()
337 { 422 {
338 throw new NotImplementedException(); 423 return "(" + this.X + ", " + this.Y + ", " + this.Z + ", " + this.W + ")";
339 } 424 }
340 425
341 private static void Conjugate(ref Quaternion quaternion, out Quaternion result) 426 private static void Conjugate(ref Quaternion quaternion, out Quaternion result)