#include "rayhdr.h"
#define TWOPI 6.28318

float raysphere(float o[], float d[], float p[], float r)
{
  //ray: (o[0] + d[0] * t, o[1] + d[1] * t, o[2] + d[2] * t)
  //sphere: (x - p[0])^2 + (y - p[1])^2 + (z - p[2])^2 == r^2
  //set f0 = o[0] - p[0]...
  //--------------------- 
  float f0 = o[0] - p[0];
  float f1 = o[1] - p[1];
  float f2 = o[2] - p[2];
  //---------------------
  //equ. for t is now:
  //(d[0] * t + f0)^2 + (d[1] * t + f1)^2 + (d[2] * t + f2)^2 == r^2
  //expand into a * t^2 + b * t + c == 0
  //---------------------
  float a = d[0] * d[0] + d[1] * d[1] + d[2] * d[2];
  float b = 2 * (d[0] * f0 + d[1] * f1 + d[2] * f2);
  float c = f0 * f0 + f1 * f1 + f2 * f2 - r * r;
  //roots are 
  //-b +/- sqrt(b * b - 4 * a * c)
  //-----------------------------
  //           2 * a 
  float disc = b * b - 4 * a * c;
  if (disc < 0) {
    return -1;
  } 

  float rtdisc = fsqrt(disc);

  float t1 = (-b - rtdisc) / (2 * a);
  if (t1 > 0) {
    return t1;
  }

  float t2 = (-b + rtdisc) / (2 * a);
  if (t2 > 0) {
    return t2;
  }

  return -1;
}

float fsqrt(float x)
{
  float r = x / 2;
  int i;
  for (i = 0; i < 10; i++) {
    r = 0.5 * (r + x / r);
  }
  return r;
}

float fcos(float x)
{
  x = x - (int) (x / TWOPI);
  float x2 = x * x;
  return 1 + x2 * (-.5 + x2 * (.0417 - x2 * 0.00138));  
}

float dot(float a[], float b[])
{
  return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}

float mag(float a[])
{
  return fsqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
}

float angle(float a[], float b[])
{
  return dot(a, b) / (mag(a) * mag(b));
}

float rayxyplane(float o[], float dir[], float z)
{
  //ray_z = o[2] + t * dir[2]
  float t = (z - o[2]) / dir[2];
  if (t < 0) return -1;
  return t; 
}

float fabs(float x)
{
  if (x < 0) return -x;
  return x;
}