1. -- 
  2. -- Jan & Uwe R. Zimmer, Australia, 2013 
  3. -- 
  4.  
  5. with Ada.Containers;                    use Ada.Containers; 
  6. with Ada.Numerics;                      use Ada.Numerics; 
  7. with Ada.Numerics.Float_Random;         use Ada.Numerics.Float_Random; 
  8. with Ada.Real_Time;                     use Ada.Real_Time; 
  9. with Ada.Text_IO;                       use Ada.Text_IO; 
  10. with Graphics_Configuration;            use Graphics_Configuration; 
  11. with Rotations;                         use Rotations; 
  12. with Swarm_Data;                        use Swarm_Data; 
  13. with Vectors_Conversions;               use Vectors_Conversions; 
  14. with Vectors_3D_LF;                     use Vectors_3D_LF; 
  15. with Vehicle_Message_Type;              use Vehicle_Message_Type; 
  16. with Vehicle_Task_Type;                 use Vehicle_Task_Type; 
  17.  
  18. package body Swarm_Control is 
  19.  
  20.    use Real_Elementary_Functions; 
  21.    use Swarm_Vectors; 
  22.  
  23.    protected body Swarm_Monitor is 
  24.  
  25.       function Id_Task return Swarm_Element_Index is 
  26.  
  27.       begin 
  28.          for Element_Index in First_Index (Swarm_State) .. Last_Index (Swarm_State) loop 
  29.             if Element (Swarm_State, Element_Index).Process_Id = Current_Task then 
  30.                return (Element_Index); 
  31.             end if; 
  32.          end loop; 
  33.          raise No_Such_Task; 
  34.       end Id_Task; 
  35.  
  36.       function Id_Task (Id : Task_Id) return Swarm_Element_Index is 
  37.  
  38.       begin 
  39.          for Element_Index in First_Index (Swarm_State) .. Last_Index (Swarm_State) loop 
  40.             if Element (Swarm_State, Element_Index).Process_Id = Id then 
  41.                return (Element_Index); 
  42.             end if; 
  43.          end loop; 
  44.          raise No_Such_Task; 
  45.       end Id_Task; 
  46.  
  47.       function Position (Id : Task_Id) return Protected_Point_3D.Monitor_Ptr is 
  48.          (Element (Swarm_State, Swarm_Monitor.Id_Task (Current_Task)).Position); 
  49.  
  50.       function Velocity (Id : Task_Id) return Protected_Vector_3D.Monitor_Ptr is 
  51.          (Element (Swarm_State, Swarm_Monitor.Id_Task (Current_Task)).Velocity); 
  52.  
  53.       function Acceleration (Id : Task_Id) return Protected_Vector_3D.Monitor_Ptr is 
  54.         (Element (Swarm_State, Swarm_Monitor.Id_Task (Current_Task)).Acceleration); 
  55.  
  56.       function Controls (Id : Task_Id) return Vehicle_Controls_P is 
  57.          (Element (Swarm_State, Swarm_Monitor.Id_Task (Current_Task)).Controls); 
  58.  
  59.       function Comms (Id : Task_Id) return Vehicle_Comms_P is 
  60.         (Element (Swarm_State, Swarm_Monitor.Id_Task (Current_Task)).Comms); 
  61.  
  62.       function Charge (Id : Task_Id) return Charge_Info is 
  63.         (Element (Swarm_State, Swarm_Monitor.Id_Task (Current_Task)).Charge); 
  64.  
  65.       function Process_abort return Barrier_Ptr is 
  66.         (Element (Swarm_State, Swarm_Monitor.Id_Task).Process_abort); 
  67.       -- 
  68.       -- 
  69.       -- 
  70.  
  71.       procedure Append_Random_Swarm (No_Of_Swarm_Elements : Positive  := Initial_No_of_Elements; 
  72.                                      Centre               : Positions := Initial_Swarm_Position; 
  73.                                      Volume_Edge_Length   : Real      := Initual_Edge_Length) is 
  74.  
  75.          Random_Float : Generator; 
  76.  
  77.       begin 
  78.          Reset (Random_Float); 
  79.          Reserve_Capacity (Swarm_State, Length (Swarm_State) + Count_Type (No_Of_Swarm_Elements)); 
  80.          for i in 1 .. No_Of_Swarm_Elements loop 
  81.             select 
  82.                pragma Warnings (Off, "potentially blocking operation in protected operation"); 
  83.                delay Tolerated_Vehicle_Activation_Delay; 
  84.                pragma Warnings (On,  "potentially blocking operation in protected operation"); 
  85.                raise Vehicle_could_not_be_created; 
  86.             then abort 
  87.                declare 
  88.                   New_Element : Swarm_Element_State := 
  89.                     (Position      => Protected_Point_3D.Allocate 
  90.                        ((Centre (x) + (Real (Random (Random_Float)) * Volume_Edge_Length) - Volume_Edge_Length / 2.0, 
  91.                          Centre (y) + (Real (Random (Random_Float)) * Volume_Edge_Length) - Volume_Edge_Length / 2.0, 
  92.                          Centre (z) + (Real (Random (Random_Float)) * Volume_Edge_Length) - Volume_Edge_Length / 2.0)), 
  93.                      Rotation      => Protected_Rotation.Allocate (Zero_Rotation), 
  94.                      Velocity      => Protected_Vector_3D.Allocate (Zero_Vector_3D), 
  95.                      Acceleration  => Protected_Vector_3D.Allocate (Zero_Vector_3D), 
  96.                      Charge        => (Level => Full_Charge, Charge_Time => Protected_Time.Allocate (Clock), Charge_No => 0, Globes_Touched => No_Globes_Touched), 
  97.                      Neighbours    => new Distance_Vectors.Vector, 
  98.                      Controls      => new Vehicle_Controls, 
  99.                      Comms         => new Vehicle_Comms, 
  100.                      Process       => new Vehicle_Task, 
  101.                      Process_abort => new Barrier, 
  102.                      Process_Id    => Null_Task_Id, 
  103.                      Vehicle_Id    => Natural'Succ (Last_Vehicle_Id), 
  104.                      Last_Update   => Clock); 
  105.                begin 
  106.                   Last_Vehicle_Id := New_Element.Vehicle_Id; 
  107.                   pragma Warnings (Off, "potentially blocking operation in protected operation"); 
  108.                   -- Freshly created vehicle tasks need to respond to this call. 
  109.                   select 
  110.                      New_Element.Process.all.Identify (New_Element.Vehicle_Id, New_Element.Process_Id); 
  111.                   or delay Tolerated_Identify_Call_Delay; 
  112.                      raise Task_did_not_repond_to_Identfiy_Call; 
  113.                   end select; 
  114.                   pragma Warnings (On,  "potentially blocking operation in protected operation"); 
  115.                   Append (Swarm_State, New_Element); 
  116.                end; 
  117.             end select; 
  118.          end loop; 
  119.       end Append_Random_Swarm; 
  120.  
  121.       -- 
  122.       -- 
  123.       -- 
  124.  
  125.       procedure Remove_Vehicle (Element_Ix : Swarm_Element_Index) is 
  126.  
  127.       begin 
  128.          if Length (Swarm_State) > 1 and then Element_Ix >= First_Index (Swarm_State) and then Element_Ix <= Last_Index (Swarm_State) then 
  129.             declare 
  130.                This_Element : Swarm_Element_State := Element (Swarm_State, Element_Ix); 
  131.             begin 
  132.                Free_Process             (This_Element.Process); 
  133.                Free                     (This_Element.Process_abort); 
  134.                Free_Neighbours          (This_Element.Neighbours); 
  135.                Free_Comms               (This_Element.Comms); 
  136.                Free_Controls            (This_Element.Controls); 
  137.                Protected_Time.Free      (This_Element.Charge.Charge_Time); 
  138.                Protected_Point_3D.Free  (This_Element.Position); 
  139.                Protected_Rotation.Free  (This_Element.Rotation); 
  140.                Protected_Vector_3D.Free (This_Element.Velocity); 
  141.                Protected_Vector_3D.Free (This_Element.Acceleration); 
  142.                Delete (Swarm_State, Element_Ix); 
  143.             end; 
  144.          end if; 
  145.       end Remove_Vehicle; 
  146.  
  147.       -- 
  148.       -- 
  149.       -- 
  150.  
  151.       function Centre_Of_Gravity return Vector_3D is 
  152.  
  153.          Acc_Positions : Vector_3D_LF := Zero_Vector_3D_LF; 
  154.  
  155.       begin 
  156.          for Element_Index in First_Index (Swarm_State) .. Last_Index (Swarm_State) loop 
  157.             Acc_Positions := Acc_Positions + To_Vector_3D_LF (Element (Swarm_State, Element_Index).Position.all.Read); 
  158.          end loop; 
  159.          return To_Vector_3D ((1.0 / Long_Float (Length (Swarm_State))) * Acc_Positions); 
  160.       end Centre_Of_Gravity; 
  161.  
  162.       -- 
  163.       -- 
  164.       -- 
  165.  
  166.       function Mean_Velocity return Vector_3D is 
  167.  
  168.          Acc_Velocity : Vector_3D_LF := Zero_Vector_3D_LF; 
  169.  
  170.       begin 
  171.          for Element_Index in First_Index (Swarm_State) .. Last_Index (Swarm_State) loop 
  172.             Acc_Velocity := Acc_Velocity + To_Vector_3D_LF (Element (Swarm_State, Element_Index).Velocity.all.Read); 
  173.          end loop; 
  174.          return To_Vector_3D ((1.0 / Long_Float (Length (Swarm_State))) * Acc_Velocity); 
  175.       end Mean_Velocity; 
  176.  
  177.       -- 
  178.       -- 
  179.       -- 
  180.  
  181.       function Mean_Velocity return Real is 
  182.  
  183.          Acc_Velocity : Real := 0.0; 
  184.  
  185.       begin 
  186.          for Element_Index in First_Index (Swarm_State) .. Last_Index (Swarm_State) loop 
  187.             Acc_Velocity := Acc_Velocity + Real (abs (To_Vector_3D_LF (Element (Swarm_State, Element_Index).Velocity.all.Read))); 
  188.          end loop; 
  189.          return Real (Acc_Velocity / Real (Length (Swarm_State))); 
  190.       end Mean_Velocity; 
  191.  
  192.       -- 
  193.       -- 
  194.       -- 
  195.  
  196.       function Maximal_Radius return Real is 
  197.  
  198.          CoG    : constant Vector_3D := Centre_Of_Gravity; 
  199.  
  200.          Radius : Real := 0.0; 
  201.  
  202.       begin 
  203.          for Element_Index in First_Index (Swarm_State) .. Last_Index (Swarm_State) loop 
  204.             declare 
  205.                Distance_from_CoG : constant Real := abs (CoG - Element (Swarm_State, Element_Index).Position.all.Read); 
  206.             begin 
  207.                Radius := Real'Max (Radius, Distance_from_CoG); 
  208.             end; 
  209.          end loop; 
  210.          return Radius; 
  211.       end Maximal_Radius; 
  212.  
  213.       -- 
  214.       -- 
  215.       -- 
  216.  
  217.       function Mean_Radius return Real is 
  218.  
  219.          CoG : constant Vector_3D := Centre_Of_Gravity; 
  220.  
  221.          Acc_Radius : Real := 0.0; 
  222.  
  223.       begin 
  224.          for Element_Index in First_Index (Swarm_State) .. Last_Index (Swarm_State) loop 
  225.             declare 
  226.                Distance_from_CoG : constant Real := abs (CoG - Element (Swarm_State, Element_Index).Position.all.Read); 
  227.             begin 
  228.                Acc_Radius := Acc_Radius + Distance_from_CoG; 
  229.             end; 
  230.          end loop; 
  231.          return Real (Acc_Radius / Real (Length (Swarm_State))); 
  232.       end Mean_Radius; 
  233.  
  234.       -- 
  235.       -- 
  236.       -- 
  237.  
  238.       function Mean_Closest_Distance return Real is 
  239.  
  240.          Acc_Distance : Real := 0.0; 
  241.  
  242.       begin 
  243.          for Element_Index in First_Index (Swarm_State) .. Last_Index (Swarm_State) loop 
  244.             declare 
  245.                This_Element     : constant Swarm_Element_State     := Element (Swarm_State, Element_Index); 
  246.                Neighbours       : constant Distance_Vectors.Vector := This_Element.Neighbours.all; 
  247.             begin 
  248.                if Distance_Vectors.Length (Neighbours) > 0 then 
  249.                   declare 
  250.                      Closest_Distance : constant Real                    := 
  251.                        Distance_Vectors.Element (Neighbours, Distance_Vectors.First_Index (Neighbours)).Distance; 
  252.                   begin 
  253.                      Acc_Distance := Acc_Distance + Closest_Distance; 
  254.                   end; 
  255.                end if; 
  256.             end; 
  257.          end loop; 
  258.          return Real (Acc_Distance / Real (Length (Swarm_State))); 
  259.       end Mean_Closest_Distance; 
  260.  
  261.       -- 
  262.  
  263.    end Swarm_Monitor; 
  264.  
  265.    -- 
  266.    -- 
  267.    -- 
  268.  
  269.    procedure Remove_Vehicle_in_Stages (Element_Ix : Swarm_Element_Index) is 
  270.  
  271.       Tolerated_Termination_Time : constant Duration := To_Duration (Milliseconds (100)); 
  272.  
  273.    begin 
  274.       Element (Swarm_State, Element_Ix).Process_abort.all.Open; 
  275.       select 
  276.          delay Tolerated_Termination_Time; 
  277.          Put_Line (Current_Error, "Warning: Vehicle task termination request ignored - attempting task abort now"); 
  278.          select 
  279.             delay Tolerated_Termination_Time; 
  280.             Put_Line (Current_Error, "Error: Vehicle task stuck in non-abortable code region - task abort failed"); 
  281.          then abort 
  282.             Abort_Task (Element (Swarm_State, Element_Ix).Process_Id); 
  283.             loop 
  284.                exit when Is_Terminated (Element (Swarm_State, Element_Ix).Process_Id); 
  285.                delay 0.0; 
  286.             end loop; 
  287.          end select; 
  288.       then abort 
  289.          loop 
  290.             exit when Is_Terminated (Element (Swarm_State, Element_Ix).Process_Id); 
  291.             delay 0.0; 
  292.          end loop; 
  293.       end select; 
  294.       Swarm_Monitor.Remove_Vehicle (Element_Ix); 
  295.    end Remove_Vehicle_in_Stages; 
  296.  
  297.    procedure Remove_Vehicles (No_Of_Swarm_Elements : Positive  := 1) is 
  298.  
  299.    begin 
  300.       if Natural (Length (Swarm_State)) >= No_Of_Swarm_Elements then 
  301.          for Element_Index in Last_Index (Swarm_State) - No_Of_Swarm_Elements + 1 .. Last_Index (Swarm_State) loop 
  302.             Remove_Vehicle_in_Stages (Element_Index); 
  303.          end loop; 
  304.       end if; 
  305.    end Remove_Vehicles; 
  306.  
  307.    -- 
  308.    -- 
  309.    -- 
  310.  
  311.    procedure Sorted_Close_Distances (Close_Dist    : in out Distance_Vectors.Vector; 
  312.                                      Element_Index : Swarm_Element_Index; 
  313.                                      Max_Distance  : Distances) is 
  314.  
  315.       This_Element  : constant Swarm_Element_State := Element (Swarm_State, Element_Index); 
  316.       This_Position : constant Positions           := This_Element.Position.all.Read; 
  317.  
  318.    begin 
  319.       Distance_Vectors.Clear (Close_Dist); 
  320.       Distance_Vectors.Reserve_Capacity (Close_Dist, Length (Swarm_State) - 1); 
  321.       for Scan_Index in First_Index (Swarm_State) .. Last_Index (Swarm_State) loop 
  322.          if Element_Index /= Scan_Index then 
  323.             declare 
  324.                Test_Element   : constant Swarm_Element_State := Element (Swarm_State, Scan_Index); 
  325.                Test_Position  : constant Positions           := Test_Element.Position.all.Read; 
  326.                Test_Direction : constant Vector_3D           := This_Position - Test_Position; 
  327.                Test_Distance  : constant Distances           := abs (Test_Direction); 
  328.             begin 
  329.                if Test_Distance <= Max_Distance then 
  330.                   Distance_Vectors.Append (Close_Dist, (Index         => Scan_Index, 
  331.                                                         Distance      => Test_Distance, 
  332.                                                         Position_Diff => Test_Direction, 
  333.                                                         Velocity_Diff => This_Element.Velocity.all.Read - Test_Element.Velocity.all.Read)); 
  334.                end if; 
  335.             end; 
  336.          end if; 
  337.       end loop; 
  338.       Sort_Distances.Sort (Close_Dist); 
  339.    end Sorted_Close_Distances; 
  340.  
  341.    -- 
  342.    -- 
  343.    -- 
  344.  
  345.    procedure Set_Acceleration (Element_Index : Swarm_Element_Index) is 
  346.  
  347.       This_Element  : constant Swarm_Element_State := Element (Swarm_State, Element_Index); 
  348.       Acceleration  :          Accelerations       := Zero_Vector_3D; 
  349.  
  350.    begin 
  351.       Sorted_Close_Distances (This_Element.Neighbours.all, Element_Index, Detection_Range); 
  352.       for Distance_Index in Distance_Vectors.First_Index (This_Element.Neighbours.all) .. Distance_Vectors.Last_Index (This_Element.Neighbours.all) loop 
  353.          declare 
  354.             Distance_Entry : constant Distance_Entries := Distance_Vectors.Element (This_Element.Neighbours.all, Distance_Index); 
  355.          begin 
  356.  
  357.             -- Uncontrolled vehicles 
  358.             if This_Element.Controls.all.Read_Throttle = 0.0 then 
  359.  
  360.                -- Attraction and repulsion forces between vehicles 
  361.                Acceleration := Acceleration + Inter_Swarm_Acceleration (Distance_Entry.Distance) * Norm (Distance_Entry.Position_Diff); 
  362.  
  363.                -- Alignment forces 
  364.                if Distance_Entry.Distance <= Velocity_Matching_Range then 
  365.                   Acceleration := Acceleration + Velocity_Matching (This_Element.Velocity.all.Read, Distance_Entry.Velocity_Diff); 
  366.                end if; 
  367.  
  368.                -- Controlled vehicles 
  369.             elsif Distance_Entry.Distance <= Unconditional_Repulse_Dist then 
  370.  
  371.                -- Unconditional repulsion for controlled vehicles 
  372.                Acceleration := Acceleration + Inter_Swarm_Repulsion (Distance_Entry.Distance) * Norm (Distance_Entry.Position_Diff); 
  373.  
  374.             end if; 
  375.  
  376.          end; 
  377.       end loop; 
  378.  
  379.       -- Controlled vehicles 
  380.       if This_Element.Controls.all.Read_Throttle /= 0.0 then 
  381.  
  382.          -- Approach the set target 
  383.          declare 
  384.             Target_Vector                     : constant Vector_3D := This_Element.Controls.all.Read_Steering - This_Element.Position.all.Read; 
  385.             Norm_Target_Vector                : constant Vector_3D := Norm (Target_Vector); 
  386.             Abs_Target_Vector                 : constant Real := abs (Target_Vector); 
  387.             Abs_Velocity                      : constant Real := abs (This_Element.Velocity.all.Read); 
  388.             Angle_Between_Target_and_Velocity : constant Real := Angle_Between (Target_Vector, This_Element.Velocity.all.Read); 
  389.          begin 
  390.             if Abs_Target_Vector < Target_Fetch_Range then 
  391.                -- Target reached, switch to idle throttle 
  392.                This_Element.Controls.all.Set_Throttle (Idle_Throttle); 
  393.             else 
  394.                -- Accelerate to constant speed towards target, dampen lateral velocities 
  395.                Acceleration := Acceleration 
  396.                  + (This_Element.Controls.all.Read_Throttle 
  397.                     * Approach_Acceleration (Abs_Velocity * Cos (Angle_Between_Target_and_Velocity)) 
  398.                     * Norm_Target_Vector) 
  399.                  - Norm (This_Element.Velocity.all.Read) * (Intented_Framerate / 5.0) * Abs_Velocity * Sin (Angle_Between_Target_and_Velocity); 
  400.             end if; 
  401.          end; 
  402.       end if; 
  403.  
  404.       -- Friction 
  405.       This_Element.Acceleration.all.Write (Acceleration - Norm (This_Element.Velocity.all.Read) * (abs (This_Element.Velocity.all.Read) * Friction)**2); 
  406.  
  407.       Replace_Element (Swarm_State, Element_Index, This_Element); 
  408.    end Set_Acceleration; 
  409.  
  410.    -- 
  411.    -- 
  412.    -- 
  413.  
  414.    procedure Set_All_Accelerations is 
  415.  
  416.    begin 
  417.       for Element_Index in First_Index (Swarm_State) .. Last_Index (Swarm_State) loop 
  418.          Set_Acceleration (Element_Index); 
  419.       end loop; 
  420.    end Set_All_Accelerations; 
  421.  
  422.    -- 
  423.    -- 
  424.    -- 
  425.  
  426.    procedure Forward_Messages (Element_Index : Swarm_Element_Index) is 
  427.  
  428.       This_Element              : constant Swarm_Element_State := Element (Swarm_State, Element_Index); 
  429.       Message_To_Be_Distributed : Inter_Vehicle_Messages; 
  430.  
  431.    begin 
  432.       while This_Element.Comms.all.Has_Outgoing_Messages loop 
  433.          This_Element.Comms.all.Fetch_Message (Message_To_Be_Distributed); 
  434.          Check_Neighbours : for Distance_Index in Distance_Vectors.First_Index (This_Element.Neighbours.all) .. Distance_Vectors.Last_Index (This_Element.Neighbours.all) loop 
  435.             declare 
  436.                Distance_Entry : constant Distance_Entries := Distance_Vectors.Element (This_Element.Neighbours.all, Distance_Index); 
  437.             begin 
  438.                if Distance_Entry.Distance <= Comms_Range then 
  439.                   Element (Swarm_State, Distance_Entry.Index).Comms.all.Push_Message (Message_To_Be_Distributed); 
  440.                else 
  441.                   exit Check_Neighbours; 
  442.                end if; 
  443.             end; 
  444.          end loop Check_Neighbours; 
  445.       end loop; 
  446.    end Forward_Messages; 
  447.  
  448.    -- 
  449.    -- 
  450.    -- 
  451.  
  452.    procedure Forward_All_Messages is 
  453.  
  454.    begin 
  455.       for Element_Index in First_Index (Swarm_State) .. Last_Index (Swarm_State) loop 
  456.          Forward_Messages (Element_Index); 
  457.       end loop; 
  458.    end Forward_All_Messages; 
  459.  
  460.    -- 
  461.    -- 
  462.    -- 
  463.  
  464.    procedure Move_Element (Element_Index : Swarm_Element_Index) is 
  465.  
  466.       This_Element : Swarm_Element_State := Element (Swarm_State, Element_Index); 
  467.       Interval     : constant Real       := Real'Min (Real (To_Duration (Clock - This_Element.Last_Update)), Max_Update_Interval); 
  468.  
  469.    begin 
  470.       This_Element.Velocity.all.Write (This_Element.Velocity.all.Read + (Interval * This_Element.Acceleration.all.Read)); 
  471.  
  472.       declare 
  473.          Move_Start : constant Positions := This_Element.Position.all.Read; 
  474.          Move_End   : constant Positions := Move_Start + (Interval * This_Element.Velocity.all.Read); 
  475.       begin 
  476.          This_Element.Position.all.Write (Move_End); 
  477.  
  478.          This_Element.Charge.Level := Vehicle_Charges 
  479.            (Real'Max (Real (Empty_Charge), 
  480.             Real'Min (Real (Full_Charge), 
  481.               Real (This_Element.Charge.Level) 
  482.               - (Interval 
  483.                 * (Charging_Setup.Constant_Discharge_Rate_Per_Sec 
  484.                   + Charging_Setup.Propulsion_Discharge_Rate_Per_Sec * abs (This_Element.Acceleration.all.Read)))))); 
  485.  
  486.          for Globe_Ix in Globes'Range loop 
  487.  
  488.             declare 
  489.                Globe_Pos    : constant Positions := Globes (Globe_Ix).Position.all.Read; 
  490.                Interratio   : constant Real      := (Globe_Pos - Move_Start) * ((Move_End - Move_Start) / (abs (Move_End - Move_Start))); 
  491.                Intersection : constant Positions := Move_Start + Interratio * (Move_End - Move_Start); 
  492.                Touching     : constant Boolean   := Globes (Globe_Ix).Active and then abs (Intersection - Globe_Pos) <= Energy_Globe_Detection and then Interratio >= 0.0 and then Interratio <= 1.0; 
  493.                Slot_Passed  : constant Boolean   := Clock - This_Element.Charge.Charge_Time.all.Read > Charging_Setup.Max_Globe_Interval; 
  494.             begin 
  495.                if (not This_Element.Charge.Globes_Touched (Globe_Ix) or else Slot_Passed) and then Touching then 
  496.  
  497.                   if Slot_Passed then 
  498.                      This_Element.Charge.Globes_Touched := No_Globes_Touched; 
  499.                      This_Element.Charge.Charge_No      := 0; 
  500.                   end if; 
  501.  
  502.                   This_Element.Charge.Charge_No                 := This_Element.Charge.Charge_No + 1; 
  503.                   This_Element.Charge.Globes_Touched (Globe_Ix) := True; 
  504.                   This_Element.Charge.Charge_Time.all.Write (Clock); 
  505.  
  506.                   if This_Element.Charge.Charge_No = Charging_Setup.Globes_Required then 
  507.                      This_Element.Charge.Level          := Full_Charge; 
  508.                      This_Element.Charge.Charge_No      := 0; 
  509.                      This_Element.Charge.Globes_Touched := No_Globes_Touched; 
  510.                   end if; 
  511.  
  512.                end if; 
  513.             end; 
  514.          end loop; 
  515.       end; 
  516.  
  517.       This_Element.Last_Update := Clock; 
  518.       Replace_Element (Swarm_State, Element_Index, This_Element); 
  519.    end Move_Element; 
  520.  
  521.    -- 
  522.    -- 
  523.    -- 
  524.  
  525.    procedure Move_All_Elements is 
  526.  
  527.    begin 
  528.       for Element_Index in First_Index (Swarm_State) .. Last_Index (Swarm_State) loop 
  529.          Move_Element (Element_Index); 
  530.       end loop; 
  531.    end Move_All_Elements; 
  532.  
  533.    -- 
  534.    -- 
  535.    -- 
  536.  
  537.    procedure Update_Rotation (Element_Index : Swarm_Element_Index) is 
  538.  
  539.       function Vector_Yaw (In_Vector : Vector_3D) return Real is 
  540.         (if In_Vector (x) = 0.0 and then In_Vector (z) = 0.0 
  541.          then 0.0 
  542.          else Arctan (In_Vector (x), In_Vector (z))); 
  543.  
  544.       function Vector_Pitch (In_Vector : Vector_3D) return Real is 
  545.         ((Pi / 2.0) - Angle_Between (In_Vector, (0.0, 1.0, 0.0))); 
  546.  
  547.       This_Element : constant Swarm_Element_State := Element (Swarm_State, Element_Index); 
  548.  
  549.       Velocity      : constant Vector_3D := This_Element.Velocity.all.Read; 
  550.       Element_Yaw   : constant Real      := Vector_Yaw   (Velocity); 
  551.       Element_Pitch : constant Real      := Vector_Pitch (Velocity); 
  552.  
  553.       Rotation      : constant Quaternion_Rotation := To_Rotation (0.0, -Element_Pitch, Element_Yaw + Pi); 
  554.       Norm_Acc      : constant Vector_3D := Rotate (This_Element.Acceleration.all.Read, Rotation); 
  555.       Lateral_Acc   : constant Real      := Norm_Acc (x) * abs (Velocity); 
  556.       Element_Roll  : constant Real := 
  557.         Real'Max (-Pi / 2.0, 
  558.                   Real'Min (Pi / 2.0, 
  559.                     Lateral_Acc * (Pi / 2.0) / Max_Assumed_Acceleration)); 
  560.  
  561.    begin 
  562.       This_Element.Rotation.all.Write (To_Rotation (Element_Roll, -Element_Pitch, -Element_Yaw + Pi)); 
  563.       Replace_Element (Swarm_State, Element_Index, This_Element); 
  564.    end Update_Rotation; 
  565.  
  566.    --- 
  567.    --- 
  568.    --- 
  569.  
  570.    procedure Update_All_Rotations is 
  571.  
  572.    begin 
  573.       for Element_Index in First_Index (Swarm_State) .. Last_Index (Swarm_State) loop 
  574.          Update_Rotation (Element_Index); 
  575.       end loop; 
  576.    end Update_All_Rotations; 
  577.  
  578.    -- 
  579.    -- 
  580.    -- 
  581.  
  582.    procedure Remove_Empties is 
  583.  
  584.    begin 
  585.       if Length (Swarm_State) > 1 then 
  586.          declare 
  587.             Element_Index : Swarm_Element_Index := First_Index (Swarm_State); 
  588.          begin 
  589.             while Element_Index <= Last_Index (Swarm_State) and then Length (Swarm_State) > 1 loop 
  590.                if Element (Swarm_State, Element_Index).Charge.Level = Empty_Charge then 
  591.                   Remove_Vehicle_in_Stages (Element_Index); 
  592.                else 
  593.                   Element_Index := Element_Index + 1; 
  594.                end if; 
  595.             end loop; 
  596.          end; 
  597.       end if; 
  598.    end Remove_Empties; 
  599.  
  600. end Swarm_Control;