Geometry.hpp

Go to the documentation of this file.

//
// This file is part of MorphoGraphX - http://www.MorphoGraphX.org
// Copyright (C) 2012-2015 Richard S. Smith and collaborators.
//
// If you use MorphoGraphX in your work, please cite:
//   http://dx.doi.org/10.7554/eLife.05864
//
// MorphoGraphX is free software, and is licensed under under the terms of the 
// GNU General (GPL) Public License version 2.0, http://www.gnu.org/licenses.
// 
#ifndef GEOMETRY_HPP
#define GEOMETRY_HPP
 
#include <Config.hpp>
 
#include <Util.hpp>
#include <Vector.hpp>
#include <Matrix.hpp>
#include <SymmetricTensor.hpp>
#include <BoundingBox.hpp>
 
#include <cmath>
#include <cuda/CudaGlobal.hpp>
#include <float.h>
#include <math.h>
 
namespace mgx 
{
  typedef unsigned char ubyte;
  typedef unsigned char uchar;
  typedef unsigned int uint;
  typedef unsigned short ushort;
  typedef unsigned long ulong;
 
  typedef SymmetricTensor SymmetricTensor;
  
  typedef Vector<2, float> Point2f;
  typedef Vector<3, float> Point3f;
  typedef Vector<4, float> Point4f;
  typedef Vector<5, float> Point5f;
  typedef Vector<6, float> Point6f;
  typedef Vector<12, float> Point12f;
  
  typedef Vector<1, double> Point1d;
  typedef Vector<2, double> Point2d;
  typedef Vector<3, double> Point3d;
  typedef Vector<4, double> Point4d;
  typedef Vector<5, double> Point5d;
  typedef Vector<6, double> Point6d;
  typedef Vector<6, double> Point6d;
  typedef Vector<8, double> Point8d;
  typedef Vector<9, double> Point9d;
  typedef Vector<12, double> Point12d;
  typedef Vector<16, double> Point16d;
  typedef Vector<18, double> Point18d;
  
  typedef Vector<2, int> Point2i;
  typedef Vector<3, int> Point3i;
  typedef Vector<4, int> Point4i;
  typedef Vector<5, int> Point5i;
  typedef Vector<6, int> Point6i;
  
  typedef Vector<1, uint> Point1u;
  typedef Vector<2, uint> Point2u;
  typedef Vector<3, uint> Point3u;
  typedef Vector<4, uint> Point4u;
  typedef Vector<5, uint> Point5u;
  typedef Vector<6, uint> Point6u;
  typedef Vector<12,uint> Point12u;
  typedef Vector<30,uint> Point30u;
 
  typedef Vector<3, ushort> Point3us;
 
  typedef Vector<3, ulong> Point3ul;
  
  typedef Vector<2, size_t> Point2s;
  typedef Vector<3, size_t> Point3s;
  typedef Vector<4, size_t> Point4s;
  typedef Vector<5, size_t> Point5s;
  typedef Vector<6, size_t> Point6s;
  
  typedef Vector<3, uchar> Point3uc;
  typedef Vector<3, ushort> Point3us;
 
  typedef Matrix<2, 2, float> Matrix2f;
  typedef Matrix<3, 3, float> Matrix3f;
  typedef Matrix<4, 4, float> Matrix4f;
 
  typedef Matrix<2, 2, uint> Matrix2u;
  typedef Matrix<3, 3, uint> Matrix3u;
  typedef Matrix<4, 4, uint> Matrix4u;
  typedef Matrix<4, 3, uint> Matrix4x3u;
  typedef Matrix<6, 3, uint> Matrix6x3u;
  
  typedef Matrix<1, 1, double> Matrix1d;
  typedef Matrix<2, 2, double> Matrix2d;
  typedef Matrix<2, 3, double> Matrix2x3d;
  typedef Matrix<2, 6, double> Matrix2x6d;
 
  typedef Matrix<3, 2, double> Matrix3x2d;
  typedef Matrix<3, 3, double> Matrix3d;
  typedef Matrix<3, 4, double> Matrix3x4d;
  typedef Matrix<3, 6, double> Matrix3x6d;
  typedef Matrix<3, 12, double> Matrix3x12d;
  typedef Matrix<3, 18, double> Matrix3x18d;
 
  typedef Matrix<4, 2, double> Matrix4x2d;
  typedef Matrix<4, 3, double> Matrix4x3d;
  typedef Matrix<4, 4, double> Matrix4d;
  typedef Matrix<4, 6, double> Matrix4x6d;
 
  typedef Matrix<5, 3, double> Matrix5x3d;
 
 
  typedef Matrix<6, 6, double> Matrix6d;
  typedef Matrix<6, 3, double> Matrix6x3d;
  typedef Matrix<6, 12, double> Matrix6x12d;
  typedef Matrix<6, 18, double> Matrix6x18d;
 
  typedef Matrix<9, 3, double> Matrix9x3d;
  typedef Matrix<9, 3, double>  Matrix9x3d;
  typedef Matrix<9, 9, double>  Matrix9d;
  typedef Matrix<9, 18, double> Matrix9x18d;
 
  typedef Matrix<12, 3, double> Matrix12x3d;
  typedef Matrix<12, 12, double> Matrix12d;
 
  typedef Matrix<18, 18, double> Matrix18d;
  
  typedef BoundingBox<3, uint> BoundingBox3u;
  typedef BoundingBox<3, int> BoundingBox3i;
  typedef BoundingBox<3, float> BoundingBox3f;
 
  // Replace with C++11 using later
  #define Point3(T) Vector<3, T>
 
  // Define 
  CU_HOST_DEVICE bool isNan(float s) { return !(s == s); }
  CU_HOST_DEVICE bool isNan(double s) { return !(s == s); }
 
  // convert to voxels and round up
  template<typename T> CU_HOST_DEVICE 
  Point3u toVoxelsCeil(const Point3(T) &p, const Point3(T) &step)
  {
    Point3u r(0, 0, 0);
    if(p.x() > 0 and step.x() > 0)
      r.x() = uint(ceil(p.x() / step.x()));
    if(p.y() > 0 and step.y() > 0)
      r.y() = uint(ceil(p.y() / step.y()));
    if(p.z() > 0 and step.z() > 0)
      r.z() = uint(ceil(p.z() / step.z()));
    return r;
  }
  
  template<typename T> CU_HOST_DEVICE 
  Vector<3, T> multMatrix4Point3(const Matrix<4, 4, T> &m, const Vector<3, T> &p)
  {
    Vector<4, T> t(p.x(), p.y(), p.z(), T(1.0));
    t = m * t;
    Vector<3, T> res(t.x() / t.t(), t.y() / t.t(), t.z() / t.t());
    return res;
  }
  
  template<typename T> 
  T CU_HOST_DEVICE interpolate(const T &a, const T &b, const T &s)
  {
    T t = trim(s, (T)0.0, (T)1.0);
    return ((1.0 - t) * a + t * b);
  }
 
  template<typename T> 
  T CU_HOST_DEVICE interpolate(const Point3(T) &t1, const Point3(T) &t2, const Point3(T) &t3, T c1, T c2, T c3, const Point3(T) &p)
  {
    T area = triangleArea(t1, t2, t3);
    T phi = (t2.x() * t3.y() - t3.x() * t2.y() + (t2.y() - t3.y()) * p.x() + (t3.x() - t2.x()) * p.y())/area * c1
          + (t3.x() * t1.y() - t1.x() * t3.y() + (t3.y() - t1.y()) * p.x() + (t1.x() - t3.x()) * p.y())/area * c2
          + (t1.x() * t2.y() - t2.x() * t1.y() + (t1.y() - t2.y()) * p.x() + (t2.x() - t1.x()) * p.y())/area * c3;
    return phi;
  } 
 
  // CU_HOST_DEVICE
  // Point3f getEulerAngles(Matrix4d &m)
  //{
  //  Point3f r(0,0,0);
  //  if(fabs(m(1,0)) > .1)
  //    r.x() = atan2(m(0,2), m(2,2)) / M_PI * 180;
  //  else
  //    r.x() = atan2(-m(2,0), m(0,0)) / M_PI * 180;
  //  r.y() = atan2(-m(1,2), m(2,2)) / M_PI * 180;
  //  r.z() = asin(m(1,0)) / M_PI * 180;
  //  return(r);
  //}
  
  template<typename T> CU_HOST_DEVICE
  bool planeLineIntersect(const Point3(T) &p, const Point3(T) &nrml, 
                                const Point3(T) &u1, const Point3(T) &u2, T &s, Point3(T) &u)
  {
    Point3(T) n = nrml;
    n.normalize();
    Point3(T) u1u2 = u2 - u1;
    T t = n * u1u2;
    // Check if parallel
    if(t == 0)
      return (false);
    s = n * (p - u1) / t;
    u = u1 + s * u1u2;
    return (true);
  }
 
  // Calculate the gradient of a signal on a triangle
  template<typename T> CU_HOST_DEVICE
  Point3(T) triangleGradient(const Point3(T) &p1, const Point3(T) &p2, const Point3(T) &p3, T c1, T c2, T c3)
  {
                  Point3(T) nrml = (p2-p1)^(p3-p1);
    T area2 = norm(nrml);
    if(area2 == 0)
      return Point3(T)();
    nrml /= area2;
  
    return (nrml ^ (c3 * (p2 - p1) + c1 * (p3 - p2) + c2 * (p1 - p3)))/area2;
  }
 
  template<typename T> CU_HOST_DEVICE
  bool lineLineIntersect(Point3(T) p1, Point3(T) p2, Point3(T) q1, Point3(T) q2, T &s, T &u, Point3(T) &intersect)
  {
    T eps = 0.0001;
    intersect = Point3(T)(0,0,0);
    Point3(T) r = p2 - p1;
    Point3(T) r2 = q2 - q1;
 
    Point3(T) rxs = r % r2;
    Point3(T) qps = (q1-p1) % r2;
    Point3(T) qpr = (q1-p1) % r;
 
    if(norm(rxs) > eps){
      if(abs(rxs.x())>abs(rxs.y()) and abs(rxs.x())>abs(rxs.z())){
        s = qps.x()/rxs.x();
        u = qpr.x()/rxs.x();
      } else if(abs(rxs.y())>abs(rxs.z())){
        s = qps.y()/rxs.y();
        u = qpr.y()/rxs.y();
      } else {
        s = qps.z()/rxs.z();
        u = qpr.z()/rxs.z();
      }
      intersect = p1 + s*r;
      return true;
    }
  
   return false;
  }
 
  // Triangle area
  template<typename T> CU_HOST_DEVICE
  T triangleArea(const Point3(T) &a, const Point3(T) &b, const Point3(T) &c) {
    return ((a - b).cross(a - c).norm() / 2.0);
  }
  
  // Signed volume of a tetrahedron
  template<typename T> CU_HOST_DEVICE
  T signedTetraVolume(const Point3(T) &a, const Point3(T) &b, const Point3(T) &c)
  {
    return ((-c.x() * b.y() * a.z() + b.x() * c.y() * a.z() + c.x() * a.y() * b.z() - a.x() * c.y() * b.z()
             - b.x() * a.y() * c.z() + a.x() * b.y() * c.z()) / 6.0f);
  }
  
  // Get (a set of) basis vectors from a plane
  template<typename T> CU_HOST_DEVICE
  void getBasisFromPlane(const Point3(T) &nrml, Point3(T)&x, Point3(T) &y, Point3(T) &z)
  {
    Point3(T) n(nrml);
    n.normalize();
    x = Point3(T)(1, 0, 0);
    y = Point3(T)(0, 1, 0);
    z = Point3(T)(0, 0, 1);
    if(z.cross(n).norm() != 0) {
      y = z.cross(n);
      y.normalize();
      x = y.cross(n);
      x.normalize();
    } else
      x = n * z * x;
    z = n;
  }
  
  // Intersect a ray with a 3D triangle
  //    Input:  ray r1,r2 triangle t0,t1,t2
  //    Output: intp = intersection point (when it exists)
  //    Return: -1 = triangle is degenerate (a segment or point)
  //             0 = disjoint (no intersect)
  //             1 = intersect in unique point I1
  //             2 = are in the same plane
  template<typename T> CU_HOST_DEVICE
  int rayTriangleIntersect(const Point3(T) &r0, const Point3(T) &r1, const Point3(T) &t0, 
                                                 const Point3(T) &t1, const Point3(T) &t2, Point3(T) &intp)
  {
    // get triangle edge vectors and plane normal
    Point3(T) u(t1 - t0);
    Point3(T) v(t2 - t0);
    Point3(T) n(u.cross(v));
    if(n.x() == 0 && n.y() == 0 && n.z() == 0)   // triangle is degenerate
      return -1;
  
    Point3(T) dir = r1 - r0;   // ray direction vector
    Point3(T) w0 = r0 - t0;
    T a = -n * w0;
    T b = n * dir;
    if(b == 0) {     // ray is parallel to triangle plane
      if(a == 0)     // ray lies in triangle plane
        return 2;
      else
        return 0;       // ray disjoint from plane
    }
  
    // get intersect point of ray with triangle plane
    T r = a / b;
    if(r < 0.0f)    // ray goes away from triangle
      return 0;     // => no intersect
    // for a segment, also test if (r > 1.0) => no intersect
  
    intp = r0 + r * dir;   // intersect point of ray and plane
  
    // is intersect point inside the triangle?
    T uu, uv, vv, wu, wv, d;
    uu = u * u;
    uv = u * v;
    vv = v * v;
    Point3(T) w = intp - t0;
    wu = w * u;
    wv = w * v;
    d = uv * uv - uu * vv;
  
    // get and test parametric coords
    T s, t;
    s = (uv * wv - vv * wu) / d;
    if(s < 0.0f || s > 1.0f)   // outside
      return 0;
    t = (uv * wu - uu * wv) / d;
    if(t < 0.0f || (s + t) > 1.0f)   // outside
      return 0;
  
    return 1;   // in
  }
  
  // Return distance from a point to a line (or segment)
  template<typename T> CU_HOST_DEVICE
  T distLinePoint(const Point3(T) &v1, const Point3(T) &_v2, const Point3(T) &_p, bool segment)
  {
    Point3(T) v2 = _v2, p = _p;
 
    v2 -= v1;
    T n = norm(v2);
    // Points are the same
    if(n == 0)
      return norm(p - v1);
  
    Point3(T) vn = v2 / n;
    p -= v1;
    Point3(T) q = vn * (p * vn);
    T dist = norm(p - q);
    if(!segment)
      return dist;
  
    // Projected point on line segment
    T qn = p * vn;
    if(qn >= 0 and qn <= n)
      return dist;
  
    // Otherwise pick closest endpoint
    dist = norm(p);
    T d = norm(p - v2);
    if(d < dist)
      dist = d;
  
    return dist;
  }
 
  // Go from image coordinates to world coordinates (centers of voxels)
  CU_HOST_DEVICE
  Point3f imageToWorld(const Point3i& img, const Point3f& step, const Point3f& shift)
  {
    return multiply(Point3f(img) + Point3f(.5f, .5f, .5f), step) + shift;
  }
  
  // Go from world coordinates to image coordinates
  CU_HOST_DEVICE 
  Point3i worldToImage(const Point3f& wrld, const Point3f& step, const Point3f& shift)
  {
    return Point3i((wrld - shift) / step - Point3f(.5f, .5f, .5f));
  }
  
  // Compute offset in stack
  CU_HOST_DEVICE size_t offset(uint x, uint y, uint z, uint xsz, uint ysz)
  {
    return ((size_t(z) * ysz + y) * xsz + x);
  }
  
  CU_HOST_DEVICE
  size_t getOffset(const int x, const int y, const int z, const Point3i& size)
  {
    return ((size_t(z) * size.y() + y) * size.x() + x);
  }
 
  CU_HOST_DEVICE
  size_t getOffset(const uint x, const uint y, const uint z, const Point3u& size)
  {
    return offset(x, y, z, size.x(), size.y());
  }
  
  CU_HOST_DEVICE size_t getOffset(const Point3i p, const Point3i& size)
  {
    return ((size_t(p.z()) * size.y() + p.y()) * size.x() + p.x());
  }
 
  inline Point3d projectPointOnPlane(const Point3d& P, const Point3d& Q, const Point3d& N) {
    return P - ((P - Q) * N) * N;
  }
}
#endif