//============================================================================
// obbtree.cpp
//============================================================================

#include "obbtree.hpp"

// OBB VERTS (in order: RTN,LTN,LBN,RBN,RTF,LTF,LBF,RBF)
Vect3d OBBtree::obbV[8];      

//----------------------------------------------------------------------------
// Builds the bounding-volume (BV) hierarchy
//----------------------------------------------------------------------------
void OBBtree::BuildHierarchy()
{
  #ifdef STATS_ON
    _TotalBVs_++;
  #endif

  pR = GetBestOrientation();
  FindTightFit();
  if (!Subdivide()) return;
  if (P) P->BuildHierarchy();
  if (N) N->BuildHierarchy();
}

int OBBtree::Subdivide()
{
  // STOP SUBDIVIDING IF NOT "ENOUGH" OBJECTS IN OBB.
  if (NumObjs <= MINOBJSLIMIT) return(0);

  // CALC THE SPLIT POINT AS THE BOX CENTER
  #if USEBOXCENTER
    Vect3d SplitPt = pR.T() * pT; // TRANSFORM BOX CENTER TO OBB COORD SYSTEM
  #else // USE MEAN OF THE POINT SETS
    Vect3d SplitPt(0,0,0);
    for (int j=0; j<NumObjs; j++) 
      SplitPt += ((PtSetObj*)(MyObjs[j]))->MeanPt;
    SplitPt /= NumObjs;
    SplitPt = pR.T() * SplitPt; // TRANSFORM BOX CENTER TO OBB COORD SYSTEM
  #endif

  // FIND SPLIT AXIS (INDEX) FOR LONGEST BOX DIMENSION
  float MaxD = max(Dim.x,Dim.y,Dim.z);  // RETURN LARGEST AXIS #
  if (MaxD == 0.0) return(0);           // NO SUBDIVISION POSSIBLE
  int sAxis = ( (MaxD==Dim.x) ? 0 : ((MaxD==Dim.y) ? 1 : 2) );

  // PARTITION OBJECTS ACCORDING TO SPLITPT AND SPLIT AXIS
  int PnumObjs=0;                         // COUNTER FOR P-CHILD OBJS
  int Temp;                               // TEMPORARY SWAP VARIABLE
  Vect3d Mid;                             // MIDDLE PT OF OBJECT
  float SplitVal = SplitPt[sAxis];        // SPLIT VALUE ALONG SPLIT AXIS
  for(int i=0; i<NumObjs; i++)
  {
    Mid = ((PtSetObj*)(MyObjs[i]))->MeanPt; // GET MEAN PT OF THE OBJECT
    if (pR.col(sAxis)*Mid < SplitVal)     // xFORM OBJ MIDPT SPLIT COORD TO OBB COORD SYS
    {
      Temp =  MyObjs[PnumObjs];           // SWAP CURRENT WITH NEXT POS IN P
      MyObjs[PnumObjs] = MyObjs[i];
      MyObjs[i] = Temp;
      PnumObjs++;
    }
  }
  if (PnumObjs==0 || PnumObjs==NumObjs)   // CHECK IF EITHER CHILD IS EMPTY
    return(0);                            // IF SO, DO NOT SUBDIVIDE!
  P = new OBBtree();                      // OTHERWISE, CREATE CHILD OBBtrees
  N = new OBBtree();
  P->MyObjs = MyObjs;                     // P-CHILD CONNECTS TO FRONT OF OBJ LIST
  P->NumObjs = PnumObjs;                  // P-CHILD CONTAINS COUNTER # OBJS
  N->MyObjs = &(MyObjs[PnumObjs]);        // N-CHILD CONNECTS TO "END" OF P
  N->NumObjs = NumObjs - PnumObjs;        // N-CHILD CONTAINS REMAINING OBJS
  return(1);                              // SUBDIVISION SUCCESSFUL
}

void OBBtree::FindTightFit()
{
  // ROTATE OBJS TO OBB COORDS AND FIND MAX AND MIN ALONG EACH AXIS
  m33 pRT = pR.T();                                 // STORE THE TRANSPOSE (REPEATED CALC)
  Vect3d Min, Max, Pt;
  Min = Max = pRT * ((PtSetObj*)(MyObjs[0]))->Pts[0]; // USE FIRST PT AS START MIN AND MAX
  for(int i=0; i<NumObjs; i++)                      // FOR EACH OBJECT
    for (int j=0; j<((PtSetObj*)(MyObjs[i]))->NumPts; j++)  // FOR EACH PT IN OBJECT
    {
      Pt = pRT * ((PtSetObj*)(MyObjs[i]))->Pts[j];    // XFORM OBJ VERTS INTO OBB COORDS
      if (Pt.x>Max.x) Max.x=Pt.x; else if (Pt.x<Min.x) Min.x=Pt.x;
      if (Pt.y>Max.y) Max.y=Pt.y; else if (Pt.y<Min.y) Min.y=Pt.y;
      if (Pt.z>Max.z) Max.z=Pt.z; else if (Pt.z<Min.z) Min.z=Pt.z;
    }
  Vect3d Mid = (Min+Max) * 0.5;  // CALCULATE ORIGIN (AVERAGE OF MAXs AND MINs)
  pT = pR*Mid;                   // TRANSFORM ORIG TO RIGHT PLACE IN WORLD
  Dim = (Max-Min) * 0.5;         // CALCULATE BOX RADII (DIMENSIONS)
}

// RETURNS A NEW "right-handed" ORIENTATION MATRIX which better fits the data.
m33 OBBtree::GetBestOrientation()
{
  if (NumObjs <= 0) return Identity();  // no tris?  exit!
  m33 Cov = GetCovariance();   // GET COVARIANCE MATRIX
  m33 Evecs;  Vect3d Evals;
  eigen(&Evecs, &Evals, Cov);  // GET EIGENVECTORS (and eigenvalues)

  // IF COLUMNS DO NOT FORM RIGHT-HANDED COORD AXES, NEGATE ONE TO MAKE IT SO.
  if ((Evecs.col(0)/Evecs.col(1))*Evecs.col(2) < 0) // IF FACING IN OPPOSITE DIRS, NEGATE
  {
    Evecs.m[0][2] = -Evecs.m[0][2];
    Evecs.m[1][2] = -Evecs.m[1][2];
    Evecs.m[2][2] = -Evecs.m[2][2];
  }

  return Evecs;
}

m33 OBBtree::GetCovariance()
{
  m33 Cov;
  Vect3d Pt, S(0,0,0);
  float S00=0, S11=0, S22=0, S01=0, S02=0, S12=0;
  int NumPts=0;
  for(int i=0; i<NumObjs; i++)                           // FOR EACH OBJECT
  {
    for (int j=0; j<((PtSetObj*)(MyObjs[i]))->NumPts; j++) // FOR EACH PT IN OBJECT
    {
      Pt = ((PtSetObj*)(MyObjs[i]))->Pts[j];
      S += Pt;
      S00 += (Pt.x*Pt.x);
      S11 += (Pt.y*Pt.y);
      S22 += (Pt.z*Pt.z);
      S01 += (Pt.x*Pt.y);
      S02 += (Pt.x*Pt.z);
      S12 += (Pt.y*Pt.z);
    }
    NumPts += ((PtSetObj*)(MyObjs[i]))->NumPts;
  }
  Cov.m[0][0] = S00 - S.x*S.x / NumPts;
  Cov.m[1][1] = S11 - S.y*S.y / NumPts;
  Cov.m[2][2] = S22 - S.z*S.z / NumPts;
  Cov.m[0][1] = S01 - S.x*S.y / NumPts;
  Cov.m[1][2] = S12 - S.y*S.z / NumPts;
  Cov.m[0][2] = S02 - S.x*S.z / NumPts;
  Cov.m[1][0] = Cov.m[0][1];
  Cov.m[2][0] = Cov.m[0][2];
  Cov.m[2][1] = Cov.m[1][2];
  return Cov;
}


//--------------------------------------------------------------------------
// View-frustum/OBB overlap test.
//--------------------------------------------------------------------------
int OBBtree::OverlapVF(ViewFrustum* VF) const
{
  #ifdef STATS_ON
    _NumBVtests_++;
  #endif

  // CALCULATE EIGHT VERTICES OF OBB
  Vect3d sX,sY,sZ,Rpt;  // REPEATED CALC STORAGE
  sX = Xaxis() * Dim.x; // APPROPRIATELY SCALE OBB AXES
  sY = Yaxis() * Dim.y;
  sZ = Zaxis() * Dim.z;
  Rpt = sX+sY+pT;  obbV[0]=Rpt+sZ; obbV[4]=Rpt-sZ; // Right-Top-Near, Right-Top-Far
  Rpt = sY-sX+pT;  obbV[1]=Rpt+sZ; obbV[5]=Rpt-sZ; // Left-Top-Near, Left-Top-Far
  Rpt = pT-sX-sY;  obbV[2]=Rpt+sZ; obbV[6]=Rpt-sZ; // Left-Bottom-Near, Left-Bottom-Far
  Rpt = sX-sY+pT;  obbV[3]=Rpt+sZ; obbV[7]=Rpt-sZ; // Right-Bottom-Near, Right-Bottom-Far

  // GO FOR TRIVIAL REJECTION OR ACCEPTANCE USING FASTER OVERLAP TEST
  int CompletelyIn = 1;  // ASSUME COMPLETELY IN UNTIL ONE COUNTEREXAMPLE
  int i;
  for (i=0; i < VF->NumPlanes; i++)
  {
    int Overlap = OverlapPlane(VF->Planes[i]);
    if (Overlap==COMPLETEOUT) return(COMPLETEOUT);
    else if (Overlap==PARTIAL) CompletelyIn=0;
  }
  if (CompletelyIn) return(COMPLETEIN);  // CHECK IF STILL COMPLETELY "IN"

  // TEST FOR VIEW-FRUSTUM EDGE PROTRUSION THROUGH OBB FACE (PROJECTORS AND NEAR EDGES ONLY)
  #ifdef FARPLANEON
    if (EdgeIntersect(VF->NearPts[0],VF->FarPts[0])) return(PARTIAL);  // FIRST PROJECTOR
  #else
    float InT, OutT;
    if (RayIntersect(VF->NearPts[0],VF->ProjDirs[0],InT,OutT)) return(PARTIAL);  // FIRST PROJECTOR
  #endif
  for (i=1; i < VF->NumSides; i++)
  {
    if (EdgeIntersect(VF->NearPts[i-1],VF->NearPts[i])) return(PARTIAL); // NEAR EDGES
    #ifdef FARPLANEON
      if (EdgeIntersect(VF->NearPts[i],VF->FarPts[i])) return(PARTIAL);    // PROJECTORS
    #else
      if (RayIntersect(VF->NearPts[i],VF->ProjDirs[i],InT,OutT)) return(PARTIAL);  // PROJECTORS
    #endif
  }
  if (EdgeIntersect(VF->NearPts[(VF->NumSides)-1],VF->NearPts[0])) return(PARTIAL); // LAST NEAR EDGE

  // TEST FOR PROTRUSION OF OBB EDGE THROUGH VIEW-FRUSTUM FACE
  if (VF->EdgeIntersect(obbV[0],obbV[4])) return(PARTIAL);
  if (VF->EdgeIntersect(obbV[1],obbV[5])) return(PARTIAL);
  if (VF->EdgeIntersect(obbV[2],obbV[6])) return(PARTIAL);
  if (VF->EdgeIntersect(obbV[3],obbV[7])) return(PARTIAL);
  if (VF->EdgeIntersect(obbV[0],obbV[3])) return(PARTIAL);
  if (VF->EdgeIntersect(obbV[3],obbV[2])) return(PARTIAL);
  if (VF->EdgeIntersect(obbV[2],obbV[1])) return(PARTIAL);
  if (VF->EdgeIntersect(obbV[1],obbV[0])) return(PARTIAL);
  if (VF->EdgeIntersect(obbV[4],obbV[7])) return(PARTIAL);
  if (VF->EdgeIntersect(obbV[7],obbV[6])) return(PARTIAL);
  if (VF->EdgeIntersect(obbV[6],obbV[5])) return(PARTIAL);
  if (VF->EdgeIntersect(obbV[5],obbV[4])) return(PARTIAL);

  // TEST FOR CONTAINMENT OF VIEW-FRUSTUM WITHIN OBB
  // IT SHOULD SUFFICE TO TEST ANY PT OF THE VIEW-FRUSTUM (we'll use VF->NearPts[0]).
  Plane tP;  // TEMPORARY PLANE
  tP.CalcCoeffsGivenNormal(-(Xaxis()),obbV[1]);
  if (tP.InOutTest(VF->NearPts[0])) return(COMPLETEOUT); // CHECK VF PT AGAINST EACH PLANE
  tP.CalcCoeffsGivenNormal(Xaxis(),obbV[4]);             // OF THE OBB, IF "OUT" ANYWHERE, NO OVERLAP!
  if (tP.InOutTest(VF->NearPts[0])) return(COMPLETEOUT); // VF IS EITHER ENTIRELY CONTAINED OR DISJOINT!
  tP.CalcCoeffsGivenNormal(Yaxis(),obbV[5]);
  if (tP.InOutTest(VF->NearPts[0])) return(COMPLETEOUT);
  tP.CalcCoeffsGivenNormal(-(Yaxis()),obbV[3]);
  if (tP.InOutTest(VF->NearPts[0])) return(COMPLETEOUT);
  tP.CalcCoeffsGivenNormal(Zaxis(),obbV[1]);
  if (tP.InOutTest(VF->NearPts[0])) return(COMPLETEOUT);
  tP.CalcCoeffsGivenNormal(-(Zaxis()),obbV[4]);
  if (tP.InOutTest(VF->NearPts[0])) return(COMPLETEOUT);

  // VF MUST BE COMPLETELY ENCLOSED SINCE PT IS NOT "OUT "OF ANY OBB PLANE.
  return(PARTIAL);
};

//---------------------------------------------------------------------------
// Oriented Bounding Box / Plane overlap test (returns type of overlap)
//  Chooses a corresponding pt of the BOX in the same octant as the given normal.
//  Calcs # of the octant: 0=RTN,1=LTN,2=LBN,3=RBN,4=RTF,5=LTF,6=LBF,7=RBF
//  also calcs the opposite corner (Neg of Normal) index.
//---------------------------------------------------------------------------
int OBBtree::OverlapPlane(Plane P) const
{
  // CALC INDICES (iPos,iNeg) FOR EXTREME PTS ALONG NORMAL AXIS (iPos in dir of norm, etc.)
  Vect3d N(P.N*Xaxis(), P.N*Yaxis(), P.N*Zaxis()); // XFORM PLANE NORM TO BOX COORD SYS
  int iPos, iNeg;
  if(N.x>0)
    if(N.y>0) { if(N.z>0) { iPos=0; iNeg=6; } else { iPos=4; iNeg=2; } }
         else { if(N.z>0) { iPos=3; iNeg=5; } else { iPos=7; iNeg=1; } }
  else
    if(N.y>0) { if(N.z>0) { iPos=1; iNeg=7; } else { iPos=5; iNeg=3; } }
         else { if(N.z>0) { iPos=2; iNeg=4; } else { iPos=6; iNeg=0; } }

  // CHECK DISTANCE TO PLANE FROM EXTREMAL POINTS TO DETERMINE OVERLAP
  if (P.DistToPt(obbV[iNeg]) > 0) return(COMPLETEOUT);
  else if (P.DistToPt(obbV[iPos]) <= 0) return(COMPLETEIN);
  else return(PARTIAL);
}

//--------------------------------------------------------------------------
// CHECKS IF AN EDGE INTERSECTS THE OBB (IF EDGE INTERSECT ANY FACE, THEN IT INTERSECTS)
// NOTE: MinPt and MaxPt are the world coord points ON THE OBB EXTREME CORNERS!
//--------------------------------------------------------------------------
int OBBtree::EdgeIntersect(Vect3d Start, Vect3d End) const
{
  // RayIntersect CHECKS IF INTERSECTION IS AFTER Start
  // IF AT LEAST ONE THE Tvals ARE IN THE INTERVAL [0,1] WE HAVE INTERSECTION
  float InT, OutT;
  Vect3d Dir = End-Start;  // CALC DIRECTION VECTOR OF EDGE
  if (RayIntersect(Start, Dir, InT, OutT))
    if (InT<=1 || OutT<=1) return(1);
  return(0);
}

//--------------------------------------------------------------------------
// Ray-OBB intersection test. returns In-Out HitTimes
//--------------------------------------------------------------------------
int OBBtree::RayIntersect(Vect3d Start, Vect3d Dir, float& InT, float& OutT) const
{
  InT=-99999, OutT=99999;    // INIT INTERVAL T-VAL ENDPTS TO -/+ "INFINITY"
  float NewInT, NewOutT;     // STORAGE FOR NEW T VALUES
  float MinD, MaxD;          // SLAB PLANE D VALS (DIST ALONG NORMAL TO PLANE)
  float NdotDir, NdotStart;  // STORAGE FOR REPEATED CALCS NEEDED FOR NewT CALC

  // X-SLAB (PARALLEL PLANES PERPENDICULAR TO X-AXIS) INTERSECTION (Xaxis is Normal)
  NdotDir = Xaxis()*Dir;         // CALC DOT PROD OF PLANE NORMAL AND RAY DIR
  NdotStart = Xaxis()*Start;     // CALC DOT PROD OF PLANE NORMAL AND RAY START PT
  MinD = Xaxis()*obbV[6];        // CALC D-VAL FOR FIRST PLANE OF SLAB (use LBF)
  MaxD = Xaxis()*obbV[0];        // CALC D-VAL FOR SECOND PLANE OF SLAB (use RTN)
  if (NdotDir == 0)              // CHECK IF RAY IS PARALLEL TO THE SLAB PLANES
  {
    if (MinD < MaxD) { if ((NdotStart < MinD) || (NdotStart > MaxD)) return(0); }
                else { if ((NdotStart < MaxD) || (NdotStart > MinD)) return(0); }
  }
  else
  {
    NewInT = (MinD - NdotStart) / NdotDir;
    NewOutT = (MaxD - NdotStart) / NdotDir;
    if (NewOutT > NewInT) { if (NewInT>InT) InT=NewInT;  if (NewOutT<OutT) OutT=NewOutT; }
    else { if (NewOutT > InT) InT=NewOutT;  if (NewInT < OutT) OutT=NewInT; }
    if (InT > OutT) return(0);
  }

  // Y-SLAB (PARALLEL PLANES PERPENDICULAR TO Y-AXIS) INTERSECTION (Yaxis is Normal)
  NdotDir = Yaxis()*Dir;         // CALC DOT PROD OF PLANE NORMAL AND RAY DIR
  NdotStart = Yaxis()*Start;     // CALC DOT PROD OF PLANE NORMAL AND RAY START PT
  MinD = Yaxis()*obbV[6];        // CALC D-VAL FOR FIRST PLANE OF SLAB (use LBF)
  MaxD = Yaxis()*obbV[0];        // CALC D-VAL FOR SECOND PLANE OF SLAB (use RTN)
  if (NdotDir == 0)              // CHECK IF RAY IS PARALLEL TO THE SLAB PLANES
  {
    if (MinD < MaxD) { if ((NdotStart < MinD) || (NdotStart > MaxD)) return(0); }
                else { if ((NdotStart < MaxD) || (NdotStart > MinD)) return(0); }
  }
  else
  {
    NewInT = (MinD - NdotStart) / NdotDir;
    NewOutT = (MaxD - NdotStart) / NdotDir;
    if (NewOutT > NewInT) { if (NewInT>InT) InT=NewInT;  if (NewOutT<OutT) OutT=NewOutT; }
    else { if (NewOutT > InT) InT=NewOutT;  if (NewInT < OutT) OutT=NewInT; }
    if (InT > OutT) return(0);
  }

  // Z-SLAB (PARALLEL PLANES PERPENDICULAR TO Z-AXIS) INTERSECTION (Zaxis is Normal)
  NdotDir = Zaxis()*Dir;         // CALC DOT PROD OF PLANE NORMAL AND RAY DIR
  NdotStart = Zaxis()*Start;     // CALC DOT PROD OF PLANE NORMAL AND RAY START PT
  MinD = Zaxis()*obbV[6];        // CALC D-VAL FOR FIRST PLANE OF SLAB (use LBF)
  MaxD = Zaxis()*obbV[0];        // CALC D-VAL FOR SECOND PLANE OF SLAB (use RTN)
  if (NdotDir == 0)              // CHECK IF RAY IS PARALLEL TO THE SLAB PLANES
  {
    if (MinD < MaxD) { if ((NdotStart < MinD) || (NdotStart > MaxD)) return(0); }
                else { if ((NdotStart < MaxD) || (NdotStart > MinD)) return(0); }
  }
  else
  {
    NewInT = (MinD - NdotStart) / NdotDir;
    NewOutT = (MaxD - NdotStart) / NdotDir;
    if (NewOutT > NewInT) { if (NewInT>InT) InT=NewInT;  if (NewOutT<OutT) OutT=NewOutT; }
    else { if (NewOutT > InT) InT=NewOutT;  if (NewInT < OutT) OutT=NewInT; }
    if (InT > OutT) return(0);
  }

  // CHECK IF INTERSECTIONS ARE "AT OR BEYOND" THE START OF THE RAY
  if (InT>=0 || OutT>=0) return(1); else return(0);
}