/*--------------------\
| <<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);
}