Unwrap.java

/*
 * $Id: Unwrap.java,v 1.33 2008/03/15 00:23:40 koga Exp $
 * 
 * Copyright (C) 2004 Koga Laboratory. All rights reserved.
 */
package org.mklab.tool.matrix;

import org.mklab.nfc.matrix.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.IntMatrix;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.DoubleNumber;
import org.mklab.nfc.scalar.RealNumericalScalar;


/**
 * 位相を修正するクラスです。
 * 
 * <p>Correct phase angles for all elements
 * 
 * @author koga
 * @version $Revision: 1.33 $
 */
public class Unwrap {

  /**
   * なめらかに位相が変化するように、位相角に <code>+-2*PI</code> を加えます。
   * 
   * @param phase 元の位相
   * @return 修正した位相 (corrected phase angles)
   */
  public static DoubleMatrix unwrap(final DoubleMatrix phase) {
    final double tolerance = Math.PI * 170.0 / 180.0;
    return unwrap(phase, tolerance);
  }

  /**
   * 位相を360度で折り返さないようにします。
   * 
   * <p><code>tolerance</code>を位相のギャップを調べるために用います。
   * 
   * @param phase 位相
   * @param tolerance 許容誤差
   * @return 360度で折り返さないようにした位相
   */
  public static DoubleMatrix unwrap(final DoubleMatrix phase, final double tolerance) {
    return unwrap(phase, new DoubleNumber(tolerance));
  }

  /**
   * 位相を360度で折り返さないようにします。
   * 
   * <p><code>tolerance</code>を位相のギャップを調べるために用います。
   * 
   * @param phase 位相
   * @param tolerance 許容誤差
   * @return 360度で折り返さないようにした位相
   */
  public static DoubleMatrix unwrap(final DoubleMatrix phase, final DoubleNumber tolerance) {
    final int outputSize = phase.getRowSize();
    final int wSize = phase.getColumnSize();

    if (outputSize == 1 || wSize == 1) {
      return unwrapColumnWise(phase, tolerance);
    }

    final DoubleMatrix modifiedPhase = unwrapColumnWise(phase.reshape(outputSize * wSize, 1), tolerance);
    return modifiedPhase.reshape(outputSize, wSize);
  }

  /**
   * <code>phase</code> が行列のとき、行毎に位相角を修正します。
   * 
   * @param phase 元の位相
   * @return corrected phase angles
   */
  public static DoubleMatrix unwrapRowWise(final DoubleMatrix phase) {
    final double tolerance = Math.PI * 170.0 / 180.0;
    return unwrapRowWise(phase, tolerance);
  }

  /**
   * <code>tolerance</code> は、位相角のギャップを調べるために使われる。
   * 
   * @param phase 元の位相
   * @param tolerance 許容誤差
   * @return 修正した位相 (corrected phase angle)
   */
  public static DoubleMatrix unwrapRowWise(final DoubleMatrix phase, final double tolerance) {
    return unwrapColumnWise(phase.transpose(), tolerance).transpose();
  }

  /**
   * <code>tolerance</code> は、位相角のギャップを調べるために使われる。
   * 
   * @param phase 元の位相
   * @param tolerance 許容誤差
   * @return 修正した位相 (corrected phase angle)
   */
  public static DoubleMatrix unwrapRowWise(final DoubleMatrix phase, final DoubleNumber tolerance) {
    return unwrapColumnWise(phase.transpose(), tolerance).transpose();
  }

  /**
   * <code>phase</code> が行列のとき、位相角を列毎に修正します。
   * 
   * @param phase 元の位相
   * @return 修正した位相 (corrected phase angles)
   */
  public static DoubleMatrix unwrapColumnWise(final DoubleMatrix phase) {
    final double tolerance = Math.PI * 170.0 / 180.0;
    return unwrapColumnWise(phase, tolerance);
  }

  /**
   * <code>tolerance</code> を位相角のギャップを調べるために用いる。
   * 
   * @param phase 元の位相
   * @param tolerance 許容誤差
   * @return 修正した位相 (corrected phase angle)
   */
  public static DoubleMatrix unwrapColumnWise(final DoubleMatrix phase, final double tolerance) {
    return unwrapColumnWise(phase, new DoubleNumber(tolerance));
  }

  /**
   * <code>tolerance</code> を位相角のギャップを調べるために用いる。
   * 
   * @param phase 元の位相
   * @param tolerance 許容誤差
   * @return 修正した位相 (corrected phase angle)
   */
  public static DoubleMatrix unwrapColumnWise(final DoubleMatrix phase, final DoubleNumber tolerance) {
    DoubleMatrix ph = phase;
    int outputSize = ph.getRowSize();
    int wSize = ph.getColumnSize();

    int mo = outputSize;
    int no = wSize;

    if (mo < no) {
      ph = ph.transpose();
      outputSize = ph.getRowSize();
      wSize = ph.getColumnSize();
    }

    DoubleMatrix ph2 = ph;

    for (int j = 1; j <= wSize; j++) {
      final DoubleMatrix p = ph.getColumnVector(j);
      final DoubleMatrix pd = Diff.diff(p);
      final IntMatrix idx = pd.absElementWise().compareElementWise(".>", tolerance).find(); //$NON-NLS-1$
      for (int i = 1; i <= idx.length(); i++) {
        final int k = idx.getIntElement(i);
        if (2 <= k && sgn(pd.getElement(k - 1)) == sgn(pd.getElement(k))) {
          continue;
        }
        p.setSubMatrix(k + 1, outputSize, 1, 1, p.getSubVector(k + 1, outputSize).subtractElementWise(2 * Math.PI * sgn(pd.getElement(k))));
      }
      ph2.setColumnVector(j, p);
    }

    if (mo < no) {
      ph2 = ph2.transpose();
    }

    return ph2;
  }

  /**
   * 符号を返します。
   * 
   * @param x 実数データ
   * @return 符合
   */
  private static final int sgn(final DoubleNumber x) {
    if (x.isGreaterThan(0)) {
      return 1;
    } else if (x.isLessThan(0)) {
      return -1;
    } else {
      return 0;
    }
  }

  /**
   * <code>tolerance</code> を位相角のギャップを調べるために用いる。
   * 
   * @param phase 元の位相
   * @param tolerance 許容誤差
   * @return 修正した位相 (corrected phase angle)
   * @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 static <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>> RM unwrapColumnWise(
      final RM phase, final RS tolerance) {
    RM ph = phase;
    int outputSize = ph.getRowSize();
    int wSize = ph.getColumnSize();

    int mo = outputSize;
    int no = wSize;

    if (mo < no) {
      ph = ph.transpose();
      outputSize = ph.getRowSize();
      wSize = ph.getColumnSize();
    }

    RM ph2 = ph;

    for (int j = 1; j <= wSize; j++) {
      final RM p = ph.getColumnVector(j);
      final RM pd = Diff.diff(p);
      final IntMatrix idx = pd.absElementWise().compareElementWise(".>", tolerance).find(); //$NON-NLS-1$
      for (int i = 1; i <= idx.length(); i++) {
        final int k = idx.getIntElement(i);
        if (2 <= k && sgn(pd.getElement(k - 1)) == sgn(pd.getElement(k))) {
          continue;
        }
        p.setSubMatrix(k + 1, outputSize, 1, 1, p.getSubVector(k + 1, outputSize).subtractElementWise(2 * Math.PI * sgn(pd.getElement(k))));
      }
      ph2.setColumnVector(j, p);
    }

    if (mo < no) {
      ph2 = ph2.transpose();
    }

    return ph2;
  }

  /**
   * 符号を返します。
   * 
   * @param x 実数データ
   * @return 符合
   * @param <RS> type of real scalar
   * @param <RM> type of real matrix
   * @param <CS> type of complex scalar
   * @param <CM> type of complex matrix
   */
  private static <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>> int sgn(
      final RS x) {
    if (x.isGreaterThan(0)) {
      return 1;
    } else if (x.isLessThan(0)) {
      return -1;
    } else {
      return 0;
    }
  }

  /**
   * <code>tolerance</code> は、位相角のギャップを調べるために使われる。
   * 
   * @param phase 元の位相
   * @param tolerance 許容誤差
   * @return 修正した位相 (corrected phase angle)
   * @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 static <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>> RM unwrapRowWise(
      final RM phase, final RS tolerance) {
    return unwrapColumnWise(phase.transpose(), tolerance).transpose();
  }

  /**
   * <code>phase</code> が行列のとき、行毎に位相角を修正します。
   * 
   * @param phase 元の位相
   * @return corrected phase angles
   * @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 static <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>> RM unwrapRowWise(
      final RM phase) {
    final RS sunit = phase.getElement(1, 1).createUnit();
    final RS tolerance = sunit.createPI().multiply(170).divide(180);
    return unwrapRowWise(phase, tolerance);
  }

}