- Main Page
- Packages
- Classes
- Files

- File List
- File Members

# D:/guillaume/guillara/3D/DXGfx/Backup/DXGfxLib/MathUtil.cs

Go to the documentation of this file.

    00001 //BSD License
    00002 //DXGfx® - http://www.eteractions.com
    00003 //Copyright (c) 2005
    00004 //by Guillaume Randon
    00005 //Permission is hereby granted, free of charge, to any person obtaining a copy of this software
    00006 //and associated documentation files (the "Software"), to deal in the Software without restriction,
    00007 //including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
    00008 //and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so,
    00009 //subject to the following conditions:
    00010 //The above copyright notice and this permission notice shall be included in all copies or substantial
    00011 //portions of the Software.
    00012 //THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
    00013 //INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE
    00014 //AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
    00015 //DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    00016 //OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
    00017 using System;
    00018 using System.Collections.Specialized;
    00019 using System.Collections.Generic;
    00020 using System.Text;
    00021 using Microsoft.DirectX;
    00022 using Microsoft.DirectX.Direct3D;
    00023 using System.Drawing;
    00024
    00025 namespace DXGfxLib
    00026 {
    00030     public class DrawableBBox
    00031     {
    00032         VertexBuffer vBuff;
    00033         IndexBuffer iBuff;
    00034
    00035         public DrawableBBox(Device d3ddevice, BBox bbox)
    00036         {
    00037             CustomVertex.PositionOnly[] points = new CustomVertex.PositionOnly[8];
    00038
    00039             Vector3[] submits = bbox.BuildSumits();
    00040
    00041             for (int i = 0; i < submits.Length; i++)
    00042             {
    00043                 points[i].Position = submits[i];
    00044             }
    00045
    00046             vBuff = new VertexBuffer(typeof(CustomVertex.PositionOnly), 8, d3ddevice, Usage.Dynamic,
    00047                 CustomVertex.PositionOnly.Format,
    00048                 Pool.SystemMemory);
    00049
    00050             GraphicsStream vbuffStm = vBuff.Lock(0, 0, 0);
    00051             vbuffStm.Write(points);
    00052             vBuff.Unlock();
    00053
    00054             Int16[] indices = { 0, 1, 1, 2, 2, 3, 3, 0,
    00055                                 4, 5, 5, 6, 6, 7, 7, 4,
    00056                                 0, 4, 1, 5, 2, 6, 3, 7};
    00057
    00058
    00059             iBuff = new IndexBuffer(d3ddevice, 24 * sizeof(Int16), Usage.Dynamic, Pool.SystemMemory, true);
    00060
    00061             GraphicsStream ibuffStm = iBuff.Lock(0, 0, 0);
    00062             ibuffStm.Write(indices);
    00063             iBuff.Unlock();
    00064         }
    00065
    00066         public void Draw(Device d3ddevice)
    00067         {
    00068             d3ddevice.Indices = iBuff;
    00069             d3ddevice.SetStreamSource(0, vBuff, 0);
    00070             d3ddevice.VertexFormat = CustomVertex.PositionOnly.Format;
    00071             //d3ddevice.DrawPrimitives(PrimitiveType.PointList, 0, 8);
    00072             d3ddevice.DrawIndexedPrimitives(PrimitiveType.LineList, 0, 0, 8, 0, 12);
    00073         }
    00074     }
    00075
    00079     public class BBox
    00080     {
    00087         public Vector3 vMin;
    00088
    00092         public Vector3 vMax;
    00093
    00098         public Vector3 Center
    00099         {
    00100             get
    00101             {
    00102                 return new Vector3(vMax.X - vMin.X, vMax.Y - vMin.Y, vMax.Z - vMin.Z);
    00103             }
    00104         }
    00105
    00113         public BBox()
    00114         {
    00115             vMin = new Vector3(-0.0005f, -0.0005f, -0.0005f);
    00116             vMax = new Vector3(0.0005f, 0.0005f, 0.0005f);
    00117         }
    00118
    00119         public void Reset()
    00120         {
    00121             vMin = new Vector3(-0.0005f, -0.0005f, -0.0005f);
    00122             vMax = new Vector3(0.0005f, 0.0005f, 0.0005f);
    00123         }
    00124
    00129         public BBox(BBox bbox)
    00130         {
    00131             vMin = bbox.vMin;
    00132             vMax = bbox.vMax;
    00133         }
    00134
    00140         public BBox(Vector3 pos, float size)
    00141         {
    00142             vMin.X = pos.X - 0.5f * size;
    00143             vMin.Y = pos.Y - 0.5f * size;
    00144             vMin.Z = pos.Z - 0.5f * size;
    00145
    00146             vMax.X = pos.X + 0.5f * size;
    00147             vMax.Y = pos.Y + 0.5f * size;
    00148             vMax.Z = pos.Z + 0.5f * size;
    00149         }
    00150
    00158         public static BBox Transform(BBox bbox, Matrix mat)
    00159         {
    00160             BBox bboxRes = new BBox();
    00161
    00162             bboxRes.vMin = Vector3.TransformCoordinate(bbox.vMin, mat);
    00163             bboxRes.vMax = Vector3.TransformCoordinate(bbox.vMax, mat);
    00164
    00165             return bboxRes;
    00166         }
    00167
    00173         public BBox Transform(Matrix mat)
    00174         {
    00175             this.vMin.TransformCoordinate(mat);
    00176             this.vMax.TransformCoordinate(mat);
    00177
    00178             return this;
    00179         }
    00180
    00185         public Vector3 Size()
    00186         {
    00187             return (new Vector3(System.Math.Abs(vMax.X - vMin.X), System.Math.Abs(vMax.Y - vMin.Y), System.Math.Abs(vMax.Z - vMin.Z)));
    00188         }
    00189
    00194         public Vector3 MinPoint()
    00195         {
    00196             return (new Vector3((vMax.X - vMin.X) * 0.5f + vMin.X,
    00197                 (vMax.Y - vMin.Y) * 0.5f + vMin.Y,
    00198                 (vMax.Z - vMin.Z) * 0.5f + vMin.Y));
    00199         }
    00200
    00201         public float MaxAAXZSize()
    00202         {
    00203             Vector3 diag = vMax - vMin;
    00204             return System.Math.Max(Vector3.Dot(diag, new Vector3(1.0f, 0.0f, 0.0f)), Vector3.Dot(diag, new Vector3(0.0f, 0.0f, 1.0f)));
    00205         }
    00206
    00212         public Vector3[] BuildSumits()
    00213         {
    00214             Vector3[] summits = new Vector3[8];
    00215
    00216             summits[0].X = vMin.X;
    00217             summits[0].Y = vMin.Y;
    00218             summits[0].Z = vMin.Z;
    00219
    00220             summits[1].X = vMax.X;
    00221             summits[1].Y = vMin.Y;
    00222             summits[1].Z = vMin.Z;
    00223
    00224             summits[2].X = vMax.X;
    00225             summits[2].Y = vMin.Y;
    00226             summits[2].Z = vMax.Z;
    00227
    00228             summits[3].X = vMin.X;
    00229             summits[3].Y = vMin.Y;
    00230             summits[3].Z = vMax.Z;
    00231
    00232             summits[4].X = vMin.X;
    00233             summits[4].Y = vMax.Y;
    00234             summits[4].Z = vMin.Z;
    00235
    00236             summits[5].X = vMax.X;
    00237             summits[5].Y = vMax.Y;
    00238             summits[5].Z = vMin.Z;
    00239
    00240             summits[6].X = vMax.X;
    00241             summits[6].Y = vMax.Y;
    00242             summits[6].Z = vMax.Z;
    00243
    00244             summits[7].X = vMin.X;
    00245             summits[7].Y = vMax.Y;
    00246             summits[7].Z = vMax.Z;
    00247
    00248             return summits;
    00249         }
    00250
    00259         public BBox BuildSwepingAABBox(Vector3 linVel, float elapsedTime)
    00260         {
    00261             return new AABBox(this, linVel, elapsedTime);
    00262         }
    00263
    00272         public bool CheckCollision(BBox bbox)
    00273         {
    00274             BBoxPlanes planes = new BBoxPlanes(this);
    00275
    00276
    00277             return planes.CheckCollision(bbox);
    00278         }
    00279
    00285         public bool Collide(BBox bbox)
    00286         {
    00287             AABBox aabb1 = new AABBox(this);
    00288             AABBox aabb2 = new AABBox(bbox);
    00289
    00290             return aabb1.Collide(aabb2);
    00291         }
    00292
    00297         public void Scale(Vector3 scale)
    00298         {
    00299             Vector3 center = Center;
    00300
    00301             Vector3 newSize = new Vector3(Size().X * scale.X, Size().Y * scale.Y, Size().Z * scale.Z);
    00302
    00303             vMin = center - 0.5f * newSize;
    00304             vMax = center + 0.5f * newSize;
    00305         }
    00306     }
    00307
    00311     public enum planeRelativePosition { front, back, intersect };
    00312
    00321     public class AABBox : BBox
    00322     {
    00326         public AABBox() : base()
    00327         {
    00328         }
    00329
    00334         public AABBox(BBox bbox)
    00335         {
    00336             Vector3[] summits = bbox.BuildSumits();
    00337
    00338             vMin.X = MathUtil.GetSmallest(new float[] { summits[0].X, summits[1].X,
    00339                                                                                                                 summits[2].X, summits[3].X,
    00340                                                                                                                 summits[4].X, summits[5].X,
    00341                                                                                                                 summits[6].X, summits[7].X});
    00342             vMin.Y = MathUtil.GetSmallest(new float[] { summits[0].Y, summits[1].Y,
    00343                                                                                                                 summits[2].Y, summits[3].Y,
    00344                                                                                                                 summits[4].Y, summits[5].Y,
    00345                                                                                                                 summits[6].Y, summits[7].Y});
    00346             vMin.Z = MathUtil.GetSmallest(new float[] { summits[0].Z, summits[1].Z,
    00347                                                                                                                 summits[2].Z, summits[3].Z,
    00348                                                                                                                 summits[4].Z, summits[5].Z,
    00349                                                                                                                 summits[6].Z, summits[7].Z});
    00350
    00351             vMax.X = MathUtil.GetHighest(new float[] { summits[0].X, summits[1].X,
    00352                                                                                                            summits[2].X, summits[3].X,
    00353                                                                                                            summits[4].X, summits[5].X,
    00354                                                                                                            summits[6].X, summits[7].X});
    00355             vMax.Y = MathUtil.GetHighest(new float[] { summits[0].Y, summits[1].Y,
    00356                                                                                                            summits[2].Y, summits[3].Y,
    00357                                                                                                            summits[4].Y, summits[5].Y,
    00358                                                                                                            summits[6].Y, summits[7].Y});
    00359             vMax.Z = MathUtil.GetHighest(new float[] { summits[0].Z, summits[1].Z,
    00360                                                                                                            summits[2].Z, summits[3].Z,
    00361                                                                                                            summits[4].Z, summits[5].Z,
    00362                                                                                                            summits[6].Z, summits[7].Z});
    00363         }
    00364
    00370         public AABBox(BBox bbox, Vector3 v)
    00371         {
    00372             Vector3[] summits = bbox.BuildSumits();
    00373
    00374             vMin.X = MathUtil.GetSmallest(new float[] { summits[0].X, summits[1].X,
    00375                                                                                                                 summits[2].X, summits[3].X,
    00376                                                                                                                 summits[4].X, summits[5].X,
    00377                                                                                                                 summits[6].X, summits[7].X,
    00378                                                                                                                 summits[0].X+v.X, summits[1].X+v.X,
    00379                                                                                                                 summits[2].X+v.X, summits[3].X+v.X,
    00380                                                                                                                 summits[4].X+v.X, summits[5].X+v.X,
    00381                                                                                                                 summits[6].X+v.X, summits[7].X+v.X});
    00382             vMin.Y = MathUtil.GetSmallest(new float[] { summits[0].Y, summits[1].Y,
    00383                                                                                                                 summits[2].Y, summits[3].Y,
    00384                                                                                                                 summits[4].Y, summits[5].Y,
    00385                                                                                                                 summits[6].Y, summits[7].Y,
    00386                                                                                                                 summits[0].Y+v.Y, summits[1].Y+v.Y,
    00387                                                                                                                 summits[2].Y+v.Y, summits[3].Y+v.Y,
    00388                                                                                                                 summits[4].Y+v.Y, summits[5].Y+v.Y,
    00389                                                                                                                 summits[6].Y+v.Y, summits[7].Y+v.Y});
    00390             vMin.Z = MathUtil.GetSmallest(new float[] { summits[0].Z, summits[1].Z,
    00391                                                                                                                 summits[2].Z, summits[3].Z,
    00392                                                                                                                 summits[4].Z, summits[5].Z,
    00393                                                                                                                 summits[6].Z, summits[7].Z,
    00394                                                                                                                 summits[0].Z+v.Z, summits[1].Z+v.Z,
    00395                                                                                                                 summits[2].Z+v.Z, summits[3].Z+v.Z,
    00396                                                                                                                 summits[4].Z+v.Z, summits[5].Z+v.Z,
    00397                                                                                                                 summits[6].Z+v.Z, summits[7].Z+v.Z});
    00398
    00399             vMax.X = MathUtil.GetHighest(new float[] { summits[0].X, summits[1].X,
    00400                                                                                                            summits[2].X, summits[3].X,
    00401                                                                                                            summits[4].X, summits[5].X,
    00402                                                                                                            summits[6].X, summits[7].X,
    00403                                                                                                            summits[0].X+v.X, summits[1].X+v.X,
    00404                                                                                                            summits[2].X+v.X, summits[3].X+v.X,
    00405                                                                                                            summits[4].X+v.X, summits[5].X+v.X,
    00406                                                                                                            summits[6].X+v.X, summits[7].X+v.X});
    00407             vMax.Y = MathUtil.GetHighest(new float[] { summits[0].Y, summits[1].Y,
    00408                                                                                                            summits[2].Y, summits[3].Y,
    00409                                                                                                            summits[4].Y, summits[5].Y,
    00410                                                                                                            summits[6].Y, summits[7].Y,
    00411                                                                                                            summits[0].Y+v.Y, summits[1].Y+v.Y,
    00412                                                                                                            summits[2].Y+v.Y, summits[3].Y+v.Y,
    00413                                                                                                            summits[4].Y+v.Y, summits[5].Y+v.Y,
    00414                                                                                                            summits[6].Y+v.Y, summits[7].Y+v.Y});
    00415             vMax.Z = MathUtil.GetHighest(new float[] { summits[0].Z, summits[1].Z,
    00416                                                                                                            summits[2].Z, summits[3].Z,
    00417                                                                                                            summits[4].Z, summits[5].Z,
    00418                                                                                                            summits[6].Z, summits[7].Z,
    00419                                                                                                            summits[0].Z+v.Z, summits[1].Z+v.Z,
    00420                                                                                                            summits[2].Z+v.Z, summits[3].Z+v.Z,
    00421                                                                                                            summits[4].Z+v.Z, summits[5].Z+v.Z,
    00422                                                                                                            summits[6].Z+v.Z, summits[7].Z+v.Z});
    00423         }
    00424
    00431         public AABBox(BBox bbox, Vector3 v, float f)
    00432             : this(bbox, Vector3.Scale(v, f))
    00433         {
    00434         }
    00435
    00436         public AABBox(float ExtendX, float ExtendY, float ExtendZ)
    00437         {
    00438             vMin.X = -0.5f * ExtendX;
    00439             vMin.Y = -0.5f * ExtendY;
    00440             vMin.Z = -0.5f * ExtendZ;
    00441
    00442             vMax.X = 0.5f * ExtendX;
    00443             vMax.Y = 0.5f * ExtendY;
    00444             vMax.Z = 0.5f * ExtendZ;
    00445         }
    00446
    00452         public bool Collide(AABBox aabb)
    00453         {
    00454             if (aabb.vMax.X < vMin.X)
    00455                 return false;
    00456             if (aabb.vMin.X > vMax.X)
    00457                 return false;
    00458
    00459             if (aabb.vMax.Y < vMin.Y)
    00460                 return false;
    00461             if (aabb.vMin.Y > vMax.Y)
    00462                 return false;
    00463
    00464             if (aabb.vMax.Z < vMin.Z)
    00465                 return false;
    00466             if (aabb.vMin.Z > vMax.Z)
    00467                 return false;
    00468
    00469             // When we end up here we have not found any separating axes for our
    00470             // AABBox
    00471             return true;
    00472         }
    00473
    00480         public bool IsInside(AABBox bbox)
    00481         {
    00482             if (this.vMin.X < bbox.vMin.X)
    00483                 return false;
    00484
    00485             if (this.vMin.Y < bbox.vMin.Y)
    00486                 return false;
    00487
    00488             if (this.vMin.Z < bbox.vMin.Z)
    00489                 return false;
    00490
    00491             if (this.vMax.X > bbox.vMax.X)
    00492                 return false;
    00493
    00494             if (this.vMax.Y > bbox.vMax.Y)
    00495                 return false;
    00496
    00497             if (this.vMax.Z > bbox.vMax.Z)
    00498                 return false;
    00499
    00500             return true;
    00501         }
    00502
    00509         public static AABBox BoxFromBoxes(AABBox bbox1, AABBox bbox2)
    00510         {
    00511             AABBox bbox = new AABBox();
    00512
    00513             bbox.vMin.X = System.Math.Min(bbox1.vMin.X, bbox2.vMin.X);
    00514             bbox.vMin.Y = System.Math.Min(bbox1.vMin.Y, bbox2.vMin.Y);
    00515             bbox.vMin.Z = System.Math.Min(bbox1.vMin.Z, bbox2.vMin.Z);
    00516
    00517             bbox.vMax.X = System.Math.Max(bbox1.vMax.X, bbox2.vMax.X);
    00518             bbox.vMax.Y = System.Math.Max(bbox1.vMax.Y, bbox2.vMax.Y);
    00519             bbox.vMax.Z = System.Math.Max(bbox1.vMax.Z, bbox2.vMax.Z);
    00520
    00521             return bbox;
    00522         }
    00523
    00530         public AABBox BoxFromBoxes(AABBox bbox2)
    00531         {
    00532             this.vMin.X = System.Math.Min(this.vMin.X, bbox2.vMin.X);
    00533             this.vMin.Y = System.Math.Min(this.vMin.Y, bbox2.vMin.Y);
    00534             this.vMin.Z = System.Math.Min(this.vMin.Z, bbox2.vMin.Z);
    00535
    00536             this.vMax.X = System.Math.Max(this.vMax.X, bbox2.vMax.X);
    00537             this.vMax.Y = System.Math.Max(this.vMax.Y, bbox2.vMax.Y);
    00538             this.vMax.Z = System.Math.Max(this.vMax.Z, bbox2.vMax.Z);
    00539
    00540             return this;
    00541         }
    00542
    00548         public static AABBox CalculateLocalBBox(Mesh meshToUse)
    00549         {
    00550             AABBox computedAABBox;
    00551
    00552             if (meshToUse != null)
    00553             {
    00554
    00555                 computedAABBox = new AABBox();
    00556                 // Retrieve the vertex buffer data in the mesh object
    00557                 VertexBuffer vb = meshToUse.VertexBuffer;
    00558
    00559                 // Lock the vertex buffer to generate a simple bounding sphere
    00560                 GraphicsStream vertexData = vb.Lock(0, 0, LockFlags.NoSystemLock);
    00561                 Geometry.ComputeBoundingBox(vertexData,
    00562                     meshToUse.NumberVertices,
    00563                     meshToUse.VertexFormat,
    00564                     out computedAABBox.vMin, out computedAABBox.vMax);
    00565                 vb.Unlock();
    00566                 vb.Dispose();
    00567             }
    00568             else
    00569             {
    00570                 throw new ArgumentNullException("SceneObject.CalculateLocalAABox() can not complete because meshToUse is null");
    00571             }
    00572
    00573             return computedAABBox;
    00574         }
    00575     }
    00576
    00580     public class BBoxPlanes
    00581     {
    00582         Plane leftPlane;
    00583         Plane rightPlane;
    00584         Plane topPlane;
    00585         Plane bottomPlane;
    00586         Plane nearPlane;
    00587         Plane farPlane;
    00588
    00592         public BBoxPlanes()
    00593         {
    00594         }
    00595
    00600         public BBoxPlanes(BBox bbox)
    00601         {
    00602             BuildPlanes(bbox);
    00603         }
    00604
    00609         public void BuildPlanes(BBox bbox)
    00610         {
    00611             leftPlane = Plane.FromPoints(bbox.vMin, new Vector3(bbox.vMin.X, bbox.vMax.Y, bbox.vMin.Z),
    00612                 new Vector3(bbox.vMin.X, bbox.vMax.Y, bbox.vMax.Z));
    00613             rightPlane = Plane.FromPoints(bbox.vMax, new Vector3(bbox.vMax.X, bbox.vMin.Y, bbox.vMax.Z),
    00614                 new Vector3(bbox.vMax.X, bbox.vMin.Y, bbox.vMin.Z));
    00615             topPlane = Plane.FromPoints(bbox.vMax, new Vector3(bbox.vMin.X, bbox.vMax.Y, bbox.vMax.Z),
    00616                 new Vector3(bbox.vMin.X, bbox.vMax.Y, bbox.vMin.Z));
    00617             bottomPlane = Plane.FromPoints(bbox.vMin, new Vector3(bbox.vMax.X, bbox.vMin.Y, bbox.vMin.Z),
    00618                 new Vector3(bbox.vMax.X, bbox.vMin.Y, bbox.vMax.Z));
    00619             nearPlane = Plane.FromPoints(bbox.vMin, new Vector3(bbox.vMin.X, bbox.vMax.Y, bbox.vMin.Z),
    00620                 new Vector3(bbox.vMax.X, bbox.vMax.Y, bbox.vMin.Z));
    00621             farPlane = Plane.FromPoints(bbox.vMax, new Vector3(bbox.vMin.X, bbox.vMax.Y, bbox.vMax.Z),
    00622                 new Vector3(bbox.vMin.X, bbox.vMax.Y, bbox.vMin.Z));
    00623         }
    00624
    00631         public bool CheckCollision(BBox bbox)
    00632         {
    00633             if ((MathUtil.GetPlaneRelativePosition(leftPlane, bbox) == planeRelativePosition.back)
    00634                 || (MathUtil.GetPlaneRelativePosition(rightPlane, bbox) == planeRelativePosition.back)
    00635                 || (MathUtil.GetPlaneRelativePosition(topPlane, bbox) == planeRelativePosition.back)
    00636                 || (MathUtil.GetPlaneRelativePosition(bottomPlane, bbox) == planeRelativePosition.back)
    00637                 || (MathUtil.GetPlaneRelativePosition(nearPlane, bbox) == planeRelativePosition.back)
    00638                 || (MathUtil.GetPlaneRelativePosition(farPlane, bbox) == planeRelativePosition.back))
    00639             {
    00640                 return false;
    00641             }
    00642
    00643             return true;
    00644         }
    00645     }
    00646
    00651     public class MathUtil
    00652     {
    00664         public static void RayFromScreenPos(Viewport viewport, Matrix proj, Matrix view, Vector2 screenPos, out Vector3 rayOrig, out Vector3 rayDir)
    00665         {
    00666             rayOrig = new Vector3(screenPos.X, screenPos.Y, 0.0f);
    00667             rayOrig.Unproject(viewport, proj, view, Matrix.Identity);
    00668
    00669             Vector3 rayDest = new Vector3(screenPos.X, screenPos.Y, 1.0f);
    00670             rayDest.Unproject(viewport, proj, view, Matrix.Identity);
    00671
    00672             rayDir = Vector3.Subtract(rayDest, rayOrig);
    00673
    00674             rayDir.Normalize();
    00675         }
    00676
    00683         public static Vector3 PositionFromMat(Matrix mat)
    00684         {
    00685             return new Vector3(mat.M41, mat.M42, mat.M43);
    00686         }
    00687
    00694         public static Vector3 YawPitchRollFromQuat(Quaternion quat)
    00695         {
    00696             Matrix mat = Matrix.RotationQuaternion(quat);
    00697             return YawPitchRollFromMat(mat);
    00698         }
    00699
    00706         public static Vector3 YawPitchRollFromMat(Matrix mat)
    00707         {
    00708             float yaw = 0.0f;
    00709             float roll = 0.0f;
    00710
    00711             float pitch = (float)Math.Asin(-mat.M32);
    00712
    00713             double threshold = 0.001; // Hardcoded constant
    00714
    00715             double test = Math.Cos(pitch);
    00716
    00717             if (test > threshold)
    00718             {
    00719
    00720                 roll = (float)Math.Atan2(mat.M12, mat.M22);
    00721
    00722                 yaw = (float)Math.Atan2(mat.M31, mat.M33);
    00723
    00724             }
    00725
    00726             else
    00727             {
    00728
    00729                 roll = (float)Math.Atan2(-mat.M21, mat.M11);
    00730
    00731                 yaw = 0.0f;
    00732
    00733             }
    00734
    00735             return new Vector3(yaw, pitch, roll);
    00736         }
    00737
    00738         public static float ResetToNullIfTooSmall(float val, double threshold)
    00739         {
    00740             float res = 0;
    00741             if (Math.Abs(val) > threshold)
    00742             {
    00743                 res = val;
    00744             }
    00745             return res;
    00746         }
    00747
    00753         public static Vector3 YawPitchRollFromMatInDegree(Matrix mat)
    00754         {
    00755             Vector3 ypr = YawPitchRollFromMat(mat);
    00756
    00757             return new Vector3(Geometry.RadianToDegree(ypr.X), Geometry.RadianToDegree(ypr.Y),
    00758                 Geometry.RadianToDegree(ypr.Z));
    00759         }
    00760
    00766         public static Vector3 RadianToDegree(Vector3 yawpitchroll)
    00767         {
    00768             Vector3 yawpitchrollOut = new Vector3();
    00769             yawpitchrollOut.X = Geometry.RadianToDegree(yawpitchroll.X);
    00770             yawpitchrollOut.Y = Geometry.RadianToDegree(yawpitchroll.Y);
    00771             yawpitchrollOut.Z = Geometry.RadianToDegree(yawpitchroll.Z);
    00772             return yawpitchrollOut;
    00773         }
    00774
    00780         public static Vector3 DegreeToRadian(Vector3 yawpitchroll)
    00781         {
    00782             Vector3 yawpitchrollOut = new Vector3();
    00783             yawpitchrollOut.X = Geometry.DegreeToRadian(yawpitchroll.X);
    00784             yawpitchrollOut.Y = Geometry.DegreeToRadian(yawpitchroll.Y);
    00785             yawpitchrollOut.Z = Geometry.DegreeToRadian(yawpitchroll.Z);
    00786             return yawpitchrollOut;
    00787         }
    00788
    00796         public static double Clamp(double val, double low, double high)
    00797         {
    00798             double res;
    00799
    00800             if (val > high)
    00801                 res = high;
    00802             else if (val < low)
    00803                 res = low;
    00804             else
    00805                 res = val;
    00806
    00807             return res;
    00808         }
    00809
    00816         public static float SignedDistance(Plane plane, Vector3 point)
    00817         {
    00818             return plane.Dot(point);
    00819         }
    00820
    00828         public static planeRelativePosition GetPlaneRelativePosition(Plane plane, BBox bbox)
    00829         {
    00830             Vector3 minPoint, maxPoint;
    00831
    00832             if (plane.A > 0.0f)
    00833             {
    00834                 minPoint.X = bbox.vMin.X;
    00835                 maxPoint.X = bbox.vMax.X;
    00836             }
    00837             else
    00838             {
    00839                 minPoint.X = bbox.vMax.X;
    00840                 maxPoint.X = bbox.vMin.X;
    00841             }
    00842
    00843             if (plane.B > 0.0f)
    00844             {
    00845                 minPoint.Y = bbox.vMin.Y;
    00846                 maxPoint.Y = bbox.vMax.Y;
    00847             }
    00848             else
    00849             {
    00850                 minPoint.Y = bbox.vMax.Y;
    00851                 maxPoint.Y = bbox.vMin.Y;
    00852             }
    00853
    00854             if (plane.C > 0.0f)
    00855             {
    00856                 minPoint.Z = bbox.vMin.Z;
    00857                 maxPoint.Z = bbox.vMax.Z;
    00858             }
    00859             else
    00860             {
    00861                 minPoint.Z = bbox.vMax.Z;
    00862                 maxPoint.Z = bbox.vMin.Z;
    00863             }
    00864
    00865             float dmin = SignedDistance(plane, minPoint);
    00866             float dmax = SignedDistance(plane, maxPoint);
    00867
    00868             if (dmin * dmax < 0.0f)
    00869             {
    00870                 return planeRelativePosition.intersect;
    00871             }
    00872             else if (dmin > 0.0f)
    00873             {
    00874                 return planeRelativePosition.front;
    00875             }
    00876
    00877             return planeRelativePosition.back;
    00878         }
    00879
    00885         public static float GetSmallest(float[] vec)
    00886         {
    00887             float res = vec[0];
    00888
    00889             for (int i = 1; i < vec.Length; i++)
    00890             {
    00891                 if (vec[i] < res)
    00892                     res = vec[i];
    00893             }
    00894             return res;
    00895         }
    00896
    00902         public static float GetHighest(float[] vec)
    00903         {
    00904             float res = vec[0];
    00905
    00906             for (int i = 1; i < vec.Length; i++)
    00907             {
    00908                 if (vec[i] > res)
    00909                     res = vec[i];
    00910             }
    00911             return res;
    00912         }
    00913
    00919         public static void CopyMatrixValues(Matrix matDest, Matrix matSrc)
    00920         {
    00921             matDest.M11 = matSrc.M11;
    00922             matDest.M12 = matSrc.M12;
    00923             matDest.M13 = matSrc.M13;
    00924             matDest.M14 = matSrc.M14;
    00925
    00926             matDest.M21 = matSrc.M21;
    00927             matDest.M22 = matSrc.M22;
    00928             matDest.M23 = matSrc.M23;
    00929             matDest.M24 = matSrc.M24;
    00930
    00931             matDest.M31 = matSrc.M31;
    00932             matDest.M32 = matSrc.M32;
    00933             matDest.M33 = matSrc.M33;
    00934             matDest.M34 = matSrc.M34;
    00935
    00936             matDest.M41 = matSrc.M41;
    00937             matDest.M42 = matSrc.M42;
    00938             matDest.M43 = matSrc.M43;
    00939             matDest.M44 = matSrc.M44;
    00940         }
    00941
    00948         public static float Distance(SceneNode n1, SceneNode n2)
    00949         {
    00950             return Distance(n1.Position, n2.Position);
    00951         }
    00952
    00959         public static float Distance(Vector3 v1, Vector3 v2)
    00960         {
    00961             Vector3 v = Vector3.Subtract(v1,v2);
    00962             return v.Length();
    00963         }
    00964
    00970         public static Matrix ExtractRot(Matrix mat)
    00971         {
    00972             Matrix rot = new Matrix();
    00973
    00974             CopyMatrixValues(rot, mat);
    00975
    00976             rot.M41 = 0.0f;
    00977             rot.M42 = 0.0f;
    00978             rot.M43 = 0.0f;
    00979
    00980             return rot;
    00981         }
    00982
    00988         public static Vector3 ExtractPosition(Matrix mat)
    00989         {
    00990             return new Vector3(mat.M41, mat.M42, mat.M43);
    00991         }
    00992     }
    00993
    00998     public class ZMask
    00999     {
    01000         public UInt32 zMask = 0;
    01001         private static UInt32[] masks;
    01002
    01003         public ZMask()
    01004         {
    01005             if (masks == null)
    01006             {
    01007                 masks = new UInt32[32];
    01008
    01009                 for (int i = 0; i < 32; i++)
    01010                 {
    01011                     masks[i] = (UInt32)System.Math.Pow(2, i);
    01012                 }
    01013             }
    01014         }
    01015
    01016         public void Fusion(ZMask zMask2)
    01017         {
    01018             zMask = zMask | (zMask2.zMask);
    01019         }
    01020
    01021         public void Calculate(BBox objectBBox, BBox worldBBox)
    01022         {
    01023             float zSlice = (worldBBox.vMax.Y - worldBBox.vMin.Y) / 32.0f;
    01024
    01025             int baseBit = (int)System.Math.Floor(objectBBox.vMin.Y / zSlice);
    01026             int highBit = (int)System.Math.Ceiling(objectBBox.vMax.Y / zSlice);
    01027
    01028             for (UInt32 i = 0; i < 32; i++)
    01029             {
    01030                 if ((i >= baseBit) && (i <= highBit))
    01031                 {
    01032                     zMask = zMask | masks[i];
    01033                 }
    01034             }
    01035         }
    01036     }
    01037 }