BoundaryGeometry.cpp

//
// Declaration of the BoundaryGeometry class
//
// Copyright (c) 200x - 2020, Achim Gsell,
//                            Paul Scherrer Institut, Villigen PSI, Switzerland
// All rights reserved.
//
// This file is part of OPAL.
//
// OPAL is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// You should have received a copy of the GNU General Public License
// along with OPAL. If not, see <https://www.gnu.org/licenses/>.
//

//#define   ENABLE_DEBUG

#include "Structure/BoundaryGeometry.h"

#include <cmath>
#include <fstream>
#include <string>
#include <algorithm>

#include "H5hut.h"
#include <cfloat>

#include "AbstractObjects/OpalData.h"
#include "Algorithms/PartBunchBase.h"
#include "Expressions/SRefExpr.h"
#include "Elements/OpalBeamline.h"
#include "Utilities/Options.h"
#include "Utilities/OpalException.h"
#include <gsl/gsl_sys.h>

extern Inform* gmsg;

#define SQR(x) ((x)*(x))
#define PointID(triangle_id, vertex_id) Triangles_m[triangle_id][vertex_id]
#define Point(triangle_id, vertex_id)   Points_m[Triangles_m[triangle_id][vertex_id]]

/*
  In the following namespaces various approximately floating point
  comparisons are implemented. The used implementation is selected
  via

  namespaces cmp = IMPLEMENTATION;

*/

/*
  First we define some macros for function common in all namespaces.
*/
#define FUNC_EQ(x, y) inline bool eq(double x, double y) { \
        return almost_eq(x, y);                            \
    }

#define FUNC_EQ_ZERO(x) inline bool eq_zero(double x) { \
        return almost_eq_zero(x);                       \
    }

#define FUNC_LE(x, y) inline bool le(double x, double y) { \
        if (almost_eq(x, y)) {                             \
            return true;                                   \
        }                                                  \
        return x < y;                                      \
    }

#define FUNC_LE_ZERO(x) inline bool le_zero(double x) { \
        if (almost_eq_zero(x)) {                        \
            return true;                                \
        }                                               \
        return x < 0.0;                                 \
    }

#define FUNC_LT(x, y) inline bool lt(double x, double y) { \
        if (almost_eq(x, y)) {                             \
            return false;                                  \
        }                                                  \
        return x < y;                                      \
    }

#define FUNC_LT_ZERO(x) inline bool lt_zero(double x) {      \
        if (almost_eq_zero(x)) {                             \
            return false;                                    \
        }                                                    \
        return x < 0.0;                                      \
    }

#define FUNC_GE(x, y) inline bool ge(double x, double y) {      \
        if (almost_eq(x, y)) {                                  \
            return true;                                        \
        }                                                       \
        return x > y;                                           \
    }

#define FUNC_GE_ZERO(x) inline bool ge_zero(double x) { \
        if (almost_eq_zero(x)) {                        \
            return true;                                \
        }                                               \
        return x > 0.0;                                 \
    }

#define FUNC_GT(x, y) inline bool gt(double x, double y) { \
        if (almost_eq(x, y)) {                             \
            return false;                                  \
        }                                                  \
        return x > y;                                      \
    }

#define FUNC_GT_ZERO(x) inline bool gt_zero(double x) { \
        if (almost_eq_zero(x)) {                        \
            return false;                               \
        }                                               \
        return x > 0.0;                                 \
    }

namespace cmp_diff {

    /*
      Link:
      https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
    */
    inline bool almost_eq(double A, double B, double maxDiff = 1e-15, double maxRelDiff = DBL_EPSILON) {
        // Check if the numbers are really close -- needed
        // when comparing numbers near zero.
        const double diff = std::abs(A - B);
        if (diff <= maxDiff)
            return true;

        A = std::abs(A);
        B = std::abs(B);
        const double largest = (B > A) ? B : A;

        if (diff <= largest * maxRelDiff)
            return true;
        return false;
    }

    inline bool almost_eq_zero(double A, double maxDiff = 1e-15) {
        const double diff = std::abs(A);
        return (diff <= maxDiff);
    }

    FUNC_EQ(x, y);
    FUNC_EQ_ZERO(x);
    FUNC_LE(x, y);
    FUNC_LE_ZERO(x);
    FUNC_LT(x, y);
    FUNC_LT_ZERO(x);
    FUNC_GE(x, y);
    FUNC_GE_ZERO(x);
    FUNC_GT(x, y);
    FUNC_GT_ZERO(x);
}

namespace cmp_ulp_obsolete {
    /*
      See:
      https://www.cygnus-software.com/papers/comparingfloats/comparing_floating_point_numbers_obsolete.htm
    */
    inline bool almost_eq(double A, double B, double maxDiff = 1e-20, int maxUlps = 1000) {
        // Make sure maxUlps is non-negative and small enough that the
        // default NAN won't compare as equal to anything.
        // assert(maxUlps > 0 && maxUlps < 4 * 1024 * 1024);

        // handle NaN's
        // Note: comparing something with a NaN is always false!
        if (A != A || B != B) {
            return false;
        }

        if (std::abs(A - B) <= maxDiff) {
            return true;
        }

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wstrict-aliasing"
        auto aInt = *(int64_t*)&A;
#pragma GCC diagnostic pop
        // Make aInt lexicographically ordered as a twos-complement int
        if (aInt < 0) {
            aInt = 0x8000000000000000 - aInt;
        }

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wstrict-aliasing"
        auto bInt = *(int64_t*)&B;
#pragma GCC diagnostic pop
        // Make bInt lexicographically ordered as a twos-complement int
        if (bInt < 0) {
            bInt = 0x8000000000000000 - bInt;
        }

        if (std::abs(aInt - bInt) <= maxUlps) {
            return true;
        }
        return false;
    }

    inline bool almost_eq_zero(double A, double maxDiff = 1e-15) {
        // no need to handle NaN's!
        return (std::abs(A) <= maxDiff);
    }
    FUNC_EQ(x, y);
    FUNC_EQ_ZERO(x);
    FUNC_LE(x, y);
    FUNC_LE_ZERO(x);
    FUNC_LT(x, y);
    FUNC_LT_ZERO(x);
    FUNC_GE(x, y);
    FUNC_GE_ZERO(x);
    FUNC_GT(x, y);
    FUNC_GT_ZERO(x);
}

namespace cmp_ulp {
    /*
      See:
      https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
    */
    inline bool almost_eq(double A, double B, double maxDiff = 1e-20, int maxUlps = 1000) {
        // handle NaN's
        // Note: comparing something with a NaN is always false!
        if (A != A || B != B) {
            return false;
        }

        // Check if the numbers are really close -- needed
        // when comparing numbers near zero.
        if (std::abs(A - B) <= maxDiff)
            return true;

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wstrict-aliasing"
        auto aInt = *(int64_t*)&A;
        auto bInt = *(int64_t*)&B;
#pragma GCC diagnostic pop

        // Different signs means they do not match.
        // Note: a negative floating point number is also negative as integer.
        if ( std::signbit(aInt) != std::signbit(bInt))
            return false;
 
        // Find the difference in ULPs.
        return (std::abs(aInt - bInt) <= maxUlps);
    }

    inline bool almost_eq_zero(double A, double maxDiff = 1e-15) {
        return (std::abs(A) <= maxDiff);
    }
    FUNC_EQ(x, y);
    FUNC_EQ_ZERO(x);
    FUNC_LE(x, y);
    FUNC_LE_ZERO(x);
    FUNC_LT(x, y);
    FUNC_LT_ZERO(x);
    FUNC_GE(x, y);
    FUNC_GE_ZERO(x);
    FUNC_GT(x, y);
    FUNC_GT_ZERO(x);
}

namespace cmp = cmp_ulp;
/*

  Some
   _   _      _
  | | | | ___| |_ __   ___ _ __
  | |_| |/ _ \ | '_ \ / _ \ '__|
  |  _  |  __/ | |_) |  __/ |
  |_| |_|\___|_| .__/ \___|_|
             |_|

  functions
 */
namespace {
struct VectorLessX {
    bool operator() (Vector_t x1, Vector_t x2) {
        return cmp::lt (x1(0), x2(0));
    }
};

struct VectorLessY {
    bool operator() (Vector_t x1, Vector_t x2) {
        return cmp::lt(x1(1), x2 (1));
    }
};

struct VectorLessZ {
    bool operator() (Vector_t x1, Vector_t x2) {
        return cmp::lt(x1(2), x2(2));
    }
};

/**
   Calculate the maximum of coordinates of geometry,i.e the maximum of X,Y,Z
 */
Vector_t get_max_extent (std::vector<Vector_t>& coords) {
    const Vector_t x = *max_element (
        coords.begin (), coords.end (), VectorLessX ());
    const Vector_t y = *max_element (
        coords.begin (), coords.end (), VectorLessY ());
    const Vector_t z = *max_element (
        coords.begin (), coords.end (), VectorLessZ ());
    return Vector_t (x(0), y(1), z(2));
}


/*
   Compute the minimum of coordinates of geometry, i.e the minimum of X,Y,Z
 */
Vector_t get_min_extent (std::vector<Vector_t>& coords) {
    const Vector_t x = *min_element (
        coords.begin (), coords.end (), VectorLessX ());
    const Vector_t y = *min_element (
        coords.begin (), coords.end (), VectorLessY ());
    const Vector_t z = *min_element (
        coords.begin (), coords.end (), VectorLessZ ());
    return Vector_t (x(0), y(1), z(2));
}

/*
  write legacy VTK file of voxel mesh
*/
static void write_voxel_mesh (
    const std::unordered_map< int, std::unordered_set<int> >& ids,
    const Vector_t& hr_m,
    const Vektor<int,3>& nr,
    const Vector_t& origin
    ) {
    /*----------------------------------------------------------------------*/
    const size_t numpoints = 8 * ids.size ();
    std::ofstream of;
    std::string fname = Util::combineFilePath({
        OpalData::getInstance()->getAuxiliaryOutputDirectory(),
        "testBBox.vtk"
    });
    of.open (fname);
    assert (of.is_open ());
    of.precision (6);

    of << "# vtk DataFile Version 2.0" << std::endl;
    of << "generated using BoundaryGeometry::computeMeshVoxelization"
       << std::endl;
    of << "ASCII" << std::endl << std::endl;
    of << "DATASET UNSTRUCTURED_GRID" << std::endl;
    of << "POINTS " << numpoints << " float" << std::endl;

    const auto nr0_times_nr1 = nr[0] * nr[1];
    for (auto& elem: ids) {
        auto id = elem.first;
        int k = (id - 1) / nr0_times_nr1;
        int rest = (id - 1) % nr0_times_nr1;
        int j = rest / nr[0];
        int i = rest % nr[0];

        Vector_t P;
        P[0] = i * hr_m[0] + origin[0];
        P[1] = j * hr_m[1] + origin[1];
        P[2] = k * hr_m[2] + origin[2];

        of << P[0]           << " " << P[1]           << " " << P[2]           << std::endl;
        of << P[0] + hr_m[0] << " " << P[1]           << " " << P[2]           << std::endl;
        of << P[0]           << " " << P[1] + hr_m[1] << " " << P[2]           << std::endl;
        of << P[0] + hr_m[0] << " " << P[1] + hr_m[1] << " " << P[2]           << std::endl;
        of << P[0]           << " " << P[1]           << " " << P[2] + hr_m[2] << std::endl;
        of << P[0] + hr_m[0] << " " << P[1]           << " " << P[2] + hr_m[2] << std::endl;
        of << P[0]           << " " << P[1] + hr_m[1] << " " << P[2] + hr_m[2] << std::endl;
        of << P[0] + hr_m[0] << " " << P[1] + hr_m[1] << " " << P[2] + hr_m[2] << std::endl;
    }
    of << std::endl;
    const auto num_cells = ids.size ();
    of << "CELLS " << num_cells << " " << 9 * num_cells << std::endl;
    for (size_t i = 0; i < num_cells; i++)
        of << "8 "
           << 8 * i << " " << 8 * i + 1 << " " << 8 * i + 2 << " " << 8 * i + 3 << " "
           << 8 * i + 4 << " " << 8 * i + 5 << " " << 8 * i + 6 << " " << 8 * i + 7 << std::endl;
    of << "CELL_TYPES " << num_cells << std::endl;
    for (size_t i = 0; i <  num_cells; i++)
        of << "11" << std::endl;
    of << "CELL_DATA " << num_cells << std::endl;
    of << "SCALARS " << "cell_attribute_data" << " float " << "1" << std::endl;
    of << "LOOKUP_TABLE " << "default" << std::endl;
    for (size_t i = 0; i <  num_cells; i++)
        of << (float)(i) << std::endl;
    of << std::endl;
    of << "COLOR_SCALARS " << "BBoxColor " << 4 << std::endl;
    for (size_t i = 0; i < num_cells; i++) {
        of << "1.0" << " 1.0 " << "0.0 " << "1.0" << std::endl;
    }
    of << std::endl;
}
}

/*___________________________________________________________________________

  Triangle-cube intersection test.

  See:
  http://tog.acm.org/resources/GraphicsGems/gemsiii/triangleCube.c

 */


#define INSIDE 0
#define OUTSIDE 1

class Triangle {
public:
    Triangle () { }
    Triangle (const Vector_t& v1, const Vector_t& v2, const Vector_t& v3) {
        pts[0] = v1;
        pts[1] = v2;
        pts[2] = v3;
    }

    inline const Vector_t& v1() const {
        return pts[0];
    }
    inline double v1(int i) const {
        return pts[0][i];
    }
    inline const Vector_t& v2() const {
        return pts[1];
    }
    inline double v2(int i) const {
        return pts[1][i];
    }
    inline const Vector_t& v3() const {
        return pts[2];
    }
    inline double v3(int i) const {
        return pts[2][i];
    }


    inline void scale (
        const Vector_t& scaleby,
        const Vector_t& shiftby
        ) {
        pts[0][0] *= scaleby[0];
        pts[0][1] *= scaleby[1];
        pts[0][2] *= scaleby[2];
        pts[1][0] *= scaleby[0];
        pts[1][1] *= scaleby[1];
        pts[1][2] *= scaleby[2];
        pts[2][0] *= scaleby[0];
        pts[2][1] *= scaleby[1];
        pts[2][2] *= scaleby[2];
        pts[0] -= shiftby;
        pts[1] -= shiftby;
        pts[2] -= shiftby;
    }


    Vector_t pts[3];
};

/*___________________________________________________________________________*/

/* Which of the six face-plane(s) is point P outside of? */

static inline int
face_plane (
    const Vector_t& p
    ) {
    int outcode_fcmp = 0;

    if (cmp::gt(p[0],  0.5)) outcode_fcmp |= 0x01;
    if (cmp::lt(p[0], -0.5)) outcode_fcmp |= 0x02;
    if (cmp::gt(p[1],  0.5)) outcode_fcmp |= 0x04;
    if (cmp::lt(p[1], -0.5)) outcode_fcmp |= 0x08;
    if (cmp::gt(p[2],  0.5)) outcode_fcmp |= 0x10;
    if (cmp::lt(p[2], -0.5)) outcode_fcmp |= 0x20;

    return(outcode_fcmp);
}

/*. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . */

/* Which of the twelve edge plane(s) is point P outside of? */

static inline int
bevel_2d (
    const Vector_t& p
    ) {
    int outcode_fcmp = 0;

    if (cmp::gt( p[0] + p[1], 1.0)) outcode_fcmp |= 0x001;
    if (cmp::gt( p[0] - p[1], 1.0)) outcode_fcmp |= 0x002;
    if (cmp::gt(-p[0] + p[1], 1.0)) outcode_fcmp |= 0x004;
    if (cmp::gt(-p[0] - p[1], 1.0)) outcode_fcmp |= 0x008;
    if (cmp::gt( p[0] + p[2], 1.0)) outcode_fcmp |= 0x010;
    if (cmp::gt( p[0] - p[2], 1.0)) outcode_fcmp |= 0x020;
    if (cmp::gt(-p[0] + p[2], 1.0)) outcode_fcmp |= 0x040;
    if (cmp::gt(-p[0] - p[2], 1.0)) outcode_fcmp |= 0x080;
    if (cmp::gt( p[1] + p[2], 1.0)) outcode_fcmp |= 0x100;
    if (cmp::gt( p[1] - p[2], 1.0)) outcode_fcmp |= 0x200;
    if (cmp::gt(-p[1] + p[2], 1.0)) outcode_fcmp |= 0x400;
    if (cmp::gt(-p[1] - p[2], 1.0)) outcode_fcmp |= 0x800;

    return(outcode_fcmp);
}

/*. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

  Which of the eight corner plane(s) is point P outside of?
*/
static inline int
bevel_3d (
    const Vector_t& p
    ) {
    int outcode_fcmp = 0;

    if (cmp::gt( p[0] + p[1] + p[2], 1.5)) outcode_fcmp |= 0x01;
    if (cmp::gt( p[0] + p[1] - p[2], 1.5)) outcode_fcmp |= 0x02;
    if (cmp::gt( p[0] - p[1] + p[2], 1.5)) outcode_fcmp |= 0x04;
    if (cmp::gt( p[0] - p[1] - p[2], 1.5)) outcode_fcmp |= 0x08;
    if (cmp::gt(-p[0] + p[1] + p[2], 1.5)) outcode_fcmp |= 0x10;
    if (cmp::gt(-p[0] + p[1] - p[2], 1.5)) outcode_fcmp |= 0x20;
    if (cmp::gt(-p[0] - p[1] + p[2], 1.5)) outcode_fcmp |= 0x40;
    if (cmp::gt(-p[0] - p[1] - p[2], 1.5)) outcode_fcmp |= 0x80;

    return(outcode_fcmp);
}

/*. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

  Test the point "alpha" of the way from P1 to P2
  See if it is on a face of the cube
  Consider only faces in "mask"
*/

static inline int
check_point (
    const Vector_t& p1,
    const Vector_t& p2,
    const double alpha,
    const int mask
    ) {
    Vector_t plane_point;

#define LERP(a, b, t) (a + t * (b - a))
    // with C++20 we can use: std::lerp(a, b, t)
    plane_point[0] = LERP(p1[0], p2[0], alpha);
    plane_point[1] = LERP(p1[1], p2[1], alpha);
    plane_point[2] = LERP(p1[2], p2[2], alpha);
#undef LERP
    return(face_plane(plane_point) & mask);
}

/*. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

  Compute intersection of P1 --> P2 line segment with face planes
  Then test intersection point to see if it is on cube face
  Consider only face planes in "outcode_diff"
  Note: Zero bits in "outcode_diff" means face line is outside of
*/
static inline int
check_line (
    const Vector_t& p1,
    const Vector_t& p2,
    const int outcode_diff
    ) {
    if ((0x01 & outcode_diff) != 0)
        if (check_point(p1,p2,( .5-p1[0])/(p2[0]-p1[0]),0x3e) == INSIDE) return(INSIDE);
    if ((0x02 & outcode_diff) != 0)
        if (check_point(p1,p2,(-.5-p1[0])/(p2[0]-p1[0]),0x3d) == INSIDE) return(INSIDE);
    if ((0x04 & outcode_diff) != 0)
        if (check_point(p1,p2,( .5-p1[1])/(p2[1]-p1[1]),0x3b) == INSIDE) return(INSIDE);
    if ((0x08 & outcode_diff) != 0)
        if (check_point(p1,p2,(-.5-p1[1])/(p2[1]-p1[1]),0x37) == INSIDE) return(INSIDE);
    if ((0x10 & outcode_diff) != 0)
        if (check_point(p1,p2,( .5-p1[2])/(p2[2]-p1[2]),0x2f) == INSIDE) return(INSIDE);
    if ((0x20 & outcode_diff) != 0)
        if (check_point(p1,p2,(-.5-p1[2])/(p2[2]-p1[2]),0x1f) == INSIDE) return(INSIDE);
    return(OUTSIDE);
}

/*. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

  Test if 3D point is inside 3D triangle
*/
#define EPS 10e-10
static inline int
SIGN3 (
    Vector_t A
    ) {
    return ((A[0] < EPS) ? 4 : 0 | (A[0] > -EPS) ? 32 : 0 |
            (A[1] < EPS) ? 2 : 0 | (A[1] > -EPS) ? 16 : 0 |
            (A[2] < EPS) ? 1 : 0 | (A[2] > -EPS) ? 8 : 0);
}
#undef EPS

static int
point_triangle_intersection (
    const Vector_t& p,
    const Triangle& t
    ) {
    /*
      First, a quick bounding-box test:
      If P is outside triangle bbox, there cannot be an intersection.
    */
    if (cmp::gt(p[0], std::max({t.v1(0), t.v2(0), t.v3(0)}))) return(OUTSIDE);
    if (cmp::gt(p[1], std::max({t.v1(1), t.v2(1), t.v3(1)}))) return(OUTSIDE);
    if (cmp::gt(p[2], std::max({t.v1(2), t.v2(2), t.v3(2)}))) return(OUTSIDE);
    if (cmp::lt(p[0], std::min({t.v1(0), t.v2(0), t.v3(0)}))) return(OUTSIDE);
    if (cmp::lt(p[1], std::min({t.v1(1), t.v2(1), t.v3(1)}))) return(OUTSIDE);
    if (cmp::lt(p[2], std::min({t.v1(2), t.v2(2), t.v3(2)}))) return(OUTSIDE);

    /*
      For each triangle side, make a vector out of it by subtracting vertexes;
      make another vector from one vertex to point P.
      The crossproduct of these two vectors is orthogonal to both and the
      signs of its X,Y,Z components indicate whether P was to the inside or
      to the outside of this triangle side.
    */
    const Vector_t vect12 = t.v1() - t.v2();
    const Vector_t vect1h = t.v1() - p;
    const Vector_t cross12_1p = cross (vect12, vect1h);
    const int sign12 = SIGN3(cross12_1p);      /* Extract X,Y,Z signs as 0..7 or 0...63 integer */

    const Vector_t vect23 = t.v2() - t.v3();
    const Vector_t vect2h = t.v2() - p;
    const Vector_t cross23_2p = cross (vect23, vect2h);
    const int sign23 = SIGN3(cross23_2p);

    const Vector_t vect31 = t.v3() - t.v1();
    const Vector_t vect3h = t.v3() - p;
    const Vector_t cross31_3p = cross (vect31, vect3h);
    const int sign31 = SIGN3(cross31_3p);

    /*
      If all three crossproduct vectors agree in their component signs,
      then the point must be inside all three.
      P cannot be OUTSIDE all three sides simultaneously.
    */
    return ((sign12 & sign23 & sign31) == 0) ? OUTSIDE : INSIDE;
}


/*. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

  This is the main algorithm procedure.
  Triangle t is compared with a unit cube,
  centered on the origin.
  It returns INSIDE (0) or OUTSIDE(1) if t
  intersects or does not intersect the cube.
*/
static int
triangle_intersects_cube (
    const Triangle& t
    ) {
    int v1_test;
    int v2_test;
    int v3_test;

    /*
      First compare all three vertexes with all six face-planes
      If any vertex is inside the cube, return immediately!
    */
    if ((v1_test = face_plane(t.v1())) == INSIDE) return(INSIDE);
    if ((v2_test = face_plane(t.v2())) == INSIDE) return(INSIDE);
    if ((v3_test = face_plane(t.v3())) == INSIDE) return(INSIDE);

   /*
     If all three vertexes were outside of one or more face-planes,
     return immediately with a trivial rejection!
   */
   if ((v1_test & v2_test & v3_test) != 0) return(OUTSIDE);

   /*
     Now do the same trivial rejection test for the 12 edge planes
   */
   v1_test |= bevel_2d(t.v1()) << 8;
   v2_test |= bevel_2d(t.v2()) << 8;
   v3_test |= bevel_2d(t.v3()) << 8;
   if ((v1_test & v2_test & v3_test) != 0) return(OUTSIDE);

   /*
     Now do the same trivial rejection test for the 8 corner planes
   */
   v1_test |= bevel_3d(t.v1()) << 24;
   v2_test |= bevel_3d(t.v2()) << 24;
   v3_test |= bevel_3d(t.v3()) << 24;
   if ((v1_test & v2_test & v3_test) != 0) return(OUTSIDE);

   /*
     If vertex 1 and 2, as a pair, cannot be trivially rejected
     by the above tests, then see if the v1-->v2 triangle edge
     intersects the cube.  Do the same for v1-->v3 and v2-->v3./
     Pass to the intersection algorithm the "OR" of the outcode
     bits, so that only those cube faces which are spanned by
     each triangle edge need be tested.
   */
   if ((v1_test & v2_test) == 0)
       if (check_line (t.v1(), t.v2(), v1_test|v2_test) == INSIDE) return(INSIDE);
   if ((v1_test & v3_test) == 0)
       if (check_line (t.v1(), t.v3(), v1_test|v3_test) == INSIDE) return(INSIDE);
   if ((v2_test & v3_test) == 0)
       if (check_line (t.v2(), t.v3(), v2_test|v3_test) == INSIDE) return(INSIDE);

   /*
     By now, we know that the triangle is not off to any side,
     and that its sides do not penetrate the cube.  We must now
     test for the cube intersecting the interior of the triangle.
     We do this by looking for intersections between the cube
     diagonals and the triangle...first finding the intersection
     of the four diagonals with the plane of the triangle, and
     then if that intersection is inside the cube, pursuing
     whether the intersection point is inside the triangle itself.

     To find plane of the triangle, first perform crossproduct on
     two triangle side vectors to compute the normal vector.
   */
   Vector_t vect12 = t.v1() - t.v2();
   Vector_t vect13 = t.v1() - t.v3();
   Vector_t norm = cross (vect12, vect13);

   /*
     The normal vector "norm" X,Y,Z components are the coefficients
     of the triangles AX + BY + CZ + D = 0 plane equation.  If we
     solve the plane equation for X=Y=Z (a diagonal), we get
     -D/(A+B+C) as a metric of the distance from cube center to the
     diagonal/plane intersection.  If this is between -0.5 and 0.5,
     the intersection is inside the cube.  If so, we continue by
     doing a point/triangle intersection.
     Do this for all four diagonals.
   */
   double d = norm[0] * t.v1(0) + norm[1] * t.v1(1) + norm[2] * t.v1(2);

   /*
     if one of the diagonals is parallel to the plane, the other will
     intersect the plane
   */
   double denom = norm[0] + norm[1] + norm[2];
   if (cmp::eq_zero(std::abs(denom)) == false) {
       /* skip parallel diagonals to the plane; division by 0 can occure */
       Vector_t hitpp = d / denom;
       if (cmp::le(std::abs(hitpp[0]), 0.5))
           if (point_triangle_intersection(hitpp,t) == INSIDE)
               return(INSIDE);
   }
   denom = norm[0] + norm[1] - norm[2];
   if (cmp::eq_zero(std::abs(denom)) == false) {
       Vector_t hitpn;
       hitpn[2] = -(hitpn[0] = hitpn[1] = d / denom);
       if (cmp::le(std::abs(hitpn[0]), 0.5))
           if (point_triangle_intersection(hitpn,t) == INSIDE)
               return(INSIDE);
   }
   denom = norm[0] - norm[1] + norm[2];
   if (cmp::eq_zero(std::abs(denom)) == false) {
       Vector_t hitnp;
       hitnp[1] = -(hitnp[0] = hitnp[2] = d / denom);
       if (cmp::le(std::abs(hitnp[0]), 0.5))
           if (point_triangle_intersection(hitnp,t) == INSIDE)
               return(INSIDE);
   }
   denom = norm[0] - norm[1] - norm[2];
   if (cmp::eq_zero(std::abs(denom)) == false) {
       Vector_t hitnn;
       hitnn[1] = hitnn[2] = -(hitnn[0] = d / denom);
       if (cmp::le(std::abs(hitnn[0]), 0.5))
           if (point_triangle_intersection(hitnn,t) == INSIDE)
               return(INSIDE);
   }

   /*
     No edge touched the cube; no cube diagonal touched the triangle.
     We're done...there was no intersection.
   */
   return(OUTSIDE);
}

/*
 * Ray class, for use with the optimized ray-box intersection test
 * described in:
 *
 *      Amy Williams, Steve Barrus, R. Keith Morley, and Peter Shirley
 *      "An Efficient and Robust Ray-Box Intersection Algorithm"
 *      Journal of graphics tools, 10(1):49-54, 2005
 *
 */

class Ray {
public:
    Ray () { }
    Ray (Vector_t o, Vector_t d) {
        origin = o;
        direction = d;
        inv_direction = Vector_t (1/d[0], 1/d[1], 1/d[2]);
        sign[0] = (inv_direction[0] < 0);
        sign[1] = (inv_direction[1] < 0);
        sign[2] = (inv_direction[2] < 0);
    }
    Ray(const Ray &r) {
        origin = r.origin;
        direction = r.direction;
        inv_direction = r.inv_direction;
        sign[0] = r.sign[0]; sign[1] = r.sign[1]; sign[2] = r.sign[2];
    }
    const Ray &operator=(const Ray& a) = delete;

    Vector_t origin;
    Vector_t direction;
    Vector_t inv_direction;
    int sign[3];
};


/*
 * Axis-aligned bounding box class, for use with the optimized ray-box
 * intersection test described in:
 *
 *      Amy Williams, Steve Barrus, R. Keith Morley, and Peter Shirley
 *      "An Efficient and Robust Ray-Box Intersection Algorithm"
 *      Journal of graphics tools, 10(1):49-54, 2005
 *
 */

class Voxel {
public:
    Voxel () { }
    Voxel (const Vector_t &min, const Vector_t &max) {
        pts[0] = min;
        pts[1] = max;
    }
    inline void scale (
        const Vector_t& scale
        ) {
        pts[0][0] *= scale[0];
        pts[0][1] *= scale[1];
        pts[0][2] *= scale[2];
        pts[1][0] *= scale[0];
        pts[1][1] *= scale[1];
        pts[1][2] *= scale[2];
    }

    // (t0, t1) is the interval for valid hits
    bool intersect (
        const Ray& r,
        double& tmin,       // tmin and tmax are unchanged, if there is
        double& tmax        // no intersection
        ) const {
        double tmin_ = (pts[r.sign[0]][0]   - r.origin[0]) * r.inv_direction[0];
        double tmax_ = (pts[1-r.sign[0]][0] - r.origin[0]) * r.inv_direction[0];
        const double tymin = (pts[r.sign[1]][1]   - r.origin[1]) * r.inv_direction[1];
        const double tymax = (pts[1-r.sign[1]][1] - r.origin[1]) * r.inv_direction[1];
        if ( cmp::gt(tmin_, tymax) || cmp::gt(tymin, tmax_) )
            return 0;       // no intersection
        if (cmp::gt(tymin, tmin_))
            tmin_ = tymin;
        if (cmp::lt(tymax, tmax_))
            tmax_ = tymax;
        const double tzmin = (pts[r.sign[2]][2]   - r.origin[2]) * r.inv_direction[2];
        const double tzmax = (pts[1-r.sign[2]][2] - r.origin[2]) * r.inv_direction[2];
        if ( cmp::gt(tmin_, tzmax) || cmp::gt(tzmin, tmax_) )
            return 0;       // no intersection
        if (cmp::gt(tzmin, tmin_))
            tmin_ = tzmin;
        tmin = tmin_;
        if (cmp::lt(tzmax, tmax_))
            tmax_ = tzmax;
        tmax = tmax_;
        return cmp::ge_zero(tmax);
    }

    inline bool intersect (
        const Ray& r
        ) const {
        double tmin = 0.0;
        double tmax = 0.0;
        return intersect(r, tmin, tmax);
    }

    inline int intersect (
        const Triangle& t
        ) const {
        Voxel v_ = *this;
        Triangle t_ = t;
        const Vector_t scaleby = 1.0 / v_.extent();
        v_.scale (scaleby);
        t_.scale (scaleby , v_.pts[0] + 0.5);
        return triangle_intersects_cube (t_);
    }

    inline Vector_t extent () const {
        return (pts[1] - pts[0]);
    }

    inline bool isInside  (
        const Vector_t& P
        ) const {
        return (
                cmp::ge(P[0], pts[0][0])
                && cmp::ge(P[1], pts[0][1])
                && cmp::ge(P[2], pts[0][2])
                && cmp::le(P[0], pts[1][0])
                && cmp::le(P[1], pts[1][1])
                && cmp::le(P[2], pts[1][2]));
    }

    Vector_t pts[2];
};

static inline Vector_t normalVector (
    const Vector_t& A,
    const Vector_t& B,
    const Vector_t& C
    ) {
    const Vector_t N = cross (B - A, C - A);
    const double magnitude = std::sqrt (SQR (N (0)) + SQR (N (1)) + SQR (N (2)));
    PAssert (cmp::gt_zero(magnitude)); // in case we have degenerated triangles
    return N / magnitude;
}

// Calculate the area of triangle given by id.
static inline double computeArea (
    const Vector_t& A,
    const Vector_t& B,
    const Vector_t& C
    ) {
    const Vector_t AB = A - B;
    const Vector_t AC = C - A;
    return(0.5 * std::sqrt (dot (AB, AB) * dot (AC, AC) - dot (AB, AC) * dot (AB, AC)));
}


/*
  ____                           _
 / ___| ___  ___  _ __ ___   ___| |_ _ __ _   _
| |  _ / _ \/ _ \| '_ ` _ \ / _ \ __| '__| | | |
| |_| |  __/ (_) | | | | | |  __/ |_| |  | |_| |
 \____|\___|\___/|_| |_| |_|\___|\__|_|   \__, |
                                          |___/
*/

BoundaryGeometry::BoundaryGeometry() :
    Definition (
        SIZE, "GEOMETRY", "The \"GEOMETRY\" statement defines the beam pipe geometry.") {

    itsAttr[FGEOM] = Attributes::makeString
        ("FGEOM",
         "Specifies the geometry file [H5hut]",
         "");

    itsAttr[TOPO] = Attributes::makeString
        ("TOPO",
         "RECTANGULAR, BOXCORNER, ELLIPTIC if FGEOM is selected topo is over-written ",
         "ELLIPTIC");

    itsAttr[LENGTH] = Attributes::makeReal
        ("LENGTH",
         "Specifies the length of a tube shaped elliptic beam pipe [m]",
         1.0);

    itsAttr[S] = Attributes::makeReal
        ("S",
         "Specifies the start of a tube shaped elliptic beam pipe [m]",
         0.0);

    itsAttr[A] = Attributes::makeReal
        ("A",
         "Specifies the major semi-axis of a tube shaped elliptic beam pipe [m]",
         0.025);

    itsAttr[B] = Attributes::makeReal
        ("B",
         "Specifies the major semi-axis of a tube shaped elliptic beam pipe [m]",
         0.025);

    itsAttr[L1] = Attributes::makeReal
        ("L1",
         "In case of BOXCORNER Specifies first part with height == B [m]",
         0.5);

    itsAttr[L2] = Attributes::makeReal
        ("L2",
         "In case of BOXCORNER Specifies first second with height == B-C [m]",
         0.2);

    itsAttr[C] = Attributes::makeReal
        ("C",
         "In case of BOXCORNER Specifies height of corner C [m]",
         0.01);

    itsAttr[XYZSCALE] = Attributes::makeReal
        ("XYZSCALE",
         "Multiplicative scaling factor for coordinates ",
         1.0);

    itsAttr[XSCALE] = Attributes::makeReal
        ("XSCALE",
         "Multiplicative scaling factor for X coordinates ",
         1.0);

    itsAttr[YSCALE] = Attributes::makeReal
        ("YSCALE",
         "Multiplicative scaling factor for Y coordinates ",
         1.0);

    itsAttr[ZSCALE] = Attributes::makeReal
        ("ZSCALE",
         "Multiplicative scaling factor for Z coordinates ",
         1.0);

    itsAttr[ZSHIFT] = Attributes::makeReal
        ("ZSHIFT",
         "Shift in z direction",
         0.0);

    itsAttr[INSIDEPOINT] = Attributes::makeRealArray
        ("INSIDEPOINT", "A point inside the geometry");

    registerOwnership(AttributeHandler::STATEMENT);

    BoundaryGeometry* defGeometry = clone ("UNNAMED_GEOMETRY");
    defGeometry->builtin = true;

    Tinitialize_m =   IpplTimings::getTimer ("Initialize geometry");
    TisInside_m =     IpplTimings::getTimer ("Inside test");
    TfastIsInside_m = IpplTimings::getTimer ("Fast inside test");
    TRayTrace_m =     IpplTimings::getTimer ("Ray tracing");
    TPartInside_m =   IpplTimings::getTimer ("Particle Inside");

    h5FileName_m = Attributes::getString (itsAttr[FGEOM]);
    try {
        defGeometry->update ();
        OpalData::getInstance ()->define (defGeometry);
    } catch (...) {
        delete defGeometry;
    }
    gsl_rng_env_setup();
    randGen_m = gsl_rng_alloc(gsl_rng_default);

    if (!h5FileName_m.empty ())
        initialize ();

}

BoundaryGeometry::BoundaryGeometry(
    const std::string& name,
    BoundaryGeometry* parent
    ) : Definition (name, parent) {
    gsl_rng_env_setup();
    randGen_m = gsl_rng_alloc(gsl_rng_default);

    Tinitialize_m =   IpplTimings::getTimer ("Initialize geometry");
    TisInside_m =     IpplTimings::getTimer ("Inside test");
    TfastIsInside_m = IpplTimings::getTimer ("Fast inside test");
    TRayTrace_m =     IpplTimings::getTimer ("Ray tracing");
    TPartInside_m =   IpplTimings::getTimer ("Particle Inside");

    h5FileName_m = Attributes::getString (itsAttr[FGEOM]);
    if (!h5FileName_m.empty ())
        initialize ();
 }

BoundaryGeometry::~BoundaryGeometry() {
    gsl_rng_free(randGen_m);
}

bool BoundaryGeometry::canReplaceBy (Object* object) {
    // Can replace only by another GEOMETRY.
    return dynamic_cast<BGeometryBase*>(object) != 0;
}

BoundaryGeometry* BoundaryGeometry::clone (const std::string& name) {
    return new BoundaryGeometry (name, this);
}

void BoundaryGeometry::update () {
    if (getOpalName ().empty ()) setOpalName ("UNNAMED_GEOMETRY");
}


void BoundaryGeometry::execute () {
    update ();
    Tinitialize_m =   IpplTimings::getTimer ("Initialize geometry");
    TisInside_m =     IpplTimings::getTimer ("Inside test");
    TfastIsInside_m = IpplTimings::getTimer ("Fast inside test");
    TRayTrace_m =     IpplTimings::getTimer ("Ray tracing");
    TPartInside_m =   IpplTimings::getTimer ("Particle Inside");
}

BoundaryGeometry* BoundaryGeometry::find (const std::string& name) {
    BoundaryGeometry* geom = dynamic_cast<BoundaryGeometry*>(
        OpalData::getInstance ()->find (name));

    if (geom == 0)
        throw OpalException ("BoundaryGeometry::find()", "Geometry \""
                             + name + "\" not found.");
    return geom;
}

void BoundaryGeometry::updateElement (ElementBase* /*element*/) {
}

int
BoundaryGeometry::intersectTriangleVoxel (
    const int triangle_id,
    const int i,
    const int j,
    const int k
    ) {
    const Triangle t(
        getPoint (triangle_id, 1),
        getPoint (triangle_id, 2),
        getPoint (triangle_id, 3)
        );

    const Vector_t P(
        i * voxelMesh_m.sizeOfVoxel [0] + voxelMesh_m.minExtent[0],
        j * voxelMesh_m.sizeOfVoxel [1] + voxelMesh_m.minExtent[1],
        k * voxelMesh_m.sizeOfVoxel [2] + voxelMesh_m.minExtent[2]
        );

    Voxel v(P, P+voxelMesh_m.sizeOfVoxel);

    return v.intersect (t);
}

/*
  Find the 3D intersection of a line segment, ray or line with a triangle.

  Input:
        kind: type of test: SEGMENT, RAY or LINE
        P0, P0: defining
            a line segment from P0 to P1 or
            a ray starting at P0 with directional vector P1-P0 or
            a line through P0 and P1
        V0, V1, V2: the triangle vertices

  Output:
        I: intersection point (when it exists)

  Return values for line segment and ray test :
        -1 = triangle is degenerated (a segment or point)
        0 =  disjoint (no intersect)
        1 =  are in the same plane
        2 =  intersect in unique point I1

  Return values for line intersection test :
        -1: triangle is degenerated (a segment or point)
        0:  disjoint (no intersect)
        1:  are in the same plane
        2:  intersect in unique point I1, with r < 0.0
        3:  intersect in unique point I1, with 0.0 <= r <= 1.0
        4:  intersect in unique point I1, with 1.0 < r

  For algorithm and implementation see:
  http://geomalgorithms.com/a06-_intersect-2.html

  Copyright 2001 softSurfer, 2012 Dan Sunday
  This code may be freely used and modified for any purpose
  providing that this copyright notice is included with it.
  SoftSurfer makes no warranty for this code, and cannot be held
  liable for any real or imagined damage resulting from its use.
  Users of this code must verify correctness for their application.
 */

int
BoundaryGeometry::intersectLineTriangle (
    const enum INTERSECTION_TESTS kind,
    const Vector_t& P0,
    const Vector_t& P1,
    const int triangle_id,
    Vector_t& I
    ) {
    const Vector_t V0 = getPoint (triangle_id, 1);
    const Vector_t V1 = getPoint (triangle_id, 2);
    const Vector_t V2 = getPoint (triangle_id, 3);

    // get triangle edge vectors and plane normal
    const Vector_t u = V1 - V0;         // triangle vectors
    const Vector_t v = V2 - V0;
    const Vector_t n = cross (u, v);
    if (n == (Vector_t)0)               // triangle is degenerate
        return -1;                      // do not deal with this case

    const Vector_t dir = P1 - P0;       // ray direction vector
    const Vector_t w0 = P0 - V0;
    const double a = -dot(n,w0);
    const double b = dot(n,dir);
    if (cmp::eq_zero(b)) {              // ray is  parallel to triangle plane
        if (cmp::eq_zero(a)) {          // ray lies in triangle plane
            return 1;
        } else {                        // ray disjoint from plane
            return 0;
        }
    }

    // get intersect point of ray with triangle plane