with Ada.Containers; use Ada.Containers;
with Ada.Numerics; use Ada.Numerics;
with Ada.Numerics.Elementary_Functions; use Ada.Numerics.Elementary_Functions;
with Ada.Numerics.Float_Random; use Ada.Numerics.Float_Random;
with Ada.Real_Time; use Ada.Real_Time;
with Ada.Task_Identification; use Ada.Task_Identification;
with Graphics_Configuration; use Graphics_Configuration;
with Rotations; use Rotations;
with Swarm_Data; use Swarm_Data;
with Vectors_Conversions; use Vectors_Conversions;
with Vectors_3D_LF; use Vectors_3D_LF;
with Vehicle_Message_Type; use Vehicle_Message_Type;
with Vehicle_Task_Type; use Vehicle_Task_Type;
package body Swarm_Control is
use Swarm_Vectors;
procedure Append_Random_Swarm (No_Of_Swarm_Elements : Positive := Initial_No_of_Elements;
Centre : Positions := Initial_Swarm_Position;
Volume_Edge_Length : Real := Initual_Edge_Length) is
Random_Float : Generator;
begin
Swarm_Lock.Lock_Write;
Reset (Random_Float);
Reserve_Capacity (Swarm_State, Length (Swarm_State) + Count_Type (No_Of_Swarm_Elements));
for i in 1 .. No_Of_Swarm_Elements loop
declare
New_Element : Swarm_Element_State :=
(Position => (Centre (x) + (Random (Random_Float) * Volume_Edge_Length) - Volume_Edge_Length / 2.0,
Centre (y) + (Random (Random_Float) * Volume_Edge_Length) - Volume_Edge_Length / 2.0,
Centre (z) + (Random (Random_Float) * Volume_Edge_Length) - Volume_Edge_Length / 2.0),
Rotation => Zero_Rotation,
Velocity => Zero_Vector_3D,
Acceleration => Zero_Vector_3D,
Charge => (Level => Full_Charge, Charge_Time => Clock, Charge_No => 0, Globes_Touched => No_Globes_Touched),
Neighbours => new Distance_Vectors.Vector,
Controls => new Vehicle_Controls,
Comms => new Vehicle_Comms,
Process => new Vehicle_Task,
Process_Id => Null_Task_Id,
Last_Update => Clock);
begin
New_Element.Process.Identify (Swarm_Element_Index (Natural (Length (Swarm_State)) + 1), New_Element.Process_Id);
Append (Swarm_State, New_Element);
end;
end loop;
Swarm_Lock.Unlock_Write;
end Append_Random_Swarm;
procedure Remove_Vehicle (Element_Ix : Swarm_Element_Index) is
begin
Swarm_Lock.Lock_Write;
if Length (Swarm_State) > 1 and then Element_Ix >= First_Index (Swarm_State) and then Element_Ix <= Last_Index (Swarm_State) then
declare
This_Element : Swarm_Element_State := Element (Swarm_State, Element_Ix);
begin
abort This_Element.Process.all;
loop
exit when Is_Terminated (This_Element.Process_Id);
end loop;
Free_Process (This_Element.Process);
Free_Neighbours (This_Element.Neighbours);
Free_Comms (This_Element.Comms);
Free_Controls (This_Element.Controls);
Delete (Swarm_State, Element_Ix);
end;
end if;
Swarm_Lock.Unlock_Write;
end Remove_Vehicle;
procedure Remove_Vehicles (No_Of_Swarm_Elements : Positive := 1) is
begin
if Natural (Length (Swarm_State)) >= No_Of_Swarm_Elements then
for Element_Index in Last_Index (Swarm_State) - No_Of_Swarm_Elements + 1 .. Last_Index (Swarm_State) loop
Remove_Vehicle (Element_Index);
end loop;
end if;
end Remove_Vehicles;
procedure Sorted_Close_Distances (Close_Dist : in out Distance_Vectors.Vector;
Element_Index : Swarm_Element_Index;
Max_Distance : Distances) is
This_Element : constant Swarm_Element_State := Element (Swarm_State, Element_Index);
This_Position : constant Positions := This_Element.Position;
begin
Distance_Vectors.Clear (Close_Dist);
Distance_Vectors.Reserve_Capacity (Close_Dist, Length (Swarm_State) - 1);
for Scan_Index in First_Index (Swarm_State) .. Last_Index (Swarm_State) loop
if Element_Index /= Scan_Index then
declare
Test_Element : constant Swarm_Element_State := Element (Swarm_State, Scan_Index);
Test_Position : constant Positions := Test_Element.Position;
Test_Direction : constant Vector_3D := This_Position - Test_Position;
Test_Distance : constant Distances := abs (Test_Direction);
begin
if Test_Distance <= Max_Distance then
Distance_Vectors.Append (Close_Dist, (Index => Scan_Index,
Distance => Test_Distance,
Position_Diff => Test_Direction,
Velocity_Diff => This_Element.Velocity - Test_Element.Velocity));
end if;
end;
end if;
end loop;
Sort_Distances.Sort (Close_Dist);
end Sorted_Close_Distances;
procedure Set_Acceleration (Element_Index : Swarm_Element_Index) is
This_Element : Swarm_Element_State := Element (Swarm_State, Element_Index);
Acceleration : Accelerations := Zero_Vector_3D;
begin
Sorted_Close_Distances (This_Element.Neighbours.all, Element_Index, Detection_Range);
for Distance_Index in Distance_Vectors.First_Index (This_Element.Neighbours.all) .. Distance_Vectors.Last_Index (This_Element.Neighbours.all) loop
declare
Distance_Entry : constant Distance_Entries := Distance_Vectors.Element (This_Element.Neighbours.all, Distance_Index);
begin
if This_Element.Controls.Read_Throttle = 0.0 then
Acceleration := Acceleration + Inter_Swarm_Acceleration (Distance_Entry.Distance) * Norm (Distance_Entry.Position_Diff);
if Distance_Entry.Distance <= Velocity_Matching_Range then
Acceleration := Acceleration + Velocity_Matching (This_Element.Velocity, Distance_Entry.Velocity_Diff);
end if;
elsif Distance_Entry.Distance <= Unconditional_Repulse_Dist then
Acceleration := Acceleration + Inter_Swarm_Repulsion (Distance_Entry.Distance) * Norm (Distance_Entry.Position_Diff);
end if;
end;
end loop;
if This_Element.Controls.Read_Throttle /= 0.0 then
declare
Target_Vector : constant Vector_3D := This_Element.Controls.Read_Steering - This_Element.Position;
Norm_Target_Vector : constant Vector_3D := Norm (Target_Vector);
Abs_Target_Vector : constant Real := abs (Target_Vector);
Abs_Velocity : constant Real := abs (This_Element.Velocity);
Angle_Between_Target_and_Velocity : constant Real := Angle_Between (Target_Vector, This_Element.Velocity);
begin
if Abs_Target_Vector < Target_Fetch_Range then
This_Element.Controls.Set_Throttle (Idle_Throttle);
else
Acceleration := Acceleration
+ (This_Element.Controls.Read_Throttle
* Approach_Acceleration (Abs_Velocity * Cos (Angle_Between_Target_and_Velocity))
* Norm_Target_Vector)
- Norm (This_Element.Velocity) * (Intented_Framerate / 5.0) * Abs_Velocity * Sin (Angle_Between_Target_and_Velocity);
end if;
end;
end if;
This_Element.Acceleration := Acceleration - Norm (This_Element.Velocity) * (abs (This_Element.Velocity) * Friction)**2;
Replace_Element (Swarm_State, Element_Index, This_Element);
end Set_Acceleration;
procedure Set_All_Accelerations is
begin
for Element_Index in First_Index (Swarm_State) .. Last_Index (Swarm_State) loop
Set_Acceleration (Element_Index);
end loop;
end Set_All_Accelerations;
procedure Forward_Messages (Element_Index : Swarm_Element_Index) is
This_Element : constant Swarm_Element_State := Element (Swarm_State, Element_Index);
Message_To_Be_Distributed : Inter_Vehicle_Messages;
begin
while This_Element.Comms.Has_Outgoing_Messages loop
This_Element.Comms.Fetch_Message (Message_To_Be_Distributed);
Check_Neighbours : for Distance_Index in Distance_Vectors.First_Index (This_Element.Neighbours.all) .. Distance_Vectors.Last_Index (This_Element.Neighbours.all) loop
declare
Distance_Entry : constant Distance_Entries := Distance_Vectors.Element (This_Element.Neighbours.all, Distance_Index);
begin
if Distance_Entry.Distance <= Comms_Range then
Element (Swarm_State, Distance_Entry.Index).Comms.Push_Message (Message_To_Be_Distributed);
else
exit Check_Neighbours;
end if;
end;
end loop Check_Neighbours;
end loop;
end Forward_Messages;
procedure Forward_All_Messages is
begin
for Element_Index in First_Index (Swarm_State) .. Last_Index (Swarm_State) loop
Forward_Messages (Element_Index);
end loop;
end Forward_All_Messages;
procedure Move_Element (Element_Index : Swarm_Element_Index) is
This_Element : Swarm_Element_State := Element (Swarm_State, Element_Index);
Interval : constant Float := Float'Min (Float (To_Duration (Clock - This_Element.Last_Update)),
Max_Update_Interval);
begin
This_Element.Velocity := This_Element.Velocity + (Interval * This_Element.Acceleration);
This_Element.Position := This_Element.Position + (Interval * This_Element.Velocity);
This_Element.Charge.Level := Vehicle_Charges
(Float'Max (Float (Empty_Charge),
Float'Min (Float (Full_Charge),
Float (This_Element.Charge.Level) - (Interval * Charging_Setup.Discharge_Rate_Per_Sec))));
for Globe_Ix in Globes'Range loop
if not This_Element.Charge.Globes_Touched (Globe_Ix)
and then abs (This_Element.Position - Globes (Globe_Ix).Position) <= Energy_Globe_Detection then
if Clock - This_Element.Charge.Charge_Time > Charging_Setup.Max_Globe_Interval then
This_Element.Charge.Globes_Touched := No_Globes_Touched;
This_Element.Charge.Charge_No := 0;
end if;
This_Element.Charge.Charge_No := This_Element.Charge.Charge_No + 1;
This_Element.Charge.Globes_Touched (Globe_Ix) := True;
This_Element.Charge.Charge_Time := Clock;
if This_Element.Charge.Charge_No = Charging_Setup.Globes_Required then
This_Element.Charge.Level := Full_Charge;
This_Element.Charge.Charge_No := 0;
This_Element.Charge.Globes_Touched := No_Globes_Touched;
end if;
end if;
end loop;
This_Element.Last_Update := Clock;
Replace_Element (Swarm_State, Element_Index, This_Element);
end Move_Element;
procedure Move_All_Elements is
begin
for Element_Index in First_Index (Swarm_State) .. Last_Index (Swarm_State) loop
Move_Element (Element_Index);
end loop;
end Move_All_Elements;
procedure Update_Rotation (Element_Index : Swarm_Element_Index) is
function Vector_Yaw (In_Vector : Vector_3D) return Float is
begin
if In_Vector (x) = 0.0 and then In_Vector (z) = 0.0 then
return 0.0;
else
return Arctan (In_Vector (x), In_Vector (z));
end if;
end Vector_Yaw;
function Vector_Pitch (In_Vector : Vector_3D) return Float is
begin
return (Pi / 2.0) - Angle_Between (In_Vector, (0.0, 1.0, 0.0));
end Vector_Pitch;
This_Element : Swarm_Element_State := Element (Swarm_State, Element_Index);
Velocity : constant Vector_3D := This_Element.Velocity;
Element_Yaw : constant Real := Vector_Yaw (Velocity);
Element_Pitch : constant Real := Vector_Pitch (Velocity);
Rotation : constant Quaternion_Rotation := To_Rotation (0.0, -Element_Pitch, Element_Yaw + Pi);
Norm_Acc : constant Vector_3D := Rotate (This_Element.Acceleration, Rotation);
Lateral_Acc : constant Real := Norm_Acc (x) * abs (Velocity);
Element_Roll : constant Real :=
Real'Max (-Pi / 2.0,
Real'Min (Pi / 2.0,
Lateral_Acc * (Pi / 2.0) / Max_Assumed_Acceleration));
begin
This_Element.Rotation := To_Rotation (Element_Roll, -Element_Pitch, -Element_Yaw + Pi);
Replace_Element (Swarm_State, Element_Index, This_Element);
end Update_Rotation;
procedure Update_All_Rotations is
begin
for Element_Index in First_Index (Swarm_State) .. Last_Index (Swarm_State) loop
Update_Rotation (Element_Index);
end loop;
end Update_All_Rotations;
procedure Remove_Empties is
begin
if Length (Swarm_State) > 1 then
declare
Element_Index : Swarm_Element_Index := First_Index (Swarm_State);
begin
while Element_Index <= Last_Index (Swarm_State) and then Length (Swarm_State) > 1 loop
if Element (Swarm_State, Element_Index).Charge.Level = Empty_Charge then
Remove_Vehicle (Element_Index);
else
Element_Index := Element_Index + 1;
end if;
end loop;
end;
end if;
end Remove_Empties;
function Centre_Of_Gravity return Vector_3D is
Acc_Positions : Vector_3D_LF := Zero_Vector_3D_LF;
begin
Swarm_Lock.Lock_Read;
for Element_Index in First_Index (Swarm_State) .. Last_Index (Swarm_State) loop
Acc_Positions := Acc_Positions + To_Vector_3D_LF (Element (Swarm_State, Element_Index).Position);
end loop;
Swarm_Lock.Unlock_Read;
return To_Vector_3D ((1.0 / Long_Float (Length (Swarm_State))) * Acc_Positions);
end Centre_Of_Gravity;
function Mean_Velocity return Vector_3D is
Acc_Velocity : Vector_3D_LF := Zero_Vector_3D_LF;
begin
Swarm_Lock.Lock_Read;
for Element_Index in First_Index (Swarm_State) .. Last_Index (Swarm_State) loop
Acc_Velocity := Acc_Velocity + To_Vector_3D_LF (Element (Swarm_State, Element_Index).Velocity);
end loop;
Swarm_Lock.Unlock_Read;
return To_Vector_3D ((1.0 / Long_Float (Length (Swarm_State))) * Acc_Velocity);
end Mean_Velocity;
function Mean_Velocity return Real is
Acc_Velocity : Long_Float := 0.0;
begin
Swarm_Lock.Lock_Read;
for Element_Index in First_Index (Swarm_State) .. Last_Index (Swarm_State) loop
Acc_Velocity := Acc_Velocity + abs (To_Vector_3D_LF (Element (Swarm_State, Element_Index).Velocity));
end loop;
Swarm_Lock.Unlock_Read;
return Real (Acc_Velocity / Long_Float (Length (Swarm_State)));
end Mean_Velocity;
function Maximal_Radius return Real is
CoG : constant Vector_3D := Centre_Of_Gravity;
Radius : Real := 0.0;
begin
Swarm_Lock.Lock_Read;
for Element_Index in First_Index (Swarm_State) .. Last_Index (Swarm_State) loop
declare
Distance_from_CoG : constant Real := abs (CoG - Element (Swarm_State, Element_Index).Position);
begin
Radius := Real'Max (Radius, Distance_from_CoG);
end;
end loop;
Swarm_Lock.Unlock_Read;
return Radius;
end Maximal_Radius;
function Mean_Radius return Real is
CoG : constant Vector_3D := Centre_Of_Gravity;
Acc_Radius : Long_Float := 0.0;
begin
Swarm_Lock.Lock_Read;
for Element_Index in First_Index (Swarm_State) .. Last_Index (Swarm_State) loop
declare
Distance_from_CoG : constant Real := abs (CoG - Element (Swarm_State, Element_Index).Position);
begin
Acc_Radius := Acc_Radius + Long_Float (Distance_from_CoG);
end;
end loop;
Swarm_Lock.Unlock_Read;
return Real (Acc_Radius / Long_Float (Length (Swarm_State)));
end Mean_Radius;
function Mean_Closest_Distance return Real is
Acc_Distance : Long_Float := 0.0;
begin
Swarm_Lock.Lock_Read;
for Element_Index in First_Index (Swarm_State) .. Last_Index (Swarm_State) loop
declare
This_Element : constant Swarm_Element_State := Element (Swarm_State, Element_Index);
Neighbours : constant Distance_Vectors.Vector := This_Element.Neighbours.all;
begin
if Distance_Vectors.Length (Neighbours) > 0 then
declare
Closest_Distance : constant Real :=
Distance_Vectors.Element (Neighbours, Distance_Vectors.First_Index (Neighbours)).Distance;
begin
Acc_Distance := Acc_Distance + Long_Float (Closest_Distance);
end;
end if;
end;
end loop;
Swarm_Lock.Unlock_Read;
return Real (Acc_Distance / Long_Float (Length (Swarm_State)));
end Mean_Closest_Distance;
end Swarm_Control;