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





Vect3D Vect3D_Add(Vect3D a, Vect3D b)
{
	Vect3D t;

	t.x=a.x+b.x;
	t.y=a.y+b.y;
	t.z=a.z+b.z;

	return(t);	
}

Vect3D Vect3D_Sub(Vect3D a, Vect3D b)
{
	Vect3D t;

	t.x=a.x-b.x;
	t.y=a.y-b.y;
	t.z=a.z-b.z;

	return(t);	
}

Vect3D Vect3D_MulScalar(Vect3D a, float scalar)
{
	Vect3D t;

	t.x=a.x*scalar;
	t.y=a.y*scalar;
	t.z=a.z*scalar;

	return(t);	
}

float Vect3D_DotProduct(Vect3D a, Vect3D b)
{
	float c=a.x*b.x+a.y*b.y+a.z*b.z;
	return c;
}

Vect3D Vect3D_Normalize(Vect3D a)
{
	Vect3D t;

	float dist=sqrt(a.x*a.x+a.y*a.y+a.z*a.z);

	t.x=a.x/dist;
	t.y=a.y/dist;
	t.z=a.z/dist;

	return(t);	
}


float Vect3D_Length(Vect3D a)
{
	return(sqrt(a.x*a.x+a.y*a.y+a.z*a.z));	
}


Vect3D Vect3D_CrossProduct(Vect3D a, Vect3D b)
{
	Vect3D t;


	t.x=a.y*b.z - a.z*b.y;
	t.y=a.z*b.x - a.x*b.z;
	t.z=a.x*b.y - a.y*b.x;

	return(t);	
}

Vect3D Vect3D_Invert(Vect3D a)
{
	Vect3D t;

	t.x=-a.x;
	t.y=-a.y;
	t.z=-a.z;

	return(t);	
}








/*
   Rotate a point p by angle theta around an arbitrary axis r
   Return the rotated point.
   Positive angles are anticlockwise looking down the axis
   towards the origin.
*/

Vect3D ArbitraryRotate(Vect3D p, Vect3D axis, float theta)
{
   Vect3D q;
   float costheta,sintheta;

   
   axis=Vect3D_Normalize(axis);
   costheta = cos(theta);
   sintheta = sin(theta);

   float minus_costheta_axisX = (1-costheta)*axis.x;
   float sintheta_axisZ = sintheta*axis.z;
   float minus_costheta_axisY=(1 - costheta) * axis.y;
   float minus_costheta_axisZ=(1 - costheta) * axis.z;

   q.x = (costheta + minus_costheta_axisX * axis.x) * p.x;
   q.x += (minus_costheta_axisX * axis.y - sintheta_axisZ) * p.y;
   q.x += (minus_costheta_axisX * axis.z + axis.y * sintheta) * p.z;

   q.y = (minus_costheta_axisX * axis.y + sintheta_axisZ) * p.x;
   q.y += (costheta + minus_costheta_axisY * axis.y) * p.y;
   q.y += (minus_costheta_axisZ * axis.y  - axis.x * sintheta) * p.z;

   q.z = (minus_costheta_axisX * axis.z - axis.y * sintheta) * p.x;
   q.z += (minus_costheta_axisY * axis.z + axis.x * sintheta) * p.y;
   q.z += (costheta + minus_costheta_axisZ * axis.z) * p.z;

   return(q);
}



Vect2D Vect2D_Add(Vect2D a, Vect2D b)
{
	Vect2D t;

	t.x=a.x+b.x;
	t.y=a.y+b.y;

	return(t);	
}

Vect2D Vect2D_Sub(Vect2D a, Vect2D b)
{
	Vect2D t;

	t.x=a.x-b.x;
	t.y=a.y-b.y;

	return(t);	
}

Vect2D Vect2D_MulScalar(Vect2D a, float scalar)
{
	Vect2D t;

	t.x=a.x*scalar;
	t.y=a.y*scalar;

	return(t);	
}

Vect2D Vect2D_OrthoVector(Vect2D a)
{
	Vect2D t;

	t.x=a.y;
	t.y=-a.x;

	return(t);	
}
