EuclideanTransf3D.cpp

#include "EuclideanTransf3D.hh"
#include <Base/Common/CompareFloatingPoint.hh>

using namespace BIAS;
using namespace std;

EuclideanTransf3D::
EuclideanTransf3D() : Matrix4x4<double>()
{}


EuclideanTransf3D::
~EuclideanTransf3D()
{}

void EuclideanTransf3D::
Set(const Matrix4x4<double>& m)
{
  bool is_transform = Equal(m[3][0], 0.0);
  is_transform = is_transform && Equal(m[3][1], 0.0);
  is_transform = is_transform && Equal(m[3][2], 0.0);
  is_transform = is_transform && Equal(m[3][3], 1.0);
  BIASASSERT(is_transform);
  for (int i=0; i<4; i++){
    for (int j=0; j<4; j++){
      (*this)[i][j] = m[i][j];
    }
  }
}

void EuclideanTransf3D::
Set(const RMatrixBase& R, const Vector3<double>& t)
{
  double *d=GetData();
  const double *r = R.GetData();
  const double *td = t.GetData();
  d[0]=r[0]; d[1]=r[1]; d[2]=r[2]; d[3]=td[0];
  d[4]=r[3]; d[5]=r[4]; d[6]=r[5]; d[7]=td[1];
  d[8]=r[6]; d[9]=r[7]; d[10]=r[8]; d[11]=td[2];
  d[12]=d[13]=d[14]=0.0; d[15]=1.0;
}

void EuclideanTransf3D::
SetXYZ(const Vector3<double>& EulerAng, const Vector3<double>& t)
{
  RMatrixBase R;
  R.SetXYZ(EulerAng[0], EulerAng[1], EulerAng[2]);
  Set(R, t);
}

void EuclideanTransf3D::
GetR(RMatrixBase& R) const
{
  for (int i=0; i<3; i++){
    for (int j=0; j<3; j++){
      R[i][j] = (*this)[i][j];
    }
  }
}


void EuclideanTransf3D::
GetT(Vector3<double>& T) const
{
  for (int i=0; i<3; i++){
    T[i] = (*this)[i][3];
  }
}

void EuclideanTransf3D::
GetAnglesXYZ(Vector3<double>& angles) const
{
  RMatrixBase R;
  GetR(R);
  R.GetRotationAnglesXYZ(angles);
}

void EuclideanTransf3D::
Dump(std::ostream& os) const
{
  Vector3<double> ang, t;
  GetT(t);
  GetAnglesXYZ(ang);
  os << "angles: "<<ang<<"\tt: "<<t<<endl; 
}