#include <math.h>

#include "1d.h"

#include "3d.h"

PT3 smul3(double f, PT3 p)
{
 return(MAKEPT3(p.x*f,p.y*f,p.z*f));
}

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

double norm23(PT3 p)
{
 return((p.x*p.x)+(p.y*p.y)+(p.z*p.z));
}

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

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

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

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

PT3 unit3(PT3 v)
{
 return(smul3(1/norm3(v),v));
}

PT3 interp3(PT3 a, double p, PT3 b)
{
 return(MAKEPT3(interp1(a.x,p,b.x),interp1(a.y,p,b.y),interp1(a.z,p,b.z)));
}

PT3 rotate3(PT3 dir, PT3 axis, double ang)
{
 PT3 xhat;
 PT3 yhat;
 PT3 zhat;
 double x;
 double y;
 double z;
 double ax;
 double ay;
 double az;
 double s;
 double c;

 zhat = unit3(axis);
 z = dot3(dir,zhat);
 dir = sub3(dir,smul3(z,zhat));
 ax = fabs(dir.x);
 ay = fabs(dir.y);
 az = fabs(dir.z);
 if ((ax == 0) && (ay == 0) && (az == 0)) return(smul3(z,zhat));
 if ((ax >= ay) && (ax >= az))
  { xhat = MAKEPT3(1,0,0);
  }
 else if ((ay >= az) && (ay >= ax))
  { xhat = MAKEPT3(0,1,0);
  }
 else
  { xhat = MAKEPT3(0,0,1);
  }
 xhat = unit3(sub3(xhat,smul3(dot3(xhat,zhat),zhat)));
 yhat = cross(zhat,xhat);
 x = dot3(dir,xhat);
 y = dot3(dir,yhat);
 c = cos(ang);
 s = sin(ang);
 return(add3( smul3(z,zhat),
	      add3( smul3((c*x)+(s*y),xhat),
		    smul3((c*y)-(s*x),yhat) ) ));
}