PlueckerMatrix.cpp

#include <Base/Geometry/PlueckerMatrix.hh>

#include <Base/Debug/Exception.hh>
#include <Base/Common/CompareFloatingPoint.hh>

using namespace BIAS;
using namespace std;

PlueckerMatrix::
PlueckerMatrix(const Matrix4x4<PLUECKERMATRIX_TYPE>& m)
  : Matrix4x4<PLUECKERMATRIX_TYPE>(m)
{
  // enforce the rank two constraint
  if (!CheckConstraint_()){
    BEXCEPTION("not a Pluecker matrix: "<<*this);
  }
}


bool PlueckerMatrix::
CheckConstraint_() const
{
  Vector<PLUECKERMATRIX_TYPE> l = GetPlueckerLineCoordinates();
  return Equal(l[0]*l[5] + l[1]*l[4] + l[2]*l[3], 0.0);
}


void PlueckerMatrix::
Set(const HomgPoint3D& p1, const HomgPoint3D& p2)
{
  *this = p1.OuterProduct(p2);
  this->SubIP(p2.OuterProduct(p1));
}


Vector<PLUECKERMATRIX_TYPE> PlueckerMatrix::
GetPlueckerLineCoordinates() const
{
  Vector<PLUECKERMATRIX_TYPE> res(6);
  res[0] = (*this)[0][1];
  res[1] = (*this)[0][2];
  res[2] = (*this)[0][3];
  res[3] = (*this)[1][2];
  res[4] = (*this)[3][1];
  res[5] = (*this)[2][3];
  return res;
}


HomgPoint3D PlueckerMatrix::
GetDirection() const
{ 
  return HomgPoint3D((*this)[0][3], (*this)[1][3], (*this)[2][3], 1.0);
}


HomgPoint3D PlueckerMatrix::
GetNormal() const
{ 
  return HomgPoint3D((*this)[0][1], (*this)[0][2], (*this)[1][2], 1.0);
}





/*

void PlueckerMatrix::
CalculateFootpoint(HomgPoint3D& p, HomgPoint3D& footp)
{
  if (p.GetW() != 0.0){
    BEXCEPTION("cannot calculate footpoint to points at infinity "<<p);
  }
  
}

PLUECKERMATRIX_TYPE PlueckerMatrix::
Distance(HomgPoint3D& p)
{
  HomgPoint3D footp;
  CalculateFootpoint(p, footp);
  footp -= p;
  return footp.NormL2();
}


*/