#include <math.h>

#include "mathutils.h"

#include "3darith.h"

double xyzlength(XYZ v)
{
 return(sqrt((v.x*v.x)+(v.y*v.y)+(v.z*v.z)));
}

double dot(XYZ a, XYZ b)
{
 return((a.x*b.x)+(a.y*b.y)+(a.z*b.z));
}

XYZ xyzscale(XYZ v, double s)
{
 return((XYZ){v.x*s,v.y*s,v.z*s});
}

XYZ xyzsub(XYZ a, XYZ b)
{
 return((XYZ){a.x-b.x,a.y-b.y,a.z-b.z});
}

XYZ xyzadd(XYZ a, XYZ b)
{
 return((XYZ){a.x+b.x,a.y+b.y,a.z+b.z});
}

XYZ xyzunit(XYZ v)
{
 return(xyzscale(v,1/xyzlength(v)));
}

XYZ xyzcross(XYZ a, XYZ b)
{
 return((XYZ){ (a.y * b.z) - (a.z * b.y),
	       (a.z * b.x) - (a.x * b.z),
	       (a.x * b.y) - (a.y * b.x) });
}

XYZ xyzrotate(XYZ v, double a, XYZ axis)
{
 double s;
 double c;

 s = sindeg(a);
 c = cosdeg(a);
 return((XYZ) { (v.x * ((axis.x * axis.x * (1-c)) + c)) +
		  (v.y * ((axis.x * axis.y * (1-c)) - (axis.z * s))) +
		  (v.z * ((axis.x * axis.z * (1-c)) + (axis.y * s))),
		(v.x * ((axis.y * axis.x * (1-c)) + (axis.z * s))) +
		  (v.y * ((axis.y * axis.y * (1-c)) + c)) +
		  (v.z * ((axis.y * axis.z * (1-c)) - (axis.x * s))),
		(v.x * ((axis.z * axis.x * (1-c)) - (axis.y * s))) +
		  (v.y * ((axis.z * axis.y * (1-c)) + (axis.x * s))) +
		  (v.z * ((axis.z * axis.z * (1-c)) + c)) });
}

XYZ subtract_component(XYZ v, XYZ u)
{
 return(xyzsub(v,xyzscale(u,dot(v,u))));
}

XYZ xyzblend(XYZ a, double f, XYZ b)
{
 return( (XYZ) { .x = ((1-f) * a.x) + (f * b.x),
		 .y = ((1-f) * a.y) + (f * b.y),
		 .z = ((1-f) * a.z) + (f * b.z) } );
}

XYZ xyzMv(XYZ Mx, XYZ My, XYZ Mz, XYZ v)
{
 return( (XYZ) { .x = (v.x * Mx.x) + (v.y * My.x) + (v.z * Mz.x),
		 .y = (v.x * Mx.y) + (v.y * My.y) + (v.z * Mz.y),
		 .z = (v.x * Mx.z) + (v.y * My.z) + (v.z * Mz.z) } );
}

XYZ float3(XYZI i)
{
 return((XYZ){i.x,i.y,i.z});
}

XYZI xyzisub(XYZI a, XYZI b)
{
 return((XYZI){a.x-b.x,a.y-b.y,a.z-b.z});
}

XYZI xyziadd(XYZI a, XYZI b)
{
 return((XYZI){a.x+b.x,a.y+b.y,a.z+b.z});
}