cone-triangle-intersection.cc

#include <cassert>
#include <cmath>
#include <iostream>
#include <vector>

#define FLOAT long double
#define epsilon 1e-8

using namespace std;

struct Vector {
  FLOAT x, y, z;
  Vector() { }
  Vector(FLOAT x, FLOAT y, FLOAT z) : x(x), y(y), z(z) { }
  Vector operator +(const Vector &other) const {
    return Vector(x + other.x, y + other.y, z + other.z);
  }
  Vector operator -(const Vector &other) const {
    return Vector(x - other.x, y - other.y, z - other.z);
  }
  FLOAT dot(const Vector &other) const {
    return x * other.x + y * other.y + z * other.z;
  }
  Vector cross(const Vector &other) const {
    return Vector(y * other.z - z * other.y, z * other.x - x * other.z, x * other.y - y * other.x);
  }
};
Vector operator *(FLOAT x, const Vector &V) {
  return Vector(x * V.x, x * V.y, x * V.z);
}
istream &operator >>(istream &in, Vector &v) {
  return in >> v.x >> v.y >> v.z;
}
ostream &operator <<(ostream &out, const Vector &v) {
  return out << '(' << v.x << ", " << v.y << ", " << v.z << ')';
}

typedef Vector Triangle[3];
istream &operator >>(istream &in, Triangle &t) {
  return in >> t[0] >> t[1] >> t[2];
}

struct Cone {
  Vector V, A;
  FLOAT theta, gamma, gamma2;
  bool isIn(const Vector &X) const {
    Vector D0 = X - V;
    FLOAT AdD0 = A.dot(D0);
    return AdD0 >= 0 && AdD0 * AdD0 >= gamma2 * D0.dot(D0);
  }
  vector<Vector> intersects(const Vector &X0, const Vector &X1) const {
    vector<Vector> result;
    Vector Delta0 = X0 - V, E = X1 - X0;
    FLOAT AE = A.dot(E), ADelta0 = A.dot(Delta0),
          c2 = AE * AE - gamma2 * E.dot(E),
          c1 = AE * ADelta0 - gamma2 * E.dot(Delta0),
          Delta4 = c1 * c1 - (ADelta0 * ADelta0 - gamma2 * Delta0.dot(Delta0)) * c2;  // inlined c0
    if (Delta4 < 0) return result;
    c1 /= -c2;
#define testSolution(x) do {                    \
  FLOAT t = (x);                                \
  if (t >= 0 && t <= 1) {                       \
    Vector R = X0 + t * E;                      \
    if (A.dot(R - V) >= 0) result.push_back(R); \
  }                                             \
} while (false)
    if (Delta4 < epsilon) {
      testSolution(c1);
      return result;
    }
    Delta4 = sqrt(Delta4) / c2;
    testSolution(c1 + Delta4);
    testSolution(c1 - Delta4);
#undef testSolution
    return result;
  }
  void debugIntersects(const Triangle &t) const {
    vector<int> inCone, outOfCone;
    for (int i = 0; i < 3; ++i) (isIn(t[i]) ? inCone : outOfCone).push_back(i);
    switch (inCone.size()) {
      case 0: {
        vector<Vector> intersections[3] = {intersects(t[0], t[1]), intersects(t[1], t[2]), intersects(t[2], t[0])};
        bool flag = false;
        // TODO: sort this
        for (int i = 0; i < 3; ++i) if (intersections[i].size() == 2) {
          cout << intersections[i][0] << '-' << intersections[i][1] << '~';
          flag = true;
        }
        if (flag) {
          cout << " is in cone."  << endl;
          break;
        }
        Vector E0 = t[1] - t[0], E1 = t[2] - t[0], N = E0.cross(E1), Delta0 = t[0] - V;
        FLOAT NDelta0 = N.dot(Delta0), NA = N.dot(A);
        if (NDelta0 * NA < 0) {
          cout << "No intersections. Further more, triangle is in the cone at the opposite direction." << endl;
          break;
        }
        Vector U = NDelta0 * A - NA * Delta0;
        FLOAT k = N.dot(A) * N.dot(N), t0 = N.dot(U.cross(E1)) / k, t1 = N.dot(U.cross(E0)) / -k;
        if (t0 >= 0 && t1 >= 0 && t0 + t1 <= 1) cout << "Cone is blocked completely by the triangle." << endl;
        else cout << "No intersections." << endl;
        break;
      }
      case 1: {
        vector<Vector> intersections[3] = {
            intersects(t[inCone[0]], t[outOfCone[0]]),
            intersects(t[inCone[0]], t[outOfCone[1]]),
            intersects(t[outOfCone[0]], t[outOfCone[1]])
        };
        assert(intersections[0].size() == 1);
        assert(intersections[1].size() == 1);
        cout << t[inCone[0]] << '-' << intersections[0][0] << '~';
        // TODO: sort this as well
        if (intersections[2].size() == 2) cout << intersections[2][0] << '-' << intersections[2][1] << '~';
        cout << intersections[1][0] << "- is in cone." << endl;
        break;
      }
      case 2: {
        vector<Vector> intersections[2] = {
            intersects(t[inCone[0]], t[outOfCone[0]]),
            intersects(t[inCone[1]], t[outOfCone[0]])
        };
        assert(intersections[0].size() == 1);
        assert(intersections[1].size() == 1);
        cout << t[inCone[0]] << '-' << t[inCone[1]] << '-' << intersections[0][0] << '~' << intersections[1][0]
             << "- is in cone." << endl;
        break;
      }
      case 3:
        cout << "The entire triangle is in cone." << endl;
        break;
      default: assert(0);
    }
  }
};
istream &operator >>(istream &in, Cone &c) {
  in >> c.V >> c.A >> c.theta;
  c.gamma = cos(c.theta);
  c.gamma2 = c.gamma * c.gamma;
  return in;
}

int main() {
  Cone c;
  Triangle t;
  cin >> c;
  while (cin >> t) c.debugIntersects(t);
  return 0;
}

Sample input:

0 0 0 1 0 0 0.78539816339744830962820223985154655
1 0 0 1 50 0 1 0 50
-1 -10000 0 -1 10000 10000 -1 10000 -10000
1 -10000 0 1 10000 10000 1 10000 -10000
1 0.1 0 1 0.1 0.1 1 0.1 -0.1
1 0 0 2 10 0 3 -5 13
50 50 0 0 50 50 50 0 50