Commit 71f850b0 authored by Andrey Filippov's avatar Andrey Filippov

Replacing Matrix operations

parent 099a66d6
<?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();
?>
<?php <?php
//require_once ('test-matrix-invert.php'); // matrix inversion require_once ('Matrix.php'); // matrix inversion
class PolynomialApproximation class PolynomialApproximation
{ {
// function __construct() { // function __construct() {
...@@ -39,32 +39,36 @@ class PolynomialApproximation ...@@ -39,32 +39,36 @@ class PolynomialApproximation
} }
} }
} }
$M = $this->zeroMatrix($N+1,$N+1); $aM = Matrix::zeroMatrix($N+1,$N+1);
$B = $this->zeroMatrix($N+1,1); $aB = Matrix::zeroMatrix($N+1,1);
for ($i=0; $i <= $N; $i++) { for ($i=0; $i <= $N; $i++) {
$B[$i][0] = $SF[$i]; $aB[$i][0] = $SF[$i];
for ($j = 0; $j <= $N; $j++) $M[$i][$j]= $S[$i + $j]; for ($j = 0; $j <= $N; $j++) $aM[$i][$j]= $S[$i + $j];
} }
$N1 = $N; $N1 = $N;
// TODO: use try/catch with solve $M = new Matrix($aM);
$B = new Matrix($aB);
if ($this->debugLevel > 1){ if ($this->debugLevel > 1){
fprintf($this->debugFile, fprintf($this->debugFile,
"polynomialApproximation1d(data,".$N.") M:\n"); "polynomialApproximation1d(data,".$N.") M:\n");
$this->print_matrix($this->debugFile, $M); $M->print($this->debugFile);
fprintf($this->debugFile, fprintf($this->debugFile,
"polynomialApproximation1d() B:\n"); "polynomialApproximation1d() B:\n");
$this->print_matrix($this->debugFile, $B); $B->print($this->debugFile);
} }
// while (!(new LUDecomposition(M)).isNonsingular() && (N1>0)){ // 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 ? while ($N1 >= 0) { // make N=0 legal ?
try { try {
$M_inv = $this->matrixInvert($M,$df); // $M_inv = $M.invert();
$R = $this->mmul($M_inv, $B); // $R = $M_inv->times($B);
$R = $M->solve($B);
if ($this->debugLevel > 1){ if ($this->debugLevel > 1){
fprintf($this->debugFile, fprintf($this->debugFile,
"polynomialApproximation1d() solution=\n"); "polynomialApproximation1d() solution=\n");
$this->print_matrix($this->debugFile, $R); $R->print($this->debugFile);
} }
$result=array_fill(0,$N+1,0); // new double [N+1]; $result=array_fill(0,$N+1,0); // new double [N+1];
for ($i = 0; $i <= $N1; $i++){ for ($i = 0; $i <= $N1; $i++){
...@@ -78,193 +82,22 @@ class PolynomialApproximation ...@@ -78,193 +82,22 @@ class PolynomialApproximation
return null; return null;
} }
$N1--; $N1--;
$M1 = $this->zeroMatrix($N+1,$N+1); $aM1 = Matrix::zeroMatrix($N+1,$N+1);
$B1 = $this->zeroMatrix($N+1,1); $aB1 = Matrix::zeroMatrix($N+1,1);
for ($i = 0; $i <= $N1; $i++) { for ($i = 0; $i <= $N1; $i++) {
$B1[$i][0] = $B[$i][0]; $aB1[$i][0] = $aB[$i][0];
for ($j = 0; $j <= $N1; $j++){ for ($j = 0; $j <= $N1; $j++){
$M1[$i][$j] = $M[$i][$j]; $aM1[$i][$j] = $aM[$i][$j];
} }
} }
$M = $M1; $M = new Matrix($aM1);
$B = $B1; $B = new Matrix($aB1);
} }
} }
return null; 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;
}
} }
?>
...@@ -51,10 +51,10 @@ http://192.168.1.250/motor.xml?Action=Stop&Distance=100&Units=1 - stop ...@@ -51,10 +51,10 @@ http://192.168.1.250/motor.xml?Action=Stop&Distance=100&Units=1 - stop
//php > var_dump($DishAngle); //php > var_dump($DishAngle);
//array(3) {[0]=> float(9.97), [1]=> float(0), [2]=> float(0)} //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"); 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); $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); $rslt = findMax1d($data, $USABLE_FRACT, $USABLE_POW, $ARGMAX_OURSIDE);
print_r($rslt); print_r($rslt);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment