#include <d2homogeneous.h>

void d2homogeneous::matrixMultiply( point2<double> & p ) const
{
  point2<double> p0(p);
  p.x = matrix[0]*p0.x + matrix[1]*p0.y + matrix[2];
  p.y = matrix[3]*p0.x + matrix[4]*p0.y + matrix[5];
}

void d2homogeneous::matrixMultiply
( 
  point2<double> & p2,
  point2<double> const & p
) const
{
  p2.x = matrix[0]*p.x + matrix[1]*p.y + matrix[2];
  p2.y = matrix[3]*p.x + matrix[4]*p.y + matrix[5];
}

void d2homogeneous::matrixMultiply
( 
  point3<double> & p2,
  point3<double> const & p
) const 
{
  point3<double> r;
  for (uint i=0; i<3; ++i)
  {
    rowk(r,i);
    p2[i] = r.dot(p);
  }
}


d2homogeneous::d2homogeneous()
{
  setIdentity();
}


d2homogeneous::d2homogeneous(doublec * matrix_)
{
  for (uint i=0; i<9; ++i)
    matrix[i] = matrix_[i];
}

void d2homogeneous::setZero()
{
  for (uint i=0; i<8; ++i)
    matrix[i] = 0.0;
  matrix[8] = 1.0;
}

void d2homogeneous::setTranslate(point2<double> const & p)
{
  setIdentity();
  matrix[2] = p.x;
  matrix[5] = p.y;
}

void d2homogeneous::setRotate(doublec theta)
{
  setZero();

  doublec cos_t = cos(theta);
  doublec sin_t = sin(theta);
  matrix[0] = cos_t;
  matrix[1] = -sin_t;
  matrix[3] = sin_t;
  matrix[4] = cos_t;
}

void d2homogeneous::setIdentity()
{
  matrix[0] = 1.0;  matrix[1] = 0.0;  matrix[2] = 0.0;
  matrix[3] = 0.0;  matrix[4] = 1.0;  matrix[5] = 0.0;
  matrix[6] = 0.0;  matrix[7] = 0.0;  matrix[8] = 1.0;
}




d2homogeneous & d2homogeneous::operator *= (d2homogeneous const & w)
{
  uint i;
  uint k;
  point3<double> r;
  point3<double> c;
  for (i=0; i<3; ++i)
  {
    rowk(r,i);
    for (k=0; k<3; ++k)
    {
      w.columnk(c,k);
      matrix[i*3+k] = r.dot(c);
    }
  }

  return *this;
}

void d2homogeneous::setRotateAboutPoint
(
  doublec theta, 
  point2<double> const & p
)
{
  setTranslate(p);

  d2homogeneous r;
  r.setRotate(theta);
  d2homogeneous pInv;
  pInv.setTranslate(p*-1.0);

  (*this)*r*pInv;
}

d2homogeneous & operator * 
(
  d2homogeneous & a, 
  d2homogeneous const & b
)
{
  return a *= b;
}

d2homogeneous::operator string() const
{
  string s;

  for (uint i=0; i<2; ++i)
  {
    stringstream ss; 
    ss << matrix[i*3] << " ";
    ss << matrix[i*3+1] << " ";
    ss << matrix[i*3+2] << " \n";

    s+=ss.str();
  }

  {
    stringstream ss;
    ss << matrix[6] << " " << matrix[7] << " " << matrix[8];
    s+=ss.str();
  }

  return s;
}