//============================================================================
// sphrtree.cpp
//============================================================================

#include "sphrtree.hpp"

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

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

int SPHEREtree::Subdivide() // RETURNS "1" IF SUCCESSFUL SUBDIVISION, "0" IF NOT.
{
  // STOP SUBDIVIDING IF NOT "ENOUGH" OBJECTS IN SPHERE.
  if (NumObjs <= MINOBJSLIMIT) return(0);

  // CALC THE SPLIT POINT AS THE BOX CENTER
  #if USEBOXCENTER
    Vect3d SplitPt = Center;
  #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;
  #endif

  // DETERMINE OPTIMAL AXIS OF SPLIT IN RELATION TO SplitPt
  int sAxis = FindSplitAxis(SplitPt);
  if (sAxis < 0) return(0);  // NO SUBDIVISION POSSIBLE

  // CREATE CHILD SPHEREtrees
  P = new SPHEREtree();
  N = new SPHEREtree();

  // PARTITION OBJECTS ACCORDING TO SPLITPT AND SPLIT AXIS
  int Temp;                             // TEMPORARY SWAP VARIABLE
  float Mid;                            // MIDDLE VALUE OF OBJECT ALONG SPLIT AXIS
  float SplitVal = SplitPt[sAxis];      // SPLIT VALUE ALONG SPLIT AXIS
  for(int i=0; i<NumObjs; i++)
  {
    Mid = ((PtSetObj*)(MyObjs[i]))->MeanPt[sAxis]; // GET MEAN PT OF THE OBJECT
    if (Mid < SplitVal)
    {
      Temp =  MyObjs[P->NumObjs];       // SWAP CURRENT WITH NEXT POS IN P
      MyObjs[P->NumObjs] = MyObjs[i];
      MyObjs[i] = Temp;
      (P->NumObjs)++;
    }
  }
  P->MyObjs = MyObjs;
  N->MyObjs = &(MyObjs[P->NumObjs]);
  N->NumObjs = NumObjs - P->NumObjs;
  return(1); // SUBDIVISION SUCCESSFUL
}

// GIVEN THE SplitPt, RETURNS THE SPLIT AXIS # (0,1,2 = x,y,z OR -1 IF NO SPLIT POSSIBLE)
int SPHEREtree::FindSplitAxis(Vect3d SplitPt)
{
  // DETERMINE IF TRIS WILL BE PUT IN BOTH PARTITIONS BASED ON EACH AXIS.
  // IF TRIS ARE PUT ON BOTH SIDES OF SplitPt ALONG AN AXIS, AXIS IS "GOOD"
  int IsInNx=0, IsInNy=0, IsInNz=0, IsInPx=0, IsInPy=0, IsInPz=0;  // PARTITION-FLAGS ALONG EACH AXIS
  int XisGood=0, YisGood=0, ZisGood=0;
  Vect3d MeanPt;

  // ONLY NECESSARY TO PROCEED UNTIL EITHER NO MORE TRIS OR ALL AXES GOOD!
  for(int i=0; i<NumObjs && !(XisGood && YisGood && ZisGood); i++)
  {
    MeanPt = ((PtSetObj*)(MyObjs[i]))->MeanPt; // MEAN PT OF CURRENT OBJECT
    if (MeanPt.x < SplitPt.x) IsInNx=1; else IsInPx=1;
    if (MeanPt.y < SplitPt.y) IsInNy=1; else IsInPy=1;
    if (MeanPt.z < SplitPt.z) IsInNz=1; else IsInPz=1;
    XisGood=(IsInNx && IsInPx); YisGood=(IsInNy && IsInPy); ZisGood=(IsInNz && IsInPz);
  }
  // IF AXIS IS GOOD, SET AXIS DIM (AD) TO SPHERE DIM, OTHERWISE INVALIDATE DIM TO 0
  Vect3d AD( XisGood?Radius:0.0, YisGood?Radius:0.0, ZisGood?Radius:0.0 );
  float MaxD = max(AD.x,AD.y,AD.z);   // RETURN LARGEST, GOOD AXIS #
  return ( (MaxD==0.0) ? -1 : ((MaxD==AD.x) ? 0 : ((MaxD==AD.y)?1:2)) );
}

// FINDS A "MINIMAL" BOUNDING SPHERE FOR THE SET OF POINTS (Center and Radius)
void SPHEREtree::FindTightFit()
{
  // FIRST PASS: find 6 minima/maxima points
  Vect3d Pt, xmin, xmax, ymin, ymax, zmin, zmax;
  xmin = ymin = zmin = xmax = ymax = zmax = ((PtSetObj*)(MyObjs[0]))->Pts[0];
  int i, j;
  for (i=0; i<NumObjs; i++)
    for (j=0; j<((PtSetObj*)(MyObjs[i]))->NumPts; j++)
    {
      Pt = ((PtSetObj*)(MyObjs[i]))->Pts[j];
      if (Pt.x<xmin.x) xmin=Pt; else if (Pt.x>xmax.x) xmax=Pt;
      if (Pt.y<ymin.y) ymin=Pt; else if (Pt.y>ymax.y) ymax=Pt;
      if (Pt.z<zmin.z) zmin=Pt; else if (Pt.z>zmax.z) zmax=Pt;
    }

  // FIND LENGTHS (SQUARED) OF VECTORS BETWEEN MINIMA AND MAXIMA PTS
  float xDiamLengthSqr = (xmax-xmin).LengthSqr();
  float yDiamLengthSqr = (ymax-ymin).LengthSqr();
  float zDiamLengthSqr = (zmax-zmin).LengthSqr();

  // FIND THE ENDPOINTS OF THE LONGEST SPAN
  Vect3d DiamEnd1=xmin, DiamEnd2=xmax;
  float MaxDiamLengthSqr=xDiamLengthSqr;
  if (yDiamLengthSqr > MaxDiamLengthSqr) { MaxDiamLengthSqr=yDiamLengthSqr; DiamEnd1=ymin; DiamEnd2=ymax; }
  if (zDiamLengthSqr > MaxDiamLengthSqr) { DiamEnd1=zmin; DiamEnd2=zmax; }

  // USING MAXDIAM AS INITIAL SPHERE DIAM, CALC INITIAL SPHERE CENTER AND RADIUS
  Center = (DiamEnd1+DiamEnd2)*0.5;
  Vect3d RadiusVect = DiamEnd2-Center;      // CALC THE RADIUS VECTOR OF THE SPHERE
  float RadiusSqr = RadiusVect.LengthSqr();
  Radius = sqrt(RadiusSqr);

  /* SECOND PASS: increment current sphere */
  float NewRadius, NewRadiusSqr;
  for (i=0; i<NumObjs; i++)
    for (j=0; j<((PtSetObj*)(MyObjs[i]))->NumPts; j++)
    {
      RadiusVect = ((PtSetObj*)(MyObjs[i]))->Pts[j] - Center; // FIND VECTOR FROM CENTER TO NEW POINT
      NewRadiusSqr = RadiusVect.LengthSqr();  // CALC LENGTH SQR TO NEW POINT
      if (NewRadiusSqr > RadiusSqr)           // IS THE NEW PT OUTSIDE THE CURRENT SPHERE?
      {
        NewRadius = sqrt(NewRadiusSqr);       // CALC NEW RADIUS VECTOR LENGTH
        Radius = (Radius+NewRadius)*0.5;      // CALC MEAN BETWEEN OLD RADIUS AND NEW RADIUS
        RadiusSqr = Radius*Radius;            // CALC NEW RADIUS LENGTH SQR
        Center += RadiusVect*((NewRadius-Radius)/NewRadius); // CALC NEW SPHERE CENTER
      }
    }
}

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

  float negRadius=-Radius, Dist;
  int CompletelyIn=1;
  for (int i=0; i<VF->NumPlanes; i++)
  {
    Dist = VF->Planes[i].DistToPt(Center);
    if (Dist > Radius) return(COMPLETEOUT);
    else if (Dist > negRadius) CompletelyIn=0;
  }
  if (CompletelyIn) return(COMPLETEIN);
  return(PARTIAL);
}