/***************************************************************************
 *   Copyright (C) 2005 by Raphael Langerhorst                             *
 *   raphael-langerhorst@gmx.at                                            *
 *                                                                         *
 *   Permission is hereby granted, free of charge, to any person obtaining *
 *   a copy of this software and associated documentation files (the       *
 *   "Software"), to deal in the Software without restriction, including   *
 *   without limitation the rights to use, copy, modify, merge, publish,   *
 *   distribute, sublicense, and/or sell copies of the Software, and to    *
 *   permit persons to whom the Software is furnished to do so, subject to *
 *   the following conditions:                                             *
 *                                                                         *
 *   The above copyright notice and this permission notice shall be        *
 *   included in all copies or substantial portions of the Software.       *
 *                                                                         *
 *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       *
 *   EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    *
 *   MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*
 *   IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR     *
 *   OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, *
 *   ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR *
 *   OTHER DEALINGS IN THE SOFTWARE.                                       *
 ***************************************************************************/

#include "GVector3.h"

#include <qstring.h>

namespace GCS
{

GVector3::GVector3() : 
    x(0),
    y(0),
    z(0)
{
}

GVector3::GVector3(const double d) :
    x(d),
    y(d),
    z(d)
{
}

GVector3::GVector3(const double x, const double y, const double z) : 
    x(x),
    y(y),
    z(z)
{
}

GVector3::GVector3(const double* component) : 
    x(component[0]), 
    y(component[1]), 
    z(component[2])
{
}

GVector3::GVector3(const GVector3& original) :
    x(original.x),
    y(original.y),
    z(original.z)
{
}

double GVector3::length() const
{
  return std::sqrt(x*x + y*y + z*z);
}

double GVector3::lengthsq() const
{
  return x*x + y*y + z*z;
}

GVector3& GVector3::reset()
{
  x=y=z=0;
  return *this;
}

GVector3& GVector3::set(double x, double y, double z)
{
  this->x = x;
  this->y = y;
  this->z = z;
  return *this;
}

GVector3& GVector3::set(const GVector3& original)
{
  return set(original.x, original.y, original.z);
}

GVector3& GVector3::normalize()
{
  double l = length();
  x /= l;
  y /= l;
  z /= l;
  return *this;
}

GVector3& GVector3::scaleXYZ(double x, double y, double z)
{
  this->x *= x;
  this->y *= y;
  this->z *= z;
  return *this;
}

GVector3& GVector3::scaleXYZ(const GVector3& scale)
{
  return scaleXYZ(scale.x,scale.y,scale.z);
}

GVector3& GVector3::add(const GVector3& right)
{
  x += right.x;
  y += right.y;
  z += right.z;
  return *this;
}

GVector3& GVector3::sub(const GVector3& right)
{
  x -= right.x;
  y -= right.y;
  z -= right.z;
  return *this;
}

GVector3& GVector3::mul(double scalar)
{
  x *= scalar;
  y *= scalar;
  z *= scalar;
  return *this;
}

double GVector3::dot(const GVector3& right) const
{
  return x*right.x +
          y*right.y +
          z*right.z;
}

GVector3 GVector3::cross(const GVector3& right) const
{
  return GVector3(
    y*right.z - z*right.y,
    z*right.x - x*right.z,
    x*right.y - y*right.x);
}

GVector3& GVector3::operator = (const GVector3& original)
{
  return set(original);
}

GVector3 GVector3::operator + (const GVector3& right) const
{
  return GVector3(x + right.x,
                   y + right.y,
                   z + right.z);
}

GVector3 GVector3::operator - (const GVector3& right) const
{
  return GVector3(x - right.x,
                   y - right.y,
                   z - right.z);
}

GVector3 GVector3::operator * (double factor) const
{
  GVector3 result(*this);
  return result.mul(factor);
}

GVector3& GVector3::operator += (const GVector3& right)
{
  x += right.x;
  y += right.y;
  z += right.z;
  return *this;
}

GVector3& GVector3::operator -= (const GVector3& right)
{
  x -= right.x;
  y -= right.y;
  z -= right.z;
  return *this;
}

bool GVector3::operator == (const GVector3& comp) const
{
  double dx,dy,dz;
  dx = comp.x - x;
  dy = comp.y - y;
  dz = comp.z - z;
  
  if (dx<0)
    dx = -dx;
  if (dy<0)
    dy = -dy;
  if (dz<0)
    dz = -dz;
    
  double tolerance = 0.00001;  //IMPORTANT, otherwise you will NEVER get two equal double values
  
  return ( dx < tolerance && dy < tolerance && dz < tolerance );
}

double GVector3::distanceTo(const GVector3& p) const
{
  GVector3 temp = p - *this;
  return temp.length();
}

double GVector3::angleTo(const GVector3& v) const
{
  if (this->length()==0 || v.length() == 0)
    return 0;
  return std::acos(this->dot(v)/(this->length()*v.length()));
}

GVector3& GVector3::turnAroundAxis(const GVector3& axis, double angle_rad)
{
  double x,y,z;
  double sin_a = std::sin(angle_rad);
  double cos_a = std::cos(angle_rad);
  
  x = ( axis.x * axis.x + cos_a * ( 1- axis.x * axis.x ) ) * this->x
  + ( axis.x * axis.y * ( 1 - cos_a ) - axis.z * sin_a ) * this->y
  + ( axis.x * axis.z * ( 1 - cos_a ) + axis.y * sin_a ) * this->z;
    
  y = ( axis.x * axis.y * ( 1 - cos_a ) + axis.z * sin_a ) * this->x
  + ( axis.y * axis.y + cos_a * ( 1- axis.y * axis.y ) ) * this->y
  + ( axis.y * axis.z * ( 1 - cos_a ) - axis.x * sin_a ) * this->z;
  
  z = ( axis.z * axis.x * ( 1 - cos_a ) - axis.y * sin_a ) * this->x
  + ( axis.y * axis.z * ( 1 - cos_a ) + axis.x * sin_a ) * this->y
  + ( axis.z * axis.z + cos_a * ( 1 - axis.z * axis.z ) ) * this->z;
  
  this->x = x;
  this->y = y;
  this->z = z;
  
  return *this;
}

GVector3& GVector3::projectTo(const GVector3& v)
{
  double v_length = v.length();
  if (v_length==0)
    return *this;
  GVector3 temp = v;
  *this = temp.mul( v.dot(*this)/(v_length*v_length) );
  return *this;
}

QString GVector3::toString()
{
  return QString("%1, %2, %3").arg(QString::number(x),QString::number(y),QString::number(z));
}

}