#include "Vec3f.h"
#include <math.h>

Vec3f::Vec3f(float x, float y, float z)
{
	this->x = x;
	this->y = y;
	this->z = z;
}

Vec3f::Vec3f(Vec3f* from, Vec3f* to)
{
	this->x = to->x - from->x;
	this->y = to->y - from->y;
	this->z = to->z - from->z;
}

Vec3f::~Vec3f()
{

}

Vec3f &Vec3f::operator = (const Vec3f& other)
{
	this->x = other.x;
	this->y = other.y;
	this->z = other.z;

	return *this;
}

void Vec3f::add( Vec3f other )
{
	this->x += other.x;
	this->y += other.y;
	this->z += other.z;
}

void Vec3f::sub( Vec3f other )
{
	this->x -= other.x;
	this->y -= other.y;
	this->z -= other.z;
}

void Vec3f::normalize()
{
	float scale = 1.0f / length();
	x *= scale;
	y *= scale;
	z *= scale;
}

void Vec3f::rotateX( float angle )
{
	float newY = cos(angle) * y - sin(angle) * z;
	this->z = sin(angle) * y + cos(angle) * z;
	this->y = newY;
}

void Vec3f::rotateY( float angle )
{
	float newX = cos(angle) * x + sin(angle) * z;
	this->z = -sin(angle) * x + cos(angle) * z;
	this->x = newX;
}

void Vec3f::rotateZ( float angle )
{
	float newX = cos(angle) * x - sin(angle) * y;
	this->y = sin(angle) * x + cos(angle) * y;
	this->x = newX;
}

void Vec3f::rotateDegX( float angleDeg )
{
	angleDeg *= DEG_TO_RAD;
	float newY = cos(angleDeg) * y - sin(angleDeg) * z;
	this->z = sin(angleDeg) * y + cos(angleDeg) * z;
	this->y = newY;
}

void Vec3f::rotateDegY( float angleDeg )
{
	angleDeg *= DEG_TO_RAD;
	float newX = cos(angleDeg) * x + sin(angleDeg) * z;
	this->z = -sin(angleDeg) * x + cos(angleDeg) * z;
	this->x = newX;
}

void Vec3f::rotateDegZ( float angleDeg )
{
	angleDeg *= DEG_TO_RAD;
	float newX = cos(angleDeg) * x - sin(angleDeg) * y;
	this->y = sin(angleDeg) * x + cos(angleDeg) * y;
	this->x = newX;
}

void Vec3f::scale(float factor)
{
	this->x *= factor;
	this->y *= factor;
	this->z *= factor;
}

float Vec3f::length()
{
	return sqrt(x*x + y*y + z*z);
}

float Vec3f::lengthSq()
{
	return x*x + y*y + z*z;
}

float Vec3f::sqDistanceTo(Vec3f* to)
{
	return		(x - to->x) * (x - to->x)
			+	(y - to->y) * (y - to->y)
			+	(z - to->z) * (z - to->z);
}

void Vec3f::rotateDeg(float angleDeg, Vec3f* other)
{
	rotate(angleDeg * DEG_TO_RAD, other);
}
void Vec3f::rotate(float angle, Vec3f* other)
{
	// Rotation in 2 Schritten

	float radius = length();

	// Bewegungsrichtung
	Vec3f normal = *this;
	normal.crossToSelf( other );

	// Laenge des Bewegungsvektors
	float l = tan(angle) * radius;

	normal.normalize();
	normal.scale(l);
	// Bewegung durchfuehren
	this->add(normal);

	// auf Radius skalieren
	this->normalize();
	this->scale(radius);	
	// Vektoren entfernen
}
void Vec3f::crossToSelf(Vec3f* other)
{
	float nx = this->y * other->z - this->z * other->y;
	float ny = this->z * other->x - this->x * other->z;
	float nz = this->x * other->y - this->y * other->x;
	x = nx;
	y = ny;
	z = nz;
}

float Vec3f::scalarProduct(Vec3f* first, Vec3f* second)
{
	return	first->x * second->x +
			first->y * second->y +
			first->z * second->z ;
}

float Vec3f::angleBetween(Vec3f* first, Vec3f* second)
{
	return acos( scalarProduct(first, second) / (first->length() * second->length()));
}

float Vec3f::angleBetweenDeg(Vec3f* first, Vec3f* second)
{
	float res = scalarProduct(first, second);
	float l1 = first->length();
	float l2 = second->length();
	res /= (l1 * l2);
	res = acos(res);
	return res * RAD_TO_DEG;
}


