#include <d2homogeneoustest.h>

#include <print.h>

#include <d2homogeneous.h>

void d2homogeneoustest01()
{
  cout << "Testing d2homogeneous class." << endl << endl;

  doublec pi=3.141592654;

  doublec degtorad = pi/180.0;


  cout << "Testing rotation with firstly a homogenouse point";
  cout << "  and then a normal point." << endl;

  d2homogeneous R;
  R.setRotate(90.0*degtorad);

  point3<double> p0(1.0,0.0,1.0);  // (1.0,0.0)
  point3<double> p(p0);
  cout << "before rotation  " << SHOW(p) << endl;
  R.matrixMultiply(p,p0);
  cout << "after rotation   " << SHOW(p) << endl;

  point2<double> q0(p0.x,p0.y);
  point2<double> q(q0);

  cout << "before rotation  " << SHOW(q) << endl;
  R.matrixMultiply(q,q0);
  cout << "after rotation   " << SHOW(q) << endl;
  cout << endl;

  cout << "Testing translation by now shifting the point" << endl;
  cout << SHOW(q) << endl;
  point2<double> dq(-0.5,-2.0);
  cout << SHOW(dq) << endl;
  cout << "q = q + dq" << endl;
  R.setTranslate(dq);
  cout << "R=" << endl;
  cout << (string)R << endl;
  
//  point2<double> t(q);
//  cout << SHOW(t) << endl;
  R.matrixMultiply(q);
  cout << SHOW(q) << endl << endl;

  cout << "Now both rotate the point and translate the " << endl;
  cout << "  point as a combined transformation" << endl;
  
  R.setRotate(90.0*degtorad);
  d2homogeneous T1;
  T1.setTranslate(dq);
  T1*R;
  q=q0;
  cout << SHOW(q) << endl;
  T1.matrixMultiply(q);
  cout << SHOW(q) << endl << endl;

  cout << "Testing rotating about a point.  Consider a unit" << endl;
  cout << "  circle with center at (2,2).  Rotating the top" << endl;
  cout << "  point (2,3) by 90 degrees about (2,2) should give " << endl;
  cout << "  { (1,2), (2,1), (3,2), (2,3) }" << endl;

  point2<double> center(2.0,2.0);
  cout << SHOW(center) << endl;
  q = point2<double>(2.0,3.0);
  R.setRotateAboutPoint(90.0*degtorad,center);
  cout << SHOW(q) << endl;
  R.matrixMultiply(q);  
  cout << SHOW(q) << endl;
  R.matrixMultiply(q);  
  cout << SHOW(q) << endl;
  R.matrixMultiply(q);  
  cout << SHOW(q) << endl;
  R.matrixMultiply(q);  
  cout << SHOW(q) << endl;
}