Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
#include "toonz/ikjacobian.h"
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
#include <stdlib.h></stdlib.h>
Toshihiro Shimizu 890ddd
#include <math.h></math.h>
Toshihiro Shimizu 890ddd
#include <assert.h></assert.h>
Toshihiro Shimizu 890ddd
#include <iostream></iostream>
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
#include "tstopwatch.h"
Toshihiro Shimizu 890ddd
using namespace std;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
inline bool NearZero(double x, double tolerance) {
Shinya Kitaoka 120a6e
  return (fabs(x) <= tolerance);
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
/*
Toshihiro Shimizu 890ddd
#ifdef _DYNAMIC
Toshihiro Shimizu 890ddd
const double BASEMAXDIST = 0.02;
Toshihiro Shimizu 890ddd
#else
Shinya Kitaoka 120a6e
const double MAXDIST = 0.08;
Toshihiro Shimizu 890ddd
#endif
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
const double DELTA = 0.4;
Toshihiro Shimizu 890ddd
const long double LAMBDA = 2.0;		// solo per DLS. ottimale : 0.24
Toshihiro Shimizu 890ddd
const double NEARZERO = 0.0000000001;
Toshihiro Shimizu 890ddd
*/
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//*******************************************************
Toshihiro Shimizu 890ddd
// Class VectorRn
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
VectorRn VectorRn::WorkVector;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
double VectorRn::MaxAbs() const {
Shinya Kitaoka 120a6e
  double result = 0.0;
Shinya Kitaoka 120a6e
  double *t     = x;
Shinya Kitaoka 120a6e
  for (long i = length; i > 0; i--) {
Shinya Kitaoka 120a6e
    if ((*t) > result) {
Shinya Kitaoka 120a6e
      result = *t;
Shinya Kitaoka 120a6e
    } else if (-(*t) > result) {
Shinya Kitaoka 120a6e
      result = -(*t);
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    t++;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
  return result;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//*************************************************************************
Toshihiro Shimizu 890ddd
// MatrixRmn
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
MatrixRmn MatrixRmn::WorkMatrix;  // Temporary work matrix
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
// Fill the diagonal entries with the value d.  The rest of the matrix is
Shinya Kitaoka 120a6e
// unchanged.
Shinya Kitaoka 120a6e
void MatrixRmn::SetDiagonalEntries(double d) {
Shinya Kitaoka 120a6e
  long diagLen = std::min(NumRows, NumCols);
Shinya Kitaoka 120a6e
  double *dPtr = x;
Shinya Kitaoka 120a6e
  for (; diagLen > 0; diagLen--) {
Shinya Kitaoka 120a6e
    *dPtr = d;
Shinya Kitaoka 120a6e
    dPtr += NumRows + 1;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
// Fill the diagonal entries with values in vector d.  The rest of the matrix is
Shinya Kitaoka 120a6e
// unchanged.
Shinya Kitaoka 120a6e
void MatrixRmn::SetDiagonalEntries(const VectorRn &d) {
Shinya Kitaoka 120a6e
  long diagLen = std::min(NumRows, NumCols);
Shinya Kitaoka 120a6e
  assert(d.length == diagLen);
Shinya Kitaoka 120a6e
  double *dPtr = x;
Shinya Kitaoka 120a6e
  double *from = d.x;
Shinya Kitaoka 120a6e
  for (; diagLen > 0; diagLen--) {
Shinya Kitaoka 120a6e
    *dPtr = *(from++);
Shinya Kitaoka 120a6e
    dPtr += NumRows + 1;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
// Fill the superdiagonal entries with the value d.  The rest of the matrix is
Shinya Kitaoka 120a6e
// unchanged.
Shinya Kitaoka 120a6e
void MatrixRmn::SetSuperDiagonalEntries(double d) {
Shinya Kitaoka 120a6e
  long sDiagLen = std::min(NumRows, (long)(NumCols - 1));
Shinya Kitaoka 120a6e
  double *to    = x + NumRows;
Shinya Kitaoka 120a6e
  for (; sDiagLen > 0; sDiagLen--) {
Shinya Kitaoka 120a6e
    *to = d;
Shinya Kitaoka 120a6e
    to += NumRows + 1;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
// Fill the superdiagonal entries with values in vector d.  The rest of the
Shinya Kitaoka 120a6e
// matrix is unchanged.
Shinya Kitaoka 120a6e
void MatrixRmn::SetSuperDiagonalEntries(const VectorRn &d) {
Shinya Kitaoka 120a6e
  long sDiagLen = std::min((long)(NumRows - 1), NumCols);
Shinya Kitaoka 120a6e
  assert(sDiagLen == d.length);
Shinya Kitaoka 120a6e
  double *to   = x + NumRows;
Shinya Kitaoka 120a6e
  double *from = d.x;
Shinya Kitaoka 120a6e
  for (; sDiagLen > 0; sDiagLen--) {
Shinya Kitaoka 120a6e
    *to = *(from++);
Shinya Kitaoka 120a6e
    to += NumRows + 1;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
// Fill the subdiagonal entries with the value d.  The rest of the matrix is
Shinya Kitaoka 120a6e
// unchanged.
Shinya Kitaoka 120a6e
void MatrixRmn::SetSubDiagonalEntries(double d) {
Shinya Kitaoka 120a6e
  long sDiagLen = std::min(NumRows, NumCols) - 1;
Shinya Kitaoka 120a6e
  double *to    = x + 1;
Shinya Kitaoka 120a6e
  for (; sDiagLen > 0; sDiagLen--) {
Shinya Kitaoka 120a6e
    *to = d;
Shinya Kitaoka 120a6e
    to += NumRows + 1;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
// Fill the subdiagonal entries with values in vector d.  The rest of the matrix
Shinya Kitaoka 120a6e
// is unchanged.
Shinya Kitaoka 120a6e
void MatrixRmn::SetSubDiagonalEntries(const VectorRn &d) {
Shinya Kitaoka 120a6e
  long sDiagLen = std::min(NumRows, NumCols) - 1;
Shinya Kitaoka 120a6e
  assert(sDiagLen == d.length);
Shinya Kitaoka 120a6e
  double *to   = x + 1;
Shinya Kitaoka 120a6e
  double *from = d.x;
Shinya Kitaoka 120a6e
  for (; sDiagLen > 0; sDiagLen--) {
Shinya Kitaoka 120a6e
    *to = *(from++);
Shinya Kitaoka 120a6e
    to += NumRows + 1;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
// Set the i-th column equal to d.
Shinya Kitaoka 120a6e
void MatrixRmn::SetColumn(long i, const VectorRn &d) {
Shinya Kitaoka 120a6e
  assert(NumRows == d.GetLength());
Shinya Kitaoka 120a6e
  double *to         = x + i * NumRows;
Shinya Kitaoka 120a6e
  const double *from = d.x;
Shinya Kitaoka 120a6e
  for (i = NumRows; i > 0; i--) {
Shinya Kitaoka 120a6e
    *(to++) = *(from++);
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
// Set the i-th column equal to d.
Shinya Kitaoka 120a6e
void MatrixRmn::SetRow(long i, const VectorRn &d) {
Shinya Kitaoka 120a6e
  assert(NumCols == d.GetLength());
Shinya Kitaoka 120a6e
  double *to         = x + i;
Shinya Kitaoka 120a6e
  const double *from = d.x;
Shinya Kitaoka 120a6e
  for (i = NumRows; i > 0; i--) {
Shinya Kitaoka 120a6e
    *to = *(from++);
Shinya Kitaoka 120a6e
    to += NumRows;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
// Sets a "linear" portion of the array with the values from a vector d
Toshihiro Shimizu 890ddd
// The first row and column position are given by startRow, startCol.
Toshihiro Shimizu 890ddd
// Successive positions are found by using the deltaRow, deltaCol values
Toshihiro Shimizu 890ddd
//	to increment the row and column indices.  There is no wrapping around.
Shinya Kitaoka 120a6e
void MatrixRmn::SetSequence(const VectorRn &d, long startRow, long startCol,
Shinya Kitaoka 120a6e
                            long deltaRow, long deltaCol) {
Shinya Kitaoka 120a6e
  long length = d.length;
Shinya Kitaoka 120a6e
  assert(startRow >= 0 && startRow < NumRows && startCol >= 0 &&
Shinya Kitaoka 120a6e
         startCol < NumCols);
Shinya Kitaoka 120a6e
  assert(startRow + (length - 1) * deltaRow >= 0 &&
Shinya Kitaoka 120a6e
         startRow + (length - 1) * deltaRow < NumRows);
Shinya Kitaoka 120a6e
  assert(startCol + (length - 1) * deltaCol >= 0 &&
Shinya Kitaoka 120a6e
         startCol + (length - 1) * deltaCol < NumCols);
Shinya Kitaoka 120a6e
  double *to   = x + startRow + NumRows * startCol;
Shinya Kitaoka 120a6e
  double *from = d.x;
Shinya Kitaoka 120a6e
  long stride  = deltaRow + NumRows * deltaCol;
Shinya Kitaoka 120a6e
  for (; length > 0; length--) {
Shinya Kitaoka 120a6e
    *to = *(from++);
Shinya Kitaoka 120a6e
    to += stride;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
// The matrix A is loaded, in into "this" matrix, based at (0,0).
luzpaz 27707d
//  The size of "this" matrix must be large enough to accommodate A.
Shinya Kitaoka 120a6e
//	The rest of "this" matrix is left unchanged.  It is not filled with
Shinya Kitaoka 38fd86
// zeroes!
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
void MatrixRmn::LoadAsSubmatrix(const MatrixRmn &A) {
Shinya Kitaoka 120a6e
  assert(A.NumRows <= NumRows && A.NumCols <= NumCols);
Shinya Kitaoka 120a6e
  int extraColStep = NumRows - A.NumRows;
Shinya Kitaoka 120a6e
  double *to       = x;
Shinya Kitaoka 120a6e
  double *from     = A.x;
Shinya Kitaoka 120a6e
  for (long i = A.NumCols; i > 0;
Shinya Kitaoka 120a6e
       i--) {  // Copy columns of A, one per time thru loop
Shinya Kitaoka 120a6e
    for (long j = A.NumRows; j > 0;
Shinya Kitaoka 120a6e
         j--) {  // Copy all elements of this column of A
Shinya Kitaoka 120a6e
      *(to++) = *(from++);
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    to += extraColStep;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
// The matrix A is loaded, in transposed order into "this" matrix, based at
Shinya Kitaoka 120a6e
// (0,0).
luzpaz 27707d
//  The size of "this" matrix must be large enough to accommodate A.
Shinya Kitaoka 120a6e
//	The rest of "this" matrix is left unchanged.  It is not filled with
Shinya Kitaoka 38fd86
// zeroes!
Shinya Kitaoka 120a6e
void MatrixRmn::LoadAsSubmatrixTranspose(const MatrixRmn &A) {
Shinya Kitaoka 120a6e
  assert(A.NumRows <= NumCols && A.NumCols <= NumRows);
Shinya Kitaoka 120a6e
  double *rowPtr = x;
Shinya Kitaoka 120a6e
  double *from   = A.x;
Shinya Kitaoka 120a6e
  for (long i = A.NumCols; i > 0; i--) {  // Copy columns of A, once per loop
Shinya Kitaoka 120a6e
    double *to = rowPtr;
Shinya Kitaoka 120a6e
    for (long j = A.NumRows; j > 0;
Shinya Kitaoka 120a6e
         j--) {  // Loop copying values from the column of A
Shinya Kitaoka 120a6e
      *to = *(from++);
Shinya Kitaoka 120a6e
      to += NumRows;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    rowPtr++;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
// Calculate the Frobenius Norm (square root of sum of squares of entries of the
Shinya Kitaoka 120a6e
// matrix)
Shinya Kitaoka 120a6e
double MatrixRmn::FrobeniusNorm() const { return sqrt(FrobeniusNormSq()); }
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
// Multiply this matrix by column vector v.
Toshihiro Shimizu 890ddd
// Result is column vector "result"
Shinya Kitaoka 120a6e
void MatrixRmn::Multiply(const VectorRn &v, VectorRn &result) const {
Shinya Kitaoka 120a6e
  assert(v.GetLength() == NumCols && result.GetLength() == NumRows);
Shinya Kitaoka 120a6e
  double *out          = result.GetPtr();  // Points to entry in result vector
Shinya Kitaoka 120a6e
  const double *rowPtr = x;  // Points to beginning of next row in matrix
Shinya Kitaoka 120a6e
  for (long j = NumRows; j > 0; j--) {
Shinya Kitaoka 120a6e
    const double *in = v.GetPtr();
Shinya Kitaoka 120a6e
    const double *m  = rowPtr++;
Shinya Kitaoka 120a6e
    *out             = 0.0f;
Shinya Kitaoka 120a6e
    for (long i = NumCols; i > 0; i--) {
Shinya Kitaoka 120a6e
      *out += (*(in++)) * (*m);
Shinya Kitaoka 120a6e
      m += NumRows;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    out++;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
// Multiply transpose of this matrix by column vector v.
Toshihiro Shimizu 890ddd
//    Result is column vector "result"
Toshihiro Shimizu 890ddd
// Equivalent to mult by row vector on left
Shinya Kitaoka 120a6e
void MatrixRmn::MultiplyTranspose(const VectorRn &v, VectorRn &result) const {
Shinya Kitaoka 120a6e
  assert(v.GetLength() == NumRows && result.GetLength() == NumCols);
Shinya Kitaoka 120a6e
  double *out          = result.GetPtr();  // Points to entry in result vector
Shinya Kitaoka 120a6e
  const double *colPtr = x;  // Points to beginning of next column in matrix
Shinya Kitaoka 120a6e
  for (long i = NumCols; i > 0; i--) {
Shinya Kitaoka 120a6e
    const double *in = v.GetPtr();
Shinya Kitaoka 120a6e
    *out             = 0.0f;
Shinya Kitaoka 120a6e
    for (long j = NumRows; j > 0; j--) {
Shinya Kitaoka 120a6e
      *out += (*(in++)) * (*(colPtr++));
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    out++;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
// Form the dot product of a vector v with the i-th column of the array
Shinya Kitaoka 120a6e
double MatrixRmn::DotProductColumn(const VectorRn &v, long colNum) const {
Shinya Kitaoka 120a6e
  assert(v.GetLength() == NumRows);
Shinya Kitaoka 120a6e
  double *ptrC = x + colNum * NumRows;
Shinya Kitaoka 120a6e
  double *ptrV = v.x;
Shinya Kitaoka 120a6e
  double ret   = 0.0;
Shinya Kitaoka 120a6e
  for (long i = NumRows; i > 0; i--) {
Shinya Kitaoka 120a6e
    ret += (*(ptrC++)) * (*(ptrV++));
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
  return ret;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
// Add a constant to each entry on the diagonal
Shinya Kitaoka 120a6e
MatrixRmn &MatrixRmn::AddToDiagonal(double d)  // Adds d to each diagonal entry
Toshihiro Shimizu 890ddd
{
Shinya Kitaoka 120a6e
  long diagLen = std::min(NumRows, NumCols);
Shinya Kitaoka 120a6e
  double *dPtr = x;
Shinya Kitaoka 120a6e
  for (; diagLen > 0; diagLen--) {
Shinya Kitaoka 120a6e
    *dPtr += d;
Shinya Kitaoka 120a6e
    dPtr += NumRows + 1;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
  return *this;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
luz paz 1414d5
// Adds the terms of the vector to the diagonal
Shinya Kitaoka 120a6e
MatrixRmn &MatrixRmn::AddToDiagonal(
Shinya Kitaoka 120a6e
    const VectorRn &v)  // Adds d to each diagonal entry
Toshihiro Shimizu 890ddd
{
Shinya Kitaoka 120a6e
  long diagLen     = std::min(NumRows, NumCols);
Shinya Kitaoka 120a6e
  double *dPtr     = x;
Shinya Kitaoka 120a6e
  const double *dv = v.x;
Shinya Kitaoka 120a6e
  for (; diagLen > 0; diagLen--) {
Shinya Kitaoka 120a6e
    *dPtr += *(dv++);
Shinya Kitaoka 120a6e
    dPtr += NumRows + 1;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
  return *this;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
MatrixRmn &MatrixRmn::MultiplyScalar(const MatrixRmn &A, double k,
Shinya Kitaoka 120a6e
                                     MatrixRmn &dst) {
Shinya Kitaoka 120a6e
  long length  = A.NumCols;
Shinya Kitaoka 120a6e
  double *dPtr = dst.x;
Shinya Kitaoka 120a6e
  for (long i = dst.NumCols; i > 0; i--) {
Shinya Kitaoka 120a6e
    double *aPtr = A.x;  // Points to beginning of row in A
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    for (long j = dst.NumRows; j > 0; j--) {
Shinya Kitaoka 120a6e
      *dPtr = *aPtr * k;
Shinya Kitaoka 120a6e
      dPtr++;
Shinya Kitaoka 120a6e
      aPtr++;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    aPtr += A.NumRows;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  return dst;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
// Multiply two MatrixRmn's
Shinya Kitaoka 120a6e
MatrixRmn &MatrixRmn::Multiply(const MatrixRmn &A, const MatrixRmn &B,
Shinya Kitaoka 120a6e
                               MatrixRmn &dst) {
Shinya Kitaoka 120a6e
  assert(A.NumCols == B.NumRows && A.NumRows == dst.NumRows &&
Shinya Kitaoka 120a6e
         B.NumCols == dst.NumCols);
Shinya Kitaoka 120a6e
  long length  = A.NumCols;
Shinya Kitaoka 120a6e
  double *bPtr = B.x;  // Points to beginning of column in B
Shinya Kitaoka 120a6e
  double *dPtr = dst.x;
Shinya Kitaoka 120a6e
  for (long i = dst.NumCols; i > 0; i--) {
Shinya Kitaoka 120a6e
    double *aPtr = A.x;  // Points to beginning of row in A
Shinya Kitaoka 120a6e
    for (long j = dst.NumRows; j > 0; j--) {
Shinya Kitaoka 120a6e
      *dPtr = DotArray(length, aPtr, A.NumRows, bPtr, 1);
Shinya Kitaoka 120a6e
      dPtr++;
Shinya Kitaoka 120a6e
      aPtr++;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    bPtr += B.NumRows;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  return dst;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
// Multiply two MatrixRmn's,  Transpose the first matrix before multiplying
Shinya Kitaoka 120a6e
MatrixRmn &MatrixRmn::TransposeMultiply(const MatrixRmn &A, const MatrixRmn &B,
Shinya Kitaoka 120a6e
                                        MatrixRmn &dst) {
Shinya Kitaoka 120a6e
  assert(A.NumRows == B.NumRows && A.NumCols == dst.NumRows &&
Shinya Kitaoka 120a6e
         B.NumCols == dst.NumCols);
Shinya Kitaoka 120a6e
  long length = A.NumRows;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  double *bPtr = B.x;  // bPtr Points to beginning of column in B
Shinya Kitaoka 120a6e
  double *dPtr = dst.x;
Shinya Kitaoka 120a6e
  for (long i = dst.NumCols; i > 0; i--) {  // Loop over all columns of dst
Shinya Kitaoka 120a6e
    double *aPtr = A.x;  // aPtr Points to beginning of column in A
Shinya Kitaoka 120a6e
    for (long j = dst.NumRows; j > 0; j--) {  // Loop over all rows of dst
Shinya Kitaoka 120a6e
      *dPtr = DotArray(length, aPtr, 1, bPtr, 1);
Shinya Kitaoka 120a6e
      dPtr++;
Shinya Kitaoka 120a6e
      aPtr += A.NumRows;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    bPtr += B.NumRows;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  return dst;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
// Multiply two MatrixRmn's.  Transpose the second matrix before multiplying
Shinya Kitaoka 120a6e
MatrixRmn &MatrixRmn::MultiplyTranspose(const MatrixRmn &A, const MatrixRmn &B,
Shinya Kitaoka 120a6e
                                        MatrixRmn &dst) {
Shinya Kitaoka 120a6e
  assert(A.NumCols == B.NumCols && A.NumRows == dst.NumRows &&
Shinya Kitaoka 120a6e
         B.NumRows == dst.NumCols);
Shinya Kitaoka 120a6e
  long length = A.NumCols;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  double *bPtr = B.x;  // Points to beginning of row in B
Shinya Kitaoka 120a6e
  double *dPtr = dst.x;
Shinya Kitaoka 120a6e
  for (long i = dst.NumCols; i > 0; i--) {
Shinya Kitaoka 120a6e
    double *aPtr = A.x;  // Points to beginning of row in A
Shinya Kitaoka 120a6e
    for (long j = dst.NumRows; j > 0; j--) {
Shinya Kitaoka 120a6e
      *dPtr = DotArray(length, aPtr, A.NumRows, bPtr, B.NumRows);
Shinya Kitaoka 120a6e
      dPtr++;
Shinya Kitaoka 120a6e
      aPtr++;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    bPtr++;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  return dst;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
// Solves the equation   (*this)*xVec = b;
Toshihiro Shimizu 890ddd
// Uses row operations.  Assumes *this is square and invertible.
Toshihiro Shimizu 890ddd
// No error checking for divide by zero or instability (except with asserts)
Shinya Kitaoka 120a6e
void MatrixRmn::Solve(const VectorRn &b, VectorRn *xVec) const {
Shinya Kitaoka 120a6e
  assert(NumRows == NumCols && NumCols == xVec->GetLength() &&
Shinya Kitaoka 120a6e
         NumRows == b.GetLength());
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Copy this matrix and b into an Augmented Matrix
Shinya Kitaoka 120a6e
  MatrixRmn &AugMat = GetWorkMatrix(NumRows, NumCols + 1);
Shinya Kitaoka 120a6e
  AugMat.LoadAsSubmatrix(*this);
Shinya Kitaoka 120a6e
  AugMat.SetColumn(NumRows, b);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Put into row echelon form with row operations
Shinya Kitaoka 120a6e
  AugMat.ConvertToRefNoFree();
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Solve for x vector values using back substitution
Shinya Kitaoka 120a6e
  double *xLast = xVec->x + NumRows - 1;  // Last entry in xVec
Shinya Kitaoka 120a6e
  double *endRow =
Shinya Kitaoka 120a6e
      AugMat.x + NumRows * NumCols - 1;  // Last entry in the current row of the
Shinya Kitaoka 120a6e
                                         // coefficient part of Augmented Matrix
Shinya Kitaoka 120a6e
  double *bPtr = endRow + NumRows;  // Last entry in augmented matrix (end of
Shinya Kitaoka 120a6e
                                    // last column, in augmented part)
Shinya Kitaoka 120a6e
  for (long i = NumRows; i > 0; i--) {
Shinya Kitaoka 120a6e
    double accum = *(bPtr--);
Shinya Kitaoka 120a6e
    // Next loop computes back substitution terms
Shinya Kitaoka 120a6e
    double *rowPtr =
Shinya Kitaoka 120a6e
        endRow;  // Points to entries of the current row for back substitution.
Shinya Kitaoka 120a6e
    double *xPtr = xLast;  // Points to entries in the x vector (also for back
Shinya Kitaoka 120a6e
                           // substitution)
Shinya Kitaoka 120a6e
    for (long j = NumRows - i; j > 0; j--) {
Shinya Kitaoka 120a6e
      accum -= (*rowPtr) * (*(xPtr--));
Shinya Kitaoka 120a6e
      rowPtr -= NumCols;  // Previous entry in the row
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    assert(*rowPtr !=
Shinya Kitaoka 120a6e
           0.0);  // Are not supposed to be any free variables in this matrix
Shinya Kitaoka 120a6e
    *xPtr = accum / (*rowPtr);
Shinya Kitaoka 120a6e
    endRow--;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
// ConvertToRefNoFree
Toshihiro Shimizu 890ddd
// Converts the matrix (in place) to row echelon form
Toshihiro Shimizu 890ddd
// For us, row echelon form allows any non-zero values, not just 1's, in the
Toshihiro Shimizu 890ddd
//		position for a lead variable.
Shinya Kitaoka 120a6e
// The "NoFree" version operates on the assumption that no free variable will be
Shinya Kitaoka 120a6e
// found.
Toshihiro Shimizu 890ddd
// Algorithm uses row operations and row pivoting (only).
luzpaz 27707d
// Augmented matrix is correctly accommodated.  Only the first square part
Shinya Kitaoka 120a6e
// participates
Toshihiro Shimizu 890ddd
//		in the main work of row operations.
Shinya Kitaoka 120a6e
void MatrixRmn::ConvertToRefNoFree() {
Shinya Kitaoka 120a6e
  // Loop over all columns (variables)
Shinya Kitaoka 120a6e
  // Find row with most non-zero entry.
Shinya Kitaoka 120a6e
  // Swap to the highest active row
Shinya Kitaoka 120a6e
  // Subtract appropriately from all the lower rows (row op of type 3)
Shinya Kitaoka 120a6e
  long numIters       = std::min(NumRows, NumCols);
Shinya Kitaoka 120a6e
  double *rowPtr1     = x;
Shinya Kitaoka 120a6e
  const long diagStep = NumRows + 1;
Shinya Kitaoka 120a6e
  long lenRowLeft     = NumCols;
Shinya Kitaoka 120a6e
  for (; numIters > 1; numIters--) {
Shinya Kitaoka 120a6e
    // Find row with most non-zero entry.
Shinya Kitaoka 120a6e
    double *rowPtr2  = rowPtr1;
Shinya Kitaoka 120a6e
    double maxAbs    = fabs(*rowPtr1);
Shinya Kitaoka 120a6e
    double *rowPivot = rowPtr1;
Shinya Kitaoka 120a6e
    long i;
Shinya Kitaoka 120a6e
    for (i = numIters - 1; i > 0; i--) {
Shinya Kitaoka 120a6e
      const double &newMax = *(++rowPivot);
Shinya Kitaoka 120a6e
      if (newMax > maxAbs) {
Shinya Kitaoka 120a6e
        maxAbs  = *rowPivot;
Shinya Kitaoka 120a6e
        rowPtr2 = rowPivot;
Shinya Kitaoka 120a6e
      } else if (-newMax > maxAbs) {
Shinya Kitaoka 120a6e
        maxAbs  = -newMax;
Shinya Kitaoka 120a6e
        rowPtr2 = rowPivot;
Shinya Kitaoka 120a6e
      }
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    // Pivot step: Swap the row with highest entry to the current row
Shinya Kitaoka 120a6e
    if (rowPtr1 != rowPtr2) {
Shinya Kitaoka 120a6e
      double *to = rowPtr1;
Shinya Kitaoka 120a6e
      for (long i = lenRowLeft; i > 0; i--) {
Shinya Kitaoka 120a6e
        double temp = *to;
Shinya Kitaoka 120a6e
        *to         = *rowPtr2;
Shinya Kitaoka 120a6e
        *rowPtr2    = temp;
Shinya Kitaoka 120a6e
        to += NumRows;
Shinya Kitaoka 120a6e
        rowPtr2 += NumRows;
Shinya Kitaoka 120a6e
      }
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    // Subtract this row appropriately from all the lower rows (row operation of
Shinya Kitaoka 120a6e
    // type 3)
Shinya Kitaoka 120a6e
    rowPtr2 = rowPtr1;
Shinya Kitaoka 120a6e
    for (i = numIters - 1; i > 0; i--) {
Shinya Kitaoka 120a6e
      rowPtr2++;
Shinya Kitaoka 120a6e
      double *to   = rowPtr2;
Shinya Kitaoka 120a6e
      double *from = rowPtr1;
Shinya Kitaoka 120a6e
      assert(*from != 0.0);
Shinya Kitaoka 120a6e
      double alpha = (*to) / (*from);
Shinya Kitaoka 120a6e
      *to          = 0.0;
Shinya Kitaoka 120a6e
      for (long j = lenRowLeft - 1; j > 0; j--) {
Shinya Kitaoka 120a6e
        to += NumRows;
Shinya Kitaoka 120a6e
        from += NumRows;
Shinya Kitaoka 120a6e
        *to -= (*from) * alpha;
Shinya Kitaoka 120a6e
      }
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    // Update for next iteration of loop
Shinya Kitaoka 120a6e
    rowPtr1 += diagStep;
Shinya Kitaoka 120a6e
    lenRowLeft--;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
// Calculate the c=cosine and s=sine values for a Givens transformation.
Toshihiro Shimizu 890ddd
// The matrix M = ( (c, -s), (s, c) ) in row order transforms the
Toshihiro Shimizu 890ddd
//   column vector (a, b)^T to have y-coordinate zero.
Shinya Kitaoka 120a6e
void MatrixRmn::CalcGivensValues(double a, double b, double *c, double *s) {
Shinya Kitaoka 120a6e
  double denomInv = sqrt(a * a + b * b);
Shinya Kitaoka 120a6e
  if (denomInv == 0.0) {
Shinya Kitaoka 120a6e
    *c = 1.0;
Shinya Kitaoka 120a6e
    *s = 0.0;
Shinya Kitaoka 120a6e
  } else {
Shinya Kitaoka 120a6e
    denomInv = 1.0 / denomInv;
Shinya Kitaoka 120a6e
    *c       = a * denomInv;
Shinya Kitaoka 120a6e
    *s       = -b * denomInv;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
// Applies Givens transform to columns i and i+1.
Toshihiro Shimizu 890ddd
// Equivalent to postmultiplying by the matrix
Toshihiro Shimizu 890ddd
//      ( c  -s )
Toshihiro Shimizu 890ddd
//		( s   c )
Toshihiro Shimizu 890ddd
// with non-zero entries in rows i and i+1 and columns i and i+1
Shinya Kitaoka 120a6e
void MatrixRmn::PostApplyGivens(double c, double s, long idx) {
Shinya Kitaoka 120a6e
  assert(0 <= idx && idx < NumCols);
Shinya Kitaoka 120a6e
  double *colA = x + idx * NumRows;
Shinya Kitaoka 120a6e
  double *colB = colA + NumRows;
Shinya Kitaoka 120a6e
  for (long i = NumRows; i > 0; i--) {
Shinya Kitaoka 120a6e
    double temp = *colA;
Shinya Kitaoka 120a6e
    *colA       = (*colA) * c + (*colB) * s;
Shinya Kitaoka 120a6e
    *colB       = (*colB) * c - temp * s;
Shinya Kitaoka 120a6e
    colA++;
Shinya Kitaoka 120a6e
    colB++;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
// Applies Givens transform to columns idx1 and idx2.
Toshihiro Shimizu 890ddd
// Equivalent to postmultiplying by the matrix
Toshihiro Shimizu 890ddd
//      ( c  -s )
Toshihiro Shimizu 890ddd
//		( s   c )
Toshihiro Shimizu 890ddd
// with non-zero entries in rows idx1 and idx2 and columns idx1 and idx2
Shinya Kitaoka 120a6e
void MatrixRmn::PostApplyGivens(double c, double s, long idx1, long idx2) {
Shinya Kitaoka 120a6e
  assert(idx1 != idx2 && 0 <= idx1 && idx1 < NumCols && 0 <= idx2 &&
Shinya Kitaoka 120a6e
         idx2 < NumCols);
Shinya Kitaoka 120a6e
  double *colA = x + idx1 * NumRows;
Shinya Kitaoka 120a6e
  double *colB = x + idx2 * NumRows;
Shinya Kitaoka 120a6e
  for (long i = NumRows; i > 0; i--) {
Shinya Kitaoka 120a6e
    double temp = *colA;
Shinya Kitaoka 120a6e
    *colA       = (*colA) * c + (*colB) * s;
Shinya Kitaoka 120a6e
    *colB       = (*colB) * c - temp * s;
Shinya Kitaoka 120a6e
    colA++;
Shinya Kitaoka 120a6e
    colB++;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
// ********************************************************************************************
Toshihiro Shimizu 890ddd
// Singular value decomposition.
Shinya Kitaoka 120a6e
// Return othogonal matrices U and V and diagonal matrix with diagonal w such
Shinya Kitaoka 120a6e
// that
Toshihiro Shimizu 890ddd
//     (this) = U * Diag(w) * V^T     (V^T is V-transpose.)
Shinya Kitaoka 120a6e
// Diagonal entries have all non-zero entries before all zero entries, but are
Shinya Kitaoka 120a6e
// not
Shinya Kitaoka 38fd86
//		necessarily sorted.  (Someday, I will write ComputedSortedSVD
Shinya Kitaoka d1f6c4
// that
Shinya Kitaoka 38fd86
// handles
Toshihiro Shimizu 890ddd
//		sorting the eigenvalues by magnitude.)
Toshihiro Shimizu 890ddd
// ********************************************************************************************
Shinya Kitaoka 120a6e
void MatrixRmn::ComputeSVD(MatrixRmn &U, VectorRn &w, MatrixRmn &V) const {
Shinya Kitaoka 120a6e
  assert(U.NumRows == NumRows && V.NumCols == NumCols &&
Shinya Kitaoka 120a6e
         U.NumRows == U.NumCols && V.NumRows == V.NumCols &&
Shinya Kitaoka 120a6e
         w.GetLength() == std::min(NumRows, NumCols));
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  double temp         = 0.0;
Shinya Kitaoka 120a6e
  VectorRn &superDiag = VectorRn::GetWorkVector(
Shinya Kitaoka 120a6e
      w.GetLength() - 1);  // Some extra work space.  Will get passed around.
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Choose larger of U, V to hold intermediate results
Shinya Kitaoka 120a6e
  // If U is larger than V, use U to store intermediate results
Shinya Kitaoka 120a6e
  // Otherwise use V.  In the latter case, we form the SVD of A transpose,
Shinya Kitaoka 120a6e
  //		(which is essentially identical to the SVD of A).
Shinya Kitaoka 120a6e
  MatrixRmn *leftMatrix;
Shinya Kitaoka 120a6e
  MatrixRmn *rightMatrix;
Shinya Kitaoka 120a6e
  if (NumRows >= NumCols) {
Shinya Kitaoka 120a6e
    U.LoadAsSubmatrix(*this);  // Copy A into U
Shinya Kitaoka 120a6e
    leftMatrix  = &U;
Shinya Kitaoka 120a6e
    rightMatrix = &V;
Shinya Kitaoka 120a6e
  } else {
Shinya Kitaoka 120a6e
    V.LoadAsSubmatrixTranspose(*this);  // Copy A-transpose into V
Shinya Kitaoka 120a6e
    leftMatrix  = &V;
Shinya Kitaoka 120a6e
    rightMatrix = &U;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Do the actual work to calculate the SVD
Shinya Kitaoka 120a6e
  // Now matrix has at least as many rows as columns
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  CalcBidiagonal(*leftMatrix, *rightMatrix, w, superDiag);
Shinya Kitaoka 120a6e
  ConvertBidiagToDiagonal(*leftMatrix, *rightMatrix, w, superDiag);
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
// ************************************************ CalcBidiagonal
Shinya Kitaoka 120a6e
// **************************
Toshihiro Shimizu 890ddd
// Helper routine for SVD computation
Toshihiro Shimizu 890ddd
// U is a matrix to be bidiagonalized.
Toshihiro Shimizu 890ddd
// On return, U and V are orthonormal and w holds the new diagonal
Toshihiro Shimizu 890ddd
//	  elements and superDiag holds the super diagonal elements.
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
void MatrixRmn::CalcBidiagonal(MatrixRmn &U, MatrixRmn &V, VectorRn &w,
Shinya Kitaoka 120a6e
                               VectorRn &superDiag) {
Shinya Kitaoka 120a6e
  assert(U.NumRows >= V.NumRows);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // The diagonal and superdiagonal entries of the bidiagonalized
Shinya Kitaoka 120a6e
  //	  version of the U matrix
Shinya Kitaoka 120a6e
  //	  are stored in the vectors w and superDiag (temporarily).
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Apply Householder transformations to U.
Shinya Kitaoka 120a6e
  // Householder transformations come in pairs.
Shinya Kitaoka 120a6e
  //   First, on the left, we map a portion of a column to zeros
Shinya Kitaoka 120a6e
  //   Second, on the right, we map a portion of a row to zeros
Shinya Kitaoka 120a6e
  const long rowStep   = U.NumCols;
Shinya Kitaoka 120a6e
  const long diagStep  = U.NumCols + 1;
Shinya Kitaoka 120a6e
  double *diagPtr      = U.x;
Shinya Kitaoka 120a6e
  double *wPtr         = w.x;
Shinya Kitaoka 120a6e
  double *superDiagPtr = superDiag.x;
Shinya Kitaoka 120a6e
  long colLengthLeft   = U.NumRows;
Shinya Kitaoka 120a6e
  long rowLengthLeft   = V.NumCols;
Shinya Kitaoka 120a6e
  while (true) {
Shinya Kitaoka 120a6e
    // Apply a Householder xform on left to zero part of a column
Shinya Kitaoka 120a6e
    SvdHouseholder(diagPtr, colLengthLeft, rowLengthLeft, 1, rowStep, wPtr);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    if (rowLengthLeft == 2) {
Shinya Kitaoka 120a6e
      *superDiagPtr = *(diagPtr + rowStep);
Shinya Kitaoka 120a6e
      break;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    // Apply a Householder xform on the right to zero part of a row
Shinya Kitaoka 120a6e
    SvdHouseholder(diagPtr + rowStep, rowLengthLeft - 1, colLengthLeft, rowStep,
Shinya Kitaoka 120a6e
                   1, superDiagPtr);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    rowLengthLeft--;
Shinya Kitaoka 120a6e
    colLengthLeft--;
Shinya Kitaoka 120a6e
    diagPtr += diagStep;
Shinya Kitaoka 120a6e
    wPtr++;
Shinya Kitaoka 120a6e
    superDiagPtr++;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  int extra = 0;
Shinya Kitaoka 120a6e
  diagPtr += diagStep;
Shinya Kitaoka 120a6e
  wPtr++;
Shinya Kitaoka 120a6e
  if (colLengthLeft > 2) {
Shinya Kitaoka 120a6e
    extra = 1;
Shinya Kitaoka 120a6e
    // Do one last Householder transformation when the matrix is not square
Shinya Kitaoka 120a6e
    colLengthLeft--;
Shinya Kitaoka 120a6e
    SvdHouseholder(diagPtr, colLengthLeft, 1, 1, 0, wPtr);
Shinya Kitaoka 120a6e
  } else {
Shinya Kitaoka 120a6e
    *wPtr = *diagPtr;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Form U and V from the Householder transformations
Shinya Kitaoka 120a6e
  V.ExpandHouseholders(V.NumCols - 2, 1, U.x + U.NumRows, U.NumRows, 1);
Shinya Kitaoka 120a6e
  U.ExpandHouseholders(V.NumCols - 1 + extra, 0, U.x, 1, U.NumRows);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Done with bidiagonalization
Shinya Kitaoka 120a6e
  return;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
// Helper routine for CalcBidiagonal
Toshihiro Shimizu 890ddd
// Performs a series of Householder transformations on a matrix
Shinya Kitaoka 120a6e
// Stores results compactly into the matrix:   The Householder vector u
Shinya Kitaoka 120a6e
// (normalized)
Toshihiro Shimizu 890ddd
//   is stored into the first row/column being transformed.
Toshihiro Shimizu 890ddd
// The leading term of that row (= plus/minus its magnitude is returned
Toshihiro Shimizu 890ddd
//	 separately into "retFirstEntry"
Shinya Kitaoka 120a6e
void MatrixRmn::SvdHouseholder(double *basePt, long colLength, long numCols,
Shinya Kitaoka 120a6e
                               long colStride, long rowStride,
Shinya Kitaoka 120a6e
                               double *retFirstEntry) {
Shinya Kitaoka 120a6e
  // Calc norm of vector u
Shinya Kitaoka 120a6e
  double *cPtr = basePt;
Shinya Kitaoka 120a6e
  double norm  = 0.0;
Shinya Kitaoka 120a6e
  long i;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  double aa0 = *cPtr;
Shinya Kitaoka 120a6e
  double aa1 = *basePt;
Shinya Kitaoka 120a6e
  double aa2 = *retFirstEntry;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  for (i = colLength; i > 0; i--) {
Shinya Kitaoka 120a6e
    norm += Square(*cPtr);
Shinya Kitaoka 120a6e
    cPtr += colStride;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
  norm = sqrt(norm);  // Norm of vector to reflect to axis  e_1
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Handle sign issues
Shinya Kitaoka 120a6e
  double imageVal;  // Choose sign to maximize distance
Shinya Kitaoka 120a6e
  if ((*basePt) < 0.0) {
Shinya Kitaoka 120a6e
    imageVal = norm;
Shinya Kitaoka 120a6e
    norm     = 2.0 * norm * (norm - (*basePt));
Shinya Kitaoka 120a6e
  } else {
Shinya Kitaoka 120a6e
    imageVal = -norm;
Shinya Kitaoka 120a6e
    norm     = 2.0 * norm * (norm + (*basePt));
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
  norm = sqrt(norm);  // Norm is norm of reflection vector
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if (norm == 0.0) {  // If the vector being transformed is equal to zero
Shinya Kitaoka 120a6e
    // Force to zero in case of roundoff errors
Shinya Kitaoka 120a6e
    cPtr = basePt;
Shinya Kitaoka 120a6e
    for (i = colLength; i > 0; i--) {
Shinya Kitaoka 120a6e
      *cPtr = 0.0;
Shinya Kitaoka 120a6e
      cPtr += colStride;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    *retFirstEntry = 0.0;
Shinya Kitaoka 120a6e
    return;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  *retFirstEntry = imageVal;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Set up the normalized Householder vector
Shinya Kitaoka 120a6e
  *basePt -= imageVal;  // First component changes. Rest stay the same.
Shinya Kitaoka 120a6e
  // Normalize the vector
Shinya Kitaoka 120a6e
  norm = 1.0 / norm;  // Now it is the inverse norm
Shinya Kitaoka 120a6e
  cPtr = basePt;
Shinya Kitaoka 120a6e
  for (i = colLength; i > 0; i--) {
Shinya Kitaoka 120a6e
    *cPtr *= norm;
Shinya Kitaoka 120a6e
    cPtr += colStride;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Transform the rest of the U matrix with the Householder transformation
Shinya Kitaoka 120a6e
  double *rPtr = basePt;
Shinya Kitaoka 120a6e
  for (long j = numCols - 1; j > 0; j--) {
Shinya Kitaoka 120a6e
    rPtr += rowStride;
Shinya Kitaoka 120a6e
    // Calc dot product with Householder transformation vector
Shinya Kitaoka 120a6e
    double dotP = DotArray(colLength, basePt, colStride, rPtr, colStride);
Shinya Kitaoka 120a6e
    // Transform with I - 2*dotP*(Householder vector)
Shinya Kitaoka 120a6e
    AddArrayScale(colLength, basePt, colStride, rPtr, colStride, -2.0 * dotP);
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
// ********************************* ExpandHouseholders
Shinya Kitaoka 120a6e
// ********************************************
Toshihiro Shimizu 890ddd
// The matrix will be square.
Toshihiro Shimizu 890ddd
//   numXforms = number of Householder transformations to concatenate
Toshihiro Shimizu 890ddd
//		Each Householder transformation is represented by a unit vector
Shinya Kitaoka 120a6e
//		Each successive Householder transformation starts one position
Shinya Kitaoka 38fd86
// later
Toshihiro Shimizu 890ddd
//			and has one more implied leading zero
Toshihiro Shimizu 890ddd
//	 basePt = beginning of the first Householder transform
Toshihiro Shimizu 890ddd
//	 colStride, rowStride: Householder xforms are stored in "columns"
Toshihiro Shimizu 890ddd
//   numZerosSkipped is the number of implicit zeros on the front each
Shinya Kitaoka 38fd86
//			Householder transformation vector (only values supported
Shinya Kitaoka d1f6c4
// are
Shinya Kitaoka 38fd86
// 0 and 1).
Shinya Kitaoka 120a6e
void MatrixRmn::ExpandHouseholders(long numXforms, int numZerosSkipped,
Shinya Kitaoka 120a6e
                                   const double *basePt, long colStride,
Shinya Kitaoka 120a6e
                                   long rowStride) {
Shinya Kitaoka 120a6e
  // Number of applications of the last Householder transform
Shinya Kitaoka 120a6e
  //     (That are not trivial!)
Shinya Kitaoka 120a6e
  long numToTransform = NumCols - numXforms + 1 - numZerosSkipped;
Shinya Kitaoka 120a6e
  assert(numToTransform > 0);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if (numXforms == 0) {
Shinya Kitaoka 120a6e
    SetIdentity();
Shinya Kitaoka 120a6e
    return;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Handle the first one separately as a special case,
Shinya Kitaoka 120a6e
  // "this" matrix will be treated to simulate being preloaded with the identity
Shinya Kitaoka 120a6e
  long hDiagStride = rowStride + colStride;
Shinya Kitaoka 120a6e
  const double *hBase =
Shinya Kitaoka 120a6e
      basePt +
Shinya Kitaoka 120a6e
      hDiagStride * (numXforms - 1);  // Pointer to the last Householder vector
Shinya Kitaoka 120a6e
  const double *hDiagPtr =
Shinya Kitaoka 120a6e
      hBase +
Shinya Kitaoka 120a6e
      colStride * (numToTransform - 1);  // Pointer to last entry in that vector
Shinya Kitaoka 120a6e
  long i;
Shinya Kitaoka 120a6e
  double *diagPtr = x + NumCols * NumRows -
Shinya Kitaoka 120a6e
                    1;  // Last entry in matrix (points to diagonal entry)
Shinya Kitaoka 120a6e
  double *colPtr =
Shinya Kitaoka 120a6e
      diagPtr - (numToTransform - 1);  // Pointer to column in matrix
Shinya Kitaoka 120a6e
  for (i = numToTransform; i > 0; i--) {
Shinya Kitaoka 120a6e
    CopyArrayScale(numToTransform, hBase, colStride, colPtr, 1,
Shinya Kitaoka 120a6e
                   -2.0 * (*hDiagPtr));
Shinya Kitaoka 120a6e
    *diagPtr += 1.0;  // Add back in 1 to the diagonal entry (since xforming the
Shinya Kitaoka 120a6e
                      // identity)
Shinya Kitaoka 120a6e
    diagPtr -= (NumRows + 1);  // Next diagonal entry in this matrix
Shinya Kitaoka 120a6e
    colPtr -= NumRows;         // Next column in this matrix
Shinya Kitaoka 120a6e
    hDiagPtr -= colStride;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Now handle the general case
Shinya Kitaoka 120a6e
  // A row of zeros must be in effect added to the top of each old column (in
Shinya Kitaoka 120a6e
  // each loop)
Shinya Kitaoka 120a6e
  double *colLastPtr = x + NumRows * NumCols - numToTransform - 1;
Shinya Kitaoka 120a6e
  for (i = numXforms - 1; i > 0; i--) {
Shinya Kitaoka 120a6e
    numToTransform++;  // Number of non-trivial applications of this Householder
Shinya Kitaoka 120a6e
                       // transformation
Shinya Kitaoka 120a6e
    hBase -= hDiagStride;  // Pointer to the beginning of the Householder
Shinya Kitaoka 120a6e
                           // transformation
Shinya Kitaoka 120a6e
    colPtr = colLastPtr;
Shinya Kitaoka 120a6e
    for (long j = numToTransform - 1; j > 0; j--) {
Shinya Kitaoka 120a6e
      // Get dot product
Shinya Kitaoka 120a6e
      double dotProd2N = -2.0 * DotArray(numToTransform - 1, hBase + colStride,
Shinya Kitaoka 120a6e
                                         colStride, colPtr + 1, 1);
Shinya Kitaoka 120a6e
      *colPtr = dotProd2N * (*hBase);  // Adding onto zero at initial point
Shinya Kitaoka 120a6e
      AddArrayScale(numToTransform - 1, hBase + colStride, colStride,
Shinya Kitaoka 120a6e
                    colPtr + 1, 1, dotProd2N);
Shinya Kitaoka 120a6e
      colPtr -= NumRows;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    // Do last one as a special case (may overwrite the Householder vector)
Shinya Kitaoka 120a6e
    CopyArrayScale(numToTransform, hBase, colStride, colPtr, 1,
Shinya Kitaoka 120a6e
                   -2.0 * (*hBase));
Shinya Kitaoka 120a6e
    *colPtr += 1.0;  // Add back one one as identity
Shinya Kitaoka 120a6e
    // Done with this Householder transformation
Shinya Kitaoka 120a6e
    colLastPtr--;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if (numZerosSkipped != 0) {
Shinya Kitaoka 120a6e
    assert(numZerosSkipped == 1);
Shinya Kitaoka 120a6e
    // Fill first row and column with identity (More generally: first
Shinya Kitaoka 120a6e
    // numZerosSkipped many rows and columns)
Shinya Kitaoka 120a6e
    double *d  = x;
Shinya Kitaoka 120a6e
    *d         = 1;
Shinya Kitaoka 120a6e
    double *d2 = d;
Shinya Kitaoka 120a6e
    for (i = NumRows - 1; i > 0; i--) {
Shinya Kitaoka 120a6e
      *(++d)           = 0;
Shinya Kitaoka 120a6e
      *(d2 += NumRows) = 0;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
// **************** ConvertBidiagToDiagonal
Shinya Kitaoka 120a6e
// ***********************************************
Toshihiro Shimizu 890ddd
// Do the iterative transformation from bidiagonal form to diagonal form using
Toshihiro Shimizu 890ddd
//		Givens transformation.  (Golub-Reinsch)
Toshihiro Shimizu 890ddd
// U and V are square.  Size of U less than or equal to that of U.
Shinya Kitaoka 120a6e
void MatrixRmn::ConvertBidiagToDiagonal(MatrixRmn &U, MatrixRmn &V, VectorRn &w,
Shinya Kitaoka 120a6e
                                        VectorRn &superDiag) const {
Shinya Kitaoka 120a6e
  // These two index into the last bidiagonal block  (last in the matrix, it
Shinya Kitaoka 120a6e
  // will be
Shinya Kitaoka 120a6e
  //	first one handled.
Shinya Kitaoka 120a6e
  long lastBidiagIdx  = V.NumRows - 1;
Shinya Kitaoka 120a6e
  long firstBidiagIdx = 0;
Shinya Kitaoka 120a6e
  // togliere
Shinya Kitaoka 120a6e
  double aa = w.MaxAbs();
Shinya Kitaoka 120a6e
  double bb = superDiag.MaxAbs();
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  double eps = 1.0e-15 * std::max(w.MaxAbs(), superDiag.MaxAbs());
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  while (true) {
Shinya Kitaoka 120a6e
    bool workLeft =
Shinya Kitaoka 120a6e
        UpdateBidiagIndices(&firstBidiagIdx, &lastBidiagIdx, w, superDiag, eps);
Shinya Kitaoka 120a6e
    if (!workLeft) {
Shinya Kitaoka 120a6e
      break;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    // Get ready for first Givens rotation
Shinya Kitaoka 120a6e
    // Push non-zero to M[2,1] with Givens transformation
Shinya Kitaoka 120a6e
    double *wPtr        = w.x + firstBidiagIdx;
Shinya Kitaoka 120a6e
    double *sdPtr       = superDiag.x + firstBidiagIdx;
Shinya Kitaoka 120a6e
    double extraOffDiag = 0.0;
Shinya Kitaoka 120a6e
    if ((*wPtr) == 0.0) {
Shinya Kitaoka 120a6e
      ClearRowWithDiagonalZero(firstBidiagIdx, lastBidiagIdx, U, wPtr, sdPtr,
Shinya Kitaoka 120a6e
                               eps);
Shinya Kitaoka 120a6e
      if (firstBidiagIdx > 0) {
Shinya Kitaoka 120a6e
        if (NearZero(*(--sdPtr), eps)) {
Shinya Kitaoka 120a6e
          *sdPtr = 0.0;
Shinya Kitaoka 120a6e
        } else {
Shinya Kitaoka 120a6e
          ClearColumnWithDiagonalZero(firstBidiagIdx, V, wPtr, sdPtr, eps);
Shinya Kitaoka 120a6e
        }
Shinya Kitaoka 120a6e
      }
Shinya Kitaoka 120a6e
      continue;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    // Estimate an eigenvalue from bottom four entries of M
Shinya Kitaoka 120a6e
    // This gives a lambda value which will shift the Givens rotations
Shinya Kitaoka 120a6e
    // Last four entries of M^T * M are  ( ( A, B ), ( B, C ) ).
Shinya Kitaoka 120a6e
    double A;
Shinya Kitaoka 120a6e
    A = (firstBidiagIdx < lastBidiagIdx - 1)
Shinya Kitaoka 120a6e
            ? Square(superDiag[lastBidiagIdx - 2])
Shinya Kitaoka 120a6e
            : 0.0;
Shinya Kitaoka 120a6e
    double BSq = Square(w[lastBidiagIdx - 1]);
Shinya Kitaoka 120a6e
    A += BSq;  // The "A" entry of M^T * M
Shinya Kitaoka 120a6e
    double C = Square(superDiag[lastBidiagIdx - 1]);
Shinya Kitaoka 120a6e
    BSq *= C;                       // The squared "B" entry
Shinya Kitaoka 120a6e
    C += Square(w[lastBidiagIdx]);  // The "C" entry
Shinya Kitaoka 120a6e
    double lambda;                  // lambda will hold the estimated eigenvalue
Shinya Kitaoka 120a6e
    lambda = sqrt(Square((A - C) * 0.5) +
Shinya Kitaoka 120a6e
                  BSq);  // Use the lambda value that is closest to C.
Shinya Kitaoka 120a6e
    if (A > C) {
Shinya Kitaoka 120a6e
      lambda = -lambda;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    lambda += (A + C) *
Shinya Kitaoka 120a6e
              0.5;  // Now lambda equals the estimate for the last eigenvalue
Shinya Kitaoka 120a6e
    double t11 = Square(w[firstBidiagIdx]);
Shinya Kitaoka 120a6e
    double t12 = w[firstBidiagIdx] * superDiag[firstBidiagIdx];
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    double c, s;
Shinya Kitaoka 120a6e
    CalcGivensValues(t11 - lambda, t12, &c, &s);
Shinya Kitaoka 120a6e
    ApplyGivensCBTD(c, s, wPtr, sdPtr, &extraOffDiag, wPtr + 1);
Shinya Kitaoka 120a6e
    V.PostApplyGivens(c, -s, firstBidiagIdx);
Shinya Kitaoka 120a6e
    long i;
Shinya Kitaoka 120a6e
    for (i = firstBidiagIdx; i < lastBidiagIdx - 1; i++) {
Shinya Kitaoka 120a6e
      // Push non-zero from M[i+1,i] to M[i,i+2]
Shinya Kitaoka 120a6e
      CalcGivensValues(*wPtr, extraOffDiag, &c, &s);
Shinya Kitaoka 120a6e
      ApplyGivensCBTD(c, s, wPtr, sdPtr, &extraOffDiag, extraOffDiag, wPtr + 1,
Shinya Kitaoka 120a6e
                      sdPtr + 1);
Shinya Kitaoka 120a6e
      U.PostApplyGivens(c, -s, i);
Shinya Kitaoka 120a6e
      // Push non-zero from M[i,i+2] to M[1+2,i+1]
Shinya Kitaoka 120a6e
      CalcGivensValues(*sdPtr, extraOffDiag, &c, &s);
Shinya Kitaoka 120a6e
      ApplyGivensCBTD(c, s, sdPtr, wPtr + 1, &extraOffDiag, extraOffDiag,
Shinya Kitaoka 120a6e
                      sdPtr + 1, wPtr + 2);
Shinya Kitaoka 120a6e
      V.PostApplyGivens(c, -s, i + 1);
Shinya Kitaoka 120a6e
      wPtr++;
Shinya Kitaoka 120a6e
      sdPtr++;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    // Push non-zero value from M[i+1,i] to M[i,i+1] for i==lastBidiagIdx-1
Shinya Kitaoka 120a6e
    CalcGivensValues(*wPtr, extraOffDiag, &c, &s);
Shinya Kitaoka 120a6e
    ApplyGivensCBTD(c, s, wPtr, &extraOffDiag, sdPtr, wPtr + 1);
Shinya Kitaoka 120a6e
    U.PostApplyGivens(c, -s, i);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    // DEBUG
Shinya Kitaoka 120a6e
    // DebugCalcBidiagCheck( V, w, superDiag, U );
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
// This is called when there is a zero diagonal entry, with a non-zero
Shinya Kitaoka 120a6e
// superdiagonal entry on the same row.
Shinya Kitaoka 120a6e
// We use Givens rotations to "chase" the non-zero entry across the row; when it
Shinya Kitaoka 120a6e
// reaches the last
Toshihiro Shimizu 890ddd
//	column, it is finally zeroed away.
Shinya Kitaoka 120a6e
// wPtr points to the zero entry on the diagonal.  sdPtr points to the non-zero
Shinya Kitaoka 120a6e
// superdiagonal entry on the same row.
Shinya Kitaoka 120a6e
void MatrixRmn::ClearRowWithDiagonalZero(long firstBidiagIdx,
Shinya Kitaoka 120a6e
                                         long lastBidiagIdx, MatrixRmn &U,
Shinya Kitaoka 120a6e
                                         double *wPtr, double *sdPtr,
Shinya Kitaoka 120a6e
                                         double eps) {
Shinya Kitaoka 120a6e
  double curSd = *sdPtr;  // Value being chased across the row
Shinya Kitaoka 120a6e
  *sdPtr       = 0.0;
Shinya Kitaoka 120a6e
  long i       = firstBidiagIdx + 1;
Shinya Kitaoka 120a6e
  while (true) {
Shinya Kitaoka 120a6e
    // Rotate row i and row firstBidiagIdx (Givens rotation)
Shinya Kitaoka 120a6e
    double c, s;
Shinya Kitaoka 120a6e
    CalcGivensValues(*(++wPtr), curSd, &c, &s);
Shinya Kitaoka 120a6e
    U.PostApplyGivens(c, -s, i, firstBidiagIdx);
Shinya Kitaoka 120a6e
    *wPtr = c * (*wPtr) - s * curSd;
Shinya Kitaoka 120a6e
    if (i == lastBidiagIdx) {
Shinya Kitaoka 120a6e
      break;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    curSd = s * (*(++sdPtr));  // New value pops up one column over to the right
Shinya Kitaoka 120a6e
    *sdPtr = c * (*sdPtr);
Shinya Kitaoka 120a6e
    i++;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
// This is called when there is a zero diagonal entry, with a non-zero
Shinya Kitaoka 120a6e
// superdiagonal entry in the same column.
Shinya Kitaoka 120a6e
// We use Givens rotations to "chase" the non-zero entry up the column; when it
Shinya Kitaoka 120a6e
// reaches the last
Toshihiro Shimizu 890ddd
//	column, it is finally zeroed away.
Shinya Kitaoka 120a6e
// wPtr points to the zero entry on the diagonal.  sdPtr points to the non-zero
Shinya Kitaoka 120a6e
// superdiagonal entry in the same column.
Shinya Kitaoka 120a6e
void MatrixRmn::ClearColumnWithDiagonalZero(long endIdx, MatrixRmn &V,
Shinya Kitaoka 120a6e
                                            double *wPtr, double *sdPtr,
Shinya Kitaoka 120a6e
                                            double eps) {
Shinya Kitaoka 120a6e
  double curSd = *sdPtr;  // Value being chased up the column
Shinya Kitaoka 120a6e
  *sdPtr       = 0.0;
Shinya Kitaoka 120a6e
  long i       = endIdx - 1;
Shinya Kitaoka 120a6e
  while (true) {
Shinya Kitaoka 120a6e
    double c, s;
Shinya Kitaoka 120a6e
    CalcGivensValues(*(--wPtr), curSd, &c, &s);
Shinya Kitaoka 120a6e
    V.PostApplyGivens(c, -s, i, endIdx);
Shinya Kitaoka 120a6e
    *wPtr = c * (*wPtr) - s * curSd;
Shinya Kitaoka 120a6e
    if (i == 0) {
Shinya Kitaoka 120a6e
      break;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    curSd = s * (*(--sdPtr));  // New value pops up one row above
Shinya Kitaoka 120a6e
    if (NearZero(curSd, eps)) {
Shinya Kitaoka 120a6e
      break;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    *sdPtr = c * (*sdPtr);
Shinya Kitaoka 120a6e
    i--;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
// Matrix A is  ( ( a c ) ( b d ) ), i.e., given in column order.
Toshihiro Shimizu 890ddd
// Mult's G[c,s]  times  A, replaces A.
Shinya Kitaoka 120a6e
void MatrixRmn::ApplyGivensCBTD(double cosine, double sine, double *a,
Shinya Kitaoka 120a6e
                                double *b, double *c, double *d) {
Shinya Kitaoka 120a6e
  double temp = *a;
Shinya Kitaoka 120a6e
  *a          = cosine * (*a) - sine * (*b);
Shinya Kitaoka 120a6e
  *b          = sine * temp + cosine * (*b);
Shinya Kitaoka 120a6e
  temp        = *c;
Shinya Kitaoka 120a6e
  *c          = cosine * (*c) - sine * (*d);
Shinya Kitaoka 120a6e
  *d          = sine * temp + cosine * (*d);
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
// Now matrix A given in row order, A = ( ( a b c ) ( d e f ) ).
Toshihiro Shimizu 890ddd
// Return G[c,s] * A, replace A.  d becomes zero, no need to return.
Toshihiro Shimizu 890ddd
//  Also, it is certain the old *c value is taken to be zero!
Shinya Kitaoka 120a6e
void MatrixRmn::ApplyGivensCBTD(double cosine, double sine, double *a,
Shinya Kitaoka 120a6e
                                double *b, double *c, double d, double *e,
Shinya Kitaoka 120a6e
                                double *f) {
Shinya Kitaoka 120a6e
  *a          = cosine * (*a) - sine * d;
Shinya Kitaoka 120a6e
  double temp = *b;
Shinya Kitaoka 120a6e
  *b          = cosine * (*b) - sine * (*e);
Shinya Kitaoka 120a6e
  *e          = sine * temp + cosine * (*e);
Shinya Kitaoka 120a6e
  *c          = -sine * (*f);
Shinya Kitaoka 120a6e
  *f          = cosine * (*f);
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
// Helper routine for SVD conversion from bidiagonal to diagonal
Shinya Kitaoka 120a6e
bool MatrixRmn::UpdateBidiagIndices(long *firstBidiagIdx, long *lastBidiagIdx,
Shinya Kitaoka 120a6e
                                    VectorRn &w, VectorRn &superDiag,
Shinya Kitaoka 120a6e
                                    double eps) {
Shinya Kitaoka 120a6e
  long lastIdx = *lastBidiagIdx;
Shinya Kitaoka 120a6e
  double *sdPtr =
Shinya Kitaoka 120a6e
      superDiag.GetPtr(lastIdx - 1);  // Entry above the last diagonal entry
Shinya Kitaoka 120a6e
  while (NearZero(*sdPtr, eps)) {
Shinya Kitaoka 120a6e
    *(sdPtr--) = 0.0;
Shinya Kitaoka 120a6e
    lastIdx--;
Shinya Kitaoka 120a6e
    if (lastIdx == 0) {
Shinya Kitaoka 120a6e
      return false;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
  *lastBidiagIdx = lastIdx;
Shinya Kitaoka 120a6e
  long firstIdx  = lastIdx - 1;
Shinya Kitaoka 120a6e
  double *wPtr   = w.GetPtr(firstIdx);
Shinya Kitaoka 120a6e
  while (firstIdx > 0) {
Shinya Kitaoka 120a6e
    if (NearZero(*wPtr, eps)) {  // If this diagonal entry (near) zero
Shinya Kitaoka 120a6e
      *wPtr = 0.0;
Shinya Kitaoka 120a6e
      break;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    if (NearZero(
Shinya Kitaoka 120a6e
            *(--sdPtr),
Shinya Kitaoka 120a6e
            eps)) {  // If the entry above the diagonal entry is (near) zero
Shinya Kitaoka 120a6e
      *sdPtr = 0.0;
Shinya Kitaoka 120a6e
      break;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    wPtr--;
Shinya Kitaoka 120a6e
    firstIdx--;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
  *firstBidiagIdx = firstIdx;
Shinya Kitaoka 120a6e
  return true;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
// ******************************************DEBUG STUFFF
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
bool MatrixRmn::DebugCheckSVD(const MatrixRmn &U, const VectorRn &w,
Shinya Kitaoka 120a6e
                              const MatrixRmn &V) const {
Shinya Kitaoka 120a6e
  // Special SVD test code
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  MatrixRmn IV(V.getNumRows(), V.getNumColumns());
Shinya Kitaoka 120a6e
  IV.SetIdentity();
Shinya Kitaoka 120a6e
  MatrixRmn VTV(V.getNumRows(), V.getNumColumns());
Shinya Kitaoka 120a6e
  MatrixRmn::TransposeMultiply(V, V, VTV);
Shinya Kitaoka 120a6e
  IV -= VTV;
Shinya Kitaoka 120a6e
  double error = IV.FrobeniusNorm();
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  MatrixRmn IU(U.getNumRows(), U.getNumColumns());
Shinya Kitaoka 120a6e
  IU.SetIdentity();
Shinya Kitaoka 120a6e
  MatrixRmn UTU(U.getNumRows(), U.getNumColumns());
Shinya Kitaoka 120a6e
  MatrixRmn::TransposeMultiply(U, U, UTU);
Shinya Kitaoka 120a6e
  IU -= UTU;
Shinya Kitaoka 120a6e
  error += IU.FrobeniusNorm();
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  MatrixRmn Diag(U.getNumRows(), V.getNumRows());
Shinya Kitaoka 120a6e
  Diag.SetZero();
Shinya Kitaoka 120a6e
  Diag.SetDiagonalEntries(w);
Shinya Kitaoka 120a6e
  MatrixRmn B(U.getNumRows(), V.getNumRows());
Shinya Kitaoka 120a6e
  MatrixRmn C(U.getNumRows(), V.getNumRows());
Shinya Kitaoka 120a6e
  MatrixRmn::Multiply(U, Diag, B);
Shinya Kitaoka 120a6e
  MatrixRmn::MultiplyTranspose(B, V, C);
Shinya Kitaoka 120a6e
  C -= *this;
Shinya Kitaoka 120a6e
  error += C.FrobeniusNorm();
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  bool ret = (fabs(error) <= 1.0e-13 * w.MaxAbs());
Shinya Kitaoka 120a6e
  assert(ret);
Shinya Kitaoka 120a6e
  return ret;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//=============================================================================
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
const double PI               = 3.1415926535897932384626433832795028841972;
Toshihiro Shimizu 890ddd
const double RadiansToDegrees = 180.0 / PI;
Toshihiro Shimizu 890ddd
const double DegreesToRadians = PI / 180;
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
const double Jacobian::DefaultDampingLambda = 1.1;
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
const double Jacobian::PseudoInverseThresholdFactor = 0.001;
Shinya Kitaoka 120a6e
const double Jacobian::MaxAngleJtranspose           = 30.0 * DegreesToRadians;
Shinya Kitaoka 120a6e
const double Jacobian::MaxAnglePseudoinverse        = 5.0 * DegreesToRadians;
Shinya Kitaoka 120a6e
const double Jacobian::MaxAngleDLS                  = 5.0 * DegreesToRadians;
Shinya Kitaoka 120a6e
const double Jacobian::MaxAngleSDLS                 = 45.0 * DegreesToRadians;
Shinya Kitaoka 120a6e
const double Jacobian::BaseMaxTargetDist            = 3.4;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
Jacobian::Jacobian(IKSkeleton *skeleton, std::vector<tpointd> &targetPos) {</tpointd>
Shinya Kitaoka 120a6e
  Jacobian::skeleton = skeleton;
Shinya Kitaoka 120a6e
  nEffector          = skeleton->getNumEffector();
Shinya Kitaoka 120a6e
  nJoint             = skeleton->getNodeCount() -
Shinya Kitaoka 120a6e
           nEffector;  // numero dei giunti meno gli effettori
Shinya Kitaoka 120a6e
  nRow = 2 * nEffector;
Shinya Kitaoka 120a6e
  nCol = nJoint;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  target = (targetPos);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  Jend.SetSize(nRow, nCol);  // Matrice jacobiana
Shinya Kitaoka 120a6e
  Jend.SetZero();
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  Jtarget.SetSize(
Shinya Kitaoka 120a6e
      nRow,
Shinya Kitaoka 120a6e
      nCol);  // Matrice jacobiana basta sulle posizioni dei targets (non usata)
Shinya Kitaoka 120a6e
  Jtarget.SetZero();
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  U.SetSize(nRow, nRow);  // matrice U per il calcolo SVD
Shinya Kitaoka 120a6e
  w.SetLength(min(nRow, nCol));
Shinya Kitaoka 120a6e
  V.SetSize(nCol, nCol);  // matrice V per il calcolo SVD
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  dS.SetLength(nRow);      // (Posizione Target ) - (posizione End effector)
Shinya Kitaoka 120a6e
  dTheta.SetLength(nCol);  // Cambiamenti degli angoli dei Joints
Shinya Kitaoka 120a6e
  dPreTheta.SetLength(nCol);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  // Usato nel: metodo del trasposto dello Jacobiano  & DLS & SDLS
Shinya Kitaoka 120a6e
  dT.SetLength(nRow);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  // Usato nel Selectively Damped Least Squares Method
Shinya Kitaoka 120a6e
  dSclamp.SetLength(nEffector);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  Jnorms.SetSize(nEffector, nCol);  // Memorizza le norme della matrice attiva J
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  DampingLambdaSqV.SetLength(nRow);
Shinya Kitaoka 120a6e
  diagMatIdentity.SetLength(nCol);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  Reset();
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
void Jacobian::Reset() {
Shinya Kitaoka 120a6e
  // Usato nel Damped Least Squares Method
Shinya Kitaoka 120a6e
  DampingLambda   = DefaultDampingLambda;
Shinya Kitaoka 120a6e
  DampingLambdaSq = Square(DampingLambda);
Shinya Kitaoka 120a6e
  for (int i            = 0; i < DampingLambdaSqV.GetLength(); i++)
Shinya Kitaoka 120a6e
    DampingLambdaSqV[i] = DampingLambdaSq;
Shinya Kitaoka 120a6e
  for (int i           = 0; i < diagMatIdentity.GetLength(); i++)
Shinya Kitaoka 120a6e
    diagMatIdentity[i] = 1.0;
Shinya Kitaoka 120a6e
  // DampingLambdaSDLS = 1.5*DefaultDampingLambda;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  dSclamp.Fill(HUGE_VAL);
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
// Calcola il vettore deltaS vector, dS, (l' errore tra end effector e target
Toshihiro Shimizu 890ddd
// Calcola le matrce jacobiana J
Shinya Kitaoka 120a6e
void Jacobian::computeJacobian() {
Shinya Kitaoka 120a6e
  // Scorro tutto lo skeleton per trovare tutti gli end effectors
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  int numNode = skeleton->getNodeCount();
Shinya Kitaoka 120a6e
  for (int index = 0; index < numNode; index++) {
Shinya Kitaoka 120a6e
    IKNode *n         = skeleton->getNode(index);
Shinya Kitaoka 120a6e
    int effectorCount = skeleton->getNumEffector();
Shinya Kitaoka 120a6e
    if (n->IsEffector()) {
Shinya Kitaoka 120a6e
      int i                    = n->getEffectorNum();
Shinya Kitaoka 120a6e
      const TPointD &targetPos = target[i];
Shinya Kitaoka 120a6e
      TPointD temp;
Shinya Kitaoka 120a6e
      // Calcolo i valori di deltaS (differenza tra end effectors e target
Shinya Kitaoka 120a6e
      // positions.)
Shinya Kitaoka 120a6e
      temp      = targetPos;
Shinya Kitaoka 120a6e
      TPointD a = n->GetS();
Shinya Kitaoka 120a6e
      temp -= n->GetS();
Shinya Kitaoka 120a6e
      if (i < effectorCount - 1) {
Shinya Kitaoka 120a6e
        temp.x = 100 * temp.x;
Shinya Kitaoka 120a6e
        temp.y = 100 * temp.y;
Shinya Kitaoka 120a6e
      }
Shinya Kitaoka 120a6e
      dS.SetCouple(i, temp);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
      // Find all ancestors (they will usually all be joints)
Shinya Kitaoka 120a6e
      // Set the corresponding entries in the Jacobians J, K.
Shinya Kitaoka 120a6e
      IKNode *m = skeleton->getParent(n);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
      while (m) {
Shinya Kitaoka 120a6e
        int j = m->getJointNum();
Shinya Kitaoka 120a6e
        // assert(j>=0 && j<skeleton->GetNumJoint());</skeleton->
Shinya Kitaoka 120a6e
        int numnode = skeleton->getNodeCount();
Shinya Kitaoka 120a6e
        assert(0 <= i && i < nEffector && 0 <= j &&
Shinya Kitaoka 120a6e
               j < (skeleton->getNodeCount() - skeleton->getNumEffector()));
Shinya Kitaoka 120a6e
        if (m->isFrozen()) {
Shinya Kitaoka 120a6e
          Jend.SetCouple(i, j, TPointD(0.0, 0.0));
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
        } else {
Shinya Kitaoka 120a6e
          temp = m->GetS();   // joint pos.
Shinya Kitaoka 120a6e
          temp -= n->GetS();  // -(end effector pos. - joint pos.)
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
          double tx = temp.x;
Shinya Kitaoka 120a6e
          temp.x    = temp.y;
Shinya Kitaoka 120a6e
          temp.y    = -tx;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
          if (i < effectorCount - 1) {
Shinya Kitaoka 120a6e
            temp.x = 100 * temp.x;
Shinya Kitaoka 120a6e
            temp.y = 100 * temp.y;
Shinya Kitaoka 120a6e
          }
Shinya Kitaoka 120a6e
          Jend.SetCouple(i, j, temp);
Shinya Kitaoka 120a6e
        }
Shinya Kitaoka 120a6e
        m = skeleton->getParent(m);
Shinya Kitaoka 120a6e
      }
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
// The delta theta values have been computed in dTheta array
Toshihiro Shimizu 890ddd
// Apply the delta theta values to the joints
Toshihiro Shimizu 890ddd
// Nothing is done about joint limits for now.
Shinya Kitaoka 120a6e
void Jacobian::UpdateThetas() {
Shinya Kitaoka 120a6e
  // Update the joint angles
Shinya Kitaoka 120a6e
  for (int index = 0; index < skeleton->getNodeCount(); index++) {
Shinya Kitaoka 120a6e
    IKNode *n = skeleton->getNode(index);
Shinya Kitaoka 120a6e
    if (n->IsJoint()) {
Shinya Kitaoka 120a6e
      int i = n->getJointNum();
Shinya Kitaoka 120a6e
      n->AddToTheta(dTheta[i]);
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
  // Aggiorno le posizioni dei joint
Shinya Kitaoka 120a6e
  skeleton->compute();
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
bool Jacobian::checkJointsLimit() {
Shinya Kitaoka 120a6e
  bool clampingDetected = false;
Shinya Kitaoka 120a6e
  /*
Shinya Kitaoka 120a6e
  Node* n = skeleton->getNode(3);
Shinya Kitaoka 120a6e
  int indexJoint = n->getJointNum();
Shinya Kitaoka 120a6e
  double theta = n->getTheta();
Shinya Kitaoka 120a6e
  double upperLimit = PI;
Shinya Kitaoka 120a6e
  double lowerLimit = 0.0;
Shinya Kitaoka 120a6e
  if(theta >upperLimit || theta 
Shinya Kitaoka 120a6e
  {
Shinya Kitaoka 120a6e
          if(theta
Shinya Kitaoka 120a6e
          clampingDetected = true;
Shinya Kitaoka 120a6e
          double clampingVar = theta - upperLimit;
Shinya Kitaoka 120a6e
          for(int i=0;i<jactive->getNumRows();i++)</jactive->
Shinya Kitaoka 120a6e
          {
Shinya Kitaoka 120a6e
                  double tmp = clampingVar*Jactive->Get(i,indexJoint);
Shinya Kitaoka 120a6e
                  dS[i] -= tmp;
Shinya Kitaoka 120a6e
                  Jactive->Set(i,indexJoint,0.0);
Shinya Kitaoka 120a6e
          }
Shinya Kitaoka 120a6e
          n->setTheta(upperLimit);
Shinya Kitaoka 120a6e
          diagMatIdentity.Set(indexJoint, 0.0);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  }*/
Shinya Kitaoka 120a6e
  return clampingDetected;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
void Jacobian::ZeroDeltaThetas() { dTheta.SetZero(); }
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
// Find the delta theta values using inverse Jacobian method
Toshihiro Shimizu 890ddd
// Uses a greedy method to decide scaling factor
Shinya Kitaoka 120a6e
void Jacobian::CalcDeltaThetasTranspose() {
Shinya Kitaoka 120a6e
  const MatrixRmn &J = Jend;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  J.MultiplyTranspose(dS, dTheta);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Scale back the dTheta values greedily
Shinya Kitaoka 120a6e
  J.Multiply(dTheta, dT);  // dT = J * dTheta
Shinya Kitaoka 120a6e
  double alpha = Dot(dS, dT) / dT.NormSq();
Shinya Kitaoka 120a6e
  assert(alpha > 0.0);
Shinya Kitaoka 120a6e
  // Also scale back to be have max angle change less than MaxAngleJtranspose
Shinya Kitaoka 120a6e
  double maxChange = dTheta.MaxAbs();
Shinya Kitaoka 120a6e
  double beta      = MaxAngleJtranspose / maxChange;
Shinya Kitaoka 120a6e
  dTheta *= min(alpha, beta);
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
void Jacobian::CalcDeltaThetasPseudoinverse() {
Shinya Kitaoka 120a6e
  MatrixRmn &J = const_cast<matrixrmn &="">(Jend);</matrixrmn>
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // costruisco matrice J1
Shinya Kitaoka 120a6e
  MatrixRmn J1;
Shinya Kitaoka 120a6e
  J1.SetSize(2, J.getNumColumns());
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  for (int i = 0; i < 2; i++)
Shinya Kitaoka 120a6e
    for (int j = 0; j < J.getNumColumns(); j++) J1.Set(i, j, J.Get(i, j));
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // COSTRUISCO VETTORI ds1 e ds2
Shinya Kitaoka 120a6e
  VectorRn dS1(2);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  for (int i = 0; i < 2; i++) dS1.Set(i, dS.Get(i));
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // calcolo dtheta1
Shinya Kitaoka 120a6e
  MatrixRmn U, V;
Shinya Kitaoka 120a6e
  VectorRn w;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  U.SetSize(J1.getNumRows(), J1.getNumRows());
Shinya Kitaoka 120a6e
  w.SetLength(min(J1.getNumRows(), J1.getNumColumns()));
Shinya Kitaoka 120a6e
  V.SetSize(J1.getNumColumns(), J1.getNumColumns());
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  J1.ComputeSVD(U, w, V);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Next line for debugging only
Shinya Kitaoka 120a6e
  assert(J1.DebugCheckSVD(U, w, V));
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Calculate response vector dTheta that is the DLS solution.
Shinya Kitaoka 120a6e
  //	Delta target values are the dS values
Shinya Kitaoka 120a6e
  //  We multiply by Moore-Penrose pseudo-inverse of the J matrix
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  double pseudoInverseThreshold = PseudoInverseThresholdFactor * w.MaxAbs();
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  long diagLength = w.GetLength();
Shinya Kitaoka 120a6e
  double *wPtr    = w.GetPtr();
Shinya Kitaoka 120a6e
  dTheta.SetZero();
Shinya Kitaoka 120a6e
  for (long i = 0; i < diagLength; i++) {
Shinya Kitaoka 120a6e
    double dotProdCol =
Shinya Kitaoka 120a6e
        U.DotProductColumn(dS1, i);  // Dot product with i-th column of U
Shinya Kitaoka 120a6e
    double alpha = *(wPtr++);
Shinya Kitaoka 120a6e
    if (fabs(alpha) > pseudoInverseThreshold) {
Shinya Kitaoka 120a6e
      alpha = 1.0 / alpha;
Shinya Kitaoka 120a6e
      MatrixRmn::AddArrayScale(V.getNumRows(), V.GetColumnPtr(i), 1,
Shinya Kitaoka 120a6e
                               dTheta.GetPtr(), 1, dotProdCol * alpha);
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  MatrixRmn JcurrentPinv(V.getNumRows(),
Shinya Kitaoka 120a6e
                         J1.getNumRows());  // pseudoinversa di J1
Shinya Kitaoka 120a6e
  MatrixRmn JProjPre(JcurrentPinv.getNumRows(),
Shinya Kitaoka 120a6e
                     J1.getNumColumns());  // Proiezione di J1
Shinya Kitaoka 120a6e
  if (skeleton->getNumEffector() > 1) {
Shinya Kitaoka 120a6e
    // calcolo la pseudoinversa di J1
Shinya Kitaoka 120a6e
    MatrixRmn VD(V.getNumRows(), J1.getNumRows());  // matrice del prodotto V*w
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    double *wPtr           = w.GetPtr();
Shinya Kitaoka 120a6e
    pseudoInverseThreshold = PseudoInverseThresholdFactor * w.MaxAbs();
Shinya Kitaoka 120a6e
    for (int j = 0; j < VD.getNumColumns(); j++) {
Shinya Kitaoka 120a6e
      double *VPtr = V.GetColumnPtr(j);
Shinya Kitaoka 120a6e
      double alpha = *(wPtr++);  // elemento matrice diagonale
Shinya Kitaoka 120a6e
      for (int i = 0; i < V.getNumRows(); i++) {
Shinya Kitaoka 120a6e
        if (fabs(alpha) > pseudoInverseThreshold) {
Shinya Kitaoka 120a6e
          double entry = *(VPtr++);
Shinya Kitaoka 120a6e
          VD.Set(i, j, entry * (1.0 / alpha));
Shinya Kitaoka 120a6e
        }
Shinya Kitaoka 120a6e
      }
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    MatrixRmn::MultiplyTranspose(VD, U, JcurrentPinv);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    // calcolo la proiezione J1
Shinya Kitaoka 120a6e
    MatrixRmn::Multiply(JcurrentPinv, J1, JProjPre);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    for (int j = 0; j < JProjPre.getNumColumns(); j++)
Shinya Kitaoka 120a6e
      for (int i = 0; i < JProjPre.getNumRows(); i++) {
Shinya Kitaoka 120a6e
        double temp = JProjPre.Get(i, j);
Shinya Kitaoka 120a6e
        JProjPre.Set(i, j, -1.0 * temp);
Shinya Kitaoka 120a6e
      }
Shinya Kitaoka 120a6e
    JProjPre.AddToDiagonal(diagMatIdentity);
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // task priority strategy
Shinya Kitaoka 120a6e
  for (int i = 1; i < skeleton->getNumEffector(); i++) {
Shinya Kitaoka 120a6e
    // costruisco matrice Jcurrent (Ji)
Shinya Kitaoka 120a6e
    MatrixRmn Jcurrent(2, J.getNumColumns());
Shinya Kitaoka 120a6e
    for (int j = 0; j < J.getNumColumns(); j++)
Shinya Kitaoka 120a6e
      for (int k = 0; k < 2; k++) Jcurrent.Set(k, j, J.Get(k + 2 * i, j));
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    // costruisco il vettore dScurrent
Shinya Kitaoka 120a6e
    VectorRn dScurrent(2);
Shinya Kitaoka 120a6e
    for (int k = 0; k < 2; k++) dScurrent.Set(k, dS.Get(k + 2 * i));
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    // Moltiplico Jcurrent per la proiezione di J(i-1)
Shinya Kitaoka 120a6e
    MatrixRmn Jdst(Jcurrent.getNumRows(), JProjPre.getNumColumns());
Shinya Kitaoka 120a6e
    MatrixRmn::Multiply(Jcurrent, JProjPre, Jdst);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    // Calcolo la pseudoinversa di Jdst
Shinya Kitaoka 120a6e
    MatrixRmn UU(Jdst.getNumRows(), Jdst.getNumRows()),
Shinya Kitaoka 120a6e
        VV(Jdst.getNumColumns(), Jdst.getNumColumns());
Shinya Kitaoka 120a6e
    VectorRn ww(min(Jdst.getNumRows(), Jdst.getNumColumns()));
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    Jdst.ComputeSVD(UU, ww, VV);
Shinya Kitaoka 120a6e
    assert(Jdst.DebugCheckSVD(UU, ww, VV));
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    MatrixRmn VVD(VV.getNumRows(), J1.getNumRows());  // matrice V*w
Shinya Kitaoka 120a6e
    VVD.SetZero();
Shinya Kitaoka 120a6e
    pseudoInverseThreshold = PseudoInverseThresholdFactor * ww.MaxAbs();
Shinya Kitaoka 120a6e
    double *wwPtr          = ww.GetPtr();
Shinya Kitaoka 120a6e
    for (int j = 0; j < VVD.getNumColumns(); j++) {
Shinya Kitaoka 120a6e
      double *VVPtr = VV.GetColumnPtr(j);
Shinya Kitaoka 120a6e
      double alpha  = 50 * (*(wwPtr++));  // elemento matrice diagonale
Shinya Kitaoka 120a6e
      for (int i = 0; i < VV.getNumRows(); i++) {
Shinya Kitaoka 120a6e
        if (fabs(alpha) > 100 * pseudoInverseThreshold) {
Shinya Kitaoka 120a6e
          double entry = *(VVPtr++);
Shinya Kitaoka 120a6e
          VVD.Set(i, j, entry * (1.0 / alpha));
Shinya Kitaoka 120a6e
        }
Shinya Kitaoka 120a6e
      }
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    MatrixRmn JdstPinv(VV.getNumRows(), J1.getNumRows());
Shinya Kitaoka 120a6e
    MatrixRmn::MultiplyTranspose(VVD, UU, JdstPinv);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    VectorRn dTemp(J1.getNumRows());
Shinya Kitaoka 120a6e
    Jcurrent.Multiply(dTheta, dTemp);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    VectorRn dTemp2(dScurrent.GetLength());
Shinya Kitaoka 120a6e
    for (int k  = 0; k < dScurrent.GetLength(); k++)
Shinya Kitaoka 120a6e
      dTemp2[k] = dScurrent[k] - dTemp[k];
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    // Moltiplico JdstPinv per dTemp2
Shinya Kitaoka 120a6e
    VectorRn dThetaCurrent(JdstPinv.getNumRows());
Shinya Kitaoka 120a6e
    JdstPinv.Multiply(dTemp2, dThetaCurrent);
Shinya Kitaoka 120a6e
    for (int k = 0; k < dTheta.GetLength(); k++) dTheta[k] += dThetaCurrent[k];
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    // Infine mi calcolo la pseudoinversa di Jcurrent e quindi la sua proiezione
Shinya Kitaoka 120a6e
    // che servirà al passo successivo
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    // calcolo la pseudoinversa di Jcurrent
Shinya Kitaoka 120a6e
    Jcurrent.ComputeSVD(U, w, V);
Shinya Kitaoka 120a6e
    assert(Jcurrent.DebugCheckSVD(U, w, V));
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    MatrixRmn VD(V.getNumRows(),
Shinya Kitaoka 120a6e
                 Jcurrent.getNumRows());  // matrice del prodotto V*w
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    double *wPtr           = w.GetPtr();
Shinya Kitaoka 120a6e
    pseudoInverseThreshold = PseudoInverseThresholdFactor * w.MaxAbs();
Shinya Kitaoka 120a6e
    for (int j = 0; j < VVD.getNumColumns(); j++) {
Shinya Kitaoka 120a6e
      double *VPtr = V.GetColumnPtr(j);
Shinya Kitaoka 120a6e
      double alpha = *(wPtr++);  // elemento matrice diagonale
Shinya Kitaoka 120a6e
      for (int i = 0; i < V.getNumRows(); i++) {
Shinya Kitaoka 120a6e
        if (fabs(alpha) > pseudoInverseThreshold) {
Shinya Kitaoka 120a6e
          double entry = *(VPtr++);
Shinya Kitaoka 120a6e
          VD.Set(i, j, entry * (1.0 / alpha));
Shinya Kitaoka 120a6e
        }
Shinya Kitaoka 120a6e
      }
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    MatrixRmn::MultiplyTranspose(VD, U, JcurrentPinv);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    // calcolo la proiezione Jcurrent
Shinya Kitaoka 120a6e
    MatrixRmn::Multiply(JcurrentPinv, Jcurrent, JProjPre);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    for (int j = 0; j < JProjPre.getNumColumns(); j++)
Shinya Kitaoka 120a6e
      for (int k = 0; k < JProjPre.getNumRows(); k++) {
Shinya Kitaoka 120a6e
        double temp = JProjPre.Get(k, j);
Shinya Kitaoka 120a6e
        JProjPre.Set(k, j, -1.0 * temp);
Shinya Kitaoka 120a6e
      }
Shinya Kitaoka 120a6e
    JProjPre.AddToDiagonal(diagMatIdentity);
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // sw.stop();
Shinya Kitaoka 120a6e
  // std::ofstream os("C:\\buttami.txt", std::ios::app);
Shinya Kitaoka 120a6e
  // sw.print(os);
Shinya Kitaoka 120a6e
  // os.close();
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Scale back to not exceed maximum angle changes
Shinya Kitaoka 120a6e
  double maxChange = 10 * dTheta.MaxAbs();
Shinya Kitaoka 120a6e
  if (maxChange > MaxAnglePseudoinverse) {
Shinya Kitaoka 120a6e
    dTheta *= MaxAnglePseudoinverse / maxChange;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
void Jacobian::CalcDeltaThetasDLS() {
Shinya Kitaoka 120a6e
  const MatrixRmn &J = Jend;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  MatrixRmn::MultiplyTranspose(J, J, U);  // U = J * (J^T)
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  U.AddToDiagonal(DampingLambdaSqV);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  // Use the next four lines instead of the succeeding two lines for the DLS
Shinya Kitaoka 120a6e
  // method with clamped error vector e.
Shinya Kitaoka 120a6e
  // CalcdTClampedFromdS();
Shinya Kitaoka 120a6e
  // VectorRn dTextra(2*nEffector);
Shinya Kitaoka 120a6e
  // U.Solve( dT, &dTextra );
Shinya Kitaoka 120a6e
  // J.MultiplyTranspose( dTextra, dTheta );
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  // Use these two lines for the traditional DLS method
Shinya Kitaoka 120a6e
  // gennaro
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  U.Solve(dS, &dT);
Shinya Kitaoka 120a6e
  J.MultiplyTranspose(dT, dTheta);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  // Scalo per non superare l'nagolo massimod i cambiamento
Shinya Kitaoka 120a6e
  double maxChange = 100 * dTheta.MaxAbs();
Shinya Kitaoka 120a6e
  if (maxChange > MaxAngleDLS) {
Shinya Kitaoka 120a6e
    dTheta *= MaxAngleDLS / maxChange;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
void Jacobian::CalcDeltaThetasDLSwithSVD() {
Shinya Kitaoka 120a6e
  const MatrixRmn &J = Jend;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  J.ComputeSVD(U, w, V);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // For Debug
Shinya Kitaoka 120a6e
  assert(J.DebugCheckSVD(U, w, V));
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Calculate response vector dTheta that is the DLS solution.
Shinya Kitaoka 120a6e
  //	Delta target values are the dS values
Shinya Kitaoka 120a6e
  //  We multiply by DLS inverse of the J matrix
Shinya Kitaoka 120a6e
  long diagLength = w.GetLength();
Shinya Kitaoka 120a6e
  double *wPtr    = w.GetPtr();
Shinya Kitaoka 120a6e
  dTheta.SetZero();
Shinya Kitaoka 120a6e
  for (long i = 0; i < diagLength; i++) {
Shinya Kitaoka 120a6e
    double dotProdCol =
Shinya Kitaoka 120a6e
        U.DotProductColumn(dS, i);  // Dot product with i-th column of U
Shinya Kitaoka 120a6e
    double alpha = *(wPtr++);
Shinya Kitaoka 120a6e
    alpha        = alpha / (Square(alpha) + DampingLambdaSq);
Shinya Kitaoka 120a6e
    MatrixRmn::AddArrayScale(V.getNumRows(), V.GetColumnPtr(i), 1,
Shinya Kitaoka 120a6e
                             dTheta.GetPtr(), 1, dotProdCol * alpha);
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Scale back to not exceed maximum angle changes
Shinya Kitaoka 120a6e
  double maxChange = dTheta.MaxAbs();
Shinya Kitaoka 120a6e
  if (maxChange > MaxAngleDLS) {
Shinya Kitaoka 120a6e
    dTheta *= MaxAngleDLS / maxChange;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
void Jacobian::CalcDeltaThetasSDLS() {
Shinya Kitaoka 120a6e
  const MatrixRmn &J = Jend;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Compute Singular Value Decomposition
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  J.ComputeSVD(U, w, V);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Next line for debugging only
Shinya Kitaoka 120a6e
  assert(J.DebugCheckSVD(U, w, V));
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Calculate response vector dTheta that is the SDLS solution.
Shinya Kitaoka 120a6e
  //	Delta target values are the dS values
Shinya Kitaoka 120a6e
  int nRows           = J.getNumRows();
Shinya Kitaoka 120a6e
  int numEndEffectors = skeleton->getNumEffector();  // Equals the number of
Shinya Kitaoka 120a6e
                                                     // rows of J divided by
Shinya Kitaoka 120a6e
                                                     // three
Shinya Kitaoka 120a6e
  int nCols = J.getNumColumns();
Shinya Kitaoka 120a6e
  dTheta.SetZero();
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Calculate the norms of the 3-vectors in the Jacobian
Shinya Kitaoka 120a6e
  long i;
Shinya Kitaoka 120a6e
  const double *jx = J.GetPtr();
Shinya Kitaoka 120a6e
  double *jnx      = Jnorms.GetPtr();
Shinya Kitaoka 120a6e
  for (i = nCols * numEndEffectors; i > 0; i--) {
Shinya Kitaoka 120a6e
    double accumSq = Square(*(jx++));
Shinya Kitaoka 120a6e
    accumSq += Square(*(jx++));
Shinya Kitaoka 120a6e
    accumSq += Square(*(jx++));
Shinya Kitaoka 120a6e
    *(jnx++) = sqrt(accumSq);
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Clamp the dS values
Shinya Kitaoka 120a6e
  CalcdTClampedFromdS();
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Loop over each singular vector
Shinya Kitaoka 120a6e
  for (i = 0; i < nRows; i++) {
Shinya Kitaoka 120a6e
    double wiInv = w[i];
Shinya Kitaoka 120a6e
    if (NearZero(wiInv, 1.0e-10)) {
Shinya Kitaoka 120a6e
      continue;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    wiInv = 1.0 / wiInv;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    double N = 0.0;  // N is the quasi-1-norm of the i-th column of U
Shinya Kitaoka 120a6e
    double alpha =
Shinya Kitaoka 120a6e
        0.0;  // alpha is the dot product of dT and the i-th column of U
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    const double *dTx = dT.GetPtr();
Shinya Kitaoka 120a6e
    const double *ux  = U.GetColumnPtr(i);
Shinya Kitaoka 120a6e
    long j;
Shinya Kitaoka 120a6e
    for (j = numEndEffectors; j > 0; j--) {
Shinya Kitaoka 120a6e
      double tmp;
Shinya Kitaoka 120a6e
      alpha += (*ux) * (*(dTx++));
Shinya Kitaoka 120a6e
      tmp = Square(*(ux++));
Shinya Kitaoka 120a6e
      alpha += (*ux) * (*(dTx++));
Shinya Kitaoka 120a6e
      tmp += Square(*(ux++));
Shinya Kitaoka 120a6e
      alpha += (*ux) * (*(dTx++));
Shinya Kitaoka 120a6e
      tmp += Square(*(ux++));
Shinya Kitaoka 120a6e
      N += sqrt(tmp);
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    // M is the quasi-1-norm of the response to angles changing according to the
Shinya Kitaoka 120a6e
    // i-th column of V
Shinya Kitaoka 120a6e
    //		Then is multiplied by the wiInv value.
Shinya Kitaoka 120a6e
    double M   = 0.0;
Shinya Kitaoka 120a6e
    double *vx = V.GetColumnPtr(i);
Shinya Kitaoka 120a6e
    jnx        = Jnorms.GetPtr();
Shinya Kitaoka 120a6e
    for (j = nCols; j > 0; j--) {
Shinya Kitaoka 120a6e
      double accum = 0.0;
Shinya Kitaoka 120a6e
      for (long k = numEndEffectors; k > 0; k--) {
Shinya Kitaoka 120a6e
        accum += *(jnx++);
Shinya Kitaoka 120a6e
      }
Shinya Kitaoka 120a6e
      M += fabs((*(vx++))) * accum;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    M *= fabs(wiInv);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    double gamma = MaxAngleSDLS;
Shinya Kitaoka 120a6e
    if (N < M) {
luzpaz 27707d
      gamma *= N / M;  // Scale back maximum permissible joint angle
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    // Calculate the dTheta from pure pseudoinverse considerations
Shinya Kitaoka 120a6e
    double scale =
Shinya Kitaoka 120a6e
        alpha *
luz paz 6454c4
        wiInv;  // This times i-th column of V is the pseudoinverse response
Shinya Kitaoka 120a6e
    dPreTheta.LoadScaled(V.GetColumnPtr(i), scale);
Shinya Kitaoka 120a6e
    // Now rescale the dTheta values.
Shinya Kitaoka 120a6e
    double max     = dPreTheta.MaxAbs();
Shinya Kitaoka 120a6e
    double rescale = (gamma) / (gamma + max);
Shinya Kitaoka 120a6e
    dTheta.AddScaled(dPreTheta, rescale);
Shinya Kitaoka 120a6e
    /*if ( gamma
Shinya Kitaoka 120a6e
            dTheta.AddScaled( dPreTheta, gamma/max );
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    else {
Shinya Kitaoka 120a6e
            dTheta += dPreTheta;
Shinya Kitaoka 120a6e
    }*/
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Scale back to not exceed maximum angle changes
Shinya Kitaoka 120a6e
  double maxChange = dTheta.MaxAbs();
Shinya Kitaoka 120a6e
  if (maxChange > 100 * MaxAngleSDLS) {
Shinya Kitaoka 120a6e
    dTheta *= MaxAngleSDLS / (MaxAngleSDLS + maxChange);
Shinya Kitaoka 120a6e
    // dTheta *= MaxAngleSDLS/maxChange;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
void Jacobian::CalcdTClampedFromdS() {
Shinya Kitaoka 120a6e
  long len = dS.GetLength();
Shinya Kitaoka 120a6e
  long j   = 0;
Shinya Kitaoka 120a6e
  for (long i = 0; i < len; i += 2, j++) {
Shinya Kitaoka 120a6e
    double normSq = Square(dS[i]) + Square(dS[i + 1]);  //+Square(dS[i+2]);
Shinya Kitaoka 120a6e
    if (normSq > Square(dSclamp[j])) {
Shinya Kitaoka 120a6e
      double factor = dSclamp[j] / sqrt(normSq);
Shinya Kitaoka 120a6e
      dT[i]         = dS[i] * factor;
Shinya Kitaoka 120a6e
      dT[i + 1]     = dS[i + 1] * factor;
Shinya Kitaoka 120a6e
      // dT[i+2] = dS[i+2]*factor;
Shinya Kitaoka 120a6e
    } else {
Shinya Kitaoka 120a6e
      dT[i]     = dS[i];
Shinya Kitaoka 120a6e
      dT[i + 1] = dS[i + 1];
Shinya Kitaoka 120a6e
      // dT[i+2] = dS[i+2];
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
void Jacobian::UpdatedSClampValue() {
Shinya Kitaoka 120a6e
  // Traverse skeleton to find all end effectors
Shinya Kitaoka 120a6e
  TPointD temp;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  int numNode = skeleton->getNodeCount();
Shinya Kitaoka 120a6e
  for (int i = 0; i < numNode; i++) {
Shinya Kitaoka 120a6e
    IKNode *n = skeleton->getNode(i);
Shinya Kitaoka 120a6e
    if (n->IsEffector()) {
Shinya Kitaoka 120a6e
      int i                    = n->getEffectorNum();
Shinya Kitaoka 120a6e
      const TPointD &targetPos = target[i];
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
      // Compute the delta S value (differences from end effectors to target
Shinya Kitaoka 120a6e
      // positions.
Shinya Kitaoka 120a6e
      // While we are at it, also update the clamping values in dSclamp;
Shinya Kitaoka 120a6e
      temp = targetPos;
Shinya Kitaoka 120a6e
      temp -= n->GetS();
Shinya Kitaoka 120a6e
      double normSi      = sqrt(Square(dS[i]) + Square(dS[i + 1]));
Shinya Kitaoka 120a6e
      double norma       = sqrt(temp.x * temp.x + temp.y * temp.y);
Shinya Kitaoka 120a6e
      double changedDist = norma - normSi;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
      if (changedDist > 0.0) {
Shinya Kitaoka 120a6e
        dSclamp[i] = BaseMaxTargetDist + changedDist;
Shinya Kitaoka 120a6e
      } else {
Shinya Kitaoka 120a6e
        dSclamp[i] = BaseMaxTargetDist;
Shinya Kitaoka 120a6e
      }
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}