/***************************************************************
 * PrMatrixTest.cpp
 *
 * This is a test program for matrix classes.
 *
 ****************************************************************/

/*
 * modification history
 *----------------------
 *
 * 11/07/97: K.C. Chang: created.
 */

#include <stdio.h>
#include "PrVector3.h"
#include "PrVector6.h"
#include "PrVector.h"
#include "PrMatrix3.h"
#include "PrMatrix6.h"
#include "PrMatrix.h"
#include "PrQuaternion.h"
#include "PrTransform.h"

int main()
{
  int i,j;
  PrVector3 v3;
  PrVector3 v32(1.0,2.0,3.0);
  PrVector3 v31=v32+v32;
  v31.display("v31");
    
  v3.display("v3 before");
  printf("\n");
  v3 = v32;
  v3.display("v3 after");
  printf("\n");

  PrVector6 v6;
  PrVector6 v62(v31,v32);
  PrVector6 v61=v62+v62;
  v61.display("v61");


  PrMatrix3 m3;
  m3[0][0]=1;    m3[0][1]=0;    m3[0][2]=1;
  m3[1][0]=0;    m3[1][1]=4;    m3[1][2]=0;
  m3[2][0]=1;    m3[2][1]=0;    m3[2][2]=10;
  PrMatrix3 m3inv;
  //    m3.inverse(m3inv);
  m3.inverseSPD(m3inv);
  (m3*m3inv).display("M*Minv");
  (m3inv*m3).display("Minv*M");
  PrVector3 y3(1.0,1.0,1.0);
  PrVector3 x3;
  //    m3.solve(y3,x3);
  m3.solveSPD(y3,x3);
  x3.display("x3");
  printf("\n");
  (m3*x3).display("m3x");
  printf("\n");
  (m3inv*y3).display("x3inv");
  printf("\n");
  (m3*(m3inv*y3)).display("m3x3inv");
  printf("\n");

  PrMatrix6 m6;
  for(i=0;i<6;i++)
    for(j=0;j<6;j++)
      m6.elementAt(i,i) = 1.0;
  m6[0]=m3;
  m6[1].zero();
  m6[2].zero();
  PrMatrix6 m6inv;
  //    m6.inverse(m6inv);
  m6.inverseSPD(m6inv);
  (m6*m6inv).display("M*Minv");
  (m6inv*m6).display("Minv*M");
  PrVector6 y6(1.0,1.0,1.0,1.0,1.0,1.0);
  PrVector6 x6;
  //    m6.solve(y6,x6);
  m6.solveSPD(y6,x6);
  x6.display("x6");
  (m6*x6).display("m6x");
  (m6inv*y6).display("x6inv");
  (m6*(m6inv*y6)).display("m6x6inv");


  PrMatrix m(6,6);
  m.zero();
  for(i=0;i<6;i++)
    m.elementAt(i,i) = 1.0;
  for(i=0;i<3;i++)
    for(j=0;j<3;j++)
      m.elementAt(i,j) = m3.elementAt(i,j);

  PrMatrix minv(6,6);
  m.inverse(minv);
  //m.inverseSPD(minv);
  (m*minv).display("M*Minv");
  (minv*m).display("Minv*M");
  PrVector y(6);
  for(i=0;i<6;i++)
    y.elementAt(i) = 1.0;
  PrVector x(6);
  m.solve(y,x);
  //m.solveSPD(y,x);
  x.display("x");
  (m*x).display("mx");
  (minv*y).display("xinv");
  (m*(minv*y)).display("mxinv");




  PrQuaternion q;
  PrTransform t;
  //  PrMatrix m(5,6);
  PrVector v(10);


  v3=v31+v32;
  v3.display();

  PrVector3 dPhi;
  PrQuaternion q1(PrVector3(0,1,0),M_PI_4);
  PrQuaternion q2(PrVector3(1,0,0),M_PI_4);
  q1.angularError(q2,dPhi);
  dPhi.display("dPhi(q)");
  printf("\n");

  PrMatrix3 xr,xrd;
  q1.matrix(xr);
  q2.matrix(xrd);
  xr.angularError(xrd,dPhi);
  dPhi.display("dPhi(R)");
  printf("\n");

  PrVector6 e;
  PrTransform T(q1,PrVector3(0,0,0));
  PrTransform Td(q2,PrVector3(1,2,3));
  T.error(Td,e);
  e.display("e");

  return (0);
}
