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

Cvector::Cvector()
{
	this->x = 0;
	this->y = 0;
	this->z = 0;
}

Cvector::Cvector(double x, double y, double z)
{
	this->x = x;
	this->y = y;
	this->z = z;
}

Cvector::~Cvector()
{
}



Cvector Cvector::operator+ (const Cvector &v)
{
	Cvector vnew;
	vnew.x = this->x + v.x;
	vnew.y = this->y + v.y;
	vnew.z = this->z + v.z;
	return vnew;
}

Cvector Cvector::operator- (const Cvector &v)
{
	Cvector vnew;
	vnew.x = this->x - v.x;
	vnew.y = this->y - v.y;
	vnew.z = this->z - v.z;
	return vnew;
}

Cvector Cvector::operator* (const double scale)
{
	Cvector vnew;
	vnew.x = x * scale;
	vnew.y = y * scale;
	vnew.z = z * scale;
	return vnew;
}

double Cvector::operator* (const Cvector &v)
{
	double dot = this->x * v.x + this->y * v.y + this->z * v.z;
	return dot;
}

Cvector Cvector::operator^ (const Cvector &v)
{
	Cvector vnew;
	vnew.x = this->y * v.z - this->z * v.y;
	vnew.y = this->z * v.x - this->x * v.z;
	vnew.z = this->x * v.y - this->y * v.x;
	return vnew;
}

double Cvector::length()
{
	double length = sqrt(this->x * this->x + this->y * this->y + this->z * this->z);
	return length;
}

void Cvector::normalize()
{
	double length = this->length();

	if (length!=0)
	{
		x /= length;
		y /= length;
		z /= length;
	}
}

Cvector Cvector::normalized()
{
	Cvector vnew = *this;
	vnew.normalize();
	return vnew;
}

Cvector Cvector::negate()
{
	Cvector vneg;
	vneg.x = -x;
	vneg.y = -y;
	vneg.z = -z;
	return vneg;
}
