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

Go to the documentation of this file.

//BSD License  
//DXGfx® - http://www.eteractions.com
//Copyright (c) 2005
//by Guillaume Randon
//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.
using System;
using System.Collections.Specialized;
using System.Collections.Generic;
using System.Text;
using Microsoft.DirectX;
using Microsoft.DirectX.Direct3D;
using System.Drawing;

namespace DXGfxLib
{
    public class DrawableBBox
    {
        VertexBuffer vBuff;
        IndexBuffer iBuff;

        public DrawableBBox(Device d3ddevice, BBox bbox)
        {
            CustomVertex.PositionOnly[] points = new CustomVertex.PositionOnly[8];

            Vector3[] submits = bbox.BuildSumits();

            for (int i = 0; i < submits.Length; i++)
            {
                points[i].Position = submits[i];
            }

            vBuff = new VertexBuffer(typeof(CustomVertex.PositionOnly), 8, d3ddevice, Usage.Dynamic, 
                CustomVertex.PositionOnly.Format,
                Pool.SystemMemory);

            GraphicsStream vbuffStm = vBuff.Lock(0, 0, 0);
            vbuffStm.Write(points);
            vBuff.Unlock();

            Int16[] indices = { 0, 1, 1, 2, 2, 3, 3, 0,
                                4, 5, 5, 6, 6, 7, 7, 4,
                                0, 4, 1, 5, 2, 6, 3, 7};


            iBuff = new IndexBuffer(d3ddevice, 24 * sizeof(Int16), Usage.Dynamic, Pool.SystemMemory, true);

            GraphicsStream ibuffStm = iBuff.Lock(0, 0, 0);
            ibuffStm.Write(indices);
            iBuff.Unlock();
        }

        public void Draw(Device d3ddevice)
        {
            d3ddevice.Indices = iBuff;
            d3ddevice.SetStreamSource(0, vBuff, 0);
            d3ddevice.VertexFormat = CustomVertex.PositionOnly.Format;
            //d3ddevice.DrawPrimitives(PrimitiveType.PointList, 0, 8);
            d3ddevice.DrawIndexedPrimitives(PrimitiveType.LineList, 0, 0, 8, 0, 12);
        }
    }

    public class BBox
    {
        public Vector3 vMin;

        public Vector3 vMax;

        public Vector3 Center
        {
            get
            {
                return new Vector3(vMax.X - vMin.X, vMax.Y - vMin.Y, vMax.Z - vMin.Z);
            }
        }

        public BBox()
        {
            vMin = new Vector3(-0.0005f, -0.0005f, -0.0005f);
            vMax = new Vector3(0.0005f, 0.0005f, 0.0005f);
        }

        public void Reset()
        {
            vMin = new Vector3(-0.0005f, -0.0005f, -0.0005f);
            vMax = new Vector3(0.0005f, 0.0005f, 0.0005f);
        }

        public BBox(BBox bbox)
        {
            vMin = bbox.vMin;
            vMax = bbox.vMax;
        }

        public BBox(Vector3 pos, float size)
        {
            vMin.X = pos.X - 0.5f * size;
            vMin.Y = pos.Y - 0.5f * size;
            vMin.Z = pos.Z - 0.5f * size;

            vMax.X = pos.X + 0.5f * size;
            vMax.Y = pos.Y + 0.5f * size;
            vMax.Z = pos.Z + 0.5f * size;
        }

        public static BBox Transform(BBox bbox, Matrix mat)
        {
            BBox bboxRes = new BBox();

            bboxRes.vMin = Vector3.TransformCoordinate(bbox.vMin, mat);
            bboxRes.vMax = Vector3.TransformCoordinate(bbox.vMax, mat);

            return bboxRes;
        }

        public BBox Transform(Matrix mat)
        {
            this.vMin.TransformCoordinate(mat);
            this.vMax.TransformCoordinate(mat);

            return this;
        }

        public Vector3 Size()
        {
            return (new Vector3(System.Math.Abs(vMax.X - vMin.X), System.Math.Abs(vMax.Y - vMin.Y), System.Math.Abs(vMax.Z - vMin.Z)));
        }

        public Vector3 MinPoint()
        {
            return (new Vector3((vMax.X - vMin.X) * 0.5f + vMin.X,
                (vMax.Y - vMin.Y) * 0.5f + vMin.Y,
                (vMax.Z - vMin.Z) * 0.5f + vMin.Y));
        }

        public float MaxAAXZSize()
        {
            Vector3 diag = vMax - vMin;
            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)));
        }

        public Vector3[] BuildSumits()
        {
            Vector3[] summits = new Vector3[8];

            summits[0].X = vMin.X;
            summits[0].Y = vMin.Y;
            summits[0].Z = vMin.Z;

            summits[1].X = vMax.X;
            summits[1].Y = vMin.Y;
            summits[1].Z = vMin.Z;

            summits[2].X = vMax.X;
            summits[2].Y = vMin.Y;
            summits[2].Z = vMax.Z;

            summits[3].X = vMin.X;
            summits[3].Y = vMin.Y;
            summits[3].Z = vMax.Z;

            summits[4].X = vMin.X;
            summits[4].Y = vMax.Y;
            summits[4].Z = vMin.Z;

            summits[5].X = vMax.X;
            summits[5].Y = vMax.Y;
            summits[5].Z = vMin.Z;

            summits[6].X = vMax.X;
            summits[6].Y = vMax.Y;
            summits[6].Z = vMax.Z;

            summits[7].X = vMin.X;
            summits[7].Y = vMax.Y;
            summits[7].Z = vMax.Z;

            return summits;
        }

        public BBox BuildSwepingAABBox(Vector3 linVel, float elapsedTime)
        {
            return new AABBox(this, linVel, elapsedTime);
        }

        public bool CheckCollision(BBox bbox)
        {
            BBoxPlanes planes = new BBoxPlanes(this);


            return planes.CheckCollision(bbox);
        }

        public bool Collide(BBox bbox)
        {
            AABBox aabb1 = new AABBox(this);
            AABBox aabb2 = new AABBox(bbox);

            return aabb1.Collide(aabb2);
        }

        public void Scale(Vector3 scale)
        {
            Vector3 center = Center;

            Vector3 newSize = new Vector3(Size().X * scale.X, Size().Y * scale.Y, Size().Z * scale.Z);

            vMin = center - 0.5f * newSize;
            vMax = center + 0.5f * newSize;
        }
    }

    public enum planeRelativePosition { front, back, intersect };

    public class AABBox : BBox
    {
        public AABBox() : base()
        {
        }

        public AABBox(BBox bbox)
        {
            Vector3[] summits = bbox.BuildSumits();

            vMin.X = MathUtil.GetSmallest(new float[] { summits[0].X, summits[1].X,
                                                                                                                summits[2].X, summits[3].X,
                                                                                                                summits[4].X, summits[5].X,
                                                                                                                summits[6].X, summits[7].X});
            vMin.Y = MathUtil.GetSmallest(new float[] { summits[0].Y, summits[1].Y,
                                                                                                                summits[2].Y, summits[3].Y,
                                                                                                                summits[4].Y, summits[5].Y,
                                                                                                                summits[6].Y, summits[7].Y});
            vMin.Z = MathUtil.GetSmallest(new float[] { summits[0].Z, summits[1].Z,
                                                                                                                summits[2].Z, summits[3].Z,
                                                                                                                summits[4].Z, summits[5].Z,
                                                                                                                summits[6].Z, summits[7].Z});

            vMax.X = MathUtil.GetHighest(new float[] { summits[0].X, summits[1].X,
                                                                                                           summits[2].X, summits[3].X,
                                                                                                           summits[4].X, summits[5].X,
                                                                                                           summits[6].X, summits[7].X});
            vMax.Y = MathUtil.GetHighest(new float[] { summits[0].Y, summits[1].Y,
                                                                                                           summits[2].Y, summits[3].Y,
                                                                                                           summits[4].Y, summits[5].Y,
                                                                                                           summits[6].Y, summits[7].Y});
            vMax.Z = MathUtil.GetHighest(new float[] { summits[0].Z, summits[1].Z,
                                                                                                           summits[2].Z, summits[3].Z,
                                                                                                           summits[4].Z, summits[5].Z,
                                                                                                           summits[6].Z, summits[7].Z});
        }

        public AABBox(BBox bbox, Vector3 v)
        {
            Vector3[] summits = bbox.BuildSumits();

            vMin.X = MathUtil.GetSmallest(new float[] { summits[0].X, summits[1].X,
                                                                                                                summits[2].X, summits[3].X,
                                                                                                                summits[4].X, summits[5].X,
                                                                                                                summits[6].X, summits[7].X,
                                                                                                                summits[0].X+v.X, summits[1].X+v.X,
                                                                                                                summits[2].X+v.X, summits[3].X+v.X,
                                                                                                                summits[4].X+v.X, summits[5].X+v.X,
                                                                                                                summits[6].X+v.X, summits[7].X+v.X});
            vMin.Y = MathUtil.GetSmallest(new float[] { summits[0].Y, summits[1].Y,
                                                                                                                summits[2].Y, summits[3].Y,
                                                                                                                summits[4].Y, summits[5].Y,
                                                                                                                summits[6].Y, summits[7].Y,
                                                                                                                summits[0].Y+v.Y, summits[1].Y+v.Y,
                                                                                                                summits[2].Y+v.Y, summits[3].Y+v.Y,
                                                                                                                summits[4].Y+v.Y, summits[5].Y+v.Y,
                                                                                                                summits[6].Y+v.Y, summits[7].Y+v.Y});
            vMin.Z = MathUtil.GetSmallest(new float[] { summits[0].Z, summits[1].Z,
                                                                                                                summits[2].Z, summits[3].Z,
                                                                                                                summits[4].Z, summits[5].Z,
                                                                                                                summits[6].Z, summits[7].Z,
                                                                                                                summits[0].Z+v.Z, summits[1].Z+v.Z,
                                                                                                                summits[2].Z+v.Z, summits[3].Z+v.Z,
                                                                                                                summits[4].Z+v.Z, summits[5].Z+v.Z,
                                                                                                                summits[6].Z+v.Z, summits[7].Z+v.Z});

            vMax.X = MathUtil.GetHighest(new float[] { summits[0].X, summits[1].X,
                                                                                                           summits[2].X, summits[3].X,
                                                                                                           summits[4].X, summits[5].X,
                                                                                                           summits[6].X, summits[7].X,
                                                                                                           summits[0].X+v.X, summits[1].X+v.X,
                                                                                                           summits[2].X+v.X, summits[3].X+v.X,
                                                                                                           summits[4].X+v.X, summits[5].X+v.X,
                                                                                                           summits[6].X+v.X, summits[7].X+v.X});
            vMax.Y = MathUtil.GetHighest(new float[] { summits[0].Y, summits[1].Y,
                                                                                                           summits[2].Y, summits[3].Y,
                                                                                                           summits[4].Y, summits[5].Y,
                                                                                                           summits[6].Y, summits[7].Y,
                                                                                                           summits[0].Y+v.Y, summits[1].Y+v.Y,
                                                                                                           summits[2].Y+v.Y, summits[3].Y+v.Y,
                                                                                                           summits[4].Y+v.Y, summits[5].Y+v.Y,
                                                                                                           summits[6].Y+v.Y, summits[7].Y+v.Y});
            vMax.Z = MathUtil.GetHighest(new float[] { summits[0].Z, summits[1].Z,
                                                                                                           summits[2].Z, summits[3].Z,
                                                                                                           summits[4].Z, summits[5].Z,
                                                                                                           summits[6].Z, summits[7].Z,
                                                                                                           summits[0].Z+v.Z, summits[1].Z+v.Z,
                                                                                                           summits[2].Z+v.Z, summits[3].Z+v.Z,
                                                                                                           summits[4].Z+v.Z, summits[5].Z+v.Z,
                                                                                                           summits[6].Z+v.Z, summits[7].Z+v.Z});
        }

        public AABBox(BBox bbox, Vector3 v, float f)
            : this(bbox, Vector3.Scale(v, f))
        {
        }

        public AABBox(float ExtendX, float ExtendY, float ExtendZ)
        {
            vMin.X = -0.5f * ExtendX;
            vMin.Y = -0.5f * ExtendY;
            vMin.Z = -0.5f * ExtendZ;

            vMax.X = 0.5f * ExtendX;
            vMax.Y = 0.5f * ExtendY;
            vMax.Z = 0.5f * ExtendZ;
        }

        public bool Collide(AABBox aabb)
        {
            if (aabb.vMax.X < vMin.X)
                return false;
            if (aabb.vMin.X > vMax.X)
                return false;

            if (aabb.vMax.Y < vMin.Y)
                return false;
            if (aabb.vMin.Y > vMax.Y)
                return false;

            if (aabb.vMax.Z < vMin.Z)
                return false;
            if (aabb.vMin.Z > vMax.Z)
                return false;

            // When we end up here we have not found any separating axes for our
            // AABBox
            return true;
        }

        public bool IsInside(AABBox bbox)
        {
            if (this.vMin.X < bbox.vMin.X)
                return false;

            if (this.vMin.Y < bbox.vMin.Y)
                return false;

            if (this.vMin.Z < bbox.vMin.Z)
                return false;

            if (this.vMax.X > bbox.vMax.X)
                return false;

            if (this.vMax.Y > bbox.vMax.Y)
                return false;

            if (this.vMax.Z > bbox.vMax.Z)
                return false;

            return true;
        }

        public static AABBox BoxFromBoxes(AABBox bbox1, AABBox bbox2)
        {
            AABBox bbox = new AABBox();

            bbox.vMin.X = System.Math.Min(bbox1.vMin.X, bbox2.vMin.X);
            bbox.vMin.Y = System.Math.Min(bbox1.vMin.Y, bbox2.vMin.Y);
            bbox.vMin.Z = System.Math.Min(bbox1.vMin.Z, bbox2.vMin.Z);

            bbox.vMax.X = System.Math.Max(bbox1.vMax.X, bbox2.vMax.X);
            bbox.vMax.Y = System.Math.Max(bbox1.vMax.Y, bbox2.vMax.Y);
            bbox.vMax.Z = System.Math.Max(bbox1.vMax.Z, bbox2.vMax.Z);

            return bbox;
        }

        public AABBox BoxFromBoxes(AABBox bbox2)
        {
            this.vMin.X = System.Math.Min(this.vMin.X, bbox2.vMin.X);
            this.vMin.Y = System.Math.Min(this.vMin.Y, bbox2.vMin.Y);
            this.vMin.Z = System.Math.Min(this.vMin.Z, bbox2.vMin.Z);

            this.vMax.X = System.Math.Max(this.vMax.X, bbox2.vMax.X);
            this.vMax.Y = System.Math.Max(this.vMax.Y, bbox2.vMax.Y);
            this.vMax.Z = System.Math.Max(this.vMax.Z, bbox2.vMax.Z);

            return this;
        }

        public static AABBox CalculateLocalBBox(Mesh meshToUse)
        {
            AABBox computedAABBox;

            if (meshToUse != null)
            {
                
                computedAABBox = new AABBox();
                // Retrieve the vertex buffer data in the mesh object
                VertexBuffer vb = meshToUse.VertexBuffer;

                // Lock the vertex buffer to generate a simple bounding sphere
                GraphicsStream vertexData = vb.Lock(0, 0, LockFlags.NoSystemLock);
                Geometry.ComputeBoundingBox(vertexData,
                    meshToUse.NumberVertices,
                    meshToUse.VertexFormat,
                    out computedAABBox.vMin, out computedAABBox.vMax);
                vb.Unlock();
                vb.Dispose();
            }
            else
            {
                throw new ArgumentNullException("SceneObject.CalculateLocalAABox() can not complete because meshToUse is null");
            }

            return computedAABBox;
        }
    }

    public class BBoxPlanes
    {
        Plane leftPlane;
        Plane rightPlane;
        Plane topPlane;
        Plane bottomPlane;
        Plane nearPlane;
        Plane farPlane;

        public BBoxPlanes()
        {
        }

        public BBoxPlanes(BBox bbox)
        {
            BuildPlanes(bbox);
        }

        public void BuildPlanes(BBox bbox)
        {
            leftPlane = Plane.FromPoints(bbox.vMin, new Vector3(bbox.vMin.X, bbox.vMax.Y, bbox.vMin.Z),
                new Vector3(bbox.vMin.X, bbox.vMax.Y, bbox.vMax.Z));
            rightPlane = Plane.FromPoints(bbox.vMax, new Vector3(bbox.vMax.X, bbox.vMin.Y, bbox.vMax.Z),
                new Vector3(bbox.vMax.X, bbox.vMin.Y, bbox.vMin.Z));
            topPlane = Plane.FromPoints(bbox.vMax, new Vector3(bbox.vMin.X, bbox.vMax.Y, bbox.vMax.Z),
                new Vector3(bbox.vMin.X, bbox.vMax.Y, bbox.vMin.Z));
            bottomPlane = Plane.FromPoints(bbox.vMin, new Vector3(bbox.vMax.X, bbox.vMin.Y, bbox.vMin.Z),
                new Vector3(bbox.vMax.X, bbox.vMin.Y, bbox.vMax.Z));
            nearPlane = Plane.FromPoints(bbox.vMin, new Vector3(bbox.vMin.X, bbox.vMax.Y, bbox.vMin.Z),
                new Vector3(bbox.vMax.X, bbox.vMax.Y, bbox.vMin.Z));
            farPlane = Plane.FromPoints(bbox.vMax, new Vector3(bbox.vMin.X, bbox.vMax.Y, bbox.vMax.Z),
                new Vector3(bbox.vMin.X, bbox.vMax.Y, bbox.vMin.Z));
        }

        public bool CheckCollision(BBox bbox)
        {
            if ((MathUtil.GetPlaneRelativePosition(leftPlane, bbox) == planeRelativePosition.back)
                || (MathUtil.GetPlaneRelativePosition(rightPlane, bbox) == planeRelativePosition.back)
                || (MathUtil.GetPlaneRelativePosition(topPlane, bbox) == planeRelativePosition.back)
                || (MathUtil.GetPlaneRelativePosition(bottomPlane, bbox) == planeRelativePosition.back)
                || (MathUtil.GetPlaneRelativePosition(nearPlane, bbox) == planeRelativePosition.back)
                || (MathUtil.GetPlaneRelativePosition(farPlane, bbox) == planeRelativePosition.back))
            {
                return false;
            }

            return true;
        }
    }

    public class MathUtil
    {
        public static void RayFromScreenPos(Viewport viewport, Matrix proj, Matrix view, Vector2 screenPos, out Vector3 rayOrig, out Vector3 rayDir)
        {
            rayOrig = new Vector3(screenPos.X, screenPos.Y, 0.0f);
            rayOrig.Unproject(viewport, proj, view, Matrix.Identity);

            Vector3 rayDest = new Vector3(screenPos.X, screenPos.Y, 1.0f);
            rayDest.Unproject(viewport, proj, view, Matrix.Identity);

            rayDir = Vector3.Subtract(rayDest, rayOrig);

            rayDir.Normalize();
        }

        public static Vector3 PositionFromMat(Matrix mat)
        {
            return new Vector3(mat.M41, mat.M42, mat.M43);
        }

        public static Vector3 YawPitchRollFromQuat(Quaternion quat)
        {
            Matrix mat = Matrix.RotationQuaternion(quat);
            return YawPitchRollFromMat(mat);
        }

        public static Vector3 YawPitchRollFromMat(Matrix mat)
        {
            float yaw = 0.0f;
            float roll = 0.0f;

            float pitch = (float)Math.Asin(-mat.M32);

            double threshold = 0.001; // Hardcoded constant

            double test = Math.Cos(pitch);

            if (test > threshold)
            {

                roll = (float)Math.Atan2(mat.M12, mat.M22);

                yaw = (float)Math.Atan2(mat.M31, mat.M33);

            }

            else
            {

                roll = (float)Math.Atan2(-mat.M21, mat.M11);

                yaw = 0.0f;

            }

            return new Vector3(yaw, pitch, roll);
        }

        public static float ResetToNullIfTooSmall(float val, double threshold)
        {
            float res = 0;
            if (Math.Abs(val) > threshold)
            {
                res = val;
            }
            return res;
        }

        public static Vector3 YawPitchRollFromMatInDegree(Matrix mat)
        {
            Vector3 ypr = YawPitchRollFromMat(mat);

            return new Vector3(Geometry.RadianToDegree(ypr.X), Geometry.RadianToDegree(ypr.Y), 
                Geometry.RadianToDegree(ypr.Z));
        }

        public static Vector3 RadianToDegree(Vector3 yawpitchroll)
        {
            Vector3 yawpitchrollOut = new Vector3();
            yawpitchrollOut.X = Geometry.RadianToDegree(yawpitchroll.X);
            yawpitchrollOut.Y = Geometry.RadianToDegree(yawpitchroll.Y);
            yawpitchrollOut.Z = Geometry.RadianToDegree(yawpitchroll.Z);
            return yawpitchrollOut;
        }

        public static Vector3 DegreeToRadian(Vector3 yawpitchroll)
        {
            Vector3 yawpitchrollOut = new Vector3();
            yawpitchrollOut.X = Geometry.DegreeToRadian(yawpitchroll.X);
            yawpitchrollOut.Y = Geometry.DegreeToRadian(yawpitchroll.Y);
            yawpitchrollOut.Z = Geometry.DegreeToRadian(yawpitchroll.Z);
            return yawpitchrollOut;
        }

        public static double Clamp(double val, double low, double high)
        {
            double res;

            if (val > high)
                res = high;
            else if (val < low)
                res = low;
            else
                res = val;

            return res;
        }

        public static float SignedDistance(Plane plane, Vector3 point)
        {
            return plane.Dot(point);
        }

        public static planeRelativePosition GetPlaneRelativePosition(Plane plane, BBox bbox)
        {
            Vector3 minPoint, maxPoint;

            if (plane.A > 0.0f)
            {
                minPoint.X = bbox.vMin.X;
                maxPoint.X = bbox.vMax.X;
            }
            else
            {
                minPoint.X = bbox.vMax.X;
                maxPoint.X = bbox.vMin.X;
            }

            if (plane.B > 0.0f)
            {
                minPoint.Y = bbox.vMin.Y;
                maxPoint.Y = bbox.vMax.Y;
            }
            else
            {
                minPoint.Y = bbox.vMax.Y;
                maxPoint.Y = bbox.vMin.Y;
            }

            if (plane.C > 0.0f)
            {
                minPoint.Z = bbox.vMin.Z;
                maxPoint.Z = bbox.vMax.Z;
            }
            else
            {
                minPoint.Z = bbox.vMax.Z;
                maxPoint.Z = bbox.vMin.Z;
            }

            float dmin = SignedDistance(plane, minPoint);
            float dmax = SignedDistance(plane, maxPoint);

            if (dmin * dmax < 0.0f)
            {
                return planeRelativePosition.intersect;
            }
            else if (dmin > 0.0f)
            {
                return planeRelativePosition.front;
            }

            return planeRelativePosition.back;
        }

        public static float GetSmallest(float[] vec)
        {
            float res = vec[0];

            for (int i = 1; i < vec.Length; i++)
            {
                if (vec[i] < res)
                    res = vec[i];
            }
            return res;
        }

        public static float GetHighest(float[] vec)
        {
            float res = vec[0];

            for (int i = 1; i < vec.Length; i++)
            {
                if (vec[i] > res)
                    res = vec[i];
            }
            return res;
        }

        public static void CopyMatrixValues(Matrix matDest, Matrix matSrc)
        {
            matDest.M11 = matSrc.M11;
            matDest.M12 = matSrc.M12;
            matDest.M13 = matSrc.M13;
            matDest.M14 = matSrc.M14;

            matDest.M21 = matSrc.M21;
            matDest.M22 = matSrc.M22;
            matDest.M23 = matSrc.M23;
            matDest.M24 = matSrc.M24;

            matDest.M31 = matSrc.M31;
            matDest.M32 = matSrc.M32;
            matDest.M33 = matSrc.M33;
            matDest.M34 = matSrc.M34;

            matDest.M41 = matSrc.M41;
            matDest.M42 = matSrc.M42;
            matDest.M43 = matSrc.M43;
            matDest.M44 = matSrc.M44;
        }

        public static float Distance(SceneNode n1, SceneNode n2)
        {
            return Distance(n1.Position, n2.Position);
        }

        public static float Distance(Vector3 v1, Vector3 v2)
        {
            Vector3 v = Vector3.Subtract(v1,v2);
            return v.Length();
        }

        public static Matrix ExtractRot(Matrix mat)
        {
            Matrix rot = new Matrix();

            CopyMatrixValues(rot, mat);

            rot.M41 = 0.0f;
            rot.M42 = 0.0f;
            rot.M43 = 0.0f;

            return rot;
        }

        public static Vector3 ExtractPosition(Matrix mat)
        {
            return new Vector3(mat.M41, mat.M42, mat.M43);
        }
    }

    public class ZMask
    {
        public UInt32 zMask = 0;
        private static UInt32[] masks;

        public ZMask()
        {
            if (masks == null)
            {
                masks = new UInt32[32];

                for (int i = 0; i < 32; i++)
                {
                    masks[i] = (UInt32)System.Math.Pow(2, i);
                }
            }
        }

        public void Fusion(ZMask zMask2)
        {
            zMask = zMask | (zMask2.zMask);
        }

        public void Calculate(BBox objectBBox, BBox worldBBox)
        {
            float zSlice = (worldBBox.vMax.Y - worldBBox.vMin.Y) / 32.0f;

            int baseBit = (int)System.Math.Floor(objectBBox.vMin.Y / zSlice);
            int highBit = (int)System.Math.Ceiling(objectBBox.vMax.Y / zSlice);

            for (UInt32 i = 0; i < 32; i++)
            {
                if ((i >= baseBit) && (i <= highBit))
                {
                    zMask = zMask | masks[i];
                }
            }
        }
    }
}

---