diff --git a/projet/AUTEURS b/projet/AUTEURS new file mode 100644 index 0000000..8ba2ad2 --- /dev/null +++ b/projet/AUTEURS @@ -0,0 +1,2 @@ +nom1,prenom1,email1 +nom2,prenom2,email2 diff --git a/projet/Makefile b/projet/Makefile new file mode 100644 index 0000000..9daede8 --- /dev/null +++ b/projet/Makefile @@ -0,0 +1,73 @@ +include Makefile.version +ASSET_DIR_PATH="\"$(shell pwd)/assets\"" +CC?=gcc +CFLAGS=-Wall `pkg-config --cflags sdl2` -I `heptc -where`/c \ + -D ASSET_DIR_PATH=$(ASSET_DIR_PATH) -D YEAR=$(ANNEE) \ + -D VERSION=$(VERSION) -g -fsanitize=undefined +LDFLAGS=`pkg-config --libs sdl2` -lm -fsanitize=undefined +HEPTC?=heptc + +HEPT_OBJ=\ + src/challenge.o \ + src/city.o \ + src/globals.o \ + src/vehicle.o \ + src/control.o \ + src/utilities.o \ + src/challenge_types.o \ + src/city_types.o \ + src/globals_types.o \ + src/vehicle_types.o \ + src/control_types.o \ + src/utilities_types.o +OBJ=$(HEPT_OBJ) \ + src/buffer.o \ + src/trace_lib.o \ + src/trace.o \ + src/debug.o \ + src/mathext.o \ + src/map.o \ + src/cutils.o \ + src/simulation_loop.o \ + src/challenge.o \ + src/main.o +TARGET=scontest + +.SUFFIXES: +.PHONY: all clean test +.PRECIOUS: %.epci %.c %.h +.SUFFIXES: + +all: $(TARGET) + +clean: + rm -f $(OBJ) $(TARGET) + rm -f $(foreach ext, mls obc epci epo log, $(wildcard src/*.$(ext))) + rm -rf src/*_c + rm -f $(subst .o,.c,$(HEPT_OBJ)) + rm -f $(subst .o,.h,$(HEPT_OBJ)) + +test: $(TARGET) + ./$< -o logs.txt assets/00.map + +$(TARGET): $(OBJ) + $(CC) $^ $(LDFLAGS) -o $@ + +%.o: %.c + $(CC) $(CFLAGS) -c $< -o $@ + +%_types.c %.c %.h %.epci : %.ept src/mathext.epci src/debug.epci src/trace.epci + cd `dirname $<` && $(HEPTC) -c -target c `basename $<` + cp $(foreach ext,c h,$(basename $<)_c/*.$(ext)) src + +%.epci: %.epi + cd `dirname $<` && $(HEPTC) `basename $<` + +src/vehicle.epci: src/globals.epci src/utilities.epci src/control.epci +src/utilities.epci: src/globals.epci +src/control.epci: src/globals.epci src/utilities.epci +src/city.epci: src/globals.epci src/utilities.epci src/vehicle.epci src/map.epci +src/map.epci: src/map.epi src/globals.epci +src/challenge.epci: src/globals.epci src/vehicle.epci src/city.epci src/map.epci +src/main.o src/map.o: src/globals.h src/globals_types.h +src/simulation_loop.o: src/globals.h src/globals_types.h src/challenge.h diff --git a/projet/Makefile.version b/projet/Makefile.version new file mode 100644 index 0000000..b49fd7d --- /dev/null +++ b/projet/Makefile.version @@ -0,0 +1,2 @@ +VERSION="\"1.1.0\"" +ANNEE="\"2025-2026\"" diff --git a/projet/README.md b/projet/README.md new file mode 100644 index 0000000..3a22b69 --- /dev/null +++ b/projet/README.md @@ -0,0 +1,6 @@ +# Projet de Programmation synchrone + +Ce répertoire contient le projet du cours de Programmation synchrone du M2 +Informatique de l'Université de Paris et de l'École d'Ingénieur Denis Diderot. + +Le [sujet](sujet/sujet-projet.pdf) contient tous les détails. diff --git a/projet/assets/00.iti b/projet/assets/00.iti new file mode 100644 index 0000000..dff51f0 --- /dev/null +++ b/projet/assets/00.iti @@ -0,0 +1,2 @@ +go 20 +stop 0. diff --git a/projet/assets/00.map b/projet/assets/00.map new file mode 100644 index 0000000..db875e1 --- /dev/null +++ b/projet/assets/00.map @@ -0,0 +1,17 @@ +map "oneway_line" +graphics "map_00_oneway_line.bmp" +guide "b-00.bmp" +init 50. 50. 23.703 +rd 1 +line 1 30 50. 50. 550. 250. +wp 1 +0 550. 250. +tl 0 +st 0 +obst 2 +30. 30. 1. 10. +200. 200. 8. 30. +iti 2 +go 20. +stop +end diff --git a/projet/assets/01.iti b/projet/assets/01.iti new file mode 100644 index 0000000..24ce11a --- /dev/null +++ b/projet/assets/01.iti @@ -0,0 +1,8 @@ +go 20 +turn 90. +go 20. +turn 90. +go 20. +turn 90. +go 20. +stop 0. diff --git a/projet/assets/01.map b/projet/assets/01.map new file mode 100644 index 0000000..6adcb12 --- /dev/null +++ b/projet/assets/01.map @@ -0,0 +1,30 @@ +map "oneway_square" +graphics "map_01_oneway_square.bmp" +guide "b-01.bmp" +init 550. 200. 90. +rd 4 +line 1 30 50. 50. 550. 50. +line 1 20 550. 50. 550. 250. +line 1 30 550. 250. 50. 250. +line 1 20 50. 250. 50. 50. +wp 5 +0 548. 50. +1 550. 148. +1 550. 248. +2 52. 250. +3 50. 52. +tl 0 +st 0 +obst 2 + 30. 30. 1. 10. +200. 200. 8. 30. +iti 8 +go 20 +turn 90. +go 20. +turn 90. +go 20. +turn 90. +go 20. +stop 0. +end diff --git a/projet/assets/02.iti b/projet/assets/02.iti new file mode 100644 index 0000000..dff51f0 --- /dev/null +++ b/projet/assets/02.iti @@ -0,0 +1,2 @@ +go 20 +stop 0. diff --git a/projet/assets/02.map b/projet/assets/02.map new file mode 100644 index 0000000..d68656d --- /dev/null +++ b/projet/assets/02.map @@ -0,0 +1,20 @@ +map "oneway_olympic" +graphics "map_02_oneway_olympic.bmp" +guide "b-02.bmp" +init 480. 100. 0. +rd 4 +line 1 30 100. 100. 500. 100. +arc 1 20 500. 150. 50. -90. 90. +line 1 30 500. 200. 100. 200. +arc 1 20 100. 150. 50. 90. 270. +wp 1 +0 449. 100. +tl 0 +st 0 +obst 2 + 30. 30. 1. 10. +200. 200. 8. 30. +iti 2 +go 20 +stop 0. +end diff --git a/projet/assets/03.iti b/projet/assets/03.iti new file mode 100644 index 0000000..a580af3 --- /dev/null +++ b/projet/assets/03.iti @@ -0,0 +1,5 @@ +go 20 +go 15. +turn -44. +go 25. +stop 0. diff --git a/projet/assets/03.map b/projet/assets/03.map new file mode 100644 index 0000000..d8dd309 --- /dev/null +++ b/projet/assets/03.map @@ -0,0 +1,29 @@ +map "oneway_ea" +graphics "map_03_oneway_ea.bmp" +guide "b-03.bmp" +init 20. 20. 35. +rd 5 +line 1 20 5. 5. 100. 100. +line 1 20 100. 100. 160. 130. +line 1 20 160. 130. 230. 130. +arc 1 20 230. 230. 100. -90. 0. +line 1 20 230. 130. 330. 30. +wp 4 +0 100. 100. +2 229. 130. +3 330. 230. +4 329. 31. +tl 2 +0 60. 50. 5 3 5 7 +2 220. 120. 5 3 5 10 +st 2 +0 0 50. 50. +2 1 220. 130. +obst 0 +iti 5 +go 20 +go 15. +turn -44. +go 25. +stop 0. +end diff --git a/projet/assets/04.iti b/projet/assets/04.iti new file mode 100644 index 0000000..e1818ab --- /dev/null +++ b/projet/assets/04.iti @@ -0,0 +1,6 @@ +go 10 +turn 90. +go 20. +turn 90. +go 10. +stop 0. diff --git a/projet/assets/04.map b/projet/assets/04.map new file mode 100644 index 0000000..3464543 --- /dev/null +++ b/projet/assets/04.map @@ -0,0 +1,27 @@ +map "olympic_cut" +graphics "map_04_olympic_cut.bmp" +guide "b-04.bmp" +init 10. 150. 0. +rd 5 +line 1 10 5. 150. 500. 150. +arc 1 20 100. 150. 50. 270. 90. +line 1 30 100. 200. 400. 200. +arc 1 20 400. 150. 50. 90. -90. +line 1 30 400. 100. 100. 100. +wp 4 +0 48. 150. +0 448. 150. +0 498. 150. +3 449. 152. +tl 0 +st 0 +obst 0 +iti 6 +go 10 +turn 90. +go 20. +turn 90. +go 10. +stop 0. +end + diff --git a/projet/assets/05.iti b/projet/assets/05.iti new file mode 100644 index 0000000..4882b8e --- /dev/null +++ b/projet/assets/05.iti @@ -0,0 +1,13 @@ +go 10 +turn 90. +go 20. +turn -90. +go 20. +go 30. +turn -90. +go 20 +turn -90. +go 10 +go 10 +go 10 +stop 0. diff --git a/projet/assets/05.map b/projet/assets/05.map new file mode 100644 index 0000000..d4e3814 --- /dev/null +++ b/projet/assets/05.map @@ -0,0 +1,42 @@ +map "olympic_cut_tl" +graphics "map_05_olympic_cut_tl.bmp" +guide "b-05.bmp" +init 10. 150. 0. +rd 6 +line 1 10 5. 150. 500. 150. +arc 1 20 100. 150. 50. 270. 90. +line 1 30 100. 200. 400. 200. +arc 1 20 400. 150. 50. 90. -90. +line 1 30 400. 100. 100. 100. +line 1 20 250. 200. 250. 100. +wp 8 +0 48. 150. +0 448. 150. +0 498. 150. +3 450. 152. +2 248. 200. +5 250. 102. +5 250. 152. +0 248. 150. +tl 2 +5 245. 160. 5 3 5 3 +0 240. 145. 5 3 5 8 +st 2 +5 0 250. 160. +0 1 240. 150. +obst 0 +iti 12 +go 10 +turn 90. +go 20. +turn -90. +go 20. +go 25. +turn -90. +go 20 +turn -90. +go 10 +go 10 +go 10 +stop 0. +end diff --git a/projet/assets/06.iti b/projet/assets/06.iti new file mode 100644 index 0000000..38a7d86 --- /dev/null +++ b/projet/assets/06.iti @@ -0,0 +1,13 @@ +go 20. +turn -90. +go 10. +go 10. +go 10. +turn -90. +go 30. +turn -90. +go 10. +turn 90. +go 25. +stop 0. + diff --git a/projet/assets/06.map b/projet/assets/06.map new file mode 100644 index 0000000..c831b03 --- /dev/null +++ b/projet/assets/06.map @@ -0,0 +1,44 @@ +map "roundabout" +graphics "map_06_roundabout.bmp" +guide "b-06.bmp" +init 60. 150. 0. +rd 10 +line 1 20 50. 150. 100. 150. +arc 1 10 150. 150. 50. 180. 270. +line 1 30 150. 100. 150. 50. +arc 1 10 150. 150. 50. -90. 0. +line 1 30 200. 150. 250. 150. +arc 1 10 150. 150. 50. 0. 90. +line 1 30 150. 200. 150. 250. +arc 1 20 150. 150. 75. 0. -90. +arc 1 20 150. 150. 75. 90. 0. +arc 1 10 150. 150. 50. 90. 180. +wp 11 +0 98. 150. +1 148. 100.4 +2 150. 77. +2 150. 52. +3 199.9 148. +4 248. 150. +5 152. 199.9 +6 150. 223. +6 150. 248. +8 224.8 153. +7 153. 75.2 +tl 0 +st 0 +obst 0 +iti 12 +go 20. +turn -90. +go 10. +go 10. +go 10. +turn -90. +go 25. +turn -90. +go 10. +turn 90. +go 25. +stop 0. +end diff --git a/projet/assets/07.iti b/projet/assets/07.iti new file mode 100644 index 0000000..d03904a --- /dev/null +++ b/projet/assets/07.iti @@ -0,0 +1,13 @@ +go 20. +turn -90. +go 10. +go 10. +go 10. +turn -90. +go 30. +turn -90. +go 10. +turn 90. +go 20. +stop 0. + diff --git a/projet/assets/07.map b/projet/assets/07.map new file mode 100644 index 0000000..25d9f63 --- /dev/null +++ b/projet/assets/07.map @@ -0,0 +1,53 @@ +map "roundabout_tl" +graphics "map_07_roundabout_tl.bmp" +guide "b-07.bmp" +init 60. 150. 0. +rd 10 +line 1 20 50. 150. 100. 150. +arc 1 10 150. 150. 50. 180. 270. +line 1 30 150. 100. 150. 50. +arc 1 10 150. 150. 50. -90. 0. +line 1 30 200. 150. 250. 150. +arc 1 10 150. 150. 50. 0. 90. +line 1 30 150. 200. 150. 250. +arc 1 20 150. 150. 75. 0. -90. +arc 1 20 150. 150. 75. 90. 0. +arc 1 10 150. 150. 50. 90. 180. +wp 11 +0 98. 150. +1 148. 100.4 +2 150. 77. +2 150. 52. +3 199.9 148. +4 223. 150. +4 248. 150. +5 152. 199.9 +6 150. 223. +6 150. 248. +8 224.8 153. +7 153. 75.2 +tl 4 +2 145. 85. 5 3 5 3 +7 165. 70.9 5 3 5 8 +4 215. 145. 5 3 5 3 +8 218.5 164.9 5 3 5 8 +st 4 +2 0 150. 85. +7 1 165. 76.5 +4 2 215. 150. +8 3 223.5 164.9 +obst 0 +iti 12 +go 20. +turn -90. +go 10. +go 10. +go 10. +turn -90. +go 25. +turn -90. +go 10. +turn 90. +go 20. +stop 0. +end diff --git a/projet/assets/08.iti b/projet/assets/08.iti new file mode 100644 index 0000000..146d0f3 --- /dev/null +++ b/projet/assets/08.iti @@ -0,0 +1,12 @@ +go 10. +turn -90. +go 30. +turn 90. +go 20. +go 10. +turn -90. +go 20. +turn 90. +go 25. +go 20. +stop 0. diff --git a/projet/assets/08.map b/projet/assets/08.map new file mode 100644 index 0000000..9e7c5a4 --- /dev/null +++ b/projet/assets/08.map @@ -0,0 +1,72 @@ +map "nycity" +graphics "map_08_nycity.bmp" +guide "b-08.bmp" +init 20. 250. 0. + +rd 20 +line 1 30 5. 250. 50. 250. +line 1 30 50. 250. 150. 250. + +line 1 30 150. 250. 200. 250. +line 1 20 50. 250. 50. 150. + +line 1 20 150. 250. 150. 150. +line 1 20 200. 250. 200. 150. + +line 1 30 50. 150. 150. 150. +line 1 30 150. 150. 200. 150. + +line 1 20 50. 150. 50. 50. +line 1 20 150. 150. 150. 50. + +line 1 20 200. 150. 200. 50. +line 1 30 50. 50. 150. 50. + +line 1 30 150. 50. 200. 50. +line 1 30 250. 50. 275. 50. + +line 1 30 200. 250. 250. 250. +line 1 20 250. 250. 250. 150. + +line 1 30 200. 150. 250. 150. +line 1 20 250. 150. 250. 100. +line 1 20 250. 150. 250. 50. +line 1 30 200. 50. 250. 50. +wp 20 +0 48. 250. +1 148. 250. +2 198. 250. +3 50. 152. +4 150. 152. +5 200. 152. +6 148. 150. +7 198. 150. +8 50. 52. +9 150. 52. +10 200. 52. +11 148. 50. +12 198. 50. +13 273. 50. +14 248. 250. +15 250. 152. +16 248. 150. +17 250. 102. +18 250. 52. +19 248. 50. +tl 0 +st 0 +obst 0 +iti 12 +go 10. +turn -90. +go 25. +turn 90. +go 20. +go 10. +turn -90. +go 20. +turn 90. +go 25. +go 20. +stop 0. +end diff --git a/projet/assets/09.iti b/projet/assets/09.iti new file mode 100644 index 0000000..024a140 --- /dev/null +++ b/projet/assets/09.iti @@ -0,0 +1,14 @@ +go 20. +go 20. +go 20. +go 20. +turn -90. +go 10. +go 20. +turn -90. +go 10. +turn 30. +go 20. +turn 100. +go 10. +stop 0. diff --git a/projet/assets/09.map b/projet/assets/09.map new file mode 100644 index 0000000..15cb926 --- /dev/null +++ b/projet/assets/09.map @@ -0,0 +1,105 @@ +map "paris" +graphics "map_09_paris.bmp" +guide "b-09.bmp" +init 40. 275. 0. + +rd 31 +line 1 30 25. 275. 100. 275. +line 1 20 100. 275. 175. 275. +line 1 30 175. 275. 250. 275. +line 1 20 250. 275. 400. 275. +line 1 30 400. 275. 475. 275. +line 1 20 50. 225. 25. 275. +line 1 20 100. 275. 100. 225. +line 1 20 175. 275. 175. 225. +line 1 20 250. 275. 250. 225. +line 1 20 400. 275. 400. 175. +line 1 30 100. 225. 50. 225. +line 1 30 175. 225. 100. 225. +line 1 20 250. 225. 175. 225. +line 1 20 400. 175. 250. 225. +line 1 30 475. 175. 400. 175. +line 1 20 50. 225. 175. 100. +line 1 30 100. 225. 100. 175. +line 1 30 175. 225. 175. 100. +line 1 30 250. 225. 250. 100. +line 1 30 400. 175. 400. 100. +line 1 20 250. 100. 175. 100. +line 1 20 400. 100. 250. 100. +line 1 30 450. 100. 400. 100. +line 1 20 175. 100. 225. 50. +line 1 20 325. 100. 225. 50. +line 1 20 400. 100. 400. 75. +line 1 20 400. 75. 325. 50. +line 1 20 400. 75. 450. 25. +line 1 30 325. 50. 225. 50. +line 1 10 225. 50. 175. 25. +line 1 10 225. 50. 250. 25. +wp 32 +0 98. 275. +1 173. 275. +2 248. 275. +3 398. 275. +4 473. 275. +5 23. 273. +6 100. 227. +7 175. 227. +8 250. 227. +9 400. 177. +10 52. 225. +11 102. 225. +12 177. 225. +13 260. 223. +14 402. 175. +15 173. 102. +16 100. 177. +17 175. 102. +18 250. 102. +19 400. 102. +20 177. 100. +21 252. 100. +21 327. 100. +22 402. 100. +23 223. 52. +24 227. 51. +25 400. 77. +26 340. 55. +27 448. 27. +28 235. 50. +29 187. 30. +30 240. 35. + +tl 6 +15 165. 105. 5 3 5 3 +17 170. 115. 5 3 5 8 +20 185. 105. 5 3 5 13 +23 215. 55. 5 3 5 3 +24 240. 60. 5 3 5 8 +28 240. 53. 5 3 5 13 + +st 6 +15 0 165. 110. +17 1 175. 115. +20 2 185. 100. +23 3 215. 60. +24 4 245. 60. +28 5 240. 50. + +obst 0 + +iti 14 +go 20. +go 20. +go 20. +go 20. +turn -90. +go 10. +go 20. +turn -90. +go 10. +turn 30. +go 20. +turn 100. +go 10. +stop 0. +end diff --git a/projet/assets/11.iti b/projet/assets/11.iti new file mode 100644 index 0000000..b1ce810 --- /dev/null +++ b/projet/assets/11.iti @@ -0,0 +1,20 @@ +go 30. +turn 90. +go 30. +turn 45. +go 30. +turn -45. +go 30. +turn -70. +go 20. +turn 120. +go 20. +turn -110. +go 20. +turn -45. +go 30. +turn -45. +go 30. +turn 135. +go 20. +stop 0. diff --git a/projet/assets/11.map b/projet/assets/11.map new file mode 100644 index 0000000..1686b19 --- /dev/null +++ b/projet/assets/11.map @@ -0,0 +1,40 @@ +map "hamburg" +rd 14 +line 1 20 15. 270. 30. 210. +arc 1 20 120. 210. 90. 180. 270. +line 1 20 120. 120. 60. 60. +line 1 20 60. 60. 15. 60. + +line 1 20 120. 120. 300. 90. +line 1 20 300. 90. 480. 120. +arc 1 20 360. 120. 120. 0. 90. +line 1 20 360. 240. 240. 240. + +arc 1 20 240. 120. 120. 90. 180. +line 1 20 240. 240. 120. 120. +arc 1 20 360. 150. 90. 90. 180. +line 1 20 270. 150. 300. 90. + +line 1 20 410. 280. 360. 240. +line 1 20 480. 120. 585. 120. +wp 11 +1 118. 120.9 +2 62. 62. +3 20. 60. +5 478. 119.3 + +6 362. 239.1 +7 242. 240. +8 120.9 122. +9 122. 122. + +11 299. 92. +12 362. 241.6 +13 570. 120. + +tl 1 +12 365. 248. 5 3 5 7 + +st 1 +12 0 370. 248. +end \ No newline at end of file diff --git a/projet/assets/12.iti b/projet/assets/12.iti new file mode 100644 index 0000000..b3ab014 --- /dev/null +++ b/projet/assets/12.iti @@ -0,0 +1,21 @@ +go 30. +go 20. +turn -45. +go 30. +turn -45. +go 30. +turn -90. +go 30. +turn -60. +go 20. +turn 120. +go 20. +turn -110. +go 20. +turn -45. +go 30. +turn -45. +go 30. +turn 135. +go 20. +stop 0. diff --git a/projet/assets/12.map b/projet/assets/12.map new file mode 100644 index 0000000..990b9d2 --- /dev/null +++ b/projet/assets/12.map @@ -0,0 +1,42 @@ +map "PRG" +rd 14 +line 1 30 585. 60. 480. 120. +line 1 30 480. 120. 420. 180. +line 1 20 420. 180. 300. 240. +line 1 20 300. 240. 300. 120. + +line 1 20 300. 120. 180. 120. +line 1 20 300. 240. 180. 240. +line 1 20 180. 120. 180. 240. +line 1 20 180. 120. 120. 60. + +line 1 20 180. 30. 120. 60. +line 1 20 120. 60. 60. 120. +arc 1 10 180. 120. 120. 90. 180. +line 1 20 60. 120. 15. 30. + +line 1 20 180. 240. 180. 290. +line 1 20 300. 290. 300. 240. + +wp 11 +2 302. 238.5 +3 300. 122. +4 182. 120. +5 182. 240. + +6 180. 238. +7 122. 62. +8 122. 59. +9 62. 118. + +11 20. 40. +12 180. 280. +13 300. 242. + +tl 2 +2 310. 240. 5 3 5 7 +13 295. 250. 8 3 5 7 +st 2 +2 0 310. 235. +13 1 300. 250. +end \ No newline at end of file diff --git a/projet/assets/13.iti b/projet/assets/13.iti new file mode 100644 index 0000000..f9c226a --- /dev/null +++ b/projet/assets/13.iti @@ -0,0 +1,12 @@ +go 20. +turn 15. +go 20. +turn 90. +go 20. +go 20. +turn 45. +go 20. +go 20. +turn -45. +go 20. +stop. diff --git a/projet/assets/13.map b/projet/assets/13.map new file mode 100644 index 0000000..bb85c78 --- /dev/null +++ b/projet/assets/13.map @@ -0,0 +1,52 @@ +map "PRD" +rd 18 +line 1 20 300.0 60.0 300.0 15.0 +line 1 20 180.0 15.0 180.0 60.0 +line 1 20 300.0 60.0 585.0 60.0 +line 1 20 30.0 20.0 120.0 180.0 + +line 1 20 180.0 60.0 120.0 60.0 +line 1 20 180.0 60.0 300.0 60.0 +line 1 20 300.0 60.0 300.0 120.0 +line 1 20 120.0 180.0 300.0 60.0 + +line 1 20 300.0 120.0 480.0 240.0 +line 1 20 300.0 120.0 210.0 180.0 +line 1 20 210.0 180.0 120.0 180.0 +line 1 20 15.0 180.0 120.0 180.0 + +line 1 20 120.0 60.0 120.0 180.0 +line 1 20 70.0 280.0 120.0 180.0 +line 1 20 120.0 180.0 120.0 280.0 +line 1 20 120.0 180.0 210.0 240.0 + +line 1 20 210.0 240.0 480.0 240.0 +line 1 20 480.0 240.0 585.0 240.0 + +wp 16 +0 300. 20. +1 180. 56. +2 570. 60. +3 117.75 176. + +4 124. 60. +5 296. 60. +6 300. 116. +7 297. 62. + +8 477 238. +10 124. 180. +11 116. 180. +12 120. 176. + +13 118. 184. +14 120. 260. +15 206. 238. +17 570. 240. + +tl 1 +16 465. 235. 5 3 5 7 +st 1 +16 0 465. 240. +end + diff --git a/projet/assets/14.iti b/projet/assets/14.iti new file mode 100644 index 0000000..3b16ae8 --- /dev/null +++ b/projet/assets/14.iti @@ -0,0 +1,12 @@ +go 30. +turn 120. +go 20. +turn -90. +go 20. +turn 45. +go 20. +turn -90. +go 20. +turn 130. +go 20. +stop. \ No newline at end of file diff --git a/projet/assets/14.map b/projet/assets/14.map new file mode 100644 index 0000000..c02d087 --- /dev/null +++ b/projet/assets/14.map @@ -0,0 +1,61 @@ +map "seine" +rd 22 +line 1 20 420. 10. 300. 70. +line 1 20 300. 70. 180. 70. +line 1 20 180. 70. 100. 10. +line 1 20 300. 70. 300. 10. + +line 1 20 180. 70. 180. 10. +line 1 20 300. 70. 300. 120. +line 1 20 180. 70. 180. 120. +line 1 20 580. 30. 300. 120. + +line 1 20 300. 120. 180. 120. +line 1 20 180. 120. 120. 120. +line 1 20 120. 120. 120. 240. +line 1 20 120. 240. 300. 120. + +line 1 20 210. 240. 120. 240. +line 1 20 300. 180. 210. 240. +line 1 20 300. 120. 300. 180. +line 1 20 300. 180. 500. 280. + +line 1 20 120. 240. 160. 280. +line 1 20 100. 280. 120. 240. +line 1 20 20. 240. 120. 240. +line 1 20 20. 40. 120. 240. + +line 1 20 120. 240. 120. 280. +line 1 20 120. 120. 20. 40. +wp 20 +0 304. 67. +1 176. 70. +2 108. 16. +3 300. 20. + +4 180. 20. +5 300. 116. +6 180. 116. +7 304. 118. + +9 124. 120. +10 210. 236. +11 297. 122. +12 124. 240. + +14 300. 176. +15 490. 275. +16 155. 275. +17 118. 244. + +18 116. 240. +19 118. 236. +20 120. 270. +21 30. 48. +tl 2 +5 295. 110. 5 3 5 8 +11 285. 125. 5 3 5 13 +st 2 +5 0 300. 110. +11 1 285. 130. +end diff --git a/projet/assets/15.iti b/projet/assets/15.iti new file mode 100644 index 0000000..f17b708 --- /dev/null +++ b/projet/assets/15.iti @@ -0,0 +1,8 @@ +go 20. +go 30. +go 10. +turn -15. +go 20. +turn 75. +stop 0. + diff --git a/projet/assets/15.map b/projet/assets/15.map new file mode 100644 index 0000000..b9484bf --- /dev/null +++ b/projet/assets/15.map @@ -0,0 +1,47 @@ +map "scade" +rd 22 +line 1 30 20. 20. 70. 20. +arc 1 20 70. 80. 60. -90. 90. +arc 1 20 70. 200. 60. 270. 90. +line 1 30 70. 260. 200. 260. + +line 1 30 200. 260. 300. 260. +line 1 30 200. 240. 300. 240. +arc 1 20 200. 210. 50. 180. 90. +arc 1 20 200. 210. 30. 180. 90. + +line 1 30 150. 110. 150. 210. +line 1 30 170. 110. 170. 210. +arc 1 20 200. 110. 50. 270. 180. +arc 1 20 200. 110. 30. 270. 180. + +line 1 30 250. 60. 200. 60. +line 1 30 250. 80. 200. 80. + +arc 1 20 300. 210. 50. 90. 0. +arc 1 20 300. 210. 30. 90. 0. +line 1 30 350. 210. 413.3 20. +line 1 30 330. 210. 380. 60. + +arc 1 30 350. 140. 120. 90. -50. +line 1 30 300. 260. 550. 260. +line 1 30 413.3 20. 550. 20. +line 1 30 470. 140. 550. 140. + +wp 11 +3 115. 260. +4 290. 260. +16 412.3 23. +17 372. 84. + +18 469.9 142.5 +18 434.85 56.15 +19 305. 260. +19 359. 260. + +19 520. 260. +20 520. 20. +21 520. 140. +tl 0 +st 0 +end diff --git a/projet/assets/16.iti b/projet/assets/16.iti new file mode 100644 index 0000000..2dfcae6 --- /dev/null +++ b/projet/assets/16.iti @@ -0,0 +1,10 @@ +go 20. +turn -34. +go 30. +turn 146. +go 20. +turn -56. +go 30. +turn -34. +go 20. +stop 0. diff --git a/projet/assets/17.iti b/projet/assets/17.iti new file mode 100644 index 0000000..35403cd --- /dev/null +++ b/projet/assets/17.iti @@ -0,0 +1,15 @@ +go 20. +turn 47. +go 20. +turn -90. +go 20. +turn 90. +go 20. +turn 124. +go 30. +turn 124. +go 20. +turn -60. +go 30. +stop 0. + diff --git a/projet/assets/18.iti b/projet/assets/18.iti new file mode 100644 index 0000000..c972c4c --- /dev/null +++ b/projet/assets/18.iti @@ -0,0 +1,8 @@ +go 20. +turn -34. +go 30. +turn -146. +go 20. +turn -51. +go 20. +stop 0. diff --git a/projet/assets/b-00.bmp b/projet/assets/b-00.bmp new file mode 100644 index 0000000..bd2859b Binary files /dev/null and b/projet/assets/b-00.bmp differ diff --git a/projet/assets/b-01.bmp b/projet/assets/b-01.bmp new file mode 100644 index 0000000..728589b Binary files /dev/null and b/projet/assets/b-01.bmp differ diff --git a/projet/assets/b-02.bmp b/projet/assets/b-02.bmp new file mode 100644 index 0000000..2971a0a Binary files /dev/null and b/projet/assets/b-02.bmp differ diff --git a/projet/assets/b-03.bmp b/projet/assets/b-03.bmp new file mode 100644 index 0000000..40ffc2e Binary files /dev/null and b/projet/assets/b-03.bmp differ diff --git a/projet/assets/b-04.bmp b/projet/assets/b-04.bmp new file mode 100644 index 0000000..d9a955a Binary files /dev/null and b/projet/assets/b-04.bmp differ diff --git a/projet/assets/b-05.bmp b/projet/assets/b-05.bmp new file mode 100644 index 0000000..9613b88 Binary files /dev/null and b/projet/assets/b-05.bmp differ diff --git a/projet/assets/b-06.bmp b/projet/assets/b-06.bmp new file mode 100644 index 0000000..fe36c5c Binary files /dev/null and b/projet/assets/b-06.bmp differ diff --git a/projet/assets/b-07.bmp b/projet/assets/b-07.bmp new file mode 100644 index 0000000..4a6a4f2 Binary files /dev/null and b/projet/assets/b-07.bmp differ diff --git a/projet/assets/b-08.bmp b/projet/assets/b-08.bmp new file mode 100644 index 0000000..fbdeb08 Binary files /dev/null and b/projet/assets/b-08.bmp differ diff --git a/projet/assets/b-09.bmp b/projet/assets/b-09.bmp new file mode 100644 index 0000000..566cf87 Binary files /dev/null and b/projet/assets/b-09.bmp differ diff --git a/projet/assets/collision.wav b/projet/assets/collision.wav new file mode 100644 index 0000000..8a09e42 Binary files /dev/null and b/projet/assets/collision.wav differ diff --git a/projet/assets/ctl_10.bmp b/projet/assets/ctl_10.bmp new file mode 100644 index 0000000..66e72fe Binary files /dev/null and b/projet/assets/ctl_10.bmp differ diff --git a/projet/assets/ctl_11.bmp b/projet/assets/ctl_11.bmp new file mode 100644 index 0000000..9db76b0 Binary files /dev/null and b/projet/assets/ctl_11.bmp differ diff --git a/projet/assets/ctl_12.bmp b/projet/assets/ctl_12.bmp new file mode 100644 index 0000000..397a2bc Binary files /dev/null and b/projet/assets/ctl_12.bmp differ diff --git a/projet/assets/ctl_13.bmp b/projet/assets/ctl_13.bmp new file mode 100644 index 0000000..6dace26 Binary files /dev/null and b/projet/assets/ctl_13.bmp differ diff --git a/projet/assets/ctl_14.bmp b/projet/assets/ctl_14.bmp new file mode 100644 index 0000000..3c2352d Binary files /dev/null and b/projet/assets/ctl_14.bmp differ diff --git a/projet/assets/direction.wav b/projet/assets/direction.wav new file mode 100644 index 0000000..1a3437a Binary files /dev/null and b/projet/assets/direction.wav differ diff --git a/projet/assets/exit.wav b/projet/assets/exit.wav new file mode 100644 index 0000000..8ee1181 Binary files /dev/null and b/projet/assets/exit.wav differ diff --git a/projet/assets/game_10.bmp b/projet/assets/game_10.bmp new file mode 100644 index 0000000..a049c3f Binary files /dev/null and b/projet/assets/game_10.bmp differ diff --git a/projet/assets/game_11.bmp b/projet/assets/game_11.bmp new file mode 100644 index 0000000..2ab2b7a Binary files /dev/null and b/projet/assets/game_11.bmp differ diff --git a/projet/assets/game_12.bmp b/projet/assets/game_12.bmp new file mode 100644 index 0000000..6cf2e3e Binary files /dev/null and b/projet/assets/game_12.bmp differ diff --git a/projet/assets/game_13.bmp b/projet/assets/game_13.bmp new file mode 100644 index 0000000..f26dfed Binary files /dev/null and b/projet/assets/game_13.bmp differ diff --git a/projet/assets/game_14.bmp b/projet/assets/game_14.bmp new file mode 100644 index 0000000..50f6245 Binary files /dev/null and b/projet/assets/game_14.bmp differ diff --git a/projet/assets/light.wav b/projet/assets/light.wav new file mode 100644 index 0000000..2c06d2c Binary files /dev/null and b/projet/assets/light.wav differ diff --git a/projet/assets/map_00_oneway_line.bmp b/projet/assets/map_00_oneway_line.bmp new file mode 100644 index 0000000..bf61887 Binary files /dev/null and b/projet/assets/map_00_oneway_line.bmp differ diff --git a/projet/assets/map_01_oneway_square.bmp b/projet/assets/map_01_oneway_square.bmp new file mode 100644 index 0000000..188c13a Binary files /dev/null and b/projet/assets/map_01_oneway_square.bmp differ diff --git a/projet/assets/map_02_oneway_olympic.bmp b/projet/assets/map_02_oneway_olympic.bmp new file mode 100644 index 0000000..f4b529d Binary files /dev/null and b/projet/assets/map_02_oneway_olympic.bmp differ diff --git a/projet/assets/map_03_oneway_ea.bmp b/projet/assets/map_03_oneway_ea.bmp new file mode 100644 index 0000000..29b76de Binary files /dev/null and b/projet/assets/map_03_oneway_ea.bmp differ diff --git a/projet/assets/map_04_olympic_cut.bmp b/projet/assets/map_04_olympic_cut.bmp new file mode 100644 index 0000000..c16d84a Binary files /dev/null and b/projet/assets/map_04_olympic_cut.bmp differ diff --git a/projet/assets/map_05_olympic_cut_tl.bmp b/projet/assets/map_05_olympic_cut_tl.bmp new file mode 100644 index 0000000..d688553 Binary files /dev/null and b/projet/assets/map_05_olympic_cut_tl.bmp differ diff --git a/projet/assets/map_06_roundabout.bmp b/projet/assets/map_06_roundabout.bmp new file mode 100644 index 0000000..ea2cfa9 Binary files /dev/null and b/projet/assets/map_06_roundabout.bmp differ diff --git a/projet/assets/map_07_roundabout_tl.bmp b/projet/assets/map_07_roundabout_tl.bmp new file mode 100644 index 0000000..8daabdd Binary files /dev/null and b/projet/assets/map_07_roundabout_tl.bmp differ diff --git a/projet/assets/map_08_nycity.bmp b/projet/assets/map_08_nycity.bmp new file mode 100644 index 0000000..ac9db11 Binary files /dev/null and b/projet/assets/map_08_nycity.bmp differ diff --git a/projet/assets/map_09_paris.bmp b/projet/assets/map_09_paris.bmp new file mode 100644 index 0000000..b07bff6 Binary files /dev/null and b/projet/assets/map_09_paris.bmp differ diff --git a/projet/assets/obst.bmp b/projet/assets/obst.bmp new file mode 100644 index 0000000..7100810 Binary files /dev/null and b/projet/assets/obst.bmp differ diff --git a/projet/assets/orange.bmp b/projet/assets/orange.bmp new file mode 100644 index 0000000..6ebaf5e Binary files /dev/null and b/projet/assets/orange.bmp differ diff --git a/projet/assets/speed.wav b/projet/assets/speed.wav new file mode 100644 index 0000000..e7e8b1f Binary files /dev/null and b/projet/assets/speed.wav differ diff --git a/projet/src/buffer.c b/projet/src/buffer.c new file mode 100644 index 0000000..20cfffa --- /dev/null +++ b/projet/src/buffer.c @@ -0,0 +1,61 @@ +#include "buffer.h" + +#include +#include +#include +#include +#include + +#define max(a, b) ((a) <= (b) ? (a) : (b)) + +void *malloc_checked(size_t size) { + void *result = malloc(size); + if (!result) { + perror("malloc()"); + exit(EXIT_FAILURE); + } + return result; +} + +char *strdup_checked(const char *s) { + char *result = strdup(s); + if (!result) { + perror("strdup()"); + exit(EXIT_FAILURE); + } + return result; +} + +buffer_t *buffer_alloc(size_t initial_size) { + buffer_t *buff = malloc_checked(sizeof *buff); + buff->data = malloc_checked(initial_size * sizeof *buff->data); + buff->size = initial_size; + buff->occupancy = 0; + return buff; +} + +void buffer_free(buffer_t *buffer) { + assert (buffer); + + free(buffer->data); + free(buffer); +} + +void buffer_resize(buffer_t *buff, size_t new_size) { + assert (buff); + assert (new_size >= buff->size); + + unsigned char *new_data = malloc_checked(new_size); + memcpy(new_data, buff->data, buff->occupancy); + free(buff->data); + buff->data = new_data; + buff->size = new_size; +} + +void buffer_write(buffer_t *buff, void *data, size_t data_size) { + assert (buff); + if (buff->occupancy + data_size > buff->size) + buffer_resize(buff, max(buff->size + data_size, 2 * buff->size)); + memcpy(buff->data + buff->occupancy, data, data_size); + buff->occupancy += data_size; +} diff --git a/projet/src/buffer.h b/projet/src/buffer.h new file mode 100644 index 0000000..f8a2743 --- /dev/null +++ b/projet/src/buffer.h @@ -0,0 +1,27 @@ +#ifndef BUFFER_H +#define BUFFER_H + +#include + +/* A simple type of append-only buffers. */ + +typedef struct buffer { + unsigned char *data; + size_t size; + size_t occupancy; +} buffer_t; + +void *malloc_checked(size_t size); +char *strdup_checked(const char *); + +buffer_t *buffer_alloc(size_t initial_size); +void buffer_free(buffer_t *buff); + +void buffer_write(buffer_t *buff, void *data, size_t data_size); + +#define buffer_foreach(ty, var, buffer) \ + for (ty *var = (ty *)buffer->data; \ + var < (ty *)(buffer->data + buffer->occupancy); \ + var++) + +#endif /* BUFFER_H */ diff --git a/projet/src/challenge.ept b/projet/src/challenge.ept new file mode 100644 index 0000000..d3a6dd4 --- /dev/null +++ b/projet/src/challenge.ept @@ -0,0 +1,21 @@ +open Globals + +node the_challenge(initial_ph : phase; top : bool) + returns (ph : phase; sta : status; sign : sign; evt : event; + scoreA, scoreB : int; time : float) +var itr : interrupt; ini_sens, sens : sensors; +let + ini_sens = { s_road = { red = 128; green = 128; blue = 128 }; + s_front = { red = 128; green = 128; blue = 128 }; + s_sonar = cSONARFAR }; + (ph, sta) = Vehicle.simulate(Map.read_itinerary(), + ini_sens fby sens, + Ok fby itr, + initial_ph, + top); + (sign, itr, sens, evt) = City.simulate(ph, time); + () = Map.soundEffects(Utilities.event_edge(evt), sta); + scoreA = City.scoringA(evt, sta); + time = City.wallclock(sta); + scoreB = City.scoringB(ph); +tel diff --git a/projet/src/city.ept b/projet/src/city.ept new file mode 100644 index 0000000..b20aa9a --- /dev/null +++ b/projet/src/city.ept @@ -0,0 +1,212 @@ +open Globals +open Utilities + +(* Utilities *) + +node wallclock(rstatus : status) returns (time : float) +var cpt : int; +let + cpt = (if rstatus = Running then 1 else 0) + (0 fby cpt); + time = timestep *. Mathext.float(cpt); +tel + +fun lookup_phase(ph : phase) returns (data : map_data) +let + data = Map.lookup_pos(ph.ph_pos); +tel + +(* Event detection *) + +fun light(lights : traflights; ph : phase) + returns (light_run : bool) +var data : map_data; +let + data = lookup_phase(ph); + light_run = ph.ph_vel >. 0.01 and data.tl_required + and (lights[> data.tl_number <]).tl_color = Red; +tel + +fun speed(ph : phase) returns (speed_excess : bool) +let + speed_excess = ph.ph_vel >. Mathext.float((lookup_phase(ph)).max_speed); +tel + +fun exited_aux(pos : position; acc : bool) returns (accnew : bool) +let + accnew = acc and (Map.lookup_pos(pos)).on_road; +tel + +fun exited(ph : phase) returns (exit_road : bool) +var positions : position^2; dummy : phase^2; +let + (positions, dummy) = + map<<2>> Vehicle.car_geometry(ph^2, [[-. cDELTA, cB /. 2.0], + [-. cDELTA, -. cB /. 2.0]]); + exit_road = not fold<<2>> exited_aux(positions, true); +tel + +node collision_aux(ph : phase; obst : obstacle; acc : bool) + returns (accnew : bool) +var ang, angle, dist, distobst : float; close : bool; +let + (ang, dist) = angle_dist(ph.ph_pos, obst.o_pos); + angle = (pi /. 180.0) *. abs(normalize(ph.ph_head -. ang)); + accnew = acc or (obst.o_pres and (dist <=. cROBST or close)); + distobst = dist -. cROBST; + close = Mathext.sin(angle) *. distobst <=. cSB + and Mathext.cos(angle) *. distobst <=. cSA + and Mathext.cos(angle) *. distobst >=. -. cSC; +tel + +node collision(ph : phase; obstacles : obstacles) + returns (collision_event : bool) +let + collision_event = fold<> collision_aux(ph^obstnum, + obstacles, + false); +tel + +node wrong_dir(ph : phase) returns (wrong : bool) +var data : map_data; error : float; ang : float; +let + data = lookup_phase(ph); + ang = ph.ph_head *. (pi /. 180.0); + error = data.dir_x *. Mathext.cos(ang) +. data.dir_y *. Mathext.sin(ang); + wrong = error <. -. 0.5; +tel + +fun aggregate_events(lightRun, speedExcess, exitRoad, collisionEvent, + wrong : bool) + returns (o : event; itr : interrupt) +let + o = { lightRun = lightRun; + speedExcess = speedExcess; + exitRoad = exitRoad; + collisionEvent = collisionEvent; + dirEvent = wrong }; + itr = if exitRoad then Halt else Ok; +tel + +node event_detection(sign : sign; ph : phase) + returns (itr : interrupt; evts : event) +let + (evts, itr) = aggregate_events (light(sign.si_tlights, ph), + speed(ph), + exited(ph), + collision(ph, sign.si_obstacles), + wrong_dir(ph)); +tel + +(* Sensor aggregation *) + +fun ground_color_detection(ph : phase) returns (road_color : color) +let + road_color = (lookup_phase(ph)).color; +tel + +fun traffic_light_detection(ph : phase; traflights : traflights) + returns (tlight_color : color) +let + tlight_color = Utilities.encode_color( + (traflights.[(lookup_phase(ph)).tl_number] + default { tl_pos = { x = 0.0; y = 0.0 }; tl_color = Other }) + .tl_color + ); +tel + +fun obstacles_detection_aux(ph : phase; obst : obstacle; acc : int) + returns (accnew : int) +var a, d, d1 : float; sonar : int; +let + (a, d) = Utilities.angle_dist(ph.ph_pos, obst.o_pos); + d1 = d -. cROBST; + sonar = if Utilities.abs(Utilities.normalize(ph.ph_head -. a)) <=. 30.0 + and d1 <=. 100.0 and obst.o_pres + then Mathext.int(d1) + else cSONARFAR; + accnew = Utilities.min_int(sonar, acc); +tel + +node obstacle_detection(ph : phase; obstacles : obstacles) + returns (sonar : int) +let + sonar = fold<> obstacles_detection_aux(ph^obstnum, + obstacles, + cSONARFAR); +tel + +node robot_sensors(ph : phase; sign : sign) returns (sens : sensors) +let + sens = { s_road = ground_color_detection(ph); + s_front = traffic_light_detection(ph, sign.si_tlights); + s_sonar = obstacle_detection(ph, sign.si_obstacles) } +tel + +(* The city *) + +fun traffic_lights_aux(p : param_tlight; time : float) returns (tl : traflight) +var cpt, period : int; light : colorQ; +let + period = Utilities.max_int(1, p.ptl_amber + p.ptl_green + p.ptl_red); + cpt = Mathext.modulo(Mathext.int(time) + p.ptl_phase, period); + if cpt < p.ptl_green then light = Green + else if cpt < p.ptl_amber + p.ptl_green then light = Amber + else light = Red + end + end; + tl = { tl_pos = p.ptl_pos; tl_color = light }; +tel + +fun traffic_lights(time : float) returns (all_lights : traflights) +var lights : param_tlights; +let + lights = Map.read_traffic_lights(); + all_lights = map<> traffic_lights_aux(lights, time^trafnum); +tel + +fun all_obstacles_aux(po : param_obst; time : float) returns (o : obstacle) +let + o = { o_pos = po.pot_pos; + o_pres = po.pot_since <=. time and time <=. po.pot_till }; +tel + +fun all_obstacles(time : float) returns (obstacles : obstacles) +let + obstacles = map<> all_obstacles_aux(Map.read_obstacles(), + time^obstnum); +tel + +node simulate(ph : phase; time : float) + returns (sign : sign; itr : interrupt; sens : sensors; evt : event) +let + sign = { si_tlights = traffic_lights(time); + si_obstacles = all_obstacles(time) }; + (itr, evt) = event_detection(sign, ph); + sens = robot_sensors(ph, sign); +tel + +(* Scoring *) + +node scoringA(e : event; rstatus : status) returns (score : int) +var penalty : int; collision_count : int; +let + score = (10000 fby score) + + if Utilities.rising_edge(rstatus = Arrived) then 1000 else 0 + + if rstatus = Running then penalty else 0; + penalty = (if Utilities.rising_edge(e.lightRun) then -500 else 0) + + (if Utilities.rising_edge(e.speedExcess) then -100 else 0) + + (if e.speedExcess then -2 else 0) + + (if Utilities.rising_edge(e.exitRoad) then -5000 else 0) + + (if Utilities.rising_edge(e.dirEvent) then -2000 else 0) + + (if collision_count = 0 then -500 else 0) + + (if collision_count < 0 then 10 else 0); + collision_count = Utilities.countdown(not e.collisionEvent, 20); +tel + +node scoringB(ph : phase) returns (score : int) +var v : float; +let + v = Utilities.variation(ph.ph_vel >=. 1.0, ph.ph_head, timestep) + /. (1.0 +. Utilities.uptime(ph.ph_vel, timestep)); + score = Mathext.int(1000.0 -. v); +tel diff --git a/projet/src/control.ept b/projet/src/control.ept new file mode 100644 index 0000000..cbb7567 --- /dev/null +++ b/projet/src/control.ept @@ -0,0 +1,9 @@ +open Globals + + +node controller(sens : sensors; iti : itielts) + returns (rspeed : wheels; arriving : bool) +let + rspeed = { left = 0.0; right = 0.0 }; + arriving = false; +tel diff --git a/projet/src/cutils.c b/projet/src/cutils.c new file mode 100644 index 0000000..d4ab5ca --- /dev/null +++ b/projet/src/cutils.c @@ -0,0 +1,88 @@ +/* This file is part of SyncContest. + Copyright (C) 2017-2020 Eugene Asarin, Mihaela Sighireanu, Adrien Guatto. */ + +#include "cutils.h" + +#include +#include +#include +#include +#include + +log_verbosity_level level = LOG_INFO; +FILE *f = NULL; +char *filename = NULL; + +void log_set_verbosity_level(log_verbosity_level l) { + level = l; +} + +void log_message_v(log_verbosity_level msg_level, const char *fmt, va_list va) { + va_list vb; + FILE *out = msg_level == LOG_INFO ? stdout : stderr; + + if (msg_level > level) + return; + + va_copy(vb, va); + if (f != NULL) { + vfprintf(f, fmt, va); + } + + vfprintf(out, fmt, vb); + fflush(out); +} + +void log_message(log_verbosity_level msg_level, const char *fmt, ...) { + va_list va; + va_start(va, fmt); + log_message_v(msg_level, fmt, va); + va_end(va); +} + +void log_fatal(const char *fmt, ...) { + va_list va; + va_start(va, fmt); + log_message_v(LOG_FATAL, fmt, va); + va_end(va); + exit(EXIT_FAILURE); +} + +void log_info(const char *fmt, ...) { + va_list va; + va_start(va, fmt); + log_message_v(LOG_INFO, fmt, va); + va_end(va); +} + +void log_debug(const char *fmt, ...) { + va_list va; + va_start(va, fmt); + log_message_v(LOG_DEBUG, fmt, va); + va_end(va); +} + +void log_init(const char *fn) { + if (!fn) { + log_info("[log] initializing, not saving to file\n"); + return; + } + + filename = strdup(fn); + assert (filename); + f = fopen(filename, "w"); + if (!f) + log_fatal("[log] could not open log file %s (fopen)", filename); + + log_info("[log] logging to %s\n", filename); +} + +void log_shutdown() { + if (f) { + log_info("[log] shutting down, closing %s\n", filename); + fclose(f); + free(filename); + } else { + log_info("[log] shutting down\n"); + } +} diff --git a/projet/src/cutils.h b/projet/src/cutils.h new file mode 100644 index 0000000..2876046 --- /dev/null +++ b/projet/src/cutils.h @@ -0,0 +1,30 @@ +/* This file is part of SyncContest. + Copyright (C) 2017-2020 Eugene Asarin, Mihaela Sighireanu, Adrien Guatto. */ + +#ifndef CUTILS_H +#define CUTILS_H + +#include + +typedef enum { + LOG_FATAL = 0, + LOG_INFO = 1, + LOG_DEBUG = 2, +} log_verbosity_level; + +/* Calling `log_init(fn)` initializes the logging subsystem, asking it to save + log messages to the file `fn`. This pointer may be NULL, in which case the + messages are not saved. */ +void log_init(const char *filename); +void log_shutdown(); + +void log_set_verbosity_level(log_verbosity_level level); + +void log_message_v(log_verbosity_level level, const char *fmt, va_list); +void log_message(log_verbosity_level level, const char *fmt, ...); + +void log_fatal(const char *fmt, ...); +void log_info(const char *fmt, ...); +void log_debug(const char *fmt, ...); + +#endif /* CUTILS_H */ diff --git a/projet/src/debug.c b/projet/src/debug.c new file mode 100644 index 0000000..6fc6f31 --- /dev/null +++ b/projet/src/debug.c @@ -0,0 +1,39 @@ +#include "debug.h" + +#include "cutils.h" + +void Debug__dbg_step(char *msg, Debug__dbg_out *o) { + log_info("%s\n", msg); +} + +void Debug__dbg_bool_step(char *msg, bool x, Debug__dbg_bool_out *o) { + log_info("%s %d\n", msg, x); +} + +void Debug__dbg_int_step(char *msg, int x, Debug__dbg_int_out *o) { + log_info("%s %d\n", msg, x); +} + +void Debug__dbg_float_step(char *msg, float x, Debug__dbg_float_out *o) { + log_info("%s %f\n", msg, x); +} + +void Debug__d_init_step(Debug__d_init_out *o) { + /* Empty by design */ +} + +void Debug__d_string_step(Debug__world _w, char *s, Debug__d_string_out *o) { + log_info("%s", s); +} + +void Debug__d_bool_step(Debug__world _w, bool b, Debug__d_bool_out *o) { + log_info("%d", b); +} + +void Debug__d_int_step(Debug__world _w, int i, Debug__d_int_out *o) { + log_info("%d", i); +} + +void Debug__d_float_step(Debug__world _w, float f, Debug__d_float_out *o) { + log_info("%f", f); +} diff --git a/projet/src/debug.epi b/projet/src/debug.epi new file mode 100644 index 0000000..56e5014 --- /dev/null +++ b/projet/src/debug.epi @@ -0,0 +1,11 @@ +external fun dbg(msg : string) returns () +external fun dbg_bool(msg : string; x : bool) returns () +external fun dbg_int(msg : string; x : int) returns () +external fun dbg_float(msg : string; x : float) returns () + +type world +external fun d_init() returns (n : world) +external fun d_string(w : world; string) returns (n : world) +external fun d_bool(w : world; bool) returns (n : world) +external fun d_int(w : world; int) returns (n : world) +external fun d_float(w : world; float) returns (n : world) diff --git a/projet/src/debug.h b/projet/src/debug.h new file mode 100644 index 0000000..9a85ac1 --- /dev/null +++ b/projet/src/debug.h @@ -0,0 +1,23 @@ +#ifndef DEBUG_H +#define DEBUG_H + +#include "stdbool.h" +#include "assert.h" +#include "pervasives.h" + +#include "hept_ffi.h" + +DECLARE_HEPT_FUN(Debug, dbg, (char *),); +DECLARE_HEPT_FUN(Debug, dbg_bool, (char *, bool),); +DECLARE_HEPT_FUN(Debug, dbg_int, (char *, int),); +DECLARE_HEPT_FUN(Debug, dbg_float, (char *, float),); + +typedef struct { } Debug__world; + +DECLARE_HEPT_FUN_NULLARY(Debug, d_init, Debug__world n); +DECLARE_HEPT_FUN(Debug, d_string, (Debug__world, char *), Debug__world n); +DECLARE_HEPT_FUN(Debug, d_bool, (Debug__world, bool), Debug__world n); +DECLARE_HEPT_FUN(Debug, d_int, (Debug__world, int), Debug__world n); +DECLARE_HEPT_FUN(Debug, d_float, (Debug__world, float), Debug__world n); + +#endif /* DEBUG_H */ diff --git a/projet/src/debug_types.h b/projet/src/debug_types.h new file mode 100644 index 0000000..d12b30d --- /dev/null +++ b/projet/src/debug_types.h @@ -0,0 +1,4 @@ +#ifndef DEBUG_TYPES_H +#define DEBUG_TYPES_H + +#endif /* DEBUG_TYPES_H */ diff --git a/projet/src/globals.ept b/projet/src/globals.ept new file mode 100644 index 0000000..6cd62e7 --- /dev/null +++ b/projet/src/globals.ept @@ -0,0 +1,238 @@ +(* Types *) + +const obstnum : int = 10 + +const trafnum : int = 6 + +const itinum : int = 50 + +type color = { red : int; green : int; blue : int } + +type colorQ = Red | Green | Amber | Other + +type position = { x : float; y : float } + +type interrupt = Wait | Halt | Ok + +type status = Preparing | Running | Arrived | Stopped + +type event = { + lightRun : bool; + speedExcess : bool; + exitRoad : bool; + collisionEvent : bool; + dirEvent : bool +} + +type obstacle = { o_pos : position; o_pres : bool } + +type obstacles = obstacle^obstnum + +type traflight = { tl_pos : position; tl_color : colorQ } + +type traflights = traflight^trafnum + +type sign = { si_tlights : traflights; si_obstacles : obstacles } + +type phase = { ph_pos : position; ph_vel : float; ph_head : float } + +type sensors = { s_road : color; s_front : color; s_sonar : int } + +type action = Go | Turn | Stop + +type itielt = { act : action; param : float } + +type itielts = itielt^itinum + +type wheels = { left : float; right : float } + +(* information on one point on the map *) +type map_data = { on_road : bool; (* is it on the road? *) + color : color; (* road color *) + max_speed : int; (* maximum allowed speed *) + tl_number : int; (* which traffic light is visible? *) + tl_required : bool; (* should we stop on red traffic light? *) + dir_x : float; (* directing vector, abscissa *) + dir_y : float (* directing vector, ordinate *) } + +(* information on one obstacle *) +type param_obst = { pot_pos : position; (* position of the obstacle *) + pot_since : float; (* appearance time *) + pot_till : float (* disappearance time *) } + +type param_obsts = param_obst^obstnum + +type param_tlight = { ptl_pos : position; (* traffic light position *) + ptl_green : int; (* time on green (s) *) + ptl_amber : int; (* time on amber (s) *) + ptl_red : int; (* time on red (s) *) + ptl_phase : int (* initial phase (s) *) } + +type param_tlights = param_tlight^trafnum + +(* Constants *) + +const pi : float = 3.1415926 + +const red : color = { red = 255; green = 0; blue = 0 } + +const green : color = { red = 0; green = 255; blue = 0 } + +const blue : color = { red = 0; green = 0; blue = 255 } + +const amber : color = { red = 255; green = 191; blue = 0 } + +const cyan : color = { red = 0; green = 255; blue = 255 } + +const gray : color = { red = 128; green = 128; blue = 128 } + +const magenta : color = { red = 255; green = 0; blue = 255 } + +const cB : float = 6.0 + +const cD : float = 5.5 + +const cDELTA : float = 0.0 + +const cMAXWHEEL : float = 500.0 + +const cMAXSPEED : float = 25.0 + +const cSONARFAR : int = 1000 + +const cROBST : float = 1.5 + +const cSA : float = 3.0 + +const cSB : float = 3.0 + +const cSC : float = 7.0 + +const idlew : wheels = { left = 0.0; right = 0.0 } + +const timestep : float = 0.01 (* in seconds *) + +(* Debugging *) + +(* TODO: define d_position *) + +fun dbg_position(msg : string; p : position) returns () +var w0, w1, w2, w3, w4, w5 : Debug.world; +let + w0 = Debug.d_string(Debug.d_init(), msg); + w1 = Debug.d_string(w0, "{ x = "); + w2 = Debug.d_float(w1, p.x); + w3 = Debug.d_string(w2, "; y = "); + w4 = Debug.d_float(w3, p.y); + w5 = Debug.d_string(w4, " }\n"); +tel + +fun dbg_phase(msg : string; ph : phase) returns () +var w0, w1, w2, w3, w4, w5, w6, w7, w8, w9 : Debug.world; +let + w0 = Debug.d_string(Debug.d_init(), msg); + w1 = Debug.d_string(w0, "{ x = "); + w2 = Debug.d_float(w1, ph.ph_pos.x); + w3 = Debug.d_string(w2, "; y = "); + w4 = Debug.d_float(w3, ph.ph_pos.y); + w5 = Debug.d_string(w4, "; head = "); + w6 = Debug.d_float(w5, ph.ph_head); + w7 = Debug.d_string(w6, "; vel = "); + w8 = Debug.d_float(w7, ph.ph_vel); + w9 = Debug.d_string(w8, " }\n"); +tel + +fun dbg_interrupt(msg : string; itr : interrupt) returns () +var w0, w1, w2 : Debug.world; +let + w0 = Debug.d_string(Debug.d_init(), msg); + switch itr + | Wait do w1 = Debug.d_string(w0, "Wait"); + | Halt do w1 = Debug.d_string(w0, "Halt"); + | Ok do w1 = Debug.d_string(w0, "Ok"); + end; + w2 = Debug.d_string(w1, "\n"); +tel + +fun dbg_status(msg : string; sta : status) returns () +var w0, w1, w2 : Debug.world; +let + w0 = Debug.d_string(Debug.d_init(), msg); + switch sta + | Preparing do w1 = Debug.d_string(w0, "Preparing"); + | Running do w1 = Debug.d_string(w0, "Running"); + | Arrived do w1 = Debug.d_string(w0, "Arrived"); + | Stopped do w1 = Debug.d_string(w0, "Stopped"); + end; + w2 = Debug.d_string(w1, "\n"); +tel + +fun dbg_wheels(msg : string; wh : wheels) returns () +var w0, w1, w2, w3, w4, w5 : Debug.world; +let + w0 = Debug.d_string(Debug.d_init(), msg); + w1 = Debug.d_string(w0, "{ left = "); + w2 = Debug.d_float(w1, wh.left); + w3 = Debug.d_string(w2, "; right = "); + w4 = Debug.d_float(w3, wh.right); + w5 = Debug.d_string(w4, " }\n"); +tel + +fun dbg_action(msg : string; act : action) returns () +var w0, w1, w2 : Debug.world; +let + w0 = Debug.d_string(Debug.d_init(), msg); + switch act + | Go do w1 = Debug.d_string(w0, "Go"); + | Turn do w1 = Debug.d_string(w0, "Turn"); + | Stop do w1 = Debug.d_string(w0, "Stop"); + end; + w2 = Debug.d_string(w1, "\n"); +tel + +fun d_color(w0 : Debug.world; c : color) + returns (w7 : Debug.world) +var w1, w2, w3, w4, w5, w6 : Debug.world; +let + w1 = Debug.d_string(w0, "{ red = "); + w2 = Debug.d_int(w1, c.red); + w3 = Debug.d_string(w2, "; green = "); + w4 = Debug.d_int(w3, c.green); + w5 = Debug.d_string(w4, "; blue = "); + w6 = Debug.d_int(w5, c.blue); + w7 = Debug.d_string(w6, " }"); +tel + +fun dbg_color(msg : string; c : color) returns () +var w0, w1, w2 : Debug.world; +let + w0 = Debug.d_string(Debug.d_init(), msg); + w1 = d_color(w0, c); + w2 = Debug.d_string(w1, "\n"); +tel + +fun dbg_colorq(msg : string; c : colorQ) returns () +var w0, w1, w2 : Debug.world; +let + w0 = Debug.d_string(Debug.d_init(), msg); + switch c + | Red do w1 = Debug.d_string(w0, "Red"); + | Green do w1 = Debug.d_string(w0, "Green"); + | Amber do w1 = Debug.d_string(w0, "Amber"); + | Other do w1 = Debug.d_string(w0, "Other"); + end; + w2 = Debug.d_string(w1, "\n"); +tel + +fun dbg_sensors(msg : string; s : sensors) returns () +var w0, w1, w2, w3, w4, w5, w6, w7 : Debug.world; +let + w0 = Debug.d_string(Debug.d_init(), msg); + w1 = Debug.d_string(w0, "{ s_road = "); + w2 = d_color(w1, s.s_road); + w3 = Debug.d_string(w2, "; s_front = "); + w4 = d_color(w3, s.s_front); + w5 = Debug.d_string(w4, "; s_sonar = "); + w6 = Debug.d_int(w5, s.s_sonar); + w7 = Debug.d_string(w6, " }\n"); +tel diff --git a/projet/src/hept_ffi.h b/projet/src/hept_ffi.h new file mode 100644 index 0000000..2d9b81b --- /dev/null +++ b/projet/src/hept_ffi.h @@ -0,0 +1,52 @@ +#ifndef HEPT_FFI_H +#define HEPT_FFI_H + +#define UNPAREN(...) __VA_ARGS__ + +#define DECLARE_HEPT_FUN(module, name, inputs, outputs) \ + typedef struct { outputs; } module ## __ ## name ## _out; \ + void module ## __ ## name ##_step(UNPAREN inputs, \ + module ## __ ## name ## _out *) + +#define DECLARE_HEPT_FUN_NULLARY(module, name, outputs) \ + typedef struct { outputs; } module ## __ ## name ## _out; \ + void module ## __ ## name ##_step(module ## __ ## name ## _out *) + +#define DEFINE_HEPT_FUN(module, name, inputs) \ + void module ## __ ## name ##_step(UNPAREN inputs, \ + module ## __ ## name ## _out *out) + +#define DEFINE_HEPT_FUN_NULLARY(module, name, inputs) \ + void module ## __ ## name ##_step(module ## __ ## name ## _out *out) + +#define DECLARE_HEPT_NODE(module, name, inputs, outputs, state) \ + typedef struct { outputs; } module ## __ ## name ## _out; \ + typedef struct { state; } module ## __ ## name ## _mem; \ + void module ## __ ## name ##_step(UNPAREN inputs, \ + module ## __ ## name ## _out *, \ + module ## __ ## name ## _mem *); \ + void module ## __ ## name ##_reset(module ## __ ## name ## _mem *) + +#define DECLARE_HEPT_NODE_NULLARY(module, name, outputs, state) \ + typedef struct { outputs; } module ## __ ## name ## _out; \ + typedef struct { state; } module ## __ ## name ## _mem; \ + void module ## __ ## name ##_step(module ## __ ## name ## _out *, \ + module ## __ ## name ## _mem *); \ + void module ## __ ## name ##_reset(module ## __ ## name ## _mem *) + +#define DEFINE_HEPT_NODE_RESET(module, name) \ + void module ## __ ## name ##_reset(module ## __ ## name ## _mem *mem) + +#define DEFINE_HEPT_NODE_STEP(module, name, inputs) \ + void module ## __ ## name ##_step(UNPAREN inputs, \ + module ## __ ## name ## _out *out, \ + module ## __ ## name ## _mem *mem) + +#define DEFINE_HEPT_NODE_NULLARY_STEP(module, name, inputs) \ + void module ## __ ## name ##_step(module ## __ ## name ## _out *out, \ + module ## __ ## name ## _mem *mem) + +/* FIXME remove when Heptagon's pervasives.h has been fixed. */ +typedef char * string; + +#endif /* HEPT_FFI */ diff --git a/projet/src/main.c b/projet/src/main.c new file mode 100644 index 0000000..c94d91a --- /dev/null +++ b/projet/src/main.c @@ -0,0 +1,133 @@ +/* This file is part of SyncContest. + Copyright (C) 2017-2020 Eugene Asarin, Mihaela Sighireanu, Adrien Guatto. */ + +#include +#include +#include +#include + +#include "trace.h" +#include "cutils.h" +#include "map.h" +#include "simulation_loop.h" + +void usage() { + fprintf(stderr, "Usage: scontest [OPTIONS] file.map\n"); + fprintf(stderr, "Options:\n"); + fprintf(stderr, " -v Be verbose\n"); + fprintf(stderr, " -g Show graphics\n"); + fprintf(stderr, " -t Start racing immediately\n"); + fprintf(stderr, " -s Simulation step/s (default: 60)\n"); + fprintf(stderr, " -o Save log messages to \n"); + fprintf(stderr, " -w Run in headless mode\n"); + fprintf(stderr, " -m Run at most synchronous steps\n"); + fprintf(stderr, " -a Enable audio\n"); + fprintf(stderr, " -e Enable obstacles\n"); + fprintf(stderr, " -h Display this message\n"); +} + +int main(int argc, char **argv) { + bool show_guide = true, headless = false, audio = false, obstacles = false; + int initial_top = false, opt; + char *log_filename = NULL; + size_t max_synchronous_steps = 0; + float sps = 60.f; + + hept_trace_init(); + + /* Parse command line. */ + while ((opt = getopt(argc, argv, "vgtf:o:wm:hae")) != -1) { + switch (opt) { + case 'v': + log_set_verbosity_level(LOG_DEBUG); + break; + + case 'g': + show_guide = false; + break; + + case 't': + initial_top = true; + break; + + case 's': + sps = atof(optarg); + break; + + case 'h': + usage(); + return EXIT_SUCCESS; + + case 'o': + log_filename = optarg; + break; + + case 'w': + headless = true; + break; + + case 'm': + max_synchronous_steps = atoi(optarg); + break; + + case 'a': + audio = true; + break; + + case 'e': + obstacles = true; + break; + + default: + usage(); + return EXIT_FAILURE; + } + } + + if (optind >= argc) { + usage(); + return EXIT_FAILURE; + } + + /* Initialize logging system. */ + log_init(log_filename); + + /* Load the map. */ + const char *filename = argv[optind]; + map_load(filename); + + /* Mark all obstacles as absent if they have been disabled. */ + if (!obstacles) { + for (size_t i = 0; i < map->obst_sz; i++) + map->obst_arr[i].pot_since = map->obst_arr[i].pot_till = -1.f; + } + + /* Run the simulation loop. */ + race_result_t r = + simulation_loop(show_guide, + initial_top, + sps, + headless, + audio, + max_synchronous_steps); + + switch (r) { + case RACE_SUCCESS: + log_info("[simulation] race succeeded\n"); + break; + case RACE_TIMEOUT: + log_info("[simulation] race quit before finishing\n"); + break; + case RACE_CRASH: + log_info("[simulation] car crashed\n"); + break; + } + + /* Free the map, shutdown logs, and return. */ + map_destroy(); + log_shutdown(); + hept_trace_quit(); + + /* Return the race result as an exit code. */ + return r; +} diff --git a/projet/src/map.c b/projet/src/map.c new file mode 100644 index 0000000..b2a08d7 --- /dev/null +++ b/projet/src/map.c @@ -0,0 +1,987 @@ +#include "map.h" + +#include + +#include "mymath.h" +#include "cutils.h" + +map_t *map; + +/* + * ========================================================================= + * Geometry + * ========================================================================= + */ + +/** Euclidian distance betwen two points (xa,ya) and (xb,yb) */ +float distance(float xa, float ya, float xb, float yb) +{ + /* use of math.h function */ + return hypotf(xa - xb, ya - yb); +} + +/** Convert angle from radian to degree */ +float todegree(float a) +{ + return (a * 180.0) / M_PI; +} + +/** Convert angle from degree to radian */ +float toradian(float a) +{ + return (a * M_PI) / 180.0; +} + +/** Get the cadran of the angle (in degree) */ +int tocadran(float a) { + if ((0 <= a && a < 90) || + (-360 <= a && a < -270)) + return 1; + if ((90 <= a && a < 180) || + (-270 <= a && a < -180)) + return 2; + if ((180 <= a && a < 270) || + (-180 <= a && a < -90)) + return 3; + return 4; +} + +/** Angle (in degree) of a segment */ +float lineAngle(float x1, float y1, float x2, float y2) +{ + /* use of math.h function for arctan */ + return todegree(atan2(y2-y1,x2-x1)); +} + +/** Angle inside an arc delimited by (from,to) */ +bool isInArc(float a, float from, float to) +{ + if (from < to) { + // trigo direction + return (from <= a && a <= to) || (from <= (a+360) && (a+360) <= to); + } + else { + // clock direction + return (to <= a && a <= from) || (to <= (a+360) && (a+360) <= from); + } +} + +/** Angles in ordred inside an arc delimited by (from,to) */ +bool fourAnglesInOrder(float from,float x,float y, float to) +{ + if (from < to) { + // trigo direction + return + (from<=x && x<=y && y<=to) || + (from<=x+360 && x+360<=y+360 && y+360<=to) || + (from<=x && x<=y+360 && y+360<=to) || + (from<=x+360 && x+360<=y && y<=to); + } else { + // clock direction + return (to <= y && y<=x && x <= from) || + (to <= y+360 && y+360<=x+360 && x+360 <= from) || + (to <= y+360 && y+360<=x && x <= from) || + (to <= y && y<=x+360 && x+360 <= from); + } +} + +/** Product of two vectors for directions */ +float dirProd(float dir1x, float dir1y, float dir2x, float dir2y) +{ + return dir1x * dir2x + dir1y * dir2y; +} + +float dirVProd(float dir1x, float dir1y, float dir2x, float dir2y) +{ + return dir1x * dir2y - dir1y * dir2x; +} + +/** Size of a direction vector */ +float dirNorm(float dir1x, float dir1y) +{ + /* use of math.h function */ + return hypot(dir1x, dir1y); +} + +/** Cos of directions */ +float dirCos(float dir1x, float dir1y, float dir2x, float dir2y) +{ + return dirProd(dir1x, dir1y, dir2x, dir2y) + / (dirNorm(dir1x, dir1y) * dirNorm(dir2x, dir2y)); +} + +/** Project point (x,y) on line d(p1,p2) and + * return result on (px, py) + */ +void +dirProjPoint( /* IN */ float x, float y, position_t * p1, position_t * p2, + /* OUT */ float *px, float *py) +{ + //compute direction vector for d(p1, p2) + float dir_x = p2->x - p1->x; + float dir_y = p2->y - p1->y; + float dirn = dirNorm(dir_x, dir_y); + + //compute P1 - H + float p1h = ((x - p1->x) * dir_x + (y - p1->y) * dir_y) / dirn; + + //compute h + (*px) = p1->x + (p1h * dir_x) / dirn; + (*py) = p1->y + (p1h * dir_y) / dirn; + + return; +} + +/* + * ========================================================================= + * Parsing + * ========================================================================= + */ + +typedef void (*map_segment_line_loader)(FILE *, /* file to read */ + const char *, /* file name */ + void *, /* pointer to the element + to be filled */ + size_t); /* line number */ + +void map_load_segment(FILE *f, + const char *filename, + map_segment_line_loader loader, + const char *segment_name, + void *psegment, + int *segment_size, + size_t element_size, + size_t segment_max_size, + size_t *pline) { + assert (f); + assert (filename); + assert (segment_name); + assert (psegment); + assert (segment_size); + assert (pline); + + char kword[15], **segment = (char **)psegment; + int rcode; + + /* read until "segment_name" encountered */ + while (true) { + rcode = fscanf(f, "%14s", kword); + (*pline)++; + + if ((rcode == EOF) || (strcmp(kword, "end") == 0)) + log_fatal("[map %s] could not find segment %s\n", filename, segment_name); + + if (strcmp(kword, segment_name) == 0) + break; + } + + /* "segment_name" has been read, now read item number */ + rcode = fscanf(f, "%d", segment_size); + if (rcode == EOF) + log_fatal("[map %s] reached end of file while looking" + " for element count in segment %s (line %zu)\n", + filename, segment_name, *pline); + if ((*segment_size < 0) || (*segment_size > segment_max_size)) + log_fatal("[map %s] too many (%d) elements in " + "segment %s (should be <= %d) (line %zu)\n", + filename, *segment_size, segment_name, segment_max_size, *pline); + (*pline)++; + + /* allocate enough elements */ + *segment = calloc(*segment_size, element_size); + assert(*segment); + + log_info("[map %s] starting to read segment %s of size %zu\n", + filename, segment_name, *segment_size); + for (size_t i = 0; i < *segment_size; i++, (*pline)++) + loader(f, filename, &(*segment)[i * element_size], *pline); + log_info("[map %s] finished reading segment %s\n", filename, segment_name); +} + +void rd_segment_line_loader(FILE *f, + const char *filename, + void *p, + size_t line) { + assert (f); + assert (filename); + assert (p); + + char kword[15]; + road_t *rd = (road_t *)p; + int rcode = fscanf(f, "%14s", kword); + + if (rcode == EOF) + log_fatal("[map %s] 'line|arc' expected (line %zu)\n", filename, line); + + if (strcmp(kword, "line") == 0) { + int nbdir = 0; + /* read "line " */ + rcode = fscanf(f, "%d %d %f %f %f %f", + &nbdir, &rd->max_speed, + &rd->u.line.startp.x, &rd->u.line.startp.y, + &rd->u.line.endp.x, &rd->u.line.endp.y); + if ((rcode == EOF) || + (nbdir < 1) || + (nbdir > 2) || + (rd->max_speed <= 0) || + (rd->max_speed >= SPEED_MAX) || + (rd->u.line.startp.x <= MIN_X) || + (rd->u.line.startp.x >= MAX_X) || + (rd->u.line.endp.x <= MIN_X) || + (rd->u.line.endp.x >= MAX_X) || + (rd->u.line.startp.y <= MIN_Y) || + (rd->u.line.startp.y >= MAX_Y) || + (rd->u.line.endp.y <= MIN_Y) || + (rd->u.line.endp.y >= MAX_Y)) + log_fatal("[map_read %s]: 'line' format error (line %zu)\n", + filename, line); + + rd->color = COL_ROADLINE; + rd->kind = (nbdir == 1) ? RD_LINE_1 : RD_LINE_2; + rd->dir_x = (rd->u.line.endp.x - rd->u.line.startp.x); + rd->dir_y = (rd->u.line.endp.y - rd->u.line.startp.y); + } else if (strcmp(kword, "arc") == 0) { + int nbdir = 0; + /* read "arc + " */ + rcode = fscanf(f, "%d %d %f %f %f %f %f", + &nbdir, &rd->max_speed, + &rd->u.arc.center.x, &rd->u.arc.center.y, + &rd->u.arc.radius, + &rd->u.arc.start_angle, + &rd->u.arc.end_angle); + if ((rcode == EOF) || + (nbdir != 1) || + (rd->max_speed <= 0) || + (rd->max_speed >= SPEED_MAX) || + (rd->u.arc.center.x <= MIN_X) || + (rd->u.arc.center.x >= MAX_X) || + (rd->u.arc.center.y <= MIN_Y) || + (rd->u.arc.center.y >= MAX_Y) || + (rd->u.arc.radius <= 0) || + (rd->u.arc.start_angle < -180.0) || + (rd->u.arc.start_angle >= 360.0) || + (rd->u.arc.end_angle < -180.0) || + (rd->u.arc.end_angle >= 360.0)) + log_fatal("[map %s]: 'arc' format error (line %zu)\n", + filename, line); + rd->color = COL_ROADLINE; + rd->kind = RD_ARC; + rd->dir_x = 0.0; + rd->dir_y = (rd->u.arc.start_angle < rd->u.arc.end_angle) ? -1.0 : 1.0; + } else { + log_fatal("[map %s] unknown road type %s (line %zu)\n", + filename, kword, line); + } +} + +void wayp_segment_line_loader(FILE *f, + const char *filename, + void *p, + size_t line) { + assert (f); + assert (filename); + assert (p); + + int rcode; + waypoint_t *wp = (waypoint_t *)p; + + /* read */ + rcode = fscanf(f, "%d %f %f", &wp->road, &wp->position.x, &wp->position.y); + if ((rcode == EOF) || + (wp->road < 0) || + (wp->road >= map->road_sz) || + (wp->position.x <= MIN_X) || + (wp->position.x >= MAX_X) || + (wp->position.y <= MIN_Y) || + (wp->position.y >= MAX_Y)) { + log_fatal("[map %s] waypoint format error (line %zu)\n", filename, line); + } +} + +void tl_segment_line_loader(FILE *f, + const char *filename, + void *p, + size_t line) { + assert (f); + assert (filename); + assert (p); + + int rcode; + tlight_t *tl = (tlight_t *)p; + + /* read */ + rcode = fscanf(f, "%d %f %f %d %d %d %d", + &tl->road, + &tl->tl.ptl_pos.x, &tl->tl.ptl_pos.y, + &tl->tl.ptl_red, &tl->tl.ptl_amber, &tl->tl.ptl_green, + &tl->tl.ptl_phase); + if ((rcode == EOF) || + (tl->road < 0) || + (tl->road >= map->road_sz) || + (tl->tl.ptl_pos.x <= MIN_X) || + (tl->tl.ptl_pos.x >= MAX_X) || + (tl->tl.ptl_pos.y <= MIN_Y) || + (tl->tl.ptl_pos.y >= MAX_Y)) + log_fatal("[map %s] traffic light format error (line %zu)\n", + filename, line); +} + +void st_segment_line_loader(FILE *f, + const char *filename, + void *p, + size_t line) { + assert (f); + assert (filename); + assert (p); + + int rcode; + stop_t *st = (stop_t *)p; + + /* read */ + rcode = fscanf(f, "%d %d %f %f", + &st->road, &st->sema, + &st->position.x, &st->position.y); + if ((rcode == EOF) || + (st->road < 0) || + (st->road >= map->road_sz) || + (st->sema < 0) || + (st->sema >= map->tlight_sz) || + (st->position.x <= MIN_X) || + (st->position.x >= MAX_X) || + (st->position.y <= MIN_Y) || + (st->position.y >= MAX_Y)) + log_fatal("[map %s] stop format error (line %zu)\n", filename, line); +} + +void obst_segment_line_loader(FILE *f, + const char *filename, + void *p, + size_t line) { + assert (f); + assert (filename); + assert (p); + + int rcode; + obst_t *obst = (obst_t *)p; + + rcode = fscanf(f, "%f %f %f %f", &obst->pot_pos.x, &obst->pot_pos.y, + &obst->pot_since, &obst->pot_till); + if ((rcode == EOF) || + (obst->pot_pos.x <= MIN_X) || + (obst->pot_pos.x >= MAX_X) || + (obst->pot_pos.y <= MIN_Y) || + (obst->pot_pos.y >= MAX_Y)) + log_fatal("[map %s] obstacle format error (line %zu)\n", filename, line); +} + +void iti_segment_line_loader(FILE *f, + const char *filename, + void *p, + size_t line) { + assert (f); + assert (filename); + assert (p); + + char kword[15]; + iti_t *iti = (iti_t *)p; + int rcode = fscanf(f, "%14s %f", kword, &iti->param); + + if (strcmp(kword, "go") == 0) { + iti->act = Globals__Go; + if ((rcode == EOF) || (iti->param <= 0.f) || (iti->param > SPEED_MAX)) + log_fatal("[map %s] itinerary format error (action Go, line %zu)\n", + filename, line); + } else if (strcmp(kword, "turn") == 0) { + iti->act = Globals__Turn; + if ((rcode == EOF) || (iti->param < -180.0) || (iti->param >= 360.0)) + log_fatal("[map %s] itinerary format error (action Turn, line %zu)\n", + filename, line); + } else if (strcmp(kword, "stop") == 0) { + iti->act = Globals__Stop; + iti->param = 0.f; /* Dummy */ + } else { + log_fatal("[map %s] itinerary format error (unknown action %s, line %zu)\n", + filename, kword, line); + } +} + +void map_read_string_line(FILE *f, const char *filename, + const char *expected_kw, char *buff, size_t *pline) { + char kword[15]; + int rcode = fscanf(f, "%14s %249s", kword, buff); + if ((rcode == EOF) || (strcmp(kword, expected_kw) != 0)) + log_fatal("[map %s] could not read %s (line %zu)\n", + filename, expected_kw, *pline); + if (buff[0] != '"' || buff[strlen(buff) - 1] != '"') + log_fatal("[map %s] string after %s should be between double quotes" + " (line %zu)\n", + filename, expected_kw, *pline); + (*pline)++; + + /* Remove surrounding double quotes. */ + memmove(buff, buff + 1, strlen(buff) - 2); + buff[strlen(buff) - 2] = 0; + + log_info("[map %s] read %s: %s\n", filename, expected_kw, buff); +} + +void map_load(const char *filename) { + map = malloc(sizeof *map); + assert(map); + bzero(map, sizeof *map); + + size_t line = 1; + FILE *f = fopen(filename, "r"); + if (!f) + log_fatal("[map %s] could not open file\n", filename); + + map_read_string_line(f, filename, "map", map->name, &line); + map_read_string_line(f, filename, "graphics", map->graphics, &line); + map_read_string_line(f, filename, "guide", map->guide, &line); + + char kword[15]; + int rcode = fscanf(f, "%14s %f %f %f", kword, + &map->init_phase.ph_pos.x, + &map->init_phase.ph_pos.y, + &map->init_phase.ph_head); + if ((rcode == EOF) || (strcmp(kword, "init") != 0)) + log_fatal("[map %s] could not read init (line %zu)\n", filename, line); + line++; + log_info("[map %s] read init: x = %f, y = %f, head = %f\n", + filename, + map->init_phase.ph_pos.x, + map->init_phase.ph_pos.y, + map->init_phase.ph_head); + + /* Read the roads. */ + map_load_segment(f, filename, rd_segment_line_loader, "rd", + &map->road_arr, &map->road_sz, sizeof *map->road_arr, + MAX_ROAD_COUNT, &line); + + /* Read the waypoints. */ + map_load_segment(f, filename, wayp_segment_line_loader, "wp", + &map->wayp_arr, &map->wayp_sz, sizeof *map->wayp_arr, + MAX_WAYP_COUNT, &line); + + /* Read the traffic lights. */ + map_load_segment(f, filename, tl_segment_line_loader, "tl", + &map->tlight_arr, &map->tlight_sz, sizeof *map->tlight_arr, + MAX_TL_COUNT, &line); + + /* Read the stops. */ + map_load_segment(f, filename, st_segment_line_loader, "st", + &map->stop_arr, &map->stop_sz, sizeof *map->stop_arr, + MAX_STOP_COUNT, &line); + + /* Read the obstacles. */ + map_load_segment(f, filename, obst_segment_line_loader, "obst", + &map->obst_arr, &map->obst_sz, sizeof *map->obst_arr, + MAX_OBST_COUNT, &line); + + /* Read the obstacles. */ + map_load_segment(f, filename, iti_segment_line_loader, "iti", + &map->iti_arr, &map->iti_sz, sizeof *map->iti_arr, + MAX_ITI_COUNT, &line); + + /* Close file and return. */ + fclose(f); +} + +void map_destroy() { + if (!map) + return; + + /* Fields have been initialized to NULL, it is safe to call free() on them. */ + free(map->road_arr); + free(map->wayp_arr); + free(map->tlight_arr); + free(map->stop_arr); + free(map->obst_arr); + free(map); + map = NULL; +} + +void Map__read_obstacles_step(Map__read_obstacles_out *o) { + /* TODO very inefficient */ + memcpy(o->obst, map->obst_arr, map->obst_sz * sizeof *o->obst); + + /* The map file might contain fewer obstacles than Globals__obstnum. */ + for (size_t i = map->obst_sz; i < MAX_OBST_COUNT; i++) { + o->obst[i].pot_pos.x = 0.f; + o->obst[i].pot_pos.y = 0.f; + o->obst[i].pot_since = -1.f; + o->obst[i].pot_till = -1.f; + } +} + +void Map__read_itinerary_step(Map__read_itinerary_out *o) { + /* TODO very inefficient */ + memcpy(o->iti, map->iti_arr, map->iti_sz * sizeof *o->iti); + + /* The map file might contain fewer itinerary steps than Globals__itinum. */ + for (size_t i = map->iti_sz; i < MAX_ITI_COUNT; i++) { + o->iti[i].act = Globals__Stop; + o->iti[i].param = 0.f; + } +} + +void Map__read_traffic_lights_step(Map__read_traffic_lights_out *o) { + assert (map->tlight_sz <= Globals__trafnum); + for (size_t i = 0; i < map->tlight_sz; i++) + o->tlights[i] = map->tlight_arr[i].tl; + for (size_t i = map->tlight_sz; i < MAX_TL_COUNT; i++) + o->tlights[i] = (Globals__param_tlight){ {-100, -100}, 0, 0, 0, 0 }; +} + +bool colors_equal(const Globals__color *a, const Globals__color *b) { + return a->red == b->red && a->green == b->green && a->blue == b->blue; +} + +bool isOnRoadLine1(road_t *rd, float x, float y, + Globals__color *col, double *d, float *dir_x, float *dir_y) { + //test that may be inside road area + // -compute projection on line + float px = 0.0; + float py = 0.0; + dirProjPoint(x, y, &(rd->u.line.startp), &(rd->u.line.endp), &px, &py); + //-compare with ends of the segment + if ((px <= fmax(rd->u.line.startp.x, rd->u.line.endp.x)+EPS) && + (px >= fmin(rd->u.line.startp.x, rd->u.line.endp.x)-EPS) && + (py <= fmax(rd->u.line.startp.y, rd->u.line.endp.y)+EPS) && + (py >= fmin(rd->u.line.startp.y, rd->u.line.endp.y)-EPS)) + //the projection is inside, compute distance to rd + (*d) = distance(x, y, px, py); + else + //the projection is outside the segment + // let us test the extremities: + (*d) = fmin(distance(x,y,rd->u.line.startp.x,rd->u.line.startp.y), + distance(x,y,rd->u.line.endp.x,rd->u.line.endp.y)); + + //-compare with the width of the road + if ((*d) >= RD_SIZE_HALF_WIDTH) { + log_debug("[geometry] (%.2f, %.2f) is too far from road %p (dist. %.2f)!\n", + x, y, rd, *d); + return false; + } + + (*col) = rd->color; + (*dir_x) = rd->u.line.endp.x - rd->u.line.startp.x; + (*dir_y) = rd->u.line.endp.y - rd->u.line.startp.y; + + if ((*d) < RD_SIZE_LINE) { + //distance less than width of the road line, + //then on road with one color + //log_debug("Position on road line: %f!\n", *d); + } + else { + //otherwise, compute the side of the road + // left or right using direction(x, y)->(px, py) + double dirPX = px - x; /* -b where b = x1 - x2 */ + double dirPY = py - y; /* a where a = y2 - y1 */ + double sinDir = dirVProd(*dir_x, *dir_y, dirPX, dirPY); + if (sinDir < 0) { + //on left + (*col) = COL_ROADLEFT; +#ifndef MAP_BMP + if ((*d) <= RD_SIZE_LINE2) + col->green = (unsigned int)(255.*((*d)-1.0)); +#endif + } else { + //on right + (*col) = COL_ROADRIGHT; +#ifndef MAP_BMP + if ((*d) <= RD_SIZE_LINE2) + col->red = (unsigned int)(255.*((*d)-1.0)); +#endif + } + //log_debug("Position on road: %f!\n", *d); + } + return true; +} + +bool isOnRoadLine2(road_t *rd, float x, float y, + Globals__color *col, double *d, float *dir_x, float *dir_y) { + /* TODO missing from original project */ + return false; +} + +bool isOnRoadArc(/* IN */ + road_t *rd, float x, float y, + /* OUT */ + Globals__color *col, double *d, + float *dir_x, float *dir_y) +{ + //center + double cx = rd->u.arc.center.x; + double cy = rd->u.arc.center.y; + + //compute angle(in degrees) from center + double a = lineAngle(cx, cy, x, y); + + //compute signed distance to the circle c + double dist_c = distance(cx, cy, x, y) - rd->u.arc.radius; + + if (isInArc(a, rd->u.arc.start_angle,rd->u.arc.end_angle)) { + //within the angle + (*d) = fabs(dist_c); + } + else { + //Point outside road angles, + // consider the distance to extremities + (*d) = fmin(distance(x,y, + cx+rd->u.arc.radius*cos(toradian(rd->u.arc.start_angle)), + cy+rd->u.arc.radius*sin(toradian(rd->u.arc.start_angle))), + distance(x,y, + cx+rd->u.arc.radius*cos(toradian(rd->u.arc.end_angle)), + cy+rd->u.arc.radius*sin(toradian(rd->u.arc.end_angle)))); + } + + if ((*d) >= RD_SIZE_HALF_WIDTH) { + //log_debug("Position too far from road: %f!\n", (*d)); + return false; + } + + (*col) = rd->color; + + //direction is normal to d(c, (x, y)) + //its sign depends on rotation + if (rd->u.arc.start_angle < rd->u.arc.end_angle){//counterclockwise + (*dir_x) = cy - y; + (*dir_y) = x - cx; + } + else {//clockwise + (*dir_x) = y - cy; /* a where a = y2 - y1 */ + (*dir_y) = cx - x; /* b where b = x1 - x2 */ + } + + if ((*d) < RD_SIZE_LINE) { + //distance less than width of the road line, + //then on road with one color + //log_debug("Point on road line: %f!\n", *d); + } else { + //otherwise, compute the side of the road + // left or right using the sense(trigo or clock) of the road + if (((rd->u.arc.start_angle < rd->u.arc.end_angle) //trigo dir + && dist_c < 0) || + ((rd->u.arc.start_angle > rd->u.arc.end_angle) // clock dir + && dist_c > 0)) { + (*col) = COL_ROADLEFT; +#ifndef MAP_BMP + if ((*d) <= RD_SIZE_LINE2) + col->green = (unsigned int)(255.*((*d)-1.0)); +#endif + } else { + (*col) = COL_ROADRIGHT; +#ifndef MAP_BMP + if ((*d) <= RD_SIZE_LINE2) + col->red = (unsigned int)(255.*((*d)-1.0)); +#endif + } + } + //log_debug("Point on road: %f!\n", *d); + return true; +} + +int isOnTLight(int x, int y, int rd, float dir_x, float dir_y) +{ + if (map->tlight_arr == NULL || + map->tlight_sz <= 0) + return -1; + //no traffic light + + for (int i = 0; i < map->tlight_sz; i++) { + tlight_t *tl = &map->tlight_arr[i]; + if (tl->road != rd) + continue; + double d = distance(x, y, tl->tl.ptl_pos.x, tl->tl.ptl_pos.y); + + /* double cosdir = dirCos(tl->tl.ptl_pos.y - y, x - tl->tl.ptl_pos.x, + dir_x, dir_y); */ //MS + double cosdir = dirCos(tl->tl.ptl_pos.x-x, tl->tl.ptl_pos.y-y, + dir_x, dir_y); //EA + if (d < TL_VIEW && cosdir > TL_COSDIR) + return i; + } + return -1; +} + +bool isAfterStop(int x, int y, int rid, float dir_x, float dir_y, int* tl) +{ + (*tl) = -1; + + if (map->stop_arr == NULL || + map->stop_sz <= 0) + return false; + //no stop points + + for (int i = 0; i < map->stop_sz; i++) { + stop_t *sp = &map->stop_arr[i]; + if (sp->road != rid) + continue; + // check the distance to the point + road_t *rd = &map->road_arr[sp->road]; + if (rd->kind == RD_LINE_1 || + rd->kind == RD_LINE_2) { + // line + // - compute projection on roadline + float px = 0.0; + float py = 0.0; + dirProjPoint(x, y, &(rd->u.line.startp), &(rd->u.line.endp), &px, &py); + // - compare with the bounds of the stop point + double dsp = distance(px, py, sp->position.x, sp->position.y); + double dir_x = px - sp->position.x; + double dir_y = py - sp->position.y; + if (dsp >= (RD_SIZE_STOP-EPS) && + dsp <= (STP_AFTER+RD_SIZE_STOP+EPS) && + (dirProd(rd->dir_x, rd->dir_y, dir_x, dir_y) >= 0)) + { + log_debug("[geometry] (%d, %d) is after stop %d (dist %.2f)\n", + x, y, i, dsp); + (*tl) = sp->sema; + return true; + } + else + { + log_debug("Position too far/not AFTER point on line: %lf!\n", dsp); + log_debug("rd->dir(%lf,%lf) <> dir(%lf,%lf)\n", rd->dir_x, rd->dir_y, + dir_x, dir_y); + } + } + else { + // arc + // -compute angle of point in degrees + double apoint = lineAngle(rd->u.arc.center.x, rd->u.arc.center.y, + sp->position.x, sp->position.y); + double apos = lineAngle(rd->u.arc.center.x, rd->u.arc.center.y, x, y); + double dsp = toradian(fabs(apoint - apos))*rd->u.arc.radius; + if (dsp >= (RD_SIZE_STOP-EPS) && + dsp <= (STP_AFTER+RD_SIZE_STOP+EPS)) { + // - check that the direction is AFTER + if (fourAnglesInOrder(rd->u.arc.start_angle, + apoint, apos, + rd->u.arc.end_angle)) + { + log_debug("Position AFTER point on arc: %lf!\n", apos); + (*tl) = sp->sema; + return true; + } + else + { + log_debug("Position too far/not AFTER point on arc: %lf!\n", apos); + return false; + } + } + else { + log_debug("Position too far/not AFTER point on arc: %lf degrees!\n", apos); + } + } + } + return false; +} + +bool +isPositionOnPoint(float x, float y, road_t* rd, position_t* p, double pWidth) +{ + if (rd->kind == RD_LINE_1 || + rd->kind == RD_LINE_2) { // line + // - compute projection on road line + float px = 0.0; + float py = 0.0; + position_t startp = {0, 0}; + position_t endp = {0, 0}; + + startp = (rd->u.line.startp); + endp = (rd->u.line.endp); + dirProjPoint(x, y, &(startp), &(endp), &px, &py); + // - compare with the bounds of the waypoint + double dwp = distance(px, py, p->x, p->y); + if (dwp <= pWidth) { + log_debug("Position near point on line: %lf!\n", dwp); + return true; + } + else { + //log_debug("Position too far from point on line: %lf!\n", dwp); + return false; + } + } + else { + // arc + // - compute angle of point in degrees + double apoint = lineAngle(rd->u.arc.center.x, rd->u.arc.center.y, p->x, p->y); + double apos = lineAngle(rd->u.arc.center.x, rd->u.arc.center.y, x, y); + double dwp = toradian(fabs(apoint - apos))*rd->u.arc.radius; + if (dwp <= pWidth) { + log_debug("Position near point on arc: %lf!\n", dwp); + return true; + } + else { + log_debug("Position too far from point on arc: %lf!\n", dwp); + return false; + } + } +} + +Globals__color getColorPoint(int rid, float x, float y) { + // first go through waypoints + log_debug("[geometry] looking for waypoints at (%.2f, %.2f) on road %d\n", + x, y, rid); + for (int i = 0; i < map->wayp_sz; i++) { + waypoint_t *wp = &map->wayp_arr[i]; + if (wp->road != rid) + { + log_debug("Waypoint not on road %d!\n", rid); + continue; + } + road_t *rd = &map->road_arr[wp->road]; + // waitpoints are ruban + if (isPositionOnPoint(x, y, rd, &(wp->position), RD_SIZE_WAYPOINT)) { + return COL_WAYPOINT; + //one waypoint by position + } + } + log_debug("[geometry] finished walking waypoints\n"); + + // then go to stop points + log_debug("[geometry] looking for stops at (%.2f, %.2f) on road %d\n", + x, y, rid); + for (int i = 0; i < map->stop_sz; i++) { + stop_t *sp = &map->stop_arr[i]; + if (sp->road != rid) { + log_debug("Stop not on road %d!\n", rid); + continue; + } + road_t *rd = &map->road_arr[sp->road]; + // stop points are ruban + if (isPositionOnPoint(x, y, rd, &sp->position, RD_SIZE_STOP)) { + log_debug("[geometry] (%.2f, %.2f) at stop %d\n", x, y, i); + return COL_STOP; + //one stop by position + } + } + log_debug("[geometry] finished walking stops\n"); + + return COL_OUT; +} + +DEFINE_HEPT_FUN(Map, lookup_pos, (Globals__position pos)) { + float x = pos.x, y = pos.y; + + out->data.on_road = false; + out->data.color = COL_OUT; + out->data.max_speed = SPEED_MIN; + out->data.tl_number = -1; + out->data.tl_required = false; + out->data.dir_x = -1.0; + out->data.dir_y = 0.0; + + log_debug("[geometry] querying pos (%.2f, %.2f)\n", x, y); + if (map == NULL) + log_fatal("[geometry] map has not been initialized\n"); + + int min_rd = -1; + double min_d = 700.; + for (int rid = 0; rid < map->road_sz; rid++) { + road_t *rd = &map->road_arr[rid]; + double d = 0.; + Globals__color col = COL_OUT; + bool onRoad = false; + float dir_X = 0.0; + float dir_Y = 0.0; + switch (rd->kind) { + case RD_LINE_1: + onRoad = isOnRoadLine1(rd, x, y, &col, &d, &dir_X, &dir_Y); + break; + case RD_LINE_2: + onRoad = isOnRoadLine2(rd, x, y, &col, &d, &dir_X, &dir_Y); + break; + case RD_ARC: + onRoad = isOnRoadArc(rd, x, y, &col, &d, &dir_X, &dir_Y); + break; + case RD_OTHER: + break; + } + + if (onRoad && (d < min_d)) { + min_d = d; + min_rd = rid; + out->data.color = col; + out->data.dir_x = dir_X; + out->data.dir_y = dir_Y; + out->data.max_speed = map->road_arr[min_rd].max_speed; + /* Update color when a waypoint or stop. */ + col = getColorPoint(rid, x, y); + if (colors_equal(&col, &COL_OUT)) + out->data.color = out->data.color; + else if (colors_equal(&col, &COL_STOP)) { + /* TODO: update red color */ + out->data.color = col; + } + else { + /* TODO: update green color */ + out->data.color = col; + } + } + } + + /* Compute the return type. */ + if (min_rd >= 0) { + log_debug("[geometry] (%.2f, %.2f) is on road %d\n", x, y, min_rd); + out->data.on_road = true; + int tl = -1; + out->data.tl_number = + isOnTLight(x, y, min_rd, out->data.dir_x, out->data.dir_y); + out->data.tl_required = isAfterStop(x, y, min_rd, + out->data.dir_x, out->data.dir_y, &tl); + if(out->data.tl_required) { + if (tl != out->data.tl_number) { + log_debug("Warning: on TL %ld != ", out->data.tl_number); + log_debug("after TL %d!\n", tl); + } + out->data.tl_number = tl; + } + } + + /* Log the result. */ + log_debug("[geometry] { on_road = %d; color = (%d, %d, %d);" + " dir = (%2.2f, %2.2f); tl = (%d, %d); }\n", + out->data.on_road, + out->data.color.red, out->data.color.green, out->data.color.blue, + out->data.dir_x, out->data.dir_y, + out->data.tl_number, out->data.tl_required); +} + + +void play_asset_wav(SDL_AudioDeviceID audio_device, asset_wav_t *wav) { + if (!audio_device) { + log_info("[sdl] no audio device\n"); + return; + } + + if (SDL_QueueAudio(audio_device, wav->buffer, wav->size)) + log_fatal("[sdl] could not queue audio\n"); +} + +asset_wav_t collision, wrong_dir, exit_road, light_run, speed_excess; +SDL_AudioDeviceID audio_device = 0; + +DEFINE_HEPT_FUN(Map, soundEffects, (Globals__event evt, Globals__status sta)) { + if (sta == Globals__Preparing) + return; + if (evt.exitRoad) { + play_asset_wav(audio_device, &exit_road); + log_info("[audio] car left the road\n"); + } else if (evt.collisionEvent) { + play_asset_wav(audio_device, &collision); + log_info("[audio] car has collided with an obstacle\n"); + } else if (evt.dirEvent) { + play_asset_wav(audio_device, &wrong_dir); + log_info("[audio] car goes in the wrong direction\n"); + } else if (evt.lightRun) { + play_asset_wav(audio_device, &light_run); + log_info("[audio] car has run a red light\n"); + } else if (evt.speedExcess) { + play_asset_wav(audio_device, &speed_excess); + log_info("[audio] car is going too fast\n"); + } +} diff --git a/projet/src/map.epi b/projet/src/map.epi new file mode 100644 index 0000000..7316d3b --- /dev/null +++ b/projet/src/map.epi @@ -0,0 +1,7 @@ +open Globals + +external fun read_obstacles() returns (obst : param_obsts) +external fun read_itinerary() returns (iti : itielts) +external fun read_traffic_lights() returns (tlights : param_tlights) +external fun lookup_pos(pos : position) returns (data : map_data) +external fun soundEffects(evt : event; sta : status) returns () diff --git a/projet/src/map.h b/projet/src/map.h new file mode 100644 index 0000000..79aad6d --- /dev/null +++ b/projet/src/map.h @@ -0,0 +1,248 @@ +#ifndef MAP_H +#define MAP_H + +#include +#include +#include +#include + +#include + +#include "pervasives.h" +#include "hept_ffi.h" +#include "globals_types.h" + +#define MAX_ROAD_COUNT 50 /* maximum number of roads */ +#define MAX_WAYP_COUNT 50 /* maximum number of waypoints */ +#define MAX_TL_COUNT Globals__trafnum /* maximum number of traffic lights */ +#define MAX_STOP_COUNT 50 /* maximum number of stops */ +#define MAX_OBST_COUNT Globals__obstnum /* maximum number of obstacles */ +#define MAX_ITI_COUNT Globals__itinum /* maximum number of itinerary steps */ + +/* + * ========================================================================= + * Reference system + * ========================================================================= + */ + +/** Size of the Carthesian space (in cm) */ +#define MIN_X 0 +#define MIN_Y 0 +#define MAX_X 600 +#define MAX_Y 300 + +/** Precision of comparison between points */ +#define EPS 0.001 + +/** Type of points used, in a Carthesian space */ +/** see @code{positionTy} in kcg_types.h */ + +float toradian(float); + +/* + * ========================================================================= + * Car size + * ========================================================================= + */ + +/** + * Car plan with o-point is the position of camera + * + * +---------------+ + * | SC |SB | + * |----------o----| SA + * | |SD | + * +---------------+ + * + * Dimensions defined as constants in kcg_const.h + * #define SA (kcg_lit_float64(3.0)) + * #define SB (kcg_lit_float64(3.0)) + * #define SD (kcg_lit_float64(3.0)) + * #define SC (kcg_lit_float64(7.0)) + */ +#define CAR_WIDTH (SB+SD) +#define CAR_HALF_WIDTH SB + +/* + * ========================================================================= + * Colors + * ========================================================================= + */ + +/** Color type @code{colorTy} defined in @see{kcg_types.h} */ + +/** True colors used + * defined in @see{kcg_const.h} as constants of SCADE + * + * extern const colorTy AMBER; + * extern const colorTy GRAY; + * extern const colorTy CYAN; + * extern const colorTy MAGENTA; + * extern const colorTy YELLOW; + * extern const colorTy BLUE; + * extern const colorTy GREEN; + * extern const colorTy RED; + */ + +/** Symbolic colors used for road, points, etc */ +#define COL_ROADLINE Globals__blue +#define COL_ROADLEFT Globals__cyan +#define COL_ROADRIGHT Globals__magenta +#define COL_WAYPOINT Globals__green +#define COL_STOP Globals__red +#define COL_OUT Globals__gray + +/** String colors used to print */ +#define STR_ROADLINE "blue" +#define STR_ROADLEFT "cyan" +#define STR_ROADRIGHT "magenta" +#define STR_WAYPOINT "yellow" +#define STR_STOP "red" +#define STR_TLIGHT "orange" +#define STR_OUT "gray" + +/* + * ========================================================================= + * Road segments and points + * ========================================================================= + */ + +/** Kind of road segments */ +typedef enum { + RD_LINE_1 = 0, /* linear road, one direction */ + RD_LINE_2, /* linear road, two directions */ + RD_ARC, /* arc road, one direction */ + RD_OTHER /* INTERNAL USE ONLY */ +} road_kind_t; + +/** Dimensions used for road (in cm) */ +#define RD_SIZE_LINE 1.0 +#define RD_SIZE_LINE2 2.0 +#define RD_SIZE_HALF_WIDTH 10.0 /* 10.0 in simulator version */ +#define RD_SIZE_WAYPOINT 1.0 /* the size of the half of the ruban */ +#define RD_SIZE_STOP 1.0 + +/** Dimensions for traffic lights (in cm) */ +#define TL_NUMBER 5 +#define TL_VIEW 50.0 +#define TL_COSDIR 0.5 +#define STP_AFTER 3.0 + +/** Speeds */ +#define SPEED_MIN 20 +#define SPEED_MAX 40 + +typedef Globals__phase phase_t; + +/** Road segment */ +typedef struct { + road_kind_t kind; /* kind to select informations */ + + Globals__color color; /* color used on the middle line */ + float dir_x, dir_y; /* direction vector/sense for line/arc (x) */ + int max_speed; /* maximal speed */ + + union { + struct { /* parameters for a linear road */ + Globals__position startp; /* on the middle line */ + Globals__position endp; /* on the middle line */ + } line; + struct { /* parameters for a arc road */ + Globals__position center; /* the circle center */ + float radius; /* the circle radius (in cm) */ + float start_angle; /* start and stop angles */ + float end_angle; + } arc; + } u; +} road_t; + +/** Waypoint used to consult the map */ +typedef struct { + int road; /* road identifier */ + Globals__position position; /* on the middle line of the road */ +} waypoint_t; + +/** Stop point used to signal a traffic light */ +typedef struct { + int road; /* road identifier */ + int sema; /* traffic light identifier */ + Globals__position position; /* on the middle line of a road */ +} stop_t; + +/** Traffic lights: added road reference to type + * @code{paramTLTy_City} defined in @see{kcg_tpes.h} */ +typedef struct { + Globals__param_tlight tl; + int road; /* road identifier that the tlight controls */ +} tlight_t; + +typedef Globals__param_obst obst_t; + +typedef Globals__itielt iti_t; + +typedef Globals__position position_t; + +/* + * ========================================================================= + * Maps + * ========================================================================= + */ + +/** One map contains roads, waypoints, traffic lights and stop points */ +typedef struct { + char name[255]; /* Name of the map */ + char graphics[255]; /* Path to graphics file (bmp) */ + char guide[255]; /* Path to guide file (bmp) */ + phase_t init_phase; /* Initial phase of the robot */ + road_t *road_arr; /* Roads */ + int tlight_sz; /* Road count */ + waypoint_t *wayp_arr; /* Waypoints */ + int road_sz; /* Waypoint count */ + tlight_t *tlight_arr; /* Traffic lights */ + int wayp_sz; /* Traffic light count */ + stop_t *stop_arr; /* Stops */ + int stop_sz; /* Stop count */ + obst_t *obst_arr; /* Obstacles */ + int obst_sz; /* Obstacle count */ + iti_t *iti_arr; /* Itinerary */ + int iti_sz; /* Itinerary step count */ +} map_t; + +extern map_t *map; + +void map_load(const char *); /* parse and load global map file */ +void map_destroy(); /* free the map loaded via load_map() */ + +/* + * ========================================================================= + * Functions exported to the Heptagon side + * ========================================================================= + */ + +typedef struct asset_wav { + SDL_AudioSpec spec; + Uint8 *buffer; + Uint32 size; +} asset_wav_t; + +extern asset_wav_t collision, wrong_dir, exit_road, light_run, speed_excess; +extern SDL_AudioDeviceID audio_device; + +DECLARE_HEPT_FUN_NULLARY(Map, + read_obstacles, + Globals__param_obsts obst); +DECLARE_HEPT_FUN_NULLARY(Map, + read_traffic_lights, + Globals__param_tlights tlights); +DECLARE_HEPT_FUN_NULLARY(Map, + read_itinerary, + Globals__itielts iti); +DECLARE_HEPT_FUN(Map, + lookup_pos, + (Globals__position), + Globals__map_data data); +DECLARE_HEPT_FUN(Map, + soundEffects, + (Globals__event, Globals__status),); + +#endif /* MAP_H */ diff --git a/projet/src/map_types.h b/projet/src/map_types.h new file mode 100644 index 0000000..be6d50b --- /dev/null +++ b/projet/src/map_types.h @@ -0,0 +1,4 @@ +#ifndef MAP_TYPES_H +#define MAP_TYPES_H + +#endif /* MAP_TYPES_H */ diff --git a/projet/src/mathext.c b/projet/src/mathext.c new file mode 100644 index 0000000..932c18d --- /dev/null +++ b/projet/src/mathext.c @@ -0,0 +1,42 @@ +#include +#include +#include + +#include "mymath.h" +#include "mathext.h" + +void Mathext__float_step(int x, Mathext__float_out *o) { + o->o = (float)x; +} + +void Mathext__int_step(float x, Mathext__int_out *o) { + o->o = (int)x; +} + +void Mathext__floor_step(float x, Mathext__floor_out *o) { + o->o = floorf(x); +} + +void Mathext__sin_step(float x, Mathext__sin_out *o) { + o->o = sinf(x); +} + +void Mathext__cos_step(float x, Mathext__cos_out *o) { + o->o = cosf(x); +} + +void Mathext__atan2_step(float y, float x, Mathext__atan2_out *o) { + o->o = atan2f(y, x); +} + +void Mathext__hypot_step(float x, float y, Mathext__hypot_out *o) { + o->o = hypotf(x, y); +} + +void Mathext__sqrt_step(float x2, Mathext__sqrt_out *o) { + o->o = sqrtf(x2); +} + +void Mathext__modulo_step(int x, int y, Mathext__modulo_out *o) { + o->o = x % y; +} diff --git a/projet/src/mathext.epi b/projet/src/mathext.epi new file mode 100644 index 0000000..2da6000 --- /dev/null +++ b/projet/src/mathext.epi @@ -0,0 +1,11 @@ +external fun float(x : int) returns (o : float) +external fun int(x : float) returns (o : int) +external fun floor(x : float) returns (o : float) + +external fun sin(x : float) returns (o : float) +external fun cos(x : float) returns (o : float) +external fun atan2(y : float; x : float) returns (o : float) +external fun hypot(x : float; y : float) returns (o : float) +external fun sqrt(x2 : float) returns (o : float) + +external fun modulo(x : int; y : int) returns (o : int) diff --git a/projet/src/mathext.h b/projet/src/mathext.h new file mode 100644 index 0000000..8f01c47 --- /dev/null +++ b/projet/src/mathext.h @@ -0,0 +1,22 @@ +#ifndef MATHEXT_H +#define MATHEXT_H + +#include "stdbool.h" +#include "assert.h" +#include "pervasives.h" + +#include "hept_ffi.h" + +DECLARE_HEPT_FUN(Mathext, float, (int), float o); +DECLARE_HEPT_FUN(Mathext, int, (float), int o); +DECLARE_HEPT_FUN(Mathext, floor, (float), float o); + +DECLARE_HEPT_FUN(Mathext, sin, (float), float o); +DECLARE_HEPT_FUN(Mathext, cos, (float), float o); +DECLARE_HEPT_FUN(Mathext, atan2, (float, float), float o); +DECLARE_HEPT_FUN(Mathext, hypot, (float, float), float o); +DECLARE_HEPT_FUN(Mathext, sqrt, (float), float o); + +DECLARE_HEPT_FUN(Mathext, modulo, (int, int), int o); + +#endif /* MATHEXT_H */ diff --git a/projet/src/mathext_types.h b/projet/src/mathext_types.h new file mode 100644 index 0000000..00a5ee7 --- /dev/null +++ b/projet/src/mathext_types.h @@ -0,0 +1,4 @@ +#ifndef MATHEXT_TYPES_H +#define MATHEXT_TYPES_H + +#endif /* MATHEXT_TYPES_H */ diff --git a/projet/src/mymath.h b/projet/src/mymath.h new file mode 100644 index 0000000..8641f1d --- /dev/null +++ b/projet/src/mymath.h @@ -0,0 +1,13 @@ +#ifndef MYMATH_H +#define MYMATH_H + +/* Avoid Heptagon's math.h. */ +#ifndef __APPLE__ +#include +#endif + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +#endif /* MYMATH_H */ diff --git a/projet/src/simulation_loop.c b/projet/src/simulation_loop.c new file mode 100644 index 0000000..12c3965 --- /dev/null +++ b/projet/src/simulation_loop.c @@ -0,0 +1,414 @@ +/* This file is part of SyncContest. + Copyright (C) 2017-2020 Eugene Asarin, Mihaela Sighireanu, Adrien Guatto. */ + +#include "simulation_loop.h" + +#include + +#include "mymath.h" +#include "challenge.h" +#include "cutils.h" +#include "map.h" + +#ifndef ASSET_DIR_PATH +#define ASSET_DIR_PATH "./assets" +#endif + +void find_absolute_asset_path(char *path, size_t path_size, + const char *filename) { + snprintf(path, path_size, "%s/%s", ASSET_DIR_PATH, filename); +} + +void load_asset_wav(const char *filename, asset_wav_t *wav) { + assert (filename); + assert (wav); + + char filepath[512]; + + find_absolute_asset_path(filepath, sizeof filepath, filename); + if (!SDL_LoadWAV(filepath, &wav->spec, &wav->buffer, &wav->size)) + log_fatal("[sdl] could not open WAV file %s (%s)\n", + filepath, SDL_GetError()); +} + +void free_asset_wav(asset_wav_t *wav) { + assert (wav); + SDL_FreeWAV(wav->buffer); + wav->buffer = NULL; +} + +SDL_Surface *load_asset_bmp_surface(const char *filename) { + SDL_Surface *r; + char filepath[512]; + + find_absolute_asset_path(filepath, sizeof filepath, filename); + + if ((r = SDL_LoadBMP(filepath)) == NULL) + log_fatal("[sdl] could not load %s\n", filepath); + + return r; +} + +SDL_Texture *load_asset_bmp_texture(const char *filename, SDL_Renderer *r, + int *texture_width, int *texture_height) { + SDL_Surface *s = load_asset_bmp_surface(filename); + SDL_Texture *t = SDL_CreateTextureFromSurface(r, s); + + if (!t) + log_fatal("[sdl] could not load texture from surface %s (%s)\n", + filename, SDL_GetError()); + SDL_FreeSurface(s); + + if (texture_width && texture_height + && SDL_QueryTexture(t, NULL, NULL, texture_width, texture_height)) + log_fatal("[sdl] could not query texture %s (%s)\n", + filename, SDL_GetError()); + + log_info("[sdl] loaded texture %s\n", filename); + return t; +} + +/* Our simulation assumes the origin is on the bottom-left corner of the window, + unlike SDL which assumes it is on the top-left corner. The function below + transforms a point from geometric space into SDL space. */ +void sdl_space_of_position(Globals__position *position, int *x, int *y) { + *x = position->x; + *y = MAX_Y - position->y; +} + +void sdl_point_of_position(Globals__position *position, SDL_Point *point) { + sdl_space_of_position(position, &point->x, &point->y); +} + +int draw_point(SDL_Renderer *rd, Globals__position *p) { + int x, y; + sdl_space_of_position(p, &x, &y); + return SDL_RenderDrawPoint(rd, x, y); +} + +int draw_line(SDL_Renderer *rd, + Globals__position *startp, Globals__position *endp) { + int x1, y1, x2, y2; + sdl_space_of_position(startp, &x1, &y1); + sdl_space_of_position(endp, &x2, &y2); + return SDL_RenderDrawLine(rd, x1, y1, x2, y2); +} + +void draw_rectangle(SDL_Renderer *rd, Globals__position *center, size_t l, + uint32_t r, uint32_t g, uint32_t b) { + SDL_Rect rect = (SDL_Rect){ 0, 0, l, l }; + /* Compute coordinates. */ + sdl_space_of_position(center, &rect.x, &rect.y); + rect.x -= l / 2; + rect.y -= l / 2; + /* Render rectangle. */ + SDL_SetRenderDrawColor(rd, r, g, b, 0x00); + if (SDL_RenderFillRect(rd, &rect) < 0) + log_fatal("[sdl] could not draw rectangle (%s)\n", SDL_GetError()); + SDL_SetRenderDrawColor(rd, 0xFF, 0xFF, 0xFF, 0); + if (draw_point(rd, center) < 0) + log_fatal("[sdl] could not draw point (%s)\n", SDL_GetError()); +} + +void draw_tile(SDL_Renderer *rd, + SDL_Texture *texture, + Globals__position *p, + double angle, + int w, + int h) { + SDL_Rect dst_rect = { 0, 0, w, h }; + Globals__position center = { p->x - w / 2, p->y + h / 2 }; + sdl_space_of_position(¢er, &dst_rect.x, &dst_rect.y); + + SDL_RenderCopyEx(rd, /* renderer */ + texture, /* texture */ + NULL, /* entire texture */ + &dst_rect, /* destination */ + angle, /* angle (cw) */ + NULL, /* center dst_rec */ + SDL_FLIP_NONE); /* no flipping */ +} + +race_result_t simulation_loop(bool show_guide, + int initial_top, + float sps, + bool headless, + bool audio, + size_t max_synchronous_steps) { + SDL_Window *w; + bool quit = false; /* Shall we quit? */ + race_result_t res = RACE_TIMEOUT; /* Did we complete the race? */ + int top = initial_top; /* Has the race started? */ + bool verbose = false, debug = false; + int car_w, car_h, obs_w, obs_h; + SDL_Texture *bg, *car, *obs; + SDL_Renderer *r; + size_t current_tick = 0; + + /* Initialize SDL and acquire resources. */ + + if (!headless) { + if (SDL_Init(SDL_INIT_VIDEO | SDL_INIT_AUDIO) < 0) + log_fatal("[sdl] could not initialize SDL library (%s)\n", + SDL_GetError()); + + if ((w = SDL_CreateWindow("Synchronous Contest " YEAR " v" VERSION, + SDL_WINDOWPOS_UNDEFINED, + SDL_WINDOWPOS_UNDEFINED, + MAX_X, + MAX_Y, + SDL_WINDOW_SHOWN)) == NULL) + log_fatal("[sdl] could not open window (%s)\n", SDL_GetError()); + + r = SDL_CreateRenderer(w, -1, SDL_RENDERER_ACCELERATED); + if (!r) + log_fatal("[sdl] could not create renderer (%s)\n", SDL_GetError()); + + if (!r) + log_fatal("[sdl] could not create renderer (%s)\n", SDL_GetError()); + + bg = load_asset_bmp_texture(show_guide ? map->guide : map->graphics, + r, + NULL, + NULL); + car = load_asset_bmp_texture("orange.bmp", + r, + &car_w, + &car_h); + obs = load_asset_bmp_texture("obst.bmp", + r, + &obs_w, + &obs_h); + + /* Load sounds and setup audio device. */ + + load_asset_wav("collision.wav", &collision); + load_asset_wav("direction.wav", &wrong_dir); + load_asset_wav("exit.wav", &exit_road); + load_asset_wav("light.wav", &light_run); + load_asset_wav("speed.wav", &speed_excess); + + if (audio) { + if (!(audio_device = + SDL_OpenAudioDevice(NULL, 0, &collision.spec, NULL, 0))) { + log_info("[sdl] could not open audio device\n"); + } else + SDL_PauseAudioDevice(audio_device, 0); + } + } + + /* Initialize synchronous state. */ + + Challenge__the_challenge_out out; + Challenge__the_challenge_mem mem; + Challenge__the_challenge_reset(&mem); + + /* Setup time counters. */ + + const uint32_t sync_dt_ms = 1000.f * Globals__timestep; + const uint32_t simulation_dt_ms = (1. / (double)sps) * 1000.; + uint32_t time_budget_ms = sync_dt_ms; /* Enough to do one initial step. */ + + log_info("[simulation] starting (%zu ms/cycle)\n", simulation_dt_ms); + + while (!quit + && (!max_synchronous_steps || current_tick < max_synchronous_steps)) { + SDL_Event e; + uint32_t start_time_ms = SDL_GetTicks(); + + /* Perform as many synchronous steps as possible within our budget. */ + while (time_budget_ms >= sync_dt_ms + && (!max_synchronous_steps + || current_tick < max_synchronous_steps)) { + time_budget_ms -= sync_dt_ms; + Challenge__the_challenge_step(map->init_phase, top, &out, &mem); + current_tick++; + /* Check robot status once simulation has started. */ + if (top) { + switch (out.sta) { + case Globals__Preparing: + case Globals__Running: + break; + case Globals__Arrived: + log_info("[simulation %08zu] race finished\n", current_tick); + res = RACE_SUCCESS; + quit = true; + break; + case Globals__Stopped: + log_info("[simulation %08zu] car stopped\n", current_tick); + res = RACE_CRASH; + quit = true; + break; + } + } + if (!debug && !verbose && !quit) { + printf("\e[?25l"); /* Disable cursor */ + printf("H %06.2f\tV %06.2f\tT %06.2f\tS %09d\r", + out.ph.ph_head, out.ph.ph_vel, out.time, out.scoreA); + printf("\e[?25h"); /* Re-enable cursor */ + } + } + + if (!headless) { + /* Process events, including key presses. */ + while (SDL_PollEvent(&e) != 0) { + switch (e.type) { + case SDL_QUIT: + quit = true; + break; + case SDL_KEYDOWN: + switch (e.key.keysym.sym) { + case SDLK_q: + quit = true; + break; + case SDLK_t: + top = true; + break; + case SDLK_d: + debug = !debug; + break; + case SDLK_v: + if (verbose) + log_set_verbosity_level(LOG_INFO); + else + log_set_verbosity_level(LOG_DEBUG); + verbose = !verbose; + break; + case SDLK_UP: + map->init_phase.ph_head += 2; + break; + case SDLK_DOWN: + map->init_phase.ph_head -= 2; + break; + } + break; + } + } + + /* Render the scene, which includes the background as well as the car. */ + SDL_SetRenderDrawColor(r, 0xFF, 0xFF, 0xFF, 0xFF); + + SDL_RenderClear(r); + SDL_RenderCopy(r, bg, NULL, NULL); + + if (!debug) + draw_tile(r, car, &out.ph.ph_pos, 360.f - out.ph.ph_head, car_w, car_h); + else { + /* In debug mode, render the car as a plain square. */ + Globals__phase *ph; Globals__position endp; float f = 20.0; + if (top) { + ph = &out.ph; + f *= ph->ph_vel / SPEED_MAX; + } else + ph = &map->init_phase; + draw_rectangle(r, &ph->ph_pos, 5, 0x00, 0x00, 0x00); + /* Draw direction vector. */ + endp.x = ph->ph_pos.x + f * cos(ph->ph_head / 360. * 2. * M_PI); + endp.y = ph->ph_pos.y + f * sin(ph->ph_head / 360. * 2. * M_PI); + SDL_SetRenderDrawColor(r, 0x00, 0x00, 0x00, 0x00); + draw_line(r, &ph->ph_pos, &endp); + /* Draw detected map vector. */ + Map__lookup_pos_out out; + Map__lookup_pos_step(ph->ph_pos, &out); + endp.x = ph->ph_pos.x + out.data.dir_x; + endp.y = ph->ph_pos.y + out.data.dir_y; + SDL_SetRenderDrawColor(r, 0xAE, 0xB4, 0xC0, 0x00); + draw_line(r, &ph->ph_pos, &endp); + /* Reset color. */ + SDL_SetRenderDrawColor(r, 0xFF, 0xFF, 0xFF, 0); + } + + /* We draw the signalization info, when relevant. */ + if (!debug) { + for (size_t i = 0; i < MAX_OBST_COUNT; i++) { + Globals__obstacle *o = &out.sign.si_obstacles[i]; + if (o->o_pres) + draw_tile(r, obs, &o->o_pos, 0.f, obs_w, obs_h); + } + + for (size_t i = 0; i < MAX_TL_COUNT; i++) { + Utilities__encode_color_out enc; + Globals__traflight *t = &out.sign.si_tlights[i]; + Utilities__encode_color_step(t->tl_color, &enc); + draw_rectangle(r, &t->tl_pos, 10, enc.a.red, enc.a.green, enc.a.blue); + } + } + + /* In debug mode, we also render the raw information coming from the + map. This is useful to understand the map file contents. */ + if (debug) { + SDL_SetRenderDrawColor(r, 0x00, 0x00, 0xFF, 0xFF); + for (size_t i = 0; i < map->road_sz; i++) { + switch (map->road_arr[i].kind) { + case RD_LINE_1: + draw_line(r, + &map->road_arr[i].u.line.startp, + &map->road_arr[i].u.line.endp); + break; + default: + /* TODO draw curved roads. */ + break; + } + } + + /* Draw waypoints. */ + for (size_t i = 0; i < map->wayp_sz; i++) + draw_rectangle(r, &map->wayp_arr[i].position, + 10, 0xFF, 0x00, 0x00); + + /* Draw traffic lights. */ + for (size_t i = 0; i < map->tlight_sz; i++) + draw_rectangle(r, &map->tlight_arr[i].tl.ptl_pos, + 10, 0x00, 0x00, 0xFF); + + /* Draw stops. */ + for (size_t i = 0; i < map->stop_sz; i++) + draw_rectangle(r, &map->stop_arr[i].position, + 10, 0x84, 0x21, 0xFF); + + /* Draw obstacles. */ + for (size_t i = 0; i < map->obst_sz; i++) + draw_rectangle(r, &map->obst_arr[i].pot_pos, + 10, 0x12, 0xAE, 0x00); + } + + SDL_RenderPresent(r); + } + + /* Sleep for our remaining per-simulation time. */ + uint32_t stop_time_ms = SDL_GetTicks(); + uint32_t frame_time_ms = stop_time_ms - start_time_ms; + if (frame_time_ms < simulation_dt_ms) { + log_debug("[simulation %08zu] %zu elapsed, sleeping for %zu ms\n", + current_tick, frame_time_ms, simulation_dt_ms - frame_time_ms); + SDL_Delay(simulation_dt_ms - frame_time_ms); + } + + /* Accumulate time for the synchronous step. */ + time_budget_ms += fmax(simulation_dt_ms, frame_time_ms); + } + log_info("[simulation %08zu] shutting down, score = %zu, time = %f\n", + current_tick, out.scoreA, out.time); + + /* Wait for audio queue to be empty. */ + if (audio_device) { + Uint32 audio_buffered; + while ((audio_buffered = SDL_GetQueuedAudioSize(audio_device)) != 0) + SDL_Delay(50); + } + + free_asset_wav(&collision); + free_asset_wav(&wrong_dir); + free_asset_wav(&exit_road); + free_asset_wav(&light_run); + free_asset_wav(&speed_excess); + + SDL_DestroyTexture(obs); + SDL_DestroyTexture(car); + SDL_DestroyTexture(bg); + SDL_DestroyRenderer(r); + SDL_DestroyWindow(w); + SDL_Quit(); + + return res; +} diff --git a/projet/src/simulation_loop.h b/projet/src/simulation_loop.h new file mode 100644 index 0000000..388e4a8 --- /dev/null +++ b/projet/src/simulation_loop.h @@ -0,0 +1,23 @@ +/* This file is part of SyncContest. + Copyright (C) 2017-2020 Eugene Asarin, Mihaela Sighireanu, Adrien Guatto. */ + +#ifndef SIMULATION_LOOP_H +#define SIMULATION_LOOP_H + +#include +#include + +typedef enum { + RACE_SUCCESS, + RACE_CRASH, + RACE_TIMEOUT +} race_result_t; + +race_result_t simulation_loop(bool show_guide, + int initial_top, + float sps, + bool headless, + bool audio, + size_t max_synchronous_steps); + +#endif /* SIMULATION_LOOP_H */ diff --git a/projet/src/trace.c b/projet/src/trace.c new file mode 100644 index 0000000..f5b0b69 --- /dev/null +++ b/projet/src/trace.c @@ -0,0 +1,70 @@ +#include "trace.h" + +#include +#include +#include +#include + +#include "trace_lib.h" + +trace_file_t *trace = NULL; + +#define HEPT_TRACE_ENV_VAR "HEPT_TRACE" + +void hept_trace_init() { + if (trace || !getenv(HEPT_TRACE_ENV_VAR)) + return; + trace = trace_file_alloc(TRACE_TIME_UNIT_S, 1); +} + +void hept_trace_quit() { + if (trace) { + trace_file_write(trace, getenv(HEPT_TRACE_ENV_VAR)); + trace_file_free(trace); + trace = NULL; + } +} + +static inline void trace_samples(trace_signal_t **signal, + const char *name, trace_signal_type_t type, + void *samples, size_t count) { + if (!trace) + return; + + if (!*signal) { + *signal = trace_file_lookup_signal(trace, name); + if (!*signal) { + *signal = trace_signal_alloc(name, type, 1 << 17); + if (!trace_file_add_signal(trace, *signal)) { + perror("trace_file_add_signal()\n"); + exit(EXIT_FAILURE); + } + } + } + assert (*signal); + trace_add_samples(*signal, samples, count); +} + +DEFINE_HEPT_NODE_RESET(Trace, trace_bool) { + mem->signal = NULL; +} + +DEFINE_HEPT_NODE_STEP(Trace, trace_bool, (string name, int v)) { + trace_samples(&mem->signal, name, TRACE_SIGNAL_TYPE_BOOL, &v, 1); +} + +DEFINE_HEPT_NODE_RESET(Trace, trace_int) { + mem->signal = NULL; +} + +DEFINE_HEPT_NODE_STEP(Trace, trace_int, (string name, int v)) { + trace_samples(&mem->signal, name, TRACE_SIGNAL_TYPE_INT, &v, 1); +} + +DEFINE_HEPT_NODE_RESET(Trace, trace_float) { + mem->signal = NULL; +} + +DEFINE_HEPT_NODE_STEP(Trace, trace_float, (string name, float v)) { + trace_samples(&mem->signal, name, TRACE_SIGNAL_TYPE_FLOAT, &v, 1); +} diff --git a/projet/src/trace.epi b/projet/src/trace.epi new file mode 100644 index 0000000..20eb209 --- /dev/null +++ b/projet/src/trace.epi @@ -0,0 +1,6 @@ +(* A basic library for producing VCD files from Heptagon code, typically used + for debugging purposes. *) + +external node trace_bool(name : string; v : bool) returns () +external node trace_int(name : string; v : int) returns () +external node trace_float(name : string; v : float) returns () diff --git a/projet/src/trace.h b/projet/src/trace.h new file mode 100644 index 0000000..c0919de --- /dev/null +++ b/projet/src/trace.h @@ -0,0 +1,17 @@ +#ifndef TRACE_H +#define TRACE_H + +#include +#include + +#include "hept_ffi.h" +#include "trace_lib.h" + +void hept_trace_init(); +void hept_trace_quit(); + +DECLARE_HEPT_NODE(Trace, trace_bool, (string, int),, trace_signal_t *signal); +DECLARE_HEPT_NODE(Trace, trace_int, (string, int),, trace_signal_t *signal); +DECLARE_HEPT_NODE(Trace, trace_float, (string, float),, trace_signal_t *signal); + +#endif /* TRACE */ diff --git a/projet/src/trace_lib.c b/projet/src/trace_lib.c new file mode 100644 index 0000000..9ef24a3 --- /dev/null +++ b/projet/src/trace_lib.c @@ -0,0 +1,328 @@ +#include "trace_lib.h" + +#include +#include +#include +#include +#include +#include + +#include "buffer.h" + +size_t trace_sizeof_signal_type(trace_signal_type_t type) { + switch (type) { + case TRACE_SIGNAL_TYPE_BOOL: + return sizeof(int); + case TRACE_SIGNAL_TYPE_INT: + return sizeof(int); + case TRACE_SIGNAL_TYPE_FLOAT: + return sizeof(float); + } + /* UNREACHABLE */ + assert (false); + return 0; +} + +typedef struct trace_signal { + char *name; + trace_signal_type_t type; + buffer_t *samples; +} trace_signal_t; + +trace_signal_t *trace_signal_alloc(const char *name, + trace_signal_type_t type, + size_t initial_buffer_size) { + trace_signal_t *res = malloc_checked(sizeof *res); + res->name = strdup_checked(name); + res->type = type; + res->samples = + buffer_alloc(initial_buffer_size * trace_sizeof_signal_type(type)); + return res; +} + +void trace_signal_free(trace_signal_t *signal) { + assert (signal); + + buffer_free(signal->samples); + free(signal->name); + free(signal); +} + +void trace_add_samples(trace_signal_t *signal, void *samples, size_t count) { + assert (signal); + assert (samples); + buffer_write(signal->samples, + samples, + count * trace_sizeof_signal_type(signal->type)); +} + +const char *trace_time_unit_repr(trace_time_unit_t u) { + const char *table[] = { "s", "ms", "us", "ns", "ps", "fs" }; + assert (TRACE_TIME_UNIT_S <= u && u <= TRACE_TIME_UNIT_FS); + return table[u]; +} + +typedef struct trace_file { + buffer_t *signals; + trace_time_unit_t time_unit; + size_t time_unit_factor; +} trace_file_t; + +trace_file_t *trace_file_alloc(trace_time_unit_t time_unit, + size_t time_unit_factor) { + trace_file_t *trace = malloc_checked(sizeof *trace); + trace->time_unit = time_unit; + trace->time_unit_factor = time_unit_factor; + trace->signals = buffer_alloc(10 * sizeof(trace_signal_t)); + + return trace; +} + +void trace_file_free(trace_file_t *trace) { + assert (trace); + + /* Free buffers. */ + buffer_foreach (trace_signal_t *, psig, trace->signals) + trace_signal_free(*psig); + buffer_free(trace->signals); + + free(trace); +} + +trace_signal_t *trace_file_lookup_signal(const trace_file_t *trace, + const char *signal_name) { + assert (trace); + assert (signal_name); + + trace_signal_t *res = NULL; + + buffer_foreach (trace_signal_t *, psig, trace->signals) { + if (strcmp((*psig)->name, signal_name) == 0) { + res = *psig; + break; + } + } + + return res; +} + +bool trace_file_add_signal(const trace_file_t *trace, trace_signal_t *signal) { + assert (trace); + assert (signal); + + if (trace_file_lookup_signal(trace, signal->name)) + return false; + + buffer_write(trace->signals, &signal, sizeof signal); + return true; +} + +typedef void (trace_backend_write_header_f)(FILE *, trace_file_t *); +typedef void (trace_backend_write_cycle_beg_f)(FILE *, size_t); +typedef void (trace_backend_write_cycle_end_f)(FILE *, size_t); +typedef void (trace_backend_write_sample_f)(FILE *, trace_signal_t *, void *); +typedef void (trace_backend_write_sample_missing_f)(FILE *, trace_signal_t *); + +typedef struct trace_backend { + char *file_extension; + trace_backend_write_header_f *write_header; + trace_backend_write_cycle_beg_f *write_cycle_beg; + trace_backend_write_cycle_end_f *write_cycle_end; + trace_backend_write_sample_f *write_sample; + trace_backend_write_sample_missing_f *write_sample_missing; +} trace_backend_t; + +void trace_file_write_vcd_header(FILE *f, trace_file_t *trace) { + time_t current_time; + time(¤t_time); + + fprintf(f, "$version Generated by trace.c $end\n"); + fprintf(f, "$date %s $end\n", ctime(¤t_time)); + fprintf(f, "$timescale %zu %s $end\n", + trace->time_unit_factor, + trace_time_unit_repr(trace->time_unit)); + + /* Dump signal declarations. */ + fprintf(f, "$scope module Top $end\n"); + buffer_foreach (trace_signal_t *, psig, trace->signals) { + fprintf(f, "$var "); + switch ((*psig)->type) { + case TRACE_SIGNAL_TYPE_BOOL: + fprintf(f, "wire 1"); + break; + case TRACE_SIGNAL_TYPE_INT: + fprintf(f, "integer %zu", 8 * sizeof(int)); + break; + case TRACE_SIGNAL_TYPE_FLOAT: + fprintf(f, "real 32"); + break; + } + fprintf(f, " %p %s $end\n", (*psig), (*psig)->name); + } + fprintf(f, "$upscope $end\n"); + fprintf(f, "$enddefinitions\n"); + + /* Dump samples. */ + fprintf(f, "$dumpvars\n"); +} + +void trace_file_write_vcd_cycle_beg(FILE *f, size_t cycle) { + fprintf(f, "#%zu\n", cycle); +} + +void trace_file_write_vcd_cycle_end(FILE *f, size_t cycle) { +} + +void trace_file_write_vcd_sample(FILE *f, trace_signal_t *sig, void *sample) { + switch (sig->type) { + case TRACE_SIGNAL_TYPE_BOOL: + fprintf(f, "%d%p\n", (*(int *)sample ? 1 : 0), sig); + break; + case TRACE_SIGNAL_TYPE_INT: + fprintf(f, "r%d %p\n", *(int *)sample, sig); + break; + case TRACE_SIGNAL_TYPE_FLOAT: + fprintf(f, "r%.16g %p\n", *(float *)sample, sig); + break; + } +} + +void trace_file_write_vcd_sample_missing(FILE *f, trace_signal_t *sig) { +} + +void trace_file_write_csv_header(FILE *f, trace_file_t *trace) { + buffer_foreach (trace_signal_t *, psig, trace->signals) { + fprintf(f, "%s,", (*psig)->name); + } + fprintf(f, "\n"); +} + +void trace_file_write_csv_cycle_beg(FILE *f, size_t cycle) { +} + +void trace_file_write_csv_cycle_end(FILE *f, size_t cycle) { + fprintf(f, "\n"); +} + +void trace_file_write_csv_sample(FILE *f, trace_signal_t *sig, void *sample) { + switch (sig->type) { + case TRACE_SIGNAL_TYPE_BOOL: + fprintf(f, "%d,", (*(int *)sample ? 1 : 0)); + break; + case TRACE_SIGNAL_TYPE_INT: + fprintf(f, "%d,", *(int *)sample); + break; + case TRACE_SIGNAL_TYPE_FLOAT: + fprintf(f, "%f,", *(float *)sample); + break; + } +} + +void trace_file_write_csv_sample_missing(FILE *f, trace_signal_t *sig) { + fprintf(f, "XXX,"); +} + +trace_backend_t backends[] = { + { + ".vcd", + trace_file_write_vcd_header, + trace_file_write_vcd_cycle_beg, + trace_file_write_vcd_cycle_end, + trace_file_write_vcd_sample, + trace_file_write_vcd_sample_missing, + }, + { + ".csv", + trace_file_write_csv_header, + trace_file_write_csv_cycle_beg, + trace_file_write_csv_cycle_end, + trace_file_write_csv_sample, + trace_file_write_csv_sample_missing, + }, +}; + +bool trace_file_write(trace_file_t *trace, const char *file_name) { + assert (trace); + assert (file_name); + + FILE *f = fopen(file_name, "w"); + + if (!f) + return false; + + trace_backend_t *backend = NULL; + char *file_ext; + + /* Search for the backend. */ + + if (!(file_ext = strrchr(file_name, '.'))) { + fprintf(stderr, "[trace] could not determine file extension of %s\n", + file_name); + return false; + } + + for (size_t i = 0; i < sizeof(backends) / sizeof(trace_backend_t); i++) { + if (!strcmp(backends[i].file_extension, file_ext)) { + backend = &backends[i]; + break; + } + } + + if (!backend) { + fprintf(stderr, "[trace] unknown file extension \"%s\"\n", file_ext); + return false; + } + + /* Write header. */ + backend->write_header(f, trace); + + /* Write samples. */ + + /* We maintain an array of pointers to the current samples in each signal, and + walk through them as long as none of them is finished. */ + size_t signal_count = trace->signals->occupancy / sizeof(trace_signal_t *); + unsigned char **psamples = calloc(signal_count, sizeof(unsigned char *)); + assert (psamples); + for (size_t i = 0; i < signal_count; i++) + psamples[i] = ((trace_signal_t **)trace->signals->data)[i]->samples->data; + + /* Loop until all signals have been depleted, consuming one sample from each + non-depleted signal at each cycle. */ + for (size_t cycle = 0;; cycle++) { + /* Check if there is at least one active sample. */ + bool active = false; + for (size_t i = 0; i < signal_count; i++) { + trace_signal_t *sig = ((trace_signal_t **)trace->signals->data)[i]; + if (psamples[i] < sig->samples->data + sig->samples->occupancy) { + active = true; + break; + } + } + + if (!active) + break; + + /* Write cycle-start marker. */ + backend->write_cycle_beg(f, cycle); + + /* Write each sample, including missing ones. */ + for (size_t i = 0; i < signal_count; i++) { + trace_signal_t *sig = ((trace_signal_t **)trace->signals->data)[i]; + + if (psamples[i] < sig->samples->data + sig->samples->occupancy) { + active = true; + backend->write_sample(f, sig, psamples[i]); + psamples[i] += trace_sizeof_signal_type(sig->type); + } else + backend->write_sample_missing(f, sig); + } + + /* Write cycle-stop marker. */ + backend->write_cycle_end(f, cycle); + } + + free(psamples); + + fclose(f); + return true; +} diff --git a/projet/src/trace_lib.h b/projet/src/trace_lib.h new file mode 100644 index 0000000..eabc8a8 --- /dev/null +++ b/projet/src/trace_lib.h @@ -0,0 +1,47 @@ +#ifndef TRACE_LIB_H +#define TRACE_LIB_H + +#include +#include + +typedef enum trace_signal_type { + TRACE_SIGNAL_TYPE_FLOAT, + TRACE_SIGNAL_TYPE_INT, + TRACE_SIGNAL_TYPE_BOOL +} trace_signal_type_t; + +size_t trace_sizeof_signal_type(trace_signal_type_t); + +typedef struct trace_signal trace_signal_t; + +trace_signal_t *trace_signal_alloc(const char *name, + trace_signal_type_t type, + size_t initial_buffer_size); +void trace_signal_free(trace_signal_t *signal); + +void trace_add_samples(trace_signal_t *signal, void *samples, size_t count); + +typedef enum trace_time_unit { + TRACE_TIME_UNIT_S, + TRACE_TIME_UNIT_MS, + TRACE_TIME_UNIT_US, + TRACE_TIME_UNIT_NS, + TRACE_TIME_UNIT_PS, + TRACE_TIME_UNIT_FS, +} trace_time_unit_t; + +const char *trace_time_unit_repr(trace_time_unit_t); + +typedef struct trace_file trace_file_t; + +trace_file_t *trace_file_alloc(trace_time_unit_t time_unit, + size_t time_unit_factor); +void trace_file_free(trace_file_t *); + +trace_signal_t *trace_file_lookup_signal(const trace_file_t *trace, + const char *signal_name); +bool trace_file_add_signal(const trace_file_t *trace, trace_signal_t *signal); + +bool trace_file_write(trace_file_t *, const char *file_name); + +#endif /* TRACE_LIB_H */ diff --git a/projet/src/trace_types.h b/projet/src/trace_types.h new file mode 100644 index 0000000..e69de29 diff --git a/projet/src/utilities.ept b/projet/src/utilities.ept new file mode 100644 index 0000000..0703953 --- /dev/null +++ b/projet/src/utilities.ept @@ -0,0 +1,216 @@ +open Globals + +(* Utility nodes *) + +fun min_float(a, b : float) returns (o : float) +let + o = if a <. b then a else b; +tel + +fun min_int(a, b : int) returns (o : int) +let + o = if a < b then a else b; +tel + +fun max_float(a, b : float) returns (o : float) +let + o = if a <. b then b else a; +tel + +fun max_int(a, b : int) returns (o : int) +let + o = if a < b then b else a; +tel + +node countdown(e : bool; ini : int) returns (o : int) +var mem : int; +let + mem = ini fby o; + o = mem - if e then 1 else 0; +tel + +fun angle_dist(from, to : position) returns (angle, dist : float) +let + dist = Mathext.hypot(to.x -. from.x, to.y -. from.y); + angle = Mathext.atan2(to.y -. from.y, to.x -. from.x) *. 180.0 /. pi; +tel + +fun bound(x, ceiling : float) returns (o : float) +let + o = if x <. -. ceiling then -. ceiling + else if x >. ceiling then ceiling + else x; +tel + +fun norm_color(a : color) returns (norm : float) +let + norm = Mathext.sqrt(Mathext.float(a.red) *. Mathext.float(a.red) + +. Mathext.float(a.green) *. Mathext.float(a.green) + +. Mathext.float(a.blue) *. Mathext.float(a.blue)); +tel + +fun compare_colors(a, b : color) returns (correlation : float) +let + correlation = (Mathext.float(a.red) *. Mathext.float(b.red) + +. Mathext.float(a.green) *. Mathext.float(b.green) + +. Mathext.float(a.blue) *. Mathext.float(b.blue)) + /. (norm_color(a) *. norm_color(b)); +tel + +(* Tries to interpret an RGB triple into a quantitative color. *) +fun decode_color(a : color) returns (q : colorQ) +var rr, gg, aa : float; +let + rr = compare_colors(a, red); + gg = compare_colors(a, green); + aa = compare_colors(a, amber); + q = if rr >. 0.85 then Red + else if gg >. 0.85 then Green + else if aa >. 0.85 then Amber + else Other; +tel + +(* Transforms a quantitative color into an RGB triple. *) +fun encode_color(q : colorQ) returns (a : color) +let + a = merge q (Red -> red) (Green -> green) (Amber -> amber) (Other -> gray); +tel + +node rising_edge(b : bool) returns (e : bool) +let + e = b and (false fby not b); +tel + +node falling_edge(b : bool) returns (e : bool) +let + e = false -> rising_edge(not b); +tel + +node after(ini : int) returns (o : bool) +var n : int; +let + n = ini fby (if o then n else n - 1); + o = n = 0; +tel + +node event_edge(e : event) returns (o : event) +let + o = { + lightRun = rising_edge(e.lightRun); + speedExcess = rising_edge(e.speedExcess); + exitRoad = rising_edge(e.exitRoad); + collisionEvent = rising_edge(e.collisionEvent); + dirEvent = rising_edge(e.dirEvent) + }; +tel + +(* Integrates an input signal using trapezium formula. (1/s) *) +node integrator(x, step, ini : float) returns (o : float) +var s : float; +let + s = ((ini -. (step *. x) /. 2.0) fby s) +. step *. x; + o = s -. (step *. x) /. 2.0; +tel + +node integrator_en(x, step, ini : float; en : bool) returns (o : float) +var oi : float :: . on en; +let + oi = integrator(x when en, step when en, ini when en); + o = merge en oi ((ini whenot en) -> (pre o whenot en)); +tel + +node derivative(x, step : float) returns (y : float) +let + y = (x -. 0.0 fby x) /. step; +tel + +node lowpass(x, a : float) returns (y : float) +let + y = a *. x +. (1.0 -. a) *. (0.0 fby y); +tel + +(* Normalize an angle to the [-180, 180) interval. *) +fun normalize(angle : float) returns (normal : float) +let + normal = angle -. 360.0 *. Mathext.floor((angle +. 180.0) /. 360.0); +tel + +fun pos2vec(pos : position) returns (vec : float^2) +let + vec = [ pos.x, pos.y ]; +tel + +fun vec2pos(vec : float^2) returns (pos : position) +let + pos = { x = vec[0]; y = vec[1] }; +tel + +fun mat_rot(alpha : float) returns (res : float^2^2) +var si, co : float; +let + si = Mathext.sin(alpha *. (pi /. 180.0)); + co = Mathext.cos(alpha *. (pi /. 180.0)); + res = [[co, -. si], [-. si, co]]; +tel + +fun vec_add<>(v1, v2 : float^n) returns (o : float^n) +let + o = map<> ( +. )(v1, v2); +tel + +fun vec_prod<>(v1, v2 : float^n) returns (o : float^n) +let + o = map<> ( *. )(v1, v2); +tel + +fun dotp<>(v1, v2 : float^n) returns (o : float) +let + o = fold<> ( +. )(vec_prod<>(v1, v2), 0.0); +tel + +fun mat_vec_prod<>(mat : float^n^n; vec : float^n) + returns (o : float^n) +let + o = map<> (dotp<>)(mat, vec^n); +tel + +(* TODO The following two nodes are simple wrapper around the generic ones + defined below. They are needed since, as of October 24 2020, the Heptagon + compiler crashes when importing nodes with static parameters from another + module; see the GH issue below. + + https://gitlab.inria.fr/synchrone/heptagon/-/issues/12 + + They should be removed once this issue has been fixed. +*) + +fun vec_add2(v1, v2 : float^2) returns (o : float^2) +let + o = vec_add<<2>>(v1, v2); +tel + +fun mat_vec_prod2(mat : float^2^2; vec : float^2) returns (o : float^2) +let + o = mat_vec_prod<<2>>(mat, vec); +tel + +fun abs(x : float) returns (o : float) +let + o = if x <. 0.0 then -. x else x; +tel + +(* Time elapsed with |x| >= 1. *) +node uptime(x, step : float) returns (t : float) +let + t = integrator(if abs(x) >=. 1.0 then 1.0 else 0.0, step, 0.0); +tel + +(* Variation of x (integral of |x'| over the time when enabled is true) adapted + for an angle normalized to [-180, 180). *) +node variation(enabled : bool; x, step : float) returns (o : float) +let + o = integrator_en(abs(normalize(x -. (0.0 fby x)) /. step), + step, + 0.0, + enabled); +tel diff --git a/projet/src/vehicle.ept b/projet/src/vehicle.ept new file mode 100644 index 0000000..63a62a5 --- /dev/null +++ b/projet/src/vehicle.ept @@ -0,0 +1,87 @@ +open Globals +open Utilities + +(* Robot nodes *) + +fun car_geometry(phase : phase; vec : float^2) + returns (newpos : position; newphase : phase) +let + newphase = { phase with .ph_pos = newpos }; + newpos = vec2pos(vec_add2(pos2vec(phase.ph_pos), + mat_vec_prod2(mat_rot(phase.ph_head), + vec))); +tel + +node driver(top : bool; sens : sensors; itr : interrupt; iti : itielts) + returns (rspeed : wheels; sta : status) +let + automaton + state Preparing + do rspeed = idlew; + sta = Preparing + until top then Running + + state Running + var arriving : bool; + do (rspeed, arriving) = Control.controller(sens, iti); + sta = Running + until arriving then Arrived | itr = Halt then Stopped + + state Arrived + do rspeed = idlew; + sta = Arrived + + state Stopped + do rspeed = idlew; + sta = Stopped + end +tel + +(* Kinematic model of the car. At the beginning the robot can be positionned + arbitrarily after "top" pressed, it runs according to physics. *) +node physical_model(top : bool; rspeed : wheels; ini_ph : phase) + returns (ph : phase) +var last alpha0 : float; last x0 : float; last y0 : float; +let + automaton + state Positioning + var dummy : phase; pos : position; + do (pos, dummy) = car_geometry(ini_ph, [ -. cDELTA, 0.0]); + x0 = pos.x; + y0 = pos.y; + alpha0 = ini_ph.ph_head; + ph = { ini_ph with .ph_vel = 0.0 }; + until top then On + + state On + var si, co, alpha, vL, vR, v : float; dummy : position; + do si = Mathext.sin((alpha *. pi) /. 180.0); + co = Mathext.cos((alpha *. pi) /. 180.0); + vL = Utilities.bound(rspeed.left, cMAXWHEEL) *. ((pi *. cD) /. 360.0); + vR = Utilities.bound(rspeed.right, cMAXWHEEL) *. ((pi *. cD) /. 360.0); + v = (vL +. vR) /. 2.0; + alpha = + Utilities.normalize(Utilities.integrator( + (vR -. vL) *. 180.0 /. (pi *. cB), + timestep, + alpha0)); + (dummy, ph) = car_geometry({ + ph_pos = { + x = Utilities.integrator(v *. co, timestep, x0); + y = Utilities.integrator(v *. si, timestep, y0) + }; + ph_vel = v; + ph_head = alpha + }, [ cDELTA, 0.0 ]); + end +tel + +node simulate(iti : itielts; sens : sensors; + itr : interrupt; ini_ph : phase; + top : bool) + returns (ph : phase; sta : status) +var rspeed : wheels; +let + (rspeed, sta) = driver(top, sens, itr, iti); + ph = physical_model(top, rspeed, ini_ph); +tel diff --git a/projet/sujet/sujet-projet.pdf b/projet/sujet/sujet-projet.pdf new file mode 100644 index 0000000..cb78c3f Binary files /dev/null and b/projet/sujet/sujet-projet.pdf differ