File: Core\CSharp\MS\Internal\Media3D\LineUtil.cs
Project: wpf\src\PresentationCore.csproj (PresentationCore)
//---------------------------------------------------------------------------
// <copyright file="LineUtil.cs" company="Microsoft">
//    Copyright (C) Microsoft Corporation.  All rights reserved.
// </copyright>
//---------------------------------------------------------------------------
 
// 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;
        }
    }
}