DoubleAdjacencyConstantMatrix.java

/*
 * Created on 2007/12/08
 * Copyright (C) 2007 Koga Laboratory. All rights reserved.
 *
 */
package org.mklab.tool.control.system;

import java.io.Writer;
import java.util.ArrayList;

import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.MatrixSizeException;
import org.mklab.nfc.rpn.ExpressionProcessor;
import org.mklab.nfc.scalar.DoubleNumber;
import org.mklab.tool.control.system.continuous.DoubleContinuousLinearDynamicSystem;
import org.mklab.tool.control.system.continuous.DoubleIntegratorSystem;
import org.mklab.tool.control.system.math.DoubleConstantSystem;
import org.mklab.tool.control.system.math.DoubleNegativeUnitSystem;
import org.mklab.tool.control.system.math.DoubleUnitSystem;


/**
 * 隣接定数行列を表すクラスです。
 * 
 * @author Anan
 * @version $Revision: 1.8 $, 2007/12/08
 */
public class DoubleAdjacencyConstantMatrix extends DoubleAdjacencyMatrix {

  /** serialVersionUID */
  private static final long serialVersionUID = 6753883274746323945L;

  /**
   * 1行1列の隣接定数行列を生成します。
   */
  DoubleAdjacencyConstantMatrix() {
    this(0, 0);
  }

  /**
   * rowSizeとcolumnSizeを持つ隣接定数行列を生成します。
   * 
   * @param rowSize 行の数
   * @param columnSize 列の数
   */
  DoubleAdjacencyConstantMatrix(final int rowSize, final int columnSize) {
    this(new DoubleSystemOperator[rowSize][columnSize]);
  }

  /**
   * 引数を成分とする隣接定数行列を生成します。
   * 
   * @param elements 成分をもつ配列
   */
  DoubleAdjacencyConstantMatrix(final DoubleSystemOperator[][] elements) {
    super(elements);
    DoubleAdjacencyMatrixUtil.setZeroSystemToNullElement(elements);
  }

  /**
   * 引数の隣接定数行列に対応する単位行列をもつ隣接定数行列を作成します。
   * 
   * @param matrix 隣接定数行列
   * @param isNegative trueならば負の単位行列，falseならば正の単位行列
   */
  private DoubleAdjacencyConstantMatrix(final DoubleAdjacencyConstantMatrix matrix, final boolean isNegative) {
    this(matrix.getRowSize(), matrix.getRowSize());
    if (isNegative == true) {
      for (int i = 0; i < this.getMatrixSize(); i++) {
        for (int j = 0; j < matrix.getColumnSize(); j++) {
          if (matrix.getElement(i + 1, j + 1) == DoubleZeroSystem.getInstance()) {
            continue;
          }
          this.elements[i][i] = new DoubleNegativeUnitSystem((matrix.getElement(i + 1, j + 1)).getInputSize());
          break;
        }
      }
    } else {
      for (int i = 0; i < this.getMatrixSize(); i++) {
        for (int j = 0; j < matrix.getColumnSize(); j++) {
          if (matrix.getElement(i + 1, j + 1) == DoubleZeroSystem.getInstance()) {
            continue;
          }
          this.elements[i][i] = new DoubleUnitSystem((matrix.getElement(i + 1, j + 1)).getInputSize());
          break;
        }
      }
    }
  }

  /**
   * 各成分の符号を反転したシステムを成分とする隣接定数行列を返します。
   * 
   * @return 各成分の符号を反転したシステムを成分とする隣接定数行列
   */
  DoubleAdjacencyConstantMatrix unaryMinus() {
    DoubleAdjacencyConstantMatrix ans = new DoubleAdjacencyConstantMatrix(getRowSize(), getColumnSize());

    for (int i = 0; i < getRowSize(); i++) {
      for (int j = 0; j < getColumnSize(); j++) {
        if (getElement(i + 1, j + 1) == DoubleZeroSystem.getInstance()) {
          continue;
        }
        ans.elements[i][j] = ((DoubleConstantSystem)getElement(i + 1, j + 1)).unaryMinus();
      }
    }

    return ans;
  }

  /**
   * matrixSizeをもつ正方隣接定数行列を作成します。
   * 
   * @param matrixSize 行列の次数
   */
  DoubleAdjacencyConstantMatrix(int matrixSize) {
    this(matrixSize, matrixSize);
  }

  /**
   * 隣接定数行列同士を足し合わせます。
   * 
   * @param addend 加える行列
   * @return 足し合わされた結果
   */
  DoubleAdjacencyConstantMatrix add(final DoubleAdjacencyConstantMatrix addend) {
    if ((this.getRowSize() != addend.getRowSize()) || (this.getColumnSize() != addend.getColumnSize())) {
      throw new MatrixSizeException(this, addend, Messages.getString("AdjacencyConstantMatrix.0")); //$NON-NLS-1$
    }

    final DoubleSystemOperator[][] answerMatrix = new DoubleSystemOperator[this.getRowSize()][this.getColumnSize()];

    for (int row = 1; row <= this.getRowSize(); row++) {
      for (int column = 1; column <= this.getColumnSize(); column++) {
        if (this.getElement(row, column) == DoubleZeroSystem.getInstance() && addend.getElement(row, column) == DoubleZeroSystem.getInstance()) {
          answerMatrix[row - 1][column - 1] = DoubleZeroSystem.getInstance();
        } else if (addend.getElement(row, column) == DoubleZeroSystem.getInstance()) {
          answerMatrix[row - 1][column - 1] = this.getElement(row, column);
        } else if (this.getElement(row, column) == DoubleZeroSystem.getInstance()) {
          answerMatrix[row - 1][column - 1] = addend.getElement(row, column);
        } else {
          answerMatrix[row - 1][column - 1] = ((DoubleConstantSystem)this.getElement(row, column)).add((DoubleConstantSystem)addend.getElement(row, column));
        }
      }
    }

    return new DoubleAdjacencyConstantMatrix(answerMatrix);
  }

  /**
   * 隣接定数行列同士を掛け合わせます。
   * 
   * @param multiplier 掛ける行列
   * @return 掛け合わされた結果
   */
  DoubleAdjacencyConstantMatrix multiply(final DoubleAdjacencyConstantMatrix multiplier) {
    if (this.getColumnSize() != multiplier.getRowSize()) {
      throw new MatrixSizeException(this, multiplier, Messages.getString("AdjacencyConstantMatrix.1")); //$NON-NLS-1$
    }
    final DoubleSystemOperator[][] answerMatrix = new DoubleSystemOperator[this.getRowSize()][multiplier.getColumnSize()];

    for (int row = 1; row <= this.getRowSize(); row++) {
      for (int column = 1; column <= multiplier.getColumnSize(); column++) {
        for (int k = 1; k <= this.getColumnSize(); k++) {
          if (this.getElement(row, k) == DoubleZeroSystem.getInstance() && multiplier.getElement(k, column) == DoubleZeroSystem.getInstance()) {
            continue;
          } else if (multiplier.getElement(k, column) == DoubleZeroSystem.getInstance()) {
            continue;
          } else if (this.getElement(row, k) == DoubleZeroSystem.getInstance()) {
            continue;
          } else {
            if (answerMatrix[row - 1][column - 1] == null) {
              answerMatrix[row - 1][column - 1] = ((DoubleConstantSystem)this.getElement(row, k)).multiply((DoubleConstantSystem)multiplier.getElement(k, column));
            } else {
              answerMatrix[row - 1][column - 1] = ((DoubleConstantSystem)answerMatrix[row - 1][column - 1]).add(((DoubleConstantSystem)this.getElement(row, k)).multiply((DoubleConstantSystem)multiplier.getElement(k,
                  column)));
            }
          }
        }
      }
    }
    return new DoubleAdjacencyConstantMatrix(answerMatrix);
  }

  /**
   * 逆行列を計算します。
   * 
   * @return 逆行列の結果
   */
  DoubleAdjacencyConstantMatrix inverse() {
    if (this.getColumnSize() == 1 && this.getColumnSize() == 1) {
      final DoubleSystemOperator[][] inverse = new DoubleSystemOperator[1][1];
      if (this.elements[0][0].getLinearSystem().getD().isZero()) {
        throw new RuntimeException(Messages.getString("AdjacencyConstantMatrix.2")); //$NON-NLS-1$
      }
      inverse[0][0] = ((DoubleConstantSystem)this.elements[0][0]).inverse();
      return new DoubleAdjacencyConstantMatrix(inverse);
    }

    int i;
    for (i = 0; i < this.getRowSize(); i++) {
      if (this.elements[i][0] == DoubleZeroSystem.getInstance() || ((DoubleConstantSystem)this.elements[i][0]).getGain().isFullRank() == false) {
        continue;
      }
      this.exchangeRow(1, i + 1);
      break;
    }

    if (((DoubleConstantSystem)this.getElement(1, 1)).getGain().isFullRank()) {
      final DoubleAdjacencyConstantMatrix A = getSubMatrix(1, 1, 1, 1).createAdjacencyConstantMatrix();
      final DoubleAdjacencyConstantMatrix B = (getSubMatrix(1, 1, 2, this.getColumnSize())).createAdjacencyConstantMatrix();
      final DoubleAdjacencyConstantMatrix C = (getSubMatrix(2, this.getRowSize(), 1, 1)).createAdjacencyConstantMatrix();
      final DoubleAdjacencyConstantMatrix D = (getSubMatrix(2, this.getRowSize(), 2, this.getColumnSize())).createAdjacencyConstantMatrix();
      final DoubleAdjacencyConstantMatrix ans = createMatrixInversionLemmaWhenAIsRegular(A, B, C, D);
      ans.exchangeColumn(1, i + 1);
      return ans;
    }

    throw new RuntimeException(Messages.getString("AdjacencyConstantMatrix.2")); //$NON-NLS-1$
  }

  /**
   * 逆行列レンマを用いて逆行列を求めます。
   * 
   * @param a A
   * @param b B
   * @param c C
   * @param d D
   * @return DoubleConstantSystem[][] MatrixInversionLemma
   */
  private DoubleAdjacencyConstantMatrix createMatrixInversionLemmaWhenAIsRegular(DoubleAdjacencyConstantMatrix a, DoubleAdjacencyConstantMatrix b, DoubleAdjacencyConstantMatrix c, DoubleAdjacencyConstantMatrix d) {

    final DoubleAdjacencyConstantMatrix aInverse = a.inverse();
    final DoubleAdjacencyConstantMatrix aDet = d.add(c.unaryMinus().multiply(aInverse).multiply(b));
    final DoubleAdjacencyConstantMatrix aDetInverse = aDet.inverse();
    final DoubleAdjacencyConstantMatrix A = aInverse.add(aInverse.multiply(b).multiply(aDetInverse).multiply(c).multiply(aInverse));
    final DoubleAdjacencyConstantMatrix B = aInverse.unaryMinus().multiply(b).multiply(aDetInverse);
    final DoubleAdjacencyConstantMatrix C = aDetInverse.unaryMinus().multiply(c).multiply(aInverse);
    final DoubleAdjacencyConstantMatrix D = aDetInverse;

    return DoubleAdjacencyConstantMatrix.combineABCDBlock(A, B, C, D);
  }

  /**
   * @see org.mklab.nfc.matrix.Grid#printElements(java.io.Writer)
   */
  @Override
  public void printElements( Writer writer) {
    throw new UnsupportedOperationException();
  }

  /**
   * {@inheritDoc}
   */
  @Override
  public void printElements( Writer writer,  int maxColumnSize) {
    throw new UnsupportedOperationException();
  }

//  /**
//   * {@inheritDoc}
//   */
//  @Override
//  public DoubleSystemOperator[][] getElements() {
//    return this.elements;
//  }

  /**
   * thisの隣接定数行列に対応する単位行列表す隣接定数行列を作り、返します。
   * 
   * @param isNegative trueならば負の単位行列，falseならば正の単位行列
   * @return AdjacencyConstantMatrix thisの隣接定数行列に対応する単位行列表す隣接定数行列
   */
  DoubleAdjacencyConstantMatrix createUnitAdjacencyConstantMatrix(boolean isNegative) {
    return new DoubleAdjacencyConstantMatrix(this, isNegative);
  }

  /**
   * 隣接定数行列に対応する単位行列を表す隣接定数行列を返します．
   * 
   * @return AdjacencyConstantMatrix 単位行列表す隣接定数行列
   */
  DoubleAdjacencyConstantMatrix createUnit() {
    DoubleAdjacencyConstantMatrix unit = new DoubleAdjacencyConstantMatrix(this.getRowSize(), this.getRowSize());

    for (int i = 0; i < this.getMatrixSize(); i++) {
      final DoubleSystemOperator element = this.getElement(i + 1, i + 1);

      if (element == DoubleZeroSystem.getInstance()) {
        unit.elements[i][i] = new DoubleUnitSystem(1);
      } else {
        unit.elements[i][i] = new DoubleUnitSystem(element.getInputSize());
      }
    }

    return unit;
  }

  /**
   * 隣接定数行列のサイズを返します。
   * 
   * @return int 隣接定数行列のサイズ
   */
  private int getMatrixSize() {
    return this.elements.length;
  }

  /**
   * 隣接定数行列同士で減算をします。
   * 
   * @param subtrahend 引く行列
   * @return AdjacencyConstantMatrix 減算を行った隣接定数行列
   */
  DoubleAdjacencyConstantMatrix subtract(final DoubleAdjacencyConstantMatrix subtrahend) {
    return this.add(subtrahend.unaryMinus());
  }

  /**
   * 隣接定数行列を単位行列から減算します。
   * 
   * @return AdjacencyConstantMatrix 単位行列から減算を行った隣接定数行列
   */
  DoubleAdjacencyConstantMatrix subtractFromUnit() {
    return this.createUnit().subtract(this);
  }

  /**
   * A,B,C,Dのそれぞれの隣接定数行列を結合した隣接定数行列を返します。
   * 
   * @param a A
   * @param b B
   * @param c C
   * @param d D
   * @return AdjacencyConstantMatrix 連結された隣接定数行列
   */
  static DoubleAdjacencyConstantMatrix combineABCDBlock(DoubleAdjacencyConstantMatrix a, DoubleAdjacencyConstantMatrix b, DoubleAdjacencyConstantMatrix c, DoubleAdjacencyConstantMatrix d) {
    final DoubleAdjacencyConstantMatrix upperMatrix = a.appendRight(b).createAdjacencyConstantMatrix();
    final DoubleAdjacencyConstantMatrix lowerMatrix = c.appendRight(d).createAdjacencyConstantMatrix();
    return upperMatrix.appendDown(lowerMatrix).createAdjacencyConstantMatrix();
  }

  /**
   * 代数ループのサブマトリックスを取り出します。
   * 
   * @param loopList 代数ループが存在する行番号
   * @return 代数ループのサブマトリックス
   */
  @SuppressWarnings("boxing")
  DoubleAdjacencyConstantMatrix getLoopMatrix(final ArrayList<Integer> loopList) {
    return (getSubMatrix(loopList.get(0) + 1, loopList.get(0) + 1 + loopList.size() - 1, loopList.get(0) + 1, loopList.get(0) + loopList.size() + 1 - 1)).createAdjacencyConstantMatrix();
  }

  /**
   * 隣接定数行列からA行列(システム行列)を生成します．
   * 
   * @param stateSizes サブシステムの状態の数の配列
   * @param inputNodeSize 全システムの状態の数
   * @return A行列(システム行列)
   */
  DoubleAdjacencyConstantMatrix createA(final int[] stateSizes, final int inputNodeSize) {
    final int numberOfSubsystem = stateSizes.length;
    final int rowMin = inputNodeSize + 1;
    final int rowMax = inputNodeSize + numberOfSubsystem;
    final int columnMin = inputNodeSize + numberOfSubsystem + 1;
    final int columnMax = inputNodeSize + 2 * numberOfSubsystem;
    return getSubMatrix(rowMin, rowMax, columnMin, columnMax).transpose().createAdjacencyConstantMatrix();
  }

  /**
   * 隣接定数行列からB行列(入力行列)を生成します．
   * 
   * @param stateSizes サブシステムの状態の数の配列
   * @param inputNodeSize 全システムの状態の数
   * @return B行列(入力行列)
   */
  DoubleAdjacencyConstantMatrix createB(int[] stateSizes, int inputNodeSize) {
    final int numberOfSubsystem = stateSizes.length;
    final int rowMin = 1;
    final int rowMax = inputNodeSize;
    final int columnMin = inputNodeSize + numberOfSubsystem + 1;
    final int columnMax = inputNodeSize + 2 * numberOfSubsystem;
    return getSubMatrix(rowMin, rowMax, columnMin, columnMax).transpose().createAdjacencyConstantMatrix();
  }

  /**
   * 隣接定数行列からC行列(出力行列)を生成します．
   * 
   * @param stateSizes サブシステムの状態の数の配列
   * @param inputNodeSize 全システムの入力ノードの数
   * @param outputNodeSize 全システムの出力ノードの数
   * @return C行列(出力行列)
   */
  DoubleAdjacencyConstantMatrix createC(int[] stateSizes, int inputNodeSize, int outputNodeSize) {
    final int numberOfSubsystem = stateSizes.length;
    final int rowMin = inputNodeSize + 1;
    final int rowMax = inputNodeSize + numberOfSubsystem;
    final int columnMin = inputNodeSize + 2 * numberOfSubsystem + 1;
    final int columnMax = inputNodeSize + 2 * numberOfSubsystem + outputNodeSize;
    return getSubMatrix(rowMin, rowMax, columnMin, columnMax).transpose().createAdjacencyConstantMatrix();
  }

  /**
   * 隣接定数行列からD行列(直達行列)を生成します．
   * 
   * @param inputNodeSize 全システムの入力ノードの数
   * @param outputNodeSize 全システムの出力ノードの数
   * @return D行列(直達行列)
   */
  DoubleAdjacencyConstantMatrix createD(int inputNodeSize, int outputNodeSize) {
    final int rowMin = 1;
    final int rowMax = inputNodeSize;
    final int columnMax = this.getColumnSize();
    final int columnMin = this.getColumnSize() - outputNodeSize + 1;
    return getSubMatrix(rowMin, rowMax, columnMin, columnMax).transpose().createAdjacencyConstantMatrix();
  }

  /**
   * 定数行列からE行列(ディスクリプタ行列)を生成します．
   * 
   * @param stateSizes サブシステムの状態の数の配列
   * @param inputNodeSize 全システムの入力ノードの数
   * @return E行列(ディスクリプタ行列)
   */
  DoubleAdjacencyConstantMatrix createE(int[] stateSizes, int inputNodeSize) {
    final int numberOfSubsystem = stateSizes.length;
    final int rowMin = inputNodeSize + numberOfSubsystem + 1;
    final int rowMax = inputNodeSize + 2 * numberOfSubsystem;
    final int columnMin = inputNodeSize + numberOfSubsystem + 1;
    final int columnMax = inputNodeSize + 2 * numberOfSubsystem;
    final DoubleAdjacencyConstantMatrix E = getSubMatrix(rowMin, rowMax, columnMin, columnMax).transpose().createAdjacencyConstantMatrix();
    final DoubleAdjacencyConstantMatrix I = E.createUnit();
    return I.subtract(E);
  }

  /**
   * 成分を文字列で返します。
   * 
   * @return 成分の文字列
   */
  String getAsString() {
    final DoubleSystemOperator[][] matrix = getElements();
    final StringBuffer string = new StringBuffer();
    final String lineSeparator = System.getProperty("line.separator"); //$NON-NLS-1$

    for (int row = 0; row < matrix.length; row++) {
      for (int column = 0; column < matrix[0].length; column++) {
        final DoubleSystemOperator system = matrix[row][column];
        if (system instanceof DoubleZeroSystem) {
          string.append("0   "); //$NON-NLS-1$
        } else if (system instanceof DoubleConstantSystem && ((DoubleConstantSystem)system).isZeroOperand()) {
          string.append(new ExpressionProcessor<DoubleNumber,DoubleMatrix>().getResult(((DoubleConstantSystem)system)) + "   "); //$NON-NLS-1$
        } else if (system instanceof DoubleConstantSystem) {
          string.append(new ExpressionProcessor<DoubleNumber,DoubleMatrix>().getResult(((DoubleConstantSystem)system)) + "   "); //$NON-NLS-1$
        }
      }
      string.append(lineSeparator);
    }

    return string.toString();
  }
  
  /**
   * 中身を文字列で返します。
   * 
   * @return String 中身の文字列
   */
  @SuppressWarnings("nls")
  String getElementsAsString() {
    final DoubleSystemOperator[][] matrix = getElements();
    final StringBuffer string = new StringBuffer();
    final String lineSeparator = System.getProperty("line.separator");

    for (int row = 0; row < matrix.length; row++) {
      for (int column = 0; column < matrix.length; column++) {
        DoubleSystemOperator system = matrix[row][column];
        if (system instanceof DoubleZeroSystem) {
          string.append("0 ");
        } else if (system instanceof DoubleUnitSystem) {
          string.append("P ");
        } else if (system instanceof DoubleNegativeUnitSystem) {
          string.append("N ");
        } else if (system instanceof DoubleIntegratorSystem) {
          string.append("q ");
        } else if (system instanceof DoubleContinuousLinearDynamicSystem) {
          string.append("L ");
        } else if (system instanceof DoubleConstantSystem) {
          string.append(new ExpressionProcessor<DoubleNumber,DoubleMatrix>().getResult(((DoubleConstantSystem)system)) + " ");
        }
      }
      string.append(lineSeparator);
    }

    return string.toString();
  }

  /**
   * 隣接行列の成分を文字列で返します。
   * @return 隣接行列の成分の文字列
   */
  public String getAdjacencyConstantMatrixAsString() {
    return getAsString();
  }
}