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

#ifndef SQR
#define SQR(a) ((a)*(a))
#endif

#define TOL 1e-10

static double normalize3d(tcgVisionPoint3D *p)
{
 double n = p->x*p->x + p->y*p->y + p->z*p->z;
 if (n) {
  n = sqrt(n);
  p->x /= n;
  p->y /= n;
  p->z /= n;
 }
 return n;
}

int tcgVisionGeomGetCoefs(tcgVisionLine2D l, double *a, double *b, double *c)
{
 double _a, _b, d;
 _a = l.v2.y - l.v1.y;
 _b = l.v1.x - l.v2.x;
 d = sqrt(_a*_a + _b*_b);
 if (d) {
  *a = _a/d;
  *b = _b/d;
  *c = -(*a*l.v1.x + *b*l.v1.y);
  return 1;
 }
 return 0;
}

int tcgVisionGeomIntersectLines (double a, double b, double c, double d, double e, double f, tcgVisionPoint2D *pinters)
{
 double det = a*e - b*d;

 if (det) {
  det = 1.0/det;
  pinters->x = (b*f - c*e)*det;
  pinters->y = (c*d - a*f)*det;
  pinters->valid = 1;
  return 1;
 }

 pinters->x = 0.0;
 pinters->y = 0.0;
 pinters->valid = 0;
 return 0;
}

int tcgVisionGeomIntersect2D (tcgVisionLine2D l1, tcgVisionLine2D l2, tcgVisionPoint2D *pinters)
{
 double f, t;
 tcgVisionPoint2D A, B, C;

 A.x = l1.v2.x - l1.v1.x; A.y = l1.v2.y - l1.v1.y;
 B.x = l2.v1.x - l2.v2.x; B.y = l2.v1.y - l2.v2.y;
 f = A.y*B.x - A.x*B.y;
 if (SQR(f) < TOL) {
  pinters->x = 0.0;
  pinters->y = 0.0;
  pinters->valid = 0;
  return 0;
 }

 C.x = l1.v1.x - l2.v1.x; C.y = l1.v1.y - l2.v1.y;

 t = (A.x*C.y - A.y*C.x) / f;
 pinters->x = l2.v1.x + t*(l2.v2.x - l2.v1.x);
 pinters->y = l2.v1.y + t*(l2.v2.y - l2.v1.y);
 pinters->valid = l1.valid && l2.valid;
 return 1;
}

int tcgVisionGeomIntersect3D (tcgVisionLine3D l1, tcgVisionLine3D l2, tcgVisionPoint3D *pinters)
{
 double t;
 tcgVisionPoint3D d1, d2, d3;
 tcgVisionPoint3D o1o2;
 tcgVisionPoint3D p1, p2;

 d1.x = l1.v2.x - l1.v1.x;
 d1.y = l1.v2.y - l1.v1.y;
 d1.z = l1.v2.z - l1.v1.z;

 d2.x = l2.v2.x - l2.v1.x;
 d2.y = l2.v2.y - l2.v1.y;
 d2.z = l2.v2.z - l2.v1.z;

 d3.x = d1.y*d2.z - d1.z*d2.y;
 d3.y = d1.z*d2.x - d1.x*d2.z;
 d3.z = d1.x*d2.y - d1.y*d2.x;
 if (normalize3d(&d3) < TOL) {
  pinters->x = 0.0;
  pinters->y = 0.0;
  return 0; /* paralelismo. */
 }

 normalize3d(&d1);
 normalize3d(&d2);

 o1o2.x = l2.v1.x - l1.v1.x;
 o1o2.y = l2.v1.y - l1.v1.y;
 o1o2.z = l2.v1.z - l1.v1.z;

 t = (o1o2.x*d2.y*d3.z) + (o1o2.y*d2.z*d3.x) + (o1o2.z*d2.x*d3.y) - (o1o2.z*d2.y*d3.x) - (o1o2.x*d2.z*d3.y) - (o1o2.y*d2.x*d3.z);
 p1.x = l1.v1.x + d1.x*t;
 p1.y = l1.v1.y + d1.y*t;
 p1.z = l1.v1.z + d1.z*t;

 t = (o1o2.x*d1.y*d3.z) + (o1o2.y*d1.z*d3.x) + (o1o2.z*d1.x*d3.y) - (o1o2.z*d1.y*d3.x) - (o1o2.x*d1.z*d3.y) - (o1o2.y*d1.x*d3.z);
 p2.x = l2.v1.x + d2.x*t;
 p2.y = l2.v1.y + d2.y*t;
 p2.z = l2.v1.z + d2.z*t;

 if ((SQR(p2.x - p1.x) + SQR(p2.y - p1.y) + SQR(p2.z - p1.z)) < TOL) {
  *pinters = p1;
  return 1;
 }
 return 0; /* pontos diferentes. */
}
