/*--------------------\
| <<Naming>>
|
| geom{SrcTypes}{Operation(s)}{DstTypes}[fd](DstList..., SrcList...);
| Sources and destinations are listed 
|  1. in size order, larger first, (see list below)
|  2. "operated on" before "environment" (project(dst, a, l))
|  3. as noted.
+---------------------+
|
| <<Types>>
|
| T:triangle     : p1[3], p2[3], p3[3]
| L:line         : origin[3], direction[3]
| R:ray          : origin[3], direction[3]
| S:segement     : a[3], b[3]
| D:edge         : a[3], b[3], counter clockwise around polygon
| P:plane        : p[4] = {A,B,C,D}, defined by A*x + B*y + C*z + D == 0
| Q:2-space line : q[3] = {A,B,C}, defined by A*x + B*y + C == 0
| V:vector       : v[3]
| U:unit vector  : v[3], norm3(v) == 1.0
| X:point        : x[3]
| W:2-direction  : w[2]
| E:2-point      : e[2]
| Z:scalar       : z
| C:class int    : i, bool or enum
|
+---------------------+
|
| <<Operations>>
|
| x:intersection
| d:distance
| c:classification
| n:nearest
| i:interpret
| p:project
|
\---------------------*/

/*--------------------------------------\
| Disclaimer:
|   This code is certified by me to work, and work well,
|   Since I have no credentials as yet,
|    (perhaps other than this code here)
|   my certification means nothing whatsoever.
|
| Simply stated: USE AT YOUR OWN RISK
|
| You have permision to use this library free
|  free programs free of charge. 
| However for anything commercial, I get 
|  a copy of the product.
\---------------------------------------*/

#include<math.h>
#include"vector.h"

#define GEOM_BUILD_DLL
#include"geom.h"

#define _F float
#define _D double
#define _CF const float
#define _CD const double

#ifndef GEOM_MT_SAFE
# define GEOM_MT_SAFE 0
#endif

#if GEOM_MT_SAFE
# define GEOM_USES_TMP(x) x;
#else
# define GEOM_USES_TMP(x) 
static float  _ptmpf[4];
static double _ptmpd[4];
static float  _ntmpf[4];
static double _ntmpd[4];
static float  _otmpf[4];
static double _otmpd[4];
#endif

static float  epsilonf = 0.001f,  neg_epsilonf = -0.001f;
static double epsilond = 0.00001, neg_epsilond = -0.00001;

#ifndef GEOM_NO_CHECK
_F geomSetEpsilonf(_F e)
{
  _F tmp = epsilonf;
  e = fabsf(e);
  epsilonf = e;
  neg_epsilonf = -e;
  return tmp;
}

_D geomSetEpsilond(_D e)
{
  _D tmp = epsilond;
  e = fabsd(e);
  epsilond = e;
  neg_epsilond = -e;
  return tmp;
}

GEOM_R(int) geomZcCf(_F f)
{
  return (f > epsilonf) || (f < neg_epsilonf);
}

GEOM_R(int) geomZcCd(_D d)
{
  return (d > epsilond) || (d < neg_epsilond);
}
#endif

GEOM_R(void) geomXXdZf(_F *z, _CF x1[3], _CF x2[3])
{
  GEOM_USES_TMP(_F _ptmpf[3])
  *z = norm3f(sub3f(_ptmpf, x1, x2));
}

GEOM_R(void) geomXXdZd(_D *z, _CD x1[3], _CD x2[3])
{
  GEOM_USES_TMP(_D _ptmpd[3])
  *z = norm3d(sub3d(_ptmpd, x1, x2));
}

GEOM_R(void) geomXXdVf(_F v[3], _CF x1[3], _CF x2[3])
{
  (void)sub3f(v, x1, x2);
}

GEOM_R(void) geomXXdVd(_D v[3], _CD x1[3], _CD x2[3])
{
  (void)sub3d(v, x1, x2);
}

GEOM_R(GEOM_CHECK_TYPE) geomViUf(_F u[3], _CF v[3])
{
  _F n = norm3f(v);
  GEOM_CHECK_VAR
  GEOM_CHECKf(n);
  (void)mul3f(u, v, 1.0f/n);
  GEOM_CHECK_RETURN
}

GEOM_R(GEOM_CHECK_TYPE) geomViUd(_D u[3], _CD v[3])
{
  _D n = norm3d(v);
  GEOM_CHECK_VAR
  GEOM_CHECKd(n);
  (void)mul3d(u, v, 1.0/n);
  GEOM_CHECK_RETURN
}

GEOM_R(GEOM_CHECK_TYPE) geomSiRf(_F o[3], _F d[3], _CF a[3], _CF b[3])
{
  GEOM_CHECK_VAR
  _F t;
  (void)cpy3f(o, a);
  sub3f(d, b, a);
  t = sqrtf(dot3f(d, d));
  GEOM_CHECKf(t);
  (void)mul3f(d, d, 1.0f/t);
  GEOM_CHECK_RETURN
}

GEOM_R(GEOM_CHECK_TYPE) geomSiRd(_D o[3], _D d[3], _CD a[3], _CD b[3])
{
  GEOM_CHECK_VAR
  _D t;
  (void)cpy3d(o, a);
  sub3d(d, b, a);
  t = sqrtd(dot3d(d, d));
  GEOM_CHECKd(t);
  (void)mul3d(d, d, 1.0/t);
  GEOM_CHECK_RETURN
}

GEOM_R(GEOM_CHECK_TYPE) geomTiPf(_F p[4], _CF p1[3], _CF p2[3], _CF p3[3])
{
  GEOM_USES_TMP(_F _ptmpf[3])
  GEOM_USES_TMP(_F _ntmpf[3])
  GEOM_CHECK_VAR
  _F t;
  crossf(p, sub3f(_ptmpf, p2, p1), sub3f(_ntmpf, p3, p1));
  t = sqrtf(dot3f(p, p));
  GEOM_CHECKf(t);
  (void)mul3f(p, p, 1.0f/t);
  p[3] = -dot3f(p1, p);
  GEOM_CHECK_RETURN
}

GEOM_R(GEOM_CHECK_TYPE) geomTiPd(_D p[4], _CD p1[3], _CD p2[3], _CD p3[3])
{
  GEOM_USES_TMP(_D _ptmpd[3])
  GEOM_USES_TMP(_D _ntmpd[3])
  GEOM_CHECK_VAR
  _D t;
  crossd(p, sub3d(_ptmpd, p2, p1), sub3d(_ntmpd, p3, p1));
  t = sqrtd(dot3d(p, p));
  GEOM_CHECKd(t);
  (void)mul3d(p, p, 1.0f/t);
  p[3] = -dot3d(p1, p);
  GEOM_CHECK_RETURN
}

GEOM_R(void) geomPXdZf(_F *z, _CF p[4], _CF x[3])
{
  *z = dot3f(p,x) + p[3];
}

GEOM_R(void) geomPXdZd(_D *z, _CD p[4], _CD x[3])
{
  *z = dot3d(p,x) + p[3];
}

/* point on plane: p_d(p_v) -> mul(p, p[3])   */
/* vector to point on plane -> sub(_ptmp, v)  */
/* project vector onto normal */ 
GEOM_R(void) geomPXdVf(_F v[3], _CF p[4], _CF x[3])
{
  (void)mul3f(v, p, -(dot3f(p, x) + p[3])); 
}

GEOM_R(void) geomPXdVd(_D v[3], _CD p[4], _CD x[3])
{
  (void)mul3d(v, p, -(dot3d(p, x) + p[3])); 
}

GEOM_R(void) geomPXnXf(_F ox[3], _CF p[4], _CF x[3])
{
  geomPXdVf(ox, p, x);
  (void)add3f(ox, ox, x);
}

GEOM_R(void) geomPXnXd(_D ox[3], _CD p[4], _CD x[3])
{
  geomPXdVd(ox, p, x);
  (void)add3d(ox, ox, x);
}
GEOM_R(GEOM_CHECK_TYPE) geomPRxZf(_F *x, _CF p[4], _CF o[3], _CF d[3])
{
  GEOM_CHECK_VAR
  _F dotdp;
  dotdp = dot3f(d,p);
  *x = -((p[3] + dot3f(o,p))/dotdp);
  GEOM_CHECKf(dotdp);
  GEOM_CHECK_RETURN
}

GEOM_R(GEOM_CHECK_TYPE) geomPRxZd(_D *x, _CD p[4], _CD o[3], _CD d[3])
{
  GEOM_CHECK_VAR
  _D dotdp;
  dotdp = dot3d(d,p);
  *x = -((p[3] + dot3d(o,p))/dotdp);
  GEOM_CHECKd(dotdp);
  GEOM_CHECK_RETURN
}

GEOM_R(GEOM_CHECK_TYPE) geomPRdVf(_F x[3], _CF p[4], _CF o[3], _CF d[3])
{
  GEOM_CHECK_VAR
  GEOM_CHECK_PASS(geomPRxZf(x, p, o, d));
  (void)mul3f(x, d, *x);
  GEOM_CHECK_RETURN;
}

GEOM_R(GEOM_CHECK_TYPE) geomPRdVd(_D x[3], _CD p[4], _CD o[3], _CD d[3])
{
  GEOM_CHECK_VAR
  GEOM_CHECK_PASS(geomPRxZd(x, p, o, d));
  (void)mul3d(x, d, *x);
  GEOM_CHECK_RETURN
}

GEOM_R(GEOM_CHECK_TYPE) geomPLxXf(_F x[3], _CF p[4], _CF o[3], _CF d[3])
{
  GEOM_CHECK_VAR
  GEOM_CHECK_PASS(geomPRdVf(x, p, o, d));
  (void)add3f(x, x, o);
  GEOM_CHECK_RETURN
}

GEOM_R(GEOM_CHECK_TYPE) geomPLxXd(_D x[3], _CD p[4], _CD o[3], _CD d[3])
{
  GEOM_CHECK_VAR
  GEOM_CHECK_PASS(geomPRdVd(x, p, o, d));
  (void)add3d(x, x, o);
  GEOM_CHECK_RETURN
}

GEOM_R(GEOM_CHECK_TYPE) geomPPxLf(_F o[3], _F d[3], _F p1[4], _F p2[4])
{
  GEOM_CHECK_VAR
  GEOM_USES_TMP(_F _ptmpf[3])
  _F t;
  crossf(_ptmpf, p1, p2);
  t = norm3f(_ptmpf);
  GEOM_CHECKf(t);
  (void)mul3f(d, _ptmpf, 1.0f/t);
  crossf(_ptmpf, d, p1);
  t = dot3f(p2, p1)*(-p1[3]) + p2[3];
  GEOM_CHECKf(t);
  mad3f(o, mul3f(_ptmpf, _ptmpf, -dot3f(_ptmpf, p2)/t), p1, -p1[3]);
  GEOM_CHECK_RETURN
}

GEOM_R(GEOM_CHECK_TYPE) geomPPxLd(_D o[3], _D d[3], _D p1[4], _D p2[4])
{
  GEOM_CHECK_VAR
  GEOM_USES_TMP(_D _ptmpd[3])
  _D t;
  crossd(_ptmpd, p1, p2);
  t = norm3d(_ptmpd);
  GEOM_CHECKd(t);
  (void)mul3d(d, _ptmpd, 1.0/t);
  crossd(_ptmpd, d, p1);
  t = dot3d(p2, p1)*(-p1[3]) + p2[3];
  GEOM_CHECKd(t);
  mad3d(o, mul3d(_ptmpd, _ptmpd, -dot3d(_ptmpd, p2)/t), p1, -p1[3]);
  GEOM_CHECK_RETURN
}

/* x = (o-v) - project(o-v,d) */
GEOM_R(void) geomLXdVf(_F x[3], _CF o[3], _CF d[3], _CF v[3])
{
  GEOM_USES_TMP(_F _ptmpf[3])
  (void)sub3f(x, x, projectu3f(_ptmpf, sub3f(x, o, v), d));
}

GEOM_R(void) geomLXdVd(_D x[3], _CD o[3], _CD d[3], _CD v[3])
{
  GEOM_USES_TMP(_D _ptmpd[3])
  (void)sub3d(x, x, projectu3d(_ptmpd, sub3d(x, o, v), d));
}

GEOM_R(void) geomLXnXf(_F x[3], _CF o[3], _CF d[3], _CF v[3])
{
  geomLXdVf(x, o, d, v);
  (void)add3f(x, x, v);
}

GEOM_R(void) geomLXnXd(_D x[3], _CD o[3], _CD d[3], _CD v[3])
{
  geomLXdVd(x, o, d, v);
  (void)add3d(x, x, v);
}

GEOM_R(void) geomLXdZf(_F *x, _CF o[3], _CF d[3], _CF v[3])
{
  GEOM_USES_TMP(_F _ptmpf[3])
  *x = sqrtf(dot3f(crossf(_ptmpf, sub3f(_ptmpf, v, o), d), _ptmpf));
}

GEOM_R(void) geomLXdZd(_D *x, _CD o[3], _CD d[3], _CD v[3])
{
  GEOM_USES_TMP(_D _ptmpd[3])
  *x = sqrtd(dot3d(crossd(_ptmpd, sub3d(_ptmpd, v, o), d), _ptmpd));
}

GEOM_R(GEOM_CHECK_TYPE) geomLLdVf(_F x[3], _CF o1[3], _CF d1[3], _CF o2[3], _CF d2[3])
{
  GEOM_USES_TMP(_F _ptmpf[3])
  GEOM_CHECK_VAR
  crossf(x, d1, d2);
  GEOM_CHECK_PASS(geomVVpVf(x, sub3f(_ptmpf, o2, o1), x));
  GEOM_CHECK_RETURN
}

GEOM_R(GEOM_CHECK_TYPE) geomLLdVd(_D x[3], _CD o1[3], _CD d1[3], _CD o2[3], _CD d2[3])
{
  GEOM_USES_TMP(_D _ptmpd[3])
  GEOM_CHECK_VAR
  crossd(x, d1, d2);
  GEOM_CHECK_PASS(geomVVpVd(x, sub3d(_ptmpd, o2, o1), x));
  GEOM_CHECK_RETURN
}

GEOM_R(GEOM_CHECK_TYPE) geomLLdZf(_F *z, _CF o1[3], _CF d1[3], _CF o2[3], _CF d2[3])
{
  GEOM_CHECK_VAR
  GEOM_USES_TMP(_F _ntmpf[3])
  GEOM_USES_TMP(_F _ptmpf[3])
  _F t;
  t = norm3f(crossf(_ntmpf, d1, d2));
  GEOM_CHECKf(t);
  *z = dot3f(sub3f(_ptmpf, o2, o1), _ntmpf)/t;
  GEOM_CHECK_RETURN
}

GEOM_R(GEOM_CHECK_TYPE) geomLLdZd(_D *z, _CD o1[3], _CD d1[3], _CD o2[3], _CD d2[3])
{
  GEOM_CHECK_VAR
  GEOM_USES_TMP(_D _ntmpd[3])
  GEOM_USES_TMP(_D _ptmpd[3])
  _D t;
  t = norm3d(crossd(_ntmpd, d1, d2));
  GEOM_CHECKd(t);
  *z = dot3d(sub3d(_ptmpd, o2, o1), _ntmpd)/t;
  GEOM_CHECK_RETURN
}

/* project v into 2-space {e1, e2} */
GEOM_R(void) geomVVXiEf(_F e[2], _CF v1[3], _CF v2[3], _CF x[3])
{
  e[0] = dot3f(x, v1);
  e[1] = dot3f(x, v2);
}

GEOM_R(void) geomVVXiEd(_D e[2], _CD v1[3], _CD v2[3], _CD x[3])
{
  e[0] = dot3d(x, v1);
  e[1] = dot3d(x, v2);
}

/* Strategy: Project lines into 2-space parallel to both, solve
   for intersection in 2-space, then reinterpret result in 3-space */
GEOM_R(GEOM_CHECK_TYPE) geomLLnXf(_F x[3], _CF o1[3], _CF d1[3], _CF o2[3], _CF d2[3])
{
  GEOM_USES_TMP(_F _ptmpf[3])
  _F t, _o1[2], _o2[2], _d1[2], _d2[2];
  GEOM_CHECK_VAR
  geomVVXiEf(_o1, d1, d2, o1);
  geomVVXiEf(_o2, d1, d2, o2);
  geomVVViWf(_d1, d1, d2, d1);
  geomVVViWf(_d2, d1, d2, d2);
  t = (_d2[0]*_d1[1] - _d1[0]*_d2[1]);
  GEOM_CHECKd(t);
  t = (_d2[1]*(_o1[0]-_o2[0]) - _d2[0]*(_o1[1]-_o2[1]))/t;
  (void)add3f(x, mul3f(x, d1, t), o1);
  GEOM_CHECK_RETURN
}

GEOM_R(GEOM_CHECK_TYPE) geomLLnXd(_D x[3], _CD o1[3], _CD d1[3], _CD o2[3], _CD d2[3])
{
  GEOM_USES_TMP(_D _ptmpd[3])
  _D t, _o1[2], _o2[2], _d1[2], _d2[2];
  GEOM_CHECK_VAR
  geomVVXiEd(_o1, d1, d2, o1);
  geomVVXiEd(_o2, d1, d2, o2);
  geomVVViWd(_d1, d1, d2, d1);
  geomVVViWd(_d2, d1, d2, d2);
  t = (_d2[0]*_d1[1] - _d1[0]*_d2[1]);
  GEOM_CHECKd(t);
  t = (_d2[1]*(_o1[0]-_o2[0]) - _d2[0]*(_o1[1]-_o2[1]))/t;
  (void)add3d(x, mul3d(x, d1, t), o1);
  GEOM_CHECK_RETURN
}

/* variations on a theme... */
GEOM_R(GEOM_CHECK_TYPE) geomLLnnXXf(_F x1[3], _F x2[3], _CF o1[3], _CF d1[3], _CF o2[3], _CF d2[3])
{
  GEOM_USES_TMP(_F _ptmpf[3])
  _F t, _o1[2], _o2[2], _d1[2], _d2[2];
  GEOM_CHECK_VAR
  geomVVXiEf(_o1, d1, d2, o1);
  geomVVXiEf(_o2, d1, d2, o2);
  geomVVViWf(_d1, d1, d2, d1);
  geomVVViWf(_d2, d1, d2, d2);
  t = (_d2[0]*_d1[1] - _d1[0]*_d2[1]);
  GEOM_CHECKd(t);
  t = (_d2[1]*(_o1[0]-_o2[0]) - _d2[0]*(_o1[1]-_o2[1]))/t;
  (void)add3f(x1, mul3f(x1, d1, t), o1);
  (void)add3f(x2, project3f(_ptmpf, sub3f(_ptmpf, o2, o1), crossf(x2, d2, d1)), x1);
  GEOM_CHECK_RETURN
}

GEOM_R(GEOM_CHECK_TYPE) geomLLnnXXd(_D x1[3], _D x2[3], _CD o1[3], _CD d1[3], _CD o2[3], _CD d2[3])
{
  GEOM_USES_TMP(_D _ptmpd[3])
  _D t, _o1[2], _o2[2], _d1[2], _d2[2];
  GEOM_CHECK_VAR
  geomVVXiEd(_o1, d1, d2, o1);
  geomVVXiEd(_o2, d1, d2, o2);
  geomVVViWd(_d1, d1, d2, d1);
  geomVVViWd(_d2, d1, d2, d2);
  t = (_d2[0]*_d1[1] - _d1[0]*_d2[1]);
  GEOM_CHECKd(t);
  t = (_d2[1]*(_o1[0]-_o2[0]) - _d2[0]*(_o1[1]-_o2[1]))/t;
  (void)add3d(x1, mul3d(x1, d1, t), o1);
  (void)add3d(x2, project3d(_ptmpd, sub3d(_ptmpd, o2, o1), crossd(x2, d2, d1)), x1);
  GEOM_CHECK_RETURN
}

/* yet another variation */
GEOM_R(GEOM_CHECK_TYPE) geomLLndXVf(_F x[3], _F v[3], _CF o1[3], _CF d1[3], _CF o2[3], _CF d2[3])
{
  GEOM_USES_TMP(_F _ptmpf[3])
  _F t, _o1[2], _o2[2], _d1[2], _d2[2];
  GEOM_CHECK_VAR
  GEOM_CHECKf(dot3f(x, x));
  geomVVXiEf(_o1, d1, d2, o1);
  geomVVXiEf(_o2, d1, d2, o2);
  geomVVViWf(_d1, d1, d2, d1);
  geomVVViWf(_d2, d1, d2, d2);
  t = (_d2[0]*_d1[1] - _d1[0]*_d2[1]);
  GEOM_CHECKd(t);
  t = (_d2[1]*(_o1[0]-_o2[0]) - _d2[0]*(_o1[1]-_o2[1]))/t;
  (void)add3f(x, mul3f(x, d1, t), o1);
  (void)projectu3f(v, sub3f(_ptmpf, o2, o1), crossf(v, d2, d1));
  GEOM_CHECK_RETURN
}

GEOM_R(GEOM_CHECK_TYPE) geomLLndXVd(_D x[3], _D v[3], _CD o1[3], _CD d1[3], _CD o2[3], _CD d2[3])
{
  GEOM_USES_TMP(_D _ptmpd[3])
  _D t, _o1[2], _o2[2], _d1[2], _d2[2];
  GEOM_CHECK_VAR
  GEOM_CHECKd(dot3d(x, x));
  geomVVXiEd(_o1, d1, d2, o1);
  geomVVXiEd(_o2, d1, d2, o2);
  geomVVViWd(_d1, d1, d2, d1);
  geomVVViWd(_d2, d1, d2, d2);
  t = (_d2[0]*_d1[1] - _d1[0]*_d2[1]);
  GEOM_CHECKd(t);
  t = (_d2[1]*(_o1[0]-_o2[0]) - _d2[0]*(_o1[1]-_o2[1]))/t;
  (void)add3d(x, mul3d(x, d1, t), o1);
  (void)projectu3d(v, sub3d(_ptmpd, o2, o1), crossd(v, d2, d1));
  GEOM_CHECK_RETURN
}

GEOM_R(GEOM_CHECK_TYPE) geomLLndXZf(_F x[3], _F *z, _CF o1[3], _CF d1[3], _CF o2[3], _CF d2[3])
{
  GEOM_USES_TMP(_F _ptmpf[3])
  _F t, _o1[2], _o2[2], _d1[2], _d2[2];
  GEOM_CHECK_VAR
  geomVVXiEf(_o1, d1, d2, o1);
  geomVVXiEf(_o2, d1, d2, o2);
  geomVVViWf(_d1, d1, d2, d1);
  geomVVViWf(_d2, d1, d2, d2);
  t = (_d2[0]*_d1[1] - _d1[0]*_d2[1]);
  GEOM_CHECKd(t);
  t = (_d2[1]*(_o1[0]-_o2[0]) - _d2[0]*(_o1[1]-_o2[1]))/t;
  (void)add3f(x, mul3f(x, d1, t), o1);
  *z = dot3f(sub3f(_ptmpf, o2, o1), x);
  GEOM_CHECK_RETURN
}

GEOM_R(GEOM_CHECK_TYPE) geomLLndXZd(_D x[3], _D *z, _CD o1[3], _CD d1[3], _CD o2[3], _CD d2[3])
{
  GEOM_USES_TMP(_D _ptmpd[3])
  _D t, _o1[2], _o2[2], _d1[2], _d2[2];
  GEOM_CHECK_VAR
  geomVVXiEd(_o1, d1, d2, o1);
  geomVVXiEd(_o2, d1, d2, o2);
  geomVVViWd(_d1, d1, d2, d1);
  geomVVViWd(_d2, d1, d2, d2);
  t = (_d2[0]*_d1[1] - _d1[0]*_d2[1]);
  GEOM_CHECKd(t);
  t = (_d2[1]*(_o1[0]-_o2[0]) - _d2[0]*(_o1[1]-_o2[1]))/t;
  (void)add3d(x, mul3d(x, d1, t), o1);
  *z = dot3d(sub3d(_ptmpd, o2, o1), x);
  GEOM_CHECK_RETURN
}

GEOM_R(void) geomRXpZf(_F *p, _CF o[3], _CF d[3], _CF x[3])
{
  GEOM_USES_TMP(_F _ptmpf[3]);
  *p = dot3f(sub3f(_ptmpf, x, o), d);
}

GEOM_R(void) geomRXpZd(_D *p, _CD o[3], _CD d[3], _CD x[3])
{
  GEOM_USES_TMP(_D _ptmpd[3]);
  *p = dot3d(sub3d(_ptmpd, x, o), d);
}

  /*----------------------------------------\
  | origin of ray -> nearest point on plane |
/-+-----------------------------------------+---------\
| direction of ray -> direction of projection of      |
|                     another point on ray with plane |
\----------------------------------------------------*/
GEOM_R(GEOM_CHECK_TYPE) geomPRpRf(_F po[3], _F pd[3], _CF p[4], _CF o[3], _CF d[3])
{
  _F t;
  GEOM_CHECK_VAR
  geomPXnXf(po, p, o);
  t = norm3f(sub3f(pd, d, project3f(pd, d, p)));
  GEOM_CHECKf(t);
  (void)mul3f(pd, pd, 1.0f/t);
  GEOM_CHECK_RETURN;
}

GEOM_R(GEOM_CHECK_TYPE) geomPRpRd(_D po[3], _D pd[3], _CD p[4], _CD o[3], _CD d[3])
{
  _D t;
  GEOM_CHECK_VAR
  geomPXnXd(po, p, o);
  t = norm3d(sub3d(pd, d, project3d(pd, d, p)));
  GEOM_CHECKd(t);
  (void)mul3d(pd, pd, 1.0f/t);
  GEOM_CHECK_RETURN;
}

GEOM_R(void) geomLXZrXf(_F r[3], _CF o[3], _CF d[3], _CF v[3], _F rad)
{
  GEOM_USES_TMP(_F _ptmpf[3])
  (void)add3f(r, rotf(_ptmpf, sub3f(_ptmpf, v, o), d, rad), o);
}

GEOM_R(void) geomLXZrXd(_D r[3], _CD o[3], _CD d[3], _CD v[3], _D rad)
{
  GEOM_USES_TMP(_D _ptmpd[3])
  (void)add3d(r, rotd(_ptmpd, sub3d(_ptmpd, v, o), d, rad), o);
}

GEOM_R(void) geomLRZrRf(_F oro[3], _F ord[3], _CF o[3], _CF d[3], _CF ro[3], _CF rd[3], _F rad)
{
  geomLXZrXf(oro, o, d, ro, rad);
  (void)rotf(ord, rd, d, rad);
}

GEOM_R(void) geomLRZrRd(_D oro[3], _D ord[3], _CD o[3], _CD d[3], _CD ro[3], _CD rd[3], _D rad)
{
  geomLXZrXd(oro, o, d, ro, rad);
  (void)rotd(ord, rd, d, rad);
}

GEOM_R(void) geomTXcCf(int *inside, _CF p1[3], _CF p2[3], _CF p3[3], _CF x[3])
{
  GEOM_USES_TMP(_F _ptmpf[4])
  GEOM_USES_TMP(_F _ntmpf[3])
  GEOM_USES_TMP(_F _otmpf[3])

  geomTiPf(_ptmpf, p1, p2, p3);
  geomTiPf(_ptmpf, p1, p2, p3);

  *inside = (
    (dot3f(sub3f(_ntmpf, x, p1),
           crossf(_otmpf, sub3f(_otmpf, p2, p1), _ptmpf)
    ) < 0.0f) &&
    (dot3f(sub3f(_ntmpf, x, p2), 
           crossf(_otmpf, sub3f(_otmpf, p3, p2), _ptmpf)
    ) < 0.0f) &&
    (dot3f(sub3f(_ntmpf, x, p3), 
           crossf(_otmpf, sub3f(_otmpf, p1, p3), _ptmpf)
    ) < 0.0f)
  );
}

GEOM_R(void) geomTXcCd(int *inside, _CD p1[3], _CD p2[3], _CD p3[3], _CD x[3])
{
  GEOM_USES_TMP(_D _ptmpd[3])
  GEOM_USES_TMP(_D _ntmpd[3])

  geomTiPd(_ptmpd, p1, p2, p3);

  *inside = (
    (dot3d(sub3d(_ntmpd, x, p1),
           crossd(_otmpd, sub3d(_otmpd, p2, p1), _ptmpd)
    ) < 0.0) &&
    (dot3d(sub3d(_ntmpd, x, p2), 
           crossd(_otmpd, sub3d(_otmpd, p3, p2), _ptmpd)
    ) < 0.0) &&
    (dot3d(sub3d(_ntmpd, x, p3), 
           crossd(_otmpd, sub3d(_otmpd, p1, p3), _ptmpd)
    ) < 0.0)
  );
}

GEOM_R(void) geomTVXcCf(int *inside, _CF p1[3], _CF p2[3], _CF p3[3], _CF v[3], _CF x[3])
{
  GEOM_USES_TMP(_F _ntmpf[3])
  GEOM_USES_TMP(_F _otmpf[3])

  *inside = (
    geomZcCf(dot3f(sub3f(_ntmpf, x, p1), crossf(_otmpf, sub3f(_otmpf, p2, p1), v))) +
    geomZcCf(dot3f(sub3f(_ntmpf, x, p2), crossf(_otmpf, sub3f(_otmpf, p3, p2), v))) +
    geomZcCf(dot3f(sub3f(_ntmpf, x, p3), crossf(_otmpf, sub3f(_otmpf, p1, p3), v)))
  ) % 3 == 0;
}

GEOM_R(void) geomTRcCf(int *inside, _CF p1[3], _CF p2[3], _CF p3[3], _CF o[3], _CF d[3])
{
  geomTVXcCf(inside, p1, p2, p3, d, o);
}

GEOM_R(void) geomTRcCd(int *inside, _CD p1[3], _CD p2[3], _CD p3[3], _CD o[3], _CD d[3])
{
  geomTVXcCd(inside, p1, p2, p3, d, o);
}

GEOM_R(void) geomTVXcCd(int *inside, _CD p1[3], _CD p2[3], _CD p3[3], _CD v[3], _CD x[3])
{
  GEOM_USES_TMP(_D _ptmpd[3])
  GEOM_USES_TMP(_D _ntmpd[3])

  *inside = (
    (dot3d(sub3d(_ntmpd, x, p1), crossd(_otmpd, sub3d(_otmpd, p2, p1), v)) < 0.0) +
    (dot3d(sub3d(_ntmpd, x, p2), crossd(_otmpd, sub3d(_otmpd, p3, p2), v)) < 0.0) +
    (dot3d(sub3d(_ntmpd, x, p3), crossd(_otmpd, sub3d(_otmpd, p1, p3), v)) < 0.0)
  ) % 3 == 0;
}

GEOM_R(void) geomPDXcCf(int *inside, _CF p[4], _CF a[3], _CF b[3], _CF x[3])
{
  GEOM_USES_TMP(_F _ptmpf[3])
  GEOM_USES_TMP(_F _ntmpf[3])

  *inside = dot3f(sub3f(_ntmpf, x, a), crossf(_ptmpf, sub3f(_ptmpf, b, a), p)) > 0;
}

GEOM_R(void) geomPDXcCd(int *inside, _CD p[4], _CD a[3], _CD b[3], _CD x[3])
{
  GEOM_USES_TMP(_D _ptmpd[3])
  GEOM_USES_TMP(_D _ntmpd[3])

  *inside = dot3d(sub3d(_ntmpd, x, a), crossd(_ptmpd, sub3d(_ptmpd, b, a), p)) > 0;
}

GEOM_R(void) geomRiPf(_F p[4], _CF o[3], _CF d[3])
{
  cpy3f(p, d);
  p[3] = -dot3f(o, d);
}

GEOM_R(void) geomRiPd(_D p[4], _CD o[3], _CD d[3])
{
  cpy3d(p, d);
  p[3] = -dot3d(o, d);
}

GEOM_R(GEOM_CHECK_TYPE) geomSXcCf(int *i, _CF a[3], _CF b[3], _CF x[3])
{
  GEOM_USES_TMP(_F _ntmpf[3])
  _F s, t, dot;
  GEOM_CHECK_VAR;

  (void)sub3f(_ntmpf, b, a);
  t = dot3f(_ntmpf, _ntmpf);
  GEOM_CHECKf(t);
  (void)mul3f(_ntmpf, _ntmpf, 1.0f/sqrtf(t));
  s = -dot3f(_ntmpf, a);
  t = -dot3f(_ntmpf, b);
  dot = dot3f(_ntmpf, x);
  *i = (
    (dot + s > 0.0f) &&
    (dot + t < 0.0f)
  );

  GEOM_CHECK_RETURN;
}

GEOM_R(GEOM_CHECK_TYPE) geomSXcCd(int *i, _CD a[3], _CD b[3], _CD x[3])
{
  GEOM_USES_TMP(_D _ntmpd[3])
  _D s, t, dot;
  GEOM_CHECK_VAR;

  (void)sub3d(_ntmpd, b, a);
  t = dot3d(_ntmpd, _ntmpd);
  GEOM_CHECKd(t);
  (void)mul3d(_ntmpd, _ntmpd, 1.0/sqrtd(t));
  s = -dot3d(_ntmpd, a);
  t = -dot3d(_ntmpd, b);
  dot = dot3d(_ntmpd, x);
  *i = (
    (dot + s > 0.0) &&
    (dot + t < 0.0)
  );

  GEOM_CHECK_RETURN;
}

GEOM_R(GEOM_CHECK_TYPE) geomVVpVf(_F dst[3], _CF v[3], _CF ref[3])
{
  GEOM_CHECK_VAR
  _F t;
  t = dot3f(ref, ref);
  GEOM_CHECKf(t);
  mul3f(dst, ref, dot3f(v, ref)/t);
  GEOM_CHECK_RETURN
}

GEOM_R(GEOM_CHECK_TYPE) geomVVpVd(_D dst[3], _CD v[3], _CD ref[3])
{
  GEOM_CHECK_VAR
  _D t;
  t = dot3d(ref, ref);
  GEOM_CHECKd(t);
  mul3d(dst, ref, dot3d(v, ref)/t);
  GEOM_CHECK_RETURN
}

GEOM_R(void) geomRZiXf(_F x[3], _CF o[3], _CF d[3], _F z)
{
  mad3f(x, o, d, z);
}

GEOM_R(void) geomRZiXd(_D x[3], _CD o[3], _CD d[3], _D z)
{
  mad3d(x, o, d, z);
}