geometry.C

/*
 * This file is part of the "Archon" framework.
 * (http://files3d.sourceforge.net)
 *
 * Copyright © 2002 by Kristian Spangsege and Brian Kristiansen.
 *
 * Permission to use, copy, modify, and distribute this software and
 * its documentation under the terms of the GNU General Public License is
 * hereby granted. No representations are made about the suitability of
 * this software for any purpose. It is provided "as is" without express
 * or implied warranty. See the GNU General Public License
 * (http://www.gnu.org/copyleft/gpl.html) for more details.
 *
 * The characters in this file are ISO8859-1 encoded.
 *
 * The documentation in this file is in "Doxygen" style
 * (http://www.doxygen.org).
 */

#include <utility>
#include <cfloat>

#include <archon/math/quartic_solve.H>
#include <archon/math/geometry.H>

using namespace std;

namespace Archon
{
  namespace Math
  {
    bool intersect(const Ray3 &ray, const Plane3 &plane, bool frontToBackOnly, double *dist)
    {
      double k = dot(plane.normal, ray.direction);
      if(k == 0 || k > 0 && frontToBackOnly) return false; // backface culling!!
      if(dist)
      {
        k = (plane.dist + dot(plane.normal, ray.origin)) / -k;
        if(k <= 0) return false; // The ray either originates on the
                                 // plane or originates on the same
                                 // side as it extends to.
        *dist = k;
      }
      return true;
    }

    int intersect(const Ray3 &ray, const Box3 &box, double &dist)
    {
      // X slab
      if(ray.direction[0] == 0 &&
         (ray.origin[0] <= box.lowerCorner[0] ||
          ray.origin[0] >= box.upperCorner[0])) return 0;
      double distNear = (box.lowerCorner[0] - ray.origin[0]) / ray.direction[0];
      double distFar = (box.upperCorner[0] - ray.origin[0]) / ray.direction[0];
      int whereNear = 1;
      int whereFar = 2;
      if(distNear > distFar)
      {
        swap(distNear, distFar);
        swap(whereNear, whereFar);
      }
      if(distFar <= 0) return 0; // The ray origin is behind the X slab

      // Y slab
      if(ray.direction[1] == 0 &&
         (ray.origin[1] <= box.lowerCorner[1] ||
          ray.origin[1] >= box.upperCorner[1])) return 0;
      double d1 = (box.lowerCorner[1] - ray.origin[1]) / ray.direction[1];
      double d2 = (box.upperCorner[1] - ray.origin[1]) / ray.direction[1];
      int w1 = 3;
      int w2 = 4;
      if(d1 > d2)
      {
        swap(d1, d2);
        swap(w1, w2);
      }
      if(d1 > distNear)
      {
        distNear = d1;
        whereNear = w1;
      }
      if(d2 < distFar)
      {
        distFar = d2;
        whereFar = w2;
      }
      if(distFar <= 0 || distNear >= distFar) return 0;

      // Z slab
      if(ray.direction[2] == 0 &&
         (ray.origin[2] <= box.lowerCorner[2] ||
          ray.origin[2] >= box.upperCorner[2])) return 0;
      d1 = (box.lowerCorner[2] - ray.origin[2]) / ray.direction[2];
      d2 = (box.upperCorner[2] - ray.origin[2]) / ray.direction[2];
      w1 = 5;
      w2 = 6;
      if(d1 > d2)
      {
        swap(d1, d2);
        swap(w1, w2);
      }
      if(d1 > distNear)
      {
        distNear = d1;
        whereNear = w1;
      }
      if(d2 < distFar)
      {
        distFar = d2;
        whereFar = w2;
      }
      if(distFar <= 0 || distNear >= distFar) return 0;

      // If we accept interior ray origination we should return distFar instead of sying no!
      if(distNear <= 0) return 0;

      dist = distNear;
      return whereNear;
    }

    bool intersect(const Ray3 &ray, const Sphere3 &sphere, double &dist)
    {
      Vector3 rp = ray.origin;
      rp -= sphere.center;
      const double b = -dot(ray.direction, ray.origin);
      if(b <= 0) return false; // The ray extends away
      const double a = ray.direction.squareSum();
      const double c = ray.origin.squareSum() - sphere.radius * sphere.radius;
      if(c <= 0) return false; // The ray originates inside
      const double d = b*b - a*c;
      if(d < 0) return false; // No solutions
      dist = (b - sqrt(d)) / a; // distFar would be (b + sqrt(d)) / a
      return true;
    }

    int intersectCylinder(const Ray3 &ray, double height, double radius,
                          double &dist, bool side, bool top, bool bottom,
                          bool enterOnly)
    {
      const Vector3 rd = ray.direction;
      const Vector3 rp = ray.origin;
      const double a =  2 * (rd[0] * rd[0] + rd[2] * rd[2]);
      const double b = -2 * (rd[0] * rp[0] + rd[2] * rp[2]);
      const double c =      (rp[0] * rp[0] + rp[2] * rp[2]) - radius * radius;
      const double d = b * b - 2 * a * c;

      if(d < 0) return 0; // No real solutions

      const double sqrt_d = sqrt(d);

      // Get the smallest of the two sollutions
      const double t = a==0 ? -numeric_limits<double>::max() : (b - sqrt_d) / a;
      // Note that if b < 0 then t < 0 since a >= 0

      const double h = height/2;

      if(h>=0)
      {
        const double y = rp[1] + rd[1] * t;

        // First intersection with infinite extension of cylinder is
        // below the bottom cap
        if(y < -h)
        {
          const double t2 = a==0 ? numeric_limits<double>::max() : (b + sqrt_d) / a;
          if(t2 <= 0) return 0; // No hit since no sollution is positive
          const double y2 = rp[1] + t2 * rd[1];
          if(y2 < -h) return 0; // No hit since both intersections occure below the bottom cap
          if(bottom)
          {
            if(rp[1] < -h)
            {
              // Intersection with bottom cap from outside
              dist = (-h - rp[1]) / rd[1];
              return 2;
            }
          }
          if(enterOnly) return 0;
          // Assume non-solid
          if(y2 <= h)
          {
            if(!side) return 0;
            // Intersection with side from inside via missing bottom cap
            dist = t2;
            return 1;
          }
          if(!top || rp[1] >= h) return 0;
          // Intersection with top cap from inside via missing bottom cap
          dist = (h - rp[1]) / rd[1];
          return 3;
        }

        // First intersection with infinite extension of cylinder is
        // above the top cap
        if(y > h)
        {
          const double t2 = a==0 ? numeric_limits<double>::max() : (b + sqrt_d) / a;
          if(t2 <= 0) return 0; // No hit since no sollution is positive
          const double y2 = rp[1] + t2 * rd[1];
          if(y2 > h) return 0; // No hit since both intersections occure above the top cap
          if(top)
          {
            if(rp[1] > h)
            {
              // Intersection with top cap from outside
              dist = (h - rp[1]) / rd[1];
              return 3;
            }
          }
          if(enterOnly) return 0;
          // Assume non-solid
          if(y2 >= -h)
          {
            if(!side) return 0;
            // Intersection with side from inside via missing top cap
            dist = t2;
            return 1;
          }
          if(!bottom || rp[1] <= -h) return 0;
          // Intersection with bottom cap from inside via missing top cap
          dist = (-h - rp[1]) / rd[1];
          return 2;
        }
      }

      // First intersection with infinite extension of cylinder is
      // between the two caps

      if(side && t>0)
      {
        // Intersection with side from outside
        dist = t;
        return 1;
      }

      if(enterOnly) return 0;

      const double t2 = a==0 ? numeric_limits<double>::max() : (b + sqrt_d) / a;
      if(t2 <= 0) return 0; // No hit since no sollution is positive
      if(h>=0)
      {
        const double y2 = rp[1] + t2 * rd[1];
        if(y2 < -h)
        {
          if(!bottom || rp[1] <= -h) return 0;
          // Intersection with bottom cap from inside via side
          dist = (-h - rp[1]) / rd[1];    
          return 2;
        }
        if(y2 > h)
        {
          if(!top || rp[1] >= h) return 0;
          // Intersection with top cap from inside via side
          dist = (h - rp[1]) / rd[1];     
          return 3;
        }
      }

      if(!side) return 0;

      // Intersection with side from inside via side
      dist = t2;
      return 1;
    }

    int intersectCone(const Ray3 &ray, double height, double bottomRadius,
                      double &dist, bool side, bool bottom,
                      bool enterOnly)
    {
      Vector3 rd = ray.direction;
      Vector3 rp = ray.origin;

      // Transform the ray into the space where the cone becomes
      // coincident with the upper nappe of the canonical infinite
      // double cone revolving around the y-axis and with apex at the
      // origin.
      rp[1] = (0.5 - rp[1]/height) * bottomRadius;
      rd[1] =      - rd[1]/height  * bottomRadius;

      double a =  2 * (rd[0] * rd[0] + rd[2] * rd[2] - rd[1] * rd[1]);
      double b = -2 * (rd[0] * rp[0] + rd[2] * rp[2] - rd[1] * rp[1]);
      double c =       rp[0] * rp[0] + rp[2] * rp[2] - rp[1] * rp[1];

      if(a > 0)
      {
        double d = b * b - 2 * a * c;

        if(d < 0) return 0; // No real solutions

        double sqrt_d = sqrt(d);

        // Get the smallest of the two sollutions
        double t = (b - sqrt_d) / a;
        // Note that if b < 0 then t < 0 since a > 0

        double y = rp[1] + rd[1] * t;

        // First intersection with infinite extension of the double cone
        // is below the apex, but since a > 0 this means that both
        // intersections occur on the lower nappe. Since the lower nappe
        // is not part of the original cone, it means that there is no
        // intersection in this case.
        if(y < 0) return 0;

        // First intersection with the infinite extension of the double cone is
        // above the cap
        if(y > bottomRadius)
        {
          double t2 = (b + sqrt_d) / a;
          if(t2 <= 0) return 0; // No hit since no sollution is positive
          double y2 = rp[1] + t2 * rd[1];
          if(y2 > bottomRadius) return 0; // No hit since both intersections occure above the cap

          // Ray is extending downwards
          if(bottom)
          {
            if(rp[1] > bottomRadius)
            {
              // Intersection with cap from outside
              dist = (bottomRadius - rp[1]) / rd[1];
              return 2;
            }
          }

          if(enterOnly || !side) return 0;
          // Intersection with side from inside via missing cap
          dist = t2;
          return 1;
        }

        // The first intersection with the infinite extension of the
        // cone is between the apex and the cap

        if(side)
        {
          if(t>0)
          {
            // Intersection with side from outside
            dist = t;
            return 1;
          }
        }
        if(enterOnly) return 0;

        double t2 = (b + sqrt_d) / a;
        if(t2 <= 0) return 0; // No hit since no sollution is positive
        double y2 = rp[1] + t2 * rd[1];
        if(y2 > bottomRadius)
        {
          // Ray is extending upwards
          if(!bottom || rp[1] >= bottomRadius) return 0;
          // Intersection with cap from inside via side
          dist = (bottomRadius - rp[1]) / rd[1];
          return 2;
        }

        if(!side) return 0;

        // Intersection with side from inside via side
        dist = t2;
        return 1;
      }


      // a <= 0

      if(rd[1] > 0)
      {
        // Ray extends upward

        double t;

        if(a == 0)
        {
          if(b < 0) return 0; // Ray-line hits lower nappe only
          if(b == 0)
          {
            if(c != 0) return 0;
            t = -rp[1] / rd[1];
          }
          else t = c / b;
        }
        else t = (b - sqrt(b * b - 2 * a * c)) / a;

        if(t <= 0)
        {
          // Ray originates inside the upper nappe
          if(rp[1] >= bottomRadius) return 0;
        }
        else
        {
          double y = rp[1] + rd[1] * t;
          if(y > bottomRadius) return 0; // Intersection is above the cap
          if(side)
          {
            dist = t;
            return 1;
          }
        }

        // Intersection with cap from inside via missing side
        if(enterOnly || !bottom) return 0;
        dist = (bottomRadius - rp[1]) / rd[1];
        return 2;
      }

      // rd[1] < 0
      // Ray extends downwards

      double t;

      if(a == 0)
      {
        if(b > 0) return 0; // Ray-line hits lower nappe only
        if(b == 0)
        {
          if(c != 0) return 0;
          t = -rp[1] / rd[1];
        }
        else t = c / b;
      }
      else t = (b + sqrt(b * b - 2 * a * c)) / a;

      if(t <= 0) return 0; // Ray originates below upper upper nappe
      double y = rp[1] + rd[1] * t;
      if(y > bottomRadius) return 0; // Intersection is above the cap
      if(rp[1] > bottomRadius && bottom)
      {
        dist = (bottomRadius - rp[1]) / rd[1];
        return 2;
      }

      if(enterOnly || !side) return 0;
      dist = t;
      return 1;
    }

    bool intersectTorus(const Ray3 &ray, double majorRadius, double minorRadius,
                        double &dist, bool surfaceOrigin, bool extToIntOnly)
    {
      const double l = ray.direction.length();
      Vector3 d = ray.direction;
      d /= l;
      const Vector3 &p = ray.origin;

      const double spx = p[0]*p[0];
      const double spy = p[1]*p[1];
      const double spz = p[2]*p[2];

      const double dpx = d[0]*p[0];
      const double dpy = d[1]*p[1];
      const double dpz = d[2]*p[2];

      const double dp  = dpx + dpy + dpz;

      const double sma = majorRadius*majorRadius;

      const double a = spx + spy + spz - minorRadius*minorRadius;
      const double b = a + sma;

      const double k3 = 4*dp;
      const double k2 = 2*(2*dp*dp + a - sma*(1 - 2*d[1]*d[1]));
      const double k1 = 4*(dp*a - sma*(dp - 2*dpy));
      const double k0 = b*b - 4*sma*(spx + spz);

      // Solve: u^4 + k3 u^3 + k2 u^2 + k1 u + k0 = 0

      double roots[4];
      const int numberOfRealRoots =
        Math::quarticSolve(k3, k2, k1, k0, roots);

      if(numberOfRealRoots == 0) return false;

      // If we know that the ray originates from the surface then we
      // know that the root closest to zero should be regarded as zero
      if(surfaceOrigin)
      {
        int minAbs = -1;
        for(int i=0; i<numberOfRealRoots; ++i)
          if(minAbs == -1 || fabs(roots[i]) < fabs(roots[minAbs])) minAbs = i;
        roots[minAbs] = 0;
      }

      // Get rid of non-positive roots
      int numberOfPositiveRoots = 0;
      for(int i=0; i<numberOfRealRoots; ++i)
        if(roots[i] > 0) roots[numberOfPositiveRoots++] = roots[i];

      if(numberOfPositiveRoots == 0) return false;

      if(extToIntOnly)
      {
        switch(numberOfPositiveRoots)
        {
        case 1:
          return false;
        case 2:
          dist = min(roots[0], roots[1]) / l;
          return true;
        case 3:
          // Find the middle root
          if(roots[0] > roots[1]) swap(roots[0], roots[1]);
          dist = (roots[2] < roots[0] ? roots[0] :
                  roots[2] > roots[1] ? roots[1] : roots[2]) / l;
          return true;
        }
      }
      else
      {
        switch(numberOfPositiveRoots)
        {
        case 1:
          dist = roots[0] / l;
          return true;
        case 2:
          dist = min(roots[0], roots[1]) / l;
          return true;
        case 3:
          dist = min(min(roots[0], roots[1]), roots[2]) / l;
          return true;
        }
      }

      // numberOfPositiveRoots == 4
      dist = min(min(roots[0], roots[1]), min(roots[2], roots[3])) / l;
      return true;
    }

    /*
     * Sources:
     *  http://www.ittc.co.jp/us/cadl/us_cadl.htm
     *  http://geometryalgorithms.com/Archive/algorithm_0104/algorithm_0104.htm
     */
    Vector3 intersect(const Plane3 &p1, const Plane3 &p2, const Plane3 &p3)
    {
      Vector3 v1 = p2.normal;
      v1 *= p3.normal;

      double bundo = dot(p1.normal, v1);
      if(bundo == 0)
        bundo = numeric_limits<double>::min(); // Just prevent division by zero

      Vector3 v2 = p2.normal;
      v2 *= p3.dist;
      Vector3 v3 = p3.normal;
      v3 *= p2.dist;
      v2 -= v3;

      v2 *= p1.normal;
      v1 *= p1.dist;
      v2 -= v1;
      v2 /= bundo;

      return v2;
    }
  }
}

---