// coordinate.cpp: implementation of the coordinate class.
//
//////////////////////////////////////////////////////////////////////


#include "coordinate.h"
#include <cmath>

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

coordinate::coordinate()
{
  x=y=z=0.0f;

}

coordinate::coordinate(float cx,float cy,float cz)
{
  x=cx;
  y=cy;
  z=cz;
}

coordinate::coordinate(const IMArray<double>& m)
{
  x=m[0];
  y=m[1];
  z=m[2];
}

coordinate::coordinate(const IMArray<float>& m)
{
  x=m[0];
  y=m[1];
  z=m[2];
}
coordinate::~coordinate()
{


}

//vectorial product
coordinate coordinate::operator ^(coordinate c)
{	

	coordinate v;
	v.x=this->y*c.z-this->z*c.y;
	v.y=this->z*c.x-this->x*c.z;
	v.z=this->x*c.y-this->y*c.x;

	return v;
}


float coordinate::operator *(coordinate c)
{	

	float f;
	f=this->x*c.x+this->y*c.y+this->z*c.z;

	return f;
}

void coordinate::operator-=(coordinate c)
{
  this->x-=c.x;
  this->y-=c.y;
  this->z-=c.z;
}

void coordinate::operator+=(float f)
{
  this->x+=f;
  this->y+=f;
  this->z+=f;
}

void coordinate::operator+=(coordinate c)
{
  this->x+=c.x;
  this->y+=c.y;
  this->z+=c.z;
}


void coordinate::operator*=(float f)
{
  this->x*=f;
  this->y*=f;
  this->z*=f;
}

coordinate coordinate::operator*(float f) const
{
  coordinate a=(*this);
  a*=f;
  return a;
}

void coordinate::operator/=(float f)
{
  this->x/=f;
  this->y/=f;
  this->z/=f;
}

coordinate coordinate::operator/(float f)
{
  coordinate a=(*this);
  a/=f;
  return a;
}
coordinate coordinate::operator-(coordinate c)
{
  coordinate a=*this;
  a-=c;
  return a;

}

coordinate coordinate::operator+(coordinate c) const
{
  coordinate a=*this;
  a+=c;
  return a;

}
float coordinate::distance(coordinate a, coordinate b)
{
  float a1=a.x-b.x;
  float a2=a.y-b.y;
  float a3=a.z-b.z;
  
  float k= sqrt(a1*a1+a2*a2+a3*a3);
  return k;

}/** No descriptions */
float coordinate::GetAngleVector(coordinate a, coordinate b){
  float proj=a*b/(a.norm()*b.norm()) ;
  if (proj < -1 ) proj=-1;
  if (proj > 1 ) proj=1;  //avoid NaN on acos ( [-1,1] interval
  return acos(proj);

}
/** No descriptions */
float coordinate::norm() const{
  return sqrt(x*x+y*y+z*z);
}

float coordinate::dihedral(coordinate a, coordinate b, coordinate c, coordinate d)
{
    coordinate vec1,vec2,vec3,v1,v2;
 
    
  vec1=a-b;
  vec2=d-c;
  vec3=c-b;

  /*vec3.x=1.0;
  vec3.y=0.0;
  vec3.z=0.0;
  vec2.x=0.0;
  vec2.y=1.0;
  vec2.z=0.0;*/
  //v1=vec3**vec2
  v1=vec3^vec2;

  //v2=vec1*vec3
  v2=vec3^vec1;
  
  float ang=GetAngleVector(v1,v2);
  

  //OK lets determine the sign
  coordinate dp=vec1^vec2;
  float proj=dp*vec3;
  if(proj<0)
    ang*=-1.0f;
  return ang;
}


coordinate coordinate::GetMiddle(coordinate a, coordinate b)
{
  a+=b;
  a*=0.5f;
  return a;
}

//Rotate using quaternions
coordinate coordinate::RotAroundAxis(coordinate a, coordinate b, coordinate c, float theta)
{
  coordinate axis=b-a;
  float kk=(sin(theta/2)/axis.norm());
  coordinate v1=axis*kk;
  
  quaternion q(cos(theta/2),v1);
  quaternion q1=q.invert();
  
  quaternion q2(0,c-a);
  
  quaternion q3=q^q2^q1;
  
  return a+q3.v;
  
  
}
