////////////////////////////////////////////////////////////////////////////////
// Point3D implementation
// Copyright Björn Ganster 
// Developed since 2000
////////////////////////////////////////////////////////////////////////////////

#include "math.h"
#include "BGBase.h"
#include "Point3D.h"
#include "intersections.h"

#include <assert.h>

#include <algorithm>

// SET_SCOPE, SET_OP and RETURN not used here for performance reasons ...
//#define FILE_NUMBER FILE_NO_POINT3D_CPP

////////////////////////////////////////////////////////////////////////////////
// Default constructor

Point3D::Point3D(double _x, double _y, double _z, double _w)
: //VirtualClass("Point3D", Point3DID),
  x(_x),
  y(_y),
  z(_z),
  w(_w)
{
}

////////////////////////////////////////////////////////////////////////////////
// Copy constructor

Point3D::Point3D(const Point3D& a)
: //VirtualClass("Point3D", Point3DID),
  x(a.x),
  y(a.y),
  z(a.z),
  w(a.w)
{
}

////////////////////////////////////////////////////////////////////////////////
// Destructor

Point3D::~Point3D()
{
}

////////////////////////////////////////////////////////////////////////////////
// Calculate the angle between ab, bc

double Point3D::angle(const Point3D& a, const Point3D& b, const Point3D& c)
{
	return angle (a-b, c-b);
}

////////////////////////////////////////////////////////////////////////////////
// Calculate the angle between two vectors (using the origin as a third point)

double Point3D::angle(const Point3D& a, const Point3D& b)
{
	double alen = a.length();
	double blen = b.length();
	if (alen != 0.0 && blen != 0.0) {
		double fraction = (a * b) / (alen * blen);
		return 180.0 * acos(putInRange (-1.0, fraction, 1.0)) / M_PI;
	} else {
		return 0.0;
	}
}

////////////////////////////////////////////////////////////////////////////////
// Find two vectors that are pependicular to a given vector

bool Point3D::findPerps(Point3D& b, Point3D& c) const
{
	double len = length();
	if (len == 0) {
		return false;
	}

	Point3D a1 = (*this);
	a1 = a1/len;
	b = Point3D(1, 0, 0);
	c = Point3D(0, 1, 0);

	// Choose the vector with the bigger absolute difference to a
	double abdist = std::min(Point3D(a1-b).length(), Point3D(a1+b).length());
	double acdist = std::min(Point3D(a1-c).length(), Point3D(a1+c).length());

	if (abdist > acdist) {
		// Base c on cross product of a, b
		c = a1.crossMultiply(b);
		c.unitLength();
		b = a1.crossMultiply(c);
	} else {
		// Base c on cross product of a, b
		b = a1.crossMultiply(c);
		b.unitLength();
		c = a1.crossMultiply(b);
	}

	return true;
}

////////////////////////////////////////////////////////////////////////////////
// Point comparison

Point3D MinCoords(const Point3D& a, const Point3D& b)
{
	Point3D minRes (a);
	if (a.x > b.x)
		minRes.x = b.x;
	if (a.y > b.y)
		minRes.y = b.y;
	if (a.z > b.z)
		minRes.z = b.z;
	return minRes;
}

Point3D MaxCoords(const Point3D& a, const Point3D& b)
{
	Point3D maxRes (a);
	if (a.x < b.x)
		maxRes.x = b.x;
	if (a.y < b.y)
		maxRes.y = b.y;
	if (a.z < b.z)
		maxRes.z = b.z;
	return maxRes;
}

double dist3D(const Point3D& a, const Point3D& b)
{
	if (a.w == 1.0 && b.w == 1.0) {
		// No homogeneous division required
		double dx = a.x - b.x;
		double dy = a.y - b.y;
		double dz = a.z - b.z;
		return sqrt(dx*dx + dy*dy + dz*dz);
	} else if (a.w == 0.0 || b.w == 0.0) {
		// Assume all directions to be at no distance ...
		return 0.0;
	} else {
		// Homogeneous division required
		double dx = (a.x/a.w) - (b.x/b.w);
		double dy = (a.y/a.w) - (b.y/b.w);
		double dz = (a.z/a.w) - (b.z/b.w);
		return sqrt(dx*dx + dy*dy + dz*dz);
	}
}

// Check for bounding box intersection
bool boxesIntersect(const Point3D& aMin, const Point3D& aMax, const Point3D& bMin, const Point3D& bMax)
{
	return (aMin.x <= bMax.x && aMax.x >= bMin.x) &&
		   (aMin.y <= bMax.y && aMax.y >= bMin.y) &&
		   (aMin.z <= bMax.z && aMax.z >= bMin.z);
}

// Unary -
Point3D Point3D::operator-() const
{
	return Point3D (-x, -y, -z, w); 
}

// Binary +
Point3D Point3D::operator+(const Point3D& a) const
{
	if (w == 1.0 && a.w == 1.0) {
		return Point3D (x+a.x, y+a.y, z+a.z, 1.0);
	} else if (w != 0.0 && a.w != 0.0) {
		return Point3D ((x / w) + (a.x / a.w), (y / w) + (a.y / a.w), 
			(z / w) + (a.z / a.w), 1.0); 
	} else if (w == 0.0) {
		return Point3D (x, y, z, w);
	} else if (a.w == 0.0) {
		return Point3D (a);
	} else {
		return Point3D (x+a.x, y+a.y, z+a.z, 0.0);
	}
}

// Binary -
Point3D Point3D::operator-(const Point3D& a) const
{ 
	if (w == 1.0 && a.w == 1.0) {
		return Point3D (x-a.x, y-a.y, z-a.z, 1.0);
	} else if (w != 0.0 && a.w != 0.0) {
		return Point3D ((x / w) - (a.x / a.w), (y / w) - (a.y / a.w), 
			(z / w) - (a.z / a.w), 1.0); 
	} else if (w == 0.0) {
		return Point3D (x, y, z, w);
	} else if (a.w == 0.0) {
		return -a;
	} else {
		return Point3D (x-a.x, y-a.y, z-a.z, 0.0);
	}
}

// Cross product
Point3D Point3D::crossMultiply(const Point3D& a) const
{
	Point3D result;
	result.x =   y*a.z - z*a.y;
	result.y = - x*a.z + z*a.x;
	result.z =   x*a.y - y*a.x;
	result.w = (w + a.w) / 2;
	return result;
}

// Binary scalar product
double Point3D::operator*(const Point3D& a) const
{
	return (x*a.x + y*a.y + z*a.z) / (w*a.w);
}

// Divide a vector by scalar
Point3D Point3D::operator/(double a) const
{
	return Point3D(x/a, y/a, z/a, w);
}

// Equals
bool Point3D::operator==(const Point3D& a) const
{
	if (w == 0.0) {
		if (a.w != 0.0) {
			return false;
		} else {
			return x == a.x && y == a.y && z == a.z;      
		}
	} else {
		return x/w == a.x/a.w && y/w == a.y/a.w && z/w == a.z/a.w; 
	}
}

// Not equals
bool Point3D::operator!=(const Point3D& a) const
{
	return !operator == (a);
}

// Assignment operator
void Point3D::operator=(const Point3D& a)
{
	x = a.x;
	y = a.y;
	z = a.z;
	w = a.w;
}

// Self-add operator
// todo: w is not taken into account ... many cases ...
void Point3D::operator+=(const Point3D& a)
{
	x += a.x;
	y += a.y;
	z += a.z;
}

// Self-subtract operator
// todo: w is not taken into account ... many cases ...
void Point3D::operator-=(const Point3D& a)
{
	x -= a.x;
	y -= a.y;
	z -= a.z;
}

// Calculate a vector's length / distance to origin
double Point3D::length() const
{
	if (w != 0.0 && w != 1.0) {
		return sqrt (x*x + y*y + z*z) / w;
	} else {
		return sqrt (x*x + y*y + z*z);
	}
}

// Make itself unit length
bool Point3D::unitLength()
{
	double l = length();
	if (l > 0.0) {
		x /= l;
		y /= l;
		z /= l;
		w = 1.0;
		return true;
	} else {
		return false;
	}
}

// Apply as vertex
void Point3D::glVertex() const
/*{
	glVertex3d (x, y, z);
}*/
{
	glVertex4d (x, y, z, w);
}

// Apply as normal
void Point3D::glNormal() const
{
	glNormal3d (x, y, z);
}


void Point3D::copy3DCoordsToArray(double* array) const
{
	array[0] = x;
	array[1] = y;
	array[2] = z;
}

void Point3D::copy3DCoordsToArray(float* array) const
{
	array[0] = (float) x;
	array[1] = (float) y;
	array[2] = (float) z;
}

// Scalar multiplication
Point3D operator*(double f, const Point3D& p)
{
	return Point3D (p.x*f, p.y*f, p.z*f, p.w);
}

Point3D operator*(const Point3D& p, double f)
{
	return Point3D (p.x*f, p.y*f, p.z*f, p.w);
}

// Calculate a polar coordinate as the angle from origin to p in the plane given by origin and normal
// dir0 gives the direction in that plane where the angle would be 0
double calcPolarCoord(const Point3D& origin, const Point3D& normal, const Point3D& dir0, const Point3D& p)
{
	Plane plane(origin, normal);
	StraightLine sl(p, p + normal);
	Point3D planeIntersection;
	sl.intersects(plane, planeIntersection);
	double val = Point3D::angle(dir0, planeIntersection - origin);

	Plane plane2(origin, dir0.crossMultiply(normal));
	if (plane2.halfSpaceTest(p) > 0) {
		return val;
	} else {
		return -val;
	}
}

// Calculate a normal for a triangle
Point3D calcNormal(const Point3D& p0, const Point3D& p1, const Point3D p2)
{
	Point3D dir1 = (p0-p1);
	Point3D dir2 = (p0-p2);
	dir1.unitLength();
	dir2.unitLength();
	Point3D normal = dir1.crossMultiply(dir2);
	normal.unitLength();
	return normal;
}
