Package Frustum Is

  Type Float is Digits 18;

  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 CubeInFrustum ( X : Float; Y : Float; Z : Float; Size : Float ) Return Boolean;

Private

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

  frustum : Frustum_Array; 

End Frustum;


Package Body Frustum Is

  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 := 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 := 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 := 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 := 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 := 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 := 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;

      Return True;

    End Loop;

  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 * Radius) ) Then

        Return False;

      End If;

      Return True;

    End Loop;

  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 * 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

    return false;

  End CubeInFrustum;

End Frustum;