#ifndef _INCLUDED_L3_VEC_H
#define _INCLUDED_L3_VEC_H

// *3D vector and line* Copyright (C) Krzysztof Bosak 1999-09-24...2000-11-14
// All rights reserved.
// kbosak@box43.pl
// http://www.kbosak.prv.pl

#include <math.h>
#include <stdarg.h>

/*
//+54% faster than 3 x div version...
// On an CeleronA, in this case div costs 3.17 times more than mul
* 1 x div + 3 x mul version *
	const double inv=1./s;
		x*=inv;
	y*=inv;
	z*=inv;
* 3 x div version *
	x/=s;
	y/=s;
	z/=s;
*/

/////////////////////////////////////////////////////////////////////////////

struct Vector3D
{
	double x;// Any value is perfect, stay public.
	double y;// Any value is perfect, stay public.
	double z;// Any value is perfect, stay public.

	inline Vector3D();
	inline Vector3D(double x, double y, double z);
	inline void Clear();

	inline const Vector3D operator-() const;
	inline Vector3D& operator+=(const Vector3D& v);
	inline Vector3D& operator-=(const Vector3D& v);
	inline Vector3D& operator*=(double s);
	inline Vector3D& operator/=(double s);

	inline bool IsNull() const;
	inline bool IsNotNull() const;
	inline double Length() const;
	inline double LengthPow2() const;
	inline void SetLength(double set_length);
	inline void Fill(double s);
	inline void RotateX(double angle);
	inline void RotateY(double angle);
	inline void RotateZ(double angle);
	inline void RotateX(double sa, double ca);
	inline void RotateY(double sa, double ca);
	inline void RotateZ(double sa, double ca);
	inline const Vector3D Versor() const;
	inline const Vector3D Colinear(const Vector3D& v) const;
	inline const Vector3D Orthogonal(const Vector3D& v) const;
	inline const Vector3D LinearCombination(double A, const Vector3D& B) const;
};
inline Vector3D::Vector3D()
{
}
inline Vector3D::Vector3D(double xi, double yi, double zi)
	:x(xi), y(yi), z(zi)
{
}
inline void Vector3D::Clear()
{
	memset(this, 0, sizeof(*this));
}
inline double operator*(const Vector3D& v, const Vector3D& w)
{
	return v.x*w.x + v.y*w.y + v.z*w.z;
}
inline const Vector3D operator+(const Vector3D& v, const Vector3D& w)
{
	return Vector3D(v.x+w.x, v.y+w.y, v.z+w.z);
}
inline const Vector3D operator-(const Vector3D& v, const Vector3D& w)
{
	return Vector3D(v.x-w.x, v.y-w.y, v.z-w.z);
}
inline const Vector3D operator*(const Vector3D& v, double s)
{
	return Vector3D(v.x*s, v.y*s, v.z*s);
}
inline const Vector3D operator*(double s, const Vector3D& v)
{
	return Vector3D(v.x*s, v.y*s, v.z*s);
}
inline const Vector3D operator/(const Vector3D& v, double s)
{
	assert(s!=0);
	const double inv=1./s;
	return Vector3D(v.x*inv, v.y*inv, v.z*inv);
}
inline const Vector3D operator+(const Vector3D& v, double s)
{
	return Vector3D(v.x+s, v.y+s, v.z+s);
}
inline const Vector3D operator+(double s, const Vector3D& v)
{
	return Vector3D(v.x+s, v.y+s, v.z+s);
}
inline const Vector3D operator-(const Vector3D& v, double s)
{
	return Vector3D(v.x-s, v.y-s, v.z-s);
}
inline const Vector3D operator-(double s, const Vector3D& v)
{
	return Vector3D(s-v.x, s-v.y, s-v.z);
}
ostream& operator<<(ostream& str, const Vector3D& v)
{
	return str<<'('<<v.x<<','<<v.y<<','<<v.z<<')';
}
istream& operator>>(istream& str, Vector3D& v)
{
	return str>>v.x>>v.y>>v.z;
}
inline const Vector3D Vector3D::operator-() const
{
	return Vector3D(-x, -y, -z);
}
inline Vector3D& Vector3D::operator+=(const Vector3D& v)
{
	x+=v.x;
	y+=v.y;
	z+=v.z;
	return *this;
}
inline Vector3D& Vector3D::operator-=(const Vector3D& v)
{
	x-=v.x;
	y-=v.y;
	z-=v.z;
	return *this;
}
inline Vector3D& Vector3D::operator*=(double s)
{
	x*=s;
	y*=s;
	z*=s;
	return *this;
}
inline Vector3D& Vector3D::operator/=(double s)
{
	assert(s!=0);
	const double inv=1./s;
	x*=inv;
	y*=inv;
	z*=inv;
	return *this;
}
inline bool Vector3D::IsNull() const
{
	return x==0 && y==0 && z==0;
}
inline bool Vector3D::IsNotNull() const
{
	return x!=0 || y!=0 || z!=0;
}
inline double Vector3D::Length() const
{	// Returns length of vector - quick but would overflow easily
	return sqrt(x*x + y*y + z*z);
}
inline double Vector3D::LengthPow2() const
{	// Returns (length of vector)^2
	return x*x + y*y + z*z;
}
inline void Vector3D::SetLength(double set_length)
{	// Sets length of vector
	assert(set_length>0);
	const double fact=set_length/Length();
	x*=fact;
	y*=fact;
	z*=fact;
}
inline void Vector3D::Fill(double s)
{	// Fill vector with specified value
	x=s;
	y=s;
	z=s;
}
inline void Vector3D::RotateX(double angle)
{	// Rotate in clockwise direction over X axis
	const double sa=sin(angle);
	const double ca=cos(angle);
	RotateX(sa, ca);
}
inline void Vector3D::RotateY(double angle)
{	// Rotate in clockwise direction over Y axis
	const double sa=sin(angle);
	const double ca=cos(angle);
	RotateY(sa, ca);
}
inline void Vector3D::RotateZ(double angle)
{	// Rotate in clockwise direction over Z axis
	const double sa=sin(angle);
	const double ca=cos(angle);
	RotateZ(sa, ca);
}
inline void Vector3D::RotateX(double sa, double ca)
{	// Rotate in clockwise direction over X axis
	// sin(angle) and cos(angle) as arguments (for speed)
	assert(sa>=-1);
	assert(sa<=1);
	assert(ca>=-1);
	assert(ca<=1);
	const double ynew=y*ca+z*sa;
	z*=ca;
	z-=y*sa;
	y=ynew;
}
inline void Vector3D::RotateY(double sa, double ca)
{	// Rotate in clockwise direction over Y axis
	// sin(angle) and cos(angle) as arguments (for speed)
	assert(sa>=-1);
	assert(sa<=1);
	assert(ca>=-1);
	assert(ca<=1);
	const double xnew=x*ca+z*sa;
	z*=ca;
	z-=x*sa;
	x=xnew;
}
inline void Vector3D::RotateZ(double sa, double ca)
{	// Rotate in clockwise direction over Z axis
	// sin(angle) and cos(angle) as arguments (for speed)
	assert(sa>=-1);
	assert(sa<=1);
	assert(ca>=-1);
	assert(ca<=1);
	const double xnew=x*ca+y*sa;
	y*=ca;
	y-=x*sa;
	x=xnew;
}
inline const Vector3D Vector3D::Versor() const
{	// Versor colinear to *this vector
	return *this / Length();
}
inline const Vector3D Vector3D::Colinear(const Vector3D& v) const //optimize!
{	// Returns colinear part to *this of vector v
	assert(v.IsNotNull());
	return Versor() * (*this*v);
}
inline const Vector3D Vector3D::Orthogonal(const Vector3D& v) const //optimize!
{	// Returns orthogonal part to *this of vector v
	return v - Colinear(v);
}
inline const Vector3D Vector3D::LinearCombination(double A, const Vector3D& B) const
{	// Returns linear combination: returns A*(*this)+B
	return Vector3D(x*A+B.x, y*A+B.y, z*A+B.z);
}

//

inline double Distance(const Vector3D& a, const Vector3D& b)
{
	const double dx=a.x-b.x;
	const double dy=a.y-b.y;
	const double dz=a.z-b.z;
	return sqrt(dx*dx+dy*dy+dz*dz);
}
inline double DistancePow2(const Vector3D& a, const Vector3D& b)
{
	const double dx=a.x-b.x;
	const double dy=a.y-b.y;
	const double dz=a.z-b.z;
	return dx*dx+dy*dy+dz*dz;
}
inline double Angle(const Vector3D& a, const Vector3D& b)
{	// Calculates angle between vectors in radians
	// return acos( a*b / (a.Length()*b.Length()) );
	return acos( (a.x*b.x+a.y*b.y+a.z*b.z) / sqrt( (a.x*a.x+a.y*a.y+a.z*a.z)*(b.x*b.x+b.y*b.y+b.z*b.z) ) );
}
inline double ScalarProduct(const Vector3D& a, const Vector3D& b)
{	// Dot product of 2 vectors: a and b
	return a*b;
}
inline const Vector3D VectorProduct(const Vector3D& a, const Vector3D& b)
{	// Cross product of 2 vectors: a and b
	return Vector3D( a.y*b.z - a.z*b.y , a.z*b.x - a.x*b.z , a.x*b.y - a.y*b.x );
}

/////////////////////////////////////////////////////////////////////////////

struct Line3D
{
	Vector3D start;
	Vector3D end;
	inline Line3D();
	inline Line3D(const Vector3D& start_, const Vector3D& end_);
	inline double Length() const;
	inline double LengthPow2() const;
};

inline Line3D::Line3D()
{
}
inline Line3D::Line3D(const Vector3D& start_arg, const Vector3D& end_arg)
	: start(start_arg),
	end(end_arg)
{
}
inline double Line3D::Length() const
{
	return (start-end).Length();
}
inline double Line3D::LengthPow2() const
{
	return (start-end).LengthPow2();
}

/////////////////////////////////////////////////////////////////////////////

#endif // _INCLUDED_L3_VEC_H