├── LICENSE ├── Makefile ├── config ├── defaultcolors.ini └── output_config.ini ├── doc └── usersdoc.pdf ├── parameters ├── arenas.default ├── flockingparams.dat ├── initparams.dat ├── obstacles.default └── unitparams.dat ├── src ├── algo.h ├── algo_gui.h ├── algo_spp_evol.c ├── algo_spp_evol.h ├── algo_spp_evol_gui.c ├── algo_spp_evol_stat.c ├── algo_stat.h ├── colors.c ├── colors.h ├── dynspecviz.c ├── dynspecviz.h ├── objects_menu.c ├── objects_menu.h ├── robotmodel.c ├── robotmodel.h ├── robotsim_main.c ├── sensors.c ├── sensors.h ├── stat.c ├── stat.h ├── utilities │ ├── arenas.c │ ├── arenas.h │ ├── datastructs.c │ ├── datastructs.h │ ├── debug_utils.c │ ├── debug_utils.h │ ├── dynamics_utils.c │ ├── dynamics_utils.h │ ├── file_utils.c │ ├── file_utils.h │ ├── interactions.c │ ├── interactions.h │ ├── math_utils.c │ ├── math_utils.h │ ├── obstacles.c │ ├── obstacles.h │ ├── output_utils.c │ ├── output_utils.h │ ├── param_utils.c │ ├── param_utils.h │ ├── pngout_utils.c │ └── pngout_utils.h ├── vizmode.h └── vizualizer │ ├── objects_2d.c │ ├── objects_2d.h │ ├── objects_3d.c │ └── objects_3d.h └── tools └── install.sh /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Eotvos Lorand University, Budapest 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | 2 | # For allowing png outputs during visualization, type this: 3 | # make name_of_algo pngout=true 4 | # 5 | PNG_OUT := $(strip $(pngout)) 6 | DEBUG_MODE := $(strip $(debug)) 7 | 8 | DEFAULT_FLAGS := -lm 9 | VIZUALIZER_FLAGS := -lGL -lGLU -lglut 10 | PNGOUTPUT_FLAGS := -lIL -lILU -lILUT 11 | ERROR_FLAGS := -Wall -Wextra 12 | CANCEL_FLAGS := -Wno-unused-variable -Wno-unused-parameter -Wno-unused-but-set-variable -Wno-switch -Wno-comment -Wno-unused-but-set-parameter -Wunused-function 13 | 14 | ############## 15 | 16 | GCC := gcc src/robotsim_main.c src/utilities/datastructs.c src/utilities/dynamics_utils.c src/utilities/math_utils.c src/utilities/file_utils.c src/utilities/param_utils.c src/sensors.c src/robotmodel.c src/colors.c src/vizualizer/objects_2d.c src/objects_menu.c src/utilities/arenas.c src/vizualizer/objects_3d.c src/stat.c src/dynspecviz.c src/utilities/output_utils.c src/utilities/debug_utils.c 17 | 18 | # Setting up png output mode 19 | ifeq ($(PNG_OUT), true) 20 | GCC += src/utilities/pngout_utils.c $(PNGOUTPUT_FLAGS) -DPNG_OUT 21 | endif 22 | 23 | # Debug mode for segfault detection 24 | ifeq ($(DEBUG_MODE), true) 25 | GCC += -DDEBUG -rdynamic 26 | endif 27 | 28 | #GCC += $(DEFAULT_FLAGS) $(VIZUALIZER_FLAGS) $(ERROR_FLAGS) $(CANCEL_FLAGS) -o robotflocksim_main 29 | GCC += $(DEFAULT_FLAGS) $(VIZUALIZER_FLAGS) $(ERROR_FLAGS) $(CANCEL_FLAGS) -o robotflocksim_main -pg 30 | 31 | # SPP model for testing evolution algorithms 32 | spp_evol: 33 | $(GCC) src/algo_spp_evol.c src/algo_spp_evol_gui.c src/algo_spp_evol_stat.c src/utilities/interactions.c src/utilities/obstacles.c 34 | -------------------------------------------------------------------------------- /config/defaultcolors.ini: -------------------------------------------------------------------------------- 1 | # Medium-dark blue background 2 | background_r=0.1 3 | background_g=0.2 4 | background_b=0.3 5 | # White arrows 6 | agents_velocityarrow_r=0.9 7 | agents_velocityarrow_g=0.8 8 | agents_velocityarrow_b=0.9 9 | # Yellow agents 10 | agents_r=0.8 11 | agents_g=0.8 12 | agents_b=0.2 13 | # Light yellow velocity labels 14 | agents_velocitylabel_r=0.5 15 | agents_velocitylabel_g=0.5 16 | agents_velocitylabel_b=0.5 17 | # Light yellow labels 18 | agents_label_r=0.9 19 | agents_label_g=0.9 20 | agents_label_b=0.2 21 | # Colliding agents are red 22 | agents_indanger_r=0.8 23 | agents_indanger_g=0.2 24 | agents_indanger_b=0.2 25 | # Agent 1 is blue - remove the #s and try it! 26 | # agent_1_r=0.0 27 | # agent_1_g=0.0 28 | # agent_1_b=1.0 29 | # agent_2_r=1.0 30 | # agent_2_g=0.0 31 | # agent_2_b=1.0 32 | # "Zero" and "one" charges for "spring" algorithm 33 | ZeroChargeColor_r=0.0 34 | ZeroChargeColor_g=1.0 35 | ZeroChargeColor_b=0.0 36 | OneChangeColor_r=0.0 37 | OneChargeColor_g=0.0 38 | OneChargeColor_b=1.0 39 | # Communication network 40 | comm_network_r=0.8 41 | comm_network_g=0.8 42 | comm_network_b=0.8 43 | # 44 | # Menu items 45 | # 46 | menu_item_r=0.1 47 | menu_item_g=0.9 48 | menu_item_b=0.4 49 | menu_selection_r=0.1 50 | menu_selection_g=0.9 51 | menu_selection_b=0.4 52 | # 53 | # Captions 54 | # 55 | collisioncaption_r=0.1 56 | collisioncaption_g=0.9 57 | collisioncaption_b=0.4 58 | elapsedtimecaption_r=0.1 59 | elapsedtimecaption_g=0.9 60 | elapsedtimecaption_b=0.4 61 | # 62 | # 63 | # 3D Coordinate system and grid 64 | axis_r=0.1 65 | axis_g=0.1 66 | axis_b=0.3 67 | # 68 | # 69 | # 70 | # Arena edge color and obstacle color for SPP model 71 | ArenaEdgeColor_r=0.1 72 | ArenaEdgeColor_g=0.3 73 | ArenaEdgeColor_b=0.8 74 | ObstacleColor_r=0.8 75 | ObstacleColor_g=0.3 76 | ObstacleColor_b=0.1 77 | # 78 | # 79 | # 80 | # Target agent color for line algorithm 81 | # TargetAgentColor_r=0.1 82 | # TargetAgentColor_g=0.9 83 | # TargetAgentColor_b=0.0 84 | LineColor_r=0.7 85 | LineColor_g=0.7 86 | LineColor_b=0.7 87 | # 88 | # 89 | # Target color for target tracking model 90 | TargetColor_r=0.1 91 | TargetColor_g=0.1 92 | TargetColor_b=0.4 93 | # Traffic circle model 94 | TrafficCircleColor_r=0.1 95 | TrafficCircleColor_g=0.7 96 | TrafficCircleColor_b=0.7 97 | -------------------------------------------------------------------------------- /config/output_config.ini: -------------------------------------------------------------------------------- 1 | # Inner states and trajectories (false or true?) 2 | SaveTrajectories=false 3 | SaveInnerStates=false 4 | # Order parameters (general) (false, timeline, stat or steadystat?) 5 | SaveDistanceBetweenUnits=false 6 | SaveDistanceBetweenNeighbours=false 7 | SaveVelocity=stat 8 | SaveCorrelation=false 9 | SaveCoM=false 10 | SaveCollisions=stat 11 | SaveCollisionRatio=stat 12 | SaveAcceleration=false 13 | # Order parameters (model-specific) (false, timeline, stat or steadystat?) 14 | # TODO!! Not complete yet... 15 | SaveModelSpecificStats=stat 16 | -------------------------------------------------------------------------------- /doc/usersdoc.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/csviragh/robotsim/ea54b97e7e2659449e721ebd4a2285beb9b0e9ce/doc/usersdoc.pdf -------------------------------------------------------------------------------- /parameters/arenas.default: -------------------------------------------------------------------------------- 1 | # 2 | # Arenas for different algorithms 3 | # ArenaShape can be changed using the F4 key during visualization 4 | # Coordinate values have to be multiplied with "ArenaSize" 5 | # (see flocking parameter file for its current value) 6 | # 7 | # Points of an arena should be listed in CW order 8 | # 9 | # 10 | 11 | # Note that shapes CIRCLE and SQUARE are reserved and automatically defined 12 | # for ArenaShape 0 and 1 13 | 14 | [triangle] 15 | 16 | point=1 1 17 | point=1 -1 18 | point=-1 -1 19 | 20 | 21 | [pentagon1] # concave pentagon 22 | 23 | point=-1 -1 24 | point=-1 1 25 | point=0 1 26 | point=0 0 27 | point=1 -1 28 | 29 | 30 | [pentagon2] # convex pentagon 31 | 32 | point=-1 -1 33 | point=-1 1 34 | point=0 1 35 | point=0.8 0.8 36 | point=1 -1 37 | 38 | [octogon] # convex octogon 39 | point = 0 1.6 40 | point = 0.4 2 41 | point = 3.6 2 42 | point = 4 1.6 43 | point = 4 0.4 44 | point = 3.6 0 45 | point = 0.4 0 46 | point = 0 0.4 47 | 48 | -------------------------------------------------------------------------------- /parameters/flockingparams.dat: -------------------------------------------------------------------------------- 1 | # Preferred SPP velocity (Multiply it with 0.010000 to get its Value in m/s) 2 | V_Flock=400.000000 3 | # Maximum repulsion velocity (Multiply it with 0.010000 to get its Value in m/s) 4 | V_Rep=619.664000 5 | # Friction velocity slack (Multiply it with 0.010000 to get its Value in m/s) 6 | V_Frict=50.000000 7 | # Maximum velocity (Multiply it with 0.010000 to get its Value in m/s) 8 | V_Max=800.000000 9 | # Equilibrium distance (Multiply it with 0.010000 to get its Value in m) 10 | R_0=2566.680000 11 | # Friction range (Multiply it with 0.010000 to get its Value in m) 12 | R_0_Offset_Frict=1000.000000 13 | # Shill dist offset (Multiply it with 0.010000 to get its Value in m) 14 | R_0_Shill=0.000000 15 | # Slope of repulsion (1/s) 16 | Slope_Rep=0.400000 17 | # Slope of friction (1/s) 18 | Slope_Frict=0.400000 19 | # Acc limit of Friction (Multiply it with 0.010000 to get its Value in m/s^2) 20 | Acc_Frict=250.000000 21 | # Slope of wall (1/s) 22 | Slope_Shill=0.400000 23 | # Velocity of shill agents (Multiply it with 0.010000 to get its Value in m/s) 24 | V_Shill=652.174000 25 | # Acc limit of shill (Multiply it with 0.010000 to get its Value in m/s^2) 26 | Acc_Shill=250.000000 27 | # Arena Radius (Multiply it with 0.010000 to get its Value in m) 28 | ArenaRadius=23000.000000 29 | # Friction coefficient (-) 30 | C_Frict=0.023802 31 | # Arena Center X (Multiply it with 0.010000 to get its Value in m) 32 | ArenaCenterX=0.000000 33 | # Arena Center Y (Multiply it with 0.010000 to get its Value in m) 34 | ArenaCenterY=0.000000 35 | # Shape of the arena (Multiply it with 0.000000 to get its real Value) 36 | ArenaShape=1.000000 37 | # Number of dimensions in the simulation (2 or 3) 38 | Dim=2.000000 39 | -------------------------------------------------------------------------------- /parameters/initparams.dat: -------------------------------------------------------------------------------- 1 | # Number of agents 2 | NumberOfAgents=100 3 | # Length of the simulated experiment (s) 4 | # 5 | Length=600.0 6 | # Initial sizes (cm) 7 | InitialX=25000.0 8 | InitialY=25000.0 9 | InitialZ=0.0 10 | # Accuracy of the Euler method (s) 11 | # v(t+delta_t) = v(t) + a(t)*delta_t 12 | DeltaT=0.01 13 | # Radius of dangerous area around copters (cm) 14 | # If distance between agents is below "Radius", then 15 | # a collision occurs. 16 | Radius=300.0 17 | # 18 | # Length to store in memory (s) 19 | # Larger stored length means faster running, 20 | # but more allocated memory 21 | LengthToStore=15.0 22 | # 23 | # Default speed of visualization 24 | VizSpeedUp=20 25 | StartOfSteadyState=0 26 | -------------------------------------------------------------------------------- /parameters/obstacles.default: -------------------------------------------------------------------------------- 1 | # This file contains parameters for virtual objects to avoid in the air. 2 | # 3 | 4 | [init] 5 | 6 | # Angle between NED coordinate system and xy plane of the obstacles CW [deg] 7 | # 0 = North, 90 = East, 180 = South, 270 = West 8 | # 9 | # N x 10 | # | / 11 | # | / 12 | # |----- E 13 | # \_ 14 | # \- y 15 | # 16 | angle=0 17 | 18 | 19 | [obstacles] 20 | 21 | # Define a list of obstacles using the following format: 22 | # 23 | # obstaclename.point=x y # comment, where 24 | # 25 | # x points towards the front [cm] 26 | # y points towards the right [cm] 27 | 28 | # Points of an obstacle should be listed in CW order 29 | 30 | # 31 | # |-------| |--------| 5 32 | # | | | | --> y 33 | # |-------| |--------| -5 34 | # -55 -5 5 55 35 | 36 | #leftrect.point=600 -600 37 | #leftrect.point=-600 -600 38 | #leftrect.point=-600 -55500 39 | #leftrect.point=600 -55500 40 | 41 | #rightrect.point=600 600 42 | #rightrect.point=600 55500 43 | #rightrect.point=-600 55500 44 | #rightrect.point=-600 600 45 | 46 | # Concave pentagon 47 | 48 | #pentagon1.point=-1000 -1000 49 | #pentagon1.point=1000 -1000 50 | #pentagon1.point=0 0 51 | #pentagon1.point=0 1000 52 | #pentagon1.point=-1000 1000 53 | 54 | -------------------------------------------------------------------------------- /parameters/unitparams.dat: -------------------------------------------------------------------------------- 1 | # Relaxation times of PID Controller (s) 2 | tau_PID_XY=1.000000 3 | tau_PID_Z=1.000000 4 | # Maximum Value of acceleration (cm / s^2) 5 | a_max=600.000000 6 | # Comm. range (cm) 7 | R_C=10000.000000 8 | # Delay of comm. (s) 9 | t_del=1.000000 10 | # GPS refresh rate (s) 11 | t_GPS=0.200000 12 | # Parameters of inner noises (cm^2 / s^2) 13 | Sigma_GPS_XY=50.000000 14 | Sigma_GPS_Z=0.000000 15 | # Parameters of outer noises (cm^2 / s^3) 16 | Sigma_outer_XY=2000.000000 17 | Sigma_outer_Z=0.000000 18 | Wind_Magn_Avg=0.000000 19 | Wind_StDev=0.000000 20 | Wind_Angle=0.000000 21 | # Packet Loss Ratio (0-1) 22 | Packet_Loss_Ratio=0.000000 23 | # Packet Loss Distance (cm) 24 | Packet_Loss_Distance=8000.000000 25 | -------------------------------------------------------------------------------- /src/algo.h: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | #ifndef ALGO_H 7 | #define ALGO_H 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "utilities/param_utils.h" 14 | #include "utilities/dynamics_utils.h" 15 | #include "utilities/datastructs.h" 16 | #include "utilities/math_utils.h" 17 | #include "utilities/debug_utils.h" 18 | #include "vizmode.h" 19 | #include "colors.h" 20 | 21 | /* Helper variables. 22 | */ 23 | int tt; 24 | 25 | /* Sets up default values of parameters of the flocking model 26 | * Number of inner states also should be defined here. 27 | */ 28 | void InitializeFlockingParams(flocking_model_params_t * FlockingParams); 29 | 30 | /* Setting up Initial values of inner states, allocating memory for 31 | * model-specific objects. 32 | */ 33 | void InitializePhase(phase_t * Phase, flocking_model_params_t * FlockingParams, 34 | sit_parameters_t * SitParams); 35 | 36 | /* Refreshing values of outer variables (e. g. "number of caught agents" in the chasing algorithm) 37 | * in every timestep 38 | */ 39 | void HandleOuterVariables(phase_t * Phase, 40 | vizmode_params_t * VizParams, 41 | sit_parameters_t * SitParams, 42 | unit_model_params_t * UnitParams, 43 | const double ActualTime, char *OutputDirectory); 44 | 45 | /* Target velocity calculation. 46 | */ 47 | void CalculatePreferredVelocity(double *OutputVelocity, 48 | double *OutputInnerState, 49 | phase_t * Phase, 50 | const int WhichAgent, 51 | flocking_model_params_t * FlockingParams, 52 | vizmode_params_t * VizParams, 53 | const double Delay, 54 | const double ActualTime, agent_debug_info_t * DebugInfo); 55 | 56 | /* Destruction of all model-specific objects initilalized in InitializePhase */ 57 | void DestroyPhase(phase_t * Phase, flocking_model_params_t * FlockingParams, 58 | sit_parameters_t * SitParams); 59 | 60 | #endif 61 | -------------------------------------------------------------------------------- /src/algo_gui.h: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* Model-Specific GUI tools 7 | */ 8 | 9 | #ifndef ALGO_GUI_H 10 | #define ALGO_GUI_H 11 | 12 | #include 13 | 14 | #include "vizmode.h" 15 | #include "colors.h" 16 | #include "dynspecviz.h" 17 | #include "utilities/param_utils.h" 18 | #include "utilities/datastructs.h" 19 | #include "vizualizer/objects_2d.h" 20 | #include "vizualizer/objects_3d.h" 21 | #include "utilities/dynamics_utils.h" 22 | #include "utilities/file_utils.h" 23 | #include "utilities/math_utils.h" 24 | 25 | /* For changing variables from algo_..._gui.c */ 26 | static int jj; 27 | 28 | /* For reseting Phases 29 | */ 30 | void ModelSpecificReset(phase_t * Phase, 31 | const double Size_X, 32 | const double Size_Y, 33 | const double Size_Z, 34 | vizmode_params_t * VizParams, 35 | flocking_model_params_t * FlockingParams, const double Radius); 36 | 37 | /* For Initializing vizualisation mode parameters 38 | */ 39 | void InitializeVizParams(vizmode_params_t * VizParams); 40 | 41 | /* For Initializing and saving model-specific colors 42 | */ 43 | void InitializeModelSpecificColors(model_specific_color_t * ModelSpecificColors, 44 | int *NumberOfModelSpecificColors); 45 | 46 | /* For drawing model-specific objects in 2D and 3D 47 | * (e.g. Lines, Obstacles, Waypoints) 48 | */ 49 | void DrawModelSpecificObjects_2D(phase_t * Phase, 50 | flocking_model_params_t * FlockingParams, 51 | unit_model_params_t * UnitModelParams, 52 | vizmode_params_t * VizParams, 53 | color_config_t * Colors, sit_parameters_t * SitParams); 54 | void DrawModelSpecificObjects_3D(phase_t * Phase, 55 | flocking_model_params_t * FlockingParams, 56 | unit_model_params_t * UnitModelParams, 57 | vizmode_params_t * VizParams, 58 | color_config_t * Colors, sit_parameters_t * SitParams); 59 | 60 | /* For displaying a model specific string in the gui 61 | */ 62 | char *GetModelSpecificStringToDisplay(phase_t * Phase, 63 | flocking_model_params_t * FlockingParams, 64 | unit_model_params_t * UnitModelParams, 65 | sit_parameters_t * SitParams, double TimeStep); 66 | 67 | /* For handling model-specific keyboard events 68 | */ 69 | void HandleSpecialKeyBoardEvent(unsigned char key, 70 | int x, 71 | int y, 72 | flocking_model_params_t * FlockingParams, 73 | vizmode_params_t * VizParams, 74 | sit_parameters_t * SitParams, const int Modifier); 75 | 76 | /* For handling model-specific "special key" events 77 | */ 78 | void HandleSpecialSpecKeyEvent(unsigned char key, 79 | int x, 80 | int y, 81 | flocking_model_params_t * FlockingParams, 82 | vizmode_params_t * VizParams, 83 | sit_parameters_t * SitParams, const int Modifier); 84 | 85 | /* For handling model-specific mouse events 86 | */ 87 | void HandleSpecialMouseEvent(int button, 88 | int state, 89 | int x, 90 | int y, 91 | flocking_model_params_t * FlockingParams, 92 | vizmode_params_t * VizParams, const int Modifier); 93 | 94 | #endif 95 | -------------------------------------------------------------------------------- /src/algo_spp_evol.h: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* "global" variables for the evolution-compatible SPP algo 7 | */ 8 | 9 | #ifndef ALGO_SPP_EVOL_H 10 | #define ALGO_SPP_EVOL_H 11 | 12 | #include "utilities/arenas.h" 13 | #include "utilities/interactions.h" 14 | #include "utilities/obstacles.h" 15 | 16 | /* Arenas structure */ 17 | arenas_t Arenas; 18 | 19 | /* Obstacles structure */ 20 | obstacles_t obstacles; 21 | 22 | /* Parameters of the basic SPP terms */ 23 | double V_Flock; 24 | double V_Max; 25 | 26 | /* Parameters of repulsion */ 27 | double V_Rep; 28 | double R_0; 29 | double Slope_Rep; 30 | 31 | /* Parameters of friction */ 32 | double C_Frict; 33 | double V_Frict; 34 | double R_0_Offset_Frict; 35 | double Slope_Frict; 36 | double Acc_Frict; 37 | 38 | /* Parameters of the wall and its shill interaction */ 39 | double V_Shill; 40 | double Acc_Shill; 41 | double Slope_Shill; 42 | double R_0_Shill; 43 | 44 | /* 2 or 3 dimensions? */ 45 | double Dim; 46 | 47 | /* Parameters of the arena */ 48 | double ArenaRadius; 49 | double ArenaShape; 50 | double ArenaCenterX; 51 | double ArenaCenterY; 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /src/algo_spp_evol_gui.c: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* Model-Specific GUI tools 7 | */ 8 | 9 | #include "algo_gui.h" 10 | #include "algo_spp_evol.h" 11 | 12 | /* Edges of the arena */ 13 | static double VerticesOfArena[12]; 14 | static double EdgesOfObstacle[MAX_OBSTACLES][MAX_OBSTACLE_POINTS * 2]; 15 | 16 | /* Colors of model-specific objects */ 17 | static float ArenaEdgeColor[3]; 18 | static float ObstacleColor[3]; 19 | 20 | /* For reseting Phases */ 21 | void ModelSpecificReset(phase_t * Phase, 22 | const double Size_X, 23 | const double Size_Y, 24 | const double Size_Z, 25 | vizmode_params_t * VizParams, 26 | flocking_model_params_t * FL_Params, const double Radius) { 27 | 28 | } 29 | 30 | void SetupVertices(vizmode_params_t * VizParams) { 31 | int i; 32 | 33 | /* Cutting trees, sorry! */ 34 | for (i = 0; i < 12; i++) { 35 | // Z components 36 | if (2 == (i % 3)) { 37 | VerticesOfArena[i] = 38 | RealToGlCoord_3D(-ArenaRadius, VizParams->MapSizeXY); 39 | // X components 40 | } else if (0 == (i % 3)) { 41 | if (i < 4) { 42 | VerticesOfArena[i] = 43 | RealToGlCoord_3D(-ArenaRadius + ArenaCenterX, 44 | VizParams->MapSizeXY); 45 | } else { 46 | VerticesOfArena[i] = 47 | RealToGlCoord_3D(ArenaRadius + ArenaCenterX, 48 | VizParams->MapSizeXY); 49 | } 50 | // Y components 51 | } else { 52 | if (1 == (i % 9)) { 53 | VerticesOfArena[i] = 54 | RealToGlCoord_3D(-ArenaRadius + ArenaCenterY, 55 | VizParams->MapSizeXY); 56 | } else { 57 | VerticesOfArena[i] = 58 | RealToGlCoord_3D(ArenaRadius + ArenaCenterY, 59 | VizParams->MapSizeXY); 60 | } 61 | } 62 | } 63 | } 64 | 65 | /* Creating vertex set for prisms and polygons to draw (obstacles and arena) */ 66 | void CreateObstacleVertexSet(obstacles_t * Obstacles, 67 | vizmode_params_t * VizParams) { 68 | int i, j; 69 | for (j = 0; j < Obstacles->o_count; j++) { 70 | for (i = 0; i < Obstacles->o[j].p_count; i++) { 71 | if (VizParams->TwoDimViz == false) { 72 | EdgesOfObstacle[j][i * 3] = 73 | RealToGlCoord_3D(Obstacles->o[j].p[i][0], 74 | VizParams->MapSizeXY); 75 | EdgesOfObstacle[j][i * 3 + 1] = 76 | RealToGlCoord_3D(Obstacles->o[j].p[i][1], 77 | VizParams->MapSizeXY); 78 | EdgesOfObstacle[j][i * 3 + 2] = 79 | RealToGlCoord_3D(-990.0, VizParams->MapSizeXY); 80 | } else { 81 | EdgesOfObstacle[j][i * 3] = 82 | RealToGlCoord_2D(Obstacles->o[j].p[i][0] - 83 | VizParams->CenterX, VizParams->MapSizeXY); 84 | EdgesOfObstacle[j][i * 3 + 1] = 85 | RealToGlCoord_2D(Obstacles->o[j].p[i][1] - 86 | VizParams->CenterY, VizParams->MapSizeXY); 87 | EdgesOfObstacle[j][i * 3 + 2] = 88 | RealToGlCoord_2D(0.0, VizParams->MapSizeXY); 89 | } 90 | } 91 | } 92 | } 93 | 94 | /* For Initializing vizualisation mode parameters */ 95 | void InitializeVizParams(vizmode_params_t * VizParams) { 96 | if (0.0 != ArenaShape) 97 | SetupVertices(VizParams); 98 | CreateObstacleVertexSet(&obstacles, VizParams); 99 | } 100 | 101 | /* For Initializing mode-specific colors */ 102 | void InitializeModelSpecificColors(model_specific_color_t * ModelSpecificColors, 103 | int *NumberOfModelSpecificColors) { 104 | CREATE_COLOR(ArenaEdgeColor, 0.0, 0.0, 0.0); 105 | CREATE_COLOR(ObstacleColor, 1.0, 0.0, 0.0); 106 | } 107 | 108 | /* For drawing model-specific objects */ 109 | void DrawModelSpecificObjects_2D(phase_t * Phase, 110 | flocking_model_params_t * FlockingParams, 111 | unit_model_params_t * UnitParams, vizmode_params_t * VizParams, 112 | color_config_t * Colors, sit_parameters_t * SitParams) { 113 | int i; 114 | double Gamma_Shill = V_Shill / Slope_Shill + R_0_Shill; 115 | 116 | // draw arena 117 | if (0.0 == ArenaShape) { 118 | DrawThickEdgedCircle(RealToGlCoord_2D(-VizParams->CenterX + 119 | ArenaCenterX, VizParams->MapSizeXY), 120 | RealToGlCoord_2D(-VizParams->CenterY + ArenaCenterY, 121 | VizParams->MapSizeXY), 122 | RealToGlCoord_2D(ArenaRadius - Gamma_Shill / 2, 123 | VizParams->MapSizeXY), RealToGlCoord_2D(Gamma_Shill, 124 | VizParams->MapSizeXY), ArenaEdgeColor); 125 | } else { 126 | SetupVertices(VizParams); 127 | DrawThickEdgedTransparentShape(RealToGlCoord_2D(ArenaCenterX - 128 | VizParams->CenterX, VizParams->MapSizeXY), 129 | RealToGlCoord_2D(ArenaCenterY - VizParams->CenterY, 130 | VizParams->MapSizeXY), 131 | RealToGlCoord_2D(2.0 * ArenaRadius - Gamma_Shill, 132 | VizParams->MapSizeXY), 133 | RealToGlCoord_2D(2.0 * ArenaRadius - Gamma_Shill, 134 | VizParams->MapSizeXY), RealToGlCoord_2D(Gamma_Shill, 135 | VizParams->MapSizeXY), 0.0, ArenaEdgeColor); 136 | } 137 | 138 | // draw obstacles 139 | CreateObstacleVertexSet(&obstacles, VizParams); 140 | for (i = 0; i < obstacles.o_count; i++) { 141 | DrawThickEdgedPolygon(EdgesOfObstacle[i], obstacles.o[i].p_count, 142 | RealToGlCoord_2D(Gamma_Shill, VizParams->MapSizeXY), 143 | ObstacleColor); 144 | } 145 | } 146 | 147 | void DrawModelSpecificObjects_3D(phase_t * Phase, 148 | flocking_model_params_t * FlockingParams, 149 | unit_model_params_t * UnitParams, 150 | vizmode_params_t * VizParams, 151 | color_config_t * Colors, sit_parameters_t * SitParams) { 152 | int i; 153 | // draw arena 154 | if (1.0 == ArenaShape) { 155 | DrawPrism_3D(VerticesOfArena, 156 | 4, 157 | RealToGlCoord_3D(ArenaRadius * 2.0, VizParams->MapSizeXY), 158 | 10000.0 * RealToGlCoord_3D(300.0, VizParams->MapSizeXY), 159 | ArenaEdgeColor); 160 | } else if (0.0 == ArenaShape || 2.0 == ArenaShape) { 161 | DrawWireSphere_3D(RealToGlCoord_2D(ArenaCenterX, VizParams->MapSizeXY), 162 | RealToGlCoord_2D(ArenaCenterY, VizParams->MapSizeXY), 163 | RealToGlCoord_2D(0.0, VizParams->MapSizeXY), 164 | RealToGlCoord_2D(ArenaRadius, VizParams->MapSizeXY), 165 | ArenaEdgeColor); 166 | 167 | } 168 | // draw obstacles 169 | CreateObstacleVertexSet(&obstacles, VizParams); 170 | for (i = 0; i < obstacles.o_count; i++) { 171 | DrawPrism_3D(EdgesOfObstacle[i], obstacles.o[i].p_count, 172 | RealToGlCoord_3D(2000.0, VizParams->MapSizeXY), 173 | 10000.0 * RealToGlCoord_3D(300.0, VizParams->MapSizeXY), 174 | ObstacleColor); 175 | } 176 | } 177 | 178 | /* For writing out a model-specific string */ 179 | char *GetModelSpecificStringToDisplay(phase_t * Phase, 180 | flocking_model_params_t * FlockingParams, 181 | unit_model_params_t * UnitModelParams, 182 | sit_parameters_t * SitParams, double TimeStep) { 183 | return ""; 184 | } 185 | 186 | /* For handling model-specific keboard events */ 187 | void HandleSpecialKeyBoardEvent(unsigned char key, 188 | int x, 189 | int y, 190 | flocking_model_params_t * FlockingParams, 191 | vizmode_params_t * VizParams, 192 | sit_parameters_t * SitParams, const int Modifier) { 193 | 194 | SetupVertices(VizParams); 195 | CreateObstacleVertexSet(&obstacles, VizParams); 196 | } 197 | 198 | /* For handling model-specific "special key" events */ 199 | void HandleSpecialSpecKeyEvent(unsigned char key, 200 | int x, 201 | int y, 202 | flocking_model_params_t * FlockingParams, 203 | vizmode_params_t * VizParams, 204 | sit_parameters_t * SitParams, const int Modifier) { 205 | 206 | SetupVertices(VizParams); 207 | CreateObstacleVertexSet(&obstacles, VizParams); 208 | 209 | if (GLUT_KEY_F6 == key) { 210 | CHANGE_PARAMETER(ArenaShape, 211 | (double) ((int) (ArenaShape + 1) % (int) 2)); 212 | } 213 | } 214 | 215 | /* For handling model-specific mouse events */ 216 | void HandleSpecialMouseEvent(int button, 217 | int state, 218 | int x, 219 | int y, 220 | flocking_model_params_t * FlockingParams, 221 | vizmode_params_t * VizParams, const int Modifier) { 222 | 223 | if (VizParams->TwoDimViz && button == GLUT_LEFT && state == GLUT_DOWN) { 224 | CHANGE_PARAMETER(ArenaCenterX, MouseCoordToReal_2D(x, 225 | VizParams->MapSizeXY, 226 | VizParams->Resolution) + VizParams->CenterX); 227 | CHANGE_PARAMETER(ArenaCenterY, -MouseCoordToReal_2D(y, 228 | VizParams->MapSizeXY, 229 | VizParams->Resolution) + VizParams->CenterY); 230 | } 231 | 232 | SetupVertices(VizParams); 233 | CreateObstacleVertexSet(&obstacles, VizParams); 234 | } 235 | -------------------------------------------------------------------------------- /src/algo_stat.h: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* Header file for algo_..._stat.c */ 7 | 8 | #ifndef ALGO_STAT 9 | #define ALGO_STAT 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "utilities/math_utils.h" 17 | #include "utilities/output_utils.h" 18 | #include "utilities/dynamics_utils.h" 19 | #include "utilities/param_utils.h" 20 | 21 | /* Macro for initializing output file 22 | */ 23 | static char ModelSpecificOutputFileName[512]; 24 | #define INITIALIZE_OUTPUT_FILE(filename, file) \ 25 | strcpy (ModelSpecificOutputFileName, StatUtils->OutputDirectory); \ 26 | strcat (ModelSpecificOutputFileName, "/"); \ 27 | strcat (ModelSpecificOutputFileName, filename); \ 28 | strcat (ModelSpecificOutputFileName, "\0"); \ 29 | file = fopen (ModelSpecificOutputFileName, "w"); 30 | 31 | /* This struct conatins all (?) necessary parameters 32 | * for creating statistical calculations 33 | */ 34 | typedef struct { 35 | 36 | double ElapsedTime; 37 | double StartOfSteadyState; 38 | char *OutputDirectory; 39 | save_mode_t SaveMode; 40 | 41 | } stat_utils_t; 42 | 43 | /* Function for opening stat files, creating header lines, etc. 44 | */ 45 | void InitializeModelSpecificStats(stat_utils_t * StatUtils); 46 | 47 | void SaveModelSpecificStats(phase_t * Phase, stat_utils_t * StatUtils, 48 | unit_model_params_t * UnitParams, 49 | flocking_model_params_t * FlockingParams, sit_parameters_t * SitParams); 50 | 51 | /* For closing stat files 52 | */ 53 | void CloseModelSpecificStats(stat_utils_t * StatUtils); 54 | 55 | #endif 56 | -------------------------------------------------------------------------------- /src/colors.h: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* Reading and applying color configs 7 | */ 8 | 9 | #ifndef COLORS_H 10 | #define COLORS_H 11 | 12 | /* Max number of model-specific colors */ 13 | #define MAX_NUMBER_OF_COLORS 32 14 | 15 | /* Macro for creating model-specific color */ 16 | #define CREATE_COLOR(n, r, g, b) \ 17 | ModelSpecificColors[*NumberOfModelSpecificColors].NameInFiles = #n; \ 18 | ModelSpecificColors[*NumberOfModelSpecificColors].RGB_Values[0] = r; \ 19 | ModelSpecificColors[*NumberOfModelSpecificColors].RGB_Values[1] = g; \ 20 | ModelSpecificColors[*NumberOfModelSpecificColors].RGB_Values[2] = b; \ 21 | ModelSpecificColors[*NumberOfModelSpecificColors].StaticValuePointer[0] = &n[0]; \ 22 | ModelSpecificColors[*NumberOfModelSpecificColors].StaticValuePointer[1] = &n[1]; \ 23 | ModelSpecificColors[*NumberOfModelSpecificColors].StaticValuePointer[2] = &n[2]; \ 24 | (*NumberOfModelSpecificColors) ++ ; 25 | 26 | #include 27 | #include 28 | #include "utilities/file_utils.h" 29 | #include "utilities/param_utils.h" 30 | 31 | /* Struct for storing basic color schemes 32 | * Every "color" is an RGB value (a float number between 0 and 1) 33 | * "EraseColor" is the color of the background 34 | */ 35 | typedef struct { 36 | 37 | /* Background */ 38 | float EraseColor[3]; 39 | 40 | /* Colors of menusystem */ 41 | float MenuItemColor[3]; 42 | float MenuSelectionColor[3]; 43 | 44 | /* Colors of "trajectory window" */ 45 | float AgentsDefaultColor[3]; 46 | float **AgentsColor; //We can define different colors for each agents 47 | 48 | /* Color of agents in dangerous situation */ 49 | float AgentsInDangerColor[3]; 50 | /* Velocity label and arrow */ 51 | float VelocityLabelColor[3]; 52 | float VelocityArrowColor[3]; 53 | /* User-defined labels */ 54 | float LabelColor[3]; 55 | 56 | /* Communication network */ 57 | float CommNetWorkColor[3]; 58 | 59 | /* */ 60 | float PausedCaptionColor[3]; 61 | float CollisionCaptionColor[3]; 62 | float ElapsedTimeCaptionColor[3]; 63 | 64 | /* 3d Coordinate System color */ 65 | float AxisColor[3]; 66 | 67 | } color_config_t; 68 | 69 | /* Structs for storing model-specific color schemes 70 | */ 71 | typedef struct { 72 | 73 | char *NameInFiles; 74 | float RGB_Values[3]; 75 | 76 | float *StaticValuePointer[3]; 77 | 78 | } model_specific_color_t; 79 | 80 | /** 81 | * Creates a color structure from the individual HSV 82 | * components (where hue is cyclic in 0-255). 83 | * 84 | * \param rgb the constructed color (3 values in the range 0-1) 85 | * \param hue the hue component 86 | * \param saturation the saturation component 87 | * \param value the value component 88 | */ 89 | void MakeRGBFromHSV(float *rgb, unsigned char hue, unsigned char saturation, 90 | unsigned char value); 91 | 92 | /* Loading color schemes from an ".ini" file 93 | * "InputFile" should contain all necessary color data 94 | * example: "background_r=..." 95 | * The variables below should be in the "InputFile" (with postfix _r, _g or _b): 96 | * background, menuitem, menuselection, etc... 97 | */ 98 | void LoadColorConfig(color_config_t * ColorConfigToSet, FILE * InputFile, 99 | int NumberOfAgents, model_specific_color_t * ModelSpecificColors, 100 | int *NumberOfModelSpecificColors); 101 | 102 | /* Setting the color of a specific agent to "color" 103 | * "WhichAgent" is the index of the agent 104 | */ 105 | void SetAgentsColor(color_config_t * ColorConfig, const int WhichAgent, 106 | const int NumberOfAgents, const float *color); 107 | 108 | /* Reset the color of every agents to "color" 109 | */ 110 | void ResetAgentsColor(color_config_t * ColorConfig, const int NumberOfAgents, 111 | const float *color); 112 | 113 | /* Load modelspecific colors 114 | * format: readed_Name=readed_value 115 | */ 116 | void LoadModelSpecificColors(char *readed_Name, char *readed_value, 117 | model_specific_color_t * ModelSpecificColors, 118 | int *NumberOfModelSpecificColors); 119 | 120 | #endif 121 | -------------------------------------------------------------------------------- /src/dynspecviz.c: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* Drawing copters, etc. (dynamics-specific objects) */ 7 | 8 | #include "dynspecviz.h" 9 | 10 | static double EyeFromCenter[3]; 11 | static double DistanceFromCenter; 12 | static double NewEyeZ; 13 | 14 | /* Draws velocity arrow above the copter */ 15 | void DrawVelocityArrow_2D(const double x, const double y, 16 | const double velocity_x, const double velocity_y, 17 | const double MapSizexy, const float *arrowcolor) { 18 | 19 | /* Drawing arrow... */ 20 | DrawArrow(RealToGlCoord_2D(x, MapSizexy), 21 | RealToGlCoord_2D(y, MapSizexy), 22 | RealToGlCoord_2D(20.0 * pow(-50.0 + sqrt(velocity_x * velocity_x + 23 | velocity_y * velocity_y), 1.0 / 3.0), 24 | MapSizexy), atan2(-velocity_y, velocity_x), arrowcolor); 25 | 26 | } 27 | 28 | /* Draws... 29 | * - a circle (with radius=40cm) as a representation of a quadcopter 30 | * - "Dangerous area" - a circle (with radius=1.5m) 31 | */ 32 | void DrawCopter_2D(const double x, const double y, const double MapSizexy, 33 | const double Radius, const float *agentcolor) { 34 | 35 | /* Real size of the copter */ 36 | DrawFullCircle(RealToGlCoord_2D(x, MapSizexy), RealToGlCoord_2D(y, 37 | MapSizexy), RealToGlCoord_2D(40, MapSizexy), agentcolor); 38 | /* Dangerous zone */ 39 | DrawCircle(RealToGlCoord_2D(x, MapSizexy), RealToGlCoord_2D(y, MapSizexy), 40 | RealToGlCoord_2D(Radius / 2.0, MapSizexy), agentcolor); 41 | 42 | } 43 | 44 | /* Labels above agents */ 45 | void DrawAgentLabel_2D(phase_t * Phase, const int WhichAgent, char *Label, 46 | bool Display, vizmode_params_t * VizParams, const float *Color) { 47 | 48 | double *Position; 49 | Position = Phase->Coordinates[WhichAgent]; 50 | 51 | if (Display == true) { 52 | 53 | DrawString(RealToGlCoord_2D(Position[0] - VizParams->CenterX + 120.0, 54 | VizParams->MapSizeXY), 55 | RealToGlCoord_2D(Position[1] - VizParams->CenterY + 120.0, 56 | VizParams->MapSizeXY), GLUT_BITMAP_TIMES_ROMAN_10, 57 | Label, Color); 58 | 59 | } 60 | 61 | } 62 | 63 | /* Draws a network arrow between two agents */ 64 | void DrawNetworkArrow_2D(phase_t * Phase, const int FromWhichAgent, 65 | const int ToWhichAgent, vizmode_params_t * VizParams, 66 | const float *color) { 67 | 68 | double *FromCoords; 69 | double *ToCoords; 70 | FromCoords = Phase->Coordinates[FromWhichAgent]; 71 | ToCoords = Phase->Coordinates[ToWhichAgent]; 72 | 73 | static double DifferenceVector[3]; 74 | VectDifference(DifferenceVector, ToCoords, FromCoords); 75 | 76 | static double ArrowCenterX; 77 | ArrowCenterX = (FromCoords[0] + ToCoords[0]) * 0.5 - VizParams->CenterX; 78 | static double ArrowCenterY; 79 | ArrowCenterY = (FromCoords[1] + ToCoords[1]) * 0.5 - VizParams->CenterY; 80 | 81 | static double angle; 82 | DifferenceVector[2] = 0; 83 | angle = acos(DifferenceVector[0] / VectAbs(DifferenceVector)); 84 | if (DifferenceVector[1] > 0) { 85 | angle *= -1; 86 | } 87 | 88 | DrawThinArrow(RealToGlCoord_2D(ArrowCenterX, VizParams->MapSizeXY), 89 | RealToGlCoord_2D(ArrowCenterY, VizParams->MapSizeXY), 90 | RealToGlCoord_2D(VectAbs(DifferenceVector) - 60, 91 | VizParams->MapSizeXY), RealToGlCoord_2D(30, 92 | VizParams->MapSizeXY), angle, color); 93 | 94 | } 95 | 96 | /* Draws a network arrow between two positions */ 97 | void DrawNetworkArrowBetweenPositions_2D(double *FromCoords, double *ToCoords, 98 | vizmode_params_t * VizParams, const float *color) { 99 | 100 | static double DifferenceVector[3]; 101 | VectDifference(DifferenceVector, ToCoords, FromCoords); 102 | 103 | static double ArrowCenterX; 104 | ArrowCenterX = (FromCoords[0] + ToCoords[0]) * 0.5 - VizParams->CenterX; 105 | static double ArrowCenterY; 106 | ArrowCenterY = (FromCoords[1] + ToCoords[1]) * 0.5 - VizParams->CenterY; 107 | 108 | static double angle; 109 | DifferenceVector[2] = 0; 110 | angle = acos(DifferenceVector[0] / VectAbs(DifferenceVector)); 111 | if (DifferenceVector[1] > 0) { 112 | angle *= -1; 113 | } 114 | 115 | DrawThinArrow(RealToGlCoord_2D(ArrowCenterX, VizParams->MapSizeXY), 116 | RealToGlCoord_2D(ArrowCenterY, VizParams->MapSizeXY), 117 | RealToGlCoord_2D(VectAbs(DifferenceVector) - 60, 118 | VizParams->MapSizeXY), RealToGlCoord_2D(30, 119 | VizParams->MapSizeXY), angle, color); 120 | 121 | } 122 | 123 | /* Drawing sensor range network */ 124 | void DrawSensorRangeNetwork_2D(phase_t * PhaseData, 125 | const double SensorRangeToDisplay, 126 | const int WhichAgent, 127 | const double Delay, 128 | const int Now, 129 | const double h, vizmode_params_t * VizParams, const float *color) { 130 | 131 | int i; 132 | 133 | double *ActualAgentsCoordinates; 134 | ActualAgentsCoordinates = PhaseData[Now].Coordinates[WhichAgent]; 135 | GetAgentsCoordinatesFromTimeLine(ActualAgentsCoordinates, PhaseData, 136 | WhichAgent, Now); 137 | 138 | static double NeighboursCoordinates[3]; 139 | static double DifferenceVector[3]; 140 | 141 | static double ArrowCenterX; 142 | static double ArrowCenterY; 143 | static double angle; 144 | 145 | for (i = 0; i < PhaseData[0].NumberOfAgents; i++) { 146 | if (i != WhichAgent) { 147 | 148 | GetAgentsCoordinatesFromTimeLine(NeighboursCoordinates, PhaseData, 149 | i, Now); 150 | VectDifference(DifferenceVector, ActualAgentsCoordinates, 151 | NeighboursCoordinates); 152 | if (VectAbs(DifferenceVector) < SensorRangeToDisplay) { 153 | GetAgentsCoordinatesFromTimeLine(NeighboursCoordinates, 154 | PhaseData, i, Now); 155 | VectDifference(DifferenceVector, ActualAgentsCoordinates, 156 | NeighboursCoordinates); 157 | 158 | ArrowCenterX = 159 | (ActualAgentsCoordinates[0] + 160 | NeighboursCoordinates[0]) * 0.5 - VizParams->CenterX; 161 | ArrowCenterY = 162 | (ActualAgentsCoordinates[1] + 163 | NeighboursCoordinates[1]) * 0.5 - VizParams->CenterY; 164 | 165 | DifferenceVector[2] = 0; 166 | angle = -atan2(DifferenceVector[1], DifferenceVector[0]); 167 | 168 | DrawThinArrow(RealToGlCoord_2D(ArrowCenterX, 169 | VizParams->MapSizeXY), 170 | RealToGlCoord_2D(ArrowCenterY, VizParams->MapSizeXY), 171 | RealToGlCoord_2D(VectAbs(DifferenceVector) - 60, 172 | VizParams->MapSizeXY), RealToGlCoord_2D(30, 173 | VizParams->MapSizeXY), angle, color); 174 | 175 | } 176 | 177 | } 178 | } 179 | 180 | } 181 | 182 | /* 3D objects */ 183 | 184 | /* Drawing sensor range network */ 185 | void DrawSensorRangeNetwork_3D(phase_t * PhaseData, 186 | const double SensorRangeToDisplay, 187 | const int WhichAgent, 188 | const double Delay, 189 | const int Now, 190 | const double h, const double MapSizexy, const float *color) { 191 | 192 | int i; 193 | 194 | double *ActualAgentsCoordinates; 195 | ActualAgentsCoordinates = PhaseData[Now].Coordinates[WhichAgent]; 196 | GetAgentsCoordinatesFromTimeLine(ActualAgentsCoordinates, PhaseData, 197 | WhichAgent, Now); 198 | 199 | static double NeighboursCoordinates[3]; 200 | NullVect(NeighboursCoordinates, 3); 201 | static double DifferenceVector[3]; 202 | NullVect(DifferenceVector, 3); 203 | 204 | for (i = 0; i < PhaseData[0].NumberOfAgents; i++) { 205 | if (i != WhichAgent) { 206 | 207 | GetAgentsCoordinatesFromTimeLine(NeighboursCoordinates, PhaseData, 208 | i, Now); 209 | VectDifference(DifferenceVector, ActualAgentsCoordinates, 210 | NeighboursCoordinates); 211 | if (VectAbs(DifferenceVector) < SensorRangeToDisplay) { 212 | 213 | glColor3f(color[0], color[1], color[2]); 214 | 215 | glBegin(GL_LINES); 216 | 217 | glVertex3f(RealToGlCoord_3D(ActualAgentsCoordinates[0], 218 | MapSizexy), 219 | RealToGlCoord_3D(ActualAgentsCoordinates[1], MapSizexy), 220 | RealToGlCoord_3D(ActualAgentsCoordinates[2], 221 | MapSizexy)); 222 | glVertex3f(RealToGlCoord_3D(NeighboursCoordinates[0], 223 | MapSizexy), 224 | RealToGlCoord_3D(NeighboursCoordinates[1], MapSizexy), 225 | RealToGlCoord_3D(NeighboursCoordinates[2], MapSizexy)); 226 | 227 | glEnd(); 228 | 229 | } 230 | 231 | } 232 | } 233 | 234 | } 235 | 236 | /* 3D camera movement */ 237 | 238 | void TranslateCameraOnXYPlane(vizmode_params_t * VizParams, double *Direction, 239 | const double StepSize) { 240 | 241 | /* Projection onto XY plane */ 242 | Direction[2] = 0.0; 243 | UnitVect(Direction, Direction); 244 | 245 | /* Translation */ 246 | VizParams->CenterX += Direction[0] * StepSize; 247 | VizParams->CenterY += Direction[1] * StepSize; 248 | VizParams->EyeX += Direction[0] * StepSize; 249 | VizParams->EyeY += Direction[1] * StepSize; 250 | 251 | } 252 | 253 | void RotateCameraAroundCenter(vizmode_params_t * VizParams, double *Axis, 254 | const double angle) { 255 | 256 | /* Creating eye from center vector */ 257 | FillVect(EyeFromCenter, 258 | VizParams->EyeX - VizParams->CenterX, 259 | VizParams->EyeY - VizParams->CenterY, 260 | VizParams->EyeZ - VizParams->CenterZ); 261 | 262 | /* Rotation */ 263 | RotateVectAroundSpecificAxis(EyeFromCenter, EyeFromCenter, Axis, angle); 264 | NewEyeZ = VizParams->CenterZ + EyeFromCenter[2]; 265 | 266 | DistanceFromCenter = VectAbsXY(EyeFromCenter); 267 | 268 | if (VizParams->DistanceFromCenterLimit_XY > DistanceFromCenter || 269 | VizParams->EyeZLimit > NewEyeZ) { 270 | return; 271 | } else { 272 | VizParams->EyeX = VizParams->CenterX + EyeFromCenter[0]; 273 | VizParams->EyeY = VizParams->CenterY + EyeFromCenter[1]; 274 | VizParams->EyeZ = NewEyeZ; 275 | } 276 | 277 | } 278 | 279 | void ZoomOnCenter(vizmode_params_t * VizParams, const double StepSize) { 280 | 281 | /* Negative StepSize means zoom in... */ 282 | 283 | FillVect(EyeFromCenter, VizParams->EyeX - VizParams->CenterX, 284 | VizParams->EyeY - VizParams->CenterY, 285 | VizParams->EyeZ - VizParams->CenterZ); 286 | DistanceFromCenter = VectAbs(EyeFromCenter); 287 | UnitVect(EyeFromCenter, EyeFromCenter); 288 | NewEyeZ = EyeFromCenter[2] * StepSize + VizParams->EyeZ; 289 | 290 | if (DistanceFromCenter + StepSize < VizParams->DistanceFromCenterLimit 291 | || NewEyeZ < VizParams->EyeZLimit) { 292 | return; 293 | } else { 294 | VizParams->EyeX += EyeFromCenter[0] * StepSize; 295 | VizParams->EyeY += EyeFromCenter[1] * StepSize; 296 | VizParams->EyeZ = NewEyeZ; 297 | } 298 | 299 | } 300 | -------------------------------------------------------------------------------- /src/dynspecviz.h: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* Drawing objects related to specific dynamical systems */ 7 | 8 | #ifndef DYNSPECVIZ_H 9 | #define DYNSPECVIZ_H 10 | 11 | #include 12 | #include "vizualizer/objects_2d.h" 13 | #include "vizualizer/objects_3d.h" 14 | #include "utilities/datastructs.h" 15 | #include "utilities/dynamics_utils.h" 16 | #include "utilities/math_utils.h" 17 | #include "vizmode.h" 18 | 19 | /* 2D */ 20 | 21 | /* Drawing copters, etc. (model-specific objects) */ 22 | 23 | /* Arrow represents the actual velocity 24 | * "x" and "y" are the positions of the copter in real coordinates 25 | * "velocity_x" and "velocity_y" are the components of the velocity vector 26 | * "MapSizexy" are the Length of the displayed area in real coordinates (cm) 27 | * "arrowcolor" is the color of the arrow (r, g, b) 28 | */ 29 | void DrawVelocityArrow_2D(const double x, 30 | const double y, 31 | const double velocity_x, 32 | const double velocity_y, 33 | const double MapSizexy, const float *arrowcolor); 34 | 35 | /* Filled circle with the size of a quadcopter (40 cm) 36 | * "x" and "y" are the positions of the copter in real coordinates (cm) 37 | * "MapSizexy" are the Length of the displayed area in real coordinates (cm) 38 | * "agentscolor" is the color of the copter (RGB) 39 | */ 40 | void DrawCopter_2D(const double x, const double y, const double MapSizexy, 41 | const double Radius, const float *agentcolor); 42 | 43 | /* Labels above agents 44 | */ 45 | void DrawAgentLabel_2D(phase_t * Phase, 46 | const int WhichAgent, 47 | char *Label, 48 | bool Display, vizmode_params_t * VizParams, const float *Color); 49 | 50 | /* Network arrow between two agents 51 | */ 52 | void DrawNetworkArrow_2D(phase_t * Phase, 53 | const int FromWhichAgent, 54 | const int ToWhichAgent, 55 | vizmode_params_t * VizParams, const float *color); 56 | 57 | /* Network arrow between two positions 58 | */ 59 | void DrawNetworkArrowBetweenPositions_2D(double *FromCoords, 60 | double *ToCoords, vizmode_params_t * VizParams, const float *color); 61 | 62 | /* Sensor Range Newtork for agent "WhichAgent" 63 | */ 64 | void DrawSensorRangeNetwork_2D(phase_t * PhaseData, 65 | const double SensorRangeToDisplay, 66 | const int WhichAgent, 67 | const double Delay, 68 | const int Now, 69 | const double h, vizmode_params_t * VizParams, const float *color); 70 | 71 | /* 3D */ 72 | 73 | /* Sensor range network for agent "WhichAgent" 74 | */ 75 | void DrawSensorRangeNetwork_3D(phase_t * PhaseData, 76 | const double SensorRangeToDisplay, 77 | const int WhichAgent, 78 | const double Delay, 79 | const int Now, 80 | const double h, const double MapSizexy, const float *color); 81 | 82 | /* 3D camera movement */ 83 | 84 | void TranslateCameraOnXYPlane(vizmode_params_t * VizParams, double *Direction, 85 | const double StepSize); 86 | void RotateCameraAroundCenter(vizmode_params_t * VizParams, double *Axis, 87 | const double angle); 88 | void ZoomOnCenter(vizmode_params_t * VizParams, const double StepSize); 89 | 90 | #endif 91 | -------------------------------------------------------------------------------- /src/objects_menu.c: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* Tools for drawing menu objects 7 | */ 8 | 9 | #include "objects_menu.h" 10 | #include 11 | 12 | /* Draws a menu item (a parameter of the model) with given label, unit of measurement and value */ 13 | void DrawMenuItem(const char key, 14 | const char *MenuLabel, 15 | const char *UnitOfMeasurement, 16 | const char *Value, const float Y, const float *color) { 17 | 18 | glColor3f(color[0], color[1], color[2]); 19 | 20 | DrawTransparentShape(-0.9, Y - 0.025, 1.8, HEIGHT_OF_MENU_ITEM, color); 21 | 22 | static char keylabel[3]; 23 | sprintf(keylabel, "'%c'", key); 24 | 25 | static double TopOfMenuItem; 26 | TopOfMenuItem = HEIGHT_OF_MENU_ITEM * 0.5 + Y; 27 | 28 | DrawString(-0.8, TopOfMenuItem - 0.015, GLUT_BITMAP_9_BY_15, keylabel, 29 | color); 30 | DrawString(-0.6, TopOfMenuItem - 0.015, GLUT_BITMAP_8_BY_13, MenuLabel, 31 | color); 32 | DrawString(-0.6, TopOfMenuItem - 0.07, GLUT_BITMAP_8_BY_13, 33 | UnitOfMeasurement, color); 34 | DrawString(0.6, TopOfMenuItem - 0.03, GLUT_BITMAP_9_BY_15, Value, color); 35 | 36 | } 37 | 38 | /* Displays two triangles next to the selected menu item */ 39 | void DrawMenuSelection(const double x, const double y, const double Length, 40 | const float *color) { 41 | 42 | DrawTriangle(x, y, Length, 0, color); 43 | DrawTriangle(x + 1.9, y, Length, M_PI, color); 44 | 45 | } 46 | 47 | /* Displays a menu item containing features of a parameter of a unit */ 48 | void DrawMenuItemOfUnitModel(const char key, 49 | const unit_param_double_t * UnitParams, const int Digits, 50 | const double Multiplier, const double y, const float *color) { 51 | 52 | static char value[10]; 53 | 54 | /* Setting up Digits after decimal point */ 55 | switch (Digits) { 56 | case 0: 57 | sprintf(value, "%1.0f", Multiplier * UnitParams->Value); 58 | break; 59 | case 1: 60 | sprintf(value, "%1.1f", Multiplier * UnitParams->Value); 61 | break; 62 | case 2: 63 | sprintf(value, "%1.2f", Multiplier * UnitParams->Value); 64 | break; 65 | case 3: 66 | sprintf(value, "%1.3f", Multiplier * UnitParams->Value); 67 | break; 68 | default: 69 | sprintf(value, "%1.2f", Multiplier * UnitParams->Value); 70 | break; 71 | } 72 | 73 | DrawMenuItem(key, UnitParams->Name, UnitParams->UnitOfMeas, value, y, 74 | color); 75 | 76 | } 77 | 78 | /* Displays a menusystem from a struct of model parameters (unit model) */ 79 | void DrawMenuItemsOfUnitModel(const unit_model_params_t * UnitParams, 80 | const float *color) { 81 | 82 | static double Offset = 0.0; 83 | 84 | /* Draw title */ 85 | DrawString(-0.8, 0.92, GLUT_BITMAP_9_BY_15, "Parameters of the robot model", 86 | color); 87 | 88 | /* Control algorith parameters */ 89 | DrawMenuItemOfUnitModel('1', &UnitParams->Tau_PID_XY, 2, 1.0, 0.8 - Offset, 90 | color); 91 | DrawMenuItemOfUnitModel('2', &UnitParams->Tau_PID_Z, 2, 1.0, 92 | 0.8 - HEIGHT_OF_MENU_ITEM - Offset, color); 93 | DrawMenuItemOfUnitModel('3', &UnitParams->a_max, 1, 0.01, 94 | 0.8 - HEIGHT_OF_MENU_ITEM * 2 - Offset, color); 95 | /* Parameters of communication */ 96 | DrawMenuItemOfUnitModel('4', &UnitParams->R_C, 1, 0.01, 97 | 0.8 - HEIGHT_OF_MENU_ITEM * 3 - Offset, color); 98 | DrawMenuItemOfUnitModel('5', &UnitParams->t_del, 2, 1.0, 99 | 0.8 - HEIGHT_OF_MENU_ITEM * 4 - Offset, color); 100 | /* Parameters of sensors */ 101 | DrawMenuItemOfUnitModel('6', &UnitParams->t_GPS, 2, 1.0, 102 | 0.8 - HEIGHT_OF_MENU_ITEM * 5 - Offset, color); 103 | DrawMenuItemOfUnitModel('7', &UnitParams->Sigma_GPS_XY, 3, 0.0001, 104 | 0.8 - HEIGHT_OF_MENU_ITEM * 6 - Offset, color); 105 | DrawMenuItemOfUnitModel('8', &UnitParams->Sigma_GPS_Z, 3, 0.0001, 106 | 0.8 - HEIGHT_OF_MENU_ITEM * 7 - Offset, color); 107 | /* Parameters of environmental noises */ 108 | DrawMenuItemOfUnitModel('9', &UnitParams->Sigma_Outer_XY, 2, 0.0001, 109 | 0.8 - HEIGHT_OF_MENU_ITEM * 8 - Offset, color); 110 | DrawMenuItemOfUnitModel('a', &UnitParams->Sigma_Outer_Z, 2, 0.0001, 111 | 0.8 - HEIGHT_OF_MENU_ITEM * 9 - Offset, color); 112 | DrawMenuItemOfUnitModel('b', &UnitParams->Wind_Magn_Avg, 2, 0.01, 113 | 0.8 - HEIGHT_OF_MENU_ITEM * 10 - Offset, color); 114 | /* Parameters of packet loss */ 115 | DrawMenuItemOfUnitModel('c', &UnitParams->packet_loss_ratio, 2, 1.0, 116 | 0.8 - HEIGHT_OF_MENU_ITEM * 11 - Offset, color); 117 | DrawMenuItemOfUnitModel('d', &UnitParams->packet_loss_distance, 1, 0.01, 118 | 0.8 - HEIGHT_OF_MENU_ITEM * 12 - Offset, color); 119 | 120 | } 121 | 122 | /* Displays a menu item containing features of a parameter of the flocking model */ 123 | void DrawMenuItemOfFlockingModel(const char key, 124 | const fl_param_double_t * Param, double Y, const float *color) { 125 | 126 | int i; 127 | static char value[7]; 128 | static bool Labeled; 129 | Labeled = false; 130 | sprintf(value, " "); 131 | 132 | static double Multipliedvalue; 133 | 134 | /* Checking existence of labels for specific values */ 135 | for (i = 0; i < (Param->NumberOfLabels); i++) { 136 | if (Param->Labels.Values[i] == Param->Value) { 137 | sprintf(value, "%s", Param->Labels.Captions[i]); 138 | Labeled = true; 139 | break; 140 | } 141 | } 142 | 143 | if (false == Labeled) { 144 | 145 | Multipliedvalue = (Param->Mult) * Param->Value; 146 | /* Setting up Digits after decimal point */ 147 | switch (Param->Digits) { 148 | case 0: 149 | sprintf(value, "%1.0f", Multipliedvalue); 150 | break; 151 | case 1: 152 | sprintf(value, "%1.1f", Multipliedvalue); 153 | break; 154 | case 2: 155 | sprintf(value, "%1.2f", Multipliedvalue); 156 | break; 157 | case 3: 158 | sprintf(value, "%1.3f", Multipliedvalue); 159 | break; 160 | default: 161 | sprintf(value, "%1.2f", Multipliedvalue); 162 | break; 163 | } 164 | 165 | } 166 | 167 | DrawMenuItem(key, Param->Name, Param->UnitOfMeas, value, Y, color); 168 | 169 | /* Display stored value, if the parameter is Constant */ 170 | if (Param->Constant == false) { 171 | return; 172 | } 173 | 174 | if (Param->Value != Param->StoredValue) { 175 | Labeled = false; 176 | /* Checking existence of labels for specific stored values */ 177 | for (i = 0; i < (Param->NumberOfLabels); i++) { 178 | if (Param->Labels.Values[i] == Param->StoredValue) { 179 | sprintf(value, "Press F12 to change to '%s'!", 180 | Param->Labels.Captions[i]); 181 | Labeled = true; 182 | break; 183 | } 184 | } 185 | if (false == Labeled) { 186 | /* Setting up Digits after decimal point */ 187 | Multipliedvalue = (Param->Mult) * Param->StoredValue; 188 | switch (Param->Digits) { 189 | case 0: 190 | sprintf(value, "Press F12 to change to %1.0f!", 191 | Multipliedvalue); 192 | break; 193 | case 1: 194 | sprintf(value, "Press F12 to change to %1.1f!", 195 | Multipliedvalue); 196 | break; 197 | case 2: 198 | sprintf(value, "Press F12 to change to %1.2f!", 199 | Multipliedvalue); 200 | break; 201 | case 3: 202 | sprintf(value, "Press F12 to change to %1.3f!", 203 | Multipliedvalue); 204 | break; 205 | default: 206 | sprintf(value, "Press F12 to change to %1.2f!", 207 | Multipliedvalue); 208 | break; 209 | } 210 | } 211 | } else { 212 | sprintf(value, " "); 213 | } 214 | 215 | DrawString(-0.5, Y - 0.015, GLUT_BITMAP_8_BY_13, value, color); 216 | 217 | } 218 | 219 | /* Displays a menusystem from a struct of model parameters (flocking model) */ 220 | void DrawMenuItemsOfFlockingModel(const flocking_model_params_t * 221 | FlockingParams, const float *color) { 222 | 223 | static double Offset = 0.0; 224 | 225 | /* Draw title */ 226 | DrawString(-0.8, 0.92, GLUT_BITMAP_9_BY_15, 227 | "Parameters of the flocking algorithm", color); 228 | 229 | int i = 0; 230 | int n = 0; 231 | static char key[10]; 232 | 233 | for (i = 0; i < FlockingParams->NumberOfParameters; i++) { 234 | if (FlockingParams->Params[i].InMenu == true) { 235 | sprintf(key, "%01x", i + 1); 236 | DrawMenuItemOfFlockingModel(key[0], &FlockingParams->Params[i], 237 | 0.8 - i * HEIGHT_OF_MENU_ITEM - Offset, color); 238 | } 239 | } 240 | 241 | } 242 | -------------------------------------------------------------------------------- /src/objects_menu.h: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* Tools for drawing menu objects 7 | */ 8 | 9 | #ifndef OBJECTS_MENU_H 10 | #define OBJECTS_MENU_H 11 | 12 | #define HEIGHT_OF_MENU_ITEM 0.113 13 | 14 | #include "vizualizer/objects_2d.h" 15 | #include "algo.h" 16 | #include "utilities/param_utils.h" 17 | 18 | /* Draws a menu item (a parameter of the model) with given label, unit of measurement and value. 19 | * "key" is the button referred to the parameter 20 | * "Y" defines the position of the item 21 | * "color" is an array that contains RGB values 22 | */ 23 | void DrawMenuItem(const char key, const char *MenuLabel, 24 | const char *UnitOfMeasurement, const char *Value, const float Y, 25 | const float *color); 26 | 27 | /* Displays two triangles next to the selected menu item 28 | * like this: 29 | * |> menu label <| 30 | * "x" and "y" are the position of the selection in GL coordinates 31 | * "Length" is the distance between the two triangles 32 | * "color" is an array that contains RGB values 33 | */ 34 | void DrawMenuSelection(const double x, const double y, const double Length, 35 | const float *color); 36 | 37 | /* Displays a parameter of the unit model 38 | * "key" is the button referred to the parameter 39 | * "Multiplier" is a Multiplier of the output. Useful for changing between cm and m, for example 40 | * "Digits" is the number of Digits displayed after decimal point. (possible values are 0, 1, 2 or 3) 41 | * "color" is an array that contains RGB values 42 | */ 43 | void DrawMenuItemOfUnitModel(const char key, const unit_param_double_t * Param, 44 | const int Digits, const double Multiplier, const double y, 45 | const float *color); 46 | 47 | /* Displays a menusystem from a struct of model parameters (unit model) 48 | * "color" is an array that contains RGB values 49 | */ 50 | void DrawMenuItemsOfUnitModel(const unit_model_params_t * Params, 51 | const float *color); 52 | 53 | /* Displays a menusystem from a struct of model parameters (flocking model) 54 | * "color" is an array that contains RGB values 55 | */ 56 | void DrawMenuItemsOfFlockingModel(const flocking_model_params_t * Params, 57 | const float *color); 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /src/robotmodel.h: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* Tools for handling the robot model 7 | */ 8 | 9 | #ifndef ROBOTMODEL_H 10 | #define ROBOTMODEL_H 11 | 12 | #include "utilities/param_utils.h" 13 | #include "utilities/math_utils.h" 14 | #include "utilities/datastructs.h" 15 | #include "vizmode.h" 16 | #include "sensors.h" 17 | #include "algo.h" 18 | #include "algo_gui.h" 19 | #include "vizmode.h" 20 | #include "utilities/debug_utils.h" 21 | #include 22 | #include 23 | 24 | /* Step positions and velocities 25 | */ 26 | void Step(phase_t * OutputPhase, phase_t * GPSPhase, phase_t * GPSDelayedPhase, 27 | phase_t * PhaseData, unit_model_params_t * UnitParams, 28 | flocking_model_params_t * FlockingParams, sit_parameters_t * SitParams, 29 | vizmode_params_t * VizParams, int TimeStepLooped, int TimeStepReal, 30 | bool CountCollisions, bool * ConditionsReset, int *Collisions, 31 | bool * AgentsInDanger, double *WindVelocityVector, 32 | double *Accelerations); 33 | 34 | /* Initialization and killing 35 | */ 36 | void InitializePreferredVelocities(phase_t * Phase, 37 | flocking_model_params_t * FlockingParams, sit_parameters_t * SitParams, 38 | unit_model_params_t * UnitParams, double *WindVelocityVector); 39 | void freePreferredVelocities(phase_t * Phase, 40 | flocking_model_params_t * FlockingParams, sit_parameters_t * SitParams); 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /src/sensors.c: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* Models of inaccuracy of the sensors 7 | * "inner noise" 8 | */ 9 | 10 | #include "sensors.h" 11 | #include "utilities/datastructs.h" 12 | #include 13 | 14 | /* Setting up initial values of GPS-measured positions and velocities */ 15 | void ResetGPSNoises(phase_t * GPSPhase, phase_t * GPSDelayedPhase) { 16 | 17 | int i, j; 18 | for (j = 0; j < GPSPhase->NumberOfAgents; j++) { 19 | for (i = 0; i < 3; i++) { 20 | GPSPhase->Coordinates[j][i] = 0; 21 | GPSPhase->Velocities[j][i] = 0; 22 | GPSDelayedPhase->Coordinates[j][i] = 0; 23 | GPSDelayedPhase->Velocities[j][i] = 0; 24 | } 25 | } 26 | 27 | } 28 | 29 | /* Fluctuations of GPS signal */ 30 | void StepGPSNoises(phase_t * WhichPhase, unit_model_params_t * UnitParams) { 31 | 32 | /* GPS fluctuations are modelled as a random Gaussian noise with damping in a central potantial around the real position of the agent. 33 | */ 34 | static double GPSPosition[3]; 35 | NullVect(GPSPosition, 3); 36 | static double GPSVelocity[3]; 37 | NullVect(GPSVelocity, 3); 38 | 39 | static double GPSNoiseToAdd[3]; 40 | static double Force[3]; // Central potential 41 | static double Damping[3]; // Damping which is proportional with GPS-measured velocity. 42 | 43 | int i; 44 | 45 | for (i = 0; i < WhichPhase->NumberOfAgents; i++) { 46 | 47 | if (Noises[i] == false) 48 | continue; 49 | 50 | GetAgentsCoordinates(GPSPosition, WhichPhase, i); 51 | GetAgentsVelocity(GPSVelocity, WhichPhase, i); 52 | 53 | /* Random acceleration vector */ 54 | NullVect(GPSNoiseToAdd, 3); 55 | GPSNoiseToAdd[0] = randomizeGaussDouble(0, 1); 56 | GPSNoiseToAdd[1] = randomizeGaussDouble(0, 1); 57 | GPSNoiseToAdd[2] = randomizeGaussDouble(0, 1); 58 | 59 | /* Quadratic potential with friction */ 60 | static double lambda_GPS_XY = 0.1; 61 | static double lambda_GPS_Z = 0.1; 62 | double D_GPS_XY = 63 | sqrt(2 * lambda_GPS_XY * UnitParams->Sigma_GPS_XY.Value) / 300; 64 | double D_GPS_Z = 65 | sqrt(2 * lambda_GPS_Z * UnitParams->Sigma_GPS_Z.Value) / 300; 66 | 67 | /* Spring-like force */ 68 | NullVect(Force, 3); 69 | VectSum(Force, Force, GPSPosition); 70 | UnitVect(Force, Force); 71 | MultiplicateWithScalar(Force, Force, 72 | -D_GPS_XY * VectAbsXY(GPSPosition) * UnitParams->t_GPS.Value, 73 | 2); 74 | Force[2] *= -D_GPS_Z * fabs(GPSPosition[2]) * UnitParams->t_GPS.Value; 75 | 76 | /* Damping */ 77 | NullVect(Damping, 3); 78 | VectSum(Damping, Damping, GPSVelocity); 79 | MultiplicateWithScalar(Damping, Damping, 80 | -lambda_GPS_XY * UnitParams->t_GPS.Value, 2); 81 | Damping[2] *= -lambda_GPS_Z * UnitParams->t_GPS.Value; 82 | 83 | /* Sigma_GPS refers to an anisotropic diffusion constant */ 84 | MultiplicateWithScalar(GPSNoiseToAdd, GPSNoiseToAdd, 85 | sqrt(2 * lambda_GPS_XY * UnitParams->t_GPS.Value * 86 | UnitParams->Sigma_GPS_XY.Value), 2); 87 | GPSNoiseToAdd[2] *= 88 | sqrt(2 * lambda_GPS_Z * UnitParams->t_GPS.Value * 89 | UnitParams->Sigma_GPS_Z.Value); 90 | VectSum(GPSNoiseToAdd, GPSVelocity, GPSNoiseToAdd); 91 | 92 | /* Add GPS-measured terms to the real terms to get full measured velocities and positions */ 93 | VectSum(GPSNoiseToAdd, GPSNoiseToAdd, Force); 94 | VectSum(GPSNoiseToAdd, GPSNoiseToAdd, Damping); 95 | 96 | InsertAgentsVelocity(WhichPhase, GPSNoiseToAdd, i); 97 | 98 | MultiplicateWithScalar(GPSNoiseToAdd, GPSNoiseToAdd, 99 | UnitParams->t_GPS.Value, 3); 100 | 101 | VectSum(GPSPosition, GPSPosition, GPSNoiseToAdd); 102 | 103 | InsertAgentsCoordinates(WhichPhase, GPSPosition, i); 104 | 105 | } 106 | 107 | } 108 | -------------------------------------------------------------------------------- /src/sensors.h: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* Models of inaccuracy of sensors 7 | * "inner noise" 8 | */ 9 | 10 | #ifndef SENSORS_H 11 | #define SENSORS_H 12 | 13 | #include "utilities/math_utils.h" 14 | #include "utilities/dynamics_utils.h" 15 | #include "utilities/param_utils.h" 16 | 17 | /* Setting up initial values of GPS-measured positions and velocities 18 | */ 19 | void ResetGPSNoises(phase_t * GPSPhase, phase_t * GPSDelayedPhase); 20 | 21 | /* Steps Stored (actual and delayed) GPS fluctuations 22 | */ 23 | void StepGPSNoises(phase_t * WhichPhase, unit_model_params_t * UnitParams); 24 | 25 | #endif 26 | -------------------------------------------------------------------------------- /src/stat.c: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* Tools for calculating basic statistical quantities */ 7 | 8 | #include "stat.h" 9 | 10 | /* Global "temporary" variables */ 11 | static double Avg = 0.0; 12 | static double StDev = 0.0; 13 | static double Min = 2e222; 14 | static double Max = 0.0; 15 | 16 | /* Setting up initial values for all (algo-independent) statistical properties */ 17 | void ResetStatistics(statistics_t * Statistics) { 18 | 19 | int i; 20 | 21 | for (i = 0; i < 8; i++) { 22 | Statistics->Data_Acceleration_Sum[i] = 0.0; 23 | Statistics->Data_Acceleration_StDev[i] = 0.0; 24 | Statistics->Data_Velocity_Sum[i] = 0.0; 25 | Statistics->Data_Velocity_StDev[i] = 0.0; 26 | if (3 < i) 27 | continue; 28 | Statistics->Data_DistanceBetweenUnits_Sum[i] = 0.0; 29 | Statistics->Data_DistanceBetweenUnits_StDev[i] = 0.0; 30 | Statistics->Data_Correlation_Sum[i] = 0.0; 31 | Statistics->Data_Correlation_StDev[i] = 0.0; 32 | if (2 < i) 33 | continue; 34 | Statistics->Data_DistanceBetweenNeighbours_Sum[i] = 0.0; 35 | Statistics->Data_DistanceBetweenNeighbours_StDev[i] = 0.0; 36 | Statistics->Data_CoM_Sum[i] = 0.0; 37 | Statistics->Data_CoM_StDev[i] = 0.0; 38 | } 39 | 40 | Statistics->Data_CollisionRatio_Sum = 0.0; 41 | Statistics->Data_CollisionRatio_StDev = 0.0; 42 | 43 | } 44 | 45 | /* Returns an array that contains the average, deviation, minimum and maximum of 46 | * distance between units 47 | */ 48 | double *StatOfDistanceBetweenUnits(phase_t * Phase) { 49 | 50 | int i, j; 51 | 52 | Avg = 0.0; 53 | StDev = 0.0; 54 | Min = 2e222; 55 | Max = 0.0; 56 | 57 | static double Dist_1_2 = 0.0; 58 | 59 | double *Coord1; 60 | double *Coord2; 61 | static double CoordDiff[3]; 62 | 63 | static double StatData[4]; 64 | 65 | for (i = 0; i < Phase->NumberOfAgents - 1; i++) { 66 | 67 | for (j = i + 1; j < Phase->NumberOfAgents; j++) { 68 | 69 | Coord1 = Phase->Coordinates[i]; 70 | Coord2 = Phase->Coordinates[j]; 71 | VectDifference(CoordDiff, Coord1, Coord2); 72 | Dist_1_2 = VectAbs(CoordDiff); 73 | 74 | if (Dist_1_2 > Max) { 75 | Max = Dist_1_2; 76 | } 77 | if (Dist_1_2 < Min) { 78 | Min = Dist_1_2; 79 | } 80 | 81 | Avg += Dist_1_2; 82 | StDev += Dist_1_2 * Dist_1_2; 83 | 84 | } 85 | 86 | } 87 | 88 | Avg *= 2.0 / (Phase->NumberOfAgents * (Phase->NumberOfAgents - 1)); 89 | StDev *= 2.0 / (Phase->NumberOfAgents * (Phase->NumberOfAgents - 1)); 90 | StDev -= Avg * Avg; 91 | 92 | if (StDev < 0.0) { 93 | StDev = 0.0; 94 | } 95 | 96 | StatData[0] = Avg; 97 | StatData[1] = sqrt(StDev); 98 | StatData[2] = Min; 99 | StatData[3] = Max; 100 | 101 | return StatData; 102 | 103 | } 104 | 105 | /* Returns an array that contains the average, deviation, minimum and maximum of 106 | * distance between nearest neighbours 107 | */ 108 | double *StatOfDistanceBetweenNearestNeighbours(phase_t * Phase) { 109 | 110 | int i, j; 111 | 112 | Avg = 0.0; 113 | StDev = 0.0; 114 | Max = 0.0; 115 | 116 | static double Min_i; 117 | Min_i = 2e222; 118 | static int Min_i_ID; 119 | Min_i_ID = Phase->NumberOfAgents + 10; 120 | 121 | static double Dist_1_2 = 0.0; 122 | 123 | double *Coord1; 124 | double *Coord2; 125 | static double CoordDiff[3]; 126 | 127 | static double StatData[3]; 128 | 129 | for (i = 0; i < Phase->NumberOfAgents; i++) { 130 | 131 | for (j = 0; j < Phase->NumberOfAgents; j++) { 132 | if (i != j) { 133 | 134 | Coord1 = Phase->Coordinates[i]; 135 | Coord2 = Phase->Coordinates[j]; 136 | VectDifference(CoordDiff, Coord1, Coord2); 137 | Dist_1_2 = VectAbs(CoordDiff); 138 | 139 | if (Dist_1_2 < Min_i) { 140 | Min_i = Dist_1_2; 141 | Min_i_ID = j; 142 | } 143 | } 144 | } 145 | 146 | Avg += Min_i; 147 | StDev += Min_i * Min_i; 148 | if (Min_i > Max) { 149 | Max = Min_i; 150 | } 151 | 152 | Min_i = 2e222; 153 | 154 | } 155 | 156 | Avg /= Phase->NumberOfAgents; 157 | StDev /= Phase->NumberOfAgents; 158 | StDev -= Avg * Avg; 159 | 160 | if (StDev < 0.0) { 161 | StDev = 0.0; 162 | } 163 | 164 | StatData[0] = Avg; 165 | StatData[1] = sqrt(StDev); 166 | StatData[2] = Max; 167 | 168 | return StatData; 169 | 170 | } 171 | 172 | /* Returns an array that contains the average, deviation, minimum and maximum of 173 | * velocity Length 174 | * and the Length and XYZ components of the average velocity 175 | */ 176 | double *StatOfVelocity(phase_t * Phase) { 177 | 178 | int i; 179 | double *Velocity; 180 | static double AvgVelocity[3]; 181 | NullVect(AvgVelocity, 3); 182 | 183 | Avg = 0.0; 184 | StDev = 0.0; 185 | Min = 2e222; 186 | Max = 0.0; 187 | 188 | static double velLength; 189 | velLength = 0.0; 190 | 191 | /* Output */ 192 | static double StatVel[8]; 193 | 194 | for (i = 0; i < Phase->NumberOfAgents; i++) { 195 | 196 | Velocity = Phase->Velocities[i]; 197 | 198 | VectSum(AvgVelocity, AvgVelocity, Velocity); 199 | 200 | velLength = VectAbs(Velocity); 201 | Avg += velLength; 202 | StDev += velLength * velLength; 203 | 204 | if (velLength > Max) { 205 | Max = velLength; 206 | } 207 | if (velLength < Min) { 208 | Min = velLength; 209 | } 210 | 211 | } 212 | 213 | Avg /= Phase->NumberOfAgents; 214 | StDev /= Phase->NumberOfAgents; 215 | StDev -= Avg * Avg; 216 | 217 | if (StDev < 0.0) { 218 | StDev = 0.0; 219 | } 220 | 221 | MultiplicateWithScalar(AvgVelocity, AvgVelocity, 222 | 1.0 / (Phase->NumberOfAgents), 3); 223 | 224 | StatVel[0] = Avg; 225 | StatVel[1] = sqrt(StDev); 226 | StatVel[2] = Min; 227 | StatVel[3] = Max; 228 | StatVel[4] = VectAbs(AvgVelocity); 229 | StatVel[5] = AvgVelocity[0]; 230 | StatVel[6] = AvgVelocity[1]; 231 | StatVel[7] = AvgVelocity[2]; 232 | 233 | return StatVel; 234 | 235 | } 236 | 237 | /* Returns an array that contains the average, deviation, minimum and maximum of 238 | * velocity scalar products 239 | */ 240 | double *StatOfCorrelation(phase_t * Phase) { 241 | 242 | int i, j; 243 | 244 | Avg = 0.0; 245 | StDev = 0.0; 246 | Min = 2e222; 247 | Max = 0.0; 248 | 249 | static double Corr_1_2; 250 | Corr_1_2 = 0.0; 251 | 252 | double *Vel1; 253 | double *Vel2; 254 | 255 | static double velLength1; 256 | static double velLength2; 257 | velLength1 = 0.0; 258 | velLength2 = 0.0; 259 | 260 | static double StatData[4]; 261 | 262 | for (i = 0; i < Phase->NumberOfAgents - 1; i++) { 263 | 264 | for (j = i + 1; j < Phase->NumberOfAgents; j++) { 265 | 266 | Vel1 = Phase->Velocities[i]; 267 | Vel2 = Phase->Velocities[j]; 268 | 269 | velLength1 = VectAbs(Vel1); 270 | velLength2 = VectAbs(Vel2); 271 | 272 | if (velLength1 != 0 && velLength2 != 0) { 273 | Corr_1_2 = 274 | ScalarProduct(Vel1, Vel2, 275 | 3) / (VectAbs(Vel1) * VectAbs(Vel2)); 276 | } else { 277 | Corr_1_2 = 0.0; 278 | } 279 | 280 | if (Corr_1_2 > Max) { 281 | Max = Corr_1_2; 282 | } 283 | if (Corr_1_2 < Min) { 284 | Min = Corr_1_2; 285 | } 286 | 287 | Avg += Corr_1_2; 288 | StDev += Corr_1_2 * Corr_1_2; 289 | 290 | } 291 | 292 | } 293 | 294 | Avg *= 2.0 / (Phase->NumberOfAgents * (Phase->NumberOfAgents - 1)); 295 | StDev *= 2.0 / (Phase->NumberOfAgents * (Phase->NumberOfAgents - 1)); 296 | StDev -= Avg * Avg; 297 | 298 | if (StDev < 0.0) { 299 | StDev = 0.0; 300 | } 301 | 302 | StatData[0] = Avg; 303 | StatData[1] = sqrt(StDev); 304 | StatData[2] = Min; 305 | StatData[3] = Max; 306 | 307 | return StatData; 308 | 309 | } 310 | 311 | /* Returns an array that contains the average, deviation, minimum and maximum of 312 | * accelerations 313 | */ 314 | double *StatOfAcceleration(double *Accelerations, const double NumberOfAgents) { 315 | 316 | int i; 317 | 318 | double Avg = 0.0; 319 | double StDev = 0.0; 320 | double Min = 2e222; 321 | double Max = 0.0; 322 | 323 | static double accLength; 324 | 325 | /* Output */ 326 | static double StatAcc[4]; 327 | 328 | for (i = 0; i < NumberOfAgents; i++) { 329 | 330 | accLength = Accelerations[i]; 331 | Avg += accLength; 332 | StDev += accLength * accLength; 333 | 334 | if (accLength > Max) { 335 | Max = accLength; 336 | } 337 | if (accLength < Min) { 338 | Min = accLength; 339 | } 340 | 341 | } 342 | 343 | Avg /= NumberOfAgents; 344 | StDev /= NumberOfAgents; 345 | StDev -= Avg * Avg; 346 | 347 | if (StDev < 0.0) { 348 | StDev = 0.0; 349 | } 350 | 351 | StatAcc[0] = Avg; 352 | StatAcc[1] = sqrt(StDev); 353 | StatAcc[2] = Min; 354 | StatAcc[3] = Max; 355 | 356 | return StatAcc; 357 | 358 | } 359 | 360 | /* Calculating a parameter that is proportional with 361 | * the number of collisions and dangerous situations. 362 | */ 363 | double RatioOfDangerousSituations(phase_t * Phase, const double RadiusOfCopter) { 364 | 365 | /* Vectors to store positions */ 366 | double *x_i; 367 | static double x_j[3]; 368 | 369 | static int NumberOfCollisions; 370 | NumberOfCollisions = 0; 371 | 372 | int i, j; 373 | 374 | /* Is there any collision? */ 375 | for (i = 0; i < Phase->NumberOfAgents; i++) { 376 | 377 | x_i = Phase->Coordinates[i]; 378 | 379 | for (j = 0; j < i; j++) { 380 | 381 | GetAgentsCoordinates(x_j, Phase, j); 382 | VectDifference(x_j, x_j, x_i); 383 | 384 | if (VectAbs(x_j) <= RadiusOfCopter) { 385 | 386 | NumberOfCollisions++; 387 | 388 | } 389 | 390 | } 391 | 392 | } 393 | 394 | /* Final result has to be divided with the number of all possible collisions */ 395 | return NumberOfCollisions * 2.0 / (Phase->NumberOfAgents * 396 | (Phase->NumberOfAgents - 1)); 397 | 398 | } 399 | -------------------------------------------------------------------------------- /src/stat.h: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* Tools for calculating basic statistical quantities */ 7 | 8 | #ifndef STAT_H 9 | #define STAT_H 10 | 11 | #include 12 | #include "utilities/output_utils.h" 13 | #include "utilities/math_utils.h" 14 | #include "utilities/datastructs.h" 15 | #include "utilities/dynamics_utils.h" 16 | 17 | /* This struct contains data arrays for storing general statistical properties 18 | */ 19 | typedef struct { 20 | 21 | /* Sums for averages */ 22 | double Data_Acceleration_Sum[4]; 23 | double Data_Correlation_Sum[4]; 24 | double Data_DistanceBetweenUnits_Sum[4]; 25 | double Data_DistanceBetweenNeighbours_Sum[3]; 26 | double Data_CollisionRatio_Sum; 27 | double Data_Velocity_Sum[8]; 28 | double Data_CoM_Sum[3]; 29 | 30 | /* stDevs */ 31 | double Data_Acceleration_StDev[4]; 32 | double Data_Correlation_StDev[4]; 33 | double Data_DistanceBetweenUnits_StDev[4]; 34 | double Data_DistanceBetweenNeighbours_StDev[3]; 35 | double Data_CollisionRatio_StDev; 36 | double Data_CoM_StDev[3]; 37 | double Data_Velocity_StDev[8]; 38 | 39 | } statistics_t; 40 | 41 | /* Macro for updating statistics 42 | * "stat" denotes an element from the statistics_t struct (e.g. CoM means Data_CoM_Sum and Data_CoM_StDev) , while 43 | * "n" denotes the number of parameters to save 44 | */ 45 | #define UPDATE_STATISTICS(stat, n) \ 46 | if (ActualSaveModes.Save##stat != STEADYSTAT || (ActualStatUtils.ElapsedTime - ActualSitParams.StartOfSteadyState) > 0.0) { \ 47 | static int d; \ 48 | for (d = 0; d < n; d++) { \ 49 | ActualStatistics.Data_##stat##_Sum[d] += StatData[d] * ActualSitParams.DeltaT; \ 50 | ActualStatistics.Data_##stat##_StDev[d] += StatData[d] * StatData [d] * ActualSitParams.DeltaT; \ 51 | } \ 52 | } 53 | 54 | /* Macro for saving statistics 55 | * "stat" denotes an element from the statistics_t struct (e.g. CoM means Data_CoM_Sum and Data_CoM_StDev) , while 56 | * "n" denotes the number of parameters to save 57 | */ 58 | #define SAVE_STATISTICS(stat, n) \ 59 | static int z; \ 60 | static double temp_value; \ 61 | static double temp_time; \ 62 | temp_time = ActualStatUtils.ElapsedTime; \ 63 | if (STEADYSTAT == ActualSaveModes.Save##stat) { \ 64 | temp_time -= ActualSitParams.StartOfSteadyState; \ 65 | }; \ 66 | if (STEADYSTAT == ActualSaveModes.Save##stat || STAT == ActualSaveModes.Save##stat) { \ 67 | fprintf (f_##stat, "%lf\t", temp_time); \ 68 | fprintf (f_##stat##_StDev, "%lf\t", temp_time); \ 69 | for (z = 0; z < n; z++) { \ 70 | temp_value = ActualStatistics.Data_##stat##_Sum[z] / temp_time; \ 71 | fprintf (f_##stat, "%lf", temp_value); \ 72 | fprintf (f_##stat##_StDev, "%lf", \ 73 | sqrt (ActualStatistics.Data_##stat##_StDev[z] / temp_time - pow (temp_value, 2))); \ 74 | if (z < n - 1) { \ 75 | fprintf (f_##stat, "\t"); \ 76 | fprintf (f_##stat##_StDev, "\t"); \ 77 | } else {\ 78 | fprintf (f_##stat, "\n"); \ 79 | fprintf (f_##stat##_StDev, "\n"); \ 80 | } \ 81 | } \ 82 | fclose (f_##stat##_StDev); \ 83 | } 84 | 85 | /* Setting up initial values for all statistical properties 86 | */ 87 | void ResetStatistics(statistics_t * Statistics); 88 | 89 | ///////////////////////////////////////////////////////////////////////////////// 90 | /* General stat functions */ 91 | 92 | /* Returns an array that contains the average, deviation, minimum and maximum of 93 | * distance between units 94 | */ 95 | double *StatOfDistanceBetweenUnits(phase_t * Phase); 96 | 97 | /* Returns an array that contains the average, deviation, minimum and maximum of 98 | * distance between nearest neighbours 99 | */ 100 | double *StatOfDistanceBetweenNearestNeighbours(phase_t * Phase); 101 | 102 | /* Returns an array that contains the average, deviation, minimum and maximum of 103 | * velocity Length 104 | * and the Length and XYZ components of the average velocity 105 | */ 106 | double *StatOfVelocity(phase_t * Phase); 107 | 108 | /* Returns an array that contains the average, deviation, minimum and maximum of 109 | * velocity scalar products 110 | */ 111 | double *StatOfCorrelation(phase_t * Phase); 112 | 113 | /* Returns an array that contains the average, deviation, minimum and maximum of 114 | * accelerations 115 | */ 116 | double *StatOfAcceleration(double *Accelerations, const double NumberOfAgents); 117 | 118 | /* Calculating a parameter that is proportional with 119 | * the number of collisions and dangerous situations. 120 | */ 121 | double RatioOfDangerousSituations(phase_t * Phase, const double RadiusOfCopter); 122 | 123 | #endif 124 | -------------------------------------------------------------------------------- /src/utilities/arenas.c: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "file_utils.h" 13 | #include "arenas.h" 14 | 15 | static int ArenaCallback(void *user, const char *section, const char *name, 16 | const char *value) { 17 | 18 | arenas_t *arenas = user; 19 | float x, y, z; 20 | const char *Sep; 21 | 22 | /* if this is a new section */ 23 | if (!arenas->a_count 24 | || strcmp(section, arenas->a[arenas->a_count - 1].name)) { 25 | if (arenas->a_count == MAX_ARENA_COUNT) { 26 | printf("ERROR: No space for more arenas.\n"); 27 | return 0; 28 | } 29 | /* store new index, name and reset vertex count */ 30 | arenas->a[arenas->a_count].index = arenas->a_count; 31 | strncpy(arenas->a[arenas->a_count].name, section, 32 | MAX_ARENA_NAME_LENGTH); 33 | arenas->a[arenas->a_count].p_count = 0; 34 | /* increase counter */ 35 | (arenas->a_count)++; 36 | } 37 | 38 | if (strcmp(name, "point")) { 39 | printf("ERROR: unknown name while parsing arena (%s)\n", name); 40 | return 0; 41 | } 42 | 43 | if (sscanf(value, "%f %f %f", &x, &y, &z) != 3) { 44 | z = 0; 45 | if (sscanf(value, "%f %f", &x, &y) != 2) { 46 | printf("ERROR: could not parse arena point\n"); 47 | return 0; 48 | } 49 | } 50 | 51 | /* store point */ 52 | arenas->a[arenas->a_count - 53 | 1].p[arenas->a[arenas->a_count - 1].p_count][0] = x; 54 | arenas->a[arenas->a_count - 55 | 1].p[arenas->a[arenas->a_count - 1].p_count][1] = y; 56 | arenas->a[arenas->a_count - 57 | 1].p[arenas->a[arenas->a_count - 1].p_count][2] = z; 58 | (arenas->a[arenas->a_count - 1].p_count)++; 59 | 60 | return 1; 61 | } 62 | 63 | int ParseArenaFile(const char *name, arenas_t * arenas, 64 | bool define_circle_and_square) { 65 | int result; 66 | memset(arenas, 0, sizeof(arenas_t)); 67 | if (define_circle_and_square) { 68 | arenas->a_count = 2; 69 | // insert predefined CIRCLE at place 0 70 | strcpy(arenas->a[ARENA_CIRCLE].name, "circle"); 71 | arenas->a[ARENA_CIRCLE].index = ARENA_CIRCLE; 72 | // insert predefined SQUARE at place 1 73 | strcpy(arenas->a[ARENA_SQUARE].name, "square"); 74 | arenas->a[ARENA_SQUARE].index = ARENA_SQUARE; 75 | arenas->a[ARENA_SQUARE].p_count = 4; 76 | arenas->a[ARENA_SQUARE].p[0][0] = -1; 77 | arenas->a[ARENA_SQUARE].p[0][1] = -1; 78 | arenas->a[ARENA_SQUARE].p[1][0] = -1; 79 | arenas->a[ARENA_SQUARE].p[1][1] = 1; 80 | arenas->a[ARENA_SQUARE].p[2][0] = 1; 81 | arenas->a[ARENA_SQUARE].p[2][1] = 1; 82 | arenas->a[ARENA_SQUARE].p[3][0] = 1; 83 | arenas->a[ARENA_SQUARE].p[3][1] = -1; 84 | } 85 | // parse rest of arenas 86 | result = IniParse(name, ArenaCallback, arenas); 87 | if (result) 88 | printf("Error occured while reading row %d of arena file! \n", result); 89 | printf("Parsed %d arenas\n", arenas->a_count); 90 | return arenas->a_count; 91 | } 92 | 93 | // works only for CONVEX arenas, in 2D! 94 | // returns 0 if WhichPoint is inside arena (CW) 95 | // returns 1 if NearestPoint contains closest edge point (in shadow) 96 | // returns 2 if NearestPoint contains closest vertex point (not in shadow) 97 | int NearestArenaPoint(double *NearestPoint, double *WhichPoint, 98 | const arena_t * Arena, const double ArenaRadius, 99 | const double ArenaCenterX, const double ArenaCenterY) { 100 | int i; 101 | static double p[MAX_ARENA_VERTICES * 2 + 2]; 102 | static double Temp1[3]; 103 | static double Temp2[3]; 104 | double dist, mindist = 1e222; 105 | 106 | // convert arena to polygon 107 | for (i = 0; i < Arena->p_count; i++) { 108 | p[i * 2] = Arena->p[i][0] * ArenaRadius + ArenaCenterX; 109 | p[i * 2 + 1] = Arena->p[i][1] * ArenaRadius + ArenaCenterY; 110 | } 111 | // close it with first vertex 112 | p[i * 2] = Arena->p[0][0] * ArenaRadius + ArenaCenterX; 113 | p[i * 2 + 1] = Arena->p[0][1] * ArenaRadius + ArenaCenterY; 114 | // check if we are inside at all. If so, nothing is returned in NearestPoint 115 | if (IsInsidePolygon(WhichPoint, p, Arena->p_count + 1)) { 116 | return 0; 117 | } 118 | // note that AtShadow works with 3D points but we do not care about 119 | // third dimension so this 2D-3D overlap bug does not count... 120 | for (i = 0; i < Arena->p_count; i++) { 121 | if (AtShadow(&p[i * 2], &p[(i + 1) * 2], WhichPoint) > 0) { 122 | // get closest point on edge 123 | VectDifference(Temp1, WhichPoint, &p[i * 2]); 124 | VectDifference(Temp2, &p[(i + 1) * 2], &p[i * 2]); 125 | Temp2[2] = 0.0; 126 | UnitVect(Temp2, Temp2); 127 | MultiplicateWithScalar(Temp2, Temp2, 128 | ScalarProduct(Temp1, Temp2, 2), 2); 129 | VectSum(NearestPoint, &p[i * 2], Temp2); 130 | NearestPoint[2] = 0; 131 | // return 1 indicating that we have found an edge and closest point 132 | // is stored in NearestPoint 133 | return 1; 134 | } else { 135 | VectDifference(Temp1, WhichPoint, &p[i * 2]); 136 | dist = VectAbsXY(Temp1); 137 | if (dist < mindist) { 138 | mindist = dist; 139 | FillVect(NearestPoint, p[i * 2], p[i * 2 + 1], 0); 140 | } 141 | } 142 | } 143 | // return 2 indicating that we have not found an edge but a closest corner, 144 | // which is stored in NearestPoint 145 | return 2; 146 | } 147 | 148 | void 149 | Shill_Wall(double *OutputVelocity, phase_t * Phase, 150 | const double ArenaCenterX, const double ArenaCenterY, 151 | const double ArenaRadius, const arena_t * Arena, 152 | const double C_Shill, const double V_Shill, 153 | const double Alpha_Shill, const double Gamma_Wall, 154 | const int WhichAgent, const int Dim_l) { 155 | 156 | const double C_VelDiff = 1.0 / 100.0; 157 | double *AgentsPosition; 158 | AgentsPosition = Phase->Coordinates[WhichAgent]; 159 | double *AgentsVelocity; 160 | AgentsVelocity = Phase->Velocities[WhichAgent]; 161 | static double GoalPositionVect[3]; 162 | FillVect(GoalPositionVect, ArenaCenterX, ArenaCenterY, 0.0); 163 | 164 | static double FromGoal[3]; 165 | static double ToArena[3]; 166 | VectDifference(FromGoal, GoalPositionVect, AgentsPosition); 167 | if (2.0 == Dim_l) { 168 | FromGoal[2] = 0.0; 169 | } 170 | 171 | static double ToArenaLength; 172 | 173 | // SQUARE 174 | if (Arena->index == ARENA_SQUARE) { 175 | int i; 176 | for (i = 0; i < Dim_l; i++) { 177 | NullVect(ToArena, 3); 178 | ToArena[i] = FromGoal[i]; 179 | UnitVect(ToArena, ToArena); 180 | MultiplicateWithScalar(ToArena, ToArena, V_Shill, 3); 181 | VectDifference(ToArena, ToArena, AgentsVelocity); 182 | if (fabs(Alpha_Shill) > 1e-12) { 183 | if (2.0 == Dim_l) 184 | ToArenaLength = VectAbsXY(ToArena); 185 | else 186 | ToArenaLength = VectAbs(ToArena); 187 | if (ToArenaLength > 0.0) { 188 | MultiplicateWithScalar(ToArena, ToArena, 189 | pow(C_VelDiff * ToArenaLength, 190 | Alpha_Shill), (int) Dim_l); 191 | } 192 | } 193 | //MultiplicateWithScalar (ToArena, ToArena, C_Shill * 194 | // (1.0 - Sigmoid(fabs(FromGoal[i]), Gamma_Wall, ArenaRadius)), (int) Dim_l); 195 | MultiplicateWithScalar(ToArena, ToArena, C_Shill * 196 | (1.0 - SigmoidLin(fabs(FromGoal[i]), 197 | 1.0 / Gamma_Wall, 1.0, 198 | ArenaRadius)), (int) Dim_l); 199 | VectSum(OutputVelocity, ToArena, OutputVelocity); 200 | } 201 | // CIRCLE 202 | } else if (Arena->index == ARENA_CIRCLE) { 203 | UnitVect(ToArena, FromGoal); 204 | MultiplicateWithScalar(ToArena, ToArena, V_Shill, 3); 205 | VectDifference(ToArena, ToArena, AgentsVelocity); 206 | if (fabs(Alpha_Shill) > 1e-12) { 207 | if (2.0 == Dim_l) 208 | ToArenaLength = VectAbsXY(ToArena); 209 | else 210 | ToArenaLength = VectAbs(ToArena); 211 | if (ToArenaLength > 0.0) { 212 | MultiplicateWithScalar(ToArena, ToArena, 213 | pow(C_VelDiff * ToArenaLength, 214 | Alpha_Shill), (int) Dim_l); 215 | } 216 | } 217 | //MultiplicateWithScalar (ToArena, ToArena, C_Shill * 218 | // (1.0 - Sigmoid(VectAbs(FromGoal), Gamma_Wall, ArenaRadius)), (int) Dim_l); 219 | MultiplicateWithScalar(ToArena, ToArena, C_Shill * 220 | (1.0 - SigmoidLin(VectAbs(FromGoal), 221 | 1.0 / Gamma_Wall, 1.0, 222 | ArenaRadius)), (int) Dim_l); 223 | VectSum(OutputVelocity, ToArena, OutputVelocity); 224 | // GENERAL ARENA POLYGON 225 | } else { 226 | // note that general polygon calculation is in 2D only 227 | if (NearestArenaPoint 228 | (ToArena, AgentsPosition, Arena, ArenaRadius, ArenaCenterX, 229 | ArenaCenterY)) { 230 | VectDifference(ToArena, ToArena, AgentsPosition); 231 | ToArenaLength = VectAbsXY(ToArena); 232 | UnitVect(ToArena, ToArena); 233 | MultiplicateWithScalar(ToArena, ToArena, V_Shill, 2); 234 | VectDifference(ToArena, ToArena, AgentsVelocity); 235 | if (fabs(Alpha_Shill) > 1e-12) { 236 | //TODO: VectAbs can be 0! 237 | MultiplicateWithScalar(ToArena, ToArena, 238 | pow(C_VelDiff * VectAbsXY(ToArena), Alpha_Shill), 2); 239 | } 240 | //MultiplicateWithScalar (ToArena, ToArena, C_Shill * 241 | // (1.0 - Sigmoid(ToArenaLength, Gamma_Wall, 0.0)), 2); 242 | MultiplicateWithScalar(ToArena, ToArena, C_Shill * 243 | (1.0 - SigmoidLin(ToArenaLength, 244 | 1.0 / Gamma_Wall, 1.0, 0.0)), 2); 245 | VectSum(OutputVelocity, ToArena, OutputVelocity); 246 | OutputVelocity[2] = 0.0; 247 | } else { 248 | // we are inside, no need to have shill wall output velocity 249 | NullVect(OutputVelocity, 3); 250 | } 251 | } 252 | // zero out third dimension once again for sure 253 | if (2.0 == Dim_l) { 254 | OutputVelocity[2] = 0.0; 255 | } 256 | } 257 | 258 | void 259 | Shill_Wall_LinSqrt(double *OutputVelocity, phase_t * Phase, 260 | const double ArenaCenterX, const double ArenaCenterY, 261 | const double ArenaRadius, const arena_t * Arena, 262 | const double V_Shill, const double R0_Offset_Shill, 263 | const double Acc_Shill, const double Slope_Shill, 264 | const int WhichAgent, const int Dim_l) { 265 | 266 | int i; 267 | double *AgentsPosition; 268 | AgentsPosition = Phase->Coordinates[WhichAgent]; 269 | double *AgentsVelocity; 270 | AgentsVelocity = Phase->Velocities[WhichAgent]; 271 | static double ArenaCenter[3]; 272 | FillVect(ArenaCenter, ArenaCenterX, ArenaCenterY, 0.0); 273 | 274 | static double ToCenter[3]; // from pos towards center 275 | static double ToArena[3]; 276 | 277 | VectDifference(ToCenter, ArenaCenter, AgentsPosition); 278 | if (2.0 == Dim_l) { 279 | ToCenter[2] = 0.0; 280 | } 281 | 282 | static double VelDiff; 283 | static double DistFromWall; // negative outside, positive inside 284 | static double MaxVelDiff; 285 | 286 | // SQUARE 287 | if (Arena->index == ARENA_SQUARE) { 288 | for (i = 0; i < Dim_l; i++) { 289 | // DistFromWall is positive if inside arena, negative if outside 290 | DistFromWall = ArenaRadius - fabs(ToCenter[i]); 291 | NullVect(ToArena, 3); 292 | ToArena[i] = ToCenter[i]; 293 | UnitVect(ToArena, ToArena); 294 | MultiplicateWithScalar(ToArena, ToArena, V_Shill, 3); 295 | VectDifference(ToArena, ToArena, AgentsVelocity); 296 | if (2.0 == Dim_l) { 297 | ToArena[2] = 0.0; 298 | } 299 | VelDiff = VectAbs(ToArena); 300 | UnitVect(ToArena, ToArena); 301 | // calculate max allowed velocity difference at a given distance based 302 | // on an optimal linsqrt breaking curve 303 | MaxVelDiff = VelDecayLinSqrt(DistFromWall, Slope_Shill, Acc_Shill, 304 | VelDiff, R0_Offset_Shill); 305 | // if velocity difference is larger than allowed, we compensate it 306 | if (VelDiff > MaxVelDiff) { 307 | MultiplicateWithScalar(ToArena, ToArena, 308 | VelDiff - MaxVelDiff, Dim_l); 309 | VectSum(OutputVelocity, OutputVelocity, ToArena); 310 | } 311 | } 312 | // CIRCLE 313 | } else if (Arena->index == ARENA_CIRCLE) { 314 | DistFromWall = ArenaRadius - VectAbs(ToCenter); 315 | UnitVect(ToArena, ToCenter); 316 | MultiplicateWithScalar(ToArena, ToArena, V_Shill, 3); 317 | VectDifference(ToArena, ToArena, AgentsVelocity); 318 | if (2.0 == Dim_l) { 319 | ToArena[2] = 0.0; 320 | } 321 | VelDiff = VectAbs(ToArena); 322 | UnitVect(ToArena, ToArena); 323 | // calculate max allowed velocity difference at a given distance based 324 | // on an optimal linsqrt breaking curve 325 | MaxVelDiff = VelDecayLinSqrt(DistFromWall, Slope_Shill, Acc_Shill, 326 | VelDiff, R0_Offset_Shill); 327 | // if velocity difference is larger than allowed, we compensate it 328 | if (VelDiff > MaxVelDiff) { 329 | MultiplicateWithScalar(ToArena, ToArena, 330 | VelDiff - MaxVelDiff, Dim_l); 331 | VectSum(OutputVelocity, OutputVelocity, ToArena); 332 | } 333 | // GENERAL ARENA POLYGON 334 | } else { 335 | // TODO: this is not implemented yet, as new method acts also inside 336 | // the arena and reaches its maximal value right at arena edge 337 | // we have to find it out how to resolve general polygon in this case. 338 | // Also with concave polygons it is not trivial how to define poly 339 | // center towards which we should go. 340 | NullVect(OutputVelocity, 3); 341 | } 342 | // zero out third dimension once again for sure 343 | if (2.0 == Dim_l) { 344 | OutputVelocity[2] = 0.0; 345 | } 346 | } 347 | -------------------------------------------------------------------------------- /src/utilities/arenas.h: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | #ifndef ARENAS_H 7 | #define ARENAS_H 8 | 9 | #include "dynamics_utils.h" 10 | 11 | #define MAX_ARENA_VERTICES 128 12 | #define MAX_ARENA_COUNT 32 13 | #define MAX_ARENA_NAME_LENGTH 32 14 | 15 | /* predefined arena shapes with predefined indices */ 16 | #define ARENA_CIRCLE 0 17 | #define ARENA_SQUARE 1 18 | 19 | /** 20 | * Struct containing info about an arena. 21 | */ 22 | typedef struct { 23 | int index; 24 | char name[MAX_ARENA_NAME_LENGTH]; 25 | double p[MAX_ARENA_VERTICES][3]; 26 | int p_count; 27 | } arena_t; 28 | 29 | /** 30 | * Struct needed to store actual number of arenas, too 31 | */ 32 | typedef struct { 33 | arena_t a[MAX_ARENA_COUNT]; 34 | int a_count; 35 | } arenas_t; 36 | 37 | /** 38 | * Parses an arena file. 39 | * 40 | * \param name the name of the configuration file. A leading tilde (~) will 41 | * be resolved to the home directory of the user. 42 | * \param arenas the arenas structure that hold the parsed information 43 | * \param define_circle_and_square should we define circle and square automatically? 44 | * 45 | * \return number of arenas parsed successfully 46 | */ 47 | int ParseArenaFile(const char *name, arenas_t * arenas, 48 | bool define_circle_and_square); 49 | 50 | /* More-or-less general interaction with specific types of arena */ 51 | void Shill_Wall(double *OutputVelocity, phase_t * Phase, 52 | const double ArenaCenterX, const double ArenaCenterY, 53 | const double ArenaSize, const arena_t * Arena, 54 | const double C_Shill, const double V_Shill, 55 | const double Alpha_Shill, const double Gamma_Wall, 56 | const int WhichAgent, const int Dim_l); 57 | 58 | void Shill_Wall_LinSqrt(double *OutputVelocity, phase_t * Phase, 59 | const double ArenaCenterX, const double ArenaCenterY, 60 | const double ArenaSize, const arena_t * Arena, 61 | const double V_Shill, const double R0_Offset_Shill, 62 | const double Acc_Shill, const double Slope_Shill, 63 | const int WhichAgent, const int Dim_l); 64 | 65 | #endif 66 | -------------------------------------------------------------------------------- /src/utilities/datastructs.c: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* 7 | * Tools for allocating, freeing and saving/loading data structures 8 | */ 9 | 10 | #include "datastructs.h" 11 | 12 | /* Tools for allocating and freeing data structures */ 13 | 14 | /* Dynamic "boolean" array (1D) */ 15 | bool *BooleanData(int size) { 16 | 17 | bool *bools; 18 | bools = (bool *) calloc(size, sizeof(bool)); 19 | 20 | if (bools == NULL) { 21 | fprintf(stderr, "Boolean vector allocation error!\n"); 22 | exit(-1); 23 | } 24 | 25 | return bools; 26 | 27 | } 28 | 29 | /* Dynamic integer array (1D) */ 30 | int *intData(int size) { 31 | 32 | int *ints; 33 | ints = (int *) calloc(size, sizeof(int)); 34 | 35 | if (ints == NULL) { 36 | fprintf(stderr, "Integer vector allocation error!\n"); 37 | exit(-1); 38 | } 39 | 40 | return ints; 41 | 42 | } 43 | 44 | /* Dynamic double array (1D, vector) */ 45 | double *doubleVector(int size) { 46 | 47 | double *vect; 48 | vect = (double *) calloc(size, sizeof(double)); 49 | 50 | if (vect == NULL) { 51 | fprintf(stderr, "Vector allocation error!\n"); 52 | exit(-1); 53 | } 54 | 55 | return vect; 56 | 57 | } 58 | 59 | /* Dynamic double array (2D, matrix with 'rows' rows and 'cols' columns) */ 60 | double **doubleMatrix(int rows, int cols) { 61 | 62 | double **mat; 63 | 64 | mat = (double **) calloc(rows, sizeof(double *)); 65 | 66 | if (mat == NULL) { 67 | fprintf(stderr, "Matrix allocation error!\n"); 68 | exit(-1); 69 | } 70 | int i; 71 | for (i = 0; i < rows; i++) { 72 | mat[i] = (double *) calloc(cols, sizeof(double)); 73 | 74 | if (mat[i] == NULL) { 75 | fprintf(stderr, "Matrix allocation error!\n"); 76 | exit(-1); 77 | } 78 | 79 | } 80 | 81 | return mat; 82 | 83 | } 84 | 85 | /* Dynamic double time-indexed array */ 86 | double ***doubleTimeIndexedMatrix(int timesteps, int rows, int cols) { 87 | 88 | double ***mat; 89 | 90 | mat = (double ***) calloc(timesteps, sizeof(double **)); 91 | 92 | if (mat == NULL) { 93 | fprintf(stderr, "Time-indexed Matrix allocation error!\n"); 94 | exit(-1); 95 | } 96 | 97 | int i; 98 | for (i = 0; i < timesteps; i++) { 99 | mat[i] = (double **) calloc(rows, sizeof(double *)); 100 | 101 | if (mat[i] == NULL) { 102 | fprintf(stderr, "Time-indexed Matrix allocation error!\n"); 103 | exit(-1); 104 | } 105 | 106 | } 107 | 108 | int j; 109 | for (i = 0; i < timesteps; i++) { 110 | for (j = 0; j < rows; j++) { 111 | mat[i][j] = (double *) calloc(cols, sizeof(double)); 112 | 113 | if (mat[i][j] == NULL) { 114 | fprintf(stderr, "Time-indexed Matrix allocation error!\n"); 115 | exit(-1); 116 | } 117 | } 118 | 119 | } 120 | 121 | return mat; 122 | 123 | } 124 | 125 | /* Free 2D dynamic arrays (matrices) */ 126 | void freeMatrix(double **Matrix, int rows, int cols) { 127 | int i; 128 | 129 | for (i = 0; i < rows; i++) { 130 | free(Matrix[i]); 131 | } 132 | free(Matrix); 133 | } 134 | 135 | /* Frees 3D dynamic array 136 | */ 137 | void 138 | freeTimeIndexedMatrix(double ***TimeIndexedMatrix, int timesteps, int rows, 139 | int cols) { 140 | 141 | int i, j; 142 | 143 | for (i = 0; i < timesteps; i++) { 144 | 145 | for (j = 0; j < rows; j++) { 146 | free(TimeIndexedMatrix[i][j]); 147 | } 148 | 149 | free(TimeIndexedMatrix[i]); 150 | 151 | } 152 | 153 | free(TimeIndexedMatrix); 154 | 155 | } 156 | 157 | /* File I/O tools */ 158 | 159 | /* Saving double vector to "outfile" */ 160 | void saveVector(FILE * outfile, double *vec, int dimension) { 161 | int i; 162 | 163 | for (i = 0; i < dimension; i++) { 164 | fprintf(outfile, "%10.10f\t", vec[i]); 165 | } 166 | 167 | fprintf(outfile, "\n"); 168 | 169 | } 170 | 171 | /* Scanning double matrix from "infile" */ 172 | void loadMatrix(FILE * infile, double **mat, int rows, int cols) { 173 | 174 | int i, j; 175 | 176 | for (j = 0; j < rows; j++) { 177 | 178 | for (i = 0; i < cols; i++) { 179 | 180 | fscanf(infile, "%lf", &(mat[j][i])); 181 | 182 | } 183 | 184 | } 185 | 186 | } 187 | 188 | /* Saving doubel matrix to "outfile" */ 189 | void saveMatrix(FILE * outfile, double **mat, int rows, int cols) { 190 | 191 | int i, j; 192 | 193 | for (j = 0; j < rows; j++) { 194 | 195 | for (i = 0; i < cols; i++) { 196 | 197 | fprintf(outfile, "%lf\t", mat[j][i]); 198 | 199 | } 200 | 201 | fprintf(outfile, "\n"); 202 | 203 | } 204 | 205 | } 206 | -------------------------------------------------------------------------------- /src/utilities/datastructs.h: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* 7 | * Tools for allocating, freeing and saving/loading data structures 8 | */ 9 | 10 | #ifndef DATASTRUCTS_H 11 | #define DATASTRUCTS_H 12 | 13 | #include 14 | #include 15 | #include 16 | typedef struct { 17 | 18 | double *states; 19 | char Name[64]; 20 | 21 | } inner_state_double_t; 22 | 23 | /* Specific state structures (not position or velocity) */ 24 | 25 | /* Tools for allocating and freeing data structures */ 26 | 27 | /* Allocates a dynamic array of boolean variables 28 | * "size" defines the size of the array 29 | */ 30 | bool *BooleanData(int size); 31 | 32 | /* Allocates a dynamic array of integer variables 33 | * "size" defines the size of the array 34 | */ 35 | int *intData(int size); 36 | 37 | /* Allocates a dynamic array of double variables 38 | * Arrays allocated with this can be used as vector structures 39 | * "size" defines the size of the array 40 | */ 41 | double *doubleVector(int size); 42 | 43 | /* Allocates a 2 dimensional dynamic array of double variables 44 | * Arrays allocated with this can be used as matrix structures 45 | * "rows" and "cols" defines the number of rows and columns of the array, respectively 46 | */ 47 | double **doubleMatrix(int rows, int cols); 48 | 49 | /* Allocates a 3-indexed tensory data row 50 | * Arrays allocated with this can be used as a time-dependent matrix structure 51 | * "rows", "cols" and "timesteps" defines the size of the array. 52 | */ 53 | double ***doubleTimeIndexedMatrix(int rows, int cols, int timesteps); 54 | 55 | /* Allocating inner state vector 56 | */ 57 | inner_state_double_t *InnerStateData(int size); 58 | 59 | /* Frees inner state vectors 60 | */ 61 | void freeInnerStates(inner_state_double_t * InnerStatesToFree, int size); 62 | 63 | /* Frees 2D dynamic arrays (matrices) 64 | * "rows" and "cols" defines the number of rows and columns of the array, respectively 65 | */ 66 | void freeMatrix(double **Matrix, int rows, int cols); 67 | 68 | /* Frees 3D dynamic array 69 | */ 70 | void freeTimeIndexedMatrix(double ***TimeIndexedMatrix, int timesteps, 71 | int rows, int cols); 72 | 73 | /* File I/O tools */ 74 | 75 | /* Saves data of a 1D double array ("vec") into a file ("outputfile") 76 | * "dimension" defines the size of the array 77 | */ 78 | void saveVector(FILE * outfile, double *vec, int dimension); 79 | 80 | /* Reads data of a 2D double array ("mat") from a file ("input") 81 | * "rows" and "cols" defines the number of rows and columns of the array, respectively 82 | */ 83 | void loadMatrix(FILE * infile, double **mat, int rows, int cols); 84 | 85 | /* Saves data of a 2D double array ("mat") into a file ("outputfile") 86 | * "rows" and "cols" defines the number of rows and columns of the array, respectively 87 | * The columns will be separated with '\t' in the output file 88 | */ 89 | void saveMatrix(FILE * outfile, double **mat, int rows, int cols); 90 | 91 | #endif 92 | -------------------------------------------------------------------------------- /src/utilities/debug_utils.c: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* Tools for debugging the code of an algorithm 7 | */ 8 | 9 | #include "debug_utils.h" 10 | 11 | /* Printing out the components of a vector to stdout */ 12 | void PrintVector(double *Vector, const int Dim) { 13 | 14 | printf("( "); 15 | 16 | int i; 17 | for (i = 0; i < Dim; i++) { 18 | 19 | printf("%lf ", Vector[i]); 20 | 21 | } 22 | 23 | printf(")\n\n"); 24 | 25 | } 26 | 27 | /* Segfault detection in debug mode */ 28 | /** 29 | * Signal handler for SIGSEGV, SIGABRT and SIGBUS to print a stack trace when 30 | * the app crashes. 31 | */ 32 | static void HandleFaults(int sig) { 33 | #ifdef DEBUG 34 | void *array[MAX_STACKTRACE_DEPTH]; 35 | size_t size; 36 | 37 | /* Print the stack trace */ 38 | size = backtrace(array, MAX_STACKTRACE_DEPTH); 39 | printf("\nUnexpected termination (signal %d) at:\n", sig); 40 | backtrace_symbols_fd(array, size, STDOUT_FILENO); 41 | 42 | /* Restore the original signal handler and send the signal again to ensure 43 | * that we also get the core dump */ 44 | signal(sig, SIG_DFL); 45 | raise(sig); 46 | #endif 47 | } 48 | 49 | void InstallSegfaultHandler() { 50 | #ifdef DEBUG 51 | signal(SIGABRT, HandleFaults); 52 | signal(SIGBUS, HandleFaults); 53 | signal(SIGSEGV, HandleFaults); 54 | signal(SIGFPE, HandleFaults); 55 | #endif 56 | if (1 == 0) 57 | HandleFaults(0); 58 | } 59 | -------------------------------------------------------------------------------- /src/utilities/debug_utils.h: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | #ifndef DEBUG_INFO_H 7 | #define DEBUG_INFO_H 8 | 9 | #include 10 | #include "dynamics_utils.h" 11 | 12 | #ifdef DEBUG 13 | #include 14 | #include 15 | #define MAX_STACKTRACE_DEPTH 32 16 | #endif 17 | 18 | /* This struct is an input of the CalculatePreferredVelocity function, 19 | * can be used only for displaying debug information. 20 | */ 21 | typedef struct { 22 | 23 | /* Sequential number, position and velocity of the actual agent */ 24 | int AgentsSeqNumber; 25 | double AgentsRealPosition[3]; 26 | double AgentsRealVelocity[3]; 27 | 28 | /* Real PhaseSpace and Inner States */ 29 | phase_t *RealPhase; 30 | 31 | } agent_debug_info_t; 32 | 33 | ////////////////////////////////////////////////////////////////////// 34 | /* Debug functions */ 35 | 36 | /* Printing out the components of a vector to the console 37 | * "Dim" is the number of components of the vector "Vector" 38 | */ 39 | void PrintVector(double *Vector, const int Dim); 40 | 41 | /* Debug mode utils for detecting segmentation faults */ 42 | void InstallSegfaultHandler(); 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /src/utilities/dynamics_utils.h: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | #ifndef DYNAMICS_UTILS_H 7 | #define DYNAMICS_UTILS_H 8 | 9 | #include 10 | #include 11 | #include 12 | #include "datastructs.h" 13 | #include "math_utils.h" 14 | #include "param_utils.h" 15 | 16 | /* Array that containts noise boolean variable 17 | * If Noise[i] is "false", then no output noise will be added to its acceleration 18 | * and the measured position will not fluctuate 19 | * 20 | * Default value is "true", but it can be changed individually for all agents 21 | * in the "InitializeInnerStates function 22 | * 23 | */ 24 | bool *Noises; 25 | 26 | /* Functions for manipluating phase spaces and dynamics. 27 | */ 28 | 29 | /* Struct that contains every agents' coordinates, velocities 30 | * and inner states 31 | * 32 | * Note that innner states are always double variables and 33 | * the number of them is determined by the "NumberOfInnerStates" variable 34 | * 35 | */ 36 | typedef struct { 37 | 38 | double **Coordinates; 39 | double **Velocities; 40 | double **InnerStates; 41 | int *RealIDs; 42 | int NumberOfInnerStates; 43 | int NumberOfAgents; 44 | 45 | } phase_t; 46 | 47 | /* Simple tools for allocating phase space and cleaning memory 48 | */ 49 | void AllocatePhase(phase_t * Phase, const int NumberOfAgents, 50 | const int NumberOfInnerStates); 51 | void freePhase(phase_t * Phase); 52 | 53 | /* Insert specific agent's coordinates and velocity (denoted by "Agent") into 54 | * an existing phase space denoted by "Phase". 55 | * "WhichAgent" denotes the ID number of the agent. 56 | */ 57 | void InsertAgentPhase(phase_t * Phase, double *Agent, const int WhichAgent); 58 | 59 | /* Get specific agent's coordinates and velocity (denoted by "Agent") from 60 | * an existing phase space denoted by "Phase". 61 | * "WhichAgent" denotes the ID number of the agent. 62 | */ 63 | void GetAgentPhase(double *Agent, phase_t * Phase, const int WhichAgent); 64 | 65 | /* Get specific agent's velocity vector (denoted by "Velocity") from 66 | * an existing phase space denoted by "Phase". 67 | * "WhichAgent" denotes the ID number of the agent. 68 | */ 69 | void GetAgentsVelocity(double *Velocity, phase_t * Phase, const int WhichAgent); 70 | 71 | /* Insert specific agent's velocity vector (denoted by "Velocity") into 72 | * an existing phase space denoted by "Phase". 73 | * "WhichAgent" denotes the ID number of the agent. 74 | */ 75 | void InsertAgentsVelocity(phase_t * Phase, double *Velocity, 76 | const int WhichAgent); 77 | 78 | /* Get specific agent's coordinate vector (denoted by "Coords") from 79 | * an existing phase space denoted by "Phase". 80 | * "WhichAgent" denotes the ID number of the agent. 81 | */ 82 | void GetAgentsCoordinates(double *Coords, phase_t * Phase, 83 | const int WhichAgent); 84 | 85 | /* Get specific agent's extrapolated coordinate vector (denoted by "Coords") from 86 | * an existing phase space denoted by "Phase", the Delay time and velocity 87 | * "WhichAgent" denotes the ID number of the agent. 88 | */ 89 | void GetAgentsCoordinatesEp(double *Coords, phase_t * Phase, 90 | const int WhichAgent, const double Delay); 91 | 92 | /* Insert specific agent's coordinate vector (denoted by "Coords") into 93 | * an existing phase space denoted by "Phase". 94 | * "WhichAgent" denotes the ID number of the agent. 95 | */ 96 | void InsertAgentsCoordinates(phase_t * Phase, double *Coords, 97 | const int WhichAgent); 98 | 99 | /* Calculate number of agents that are closer than "RadiusOfLocalArea" to a specific agent 100 | * denoted by its ID ("WhichAgent") 101 | * With "Phase", the actual phase space is denoted. 102 | */ 103 | int NumberOfNearbyUnits(phase_t * Phase, const int WhichAgent, 104 | const double RadiusOfLocalArea); 105 | 106 | /* Calculates euclidean distance between two agents denoted by their ID number ("A1" and "A2") 107 | * "Dim" determines the number of spatial dimensions (can be 2 or 3) 108 | * With "Phase", the actual phase space is denoted. 109 | */ 110 | double DistanceBetweenAgents(phase_t * Phase, const int A1, const int A2, 111 | const int Dim); 112 | 113 | /* Gets global centre of mass (CoM) from a phase space "Phase" 114 | * CoM coordinates will be placed into "CoMCoord" vector. 115 | */ 116 | void GetCoM(double *CoMCoord, phase_t * Phase); 117 | 118 | /* Gets local centre of mass (CoM) from a phase space ("Phase") 119 | * from a viewpoint of a specific agent denoted by its ID number ("WhichAgent") 120 | * Vision range of an agent is defined as "RadiusOfLocalArea". 121 | * CoM coordinates will be placed into "CoMCoord" vector. 122 | */ 123 | void GetLocalCoM(double *CoMCoord, phase_t * Phase, const int WhichAgent, 124 | const double RadiusOfLocalArea); 125 | 126 | /* Calculating CoM for specific agents 127 | * 128 | * Phase space is denoted as "Phase" 129 | * CoM coordinates will be placed into "CoMCoord" vector. 130 | * "ExceptWhichAgents" is a set of ID numbers (this agents will not be taken into account) 131 | * 132 | */ 133 | void GetAgentSpecificCoM(double *CoMCoord, phase_t * Phase, 134 | int *ExceptWhichAgents); 135 | 136 | /* Calculating local CoM for specific agents 137 | * from a viewpoint of a specific agent denoted by its ID number ("AroundWhichAgent") 138 | * 139 | * Phase space is denoted as "Phase" 140 | * CoM coordinates will be placed into "CoMCoord" vector. 141 | * "ExceptWhichAgents" is a set of ID numbers (this agents will not be taken into account) 142 | * "NeighbourhoodRange" defines the vision range of "AroundWhichAgent"th agent. 143 | * 144 | */ 145 | void GetAgentSpecificLocalCoM(double *CoMCoord, phase_t * Phase, 146 | const int AroundWhichAgent, 147 | int *ExceptWhichAgents, const double NeighbourhoodRange); 148 | 149 | /* Calculates average velocity vector and place it into the "CoMVel" vector. 150 | * "Phase" is the actual phase space. 151 | */ 152 | void GetAvgOfVelocity(double *CoMVel, phase_t * Phase); 153 | 154 | /* Calculates local average XY velocity vector 155 | * from the viewpoint of the "WhichAgent"th unit and place it into the "CoMVel" vector. 156 | * "Phase" is the actual phase space, "AreaRadius" is the vision range of the "WhichAgent"th agent. 157 | */ 158 | void GetLocalAverageOfXYVelocity(double *CoMVel, phase_t * Phase, 159 | const int WhichAgent, const double AreaRadius); 160 | 161 | /* Get average of tangential velocities around a centre point denoted by "RefPoint". 162 | * Final output velocity will be placed in "OutputLAPVel". 163 | */ 164 | void GetAverageOfXYTangentialVelocity(double *OutputLAPVel, phase_t * Phase, 165 | double *RefPoint, const int WhichAgent); 166 | 167 | /* Get local average of tangential velocities from the viewpoint of the "WhichAgent"th agent, 168 | * around a centre point denoted by "RefPoint" 169 | * 170 | * "AreaSize" denotes the vision range of the agent. 171 | * Final output velocity will be placed in "OutputLAPVel". 172 | */ 173 | void GetLocalAverageOfXYTangentialVelocity(double *OutputLAPVel, 174 | phase_t * Phase, double *RefPoint, 175 | const int WhichAgent, const double AreaSize); 176 | 177 | /* Get local average of tangential velocities from the viewpoint of the "WhichAgent"th agent, 178 | * around a centre point denoted by "RefPoint" 179 | * with the exception of the agents listed in "ExceptWhichAgents" interger array. 180 | * 181 | * "AreaSize" denotes the vision range of the agent. 182 | * Final output velocity will be placed in "OutputLAPVel". 183 | */ 184 | void GetAgentSpecificLocalAverageOfXYTangentialVelocity(double *OutputLAPVel, 185 | phase_t * Phase, double *RefPoint, int 186 | *ExceptWhichAgents, const int WhichAgent, const double AreaSize); 187 | 188 | /* Get local average of tangential velocity on a specific plane. 189 | * The meaning of the other variables are the same as presented above. 190 | * 191 | * This plane is defined by its normal vector, "Axis" 192 | */ 193 | void GetLocalAverageOfTangentialVelocity(double *OutputLAPVel, 194 | phase_t * Phase, 195 | double *RefPoint, 196 | double *Axis, const int WhichAgent, const double AreaSize); 197 | 198 | /* Get coordinates from a timeline (timeline means a set of phase spaces) 199 | * "WhichStep" defines the timestep, "WhichAgents" is the ID number of the specific agent, 200 | * "PhaseData" is the timeline and "Coords" is the output position vector. 201 | */ 202 | void GetAgentsCoordinatesFromTimeLine(double *Coords, phase_t * PhaseData, 203 | const int WhichAgent, const int WhichStep); 204 | 205 | /* Get velocity vector from a timeline (timeline means a set of phase spaces) 206 | * "WhichStep" defines the timestep, "WhichAgents" is the ID number of the specific agent, 207 | * "PhaseData" is the timeline and "Coords" is the output position vector. 208 | */ 209 | void GetAgentsVelocityFromTimeLine(double *Velocity, phase_t * PhaseData, 210 | const int WhichAgent, const int WhichStep); 211 | 212 | /* Inserting positions and velocities into timeline 213 | */ 214 | void InsertPhaseToDataLine(phase_t * PhaseData, phase_t * Phase, 215 | const int WhichStep); 216 | 217 | /* Inserting inner states into inner state timeline 218 | */ 219 | void InsertInnerStatesToDataLine(phase_t * PhaseData, phase_t * Phase, 220 | const int WhichStep); 221 | 222 | /* Shifting position and velocity data line 223 | */ 224 | void ShiftDataLine(phase_t * PhaseData, const int HowManyRows, 225 | const int HowManyRowsToSave); 226 | 227 | /* Shifting inner state data line 228 | */ 229 | void ShiftInnerStateDataLine(phase_t * PhaseData, const int HowManyRows, 230 | const int HowManyRowsToSave); 231 | 232 | /* Setting up random Initial conditions (with zero velocities and randomly distributed positions.) 233 | */ 234 | void InitCond(phase_t ** PhaseData, 235 | const double InitialSizeX, 236 | const double InitialSizeY, 237 | const double InitialSizeZ, const double SizeOfCopter); 238 | 239 | /* Waiting - filling up timelines with initial conditions */ 240 | void Wait(phase_t * PhaseData, const double TimeToWait, const double h); 241 | 242 | /* Setting up random conditions (with zero velocities) 243 | */ 244 | void RandomizePhase(phase_t * Phase, const double XSize, const double YSize, 245 | const double ZSize, const double XCenter, 246 | const double YCenter, const double ZCenter, 247 | const int fromAgent, const int toAgent, const double RadiusOfCopter); 248 | 249 | /* Setting up random conditions inside a ring (with zero velocities) 250 | */ 251 | void PlaceAgentsInsideARing(phase_t * Phase, const double SizeOfRing, 252 | const int fromAgent, const int toAgent, 253 | const double XCenter, const double YCenter, 254 | const double ZCenter, const double ZSize, const double RadiusOfCopter); 255 | 256 | /* Setting up random positions inside a sphere 257 | */ 258 | void PlaceAgentsInsideASphere(phase_t * Phase, const double SizeOfSphere, 259 | const int fromAgent, const int toAgent, 260 | const double XCenter, const double YCenter, 261 | const double ZCenter, const double RadiusOfCopter); 262 | 263 | /* Setting up random initial conditions on a specific plane 264 | * A local XY coordinate system is given on the plane by "XAxis" and the normalvector of the plane ("PlaneNormalVect") 265 | */ 266 | void RandomizePhaseOnPlane(phase_t * Phase, 267 | const double XSize, 268 | const double YSize, 269 | const double XCenter, 270 | const double YCenter, 271 | const double ZCenter, 272 | double *PlaneNormalVect, 273 | double *XAxis, 274 | const int fromAgent, const int toAgent, const double RadiusOfCopter); 275 | void PlaceAgentsOnXYPlane(phase_t * Phase, 276 | const double XSize, 277 | const double YSize, 278 | const double XCenter, 279 | const double YCenter, 280 | const double ZCenter, 281 | const int fromAgent, const int toAgent, const double RadiusOfCopter); 282 | void PlaceAgentsOnXZPlane(phase_t * Phase, 283 | const double XSize, 284 | const double ZSize, 285 | const double XCenter, 286 | const double YCenter, 287 | const double ZCenter, 288 | const int fromAgent, const int toAgent, const double RadiusOfCopter); 289 | void PlaceAgentsOnYZPlane(phase_t * Phase, 290 | const double YSize, 291 | const double ZSize, 292 | const double XCenter, 293 | const double YCenter, 294 | const double ZCenter, 295 | const int fromAgent, const int toAgent, const double RadiusOfCopter); 296 | 297 | /* Placing agents onto specific lines */ 298 | void PlaceAgentsOntoALine(phase_t * Phase, const int fromAgent, 299 | const int toAgent, double *Tangential, 300 | const double XCenter, const double YCenter, 301 | const double ZCenter, const double LineLength, 302 | const double RadiusOfCopter); 303 | 304 | /* Counting collisions (this is for internal use only :) ) 305 | */ 306 | int HowManyCollisions(phase_t * ActualPhase, bool * AgentsInDanger, 307 | const bool CountCollisions, const double RadiusOfCopter); 308 | 309 | /* Swaps the states of two agents (ith and jth) 310 | */ 311 | void SwapAgents(phase_t * Phase, const int i, const int j); 312 | 313 | /* Orders agents by distance from a given position 314 | */ 315 | void OrderAgentsByDistance(phase_t * Phase, double *ReferencePosition); 316 | 317 | /* Packing of nearby agents to the first blocks of the phase space 318 | * Returns the number of agents which are closer than R_C and whose packets 319 | * are not lost 320 | */ 321 | int SelectNearbyVisibleAgents(phase_t * Phase, double *ReferencePosition, 322 | const double Range, const double PacketLossQuadraticCoeff); 323 | 324 | #endif 325 | -------------------------------------------------------------------------------- /src/utilities/file_utils.c: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | #include "file_utils.h" 7 | #include 8 | #include 9 | 10 | /* Functions for handling strings and files */ 11 | 12 | char *RStrip(char *s) { 13 | char *p = s + strlen(s); 14 | while (p > s && isspace(*--p)) { 15 | *p = '\0'; 16 | } 17 | return s; 18 | } 19 | 20 | char *LSkip(const char *s) { 21 | while (*s && isspace(*s)) { 22 | s++; 23 | } 24 | return (char *) s; 25 | } 26 | 27 | char *FindCharOrComment(const char *s, char c) { 28 | 29 | int WasWhitespace = 0; 30 | while (*s && *s != c && !(WasWhitespace && *s == ';')) { 31 | WasWhitespace = isspace(*s); 32 | s++; 33 | } 34 | 35 | return (char *) s; 36 | 37 | } 38 | 39 | char *StrnCpy0(char *Dest, const char *Src, int size) { 40 | 41 | strncpy(Dest, Src, size); 42 | Dest[size - 1] = '\0'; 43 | return Dest; 44 | 45 | } 46 | 47 | /* Recovers fileName from FILE * struct */ 48 | char *recover_fileName(FILE * f) { 49 | 50 | int fd; 51 | char fd_path[255]; 52 | char *fileName = malloc(255); 53 | 54 | fd = fileno(f); 55 | sprintf(fd_path, "/proc/self/fd/%d", fd); 56 | readlink(fd_path, fileName, 255); 57 | 58 | return fileName; 59 | 60 | } 61 | 62 | /* File parsers */ 63 | int IniParseFile(FILE * file, 64 | int (*handler) (void *, const char *, const char *, 65 | const char *), void *user) { 66 | 67 | char line[MAX_LINE]; 68 | char section[MAX_SECTION] = ""; 69 | char prev_Name[MAX_Name] = ""; 70 | 71 | char *start, *end, *Name, *value; 72 | 73 | int lineno = 0; 74 | int error = 0; 75 | 76 | while (fgets(line, sizeof(line), file) != NULL) { 77 | 78 | lineno++; 79 | start = LSkip(RStrip(line)); 80 | 81 | if (*prev_Name && *start && start > line) { 82 | if (!handler(user, section, prev_Name, start) && !error) { 83 | error = lineno; 84 | } 85 | 86 | } else { 87 | if (*start == ';' || *start == '#') { 88 | 89 | } else if (*start == '[') { 90 | end = FindCharOrComment(start + 1, ']'); 91 | if (*end == ']') { 92 | *end = '\0'; 93 | StrnCpy0(section, start + 1, sizeof(section)); 94 | *prev_Name = '\0'; 95 | } else if (!error) { 96 | error = lineno; 97 | } 98 | 99 | } else if (*start && *start != ';') { 100 | 101 | end = FindCharOrComment(start, '='); 102 | if (*end != '=') { 103 | end = FindCharOrComment(start, ':'); 104 | } 105 | if (*end == '=' || *end == ':') { 106 | *end = '\0'; 107 | Name = RStrip(start); 108 | value = LSkip(end + 1); 109 | end = FindCharOrComment(value, '\0'); 110 | if (*end == ':') { 111 | *end = '\0'; 112 | } 113 | 114 | RStrip(value); 115 | StrnCpy0(prev_Name, Name, sizeof(prev_Name)); 116 | if (!handler(user, section, Name, value) && !error) { 117 | error = lineno; 118 | 119 | } 120 | 121 | } else if (!error) { 122 | error = lineno; 123 | } 124 | 125 | } 126 | } 127 | 128 | } 129 | 130 | return error; 131 | 132 | } 133 | 134 | int IniParse(const char *fileName, 135 | int (*handler) (void *, const char *, const char *, const char *), 136 | void *user) { 137 | 138 | FILE *file; 139 | int error; 140 | 141 | file = fopen(fileName, "r"); 142 | if (!file) { 143 | return -1; 144 | } 145 | error = IniParseFile(file, handler, user); 146 | fclose(file); 147 | return error; 148 | 149 | } 150 | 151 | /* Checks existence of input file (and returns it) */ 152 | FILE *CheckInputFile(char *final_fileName, int argc, char *argv[], 153 | char *actual_directory, char *default_fileName, char *optionflag) { 154 | 155 | int i; 156 | 157 | if (final_fileName != default_fileName) { 158 | strcpy(final_fileName, default_fileName); 159 | } 160 | 161 | for (i = 0; i < argc - 1; i++) { 162 | if (strcmp(argv[i], optionflag) == 0) { 163 | strcpy(final_fileName, argv[i + 1]); 164 | strcat(final_fileName, "\0"); 165 | break; 166 | } 167 | } 168 | 169 | return fopen(final_fileName, "r"); 170 | 171 | } 172 | 173 | /* Checks existence of Output file (and returns it) */ 174 | void CheckOutputFile(char *final_fileName, int argc, char *argv[], 175 | FILE ** DefaultOutputFile, char *actual_directory, 176 | char *default_fileName, char *optionflag) { 177 | 178 | int i; 179 | 180 | if (final_fileName != default_fileName) { 181 | strcpy(final_fileName, default_fileName); 182 | } 183 | 184 | for (i = 0; i < argc - 1; i++) { 185 | if (strcmp(argv[i], optionflag) == 0) { 186 | if (access(argv[i + 1], W_OK) != -1) { 187 | if (*DefaultOutputFile != NULL) { 188 | fclose(*DefaultOutputFile); 189 | } 190 | *DefaultOutputFile = fopen(argv[i + 1], "w"); 191 | printf("Selected output file found! (%s)\n", argv[i + 1]); 192 | strcpy(final_fileName, argv[i + 1]); 193 | strcat(final_fileName, "\0"); 194 | } 195 | } 196 | } 197 | 198 | } 199 | 200 | /* Count number of parameter sets (flockingparams and unitparams) */ 201 | void CountNumberOfInputs(int *NumberOfFlParams, int *NumberOfUParams, int argc, 202 | char *argv[]) { 203 | 204 | int i; 205 | for (i = 0; i < argc - 1; i++) { 206 | if (0 == strcmp(argv[i], "-u")) { /* YodaCode forever!!!44!! */ 207 | (*NumberOfUParams)++; 208 | } else if (0 == strcmp(argv[i], "-f")) { 209 | (*NumberOfFlParams)++; 210 | } 211 | } 212 | 213 | } 214 | -------------------------------------------------------------------------------- /src/utilities/file_utils.h: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | #ifndef FILE_UTILS_H 7 | #define FILE_UTILS_H 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | /* File size limits */ 17 | #define MAX_SECTION 32 18 | #define MAX_LINE 256 19 | #define MAX_Name 64 20 | 21 | /* Functions for handling strings */ 22 | 23 | char *RStrip(char *s); 24 | char *LSkip(const char *s); 25 | char *FindCharOrComment(const char *s, char c); 26 | char *StrnCpy0(char *Dest, const char *Src, int size); 27 | 28 | /* Function for parsing files and setting up parameters*/ 29 | 30 | /* Recovers fileName from 'FILE *' 31 | */ 32 | char *recover_fileName(FILE * f); 33 | 34 | /* File parsers */ 35 | int IniParseFile(FILE * file, 36 | int (*handler) (void *, const char *, const char *, 37 | const char *), void *user); 38 | 39 | int IniParse(const char *fileName, 40 | int (*handler) (void *, const char *, const char *, 41 | const char *), void *user); 42 | 43 | /* Checks existence of input file 44 | * final_fileName will be set to the Name of a "preferred input file", if the list argv[] contains a valid input fileName after 45 | * an option flag formatted as "-*" 46 | * otherwise, it will be set to "default_fileName" 47 | * 48 | * returns the opened file or NULL if fails. 49 | */ 50 | FILE *CheckInputFile(char *final_fileName, int argc, char *argv[], 51 | char *actual_directory, char *default_fileName, char *optionflag); 52 | 53 | /* Checks existence of output file 54 | * final_fileName will be set to the Name of a "preferred input file", if the list argv[] contains a valid input fileName after 55 | * an option flag formatted as "-*" 56 | */ 57 | void CheckOutputFile(char *final_fileName, int argc, char *argv[], 58 | FILE ** DefaultOutputFile, char *actual_directory, 59 | char *default_fileName, char *optionflag); 60 | 61 | /* ??? 62 | */ 63 | void CountNumberOfInputs(int *NumberOfFlParams, int *NumberOfUParams, 64 | int argc, char *argv[]); 65 | 66 | #endif 67 | -------------------------------------------------------------------------------- /src/utilities/interactions.c: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* 7 | * This file contains unversal interaction terms. 8 | */ 9 | 10 | #include 11 | #include "interactions.h" 12 | 13 | #define MIN(a,b) (((a)<(b))?(a):(b)) 14 | #define MAX(a,b) (((a)>(b))?(a):(b)) 15 | 16 | void FrictionLinSqrt(double *OutputVelocity, phase_t * Phase, 17 | const double C_Frict_l, const double V_Frict_l, 18 | const double Acc_l, const double p_l, const double R_0_l, 19 | const int WhichAgent, const int Dim_l) { 20 | 21 | NullVect(OutputVelocity, 3); 22 | 23 | int i; 24 | int n = 0; 25 | 26 | double *AgentsCoordinates; 27 | double *AgentsVelocity; 28 | double *NeighboursCoordinates; 29 | double *NeighboursVelocity; 30 | 31 | AgentsCoordinates = Phase->Coordinates[WhichAgent]; 32 | AgentsVelocity = Phase->Velocities[WhichAgent]; 33 | 34 | static double DifferenceVector[3]; 35 | static double DistanceFromNeighbour; 36 | static double VelDiff; 37 | static double MaxVelDiff; 38 | 39 | /* Friction-like term */ 40 | for (i = 0; i < Phase->NumberOfAgents; i++) { 41 | 42 | if (i == WhichAgent) 43 | continue; 44 | /* Get distance from neighbor */ 45 | NeighboursCoordinates = Phase->Coordinates[i]; 46 | NullVect(DifferenceVector, 3); 47 | VectDifference(DifferenceVector, NeighboursCoordinates, 48 | AgentsCoordinates); 49 | DistanceFromNeighbour = VectAbs(DifferenceVector); 50 | /* Get velocity difference from neighbor */ 51 | NeighboursVelocity = Phase->Velocities[i]; 52 | VectDifference(DifferenceVector, NeighboursVelocity, AgentsVelocity); 53 | if (2 == Dim_l) { 54 | DifferenceVector[2] = 0.0; 55 | } 56 | VelDiff = VectAbs(DifferenceVector); 57 | UnitVect(DifferenceVector, DifferenceVector); 58 | // calculate max allowed velocity difference at a given distance based 59 | // on an optimal linsqrt breaking curve and allow for V_Frict slack 60 | MaxVelDiff = 61 | MAX(V_Frict_l, 62 | VelDecayLinSqrt(DistanceFromNeighbour, p_l, Acc_l, VelDiff, 63 | R_0_l)); 64 | // if velocity difference is larger than allowed, we compensate it 65 | if (VelDiff > MaxVelDiff) { 66 | MultiplicateWithScalar(DifferenceVector, DifferenceVector, 67 | C_Frict_l * (VelDiff - MaxVelDiff), Dim_l); 68 | VectSum(OutputVelocity, OutputVelocity, DifferenceVector); 69 | } 70 | } 71 | } 72 | 73 | void RepulsionLin(double *OutputVelocity, 74 | phase_t * Phase, const double V_Rep_l, const double p_l, 75 | const double R_0_l, const int WhichAgent, const int Dim_l, 76 | const bool normalize) { 77 | 78 | NullVect(OutputVelocity, 3); 79 | 80 | int i; 81 | int n = 0; 82 | 83 | double *AgentsCoordinates; 84 | double *NeighboursCoordinates; 85 | 86 | AgentsCoordinates = Phase->Coordinates[WhichAgent]; 87 | 88 | static double DifferenceVector[3]; 89 | static double DistanceFromNeighbour; 90 | /* Repulsive interaction term */ 91 | for (i = 0; i < Phase->NumberOfAgents; i++) { 92 | if (i == WhichAgent) 93 | continue; 94 | NeighboursCoordinates = Phase->Coordinates[i]; 95 | VectDifference(DifferenceVector, AgentsCoordinates, 96 | NeighboursCoordinates); 97 | if (2 == Dim_l) { 98 | DifferenceVector[2] = 0.0; 99 | } 100 | DistanceFromNeighbour = VectAbs(DifferenceVector); 101 | /* Check if we interact at all */ 102 | if (DistanceFromNeighbour >= R_0_l) 103 | continue; 104 | n += 1; 105 | 106 | UnitVect(DifferenceVector, DifferenceVector); 107 | 108 | MultiplicateWithScalar(DifferenceVector, DifferenceVector, 109 | SigmoidLin(DistanceFromNeighbour, p_l, V_Rep_l, R_0_l), Dim_l); 110 | 111 | VectSum(OutputVelocity, OutputVelocity, DifferenceVector); 112 | } 113 | 114 | /* divide result by number of interacting units */ 115 | if (normalize && n > 1) { 116 | double length = VectAbs(OutputVelocity) / n; 117 | UnitVect(OutputVelocity, OutputVelocity); 118 | MultiplicateWithScalar(OutputVelocity, OutputVelocity, length, Dim_l); 119 | } 120 | //printf("Number of Repulsive neighbours: %d Norm of repulsive term relative to max repulsion velocity: %f\n", n, VectAbs (OutputVelocity)/V_Rep_l); 121 | } 122 | 123 | 124 | -------------------------------------------------------------------------------- /src/utilities/interactions.h: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* 7 | * This file contains unversal interaction terms. 8 | */ 9 | 10 | #ifndef INTERACTIONS_H 11 | #define INTERACTIONS_H 12 | 13 | #include "math_utils.h" 14 | #include "dynamics_utils.h" 15 | #include "arenas.h" 16 | 17 | /* friction / alignment */ 18 | 19 | void FrictionLinSqrt(double *OutputVelocity, phase_t * Phase, 20 | const double C_Frict_l, const double V_Frict_l, 21 | const double Acc_l, const double p_l, 22 | const double R_0_l, const int WhichAgent, const int Dim_l); 23 | 24 | /* repulsion */ 25 | 26 | void RepulsionLin(double *OutputVelocity, 27 | phase_t * Phase, const double V_Rep_l, const double p_l, 28 | const double R_0_l, const int WhichAgent, const int Dim_l, 29 | const bool normalize); 30 | 31 | #endif 32 | -------------------------------------------------------------------------------- /src/utilities/math_utils.h: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* 7 | * Useful mathematical tools (e. g. linear algebra tools, randomizers, etc.) 8 | */ 9 | 10 | #ifndef MATH_UTILS_H 11 | #define MATH_UTILS_H 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | /* Randomizer functions */ 20 | 21 | /* Returns a random double value between minValue and maxValue (uniform distribution) 22 | */ 23 | double randomizeDouble(const double minValue, const double maxValue); 24 | 25 | /* Returns a random double value (Gaussian distribution with given Mean and StdDev) 26 | * The function uses the so-called Box-Muller transformation rule 27 | * Mean defines the center of the Gaussian distribution 28 | * StdDev defines the "Sigma" value. 29 | */ 30 | double randomizeGaussDouble(const double Mean, const double StdDev); 31 | 32 | /* Returns a random double value (Power-law distribution with given exponent in a given range) 33 | * http://mathworld.wolfram.com/RandomNumber.html 34 | * x0, x1: boundaries of the range 35 | * n is the exponent 36 | */ 37 | double randomizePowerLawDouble(const double x0, const double x1, 38 | const double n); 39 | 40 | // integer power 41 | int ipow(int base, int exp); 42 | 43 | /* Linear algebra */ 44 | 45 | /* Fills a vector with the input values (x, y, z components) 46 | * VectToFill is the output vector 47 | * x, y and z are the 0th, 1st and 2nd components of the output vector, respectively 48 | */ 49 | void FillVect(double *VectToFill, const double x, const double y, 50 | const double z); 51 | 52 | /* Returns the Magnitude of the input vector (3 dimensions) 53 | */ 54 | double VectAbs(double *InputVector); 55 | 56 | /* Returns the XY-projected Magnitude of the input vector 57 | * Useful for calculating pair-potentials of 2 dimensional models 58 | */ 59 | double VectAbsXY(double *InputVector); 60 | 61 | /* Difference of two vectors (3D) 62 | * "ABDiff" is the difference of the vectors "VectA" and "VectB" 63 | * ABDiff = VectA - VectB 64 | */ 65 | void VectDifference(double *ABDiff, double *VectA, double *VectB); 66 | 67 | /* Sum of two vectors (3D) 68 | * "ABSum" is the sum of "VectA" and "VectB" 69 | * ABSum = VectA + VectB 70 | */ 71 | void VectSum(double *ABSum, double *VectA, double *VectB); 72 | 73 | /* Fills up a vector with zero values 74 | * "VectorToNull" will be a vector filled with zeroes 75 | * "Dim" defines the number of components 76 | */ 77 | void NullVect(double *VectorToNull, const int Dim); 78 | 79 | /* Fills up a matrix with zero values 80 | * "MatrixToNull" will be a matrix filled with zeroes 81 | * "rows" and "cols" define the number of components in 2D 82 | */ 83 | void NullMatrix(double **MatrixToNull, const int rows, const int cols); 84 | 85 | /* Multiplicates a vector with a scalar value (3D) 86 | * "OutputVector" will be "VectorToMultiplicate" Multiplicated by "Scalar" 87 | * "Dim" defines the number of components 88 | */ 89 | void MultiplicateWithScalar(double *OutputVector, 90 | double *VectorToMultiplicate, const double Scalar, const int Dim); 91 | 92 | /* Returns the scalar product of two vectors 93 | * The return value will be the inner product of the vectors "A" and "B" 94 | * "Dim" defines the number of components 95 | */ 96 | double ScalarProduct(double *A, double *B, const int Dim); 97 | 98 | /* Outer product of two vectors (3D) 99 | * "Result" will be the vectorial product of the vectors "Vector1" and "Vector2" 100 | * Only works fine in 3 dimensions 101 | */ 102 | void VectorialProduct(double *Result, double *Vector1, double *Vector2); 103 | 104 | /* Creates a unit vector (parallel with the input vector) 105 | * "OutputVector" will be "InputVector" divided by the Length of "InputVector" 106 | * If the Length of "InputVector" is 0, then OutputVector will be filled with zeroes 107 | * Works fine in 3 dimensions 108 | */ 109 | void UnitVect(double *OutputVector, double *InputVector); 110 | 111 | /* Same as UnitVect, but it doesn't give back a 1 long vector, but a value long one 112 | */ 113 | 114 | void NormalizeVector(double *OutputVector, double *InputVector, double value); 115 | 116 | /* Rotates vector around Z axis 117 | * "OutputVector" will be the "InputVector" rotated with "Angle" around Z axis 118 | * "Angle" should be in radians 119 | */ 120 | void RotateVectXY(double *OutputVector, double *InputVector, 121 | const double Angle); 122 | 123 | /* Rotates vector around X axis 124 | * "OutputVector" will be the "InputVector" rotated with "Angle" around x axis 125 | * "Angle" should be in radians 126 | */ 127 | void RotateVectZY(double *OutputVector, double *InputVector, 128 | const double Angle); 129 | 130 | /* Rotates vector around Y axis 131 | * "OutputVector" will be the "InputVector" rotated with "Angle" around Y axis 132 | * "Angle" should be in radians 133 | */ 134 | void RotateVectZX(double *OutputVector, double *InputVector, 135 | const double Angle); 136 | 137 | /* Rodrigues formula (for rotating vectors around a specific axis) 138 | * "OutputVector will be the "InputVector" rotated around a specific axis defined by "Axis" 139 | * angle determines the angle of the rotation. 140 | */ 141 | void RotateVectAroundSpecificAxis(double *OutputVector, double *InputVector, 142 | double *Axis, const double Angle); 143 | 144 | /* Gives the angle of two 3D vectors in 145 | */ 146 | double AngleOfTwoVectors(double *V, double *W, int dim); 147 | // Gives the distance of two points in 2D; 148 | double DistanceOfTwoPoints2D(double *P1, double *P2); 149 | 150 | /* Projects vectors onto a specific line 151 | * "LineDirectionVector" defines the orientation of the line 152 | */ 153 | 154 | void ProjectVectOntoLine(double *OutputVector, double *InputVector, 155 | double *LineDirectionVector); 156 | 157 | /* Projects vectors onto a specific plane 158 | * "PlaneDirectionVector" is a normal vector which gives the direction perpendicular to the plane. 159 | */ 160 | void ProjectVectOntoPlane(double *OutputVector, double *InputVector, 161 | double *PlaneNormalVector); 162 | 163 | /* Calculating distance between a point and a line 164 | * SegmentEndPoint1 and SegmentEndPoint2 are two vectors defining the line 165 | */ 166 | double DistanceFromLine(double *Point, double *SegmentEndpoint1, 167 | double *SegmentEndPoint2); 168 | double DistanceFromLineXY(double *Point, double *SegmentEndPoint1, 169 | double *SegmentEndPoint2); 170 | 171 | /* Checking that the position of "WhichPoint" is inside the "shadow" of x_1 and x_2 (in 2D) 172 | * Returns 1 if CW-outside, -1 if CW-inside and 0 if not in shadow at all. 173 | */ 174 | int AtShadow(double *x1, double *x2, double *WhichPoint); 175 | 176 | /* Checking that the position of "WhichPoint" is inside a polygon given by the vertex-set "Polygon" 177 | */ 178 | bool IsInsidePolygon(double *WhichPoint, double *Polygon, 179 | const int NumberOfVertices); 180 | 181 | /* Multiplicate two N-dimensional square matrices 182 | */ 183 | void MultiplicateSquareMatrices(double **Res, double **Mat1, double **Mat2, 184 | const int N); 185 | 186 | /* Calculates a power of an N-dimensional square matrix 187 | */ 188 | void PowerFuncMatrix(double **Res, double **Mat, const int N, 189 | const int Exponent); 190 | 191 | /* Other useful tools */ 192 | 193 | double ClampScalar(const double x, const double x_min, const double x_max); 194 | 195 | /* 196 | * Sinusoidal sigmoid curve 197 | * 198 | * 1 ---- 199 | * g \ 200 | * r0---> x 201 | */ 202 | double Sigmoid(const double x, const double gamma, const double r0); 203 | 204 | /* 205 | * Linear-gain thresholded sigmoid v(x) curve 206 | * 207 | * v_max ---- 208 | * p \ 209 | * r0---> x 210 | */ 211 | double SigmoidLin(const double x, const double p, const double v_max, 212 | const double r0); 213 | 214 | /* 215 | * Square root - linear combined velocity decay curve 216 | * The final value never goes over the "reference" square root function determined by "r0" and "acc" 217 | * 218 | * v(x) 219 | * ^ 220 | * | /---- v0 221 | * | / p,a 222 | * |------r0-----------> x 223 | */ 224 | double VelDecayLinSqrt(const double x, const double p, const double acc, 225 | const double v_max, const double r0); 226 | 227 | /* 228 | * The inverse of the VelDecayLinSqrt function 229 | */ 230 | double StoppingDistanceLinSqrt(double v, double a, double p); 231 | 232 | /* Calculating optimal size of an arena for "NumberOfAgents" agents 233 | */ 234 | double RadiusOfWayPointAreaFromNumberOfAgents(const int NumberOfAgents, 235 | const double SizeOfAgent, const double Gamma); 236 | 237 | /* Returns the tangent points ("TangentPoint1" and "TangentPoint2") 238 | * and number of target points (return value) 239 | * of a circle relative to "Point". 240 | * 241 | * tangent line: __ _ _ 242 | * _______------ / \ 243 | * point: * | | 244 | * \__/ 245 | * 246 | */ 247 | int TangentsOfCircle(double *TangentPoint1, double *TangentPoint2, 248 | double *Point, double *CentreOfCircle, const double Radius); 249 | int TangentsOfSphereSlice(double *TangentPoint1, double *TangentPoint2, 250 | double *Point, double *CentreOfCircle, 251 | double *NormalVect, const double Radius); 252 | 253 | /* Intersection of the line segments A and B. 254 | * Line segment A is given by its two endpoints (A1, A2) 255 | * Half-line B is given by its endpoint B and the vector of the the half-line 256 | * The coordinates of the intersection points will be in the "Intersection" dynamic array 257 | * If the return value is "-1", then the intersection point doesn't exist. 258 | */ 259 | // !!! TODO: What if a line segment contains the other line segment? 260 | bool IntersectionOfLineSegmentAndHalfLine(double *Intersection, double *A1, 261 | double *A2, double *B, double *VB); 262 | bool IntersectionOfLineSegments(double *Intersection, double *A1, double *A2, 263 | double *B1, double *B2); 264 | double IntersectionOfLines2D(double *Intersection, double *RefPointA1, 265 | double *RefPointA2, double *RefPointB1, double *RefpointB2); 266 | double IntersectionOfLines2D_Dir(double *Intersection, double *RefPointA, 267 | double *DirectionA, double *RefPointB, double *DirectionB); 268 | double IntersectionOfLineSegmentAndLine2D(double *Intersection, 269 | double *RefPointOfLine1, 270 | double *RefPoinrOfLine2, double *EndPoint1, double *EndPoint2); 271 | 272 | /* Gives back the coordinates of two points on a line that are exactly "radius" far from the origo */ 273 | /* Also returns the number of valid points, and two NULLs if no intersection */ 274 | /* The two output vectors come in the order: the further in the direction of "DirectionOfLine", and the other */ 275 | /* You don't have to create an empty vector for the intersection you don't care about, just simply give a NULL instead */ 276 | 277 | int PointsOnLineAtAGivenDistanceFromOrigo3D(double *intersection1, 278 | double *intersection2, 279 | double radius, double *PointOnLine, double *DirectionOfLine); 280 | 281 | /* Gives back the coordinates of two points ("Point1" and "Point2") on a line segment (defined by "EndPoint1" and "EndPoint2") 282 | * that are exactly "Distance" cm far from a reference point ("RefPoint"). Returns the number of valid points (0, 1 or 2) 283 | * This function also can be interpreted as the intersection points of a sphere and a line 284 | */ 285 | int PointsOnLineSegmentAtAGivenDistance3D(double *Point1, double *Point2, 286 | double *EndPoint1, 287 | double *EndPoint2, double *RefPoint, const double Distance); 288 | 289 | /* Calculates the closest points on two lines 290 | * For algo details see: http://geomalgorithms.com/a07-_distance.html#dist3D_Segment_to_Segment 291 | * Line A is P0 + s*u, line B is Q0 + tv, s and t are calculated as scalars for closest point. 292 | * return value is true if lines are parallel 293 | */ 294 | bool ClosestPointOfLines3D(double *P0, double *u, double *Q0, double *v, 295 | double *s, double *t); 296 | 297 | /* Generates a random vector on a unit halph-sphere 298 | * Coordination of the halph-sphere is given by an axis ("Axis"), which is the normal vector of the base circle 299 | * Result vector will be placed into "OutputVector". 300 | */ 301 | void GenerateVectOnHalfSphere(double *OutputVector, double *Axis); 302 | 303 | /* Is there any intersection between two polygons? 304 | * "Polygon1" and "Polygon2" are 2D arrays with NumberOfVertices(1/2) * 2 components. 305 | */ 306 | bool IntersectingPolygons(double **Polygon1, const int NumberOfVertices1, 307 | double **Polygon2, const int NumberOfVertices2); 308 | 309 | /* Calculating the CoM of polygon points in 2D 310 | * "Polygon" is the set of vertices, it has "NumberOfVertices*2" components. 311 | * "CentrePoint" will contain the output centre of mass 312 | */ 313 | void CentreOfPolygon2D(double *CentrePoint, double *Polygon, 314 | const int NumberOfVertices); 315 | 316 | /* Calculating the CoM (centroid) of a polygon in 2D 317 | * "Polygon" is the set of vertices, it has "NumberOfVertices*2" components. 318 | * "CentroidPoint" will contain the centroid point of the polygon 319 | */ 320 | void CentroidOfPolygon2D(double *CentroidPoint, double *Polygon, 321 | const int NumberOfVertices); 322 | 323 | /* Calculating the intersection points of a polygon and a line. 324 | * Also returns the number of intersections. 325 | * Line is given by the reference points "RefPoint1" and "RefPoint2" 326 | */ 327 | void IntersectionOfLineAndPolygon2D(double **IntersectionPoints, 328 | int *NumberOfIntersections, 329 | double *RefPoint1, double *RefPoint2, 330 | double *Polygon, const int NumberOfVertices); 331 | 332 | /* Creating envelope polygon around set of polygons (in 2D, XY coordinate system) 333 | * "EnvelopeSquareCoords" is a 4*2 matrix, contains the output coordinates of the envelope square 334 | * "Polygons" contains the coordinates of all polygons. It has "NumberOfPolygons" rows, 335 | * and "NumberOfVertices[i]*2" columns at the ith row (for XY coordinates placed). 336 | */ 337 | void CreateEnvelopeSquareAroundPolygons(double **EnvelopeSquareCoords, 338 | double **Polygons, int *NumberOfVertices, const int NumberOfPolygons); 339 | 340 | /* 341 | Point1 and Point2 are on the same side of ReferencePoint if: 342 | x-2 x-2 343 | | | 344 | x-1 | and not if | 345 | \_ | x-Ref 346 | \_ | _/ 347 | x-Ref x-1 348 | */ 349 | bool TwoPointsOnSameSideOfAPoint(double *Point1, double *Point2, 350 | double *ReferencePoint); 351 | 352 | #endif 353 | -------------------------------------------------------------------------------- /src/utilities/obstacles.c: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "file_utils.h" 13 | #include "obstacles.h" 14 | 15 | typedef struct { 16 | obstacles_t *obstacles; 17 | double angle; 18 | char last_obstacle_name[MAX_OBSTACLE_NAME_LENGTH]; 19 | } obstacle_callback_data_t; 20 | 21 | static int obstacle_callback(void *user, const char *section, const char *name, 22 | const char *value) { 23 | obstacle_callback_data_t *data = user; 24 | float x, y; 25 | const char *sep; 26 | char obstacle_name[MAX_OBSTACLE_NAME_LENGTH]; 27 | 28 | if (!strcmp(section, "init")) { 29 | /* Initialization section */ 30 | if (!strcmp(name, "angle")) { 31 | /* parse angle between obstacle coordinate system and NE(-D) */ 32 | data->angle = atof(value) * M_PI / 180.0; 33 | return 1; 34 | } else { 35 | printf("ERROR: unknown name in section init (%s)\n", name); 36 | return 0; 37 | } 38 | } else if (!strcmp(section, "obstacles")) { 39 | /* check for propoer format */ 40 | sep = strchr(name, '.'); 41 | if (sep == 0 || sep < name) { 42 | printf("Handler for %s/%s not implemented yet\n", section, name); 43 | return 0; 44 | } 45 | /* get obstacle name */ 46 | strncpy(obstacle_name, name, sep - name); 47 | obstacle_name[sep - name] = 0; 48 | /* it is a new obstacle name */ 49 | if (strcmp(obstacle_name, data->last_obstacle_name)) { 50 | /* increase obstacle counter */ 51 | if (data->obstacles->o_count == MAX_OBSTACLES) { 52 | printf("Max number of obstacles (%d) reached; ignoring " 53 | "remaining obstacles.\n", MAX_OBSTACLES); 54 | return 0; 55 | } 56 | data->obstacles->o_count++; 57 | data->obstacles->o[data->obstacles->o_count - 1].p_count = 0; 58 | /* save name (twice) */ 59 | strncpy(data->last_obstacle_name, obstacle_name, sep - name); 60 | data->last_obstacle_name[sep - name] = 0; 61 | strncpy(data->obstacles->o[data->obstacles->o_count - 1].name, 62 | obstacle_name, sep - name); 63 | data->obstacles->o[data->obstacles->o_count - 1].name[sep - name] = 64 | 0; 65 | } 66 | sep++; 67 | /* Next obstacle point */ 68 | if (!strcmp(sep, "point")) { 69 | if (data->obstacles->o[data->obstacles->o_count - 1].p_count == 70 | MAX_OBSTACLE_POINTS) { 71 | printf("Max number of obstacle points (%d) reached in obstacle '%s'; " "ignoring remaining obstacles.\n", MAX_OBSTACLE_POINTS, obstacle_name); 72 | return 0; 73 | } else if (sscanf(value, "%f %f", &x, &y) != 2) { 74 | printf("Error parsing obstacle %s point #%d: %s\n", 75 | obstacle_name, 76 | data->obstacles->o[data->obstacles->o_count - 77 | 1].p_count, value); 78 | return 0; 79 | } else { 80 | /* convert from local frame to NE(-D) */ 81 | data->obstacles->o[data->obstacles->o_count - 82 | 1].p[data->obstacles->o[data->obstacles->o_count - 83 | 1].p_count][0] = 84 | x * cos(data->angle) - y * sin(data->angle); 85 | data->obstacles->o[data->obstacles->o_count - 86 | 1].p[data->obstacles->o[data->obstacles->o_count - 87 | 1].p_count][1] = 88 | x * sin(data->angle) + y * cos(data->angle); 89 | /* increase obstacle point counter */ 90 | data->obstacles->o[data->obstacles->o_count - 1].p_count++; 91 | return 1; 92 | } 93 | } 94 | } 95 | 96 | printf("Unknown section/key in obstacle file: %s/%s\n", section, name); 97 | return 0; 98 | } 99 | 100 | int ParseObstacleFile(const char *name, obstacles_t * obstacles) { 101 | obstacle_callback_data_t data = { 102 | .obstacles = obstacles, 103 | .angle = 0, 104 | .last_obstacle_name[0] = 0 105 | }; 106 | int result; 107 | int i, j; 108 | float x, y; 109 | 110 | obstacles->o_count = 0; 111 | result = IniParse(name, obstacle_callback, &data); 112 | 113 | if (result) { 114 | printf("%s: parse error on line %d\n", name, result); 115 | return 0; 116 | } 117 | 118 | printf("Parsed %d obstacles.\n", obstacles->o_count); 119 | 120 | /* calculate center of obstacles */ 121 | for (i = 0; i < obstacles->o_count; i++) { 122 | x = y = 0; 123 | for (j = 0; j < obstacles->o[i].p_count; j++) { 124 | x += obstacles->o[i].p[j][0]; 125 | y += obstacles->o[i].p[j][1]; 126 | } 127 | if (j == 0) { 128 | obstacles->o[i].center[0] = 0; 129 | obstacles->o[i].center[1] = 0; 130 | } else { 131 | obstacles->o[i].center[0] = x / j; 132 | obstacles->o[i].center[1] = y / j; 133 | } 134 | } 135 | 136 | /* return number of obstacles parsed */ 137 | return obstacles->o_count; 138 | } 139 | -------------------------------------------------------------------------------- /src/utilities/obstacles.h: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | #ifndef OBSTACLES_H 7 | #define OBSTACLES_H 8 | 9 | #define MAX_OBSTACLES 32 10 | #define MAX_OBSTACLE_POINTS 10 11 | #define MAX_OBSTACLE_NAME_LENGTH 32 12 | 13 | /** 14 | * Struct containing info about a single obstacle used in flights with obstacles. 15 | */ 16 | typedef struct { 17 | char name[MAX_OBSTACLE_NAME_LENGTH]; /* name of the obstacle */ 18 | double p[MAX_OBSTACLE_POINTS][3]; /* list of polygon points */ 19 | double center[2]; /* center of the obstacle polygon */ 20 | int p_count; /* number of points in the obstacle polygon */ 21 | } obstacle_t; 22 | 23 | /** 24 | * Struct containing all info about obstacles 25 | */ 26 | typedef struct { 27 | obstacle_t o[MAX_OBSTACLES]; /* list of obstacles */ 28 | int o_count; /* number of obstacles in the list */ 29 | } obstacles_t; 30 | 31 | /** 32 | * Parses an obstacle file. 33 | * 34 | * \param name the name of the configuration file. A leading tilde (~) will 35 | * be resolved to the home directory of the user. 36 | * \param obstacles the obstacles structure that hold the parsed information 37 | * 38 | * \return number of obstacles parsed successfully 39 | */ 40 | int ParseObstacleFile(const char *name, obstacles_t * obstacles); 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /src/utilities/output_utils.c: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | #include "output_utils.h" 7 | #include "file_utils.h" 8 | 9 | /* Sets default values for all output mode */ 10 | void SetDefaultOutputModes(output_modes_t * OutputModes) { 11 | 12 | /* Default saving mode means that we save nothing... */ 13 | 14 | // Trajectories and inner states 15 | OutputModes->SaveTrajectories = false; 16 | OutputModes->SaveInnerStates = false; 17 | 18 | // Order parameters 19 | OutputModes->SaveDistanceBetweenUnits = FALSE; 20 | OutputModes->SaveDistanceBetweenNeighbours = FALSE; 21 | OutputModes->SaveVelocity = FALSE; 22 | OutputModes->SaveCorrelation = FALSE; 23 | OutputModes->SaveCoM = FALSE; 24 | OutputModes->SaveCollisions = FALSE; 25 | OutputModes->SaveAcceleration = FALSE; 26 | 27 | // Mode-specific order parameters (still not complete) 28 | OutputModes->SaveModelSpecifics = FALSE; 29 | 30 | } 31 | 32 | save_mode_t CheckSaveMode(char *ReadedValue, char *ReadedName) { 33 | 34 | if (strcmp(ReadedValue, "false") == 0) { 35 | return FALSE; 36 | } else if (strcmp(ReadedValue, "timeline") == 0) { 37 | return TIMELINE; 38 | } else if (strcmp(ReadedValue, "stat") == 0) { 39 | return STAT; 40 | } else if (strcmp(ReadedValue, "steadystat") == 0) { 41 | return STEADYSTAT; 42 | } else { 43 | fprintf(stderr, 44 | "For the \"%s\" variable, the valid options are \"timeline\", \"stat\", \"steadystat\" and \"false\"\n(Default is \"false\")\n", 45 | ReadedValue); 46 | return FALSE; 47 | } 48 | 49 | } 50 | 51 | /* Reads output modes from config file */ 52 | void ReadOutputModes(output_modes_t * OutputModes, FILE * InputFile) { 53 | 54 | if (NULL == InputFile) { 55 | 56 | SetDefaultOutputModes(OutputModes); 57 | 58 | printf("WARNING! I couldn't find config.ini file!\n"); 59 | 60 | return; 61 | 62 | } 63 | 64 | SetDefaultOutputModes(OutputModes); 65 | 66 | /* format of an input line in the InputFile: 67 | * ReadedName=ReadedValue 68 | */ 69 | char line[256]; 70 | 71 | char *start, *end; 72 | 73 | char *ReadedName; 74 | char *ReadedValue; 75 | 76 | int lineno = 0; 77 | 78 | while (fgets(line, sizeof(line), InputFile) != NULL) { 79 | 80 | start = LSkip(RStrip(line)); 81 | 82 | if (*start == ';' || *start == '#') { 83 | //These lines are skipped... 84 | } else { 85 | 86 | end = FindCharOrComment(start, '='); 87 | if (*end == '=') { 88 | *end = '\0'; 89 | ReadedName = RStrip(start); 90 | ReadedValue = LSkip(end + 1); 91 | end = FindCharOrComment(ReadedValue, '\0'); 92 | } 93 | 94 | /* Reading logical values from input lines */ 95 | if (strcmp(ReadedName, "SaveTrajectories") == 0) { 96 | OutputModes->SaveTrajectories = 97 | (strcmp(ReadedValue, "true") == 0); 98 | if (strcmp(ReadedValue, "true") != 0 99 | && strcmp(ReadedValue, "false") != 0) { 100 | fprintf(stderr, 101 | "For the \"SaveTrajectories\" variable, the valid options are \"true\" and \"false\"\n(Default is \"false\")\n"); 102 | } 103 | } else if (strcmp(ReadedName, "SaveInnerStates") == 0) { 104 | OutputModes->SaveInnerStates = 105 | (strcmp(ReadedValue, "true") == 0); 106 | if (strcmp(ReadedValue, "true") != 0 107 | && strcmp(ReadedValue, "false") != 0) { 108 | fprintf(stderr, 109 | "For the \"SaveInnerStates\" variable, the valid options are \"true\" and \"false\"(Default is \"false\")\n"); 110 | } 111 | } else if (strcmp(ReadedName, "SaveCorrelation") == 0) { 112 | OutputModes->SaveCorrelation = CheckSaveMode(ReadedValue, ReadedName); //(strcmp (ReadedValue, "true") == 0); 113 | } else if (strcmp(ReadedName, "SaveVelocity") == 0) { 114 | OutputModes->SaveVelocity = 115 | CheckSaveMode(ReadedValue, ReadedName); 116 | } else if (strcmp(ReadedName, "SaveDistanceBetweenNeighbours") == 0) { 117 | OutputModes->SaveDistanceBetweenNeighbours = 118 | CheckSaveMode(ReadedValue, ReadedName); 119 | } else if (strcmp(ReadedName, "SaveCoM") == 0) { 120 | OutputModes->SaveCoM = CheckSaveMode(ReadedValue, ReadedName); 121 | } else if (strcmp(ReadedName, "SaveDistanceBetweenUnits") == 0) { 122 | OutputModes->SaveDistanceBetweenUnits = 123 | CheckSaveMode(ReadedValue, ReadedName); 124 | } else if (strcmp(ReadedName, "SaveCollisionRatio") == 0) { 125 | OutputModes->SaveCollisionRatio = 126 | CheckSaveMode(ReadedValue, ReadedName); 127 | } else if (strcmp(ReadedName, "SaveCollisions") == 0) { 128 | OutputModes->SaveCollisions = 129 | CheckSaveMode(ReadedValue, ReadedName); 130 | } else if (strcmp(ReadedName, "SaveAcceleration") == 0) { 131 | OutputModes->SaveAcceleration = 132 | CheckSaveMode(ReadedValue, ReadedName); 133 | } else if (strcmp(ReadedName, "SaveModelSpecificStats") == 0) { 134 | OutputModes->SaveModelSpecifics = 135 | CheckSaveMode(ReadedValue, ReadedName); 136 | } 137 | 138 | } 139 | 140 | } 141 | 142 | } 143 | 144 | /* Prints actual state into file 145 | */ 146 | void 147 | WriteOutTrajectories(phase_t * Phase, const bool SaveTrajs, 148 | const bool SaveInnerStates, const double ActualTime, 149 | FILE * f_OutPhase, FILE * f_OutInnerStates) { 150 | 151 | int j, h; 152 | 153 | if (SaveTrajs) { 154 | fprintf(f_OutPhase, "%lf\t", ActualTime); 155 | } 156 | if (SaveInnerStates) { 157 | fprintf(f_OutInnerStates, "%lf\t", ActualTime); 158 | } 159 | 160 | for (j = 0; j < Phase->NumberOfAgents; j++) { 161 | 162 | if (true == SaveTrajs) { 163 | for (h = 0; h < 3; h++) { 164 | fprintf(f_OutPhase, "%lf\t", Phase->Coordinates[j][h]); 165 | } 166 | for (h = 0; h < 3; h++) { 167 | fprintf(f_OutPhase, "%lf\t", Phase->Velocities[j][h]); 168 | } 169 | } 170 | 171 | for (h = 0; h < Phase->NumberOfInnerStates; h++) { 172 | if (true == SaveInnerStates) { 173 | fprintf(f_OutInnerStates, "%lf\t", Phase->InnerStates[j][h]); 174 | } 175 | } 176 | 177 | } 178 | 179 | if (SaveTrajs) { 180 | fprintf(f_OutPhase, "\n"); 181 | } 182 | if (SaveInnerStates) { 183 | fprintf(f_OutInnerStates, "\n"); 184 | } 185 | 186 | } 187 | -------------------------------------------------------------------------------- /src/utilities/output_utils.h: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* This file contains structs and function declarations 7 | * for setting up special output modes 8 | */ 9 | 10 | #ifndef OUTPUT_UTILS_H 11 | #define OUTPUT_UTILS_H 12 | 13 | #include 14 | #include 15 | #include "dynamics_utils.h" 16 | 17 | typedef enum { 18 | FALSE = 0, 19 | TIMELINE = 1, 20 | STAT = 2, 21 | STEADYSTAT = 3 22 | } save_mode_t; 23 | 24 | /* With this struct, one can set which outputs have to be saved 25 | */ 26 | typedef struct { 27 | 28 | /* Position, velocity and inner states */ 29 | bool SaveTrajectories; 30 | bool SaveInnerStates; 31 | 32 | /* Order parameters */ 33 | save_mode_t SaveDistanceBetweenUnits; 34 | save_mode_t SaveDistanceBetweenNeighbours; 35 | save_mode_t SaveVelocity; 36 | save_mode_t SaveCorrelation; 37 | save_mode_t SaveCoM; 38 | save_mode_t SaveCollisions; 39 | save_mode_t SaveCollisionRatio; 40 | save_mode_t SaveAcceleration; 41 | 42 | /* Model-specific parameters */ 43 | save_mode_t SaveModelSpecifics; 44 | 45 | } output_modes_t; 46 | 47 | /* Sets default values for all output mode 48 | */ 49 | void SetDefaultOutputModes(output_modes_t * OutputModes); 50 | 51 | /* Reads output modes from config file 52 | */ 53 | void ReadOutputModes(output_modes_t * OutputModes, FILE * InputFile); 54 | 55 | /* Prints actual state into file 56 | */ 57 | void WriteOutTrajectories(phase_t * Phase, const bool SaveTrajs, 58 | const bool SaveInnerStates, 59 | const double ActualTime, FILE * f_OutPhase, FILE * f_OutInnerStates); 60 | 61 | #endif 62 | -------------------------------------------------------------------------------- /src/utilities/param_utils.h: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* This file contains useful structs for creating a model 7 | * and drawing a menu system automatically 8 | * Tools for handling strings and input files are also declared here. 9 | */ 10 | 11 | #ifndef PARAM_UTILS_H 12 | #define PARAM_UTILS_H 13 | 14 | #include "file_utils.h" 15 | #include 16 | #include 17 | 18 | /* Number of parameter sets */ 19 | static int NumberOfFlockingParamSets = 0; 20 | static int NumberOfUnitParamSets = 0; 21 | static int SelectedFlockingParamSet = 0; 22 | static int SelectedUnitParamSet = 0; 23 | 24 | /* Max number of flocking model parameters */ 25 | #define MAX_FLOCKING_PARAMS 64 26 | 27 | /* Macro to create variables */ 28 | #define CREATE_FLOCKING_PARAM(n, ...) \ 29 | CreateFlockingParam(FlockingParams, (fl_param_double_t) { \ 30 | .NameInFiles = #n, \ 31 | .StaticValuePointer = &n, \ 32 | .InMenu = true, \ 33 | .Constant = false, \ 34 | .NumberOfLabels = 0, \ 35 | __VA_ARGS__}); 36 | 37 | /* Hidden variable (not visible in menu system) */ 38 | #define CREATE_HIDDEN_FLOCKING_PARAM(n, ...) \ 39 | CreateFlockingParam(FlockingParams, (fl_param_double_t) { \ 40 | .NameInFiles = #n, \ 41 | .StaticValuePointer = &n, \ 42 | .InMenu = false, \ 43 | .Constant = false, \ 44 | .NumberOfLabels = 0, \ 45 | __VA_ARGS__}); 46 | 47 | /* Constant variable (Changes only when F12 is pressed) */ 48 | #define CREATE_CONSTANT_FLOCKING_PARAM(n, ...) \ 49 | CreateFlockingParam(FlockingParams, (fl_param_double_t) { \ 50 | .NameInFiles = #n, \ 51 | .StaticValuePointer = &n, \ 52 | .InMenu = true, \ 53 | .Constant = true, \ 54 | .NumberOfLabels = 0, \ 55 | __VA_ARGS__}); 56 | 57 | /* Constant hidden variable (Changes only when F12 is pressed and not visible in menu system) */ 58 | #define CREATE_HIDDEN_CONSTANT_FLOCKING_PARAM(n, ...) \ 59 | CreateFlockingParam(FlockingParams, (fl_param_double_t) { \ 60 | .NameInFiles = #n, \ 61 | .StaticValuePointer = &n, \ 62 | .InMenu = false, \ 63 | .Constant = true, \ 64 | .NumberOfLabels = 0, \ 65 | __VA_ARGS__}); 66 | 67 | /* Searching for correct parameter and change the value of it */ 68 | #define CHANGE_PARAMETER(param_Name, param_value) \ 69 | jj = FlockingParams->NumberOfParameters; \ 70 | while (jj >= -1 && strcmp(FlockingParams->Params[jj].NameInFiles, #param_Name) != 0) { jj --; } \ 71 | if (jj > -1) {FlockingParams->Params[jj].Value = param_value; } \ 72 | RefreshFlockingParams (FlockingParams); 73 | 74 | /* Searching for correct parameter and change the possible maximum of it */ 75 | #define CHANGE_PARAMETER_MAX(param_Name, param_max_value) \ 76 | jj = FlockingParams->NumberOfParameters; \ 77 | while (jj >= -1 && strcmp(FlockingParams->Params[jj].NameInFiles, #param_Name) != 0) { jj --; } \ 78 | if (jj > -1) {FlockingParams->Params[jj].Max = param_max_value; } \ 79 | RefreshFlockingParams (FlockingParams); 80 | 81 | /* Searching for correct parameter and change the possible minimum of it */ 82 | #define CHANGE_PARAMETER_MIN(param_Name, param_min_value) \ 83 | jj = FlockingParams->NumberOfParameters; \ 84 | while (jj >= -1 && strcmp(FlockingParams->Params[jj].NameInFiles, #param_Name) != 0) { jj --; } \ 85 | if (jj > -1) {FlockingParams->Params[jj].Min = param_min_value; } \ 86 | RefreshFlockingParams (FlockingParams); 87 | 88 | /* Setting up a label-set for a given parameter. 89 | */ 90 | #define SET_LABEL_FOR_PARAMETER_VALUE(param_Name, param_value, param_label) \ 91 | if (sizeof(param_label) > 7) {fprintf (stderr, "Parameter value label should be shorter than 7 characters!\n"); exit(-1);} \ 92 | tt = FlockingParams->NumberOfParameters; \ 93 | while (tt >= -1 && strcmp(FlockingParams->Params[tt].NameInFiles, #param_Name) != 0) { tt --; } \ 94 | if (tt > -1) { \ 95 | strcpy (FlockingParams->Params[tt].Labels.Captions[FlockingParams->Params[tt].NumberOfLabels], param_label); \ 96 | FlockingParams->Params[tt].Labels.Values[FlockingParams->Params[tt].NumberOfLabels] = param_value; \ 97 | (FlockingParams->Params[tt].NumberOfLabels)++; \ 98 | } \ 99 | RefreshFlockingParams (FlockingParams); 100 | 101 | /* Model of a robot */ 102 | 103 | /* Parameter of the model of a unit (double) 104 | */ 105 | typedef struct { 106 | 107 | /* Name (for menu system and input files) */ 108 | char Name[64]; 109 | /* unit of measurement (for menu system) */ 110 | char UnitOfMeas[10]; 111 | /* value in double precision */ 112 | double Value; 113 | 114 | } unit_param_double_t; 115 | 116 | /* Parameters of the model of a single unit 117 | */ 118 | typedef struct { 119 | 120 | /* Relaxation times of PID controller */ 121 | unit_param_double_t Tau_PID_XY; 122 | unit_param_double_t Tau_PID_Z; 123 | /* Maximum acceleration */ 124 | unit_param_double_t a_max; 125 | /* Refresh rate of GPS device */ 126 | unit_param_double_t t_GPS; 127 | /* Time delay of communication */ 128 | unit_param_double_t t_del; 129 | /* Range of communication */ 130 | unit_param_double_t R_C; 131 | 132 | /* Packet loss is a random process modelled with quadradically increasing 133 | probability with distance, reaching a given packet_loss_ratio at 134 | packet_loss_distance, i.e., y = ax^2 where a = ratio/distance^2 */ 135 | unit_param_double_t packet_loss_ratio; /* 0-1 */ 136 | unit_param_double_t packet_loss_distance; /* 0- */ 137 | 138 | /* Noise parameters */ 139 | 140 | /* Noises are modelled with Wiener processes 141 | * with given Constant Sigma. 142 | * Inner noises are the inaccuracy of the sensors (e.g. GPS), 143 | * outer noises are the effects of the wind, inaccuracy of PID controller, etc 144 | */ 145 | 146 | /* stdev of inner noise (XY) */ 147 | unit_param_double_t Sigma_GPS_XY; 148 | /* stdev of outer noise (XY) */ 149 | unit_param_double_t Sigma_Outer_XY; 150 | /* stdev of inner noise (Z) */ 151 | unit_param_double_t Sigma_GPS_Z; 152 | /* stdev of outer noise (Z) */ 153 | unit_param_double_t Sigma_Outer_Z; 154 | 155 | /* Effects of wind (stdev, magnitude avg, angle) */ 156 | unit_param_double_t Wind_Magn_Avg; 157 | unit_param_double_t Wind_StDev; 158 | unit_param_double_t Wind_Angle; 159 | unit_param_double_t ViscosityCoeff; 160 | 161 | char FileName[512]; 162 | 163 | } unit_model_params_t; 164 | 165 | /* Flocking model basic structure */ 166 | 167 | /* parameter of the flocking model (double) 168 | */ 169 | typedef struct { 170 | 171 | /* Name (for menu system and input files) */ 172 | char Name[64]; 173 | /* unit of measurement (for menu system) */ 174 | char UnitOfMeas[10]; 175 | /* value in double precision */ 176 | double Value; 177 | 178 | /* Parameters for display parameter in the menu system... */ 179 | 180 | /* number of Digits (menu display) */ 181 | int Digits; 182 | /* size of step (when changing) */ 183 | double SizeOfStep; 184 | /* Multiplication (for example 1.0/100.0 between cm and m) */ 185 | double Mult; 186 | 187 | /* min and max values */ 188 | double Min; 189 | double Max; 190 | 191 | /* Name in the input file */ 192 | char NameInFiles[64]; 193 | 194 | /* Is it appears in the menu? */ 195 | bool InMenu; 196 | 197 | /* Can be changed during measurement? 198 | * Or can only be changed by pressing F12? */ 199 | bool Constant; 200 | double StoredValue; 201 | 202 | /* This variable is for inner use to be able to define static 203 | parameters to use the param values simply. */ 204 | double *StaticValuePointer; 205 | 206 | /* With this struct, up to 32 labels can be added 207 | * for better representation of parameter values in menus during visualization. 208 | * Each label can be 7 character long 209 | */ 210 | int NumberOfLabels; 211 | struct { 212 | double Values[32]; 213 | char Captions[32][7]; 214 | } Labels; 215 | 216 | } fl_param_double_t; 217 | 218 | /* Struct to store parameters of the flocking model 219 | */ 220 | typedef struct { 221 | 222 | char FileName[256]; 223 | 224 | int NumberOfParameters; 225 | fl_param_double_t Params[MAX_FLOCKING_PARAMS]; /* increase if more parameters are needed */ 226 | 227 | /* With the variables below, model-specific input files can be defined with specific option flags 228 | */ 229 | int NumberOfInputs; 230 | char *Inputs[64]; 231 | 232 | /* Number of inner states */ 233 | int NumberOfInnerStates; 234 | 235 | } flocking_model_params_t; 236 | 237 | /* Other */ 238 | 239 | /* Parameters of the actual situation 240 | * e. g. number of agents, Initial area size, etc. 241 | */ 242 | typedef struct { 243 | 244 | /* Number of agents (cannot be changed during a measurement) */ 245 | int NumberOfAgents; 246 | /* Initial X, Y, Z positions */ 247 | double InitialX; 248 | double InitialY; 249 | double InitialZ; 250 | /* Length of the measurement (sec) */ 251 | double Length; 252 | /* Accuracy of the Euler method for solving the ODE */ 253 | double DeltaT; 254 | 255 | /* Radius of dangerous area around copters */ 256 | double Radius; 257 | 258 | /* Time length to store */ 259 | double LengthToStore; 260 | 261 | /* Default visualization speed */ 262 | int VizSpeedUp; 263 | 264 | /* Estimated tarting time of steady state - 265 | * Averaging order parameters is only neccessary from here. 266 | */ 267 | double StartOfSteadyState; 268 | 269 | } sit_parameters_t; 270 | 271 | /* Functions for setting parameters */ 272 | 273 | /* Changes one of the 'robot model' parameters 274 | * "WhichParam" is the index of the parameter 275 | * "StepSize" will be added to the value of the parameter 276 | * "DeltaT" is the accuracy of the Euler method 277 | */ 278 | void ChangeUnitModelParameter(unit_model_params_t * Params, int WhichParam, 279 | double StepSize, double DeltaT); 280 | 281 | void SetNamesOfUnitModelParams(unit_model_params_t * UnitParams); 282 | 283 | /* Gets the parameter set 284 | * {TauPID_XY, Tau_PID_Z, t_GPS, simga_GPS_XY, Sigma_GPS_Z, Sigma_Outer_XY, Sigma_Outer_Z, t_del, R_C, ...} 285 | * from "InputFile" 286 | */ 287 | void GetUnitModelParamsFromFile(unit_model_params_t * UnitParams, 288 | FILE * InputFile); 289 | 290 | /* Saves the parameter set 291 | * {TauPID, t_GPS, simga_GPS_XY, Sigma_GPS_Z, Sigma_Outer_XY, Sigma_Outer_Z, t_del, R_C, ...} 292 | * into "OutputFile" 293 | */ 294 | void SaveUnitModelParamsToFile(FILE * OutputFile, 295 | unit_model_params_t * UnitParams); 296 | 297 | /* Gets the parameter set 298 | * {Number of agents, Initial sizes {x, y, z}, Length of measurement, DeltaT} 299 | * from "InputFile" 300 | */ 301 | sit_parameters_t GetSituationParamsFromFile(FILE * InputFile); 302 | 303 | /* Changes one of the 'flocking algorithm' parameters 304 | */ 305 | void ChangeFlockingModelParameter(flocking_model_params_t * FlockingParams, 306 | const int WhichParam, const double StepSize); 307 | 308 | /* Saves the parameter set of the specific algorithm 309 | */ 310 | void SaveFlockingModelParamsToFile(FILE * OutputFile, 311 | flocking_model_params_t * FlockingParams); 312 | 313 | /* Get the index of a flocking model parameter in the model params list 314 | * based on its Name. 315 | * 316 | * Returns -1 if not found. 317 | */ 318 | int GetFlParamIndexByName(flocking_model_params_t * FlockingParams, 319 | const char *Name); 320 | 321 | /* Allocate memory for a param and store it in the params structure. 322 | * 323 | * Note that this function must be called 324 | * in the InitializeFlockingParams() function of your model file!!! 325 | */ 326 | void CreateFlockingParam(flocking_model_params_t * FlockingParams, 327 | fl_param_double_t Param); 328 | 329 | /* Refresh static double variables defined for flocking model parameters 330 | * in the model file from the main parameter structure, to simplify usage. 331 | * 332 | * Note that this function must be called only from your model file!!! 333 | */ 334 | void RefreshFlockingParams(flocking_model_params_t * FlockingParams); 335 | 336 | /* 337 | */ 338 | bool FillParameterSetsFromFile(flocking_model_params_t * FlockingParamSets, 339 | unit_model_params_t * UnitParamSets, 340 | int argc, 341 | int *NumberOfFlockingModelParamSets, 342 | int *NumberOfUnitModelParamSets, 343 | char *argv[], char *CurrentDirectory, char *ParamsFileName); 344 | 345 | #endif 346 | -------------------------------------------------------------------------------- /src/utilities/pngout_utils.c: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | #include "pngout_utils.h" 7 | 8 | /* Take a screenshot */ 9 | 10 | void TakeScreenshot(const char *screenshotFile) { 11 | 12 | ILuint imageID = ilGenImage(); 13 | ilBindImage(imageID); 14 | ilutGLScreen(); 15 | ilEnable(IL_FILE_OVERWRITE); 16 | ilSaveImage(screenshotFile); 17 | ilDeleteImage(imageID); 18 | 19 | } 20 | -------------------------------------------------------------------------------- /src/utilities/pngout_utils.h: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | #ifndef PNGOUT_UTILS_H 7 | #define PNGOUT_UTILS_H 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | //Why do we need to re-prototype this function? 15 | ILboolean ilutGLScreen(); 16 | 17 | /* If PNGOutPic is true, then a singe image will be saved 18 | */ 19 | static bool PNGOutPic = false; 20 | 21 | /* How many video-frame sets have been saved? 22 | */ 23 | static int VidNum = 0; 24 | 25 | /* Take a screenshot 26 | */ 27 | void TakeScreenshot(const char *screenshotFile); 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /src/vizmode.h: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* Basic types and structs for modes 7 | */ 8 | 9 | #ifndef VIZMODE_H 10 | #define VIZMODE_H 11 | 12 | #include 13 | 14 | /* This struct contains the details of the actual vizualization mode. 15 | */ 16 | typedef struct { 17 | 18 | /* Vizualization ON / OFF */ 19 | bool VizEnabled; 20 | bool ExperimentOver; 21 | 22 | /* Visualization Speed */ 23 | int VizSpeedUp; 24 | bool Paused; 25 | /* Two or three dimensions? */ 26 | bool TwoDimViz; 27 | /* CoM at the center of the screen? */ 28 | bool CoMFollowing; 29 | /* Which agent is selected? */ 30 | /* "NumberOfAgents+1" means that agent-following is off */ 31 | int WhichAgentIsSelected; 32 | /* Display parameters of unit model */ 33 | bool UnitParamsDisplayed; 34 | /* Display parameters of flocking algorithm */ 35 | bool FlockingParamsDisplayed; 36 | /* Display GPS ghosts */ 37 | bool DisplayGPSGhosts; 38 | /* Display labels above agents */ 39 | bool DisplayAgentLabels; 40 | /* Display Communication network */ 41 | bool DisplayCommNetwork; 42 | /* Length of displayed tail */ 43 | bool DisplayTail; 44 | int LengthOfTail; 45 | 46 | /* Which parameter is selected? */ 47 | /* 0 means "Visualization speed" */ 48 | int WhichParamIsSelected; 49 | 50 | /* Map sizes and center */ 51 | double MapSizeXY; 52 | double MapSizeZ; 53 | double CenterX; 54 | double CenterY; 55 | double CenterZ; 56 | 57 | /* Eye position */ 58 | double EyeX; 59 | double EyeY; 60 | double EyeZ; 61 | 62 | float UpX; 63 | float UpY; 64 | float UpZ; 65 | 66 | /* Position limits */ 67 | double DistanceFromCenterLimit; 68 | double DistanceFromCenterLimit_XY; 69 | double EyeZLimit; 70 | 71 | /* Camera trajectory mode */ 72 | bool CamTraj; 73 | 74 | /* Resolution of the visualization window. It is always square-shaped... */ 75 | double Resolution; 76 | 77 | } vizmode_params_t; 78 | 79 | #endif 80 | -------------------------------------------------------------------------------- /src/vizualizer/objects_2d.c: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* Tools for drawing objects in 2D visualization 7 | */ 8 | 9 | #include "objects_2d.h" 10 | #include 11 | #include 12 | #include 13 | 14 | /* Useful global variables */ 15 | static float ActualColor[3]; 16 | 17 | /* Windows and other general tools */ 18 | 19 | /* Displays a window with given width and height at (posX, posY) coordinates */ 20 | void DisplayWindow(int width, int height, int posX, int posY) { 21 | 22 | glutInitWindowSize(width, height); 23 | glutInitWindowPosition(posX, posY); 24 | glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_DEPTH); 25 | 26 | //glutInitDisplayMode(GLUT_RGBA | GLUT_DEPTH); //Remove these slashes if you don't need double buffered mode 27 | glMatrixMode(GL_MODELVIEW); 28 | glLoadIdentity(); 29 | glMatrixMode(GL_PROJECTION); 30 | glLoadIdentity(); 31 | 32 | } 33 | 34 | /* Simple euclidean objects in GL coords */ 35 | 36 | /* Draws a shape with given center coordinates (in GL coords), width, height and rotation angle */ 37 | void DrawShape(double CenterX, double CenterY, double width, double height, 38 | double angle, const float *color) { 39 | 40 | glColor3f(color[0], color[1], color[2]); 41 | glBegin(GL_QUADS); 42 | { 43 | glVertex2f(CenterX + cos(angle) * width / 2.0 + 44 | sin(angle) * height / 2.0, 45 | CenterY - sin(angle) * width / 2.0 + cos(angle) * height / 2.0); 46 | glVertex2f(CenterX - cos(angle) * width / 2.0 + 47 | sin(angle) * height / 2.0, 48 | CenterY + sin(angle) * width / 2.0 + cos(angle) * height / 2.0); 49 | glVertex2f(CenterX - cos(angle) * width / 2.0 - 50 | sin(angle) * height / 2.0, 51 | CenterY + sin(angle) * width / 2.0 - cos(angle) * height / 2.0); 52 | glVertex2f(CenterX + cos(angle) * width / 2.0 - 53 | sin(angle) * height / 2.0, 54 | CenterY - sin(angle) * width / 2.0 - cos(angle) * height / 2.0); 55 | } 56 | glEnd(); 57 | 58 | } 59 | 60 | /* Draws a line with given endpoint coordinates (in GL coords) */ 61 | void DrawLine(double x1, double y1, double x2, double y2, double width, 62 | const float *color) { 63 | 64 | /* Line is a thin shape */ 65 | DrawShape(0.5 * (x1 + x2), 0.5 * (y1 + y2), width, 66 | sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1)), atan2(x2 - x1, 67 | y2 - y1), color); 68 | 69 | } 70 | 71 | /* Draws an empty tetragon with given vertices (?) */ 72 | void DrawTetragon(double x1, double y1, 73 | double x2, double y2, 74 | double x3, double y3, 75 | double x4, double y4, double width, const float *color) { 76 | 77 | } 78 | 79 | /* Draws a fulle tetragon with given vertices (?) */ 80 | void DrawFullTetragon(double x1, double y1, 81 | double x2, double y2, 82 | double x3, double y3, double x4, double y4, const float *color) { 83 | 84 | } 85 | 86 | /* Draws a trinagle (platonic, if the width and height of the window are equal) */ 87 | void DrawTriangle(double CenterX, double CenterY, double LengthOfEdge, 88 | double Angle, const float *color) { 89 | 90 | glColor3f(color[0], color[1], color[2]); 91 | double m = LengthOfEdge / (2 * sqrt(3)); 92 | double b = 2.0 * m; 93 | 94 | glBegin(GL_TRIANGLES); 95 | { 96 | glVertex2f(CenterX - cos(Angle) * m + sin(Angle) * LengthOfEdge / 2, 97 | CenterY + sin(Angle) * m + cos(Angle) * LengthOfEdge / 2); 98 | glVertex2f(CenterX + cos(Angle) * b, CenterY - sin(Angle) * b); 99 | glVertex2f(CenterX - cos(Angle) * m - sin(Angle) * LengthOfEdge / 2, 100 | CenterY + sin(Angle) * b - cos(Angle) * LengthOfEdge / 2); 101 | } 102 | glEnd(); 103 | 104 | } 105 | 106 | /* Draws an empty circle with given center coordinates (GL coords) and radius */ 107 | void DrawCircle(double Centerx, double Centery, double radius, 108 | const float *color) { 109 | 110 | glColor3f(color[0], color[1], color[2]); 111 | float i; 112 | 113 | /* Cicrle equals several lines connected to an N-gon, where N is large enough */ 114 | glBegin(GL_LINE_STRIP); 115 | for (i = 0; i < 2 * M_PI; i += 0.01) { 116 | 117 | glVertex3f(Centerx + radius * cos(i), -(-Centery + radius * sin(i)), 1); 118 | 119 | } 120 | glEnd(); 121 | 122 | } 123 | 124 | /* Draws an empty circle with thick edges with given center coordinates (GL coord) and radius */ 125 | void DrawThickEdgedCircle(double Centerx, double Centery, double radius, 126 | double EdgeThickness, const float *color) { 127 | 128 | glColor3f(color[0], color[1], color[2]); 129 | double i; 130 | 131 | /* Circle equals several shapes... */ 132 | for (i = 0; i < 2 * M_PI; i += 0.1) { 133 | DrawShape(Centerx + radius * cos(i), -(-Centery + radius * sin(i)), 134 | EdgeThickness, 0.1 * radius, i, color); 135 | } 136 | 137 | } 138 | 139 | void DrawGradientColoredCircle(const double Centerx, const double Centery, 140 | const double radius, const double EdgeThickness, const float *color1, 141 | const float *color2, const int HowManySteps) { 142 | 143 | int i; 144 | ActualColor[0] = color1[0]; 145 | ActualColor[1] = color1[1]; 146 | ActualColor[2] = color1[2]; 147 | 148 | for (i = 0; i < HowManySteps; i++) { 149 | 150 | ActualColor[0] += (color2[0] - color1[0]) / HowManySteps; 151 | ActualColor[1] += (color2[1] - color1[1]) / HowManySteps; 152 | ActualColor[2] += (color2[2] - color1[2]) / HowManySteps; 153 | 154 | DrawThickEdgedCircle(Centerx, Centery, 155 | radius - EdgeThickness / 2.0 + i * EdgeThickness / HowManySteps, 156 | EdgeThickness / HowManySteps, ActualColor); 157 | 158 | } 159 | 160 | } 161 | 162 | /* Draws a full circle with given center coordinates (GL coords) and radius */ 163 | void DrawFullCircle(double Centerx, double Centery, double radius, 164 | const float *color) { 165 | 166 | double i; 167 | 168 | /* A full circle can be drawed by several rotated shapes */ 169 | for (i = 0; i < 2 * M_PI; i += 0.1) { 170 | 171 | DrawShape(Centerx, Centery, radius * M_SQRT2, radius * M_SQRT2, i, 172 | color); 173 | 174 | } 175 | 176 | } 177 | 178 | /* Draws an arrow with given center coordinates ("x" and "y") and Length ("Length") */ 179 | void DrawArrow(double x, double y, double Length, double angle, 180 | const float *color) { 181 | 182 | /* An arrow is a shape with a triangle on one of its end */ 183 | DrawTriangle(x + cos(angle) * Length / 2.0, y - sin(angle) * Length / 2.0, 184 | Length, angle, color); 185 | DrawShape(x, y, Length, Length / 4.0, angle, color); 186 | 187 | } 188 | 189 | /* Draws an arrow with given center coordinates ("x" and "y") and Length ("Length") */ 190 | void DrawThinArrow(double x, double y, double Length, double TriangleEdgeLength, 191 | double angle, const float *color) { 192 | 193 | /* An arrow is a shape with a triangle on one of its end */ 194 | DrawTriangle(x + cos(angle) * Length / 2, y - sin(angle) * Length / 2, 195 | TriangleEdgeLength, angle, color); 196 | DrawShape(x, y, Length, 0.003, angle, color); 197 | 198 | } 199 | 200 | /* Draws a transparent (empty) shape */ 201 | void DrawTransparentShape(double x, double y, double width, double height, 202 | const float *color) { 203 | 204 | glColor3f(color[0], color[1], color[2]); 205 | 206 | glBegin(GL_LINES); 207 | 208 | glVertex3f(x, y, 1); 209 | glVertex3f(x + width, y, 1); 210 | glVertex3f(x + width, y, 1); 211 | glVertex3f(x + width, y + height, 1); 212 | glVertex3f(x, y + height, 1); 213 | glVertex3f(x + width, y + height, 1); 214 | glVertex3f(x, y + height, 1); 215 | glVertex3f(x, y, 1); 216 | 217 | glEnd(); 218 | 219 | } 220 | 221 | /* Draws a polygon with "NumberOfVertices" vertices */ 222 | void DrawPolygon(float *Vertices, const int NumberOfVertices, 223 | const float *color) { 224 | 225 | glColor3f(color[0], color[1], color[2]); 226 | int i; 227 | 228 | glBegin(GL_LINE_STRIP); 229 | for (i = 1; i < NumberOfVertices + 1; i++) { 230 | glVertex3f(Vertices[(i % NumberOfVertices) * 3], 231 | Vertices[(i % NumberOfVertices) * 3 + 1], 1); 232 | } 233 | glEnd(); 234 | 235 | } 236 | 237 | /* Draws a transparent (empty) shape with given edge thickness */ 238 | void DrawThickEdgedTransparentShape(const double x, 239 | const double y, 240 | const double width, 241 | const double height, 242 | const double EdgeThickness, double angle, const float *color) { 243 | 244 | angle *= -1; //Oops... 245 | DrawShape(x + width / 2.0 * cos(angle), 246 | y - width / 2.0 * sin(angle), 247 | EdgeThickness, height + EdgeThickness, angle, color); 248 | DrawShape(x - height / 2.0 * sin(angle), 249 | y - height / 2.0 * cos(angle), 250 | width + EdgeThickness, EdgeThickness, angle, color); 251 | DrawShape(x - width / 2.0 * cos(angle), 252 | y + width / 2.0 * sin(angle), 253 | EdgeThickness, height + EdgeThickness, angle, color); 254 | DrawShape(x + height / 2.0 * sin(angle), 255 | y + height / 2.0 * cos(angle), 256 | width + EdgeThickness, EdgeThickness, angle, color); 257 | 258 | } 259 | 260 | /* Draws a gradient-colored transparent shape with given edge thickness */ 261 | void DrawGradientColoredShape(const double x, 262 | const double y, 263 | const double width, 264 | const double height, 265 | const double EdgeThickness, 266 | double angle, 267 | const float *color1, const float *color2, const int HowManySteps) { 268 | 269 | ActualColor[0] = color1[0]; 270 | ActualColor[1] = color1[1]; 271 | ActualColor[2] = color1[2]; 272 | int i; 273 | 274 | for (i = 0; i < HowManySteps; i++) { 275 | 276 | ActualColor[0] += (color2[0] - color1[0]) / HowManySteps; 277 | ActualColor[1] += (color2[1] - color1[1]) / HowManySteps; 278 | ActualColor[2] += (color2[2] - color1[2]) / HowManySteps; 279 | 280 | DrawThickEdgedTransparentShape(x, y, 281 | width + i * EdgeThickness / HowManySteps, 282 | height + i * EdgeThickness / HowManySteps, 283 | EdgeThickness / HowManySteps, angle, ActualColor); 284 | } 285 | 286 | } 287 | 288 | /* Draws a polygon with "NumberOfVertices" vertices and given edge thickness */ 289 | void DrawThickEdgedPolygon(double *Vertices, const int NumberOfVertices, 290 | const double EdgeThickness, const float *color) { 291 | 292 | int i; 293 | 294 | for (i = 0; i < NumberOfVertices; i++) { 295 | DrawLine(Vertices[((i) % NumberOfVertices) * 3], 296 | Vertices[((i) % NumberOfVertices) * 3 + 1], 297 | Vertices[(i + 1) % NumberOfVertices * 3], 298 | Vertices[(i + 1) % NumberOfVertices * 3 + 1], EdgeThickness, 299 | color); 300 | DrawFullCircle(Vertices[((i) % NumberOfVertices) * 3], 301 | Vertices[((i) % NumberOfVertices) * 3 + 1], EdgeThickness / 2.0, 302 | color); 303 | } 304 | 305 | } 306 | 307 | /* Draws a polygon with "NumberOfVertices" vertices and given edge thickness */ 308 | void DrawGradientColoredPolygon(double *Vertices, 309 | const int NumberOfVertices, 310 | const double EdgeThickness, 311 | const float *color1, const float *color2, const int HowManySteps) { 312 | 313 | //HARDCORE TODO!!! 314 | 315 | int i, j; 316 | 317 | for (i = 1; i < NumberOfVertices + 1; i++) { 318 | 319 | ActualColor[0] = color1[0]; 320 | ActualColor[1] = color1[1]; 321 | ActualColor[2] = color1[2]; 322 | 323 | for (j = 0; j < HowManySteps; j++) { 324 | 325 | ActualColor[0] += (color2[0] - color1[0]) / HowManySteps; 326 | ActualColor[1] += (color2[1] - color1[1]) / HowManySteps; 327 | ActualColor[2] += (color2[2] - color1[2]) / HowManySteps; 328 | 329 | DrawLine(Vertices[((i - 1) % NumberOfVertices) * 3] + 330 | j * EdgeThickness / HowManySteps, 331 | Vertices[((i - 1) % NumberOfVertices) * 3 + 1] - 332 | j * EdgeThickness / HowManySteps, 333 | Vertices[i % NumberOfVertices * 3] + 334 | j * EdgeThickness / HowManySteps, 335 | Vertices[i % NumberOfVertices * 3 + 1] - 336 | j * EdgeThickness / HowManySteps, 337 | EdgeThickness / HowManySteps, ActualColor); 338 | 339 | DrawLine(Vertices[((i - 1) % NumberOfVertices) * 3] - 340 | j * EdgeThickness / HowManySteps, 341 | Vertices[((i - 1) % NumberOfVertices) * 3 + 1] + 342 | j * EdgeThickness / HowManySteps, 343 | Vertices[i % NumberOfVertices * 3] - 344 | j * EdgeThickness / HowManySteps, 345 | Vertices[i % NumberOfVertices * 3 + 1] + 346 | j * EdgeThickness / HowManySteps, 347 | EdgeThickness / HowManySteps, ActualColor); 348 | 349 | } 350 | } 351 | 352 | } 353 | 354 | /* Draws a string with given label, font and position */ 355 | void DrawString(float x, float y, void *font, const char *string, 356 | const float *color) { 357 | 358 | glColor3f(color[0], color[1], color[2]); 359 | glRasterPos2f(x, y); 360 | 361 | glutBitmapString(font, (const unsigned char *) string); 362 | 363 | } 364 | 365 | /* Draws a Lengthscale wth given Length at a given position */ 366 | void DrawScale(double Length, double x, double y, double MapSizexy, 367 | const float *color) { 368 | 369 | glColor3f(color[0], color[1], color[2]); 370 | 371 | glBegin(GL_LINES); 372 | 373 | glVertex3f(x, y - 0.05, 1.0); 374 | glVertex3f(x, y, 1.0); 375 | glVertex3f(x, y - 0.025, 1.0); 376 | glVertex3f(x + RealToGlCoord_2D(Length, MapSizexy), y - 0.025, 1.0); 377 | glVertex3f(x + RealToGlCoord_2D(Length, MapSizexy), y, 1.0); 378 | glVertex3f(x + RealToGlCoord_2D(Length, MapSizexy), y - 0.05, 1.0); 379 | 380 | glEnd(); 381 | 382 | /* Length of the scale in real coordinates (in metres) */ 383 | static char ScaleLength[10]; 384 | sprintf(ScaleLength, "%1.1f m", Length / 100.0); 385 | DrawString(x, y + 0.0125, GLUT_BITMAP_TIMES_ROMAN_10, ScaleLength, color); 386 | 387 | } 388 | 389 | /* Utilities */ 390 | 391 | /* Converts "real" coordinates into GL coordinates */ 392 | double RealToGlCoord_2D(double coord, double MapSizexy) { 393 | 394 | return coord / MapSizexy; 395 | 396 | } 397 | 398 | /* Converts "mouse" coordinates to real coords */ 399 | double MouseCoordToReal_2D(int mouseCoord, double MapSizexy, 400 | const double Resolution) { 401 | 402 | /* 300 and 600 are properties of the "vizualizer" window */ 403 | return MapSizexy * (2.0 * (double) (mouseCoord - 404 | 0.5 * Resolution) / Resolution); 405 | 406 | } 407 | -------------------------------------------------------------------------------- /src/vizualizer/objects_2d.h: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* Tools for drawing objects in 2D visualization 7 | */ 8 | 9 | #ifndef OBJECTS_2D_H 10 | #define OBJECTS_2D_H 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | /* Windows and other general tools */ 20 | 21 | /* Displays a window with given width and height at (posX, posY) coordinates 22 | * "width", "height", "posX" and "posY" are in pixels. 23 | */ 24 | void DisplayWindow(int width, int height, int posX, int posY); 25 | 26 | /* Simple euclidean objects in GL coords */ 27 | 28 | /* Draws a shape with given center coordinates (in GL coords), width, height and rotation angle 29 | * "CenterX" and "CenterY" are the coordinates of the center of the shape 30 | * "color" is an array that contains RGB values 31 | */ 32 | void DrawShape(double CenterX, double CenterY, double width, double height, 33 | double angle, const float *color); 34 | 35 | /* Draws a line with given endpoint coordinates (in GL coords). 36 | * (x1, y1) and (x2, y2) are the endpoint coordinates 37 | * "width" is the width of the line 38 | * "color" is an array that contains RGB values 39 | */ 40 | void DrawLine(double x1, double y1, double x2, double y2, double width, 41 | const float *color); 42 | 43 | /* Draws an empty tetragon with given vertices (?) 44 | * (x1, y1) , (x2, y2) , (x3, y3) , (x4, y4) are the vertices of the tetragon 45 | * "width" is the width of the edges 46 | * "color" is an array that contains RGB values 47 | */ 48 | void DrawTetragon(double x1, double y1, double x2, 49 | double y2, double x3, double y3, 50 | double x4, double y4, double width, const float *color); 51 | 52 | /* Draws a fulle tetragon with given vertices (?) 53 | * (x1, y1) , (x2, y2) , (x3, y3) , (x4, y4) are the vertices of the tetragon 54 | * "color" is an array that contains RGB values 55 | */ 56 | void DrawFullTetragon(double x1, double y1, double x2, double y2, double x3, 57 | double y3, double x4, double y4, const float *color); 58 | 59 | /* Draws a trinagle (platonic, if the windth and height of the window are equal) 60 | * "CenterX" and "CenterY" are the GL coordinates of the center of the tirangle 61 | * "LenghtOfEdge" defines the size of the triangle (relative to the window) 62 | * The final triangle will be rotated with "Angle" 63 | * "color" is an array that contains RGB values 64 | */ 65 | void DrawTriangle(double CenterX, double CenterY, double LengthOfEdge, 66 | double Angle, const float *color); 67 | 68 | /* Draws an empty circle with given center coordinates (GL coords) and radius. 69 | * "color" is an array that contains RGB values 70 | */ 71 | void DrawCircle(double Centerx, double Centery, double radius, 72 | const float *color); 73 | 74 | /* Draws an empty circle with thick edges with given center coordinates (GL coord) and radius 75 | * "color" is an array that contains RGB values 76 | */ 77 | void DrawThickEdgedCircle(double Centerx, double Centery, double radius, 78 | double EdgeThickness, const float *color); 79 | 80 | /* Draws an empty circle with gradient-colored edges 81 | */ 82 | void DrawGradientColoredCircle(const double Centerx, const double Centery, 83 | const double radius, const double EdgeThickness, const float *color1, 84 | const float *color2, const int HowManySteps); 85 | 86 | /* Draws a full circle with given center coordinates (GL coords) and radius. 87 | * "color" is an array that contains RGB values 88 | */ 89 | void DrawFullCircle(double Centerx, double Centery, double radius, 90 | const float *color); 91 | 92 | /* Draws an arrow with given center coordinates ("x" and "y") and Length ("Length") 93 | * The final arrow will be rotated by "angle" 94 | * The width of the arrow will be Length/4 95 | * "color" is an array that contains RGB values 96 | */ 97 | void DrawArrow(double x, double y, double Length, double angle, 98 | const float *color); 99 | 100 | /* Draws an arrow with given center coordinates ("x" and "y") and Length ("Length") 101 | * "TriangleEdgeLength" defines the size of the head of the arrow 102 | * The final arrow will be rotated with "angle". 103 | * The width of the arrow will be 0.003 in GL coords 104 | * "color" is an array that contains RGB values 105 | */ 106 | void DrawThinArrow(double x, double y, double Length, double TriangleEdgeLength, 107 | double angle, const float *color); 108 | 109 | /* Draws a transparent (empty) shape. 110 | * x, y, width and height are in GL coordinates 111 | * "color" is an array that contains RGB values 112 | */ 113 | void DrawTransparentShape(double x, double y, double width, double height, 114 | const float *color); 115 | 116 | /* Draws a polygon with "NumberOfVertices" vertices 117 | */ 118 | void DrawPolygon(float *Vertices, const int NumberOfVertices, 119 | const float *color); 120 | 121 | /* Draws a transparent (empty) shape with given edge thickness. 122 | * EdgeThickness, x, y, width and height are in GL coordinates 123 | * "color" is an array that contains RGB values 124 | */ 125 | void DrawThickEdgedTransparentShape(const double x, const double y, 126 | const double width, const double height, const double EdgeThickness, 127 | double angle, const float *color); 128 | 129 | /* Draws a gradient-colored transparent shape with given edge thickness 130 | */ 131 | void DrawGradientColoredShape(const double x, const double y, 132 | const double width, const double height, const double EdgeThickness, 133 | double angle, const float *color1, const float *color2, 134 | const int HowManySteps); 135 | 136 | /* Draws a polygon with "NumberOfVertices" vertices and given edge thickness 137 | */ 138 | void DrawThickEdgedPolygon(double *Vertices, const int NumberOfVertices, 139 | const double EdgeThickness, const float *color); 140 | 141 | /* Draws a string with given label, font and position. 142 | * "color" is an array that contains RGB values 143 | * "font" should be chosen from GLUT font types 144 | */ 145 | void DrawString(float x, float y, void *font, const char *string, 146 | const float *color); 147 | 148 | /* Draws a Lengthscale wth given Length at a given position 149 | * "MapSizexy" are in real coordinates 150 | * "Length", "x" and "y" are in GL coordinates 151 | */ 152 | void DrawScale(double Length, double x, double y, double MapSizexy, 153 | const float *color); 154 | 155 | /* Utilities */ 156 | 157 | /* Converts "real" coordinates into GL coordinates 158 | * "MapSizexy" and "coord" are in real coordinates (cm) 159 | */ 160 | double RealToGlCoord_2D(double coord, double MapSizexy); 161 | 162 | /* Converts "mouse" coordinates to real coords. 163 | * mouseCoord is in pixels (position of the mouse) 164 | * MapSizexy is the size of the map (in real coordinates, cm) 165 | * Only works when the map is a rectangle 166 | */ 167 | double MouseCoordToReal_2D(int mouseCoord, const double MapSizexy, 168 | const double Resolution); 169 | 170 | #endif 171 | -------------------------------------------------------------------------------- /src/vizualizer/objects_3d.c: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* 7 | * Tools for drawing 3D objects 8 | */ 9 | 10 | #include "objects_3d.h" 11 | 12 | #include 13 | 14 | /* Sphere */ 15 | void DrawSolidSphere_3D(const float x, const float y, const float z, 16 | const float r, const float *color) { 17 | 18 | glTranslatef(x, y, z); 19 | glColor3f(color[0], color[1], color[2]); 20 | glutSolidSphere(r, 30, 30); 21 | glTranslatef(-x, -y, -z); 22 | 23 | } 24 | void DrawWireSphere_3D(const float x, const float y, const float z, 25 | const float r, const float *color) { 26 | 27 | glTranslatef(x, y, z); 28 | glColor3f(color[0], color[1], color[2]); 29 | glutWireSphere(r, 30, 30); 30 | glTranslatef(-x, -y, -z); 31 | 32 | } 33 | 34 | /* Tetrahedron */ 35 | void DrawSolidTetrahedron_3D(const float x, 36 | const float y, const float z, const float r, const float *color) { 37 | 38 | glTranslatef(x, y, z); 39 | glColor3f(0.0, 0.0, 0.0); 40 | glScalef(r, r, r); 41 | glutWireTetrahedron(); 42 | glColor3f(color[0], color[1], color[2]); 43 | glutSolidTetrahedron(); 44 | glScalef(1.0 / r, 1.0 / r, 1.0 / r); 45 | glTranslatef(-x, -y, -z); 46 | 47 | } 48 | 49 | /* Tetrahedron */ 50 | void DrawWiredTetrahedron_3D(const float x, 51 | const float y, 52 | const float z, const float r, const float width, const float *color) { 53 | 54 | glTranslatef(x, y, z); 55 | glColor3f(0.0, 0.0, 0.0); 56 | glScalef(r, r, r); 57 | glLineWidth(width); 58 | glutWireTetrahedron(); 59 | glLineWidth(1.0); 60 | glColor3f(color[0], color[1], color[2]); 61 | //glutSolidTetrahedron (); 62 | glScalef(1.0 / r, 1.0 / r, 1.0 / r); 63 | glTranslatef(-x, -y, -z); 64 | 65 | } 66 | 67 | /* Line */ 68 | void DrawLine_3D(const float x1, 69 | const float y1, 70 | const float z1, 71 | const float x2, 72 | const float y2, const float z2, const float width, const float *color) { 73 | 74 | /* Setting up color */ 75 | glColor3f(color[0], color[1], color[2]); 76 | 77 | /* Setting up width of the line (in GL coordinates) */ 78 | glLineWidth(width); 79 | 80 | /* Draw line */ 81 | glBegin(GL_LINES); 82 | glVertex3f(x1, y1, z1); 83 | glVertex3f(x2, y2, z2); 84 | glEnd(); 85 | 86 | /* Reset previous line width */ 87 | glLineWidth(1.0); 88 | 89 | } 90 | 91 | /* Circle */ 92 | void DrawCircle_3D(const float x, const float y, const float z, 93 | const float radius, 94 | const float width, double *NormalVect, const float *color) { 95 | 96 | // STOP READING. NOW! (This code is not optimal... ) 97 | 98 | glColor3f(color[0], color[1], color[2]); 99 | double i; 100 | int j, k; 101 | 102 | /* Normalizing normal vect, if it is necessary */ 103 | static double Magnitude; 104 | Magnitude = 105 | sqrt(NormalVect[0] * NormalVect[0] + NormalVect[1] * NormalVect[1] + 106 | NormalVect[2] * NormalVect[2]); 107 | if (Magnitude != 0.0 && Magnitude != 1.0) { 108 | for (j = 0; j < 3; j++) { 109 | NormalVect[j] /= Magnitude; 110 | } 111 | } 112 | 113 | /* Circle equals several lines connected to an N-gon, where N is large enough */ 114 | static double ToSideVect[3]; 115 | static double Temp[3]; 116 | static const double a = 0.2, b = 0.6; 117 | static const double sinAngle = 0.009999833334166664; // sin (0.01) 118 | static const double cosAngle = 0.999950000416665300; // cos (0.01) 119 | 120 | /* Creating a normalized perpendicular vector */ 121 | ToSideVect[0] = NormalVect[1] * a - NormalVect[2] * b; 122 | ToSideVect[1] = -NormalVect[0] * a; 123 | ToSideVect[2] = NormalVect[0] * b; 124 | 125 | Magnitude = 126 | sqrt(ToSideVect[0] * ToSideVect[0] + ToSideVect[1] * ToSideVect[1] + 127 | ToSideVect[2] * ToSideVect[2]); 128 | 129 | for (j = 0; j < 3; j++) { 130 | ToSideVect[j] /= Magnitude; 131 | } 132 | 133 | glLineWidth(width); 134 | 135 | glBegin(GL_LINES); 136 | for (i = 0; i < 2 * M_PI; i += 0.01) { 137 | 138 | glVertex3f(x + radius * ToSideVect[0], y + radius * ToSideVect[1], 139 | z + radius * ToSideVect[2]); 140 | 141 | memcpy(Temp, ToSideVect, 3 * sizeof(double)); 142 | ToSideVect[0] = 143 | (cosAngle + NormalVect[0] * NormalVect[0] * (1 - 144 | cosAngle)) * Temp[0] 145 | + (NormalVect[0] * NormalVect[1] * (1 - cosAngle) - 146 | NormalVect[2] * sinAngle) * Temp[1] 147 | + (NormalVect[0] * NormalVect[2] * (1 - cosAngle) + 148 | NormalVect[1] * sinAngle) * Temp[2]; 149 | ToSideVect[1] = 150 | (NormalVect[0] * NormalVect[1] * (1 - cosAngle) + 151 | NormalVect[2] * sinAngle) * Temp[0] 152 | + (cosAngle + NormalVect[1] * NormalVect[1] * (1 - 153 | cosAngle)) * Temp[1] 154 | + (NormalVect[1] * NormalVect[2] * (1 - cosAngle) - 155 | NormalVect[0] * sinAngle) * Temp[2]; 156 | ToSideVect[2] = 157 | (NormalVect[2] * NormalVect[0] * (1 - cosAngle) - 158 | NormalVect[1] * sinAngle) * Temp[0] 159 | + (NormalVect[2] * NormalVect[1] * (1 - cosAngle) + 160 | NormalVect[0] * sinAngle) * Temp[1] 161 | + (cosAngle + NormalVect[2] * NormalVect[2] * (1 - 162 | cosAngle)) * Temp[2]; 163 | 164 | } 165 | glEnd(); 166 | 167 | glLineWidth(1.0); 168 | 169 | } 170 | 171 | void DrawPrism_3D(double *Edges_On_XY_Plane, 172 | const int NumberOfVertices, 173 | const float height, const float WireWidth, const float *color) { 174 | 175 | int i; 176 | 177 | glColor3f(color[0], color[1], color[2]); 178 | glLineWidth(WireWidth); 179 | 180 | glBegin(GL_LINES); 181 | for (i = 1; i < NumberOfVertices + 1; i++) { 182 | glVertex3f(Edges_On_XY_Plane[((i - 1) % NumberOfVertices) * 3], 183 | Edges_On_XY_Plane[((i - 1) % NumberOfVertices) * 3 + 1], 184 | Edges_On_XY_Plane[((i - 1) % NumberOfVertices) * 3 + 2]); 185 | glVertex3f(Edges_On_XY_Plane[(i % NumberOfVertices) * 3], 186 | Edges_On_XY_Plane[(i % NumberOfVertices) * 3 + 1], 187 | Edges_On_XY_Plane[(i % NumberOfVertices) * 3 + 2]); 188 | glVertex3f(Edges_On_XY_Plane[((i - 1) % NumberOfVertices) * 3], 189 | Edges_On_XY_Plane[((i - 1) % NumberOfVertices) * 3 + 1], 190 | Edges_On_XY_Plane[((i - 1) % NumberOfVertices) * 3 + 2] + 191 | height); 192 | glVertex3f(Edges_On_XY_Plane[(i % NumberOfVertices) * 3], 193 | Edges_On_XY_Plane[(i % NumberOfVertices) * 3 + 1], 194 | Edges_On_XY_Plane[(i % NumberOfVertices) * 3 + 2] + height); 195 | glVertex3f(Edges_On_XY_Plane[((i - 1) % NumberOfVertices) * 3], 196 | Edges_On_XY_Plane[((i - 1) % NumberOfVertices) * 3 + 1], 197 | Edges_On_XY_Plane[((i - 1) % NumberOfVertices) * 3 + 2]); 198 | glVertex3f(Edges_On_XY_Plane[((i - 1) % NumberOfVertices) * 3], 199 | Edges_On_XY_Plane[((i - 1) % NumberOfVertices) * 3 + 1], 200 | Edges_On_XY_Plane[((i - 1) % NumberOfVertices) * 3 + 2] + 201 | height); 202 | } 203 | glEnd(); 204 | 205 | glLineWidth(1.0); 206 | 207 | } 208 | 209 | void DrawCylinder_3D(const float x_center, 210 | const float y_center, 211 | const float z_center, 212 | const float radius, 213 | const float height, const float WireWidth, const float *color) { 214 | 215 | float i; 216 | 217 | glColor3f(color[0], color[1], color[2]); 218 | glLineWidth(WireWidth); 219 | 220 | /* Floor */ 221 | glBegin(GL_LINE_STRIP); 222 | for (i = 0.0; i < 2 * M_PI; i += 0.01) { 223 | glVertex3f(x_center + radius * cos(i), -(-y_center + radius * sin(i)), 224 | z_center); 225 | } 226 | glEnd(); 227 | 228 | /* Roof */ 229 | glBegin(GL_LINE_STRIP); 230 | for (i = 0.0; i < 2 * M_PI; i += 0.01) { 231 | glVertex3f(x_center + radius * cos(i), -(-y_center + radius * sin(i)), 232 | z_center + height); 233 | } 234 | glEnd(); 235 | 236 | /* 16 Pillars */ 237 | glBegin(GL_LINES); 238 | for (i = 0; i < 2 * M_PI; i += 2 * M_PI / 16) { 239 | glVertex3f(x_center + radius * cos(i), -(-y_center + radius * sin(i)), 240 | z_center); 241 | glVertex3f(x_center + radius * cos(i), -(-y_center + radius * sin(i)), 242 | z_center + height); 243 | } 244 | glEnd(); 245 | 246 | glLineWidth(1.0); 247 | 248 | } 249 | 250 | /* Rectangle */ 251 | void DrawHorizontalRectangle_3D(const float x, 252 | const float y, 253 | const float z, 254 | const float width, const float height, const float *color) { 255 | 256 | } 257 | 258 | /* A room with floor and transparent walls */ 259 | void DrawCubicRoom_3D(const float x, 260 | const float y, const float z, const float L, const float tiledensity) { 261 | 262 | /* Lines */ 263 | glBegin(GL_LINES); 264 | 265 | glVertex3f(x - L / 2, y - L / 2, z - L / 2); 266 | glVertex3f(x - L / 2, y - L / 2, z + L / 2); 267 | glVertex3f(x - L / 2, y + L / 2, z - L / 2); 268 | glVertex3f(x - L / 2, y + L / 2, z + L / 2); 269 | glVertex3f(x + L / 2, y - L / 2, z - L / 2); 270 | glVertex3f(x + L / 2, y - L / 2, z + L / 2); 271 | glVertex3f(x + L / 2, y + L / 2, z - L / 2); 272 | glVertex3f(x + L / 2, y + L / 2, z + L / 2); 273 | glVertex3f(x - L / 2, y - L / 2, z - L / 2); 274 | glVertex3f(x - L / 2, y - L / 2, z + L / 2); 275 | glVertex3f(x - L / 2, y + L / 2, z - L / 2); 276 | glVertex3f(x - L / 2, y + L / 2, z + L / 2); 277 | 278 | glEnd(); 279 | 280 | } 281 | 282 | void DrawCopter_3D(const double x, const double y, const double z, 283 | const double MapSizexy, const double Radius, const float *color) { 284 | 285 | DrawSolidSphere_3D(RealToGlCoord_3D(x, MapSizexy), RealToGlCoord_3D(y, 286 | MapSizexy), RealToGlCoord_3D(z, MapSizexy), 287 | RealToGlCoord_3D(Radius / 2.0, MapSizexy), color); 288 | 289 | } 290 | 291 | void DrawAgentLabel_3D(double **Phase, 292 | const int WhichAgent, 293 | char *Label, 294 | bool Display, 295 | double CenterX, double CenterY, double MapSizeXY, const float *Color) { 296 | 297 | } 298 | 299 | /* Utilities */ 300 | 301 | double RealToGlCoord_3D(const double coord, const double MapSizexy) { 302 | 303 | return coord / MapSizexy; 304 | 305 | } 306 | -------------------------------------------------------------------------------- /src/vizualizer/objects_3d.h: -------------------------------------------------------------------------------- 1 | //MIT License 2 | //Copyright (c) 2018 Eotvos Lorand University, Budapest 3 | 4 | /* vim:set ts=4 sw=4 sts=4 et: */ 5 | 6 | /* Tools for drawing objects in 3D visualization 7 | */ 8 | 9 | #ifndef OBJECTS_3D_H 10 | #define OBJECTS_3D_H 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | void DrawSolidSphere_3D(const float x, 22 | const float y, const float z, const float r, const float *color); 23 | 24 | void DrawWireSphere_3D(const float x, 25 | const float y, const float z, const float r, const float *color); 26 | 27 | void DrawSolidTetrahedron_3D(const float x, 28 | const float y, const float z, const float r, const float *color); 29 | 30 | void DrawWiredTetrahedron_3D(const float x, 31 | const float y, 32 | const float z, const float r, const float width, const float *color); 33 | 34 | void DrawLine_3D(const float x1, 35 | const float y1, 36 | const float z1, 37 | const float x2, 38 | const float y2, const float z2, const float width, const float *color); 39 | 40 | void DrawCircle_3D(const float x, const float y, const float z, 41 | const float radius, 42 | const float width, double *NormalVect, const float *color); 43 | 44 | void DrawHorizontalRectangle_3D(const float x, 45 | const float y, 46 | const float z, 47 | const float width, const float height, const float *color); 48 | 49 | void DrawPrism_3D(double *Edges_On_XY_Plane, 50 | const int NumberOfVertices, 51 | const float height, const float WireWidth, const float *color); 52 | 53 | void DrawCylinder_3D(const float x_center, 54 | const float y_center, 55 | const float z_center, 56 | const float radius, 57 | const float height, const float WireWidth, const float *color); 58 | 59 | void DrawCopter_3D(double x, 60 | double y, 61 | double z, 62 | const double MapSizexy, const double Radius, const float *color); 63 | 64 | void DrawAgentLabel_3D(double **Phase, 65 | const int WhichAgent, 66 | char *Label, 67 | bool Display, 68 | double CenterX, double CenterY, double MapSizeXY, const float *Color); 69 | 70 | double RealToGlCoord_3D(const double coord, const double MapSizexy); 71 | 72 | #endif 73 | -------------------------------------------------------------------------------- /tools/install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # This is an .sh script for installing necessary tools for running "robotsim" 4 | 5 | echo "Installing freeglut... Please stand by" 6 | sudo apt-get install freeglut3-dev 7 | 8 | echo "Installing devil package... Please stand by" 9 | sudo apt-get install libdevil-dev 10 | 11 | echo "Installing python... Please stand by" 12 | sudo apt-get install python 13 | --------------------------------------------------------------------------------