-----
-- @Author Matt Emborsky <memborsky@gammacode.com>
-- @Date April 29, 2007
--
-- Frustum Calling in OpenGL by Mark Morley, http://www.crownandcutlass.com/features/technicaldetails/frustum.html
-----
Generic

  NumPoints : Integer;

Package Frustum Is

  Type Float is Digits 18;

  Type Pointrec is Record
    x : Float;
    y : Float;
    z : Float;
  End Record;

  Type Pointlist is Array(0..NumPoints) of Pointrec;

  Procedure ExtractFrustum;

  Function PointInFrustum ( X : Float; Y : Float; Z : Float ) Return Boolean;

  Function SphereInFrustum ( X : Float; Y : Float; Z : Float; Radius : Float ) Return Boolean;
  Function SphereInFrustum ( X : Float; Y : Float; Z : Float; Radius : Float ) Return Float;
  Function SphereInFrustum ( X : Float; Y : Float; Z : Float; Radius : Float ) Return Integer;

  Function CubeInFrustum ( X : Float; Y : Float; Z : Float; Size : Float ) Return Boolean;
  Function CubeInFrustum ( X : Float; Y : Float; Z : Float; Size : Float ) Return Integer;

  Function PolygonInFrustum ( NumPoints : Integer; Points : Pointlist ) Return Boolean;

Private

  Type Frustum_Array is Array (0..6, 0..4) of Float;

  frustum : Frustum_Array; 

End Frustum;




-----
-- @Author Matt Emborsky <memborsky@gammacode.com>
-- @Date April 29, 2007
--
-- Frustum Calling in OpenGL by Mark Morley, http://www.crownandcutlass.com/features/technicaldetails/frustum.html
-----
With Ada.Numerics.Generic_Elementary_Functions;

Package Body Frustum Is

  Package Math is New Ada.Numerics.Generic_Elementary_Functions(Float_Type => Float);

  Procedure ExtractFrustum is

    Type Float_Array is Array (0..16) of Float;

    proj : Float_Array;
    modl : Float_Array;
    clip : Float_Array;

    t : Float;

  Begin
  
    -- Get the current PROJECTION matrix from OpenGL
    -- glGetFLoatv( GL_PROJECTION_MATRIX, proj )
    
    -- Get the current MODELVIEW matrix from OpenGL
    -- glGetFloatv( GL_MODELVIEW_MATRIX, modl ) 

    -- Combine the two matrices (multiply projection by modelview)
    clip(0) := modl(0) * proj(0) + modl(1) * proj(4) + modl(2) * proj(8) + modl(3) * proj(12);
    clip(1) := modl(0) * proj(1) + modl(1) * proj(5) + modl(2) * proj(9) + modl(3) * proj(13);
    clip(2) := modl(0) * proj(2) + modl(1) * proj(6) + modl(2) * proj(10) + modl(3) + proj(14);
    clip(3) := modl(0) * proj(3) + modl(1) * proj(7) + modl(2) * proj(11) + modl(3) + proj(15);

    clip(4) := modl(4) * proj(0) + modl(5) * proj(4) + modl(6) * proj(8) + modl(7) * proj(12);
    clip(5) := modl(4) * proj(1) + modl(5) * proj(5) + modl(6) * proj(9) + modl(7) * proj(13);
    clip(6) := modl(4) * proj(2) + modl(5) * proj(6) + modl(6) * proj(10) + modl(7) * proj(14);
    clip(7) := modl(4) * proj(3) + modl(5) * proj(7) + modl(6) * proj(11) + modl(7) * proj(15);

    clip(8) := modl(8) * proj(0) + modl(9) * proj(4) + modl(10) * proj(8) + modl(11) * proj(12);
    clip(9) := modl(8) * proj(1) + modl(9) * proj(5) + modl(10) * proj(9) + modl(11) * proj(13);
    clip(10) := modl(8) * proj(2) + modl(9) * proj(6) + modl(10) * proj(10) + modl(11) * proj(14);
    clip(11) := modl(8) * proj(3) + modl(9) * proj(7) + modl(10) * proj(11) + modl(11) * proj(15);

    clip(12) := modl(12) * proj(0) + modl(13) * proj(4) + modl(14) * proj(8) + modl(15) * proj(12);
    clip(13) := modl(12) * proj(1) + modl(13) * proj(5) + modl(14) * proj(9) + modl(15) * proj(13);
    clip(14) := modl(12) * proj(2) + modl(13) * proj(6) + modl(14) * proj(10) + modl(15) * proj(14);
    clip(15) := modl(12) * proj(3) + modl(13) * proj(7) + modl(14) * proj(11) + modl(15) * proj(15);

    -- Extract the numbers from the RIGHT plane
    frustum(0, 0) := clip(3) - clip(0);
    frustum(0, 1) := clip(7) - clip(4);
    frustum(0, 2) := clip(11) - clip(8);
    frustum(0, 3) := clip(15) - clip(12);

    -- Normalize the result
    t := Math.Sqrt( frustum(0, 0) * frustum(0, 0) + frustum(0, 1) * frustum(0, 1) + frustum(0, 2) * frustum(0, 2));
    frustum(0, 0) := frustum(0, 0) / t;
    frustum(0, 1) := frustum(0, 1) / t;
    frustum(0, 2) := frustum(0, 2) / t;
    frustum(0, 3) := frustum(0, 3) / t;

    -- Extract the numbers from the LEFT plane
    frustum(1, 0) := clip(3) + clip(0);
    frustum(1, 1) := clip(7) + clip(4);
    frustum(1, 2) := clip(11) + clip(8);
    frustum(1, 3) := clip(15) + clip(12);

    -- Normalize the result
    t := Math.Sqrt( frustum(1, 0) * frustum(1, 0) + frustum(1, 1) * frustum(1, 1) + frustum(1, 2) * frustum(1, 2) );
    frustum(1, 0) := frustum(1, 0) / t;
    frustum(1, 1) := frustum(1, 1) / t;
    frustum(1, 2) := frustum(1, 2) / t;
    frustum(1, 3) := frustum(1, 3) / t;

    -- Extract the BOTTOM plane
    frustum(2, 0) := clip(3) + clip(1);
    frustum(2, 1) := clip(7) + clip(5);
    frustum(2, 2) := clip(11) + clip(9);
    frustum(2, 3) := clip(15) + clip(13);

    -- Normalize the results
    t := Math.Sqrt( frustum(2, 0) * frustum(2, 0) + frustum(2, 1) * frustum(2, 1) + frustum(2, 2) * frustum(2, 2) ); 
    frustum(2, 0) := frustum(2, 0) / t;
    frustum(2, 1) := frustum(2, 1) / t;
    frustum(2, 2) := frustum(2, 2) / t;
    frustum(2, 3) := frustum(2, 3) / t;
   
   -- Extract the TOP plane
   frustum(3, 0) := clip(3) - clip(1);
   frustum(3, 1) := clip(7) - clip(5);
   frustum(3, 2) := clip(11) - clip(9);
   frustum(3, 3) := clip(15) - clip(13);

   -- Normalize the result
   t := Math.Sqrt( frustum(3, 0) * frustum(3, 0) + frustum(3, 1) * frustum(3, 1) + frustum(3, 2) * frustum(3, 2) );
   frustum(3, 0) := frustum(3, 0) / t;
   frustum(3, 1) := frustum(3, 1) / t;
   frustum(3, 2) := frustum(3, 2) / t;
   frustum(3, 3) := frustum(3, 3) / t;

   -- Extract the FAR plane
   frustum(4, 0) := clip(3) - clip(2);
   frustum(4, 1) := clip(7) - clip(6);
   frustum(4, 2) := clip(11) - clip(10);
   frustum(4, 3) := clip(15) - clip(14);

   -- Normalize the result
   t := Math.Sqrt( frustum(4, 0) * frustum(4, 0) + frustum(4, 1) * frustum(4, 1) + frustum(4, 2) * frustum(4, 2) );
   frustum(4, 0) := frustum(4, 0) / t;
   frustum(4, 1) := frustum(4, 1) / t;
   frustum(4, 2) := frustum(4, 2) / t;
   frustum(4, 3) := frustum(4, 3) / t;

   -- Extract the NEAR plane
   frustum(5, 0) := clip(3) + clip(2);
   frustum(5, 1) := clip(7) + clip(6);
   frustum(5, 2) := clip(11) + clip(10);
   frustum(5, 3) := clip(15) + clip(14);

   -- Normalize the result
   t := Math.Sqrt( frustum(5, 0) * frustum(5, 0) + frustum(5, 1) * frustum(5, 1) + frustum(5, 2) * frustum(5, 2) );
   frustum(5, 0) := frustum(5, 0) / t;
   frustum(5, 1) := frustum(5, 1) / t;
   frustum(5, 2) := frustum(5, 2) / t;
   frustum(5, 3) := frustum(5, 3) / t;

  End ExtractFrustum;



  Function PointInFrustum ( X : Float; Y : Float; Z : Float ) Return Boolean is

  Begin

    For position in 0 .. 5 Loop

      If frustum(position, 0) * X + frustum(position, 1) * Y + frustum(position, 2) * Z + frustum(position, 3) <= 0.0 Then
        
        Return False;

      End If;

    End Loop;

    Return True;

  End PointInFrustum;



  Function SphereInFrustum ( X : Float; Y : Float; Z : Float; Radius : Float ) Return Boolean is

  Begin

    For position in 0 .. 5 Loop

      If frustum(position, 0) * X + frustum(position, 1) * Y + frustum(position, 2) * Z + frustum(position, 3) <= (-1.0 * Radius) Then

        Return False;

      End If;

    End Loop;

    Return True;

  End SphereInFrustum;


  
  Function SphereInFrustum ( X : Float; Y : Float; Z : Float; Radius : Float ) Return Float is

    result : Float;

  Begin

    For position in 0 .. 5 Loop

      result := ((frustum(position, 0) * X) + (frustum(position, 1) * Y) + (frustum(position, 2) * Z) + (frustum(position, 3)));

      If (result <= (-1.0 * Radius)) Then
      
        Return 0.0;

      End If;

    End Loop;

    Return (result + Radius);

  End SphereInFrustum;



  Function CubeInFrustum ( X : Float; Y : Float; Z : Float; Size : Float ) Return Boolean is

  Begin

    For position in 0 .. 5 Loop

      If ( frustum(position, 0) * (X - Size) + frustum(position, 1) * (Y - Size) + frustum(position, 2) * (Z - Size) + frustum(position, 3) > 0.0 )
        or ( frustum(position, 0) * (X + Size) + frustum(position, 1) * (Y - Size) + frustum(position, 2) * (Z - Size) + frustum(position, 3) > 0.0 )
        or ( frustum(position, 0) * (X - Size) + frustum(position, 1) * (Y + Size) + frustum(position, 2) * (Z - Size) + frustum(position, 3) > 0.0 )
        or ( frustum(position, 0) * (X + Size) + frustum(position, 1) * (Y + Size) + frustum(position, 2) * (Z - Size) + frustum(position, 3) > 0.0 )
        or ( frustum(position, 0) * (X - Size) + frustum(position, 1) * (Y - Size) + frustum(position, 2) * (Z + Size) + frustum(position, 3) > 0.0 )
        or ( frustum(position, 0) * (X + Size) + frustum(position, 1) * (Y - Size) + frustum(position, 2) * (Z + Size) + frustum(position, 3) > 0.0 )
        or ( frustum(position, 0) * (X - Size) + frustum(position, 1) * (Y + Size) + frustum(position, 2) * (Z + Size) + frustum(position, 3) > 0.0 )
        or ( frustum(position, 0) * (X + Size) + frustum(position, 1) * (Y + Size) + frustum(position, 2) * (Z + Size) + frustum(position, 3) > 0.0 ) Then

          Return False;

      End If;

    End Loop;

    Return True;

  End CubeInFrustum;



  Function SphereInFrustum ( X : Float; Y : Float; Z : Float; Radius : Float ) Return Integer is

    c : Integer := 0;
    d : Float;

  Begin

    For position in 0 .. 5 Loop

      d := frustum(position, 0) * X + frustum(position, 1) * Y + frustum(position, 2) * Z + frustum(position, 3);

      If (d <= (-1.0 * Radius)) Then

        Return 0;

      Elsif ( d > Radius ) Then

        c := c + 1;

      End If;

    End Loop;

    If c = 6 Then
    
      Return 2;

    Else

      Return 1;

    End If;

  End SphereInFrustum;



  Function CubeInFrustum ( X : Float; Y : Float; Z : Float; Size : Float ) Return Integer is

    c   : Integer;
    c2  : Integer := 0;

  Begin

    For position in 0..5 Loop

      c := 0;

      If ( frustum(position, 0) * (X - Size) + frustum(position, 1) * (Y - Size) + frustum(position, 2) * (Z - Size) + frustum(position, 3) > 0.0 )
        or ( frustum(position, 0) * (X + Size) + frustum(position, 1) * (Y - Size) + frustum(position, 2) * (Z - Size) + frustum(position, 3) > 0.0 )
        or ( frustum(position, 0) * (X - Size) + frustum(position, 1) * (Y + Size) + frustum(position, 2) * (Z - Size) + frustum(position, 3) > 0.0 )
        or ( frustum(position, 0) * (X + Size) + frustum(position, 1) * (Y + Size) + frustum(position, 2) * (Z - Size) + frustum(position, 3) > 0.0 )
        or ( frustum(position, 0) * (X - Size) + frustum(position, 1) * (Y - Size) + frustum(position, 2) * (Z + Size) + frustum(position, 3) > 0.0 )
        or ( frustum(position, 0) + (X + Size) + frustum(position, 1) * (Y - Size) + frustum(position, 2) * (Z + Size) + frustum(position, 3) > 0.0 )
        or ( frustum(position, 0) + (X - Size) + frustum(position, 1) * (Y + Size) + frustum(position, 2) * (Z + Size) + frustum(position, 3) > 0.0 )
        or ( frustum(position, 0) + (X + Size) + frustum(position, 1) * (Y + Size) + frustum(position, 2) * (Z + Size) + frustum(position, 3) > 0.0 ) Then

          c := c + 1;

      Elsif c = 0 Then

        Return 0;

      Elsif c = 8 Then

        c2 := c2 + 1;

      End If;

    End Loop;

    If c2 = 6 Then

      Return 2;

    Else

      Return 1;

    End If;

  End CubeInFrustum;



  Function PolygonInFrustum ( NumPoints : Integer; Points : Pointlist ) Return Boolean is

  Begin

    For f in 0 .. 5 Loop

      For p in 0 .. NumPoints Loop
        
        Exit When (frustum(f, 0) * Points(p).x + frustum(f, 1) * Points(p).y + frustum(f, 2) * Points(p).z + frustum(f, 3) > 0.0);

        If p = NumPoints Then

          Return False;

        End If;

      End Loop;

    End Loop;

    Return True;

  End PolygonInFrustum;

End Frustum;