#include <math.h>

#include "3d.h"

PT3 add3(PT3 a, PT3 b)
{
 return((PT3){.x=a.x+b.x,.y=a.y+b.y,.z=a.z+b.z});
}

PT3 sub3(PT3 a, PT3 b)
{
 return((PT3){.x=a.x-b.x,.y=a.y-b.y,.z=a.z-b.z});
}

PT3 scale3(PT3 p, double s)
{
 return((PT3){.x=p.x*s,.y=p.y*s,.z=p.z*s});
}

PT3 unit3(PT3 a)
{
 return(scale3(a,1/norm3(a)));
}

double norm3(PT3 a)
{
 return(sqrt((a.x*a.x)+(a.y*a.y)+(a.z*a.z)));
}

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

double dist3(PT3 a, PT3 b)
{
 return(sqrt(((a.x-b.x)*(a.x-b.x))+((a.y-b.y)*(a.y-b.y))+((a.z-b.z)*(a.z-b.z))));
}

PT3 cross3(PT3 a, PT3 b)
{
 return((PT3){
	.x = (a.y * b.z) - (a.z * b.y),
	.y = (a.z * b.x) - (a.x * b.z),
	.z = (a.x * b.y) - (a.y * b.x) });
}

/*
 * Conceptually, a transform is a homogenous 4x4 matrix, which
 *  transforms p to p' according to
 *
 * [ x.m[0][0] x.m[1][0] x.m[2][0] x.m[3][0] ] [ p.x ]     [ p'.x ]
 * [ x.m[0][1] x.m[1][1] x.m[2][1] x.m[3][1] ] [ p.y ]  =  [ p'.y ]
 * [ x.m[0][2] x.m[1][2] x.m[2][2] x.m[3][2] ] [ p.z ]     [ p'.z ]
 * [ x.m[0][3] x.m[1][3] x.m[2][3] x.m[3][3] ] [  1  ]     [   1  ]
 *
 * But this requires that m[0][3], m[1][3], and m[2][3] be 0 and
 *  m[3][3] be 1, so we don't bother storing them (see the XFORM struct
 *  definition in 3d.h).
 */
PT3 transform(PT3 p, XFORM x)
{
 return((PT3){
	.x = (p.x * x.m[0][0]) + (p.y * x.m[1][0]) + (p.z * x.m[2][0]) + x.m[3][0],
	.y = (p.x * x.m[0][1]) + (p.y * x.m[1][1]) + (p.z * x.m[2][1]) + x.m[3][1],
	.z = (p.x * x.m[0][2]) + (p.y * x.m[1][2]) + (p.z * x.m[2][2]) + x.m[3][2] });
}

XFORM transform_identity(void)
{
 return((XFORM){
	.m = { { 1, 0, 0 },
	       { 0, 1, 0 },
	       { 0, 0, 1 },
	       { 0, 0, 0 } } });
}

/*
 * A transform in full generality has 12 degrees of freedom.  But
 *  that's a lot more generality than we need, since we restrict
 *  ourselves to rigid rotations and translations.  We take only three
 *  points, which even at that include three redundant degrees of
 *  freedom.  If we call the arguments to find_transform t0, tx, ty,
 *  then we transform (0,0,0) to t0, the X axis to the t0-tx line, and
 *  the X-Y plane to the t0-tx-ty plane, with ty being in the
 *  positive-Y direction from the t0-tx line.
 *
 * Conveniently, the transform matrix is just the transformed unit
 *  vectors, plus a translation component.  t0 gives us the translation
 *  component, x.m[3][*], directly.  Then we compute the unit vector in
 *  the x0-tx direction, giving us x.m[0][*].  Then we get the
 *  transformed-Y-axis vector from ty by subtracting off t0, then the
 *  transformed-X-axis component.  Turning this into a unit vector
 *  gives us x.m[1][*].  We then compute x.m[2][*], the transformed Z
 *  axis, as the cross product of the transformed X and Y axes.
 */
XFORM find_transform(PT3 t0, PT3 tx, PT3 ty)
{
 PT3 tz;

 tx = unit3(sub3(tx,t0));
 ty = sub3(ty,t0);
 ty = unit3(sub3(ty,scale3(tx,dot3(ty,tx))));
 tz = cross3(tx,ty);
 return((XFORM){
	.m = { { tx.x, tx.y, tx.z },
	       { ty.x, ty.y, ty.z },
	       { tz.x, tz.y, tz.z },
	       { t0.x, t0.y, t0.z } } });
}