Ellipse.cpp

#include <Geometry/Ellipse.hh>

#include <Base/Math/Vector2.hh>
#include <iostream>
#include <limits>
#include <fstream>


using namespace BIAS;
using namespace std;

Ellipse::Ellipse()
{}


void Ellipse::
    SetParametric(const double scaleX, const double scaleY,
                  const double rotAngleDeg,
                  const double centerX, const double centerY)
{
  //parametricTransform_.SetZero();
//  parametricTransform_[0][0] = scaleX;
//  parametricTransform_[1][1] = scaleY;
  Matrix2x2<double> R;
  double rotAngleRAD = rotAngleDeg*M_PI/180.0;
  parametricTransform_[0][0] =
      parametricTransform_[1][1] = cos(rotAngleRAD);
  parametricTransform_[0][0] *= scaleX;
  parametricTransform_[1][1] *= scaleY;
  parametricTransform_[0][1] = -sin(rotAngleRAD)*scaleY;
  parametricTransform_[1][0] = sin(rotAngleRAD)*scaleX;
  cout<<parametricTransform_<<endl;

  centerX_ = centerX;
  centerY_ = centerY;

  CalcImplicitFromParameteric_();

}

void Ellipse::
    CalculateAxisAlignedBoundingBox(double &minX, double &minY,
                                    double &maxX, double &maxY) const
{
  //use parametric form
  // which is parametricTransform_ = M^T = (Ma Mc)
  //                                       (Mb Md)
  const double Ma = parametricTransform_[0][0];
  const double Mb = parametricTransform_[1][0];
  const double Mc = parametricTransform_[0][1];
  const double Md = parametricTransform_[1][1];

  double tx[2];
  double ty[2];
  tx[0]= atan(Mc / Ma);
  ty[0] = atan(Md / Mb);
  tx[1] = tx[0]+M_PI;
  ty[1] = ty[0]+M_PI;

//  double xExt[2];
//  double yExt[2];
  minX = minY = numeric_limits<double>::max();
  maxX = maxY = -minX;
//  minY = DBL_MAX;
//  maxY = -DBL_MAX;

  for(unsigned int i=0; i<2; i++){
    double x = Ma*cos(tx[i])+Mc*sin(tx[i]);
    double y = Mb*cos(ty[i])+Md*sin(ty[i]);

    if(minX>x) minX=x;
    if(minY>y) minY=y;
    if(x>maxX) maxX=x;
    if(y>maxY) maxY=y;

  }
  minX += centerX_;
  maxX += centerX_;
  minY += centerY_;
  maxY += centerY_;

//  minX = Ma;
//  minY = Mb;
//  maxX = Mc;
//  maxY = Md;



}

void Ellipse::
    CalcImplicitFromParameteric_()
{
  Matrix2x2<double> invParametricTransform = parametricTransform_.Invert();

  implicitForm_ = invParametricTransform.Transpose()*invParametricTransform;

}


void Ellipse::
    SetImplicit(const BIAS::Matrix2x2<double>& implicitForm,
                const double centerX, const double centerY)
{
  implicitForm_ = implicitForm;

  CalcParametericFromImplicit_();

  centerX_ = centerX;
  centerY_ = centerY;

}

void Ellipse::
    SetGaussianFilterRange(const double& sigmaX, const double& sigmaY,
                           const double F)
{
  Matrix2x2<double> Sigma;
  Sigma.SetZero();
  Sigma[0][0] = 1.0/(sigmaX*sigmaX);
  Sigma[1][1] = 1.0/(sigmaY*sigmaY);
  SetImplicit(Sigma, F, 0 , 0);
}

void Ellipse::
    SetImplicit(const BIAS::Matrix2x2<double>& implicitForm,
                 const double F,
                 const double centerX, const double centerY)
{
  implicitForm_ = implicitForm;
//  cout<<"im form = "<<implicitForm_<<endl;
  implicitForm_ /= F;
//  cout<<"im form scaled = "<<implicitForm_<<endl;

  CalcParametericFromImplicit_();

  centerX_ = centerX;
  centerY_ = centerY;

}

#define MYSGN(x) ((x<0) ? -1.0 : 1.0)
void Ellipse::
    CalcParametericFromImplicit_()
{
  //assumming implicitForm_ = Q = ( A  B/2)
  //                              ( B/2 C )
  double B = implicitForm_[0][1] * 2;
 // cout<<"B = "<<B<<endl;
  double p = implicitForm_[0][0] - implicitForm_[1][1];
//  cout<<"p = "<<p<<endl;

  double t = MYSGN(p)*sqrt(p*p + B*B);
//  cout<<"t = "<<t<<endl;

  if(t != 0 ) {

    double q = implicitForm_[0][0] + implicitForm_[1][1];
//    cout<<"q = "<<q<<endl;
    parametricTransform_[0][0] = sqrt( (t+p)/(t*(q+t)) );
    parametricTransform_[0][1] = MYSGN(B*p)*sqrt( (t-p)/(t*(q+t)) );
    parametricTransform_[1][0] = -MYSGN(B*p)*sqrt( (t-p)/(t*(q-t)) );
    parametricTransform_[1][1] = sqrt( (t+p)/(t*(q-t)) );
    parametricTransform_.TransposeIP();
  } else {
    parametricTransform_.SetZero();
    parametricTransform_[0][0] = parametricTransform_[1][1] =
                                 1.0/sqrt(implicitForm_[0][0]);
  }
//  cout<<"calc pt = "<<parametricTransform_<<endl;
}

BIAS::Matrix2x2<double> Ellipse::
    ReturnParametricTransformation() const
{
  return parametricTransform_;
}

BIAS::Matrix2x2<double> Ellipse::
    ReturnImplicitRepresentation() const
{
  return implicitForm_;
}