    // @(#)root/gl:$Name:  $:$Id: TGLBoundingBox.cxx,v 1.8 2005/06/24 14:53:02 brun Exp $
    // Author:  Richard Maunder  25/05/2005

    /*************************************************************************
     * Copyright (C) 1995-2000, Rene Brun and Fons Rademakers.               *
     * All rights reserved.                                                  *
     *                                                                       *
     * For the licensing terms see $ROOTSYS/LICENSE.                         *
     * For the list of contributors see $ROOTSYS/README/CREDITS.             *
     *************************************************************************/

    // TODO: Function descriptions
    // TODO: Class def - same as header

    #include "TGLBoundingBox.h"
    #include "TGLIncludes.h"
    #include "Riostream.h"

    ClassImp(TGLBoundingBox)

    //______________________________________________________________________________
     TGLBoundingBox::TGLBoundingBox()
    {
       SetEmpty();
    }

    //______________________________________________________________________________
     TGLBoundingBox::TGLBoundingBox(const TGLVertex3 vertex[8])
    {
       Set(vertex);
    }

    //______________________________________________________________________________
     TGLBoundingBox::TGLBoundingBox(const Double_t vertex[8][3])
    {
       Set(vertex);
    }

    //______________________________________________________________________________
     TGLBoundingBox::TGLBoundingBox(const TGLVertex3 & lowVertex, const TGLVertex3 & highVertex)
    {
       SetAligned(lowVertex, highVertex);
    }

    //______________________________________________________________________________
     TGLBoundingBox::TGLBoundingBox(const TGLBoundingBox & other)
    {
       Set(other);
    }

    //______________________________________________________________________________
     TGLBoundingBox::~TGLBoundingBox()
    {
    }

    //______________________________________________________________________________
     void TGLBoundingBox::Set(const TGLVertex3 vertex[8])
    {
       for (UInt_t v = 0; v < 8; v++) {
          fVertex[v] = vertex[v];
       }
       UpdateVolume();
    }

    //______________________________________________________________________________
     void TGLBoundingBox::Set(const Double_t vertex[8][3])
    {
       for (UInt_t v = 0; v < 8; v++) {
          for (UInt_t a = 0; a < 3; a++) {
             fVertex[v][a] = vertex[v][a];
          }
       }
       UpdateVolume();
    }

    //______________________________________________________________________________
     void TGLBoundingBox::Set(const TGLBoundingBox & other)
    {
       for (UInt_t v = 0; v < 8; v++) {
          fVertex[v].Set(other.fVertex[v]);
       }
       UpdateVolume();
    }

    //______________________________________________________________________________
     void TGLBoundingBox::SetEmpty()
    {
       for (UInt_t v = 0; v < 8; v++) {
          fVertex[v].Fill(0.0);
       }
       UpdateVolume();
    }

    //______________________________________________________________________________
     void TGLBoundingBox::SetAligned(const TGLVertex3 & lowVertex, const TGLVertex3 & highVertex)
    {
       // Set aligned box from two axis aligned low/high vertices
       //   7-------6
       //  /|      /|
       // 3-------2 |
       // | 4-----|-5
       // |/      |/
       // 0-------1

       TGLVector3 diff = highVertex - lowVertex;
       fVertex[0] = lowVertex;
       fVertex[1] = lowVertex; fVertex[1].X() += diff.X();
       fVertex[2] = lowVertex; fVertex[2].X() += diff.X(); fVertex[2].Y() += diff.Y();
       fVertex[3] = lowVertex; fVertex[3].Y() += diff.Y();
       fVertex[4] = highVertex; fVertex[4].X() -= diff.X(); fVertex[4].Y() -= diff.Y();
       fVertex[5] = highVertex; fVertex[5].Y() -= diff.Y();
       fVertex[6] = highVertex;
       fVertex[7] = highVertex; fVertex[7].X() -= diff.X();
       UpdateVolume();
    }

    //______________________________________________________________________________
     void TGLBoundingBox::SetAligned(UInt_t nbPnts, const Double_t * pnts)
    {
       // Set aligned box using one or more points
       if (nbPnts < 1 || !pnts) {
          assert(false);
          return;
       }

       // Single point gives a zero volume BB
       TGLVertex3 low(pnts[0], pnts[1], pnts[2]);
       TGLVertex3 high(pnts[0], pnts[1], pnts[2]);

       for (UInt_t p = 1; p < nbPnts; p++) {
          for (UInt_t i = 0; i < 3; i++) {
             if (pnts[3*p + i] < low[i]) {
                low[i] = pnts[3*p + i] ;
             }
             if (pnts[3*p + i] > high[i]) {
                high[i] = pnts[3*p + i] ;
             }
          }
       }

       SetAligned(low, high);
    }

    //______________________________________________________________________________
     void TGLBoundingBox::Scale(Double_t scale)
    {
       TGLVertex3 center = Center();
       TGLVector3 newVector;
       for (UInt_t v = 0; v < 8; v++) {
          newVector.Set(fVertex[v] - center);
          newVector *= scale;
          fVertex[v].Set(center + newVector);
       }
       UpdateVolume();
    }

    //______________________________________________________________________________
     void TGLBoundingBox::Translate(const TGLVector3 & offset)
    {
       for (UInt_t v = 0; v < 8; v++) {
          fVertex[v] = fVertex[v] + offset;
       }

       // No volume change
    }

    //______________________________________________________________________________
     void TGLBoundingBox::Transform(const TGLMatrix & matrix)
    {
       for (UInt_t v = 0; v < 8; v++) {
          matrix.TransformVertex(fVertex[v]);
       }

       // Could change volume
       UpdateVolume();
    }

    //______________________________________________________________________________
     EOverlap TGLBoundingBox::Overlap(const TGLPlane & plane) const
    {
       // TODO: Cheaper sphere test
       // Test all 8 box vertices against plane
       Int_t VerticesInsidePlane = 8;
       for (UInt_t v = 0; v < 8; v++) {
          if (plane.DistanceTo(fVertex[v]) < 0.0) {
             VerticesInsidePlane--;
          }
       }

       if ( VerticesInsidePlane == 0 ) {
          return kOutside;
       } else if ( VerticesInsidePlane == 8 ) {
          return kInside;
       } else {
          return kPartial;
       }
    }

    //______________________________________________________________________________
     Bool_t TGLBoundingBox::AlignedContains(const TGLBoundingBox & other) const
    {
       // Test is passed box is contained by us. This will ONLY work if we are axis aligned

       //   7-------6
       //  /|      /|
       // 3-------2 |
       // | 4-----|-5
       // |/      |/
       // 0-------1
       // TODO: Rounding errors - need a better test anyway....
       // This can probably be found as part of Intersect() with three overlap
       // cases
       //assert(fVertex[2].Z() == fVertex[0].Z()); // Front
       //assert(fVertex[4].Z() == fVertex[6].Z()); // Back
       //assert(fVertex[5].Y() == fVertex[0].Y()); // Bottom
       //assert(fVertex[3].Y() == fVertex[6].Y()); // Top
       //assert(fVertex[7].X() == fVertex[0].X()); // Left
       //assert(fVertex[1].X() == fVertex[6].X()); // Right

       // Just test the all other's vertexes lie within our axis limits
       for (UInt_t v = 0; v < 8; v++) {
          for (UInt_t a = 0; a < 3; a++)
          {
             if ((other.fVertex[v][a] < fVertex[0][a]) ||
                 (other.fVertex[v][a] > fVertex[6][a])) {
                return kFALSE;
             }
          }
       }

       return kTRUE;
    }

    //______________________________________________________________________________
     Bool_t TGLBoundingBox::Intersect(const TGLBoundingBox & a, const TGLBoundingBox & b)
    {
       // TODO: For some reason this intersection test gives incorrect result if first
       // BB is smaller than other - no idea why as should be symetric - need to investigate.
       //assert(Volume() > other.Volume());

       TGLVector3 aHL = a.Extents() / 2.0; // Half length extents
       TGLVector3 bHL = b.Extents() / 2.0; // Half length extents

       // Perform seperating axes search - test is greatly simplified
       // if we convert  into our local frame

       // Find translation in parent frame
       TGLVector3 parentT = b.Center() - a.Center();

        // Find translation in A's frame
       TGLVector3 aT(Dot(parentT, a.Axis(0)), Dot(parentT, a.Axis(1)), Dot(parentT, a.Axis(2)));

        // Find B's basis with respect to A's local frame
        // Get rotation matrix
        Double_t   roaT[3][3];
        UInt_t     i, k;
        for (i=0 ; i<3 ; i++) {
           for (k=0; k<3; k++) {
              roaT[i][k] = Dot(a.Axis(i), b.Axis(k));
           }
        }

        // Perform separating axis test for all 15 potential
        // axes. If no seperating axes found, the two boxes overlap.
        Double_t ra, rb, t;

        // A's 3 basis vectors
        for (i=0; i<3; i++) {
           ra = aHL[i];
           rb = bHL[0]*fabs(roaT[i][0]) + bHL[1]*fabs(roaT[i][1]) + bHL[2]*fabs(roaT[i][2]);
           t = fabs(aT[i]);
           if (t > ra + rb)
              return kFALSE;
        }

        // B's 3 basis vectors
        for (k=0; k<3; k++) {
           ra = aHL[0]*fabs(roaT[0][k]) + aHL[1]*fabs(roaT[1][k]) + aHL[2]*fabs(roaT[2][k]);
           rb = bHL[k];
           t = fabs(aT[0]*roaT[0][k] + aT[1]*roaT[1][k] + aT[2]*roaT[2][k]);
           if (t > ra + rb)
              return kFALSE;
        }

        // Now the 9 cross products

        // A0 x B0
        ra = aHL[1]*fabs(roaT[2][0]) + aHL[2]*fabs(roaT[1][0]);
        rb = bHL[1]*fabs(roaT[0][2]) + bHL[2]*fabs(roaT[0][1]);
        t = fabs(aT[2]*roaT[1][0] - aT[1]*roaT[2][0]);
        if (t > ra + rb)
           return kFALSE;

        // A0 x B1
        ra = aHL[1]*fabs(roaT[2][1]) + aHL[2]*fabs(roaT[1][1]);
        rb = bHL[0]*fabs(roaT[0][2]) + bHL[2]*fabs(roaT[0][0]);
        t = fabs(aT[2]*roaT[1][1] - aT[1]*roaT[2][1]);
        if (t > ra + rb)
           return kFALSE;

        // A0 x B2
        ra = aHL[1]*fabs(roaT[2][2]) + aHL[2]*fabs(roaT[1][2]);
        rb = bHL[0]*fabs(roaT[0][1]) + bHL[1]*fabs(roaT[0][0]);
        t = fabs(aT[2]*roaT[1][2] - aT[1]*roaT[2][2]);
        if (t > ra + rb)
           return kFALSE;

        // A1 x B0
        ra = aHL[0]*fabs(roaT[2][0]) + aHL[2]*fabs(roaT[0][0]);
        rb = bHL[1]*fabs(roaT[1][2]) + bHL[2]*fabs(roaT[1][1]);
        t = fabs(aT[0]*roaT[2][0] - aT[2]*roaT[0][0]);
        if (t > ra + rb)
           return kFALSE;

        // A1 x B1
        ra = aHL[0]*fabs(roaT[2][1]) + aHL[2]*fabs(roaT[0][1]);
        rb = bHL[0]*fabs(roaT[1][2]) + bHL[2]*fabs(roaT[1][0]);
        t = fabs(aT[0]*roaT[2][1] - aT[2]*roaT[0][1]);
        if (t > ra + rb)
           return kFALSE;

        // A1 x B2
        ra = aHL[0]*fabs(roaT[2][2]) + aHL[2]*fabs(roaT[0][2]);
        rb = bHL[0]*fabs(roaT[1][1]) + bHL[1]*fabs(roaT[1][0]);
        t = fabs(aT[0]*roaT[2][2] - aT[2]*roaT[0][2]);
        if (t > ra + rb)
           return kFALSE;

        // A2 x B0
        ra = aHL[0]*fabs(roaT[1][0]) + aHL[1]*fabs(roaT[0][0]);
        rb = bHL[1]*fabs(roaT[2][2]) + bHL[2]*fabs(roaT[2][1]);
        t = fabs(aT[1]*roaT[0][0] - aT[0]*roaT[1][0]);
        if (t > ra + rb)
           return kFALSE;

        // A2 x B1
        ra = aHL[0]*fabs(roaT[1][1]) + aHL[1]*fabs(roaT[0][1]);
        rb = bHL[0]*fabs(roaT[2][2]) + bHL[2]*fabs(roaT[2][0]);
        t = fabs(aT[1]*roaT[0][1] - aT[0]*roaT[1][1]);
        if (t > ra + rb)
           return kFALSE;

        // A2 x B2
        ra = aHL[0]*fabs(roaT[1][2]) + aHL[1]*fabs(roaT[0][2]);
        rb = bHL[0]*fabs(roaT[2][1]) + bHL[1]*fabs(roaT[2][0]);
        t = fabs(aT[1]*roaT[0][2] - aT[0]*roaT[1][2]);
        if (t > ra + rb)
           return kFALSE;

        // No separating axis - two boxes overlap
        return true;
    }

    //______________________________________________________________________________
     void TGLBoundingBox::Draw() const
    {
       glBegin(GL_LINE_LOOP);
       glVertex3dv(fVertex[0].CArr());
       glVertex3dv(fVertex[1].CArr());
       glVertex3dv(fVertex[2].CArr());
       glVertex3dv(fVertex[3].CArr());
       glVertex3dv(fVertex[7].CArr());
       glVertex3dv(fVertex[6].CArr());
       glVertex3dv(fVertex[5].CArr());
       glVertex3dv(fVertex[4].CArr());
       glEnd();
       glBegin(GL_LINES);
       glVertex3dv(fVertex[1].CArr());
       glVertex3dv(fVertex[5].CArr());
       glVertex3dv(fVertex[2].CArr());
       glVertex3dv(fVertex[6].CArr());
       glVertex3dv(fVertex[0].CArr());
       glVertex3dv(fVertex[3].CArr());
       glVertex3dv(fVertex[4].CArr());
       glVertex3dv(fVertex[7].CArr());
       glEnd();
    }

    //______________________________________________________________________________
     Double_t TGLBoundingBox::Min(UInt_t index) const
    {
       Double_t min = fVertex[0][index];
       for (UInt_t v = 1; v < 8; v++) {
          if (fVertex[v][index] < min) {
             min = fVertex[v][index];
          }
       }
       return min;
    }

    //______________________________________________________________________________
     Double_t TGLBoundingBox::Max(UInt_t index) const
    {
       Double_t max = fVertex[0][index];
       for (UInt_t v = 1; v < 8; v++) {
          if (fVertex[v][index] > max) {
             max = fVertex[v][index];
          }
       }
       return max;
    }

    //______________________________________________________________________________
     void TGLBoundingBox::Dump() const
    {
       for (UInt_t i = 0; i<8; i++) {
          std::cout << "[" << i << "] (" << fVertex[i].X() << "," << fVertex[i].Y() << "," << fVertex[i].Z() << ")" << std::endl;
       }
       std::cout << "Center ";
       Center().Dump();
       std::cout << " Volume " << Volume() << std::endl;
    }