#include "GeoAlgs.h"

bool cylinderIntersection(Vector3DW& cylAxisOrigin, Vector3DW& cylAxisDirection, real cylRadius, Vector3DW& rOrigin, Vector3DW& rDirection, Vector3DW& p1, Vector3DW& p2)
{
  Vector3DW uv = cylAxisDirection % rDirection;
  real distance = ((cylAxisOrigin - rOrigin) * uv) / (uv.getNorm());
  if(distance > cylRadius) return false;

  real dpl = (rDirection - cylAxisDirection * (rDirection * cylAxisDirection)).getNorm();
  Vector3DW cpl = cylAxisOrigin + cylAxisDirection * ((rOrigin - cylAxisOrigin) * cylAxisDirection);
  real OC = (cpl - rOrigin).getNorm();
  real L = (real)sqrt(OC*OC - distance*distance);
  real HC = (real)sqrt(cylRadius*cylRadius - distance*distance);
  real t1 = (L - HC) / dpl;
  real t2 = (L + HC) / dpl;

  p1 = rOrigin + (rDirection * t1);
  p2 = rOrigin + (rDirection * t2);

  return true;
}

bool cylinderIntersection2(real cylRadius, real cylHeight, Vector3DW rOrigin, Vector3DW rDirection)
{
  Vector3DW rOrigin1 = rOrigin;
  Vector3DW rDirection1 = rDirection;

  rOrigin1.setZ(0);
  rDirection1.setZ(0);
  real normD = rDirection1.getNorm();
  rDirection1.normalize();

  Vector3DW uv = Vector3DW(0, 0, 1) % rDirection1;
  real distance = (real)fabs((rOrigin1 * uv) / (uv.getNorm()));
  if(distance > cylRadius) return false;

  real L = (rOrigin1 * rDirection1) * (real)-1;
  real HC = (real)sqrt(cylRadius*cylRadius - distance*distance);
  real t1 = (L - HC) / normD;
  real t2 = (L + HC) / normD;

  if((t1 < 0)&&(t2 < 0)) return false;

  Vector3DW pt1 = rOrigin + (rDirection * t1);
  Vector3DW pt2 = rOrigin + (rDirection * t2);

  real d1 = pt1.getZ();
  real d2 = pt2.getZ();

  if(d1 > d2)
  {
	real raux = d1;
	d1 = d2;
	d2 = raux;
  }

  if((d2 < 0)||(d1 > cylHeight)) return false;

  return true;
}