BlockContinuousImplicitDynamicSystem.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.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.ode.SolverStopException;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.RealNumericalScalar;
import org.mklab.tool.control.system.SystemOperator;


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

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

    RM e = this.sunit.createZeroGrid(0, 1);
    for (final ContinuousDynamicSystem<RS,RM,CS,CM> system : this.continuousDynamicSystems) {
      if (system instanceof ContinuousImplicitDynamicSystem) {
        e = e.appendDown(((ContinuousImplicitDynamicSystem<RS,RM,CS,CM>)system).stateEquation(t, system.getState(),  system.getStateDerivative(), getInputNodeValueOf((SystemOperator<RS,RM,CS,CM>)system)));
      } else {
        if (((ContinuousExplicitDynamicSystem<RS,RM,CS,CM>)system).isDifferentialAlgebraicSystem()) {
          final RM m = ((ContinuousExplicitDynamicSystem<RS,RM,CS,CM>)system).getMatrixM();
          final RM leftHandSide = m.multiply(system.getStateDerivative());
          final RM rightHandSide = ((ContinuousExplicitDynamicSystem<RS,RM,CS,CM>)system).stateEquation(t, system.getState(), getInputNodeValueOf((SystemOperator<RS,RM,CS,CM>)system));
          e = e.appendDown(leftHandSide.subtract(rightHandSide));
        } else {
          final RM leftHandSide = system.getStateDerivative();
          final RM rightHandSide = ((ContinuousExplicitDynamicSystem<RS,RM,CS,CM>)system).stateEquation(t, system.getState(), getInputNodeValueOf((SystemOperator<RS,RM,CS,CM>)system));
          e = e.appendDown(leftHandSide.subtract(rightHandSide));
        }
      }
    }

    return e;
  }

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