// ==========================================================================
// $Id: obbox.cpp,v 1.1 2011/10/19 03:24:40 jlang Exp $
// CSI2372 example Code for lecture 4
// ==========================================================================
// (C)opyright:
//
//   Jochen Lang
//   EECS, University of Ottawa
//   800 King Edward Ave.
//   Ottawa, On., K1N 6N5
//   Canada. 
//   http://www.eecs.uottawa.ca
// 
// Creator: jlang (Jochen Lang)
// Email:   jlang@eecs.uottawa.ca
// ==========================================================================
// $Log: obbox.cpp,v $
// Revision 1.1  2011/10/19 03:24:40  jlang
// Added code for lecture 7
//
//
// ==========================================================================
#include <iostream>
#include <cmath>

#include "obbox.h"


using std::cout;
using std::endl;

 
OBBox::OBBox( Point2D _center, Vector2D _direction, 
	      double _width, double _height) :
  d_center(_center), d_major(_direction),   
  d_minor(_direction.getY(),-_direction.getX()),
  d_width(_width), d_height(_height)
{
  d_major.normalize();
  d_minor.normalize();
}

double OBBox::planeTestMajor( const Point2D& _pt ) const {
  return Vector2D(_pt).subtract(d_center).dot(d_major);
}

double OBBox::planeTestMinor( const Point2D& _pt ) const {
  return Vector2D(_pt).subtract(d_center).dot(d_minor);
}
  
bool OBBox::enclose( Point2D _extrema[], int _size ) {
  // Calculate centroid of points
  double cx=0.0, cy=0.0;
  for (int i=0; i<_size; ++i) {
    cx += _extrema[i].getX()/_size;
    cy += _extrema[i].getY()/_size;
  }
  d_center = Point2D(cx,cy);
  // Calculate distribution of points
  double xx=0.0, xy=0.0, yy=0.0;
  for (int i=0; i<_size; ++i) {
    double x = _extrema[i].getX() - cx;
    double y = _extrema[i].getY() - cy;
    xx += x*x;
    xy += x*y;
    yy += y*y;
  }
  // now find principal axis
  double det = (xx-yy)*(xx-yy) + 4*xy*xy;
  if ( det > 0 ) {
    det = std::sqrt(det);
    double lambda1 = (xx+yy+det)/2.0; 
    double lambda2 = (xx+yy-det)/2.0;
    d_major = Vector2D( -xy, xx-lambda1 ); 
    d_major = d_major.normalize();
    d_minor = Vector2D( xx-lambda1, xy );
    d_minor = d_minor.normalize();
  } else {
    d_width = 0.0;
    d_height = 0.0;
    return false;
  }
  // now we just need to find the actual width and height of the box
  d_width = 0.0, d_height = 0.0;
  for (int i=0; i<_size; ++i) {
    double w = std::abs(planeTestMajor( _extrema[i] ));
    if ( w > d_width ) d_width = w;
    double h = std::abs(planeTestMinor( _extrema[i] ));
    if ( h > d_height ) d_height = h;
  }
  return true;
}

bool OBBox::isInside( const Point2D& _pt ) const {
  if ( std::abs(planeTestMajor(_pt)) <= d_width &&
       std::abs(planeTestMinor(_pt)) <= d_height )
    return true;
  else
    return false;
}

void OBBox::print() const {
  cout << "Box in orientation: ";
  d_major.print(); 
  cout << " centered at ";
  d_center.print(); 
  cout << " with (wxh): "  << d_width << " x " << d_height;
  return;
}