DoubleBlockContinuousImplicitDynamicSystem.java

/**
 * $Id$
 *
 * Copyright (C) 2004-2005 Koga Laboratory. All rights reserved.
 */

package org.mklab.tool.control.system.continuous;

import java.util.List;

import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.ode.SolverStopException;
import org.mklab.tool.control.system.DoubleSystemOperator;


/**
 * 陰的微分方程式で表現されるブロック連続時間動的システムを表わすクラスです。
 * 
 * @author koga
 * @version $Revision$
 */
public class DoubleBlockContinuousImplicitDynamicSystem extends DoubleBlockContinuousDynamicSystem implements DoubleContinuousImplicitDynamicSystem {
  /**
   * 新しく生成された<code>BlockContinuousDynamicSystem</code>オブジェクトを初期化します。
   * 
   * @param elements 隣接行列
   * @param inputNodes 入力ノードの番号のリスト(番号は1から始まります)
   * @param outputNodes 出力ノードの番号のリスト(番号は1から始まります)
   */
  public DoubleBlockContinuousImplicitDynamicSystem(final DoubleSystemOperator[][] elements, final List<Integer> inputNodes, final List<Integer> outputNodes) {
    super(elements, inputNodes, outputNodes);
  }

  /**
   * {@inheritDoc}
   */
  public DoubleMatrix stateEquation(final double t, final DoubleMatrix x, DoubleMatrix dx,  final DoubleMatrix u) throws SolverStopException {
    setState(x);
    setStateDerivative(dx);

    DoubleMatrix e = new DoubleMatrix(0, 1);
    for (final DoubleContinuousDynamicSystem system : this.continuousDynamicSystems) {
      if (system instanceof DoubleContinuousImplicitDynamicSystem) {
        e = e.appendDown(((DoubleContinuousImplicitDynamicSystem)system).stateEquation(t, system.getState(),  system.getStateDerivative(), getInputNodeValueOf((DoubleSystemOperator)system)));
      } else {
        if (((DoubleContinuousExplicitDynamicSystem)system).isDifferentialAlgebraicSystem()) {
          final DoubleMatrix m = ((DoubleContinuousExplicitDynamicSystem)system).getMatrixM();
          final DoubleMatrix leftHandSide = m.multiply(system.getStateDerivative());
          final DoubleMatrix rightHandSide = ((DoubleContinuousExplicitDynamicSystem)system).stateEquation(t, system.getState(), getInputNodeValueOf((DoubleSystemOperator)system));
          e = e.appendDown(leftHandSide.subtract(rightHandSide));
        } else {
          final DoubleMatrix leftHandSide = system.getStateDerivative();
          final DoubleMatrix rightHandSide = ((DoubleContinuousExplicitDynamicSystem)system).stateEquation(t, system.getState(), getInputNodeValueOf((DoubleSystemOperator)system));
          e = e.appendDown(leftHandSide.subtract(rightHandSide));
        }
      }
    }

    return e;
  }

  /**
   * {@inheritDoc}
   */
  public DoubleMatrix differentialEquation(final double t, final DoubleMatrix x, final DoubleMatrix dx, final DoubleMatrix inputOutput) throws SolverStopException {
    final DoubleMatrix u = inputOutput.getSubVector(1, getInputSize());
    return stateEquation(t, x, dx, u);
  }
}