/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

/* 2x2 matrix class */

#ifndef __MAT2D_H__
#define __MAT2D_H__

#include <math.h>
#include <stdio.h>

template <class num>
class mat2d {
public:
  num mat[4];
  
  mat2d() {mat[0] = mat[1] = mat[2] = mat[3] = 0;}
  mat2d(num ul, num ur, num ll, num lr) 
    {mat[0] = ul; mat[1] = ur; mat[2] = ll; mat[3] = lr;}
  mat2d(double theta)
    {num s = sin(theta); mat[0] = mat[3] = (num)cos(theta); 
    mat[1] = s; mat[2] = -s;}


  void print();
  /* We modify whatever we call the method on */
  void rot(double theta);
  void mul(mat2d<num> &b);
  void trans();
  void inv();
  void add(mat2d<num> &b);
  void sub(mat2d<num> &b);
  void copy(mat2d<num> &m);
};

template <class num>
inline void mat2d<num>::print() { /* Note: Requires double! */
  printf("[%f %f]\n"
	 "[%f %f]\n",mat[0], mat[1], mat[2], mat[3]);
}

template <class num>
inline void mat2d<num>::rot(double theta) {
  num s = sin(theta);
  mat[0] = mat[3] = (num)cos(theta);
  mat[1] = -s; mat[2] = s;
}

template <class num>
inline void mat2d<num>::mul(mat2d<num> &b) {
  num sav = mat[0];
  mat[0] = mat[0]*b.mat[0]+mat[1]*b.mat[2];
  mat[1] = sav*b.mat[1]+mat[1]*b.mat[3];
  sav = mat[2];
  mat[2] = mat[2]*b.mat[0]+mat[3]*b.mat[2];
  mat[3] = sav*b.mat[1]+mat[3]*b.mat[3];
}

template <class num>
inline void mat2d<num>::trans() { /* Transpose */
  num sav = mat[1];
  mat[1] = mat[2];
  mat[2] = sav;
}

template <class num>
inline void mat2d<num>::inv() { /* Invert: assumes invertible matrix */
  num sav = mat[0];
  num div = (mat[0]*mat[3] - mat[1]*mat[2]);
  if (div == ((num)0)) return; /* Silently incorrect */
  div = ((num)1) / div;
  mat[0] = div*mat[3];
  mat[1] = -div*mat[1];
  mat[2] = -div*mat[2];
  mat[3] = div*sav;
}

template <class num>
inline void mat2d<num>::copy(mat2d<num> &m) {
  m.mat[0] = mat[0];
  m.mat[1] = mat[1];
  m.mat[2] = mat[2];
  m.mat[3] = mat[3];
}

template <class num>
inline void mat2d<num>::add(mat2d<num> &b) {
  mat[0] += b.mat[0];
  mat[1] += b.mat[1];
  mat[2] += b.mat[2];
  mat[3] += b.mat[3];
}

template <class num>
inline void mat2d<num>::sub(mat2d<num> &b) {
  mat[0] -= b.mat[0];
  mat[1] -= b.mat[1];
  mat[2] -= b.mat[2];
  mat[3] -= b.mat[3];
}

/* inline void angle_wrap(double &angle) { */
/*   if (angle > M_PI) */
/*     do angle -= (2.0*M_PI); while (angle > M_PI); */
/*   else */
/*     while (angle <= -M_PI) angle += (2.0*M_PI); */
/* } */
/*
inline void angle_wrap(double &angle) {
  angle=fmod(angle,2*M_PI);
  if (angle < 0.0) angle+=2*M_PI;
  else if (angle >= M_PI) angle-=2*M_PI;
}
*/

#endif