Replacing Matrix operations

Matrix.php

+<?php
+
+class Matrix
+{
+    private $M; // this matrix as a 2-D array
+    private $Tol;       // used in LUPDecompose
+    // LUP Decomposition, calculated on deamand (before first use of LUPSolve, LUPInvert, LUPDeterminant)
+    private $A =   null; // LU decomposition of the matrix
+    private $P =   null; // permutation array as defined in Wikipedia code:
+    public function __construct($M, $Tol = 1E-6){
+//        print("__construct\n");
+        if (is_array($M)){
+            $this->M = $M;
+            if (!is_array($M[0])){ // single-column array
+                for ($i = 0; $i< sizeof($M); $i++){
+                    $this->M[i] = array($M[i]);
+                }
+            }
+        }
+        $this->Tol = $Tol;
+    }
+    public function get(){
+        return $this->M;
+    }
+    
+    public function getColumnPackedCopy(){
+        
+    }
+    
+    public function times($B){
+        if ($B instanceof Matrix){
+            $B = $B->get();
+        }
+        if (is_array($B)) {
+            $rows1 = sizeof($this->M);
+            $cols1 = sizeof($this->M[0]);
+            if ($cols1 != sizeof($B)){
+                throw new Exception('Dimensions mismatch.');
+            }
+            $cols2 = sizeof($B[0]);
+            $R = $this->zeroMatrix($rows1, $cols2);
+            for ($i = 0; $i < $rows1; $i++){
+                for ($j = 0; $j < $cols2; $j++){
+                    for ($k = 0; $k < $cols1; $k++){
+                        $R[$i][$j] += $this->M[$i][$k] * $B[$k][$j];
+                    }
+                }
+            }
+        } else { // times scalar
+            $R=$this->M;
+            $rows = sizeof($this->M);
+            $cols = sizeof($this->M[0]);
+            for ($i = 0; $i < $rows; $i++){
+                for ($j = 0; $j < $cols; $j++){
+                    $R[$i][$j] *= $B;
+                }
+            }
+        }
+        return new Matrix ($R, $this->Tol);
+    }
+
+    public function plus($B) {
+        $R = $this->M;
+        $rows = sizeof($this->M);
+        $cols = sizeof($this->M[0]);
+        if (($rows != sizeof($B[0])) || ($cols != sizeof($B[0]))){
+            throw new Exception('Dimensions mismatch.');
+        }
+        for ($i = 0; $i < $rows; $i ++) {
+            for ($j = 0; $j < $cols; $j ++) {
+                $R[$i][$j] += $B[$i][$j];
+            }
+        }
+        return new Matrix ($R, $this->Tol);
+    }
+    
+    
+    public function print($debugFile = null, $decimals = 6)
+    {
+        if ($debugFile === null){
+            $debugFile = fopen('php://stdout', 'w');
+        }
+        foreach ($this->M as $row) {
+            fprintf($debugFile, "[");
+            foreach ($row as $i) {
+                fprintf($debugFile, "\t" . sprintf("%01.{$decimals}f", round($i, $decimals)));
+            }
+            fprintf($debugFile,  "\t]\n");
+        }
+    }
+    
+    
+    /*
+     *        The permutation matrix is not stored as a matrix, but in an integer vector P of size N+1
+     *        containing column indexes where the permutation matrix has "1". The last element P[N]=S+N,
+     *        where S is the number of row exchanges needed for determinant computation, det(P)=(-1)^S
+     */
+    // Ported from C code in Wikipedia https://en.wikipedia.org/wiki/LU_decomposition
+    /* INPUT: A - array of pointers to rows of a square matrix having dimension N
+     *        Tol - small tolerance number to detect failure when the matrix is near degenerate
+     * OUTPUT: Matrix A is changed, it contains a copy of both matrices L-E and U as A=(L-E)+U such that P*A=L*U.
+     *        The permutation matrix is not stored as a matrix, but in an integer vector P of size N+1
+     *        containing column indexes where the permutation matrix has "1". The last element P[N]=S+N,
+     *        where S is the number of row exchanges needed for determinant computation, det(P)=(-1)^S
+     */
+    /**
+     * Perform LU decomposition
+     * @param $A - square 2D array as input matrix, see Wikipedia comments above 
+     * @param $Tol - tolerance to catch singilar matrix
+     * @return - array $P for permutation matrix on success, null on failure (singular)
+     */
+
+    public function LUPDecompose (){
+        $N = sizeof($this->M);
+        $this->A = $this->M;
+        $this->P = array_fill(0,$N+1, 0.0);
+        for ($i = 0; $i <= $N; $i++) $this->P[$i] = $i; //Unit permutation matrix, P[N] initialized with N
+        for ($i = 0; $i < $N; $i++) {
+            $maxA = 0.0;
+            $imax = $i;
+            for ($k = $i; $k < $N; $k++) {
+                $absA = abs($this->A[$k][$i]);
+                if ($absA > $maxA) {
+                    $maxA = $absA;
+                    $imax = $k;
+                }
+            }
+            if ($maxA < $this->Tol) {
+                throw new Exception('Matrix is degenerate.'); //failure, matrix is degenerate
+            }
+            
+            if ($imax != $i) {
+                //pivoting P
+                $j = $this->P[$i];
+                $this->P[$i] = $this->P[$imax];
+                $this->P[$imax] = $j;
+                
+                //pivoting rows of A
+                $ptr =   $this->A[$i];
+                $this->A[$i] = $this->A[$imax];
+                $this->A[$imax] = $ptr;
+                
+                //counting pivots starting from N (for determinant)
+                $this->P[$N]++;
+            }
+            
+            for ($j = $i + 1; $j < $N; $j++) {
+                $this->A[$j][$i] /= $this->A[$i][$i];
+                
+                for ($k = $i + 1; $k < $N; $k++)
+                    $this->A[$j][$k] -= $this->A[$j][$i] * $this->A[$i][$k];
+            }
+        }
+    }
+    /* INPUT: A,P filled in LUPDecompose; b - rhs vector; N - dimension
+     * OUTPUT: x - solution vector of A*x=b
+     */
+    public function solve($b) {
+        if ($this->A === null){
+            $this->LUPDecompose();
+        }
+        if ($b instanceof Matrix){
+            $b = $b->get();
+        }
+        
+        $N = sizeof($this->M);
+        $x = array_fill(0,$N,0.0);
+        for ($i = 0; $i < $N; $i++) {
+            $x[$i] = $b[$this->P[$i]];
+            for ($k = 0; $k < $i; $k++)
+                $x[$i] -= $this->M[$i][$k] * $x[$k];
+        }
+        for ($i = $N - 1; $i >= 0; $i--) {
+            for ($k = $i + 1; $k < $N; $k++)
+                $x[$i] -= $this->M[$i][$k] * $x[$k];
+                $x[$i] = $x[$i] / $this->M[$i][$i];
+        }
+        return new Matrix ($x, $this->Tol);
+    }
+
+        /*
+     * INPUT: A,P filled in LUPDecompose; N - dimension
+     * OUTPUT: IA is the inverse of the initial matrix
+     */
+    public function invert()
+    {
+        if ($this->A === null){
+            $this->LUPDecompose();
+        }
+        $N = sizeof($this->A);
+        $IA = $this->zeroMatrix($N, $N);
+        for ($j = 0; $j < $N; $j ++) {
+            for ($i = 0; $i < $N; $i ++) {
+                if ($this->P[$i] == $j)
+                    $IA[$i][$j] = 1.0;
+                else
+                    $IA[$i][$j] = 0.0;
+                for ($k = 0; $k < $i; $k ++)
+                    $IA[$i][$j] -= $this->A[$i][$k] * $IA[$k][$j];
+            }
+            for ($i = $N - 1; $i >= 0; $i --) {
+                for ($k = $i + 1; $k < $N; $k ++)
+                    $IA[$i][$j] -= $this->A[$i][$k] * $IA[$k][$j];
+                $IA[$i][$j] = $IA[$i][$j] / $this->A[$i][$i];
+            }
+        }
+        return new Matrix ($IA, $this->Tol);
+    }
+
+     /*
+     * INPUT: A,P filled in LUPDecompose; N - dimension.
+     * OUTPUT: Function returns the determinant of the initial matrix
+     */
+    public function det()
+    {
+        if ($this->A === null){
+            $this->LUPDecompose();
+        }
+        $N = sizeof($this->A);
+        $det = $this->A[0][0];
+        for ($i = 1; $i < $N; $i ++)
+            $det *= $this->A[$i][$i];
+            if (($this->P[$N] - $N) % 2 == 0)
+            return $det;
+        else
+            return - $det;
+    }
+    
+    public function isNonsingular(){
+        if ($this->A === null){
+            try {
+            $this->LUPDecompose();
+            } catch (Exception $e){
+                return 0;  // Singular
+            }
+        }
+        return ($this->det())!=0.0; // Non-singular
+    }
+    
+    public static function zeroMatrix($m,$n=1){
+        $M = array_fill(0, $m, null);
+        for ($i = 0; $i < $m; $i++){
+            $M[$i] = array_fill(0, $n, 0.0);
+        }
+        return $M;
+    }
+    
+    public static function identityMatrix($n)
+    {
+        $I = array();
+        for ($i = 0; $i < $n; ++ $i) {
+            for ($j = 0; $j < $n; ++ $j) {
+                $I[$i][$j] = ($i == $j) ? 1 : 0;
+            }
+        }
+        return $I;
+    }
+    
+}
+
+// test code here
+$A = array(
+    array( 10, -15,  30,   6, -8 ),
+    array(  0,  -4,  60,  11, -5 ),
+    array(  8,   9,   2,   3,  7 ),
+    array( 25,  10,  -9,   9,  0 ),
+    array( 13,   3, -12,   5,  2 ),
+);
+$SING = array(
+    array( 10, -15,  30,   6, -8 ),
+    array(  0,  -4,  60,  11, -5 ),
+    array(  8,   9,   2,   3,  7 ),
+    array( 25,  10,  -9,   9,  0 ),
+    array( 10, -15,  30,   6, -8 ),
+);
+
+
+$M1 = new Matrix($A);
+print("\nM1=\n");
+$M1->print(); 
+//exit(0);
+$IM1 = $M1->invert();
+print("\nIM1=\n");
+$IM1->print();
+$M2 = $IM1->invert();
+print("\nM2=\n");
+$M2->print();
+
+$det= $M1->det();
+print("\ndet(M1)=".$det."\n");
+$idet= $IM1->det();
+print("\ndet(IM1)=".$idet."\n");
+print("\ndet(M1)*det(IM1)=".($det * $idet)."\n");
+
+$MS = new Matrix($SING);
+print("\nM1 nonsingular=".$M1->isNonsingular()."\n");
+print("\nMS nonsingular=".$MS->isNonsingular()."\n");
+
+print("\nM1 * IM1=\n");
+$M1->times($IM1)->print();
+
+?>

PolynomialApproximation.php

 <?php
-//require_once ('test-matrix-invert.php'); // matrix inversion
+require_once ('Matrix.php'); // matrix inversion
 class PolynomialApproximation
 {
 //    function __construct() {
@@ -39,32 +39,36 @@ class PolynomialApproximation
                 }
             }
         }
-        $M = $this->zeroMatrix($N+1,$N+1);
-        $B = $this->zeroMatrix($N+1,1);
+        $aM = Matrix::zeroMatrix($N+1,$N+1);
+        $aB = Matrix::zeroMatrix($N+1,1);
         for ($i=0; $i <= $N; $i++) {
-            $B[$i][0] = $SF[$i];
-            for ($j = 0; $j <= $N; $j++) $M[$i][$j]= $S[$i + $j];
+            $aB[$i][0] = $SF[$i];
+            for ($j = 0; $j <= $N; $j++) $aM[$i][$j]= $S[$i + $j];
         }
         $N1 = $N;
-        // TODO: use try/catch with solve
+        $M =  new Matrix($aM);
+        $B =  new Matrix($aB);
+        
         if ($this->debugLevel > 1){
             fprintf($this->debugFile,
                 "polynomialApproximation1d(data,".$N.") M:\n");
-            $this->print_matrix($this->debugFile, $M);
+            $M->print($this->debugFile);
             fprintf($this->debugFile,
                 "polynomialApproximation1d() B:\n");
-            $this->print_matrix($this->debugFile, $B);
+            $B->print($this->debugFile);
         }
             // while (!(new LUDecomposition(M)).isNonsingular() && (N1>0)){
-        $df = ($this->debugLevel > 2) ? $this->debugFile : null;
+//        $df = ($this->debugLevel > 2) ? $this->debugFile : null;
         while ($N1 >= 0) { // make N=0 legal ?
             try {
-                $M_inv = $this->matrixInvert($M,$df);
-                $R = $this->mmul($M_inv, $B);
+//                $M_inv = $M.invert();
+//                $R = $M_inv->times($B);
+                $R = $M->solve($B); 
+                
                 if ($this->debugLevel > 1){
                     fprintf($this->debugFile,
                         "polynomialApproximation1d() solution=\n");
-                    $this->print_matrix($this->debugFile, $R);
+                    $R->print($this->debugFile);
                 }
                 $result=array_fill(0,$N+1,0); // new double [N+1];
                 for ($i = 0; $i <= $N1; $i++){
@@ -78,193 +82,22 @@ class PolynomialApproximation
                     return null;
                 }
                 $N1--;
-                $M1 = $this->zeroMatrix($N+1,$N+1);
-                $B1 = $this->zeroMatrix($N+1,1);
+                $aM1 = Matrix::zeroMatrix($N+1,$N+1);
+                $aB1 = Matrix::zeroMatrix($N+1,1);
                 for ($i = 0; $i <= $N1; $i++) {
-                    $B1[$i][0] = $B[$i][0];
+                    $aB1[$i][0] = $aB[$i][0];
                     for ($j = 0; $j <= $N1; $j++){
-                        $M1[$i][$j] = $M[$i][$j];
+                        $aM1[$i][$j] = $aM[$i][$j];
                     }
                 }
-                $M = $M1;
-                $B = $B1;
+                $M =  new Matrix($aM1);
+                $B =  new Matrix($aB1);
             }
         }
         return null;
     }
     
     
-    public function mmul($A, $B){
-        $M = sizeof($A);
-        $N = sizeof($A[0]);
-        if ($N != sizeof($B)){
-            throw new Exception('DImensions mismatch.');
-        }
-        $K = sizeof($B[0]);
-        $R = $this->zeroMatrix($M,$K);
-        for ($i = 0; $i < $M; $i++){
-            for ($j = 0; $j < $K; $j++){
-                for ($k = 0; $k < $N; $k++){
-                    $R[$i][$j] += $A[$i][$k] * $B[$k][$j];
-                }
-            }
-        }
-        return $R;
-    }
-    
-    public function zeroMatrix($m,$n=1){
-        $M = array_fill(0, $m, null);
-        for ($i = 0; $i < $m; $i++){
-            $M[$i] = array_fill(0, $n, 0.0);
-        }
-        return $M;
-    }
-    
-    /**
-     * Inverts a given matrix(downloaded from https://gist.github.com/unix1/7510208 )
-     *
-     * @param array $A matrix to invert
-     * @param string $debugFile if not null - output file for debug info
-     *
-     * @return array inverted matrix
-     */
-    public function matrixInvert($A, $debugFile = null)
-    {
-        /// @todo check rows = columns
-        if ($debugFile !== null) {
-            fprintf($debugFile, "\Input matrix for inversion:\n");
-            $this->print_matrix($debugFile, $A);
-        }
-        
-        
-        $n = count($A);
-        
-        // get and append identity matrix
-        $I = $this->identity_matrix($n);
-        for ($i = 0; $i < $n; ++ $i) {
-            $A[$i] = array_merge($A[$i], $I[$i]);
-        }
-        
-        if ($debugFile !== null) {
-            fprintf($debugFile, "\nStarting matrix:\n");
-            //		echo "\nStarting matrix: ";
-            $this->print_matrix($debugFile, $A);
-        }
-        
-        // forward run
-        for ($j = 0; $j < $n-1; ++ $j) {
-            // for all remaining rows (diagonally)
-            for ($i = $j+1; $i < $n; ++ $i) {
-                // if the value is not already 0
-                if ($A[$i][$j] != 0) {
-                    // adjust scale to pivot row
-                    // subtract pivot row from current
-                    $scalar = $A[$j][$j] / $A[$i][$j]; // division by zero
-                    for ($jj = $j; $jj < $n*2; ++ $jj) {
-                        $A[$i][$jj] *= $scalar;
-                        $A[$i][$jj] -= $A[$j][$jj];
-                    }
-                }
-            }
-            //		if ($debug) {
-            if ($debugFile !== null) {
-                fprintf($debugFile, "\nForward iteration $j:\n");
-                //			echo "\nForward iteration $j: ";
-                $this->print_matrix($debugFile, $A);
-            }
-        }
-        
-        // reverse run
-        for ($j = $n-1; $j > 0; -- $j) {
-            for ($i = $j-1; $i >= 0; -- $i) {
-                if ($A[$i][$j] != 0) {
-                    $scalar = $A[$j][$j] / $A[$i][$j];
-                    for ($jj = $i; $jj < $n*2; ++ $jj) {
-                        $A[$i][$jj] *= $scalar;
-                        $A[$i][$jj] -= $A[$j][$jj];
-                    }
-                }
-            }
-            //		if ($debug) {
-            if ($debugFile !== null) {
-                fprintf($debugFile, "\nReverse iteration $j:\n");
-                //			echo "\nReverse iteration $j: ";
-                $this->print_matrix($debugFile, $A);
-            }
-        }
-        
-        // last run to make all diagonal 1s
-        /// @note this can be done in last iteration (i.e. reverse run) too!
-        for ($j = 0; $j < $n; ++ $j) {
-            if ($A[$j][$j] !== 1) {
-                if ($A[$j][$j] == 0.0) {
-                    //		        echo "Denominator is zero, throwing exception: \n";
-                    throw new Exception('Singular matrix.');
-                }
-                $scalar = 1 / $A[$j][$j];
-                for ($jj = $j; $jj < $n*2; ++ $jj) {
-                    $A[$j][$jj] *= $scalar;
-                }
-            }
-            //		if ($debug) {
-            if ($debugFile !== null) {
-                fprintf($debugFile, "\n1-out iteration $j:\n");
-                //			echo "\n1-out iteration $j: ";
-                $this->print_matrix($debugFile, $A);
-            }
-        }
-        
-        // take out the matrix inverse to return
-        $Inv = array();
-        for ($i = 0; $i < $n; ++ $i) {
-            $Inv[$i] = array_slice($A[$i], $n);
-        }
-        
-        return $Inv;
-    }
-    
-    /**
-     * Prints matrix
-     * @param resource $debugFile debug file or null
-     * @param array $A matrix
-     * @param integer $decimals number of decimals
-     */
-    function print_matrix($debugFile, $A, $decimals = 6)
-    {
-        if ($debugFile === null){
-            $debugFile = fopen('php://stdout', 'w');
-        }
-        foreach ($A as $row) {
-            fprintf($debugFile, "[");
-            foreach ($row as $i) {
-                fprintf($debugFile, "\t" . sprintf("%01.{$decimals}f", round($i, $decimals)));
-            }
-            fprintf($debugFile,  "\t]\n");
-        }
-    }
-    
-    /**
-     * Produces an identity matrix of given size
-     *
-     * @param integer $n size of identity matrix
-     *
-     * @return array identity matrix
-     */
-    function identity_matrix($n)
-    {
-        $I = array();
-        for ($i = 0; $i < $n; ++ $i) {
-            for ($j = 0; $j < $n; ++ $j) {
-                $I[$i][$j] = ($i == $j) ? 1 : 0;
-            }
-        }
-        return $I;
-    }
-    
-    
-        
-    
-    
     
 }
-
+?>

test_01.php

@@ -51,10 +51,10 @@ http://192.168.1.250/motor.xml?Action=Stop&Distance=100&Units=1 - stop
 
 //php > var_dump($DishAngle);
 //array(3) {[0]=> float(9.97), [1]=> float(0), [2]=> float(0)}
-$dbg_file = fopen("/home/eyesis/git/motosat/attic/logs/test03.log","w");
+$dbg_file = fopen("/home/eyesis/git/motosat/attic/logs/test04.log","w");
 fprintf($dbg_file,"test log\n");
 
-if (false) {
+if (true) {
     $data = Array(5,6,7,9,9,10,11,11,12,12,11,11,10,10,10,36,40,45,47,51,51,51,49,48,43,38,30,30,30,10,10,10,10);
     $rslt = findMax1d($data, $USABLE_FRACT, $USABLE_POW, $ARGMAX_OURSIDE);
     print_r($rslt);