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 | |
parent | removed the OpenSim.Region.ScriptEngine.DotNetEngine reference from OpenSim.... (diff) | |
download | opensim-SC-2d5f5e2b32c5b8cc5bf6492964e06724b8935fd1.zip opensim-SC-2d5f5e2b32c5b8cc5bf6492964e06724b8935fd1.tar.gz opensim-SC-2d5f5e2b32c5b8cc5bf6492964e06724b8935fd1.tar.bz2 opensim-SC-2d5f5e2b32c5b8cc5bf6492964e06724b8935fd1.tar.xz |
applied Darok"s BulletXPlugin changes 003.patch
Diffstat (limited to 'libraries')
-rw-r--r-- | libraries/ModifiedBulletX/ModifiedBulletX.suo | bin | 138240 -> 138240 bytes | |||
-rw-r--r-- | libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/HeightfieldTerrainShape.cs | 360 | ||||
-rw-r--r-- | libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj | 1 | ||||
-rw-r--r-- | libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs | 11 | ||||
-rw-r--r-- | libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs | 91 |
5 files changed, 458 insertions, 5 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX.suo b/libraries/ModifiedBulletX/ModifiedBulletX.suo index 4f0acd9..6244108 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX.suo +++ b/libraries/ModifiedBulletX/ModifiedBulletX.suo | |||
Binary files differ | |||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/HeightfieldTerrainShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/HeightfieldTerrainShape.cs new file mode 100644 index 0000000..0f30f1f --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/HeightfieldTerrainShape.cs | |||
@@ -0,0 +1,360 @@ | |||
1 | /* | ||
2 | * WARNING!: this class is not in the original BulletX | ||
3 | * By the way it's based on the Bullet btHeightfieldTerrainShape: | ||
4 | * http://www.continuousphysics.com/Bullet/BulletFull/classbtHeightfieldTerrainShape.html | ||
5 | ***************************************************************************************** | ||
6 | * 3RD PARTY LICENSE. The next it's the original 3rd party lincense of Bullet: | ||
7 | * ---------------------------------------------------------------------------- | ||
8 | Bullet Continuous Collision Detection and Physics Library | ||
9 | Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ | ||
10 | |||
11 | This software is provided 'as-is', without any express or implied warranty. | ||
12 | In no event will the authors be held liable for any damages arising from the use of this software. | ||
13 | Permission is granted to anyone to use this software for any purpose, | ||
14 | including commercial applications, and to alter it and redistribute it freely, | ||
15 | subject to the following restrictions: | ||
16 | |||
17 | 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. | ||
18 | 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | * ------------------------------------------------------------------------------ | ||
21 | */ | ||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class HeightfieldTerrainShape : ConcaveShape | ||
30 | { | ||
31 | private Vector3 _localAabbMin; | ||
32 | private Vector3 _localAabbMax; | ||
33 | private Vector3 _localScaling = new Vector3(1f,1f,1f); | ||
34 | private int _width; | ||
35 | private int _length; | ||
36 | private float[] _heightfieldData; | ||
37 | private float _maxHeight; | ||
38 | private int _upAxis; | ||
39 | private bool _useFloatData; | ||
40 | private bool _flipQuadEdges; | ||
41 | private bool _useDiamondSubdivision = false; | ||
42 | private float _defaultCollisionMargin = 0.6f; | ||
43 | |||
44 | public HeightfieldTerrainShape(int width, int length, float[] heightfieldData, float maxHeight, | ||
45 | int upAxis, bool useFloatData, bool flipQuadEdges) | ||
46 | { | ||
47 | _width = width; | ||
48 | _length = length; | ||
49 | _heightfieldData = heightfieldData; | ||
50 | _maxHeight = maxHeight; | ||
51 | _upAxis = upAxis; | ||
52 | _useFloatData = useFloatData; | ||
53 | _flipQuadEdges = flipQuadEdges; | ||
54 | this.Margin = _defaultCollisionMargin; | ||
55 | |||
56 | float quantizationMargin = 1f; | ||
57 | |||
58 | //enlarge the AABB to avoid division by zero when initializing the quantization value | ||
59 | Vector3 clampValue = new Vector3(quantizationMargin, quantizationMargin, quantizationMargin); | ||
60 | Vector3 halfExtents = new Vector3(0, 0, 0); | ||
61 | |||
62 | switch (_upAxis) | ||
63 | { | ||
64 | case 0: | ||
65 | halfExtents.X = _maxHeight; | ||
66 | halfExtents.Y = _width; | ||
67 | halfExtents.Z = _length; | ||
68 | break; | ||
69 | case 1: | ||
70 | halfExtents.X = _width; | ||
71 | halfExtents.Y = _maxHeight; | ||
72 | halfExtents.Z = _length; | ||
73 | break; | ||
74 | case 2: | ||
75 | halfExtents.X = _width; | ||
76 | halfExtents.Y = _length; | ||
77 | halfExtents.Z = _maxHeight; | ||
78 | break; | ||
79 | default: | ||
80 | //need to get valid _upAxis | ||
81 | //btAssert(0); | ||
82 | throw new Exception("HeightfieldTerrainShape: need to get valid _upAxis"); | ||
83 | } | ||
84 | |||
85 | halfExtents *= 0.5f; | ||
86 | |||
87 | _localAabbMin = -halfExtents - clampValue; | ||
88 | _localAabbMax = halfExtents + clampValue; | ||
89 | //Vector3 aabbSize = new Vector3(); | ||
90 | //aabbSize = m_localAabbMax - m_localAabbMin; | ||
91 | |||
92 | } | ||
93 | |||
94 | protected Vector3 LocalAabbMin | ||
95 | { get { return _localAabbMin; } set { _localAabbMin = value; } } | ||
96 | protected Vector3 LocalAabbMax | ||
97 | { get { return _localAabbMax; } set { _localAabbMax = value; } } | ||
98 | public override string Name | ||
99 | { | ||
100 | get | ||
101 | { | ||
102 | return "HeightfieldTerrain"; | ||
103 | } | ||
104 | } | ||
105 | public override Vector3 LocalScaling | ||
106 | { | ||
107 | get | ||
108 | { | ||
109 | return _localScaling; | ||
110 | } | ||
111 | set | ||
112 | { | ||
113 | _localScaling = value; | ||
114 | } | ||
115 | } | ||
116 | public override float Margin | ||
117 | { | ||
118 | get | ||
119 | { | ||
120 | return base.Margin; | ||
121 | } | ||
122 | set | ||
123 | { | ||
124 | base.Margin = value; | ||
125 | } | ||
126 | } | ||
127 | public override BroadphaseNativeTypes ShapeType | ||
128 | { | ||
129 | get { return BroadphaseNativeTypes.Terrain; } | ||
130 | } | ||
131 | public Vector3 HalfExtents | ||
132 | { | ||
133 | get | ||
134 | { | ||
135 | Vector3 halfExtents = new Vector3(); | ||
136 | switch (_upAxis) | ||
137 | { | ||
138 | case 0: | ||
139 | halfExtents.X = 2f;//_maxHeight; | ||
140 | halfExtents.Y = _width; | ||
141 | halfExtents.Z = _length; | ||
142 | break; | ||
143 | case 1: | ||
144 | halfExtents.X = _width; | ||
145 | halfExtents.Y = 2f;// _maxHeight; | ||
146 | halfExtents.Z = _length; | ||
147 | break; | ||
148 | case 2: | ||
149 | halfExtents.X = _width; | ||
150 | halfExtents.Y = _length; | ||
151 | halfExtents.Z = 2f;// _maxHeight; | ||
152 | break; | ||
153 | default: | ||
154 | //need to get valid m_upAxis | ||
155 | //btAssert(0); | ||
156 | throw new Exception("HeightfieldTerrainShape: need to get valid _upAxis"); | ||
157 | //break; | ||
158 | } | ||
159 | halfExtents *= 0.5f; | ||
160 | return halfExtents; | ||
161 | } | ||
162 | } | ||
163 | |||
164 | public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) | ||
165 | { | ||
166 | //(void)callback; | ||
167 | //(void)aabbMax; | ||
168 | //(void)aabbMin; | ||
169 | |||
170 | //quantize the aabbMin and aabbMax, and adjust the start/end ranges | ||
171 | |||
172 | int[] quantizedAabbMin = new int[3]; | ||
173 | int[] quantizedAabbMax = new int[3]; | ||
174 | |||
175 | Vector3 localAabbMin = aabbMin * new Vector3(1f/_localScaling.X,1f/_localScaling.Y,1f/_localScaling.Z ); | ||
176 | Vector3 localAabbMax = aabbMax * new Vector3(1f/_localScaling.X,1f/_localScaling.Y,1f/_localScaling.Z); | ||
177 | |||
178 | quantizeWithClamp(ref quantizedAabbMin, localAabbMin); | ||
179 | quantizeWithClamp(ref quantizedAabbMax, localAabbMax); | ||
180 | |||
181 | |||
182 | |||
183 | int startX=0; | ||
184 | int endX=_width-1; | ||
185 | int startJ=0; | ||
186 | int endJ=_length-1; | ||
187 | |||
188 | switch(_upAxis) | ||
189 | { | ||
190 | case 0: | ||
191 | quantizedAabbMin[1]+=_width/2-1; | ||
192 | quantizedAabbMax[1]+=_width/2+1; | ||
193 | quantizedAabbMin[2]+=_length/2-1; | ||
194 | quantizedAabbMax[2]+=_length/2+1; | ||
195 | |||
196 | if (quantizedAabbMin[1]>startX) | ||
197 | startX = quantizedAabbMin[1]; | ||
198 | if (quantizedAabbMax[1]<endX) | ||
199 | endX = quantizedAabbMax[1]; | ||
200 | if (quantizedAabbMin[2]>startJ) | ||
201 | startJ = quantizedAabbMin[2]; | ||
202 | if (quantizedAabbMax[2]<endJ) | ||
203 | endJ = quantizedAabbMax[2]; | ||
204 | break; | ||
205 | case 1: | ||
206 | quantizedAabbMin[0]+=_width/2-1; | ||
207 | quantizedAabbMax[0]+=_width/2+1; | ||
208 | quantizedAabbMin[2]+=_length/2-1; | ||
209 | quantizedAabbMax[2]+=_length/2+1; | ||
210 | |||
211 | if (quantizedAabbMin[0]>startX) | ||
212 | startX = quantizedAabbMin[0]; | ||
213 | if (quantizedAabbMax[0]<endX) | ||
214 | endX = quantizedAabbMax[0]; | ||
215 | if (quantizedAabbMin[2]>startJ) | ||
216 | startJ = quantizedAabbMin[2]; | ||
217 | if (quantizedAabbMax[2]<endJ) | ||
218 | endJ = quantizedAabbMax[2]; | ||
219 | break; | ||
220 | case 2: | ||
221 | quantizedAabbMin[0]+=_width/2-1; | ||
222 | quantizedAabbMax[0]+=_width/2+1; | ||
223 | quantizedAabbMin[1]+=_length/2-1; | ||
224 | quantizedAabbMax[1]+=_length/2+1; | ||
225 | |||
226 | if (quantizedAabbMin[0]>startX) | ||
227 | startX = quantizedAabbMin[0]; | ||
228 | if (quantizedAabbMax[0]<endX) | ||
229 | endX = quantizedAabbMax[0]; | ||
230 | if (quantizedAabbMin[1]>startJ) | ||
231 | startJ = quantizedAabbMin[1]; | ||
232 | if (quantizedAabbMax[1]<endJ) | ||
233 | endJ = quantizedAabbMax[1]; | ||
234 | break; | ||
235 | default: | ||
236 | //need to get valid m_upAxis | ||
237 | throw new Exception("HeightfieldTerrainShape: need to get valid _upAxis"); | ||
238 | //break; | ||
239 | } | ||
240 | |||
241 | for(int j=startJ; j<endJ; j++) | ||
242 | { | ||
243 | for(int x=startX; x<endX; x++) | ||
244 | { | ||
245 | Vector3[] vertices = new Vector3[3]; | ||
246 | //if (m_flipQuadEdges || (m_useDiamondSubdivision && ((j + x) & 1))) | ||
247 | if (_flipQuadEdges || (_useDiamondSubdivision && (((j + x) & 1) > 0))) | ||
248 | { | ||
249 | //first triangle | ||
250 | getVertex(x,j,ref vertices[0]); | ||
251 | getVertex(x+1,j,ref vertices[1]); | ||
252 | getVertex(x+1,j+1,ref vertices[2]); | ||
253 | //callback->processTriangle(vertices,x,j); | ||
254 | callback.ProcessTriangle(vertices,x,j); | ||
255 | |||
256 | //second triangle | ||
257 | getVertex(x,j,ref vertices[0]); | ||
258 | getVertex(x+1,j+1,ref vertices[1]); | ||
259 | getVertex(x,j+1,ref vertices[2]); | ||
260 | //callback->processTriangle(vertices,x,j); | ||
261 | callback.ProcessTriangle(vertices, x, j); | ||
262 | } | ||
263 | else | ||
264 | { | ||
265 | //first triangle | ||
266 | getVertex(x,j,ref vertices[0]); | ||
267 | getVertex(x,j+1,ref vertices[1]); | ||
268 | getVertex(x+1,j,ref vertices[2]); | ||
269 | //callback->processTriangle(vertices,x,j); | ||
270 | callback.ProcessTriangle(vertices,x,j); | ||
271 | |||
272 | //second triangle | ||
273 | getVertex(x+1,j,ref vertices[0]); | ||
274 | getVertex(x,j+1,ref vertices[1]); | ||
275 | getVertex(x+1,j+1,ref vertices[2]); | ||
276 | //callback->processTriangle(vertices,x,j); | ||
277 | callback.ProcessTriangle(vertices,x,j); | ||
278 | } | ||
279 | } | ||
280 | } | ||
281 | } | ||
282 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | ||
283 | { | ||
284 | //aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); | ||
285 | //aabbMax = new Vector3(1e30f, 1e30f, 1e30f); | ||
286 | |||
287 | Vector3 halfExtents = (_localAabbMax - _localAabbMin) * _localScaling * 0.5f; | ||
288 | |||
289 | Vector3 center = t.Translation; | ||
290 | Vector3 extent = new Vector3(Math.Abs(halfExtents.X), Math.Abs(halfExtents.Y), Math.Abs(halfExtents.Z)); | ||
291 | extent += new Vector3(Margin, Margin, Margin); | ||
292 | |||
293 | aabbMin = center - extent; | ||
294 | aabbMax = center + extent; | ||
295 | } | ||
296 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | ||
297 | { | ||
298 | //moving concave objects not supported | ||
299 | inertia = new Vector3(); | ||
300 | } | ||
301 | public float getHeightFieldValue(int x,int y) | ||
302 | { | ||
303 | float val = 0f; | ||
304 | if (_useFloatData) | ||
305 | { | ||
306 | val = _heightfieldData[(y * _width) + x]; | ||
307 | } | ||
308 | else | ||
309 | { | ||
310 | //assume unsigned short int | ||
311 | int heightFieldValue = (int)_heightfieldData[(y * _width) + x]; | ||
312 | val = heightFieldValue * _maxHeight/65535f; | ||
313 | } | ||
314 | return val; | ||
315 | } | ||
316 | public void getVertex(int x,int y,ref Vector3 vertex) | ||
317 | { | ||
318 | if (x < 0) x = 0; | ||
319 | if (y < 0) y = 0; | ||
320 | if (x >= _width) x = _width - 1; | ||
321 | if (y >= _length) y = _length - 1; | ||
322 | float height = getHeightFieldValue(x,y); | ||
323 | switch(_upAxis) | ||
324 | { | ||
325 | case 0: | ||
326 | vertex.X = height; | ||
327 | vertex.Y = (- _width/2 ) + x; | ||
328 | vertex.Z = (- _length/2 ) + y; | ||
329 | break; | ||
330 | case 1: | ||
331 | vertex.X = (- _width/2 ) + x; | ||
332 | vertex.Y = height; | ||
333 | vertex.Z = (- _length/2 ) + y; | ||
334 | break; | ||
335 | case 2: | ||
336 | vertex.X = (- _width/2 ) + x; | ||
337 | vertex.Y = (- _length/2 ) + y; | ||
338 | vertex.Z = height; | ||
339 | break; | ||
340 | default: | ||
341 | //need to get valid m_upAxis | ||
342 | throw new Exception("HeightfieldTerrainShape: need to get valid _upAxis"); | ||
343 | //break; | ||
344 | } | ||
345 | vertex *= _localScaling; | ||
346 | } | ||
347 | public void quantizeWithClamp(ref int[] _out,Vector3 point) | ||
348 | { | ||
349 | Vector3 clampedPoint = point; | ||
350 | MathHelper.SetMax(ref clampedPoint,_localAabbMin); | ||
351 | MathHelper.SetMin(ref clampedPoint, _localAabbMax); | ||
352 | Vector3 v = clampedPoint; | ||
353 | |||
354 | _out[0] = (int)(v.X); | ||
355 | _out[1] = (int)(v.Y); | ||
356 | _out[2] = (int)(v.Z); | ||
357 | //correct for | ||
358 | } | ||
359 | } | ||
360 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj b/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj index 06b98bc..3aead70 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj | |||
@@ -78,6 +78,7 @@ | |||
78 | <Compile Include="Collision\CollisionShapes\CylinderShapeZ.cs" /> | 78 | <Compile Include="Collision\CollisionShapes\CylinderShapeZ.cs" /> |
79 | <Compile Include="Collision\CollisionShapes\EmptyShape.cs" /> | 79 | <Compile Include="Collision\CollisionShapes\EmptyShape.cs" /> |
80 | <Compile Include="Collision\CollisionShapes\FilteredCallback.cs" /> | 80 | <Compile Include="Collision\CollisionShapes\FilteredCallback.cs" /> |
81 | <Compile Include="Collision\CollisionShapes\HeightfieldTerrainShape.cs" /> | ||
81 | <Compile Include="Collision\CollisionShapes\InternalTriangleIndexCallback.cs" /> | 82 | <Compile Include="Collision\CollisionShapes\InternalTriangleIndexCallback.cs" /> |
82 | <Compile Include="Collision\CollisionShapes\LocalSupportVertexCallback.cs" /> | 83 | <Compile Include="Collision\CollisionShapes\LocalSupportVertexCallback.cs" /> |
83 | <Compile Include="Collision\CollisionShapes\MinkowskiSumShape.cs" /> | 84 | <Compile Include="Collision\CollisionShapes\MinkowskiSumShape.cs" /> |
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs index 22842ca..9b88ab3 100644 --- a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs +++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs | |||
@@ -573,7 +573,11 @@ namespace MonoXnaCompactMaths | |||
573 | 573 | ||
574 | public static Matrix operator /(Matrix matrix1, float divider) | 574 | public static Matrix operator /(Matrix matrix1, float divider) |
575 | { | 575 | { |
576 | throw new NotImplementedException(); | 576 | return new Matrix( |
577 | matrix1.M11 / divider, matrix1.M12 / divider, matrix1.M13 / divider, matrix1.M14 / divider, | ||
578 | matrix1.M21 / divider, matrix1.M22 / divider, matrix1.M23 / divider, matrix1.M24 / divider, | ||
579 | matrix1.M31 / divider, matrix1.M32 / divider, matrix1.M33 / divider, matrix1.M34 / divider, | ||
580 | matrix1.M41 / divider, matrix1.M42 / divider, matrix1.M43 / divider, matrix1.M44 / divider); | ||
577 | } | 581 | } |
578 | 582 | ||
579 | 583 | ||
@@ -658,7 +662,10 @@ namespace MonoXnaCompactMaths | |||
658 | 662 | ||
659 | public override string ToString() | 663 | public override string ToString() |
660 | { | 664 | { |
661 | throw new NotImplementedException(); | 665 | return "[(" + this.M11 + ", " + this.M12 + ", " + this.M13 + ", " + this.M14 + ")\n (" |
666 | + this.M21 + ", " + this.M22 + ", " + this.M23 + ", " + this.M24 + ")\n (" | ||
667 | + this.M31 + ", " + this.M32 + ", " + this.M33 + ", " + this.M34 + ")\n (" | ||
668 | + this.M41 + ", " + this.M42 + ", " + this.M43 + ", " + this.M44 + ")]"; | ||
662 | } | 669 | } |
663 | 670 | ||
664 | 671 | ||
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) |