/*************************************************************************
 *                                                                       *
 * Vega FEM Simulation Library Version 4.0                               *
 *                                                                       *
 * "mesh" library , Copyright (C) 2018 USC                               *
 * All rights reserved.                                                  *
 *                                                                       *
 * Code authors: Yijing Li, Jernej Barbic                                *
 * http://www.jernejbarbic.com/vega                                      *
 *                                                                       *
 * Research: Jernej Barbic, Hongyi Xu, Yijing Li,                        *
 *           Danyong Zhao, Bohan Wang,                                   *
 *           Fun Shing Sin, Daniel Schroeder,                            *
 *           Doug L. James, Jovan Popovic                                *
 *                                                                       *
 * Funding: National Science Foundation, Link Foundation,                *
 *          Singapore-MIT GAMBIT Game Lab,                               *
 *          Zumberge Research and Innovation Fund at USC,                *
 *          Sloan Foundation, Okawa Foundation,                          *
 *          USC Annenberg Foundation                                     *
 *                                                                       *
 * This file is modified from Robert Bridson's Tunicate library:         *
 * https://www.cs.ubc.ca/~rbridson/tunicate/                             *
 * You can redistribute our changes and/or                               *
 * modify them under the terms of the BSD-style license that is          *
 * included with this library in the file LICENSE.txt                    *
 *                                                                       *
 * This library is distributed in the hope that it will be useful,       *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the file     *
 * LICENSE.TXT for more details.                                         *
 *                                                                       *
 *************************************************************************/

// Released into the public domain by Robert Bridson, 2009.

#include <cassert>
#include <cmath>

namespace vega 
{

// Modified Bridson's code to use Shewchuk's orient2/3d because the later is faster
double orient2d(const double pa[2], const double pb[2], const double pc[2]);
double orient3d(const double pa[3], const double pb[3], const double pc[3], const double pd[3]);

//==============================================================================
static bool
same_sign(double a, double b)
{
   return (a<=0 && b<=0) || (a>=0 && b>=0);
}

//==============================================================================
int
simplex_intersection1d(int k,
                       const double* x0,
                       const double* x1,
                       const double* x2,
                       double* alpha0,
                       double* alpha1,
                       double* alpha2)
{
   assert(1<=k && k<=2);
   assert(alpha0 && alpha1 && alpha2);
   if(k==1)
   {
      if(x1[0]<x2[0])
      {
         if(x0[0]<x1[0]) return 0;
         else if(x0[0]>x2[0]) return 0;
         *alpha0=1;
         *alpha1=(x2[0]-x0[0])/(x2[0]-x1[0]);
         *alpha2=(x0[0]-x1[0])/(x2[0]-x1[0]);
         return 1;
      }else if(x1[0]>x2[0])
      {
         if(x0[0]<x2[0]) return 0;
         else if(x0[0]>x1[0]) return 0;
         *alpha0=1;
         *alpha1=(x2[0]-x0[0])/(x2[0]-x1[0]);
         *alpha2=(x0[0]-x1[0])/(x2[0]-x1[0]);
         return 1;
      }else // x1[0]==x2[0]
      {
         if(x0[0]!=x1[0]) return 0;
         *alpha0=1;
         *alpha1=0.5;
         *alpha2=0.5;
         return 1;
      }
   }else
      return simplex_intersection1d(1, x2, x1, 0, alpha2, alpha1, alpha0);
}

//==============================================================================
// degenerate test in 2d - assumes three points lie on the same line
static int
simplex_intersection2d(int k,
                       const double* x0,
                       const double* x1,
                       const double* x2,
                       double* alpha0,
                       double* alpha1,
                       double* alpha2)
{
   assert(k==1);
   // try projecting each coordinate out in turn
   double ax0, ax1, ax2;
   if(!simplex_intersection1d(1, x0+1, x1+1, x2+1, &ax0, &ax1, &ax2)) return 0;
   double ay0, ay1, ay2;
   if(!simplex_intersection1d(1, x0, x1, x2, &ay0, &ay1, &ay2)) return 0;
   // decide which solution is more accurate for barycentric coordinates
   double checkx=std::fabs(-ax0*x0[0]+ax1*x1[0]+ax2*x2[0])
                +std::fabs(-ax0*x0[1]+ax1*x1[1]+ax2*x2[1]);
   double checky=std::fabs(-ay0*x0[0]+ay1*x1[0]+ay2*x2[0])
                +std::fabs(-ay0*x0[1]+ay1*x1[1]+ay2*x2[1]);
   if(checkx<=checky)
   {
      *alpha0=ax0;
      *alpha1=ax1;
      *alpha2=ax2;
   }else
   {
      *alpha0=ay0;
      *alpha1=ay1;
      *alpha2=ay2;
   }
   return 1;
}

//==============================================================================
int
simplex_intersection2d(int k,
                       const double* x0,
                       const double* x1,
                       const double* x2,
                       const double* x3,
                       double* alpha0,
                       double* alpha1,
                       double* alpha2,
                       double* alpha3)
{
   assert(1<=k && k<=3);
   double sum1, sum2;
   switch(k)
   {
      case 1: // point vs. triangle
         *alpha1=-orient2d(x0, x2, x3);
         *alpha2= orient2d(x0, x1, x3);
         if(!same_sign(*alpha1, *alpha2)) return 0;
         *alpha3=-orient2d(x0, x1, x2);
         if(!same_sign(*alpha1, *alpha3)) return 0;
         if(!same_sign(*alpha2, *alpha3)) return 0;
         sum2=*alpha1+*alpha2+*alpha3;
         if(sum2) // triangle not degenerate?
         {
            *alpha0=1;
            *alpha1/=sum2;
            *alpha2/=sum2;
            *alpha3/=sum2;
            return 1;
         }else // triangle is degenerate and point lies on same line
         {
            if(simplex_intersection2d(1, x0, x1, x2, alpha0, alpha1, alpha2))
            {
               *alpha3=0;
               return 1;
            }
            if(simplex_intersection2d(1, x0, x1, x3, alpha0, alpha1, alpha3))
            {
               *alpha2=0;
               return 1;
            }
            if(simplex_intersection2d(1, x0, x2, x3, alpha0, alpha2, alpha3))
            {
               *alpha1=0;
               return 1;
            }
            return 0;
         }

      case 2: // segment vs. segment
         *alpha0= orient2d(x1, x2, x3);
         *alpha1=-orient2d(x0, x2, x3);
         if(!same_sign(*alpha0, *alpha1)) return 0;
         *alpha2= orient2d(x0, x1, x3);
         *alpha3=-orient2d(x0, x1, x2);
         if(!same_sign(*alpha2, *alpha3)) return 0;
         sum1=*alpha0+*alpha1;
         sum2=*alpha2+*alpha3;
         if(sum1 && sum2)
         {
            *alpha0/=sum1;
            *alpha1/=sum1;
            *alpha2/=sum2;
            *alpha3/=sum2;
            return 1;
         }else // degenerate: segments lie on the same line
         {
            if(simplex_intersection2d(1, x0, x2, x3, alpha0, alpha2, alpha3))
            {
               *alpha1=0;
               return 1;
            }
            if(simplex_intersection2d(1, x1, x2, x3, alpha1, alpha2, alpha3))
            {
               *alpha0=0;
               return 1;
            }
            if(simplex_intersection2d(1, x2, x0, x1, alpha2, alpha0, alpha1))
            {
               *alpha3=0;
               return 1;
            }
            if(simplex_intersection2d(1, x3, x0, x1, alpha3, alpha0, alpha1))
            {
               *alpha2=0;
               return 1;
            }
            return 0;
         }
      case 3: // triangle vs. point
         return simplex_intersection2d(1, x3, x2, x1, x0,
                                          alpha3, alpha2, alpha1, alpha0);
      default:
         return -1; // should never get here
   }
}

//==============================================================================
// degenerate test in 3d - assumes four points lie on the same plane
int
simplex_intersection3d(int k,
                       const double* x0,
                       const double* x1,
                       const double* x2,
                       const double* x3,
                       double* alpha0,
                       double* alpha1,
                       double* alpha2,
                       double* alpha3)
{
   assert(k<=2);
   // try projecting each coordinate out in turn
   double ax0, ax1, ax2, ax3;
   if(!simplex_intersection2d(k, x0+1, x1+1, x2+1, x3+1, &ax0, &ax1, &ax2,&ax3))
      return 0;
   double ay0, ay1, ay2, ay3;
   double p0[2]={x0[0], x0[2]}, p1[2]={x1[0], x1[2]},
          p2[2]={x2[0], x2[2]}, p3[2]={x3[0], x3[2]};
   if(!simplex_intersection2d(k, p0, p1, p2, p3, &ay0, &ay1, &ay2, &ay3))
      return 0;
   double az0, az1, az2, az3;
   if(!simplex_intersection2d(k, x0, x1, x2, x3, &az0, &az1, &az2, &az3))
      return 0;
   // decide which solution is more accurate for barycentric coordinates
   double checkx, checky, checkz;
   if(k==1)
   {
      checkx=std::fabs(-ax0*x0[0]+ax1*x1[0]+ax2*x2[0]+ax3*x3[0])
            +std::fabs(-ax0*x0[1]+ax1*x1[1]+ax2*x2[1]+ax3*x3[1])
            +std::fabs(-ax0*x0[2]+ax1*x1[2]+ax2*x2[2]+ax3*x3[2]);
      checky=std::fabs(-ay0*x0[0]+ay1*x1[0]+ay2*x2[0]+ay3*x3[0])
            +std::fabs(-ay0*x0[1]+ay1*x1[1]+ay2*x2[1]+ay3*x3[1])
            +std::fabs(-ay0*x0[2]+ay1*x1[2]+ay2*x2[2]+ay3*x3[2]);
      checkz=std::fabs(-az0*x0[0]+az1*x1[0]+az2*x2[0]+az3*x3[0])
            +std::fabs(-az0*x0[1]+az1*x1[1]+az2*x2[1]+az3*x3[1])
            +std::fabs(-az0*x0[2]+az1*x1[2]+az2*x2[2]+az3*x3[2]);
   }else
   {
      checkx=std::fabs(-ax0*x0[0]-ax1*x1[0]+ax2*x2[0]+ax3*x3[0])
            +std::fabs(-ax0*x0[1]-ax1*x1[1]+ax2*x2[1]+ax3*x3[1])
            +std::fabs(-ax0*x0[2]-ax1*x1[2]+ax2*x2[2]+ax3*x3[2]);
      checky=std::fabs(-ay0*x0[0]-ay1*x1[0]+ay2*x2[0]+ay3*x3[0])
            +std::fabs(-ay0*x0[1]-ay1*x1[1]+ay2*x2[1]+ay3*x3[1])
            +std::fabs(-ay0*x0[2]-ay1*x1[2]+ay2*x2[2]+ay3*x3[2]);
      checkz=std::fabs(-az0*x0[0]-az1*x1[0]+az2*x2[0]+az3*x3[0])
            +std::fabs(-az0*x0[1]-az1*x1[1]+az2*x2[1]+az3*x3[1])
            +std::fabs(-az0*x0[2]-az1*x1[2]+az2*x2[2]+az3*x3[2]);
   }
   if(checkx<=checky && checkx<=checkz)
   {
      *alpha0=ax0;
      *alpha1=ax1;
      *alpha2=ax2;
      *alpha2=ax3;
   }else if(checky<=checkz)
   {
      *alpha0=ay0;
      *alpha1=ay1;
      *alpha2=ay2;
      *alpha2=ay3;
   }else
   {
      *alpha0=az0;
      *alpha1=az1;
      *alpha2=az2;
      *alpha2=az3;
   }
   return 1;
}

//==============================================================================
int
simplex_intersection3d(int k,
                       const double* x0,
                       const double* x1,
                       const double* x2,
                       const double* x3,
                       const double* x4,
                       double* alpha0,
                       double* alpha1,
                       double* alpha2,
                       double* alpha3,
                       double* alpha4)
{
   assert(1<=k && k<=4);
   double sum1, sum2;
   switch(k)
   {
      case 1: // point vs. tetrahedron
         *alpha1=-orient3d(x0, x2, x3, x4);
         *alpha2= orient3d(x0, x1, x3, x4);
         if(!same_sign(*alpha1, *alpha2)) return 0;
         *alpha3=-orient3d(x0, x1, x2, x4);
         if(!same_sign(*alpha1, *alpha3)) return 0;
         if(!same_sign(*alpha2, *alpha3)) return 0;
         *alpha4= orient3d(x0, x1, x2, x3);
         if(!same_sign(*alpha1, *alpha4)) return 0;
         if(!same_sign(*alpha2, *alpha4)) return 0;
         if(!same_sign(*alpha3, *alpha4)) return 0;
         *alpha0=1;
         sum2=*alpha1+*alpha2+*alpha3+*alpha4;
         if(sum2)
         {
            *alpha1/=sum2;
            *alpha2/=sum2;
            *alpha3/=sum2;
            *alpha4/=sum2;
            return 1;
         }else // degenerate: point and tetrahedron in same plane
         {
            if(simplex_intersection3d(1, x0, x2, x3, x4,
                                         alpha0, alpha2, alpha3, alpha4))
            {
               *alpha1=0;
               return 1;
            }
            if(simplex_intersection3d(1, x0, x1, x3, x4,
                                         alpha0, alpha1, alpha3, alpha4))
            {
               *alpha2=0;
               return 1;
            }
            if(simplex_intersection3d(1, x0, x1, x2, x4,
                                         alpha0, alpha1, alpha2, alpha4))
            {
               *alpha3=0;
               return 1;
            }
            if(simplex_intersection3d(1, x0, x1, x2, x3,
                                         alpha0, alpha1, alpha2, alpha3))
            {
               *alpha4=0;
               return 1;
            }
            return 0;
         }

      case 2: // segment (x0, x1) vs. triangle (x2, x3, x4)
         *alpha0= orient3d(x1, x2, x3, x4);
         *alpha1=-orient3d(x0, x2, x3, x4);
         if(!same_sign(*alpha0, *alpha1)) return 0;
         *alpha2= orient3d(x0, x1, x3, x4);
         *alpha3=-orient3d(x0, x1, x2, x4);
         if(!same_sign(*alpha2, *alpha3)) return 0;
         *alpha4= orient3d(x0, x1, x2, x3);
         if(!same_sign(*alpha2, *alpha4)) return 0;
         if(!same_sign(*alpha3, *alpha4)) return 0;
         sum1=*alpha0+*alpha1;
         sum2=*alpha2+*alpha3+*alpha4;
         if(sum1 && sum2)
         {
            *alpha0/=sum1;
            *alpha1/=sum1;
            *alpha2/=sum2;
            *alpha3/=sum2;
            *alpha4/=sum2;
            return 1;
         }else // degenerate: segment and triangle in same plane
         {
            if(simplex_intersection3d(1, x1, x2, x3, x4,
                                         alpha1, alpha2, alpha3, alpha4))
            {
               *alpha0=0;
               return 1;
            }
            if(simplex_intersection3d(1, x0, x2, x3, x4,
                                         alpha0, alpha2, alpha3, alpha4))
            {
               *alpha1=0;
               return 1;
            }
            if(simplex_intersection3d(2, x0, x1, x3, x4,
                                         alpha0, alpha1, alpha3, alpha4))
            {
               *alpha2=0;
               return 1;
            }
            if(simplex_intersection3d(2, x0, x1, x2, x4,
                                         alpha0, alpha1, alpha2, alpha4))
            {
               *alpha3=0;
               return 1;
            }
            if(simplex_intersection3d(2, x0, x1, x2, x3,
                                         alpha0, alpha1, alpha2, alpha3))
            {
               *alpha4=0;
               return 1;
            }
            return 0;
         }

      case 3: // triangle vs. segment
      case 4: // tetrahedron vs. point
         return simplex_intersection3d(5-k, x4, x3, x2, x1, x0,
                                        alpha4, alpha3, alpha2, alpha1, alpha0);
      default:
         return -1; // should never get here
   }
}

// Currently we don't need 3d+time tests, so I commented out Bridson's code below
/*
//==============================================================================
// degenerate test in 3d+time - assumes five points lie on the same hyper-plane
static int
simplex_intersection_time3d(int k,
                            const double* x0, int time0,
                            const double* x1, int time1,
                            const double* x2, int time2,
                            const double* x3, int time3,
                            const double* x4, int time4,
                            double* alpha0,
                            double* alpha1,
                            double* alpha2,
                            double* alpha3,
                            double* alpha4)
{
   assert(k<=2);
   assert(time0==0 || time0==1);
   assert(time1==0 || time1==1);
   assert(time2==0 || time2==1);
   assert(time3==0 || time3==1);
   assert(time4==0 || time4==1);
   // try projecting each coordinate out in turn
   double ax0, ax1, ax2, ax3, ax4;
   double r0[3]={x0[0], x0[2], time0}, r1[3]={x1[0], x1[2], time1},
          r2[3]={x2[0], x2[2], time2}, r3[3]={x3[0], x3[2], time3},
          r4[3]={x4[0], x4[2], time4};
   if(!simplex_intersection3d(k, r0, r1, r2, r3, r4,
                                 &ax0, &ax1, &ax2, &ax3, &ax4)) return 0;
   double ay0, ay1, ay2, ay3, ay4;
   double p0[3]={x0[0], x0[2], time0}, p1[3]={x1[0], x1[2], time1},
          p2[3]={x2[0], x2[2], time2}, p3[3]={x3[0], x3[2], time3},
          p4[3]={x4[0], x4[2], time4};
   if(!simplex_intersection3d(k, p0, p1, p2, p3, p4,
                                 &ay0, &ay1, &ay2, &ay3, &ay4)) return 0;
   double az0, az1, az2, az3, az4;
   double q0[3]={x0[0], x0[1], time0}, q1[3]={x1[0], x1[1], time1},
          q2[3]={x2[0], x2[1], time2}, q3[3]={x3[0], x3[1], time3},
          q4[3]={x4[0], x4[1], time4};
   if(!simplex_intersection3d(k, q0, q1, q2, q3, q4,
                                 &az0, &az1, &az2, &az3, &az4)) return 0;
   double at0, at1, at2, at3, at4;
   if(!simplex_intersection3d(k, x0, x1, x2, x3, x4,
                                 &at0, &at1, &at2, &at3, &at4)) return 0;
   // decide which solution is more accurate for barycentric coordinates
   double checkx, checky, checkz, checkt;
   if(k==1)
   {
      checkx=std::fabs(-ax0*x0[0]+ax1*x1[0]+ax2*x2[0]+ax3*x3[0]+ax4*x4[0])
            +std::fabs(-ax0*x0[1]+ax1*x1[1]+ax2*x2[1]+ax3*x3[1]+ax4*x4[1])
            +std::fabs(-ax0*x0[2]+ax1*x1[2]+ax2*x2[2]+ax3*x3[2]+ax4*x4[2])
            +std::fabs(-ax0*time0+ax1*time1+ax2*time2+ax3*time3+ax4*time4);
      checky=std::fabs(-ay0*x0[0]+ay1*x1[0]+ay2*x2[0]+ay3*x3[0]+ay4*x4[0])
            +std::fabs(-ay0*x0[1]+ay1*x1[1]+ay2*x2[1]+ay3*x3[1]+ay4*x4[1])
            +std::fabs(-ay0*x0[2]+ay1*x1[2]+ay2*x2[2]+ay3*x3[2]+ay4*x4[2])
            +std::fabs(-ay0*time0+ay1*time1+ay2*time2+ay3*time3+ay4*time4);
      checkz=std::fabs(-az0*x0[0]+az1*x1[0]+az2*x2[0]+az3*x3[0]+az4*x4[0])
            +std::fabs(-az0*x0[1]+az1*x1[1]+az2*x2[1]+az3*x3[1]+az4*x4[1])
            +std::fabs(-az0*x0[2]+az1*x1[2]+az2*x2[2]+az3*x3[2]+az4*x4[2])
            +std::fabs(-az0*time0+az1*time1+az2*time2+az3*time3+az4*time4);
      checkt=std::fabs(-at0*x0[0]+at1*x1[0]+at2*x2[0]+at3*x3[0]+at4*x4[0])
            +std::fabs(-at0*x0[1]+at1*x1[1]+at2*x2[1]+at3*x3[1]+at4*x4[1])
            +std::fabs(-at0*x0[2]+at1*x1[2]+at2*x2[2]+at3*x3[2]+at4*x4[2])
            +std::fabs(-at0*time0+at1*time1+at2*time2+at3*time3+at4*time4);
   }else
   {
      checkx=std::fabs(-ax0*x0[0]-ax1*x1[0]+ax2*x2[0]+ax3*x3[0]+ax4*x4[0])
            +std::fabs(-ax0*x0[1]-ax1*x1[1]+ax2*x2[1]+ax3*x3[1]+ax4*x4[1])
            +std::fabs(-ax0*x0[2]-ax1*x1[2]+ax2*x2[2]+ax3*x3[2]+ax4*x4[2])
            +std::fabs(-ax0*time0-ax1*time1+ax2*time2+ax3*time3+ax4*time4);
      checky=std::fabs(-ay0*x0[0]-ay1*x1[0]+ay2*x2[0]+ay3*x3[0]+ay4*x4[0])
            +std::fabs(-ay0*x0[1]-ay1*x1[1]+ay2*x2[1]+ay3*x3[1]+ay4*x4[1])
            +std::fabs(-ay0*x0[2]-ay1*x1[2]+ay2*x2[2]+ay3*x3[2]+ay4*x4[2])
            +std::fabs(-ay0*time0-ay1*time1+ay2*time2+ay3*time3+ay4*time4);
      checkz=std::fabs(-az0*x0[0]-az1*x1[0]+az2*x2[0]+az3*x3[0]+az4*x4[0])
            +std::fabs(-az0*x0[1]-az1*x1[1]+az2*x2[1]+az3*x3[1]+az4*x4[1])
            +std::fabs(-az0*x0[2]-az1*x1[2]+az2*x2[2]+az3*x3[2]+az4*x4[2])
            +std::fabs(-az0*time0-az1*time1+az2*time2+az3*time3+az4*time4);
      checkt=std::fabs(-at0*x0[0]-at1*x1[0]+at2*x2[0]+at3*x3[0]+at4*x4[0])
            +std::fabs(-at0*x0[1]-at1*x1[1]+at2*x2[1]+at3*x3[1]+at4*x4[1])
            +std::fabs(-at0*x0[2]-at1*x1[2]+at2*x2[2]+at3*x3[2]+at4*x4[2])
            +std::fabs(-at0*time0-at1*time1+at2*time2+at3*time3+at4*time4);
   }
   if(checkx<=checky && checkx<=checkz && checkx<=checkt)
   {
      *alpha0=ax0;
      *alpha1=ax1;
      *alpha2=ax2;
      *alpha3=ax3;
   }else if(checky<=checkz && checky<=checkt)
   {
      *alpha0=ay0;
      *alpha1=ay1;
      *alpha2=ay2;
      *alpha3=ay3;
   }else if(checkz<=checkt)
   {
      *alpha0=az0;
      *alpha1=az1;
      *alpha2=az2;
      *alpha3=az3;
   }else
   {
      *alpha0=at0;
      *alpha1=at1;
      *alpha2=at2;
      *alpha3=at3;
   }
   return 1;
}

//==============================================================================
int
simplex_intersection_time3d(int k,
                            const double* x0, int t0,
                            const double* x1, int t1,
                            const double* x2, int t2,
                            const double* x3, int t3,
                            const double* x4, int t4,
                            const double* x5, int t5,
                            double* alpha0,
                            double* alpha1,
                            double* alpha2,
                            double* alpha3,
                            double* alpha4,
                            double* alpha5)
{
   assert(1<=k && k<=5);
   assert(t0==0 || t0==1);
   assert(t1==0 || t1==1);
   assert(t2==0 || t2==1);
   assert(t3==0 || t3==1);
   assert(t4==0 || t4==1);
   assert(t5==0 || t5==1);
   double sum1, sum2;
   switch(k)
   {
      case 1: // point vs. pentachoron
         *alpha1=-orientation_time3d(x0, t0, x2, t2, x3, t3, x4, t4, x5, t5);
         *alpha2= orientation_time3d(x0, t0, x1, t1, x3, t3, x4, t4, x5, t5);
         if(!same_sign(*alpha1, *alpha2)) return 0;
         *alpha3=-orientation_time3d(x0, t0, x1, t1, x2, t3, x4, t4, x5, t5);
         if(!same_sign(*alpha1, *alpha3)) return 0;
         *alpha4= orientation_time3d(x0, t0, x1, t1, x2, t2, x3, t3, x5, t5);
         if(!same_sign(*alpha1, *alpha4)) return 0;
         *alpha5=-orientation_time3d(x0, t0, x1, t1, x2, t2, x3, t3, x4, t4);
         if(!same_sign(*alpha1, *alpha5)) return 0;
         sum2=*alpha1+*alpha2+*alpha3+*alpha4+*alpha5;
         if(sum2)
         {
            *alpha0=1;
            *alpha1/=sum2;
            *alpha2/=sum2;
            *alpha3/=sum2;
            *alpha4/=sum2;
            *alpha5/=sum2;
            return 1;
         }else
         {
            if(simplex_intersection_time3d(1, x0, t0, x2, t2, x3, t3, x4, t4,
                               x5, t5, alpha0, alpha2, alpha3, alpha4, alpha5))
                               {
               *alpha1=0;
               return 1;
            }
            if(simplex_intersection_time3d(1, x0, t0, x1, t1, x3, t3, x4, t4,
                               x5, t5, alpha0, alpha1, alpha3, alpha4, alpha5))
                               {
               *alpha2=0;
               return 1;
            }
            if(simplex_intersection_time3d(1, x0, t0, x1, t1, x2, t2, x4, t4,
                               x5, t5, alpha0, alpha1, alpha2, alpha4, alpha5))
                               {
               *alpha3=0;
               return 1;
            }
            if(simplex_intersection_time3d(1, x0, t0, x1, t1, x2, t2, x3, t3,
                               x5, t5, alpha0, alpha1, alpha2, alpha3, alpha5))
                               {
               *alpha4=0;
               return 1;
            }
            if(simplex_intersection_time3d(1, x0, t0, x1, t1, x2, t2, x3, t3,
                               x4, t4, alpha0, alpha1, alpha2, alpha3, alpha4))
                               {
               *alpha5=0;
               return 1;
            }
            return 0;
         }

      case 2: // segment vs. tetrahedron
         *alpha0= orientation_time3d(x1, t1, x2, t2, x3, t3, x4, t4, x5, t5);
         *alpha1=-orientation_time3d(x0, t0, x2, t2, x3, t3, x4, t4, x5, t5);
         if(!same_sign(*alpha0, *alpha1)) return 0;
         *alpha2= orientation_time3d(x0, t0, x1, t1, x3, t3, x4, t4, x5, t5);
         *alpha3=-orientation_time3d(x0, t0, x1, t1, x2, t2, x4, t4, x5, t5);
         if(!same_sign(*alpha2, *alpha3)) return 0;
         *alpha4= orientation_time3d(x0, t0, x1, t1, x2, t2, x3, t3, x5, t5);
         if(!same_sign(*alpha2, *alpha4)) return 0;
         *alpha5=-orientation_time3d(x0, t0, x1, t1, x2, t2, x3, t3, x4, t4);
         if(!same_sign(*alpha2, *alpha5)) return 0;
         sum1=*alpha0+*alpha1;
         sum2=*alpha2+*alpha3+*alpha4+*alpha5;
         if(sum1 && sum2)
         {
            *alpha0/=sum1;
            *alpha1/=sum1;
            *alpha2/=sum2;
            *alpha3/=sum2;
            *alpha4/=sum2;
            *alpha5/=sum2;
            return 1;
         }else
         {
            if(simplex_intersection_time3d(1, x1, t1, x2, t2, x3, t3, x4, t4,
                               x5, t5, alpha1, alpha2, alpha3, alpha4, alpha5))
                               {
               *alpha0=0;
               return 1;
            }
            if(simplex_intersection_time3d(1, x0, t0, x2, t2, x3, t3, x4, t4,
                               x5, t5, alpha0, alpha2, alpha3, alpha4, alpha5))
                               {
               *alpha1=0;
               return 1;
            }
            if(simplex_intersection_time3d(2, x0, t0, x1, t1, x3, t3, x4, t4,
                               x5, t5, alpha0, alpha1, alpha3, alpha4, alpha5))
                               {
               *alpha2=0;
               return 1;
            }
            if(simplex_intersection_time3d(2, x0, t0, x1, t1, x2, t2, x4, t4,
                               x5, t5, alpha0, alpha1, alpha2, alpha4, alpha5))
                               {
               *alpha3=0;
               return 1;
            }
            if(simplex_intersection_time3d(2, x0, t0, x1, t1, x2, t2, x3, t3,
                               x5, t5, alpha0, alpha1, alpha2, alpha3, alpha5))
                               {
               *alpha4=0;
               return 1;
            }
            if(simplex_intersection_time3d(2, x0, t0, x1, t1, x2, t2, x3, t3,
                               x4, t4, alpha0, alpha1, alpha2, alpha3, alpha4))
                               {
               *alpha5=0;
               return 1;
            }
            return 0;
         }

      case 3: // triangle vs. triangle
         *alpha0= orientation_time3d(x1, t1, x2, t2, x3, t3, x4, t4, x5, t5);
         *alpha1=-orientation_time3d(x0, t0, x2, t2, x3, t3, x4, t4, x5, t5);
         if(!same_sign(*alpha0, *alpha1)) return 0;
         *alpha2= orientation_time3d(x0, t0, x1, t1, x3, t3, x4, t4, x5, t5);
         if(!same_sign(*alpha0, *alpha2)) return 0;
         *alpha3=-orientation_time3d(x0, t0, x1, t1, x2, t2, x4, t4, x5, t5);
         *alpha4= orientation_time3d(x0, t0, x1, t1, x2, t2, x3, t3, x5, t5);
         if(!same_sign(*alpha3, *alpha4)) return 0;
         *alpha5=-orientation_time3d(x0, t0, x1, t1, x2, t2, x3, t3, x4, t4);
         if(!same_sign(*alpha3, *alpha5)) return 0;
         sum1=*alpha0+*alpha1+*alpha2;
         sum2=*alpha3+*alpha4+*alpha5;
         if(sum1 && sum2)
         {
            *alpha0/=sum1;
            *alpha1/=sum1;
            *alpha2/=sum1;
            *alpha3/=sum2;
            *alpha4/=sum2;
            *alpha5/=sum2;
            return 1;
         }else
         {
            if(simplex_intersection_time3d(2, x1, t1, x2, t2, x3, t3, x4, t4,
                               x5, t5, alpha1, alpha2, alpha3, alpha4, alpha5))
                               {
               *alpha0=0;
               return 1;
            }
            if(simplex_intersection_time3d(2, x0, t0, x2, t2, x3, t3, x4, t4,
                               x5, t5, alpha0, alpha2, alpha3, alpha4, alpha5))
                               {
               *alpha1=0;
               return 1;
            }
            if(simplex_intersection_time3d(2, x0, t0, x1, t1, x3, t3, x4, t4,
                               x5, t5, alpha0, alpha1, alpha3, alpha4, alpha5))
                               {
               *alpha2=0;
               return 1;
            }
            if(simplex_intersection_time3d(2, x4, t4, x5, t5, x0, t0, x1, t1,
                               x2, t2, alpha4, alpha5, alpha0, alpha1, alpha2))
                               {
               *alpha3=0;
               return 1;
            }
            if(simplex_intersection_time3d(2, x3, t3, x5, t5, x0, t0, x1, t1,
                               x2, t2, alpha3, alpha5, alpha0, alpha1, alpha2))
                               {
               *alpha4=0;
               return 1;
            }
            if(simplex_intersection_time3d(2, x3, t3, x4, t4, x0, t0, x1, t1,
                               x2, t2, alpha3, alpha4, alpha0, alpha1, alpha2))
                               {
               *alpha5=0;
               return 1;
            }
            return 0;
         }

      case 4: // tetrahedron vs. segment
      case 5: // pentachoron vs. point
         return simplex_intersection_time3d(6-k, x5, t5, x4, t4, x3, t3, x2, t2,
                x1, t1, x0, t0, alpha5, alpha4, alpha3, alpha2, alpha1, alpha0);
      default:
         return -1; // should never get here
   }
}

//==============================================================================
// degenerate test in 4d - assumes five points lie on the same hyper-plane
static int
simplex_intersection4d(int k,
                       const double* x0,
                       const double* x1,
                       const double* x2,
                       const double* x3,
                       const double* x4,
                       double* alpha0,
                       double* alpha1,
                       double* alpha2,
                       double* alpha3,
                       double* alpha4)
{
   assert(k<=2);
   // try projecting each coordinate out in turn
   double ax0, ax1, ax2, ax3, ax4;
   if(!simplex_intersection3d(k, x0+1, x1+1, x2+1, x3+1, x4+1,
                                 &ax0, &ax1, &ax2, &ax3, &ax4)) return 0;
   double ay0, ay1, ay2, ay3, ay4;
   double p0[3]={x0[0], x0[2], x0[3]}, p1[3]={x1[0], x1[2], x1[3]},
          p2[3]={x2[0], x2[2], x2[3]}, p3[3]={x3[0], x3[2], x3[3]},
          p4[3]={x4[0], x4[2], x4[3]};
   if(!simplex_intersection3d(k, p0, p1, p2, p3, p4,
                                 &ay0, &ay1, &ay2, &ay3, &ay4)) return 0;
   double az0, az1, az2, az3, az4;
   double q0[3]={x0[0], x0[1], x0[3]}, q1[3]={x1[0], x1[1], x1[3]},
          q2[3]={x2[0], x2[1], x2[3]}, q3[3]={x3[0], x3[1], x3[3]},
          q4[3]={x4[0], x4[1], x4[3]};
   if(!simplex_intersection3d(k, q0, q1, q2, q3, q4,
                                 &az0, &az1, &az2, &az3, &az4)) return 0;
   double at0, at1, at2, at3, at4;
   if(!simplex_intersection3d(k, x0, x1, x2, x3, x4,
                                 &at0, &at1, &at2, &at3, &at4)) return 0;
   // decide which solution is more accurate for barycentric coordinates
   double checkx, checky, checkz, checkt;
   if(k==1)
   {
      checkx=std::fabs(-ax0*x0[0]+ax1*x1[0]+ax2*x2[0]+ax3*x3[0]+ax4*x4[0])
            +std::fabs(-ax0*x0[1]+ax1*x1[1]+ax2*x2[1]+ax3*x3[1]+ax4*x4[1])
            +std::fabs(-ax0*x0[2]+ax1*x1[2]+ax2*x2[2]+ax3*x3[2]+ax4*x4[2])
            +std::fabs(-ax0*x0[3]+ax1*x1[3]+ax2*x2[3]+ax3*x3[3]+ax4*x4[3]);
      checky=std::fabs(-ay0*x0[0]+ay1*x1[0]+ay2*x2[0]+ay3*x3[0]+ay4*x4[0])
            +std::fabs(-ay0*x0[1]+ay1*x1[1]+ay2*x2[1]+ay3*x3[1]+ay4*x4[1])
            +std::fabs(-ay0*x0[2]+ay1*x1[2]+ay2*x2[2]+ay3*x3[2]+ay4*x4[2])
            +std::fabs(-ay0*x0[3]+ay1*x1[3]+ay2*x2[3]+ay3*x3[3]+ay4*x4[3]);
      checkz=std::fabs(-az0*x0[0]+az1*x1[0]+az2*x2[0]+az3*x3[0]+az4*x4[0])
            +std::fabs(-az0*x0[1]+az1*x1[1]+az2*x2[1]+az3*x3[1]+az4*x4[1])
            +std::fabs(-az0*x0[2]+az1*x1[2]+az2*x2[2]+az3*x3[2]+az4*x4[2])
            +std::fabs(-az0*x0[3]+az1*x1[3]+az2*x2[3]+az3*x3[3]+az4*x4[3]);
      checkt=std::fabs(-at0*x0[0]+at1*x1[0]+at2*x2[0]+at3*x3[0]+at4*x4[0])
            +std::fabs(-at0*x0[1]+at1*x1[1]+at2*x2[1]+at3*x3[1]+at4*x4[1])
            +std::fabs(-at0*x0[2]+at1*x1[2]+at2*x2[2]+at3*x3[2]+at4*x4[2])
            +std::fabs(-at0*x0[3]+at1*x1[3]+at2*x2[3]+at3*x3[3]+at4*x4[3]);
   }else
   {
      checkx=std::fabs(-ax0*x0[0]-ax1*x1[0]+ax2*x2[0]+ax3*x3[0]+ax4*x4[0])
            +std::fabs(-ax0*x0[1]-ax1*x1[1]+ax2*x2[1]+ax3*x3[1]+ax4*x4[1])
            +std::fabs(-ax0*x0[2]-ax1*x1[2]+ax2*x2[2]+ax3*x3[2]+ax4*x4[2])
            +std::fabs(-ax0*x0[3]-ax1*x1[3]+ax2*x2[3]+ax3*x3[3]+ax4*x4[3]);
      checky=std::fabs(-ay0*x0[0]-ay1*x1[0]+ay2*x2[0]+ay3*x3[0]+ay4*x4[0])
            +std::fabs(-ay0*x0[1]-ay1*x1[1]+ay2*x2[1]+ay3*x3[1]+ay4*x4[1])
            +std::fabs(-ay0*x0[2]-ay1*x1[2]+ay2*x2[2]+ay3*x3[2]+ay4*x4[2])
            +std::fabs(-ay0*x0[3]-ay1*x1[3]+ay2*x2[3]+ay3*x3[3]+ay4*x4[3]);
      checkz=std::fabs(-az0*x0[0]-az1*x1[0]+az2*x2[0]+az3*x3[0]+az4*x4[0])
            +std::fabs(-az0*x0[1]-az1*x1[1]+az2*x2[1]+az3*x3[1]+az4*x4[1])
            +std::fabs(-az0*x0[2]-az1*x1[2]+az2*x2[2]+az3*x3[2]+az4*x4[2])
            +std::fabs(-az0*x0[3]-az1*x1[3]+az2*x2[3]+az3*x3[3]+az4*x4[3]);
      checkt=std::fabs(-at0*x0[0]-at1*x1[0]+at2*x2[0]+at3*x3[0]+at4*x4[0])
            +std::fabs(-at0*x0[1]-at1*x1[1]+at2*x2[1]+at3*x3[1]+at4*x4[1])
            +std::fabs(-at0*x0[2]-at1*x1[2]+at2*x2[2]+at3*x3[2]+at4*x4[2])
            +std::fabs(-at0*x0[3]-at1*x1[3]+at2*x2[3]+at3*x3[3]+at4*x4[3]);
   }
   if(checkx<=checky && checkx<=checkz && checkx<=checkt)
   {
      *alpha0=ax0;
      *alpha1=ax1;
      *alpha2=ax2;
      *alpha3=ax3;
   }else if(checky<=checkz && checky<=checkt)
   {
      *alpha0=ay0;
      *alpha1=ay1;
      *alpha2=ay2;
      *alpha3=ay3;
   }else if(checkz<=checkt)
   {
      *alpha0=az0;
      *alpha1=az1;
      *alpha2=az2;
      *alpha3=az3;
   }else
   {
      *alpha0=at0;
      *alpha1=at1;
      *alpha2=at2;
      *alpha3=at3;
   }
   return 1;
}

//==============================================================================
int
simplex_intersection4d(int k,
                       const double* x0,
                       const double* x1,
                       const double* x2,
                       const double* x3,
                       const double* x4,
                       const double* x5,
                       double* alpha0,
                       double* alpha1,
                       double* alpha2,
                       double* alpha3,
                       double* alpha4,
                       double* alpha5)
{
   assert(1<=k && k<=5);
   double sum1, sum2;
   switch(k)
   {
      case 1: // point vs. pentachoron
         *alpha1=-orientation4d(x0, x2, x3, x4, x5);
         *alpha2= orientation4d(x0, x1, x3, x4, x5);
         if(!same_sign(*alpha1, *alpha2)) return 0;
         *alpha3=-orientation4d(x0, x1, x2, x4, x5);
         if(!same_sign(*alpha1, *alpha3)) return 0;
         *alpha4= orientation4d(x0, x1, x2, x3, x5);
         if(!same_sign(*alpha1, *alpha4)) return 0;
         *alpha5=-orientation4d(x0, x1, x2, x3, x4);
         if(!same_sign(*alpha1, *alpha5)) return 0;
         sum2=*alpha1+*alpha2+*alpha3+*alpha4+*alpha5;
         if(sum2)
         {
            *alpha0=1;
            *alpha1/=sum2;
            *alpha2/=sum2;
            *alpha3/=sum2;
            *alpha4/=sum2;
            *alpha5/=sum2;
            return 1;
         }else
         {
            if(simplex_intersection4d(1, x0, x2, x3, x4, x5,
                                       alpha0, alpha2, alpha3, alpha4, alpha5))
                                       {
               *alpha1=0;
               return 1;
            }
            if(simplex_intersection4d(1, x0, x1, x3, x4, x5,
                                       alpha0, alpha1, alpha3, alpha4, alpha5))
                                       {
               *alpha2=0;
               return 1;
            }
            if(simplex_intersection4d(1, x0, x1, x2, x4, x5,
                                       alpha0, alpha1, alpha2, alpha4, alpha5))
                                       {
               *alpha3=0;
               return 1;
            }
            if(simplex_intersection4d(1, x0, x1, x2, x3, x5,
                                       alpha0, alpha1, alpha2, alpha3, alpha5))
                                       {
               *alpha4=0;
               return 1;
            }
            if(simplex_intersection4d(1, x0, x1, x2, x3, x4,
                                       alpha0, alpha1, alpha2, alpha3, alpha4))
                                       {
               *alpha5=0;
               return 1;
            }
            return 0;
         }

      case 2: // segment vs. tetrahedron
         *alpha0= orientation4d(x1, x2, x3, x4, x5);
         *alpha1=-orientation4d(x0, x2, x3, x4, x5);
         if(!same_sign(*alpha0, *alpha1)) return 0;
         *alpha2= orientation4d(x0, x1, x3, x4, x5);
         *alpha3=-orientation4d(x0, x1, x2, x4, x5);
         if(!same_sign(*alpha2, *alpha3)) return 0;
         *alpha4= orientation4d(x0, x1, x2, x3, x5);
         if(!same_sign(*alpha2, *alpha4)) return 0;
         *alpha5=-orientation4d(x0, x1, x2, x3, x4);
         if(!same_sign(*alpha2, *alpha5)) return 0;
         sum1=*alpha0+*alpha1;
         sum2=*alpha2+*alpha3+*alpha4+*alpha5;
         if(sum1 && sum2)
         {
            *alpha0/=sum1;
            *alpha1/=sum1;
            *alpha2/=sum2;
            *alpha3/=sum2;
            *alpha4/=sum2;
            *alpha5/=sum2;
            return 1;
         }else
         {
            if(simplex_intersection4d(1, x1, x2, x3, x4, x5,
                                       alpha1, alpha2, alpha3, alpha4, alpha5))
                                       {
               *alpha0=0;
               return 1;
            }
            if(simplex_intersection4d(1, x0, x2, x3, x4, x5,
                                       alpha0, alpha2, alpha3, alpha4, alpha5))
                                       {
               *alpha1=0;
               return 1;
            }
            if(simplex_intersection4d(2, x0, x1, x3, x4, x5,
                                       alpha0, alpha1, alpha3, alpha4, alpha5))
                                       {
               *alpha2=0;
               return 1;
            }
            if(simplex_intersection4d(2, x0, x1, x2, x4, x5,
                                       alpha0, alpha1, alpha2, alpha4, alpha5))
                                       {
               *alpha3=0;
               return 1;
            }
            if(simplex_intersection4d(2, x0, x1, x2, x3, x5,
                                       alpha0, alpha1, alpha2, alpha3, alpha5))
                                       {
               *alpha4=0;
               return 1;
            }
            if(simplex_intersection4d(2, x0, x1, x2, x3, x4,
                                       alpha0, alpha1, alpha2, alpha3, alpha4))
                                       {
               *alpha5=0;
               return 1;
            }
            return 0;
         }

      case 3: // triangle vs. triangle
         *alpha0= orientation4d(x1, x2, x3, x4, x5);
         *alpha1=-orientation4d(x0, x2, x3, x4, x5);
         if(!same_sign(*alpha0, *alpha1)) return 0;
         *alpha2= orientation4d(x0, x1, x3, x4, x5);
         if(!same_sign(*alpha0, *alpha2)) return 0;
         *alpha3=-orientation4d(x0, x1, x2, x4, x5);
         *alpha4= orientation4d(x0, x1, x2, x3, x5);
         if(!same_sign(*alpha3, *alpha4)) return 0;
         *alpha5=-orientation4d(x0, x1, x2, x3, x4);
         if(!same_sign(*alpha3, *alpha5)) return 0;
         sum1=*alpha0+*alpha1+*alpha2;
         sum2=*alpha3+*alpha4+*alpha5;
         if(sum1 && sum2)
         {
            *alpha0/=sum1;
            *alpha1/=sum1;
            *alpha2/=sum1;
            *alpha3/=sum2;
            *alpha4/=sum2;
            *alpha5/=sum2;
            return 1;
         }else
         {
            if(simplex_intersection4d(2, x1, x2, x3, x4, x5,
                                       alpha1, alpha2, alpha3, alpha4, alpha5))
                                       {
               *alpha0=0;
               return 1;
            }
            if(simplex_intersection4d(2, x0, x2, x3, x4, x5,
                                       alpha0, alpha2, alpha3, alpha4, alpha5))
                                       {
               *alpha1=0;
               return 1;
            }
            if(simplex_intersection4d(2, x0, x1, x3, x4, x5,
                                       alpha0, alpha1, alpha3, alpha4, alpha5))
                                       {
               *alpha2=0;
               return 1;
            }
            if(simplex_intersection4d(2, x4, x5, x0, x1, x2,
                                       alpha4, alpha5, alpha0, alpha1, alpha2))
                                       {
               *alpha3=0;
               return 1;
            }
            if(simplex_intersection4d(2, x3, x5, x0, x1, x2,
                                       alpha3, alpha5, alpha0, alpha1, alpha2))
                                       {
               *alpha4=0;
               return 1;
            }
            if(simplex_intersection4d(2, x3, x4, x0, x1, x2,
                                       alpha3, alpha4, alpha0, alpha1, alpha2))
                                       {
               *alpha5=0;
               return 1;
            }
            return 0;
         }

      case 4: // tetrahedron vs. segment
      case 5: // pentachoron vs. point
         return simplex_intersection4d(6-k, x5, x4, x3, x2, x1, x0,
                                alpha5, alpha4, alpha3, alpha2, alpha1, alpha0);
      default:
         return -1; // should never get here
   }
}
*/

} // namespace vega
