diff options
author | dan miller | 2007-10-07 14:40:02 +0000 |
---|---|---|
committer | dan miller | 2007-10-07 14:40:02 +0000 |
commit | 2d5f5e2b32c5b8cc5bf6492964e06724b8935fd1 (patch) | |
tree | b76a0d37987e75822ea0c3b3cff914a495548054 /libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs | |
parent | removed the OpenSim.Region.ScriptEngine.DotNetEngine reference from OpenSim.... (diff) | |
download | opensim-SC_OLD-2d5f5e2b32c5b8cc5bf6492964e06724b8935fd1.zip opensim-SC_OLD-2d5f5e2b32c5b8cc5bf6492964e06724b8935fd1.tar.gz opensim-SC_OLD-2d5f5e2b32c5b8cc5bf6492964e06724b8935fd1.tar.bz2 opensim-SC_OLD-2d5f5e2b32c5b8cc5bf6492964e06724b8935fd1.tar.xz |
applied Darok"s BulletXPlugin changes 003.patch
Diffstat (limited to '')
-rw-r--r-- | libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs | 91 |
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) |