geom-3d.cpp

#define LINE 0
#define SEGMENT 1
#define RAY 2

struct point {
  double x, y, z;
  point(){};
  point(double _x, double _y, double _z) {
    x = _x;
    y = _y;
    z = _z;
  }
  point operator+(point p) {
    return point(x + p.x, y + p.y, z + p.z);
  }
  point operator-(point p) {
    return point(x - p.x, y - p.y, z - p.z);
  }
  point operator*(double c) {
    return point(x * c, y * c, z * c);
  }
};

double dot(point a, point b) {
  return a.x * b.x + a.y * b.y + a.z * b.z;
}

point cross(point a, point b) {
  return point(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z,
               a.x * b.y - a.y * b.x);
}

double distSq(point a, point b) { return dot(a - b, a - b); }

// compute a, b, c, d such that all points lie on ax + by +
// cz = d. TODO: test this
double planeFromPts(point p1, point p2, point p3, double &a,
                    double &b, double &c, double &d) {
  point normal = cross(p2 - p1, p3 - p1);
  a = normal.x;
  b = normal.y;
  c = normal.z;
  d = -a * p1.x - b * p1.y - c * p1.z;
}

// project point onto plane. TODO: test this
point ptPlaneProj(point p, double a, double b, double c,
                  double d) {
  double l = (a * p.x + b * p.y + c * p.z + d) /
             (a * a + b * b + c * c);
  return point(p.x - a * l, p.y - b * l, p.z - c * l);
}

// distance from point p to plane aX + bY + cZ + d = 0
double ptPlaneDist(point p, double a, double b, double c,
                   double d) {
  return fabs(a * p.x + b * p.y + c * p.z + d) /
         sqrt(a * a + b * b + c * c);
}

// distance between parallel planes aX + bY + cZ + d1 = 0
// and aX + bY + cZ + d2 = 0
double planePlaneDist(double a, double b, double c, double d1,
                      double d2) {
  return fabs(d1 - d2) / sqrt(a * a + b * b + c * c);
}

// square distance between point and line, ray or segment
double ptLineDistSq(point s1, point s2, point p, int type) {
  double pd2 = distSq(s1, s2);
  point r;
  if (pd2 == 0)
    r = s1;
  else {
    double u = dot(p - s1, s2 - s1) / pd2;
    r = s1 + (s2 - s1) * u;
    if (type != LINE && u < 0.0)
      r = s1;
    if (type == SEGMENT && u > 1.0)
      r = s2;
  }
  return distSq(r, p);
}

// Distance between lines ab and cd. TODO: Test this
double lineLineDistance(point a, point b, point c, point d) {
  point v1 = b - a;
  point v2 = d - c;
  point cr = cross(v1, v2);
  if (dot(cr, cr) < EPS) {
    point proj = v1 * (dot(v1, c - a) / dot(v1, v1));
    return sqrt(dot(c - a - proj, c - a - proj));
  } else {
    point n = cr / sqrt(dot(cr, cr));
    point p = dot(n, c - a);
    return sqrt(dot(p, p));
  }
}

// Distance between line segments ab and cd
double segmentSegmentDistance(point a, point b, point c,
                              point d) {
  point u = b - a, v = d - c, w = a - c;
  double a = dot(u, u), b = dot(u, v), c = dot(v, v),
         d = dot(u, w), e = dot(v, w);
  double D = a * c - b * b;
  double sc, sN, sD = D;
  double tc, tN, tD = D;

  // compute the line parameters of the two closest points
  if (D < EPS) { // the lines are almost parallel
    sN = 0.0;    // force using point P0 on segment S1
    sD = 1.0;    // to prevent possible division by 0.0 later
    tN = e;
    tD = c;
  } else { // get the closest points on the infinite lines
    sN = (b * e - c * d);
    tN = (a * e - b * d);
    if (sN < 0.0) { // sc < 0 => the s=0 edge is visible
      sN = 0.0;
      tN = e;
      tD = c;
    } else if (sN > sD) { // sc > 1 => the s=1 edge is visible
      sN = sD;
      tN = e + b;
      tD = c;
    }
  }

  if (tN < 0.0) { // tc < 0 => the t=0 edge is visible
    tN = 0.0;
    // recompute sc for this edge
    if (-d < 0.0)
      sN = 0.0;
    else if (-d > a)
      sN = sD;
    else {
      sN = -d;
      sD = a;
    }
  } else if (tN > tD) { // tc > 1 => the t=1 edge is visible
    tN = tD;
    // recompute sc for this edge
    if ((-d + b) < 0.0)
      sN = 0;
    else if ((-d + b) > a)
      sN = sD;
    else {
      sN = (-d + b);
      sD = a;
    }
  }
  // finally do the division to get sc and tc
  sc = (abs(sN) < EPS ? 0.0 : sN / sD);
  tc = (abs(tN) < EPS ? 0.0 : tN / tD);
  // get the difference of the two closest points
  point dP = w + (sc * u) - (tc * v); // = S1(sc) - S2(tc)
  return sqrt(dot(dP, dP)); // return the closest distance
}

double signedTetrahedronVol(point A, point B, point C,
                            point D) {
  double A11 = A.x - B.x;
  double A12 = A.x - C.x;
  double A13 = A.x - D.x;
  double A21 = A.y - B.y;
  double A22 = A.y - C.y;
  double A23 = A.y - D.y;
  double A31 = A.z - B.z;
  double A32 = A.z - C.z;
  double A33 = A.z - D.z;
  double det = A11 * A22 * A33 + A12 * A23 * A31 +
               A13 * A21 * A32 - A11 * A23 * A32 -
               A12 * A21 * A33 - A13 * A22 * A31;
  return det / 6;
}

// Parameter is a vector of vectors of points - each
// interior vector represents the 3 points that make up 1
// face, in any order. Note: The polyhedron must be convex,
// with all faces given as triangles.
double polyhedronVol(vector<vector<point>> poly) {
  int i, j;
  point cent(0, 0, 0);
  for (i = 0; i < poly.size(); i++)
    for (j = 0; j < 3; j++)
      cent = cent + poly[i][j];
  cent = cent * (1.0 / (poly.size() * 3));
  double v = 0;
  for (i = 0; i < poly.size(); i++)
    v += fabs(signedTetrahedronVol(cent, poly[i][0], poly[i][1],
                                   poly[i][2]));
  return v;
}