LineUtil.cs source code in C# .NET

Source code for the .NET framework in C#

                        

Code:

/ 4.0 / 4.0 / DEVDIV_TFS / Dev10 / Releases / RTMRel / wpf / src / Core / CSharp / MS / Internal / Media3D / LineUtil.cs / 1305600 / LineUtil.cs

                            //---------------------------------------------------------------------------- 
// 
//    Copyright (C) Microsoft Corporation.  All rights reserved.
// 
//--------------------------------------------------------------------------- 

// GLOSSARY 
//     Kernel of matrix M : Space of vectors that vanish (go to 0) when multiplied by M 
// Null-space of matrix M : Same thing
//          Line matrix : Representation of a line using a matrix whose kernel is points on the line 
//                      L : Matrix representing a line
//                     LT : Transpose of L
//
// This file offers a small line transform utility function.  Given a line (lin) defined by Point3D 
// origin & Vector3D direction and a model matrix M it returns (in-place) a line (lout) in "model
// space" so that any point on the line when transformed by M is on the original line. 
// 
// In other words   x in lout ===>  x M in lin
// 
// This works even if M is rank-deficient, but if M is rank 2 or less then lout is not uniquely
// determined.
//
// The basic technique is as follows, we represent lin as a matrix where points on the line are in 
// the null-space (kernel) of lin (this is straightforward.)  Letting the symbols lin & lout refer
// to both the line & the corresponding matrix this means: 
// 
//        x in lin ===> x lin = [0,0,0,0]
// 
// Then we can transform a line matrix into model space by left multiplication with M
//
//        lout = M lin
// 
// That way,
// 
//        x in lout ===> x lout = [0,0,0,0] ===> x M lin = [0,0,0,0] ===> x M in lin 
//
// Which is what we want.  The hard part is going back from the matrix lout to a point and a vector. 
// The smallest two eigenvalues of the l matrix are zero.  The corresponding eigenvectors are two
// different points on the line.
//
// I find the eigenvectors and eigenvalues of a line matrix L by first computing the normal matrix 
// N = L LT which has symmetric eigenvectors equal to the left eigenvectors of L.  I find the
// eigenvectors of N using a Jacobi method for symmetric eigenvalue problems. 
// 
// The method is described in Chapter 8.4 of Golub & Van Loan (Matrix Computation), but here's a
// brief summary. 
//
// We apply a series of 2D rotations (A1...An) to the matrix N that each make the matrix more
// diagonal.  So if the whole sequence is A = A1 ... An then the final matrix E = AT N A is
// diagonal. 
//
// Because A is orthonormal its columns are the eigenvectors of N and the diagonal elements of E are 
// the eigenvalues. 
//
// NOTES 
//
// Forming the normal matrix N involves fourth powers of the input values.  I mitigate this by
// scaling the matrix so that its largest value is 1 before squaring it.  There may be a better
// method (perhaps even a modified Jacobi method) that would work directly on L and perhaps be more 
// stable.
// 
// None of this code understands rays.  This is all in terms of lines, lines, lines, lines! 

using System; 
using System.Collections.Generic;
using System.Diagnostics;
using System.Windows;
using System.Windows.Media.Media3D; 

using MS.Utility; 
 
namespace MS.Internal.Media3D
{ 
    [Flags]
    internal enum FaceType
    {
        None     = 0, 
        Front    = 1 << 0,
        Back     = 1 << 1, 
    }; 

    internal static class LineUtil 
    {
        // Coordinates of elements above the diagonal.
        readonly static int[,] s_pairs = new int[,]{ {0,1}, {0,2}, {0,3}, {1,2}, {1,3}, {2,3} };
        const int s_pairsCount = 6; 

        public static void Transform(Matrix3D modelMatrix, 
                                     ref Point3D origin, ref Vector3D direction, out bool isRay) 
        {
            if (modelMatrix.InvertCore()) 
            {
                Point4D o = new Point4D(origin.X,origin.Y,origin.Z,1);
                Point4D d = new Point4D(direction.X,direction.Y,direction.Z,0);
 
                modelMatrix.MultiplyPoint(ref o);
                modelMatrix.MultiplyPoint(ref d); 
 
                if (o.W == 1 && d.W == 0)
                { 
                    // Affine transformation

                    origin = new Point3D(o.X, o.Y, o.Z);
                    direction = new Vector3D(d.X, d.Y, d.Z); 

                    isRay = true; 
                } 
                else
                { 
                    // Non-affine transformation (likely projection)

                    // Form 4x2 matrix with two points on line in two columns.
                    double[,] linepoints = new double[,]{{o.X,d.X},{o.Y,d.Y},{o.Z,d.Z},{o.W,d.W}}; 

                    ColumnsToAffinePointVector(linepoints,0,1,out origin, out direction); 
 
                    isRay = false;
                } 
            }
            else
            {
                TransformSingular(ref modelMatrix, ref origin, ref direction); 

                isRay = false; 
            } 
        }
 
        // modelMatrix is passed by reference for efficiency only.  It is not modified.
        private static void TransformSingular(ref Matrix3D modelMatrix,
                                              ref Point3D origin, ref Vector3D direction)
        { 
            double [,] matrix = TransformedLineMatrix(ref modelMatrix, ref origin, ref direction);
            matrix = Square(matrix); 
 
            double[,] eigen = new double[,]{ {1,0,0,0}, {0,1,0,0}, {0,0,1,0}, {0,0,0,1} };
 
            // We'll just do 5 iterations with each pair because according to my results & Golub &
            // Van Loan this process converges quickly.
            int iterations = 5 * s_pairsCount;
            for (int iter = 0; iter < iterations; ++iter) 
            {
                int pair = iter % s_pairsCount; 
                JacobiRotation jrot = new JacobiRotation(s_pairs[pair,0],s_pairs[pair,1],matrix); 
                matrix = jrot.LeftRightMultiply(matrix);
                eigen = jrot.RightMultiply(eigen); 
            }

            // That was it as far as finding eigenvectors
 
            int evec1,evec2;
            FindSmallestTwoDiagonal(matrix, out evec1, out evec2); 
 
            // The eigenvectors corresponding to the two smallest eigenvalues are columns evec1 &
            // evec2.  These, in homogeneous space, are two different points on our line.  We are 
            // going to convert them to an affine point & vector.
            ColumnsToAffinePointVector(eigen, evec1, evec2, out origin, out direction);
        }
 
        private static void ColumnsToAffinePointVector(double[,] matrix, int col1, int col2, out Point3D origin, out Vector3D direction)
        { 
            // The col1 & col2 columsn of matrix are two different homogeneous points on a line.  We 
            // are going to convert them to an affine point & vector using the following procedure.
            //   1. Pick the eigenvector with the largest W, call it big 
            //   2. Scale it by 1/W to form an affine point, still call it big
            //   3. Add a weighted multiple of big to small to make small.W zero.
            //
            // If both w are zero then we have a projective line that intersects no affine points 
            // (i.e. it's a line at infinity.)  This will cause overflow but that's fine because the
            // line at infinity doesn't intersect anything & the caller of this function needs be 
            // able to handle results that come back with inf or nan by "doing nothing." 

            // Step 1. 
            if (matrix[3,col1]*matrix[3,col1] < matrix[3,col2]*matrix[3,col2])
            {
                int temp = col1;
                col1 = col2; 
                col2 = temp;
            } 
 
            // Step 2.
            double s = 1/matrix[3,col1]; 
            origin = new Point3D(s*matrix[0,col1],
                                 s*matrix[1,col1],
                                 s*matrix[2,col1]);
 
            // Step 3.
            s = -matrix[3,col2]; 
            direction = new Vector3D(matrix[0,col2]+s*origin.X, 
                                     matrix[1,col2]+s*origin.Y,
                                     matrix[2,col2]+s*origin.Z); 
        }


        // Returns the indices of the smallest two diagonal elements of matrix 
        private static void FindSmallestTwoDiagonal(double[,] matrix, out int evec1, out int evec2)
        { 
            evec1 = 0; 
            evec2 = 1;
            // And corresponding squared eigenvalues. 
            double eval1 = matrix[0,0]*matrix[0,0];
            double eval2 = matrix[1,1]*matrix[1,1];

            for (int i = 2; i < 4; ++i) 
            {
                // Replace second smallest if necessary. 
                double val = matrix[i,i]*matrix[i,i]; 
                if (val < eval1)
                { 
                    if (eval1 < eval2)
                    {
                        eval2 = val;
                        evec2 = i; 
                    }
                    else 
                    { 
                        eval1 = val;
                        evec1 = i; 
                    }
                }
                else if (val < eval2)
                { 
                    eval2 = val;
                    evec2 = i; 
                } 
            }
        } 

        // Returns the "line matrix" corresponding to the line (origin,direction) transformed by the
        // inverse *transform* of modelMatrix.  (To transform a line by the inverse transform
        // requires multiplying by the non-inverted matrix.) 
        private static double[,] TransformedLineMatrix(ref Matrix3D modelMatrix,
                                                       ref Point3D origin, ref Vector3D direction) 
        { 
                double x1 = origin.X;
                double y1 = origin.Y; 
                double z1 = origin.Z;
                // w1 = 1
                double x2 = direction.X;
                double y2 = direction.Y; 
                double z2 = direction.Z;
                // w2 = 0 
 
                // To prove to yourself that this matrix is correct just multiply by the two
                // (homogeneous) points on the line.  Any other homogeneous point on the line is a 
                // linear combination of them.

                double a = y2*z1-y1*z2;
                double b = x1*z2-x2*z1; 
                double c = x2*y1-x1*y2;
 
                Matrix3D m = modelMatrix * 
                             new Matrix3D(a,  y2,  z2,   0,
                                          b, -x2,   0,  z2, 
                                          c,   0, -x2, -y2,
                                          0,   c,  -b,   a);
                double[,] matrix = new double[4,4];
                matrix[0,0] = m.M11; 
                matrix[0,1] = m.M12;
                matrix[0,2] = m.M13; 
                matrix[0,3] = m.M14; 
                matrix[1,0] = m.M21;
                matrix[1,1] = m.M22; 
                matrix[1,2] = m.M23;
                matrix[1,3] = m.M24;
                matrix[2,0] = m.M31;
                matrix[2,1] = m.M32; 
                matrix[2,2] = m.M33;
                matrix[2,3] = m.M34; 
                matrix[3,0] = m.OffsetX; 
                matrix[3,1] = m.OffsetY;
                matrix[3,2] = m.OffsetZ; 
                matrix[3,3] = m.M44;
                return matrix;
        }
 
        // Scales M so that its largest element is 1 and then returns M MT
        // (MT=transpose(M)) 
        private static double [,] Square(double[,] m) 
        {
            double[,] o = new double[4,4]; 

            // Scale the matrix so that its largest element is 1.
            double maxvalue = 0;
            for (int i = 0; i < 4; ++i) 
            {
                for (int j = 0; j < 4; ++j) 
                { 
                    maxvalue = Math.Max(maxvalue,m[i,j]*m[i,j]);
                } 
            }
            maxvalue = Math.Sqrt(maxvalue);
            for (int i = 0; i < 4; ++i)
            { 
                for (int j = 0; j < 4; ++j)
                { 
                    m[i,j] /= maxvalue; 
                }
            } 

            // Compute its square.
            for (int i = 0; i < 4; ++i)
            { 
                for (int j = 0; j < 4; ++j)
                { 
                    double d = 0; 
                    for (int k = 0; k < 4; ++k)
                    { 
                        d += m[i,k]*m[j,k];
                    }
                    o[i,j] = d;
                } 
            }
            return o; 
        } 

        // See section 8.4 of Golub & Van Loan "Matrix Computation" 
        // Remember, J is this Jacobi rotation.  JT is the transpose.
        // Also section 5.1.8 shows the formula for multiplying by the Jacobi rotation
        // Though as they helpfully point out a Jacobi rotation is the same as a Givens rotation
        private struct JacobiRotation 
        {
            public JacobiRotation(int p, int q, double[,] a) 
            { 
                // Constructs a 2D rotation matrix M as follows
                // Start with identity. 
                // Zero the p & q rows & columns
                // Then set the intersections of these rows & columns as follows
                //    [ M_pp M_pq ]  =  [ c s]
                //    [ M_qp M_qq ]     [-s c] 
                //
                // Where c & s are a cosine-sine pair calculated so that multiplying a by this will 
                // descrease the the off-diagonal elements. 

                _p = p; 
                _q = q;

                double tau = (a[q,q] - a[p,p])/(2*a[p,q]);
                if (tau < Double.MaxValue && tau > -Double.MaxValue) 
                {
                    double root = Math.Sqrt(1+tau*tau); 
                    // Choose the smaller of -tau +/- root 
                    double t = -tau < 0 ? -tau + root : -tau - root;
                    _c = 1/Math.Sqrt(1+t*t); 
                    _s = t * _c;
                }
                else
                { 
                    _c = 1;
                    _s = 0; 
                } 
            }
 
            // These functions overwrite & return their argument.

            // returns JT a J
            public double[,] LeftRightMultiply(double [,] a) 
            {
                return RightMultiply(LeftMultiplyTranspose(a)); 
            } 

            // returns a J 
            public double[,] RightMultiply(double [,] a)
            {
                for (int j = 0; j < 4; ++j)
                { 
                    double tau1 = a[j,_p];
                    double tau2 = a[j,_q]; 
 
                    a[j,_p] = _c * tau1 - _s * tau2;
                    a[j,_q] = _s * tau1 + _c * tau2; 
                }

                return a;
            } 

 
            // returns JT a 
            public double[,] LeftMultiplyTranspose(double [,] a)
            { 
                for (int j = 0; j < 4; ++j)
                {
                    double tau1 = a[_p,j];
                    double tau2 = a[_q,j]; 

                    a[_p,j] = _c * tau1 - _s * tau2; 
                    a[_q,j] = _s * tau1 + _c * tau2; 
                }
 
                return a;
            }

            private int _p, _q; 
            private double _c, _s;
        } 
 
        // This method determines if the line/ray intersects the triangle.
        // If "origin" and "direction" truely represent a line, "type" should be front and back because 
        // we don't have any true direction.
        //
        //     origin/direction define the line/ray
        //     v0/v1/v2 define the triangle 
        //
        // origin, direction, v0, v1, v2 are passed by ref for perf.  They are NOT MODIFIED 
        // 
        // If this method returns false, ignore the values of hitCoord and dist.
        // 
        // Ported from dxg\d3dx9\mesh\intersect.cpp (12/04/03)
        // which is an implementation of "Fast, Minimum Storage Ray-Triangle Intersection" by Moller + Trumbore
        internal static bool ComputeLineTriangleIntersection(
            FaceType type, 
            ref Point3D origin,
            ref Vector3D direction, 
            ref Point3D v0, 
            ref Point3D v1,
            ref Point3D v2, 
            out Point hitCoord,
            out double dist)
        {
            Vector3D e1; 
            Point3D.Subtract(ref v1, ref v0, out e1);
            Vector3D e2; 
            Point3D.Subtract(ref v2, ref v0, out e2); 

            Vector3D r; 
            Vector3D.CrossProduct(ref direction, ref e2, out r);

            double a = Vector3D.DotProduct(ref e1, ref r);
 
            Vector3D s;
            if (a > 0 && (type & FaceType.Front) != 0) 
            { 
                Point3D.Subtract(ref origin, ref v0, out s);
            } 
            else if (a < 0 && (type & FaceType.Back) != 0)
            {
                Point3D.Subtract(ref v0, ref origin, out s);
                a = -a; 
            }
            else 
            { 
                hitCoord = new Point();
                dist = 0; 
                return false;
            }

            double u = Vector3D.DotProduct(ref s, ref r); 
            if ((u < 0) || (a < u))
            { 
                hitCoord = new Point(); 
                dist = 0;
                return false; 
            }

            Vector3D q;
            Vector3D.CrossProduct(ref s, ref e1, out q); 

            double v = Vector3D.DotProduct(ref direction, ref q); 
            if ((v < 0) || (a < (u + v))) 
            {
                hitCoord = new Point(); 
                dist = 0;
                return false;
            }
 
            double t = Vector3D.DotProduct(ref e2, ref q);
            double f = 1 / a; 
 
            t = t * f;
            u = u * f; 
            v = v * f;

            hitCoord = new Point(u, v);
            dist = t; 

            return true; 
        } 

        // This function returns true if the probe line intersects the bbox volume (not 
        // just the surface of the box).  Does LINE and RAY intersection tests.
        //
        // Based on Woo's method presented in Gems I, p. 395.  See also "Real-Time
        // Rendering", Haines, sec 10.4.2. 
        //
        //     origin/direction define the non-oriented line or ray 
        //     box is the volume to intersect 
        //
        // origin, direction, and box are passed by ref for perf.  They are NOT MODIFIED 
        //
        // Ported from dxg\d3dx9\mesh\intersect.cpp (12/04/03)
        internal static bool ComputeLineBoxIntersection(ref Point3D origin, ref Vector3D direction, ref Rect3D box, bool isRay)
        { 
            // Reject empty bounding boxes.
            if (box.IsEmpty) 
            { 
                return false;
            } 

            bool inside = true;
            bool[] middle = new bool[3];        // True if ray origin in middle for coord i.
            double[] plane = new double[3];     // Candidate BBox Planes 
            int i;                              // General Loop Counter
 
            // Find all candidate planes; select the plane nearest to the ray origin 
            // for each coordinate.
 
            double[] rgfMin = new double[] { box.X, box.Y, box.Z };
            double[] rgfMax = new double[] { box.X + box.SizeX, box.Y + box.SizeY, box.Z + box.SizeZ };
            double[] rgfRayPos = new double[] { origin.X, origin.Y, origin.Z };
            double[] rgfRayDir = new double[] { direction.X, direction.Y, direction.Z }; 

            for (i = 0; i < 3; ++i) 
            { 
                if (rgfRayPos[i] < rgfMin[i])
                { 
                    middle[i] = false;
                    plane[i] = rgfMin[i];
                    inside = false;
                } 
                else if (rgfRayPos[i] > rgfMax[i])
                { 
                    middle[i] = false; 
                    plane[i] = rgfMax[i];
                    inside = false; 
                }
                else
                {
                    middle[i] = true; 
                }
            } 
 
            // If the ray origin is inside the box, then it must intersect the volume
            // of the bounding box. 
            if (inside)
            {
                return true;
            } 

            double rayt; 
            if (isRay) 
            {
                // If we never end up finding the furthest plane, the box will be 
                // rejected since rayt is negative
                rayt = -1;
            }
            else 
            {
                // Can't use -1 in the line case because rayt^2 is 1 and we 
                // would miss valid ts in the furthest plane search 
                rayt = 0;
            } 

            int maxPlane = 0;
            for (i = 0; i < 3; ++i)
            { 
                if (!middle[i] && (rgfRayDir[i] != 0))
                { 
                    double t = (plane[i] - rgfRayPos[i]) / rgfRayDir[i]; 

                    if (isRay) 
                    {
                        if (t > rayt)
                        {
                            rayt = t; 
                            maxPlane = i;
                        } 
                    } 
                    else
                    { 
                        // In the original ray algorithm this test to find the furthest plane from the
                        // origin was t > rayt which only considered planes in the positive direction.
                        // I changed it to compare squared values so that we look for the farthest
                        // plane in either direction. 

                        // Note that if the line intersects the box then all of the planes considered 
                        // in this loop must be on the same side of the origin (because we are finding 
                        // the intersection of the line with the space formed by the intersection of
                        // the half-spaces formed by the planes -- which incidentally point away from 
                        // the origin.)
                        if (t * t > rayt * rayt)
                        {
                            rayt = t; 
                            maxPlane = i;
                        } 
                    } 
                }
            } 

            // If the box is behind the ray, or if the box is beyond the extent of the
            // ray, then return no-intersect.
 
            if (isRay && rayt < 0)
            { 
                return false; 
            }
 
            // The intersection candidate point is within acceptible range; test each
            // coordinate here to ensure that it actually hits the box.

            for (i = 0; i < 3; ++i) 
            {
                if (i != maxPlane) 
                { 
                    double c = rgfRayPos[i] + (rayt * rgfRayDir[i]);
                    if ((c < rgfMin[i]) || (rgfMax[i] < c)) 
                        return false;
                }
            }
 
            return true;
        } 
    } 
}

// File provided for Reference Use Only by Microsoft Corporation (c) 2007.
// Copyright (c) Microsoft Corporation. All rights reserved.
//---------------------------------------------------------------------------- 
// 
//    Copyright (C) Microsoft Corporation.  All rights reserved.
// 
//--------------------------------------------------------------------------- 

// GLOSSARY 
//     Kernel of matrix M : Space of vectors that vanish (go to 0) when multiplied by M 
// Null-space of matrix M : Same thing
//          Line matrix : Representation of a line using a matrix whose kernel is points on the line 
//                      L : Matrix representing a line
//                     LT : Transpose of L
//
// This file offers a small line transform utility function.  Given a line (lin) defined by Point3D 
// origin & Vector3D direction and a model matrix M it returns (in-place) a line (lout) in "model
// space" so that any point on the line when transformed by M is on the original line. 
// 
// In other words   x in lout ===>  x M in lin
// 
// This works even if M is rank-deficient, but if M is rank 2 or less then lout is not uniquely
// determined.
//
// The basic technique is as follows, we represent lin as a matrix where points on the line are in 
// the null-space (kernel) of lin (this is straightforward.)  Letting the symbols lin & lout refer
// to both the line & the corresponding matrix this means: 
// 
//        x in lin ===> x lin = [0,0,0,0]
// 
// Then we can transform a line matrix into model space by left multiplication with M
//
//        lout = M lin
// 
// That way,
// 
//        x in lout ===> x lout = [0,0,0,0] ===> x M lin = [0,0,0,0] ===> x M in lin 
//
// Which is what we want.  The hard part is going back from the matrix lout to a point and a vector. 
// The smallest two eigenvalues of the l matrix are zero.  The corresponding eigenvectors are two
// different points on the line.
//
// I find the eigenvectors and eigenvalues of a line matrix L by first computing the normal matrix 
// N = L LT which has symmetric eigenvectors equal to the left eigenvectors of L.  I find the
// eigenvectors of N using a Jacobi method for symmetric eigenvalue problems. 
// 
// The method is described in Chapter 8.4 of Golub & Van Loan (Matrix Computation), but here's a
// brief summary. 
//
// We apply a series of 2D rotations (A1...An) to the matrix N that each make the matrix more
// diagonal.  So if the whole sequence is A = A1 ... An then the final matrix E = AT N A is
// diagonal. 
//
// Because A is orthonormal its columns are the eigenvectors of N and the diagonal elements of E are 
// the eigenvalues. 
//
// NOTES 
//
// Forming the normal matrix N involves fourth powers of the input values.  I mitigate this by
// scaling the matrix so that its largest value is 1 before squaring it.  There may be a better
// method (perhaps even a modified Jacobi method) that would work directly on L and perhaps be more 
// stable.
// 
// None of this code understands rays.  This is all in terms of lines, lines, lines, lines! 

using System; 
using System.Collections.Generic;
using System.Diagnostics;
using System.Windows;
using System.Windows.Media.Media3D; 

using MS.Utility; 
 
namespace MS.Internal.Media3D
{ 
    [Flags]
    internal enum FaceType
    {
        None     = 0, 
        Front    = 1 << 0,
        Back     = 1 << 1, 
    }; 

    internal static class LineUtil 
    {
        // Coordinates of elements above the diagonal.
        readonly static int[,] s_pairs = new int[,]{ {0,1}, {0,2}, {0,3}, {1,2}, {1,3}, {2,3} };
        const int s_pairsCount = 6; 

        public static void Transform(Matrix3D modelMatrix, 
                                     ref Point3D origin, ref Vector3D direction, out bool isRay) 
        {
            if (modelMatrix.InvertCore()) 
            {
                Point4D o = new Point4D(origin.X,origin.Y,origin.Z,1);
                Point4D d = new Point4D(direction.X,direction.Y,direction.Z,0);
 
                modelMatrix.MultiplyPoint(ref o);
                modelMatrix.MultiplyPoint(ref d); 
 
                if (o.W == 1 && d.W == 0)
                { 
                    // Affine transformation

                    origin = new Point3D(o.X, o.Y, o.Z);
                    direction = new Vector3D(d.X, d.Y, d.Z); 

                    isRay = true; 
                } 
                else
                { 
                    // Non-affine transformation (likely projection)

                    // Form 4x2 matrix with two points on line in two columns.
                    double[,] linepoints = new double[,]{{o.X,d.X},{o.Y,d.Y},{o.Z,d.Z},{o.W,d.W}}; 

                    ColumnsToAffinePointVector(linepoints,0,1,out origin, out direction); 
 
                    isRay = false;
                } 
            }
            else
            {
                TransformSingular(ref modelMatrix, ref origin, ref direction); 

                isRay = false; 
            } 
        }
 
        // modelMatrix is passed by reference for efficiency only.  It is not modified.
        private static void TransformSingular(ref Matrix3D modelMatrix,
                                              ref Point3D origin, ref Vector3D direction)
        { 
            double [,] matrix = TransformedLineMatrix(ref modelMatrix, ref origin, ref direction);
            matrix = Square(matrix); 
 
            double[,] eigen = new double[,]{ {1,0,0,0}, {0,1,0,0}, {0,0,1,0}, {0,0,0,1} };
 
            // We'll just do 5 iterations with each pair because according to my results & Golub &
            // Van Loan this process converges quickly.
            int iterations = 5 * s_pairsCount;
            for (int iter = 0; iter < iterations; ++iter) 
            {
                int pair = iter % s_pairsCount; 
                JacobiRotation jrot = new JacobiRotation(s_pairs[pair,0],s_pairs[pair,1],matrix); 
                matrix = jrot.LeftRightMultiply(matrix);
                eigen = jrot.RightMultiply(eigen); 
            }

            // That was it as far as finding eigenvectors
 
            int evec1,evec2;
            FindSmallestTwoDiagonal(matrix, out evec1, out evec2); 
 
            // The eigenvectors corresponding to the two smallest eigenvalues are columns evec1 &
            // evec2.  These, in homogeneous space, are two different points on our line.  We are 
            // going to convert them to an affine point & vector.
            ColumnsToAffinePointVector(eigen, evec1, evec2, out origin, out direction);
        }
 
        private static void ColumnsToAffinePointVector(double[,] matrix, int col1, int col2, out Point3D origin, out Vector3D direction)
        { 
            // The col1 & col2 columsn of matrix are two different homogeneous points on a line.  We 
            // are going to convert them to an affine point & vector using the following procedure.
            //   1. Pick the eigenvector with the largest W, call it big 
            //   2. Scale it by 1/W to form an affine point, still call it big
            //   3. Add a weighted multiple of big to small to make small.W zero.
            //
            // If both w are zero then we have a projective line that intersects no affine points 
            // (i.e. it's a line at infinity.)  This will cause overflow but that's fine because the
            // line at infinity doesn't intersect anything & the caller of this function needs be 
            // able to handle results that come back with inf or nan by "doing nothing." 

            // Step 1. 
            if (matrix[3,col1]*matrix[3,col1] < matrix[3,col2]*matrix[3,col2])
            {
                int temp = col1;
                col1 = col2; 
                col2 = temp;
            } 
 
            // Step 2.
            double s = 1/matrix[3,col1]; 
            origin = new Point3D(s*matrix[0,col1],
                                 s*matrix[1,col1],
                                 s*matrix[2,col1]);
 
            // Step 3.
            s = -matrix[3,col2]; 
            direction = new Vector3D(matrix[0,col2]+s*origin.X, 
                                     matrix[1,col2]+s*origin.Y,
                                     matrix[2,col2]+s*origin.Z); 
        }


        // Returns the indices of the smallest two diagonal elements of matrix 
        private static void FindSmallestTwoDiagonal(double[,] matrix, out int evec1, out int evec2)
        { 
            evec1 = 0; 
            evec2 = 1;
            // And corresponding squared eigenvalues. 
            double eval1 = matrix[0,0]*matrix[0,0];
            double eval2 = matrix[1,1]*matrix[1,1];

            for (int i = 2; i < 4; ++i) 
            {
                // Replace second smallest if necessary. 
                double val = matrix[i,i]*matrix[i,i]; 
                if (val < eval1)
                { 
                    if (eval1 < eval2)
                    {
                        eval2 = val;
                        evec2 = i; 
                    }
                    else 
                    { 
                        eval1 = val;
                        evec1 = i; 
                    }
                }
                else if (val < eval2)
                { 
                    eval2 = val;
                    evec2 = i; 
                } 
            }
        } 

        // Returns the "line matrix" corresponding to the line (origin,direction) transformed by the
        // inverse *transform* of modelMatrix.  (To transform a line by the inverse transform
        // requires multiplying by the non-inverted matrix.) 
        private static double[,] TransformedLineMatrix(ref Matrix3D modelMatrix,
                                                       ref Point3D origin, ref Vector3D direction) 
        { 
                double x1 = origin.X;
                double y1 = origin.Y; 
                double z1 = origin.Z;
                // w1 = 1
                double x2 = direction.X;
                double y2 = direction.Y; 
                double z2 = direction.Z;
                // w2 = 0 
 
                // To prove to yourself that this matrix is correct just multiply by the two
                // (homogeneous) points on the line.  Any other homogeneous point on the line is a 
                // linear combination of them.

                double a = y2*z1-y1*z2;
                double b = x1*z2-x2*z1; 
                double c = x2*y1-x1*y2;
 
                Matrix3D m = modelMatrix * 
                             new Matrix3D(a,  y2,  z2,   0,
                                          b, -x2,   0,  z2, 
                                          c,   0, -x2, -y2,
                                          0,   c,  -b,   a);
                double[,] matrix = new double[4,4];
                matrix[0,0] = m.M11; 
                matrix[0,1] = m.M12;
                matrix[0,2] = m.M13; 
                matrix[0,3] = m.M14; 
                matrix[1,0] = m.M21;
                matrix[1,1] = m.M22; 
                matrix[1,2] = m.M23;
                matrix[1,3] = m.M24;
                matrix[2,0] = m.M31;
                matrix[2,1] = m.M32; 
                matrix[2,2] = m.M33;
                matrix[2,3] = m.M34; 
                matrix[3,0] = m.OffsetX; 
                matrix[3,1] = m.OffsetY;
                matrix[3,2] = m.OffsetZ; 
                matrix[3,3] = m.M44;
                return matrix;
        }
 
        // Scales M so that its largest element is 1 and then returns M MT
        // (MT=transpose(M)) 
        private static double [,] Square(double[,] m) 
        {
            double[,] o = new double[4,4]; 

            // Scale the matrix so that its largest element is 1.
            double maxvalue = 0;
            for (int i = 0; i < 4; ++i) 
            {
                for (int j = 0; j < 4; ++j) 
                { 
                    maxvalue = Math.Max(maxvalue,m[i,j]*m[i,j]);
                } 
            }
            maxvalue = Math.Sqrt(maxvalue);
            for (int i = 0; i < 4; ++i)
            { 
                for (int j = 0; j < 4; ++j)
                { 
                    m[i,j] /= maxvalue; 
                }
            } 

            // Compute its square.
            for (int i = 0; i < 4; ++i)
            { 
                for (int j = 0; j < 4; ++j)
                { 
                    double d = 0; 
                    for (int k = 0; k < 4; ++k)
                    { 
                        d += m[i,k]*m[j,k];
                    }
                    o[i,j] = d;
                } 
            }
            return o; 
        } 

        // See section 8.4 of Golub & Van Loan "Matrix Computation" 
        // Remember, J is this Jacobi rotation.  JT is the transpose.
        // Also section 5.1.8 shows the formula for multiplying by the Jacobi rotation
        // Though as they helpfully point out a Jacobi rotation is the same as a Givens rotation
        private struct JacobiRotation 
        {
            public JacobiRotation(int p, int q, double[,] a) 
            { 
                // Constructs a 2D rotation matrix M as follows
                // Start with identity. 
                // Zero the p & q rows & columns
                // Then set the intersections of these rows & columns as follows
                //    [ M_pp M_pq ]  =  [ c s]
                //    [ M_qp M_qq ]     [-s c] 
                //
                // Where c & s are a cosine-sine pair calculated so that multiplying a by this will 
                // descrease the the off-diagonal elements. 

                _p = p; 
                _q = q;

                double tau = (a[q,q] - a[p,p])/(2*a[p,q]);
                if (tau < Double.MaxValue && tau > -Double.MaxValue) 
                {
                    double root = Math.Sqrt(1+tau*tau); 
                    // Choose the smaller of -tau +/- root 
                    double t = -tau < 0 ? -tau + root : -tau - root;
                    _c = 1/Math.Sqrt(1+t*t); 
                    _s = t * _c;
                }
                else
                { 
                    _c = 1;
                    _s = 0; 
                } 
            }
 
            // These functions overwrite & return their argument.

            // returns JT a J
            public double[,] LeftRightMultiply(double [,] a) 
            {
                return RightMultiply(LeftMultiplyTranspose(a)); 
            } 

            // returns a J 
            public double[,] RightMultiply(double [,] a)
            {
                for (int j = 0; j < 4; ++j)
                { 
                    double tau1 = a[j,_p];
                    double tau2 = a[j,_q]; 
 
                    a[j,_p] = _c * tau1 - _s * tau2;
                    a[j,_q] = _s * tau1 + _c * tau2; 
                }

                return a;
            } 

 
            // returns JT a 
            public double[,] LeftMultiplyTranspose(double [,] a)
            { 
                for (int j = 0; j < 4; ++j)
                {
                    double tau1 = a[_p,j];
                    double tau2 = a[_q,j]; 

                    a[_p,j] = _c * tau1 - _s * tau2; 
                    a[_q,j] = _s * tau1 + _c * tau2; 
                }
 
                return a;
            }

            private int _p, _q; 
            private double _c, _s;
        } 
 
        // This method determines if the line/ray intersects the triangle.
        // If "origin" and "direction" truely represent a line, "type" should be front and back because 
        // we don't have any true direction.
        //
        //     origin/direction define the line/ray
        //     v0/v1/v2 define the triangle 
        //
        // origin, direction, v0, v1, v2 are passed by ref for perf.  They are NOT MODIFIED 
        // 
        // If this method returns false, ignore the values of hitCoord and dist.
        // 
        // Ported from dxg\d3dx9\mesh\intersect.cpp (12/04/03)
        // which is an implementation of "Fast, Minimum Storage Ray-Triangle Intersection" by Moller + Trumbore
        internal static bool ComputeLineTriangleIntersection(
            FaceType type, 
            ref Point3D origin,
            ref Vector3D direction, 
            ref Point3D v0, 
            ref Point3D v1,
            ref Point3D v2, 
            out Point hitCoord,
            out double dist)
        {
            Vector3D e1; 
            Point3D.Subtract(ref v1, ref v0, out e1);
            Vector3D e2; 
            Point3D.Subtract(ref v2, ref v0, out e2); 

            Vector3D r; 
            Vector3D.CrossProduct(ref direction, ref e2, out r);

            double a = Vector3D.DotProduct(ref e1, ref r);
 
            Vector3D s;
            if (a > 0 && (type & FaceType.Front) != 0) 
            { 
                Point3D.Subtract(ref origin, ref v0, out s);
            } 
            else if (a < 0 && (type & FaceType.Back) != 0)
            {
                Point3D.Subtract(ref v0, ref origin, out s);
                a = -a; 
            }
            else 
            { 
                hitCoord = new Point();
                dist = 0; 
                return false;
            }

            double u = Vector3D.DotProduct(ref s, ref r); 
            if ((u < 0) || (a < u))
            { 
                hitCoord = new Point(); 
                dist = 0;
                return false; 
            }

            Vector3D q;
            Vector3D.CrossProduct(ref s, ref e1, out q); 

            double v = Vector3D.DotProduct(ref direction, ref q); 
            if ((v < 0) || (a < (u + v))) 
            {
                hitCoord = new Point(); 
                dist = 0;
                return false;
            }
 
            double t = Vector3D.DotProduct(ref e2, ref q);
            double f = 1 / a; 
 
            t = t * f;
            u = u * f; 
            v = v * f;

            hitCoord = new Point(u, v);
            dist = t; 

            return true; 
        } 

        // This function returns true if the probe line intersects the bbox volume (not 
        // just the surface of the box).  Does LINE and RAY intersection tests.
        //
        // Based on Woo's method presented in Gems I, p. 395.  See also "Real-Time
        // Rendering", Haines, sec 10.4.2. 
        //
        //     origin/direction define the non-oriented line or ray 
        //     box is the volume to intersect 
        //
        // origin, direction, and box are passed by ref for perf.  They are NOT MODIFIED 
        //
        // Ported from dxg\d3dx9\mesh\intersect.cpp (12/04/03)
        internal static bool ComputeLineBoxIntersection(ref Point3D origin, ref Vector3D direction, ref Rect3D box, bool isRay)
        { 
            // Reject empty bounding boxes.
            if (box.IsEmpty) 
            { 
                return false;
            } 

            bool inside = true;
            bool[] middle = new bool[3];        // True if ray origin in middle for coord i.
            double[] plane = new double[3];     // Candidate BBox Planes 
            int i;                              // General Loop Counter
 
            // Find all candidate planes; select the plane nearest to the ray origin 
            // for each coordinate.
 
            double[] rgfMin = new double[] { box.X, box.Y, box.Z };
            double[] rgfMax = new double[] { box.X + box.SizeX, box.Y + box.SizeY, box.Z + box.SizeZ };
            double[] rgfRayPos = new double[] { origin.X, origin.Y, origin.Z };
            double[] rgfRayDir = new double[] { direction.X, direction.Y, direction.Z }; 

            for (i = 0; i < 3; ++i) 
            { 
                if (rgfRayPos[i] < rgfMin[i])
                { 
                    middle[i] = false;
                    plane[i] = rgfMin[i];
                    inside = false;
                } 
                else if (rgfRayPos[i] > rgfMax[i])
                { 
                    middle[i] = false; 
                    plane[i] = rgfMax[i];
                    inside = false; 
                }
                else
                {
                    middle[i] = true; 
                }
            } 
 
            // If the ray origin is inside the box, then it must intersect the volume
            // of the bounding box. 
            if (inside)
            {
                return true;
            } 

            double rayt; 
            if (isRay) 
            {
                // If we never end up finding the furthest plane, the box will be 
                // rejected since rayt is negative
                rayt = -1;
            }
            else 
            {
                // Can't use -1 in the line case because rayt^2 is 1 and we 
                // would miss valid ts in the furthest plane search 
                rayt = 0;
            } 

            int maxPlane = 0;
            for (i = 0; i < 3; ++i)
            { 
                if (!middle[i] && (rgfRayDir[i] != 0))
                { 
                    double t = (plane[i] - rgfRayPos[i]) / rgfRayDir[i]; 

                    if (isRay) 
                    {
                        if (t > rayt)
                        {
                            rayt = t; 
                            maxPlane = i;
                        } 
                    } 
                    else
                    { 
                        // In the original ray algorithm this test to find the furthest plane from the
                        // origin was t > rayt which only considered planes in the positive direction.
                        // I changed it to compare squared values so that we look for the farthest
                        // plane in either direction. 

                        // Note that if the line intersects the box then all of the planes considered 
                        // in this loop must be on the same side of the origin (because we are finding 
                        // the intersection of the line with the space formed by the intersection of
                        // the half-spaces formed by the planes -- which incidentally point away from 
                        // the origin.)
                        if (t * t > rayt * rayt)
                        {
                            rayt = t; 
                            maxPlane = i;
                        } 
                    } 
                }
            } 

            // If the box is behind the ray, or if the box is beyond the extent of the
            // ray, then return no-intersect.
 
            if (isRay && rayt < 0)
            { 
                return false; 
            }
 
            // The intersection candidate point is within acceptible range; test each
            // coordinate here to ensure that it actually hits the box.

            for (i = 0; i < 3; ++i) 
            {
                if (i != maxPlane) 
                { 
                    double c = rgfRayPos[i] + (rayt * rgfRayDir[i]);
                    if ((c < rgfMin[i]) || (rgfMax[i] < c)) 
                        return false;
                }
            }
 
            return true;
        } 
    } 
}

// File provided for Reference Use Only by Microsoft Corporation (c) 2007.
// Copyright (c) Microsoft Corporation. All rights reserved.

                        

Link Menu

Network programming in C#, Network Programming in VB.NET, Network Programming in .NET
This book is available now!
Buy at Amazon US or
Buy at Amazon UK