From 173077a46cc13e91a6ddd3016746f7b9a6df44ef Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Wed, 11 Mar 2026 20:20:59 -0700 Subject: [PATCH 01/45] first try --- auto_sim/.vscode/settings.json | 3 + auto_sim/Pipfile | 14 ++ auto_sim/Pipfile.lock | 235 +++++++++++++++++++++++++++++++++ auto_sim/fsae.png | Bin 0 -> 71750 bytes auto_sim/main.py | 44 ++++++ auto_sim/render.py | 72 ++++++++++ 6 files changed, 368 insertions(+) create mode 100644 auto_sim/.vscode/settings.json create mode 100644 auto_sim/Pipfile create mode 100644 auto_sim/Pipfile.lock create mode 100644 auto_sim/fsae.png create mode 100644 auto_sim/main.py create mode 100644 auto_sim/render.py diff --git a/auto_sim/.vscode/settings.json b/auto_sim/.vscode/settings.json new file mode 100644 index 0000000..3212c3c --- /dev/null +++ b/auto_sim/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "python-envs.defaultEnvManager": "ms-python.python:pipenv" +} \ No newline at end of file diff --git a/auto_sim/Pipfile b/auto_sim/Pipfile new file mode 100644 index 0000000..51fcdf3 --- /dev/null +++ b/auto_sim/Pipfile @@ -0,0 +1,14 @@ +[[source]] +url = "https://pypi.org/simple" +verify_ssl = true +name = "pypi" + +[packages] +pygame = "*" +numpy = "*" +scipy = "*" + +[dev-packages] + +[requires] +python_version = "3.13" diff --git a/auto_sim/Pipfile.lock b/auto_sim/Pipfile.lock new file mode 100644 index 0000000..b14c311 --- /dev/null +++ b/auto_sim/Pipfile.lock @@ -0,0 +1,235 @@ +{ + "_meta": { + "hash": { + "sha256": "df9bfc6c1f8bccd923516feffa8df94caa648ecd292de15ed90937b733ef8ed8" + }, + "pipfile-spec": 6, + "requires": { + "python_version": "3.13" + }, + "sources": [ + { + "name": "pypi", + "url": "https://pypi.org/simple", + "verify_ssl": true + } + ] + }, + "default": { + "numpy": { + "hashes": [ + "sha256:0200b25c687033316fb39f0ff4e3e690e8957a2c3c8d22499891ec58c37a3eb5", + "sha256:0448e7f9caefb34b4b7dd2b77f21e8906e5d6f0365ad525f9f4f530b13df2afc", + "sha256:0a195f4216be9305a73c0e91c9b026a35f2161237cf1c6de9b681637772ea657", + "sha256:0a60e17a14d640f49146cb38e3f105f571318db7826d9b6fef7e4dce758faecd", + "sha256:120df8c0a81ebbf5b9020c91439fccd85f5e018a927a39f624845be194a2be02", + "sha256:148d59127ac95979d6f07e4d460f934ebdd6eed641db9c0db6c73026f2b2101a", + "sha256:1ec84fd7c8e652b0f4aaaf2e6e9cc8eaa9b1b80a537e06b2e3a2fb176eedcb26", + "sha256:22654fe6be0e5206f553a9250762c653d3698e46686eee53b399ab90da59bd92", + "sha256:22c31dc07025123aedf7f2db9e91783df13f1776dc52c6b22c620870dc0fab22", + "sha256:23b46bb6d8ecb68b58c09944483c135ae5f0e9b8d8858ece5e4ead783771d2a9", + "sha256:2629289168f4897a3c4e23dc98d6f1731f0fc0fe52fb9db19f974041e4cc12b9", + "sha256:26952e18d82a1dbbc2f008d402021baa8d6fc8e84347a2072a25e08b46d698b9", + "sha256:29363fbfa6f8ee855d7569c96ce524845e3d726d6c19b29eceec7dd555dab152", + "sha256:297837823f5bc572c5f9379b0c9f3a3365f08492cbdc33bcc3af174372ebb168", + "sha256:2abad5c7fef172b3377502bde47892439bae394a71bc329f31df0fd829b41a9e", + "sha256:2b3f8d2c4589b1a2028d2a770b0fc4d1f332fb5e01521f4de3199a896d158ddd", + "sha256:2ddb7919366ee468342b91dea2352824c25b55814a987847b6c52003a7c97f15", + "sha256:2e03c05abaee1f672e9d67bc858f300b5ccba1c21397211e8d77d98350972093", + "sha256:32e3bef222ad6b052280311d1d60db8e259e4947052c3ae7dd6817451fc8a4c5", + "sha256:33b3bf58ee84b172c067f56aeadc7ee9ab6de69c5e800ab5b10295d54c581adb", + "sha256:45f003dbdffb997a03da2d1d0cb41fbd24a87507fb41605c0420a3db5bd4667b", + "sha256:483a201202b73495f00dbc83796c6ae63137a9bdade074f7648b3e32613412dd", + "sha256:48da3a4ee1336454b07497ff7ec83903efa5505792c4e6d9bf83d99dc07a1e18", + "sha256:4b42639cdde6d24e732ff823a3fa5b701d8acad89c4142bc1d0bd6dc85200ba5", + "sha256:4bd4741a6a676770e0e97fe9ab2e51de01183df3dcbcec591d26d331a40de950", + "sha256:4d382735cecd7bcf090172489a525cd7d4087bc331f7df9f60ddc9a296cf208e", + "sha256:52077feedeff7c76ed7c9f1a0428558e50825347b7545bbb8523da2cd55c547a", + "sha256:54f29b877279d51e210e0c80709ee14ccbbad647810e8f3d375561c45ef613dd", + "sha256:5884ce5c7acfae1e4e1b6fde43797d10aa506074d25b531b4f54bde33c0c31d4", + "sha256:5e10da9e93247e554bb1d22f8edc51847ddd7dde52d85ce31024c1b4312bfba0", + "sha256:61b0cbabbb6126c8df63b9a3a0c4b1f44ebca5e12ff6997b80fcf267fb3150ef", + "sha256:65f3c2455188f09678355f5cae1f959a06b778bc66d535da07bf2ef20cd319d5", + "sha256:679f2a834bae9020f81534671c56fd0cc76dd7e5182f57131478e23d0dc59e24", + "sha256:6bd06731541f89cdc01b261ba2c9e037f1543df7472517836b78dfb15bd6e476", + "sha256:715de7f82e192e8cae5a507a347d97ad17598f8e026152ca97233e3666daaa71", + "sha256:737f630a337364665aba3b5a77e56a68cc42d350edd010c345d65a3efa3addcc", + "sha256:7395e69ff32526710748f92cd8c9849b361830968ea3e24a676f272653e8983e", + "sha256:76dbb9d4e43c16cf9aa711fcd8de1e2eeb27539dcefb60a1d5e9f12fae1d1ed8", + "sha256:76f0f283506c28b12bba319c0fab98217e9f9b54e6160e9c79e9f7348ba32e9c", + "sha256:77e76d932c49a75617c6d13464e41203cd410956614d0a0e999b25e9e8d27eec", + "sha256:7aa4e54f6469300ebca1d9eb80acd5253cdfa36f2c03d79a35883687da430875", + "sha256:7d1ce23cce91fcea443320a9d0ece9b9305d4368875bab09538f7a5b4131938a", + "sha256:7e58765ad74dcebd3ef0208a5078fba32dc8ec3578fe84a604432950cd043d79", + "sha256:7f3408ff897f8ab07a07fbe2823d7aee6ff644c097cc1f90382511fe982f647f", + "sha256:8ba7b51e71c05aa1f9bc3641463cd82308eab40ce0d5c7e1fd4038cbf9938147", + "sha256:8e236dbda4e1d319d681afcbb136c0c4a8e0f1a5c58ceec2adebb547357fe857", + "sha256:94f3c4a151a2e529adf49c1d54f0f57ff8f9b233ee4d44af623a81553ab86368", + "sha256:9684823a78a6cd6ad7511fc5e25b07947d1d5b5e2812c93fe99d7d4195130720", + "sha256:a016db5c5dba78fa8fe9f5d80d6708f9c42ab087a739803c0ac83a43d686a470", + "sha256:a111698b4a3f8dcbe54c64a7708f049355abd603e619013c346553c1fd4ca90b", + "sha256:a1988292870c7cb9d0ebb4cc96b4d447513a9644801de54606dc7aabf2b7d920", + "sha256:a315e5234d88067f2d97e1f2ef670a7569df445d55400f1e33d117418d008d52", + "sha256:a749547700de0a20a6718293396ec237bb38218049cfce788e08fcb716e8cf73", + "sha256:a97cbf7e905c435865c2d939af3d93f99d18eaaa3cabe4256f4304fb51604349", + "sha256:abdce0f71dcb4a00e4e77f3faf05e4616ceccfe72ccaa07f47ee79cda3b7b0f4", + "sha256:b346845443716c8e542d54112966383b448f4a3ba5c66409771b8c0889485dd3", + "sha256:b44fd60341c4d9783039598efadd03617fa28d041fc37d22b62d08f2027fa0e7", + "sha256:bb2e3cf95854233799013779216c57e153c1ee67a0bf92138acca0e429aefaee", + "sha256:bc71942c789ef415a37f0d4eab90341425a00d538cd0642445d30b41023d3395", + "sha256:be3b8487d725a77acccc9924f65fd8bce9af7fac8c9820df1049424a2115af6c", + "sha256:c59020932feb24ed49ffd03704fbab89f22aa9c0d4b180ff45542fe8918f5611", + "sha256:c6b124bfcafb9e8d3ed09130dbee44848c20b3e758b6bbf006e641778927c028", + "sha256:c9619741e9da2059cd9c3f206110b97583c7152c1dc9f8aafd4beb450ac1c89d", + "sha256:cd32fbacb9fd1bf041bf8e89e4576b6f00b895f06d00914820ae06a616bdfef7", + "sha256:d1b90d840b25874cf5cd20c219af10bac3667db3876d9a495609273ebe679070", + "sha256:d213c7e6e8d211888cc359bab7199670a00f5b82c0978b9d1c75baf1eddbeac0", + "sha256:d5f51900414fc9204a0e0da158ba2ac52b75656e7dce7e77fb9f84bfa343b4cc", + "sha256:d71e379452a2f670ccb689ec801b1218cd3983e253105d6e83780967e899d687", + "sha256:d84f0f881cb2225c2dfd7f78a10a5645d487a496c6668d6cc39f0f114164f3d0", + "sha256:decb0eb8a53c3b009b0962378065589685d66b23467ef5dac16cbe818afde27f", + "sha256:e7dd01a46700b1967487141a66ac1a3cf0dd8ebf1f08db37d46389401512ca97", + "sha256:eb610595dd91560905c132c709412b512135a60f1851ccbd2c959e136431ff67" + ], + "index": "pypi", + "markers": "python_version >= '3.11'", + "version": "==2.4.3" + }, + "pygame": { + "hashes": [ + "sha256:00827aba089355925902d533f9c41e79a799641f03746c50a374dc5c3362e43d", + "sha256:10e3d2a55f001f6c0a6eb44aa79ea7607091c9352b946692acedb2ac1482f1c9", + "sha256:1206125f14cae22c44565c9d333607f1d9f59487b1f1432945dfc809aeaa3e88", + "sha256:14f9dda45469b254c0f15edaaeaa85d2cc072ff6a83584a265f5d684c7f7efd8", + "sha256:15efaa11a80a65dd589a95bebe812fa5bfc7e14946b638a424c5bd9ac6cca1a4", + "sha256:163e66de169bd5670c86e27d0b74aad0d2d745e3b63cf4e7eb5b2bff1231ca8d", + "sha256:173badf82fa198e6888017bea40f511cb28e69ecdd5a72b214e81e4dcd66c3b1", + "sha256:17498a2b043bc0e795faedef1b081199c688890200aef34991c1941caa2d2c89", + "sha256:20349195326a5e82a16e351ed93465a7845a7e2a9af55b7bc1b2110ea3e344e1", + "sha256:21160d9093533eb831f1b708e630706e5ac16b30750571ec27bc3b8364814f38", + "sha256:27eb17e3dc9640e4b4683074f1890e2e879827447770470c2aba9f125f74510b", + "sha256:28b43190436037e428a5be28fc80cf6615304fd528009f2c688cc828f4ff104b", + "sha256:2a3a1288e2e9b1e5834e425bedd5ba01a3cd4902b5c2bff8ed4a740ccfe98171", + "sha256:2a615d78b2364e86f541458ff41c2a46181b9a1e9eabd97b389282fdf04efbb3", + "sha256:325a84d072d52e3c2921eff02f87c6a74b7e77d71db3bdf53801c6c975f1b6c4", + "sha256:33006f784e1c7d7e466fcb61d5489da59cc5f7eb098712f792a225df1d4e229d", + "sha256:3a9e7396be0d9633831c3f8d5d82dd63ba373ad65599628294b7a4f8a5a01a65", + "sha256:3acd8c009317190c2bfd81db681ecef47d5eb108c2151d09596d9c7ea9df5c0e", + "sha256:3bede70ec708057e305815d6546012669226d1d80566785feca9b044216062e7", + "sha256:481cfe1bdbb7fe00acc5950c494c26f00240888619bdc396fc8c39a734797432", + "sha256:4a8ea113b1bf627322a025a1a5a87e3818a7f55ab3a4077ff1ae5c8c60576614", + "sha256:4c1623180e70a03c4a734deb9bac50fc9c82942ae84a3a220779062128e75f3b", + "sha256:4ee7f2771f588c966fa2fa8b829be26698c9b4836f82ede5e4edc1a68594942e", + "sha256:56fb02ead529cee00d415c3e007f75e0780c655909aaa8e8bf616ee09c9feb1f", + "sha256:56ffca6059b165bbf64f4b4be23b8068f6a0e220780e4f96ec0bb5ac3c63ec39", + "sha256:5d09fd950725d187aa5207c0cb8eb9ab0d2f8ce9ab8d189c30eeb470e71b617e", + "sha256:6582aa71a681e02e55d43150a9ab41394e6bf4d783d2962a10aea58f424be060", + "sha256:7103c60939bbc1e05cfc7ba3f1d2ad3bbf103b7828b82a7166a9ab6f51950146", + "sha256:7bffdd3eaf394d9645331d1c3a5df9d782ebcc3c5a78f3b657c7879a828dd111", + "sha256:811e7b925146d8149d79193652cbb83e0eca0aae66476b1cb310f0f4226b8b5c", + "sha256:813af4fba5d0b2cb8e58f5d95f7910295c34067dcc290d34f1be59c48bd1ea6a", + "sha256:816e85000c5d8b02a42b9834f761a5925ef3377d2924e3a7c4c143d2990ce5b8", + "sha256:818b4eaec9c4acb6ac64805d4ca8edd4062bebca77bd815c18739fe2842c97e9", + "sha256:84fc4054e25262140d09d39e094f6880d730199710829902f0d8ceae0213379e", + "sha256:8a78fd030d98faab4a8e27878536fdff7518d3e062a72761c552f624ebba5a5f", + "sha256:91476902426facd4bb0dad4dc3b2573bc82c95c71b135e0daaea072ed528d299", + "sha256:94afd1177680d92f9214c54966ad3517d18210c4fbc5d84a0192d218e93647e0", + "sha256:97ac4e13847b6b293ecaffa5ffce9886c98d09c03309406931cc592f0cea6366", + "sha256:9beeb647e555afb5657111fa83acb74b99ad88761108eaea66472e8b8547b55b", + "sha256:9dd5c054d4bd875a8caf978b82672f02bec332f52a833a76899220c460bb4b58", + "sha256:a1bf7ab5311bbced70320f1a56701650b4c18231343ae5af42111eea91e0949a", + "sha256:a4b8f04fceddd9a3ac30778d11f0254f59efcd1c382d5801271113cea8b4f2f3", + "sha256:a620883d589926f157b8f1d1f543183ac52e5c30507dea445e3927ae0bee1c54", + "sha256:ac3f033d2be4a9e23660a96afe2986df3a6916227538a6a0061bc218c5088507", + "sha256:ae6039f3a55d800db80e8010f387557b528d34d534435e0871326804df2a62f2", + "sha256:b46e68cd168f44d0224c670bb72186688fc692d7079715f79d04096757d703d0", + "sha256:b7f9f8e6f76de36f4725175d686601214af362a4f30614b4dae2240198e72e6f", + "sha256:bbb7167c92103a2091366e9af26d4914ba3776666e8677d3c93551353fffa626", + "sha256:c0b11356ac96261162d54a2c2b41a41978f00525631b01ec9c4fe26b01c66595", + "sha256:c31dbdb5d0217f32764797d21c2752e258e5fb7e895326538d82b5f75a0cd856", + "sha256:c47a6938de93fa610accd4969e638c2aebcb29b2fca518a84c3a39d91ab47116", + "sha256:c8040ea2ab18c6b255af706ec01355c8a6b08dc48d77fd4ee783f8fc46a843bf", + "sha256:ce8cc108b92de9b149b344ad2e25eedbe773af0dc41dfb24d1f07f679b558c60", + "sha256:d1a7f2b66ac2e4c9583b6d4c6d6f346fb10a3392c04163f537061f86a448ed5c", + "sha256:d29eb9a93f12aa3d997b6e3c447ac85b2a4b142ab2548441523a8fcf5e216042", + "sha256:da3ad64d685f84a34ebe5daacb39fff14f1251acb34c098d760d63fee768f50c", + "sha256:ef07c0103d79492c21fced9ad68c11c32efa6801ca1920ebfd0f15fb46c78b1c", + "sha256:f3935459109da4bb0b3901da9904f0a3e52028a3332a355d298b1673a334cf21", + "sha256:f84f15d146d6aa93254008a626c56ef96fed276006202881a47b29757f0cd65a", + "sha256:fb6e8d0547f30ddc845f4fd1e33070ef548233ad0dbf21f7ecea768883d1bbdc" + ], + "index": "pypi", + "markers": "python_version >= '3.6'", + "version": "==2.6.1" + }, + "scipy": { + "hashes": [ + "sha256:010f4333c96c9bb1a4516269e33cb5917b08ef2166d5556ca2fd9f082a9e6ea0", + "sha256:02ae3b274fde71c5e92ac4d54bc06c42d80e399fec704383dcd99b301df37458", + "sha256:08b900519463543aa604a06bec02461558a6e1cef8fdbb8098f77a48a83c8118", + "sha256:131f5aaea57602008f9822e2115029b55d4b5f7c070287699fe45c661d051e39", + "sha256:158dd96d2207e21c966063e1635b1063cd7787b627b6f07305315dd73d9c679e", + "sha256:1cc682cea2ae55524432f3cdff9e9a3be743d52a7443d0cba9017c23c87ae2f6", + "sha256:1f95b894f13729334fb990162e911c9e5dc1ab390c58aa6cbecb389c5b5e28ec", + "sha256:200e1050faffacc162be6a486a984a0497866ec54149a01270adc8a59b7c7d21", + "sha256:2040ad4d1795a0ae89bfc7e8429677f365d45aa9fd5e4587cf1ea737f927b4a1", + "sha256:2b64ca7d4aee0102a97f3ba22124052b4bd2152522355073580bf4845e2550b6", + "sha256:2ceb2d3e01c5f1d83c4189737a42d9cb2fc38a6eeed225e7515eef71ad301dce", + "sha256:35c3a56d2ef83efc372eaec584314bd0ef2e2f0d2adb21c55e6ad5b344c0dcb8", + "sha256:37425bc9175607b0268f493d79a292c39f9d001a357bebb6b88fdfaff13f6448", + "sha256:3877ac408e14da24a6196de0ddcace62092bfc12a83823e92e49e40747e52c19", + "sha256:3fd1fcdab3ea951b610dc4cef356d416d5802991e7e32b5254828d342f7b7e0b", + "sha256:41b71f4a3a4cab9d366cd9065b288efc4d4f3c0b37a91a8e0947fb5bd7f31d87", + "sha256:43af8d1f3bea642559019edfe64e9b11192a8978efbd1539d7bc2aaa23d92de4", + "sha256:45abad819184f07240d8a696117a7aacd39787af9e0b719d00285549ed19a1e9", + "sha256:4b400bdc6f79fa02a4d86640310dde87a21fba0c979efff5248908c6f15fad1b", + "sha256:4eb6c25dd62ee8d5edf68a8e1c171dd71c292fdae95d8aeb3dd7d7de4c364082", + "sha256:581b2264fc0aa555f3f435a5944da7504ea3a065d7029ad60e7c3d1ae09c5464", + "sha256:5cf36e801231b6a2059bf354720274b7558746f3b1a4efb43fcf557ccd484a87", + "sha256:5e3c5c011904115f88a39308379c17f91546f77c1667cea98739fe0fccea804c", + "sha256:6609bc224e9568f65064cfa72edc0f24ee6655b47575954ec6339534b2798369", + "sha256:6e3dcd57ab780c741fde8dc68619de988b966db759a3c3152e8e9142c26295ad", + "sha256:6fac755ca3d2c3edcb22f479fceaa241704111414831ddd3bc6056e18516892f", + "sha256:744b2bf3640d907b79f3fd7874efe432d1cf171ee721243e350f55234b4cec4c", + "sha256:74cbb80d93260fe2ffa334efa24cb8f2f0f622a9b9febf8b483c0b865bfb3475", + "sha256:766e0dc5a616d026a3a1cffa379af959671729083882f50307e18175797b3dfd", + "sha256:7bdf2da170b67fdf10bca777614b1c7d96ae3ca5794fd9587dce41eb2966e866", + "sha256:7ff200bf9d24f2e4d5dc6ee8c3ac64d739d3a89e2326ba68aaf6c4a2b838fd7d", + "sha256:844e165636711ef41f80b4103ed234181646b98a53c8f05da12ca5ca289134f6", + "sha256:8a604bae87c6195d8b1045eddece0514d041604b14f2727bbc2b3020172045eb", + "sha256:94055a11dfebe37c656e70317e1996dc197e1a15bbcc351bcdd4610e128fe1ca", + "sha256:95d8e012d8cb8816c226aef832200b1d45109ed4464303e997c5b13122b297c0", + "sha256:9cdc1a2fcfd5c52cfb3045feb399f7b3ce822abdde3a193a6b9a60b3cb5854ca", + "sha256:9ecb4efb1cd6e8c4afea0daa91a87fbddbce1b99d2895d151596716c0b2e859d", + "sha256:a3472cfbca0a54177d0faa68f697d8ba4c80bbdc19908c3465556d9f7efce9ee", + "sha256:a4328d245944d09fd639771de275701ccadf5f781ba0ff092ad141e017eccda4", + "sha256:a48a72c77a310327f6a3a920092fa2b8fd03d7deaa60f093038f22d98e096717", + "sha256:a720477885a9d2411f94a93d16f9d89bad0f28ca23c3f8daa521e2dcc3f44d49", + "sha256:a77cbd07b940d326d39a1d1b37817e2ee4d79cb30e7338f3d0cddffae70fcaa2", + "sha256:a9956e4d4f4a301ebf6cde39850333a6b6110799d470dbbb1e25326ac447f52a", + "sha256:adb2642e060a6549c343603a3851ba76ef0b74cc8c079a9a58121c7ec9fe2350", + "sha256:beeda3d4ae615106d7094f7e7cef6218392e4465cc95d25f900bebabfded0950", + "sha256:c80be5ede8f3f8eded4eff73cc99a25c388ce98e555b17d31da05287015ffa5b", + "sha256:cc90d2e9c7e5c7f1a482c9875007c095c3194b1cfedca3c2f3291cdc2bc7c086", + "sha256:cd96a1898c0a47be4520327e01f874acfd61fb48a9420f8aa9f6483412ffa444", + "sha256:d2650c1fb97e184d12d8ba010493ee7b322864f7d3d00d3f9bb97d9c21de4068", + "sha256:d30e57c72013c2a4fe441c2fcb8e77b14e152ad48b5464858e07e2ad9fbfceff", + "sha256:d59c30000a16d8edc7e64152e30220bfbd724c9bbb08368c054e24c651314f0a", + "sha256:dbc12c9f3d185f5c737d801da555fb74b3dcfa1a50b66a1a93e09190f41fab50", + "sha256:e18f12c6b0bc5a592ed23d3f7b891f68fd7f8241d69b7883769eb5d5dfb52696", + "sha256:e19ebea31758fac5893a2ac360fedd00116cbb7628e650842a6691ba7ca28a21", + "sha256:e30bdeaa5deed6bc27b4cc490823cd0347d7dae09119b8803ae576ea0ce52e4c", + "sha256:eb092099205ef62cd1782b006658db09e2fed75bffcae7cc0d44052d8aa0f484", + "sha256:eee2cfda04c00a857206a4330f0c5e3e56535494e30ca445eb19ec624ae75118", + "sha256:f4115102802df98b2b0db3cce5cb9b92572633a1197c77b7553e5203f284a5b3", + "sha256:f590cd684941912d10becc07325a3eeb77886fe981415660d9265c4c418d0bea", + "sha256:f8885db0bc2bffa59d5c1b72fad7a6a92d3e80e7257f967dd81abb553a90d293", + "sha256:fcb310ddb270a06114bb64bbe53c94926b943f5b7f0842194d585c65eb4edd76" + ], + "index": "pypi", + "markers": "python_version >= '3.11'", + "version": "==1.17.1" + } + }, + "develop": {} +} diff --git a/auto_sim/fsae.png b/auto_sim/fsae.png new file mode 100644 index 0000000000000000000000000000000000000000..2e94f53afcda14aecb9455d25b01346c5c939d5f GIT binary patch literal 71750 zcmbrlbySpLw+A}XB@H6oAgMG+gLDi?N=XSQ-7$beNH+u0jdV(Pcc;?b-F;vD&N=s- z`|n+A7#Y?e@B7T&`}x%q1XhxMiAIbD0)bx2%1Ek$KyY9X=mj*`OD26eh6s5HDgqyq5*jPaZ+=(kn3TTMrv z*B;}g{#8&y=+`#R=RXWB^|~m(X67wfN6As`<7X!<+`18e@jy~9(v$fD zG+sBK9~b@2P-rs+Y<*9hpy4I7FggI`=e=EJ4x~{w1S)HNC&{vvcGmZHPp#YIp2Kw@7{+<(r zje+D12l~5uz!hv;gJq-{cmZ!y)@-~QHTbhdj{S>F6 zL&8Fk%W2I%UER7UQbx#c<$IwMjO&XIh5K$f+$sKL(XPezc97Rocnv+wbrC2-%XyDu z;lk3=rP~AVOXjEDMTr1!gC7Pqr2O{li+g>34kT}+!D3)XN@P$Xyb@$(A7-tyv zUG0m!|8H@-NS~GdFZt_1;0Qrh#O$Aybxcvzh{8fK?QyJ0!lYd`=6+VOe--@IV!l6j(4_7yR^o@uM)4dBHMCGlSuC*+k$h?h^0@S{ z(*optKh$}+DRJ~~FItY#DIU)9^ZV2}{~ouams)DT%8(eRvCEcu!C@U%w zdwM=-WLB=ZkbCUQr^TN%igNogw4juCQVC+ROSmEs$WkBkgPG`ZK7N)bYp3ylsgn?6 zpO!3_8971@d5^g^rQ`XhJ*+3b<^E!R=aqq1eFHP}j|o~sK!D9P_F@!`^da5eM+1Wv zL=ac#uS!v|L0@~4_LzEPG~5jyzZ(1p_{%5;r!Yr6wR-7c=+~Dk#V;9AC-Y+%$YEZr zy59HRPZROYHL>9dDEOeFSV?YQ(R}=$YWk<7PWV56gY#qKHR+_5`!N(UpxWt+B?!CM zCp{F~fl^0JZ=H8Wr-zcT8k&*_tyymG>W>>*o_bt2|Cp60qT;8)ccJ2UCJ*=zLiCy( zhK_DWpgz(MS>r;*{NQhRAt?W@V4>b=RGXR@4N-*=U*}Xn4&-|1&v{K@aI+aL@Wld} zn3%}LgJpHQ$=iH#5z$+FF^U1#HO2^s*hOpa)r?Pv^lp%`1?7Z?0o)TI@+j(2bbty9 zlxk;$^)trFaTijy(LdG9^G*wul}pQ}3*WInmY-~Pg)mr8qOr@S+rqPg?Frt-BN)J+ zV+F`v{bm8K>u;hMnOg?fvTu(ORZlO-v^19{$B~9=izY%eBlTy4klu`)7fUnQR6|l{BQr(uLx=I9|#%YfQ2ZJ<_Cv?qS$Xv zHh)Z$=2fB7K@)!_cj*|%{QQ}$chO_XVO2+lFWv9H#k=eJ%@WrpOtGCQp3rJr3oA6Hgh5G)X>19(@9YtNls|gd)ogav?N%|on=TIUzFwcWo?i3b z(n{Bu!%j^?O<^X=x7>BYNS1^Vc$7to1oVf7Ph&4q+nyra>E+3MLobA%)VLR>;IZ6R z!(4ed%xW=Q9~6WLQv`QWSGb+E?bV%Ck+z?4AO};*;up7n)Xqdvy?XwjV7nC2@?-z0 zG$ci|Fv%=w{)8x;1#C3LqCX~oU2kBOA>Fx2LBM!CIZFNAyDNU?bzLWidGvSF-CL&y zW960C;jhS415uuDpUSm5&EphuIEctdr1|jMb=}F0jg7f;_lk9Xj4n7S21!yI3O)>? z|4f!!J#4UUOY_`F3OR(WUS3@2MA;s*YoJ3{&GDhpDg2rm(K07*VYt3V1o{{E#L)IR zOM=DcHmt%}Qb9SD++wY(r`^A?$R2pwM)&@jhoZKa$^p(-`>X{U(HoK7h!$P3dW#&q zM*%yu7=nIEdfR*pV<-1M=_w^YJdmAqoR=4~QHC?qZ}@JO>ZyUm{W2SSTi1vu!Tx?H z8Nb)_YSEhINk94H!U?GLEaKFxmSh@B5`> zkG(@V7V3|@{c!3|do6G|a5`*IA8}}3sfcW9x{47($>1?hwJ6zpXUF43_$%GaE#;8} zmuc1_;nBM>MQR3gd@3T8<@E_=>w0~>{w7?;`E+$x<}nelR>tlQxy6MwPF@u}e35vm z&Dhn~~rXA!8MsuXqrST0e&6`0iYgsOJT~#VX+PRY8nEDF{#L5C z6GDF>q8u(`zDGKx$`B=3cWJvwd2~B94)Lp1<`>40M@6p2Z%HSI63E|}UQ=PCOsawG z2kNAlU*H6kK6Nco!sMnxqdFFEMWSSgI6}QpTRUnIzOU%bh0;)X&Azk4Gb#^!AH4lH6&F|Bl&XpsQ9ptcU|#W{b`u~9N1k3V7q!}b1CFCO)GaTrrK z^UYG54H5bWNEt1lK2lesOpSZMH=`Q}1a82~-6eUpv;H7tN=?4Q;%60ME>8}EOB6#xUZ?@C3bkVrgAxNHE7iO zTMx+N(I3P4?5KGeRODYg5w9;BwCxe(1UunhR>H`*jvP&q=c9S`$KRj8$<@Si&EQ!R zGKRZ5q|3DMtifaM^UJlbt6yzvfoL)B^WzLCzRG23MwG1mHxMdAzo9@w%egFQjPS>V@V@P)9dCY*b??g-0=tGwKo zzsq>a*5hbkJ71x4Q87#Q{hyTUdZnlqMt=sV6Xw;RCuX#63)yjv!(BN0SdTEI!~(@& z)2LR3OcocD`#K#_7Xx=TbOpi&#itV_G2`zduLd%x6~>i$ zgGhse)`6`|(=56qEKtL|^$|{<_OIpv?P-Bd3$B#Xs^dqV`YVa$qo747t#W~jMC6f= z^0wbk$>vnem4}BD8=Z2kqTJ$%I;B%S)_K=s7Zn>^eq{$5%@r}uAX2u%W1}$?+#sc& zU&sya&TMpk+c#ELX3-0Y;Sv+fdJ=4i);7VN;Vy~#N^U6wam(e1#Ba= zT*Cf$Ph%dhPI5kw3&4F?Mmu*G{}4!O*)dC{hjo&BK?H#fJ%eg7{JDA`;MH4(gSi2)KS|-Ck(x^L*JrA z_1Mv4X_eMTR3b$4QJAO85qG^sPRK&4StH5ph59Cb+ZwkmNeIDpP0Y)n$Hsf6Q#uY` zDGP7i->gnVK=6+|w#5p;j8pfSNN5Wgr?U-xD_Y ze}O*IQ$NJRjW0DhJ+`VUx3Dw$g^S|Uo;Ic)+bibJ=PZ4#5bXZH1>%#dvnL|~z)6_2Xk;&t0TH+cXqqE4Ns}>p!hy|>RhgvCK*Xx|z zEnBzeG!$LR!pCgbZl9RcMwEZ?X1|Z;#EGa`Nia+6sUf$)VHD?lq!z#a)$%w{yBDs5 zgL0kHM7^;&bHUTbhA)S7;G|vss{16uX7=dzwA1e!Md48b0;K&nJIS|7u^EQmy^l2C zL&Ocx)*tM9G4;2QyJ$a5F$vBRD6u7MX?>d`%A+?lC{=d_GuG#b-bBrqh^+z zU4B&VukN0xS-*hEk2ws;ItYnTyy1Gu=^9bnInex{rG2@XBcQlIY2S`l)RPzG1bH~Et zd4Vs@ESgG_`o&-zYzzOd2#!ZEtd)~ew9-h4+&uzLO*1caXQ6l#lwizH4C)p-r?>KY z9$@G=E8WmV=QOJX==<7|kcH>oNsGgeeaH16dPjETa~4_|8`mj{`ikOLwmy7G3U9DL zu16l2uLCFq{jh}Yg-LIYJFeGzyoYc9JyjVz^2xXhF+SQn-m)9blo$qR#dOIaSFo^F zd9jq!u*b_qjn*k>Vx4;NtA)vK1kh=vJ0^8PS%$5V{kJ6wgQ6}9N_5}m@x6`g<3uiVp_{t}QYy5B>hn_q$Y|vF z2>xfT^ETW=(CA}gPNPYzPZlu{5twuZ;Dd0Q1Plw^s{#1k>iMR4sUkqmP0zR*t!O%y9aLV zL$!{r#XTBz6bcX4OCxKx13&3396xcY#_MtBT0VgvzE|@*+Yjbka4kk17W&@m=`N~l z@i;aiDVrSlH(67rFum+m?Sy2}0;3 zvv71t$vO(nMm(s&2(a+@BRXPKUYMv@B;&+jmJv+vPU}LQV7WhPVDE(&>!WC`h$dQo`Q{#g3f(qNMAhmFU+z#M@(9`ac*7I z+uPBX3H2|lN(e{+a2@GPx-}9ZzZe%s?ef-I^cR*(-#@k{dp!yyx}# zBY;=V@GBaCT7x-TcBV#1fEL%bd3xLR8p%m7mLLv(e@uL@|L!c-|bqe~G z;`88c=PcqB7GyDJ1l0@=PZfj(r}TUbCJ`1xOf{ixf%7WAFN07x59xa{+wtsM`yymA zL#L$u77g3u68H$)8q0|aR@M)vmzn+Xf!I+g=Cq-QC71(SXZ(`AM`szVgc5wZ#b*a6 zc?2Q;#bCzIbhviM#>U<*VebjibymRGz@zOmc9iYKr;}HOXkG%Ig4av?e*?t^!f<5Tmp#qeb?EvzysJp#gymf^cqLr*d%XK<~W?%wO@ zsn13p(O~-XRqx#fSRDyQflnZ322D&&osQ0``i)jvnMIaDb^t)3s%fY>LeMZKP;db0 z3G6Zo>{&Ap=^7<0tfF;v{;hp)9wA*75bD7i;T&aFrR&%<9FEy(^iKQUDPpaM!kf~7 zh5xBxLiW$Qy=(>PtudKdw}^QT1W*RJ70n-xaH)rm6iwg}gdO5fmI?3SngM2fov(%j zJpL3}1)#{xk@qQtHW>lGjf$$uz)K%O2 zH&dLAIRB|E;;{UDS>vx?Rppa=iRp?}!w$ha+A^+G#9@9mud@qSSoW<=<0xVZyh5(D z$3d-P4@A63+pC5BK6H!5l~L^YUvm?RNYL@Udr#{H2Q|%zY~DO$x&oj^7@1;+;kdcD zvgxD?O$ipy70|3soS&fxVOTmKg~?>!Di=I9>$j#3I2k%(yprm%J-k34wzG2_ zqaIH2hBuQQUW3g@9e4YygeH4W@aubRB)3JJ*@OzHl5?<)NINy!8RFaM5_K_`hAkJD z+nqbQ7)mJf*4N{1@9&E$yS&0pA)6hY@P8?!H|((G&@{~ty-uwN>;BDR)LPR?d+xXGm5i@k5(omOM7c*U;}vK^C^=?mYoBsKg(-a1S6wQjdaouUbR%X<3aNGkg zS~QVtyJTT~?f%i8cw6-s-U8#CEe5e0-dzEGuy*@zJf+m_`Qkyr=2(Q?O2;!7)B-Ps zR|e{YwSq3F+TC9$EuS3S7fRaKB2AaAr`$|o#qBXLo2n=D2>M9nk!)ao>YfiE2EsTi zgH3zp4d@KL{pCd1^Wp<`8-cY3wq5McOYzn^PtQYNVRUysdr@_zLwZ}pp))n7X{ac-e!iOXu+wsO5FXQM%!wu(yIZ#r4h?RJSAOCRoN7Ks zZ$E4(aoUnYFH5J6OowAl2uv(X4?oiS=}4Ctqz5=uVD-P!>4^I1onfe%>(IvCoMp<~XRft5V5< z8)8rp?lJ8EgioNoYA9F|tb20&1zIBS*FQMJv?9aj2Z&dR^6W&v2b+zacrq6&buepF zooT>ZOGMm(j%rLW5~=iU1J?@Au}=G`?9=BfXgSTNVZ=w{MkuJvGVWVPWOGm5b0FI> zM}YcdViY`TAnV7Q+|ZinP6^rN-Lhf+i9qyyp-bFS5hz-vvGPu8(6q9xv}5GPcg6H$ z3On*MRw7W^K>mQ3%?~EM5f^(82_O+f4&G!;CVTzvnpSzCTzCI3cj(h0Enw ziBQtPjOn_~QYLaasxRH(mOG&jbon2f3wUTUXcS44-^v=%23+x(0QS}i@r!y5K-t5K zspOXMDJ%ZCphwo-+D#5jw+|kIy9z(Ckl+}3T?sx5rSJmu2~cy42SzCc-V5w@+wkP( z(uVITb8{hwB?z~71!qN| z<$IE>ZUCbgj~u3ez5~ds9k2|#No>N^H61_DY#$srZGPb<)^(HrK>n;f4Zpu??#e6` zct#qwRA%HC<-oj1!vaXa>5bqZgdFe?0ojVPQkH>C^V8{`%CyVk^m>n?wp}Z|%I_8o z5lv_e_0EWF^l$Mfoln_`Vnx^7*N{S>OsP>l)D5X|dVai2=~w>_-qtRyNFC0K2uV(m z9y?^*%dy}(Y;-u%v7Zf3|7JGyLvADsbIhF9@km9J4O2FGzMo~S-x-7FN|x6hPArix zVZ2KdhDd&dm1xg{J`bmkm8ugCDW7Q=%TUUR?hMkDIlM&se^)OGQ7$?U{Sq67$=eQh zkuU<6nHlY| zw|j;wz+Mc%vY)v@3iQ7ImecvZ34=a(@zt1=s|Fev+&2Vg;@4jC_se-RNI4&Z@t7>2 znGV#_$)9B_x@2LYG-E8{bYgK#&1jL;>feQ8(JlouMw^)OJ-RC|kz!je2Pj-p3zLMc z=!MS`M#4lMHJ>JWoPxr^dhB`Ug1gvZQsT|>HILHbaYMy}aHQNU0;*nKTuL32mfZG~seB1eovWf90M$UNw<`j_1+MKzD9K^dFyD zu}XoUKt8{12NrU9Bac9>rkA%kqTT4Wr(wP&;j11!ytX^<7`jaz+=&*^f5tc!~iQDxt6 zhlm9K-9|3=RDp%g4kVt-W!}dwmA(DmK(X~Vu>S8y;z|}JTT1H!+vmOL?2Q%tNz8(X z9=$dIIotqT0tISKF)!NQYADhWR8?>3fXw08KEJ;wBe+n0ZQ8}I*UM695b9&)pC~^v zlLEhyAd0iywTlL!$)HxzEtyt#Js%Hb%(rdQT+y?EoJJ{Y-&rsISX^YX2$rON1`G0Q znxjiIA@DAT@Syt(m*xmXUwFQ&TSURTd--daGMU}t-c&-i1VCHxcpD|OUZw}F{jsdh zJ;bSvp7azdpd@&^gSLYO0+1{AK@5G|1g*UfiQ8&+&^$Pj1Wgl%vgVp&QI+y+QZ^x6 zf`EZAgoy0G3hNq#+V6%btl9fhTmE zoPYjun)`b11k~v_n(`H9P6N3~QtZaYsK4E5HjRpj=PK?u2mv|3PuvTSCpFc-n`r)HvU$Zg|OrN{I_um zGk)-FxhJzxIZ?IM>-OfPnw9w*UTl=>DDB>9!K7FEuBMf1u1TxU6dMJ}7Xakk7mD0w zP<*<&n>x&BK`}Cts>qf+Kbfv^{nJsla`bs_gs&oBaw zgMU${9r;k8^VE^Sl-94jRk~Y~Hv%LB%Dh5j7|cVFz_I?|qeqkidmiFw!Qv!-a0Q1f z@6Cy=K2_Zu^LiDZ8cb{hIUbEJ0L*5PP-N#Y->dFAL-Hp%NQ=v-Es1CPeuv~TnEtB_ z$z5SgcO^gGjW|xwaJbWxWt`8mY<_DfsKP>+!mlLO1&0Jju{bfMO^|IwQ**9KWsoqMt#{BWc2?9etu^sM` zq0D%ku_>y`Z0fv==GhK)KWtuSj-P26dFrE@wCqkl+2-~CI-R&Fyk&jFp?e8JpjI3& zC=OvC5YKb*Mo{M`d8w2-y}=}SQ&yK07%j*^M>R3++h~BPZVJ9>%MtG@UrR!|{Wed^ zYecXZCP|AcR;SzTKlV;xB)iP}hz{h%0yvU)UfOZCO>F01aY0KP%pWd1-FJz#$$OC?@QC0(RfEXv^0qQ)HRZi=bK zQ;5uTMa@J-8^jO05s{S@t8$^HJU+d$3CU9g&soz5Yko)n5bZ zmKx)tHjdi+bh$=77O;tD|AqpfrrGsa-IbTO^nkvHWEsCzoY>JC*x(@Km}>cf4Cggd zOu4A^JUc)0VMBa2@9d82yZ16^&np(~WB>HO^0r(XBfmVPP=2yW#TqNq!(6G{8>$g;*?YeiW@=5A4LG8A0z6Awi!Y9)*C6BUC;if=S_1 z4lu~SdzHMrdyvtS0r)EsJc-RM^+wa*u?kT=-vS7eNJ>k=Ql>M2oC6Nm8C9?6D%k^tOcq9h;9~OXpIW0hJxc?2Q*=DD`6@cnM zGQP{uHU@Ip<)H{Y!EJb1qrrM{p=kp=v zBsdlLZ+NMa6O$50vdHF;4#c2P0y=uP&Yp!lp#~QtV{e#fmk|!UGlSeny(PQV!GY}Z z>Nb&h#`g~$u7#d1$6pLLcL#0MnP7=S?-qUp#($!4^Nb7D%IQ>3=jqxK-q{q(&lWUs z3obm>&BA==%^KD-EL z$Lc2%dwGtQ$pd&kHFt92LPbu>JN25!010(rSlY0`jMARxTQLZvIDj9#=S)Bj6|r>l zBr}YbqsB&|Bt^3@V0_+=n2MiEI%lsa_t!7IzNmgUCm_A$5Z_o2;dEI1p{BH3MB$tD$ zBxr}*B!v3AYtHTn`o7F9kD@fS={Z+GE^^b_5XpJQI2RKyk%9fJt(2w;MYvE11P)2S zPa7NDq)7oOgs0US;hH?JQZvD`$*Urxa;-Cw_dcXc49E3(G%EH=eoC?h>EDdKc3`*?N? z9w;#}@py&)l+yZMkf^#`?KvLuQJaqO8sEd}z$_r1mzyKFG@ZpI8t4w@yuz3rNhTH( zB<}%e7ZPaRV;M6vJdB1EZRMBOrbNi@m3!B7E{ zfZ%|fQwx(1m6K-{N~|S1!cIv*P9%-q&wH)thgW`7-@W~cH&S}*E`o;l4kVlNe8+GY zr$QzP>a|CO%rVKOH56Fd;5LvRad*{KwQo%PX=)$PBg*ty!JvOdTvgtJ^~(VA`q>fz z*a0%YC!MiuBmmMq^3);U9e$vLgMp+{9^kiKclY2Wc(#8A`VOcS=zu)?(ffapn95p6Xhdpe+h1Nu*dYToSEa`UhD)qJi4J?+I zUJu8Mcb|CQHd;sGR=^mWuhSKkj;Z4eClD)mkJiCFeaF=JPmf(B?C4yNt-PAx91)H% z=kf`^+>IXqDEf>gT3^)2R=Ld)IjD8e5;sTHZR}Bc?=|v~z%k1A#Go~SUet|j!CG9E z8#|eC+P)aeSGw9UIx}&o5KOglHG2WNg?mP7d4Yi`d&;0l5;tm9NxRy0m~wd?EDkbubspMqXCX_O`HfZfyvj9jJ@O;diCT61__ElUY}CEw0(+N<=O)eVjirW%n5^3Do=nGaM? zRkxPNdfiicv#9idli~>9knveWbyx~i-~rCI#I1bI;0&wxjx+oM1C5*h5pV3E}8m zOl#`G`;;NhAxITRnz!-fPx)U5VcZR)f5yHzxj1Pxk<=Ff!7@i@M@n)V2aUJ-DG7)m zz(lKk4z`vXOC$fGn|Y}OBv8x&77gtE+WGn4&beN-g$eGYs|M5YNa`uu$992J<;r&* zL*|epm$n>UYEs!e4gSogrJhd)(H3vd^KGLEsI+ zf4K($tt=sOCz;sEr;4>-Z}UC&Wcz_1Q~{>^S`T|mFhDOb-JS(sKm*{c?90U%+1pK~ zy&Cd4tlRDX0EQ`V{5sRD5^=qL3&;ZhX935S-BqvN^{;5J^}Z^kc{}7+pxS4zlC{1y zACYdfqI&zM%y;IM3|UCQ#F!zQ(9vzheET%VzkvC3q+f>Cr!U_k+3f)C@pO)%=;{8K zK{=r0@SmkU#|EEHM1;OP93DNr^SZivB8u?b@Fw;?_ZQ)Qe9kT22Z-EoJYKilzkWO) z@D6XezjUz!$i&a&M5#vBAEK1sn_rS(9z7pE(Dx8(<$y?YqHCMIfJ&wg8Rp_ zOcD+p_b`7;)CmX;KvL6EcB4w7STg~H@?+!Syh_izF*Gn`8Zf;191j$fmJ!ty_d)>k0l>VY0GpJ zdtA5%z;MWVN{3<^G~#`5nWE^5u()vnwXxV2n>2 zWoFdd-cv4qm}Bopo+Mu2Rmt$+>ieD+WYG07vNw{@f}S~-qmzUeplj8e6)?XN`wtVU zrVDf~s$_(E>KdjLwd>RnGJ3Jgug=s?>>_ki|fR2Au zYqBL3$yun?&~FlD&rDRUTf6Z3Zf&U=@UI4ka>zs$?iqG^_DE{vc6RlJwg<0tQ73?c zLzfNblT~diokbFRwZtBXugzD6kg~x|9xxfAPiNWwkp%_@6tAqzGAUW!`1#vTF|XLT zo;9~oL3V02d&UTfMc@7V1zHiT03X2ZE>8GEo9ey7HQea@Ld5AB8!!ZE044K_a0Erm zJre+aEr6iR$qLpZhEaLhRH*EFy^GR?g60x2;sWOsEabnscz3H&E4>U8@LX<`SHR{` zW8#S117Oi+Eiy3+r1?M`_V^+{f_Q!~ZiRimer@muo>~fWK(L@#Bt48kl(*Eh_-W7T z^O~YTXqQo~Kujm#L;LbtNdm%G7kZWz+1Ped1{fP;dw5S$3%NP{2yn`N6T;)+hQsTb zw41@LZc{JPn)!;g5jKP7`$>S;Se5T0dA#aoBa1$nrv8Ux097oOV45LOM#-8@-KlVF zRNJ7>z)P#vv&a_PZPhB~R}|Urk!njI(B|Z$_IF;nI|ez5#7l`bY~0-mPn$z=LOZAT zND-Gw`mc_VZQquC8~q0aYK3SM*}gT0y;xe+3as?I0)~f6b|b(nr6xtA213f3^toXa zP1m@91$X>!!`?soP_E{aFtQfL%kJYW@N{8tg8=tJWRsJhE??)3JGHJdYcJdEIWhQ<~zPJoQB2n zp^02y(rB%yby(Q2I!uNe63>pW>wSynZ%Tz5L@`h~nmR0S&+aK=n8C8%T1kXs)R;Mxbb00Bo~ ze;X18#BXzae+$>)uviG?+<;Hh-v)jEqMvRle<4aM_aQZ%QV7>y{DtY<-w$?ygRut` zd*gL1Nh~xl-}r#lNRfzN=nO85`1Z6B^2;|n;h93OLh;y@>H}?I1TlZ8PzV@4--wrY zv)+O7ZPcT^vS5JjY5SQ5kWIKfxpyLekU!mKHC2T#4KZF~ZyP(s{*$V6Qu}Fl_77hvlg+ROYvv!P9IujV!8KGiFb{K zEeLS)08&=wG=L5CZ|s1ng3>S~uCz?n(dnr(#by7_RuM`6v%0VcAcp~;I@;wd&%tby zhTZcfLlcF}!h* zUm3WoHRLGz*%=h1!Z<~=t($%ag6&(?)3*YeH+{A~EG{%{X%gDfzdMyP8kCqG4Oa9n)4i zgn*9$EX4E5@xYQj3+i`1qyFn`^PZFQdpKa80ye82S11@OLg+GQ#DJ7cB46!b-Mzjc zQqC;AM4kDc*z@@sUb^iu2}P=v9~et0Zb`c1R1b~f({>hCInH9!jBr)V1a^_x;D5U0fDdL4+~KC7?%50ya`swR($Wv%4c?W4%=J1^+Up|wVce3RP%o`wg7Dl zJS=(8C%9hic;yG=tZ(W3C|F_@@w!oeKI}7F>A|q*wDiI(f&p~B#E{mRFvB+c)L3Y3 zD0)+*<}%~U_OvlsvmyGwJ9(FuL3y0AwQWvNRu7_0DBg5Olh^;pd{mldN0@g(_E^Kc zE99b|lmzvrEJsH}KsGf_XS1TaNLj9hTxSN_LaB~S7ht3?AxS;vwffEA?r(l#;5USx za5A#_7MGcpy`4jGAm9b;NoK>1g5oQHB=15|d$2Ul&Q?*kkIGqdtmv*(@v$@j%y?P? z96pg|A*>GW!Uf9spNP+~__gQ1jv`U-;USmGL&((*u*>dV!NLW1?pvFMogA_$H<9gS zIJ(w)ucMc@|7N3cqz=lb#w1b9D?63Qbvy)xo?Ig&qz>O4+3*1?O}+Bg*^;6?ZU~;c z2Uy*2chyN{fMqf(I2kb{@i5FE?+C;#y21}6hRaRQK<>Pb+OY;lKiER11%2R z&BKFShIwh82j=sMtP+<+qoJHvp=e2^(||;a<+UtCG&{Q&$?)f>7CJKViOKP1Z&5po zDlLEJPU9ajinoF z`OTS5l;YG_!t8QC`rua+CH>Yf z0D}q0IQ{L!Dt*INa|-CcKpK~NrR+?fX5}qV(LgM4jcksWRiN6Ey!FzGHzTXbY7DTb z%w#1NGQi-3fL+K|xFR5twq6h|*1d)&@mQ|Z6u-3It^n%%L()d`-RO(^$rXv=NWrEM zVD)vbj@p)6r4$SliO(&~n>8vk_D14Vmo(u1l=cfNBL4@`PgTNxydW^~mKyDfH>H|v z5b-HU9)jCx&Ha>K!muteT}Nj(E;fZKGm8H4jXYpo`{spBOd2>%mM0kDhKC@0aTGFn!o4pSoIwQ2mw$4vB zKEj-xPZ3KJ^xZrBt;nIKL;X88#js6$kxx?fE)*q1I+l< zfc~r$K*O3efC4Q91l20jJc_xR1;3Q{JciH9+I&E7x#Wz;gLMHM`(hIi*f$_uKFA3Ib}DSHrK{}&BeS#H6)yv40nQyMv0JASXaNi@hGYzI{{7%& zAawVPT_Xff$IQ*7@1hx^@X!#?kmK16yqF9aGYgraS}fIHybe|aP7o|r`FZ1=t08&P zsbzr>_C<VGsVPf1+VoHEV68)U)9C%8zUn*!s*4x z0J#Fl4UiwtY$CyrOpVwz#^93*? zI;vUWfnH)M=wbFRX-OAY)zBn{#v%5TJ)7~#g8oTOXh-f6`e`DrHE#)2^XuwgMl#D! z4Q#E;GMNR+(J{>QE6E`)?%+5#8t?gXlYhfdFVsN*4!tn0S0bygBrlI!?~QG*294{V zJfE|l?A?p^S_MSl_KbASQj@CE?z?BM1MK*KTCh2O_AFzB3L)2pLD32XXczRC{kQih zS(PCbZ)|vz@lSjM3R_`KBz)R5y@uTNxl2dVg5=U>(g8xT(<{j~z|9;Fv%2mNi%q&e;EO%e=K-~B zU}vsOi4}}pUqUY3s)`Z|Dd$DIza2-TVK^*fvf7Qw%yXumSNS*R140ZtJ=AZZV!#Y#9Sxbp~$|b|Ca-$4w=2 zIUE)I4sOiY#rE4`g%wcPUK9S2BZECU&drXSF(#@ zzrgRGv^cWyCVJx2>awXaC$(a@)Jlo2I#k|&tZ8>vy^@LiI}DTGKOsFEX#m*=QZoM! zV`m;tW!vt5iOlmq5TcMWPs@~H znOer*d3&Ds-TQa!_kH(14tjcyqd&OUbzj$ceb4XrbCN#~JMt>^;s;Zo_XDu>I}0SP zzE^iZ>wP6s<_X83F0Hr3xE7LQ+jP^cMO*BP*9cBu9cy3F2?s2glj9pJ%|^L*O_lu$ z{}Qg|;c!$W8@58vjklN5&kb=anRKcL&Q~~|Ga1W&+`8^>o`VSnS8lj-j)97UD^}O; zQ6O2U$VHMLEX7tDheOsWh`cVW?TlR=dgL=wyW0o`bx7931|8aE=MyjjAd2RhA5w@k z*vKi6CPhK?2cKA%HotJYzm`n&ps5Iz(Z?^ivA6r5&r3*^7Z^_?DwTts9!C{@oqniQIa!{qt_BKJiGjF;)2nk zadp+jiEBO9X;p+-o{2seCYu5FL!}0ijFs~D`MLYF4_R7oIQ{e1VC0337bx3{`l#^G zb;Z_V8!3MZb^2M8(p09f-jf6Hgxyo?{vRv+Sccqv=Syak6 zrM%L`XfoXOn|%?jGTQjCI@(j8wNfsRd~NE;*+GH&R-;?IR7AAspW+gaY2=1}{1}M@ zk$l55{TzdL*W&$1T0T`*`se|B?H7FL=zb5(_ZBIKF}%fA>l*$i013wbj3|Vc zHJ_|k8skWakxg_O8g!T?m+droGf2ws@O7OIQB4*56L*vTDft0|0vGPN!1q_`3 ztP!G)%!mdRCkr}c=&re#->hq!vOJs?{8sz{fBDo?PQQl) z{8TOkoYld+KfZF|^yDR6Zj&VCyRIu@^m+N;2+G~OgCs*@TmqLt8!OR8UA`a?!6b~a z2+(Y3nppPe5pPmp#)l9G))T=Aeq$ww?oD?rdy#3@)Gnv?v&X+lg&K%y#=03;L?DzS zd+cP%2{av@NorLN&en^7?O)j(***j%n)>Hvwm8Ati%Mxs9v0Z+$*v@3!&8cO0ashq zlM8$g{pD~F7^wS)&^-6cqweK7+E*G>%mTV&w@MmTy(XeU7q?UsJdO(JV#Zv)w)fVc zU}wi_8{PRs&kGIUnp=yHDKYCk!y4^L+i%%pyIATVt(XZiL+@Lh)96QeyG|yed&hse zVnXkb3pHf(eK^B2 z1D2`Yz9W%C?^V$`C@HgXVdL=A`l;F0A$m10Z{niQ^bNT$LN77QX@2ke0@rZpP~$I0 z|b>iW|D6b-jOUp}l1_mS^-x z=CW+9Uj^E+zWdJh>vqe$wjWos@CgBZS=x^GMBWuv){NY>CnUoT z)~(mwmF=TPH006E?YQ=&&dv$horWJdDv*Q%&$DWSkpTfBBX%vNXNEhoN{&L=@C>YV z9_G|59y_7b6PyZre-0nKi#$gQ-pIyLlkAjB_^M?WyjOJ6W=dU^*2756I!19MT`((n zc>?q9=C(2Q@Vjp53mcn=`1*mGX8dF7w&SlejZ%)d+T@#AHD&r?H>B%|4d=!#dZJE9lMx+acdgY($K@uZbiZq?+ zv8PtUb+RT(u+YHmr>tjoKl#vr!tB;XihpSFD>nifWGq8I7wHwmXj44Mv2KpMFEP1p zI2G%gBHOAHp<#b_B96`tC?{Bz!jY;L7$&UE16b@*An;s}*1 z#CmI5wn%;vt39PMNO@38n7$GcNC-ki`{oyM$u*hAfd_XZYpF-m&s%LM;iwb4d?4jn zm87=DAXup@0=lw4x$M0g?95YJ@FZ2WM-B(5{bJ4gNP)SN9PRyd_ueyYE)9(EmgE3* ztw;NIy~RM$a2Tp1+umF(o%G4t^&ScM^O5Rd-*0{+@ zTePvodMkfXi6v%xw(Uftr&qKthv!iuM=*H3Z8db$`e?y^0yl!uKSW1xQt<8kw&FsO zlxsdXPhnI?6m?*_ieGr(_IKBf;rMVJdV+{-X+ra#XRA@Cx(e+WS|yY+&Frgz^}V;5 zd^$ZHtxQYVP${g2oEe)^bGjuWOkKo*nMB(;jq~7L?|bkEB3dT@L=npui&VmypD{ok z3O}&KK5;>Magc>hP3071j)1QpmG|3;Y0MboV!K@UGdR1na~NSMwT`|VEwm<#ub4{V zNz#|H?`jA5bA!?0G^`evlP_}%C$-#&K3+b&yCHAKfMQJTBn&?fTNzHTrgI;0qZ)SG zKQssokWQt#!3<9gpU?Nqj!+B`wDSW+E@6m8vjeJ(Y=X6oWM`-Q^$2J9FToSkHWA@k zoSq(a5*+?rw%A;3*@&bvuJK;k-9Jfm>9V!uS`~_-%)Z5iW%68Ln1oC=jtzmSWa>fu zIQLbe@POtO-wdXMB5qUnzgpz;k7)$-g%GKegwJ5UjhK{FTcD-wGrxeoUbfreGG!-6 zJW80_EPaN99iNTAQX}>=%DZUnuUsyPHG3VgDYmO%+UY**b$Af38*r<<%#;8?DL&if z?#>V1bh*9XOn3XLNI0ZkFG+rGZU$j&W?{yE1ZO{&rib)4)(#$H(Q`>|+9QRj8r>&{ zs?X@B?yj_#-0<>Zr%Lj~@r-{T!hZZ+HWYZ@Jd~AFl4kAZQb%L?{wQbpcIU1~*Wt~+ zlm~DZR8GGB9;=8sJWHphQ9w1}=~yO`3Oin@g?O=u=wgYs@WgzBp>Kk^l}e?j#jFT( z1Z}1sC!2c7Xu}Lkir)kyY=W<>cRqDx0;Nj^Y!JVc%{bet#Lr%dmo$||x$ZH_runEV z_-wsX3A9Nf)HlxVd=`xvU1%YFlESUl^C9~@W2LXsL9cxu^-r6I?TC6FYzg2Uo1`C( zIHR}oGI8t1^F!!Zk7I_o6j`SRlhfGOo72}xf)4$Tl5qnlkEz$B`ypTGdj+ zko!;|ywo+fJG%oeZjS8*xnBb%iKzQjJFO|1C0Q%lU#Fn>b65U169zJ3x^Gh|E9Hik zVV|*6mJTwOK1n6!X42W|uDLiiv9r=Q^P{`ppe@+b8?fO?$w50(yy?*C0%3Wy*872E zXX^1cMS3Us;>4z>do)G2?rgqyZn1lT$Kmt_$;KjMcBOsiyR-fbjqKn2a`GGGhlJ>e z#8ydTe|<5z__k_kFlMN+mv<71t2CLi9RE!^4x}E+nE&Pa4=ulc{#SN@|7$LwPLX#} zp*M}aj+8+0qvs}HO=0=x#E!L8 z?6)8GJn=h$@6b3+@m;Y6*?$eBANCddL}Z8dMVL8zb?JYidxyFSpE{}@a?y&1^ngp^ z{x5O=fGD{(L|eMPKkR9G zJ4T+XE@|r6k}oQu^E)-!6PV99gBRBntb60qtUm(t%44;`w)*^qeA$z^80b_+RjdW_ z+U6K2i5-2_Z6}{z(i}6oN`TKRa(n6GV~DBGw_Z^wyKNP##r;gUl)<)vE2w<>Q4cUnHAHE>NkNEq%?Aq9iK-#SfIB)lwo5!z}ctxtd$hGB&zOY^QAiG6+Afl`ZDv9gKo<0-Dt@m5r z8@7Kwu%_(e6!vnP-*(mf8Tf9&PUL9oGfr_#Hv~EJwBqa0Ju8`XJ8L=aBYvA>W2xq^ z@4RL^zFqYO{`35Z_Zv`~Afa}INVS0%zFgmx^=b};YvUYm6($P8y|%iaw;>%Ei1$vu?&mMng6LQs2zl{F7g* z@uMiTRxwr0%!E(oV@R*E55Mn_x+s3*v`xYB7d_)N()Wr37d`0*8&ooRwVXGW9^Ov#JJ6|sL-Fn0seYqiq(2I> zShTX?a@3 zicGz?eMT#LsX0q_YI)$(5A+Dm!Rofze6~fDKFX zuJoIOk0jG%*H3^#sUpFyWMV=L%p%^AQ>r!bU1VGh(!TfrceE(8M4vQemb9L$&f>ib zB`K=4m?#Oi-Q>2r->5ZJh-&=)DQ5`Xm}mlINhKxSD?cj}F!OU=aBp`(r{xV+?+}zT zL$OsgsJ|X#HFLL-`_i6PcYM_EpP8W)9$)?@7_)ZwZK6$ zhRtth?)sknk=nWC3S6hpp@(F856lz}1dz%J#OGX#pSZ#zBa*To=LW{#Sj|swZf?_G zpk3n{`Y{p(Pm{}tQ{uCx(4$576ZAJep|{wMQt-xdH{n(^3-Iv{Z6=8dAuD#Odol{1 z#St2#4wj}$-DtMpDNWk+ZrG`)*kvXfZzneA#=gZ0mr)WVj9*0T+NXi;1R6`oJysU) zPV=U;eRYjDrWUS_`&b@$9hqQmhNGTf&%7i7LZ zO$9qK?8>a1y;8la_xfCS2q}W+>O4$#u*&@<`L8Z?_%EcR;(GpGt7`Y4jG_T*yMYQl;QyU3Jhz1z|I&6N`JMI2hsQ?z6>Zt+&#CXb@!Q}a1`8H1`j zh@j6wHr&seZRbO6{*S6QcYp@Kbq1I&ajmC$&L^r@SXf=EB|fCF|J6;9){Q4&)A?Gm z=b*&b7$mogP~m(&5gy5_{_a$+wWioxppcCc;vj-b^JpD@R#(bFUOz=XgE9mYr)8Ux z+DsLK;f=Z1_b$0RX)Y}vS<5YZ=nc2OeUDY^_62)UuR*ttdlU3CgpKMu-<28UjkRn^ z(r6sJ8-yXLWiR4@Q257+bqIMhI^G-#$Ci}dyH!h(ES1g z*(qO)#bFYwA<5F_Yg{&i>W~_)?Cd@T;GW$hnsY+Ra;Zl7h8)DaN+>NMuOu8@ zx)1C`M)PfBWOPU;;5D1l6YDfZ5G8;G2u9jZH?9jqcq7h{{9POM#-Nnf2h{a=8e)d% z9mct;5i5yvzMjM&nj!Wm1jn>qn7&uuJunb(jmIA>rMFQF_hPkfPgeOLn!$kIIvE0m z*&E&5lP(>o<_#MvFMi6yDJz+%UaP10;_YG3POm_v1Cb{PbDK4KRN5D~)-JI97s&+j z?)9N*i~Na78@kmTJ(hIUUrHAs^6of0@{yX=?4y}qgYY&BR&X@^~Mzil>6^u}Bn zP`9o7QomGXA#wt|%6@<1MIlT^gZhtCD^jk}x|4hoh(XXQ={1ykt)+?M=%FERT1ng4w-nP1Og8``@bBmBur zs+=wRtMBVvWzvxQ>jbw#Aav2Rnk-F~b5|Y)pd|aXd*0W4Z1;ZtECp*t8_Hc+UVUhH#K2o$M5W;zY#d=XynDuh8vGH8CUFXp2 zunKUxG)C%7l2C5c)WCDvSQrk6;3ixY6nREJgFXSbb`d^(5+k!Yto3;M?glp>R?BRE zM{=#f7#vG4=Ws`+TkOk#rz<^O_5TqEen|*EAfSQ==DUTDNB}9X` zBobw9rke4h`?1PHL%rVXp8MUJ<+CJ|uY8kkaj_IG{G=02QkZ;SLI{L1xjLrK2mpb&4YggTMkHXGDL^QKB-!N&& zh-6_m<<>IJ1Inm$z+R#1B(&3jk^%nhonL;N0(}E<3|T;w@Y}t(Pknsb`#>>+G!oKU zhlVB~)N-HL)SJ`h-G_a>4|1D%n?6zjd^i5UXM231%JaBG-EUUJx(jyW2T_)1MV>^D zS#EN^u3k{?8p}x|`pCVvy&u6XPoO>k3IO4T?M#Y(X61w4_uRG`d<@VrJ#my;;t7z; zA|tM~$YKNwk61Wu;>AT9|Lth?orxFT39&TcDy6OjajAguu>xM% zEym}UU&fxpNuIHz6D;F>7v#?Ak)8E+cSO^V`1h5`mqf=elTW^9Ppvy{*#Fd*qx7D0 zZN$5VY=dKVZ0j_~Wu7b38U?o$qClHS!RHzzCGSvxIo{|RMzB7}3bvXVh)mpgqZ!rN zdh4>C;9Sw0rrt-(FQdqR)_o-OO^nn}8un0V=NR;nDLDS{=-(xCd^kxAYw%4%Z>Q(x zvW}sIibuZT%Ue~4_*7J%RJ#TX5wDx?@AR*%ofEwVQ~^zt3~b}P(9>Y2xR@PGH*tog zy4JErCal7heZ`|_JxYZ!4_w#)vHHuc6?>C6vZ^;So6 zbZQmBQwXb6t7mE4NfG=iGPtznkUbc=E(g^_`(60MscDU(%}yXDt7o5YN4##O_fk#2iY`V*pcyO#bmne}OQ%rT!KKW(4D z1s$RjM_)NG@SM>kdZ&`^Tm7!2Cy`UuH>=DZvoR?KU8{ksWd0S^#{ws&} zL6Tx1y~q{-in{X;X%pBS@!6OZ#WOjj1%uU?{N^i0v5oHU$i9OK19nW)sUF;2z8{4> zw;wk|p&)&5PpEaGg#Xz_&+S<{Zz=VMuYu;5wNTn#O<5|3WQpFtjVWuoTRRACcfdC7 z{FyQVX^&L+I4<}5eGW=VW=X(JY~7I_($u^5a@5g$TQpB91220m67TRsED+ZSMgRK zH!rs*dBPokk-92>I6gwLTNOW?MzZmHD5$fO7GYQ+w$m@ltO(_bFOGj+Z7#u9iU1j7 z+4Z$^04ul-Vi3UAT*%!XJSdrW90u{SQ@v{UQB7p%vaxc8;UoWuYnqlo0_Wcr40P>p zG-!zvS@`V`&~Vi}B-h|cN3-$;#31-XuKL^hmaR9_#d29!+ER%Cqf(+@9xm5zmh}8o zDh>Lr_Nj=pdID>sq2_}+`@*%rks@hPUFGU!sTpZFFT}R`-ggjb=U`TM+%jZny;VK| z(4+7i+AdJ&{8#6SCz>c?sJ3t9zk7V`t5|BmmBAk)H+RPB_DiHZx>6BIP%=TW-q#8+ z73+OJSb|1QFbKlGhZj7txkTz|1Vm3D9pgBC%;~G_36PRJVZOjX@-pa(MCJhx-Q7-? z<}G6#eP*7}ks$U3kdiM46-n5f#lcH=0#R+7O?O%Q78ypdOdGGFl<)< z{IeU*mD3a2%4!+pNC=N?se_c zr&9~+Cu~ako_9@BC)-?#xPTxnIDo+vd?%UwY~)DFZu-SxkDvgeFFwQiKMZv}?3!3A z7M3aS#|jj)r>aIShQL~V?eh6$D3jpn0M!ntE&#l!6cNOXq6s>r=q+3MSnxO07s!pB z*KRu1uB}e~&Yzgj+?3!~tJV&=r9T08`o8HGL!>(3viqoUVU5>wqnj%TxH0YzEbobonI>=Su9TFc|TG&MmCqI{O$DS8}d{$MFSnMj3O9IW5qwH^op(N)kiYw=I5Bc)VC zPR|Sbn-n(TL7)3`3N_Nm=evnRrd~B%{5uxSc-iNP;KS+j9kP2HpU_{k6^_-iBaA?p z7QvtR8$p3(X3jaPfExlDp(V4y*W6XBnLzBW!=wVHCj0#mLwvGF01?NRjT*E z>$*x=zMgMGcSZCwvD&s0g5w3+hU1@l6(9sgeh8j{p#B$oHjY&52QMTST8DdLM)~E! z%&mfv<7{`6)8ZkgWj<*HIkW7SiK{|DG2R%Kt-h^XM_cN{=&B1XQ2|{U#+*|5>#1J? z#*GELSOtFwS2qNVG6}uZlx&ksh+l9y6-zf#Xw-g2B#E9NMl1IkmMPqceDql&o06T} zYr7~I_~)37a8U_gV~@|8=;9>`);J2pX4zI0uO?Fn9rhE&Tk!Kk?_%dzs~s<|iROyv zZSh7I_jBnv5we#rbs|}WR6H9037ZwI_+F12z-u&P=K~e>?Y@!|lX^QG|NVOJADKdM z$UV`X`1WUFA8(gI<3Ir;HqzvEwzsy5fXDBsO?l70t)+r04bSt{#^>i@$_;rvP>FFT zMkH3DY;|iqttS!e-ZkWY{oA$l!t?j@dFD%Kt1VKF=C^6Z-f>;wVb#p%k^Xq~gCZLL zZW3M|#97GQ%p@UC_weQ?Mx&EH?CL9C!?%~wxg_GA_zDqdfzGbYxy3rZ`O@OrQ<7$( z#cmdzlg4C7XcG(9WV-lBGy~svxbDHM@eo@WN@nsYiaf%T(_wWXuQPw6;|>btsnsPa z<$ZWoGk(pUA#V!M#0^3p6xH|`z!H7k>tqPv_QK6K=`OCXk?Gd@e-o{J0_Q0*!-mkS z0w^Xw?{O3~FqB{l$kR*9XQ*3Z26ykN7)Haylp*(}-_4fp!-swVzhT|lqy5d}{8wj7 zEgWK};$u~{L&~a1DcdbWa{2T)DwVAsEQ_AqTh2D3h^P>=;-#Q4zWr$2b1vJQ!zigb z%>}?4olELLiO*FSx$##RxLKqKWAQgLlN##<-Z?jjuw)n1!@(tp>+C&TCOG@?%mYxH z!O$7a&nl7~zH^o2t}VRb>;S31LHpgAf;qrfq#96g)$@CYXKp`$ko1%Da5lZ;Y1a0} zaC6&fh#>mkteq7RQT6uM`sCIM8AxZsg$Iq~;yAQf>D+&j-Q_Rjp?iEu?Yzy=w`DcZp7q%6(1ze?N~f8!T6RmFTE^2DFA~baL0G)Phxzof=|rM* zVM4~;_1fodIX6UA!3@2$AhTy*x8r2fHFsBGG zh!enC%jLCwsZTEyiDsi>mN3qHe=h*jC*bkya87N3|7q28JS9*8JGtHJyYocfn;Q#+ zRJi24xvnAr*F}(V5vangcmc^457{EhN=L1(Ro=2h7fW`l3#t2e)!@DedehnT)?=Vr z1B){bhygJ80}{`cSJSP;oNsYSpNoNVEh*%ci^KG4=bPA$8@$g$?j%DK-(~jQM5m2- z%dNNPHRRJZ;H0XkkN{sf4l)OE?M8m;m&q(8&nGqm>dFO}^ayjhXO#-!RLuZ>ytLWU z`^yd2dyH8RES;if3(+c-C6~Ssfi9wZ0%Sl5_IzHn#fnqp6)Jls1C3%5)il1i*XJ%Z zuPaxdF?Nyjyx*HWO}YJ}BgxY)*YC&0o-RA*Bk%itGRb?-g75&j5UAT#8^+W$BpCZT z11u>fcNsjweqH*yE}b;$i%66w()E0(3&uTpb?Nur`ST-7ynP)un=^&%vXpFMrj@%l z)Et&oGh7!Oq(@z_5l!~{{owN3SXMzySw@^2Giq=}$7+76kC{J&K5b~Dq82D6J#IkC zna_KQ0<)MBG-##Cq}#rvGW;#-lZDqXuXNLmbx9%BUZQpzHb(L| zJ{^c*V$$bpW;AoAIr3#5XC!Sr@Ps>lIbcfQ(F8?TtocMe=S~Va4Fgb=Gh;VE;0F`@ zVZo=7F7`*==Xb7}9>1NHPWpvd5rlFRIzy3e~)H!3XDy=Ct<-MDTK#kE=$%jX*mL9euRj$HP? z>tl_fLErL_MPssmohh!47Lto8l)({}U97B(d>IF=*>VR!COJ=bkEue%L!9baa_viR7@~X%2-=mo+<>2~uIqRlPMH@&PswD6;r z1)>7ifj4#jGsIiEzgecz3!~`;R0e5YI!>~kG(nZhGosK2W;w1A#Uaf%@|j-GXX2^@pymbfw+XR!OJ*CxM*LLLrFx0W?>=W4 z!&}t*ZD@+R@jei8kd`z5ct4X>aS@-gjB;b6QMQ^^lJUz4U)d*NS2%_$szXKSemMO( zq<&ATeuem)wSSW6XwM&s;cCZY&|FGg4R1C3UBNM)T$?Nbdl!gUfC5VGn0>r(@2%f= ztxt9%!*U2NMny#hnS4_=@{sBwv~v-00l6D^-lj+Nv|{&rjd1;@roH!Rx%2n=&+zw; z4hz0@byv%@`5}w-jNsFur!3b*%k~15LydDt3S(1yaYAQjUT-t!hw{yCNJg1knhO; zYF8b+iUv(tI3c$qpCNw^!@cf4BaKj`PG>sl)eWjF#7uw6N?coam?^kOor||8K}lmA zJgPrLh-oTMJk(inn>8F;Z}5< zF#!kUc}IwV-NIjhNh_XHp0{`-ohzzu-=-qk@{$}y>=?jPh|_d^0jO#qsnf@lNgKb2 z+I<8iRTLN>qN@PXo66a%(6-vi#}#uC_4s6Ryy4|1>&mxW4Kxz6VQrmRU4Efi&Nz|! zWU)GAIx~({kR{wy#Ly+fV`y%1P7Kh}R|JeWWm7d8u2-Lb*aYqXnyplAIapq8x0L>V zG-zi2gH9einsOc9GLl(3Nh;y425skOXp8PJu4n41>FDSPQDU5`Ry9OCf4;k+Esq_@!d8lM>b~ z-4&y_BDt~IOPxfxFRUMOS*Ew*x-h>??vAbW)Ov+D#%UgHWF>T}(aqtM8xQ}>w^FmU zjOOJ|%Wp47%%*Th9VwG_YN0z@5ahtQ0}xfDgR~;*ha0&DfYQ9gXpNiK3k05%aw@I< zoHnLJGf%nnDH+w`-CiqZzNe&HR?hJGfNhm1+_ps2mg5&(FWlb-|L@%l9@PRVnY?iW zUvX=ltA>3%WiEn`>Ee#}N_VAaw~0buWiHmVmtFNKx5FWJ&c9|7z-R)Pv=ZKW=S$8v z@V7(%fE{!ooFKMf`ymN&{|4>2HN9_>1y>1K4h|!Ej}xtz!|O==i5L5r?bND!Rjr7F zTz;E2zioNvnH*Z5AJKzn=D*DajtqB2w{%;`(40#T8`;x)H1(bVAl%=jK^cHqVsCTQ zfnxPG*7BXSZNA}#YUS-X5+WOym#Zf%dg35wopo~pDvn6wai#8hP(7gyKL&PJ`wQZI zO4B%_N+lg1Yf4v=4*Tckr%?I{U1ktj+Uc)9$j>ma*HU&#T!sM#U%#eAJeD|&`JOk* zs`2#OnPL)w3qqw?CscaQgf$y#2x0RFR_)e`ggquY-l+K^l2wJPo2UGl0i3wK=x-dS z;BU>#xD>(GHXR4E0o$-Ti`L^GqEY2J-ROMPYE8?lM#Gim>)Ar0*o3hHd+pgHIw|GP zx8FNH;=w5bWf}S*V8nT1o#gmo23(`0Yu$&5K;Z)6**)}CRyTRWm>BehTu?F>=^sP_ zjXd$~x6R%pGjMI3ws~8us&y57Qwg9fd0ggr9L@M5$i&UpZv*>a9MZ+d)?evi#5}Ma zfzcU%Z^l{>elGBtI%1mfSax=;$t&e>@3N5x7=4p29U%fD%5-d84n2eDsHz5rtHig5 zW+jpiU+6AE5sT8DwEM}FGxgfEgFfj#4MxQV&tp%okcZb&C_DV*=!(vj{!EOy$t*AX zgcg5Sdz6Hx->g86A)i&YNRiM3%Rw&#Y@$}1WAFPM*C3xRA%!ycVdV)$my8+vjh;U& zpb=UPMVvSvmQo{iqra1LW`4-uQR+!?aqzj^2Ozr^P&xfX`kj!vQu|ck<(iRO)gJvT z3{*s$w`K`i{C2CRSldwZ47krCAZeHzsbR-TsRj@2uJJg5S$GNc29%NIG69{wp%Puj zBPFF~v71V!T2B}N5$ShobENjbJI$FH_VDZiv&10WBd+pYLABd|+W;sC%KC+$CaazFDrl56XA<(>;VYmy?R!Bt zw*MHPu1$azN~Rm|H_++@$Ix$uqte%3xS;RBcr__^2NHw>CiAnfZpnW)l9lV959RCo z3Uw7mH8f(ADydecV!kKG;PKongIqQH>HUBg4Xly;Fe%J*&1LxXwrCA~@1Y)3Ok9GMtlQqn2icyle1daEex?&Bv*9+y>a$WW+#&f&YzE{!Af z&5PL{GhE402#{>XYNaLqo0mqWEP)6yj-HG+%U*C@y1E{1ewzy}y`6hEfw}hl2w4GZ zPA4rrWbasu6z$(>Q`_6tIanfJ86=FJ+aLMwDy46KIDy@f7F`QDoUbL95d z*T*gw*UFrmcfC;kZHz?IJx}Iz5?uf;m&!V#Zmkga6epgNm>%>Wp9!*&fWvH>3b`>u zuvR$noLoQ4V!!fDrQ>CbRi5S@JGn^V2Uhd1F+V{DZOG&yKD5s_{{*Gw>iu$WBx_mNzSr#z!qP^%TPUQQW9H+BZ>;1_$0xkty3 z3zRlc#L$YTT@nswF(HzbZ_0Ej<>{_ffoe_h)6CqL(||pJ$ud#E#kqGO(xUT~X9Y&u zrG++Z(4e3NjlRh+j;r*k*>VA~%*h5O8AO)Y|HoI#sQ+oHg-BLb4NX^#jKN zL;mA6&I*fNyH`!60pm_XhZj6b0U(CzfJM}d@wYZrD$KeNJ^p1Phf<@ax<0xLq ziV&(+KD}X^v7eBcf$9)6Ul}LJP-fl|eK&V^)IL>Hf)Tm;uxRt++|h6>{b}I#qdo64Z{k9GFn9-qA5;nR93H?JFVodJBWZnIcP zalow^#TPf1h@njRS{^5eRS>wGBC9X&3T;gI0_X`cFbE#v-IF%oBCz)^%%A=pvMWBN zv}eV8k=I$fb)u!b=Q6G!`hNairG1-=W%-x*su9dFd_is;U#Bs?b=>tl`%(I5Je`Rp^md8E_azO^yQ3L}_zc2TIgku(eL&QC7YnL; z=E2|>f*BmAuf^Ab>u_mjh^g6+uw>n6x8KV>`gVVgZaHi}U7;D=H%EZ!ST<65-21_B zK#k%wt*tfxZJb^Xm*sFT#I7G=bp5MewoO zc9zn=%dACNNUM;@i9v~o7d;VOvORV16qAf`qC*_jXNwK-)vl=eGf_HP{NrkKJq7r_ zrXm3&&-`ra{)&pziS0VxbHCC4Jo8F5l|2n@^t7bNNUbV5&X349T;O7JGc$SZT;yrd z4~@7pd+96tsRx<_``>T~k8o{~ z+OaS`Z5j5#&~NCx(H*xv8A;Wf7WtE&vzmeC?5PP_pCZ5K7)=#Ln(G$e4baDmD?cO* zV$RB4Np&YylN;6R#j-w)j@WfsgIjTO(&h2NdA5}B@z-S{z|Jrdy4EYp>41gd0=a?D zv<>Nm#B)m3>xZc#Ug@tXR6p|Gw9vL2(RDY$zSj0!pG&zSnlEA^#6l9J1P!%!F8Rvv za`n&@{ARCKR{NT9+r1+Y6@k%&jFol8Ayz>?T^URzxUg@3uUm#mewmL8L((DC1M^wv zmD+Btg1eaC&l5aFn?|%bXlF%p zNdEKb4qFjMn9Cs`@a3Y?%A&cc42(mEC^Te=rS_ZioEF#TXorJCzR1G4*2dzn&Qk>G zhs!V@@q__Jtir5_9IRzf6~e@m^Q-!Z)y`d}KcI^c$Rm%{%DHu$p0Z)o=mM2Y;K^T> zZ+XIBpbS9N!dYjMQbHW|HJ8a)w-^_IVwc*j_q-e+!!V`?lh-K*?y*u06cDhX-)=9R zfQ=aATgH1~P*Jk7E7RByFQRpJYFauPtgZCEVPDxWQ}1b=NY(R zHNy?iL}y?lnUqj{a5p|UZC|!7q@4{g z{P)s&&hv-D2W;Ao)yC&(`24khSxVdfvS*!K#r(4%#mX)mM#BCKPa)ope0b4S(uVQ8 z{U-)xICL;Jvd2uY(Vas*ZS!91xh}nKwpT6Ljizu1A5qQ;3Wqzc|LPOsH<9(BWtMQ$ zt-Z;WOZot2&HU+(6z%=vg#T2(X~GA$5f~wpTp`m#*%Zz!AdOk3A0%h8=u!LOC&#Ja z{o@Txih>zYgh#c>EMAO5mc88Mg?cf6G+_lgYd*?Jww`9GbZ-05f4T+7AD#ewN36;1 zfbrc>9bF{TLt_@Q`7mtZTDCcx?pCX#rEDs9$tSBFTd6Md&{jtycH+NYBTlrVc2@kq zsF6h~#^YQv441CddM`R;$|DWQ?|TEqZFm&=G?`iEwYn>O`ZgheaScToaCACetsUjC zy7qH_adOtx^BG}|%^jQsS47UGTfg(_6drs`#~fT=c#qX}Dgik;y0i~Yj#bHz#A*h) z0?1W>w{ZDtp7KNG(!-dQa)r%r3~64z+Utodk{NGKqZN#-{?`M*^@?nz`{`ZtB2C=gxTAckH?^422+_nRPEd??%4ixfBgl#74u!uZG93SmStml~(2g@RA)q9rRqi zk}R$e`j8tx0p-@f>6x}uzpUQ!q$eJ{;wBDe8Be1^Up3e(4Gm~2ltqV|pjgmOtMP>1 zRgUxc-Jryn$1MPL^fS#YoWP={X|Rh-9|v=v__>$;QTpf zZN0jNZ8nyx@TrsBWk9{rBdm0W?HFDcPo*}=$!9a!DlAsQT5nYm(0>1im8$<+wb;94 zu#VIn#LYg0E4(d`Ug+5jp$44eVVbEGiYXs!elJm5Z_^SBk^V1H#K-%t#GMl5;WJ*+ zi-RdzhUaA8GJiiI|JNjUrummevy3EIhP zXm#WY+p)QdO&4*fu`f@Z$7A;X*H!xc6qo3dd0N@)qaatqkXizk;wWHSSY9U6H3?wb zHx)T^?>Z!)xREDn{11Efrr-?+8T<)Kr(=Ha* z5>p??zf-)ds|(mT{d~jGw|*nUYzI5UZ0OX&VWX0i@6(ggWbeOCDMI8~00Jcjl3y!| zFBzogkI!z&MCX`h1lId+4D=q*dK_0wluV9L@EHK=T70w*Hj>$CoOtS*`~0X)Z9#dD z^~;dYYXzk)%LhBjc$Dds@xKzcWF!+UR$3lqjIInZf#WPN!hJ*;D~Nnh$OK~eCRKj{ zVSZueqP7!`jA~?;#d580vQUQnL{D$Pq#D{&2AsF#{mvBMd7&rB^SlH8lp^3q9!xpk zS}(JF=-Iwuak`A4+e6$7bB91PCncrue{mCw?B5b|3=(%;@Ei+id8r~~^63xsk4YUu zh>txqj;G?k+ZA|Ji|Vga1oOcZU2+oQ$k3vpIR=3Uhb2=Z9WlSMcO-;8)Fi!CJMnws zJelYI)9q=W_2bx$WF-{_2OIa?>Gr%xK+c!jZA(oC6C{shUT}vY3LK?Z zZpC+mTjbM5sF1+4xU1#P(r?v4Y5_Oad6&{_{TH$41sEwrOJx~A;sQHXJ;kA=?>3LF z#2pbWHT&xJvGli7i}zI#6#CTN!D zV~$l9$iXlFk85J(DdUo*T2eV^v6KyoKN++9^#n#h9wuT84|X2E5GUvm!nZuud#{sO zc=c{L^KUk_;FE7--5%2t*=+}TX^7bM#b)1lGwDh_IIh;n>^g+?H|MN2)iunnkA!pC z7JG1`>ux_6{L*Ld$d>@V{BLY2j#XyQ$SdKXIGDvl1$-}Ie-v{ylYyHi-Xd}MOmtg} z$qj6sY(G}RjmDQt&&T%vaF9%L2wJA<*X*-?UnclK%i&D#vLM4Lojj>aGYA*~IueZ$EP-w;jaim`(#vqeyR_N?os6(B_5QxtV~sx4+)`px z6YZi;#$%BSeHqC*ML})A$0D5_3pVS`9R$V#f$)%ztCmh55nlRs$BUzBkU8g*hiP@hrhj>G%2!+g z{mhj!n>x{9QDFD~R#zMM=CW3gL4S$(o-k9Hyi-{U9e>#BB$t6|_N1x{);0oOpzj=) z&RMIx{_gB_RjiPqPUnK1h!B~vrYY*LN8UrTu9<%P|6esza87=duXe3e?)2s>LwdBx zu2-BAr9N-%K@+&(U!E>xp`mKSUiL>u0e#OfIJ{>LKk9%@H*ZSh+;isd`VZwt>Ui7| zcBkCDFPC)H1*5X{WY|xJA-$ruWs3x$dU}Tn-}OL8B?g8C1cX$wmQ)dV|ApJp0sQ*$JeyYK=y-n1tqQ#(eo)$lYabr2*$_ti$5x`r}9s;Xgd~ zVh?y(%iM)W{HTT%Q4#Sc`1B|H?%(;C39}DwWE(|f%X)C2pSYsJvt`Lu@t~E|zcZPt zaN{YG){6@H$7)83e9Sz3^tu#T#Onnk59>XAzst9C`N<_oj)Ie7Z_AhrIipZwJ=Pc& zE>^uSXy^ISZ0@crEM>zLUsT*VDTWX9vw#0gD^GF3k2rM3LjzPB`K?t28nM#JTj7|D z)OHy77sPb@PAZ=!pPDZR1~rQ}z~;zEJVV;~rf;7Tqlpb;;ar=H5SbNLkT-aVAOv2;G8>fWh%f)0u0bB(k7Z|HSPd-EU>O|A& zi|i}`alZItE#R1NucGyPTIphBRq5hMllqT_#J?|LhL~fDX;)-^HuOLr6^T`Cb@<`) zVI+lB3sXQm3!fmz>59<7{oEC% zj^6~v1=#uF1G)|g5d?(}IIYea-$O7=W{CqL4+xYazd{0XWz4_#r5T7XyrB$h?3Cs# zWRWpo$h)b8;ly_8Zms%wK8qk)ap^s8Xq&JndkNR|e^@~_VW;L}3v`w@hXdWe*g2~M zac^F7zcu&CW-U&yBil(jyNp@WAd63ugiy=x!BybG@|ao&=9*pWs^Keq+E$vCe3;+Q zV?o{P@4LGha(DkntDueMwIUX3-=}r~p#7a@Z(jlhkexNI06r{)Yq+*1fgtn&I3Ep; z)d+;VjQrX-tGZG8S6~NLDb?C=s5tc8bVa2%!1_T;rv@Vf*UO70ZieDT5#=*GhPJVH z<}5X~m>?HF@18j8h10xeh5p^kvLMXhTW%zhsy4!FB(7{`_G{{IQBU6=m4F8_5dxZW;m zxWbS(Yz5;XgV;Hfg2PqOXeSQ~K2w`~udO7uWyrjY?hyJ{ve>K}5V;bW7$iAd{TTUP zX=j3-vW&0-@e>PDhuGc-A$lb>hZ&3BQ)q?Rr+b4j+YrOOW8*pHmaQN`_?YvbRo9#+ zO?+*cCya&+;7EB&mfjTxo7%G!tw3YkN`;o=JK=br`wf*z%{{9D- ze`e9WUMGC(sviB6CF=ZBBp@2*se&Naa@vN0mUL{B0N(krvi%3F)FkvVpHqD@c&M#% zj$hqYv|K$g#9P09tip%~j|vz4>}3&BHZHAKwm1^BS6i%RZMgoWS+!tNdSB*b;Q9M> zbd2>fFf_;mc__Q_x)U0rwZS-RsVIbJ{5R7WjByV`$)&&1kP91UT_j6OUq~>VZ0soE za*87qzzmQ}Dtd%=A3}vt?cciNzki8=849z^0$<#3+Z65^8g{4BK3voZnI+>Izv-e2 z^xaiU|BtY@fXZ^~)`lsiRJyx6l@t9`2si&!1IeQ8G^13EC~5 zBT1%yrOjn3UK@iAYBhg`0AULdC10DGbV;^VxLUwBpU84s@MEFf>Yal(s#{JvK&}ZqnMTHAjQNFteB$}EmPPE4*&&$)E{JEt2$e*C6@q~hWQ_kD zj(hUW3wYryl?8OZ-$6<8G}m73Qfivc=T*A{56kz?CW0!UnO7x!6E$3wnKnN1H$5#; z-s4rPNih^5k+VwvA1&_E#_Y%Vz=v;tW<`9Lt|z07>g<3q!q0YFrt_QqQJDeARl`8EeKRXdfaIE zz;*|*uxflGl1B!7`4K_Su=4L#y?>MQ^KXEp1+eA?*D%j<^IgVu=%*7-pB?WLF+Jvp z+HILPDZOY}w}^m7xef$)V6PaY&WJocx>@pef+{1kXO1P!1phhni2uZ{UecdCFA(cS z|MPa`o8nlO@A&GOY z@br*d7PY$f=>S`gvM`z3E{lT~IQNeq(&Y>MjoH`!lm)})@0NLA{3uLAH%6GCb4L65 zK-j-Q+dZ;f$C5H3_(Zv3s|wh7eM1ww7Yx%9TV2aZ<}8%Euxai|Bj$k;6wJ(KCS9JJ zzCH(MPers?!M|E-m?VK`0#q~#D9awNu|}$J*dUR!2RqLlrZ0h<)WWd!vqhFKGIVzh z4ZFHO3XT8w8r`E7ooy&ot$_{hCUlGY0%MA7_@+9$UcUq+00v(B)A{m<#7$-0Irl`_|HVAwUGC%Z{(8UNbUbgSI9`Ch6w0SE-toO!c~>F4_>W`Z@va$ z(25`dd+m&21dY^L}Nn>S>&ByiL=<836{GtOgS#WmN(w3M2wzvwu@z zm36C8z)DcV8*UCz_p1-RvpY%q??^p=KutQxF$*XK4RZ(^iyk0fIM1w$MbGxq71-ao z+_%zIY_1AfVKh_vUqZ_az@GYze0N)~$InjIZ}5}_5SX}#@3mAyyxLt!Wr6L>_$U<1s8o*u zyyHwGhi)tFDrnL?FnA8fx`2;Pbh4L4B1% zj32feJHoM4(tESKj~t;%WaE&WV^D>=LE_I1rgeDV2kM)T@wG&;j;QhLEu1Z2R?y)8 zKVt1lYR~Na&ee=`n}ngXOS1b;lMvYq0iN0~ij&RMVoRR+af0c)Qnx2d%@{FU5mYba1A(bpV=A2m=&s(&^EbyuaTK(r#50t|iF4U-mU2ZG~iKP)DaF1XLYZ2h0evFhQb zu>#17)jS@PS07p1AMtzxpkhBLR(UfmQW@(Es5!4-fDrgYxt6o?qk1!(4QtryiO~$N zdWLalpKbxAOs#}QkmG-b)7(!}%^G0qAWZlF-U}(!l5X9p&xYd0wzU5Gjai|IM84fTgw+Hm~$uqO%G6 zSXsQaV~CsaGqJlDQl@te6lHl-c5T<2TItANE~~2{0*?AE8jwEK8;qsyBzO<|r3wMx z?OBWiP^5DN0Gpani78$7NP`ku;7yv&t|MZ3^<3F27 z3xBWAwkMsyeY^Pmxry9s$FBfPMwbugkuDic0_%74F-faN&NqsM24M-?r%v7kP*{E& z7u-wKhjPhPk?8}YiYO;PYx?ksfBx2v@(_H)kvmBnD5HGK3oh#$2ca4@E?|e1~WqfeK#(my>jchqwi04F{I}Lye*3|JI z0o&hCG+P>6tX>CI?Ezg~N?iQx;CgdgaSVrgPO6vANrfx@F1&Ep!>$5vcaf1G{C`Lm z?Vr#($xhdUrrF?OKx|{AQ?nXJs2ksuEv5%Gu_33`PtWcA@rieL_dlPa_{!WlmbV(!w)8J9Xd<1cLENYNy&Ahh<5- zk=lg#PLBn!sSwXGeoGg-i~20#2zd*HUVuRNKD9{~%;qG~$w8aEnbNq~ zFS69{nntd=Pu z23=`T7Z`;7ul2Z7xYmRcrVA1_XK%{*jetEyabgasAKd4L2JT(>B6UWJG>(@&?59yj zX%#t2M3F0*q=&0KHo&ycwjbD%5KL^r`^|#5z<^A~J`-@Y8=knWcc6K?qlAakB-i&Tk9f7l0x*XR- zM-BnP0c(D;pO+=To=u4KBhn0jGY_)v_o;$O-alM#&%dBYN>HgAuFSN-^;0X@{MS$b zmkW7`xJnH+wh|)xmlz_*1X_5}^H?}@2aIi>SB*FjOIH}%Qp-r8zL`GwDuF$-VR8U; zgGT)9KCRPGrE!3?mQjLp|M#-xLHz+~=gnIt)ge5Mb_8)lBp`Wt64xHe><5g%+0iTp zEk@8Qah)j?ya_|eE%4vB4uas$5gQKVr`@&swKpVDbyXg?dP6CQ;=v>CmrKOpMP2p0 z7l0Oj&y!ED2;7z(nSkh=FdR!vH^&_a_QiRLrP;Ob#`R90r#5;0L3_$`aW(7B_KtNu zBJ2lgU9f-ebN8#g@xdBh>@F)87+G7rtsMigF{pphBKFdk3e*V9??yz@#y0j9F`?R9 zsEG=2j)xjptl7?1Z$PlTW|JtXaPs1Nn8A@2$NNEDZ6@Y0n^D>D*-^pa&X*Y!pD( zmb5?wF#*tYpA}radYvMz!B3Q@uEP(!|6hw3``<%VY$FuQm)VRQi1Q3Bym4k_ve++| z&-k2R&?BYOg-FqWLktJCbQxcP@a7fE|DQk1me=)-ty7`Dm#g!H`Jy`vw{Q4_Z|;Q+ z>!%GR+D{ylNV{p_J-6TId7ON>0-iaYyjQn+`ez`(dm46TVNS~Bc>e4#^oI{Lux5ysL! zTA(MqB~P1~|0OjcG8I133^9m1@UZLroO9ihH>~_Jp9S~Z)hA@&1=;n^aSJa!jr!T< zTkW91tdRc@p8nJ3hd&GJRvkc*Dz0aSFo_GHs)Nlwg(4^!upMMRzLy!-4?epi&H9xy z#2^ff9(VJZ0rk1&W!1Nb|U{-&(x=M>q zHW)Iw`r)=sZ{?&i$(bS5yAoh)z-8~p$mU2!uYwQ;VNQy?xnbu2wFx9?#xGy|Arun& z{jVee;NDm2!UZ#CJr)6!n%2;&9#IWtFJK#0)12_!>p*a)b%bt@f zBEaH4bDs&5?x|+R2S@$36NEI=z#{QwpzNZ8R~;ej7wk}Y-t03r zglW#2j{~@-r-eRS7g9Y%{#Izt1J62 zPqSi)(+RHnTs3-SS&sZj1~&I_Gc%9QPQE6?j&FnHm+Q25Uvf0$9!UvFkQUDR&xEO-MDz3Jys6Y(xG>8Z=#4e$ zm2<26cSTD;J8$7v`km;d^NX3?3%<0HGvGO63aTIQ!uYq`fI16Zf&2&1wgZ? z#FRM9yITrGiLTUuQ2o?*Mt0HqLl)?ql|#~ zFvHD+3x~0jH)7f2Qu1MMCs^d7J`kb*l+WlCP#~BSzsUKu{;4@8V?th3!W}e< zzyK5RWmnnVE5Le55R4Ezp*R$ZCJM!unHWuvfct$3GwXi%%ePP|yDW zWr_^*z>)4e_!z!#{gM{hd@cs4HK7_!VAgYb=hbwO(WnhIK?V`aJrnIu6#vcvz%4!3 zi7F&rrvE9WZzYe7Y#FYm@ta5g&xC`|AlEvd6WFf3B{--J$n97|<8IlyxgVROyBn z=&I;8odG^wKY$g0_~E_q2{Gsofl?1J)6DVT1-yRIv}M5T`j+nQ>HEjr5u& znUEE>-#8V9zMG`g+1{UWP;Owv8PVA`gZ=zWoiXVnw9=~880)YPZ-=R}YDIXpTROOr z{~m}}5el~#dUOMw>ttm_{^sEz(boyHh`L=vH%$rQyY6Vbp^@Y&{HN2rWN>>8Zr1|* zI-q?eb3X~I?_DHiylqO}R4V*8%JNx0$P%T8>(&^dH!z|D2{ZV5jW!MEsW|S`9;>3; z{@<5H-Lw!hJ&vVdI-zA42ZhFhRYM5m`)J!dl7khEXpXid0LRGDMOiHx{`!4bBc`uL zW6zK(`G!)3qJl7dM4JX1qMyhpU8BuC>NTmY%|1zL-?NhgTh>v9D71|HVujQA-(k9~ zw4=4U*@9uo)aLiz8xT@Su>B?iH%IplpYU+N#_1K`bg$QYSg_XE;Xnl@TiV)#?@TCA zr$-3t$3ETjallxCcG zl(F`)IsIcM8#XbeCOvLO58Q{rMGnnBjg0K==&rg`3ov1TO)lf6D`hr0L+Rwu&bL?;_@NJhi3uj5P zBF{Koy~#T-=GUXdS2rXmQbh=>REax>B!&kJ_l@k%!u3`4bPc@){;~Gv2w^Bit#Tyy ztZXaRKXdVHG?GWB#9BW931IK0Effib4vgSzCw{ft4uS~h$TT4Iv1|88e$64EB~i?q zBL7`UYrWzbK#KEsrjVX{HF=p2FF1PN?lS|B6;9KIL(B(X7z9|N0%ZzeU0of(Siep% zlUt;6NiqX7Oz;FW>mLOIHzfLGCg7&8pOGS!qb&|FY3gzaGYF83RUFq~(dNSfwl4C` zY7*U37Z-!f%<3I0FS1^9yCT~gq@F6SF=SF}mIiFFFn@JlC-box8yzPEZjD6Te`e5W zDx0o7!bK}yTq3Dcu|SN$78{m1S@hZAtea^vtLfIgrP4BRI-As<`F*_K{byDuu!mw- z_dX`D5{!PtC^;YydS{Bj6nsC&-|v(U7wbkClbN0$t>Z?^ST```^jKU+th_{qaS)Ds z@F4@aMIV8GH+b!+k~sjVt50_YN98G}8*)1x%(cili9O&v|2!e|nQ z{@wtMN)^idkY~0vA}Fdqe;}v6@cNITSq{MBWT_*dG0xVmCxsu6=ezf%e2wOFvlK?i zAO}9Y%RpxYV&}dK%^Kw0ct9z!B^jC!V31=y)syL#Fs88^+nYO^B@r;jLcvqvM9Am1 z-ZEPTP+q7_T*RW?!a@Ba(0qJ;$8)Y`O?Qryqbd;l>Dk^|?e}=68trbN|Ax*{P>)^U z5*W*-e^-EZ_+~g`P-$Ay*NE*OZuN0jy4rf4z4Ol+U6kNq0!G6lh>hj)Wg$-}OS9tl zeVxtg_qpT!)fq6|JY&KF;(Ezf;W;1lFf&LroIC+h7!VeDyy(ss?OTDn`G<_nV6_sP z36t_fY)xm1*O;YmK0vL`Zfk7l6zC@u)sBy^&`+k}Ze)1x$3F!;?FV6Epr&8ErIyVPPnIGHiEKYYphR7@kwbG55!#L&gu12~w63*WI?L)>zK z*DU@Epl96=ene#hMIG1zLG22GPA+_KSsp!95~>CNkzm1){b#A7dvO-(1$lbP)+R;L z54Erz!&8vCnV*#BrvC_^!h~15AcbREVt34Dgz2|&2S!G^bPy1%kDZlP7G8h}bz76B z0;U*_kM{@R%rA(+DkJWXF2ah=$L;NJ@V)}#GU`K5P$zoVV=qLY0hFY~0P23;$K`nW zcTzDK!BJmkCB9!Z=&l?=W5fS0+KDmaN&w#lVWWrBH3UXtly_iF2OK1bnL4A6^Z-Oc zVWHU=8?!dYbn{mKg9QJx#yQ|zC1&CmCQbFqk!E@G;wIgM{nZLA)Dn*gjZy`Ko&cJS zq0lEEIRr*bF z_3%qc;U?8BTWddmrEFZMok@qL4^bM+)s@xNL`)j*mYXH(kO=+yfAfJvSSW!WC?dnV zd58b1IDzFRKsx;WM90301X_8&|C{RndaTWKt}yHmSb127d;uVTSz(=l0$4Hp$E5M; zZY9!x8F0bj0GbP%`|5&i3<#z;Z-Fxdo{{IXfsRjVH}@X96uUZ+>8-}#y#t%!p(wKt zAEvUE_oe;!LCDRrzuU4~=Rc7Me`P(4{08CYk+B`? zQGUR~_5w6o$HFO?S}QQ!R?Kyp3D6hdX2E|K>Zb`J%s&*Ofxl&o<5#mn`$**20x|%8 zS}<%9%^rK~@9$>^Z|Qo*B3>+A6M8YAnEBouVTJ$8d1}+v5V`idBA;ByX+snT5p%Th zj{KGXvDRYUtd%BAMwzn6nU; zu`*h7NJG_hasVZeA34FlWRZ zpR0_Lz(~umd2@g;tLHG&qsw5Gq>}djI7+tus7Ejhw{jggcRP-2NIU~R9SA^Z2(l5i z0l_%C@A(^2KkgBAhMSJ36*SzJm&5C;U@L)MVn+;i@>001KqaO<_q2VCJ+O{~mSJc! zR}NHAr*i9F{uke_!$Zc4h2&D?&Dnul|I9|-y7|FsCZG9bfLCYsIxx(F~H0S6ng z53I|;q|JZkB2H0EEE{xyY|SrP4zQMFvDonFq)Tft%?LHxJb`Ji+>CCcs3RsbrpwWcGPKPgD$taT|F)N<(CsWH9$)=&|U zIYqOLeoLQ_{WdzA7rD1{;rRgrTPm_U)2z{Gr?Fhxa@o3iyR6q39B*_(k6Nyux>oG8 zDNgsc$z~4|8}Clqo7KQjGtO0?q1Ve0t^Hz|OmRQiJ%!wUriW!L2{BYqk~Er$AiR$R zn{qaRJPY;Zo_W{+))&dVQ;+Yz56B2Y!Z$D7%NzgJkz`mMxL;5l$%yyY4ek+Z)L5kL zo@FB!-_2LZwa%-LBeCXZJAWy37q8q^G6;a5cial#An%F~|NQH5)2P_6NNGYa7Qr}D zJWBl{XG8-bdLZ#dQ0}V_NhBcoe#Px?w5R4_cO}#JYkS`t4JO}ar{)9Q^V3PJs1ywx z4=hNk>?A@kHqi|_gndRsH}eg}1zoJTLO?iO{Fk`y(68g9^z;%0icN^a&gWQ>drg9d zhJuFVrZxEf-VenknN|eG3!E~8L?vUy@%#&x5vv%eB!34 z{{BT7u4QG757vy;W{y-Jtfx9^#rG>}OKacFJdGk5=A`0l@)re!rmxRHP%%4J(cJbI z_*@EmFkz?}cjm^VHO!wqe=dQKkFU5mcjTH=Uq4fetiR+TSh;oSSKVVKQ!9l9F?xu8 z!WFsev!9v*DGnQ`$}1*><H;**t4gW_D8XS#N#r0mb1<80U_m^YK`#n$uE_{cLdsR$sTcF$Z?4X&^hqNU0m1Q z@2q5DxiXcv2&d*0UIz&x(;r={Nv8B-PvA{tC24inM+Vbo@~X)yS)e6gtHl38{LmI` zS^kq`z7mEO6Zyr97rAY1qi=o?KcfF!Mi9rX_nSdlm|5*o78P^W50|8DazEQx^ljOm zm@lA7HRiVUJhqNe3T6)MxnXG@b4Pj}FjOdYLBi?$++Holst9G_uj=Mr?ecp@agTXn z6FOWTu6NQ_HM*hw?;K?~xf6EQ6Mxhd7Svb^Z?Oq^KRjqYF^_lOyo51EMq2T5*YhR` z^EM2mQq5}`p?<*yd%40acGC(|%%qsu)5{qq!-|63ewqy2m(=6A)egd4HvO*kJTkbFol{e?hB71aUENBnw5PwxMp93g z&Rqu=p6H?dYZs{~q{D{EJ+0x&~zKp!*Z#aY?Q>cAH z5MvR`Evqjj^hs zcBJBD-}7kAmEY}^dUvk}Y6}z`wtl~ME(*5E@&1`1!TRHM$4#Vf*VIE)IM)R zG=Bcp@*LE2nrzPejQK!0Za79F&Pi&CmzivtA}m#bRzxqA1Ra}pLnUXOFGVqU`rutS zV!P#-ylT;D)kurO!ai4g_8(JPyHLYd^$Y8N2cGXDk8D|4B_3EQNPNx&tNitHsmC&3c??h1j%?IL0I;+gkdZDi0%^zfoBT zRKBKG@TNlGFW8$6SYHH-=P{xtj|5?Mki}YJ7b#~wQ+tRnzHwOh_W)JWp*akC9&mr# zFJ8f{El${XWsIabTz(*HzjG5&B-#@o`|-{0)BfuLp)$<~OifJI>@P+qOI-4zvFJY5 z6Q8ioM`sJ$wO=B@MTy)DC(l+LBRyV{;LXAq-=f$H!pYB}v45du;O=M4cF!=^Qo$&6 z4^6UoRZ-*-X(I?>ch4!TLzMT-ia8O66voeP(875THA0Vo%q~X2AT(ekFHq z<7|W)<(1-&b!&x#Pj>&9x}!8xc>7*Xd>x6A3GVAW#D39B*1NpkB3;Y*muEHSjQ-%R z$5Lk}jB>hQ0JtzL7k=1W5*+LuxgC02h(K!@O?* z?q}*A3oqK5t$BQ|<_rgS1de{*m@SZ8Kraz!Szs}GaMSGItF3TtNI#Aq(Fn8_`i_tP zf&yarJbD0rKaGnNwt(WOjaIQ11JaiHW>!a6sIT#L*G1n&(RL!cZO;&OCK5w#VJl)- zcro$~3fo(;CY?dJB4ic@fAA;nO=1>DJam34K4iP=miWY;i)RRXdk%YM*P0AO&6Bpd zUjEWzGibl2d24a4l$Lp?TV3_U)--8YaSoD;1^9_nKGuVqrqy z*RNl5=T&IiUhQ{QI88TZ=XOIo5|6WH9v&$linc@+*`szO@)_MbR@466c}JCL-Ic!= zxux5^3yIb<<&F)Z!>iQab6V=Ki{#RWRU>Kr_A%Z8c3a^gO%)5`)ar%x;(3LS)*hGU>QypQ0Un)XzYvAlS%2nu${$7h!l^HD--j%?jNwVUk7y5V=7|# z{F%CP8*M#mNP3rrr{(F%UEg5mB~d6g43C2{u(H!HfJy3gnW$yxg94N31NhdD8+9r| ze08Omc&rBf7Q%Nos@odx<^$b-|0uDbianLDW_wjafXp2L?~|~mWHSvb;P|Ydr{P{L zPWsziwY6~m_}K!xpkTy#EvACL!S}iNhRiFnSR70vBVAqpW*07UzoZHhYFL+wBD^kR zvd803BK3kMJPM*hOH=6$%6C$DY7H?vG&k4~!l{1iZfZPOb1kVH3hrL)bNsD`jesLRC;Zlb(K3B?N z>g*-EWUk|v_WkjDNIsjMo&oL_t6f$M*^gT)kEZ7{9rGw@i2-PgOf^Cxyc&djN=9W}Z=@xjzL2y1H!)!m9*KlCBVP`ckFX5BY0 z`3N2^e7hjBi{9jeuh`Yp#dwti<4^IY=>0=)QLU02bxB7#0BUi>aC@N_C} z{D81Yazr;#{8mZ|rcam*v!_RvHh%Q@MGp-l2Iy@Sse?(dyZ6^JMJZpWb4T{AfFrvO z_x{;>Vqo)hHZl3*hjF4v#Cl6a;>TV7awB4#yKs_$MFI5a=!gM46dClxgGSnR#fxRF zPvECAa&pKYO3T)q`Bl}`1G^wdz}oF7E26pcB$`1uRyKbYB)AJxQnMPi1*WEc0Oyf6 zMXrH1)DoI~i=CV4#1A@mPI@Cj-yt(hDJiKB!e+WGB(pclMQ?FbWM}n^FUuB>aQE>;#>j#}m651HYeZv}$Cs?1nFd;x7hHsuWAa2~fKqS^A^$^NcXMK&?N z!`ED7;y2O6lP(2#SLM2XQjHU2mw%P9@uKh9>;>VIo!Urwx?}0vzaQC4M?M6~*|bY! zKBCHr^tm?cdmKoz9w6ek@p+MYoQe+JD(t|%oq>NAfcF7Z-ux@%%_I24v7zDZlIIH@796J2Qz*sfR}n zm1DytPO>6pw~Ys?keS%o66EM`gnh)mw9`t%gRW1At7L8ix6ot3xqF{()M4jR`=B1O zxlzChBg*S!Ft)0_P-nf%Gwz&e-)^e9N&mVakuA9l$@^jctc>J?+w0C9i)mebzqDu@ z{?TpW<+=|+w<0o+Xfn;Og4=aoUVgh?V0$8X+I4%fXSVwwzsS0HAAz^xMK|y0-oZ>N zbI~<(?HFAzjFiu~<@Lfeo8qVy+{QSNlzq91%niyBg#R@`luuBrNh<9MLnK+fv;X7~ zW~VXQa~t_`WN!|mcazASDHmbMi123qJwUp}47nAH#QAWGz@%@SE8A^Pio^Z3XlM@( zIhLcuH%&uBb8SVVtb=|UR@8dr0c{C<&66>1273+11K=VxN;nwk2IccW#3&I5wyo}D zZQeZfthS+f`-V;MqF|0mNQg+7ce0&>IFu562#LBOf|=Yj!66utFLQcN2EJP#_qPRm zGV3f^DlwhtMkTtj{OjRfxVjH`o&KA!Kuh=2vjIg>SiR8v>q`NjI;NXCqeEh@2hUwr z8{bQ=`~56Kx52%>WS@Qar_ocsyL@ENkaVhar?34t@20SVw2eJGh!AMQ?z4sTdwg$;_ zw}cPL&D($fy>Utm+KHvS?-OP=YWdwUc)i!T?8YW^wP^6k@VoO3A)KF@v%%3qsW$~r zhtDx%h`Dxl+M$nmjVU^fQ%NK#V8gJ0j?-qx4YoVkvs7=-iHS=WG$&^_D3 z<<}nv1rAa>tNvO6>0$CvW`Zfbju%Dzjir_el>XtN048t;owFPtiHIZZ%}%BF&&Pdh z_e+Z>tf$joRn-r(^QR(@J5mbnimuZJ?c`@PLfSW_+U5q*4$*zuv3E(AMc7J9!ly2T zx{vv?8?ufC1tWzwQS*9UD+w#k4~OHCdD|STF%Fa%=1!gtwOa6pJM!ysM^b}$=huSD z;}_k7s8(P|3JVL94y8*zkS3NP+7~VL5idQ>$>H@9X3MSlN$+Mt3x|_Se$*Y_Yy4>9%RTerE}sLXWkO%(X&K+88W=q;+9gRRb^pq&ypJg!smJz(0F%?NRo(SKD1D`<7HHsVkE-z zu*y>MIu#7Lw_|CI`cTZped%605fyrL4}Figjf;^~GSNCuLTTl~@1c*M_Ttj{{pF9f z=cE&nX1thv<-hjpAK5rnvK$o-5pdW(&NNK)STYAkFMopr1?!83TUBpqtJcP(3~6`j zC(RsliMmBfV<(=?jWz&pXp{FQ9w|gP&I&X!rWFs|~r-=d^e?HVJ@=QW|bkm9U z?;lBq)>$CK=7O1>ODzlF0AE$VS0sd&>Dj%|*$(<4iQTn$8xp*M^!JjAIEtJ&%GlT# z9rTs>gkhVNI0`jhd+(OMy({7BXez-VYqfs%E7NoI?AjE~PX#)EShLR1?v0^<8OTgDz`0f;weL^`Vy0( z0E8!loEyzHYf|{9@W8^&j+iCuCODkV&}cE%;Mi6k8|*3T4WLqlthRYv5IuK01oZH_c&q;X*^zdqjV%HgQ7~D`vd!Gz zHn)OT79A%hwJu9QYgwh8C1?fTSRAc@(ocdhdh`QR=g6#AS&A0k2fsl4+v zd{SvED~8gtGCyWP2*Sgbro@H36)A(9k&#I5Bt58L6*ns@duQ643r`BK+5v?6MsFLm(?2|?Mui)lv)vWDrnsWt9R?~<(7Iq)Sm8hER@7g_U>jbE6J_#4iQ-ry-w&&xHl5GIuda3iY}2;S#4I@wz~!HUik=Nhw;PtO=b~19j)4{1d3BVz1WID z+#f`==$~!$bH7^7Yn+{*{RY!ngSI%iyc|h2puGKX(5g>?@$4+ZL&$CfH8B#A5i>O6 zT-34lhU>X@9VuH#Z3jHTs3m6@5W0X5_=zN?A6WZHeu&`Wlgd>E^iHphuFLD*fP=F^ zIv#2UuciWmXyJ2oQs44E@UXE&h3KV``6sDE-DItKv%53V~d=|IHlhCI8Cf@~f+UF3w0j74#HbBG1S<{J1CW%F=%>gEf zG_tRVNU0mW?_yU;AJ%wbvfuBF;;x7ZVj@=&rhBe(aWT)XBp| zpyl#KbEs7wN?|AXMp>OUvNQbdN-eM&4gzQA*M$zc&|6R}*JZJVzCT1MBv*CqUcQ?h z(4yAz_L6eCnUd0U-#w@G+L=_@h;F}+*j#9cT$}zlA#8&DazTAiNY^R%UNPZXj1&PP ze9N%yinn$=ZUo&0H>mA#>c=^q6sAlV6N>>iDgCiai-sV?Fxq0{ zPSi?XXMFWVK4imV>lr3Kfn+?D6xP?`IBbTVNoxn1@x@R%dP|&mWWsULzhtS(`fcuK zM^PV>**|HzoKLEV{892@1v7&it9>`Pk%+jytU&vY`FUUO(GDxoVblh&zUT!wuhEkW zv^g-gpICgeJZ5&W!-6L~dHXd$a_01uUB5C{)o4rX3w>(Qj=)8u9r!2rN$oSjfsdusw-)Atjl%9<19vJFDb7*CI@ zUKqA2c9M(nL^XRUba82!9+Wq-2LalRe@$QU4ef}psMC+Ey8l3kp)IZF#nRPfInEPr zXVi>Wnimvoeh1=K%yp_>;R!-;b%ig+0zI?Il{Hs>+OYLXQUqb*h%=NgvEo5H_0G<0 zn*98Mp&7)|!9Dfm-rCa-F<;I-1^eZrVhrhF99oyBop|B%+noefe2#rHm0p(XsFGGo z@4E7ntiCpg{#TtEY&@`FM(J6U`*S6%R?2J^>i3$f#sIj`X^bTFT}+Zvl~~wwBidf` ziB14|UYm!_9|b7d2R9AryDRxru+*tD-ykzEq;kGaWI4MYd?Npb%+T|7h?hS@S<+Mj zDJVC}eP3b0UCIKr~US$JTvn| zj(c#m52`wC+V#%Qlppg|1QaLsyA*3z90wZ zRmn=TeDat(W@&yC94kYH9~1{CQsA!8u6ehPCb5om?~`TvUV}y!-*&a#J9^+?g#blL zT0>i(x$m80$i%L~fqo_nTDj3hh3udHOHyT8*A?%psgG8nh3Nk}uGb2kFHG!~4^<86 zE5Lf68~Lr_Uo*ZfO1YML23D+Eo#>9qsWxoo`cWg->x=TsfKA8q8x|*ijA%N5`WpSh z2Fo<9s=qmH<MG=r3sF%>%h_U zZ~1;cw<^av^rFmKzDCOQx}UcE%pueYotJ0 znFh2V45_u>(Qo>x6X<O|fHCeO>-j(gjj;2P z))!3Kuhy$UOZd~Suvq+he|;H>Gd%B!tuU9sthzmjRM^^iJOcKE??Bt4owXb_=Jj@!PrxSKI5kj&Qvp1BcF)?c)^$5t`Y!Hz#J&b(=*re~t?_ zP{@lN`a6h6b`xQNwrm8%Of3(&)YJv%A}xPbaN!_+s|2 zdizkwtOU`Kc)#Dq)&G(WBrgOM8DX+v47ACvusbyOz zRYxNzQZNC4Bama6WCjE7E&s^DVR0b*4B-qbj}C@-;? zV-4Efs?cY1g=FLR8s@sBW)Jkq9)dZuHMCn75;xQ5@?RS!V=j{e_c<_C?HW|swz*^2%dn8)c10pExVx*4s^2XPLcOY`NhSu%cR4?zxA;~16}OS z&7g7Zo58qhp@=OlG(d~K=YcT4IS_QwwQdJYxInzO+_sPq( z$!5)(NYkV^Yh+^02CF9j??U!Zdr2Zj7BpX_Z5A88*{!xyt=>R449!he!oq?+&j+H& z(gZ%urJuxqzFk2TX_%7f4yc0b+$19NsRc(?dHILZ&uk8^w4%cHXj;W?X}PJI;$}lZ z#6?}}+3B$#RaN`>@QBJ#{o53wR#zXYJqJ4%0D1bZdB1q2KS7_%OFM}$nUfw)BXDLW z-i|TA3BthmcYf_MdOz~1cm1fc=eDy@*nIto=O5H?##lWU~ zL)N~-;mX}HeU&q4``YTBVrases;7V*+ABgnVJz!o9dj3NcI3odb&=LZve7=86n@SSc3-87zD?8#7GE z0{H=b3L49JAlMQ5jw@Pi)s=?QGo*jnctKug$nb{!EGS}hxc+Ce)%|KbBQx|nS z!5Rzc&bddX_lQ4I_Mv=DGR>Olz@Jh2+267@e#FeV;PVs;b5?RtG}GnPE7ES+j;wRl z#As~QgxzpFws!C9-=QLz$0k>h3_HK?s0vnwqX#|21oEl;O5$%K~6M z-wWS|(*pF5~mF0bOF#Gtr};XTJgx&9|#$a#hp$y>XY9mmONz-Ab=~r+z{! ziN4*$9BMLanIZj3N%{^KMS(}omX-%~dM%D>Dvvzn_qS6-&-$926c6fmmtWn`*m8?A zVoli%y6-i|UCLJvGz0`PHW;Ui8No*LwA6{(7x|4yjoWaHpFz^8sW#>#z{I47B%(fmb@LI$u!?IiY zl|{h>OIplBWJaSPo5p%1=!6Y9|8-`rBa<>G=icuASl@*@Xl{<#<?P z-pbT=jd$%eUZLC?O?SryyDJNBrw7TbMiS%@6yfgM)6>)9E|!wF9?ZCIgYvvs-{<|S z?jBaQg2Izo9A6uBm>r^pT)|Y|ip!ai(H}7ht)!&8(eIMd$IHkE4AZk!>0eI+M_b{( z#MgP4@4f`obZ33yn-C(U22t5bImnr;vA66&R7Z$t&Ey-ovZ&iWH?;+@?C;b7JWVcv(fhG+3}BC z&8OyuSyP@ZnT2g#F(Lx0lDVO53dKHKEAL)nDsrOrgk#wJ*Mz5x{YU)k&?5yNERB7G zX7;zHA^%-DTxn8Dm-y|M+Z%F(m1pFr*m$tQS$IMvPhnk!lp6^N<6mRQj+AlJcw-J@OLJr=r zjlr6?Mp>1at#fmc_PrtR^uoMXz=(peFGNawm*hJR;l< z-8jV~VlBiBHtH2E8KsD*t=Hq6F!~}ZI|M;t%*+&=N=0Off2gkG(l;LBoWd#yWl1ix zOlX>-#Z+uF6%^}^>80&xK>s_-ih!j$A#{fTvxX>g&j789IS^_Bqy$QI`k;y3o>;W; zwa%`d-+Mfb7ed?YsigkKspzu-kEKG#E`S5klZ@@0XN z`i($_GQiiH3|M(nVB$8ue9CId&i#`h9adh`*#}9#WD4sKhLOT|bSwyR$^uK>+?pKD zoL;(?_n3x57Fd|x4*^m8DL6wS_W|56|5s;c9ah!WwtEB-DUnuEx};NBbayGJw19LY zut;eE=?3Xm0cns0NSEY7kdW>U>F|u{-rxJZ=lps0<+V3nd++61bB;O2^W69Ee%7~s z@pJiVuxo3q9eC-KegmkShXs`C0&4#6-zQ9>RD!GH&%jIz@Xa9ar!1Pee;A~NT0HK# zJ~~=>lT4)Toe}b%db9T?oM3jlb4_(abz*3DMySwxm5?J7@xnJa*)7)d4S9ySD_3?rxapmS@q1>1ekhjgb%m;z<~$n%TuuU{TO>2cVBbN)S#tFwgh zQ0jeJXFl)pcELHTw@?B@nRL&Z;Y~98!qy&G9@*bAWs%spewdIBai23F%~U$>P1urP z=A2on;|z|+t=nwbuyPwcqoMRUt{zd@-{>Gx?4FR6v}klfeKwSeaQJIIu78S-Nd7uJ zIT^OJvvyOYB1mav_v=i~$WI(v)Dv^-S2k<0xf+COlTs`37m5crT4^!4m?b=%{h~CY z8bRLn>re9fT6a_FoMg47-aD#UCgO2FmI-03w2XVjhP+MV+7`Rc3_ADlab+5Mrc6Px z$YAj01b|Kk$=)ET9L-ZH!+w<<cIFkz0apxO=0){<$1N-RwJ0Y_EX>D}@GDf;VAunbZIpgM%3; zX8rftDSiTwyDi}iQKXY;&;Zex)zp@g#fne5k+{3ART%Yx!Wh8^Bz*gE zFVjnE7l2h2r}OrQ_w!XtzPt7iB2wFvxkppRr$WU%d7$6c&ztB)@Kgf^UAg>)iASv+ z3$%(@jHQRM3h7;J!c?oYI`xz}J0Xh)nwfMTC795@ih&(^#L_jva$&Q=SE<-hXNPmgnCE4G*?h<5ic;NX<@Gu9Wh!3vR^VeNPsL6C=-a>NI97m?wwjqS|e<4PkAQ7;|7k?$3puTL>Nt6Cbed78>7mXv*}0c?5-9>ZWXDLr{!!HR%l zRs2&$wcD;R;X9-%SDsGAsPDTS?iN}0v2`|bSx!h|H$0f9(aMY9H2TU6U1OmW6A|Cb zaWQ#y2fxv^vx1}#s1FSbg*A=;!BlFE z>1XuFVdG4X9mm_uLxtxQ^KJky9h68C!21N2pjAuC^IBKTBu;|_gv&VrA3w)80N(-y zYWRN?mzQ?~q5%++j>j+YzBV>$3c3#tZp<2em*K&m(*kfu@1;~KOrwET z#$$d=!Q_Yo zEH2sDcEUz!D(iigBo#tCY04D%2b`+Z>gpx`4n11_Aha+BRW5veF8l?@4gxg=QE?zr z4Z6B60)P#2J8Zr)R|Zz9#pD2yI|im1Eq-zzA~*kBbvZa4Z}#AIu(9=hUPZ*Jt#yNZ zPP8|jok;86iSm;_M_XJ{ucf@bJSR7g{1>z*9XU`$JrYJXVTkQ_LB0Kd;8*3}YjzhI3HO1Pp`A0(uYQ8AS@ zKRV9bcD&c#jW1J}jEVHLn&&rpl!GPH+&~(}d1^Kp*KaB*!WfiY8CbML+pG52YX7zH zh8+NkLHvP*7p&sv@Cj$Wu?ZmP;z*ZlrI$*?1@sZ!!^2y;koXC9z*YjyOFTq0gmB(l zrq7+7sll@l7UuQjaS5KVpn#V<%kJua5`_D(ZgKo|i8UWi#JC|7fRY=jH-gsL880}u zBq2%U&7rUi256>?!=&TrtX z1&`i)kYoUalDwU2@Q?SWfP}a!Z~wbJW+F|$40KPBgcfmK%5@S?3HjoV^r3YLXV=AL zR2z8cC(kkk5VwM|7me2O5gTgZOtYlwY;|e#$pgM4&o3M?_#}V(^{tvqiy0Nx89%_` zPGoqQ9Pxn0OtvuC`%m>_HWFchQ+mZ{tupvhhxS0m%=EHb^l$9Qu2(=Q3j^VtPX9i#$cSw? zT#cGK5Xlo(Xj{9|lRkj`tTs`Ku?zXa<0CjY4sflz*w_ys)4MAJp)ppXKTD3X@7cuC zP8|%?Z^PY9IiR-y7SmU=u!(rG$!`uDRLs^k_O8rpZ7fJq&z}8h(s+-6voDfu!Qy+4 z_86x@!!koSS&uHP_u;!Hu|yRWIUeS;Sub7!0h5e}GI&PWEWBuGpCwCSL}sGAy88C+ z*pSf>c+!At{rRgG0|`Gz2k=v*JwKRrg0IL0TVyo<@$spJD5rGV;2T5d?;lHBfyZC5 zB-j||bwc@VrHtwfK?M~mzU6RAU|3Teqhguavwvv^&M*y(gm0uHHaBd-pzv$bznfJs z;GLPyo!b_UAN<1mpG%iCgB_SZ1x#p z0TE1%f+Dk%9~f~}H8d5ZY4+i1#d%M(j1DxJmzIwx)UUUHjwxo0_J*}1Q&|(drPjWe z<1e^1emTv!7IHeo0U9Cbj7(Py`ZWy&OcR=xh=qe?)tK5A?gFt#4|$HAT$!0nlO+b5 zy$6`y@z6BNy>0Oz(aU@DSHbq6^gFwrHG8CCkGbPbvx5cgWh0wZUrd!oI<`3Aob)em zw)>KabYBX8tl9>^y{kO;zhnng{moo14;-?=K1E|zSWFB$;DpfWH5r!M71l_Z5U|Fh zi;E5Pa}hk;c>Kc9@Xmw5!1lVe9#ooS6YZ3IZnU?6TFLqPmCPT6-_i5={bwvaF;7kq z*F}0a-WdwQJ5Vg?7s!?miVq{mK8ZEc!hIfXukYY4u4G-AzWfUHr6AxV@7o*uF*`im z{;I4Xr}lW3}lIi`k z8r&9J&7HSg8PY4lU&(QKkX!GOixtX5`f9W;~NZK=WHnZCvrME!_IRYPG;fBCBje;l}>P8ptf73FgByu(RIruQFY_mUM$d!ymknCh_hQu*XLU#BVmsjX-A@7nON{=2qYyJ7J{8_nhK77argW=Tm2e)c4t4g%W z$)sXcm(2HiXx|#cnoEsR10b*5Ep(b(d)ZsEsJ%;044ekLZXbl;cfh*3o$tFbT5Z{E zisV(NhhQcx3AiW~Ah<;u{uteR0-X*Omg}>6O@HFehRQPL_K8UyepT(gt!Ri!JE@Q1 z31jBS>|)E0$$6+A&iLx{Vpv46EjqEEPJwY;4y7CMgPej9a5V&i!Hlj?6P_cW8;u)% z_}ul<`{^?s0IoAYf28rrwWp<{w9QtRp#Aps>gd1e4nwzYZ!m$53=E0Z-`bG13KMrg zRSKNLj_2ytkuVQflN|q4cqO?Uv8LY7W-=uwox~gVxqqgF(S#*CJi;gG=g9D;F?_aD zY%d6lnI)4pmN_IIQ}0#*^$!&DkU4$rt|Q!*`!MR;q{7~YhTlA{x=5MaQfisZB)N#C zUZ9x!(*elPv=dFbsvB{R8wd9A@1aQUHVql8Rjh zjC`-LquJvJm@Owyoq*A|zK(c1I2cP!WN~3MTdMTq`$q@TOGL_?MwUQQ()LreBYh3c z7N(8h#jTxeQWUDRJ{y_$4`m5M-#;XM$~Uy6AhQp*i&MbwkVev9>T32(9QojmXbQ`%|=W!-*ztM`oin(}YEzujk&rk|Ney zkG?9I92kdqCXpK0f9mz=a!Ww%J}csJ7clHlH)Q{*y#4c(^>2_Q1i}QLi0Vn{QCo0^ z_6rz3eXiM+$6uk4i8TK?tyxpakCJx5UNo?esAx>)!I{m|xc-+Ibc=bHMuhjYmed&0 z(^fEW#K%+Kup0l*hgTzEW_5i!53SM2#K`Dt#4{bOIx>a=UQj`Bel0%{CLFq3tgZZtE}>eXbAK?Q;B&sL-gmJT@ieWXx&${A&YHQwQwXA`1n1k*2#we}B`gM( z6wA!%drI2++kta?@?XlSyTxolr+Oc#>!-gO??5)zi<%$6RJzMc!)oUqiwsjv=Jf4V z;_5taBNeN1Fy^Aq>ki(KeeqA%RcaV}+6j+`X&4W2-Yfucae8@s>M2RB49H*^^woimhf%z63YvBt|B{ zI}aBf;v-LwJ)8`pT(SJW%E}wS-L*zFicL zg7P>LMC!m2?kyfX)woG!>U_qyE_?(Gh>em(q{0lYe@7?bshBx|rrCDAL|m>Sq3&%= z!ZZ(94ki;zNVQ^NKYjhOlVx-L&(Om|9!NXgtTEKzC40zewYJqbe*}ki)901-_N;iAsXiL|(0v0kb7BrJk?B!!X#AK(0>_Xri{a4HFO=r}_ zGfDOLQWS;7*>cL1?VJ827Bd_^gAF-wC`i+d*cX#9VG{}FQ=E4`WV%+p9w)h*SVE2U z2XOh2fPXH2fa?D?51<$o&u5$n&Jq6c1h$!?Wu%_SKekJ;>a) z4sUS=O;tdxb$)Fm<>;_NHTlQRUer!p3yzsSs$!@b&prpu8B{WWVgPbD9vgXz6jKwS zAZpf{AU09PlpEmr5Lrg$ARY-)PJq6%dA3g7X@WyE7!{?U4njABkU5S+J^%rW~dhlo-P4=t}(g(EK0P270TK0Fjk)!`8GtN4hA} z&Kj|UBBxgAM)uVOBq-@28*9+ zi|pk09jDqnLdc9+;pSOjcN1q{qSxP6#czjzK%c{Ld8hHc0cE&fgTbfV{ z7CR9(WHYSm^)dysv?ZbT-e)T=J&*WooFmK~T6fJvVhRm>3W6#A z!3s!q3;8PX5_F<_h2N(P(#0Z`2~TCr$-?x-Ki=xgN8}WKSHFF=kbFSYy|!Z?L+R~> zTe@%MG>TS03@h(8y>3x$7nE&oo32=@!I*|ZvX02YwPpbzxT+vUKkZ%UfVUxL=M4O< zHwF#dLWZF38Jn1Zx<|E)-HwYlpW>m(1!*J$?y#Gm(#-7at&>q|f4Q_#bHIYjy^VZ~ zhhkqhJyi$Gw2Fg|Gy3i_28t(ILS79$#Vij{CVxD;Wn+m(1}53i?fQ&1f3G@Ha&n0O zEoY@Lk`U>!nENjDx3|JUM_q3a_!=76Bs~92*v>El5)U%SK84*VPJlGLg#{B>ORs+X zVyoEPO-y;F7a4LPtWSMc1Tdya;|uAwGF0b^t;XXT_drPwfF_60=u zg>N2TcaDpDDT~?gQ3`vUy|HIgY11dJ{StW>n}i z9V1Ts4-0Ra6o(MrPmm9MbnS-id0J2=4d3f8T&|#Nb|EWu9=QyV|K|vt$rwt_$_ZPM zGtVK94l{;2LBERQq1h=27ybJVwuW2V%ja6`2kfb4t1ikMdO7Q>V_1!zDE0_lcpE}3 z)(@K|NS9=y{=+9AdL&cnWFpN&7FlQjeYXp6Ock}wS)#4djC(}z&KvN$o&I66o|0-c zp6+?k5QNPW;+1tnw+)I=flvh+3J`yKeu^t)dMR$a=QAmwK~Geav04mkp|Iq{8ddI> zB`jU*pKL>qTeSmBV-Ygxyt$_xs0q|HS%geO^BujsDKDP+$BlL4ZB3M_`2uWQV)Nq6 zTMUeP0O+b!?EnYg6`QNuw-<+(D}TlM!d?`ks* zHSfAL?jifggRdU)CzgPTCKL}Y9z>!ydd6)sGru7zI**e%LIZ`m2apLq#rGQ*Q8hb%n)S?6>cfFANwoLI9f(awH6#V_X()>_CIb z18HXHrpz{YWB$XY^9?jhRoJ=Z^cp_XxecaBbevHCX(N-43Wm7s zr@29VAu!R>wGHNc^&Je1GCHED?51n_I~ZkomoM*AW{^5=IM_p1y{#ezOjM=|k8#m>mI? zK1oSu6(C0gVH#7JGw&`g@SR?QGsVY86oN+v3@*8P`Xt{Ro_lhNeDhl^N&;I`hG7t2AU|LKZMESfB$3imeXWd*24fEEL{B zk&w1_btI_qMlcQx_J#i-O50A%|G5?{a_pSEBXFMoPtKGSMCZg8N=5e=6je0u9$X!2 zu<d}t0tayt+?7Q z^#4XDjnpz&Jf#Cq03iRM4eJovfDQ{H;-C!(O2WN)!%s%;02uf$4bk)|)ln-ii;gN{ zc8+2qnHN<92^xI3ep+A_8tr(X0c*%EF&4Zbo!J_Stjq19FHkPPMz{Pn3Mg^gcHB&v zspN$SRLLnn#JI^%Rj7b!5gb@X^Y1@8}NLqdY*m0z;Z=Qg#tyLj=dh( zA!u+s%m~043MqtTb4?Ramp6uB^urFQ7E`7(k#W6KxJm>!tjxkBs@wNin0R665Fqzv zyt$`-Ay(lJy&u<#kCc*eS8k3Mh+Jo$R#-6x{15n80OU9cXK&6JvA=MBta^a!I67+ka%7j90?O`^B_j)zV4QFK*U-kxpmVg%!QR zA|mYNOF+Qp21hQaf^P@x-f>0KGa(mqyR#HXsKBH_ zZnD8}cW$iyZ?SN4%#?_vxvk2dhlSdK=#$IXz`ur;>(``S_4)h_2HAGp?cn9>Kc&Wc`^GD^vKW!a@MiqAD^khg1Q6>n-%#?Q%=2+1 zo@IeZ60GZB(3vjujF^u91}gD8nOtVhbdz;mml70|2ds)Rl22Dmxqf_b1)B)aNWM7w zqUro=fU6I8u6oK4#F@d z{))UM4;^JRy<$2EZ=N-_=p4S^wS=)SM$Cp#JbLIT>2^HKHf2jYQC^HOGht-SiP=IyCO%z=e zl#aJzZa)5IenP4$>~QLEi&>i8pwQ0DvQn4q>VaTHTH4g|VqxKO7-9DTNTRENho<@~ z+J2$Q&5V`d_k*@Q%23;{ucuvFT}wk{#hG5*TNeJGXoS^94@Vq^QlJQmhrw$lhp)a< zPplx1ehlvGo0oII31~7)aN@m`$;??ICCdM)%(uZnr9{ zjV@~@Mjskl0M-@N`ErOESZ8Tuq}kxNpgH{9U_lIJ0V}|aH7FPkV0b+w2(( zBX!Osj;x{V^C&?Jf2J1rUw8GJZ$qzLW>hD94q5%w;5Ci6;t?Oh;g+A^SDzPz3!2}A-O3T5Rw~kud}9R&fhGty_8t_y=;lc`o9Hw%uqk9 z7hJd3|6aGp8fmAP(BmArXqoR2Vwj`;jPID^GGqDg1$-p^%0fdd913Oz&^`IE8XUJ` z;5A;Q$W|BYG}iG3xIy(|_uW9vn;nDA4v3#;)JC!StK4Ev6|zX^yi<+D{=CXJRh!d1 zcu3^rB-6ci{wT!G_gnhLOG{#^&R)A34Jut_Q_@lHDwD_4Dz|^97eeQi&29q*nA6ma zXXN=~bB{j#_f^TNd&^w(tJfBlJ6`#c6H>Ww50&@Jz3IO_XL{bSp1_fe;mGlWlIjk; zIvca~_u%Xh+v@?A?`OD}Q#3c8D3$t@j8FEpuQPqNgjulqs`R}}|sF26{VF^0KG*9dL+k6wt=NF(nbZ+zoBl(14s z!oUjm7~PGeoP@!UsWk)aLs_Ek_7(z(e5ALXGg!t(`_>$V7@k@#(uag15zkW@mE>O) zgPPdM=h27p7gMQ~=vUs%%O?RvKL-7!8lq~sJ68y-yZafJCj<#iCtuTU00#}sbDC+f z$F;V$j{md&@~dF@slV{_NSF&K3lZ0Rv;fx=NoR9qr%wx&ms*_v8M-6n{X1w0%+OO= zb=_2vpF;eSxxN6lW2S&#*%7K*byj5?HYw-Q*hba3H;_m5^hi}2 zIe=Ec{ep^_OZWJB2jS1-q<|l4>uQ1@U+eggE4`S=bU8;%2ETks#gwH=)pu<*#Gx)t zSP*ZOvz@-Dy(-;x#04CONx#}0H-xR!&XsD2z%Hfk6>I==OfuLRjVMVgx}ldey0Hz~X;K1I<$bC`~(P5|4TvV0NfJ3}TQBbC0!5zx~cm!!igepj{eEbw(yr)f8h%g@Qs>1FiGX8Mi=7sC*YGIW)WPF}zF=9Ry%S3GcG6@eRczf#Vc}$mL?z6s^ zAAU=K4^{Ok1=}b4yXC3;EXJYCO2EcKY?s$p)3xBZMG0j(X?6g;$OPBo;<@x4X{mH4s#7Xi^;w&BLq4$hGiuTON$bm>cSWM$}^`-Qw$>RZ!RGG|-;fyF~ z%D@t^8_^PY9GD)d+;08osgwGpR9t;Lz5;dz~n_t*2 zTF!EeMRiKE<7!dM9lZW_pa$9krxm4SXm!DBRA$)*B4jDIe%aUk(41XyZr!0uwWMG+ z)5N>~MD)p2;NTJyR=|zQsa+fi(^gj18q+N^ z$v8DjD+HrXm6Dl5PXh(RaPR3uE{HOT;Nql3W=F29s+Ba=dz|sD*LrJr3?ghs5N}%q_#?d;C*YoCTn{FhH-ZcRb+Xa5rcLlqhigVw{9$ zeaXOZwOZlPtSv;$l)NBF7Iy-HVEB#sG9I>p?pF{XBZtT?kW91!v?$w=a4O`tMI|e?63G6Hq!h%qsu&PUbR_~=)G=^{z31mmu69eCP~M7P9aceQ(#$SCzU-e)WgNV`o{j1dq4|Rkw!3)U z8?G+{Pds5%AGRnOT+NEQ2?8cPqreP|h3C;F8RDUPdU}Jm3*_%jzb6$g+(_nlg2^0( zlq*)bUAWX^YQ4q$T~=aJR3XhmIwHO;Y*;w1@fU}F(|Ed$f)L`HMzB~SdIAh@!>2F# z?c(Y4aC(+}l~YF2TfqtMzW%s9_BSo4U%QBy+f(;?y`6~`m6X)o>(j?Y&SOV6v9@$J zS;(7X$8?&iEXEuBK?OD3!G-rGI5goc>XyPFdcrn2T*N!X}k^@lb;-Y-% zQ~<`K#2Fd>g;3+rgi<*to!P_JY}WD#=`Hm1I)WFP>5F`L35=OBt!T0`g|Qe)ncZga zY%u;cq5z(m?;%BkilrjsLj+7Wak5pXDc*AE+(^c3C}pXh?vg1y);K8uAIO~CdRj^4 zprP$FUQyz}&b|->>&`87a(wesLKiD9<0Djz&O0mCe%T(a3_MZWc#~8&i*hHg9p{BZ zLPBP5knDcV9Ck~n+9BT=0wsfkaQth*gs{t`{`(<;L(>o0Mb1vh4rzEh6*)n|5}`8V z=tXiN;k10sjajI_uoy-+7vR7!E>H){>c6`}WNihf7?=*8@~s)Jj;vKrRnuca{@l@4 zj#n+-F`BM>Pp+(RIdlhZsEr%%u{eChvXeXBk>}I#>Y|5<^z4|fv0b`SG3YZPU%9*g zMr#jY0>e}wD{h<2qtjB+vl$dc(~jgSSu&g{1vzo>AGLMT|NTCHzfD)z%NG=dX{2!vcnmQ(A;o-l2B8JAOAic{l~t+7fop|P8_ zX!(N5;WN#S2((qpJnBB70T$B?nyjh^Vk0W1Hnbx?BXR&kpV>*>qqcTE&(@`ojZQu0 z+lupxSKm^}eobt+w(U5-Ak@lOXS7)~6(xF-VPz`X!^g)d4Q7rR?yZOay8ZA0?oQ6| z-%`dWn+Ieh&L8-=SEk-Y>)OLle6hhZ6dZORj*R?udD?Ve`4`&@$^O6ZnFu#ks(}-{ zXEXBGRi#M|?05|!&VPuCC>>xlJDh({mRD>3i;j3iev|Y7**fo9_FwD~_TSDW@)G94?9~^qBxbz>bae=Wr+2zQ^ycoJ%eM*B<3`)c*td**rA03;lf$Wf^U1I$X=K8NUhncH=XY+3)U$j=>=>4=+q*f-44byu zOgAWm@iTg!oNvmgCEhpi0uV;X_uUJ^O2Q|5)#@|hMhlA7_f`mef7J+a5IAF2N!rKT z2_@Mb1zY(YdMx3FsbuuPFE&nyGkbU6KaQ@=(UMWTM5FN)QX{`adwp6k!9qGMeL~H0 zX}B&UChGX;S#~m~!Ed7(`%@ntNsj#r;lqck@!;r4njfXA)_>I6-fsJ-j@Mm0IeFj< zs{@CC3b8QCZPXP2Y+`<~F#^J{hEM=xWg(mk`knNyXLOC=D=S5xBP-1isMcnaGkB6@ zUn;Pe?=C2X^bNjkHSRE8A3oPS$5KDcOKxs4owcj0>A_0VMYuVc%I+eGqJs7761yoJ zig$Rk_I+LxXn3cmpA#DjEJ@&l4^+97SyZ&EXADX3rRy z2V|a}Pj`-7JqP90%VN89mhW%$;NK5I4zHFEy1*Y)k}fNt;cHJa`P)B$HuF*1-d!s% z1(>P~DJtgVLo1ic3h{<98uY?D_r!LQHJOGUqErBLqu_UDlZhwpL@Jbnp}}CD()fjK zkGy}gM@c^cp0QLwH$IW>3zI(+wpl|FUBS3kcyFNSW{afu%yVfJsIddy%W|WtjR0U- z+{OZbAVn6hQ+OLmK*3Xb1Osgd7!F7Z#w^i1z?prDm%#Vxke$njirMOl|MW?xOLd(D zGgV9Me%^*Zs!d;BcosHFWBmc`sU23gRPA)1=?dHO<)9!Scbb|cq00vBfrp|R;P<^U z-u=-2wbIOpwc#I-gNb)1UD}e0d6rf}VLDK8UqX?*V<);=baWLQ6v$A`tms{6;AYip zT?gD?+D$!wN##MsPZ%w8wugsFYH;&mLFvI5^OV9bM_p@~9q*8HDOR6{XkNTwZsbCFJ4#@+E4Pg&ZgNV9w ztZ8-$LgUSurlwnh6HLAW-yzogNH3eucnM$QL(qfz!Ke4u#o8WaYwSzB_z9pt0pMW8 zbaMrp(jaROADrJ~BCyzmmz0)#A=Vg7pZ5u%&92-)E2v=wx1w$E#5f$D&Gk`FQBp#c zR1UNF@6gLi2J9AwC2SRrH!f?-kF)>I;f<0B6~jP5$9w829;A;>_kR-mR3Y$NHTiB-29uBd4I(%n3}?^U?jz5})XgowD+h|Aq0C?Kjl)h()mjg! zd#G3fw0&mo_8qF!V6KEG>Fu%KCJAq)KECyWH6Qade_JMqko8Sh?)(hTMqM%YyI716 zKT}Q>AS9&876NmB5mKWt4K0`?_p6}S-}mt;F#0;8D9c5k>*ym+RMyT{;dQ^pX(vci zgi~&hP299o-xT%Q*Diropt6W5;AUa=#0!sTPbwDgR`6q)gx6kJRK*)$4PdFL=VMrZ zUc`jo*P!cPt&_0&)cmAVndzyL-YuMkHJ7nK~?U8Ny;z-I$4hQ|#vw4!`jUw?s zOD-8}Ssy;4VlC}USY`AS?D*5>GAc<(;%koHv8pYa_fO0Gnfx9V8Fvs~6c!*Dy+Ua9 zE&FbX{>Xbre^g|wt|9!KWtQpib(PQMi6~5b2CcJq)yQM4BaBJip7Jn&PrCeM(Qzkc_*7TkF)844C$_RAE z%n^Qdf2;nJo$1r%Pdt!c89d>8qs6E_3%sB^=1@>MBS3mO6Me;QVXA`0`}O%kc7CJ%h7(|1^rY ze{Code0jspy3n7^=i9iFz_8-WX-h}GQwiw4S1GbIi2;s3UblO=xJ^3{oH0S1B3xXC zL`CO2n@IKhjbSIN@Nnt5YcYB*U6Bk~x%Y|xVoFf_d%52>#u^+x7yu1`At{^S)Z{xePak8&yI(2iS) z(Lyg= tuple[int, int]: + global w, h, VEHICLE_X_M, VEHICLE_Y_M, VEHICLE_THETA_RAD + r = R.from_euler('z', -VEHICLE_THETA_RAD, degrees=False) + rel_x, rel_y = x - VEHICLE_X_M, y - VEHICLE_Y_M + rotated_x, rotated_y,_ = r.apply([rel_x, rel_y, 0]) + screen_x = int(w / 2 + rotated_x * PIXELS_PER_M) + screen_y = int((h * 0.67) - rotated_y * PIXELS_PER_M) + # Simple transformation - replace with actual coordinate transformation logic + return (screen_x, screen_y) + +def render_world(screen: pygame.Surface): + global w, h + w, h = screen.get_width(), screen.get_height() + + # draw the car at 1/10th scale + car_rotated = pygame.transform.rotate(car, -90) + scale_factor = 1.2 / car_rotated.get_width() * PIXELS_PER_M + car_scaled = pygame.transform.scale(car_rotated, ( + car_rotated.get_width() * scale_factor, car_rotated.get_height() * scale_factor + )) + car_rect = car_scaled.get_rect(center=transform(VEHICLE_X_M, VEHICLE_Y_M)) + screen.blit(car_scaled, car_rect) + + for cone in CONE_POSITIONS: + pygame.draw.circle( + screen, cone.color.value, + transform(cone.x, cone.y), 10 + ) \ No newline at end of file From ad874e96c42342c37c558fd44914a2bd5c00c751 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Wed, 11 Mar 2026 20:27:33 -0700 Subject: [PATCH 02/45] move state around --- auto_sim/controller.py | 0 auto_sim/main.py | 9 ++++++--- auto_sim/render.py | 26 +++++++++----------------- auto_sim/sim.py | 20 ++++++++++++++++++++ 4 files changed, 35 insertions(+), 20 deletions(-) create mode 100644 auto_sim/controller.py create mode 100644 auto_sim/sim.py diff --git a/auto_sim/controller.py b/auto_sim/controller.py new file mode 100644 index 0000000..e69de29 diff --git a/auto_sim/main.py b/auto_sim/main.py index fb0eec8..3b28cca 100644 --- a/auto_sim/main.py +++ b/auto_sim/main.py @@ -1,6 +1,7 @@ # Example file showing a basic pygame "game loop" import pygame -from render import handle_key, init, render_world, sim_step +from render import handle_key, init, render_world +from sim import VehicleState, sim_step import ctypes import platform @@ -23,6 +24,8 @@ def set_dpi_awareness(): time: float = 0.0 init() +vehicle_state: VehicleState = VehicleState(0, 0, 0, 0, 0, 0) + while running: # handle events # pygame.QUIT event means the user clicked X to close your window @@ -34,8 +37,8 @@ def set_dpi_awareness(): # fill the screen with a color to wipe away anything from last frame screen.fill("black") - sim_step(clock.get_time()) - render_world(screen) + sim_step(vehicle_state, clock.get_time()) + render_world(vehicle_state, screen) # flip() the display to put your work on screen pygame.display.flip() diff --git a/auto_sim/render.py b/auto_sim/render.py index 2ab7d15..1317858 100644 --- a/auto_sim/render.py +++ b/auto_sim/render.py @@ -2,9 +2,9 @@ from dataclasses import dataclass from enum import Enum -import numpy as np import pygame from scipy.spatial.transform import Rotation as R +from sim import VehicleState class ConeColor(Enum): BLUE = "blue" @@ -20,9 +20,7 @@ class Cone: Cone(10, 10, ConeColor.BLUE), Cone(-10, 10, ConeColor.YELLOW), ] # TODO figure out where the cones need to be -(VEHICLE_X_M, VEHICLE_Y_M) = 0.0, 0.0 # position -(VEHICLE_V_X_MPS, VEHICLE_V_Y_MPS) = 0.0, 0.0 # velocity -(VEHICLE_THETA_RAD, VEHICLE_OMEGA_RAD_PER_SEC) = 0.0, 0.0 # orientation and angular velocity + PIXELS_PER_M = 80.0 w: int h: int @@ -36,23 +34,17 @@ def init(): def handle_key(key: int): pass -def sim_step(dt: int): - global VEHICLE_X_M, VEHICLE_Y_M, VEHICLE_V_X_MPS, VEHICLE_V_Y_MPS, VEHICLE_THETA_RAD, VEHICLE_OMEGA_RAD_PER_SEC - VEHICLE_X_M += VEHICLE_V_X_MPS * dt - VEHICLE_Y_M += VEHICLE_V_Y_MPS * dt - VEHICLE_THETA_RAD += (VEHICLE_OMEGA_RAD_PER_SEC * dt) % (2 * np.pi) - -def transform(x: float, y: float) -> tuple[int, int]: - global w, h, VEHICLE_X_M, VEHICLE_Y_M, VEHICLE_THETA_RAD - r = R.from_euler('z', -VEHICLE_THETA_RAD, degrees=False) - rel_x, rel_y = x - VEHICLE_X_M, y - VEHICLE_Y_M +def transform(x: float, y: float, vehicle_state: VehicleState) -> tuple[int, int]: + global w, h + r = R.from_euler('z', -vehicle_state.theta, degrees=False) + rel_x, rel_y = x - vehicle_state.x, y - vehicle_state.y rotated_x, rotated_y,_ = r.apply([rel_x, rel_y, 0]) screen_x = int(w / 2 + rotated_x * PIXELS_PER_M) screen_y = int((h * 0.67) - rotated_y * PIXELS_PER_M) # Simple transformation - replace with actual coordinate transformation logic return (screen_x, screen_y) -def render_world(screen: pygame.Surface): +def render_world(vehicle_state: VehicleState, screen: pygame.Surface): global w, h w, h = screen.get_width(), screen.get_height() @@ -62,11 +54,11 @@ def render_world(screen: pygame.Surface): car_scaled = pygame.transform.scale(car_rotated, ( car_rotated.get_width() * scale_factor, car_rotated.get_height() * scale_factor )) - car_rect = car_scaled.get_rect(center=transform(VEHICLE_X_M, VEHICLE_Y_M)) + car_rect = car_scaled.get_rect(center=transform(vehicle_state.x, vehicle_state.y, vehicle_state)) screen.blit(car_scaled, car_rect) for cone in CONE_POSITIONS: pygame.draw.circle( screen, cone.color.value, - transform(cone.x, cone.y), 10 + transform(cone.x, cone.y, vehicle_state), 10 ) \ No newline at end of file diff --git a/auto_sim/sim.py b/auto_sim/sim.py new file mode 100644 index 0000000..f0ab060 --- /dev/null +++ b/auto_sim/sim.py @@ -0,0 +1,20 @@ +from dataclasses import dataclass +import numpy as np + +@dataclass +class VehicleState: + x: float + y: float + v_x: float + v_y: float + theta: float + omega: float + +# (VEHICLE_X_M, VEHICLE_Y_M) = 0.0, 0.0 # position +# (VEHICLE_V_X_MPS, VEHICLE_V_Y_MPS) = 0.0, 0.0 # velocity +# (VEHICLE_THETA_RAD, VEHICLE_OMEGA_RAD_PER_SEC) = 0.0, 0.0 # orientation and angular velocity + +def sim_step(state: VehicleState, dt: int): + state.x += state.v_x * dt + state.y += state.v_y * dt + state.theta += (state.omega * dt) % (2 * np.pi) \ No newline at end of file From 3e5acae0236c302fb1eaab436b78ae0974e9e14b Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Wed, 11 Mar 2026 20:32:15 -0700 Subject: [PATCH 03/45] sim --- auto_sim/controller.py | 12 ++++++++++++ auto_sim/main.py | 4 +++- auto_sim/sim.py | 10 +++++----- 3 files changed, 20 insertions(+), 6 deletions(-) diff --git a/auto_sim/controller.py b/auto_sim/controller.py index e69de29..c1ed2c3 100644 --- a/auto_sim/controller.py +++ b/auto_sim/controller.py @@ -0,0 +1,12 @@ +from dataclasses import dataclass +from sim import VehicleState + +@dataclass +class ControllerOutput: + a_x: float + a_y: float + omega_dot: float + +def controller(state: VehicleState) -> ControllerOutput: + # TODO implement controller logic + return ControllerOutput(0, 0, 0) \ No newline at end of file diff --git a/auto_sim/main.py b/auto_sim/main.py index 3b28cca..1f7703f 100644 --- a/auto_sim/main.py +++ b/auto_sim/main.py @@ -1,5 +1,6 @@ # Example file showing a basic pygame "game loop" import pygame +from controller import controller from render import handle_key, init, render_world from sim import VehicleState, sim_step import ctypes @@ -37,7 +38,8 @@ def set_dpi_awareness(): # fill the screen with a color to wipe away anything from last frame screen.fill("black") - sim_step(vehicle_state, clock.get_time()) + controls = controller(vehicle_state) + sim_step(vehicle_state, controls, clock.get_time()) render_world(vehicle_state, screen) # flip() the display to put your work on screen diff --git a/auto_sim/sim.py b/auto_sim/sim.py index f0ab060..dc50faa 100644 --- a/auto_sim/sim.py +++ b/auto_sim/sim.py @@ -1,6 +1,8 @@ from dataclasses import dataclass import numpy as np +from controller import ControllerOutput + @dataclass class VehicleState: x: float @@ -10,11 +12,9 @@ class VehicleState: theta: float omega: float -# (VEHICLE_X_M, VEHICLE_Y_M) = 0.0, 0.0 # position -# (VEHICLE_V_X_MPS, VEHICLE_V_Y_MPS) = 0.0, 0.0 # velocity -# (VEHICLE_THETA_RAD, VEHICLE_OMEGA_RAD_PER_SEC) = 0.0, 0.0 # orientation and angular velocity - -def sim_step(state: VehicleState, dt: int): +def sim_step(state: VehicleState, control: ControllerOutput, dt: int): + state.v_x += control.a_x * dt + state.v_y += control.a_y * dt state.x += state.v_x * dt state.y += state.v_y * dt state.theta += (state.omega * dt) % (2 * np.pi) \ No newline at end of file From b64f49ca749349845e3ead601fc56498a98ecd03 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Wed, 11 Mar 2026 20:38:48 -0700 Subject: [PATCH 04/45] more files --- auto_sim/controller.py | 2 +- auto_sim/render.py | 10 ++++++++++ auto_sim/sim.py | 11 +---------- auto_sim/vehicle_state.py | 10 ++++++++++ 4 files changed, 22 insertions(+), 11 deletions(-) create mode 100644 auto_sim/vehicle_state.py diff --git a/auto_sim/controller.py b/auto_sim/controller.py index c1ed2c3..9afb581 100644 --- a/auto_sim/controller.py +++ b/auto_sim/controller.py @@ -1,5 +1,5 @@ from dataclasses import dataclass -from sim import VehicleState +from vehicle_state import VehicleState @dataclass class ControllerOutput: diff --git a/auto_sim/render.py b/auto_sim/render.py index 1317858..2251ff7 100644 --- a/auto_sim/render.py +++ b/auto_sim/render.py @@ -44,10 +44,20 @@ def transform(x: float, y: float, vehicle_state: VehicleState) -> tuple[int, int # Simple transformation - replace with actual coordinate transformation logic return (screen_x, screen_y) +def drawGrid(screen: pygame.Surface, color=(20, 20, 20)): + global h, w + blockSize = 20 #Set the size of the grid block + for x in range(0, w, blockSize): + for y in range(0, h, blockSize): + rect = pygame.Rect(x, y, blockSize, blockSize) + pygame.draw.rect(screen, color, rect, 1) + def render_world(vehicle_state: VehicleState, screen: pygame.Surface): global w, h w, h = screen.get_width(), screen.get_height() + drawGrid(screen) + # draw the car at 1/10th scale car_rotated = pygame.transform.rotate(car, -90) scale_factor = 1.2 / car_rotated.get_width() * PIXELS_PER_M diff --git a/auto_sim/sim.py b/auto_sim/sim.py index dc50faa..0c7f5d4 100644 --- a/auto_sim/sim.py +++ b/auto_sim/sim.py @@ -1,16 +1,7 @@ -from dataclasses import dataclass import numpy as np from controller import ControllerOutput - -@dataclass -class VehicleState: - x: float - y: float - v_x: float - v_y: float - theta: float - omega: float +from vehicle_state import VehicleState def sim_step(state: VehicleState, control: ControllerOutput, dt: int): state.v_x += control.a_x * dt diff --git a/auto_sim/vehicle_state.py b/auto_sim/vehicle_state.py new file mode 100644 index 0000000..fb2586d --- /dev/null +++ b/auto_sim/vehicle_state.py @@ -0,0 +1,10 @@ +from dataclasses import dataclass + +@dataclass +class VehicleState: + x: float + y: float + v_x: float + v_y: float + theta: float + omega: float \ No newline at end of file From 5ad71250929a6e451e7bac175bdf8b5069d74430 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Wed, 11 Mar 2026 20:42:40 -0700 Subject: [PATCH 05/45] more --- auto_sim/cone.py | 12 ++++++++++++ auto_sim/controller.py | 3 ++- auto_sim/main.py | 11 ++++++++--- auto_sim/render.py | 20 +++----------------- 4 files changed, 25 insertions(+), 21 deletions(-) create mode 100644 auto_sim/cone.py diff --git a/auto_sim/cone.py b/auto_sim/cone.py new file mode 100644 index 0000000..87e1eb0 --- /dev/null +++ b/auto_sim/cone.py @@ -0,0 +1,12 @@ +from enum import Enum +from dataclasses import dataclass + +class ConeColor(Enum): + BLUE = "blue" + YELLOW = "yellow" + +@dataclass +class Cone: + x: int + y: int + color: ConeColor \ No newline at end of file diff --git a/auto_sim/controller.py b/auto_sim/controller.py index 9afb581..75bb60e 100644 --- a/auto_sim/controller.py +++ b/auto_sim/controller.py @@ -1,4 +1,5 @@ from dataclasses import dataclass +from cone import Cone from vehicle_state import VehicleState @dataclass @@ -7,6 +8,6 @@ class ControllerOutput: a_y: float omega_dot: float -def controller(state: VehicleState) -> ControllerOutput: +def controller(state: VehicleState, cones: list[Cone]) -> ControllerOutput: # TODO implement controller logic return ControllerOutput(0, 0, 0) \ No newline at end of file diff --git a/auto_sim/main.py b/auto_sim/main.py index 1f7703f..1f2554a 100644 --- a/auto_sim/main.py +++ b/auto_sim/main.py @@ -1,5 +1,5 @@ -# Example file showing a basic pygame "game loop" import pygame +from cone import Cone, ConeColor from controller import controller from render import handle_key, init, render_world from sim import VehicleState, sim_step @@ -16,6 +16,11 @@ def set_dpi_awareness(): ctypes.windll.user32.SetProcessDPIAware() set_dpi_awareness() +CONE_POSITIONS: list[Cone] = [ + Cone(10, 10, ConeColor.BLUE), + Cone(-10, 10, ConeColor.YELLOW), +] # TODO figure out where the cones need to be + # pygame setup pygame.init() screen = pygame.display.set_mode((1900, 1200), pygame.RESIZABLE) @@ -38,9 +43,9 @@ def set_dpi_awareness(): # fill the screen with a color to wipe away anything from last frame screen.fill("black") - controls = controller(vehicle_state) + controls = controller(vehicle_state, CONE_POSITIONS) sim_step(vehicle_state, controls, clock.get_time()) - render_world(vehicle_state, screen) + render_world(vehicle_state, CONE_POSITIONS, screen) # flip() the display to put your work on screen pygame.display.flip() diff --git a/auto_sim/render.py b/auto_sim/render.py index 2251ff7..a56a3e5 100644 --- a/auto_sim/render.py +++ b/auto_sim/render.py @@ -4,23 +4,9 @@ import pygame from scipy.spatial.transform import Rotation as R +from cone import Cone from sim import VehicleState -class ConeColor(Enum): - BLUE = "blue" - YELLOW = "yellow" - -@dataclass -class Cone: - x: int - y: int - color: ConeColor - -CONE_POSITIONS: list[Cone] = [ - Cone(10, 10, ConeColor.BLUE), - Cone(-10, 10, ConeColor.YELLOW), -] # TODO figure out where the cones need to be - PIXELS_PER_M = 80.0 w: int h: int @@ -52,7 +38,7 @@ def drawGrid(screen: pygame.Surface, color=(20, 20, 20)): rect = pygame.Rect(x, y, blockSize, blockSize) pygame.draw.rect(screen, color, rect, 1) -def render_world(vehicle_state: VehicleState, screen: pygame.Surface): +def render_world(vehicle_state: VehicleState, cones: list[Cone], screen: pygame.Surface): global w, h w, h = screen.get_width(), screen.get_height() @@ -67,7 +53,7 @@ def render_world(vehicle_state: VehicleState, screen: pygame.Surface): car_rect = car_scaled.get_rect(center=transform(vehicle_state.x, vehicle_state.y, vehicle_state)) screen.blit(car_scaled, car_rect) - for cone in CONE_POSITIONS: + for cone in cones: pygame.draw.circle( screen, cone.color.value, transform(cone.x, cone.y, vehicle_state), 10 From c256c4e63f256f38e08ede13fa2afc1e00f62323 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 12 Mar 2026 01:00:33 -0700 Subject: [PATCH 06/45] add grid --- auto_sim/main.py | 2 +- auto_sim/render.py | 43 ++++++++++++++++++++++++++++++------------- 2 files changed, 31 insertions(+), 14 deletions(-) diff --git a/auto_sim/main.py b/auto_sim/main.py index 1f2554a..ca85960 100644 --- a/auto_sim/main.py +++ b/auto_sim/main.py @@ -39,7 +39,7 @@ def set_dpi_awareness(): if event.type == pygame.QUIT: running = False elif event.type == pygame.KEYDOWN: - handle_key(event.key) + handle_key(event.key, vehicle_state) # fill the screen with a color to wipe away anything from last frame screen.fill("black") diff --git a/auto_sim/render.py b/auto_sim/render.py index a56a3e5..63559cf 100644 --- a/auto_sim/render.py +++ b/auto_sim/render.py @@ -1,11 +1,9 @@ -# global state -from dataclasses import dataclass -from enum import Enum - import pygame from scipy.spatial.transform import Rotation as R from cone import Cone from sim import VehicleState +from math import ceil, degrees +import numpy as np PIXELS_PER_M = 80.0 w: int @@ -17,8 +15,16 @@ def init(): global car car = pygame.image.load('fsae.png').convert_alpha() -def handle_key(key: int): - pass +def handle_key(key: int, state: VehicleState): + match key: + case pygame.K_d: + state.theta += 0.1 + case pygame.K_a: + state.theta -= 0.1 + case pygame.K_w: + state.x += 0.1 + case pygame.K_s: + state.x -= 0.1 def transform(x: float, y: float, vehicle_state: VehicleState) -> tuple[int, int]: global w, h @@ -30,19 +36,30 @@ def transform(x: float, y: float, vehicle_state: VehicleState) -> tuple[int, int # Simple transformation - replace with actual coordinate transformation logic return (screen_x, screen_y) -def drawGrid(screen: pygame.Surface, color=(20, 20, 20)): +def drawGrid(screen: pygame.Surface, state: VehicleState, color=(20, 20, 20), spacing=50): global h, w - blockSize = 20 #Set the size of the grid block - for x in range(0, w, blockSize): - for y in range(0, h, blockSize): - rect = pygame.Rect(x, y, blockSize, blockSize) - pygame.draw.rect(screen, color, rect, 1) + l: int = ceil(np.hypot(w/2, h/2)) + l = l // spacing * spacing + + grid_surf = pygame.Surface((2*l,2*l), pygame.SRCALPHA) + + offset_x = (state.x * PIXELS_PER_M) % spacing + offset_y = (state.y * PIXELS_PER_M) % spacing + for x in range(0, 2*l, spacing): + pygame.draw.line(grid_surf, color, (x - offset_x, 0), (x - offset_x, 2*l)) + for y in range(0, 2*l, spacing): + pygame.draw.line(grid_surf, color, (0, y - offset_y), (2*l, y - offset_y)) + rotated_surf = pygame.transform.rotate(grid_surf, -degrees(state.theta)) + rect = rotated_surf.get_rect(center=(w/2, h/2)) + screen.blit(rotated_surf, rect.topleft) def render_world(vehicle_state: VehicleState, cones: list[Cone], screen: pygame.Surface): global w, h w, h = screen.get_width(), screen.get_height() - drawGrid(screen) + # grid for context + drawGrid(screen, vehicle_state) + pygame.draw.circle(screen, "white", transform(0, 0, vehicle_state), 5) # draw the car at 1/10th scale car_rotated = pygame.transform.rotate(car, -90) From f288621c841bb56f90a5bcb64b7b07f2a8b83339 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 12 Mar 2026 01:11:02 -0700 Subject: [PATCH 07/45] a bit more --- auto_sim/constants.py | 1 + auto_sim/controller.py | 9 +++++++-- auto_sim/main.py | 19 ++++++++----------- auto_sim/render.py | 4 ++-- 4 files changed, 18 insertions(+), 15 deletions(-) create mode 100644 auto_sim/constants.py diff --git a/auto_sim/constants.py b/auto_sim/constants.py new file mode 100644 index 0000000..06c3380 --- /dev/null +++ b/auto_sim/constants.py @@ -0,0 +1 @@ +VEHICLE_WIDTH_M = 1.2 \ No newline at end of file diff --git a/auto_sim/controller.py b/auto_sim/controller.py index 75bb60e..c9cc941 100644 --- a/auto_sim/controller.py +++ b/auto_sim/controller.py @@ -1,6 +1,7 @@ from dataclasses import dataclass from cone import Cone from vehicle_state import VehicleState +import pygame @dataclass class ControllerOutput: @@ -9,5 +10,9 @@ class ControllerOutput: omega_dot: float def controller(state: VehicleState, cones: list[Cone]) -> ControllerOutput: - # TODO implement controller logic - return ControllerOutput(0, 0, 0) \ No newline at end of file + keys = pygame.key.get_pressed() + return ControllerOutput( + keys[pygame.K_w] * 0.0001 - keys[pygame.K_s] * 0.0001, + keys[pygame.K_d] * 0.0001 - keys[pygame.K_a] * 0.0001, + 0 + ) \ No newline at end of file diff --git a/auto_sim/main.py b/auto_sim/main.py index ca85960..07e76d1 100644 --- a/auto_sim/main.py +++ b/auto_sim/main.py @@ -6,15 +6,14 @@ import ctypes import platform -def set_dpi_awareness(): - if platform.system() == "Windows": - try: - # Set DPI Awareness (Windows 10/8) - ctypes.windll.shcore.SetProcessDpiAwareness(2) - except AttributeError: - # Windows 7/Vista - ctypes.windll.user32.SetProcessDPIAware() -set_dpi_awareness() +# dpi awareness for windows - prevents blurry rendering on high dpi displays +if platform.system() == "Windows": + try: + # Set DPI Awareness (Windows 10/8) + ctypes.windll.shcore.SetProcessDpiAwareness(2) + except AttributeError: + # Windows 7/Vista + ctypes.windll.user32.SetProcessDPIAware() CONE_POSITIONS: list[Cone] = [ Cone(10, 10, ConeColor.BLUE), @@ -38,8 +37,6 @@ def set_dpi_awareness(): for event in pygame.event.get(): if event.type == pygame.QUIT: running = False - elif event.type == pygame.KEYDOWN: - handle_key(event.key, vehicle_state) # fill the screen with a color to wipe away anything from last frame screen.fill("black") diff --git a/auto_sim/render.py b/auto_sim/render.py index 63559cf..baa34c7 100644 --- a/auto_sim/render.py +++ b/auto_sim/render.py @@ -1,6 +1,7 @@ import pygame from scipy.spatial.transform import Rotation as R from cone import Cone +from constants import VEHICLE_WIDTH_M from sim import VehicleState from math import ceil, degrees import numpy as np @@ -61,9 +62,8 @@ def render_world(vehicle_state: VehicleState, cones: list[Cone], screen: pygame. drawGrid(screen, vehicle_state) pygame.draw.circle(screen, "white", transform(0, 0, vehicle_state), 5) - # draw the car at 1/10th scale car_rotated = pygame.transform.rotate(car, -90) - scale_factor = 1.2 / car_rotated.get_width() * PIXELS_PER_M + scale_factor = VEHICLE_WIDTH_M / car_rotated.get_width() * PIXELS_PER_M car_scaled = pygame.transform.scale(car_rotated, ( car_rotated.get_width() * scale_factor, car_rotated.get_height() * scale_factor )) From 247364f63ad62a96b38269c8b0db6216247bb786 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 12 Mar 2026 02:05:42 -0700 Subject: [PATCH 08/45] controller sub --- auto_sim/.gitignore | 3 + auto_sim/Pipfile | 2 + auto_sim/Pipfile.lock | 20 ++++++- auto_sim/cone.py | 12 ---- auto_sim/controller.py | 3 +- auto_sim/main.py | 11 ++-- auto_sim/render.py | 4 +- auto_sim/sim.py | 6 +- auto_sim/test_controller_2/main.cpp | 68 +++++++++++++++++++++++ auto_sim/test_controller_2/pyproject.toml | 18 ++++++ auto_sim/test_controller_2/setup.py | 29 ++++++++++ auto_sim/vehicle_state.py | 10 ---- 12 files changed, 150 insertions(+), 36 deletions(-) create mode 100644 auto_sim/.gitignore delete mode 100644 auto_sim/cone.py create mode 100644 auto_sim/test_controller_2/main.cpp create mode 100644 auto_sim/test_controller_2/pyproject.toml create mode 100644 auto_sim/test_controller_2/setup.py delete mode 100644 auto_sim/vehicle_state.py diff --git a/auto_sim/.gitignore b/auto_sim/.gitignore new file mode 100644 index 0000000..8d19a12 --- /dev/null +++ b/auto_sim/.gitignore @@ -0,0 +1,3 @@ +__pycache__ +build +*.egg-info \ No newline at end of file diff --git a/auto_sim/Pipfile b/auto_sim/Pipfile index 51fcdf3..fdb120a 100644 --- a/auto_sim/Pipfile +++ b/auto_sim/Pipfile @@ -7,6 +7,8 @@ name = "pypi" pygame = "*" numpy = "*" scipy = "*" +setuptools = "*" +pybind11 = "*" [dev-packages] diff --git a/auto_sim/Pipfile.lock b/auto_sim/Pipfile.lock index b14c311..52b1eea 100644 --- a/auto_sim/Pipfile.lock +++ b/auto_sim/Pipfile.lock @@ -1,7 +1,7 @@ { "_meta": { "hash": { - "sha256": "df9bfc6c1f8bccd923516feffa8df94caa648ecd292de15ed90937b733ef8ed8" + "sha256": "63ddba2de7dab27b129c0ff4adb1a6d5b6dc3b9212d0fc8202ab757e40506fcb" }, "pipfile-spec": 6, "requires": { @@ -95,6 +95,15 @@ "markers": "python_version >= '3.11'", "version": "==2.4.3" }, + "pybind11": { + "hashes": [ + "sha256:432f01aeb68e361a3a7fc7575c2c7f497595bf640f747acd909ff238dd766e06", + "sha256:f8a6500548919cc33bcd220d5f984688326f574fa97f1107f2f4fdb4c6fb019f" + ], + "index": "pypi", + "markers": "python_version >= '3.8'", + "version": "==3.0.2" + }, "pygame": { "hashes": [ "sha256:00827aba089355925902d533f9c41e79a799641f03746c50a374dc5c3362e43d", @@ -229,6 +238,15 @@ "index": "pypi", "markers": "python_version >= '3.11'", "version": "==1.17.1" + }, + "setuptools": { + "hashes": [ + "sha256:7d872682c5d01cfde07da7bccc7b65469d3dca203318515ada1de5eda35efbf9", + "sha256:a59e362652f08dcd477c78bb6e7bd9d80a7995bc73ce773050228a348ce2e5bb" + ], + "index": "pypi", + "markers": "python_version >= '3.9'", + "version": "==82.0.1" } }, "develop": {} diff --git a/auto_sim/cone.py b/auto_sim/cone.py deleted file mode 100644 index 87e1eb0..0000000 --- a/auto_sim/cone.py +++ /dev/null @@ -1,12 +0,0 @@ -from enum import Enum -from dataclasses import dataclass - -class ConeColor(Enum): - BLUE = "blue" - YELLOW = "yellow" - -@dataclass -class Cone: - x: int - y: int - color: ConeColor \ No newline at end of file diff --git a/auto_sim/controller.py b/auto_sim/controller.py index c9cc941..c222515 100644 --- a/auto_sim/controller.py +++ b/auto_sim/controller.py @@ -1,6 +1,5 @@ from dataclasses import dataclass -from cone import Cone -from vehicle_state import VehicleState +from Controller import Cone, VehicleState import pygame @dataclass diff --git a/auto_sim/main.py b/auto_sim/main.py index 07e76d1..ff7dad3 100644 --- a/auto_sim/main.py +++ b/auto_sim/main.py @@ -1,8 +1,7 @@ import pygame -from cone import Cone, ConeColor -from controller import controller -from render import handle_key, init, render_world -from sim import VehicleState, sim_step +from Controller import compute, Cone, ConeColor, VehicleState +from render import init, render_world +from sim import sim_step import ctypes import platform @@ -29,7 +28,7 @@ time: float = 0.0 init() -vehicle_state: VehicleState = VehicleState(0, 0, 0, 0, 0, 0) +vehicle_state: VehicleState = VehicleState() while running: # handle events @@ -40,7 +39,7 @@ # fill the screen with a color to wipe away anything from last frame screen.fill("black") - controls = controller(vehicle_state, CONE_POSITIONS) + controls = compute(vehicle_state, CONE_POSITIONS) sim_step(vehicle_state, controls, clock.get_time()) render_world(vehicle_state, CONE_POSITIONS, screen) diff --git a/auto_sim/render.py b/auto_sim/render.py index baa34c7..9770d0b 100644 --- a/auto_sim/render.py +++ b/auto_sim/render.py @@ -1,10 +1,10 @@ import pygame from scipy.spatial.transform import Rotation as R -from cone import Cone from constants import VEHICLE_WIDTH_M from sim import VehicleState from math import ceil, degrees import numpy as np +from Controller import Cone PIXELS_PER_M = 80.0 w: int @@ -72,6 +72,6 @@ def render_world(vehicle_state: VehicleState, cones: list[Cone], screen: pygame. for cone in cones: pygame.draw.circle( - screen, cone.color.value, + screen, cone.c.value, transform(cone.x, cone.y, vehicle_state), 10 ) \ No newline at end of file diff --git a/auto_sim/sim.py b/auto_sim/sim.py index 0c7f5d4..0b93bf4 100644 --- a/auto_sim/sim.py +++ b/auto_sim/sim.py @@ -1,11 +1,11 @@ import numpy as np from controller import ControllerOutput -from vehicle_state import VehicleState +from Controller import VehicleState def sim_step(state: VehicleState, control: ControllerOutput, dt: int): - state.v_x += control.a_x * dt - state.v_y += control.a_y * dt + state.v_x += control.ax * dt + state.v_y += control.ay * dt state.x += state.v_x * dt state.y += state.v_y * dt state.theta += (state.omega * dt) % (2 * np.pi) \ No newline at end of file diff --git a/auto_sim/test_controller_2/main.cpp b/auto_sim/test_controller_2/main.cpp new file mode 100644 index 0000000..e40d54d --- /dev/null +++ b/auto_sim/test_controller_2/main.cpp @@ -0,0 +1,68 @@ +#include +#include +#include + +struct ControlOutput { + double ax; + double ay; + double omega_dot; + ControlOutput(double _ax, double _ay, double _omega_dot) : ax(_ax), ay(_ay), omega_dot(_omega_dot) {} +}; +struct VehicleState { + double x; + double y; + double theta; + double v_x; + double v_y; + double omega; + VehicleState() : x(0), y(0), theta(0), v_x(0), v_y(0), omega(0) {} +}; +enum class ConeColor { + BLUE, + YELLOW +}; +struct Cone { + double x; + double y; + ConeColor c; + Cone(double _x, double _y, ConeColor _c) : x(_x), y(_y), c(_c) {} +}; + +ControlOutput compute(const VehicleState& ve, const std::vector& cones) { + // Implementation for compute function + return {0,0,0}; +} + +namespace py = pybind11; + +PYBIND11_MODULE(Controller, m) { + py::class_(m, "ControlOutput") + .def(py::init()) + .def_readwrite("ax", &ControlOutput::ax) + .def_readwrite("ay", &ControlOutput::ay) + .def_readwrite("omega_dot", &ControlOutput::omega_dot); + + py::class_(m, "VehicleState") + .def(py::init<>()) + .def_readwrite("x", &VehicleState::x) + .def_readwrite("y", &VehicleState::y) + .def_readwrite("theta", &VehicleState::theta) + .def_readwrite("v_x", &VehicleState::v_x) + .def_readwrite("v_y", &VehicleState::v_y) + .def_readwrite("omega", &VehicleState::omega); + + py::class_(m, "Cone") + .def(py::init()) + .def_readwrite("x", &Cone::x) + .def_readwrite("y", &Cone::y) + .def_readwrite("c", &Cone::c); + + py::enum_(m, "ConeColor") + .value("BLUE", ConeColor::BLUE) + .value("YELLOW", ConeColor::YELLOW) + .export_values(); + + m.def("compute", &compute, R"pbdoc( + Compute control output + )pbdoc"); +} \ No newline at end of file diff --git a/auto_sim/test_controller_2/pyproject.toml b/auto_sim/test_controller_2/pyproject.toml new file mode 100644 index 0000000..355770c --- /dev/null +++ b/auto_sim/test_controller_2/pyproject.toml @@ -0,0 +1,18 @@ +[build-system] +requires = [ + "setuptools>=42", + "pybind11>=2.10.0", +] +build-backend = "setuptools.build_meta" + +[tool.ruff] +target-version = "py313" + +[tool.ruff.lint] +extend-select = [ + "B", # flake8-bugbear + "I", # isort + "PGH", # pygrep-hooks + "RUF", # Ruff-specific + "UP", # pyupgrade +] \ No newline at end of file diff --git a/auto_sim/test_controller_2/setup.py b/auto_sim/test_controller_2/setup.py new file mode 100644 index 0000000..6fc00b1 --- /dev/null +++ b/auto_sim/test_controller_2/setup.py @@ -0,0 +1,29 @@ +# Available at setup time due to pyproject.toml +from pybind11.setup_helpers import Pybind11Extension, build_ext +from setuptools import setup + +__version__ = "0.0.1" + +ext_modules = [ + Pybind11Extension( + "Controller", + ["main.cpp"], + ), +] + +setup( + name="Controller", + version=__version__, + author="Edwin Zheng", + author_email="ezheng09@student.ubc.ca", + url="https://github.com/UBCFormulaElectric/sim", + description="A simple controller for testing", + long_description="", + ext_modules=ext_modules, + extras_require={"test": "pytest"}, + # Currently, build_ext only provides an optional "highest supported C++ + # level" feature, but in the future it may provide more features. + cmdclass={"build_ext": build_ext}, + zip_safe=False, + python_requires=">=3.9", +) \ No newline at end of file diff --git a/auto_sim/vehicle_state.py b/auto_sim/vehicle_state.py deleted file mode 100644 index fb2586d..0000000 --- a/auto_sim/vehicle_state.py +++ /dev/null @@ -1,10 +0,0 @@ -from dataclasses import dataclass - -@dataclass -class VehicleState: - x: float - y: float - v_x: float - v_y: float - theta: float - omega: float \ No newline at end of file From 75bd2c03803531860ca934fa1c196a24ab8727f6 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 12 Mar 2026 02:10:01 -0700 Subject: [PATCH 09/45] oops wrong color --- auto_sim/render.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/auto_sim/render.py b/auto_sim/render.py index 9770d0b..0021d37 100644 --- a/auto_sim/render.py +++ b/auto_sim/render.py @@ -4,7 +4,7 @@ from sim import VehicleState from math import ceil, degrees import numpy as np -from Controller import Cone +from Controller import Cone, ConeColor PIXELS_PER_M = 80.0 w: int @@ -54,6 +54,15 @@ def drawGrid(screen: pygame.Surface, state: VehicleState, color=(20, 20, 20), sp rect = rotated_surf.get_rect(center=(w/2, h/2)) screen.blit(rotated_surf, rect.topleft) +def int_to_color(value: ConeColor) -> str: + match value: + case ConeColor.BLUE: + return "blue" + case ConeColor.YELLOW: + return "yellow" + case _: + return "white" + def render_world(vehicle_state: VehicleState, cones: list[Cone], screen: pygame.Surface): global w, h w, h = screen.get_width(), screen.get_height() @@ -72,6 +81,6 @@ def render_world(vehicle_state: VehicleState, cones: list[Cone], screen: pygame. for cone in cones: pygame.draw.circle( - screen, cone.c.value, + screen, int_to_color(cone.c), transform(cone.x, cone.y, vehicle_state), 10 ) \ No newline at end of file From e13c1a88e57cffefd3cf2ef2666318fefd8087d7 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 12 Mar 2026 10:43:20 -0700 Subject: [PATCH 10/45] move sim to C++ --- auto_sim/controller.py | 17 ----------------- auto_sim/main.py | 3 +-- auto_sim/sim.py | 11 ----------- auto_sim/test_controller_2/main.cpp | 16 +++++++++++++++- 4 files changed, 16 insertions(+), 31 deletions(-) delete mode 100644 auto_sim/controller.py delete mode 100644 auto_sim/sim.py diff --git a/auto_sim/controller.py b/auto_sim/controller.py deleted file mode 100644 index c222515..0000000 --- a/auto_sim/controller.py +++ /dev/null @@ -1,17 +0,0 @@ -from dataclasses import dataclass -from Controller import Cone, VehicleState -import pygame - -@dataclass -class ControllerOutput: - a_x: float - a_y: float - omega_dot: float - -def controller(state: VehicleState, cones: list[Cone]) -> ControllerOutput: - keys = pygame.key.get_pressed() - return ControllerOutput( - keys[pygame.K_w] * 0.0001 - keys[pygame.K_s] * 0.0001, - keys[pygame.K_d] * 0.0001 - keys[pygame.K_a] * 0.0001, - 0 - ) \ No newline at end of file diff --git a/auto_sim/main.py b/auto_sim/main.py index ff7dad3..fb6355a 100644 --- a/auto_sim/main.py +++ b/auto_sim/main.py @@ -1,7 +1,6 @@ import pygame -from Controller import compute, Cone, ConeColor, VehicleState +from Controller import compute, sim_step, Cone, ConeColor, VehicleState from render import init, render_world -from sim import sim_step import ctypes import platform diff --git a/auto_sim/sim.py b/auto_sim/sim.py deleted file mode 100644 index 0b93bf4..0000000 --- a/auto_sim/sim.py +++ /dev/null @@ -1,11 +0,0 @@ -import numpy as np - -from controller import ControllerOutput -from Controller import VehicleState - -def sim_step(state: VehicleState, control: ControllerOutput, dt: int): - state.v_x += control.ax * dt - state.v_y += control.ay * dt - state.x += state.v_x * dt - state.y += state.v_y * dt - state.theta += (state.omega * dt) % (2 * np.pi) \ No newline at end of file diff --git a/auto_sim/test_controller_2/main.cpp b/auto_sim/test_controller_2/main.cpp index e40d54d..86853bd 100644 --- a/auto_sim/test_controller_2/main.cpp +++ b/auto_sim/test_controller_2/main.cpp @@ -1,6 +1,8 @@ #include #include #include +#include +#include struct ControlOutput { double ax; @@ -28,6 +30,14 @@ struct Cone { Cone(double _x, double _y, ConeColor _c) : x(_x), y(_y), c(_c) {} }; +void sim_step(VehicleState& state, const ControlOutput& control, const int dt) { + state.v_x += control.ax * dt; + state.v_y += control.ay * dt; + state.x += state.v_x * dt; + state.y += state.v_y * dt; + state.theta += std::fmod(state.omega * dt, 2 * std::numbers::pi); +} + ControlOutput compute(const VehicleState& ve, const std::vector& cones) { // Implementation for compute function return {0,0,0}; @@ -35,7 +45,7 @@ ControlOutput compute(const VehicleState& ve, const std::vector& cones) { namespace py = pybind11; -PYBIND11_MODULE(Controller, m) { +PYBIND11_MODULE(Controller, m, py::mod_gil_not_used()) { py::class_(m, "ControlOutput") .def(py::init()) .def_readwrite("ax", &ControlOutput::ax) @@ -65,4 +75,8 @@ PYBIND11_MODULE(Controller, m) { m.def("compute", &compute, R"pbdoc( Compute control output )pbdoc"); + + m.def("sim_step", &sim_step, R"pbdoc( + Simulate one step of the vehicle dynamics + )pbdoc"); } \ No newline at end of file From 5aaaa4b281e7f7053ee952ebb817343ab82f7995 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 12 Mar 2026 10:48:48 -0700 Subject: [PATCH 11/45] runnable --- auto_sim/render.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/auto_sim/render.py b/auto_sim/render.py index 0021d37..5cea5be 100644 --- a/auto_sim/render.py +++ b/auto_sim/render.py @@ -1,7 +1,7 @@ import pygame from scipy.spatial.transform import Rotation as R from constants import VEHICLE_WIDTH_M -from sim import VehicleState +from Controller import VehicleState from math import ceil, degrees import numpy as np from Controller import Cone, ConeColor From e7820f38e52f760ae84c887817d0274b1133997d Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 12 Mar 2026 10:50:37 -0700 Subject: [PATCH 12/45] specify frame --- auto_sim/test_controller_2/main.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/auto_sim/test_controller_2/main.cpp b/auto_sim/test_controller_2/main.cpp index 86853bd..fa12b46 100644 --- a/auto_sim/test_controller_2/main.cpp +++ b/auto_sim/test_controller_2/main.cpp @@ -5,17 +5,22 @@ #include struct ControlOutput { + // in local frame double ax; double ay; + // no frame double omega_dot; ControlOutput(double _ax, double _ay, double _omega_dot) : ax(_ax), ay(_ay), omega_dot(_omega_dot) {} }; struct VehicleState { + // in global frame double x; double y; double theta; + // in local frame double v_x; double v_y; + // no frame double omega; VehicleState() : x(0), y(0), theta(0), v_x(0), v_y(0), omega(0) {} }; From 97c3b475db6285c4d2213a885e6eee769fe45d71 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 12 Mar 2026 14:07:48 -0700 Subject: [PATCH 13/45] cones --- auto_sim/.gitignore | 3 ++- auto_sim/README.md | 5 +++++ auto_sim/main.py | 14 ++++++++++---- auto_sim/render.py | 4 ++-- 4 files changed, 19 insertions(+), 7 deletions(-) create mode 100644 auto_sim/README.md diff --git a/auto_sim/.gitignore b/auto_sim/.gitignore index 8d19a12..eb50f59 100644 --- a/auto_sim/.gitignore +++ b/auto_sim/.gitignore @@ -1,3 +1,4 @@ __pycache__ build -*.egg-info \ No newline at end of file +*.egg-info +labels \ No newline at end of file diff --git a/auto_sim/README.md b/auto_sim/README.md new file mode 100644 index 0000000..8addb73 --- /dev/null +++ b/auto_sim/README.md @@ -0,0 +1,5 @@ +# TODO +- simulation loop +- basic boundary checking +- basic controller? +- full MPC \ No newline at end of file diff --git a/auto_sim/main.py b/auto_sim/main.py index fb6355a..b8a36ee 100644 --- a/auto_sim/main.py +++ b/auto_sim/main.py @@ -1,3 +1,5 @@ +import math + import pygame from Controller import compute, sim_step, Cone, ConeColor, VehicleState from render import init, render_world @@ -13,10 +15,13 @@ # Windows 7/Vista ctypes.windll.user32.SetProcessDPIAware() -CONE_POSITIONS: list[Cone] = [ - Cone(10, 10, ConeColor.BLUE), - Cone(-10, 10, ConeColor.YELLOW), -] # TODO figure out where the cones need to be +CONE_POSITIONS: list[Cone] = [] +with open("labels/0000001.txt", "r") as f: + lines = f.readlines() + for line in lines: + x, y, _, _, _, _, _, color = line.strip().split() + cone_color = ConeColor.YELLOW if color == "Cone_Yellow" else ConeColor.BLUE + CONE_POSITIONS.append(Cone(float(x), float(y), cone_color)) # pygame setup pygame.init() @@ -28,6 +33,7 @@ init() vehicle_state: VehicleState = VehicleState() +vehicle_state.theta = math.pi/2 while running: # handle events diff --git a/auto_sim/render.py b/auto_sim/render.py index 5cea5be..e69191e 100644 --- a/auto_sim/render.py +++ b/auto_sim/render.py @@ -6,7 +6,7 @@ import numpy as np from Controller import Cone, ConeColor -PIXELS_PER_M = 80.0 +PIXELS_PER_M = 30.0 w: int h: int @@ -82,5 +82,5 @@ def render_world(vehicle_state: VehicleState, cones: list[Cone], screen: pygame. for cone in cones: pygame.draw.circle( screen, int_to_color(cone.c), - transform(cone.x, cone.y, vehicle_state), 10 + transform(cone.x, cone.y, vehicle_state), 0.2 * PIXELS_PER_M ) \ No newline at end of file From a7c7b54af53991d4fa1d28d7c6f5860aafe541ba Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 12 Mar 2026 15:06:42 -0700 Subject: [PATCH 14/45] fix rendering --- auto_sim/main.py | 4 +++- auto_sim/render.py | 19 ++++++++++--------- auto_sim/test_controller_2/main.cpp | 14 +++++++++----- 3 files changed, 22 insertions(+), 15 deletions(-) diff --git a/auto_sim/main.py b/auto_sim/main.py index b8a36ee..5f0f354 100644 --- a/auto_sim/main.py +++ b/auto_sim/main.py @@ -33,7 +33,9 @@ init() vehicle_state: VehicleState = VehicleState() -vehicle_state.theta = math.pi/2 +vehicle_state.theta = 0 +vehicle_state.v_x = 1.0 +# vehicle_state.omega = 0.5 while running: # handle events diff --git a/auto_sim/render.py b/auto_sim/render.py index e69191e..d287f61 100644 --- a/auto_sim/render.py +++ b/auto_sim/render.py @@ -29,7 +29,7 @@ def handle_key(key: int, state: VehicleState): def transform(x: float, y: float, vehicle_state: VehicleState) -> tuple[int, int]: global w, h - r = R.from_euler('z', -vehicle_state.theta, degrees=False) + r = R.from_euler('z', vehicle_state.theta + np.pi/2, degrees=False) rel_x, rel_y = x - vehicle_state.x, y - vehicle_state.y rotated_x, rotated_y,_ = r.apply([rel_x, rel_y, 0]) screen_x = int(w / 2 + rotated_x * PIXELS_PER_M) @@ -39,19 +39,18 @@ def transform(x: float, y: float, vehicle_state: VehicleState) -> tuple[int, int def drawGrid(screen: pygame.Surface, state: VehicleState, color=(20, 20, 20), spacing=50): global h, w - l: int = ceil(np.hypot(w/2, h/2)) - l = l // spacing * spacing + l: int= ceil(np.hypot(w/2, h/2) / spacing) * spacing grid_surf = pygame.Surface((2*l,2*l), pygame.SRCALPHA) offset_x = (state.x * PIXELS_PER_M) % spacing offset_y = (state.y * PIXELS_PER_M) % spacing for x in range(0, 2*l, spacing): - pygame.draw.line(grid_surf, color, (x - offset_x, 0), (x - offset_x, 2*l)) + pygame.draw.line(grid_surf, color, (x + offset_x, 0), (x + offset_x, 2*l)) for y in range(0, 2*l, spacing): - pygame.draw.line(grid_surf, color, (0, y - offset_y), (2*l, y - offset_y)) - rotated_surf = pygame.transform.rotate(grid_surf, -degrees(state.theta)) - rect = rotated_surf.get_rect(center=(w/2, h/2)) + pygame.draw.line(grid_surf, color, (0, y + offset_y), (2*l, y + offset_y)) + rotated_surf = pygame.transform.rotate(grid_surf, degrees(state.theta - np.pi/2)) + rect = rotated_surf.get_rect(center=(w/2, (h * 0.67))) screen.blit(rotated_surf, rect.topleft) def int_to_color(value: ConeColor) -> str: @@ -69,9 +68,11 @@ def render_world(vehicle_state: VehicleState, cones: list[Cone], screen: pygame. # grid for context drawGrid(screen, vehicle_state) - pygame.draw.circle(screen, "white", transform(0, 0, vehicle_state), 5) - car_rotated = pygame.transform.rotate(car, -90) + p = transform(0, 0, vehicle_state) + pygame.draw.circle(screen, "white",p , 5) + + car_rotated = pygame.transform.rotate(car,- 90) scale_factor = VEHICLE_WIDTH_M / car_rotated.get_width() * PIXELS_PER_M car_scaled = pygame.transform.scale(car_rotated, ( car_rotated.get_width() * scale_factor, car_rotated.get_height() * scale_factor diff --git a/auto_sim/test_controller_2/main.cpp b/auto_sim/test_controller_2/main.cpp index fa12b46..a6110a1 100644 --- a/auto_sim/test_controller_2/main.cpp +++ b/auto_sim/test_controller_2/main.cpp @@ -36,11 +36,15 @@ struct Cone { }; void sim_step(VehicleState& state, const ControlOutput& control, const int dt) { - state.v_x += control.ax * dt; - state.v_y += control.ay * dt; - state.x += state.v_x * dt; - state.y += state.v_y * dt; - state.theta += std::fmod(state.omega * dt, 2 * std::numbers::pi); + const double dt_s = dt / 1000.0; + state.v_x += (control.ax + state.omega * state.v_y) * dt_s; + state.v_y += (control.ay - state.omega * state.v_x) * dt_s; + + state.theta += std::fmod(state.omega * dt_s, 2 * std::numbers::pi); + const double v_x_world = state.v_x * std::cos(state.theta) - state.v_y * std::sin(state.theta); + const double v_y_world = state.v_x * std::sin(state.theta) + state.v_y * std::cos(state.theta); + state.x += v_x_world * dt_s; + state.y += v_y_world * dt_s; } ControlOutput compute(const VehicleState& ve, const std::vector& cones) { From e72b9c3d009d8498ffa2bc85327518fd9499e9e9 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 12 Mar 2026 15:16:33 -0700 Subject: [PATCH 15/45] fixed coordinates --- auto_sim/main.py | 4 ++-- auto_sim/render.py | 9 ++++++--- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/auto_sim/main.py b/auto_sim/main.py index 5f0f354..9992e13 100644 --- a/auto_sim/main.py +++ b/auto_sim/main.py @@ -33,9 +33,9 @@ init() vehicle_state: VehicleState = VehicleState() -vehicle_state.theta = 0 +# vehicle_state.theta = math.radians(90) vehicle_state.v_x = 1.0 -# vehicle_state.omega = 0.5 +vehicle_state.omega = 0.5 while running: # handle events diff --git a/auto_sim/render.py b/auto_sim/render.py index d287f61..0f3eb06 100644 --- a/auto_sim/render.py +++ b/auto_sim/render.py @@ -28,8 +28,11 @@ def handle_key(key: int, state: VehicleState): state.x -= 0.1 def transform(x: float, y: float, vehicle_state: VehicleState) -> tuple[int, int]: + """ + Transforms from global coordinates to screen space coordinates + """ global w, h - r = R.from_euler('z', vehicle_state.theta + np.pi/2, degrees=False) + r = R.from_euler('z', -vehicle_state.theta + np.pi/2, degrees=False) rel_x, rel_y = x - vehicle_state.x, y - vehicle_state.y rotated_x, rotated_y,_ = r.apply([rel_x, rel_y, 0]) screen_x = int(w / 2 + rotated_x * PIXELS_PER_M) @@ -49,7 +52,7 @@ def drawGrid(screen: pygame.Surface, state: VehicleState, color=(20, 20, 20), sp pygame.draw.line(grid_surf, color, (x + offset_x, 0), (x + offset_x, 2*l)) for y in range(0, 2*l, spacing): pygame.draw.line(grid_surf, color, (0, y + offset_y), (2*l, y + offset_y)) - rotated_surf = pygame.transform.rotate(grid_surf, degrees(state.theta - np.pi/2)) + rotated_surf = pygame.transform.rotate(grid_surf, degrees(-state.theta - np.pi/2)) rect = rotated_surf.get_rect(center=(w/2, (h * 0.67))) screen.blit(rotated_surf, rect.topleft) @@ -72,7 +75,7 @@ def render_world(vehicle_state: VehicleState, cones: list[Cone], screen: pygame. p = transform(0, 0, vehicle_state) pygame.draw.circle(screen, "white",p , 5) - car_rotated = pygame.transform.rotate(car,- 90) + car_rotated = pygame.transform.rotate(car,-90) scale_factor = VEHICLE_WIDTH_M / car_rotated.get_width() * PIXELS_PER_M car_scaled = pygame.transform.scale(car_rotated, ( car_rotated.get_width() * scale_factor, car_rotated.get_height() * scale_factor From 0fa1683aabf1f52bedcb93dcb3e8972f844401e0 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 12 Mar 2026 15:34:42 -0700 Subject: [PATCH 16/45] fix grid rerenders --- auto_sim/main.py | 2 +- auto_sim/render.py | 29 ++++++++++++++++++++--------- 2 files changed, 21 insertions(+), 10 deletions(-) diff --git a/auto_sim/main.py b/auto_sim/main.py index 9992e13..aaaab19 100644 --- a/auto_sim/main.py +++ b/auto_sim/main.py @@ -35,7 +35,7 @@ vehicle_state: VehicleState = VehicleState() # vehicle_state.theta = math.radians(90) vehicle_state.v_x = 1.0 -vehicle_state.omega = 0.5 +# vehicle_state.omega = 0.5 while running: # handle events diff --git a/auto_sim/render.py b/auto_sim/render.py index 0f3eb06..824c8c6 100644 --- a/auto_sim/render.py +++ b/auto_sim/render.py @@ -40,20 +40,31 @@ def transform(x: float, y: float, vehicle_state: VehicleState) -> tuple[int, int # Simple transformation - replace with actual coordinate transformation logic return (screen_x, screen_y) +def create_surface(l: int, spacing: int, color: str) -> pygame.Surface: + grid_surf = pygame.Surface((2*l,2*l), pygame.SRCALPHA) + for x in range(0, 2*l, spacing): + pygame.draw.line(grid_surf, color, (x, 0), (x, 2*l)) + for y in range(0, 2*l, spacing): + pygame.draw.line(grid_surf, color, (0, y), (2*l, y)) + return grid_surf + +old_spacing, old_color, old_l = 0, (20, 20, 20), 0 +old_grid_surf = None def drawGrid(screen: pygame.Surface, state: VehicleState, color=(20, 20, 20), spacing=50): - global h, w + global h, w, old_spacing, old_color, old_l, old_grid_surf l: int= ceil(np.hypot(w/2, h/2) / spacing) * spacing - - grid_surf = pygame.Surface((2*l,2*l), pygame.SRCALPHA) + if spacing != old_spacing or color != old_color or l != old_l or old_grid_surf is None: + print("RERENDERING GRID") + old_spacing = spacing + old_color = color + old_l = l + old_grid_surf = create_surface(2000, spacing, color) + grid_surf = old_grid_surf offset_x = (state.x * PIXELS_PER_M) % spacing - offset_y = (state.y * PIXELS_PER_M) % spacing - for x in range(0, 2*l, spacing): - pygame.draw.line(grid_surf, color, (x + offset_x, 0), (x + offset_x, 2*l)) - for y in range(0, 2*l, spacing): - pygame.draw.line(grid_surf, color, (0, y + offset_y), (2*l, y + offset_y)) + offset_y = (state.y * PIXELS_PER_M) % spacing rotated_surf = pygame.transform.rotate(grid_surf, degrees(-state.theta - np.pi/2)) - rect = rotated_surf.get_rect(center=(w/2, (h * 0.67))) + rect = rotated_surf.get_rect(center=(w/2 + offset_y, (h * 0.67) + offset_x)) screen.blit(rotated_surf, rect.topleft) def int_to_color(value: ConeColor) -> str: From d93f05b010d5cb43859e1133ccf333776c68cc76 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 12 Mar 2026 15:37:42 -0700 Subject: [PATCH 17/45] hehe data structure --- auto_sim/render.py | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/auto_sim/render.py b/auto_sim/render.py index 824c8c6..7b1b496 100644 --- a/auto_sim/render.py +++ b/auto_sim/render.py @@ -48,16 +48,14 @@ def create_surface(l: int, spacing: int, color: str) -> pygame.Surface: pygame.draw.line(grid_surf, color, (0, y), (2*l, y)) return grid_surf -old_spacing, old_color, old_l = 0, (20, 20, 20), 0 +old_state = None, None, None old_grid_surf = None def drawGrid(screen: pygame.Surface, state: VehicleState, color=(20, 20, 20), spacing=50): - global h, w, old_spacing, old_color, old_l, old_grid_surf + global h, w, old_state, old_grid_surf l: int= ceil(np.hypot(w/2, h/2) / spacing) * spacing - if spacing != old_spacing or color != old_color or l != old_l or old_grid_surf is None: + if (spacing, color, l) != old_state or old_grid_surf is None: print("RERENDERING GRID") - old_spacing = spacing - old_color = color - old_l = l + old_state = (spacing, color, l) old_grid_surf = create_surface(2000, spacing, color) grid_surf = old_grid_surf From 70661c0161326211290ee1efd00c3c54b1c93863 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 12 Mar 2026 15:40:54 -0700 Subject: [PATCH 18/45] fixed render to m --- auto_sim/render.py | 37 +++++++++++++++++++------------------ 1 file changed, 19 insertions(+), 18 deletions(-) diff --git a/auto_sim/render.py b/auto_sim/render.py index 7b1b496..f4715e5 100644 --- a/auto_sim/render.py +++ b/auto_sim/render.py @@ -6,7 +6,7 @@ import numpy as np from Controller import Cone, ConeColor -PIXELS_PER_M = 30.0 +PIXELS_PER_M = 50.0 w: int h: int @@ -40,27 +40,28 @@ def transform(x: float, y: float, vehicle_state: VehicleState) -> tuple[int, int # Simple transformation - replace with actual coordinate transformation logic return (screen_x, screen_y) -def create_surface(l: int, spacing: int, color: str) -> pygame.Surface: +def create_surface(l: int, spacing_px: int, color: str) -> pygame.Surface: grid_surf = pygame.Surface((2*l,2*l), pygame.SRCALPHA) - for x in range(0, 2*l, spacing): + for x in range(0, 2*l, spacing_px): pygame.draw.line(grid_surf, color, (x, 0), (x, 2*l)) - for y in range(0, 2*l, spacing): + for y in range(0, 2*l, spacing_px): pygame.draw.line(grid_surf, color, (0, y), (2*l, y)) return grid_surf old_state = None, None, None old_grid_surf = None -def drawGrid(screen: pygame.Surface, state: VehicleState, color=(20, 20, 20), spacing=50): +def drawGrid(screen: pygame.Surface, state: VehicleState, color=(20, 20, 20), spacing_m=1): global h, w, old_state, old_grid_surf - l: int= ceil(np.hypot(w/2, h/2) / spacing) * spacing - if (spacing, color, l) != old_state or old_grid_surf is None: + spacing_px = int(spacing_m * PIXELS_PER_M) + l: int= ceil(np.hypot(w/2, h/2) / spacing_m) * spacing_m + if (spacing_m, color, l) != old_state or old_grid_surf is None: print("RERENDERING GRID") - old_state = (spacing, color, l) - old_grid_surf = create_surface(2000, spacing, color) + old_state = (spacing_m, color, l) + old_grid_surf = create_surface(2000, spacing_px, color) grid_surf = old_grid_surf - offset_x = (state.x * PIXELS_PER_M) % spacing - offset_y = (state.y * PIXELS_PER_M) % spacing + offset_x = (state.x * PIXELS_PER_M) % spacing_px + offset_y = (state.y * PIXELS_PER_M) % spacing_px rotated_surf = pygame.transform.rotate(grid_surf, degrees(-state.theta - np.pi/2)) rect = rotated_surf.get_rect(center=(w/2 + offset_y, (h * 0.67) + offset_x)) screen.blit(rotated_surf, rect.topleft) @@ -84,16 +85,16 @@ def render_world(vehicle_state: VehicleState, cones: list[Cone], screen: pygame. p = transform(0, 0, vehicle_state) pygame.draw.circle(screen, "white",p , 5) + for cone in cones: + pygame.draw.circle( + screen, int_to_color(cone.c), + transform(cone.x, cone.y, vehicle_state), 0.2 * PIXELS_PER_M + ) + car_rotated = pygame.transform.rotate(car,-90) scale_factor = VEHICLE_WIDTH_M / car_rotated.get_width() * PIXELS_PER_M car_scaled = pygame.transform.scale(car_rotated, ( car_rotated.get_width() * scale_factor, car_rotated.get_height() * scale_factor )) car_rect = car_scaled.get_rect(center=transform(vehicle_state.x, vehicle_state.y, vehicle_state)) - screen.blit(car_scaled, car_rect) - - for cone in cones: - pygame.draw.circle( - screen, int_to_color(cone.c), - transform(cone.x, cone.y, vehicle_state), 0.2 * PIXELS_PER_M - ) \ No newline at end of file + screen.blit(car_scaled, car_rect) \ No newline at end of file From 55688b7c0172bb0811835db1b7f4233bd4a468fa Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 12 Mar 2026 17:29:58 -0700 Subject: [PATCH 19/45] im retarded apparently --- auto_sim/main.py | 6 +++--- auto_sim/render.py | 12 ++++++++---- auto_sim/test_controller_2/main.cpp | 13 +++++++------ 3 files changed, 18 insertions(+), 13 deletions(-) diff --git a/auto_sim/main.py b/auto_sim/main.py index aaaab19..746a526 100644 --- a/auto_sim/main.py +++ b/auto_sim/main.py @@ -33,9 +33,9 @@ init() vehicle_state: VehicleState = VehicleState() -# vehicle_state.theta = math.radians(90) -vehicle_state.v_x = 1.0 -# vehicle_state.omega = 0.5 +vehicle_state.v_x = 2.0 +# vehicle_state.theta = math.radians(23.54) +vehicle_state.omega = -0.2 while running: # handle events diff --git a/auto_sim/render.py b/auto_sim/render.py index f4715e5..befd9b0 100644 --- a/auto_sim/render.py +++ b/auto_sim/render.py @@ -50,7 +50,7 @@ def create_surface(l: int, spacing_px: int, color: str) -> pygame.Surface: old_state = None, None, None old_grid_surf = None -def drawGrid(screen: pygame.Surface, state: VehicleState, color=(20, 20, 20), spacing_m=1): +def drawGrid(screen: pygame.Surface, state: VehicleState, color=(45, 45, 45), spacing_m=1): global h, w, old_state, old_grid_surf spacing_px = int(spacing_m * PIXELS_PER_M) l: int= ceil(np.hypot(w/2, h/2) / spacing_m) * spacing_m @@ -60,10 +60,14 @@ def drawGrid(screen: pygame.Surface, state: VehicleState, color=(20, 20, 20), sp old_grid_surf = create_surface(2000, spacing_px, color) grid_surf = old_grid_surf - offset_x = (state.x * PIXELS_PER_M) % spacing_px - offset_y = (state.y * PIXELS_PER_M) % spacing_px + # Calculate grid offset to align with world coordinates + r = R.from_euler('z', -state.theta + np.pi/2, degrees=False) + x, y = r.apply([state.x % spacing_m, state.y % spacing_m, 0])[:2] rotated_surf = pygame.transform.rotate(grid_surf, degrees(-state.theta - np.pi/2)) - rect = rotated_surf.get_rect(center=(w/2 + offset_y, (h * 0.67) + offset_x)) + rect = rotated_surf.get_rect(center=( + w/2 - x * PIXELS_PER_M, + 0.67 * h + y * PIXELS_PER_M + )) screen.blit(rotated_surf, rect.topleft) def int_to_color(value: ConeColor) -> str: diff --git a/auto_sim/test_controller_2/main.cpp b/auto_sim/test_controller_2/main.cpp index a6110a1..08874f1 100644 --- a/auto_sim/test_controller_2/main.cpp +++ b/auto_sim/test_controller_2/main.cpp @@ -7,10 +7,9 @@ struct ControlOutput { // in local frame double ax; - double ay; // no frame double omega_dot; - ControlOutput(double _ax, double _ay, double _omega_dot) : ax(_ax), ay(_ay), omega_dot(_omega_dot) {} + ControlOutput(const double _ax, const double _omega_dot) : ax(_ax), omega_dot(_omega_dot) {} }; struct VehicleState { // in global frame @@ -38,7 +37,10 @@ struct Cone { void sim_step(VehicleState& state, const ControlOutput& control, const int dt) { const double dt_s = dt / 1000.0; state.v_x += (control.ax + state.omega * state.v_y) * dt_s; - state.v_y += (control.ay - state.omega * state.v_x) * dt_s; + + const double beta = std::atan2(state.v_y, state.v_x); + const double tire_ay = -1 * beta; // simple tire model for lateral forces + state.v_y += (tire_ay - state.omega * state.v_x) * dt_s; state.theta += std::fmod(state.omega * dt_s, 2 * std::numbers::pi); const double v_x_world = state.v_x * std::cos(state.theta) - state.v_y * std::sin(state.theta); @@ -49,16 +51,15 @@ void sim_step(VehicleState& state, const ControlOutput& control, const int dt) { ControlOutput compute(const VehicleState& ve, const std::vector& cones) { // Implementation for compute function - return {0,0,0}; + return {0,0}; } namespace py = pybind11; PYBIND11_MODULE(Controller, m, py::mod_gil_not_used()) { py::class_(m, "ControlOutput") - .def(py::init()) + .def(py::init()) .def_readwrite("ax", &ControlOutput::ax) - .def_readwrite("ay", &ControlOutput::ay) .def_readwrite("omega_dot", &ControlOutput::omega_dot); py::class_(m, "VehicleState") From 475cff939345d5513721840b5bbc100296ff7056 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 12 Mar 2026 17:31:51 -0700 Subject: [PATCH 20/45] oops hardcode --- auto_sim/render.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/auto_sim/render.py b/auto_sim/render.py index befd9b0..b5b677a 100644 --- a/auto_sim/render.py +++ b/auto_sim/render.py @@ -57,7 +57,7 @@ def drawGrid(screen: pygame.Surface, state: VehicleState, color=(45, 45, 45), sp if (spacing_m, color, l) != old_state or old_grid_surf is None: print("RERENDERING GRID") old_state = (spacing_m, color, l) - old_grid_surf = create_surface(2000, spacing_px, color) + old_grid_surf = create_surface(2*l, spacing_px, color) grid_surf = old_grid_surf # Calculate grid offset to align with world coordinates From 32fd94d0faa95c78f19c21d114cd25e444e2a729 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 12 Mar 2026 18:02:19 -0700 Subject: [PATCH 21/45] better lines --- auto_sim/render.py | 27 +++++++++++++++++---------- auto_sim/vignette.png | Bin 0 -> 51741 bytes 2 files changed, 17 insertions(+), 10 deletions(-) create mode 100644 auto_sim/vignette.png diff --git a/auto_sim/render.py b/auto_sim/render.py index b5b677a..c7b49ff 100644 --- a/auto_sim/render.py +++ b/auto_sim/render.py @@ -11,10 +11,17 @@ h: int car: pygame.Surface +vignette: pygame.Surface def init(): - global car + global car, vignette car = pygame.image.load('fsae.png').convert_alpha() + vignette_image = pygame.image.load('vignette.png').convert_alpha() + vignette_image = pygame.transform.scale(vignette_image, (1800,1800)) + + vignette = pygame.surface.Surface((3000, 3000), pygame.SRCALPHA) + vignette.fill((0,0,0,0)) + vignette.blit(vignette_image, vignette_image.get_rect(center=(1500, 1500)), special_flags=pygame.BLEND_RGBA_ADD) def handle_key(key: int, state: VehicleState): match key: @@ -40,35 +47,35 @@ def transform(x: float, y: float, vehicle_state: VehicleState) -> tuple[int, int # Simple transformation - replace with actual coordinate transformation logic return (screen_x, screen_y) -def create_surface(l: int, spacing_px: int, color: str) -> pygame.Surface: +def create_grid_surface(l: int, spacing_px: int, color: str) -> pygame.Surface: grid_surf = pygame.Surface((2*l,2*l), pygame.SRCALPHA) for x in range(0, 2*l, spacing_px): - pygame.draw.line(grid_surf, color, (x, 0), (x, 2*l)) + pygame.draw.line(grid_surf, color, (x, 0), (x, 2*l), 2) for y in range(0, 2*l, spacing_px): - pygame.draw.line(grid_surf, color, (0, y), (2*l, y)) + pygame.draw.line(grid_surf, color, (0, y), (2*l, y), 2) return grid_surf old_state = None, None, None old_grid_surf = None def drawGrid(screen: pygame.Surface, state: VehicleState, color=(45, 45, 45), spacing_m=1): - global h, w, old_state, old_grid_surf + global h, w, old_state, old_grid_surf, vignette spacing_px = int(spacing_m * PIXELS_PER_M) - l: int= ceil(np.hypot(w/2, h/2) / spacing_m) * spacing_m + l: int= ceil(np.hypot(w/2, h/2) * 0.4 / spacing_m) * spacing_m if (spacing_m, color, l) != old_state or old_grid_surf is None: print("RERENDERING GRID") old_state = (spacing_m, color, l) - old_grid_surf = create_surface(2*l, spacing_px, color) + old_grid_surf = create_grid_surface(2*l, spacing_px, color) grid_surf = old_grid_surf # Calculate grid offset to align with world coordinates r = R.from_euler('z', -state.theta + np.pi/2, degrees=False) x, y = r.apply([state.x % spacing_m, state.y % spacing_m, 0])[:2] rotated_surf = pygame.transform.rotate(grid_surf, degrees(-state.theta - np.pi/2)) - rect = rotated_surf.get_rect(center=( + screen.blit(rotated_surf, rotated_surf.get_rect(center=( w/2 - x * PIXELS_PER_M, 0.67 * h + y * PIXELS_PER_M - )) - screen.blit(rotated_surf, rect.topleft) + )).topleft) + screen.blit(vignette, vignette.get_rect(center=(w/2, 0.67*h)), special_flags=pygame.BLEND_RGBA_MULT) def int_to_color(value: ConeColor) -> str: match value: diff --git a/auto_sim/vignette.png b/auto_sim/vignette.png new file mode 100644 index 0000000000000000000000000000000000000000..a0e0ed04e01c12ab933cf9e4851929d1ae21765a GIT binary patch literal 51741 zcmX7PcQ{<%^ZzQ#B3L!L-PL=EUf0^NO7tG0gy;m(I~!JCy?25{bP3Vk`YIua8X-}m zM2m>->+}2lai06!dCr`f`?_=I%$b=Rr>CP%PRc|I0077}P)Gv+fB^Ep0V24k?AM)H z++Xl~4Ac>Liz9;J_ctUjQRY4X02$5y1|A@^KfPkQ&ppcM|u&}U*h={1DsF;|TxVZSEM~@^VBqSvzrKF^!rKM$LWMpM! z<>ch#<>eI=6ciN|m6Vi}m6hRexQdDj0)bFfRYf9^j~_o)Q&Uq{S4W{x8X6j!nwnZ# zTH4y$Po6x{(b3V>)z#C})7RHGFfcGQG&C|YGB!3gF)=YUH8nFcGdDN4u&{Xg^r@w# zrInS{vuDq&t*vcrY;0|9?dG&C$M zEId3sA|fI(GBPSEDmpqEjYhwI{W>NlCN?$}gTchb#l^?RCnO{!CMG5&B_$^(r=+A{ zvDnnq)U>p;^z`(MjEv08%&e@e?Ck8EoSfX;+`PQJ{QUfaf`Y=r!lI(0;^N|xl9JNW z(z3F$^78VEii*n0$~SM`R8>_~S6A26)VzKB_T9U8wY9bH-@mV`tE;cCZ)j*}Y;0_5 zYHDt7ZfR+0ZEgMV;lsy|AKTj6+S}VdefreV(b3u2+11t6-QC^O)AQed|Mm9v_VxAk z_xBGB3=9qq4h;PfScqPELOL@?~mjYI=HlW@hH= z*RQj)vvYHE^Yily3k!>ji%UyO-@biYUS9tG{rk$w%IfOsj~_pN{`|SNwzj^$zOk{f zxw*NuwY9yy{p;7Sot>TC-QB&tz5V_D-@kt!92^`T9v&SX9UmW`oSgjm^XK&R^zYxl zXJ=>s{{1^YKfk!RxV*f)y1Kf)zP`D+xxKx;ySrO!unGkLhCXN@Rg3}(_6rm2cIBF* zSeM~CNI4WBt9-L8bp?JEblEO)_V0;u z+h-+oL1EZY!j1cku17N^l&7lr+Ae(lwEWw?Dz1G;y=P%n;%5Jz#oOg_QRM8SwsP0V zJxgxb<-41;9pBGBm#NmK^Xzuq_i}bl)gnSb7g;+5xK}|ua3yre@F||XTo)aL9no!j z2@ua;Nl2lDu8M{N7T*vvt%ZI&ifpgRP0rYgfd#e{}OPr(GyaLW2ya23DqsUWra_$ zIfveLsRxm8=+<0ao;Ca4O};$A+qnMT1H|5|{OBIK@PCEsLB^)@vDm{LrHF!_VBeXW zH~Y!X2E{YeH(+m%OMv(7fhQg=7F(Zt%Lso~JjIR@@_+KrOai03!c=0St z#D1eTV)^x;7k-$K06RR47gD8lD@<_@7eIC|mm>8zBUw0a!cVs_YC{zzdoP#-_vk!e zgh#4&X7(aF#M0!RcER`mk4R2h0N_59QbeulJl4E-cBX@u>%T&cgKgl?YVRdnx2Mkst; z8&3NtKyon;zMzZfoG|M`r(S)#2D4NC@UG@?JA9MwAN1!Q-9)g@BNGMyd;lMh)a2Hev`v4FTlF>h7#&2c4G-Y7+wq5IvVwwFWgNU6yh0% zxoxudmmhR|3g17jlkZwT#oL%#3XexGJUy7lHVsiyc}un2xgc&8;S=Hwlep4a15ZKE zpqQA%LDrxH%!NGj;is9aC+u)Pz(W14>G828;;f~7l=V&yddC8BR6TQBA3+=Zo{rn8 zqe`0ftiJo9iqcl5#[$c7gGo_O!6oS|H#fzxT#fh>^m6624L2hB3;F5f245QDA1 z#g@q0)E!7$g;(9kDH!zgLx=y|8$~z~*2l_tayo@o=`8<_3mqb{aekTtXo^)A-uQ>b zs-L4+&8}E2W>)G)*ylTZ&dxsOD@6prllO+KNM3wu7G*trpXYA0a}M?nxs;74s9ZDi zH?DQ@WuH$P^>xczbeGIB?+CU04E z7TghfGJFTQw;LmQZr?-O1(IXG!wKmt5_m1?xqK)ORGA>P7!TEz9sJ=|kH2AWnCgPM z*mv2MRV3&xJ9y*HKDOcRX$i9lg@cgibZmbejOl;3|mfpTa zZoFo56)~y37|Oi;&*K_gHgia@k>R8*W45e#a`OhQAXatgfDMIn{{k~s*@bp_H60Fn z1f}e(64|FsQeM2ZLUs#B*ZxUT%Du|(PQy|{CUXu5?WMDH`-ZdslrkG^c1B@4k5w_+ zhh(^wq9>XMG8})0w!e=Y9CVjxD`eIlR1)iseD`nCwe~#Slj{=Vxr0DxQd$EDH;yMr zKd&4t{p|qcy43=H{H9^3Ma(AjyvDBLc^2%devS#RTW8w~gS*}EgEJ$` zm?_(9Yi2J)(bv>)Zli&;&FB{UTdpJYX7`@Qw!9%ejt{0Fy(!6;u~HMke^6~IklQqdbp zd%Y~)EXg!Eog7eQ7Od(Vf7S!`z$+u9-3?pH-_VGhzU;s5zLNfL@AUkBKcqfk#`9tp zP;cpv9kv-#4?BI89sKl$g~Jh~!v6-{#vCRM?V-eu&e2la$>6Bh4qM7mvqK@Y#8rk7 zw8v!?_CZxOx~{+nJ<YKEb!5WO)Pd)Osr&W2 zwW?|F%JRQ@P1qUo1rcWT=c5^dh}u|*iWnP@$rtjK!5;IzLG0RL6c9=FniKmQRK<_K z!56sBji=XBEIDtI`8_*hu{6kQ0ra&ReB$V*hvmkbKlp}XSJ;>Wf(>4P4_XO*Z3Oqb z&ODYfemwuh4CAE4Rue{4xY~;nmichol10`dvXtb5)^Ule`?5yKwGoTI_E{6(ez$zd zWXW6^simOz8tkAh>38%j-(mOj-1GMjTKkt~u*P>b-M3euKz8`qnLRu9)k#sgmk9fr z{uaN(*%1!`<**ATZ^Wi5$Rrgy2=SD=oIOzS( z#6{q_PRpaf$(b@MW$!W5Cq0DNs1!{EeS7+f)5S}svfXsADaCG8_zUVX?=KtKfR|V1 z%yj-wFugTc{slhR#&Ab1I}wIYL{R?3f{uO1ve>s|I!)=WCc-Vj)i*Zi`6uTTtRPf5 z9eYSr5q4`KpO(l=9J-$T>{n1Fb1ogFUgXby49jqTwug-v@JDrwJ)?O-sQt&kkF4hE zfdfN(or;yEZSgaGY&paM_)Hu}%)LMJm~q~ z$9e2oag?WHt5tIB}F2o=KS&=tYW62bvp1>pjvm@@Ng z^ZOKXhLe~WM2u|=mFm8i?}~2CputM%9ORkk5~(l6`>r_TTS2NvO%79a_YQD&hLYGm zHYGm0JVem_TJV!TJ2{)E@?U~i*M--C4DBz2QeE>lIqt0;+&$yTu@Q?0o2ytW&v6~e z_KswJ`2DsSFYbLDEnXk2-$^kaOG32z7c#92KE9e; zzC18`xKv?8mqHiTTll$iq_Fu7n6QPC#^5c%6v6xaANMGcW8SOOnOJ`OZYW4j%xktw zoTmX|!PFh<`T(gAiBtv(Zi5_39{E&EUmG|bdZMt0cmqoBJdDKcYNnIaX8HC^sJ*)m z0cFy=<$H#CH(%Em3b~ak8)Qa^Xa9DS;R>p~9|lZez1h+|p&L&D$GV>~-nC*_vazZm zMt`I_)YD^~JE!)!1joOv8m7kAF?`M^XwvbJEtPcyMJAmcU8f$pg&d!{0!Bh|EDBMB z1*v=r9-+Fb6vtj@@Ue7zvM=iv&o5M8nbEBYA`Ac_^E{C>J^IpM@s59WCM7iVC$VeO z)V@d2w;gO$gC5K9`gvzo$th=}PZn4yu^4_2oxvFK`x$ykc!$F+7~Wf~u=#_B!0yQ3 zPz7XK0Kze?663Ys4aH0fo0eWvAXbRM-vfxi!E4V>DTcvq>GUrQrRX~pos+)VUx>cW zo?PFt53kra4c@aw*r+ngm&sEw|AtIcXlBZd1)xvZX>pOh~Plj`%s1R-?nnHa%u^K2Zf z;Ng)Kg41HNG^#DnU`6 z^yaIgVlsH? zSbj|0NL*3L!6ZI+ZBiylilwXQE7Nb^T9P_xpjM8nI(>Q0yR{Sc5NZv=@kuS!=KZ4m zZt3;nCL%3+R0jhfHGy*ngv3T28QlQBxJ$?z5FO<2&~FkUsP=#^*T!c&^MLgQ z`qp3PKbo&HD91M#0j#vWSGs%`O2C>$r?kFdH8bnTh?847=rgl08R#J?Mu`7?gMDcL z*JR_FCv#G<#d6YLj;|Ss zFB8Gv7woM5b^eE>T&|yIE>`HGgqXlv+QivkqX*)3!2ybK#DsCX%0kFDENX;G3~M10 z&2u|5nq;j}Msz{%+|7tQNI{pi`pcr#G6rGH&_jI8U^&0gY!U)lvBbQ@H^EG{vbgPs zr^UXwKGNYzXMYGZ^HMU#GNr;)jA(05)fpf3n~^xK439M4$0>)z80Go0F#9Q9fmS`j zx=%Cw+O9R{JS-`mo%9l_5kvh`F-ukQ@V9;H!UUv=*y?;NW4zt>Jy(>^GW8X&+3nUgq|HeB!5o!noMJIj0^Tm z1xYbs(`536FXO*nq}I5Y)WOoBY-nk zS`vmqq5t?%#%G}aRv&96=|$+T(BKDuwUgGRSu+n{gswio!I-T`+?5Gvc>jA8VwQx; zs0fw~l&yk{YtE&;ZQY7M`^uI>LJg(NKU&9;YmxI#%T-sCh{+M}0l9;I)R|Mbg#}!i z!0Dx>6TigC6qhs?kCxLV{Lvj!myIxptv`z)mvdwiFE4izwC1xne{m_6b-HgIR5gt{ z=3szt{Cuo>BD@P_;i-WAe2nf?ypLPSmZ6Wt)6RGus-TNLve;4Ifqnw~VH@14^Luqb zsl#Jf%_yW_o_~cXyyV8*X&t3Y0Zy}-BG64T_%KIPH4iXQew#nTI|#p6sr41;Q9 z14y8UWT@WQc`C=6FJSkOmxb?!4P12iP(YEL*g?*Z6BH2S<9G3T34h_lbd4nuZhIws ziE9Pm=Gcbqdt6RGcr8UfWy|TN#8y7Dwz17lc||{O!A6S51dI4#Ef^5X$D~d>09%_R z7k}^U@KqOVAb=9!Z~KpPx7(8!Z%31Zw+SAp6UqqfAqC4f zifBeHFA9JZuPs%?e}$A72fq&_cakZAyriBieX6g)$*YXRx?o;Y;5NQnY%)a9Dt-w$ zt#dzJxrHmzE~)+K&accy`$i#pS>}LRV_= zaa5>&f;avNg7pe#e;@~u9Or@YEf75x0`q&^!Aqy)5eu@J6vbgaUAFoLsor=Z zo#Gt5FVmCgHBj(RM;^&VkzRsoPbg3rq2vK#`3#xB=z1y3*E^MQ4>*ff1T~(R*O6QZ_&EqKLWesYFcD5*1z&F%HLc|X~{XkI8l_=?Sq>4 z8TS}3P6g(LcE-@hL^%z2shd>S0!#Vm* zhkY{r$Wp==B-l{I1Rj1_iZynPy|_ zXddp?+jqkmnWUDgL*56`5%=iR4afdhJS^dYQrxJWx#pPG1!Ga7HIb)ucE783(4ezc@Q~(yK89=0Xw5>GCDO zcYI=KZOH+urZ+=@iqKV394x;qEcyIh*!0|^5RrMN2!fAI1g;(vq~gQT6s{f(x7t4G zt$ouEsOrWMDsnJ#hjOI7cS_hg9^#EKnVEA-&Ndy{B%s3pM ziZ&-N4L|w0@EHGa0?GY}nZ6cU#aza&)l5gm)b!^Ou!5A!y_P078e9((x_DPJk~bFX z!>JG}R~tVIZY017E544-rxBk8a}7K{wm{swppE*7RKr{T(GPn=mMVTDV*X6~sUuN^ zks|M)!iMKpU zc_|z9)7NbHXC7`ID=3vWee3? zl$LlYbR{xmuM$(z_g_76=~OyHQwNAl*M4Yl)+M18|Pq`w@lycH41>%(P z48>k3*<9p=UEkFwRN9?5mRON>2(Iw?6Ta@1Bg06VFTPu_YdLU?0(o|M*>^6n5LFQ~}Lw>~5 zmi{s86i<&g8(1CgTm;Y;y!=AXg#$vJDzf+PY^X5uJ7yRpLg?vCS|i*c5Xuy09I5qdg8|dt`RvE zDQ9SuiO$&3{dz08D(Y*88&rZCX17vPIa1EV+j2n!3@0e>=YD zYhkz`yPIuPs+W@T;4r(wWlRN&I<0+V*9hjz%1Q5+sfot}+I{`w&)+w$P5p8R(pG6$ zu)T=)mqVZkST)e-OFpP83m>b`JuFB>Hy(BzH{Dk8@-bPXEhn6uira>UnXj@`V7M*{ zl1W6f@U%So>^wy(42IrzX+r3%36lR^T;^L5z&uPa>t6dn0u`DhhiZ-zp&*fKjRxzu zX}6h?ou_zE^4!aJW?!^_{Ee>7;7me~LV7~@4Htzr7&?;Z@8sa0$UrFx?AYOWrGG;Z zts%5-5;gO4WajEa&5gUX*bIs0BD51_s(Y8O%?}0(mK+AHOJnVK7r;&YT1*JWJMk~? ztl7tGbLS~*VkXmW8W$4$5AUS|?FHN%D9ktin}0CCf{i5!qy^vo!v~Fl@V&2g77xJO{xiF5Zvl)xeCrfF42O*&{ zlajDGegrQ9hfZ*y8O}EoZJ5-v49M>56|#^77h?0bKvRkxb!qeGgZ{htRQpW@UopM- zv%Iu~jbPx2V^mBJMJHhyQ!S4Z5~&jO{Wm`j!|({GuTU2hi^RDH)5!T*&A~gp(|&7_ z1Ol+C2cvC5d!IXM(&WYlbHebkjl$lrcGXQ}M>`9fD*QkXTPbtCo>Y&qFZWeb(Lu(dVXpBmlOy#_{L&{V3X6_Yf>`=$&O^(dcCE)Cley(Z67+zFr|S;UAn`Wb{<75V8M8NKa0mwr3h*x0Zefgc|gq)JlP`(DUpACcwt*|u*rjyVJ zJ2D{wyf~U>3bS!KPAwF3M-d%5H9$`Vlv)chb77MAqvIT( zGv;6U(kL~-Nl69koBuVR0nSX@dKsV&r?+4S>K~O>S=Zqh*{dN1;(p{=D||zW_e}Ro zon#;5==VH;wkcc=8`|_vI6UCR9}WkRFwM+?Rg!IW5T4*%2uc(5NpSe_+D{>X{xOD2 zF`E8X^ZXZ`>|ax{Hvf^inqLcD_JWtzqy4$XbH(z0Zo`%&FdE>_-lca_RKoh~u;Aq=eLrKc*>BI%cJZ5?Wq!bIiS^g;Z)DYi!D=*+cSwYx&Ue#Q~px*1$F1p&6?U9+X6%hBm>QT}@&9uROFutj95eG75j6o1)PlWr94&j4T zKs}UN%7uu}QJW#}0!f&-{T0d}1XL#2Z~C+gs}$cD-hY2+QwFh@qlO7g#KrUQVRCQn zHYMzb9mJrat4T4njAWelq_{40CoOop-7y3VCB88x<|Hiyu9fkc0W}CV@R`;FMTH><4<%wK-LPG?qx3jjICoJOwKY@u&7?hn&^ZV*9LF zf2`;9r2Pr8ri9UxLKCb#57ufmRbCc9;ATr{j~cg*vQVMO6Ewh952=z)tG4l?p2f{RbdwEeryrG}NZM_u?> zQ8N?eOE>e1qVe_%0Q;+M$JZdpW#Na3<;oeG%NPM*BwaZ~?4eZ%8-!S!9;2SmkK&&* z&MF<9mEuxEb1^-qO?g4$6&z=hnk@Cdu(&bF%(FmO1VFfDuJOgO1}&%`_KNKSr!x-_MT$So7VVmE=jAJew*$X##tLzK=lUZhMU^ zYiL|c^ew;mPCJmRw4f7zi@^TpA6vR@STO?Gc@`zVhU$MCb#K=*fTfH&C7*Z%6w-0l z3?CUVGik9z0_~ma@hTq1<5fJ?_Z7S}jReux>C9FNGs20zF1(W=!!73hZ|zShlyt1s$c*43?)qPC_JF!Wfm+ zRK(e2VdN7$0X76ID)z&6tVlGSea15?Kxfo|bFp&+BOz4{=qC$;9Vai)?`PA?AHu8xU-!r^LQIw8CBgZM~-?u zVx-dNp2T#@t#xawL)#%&K}ScZ&zs?}+eE~tjq*1|qH3T^!qW)3P>%aO+%)~?F0p~9 zvPgCa?S9n&{ z&||*uT*Xbk#VI%b$E{`W0HD=>5sPj8={-C^0xDz^r;>zvQer+s)l`qvm7IfmlhhcD z)C8qYP~K0m!!IW} zqX%S7EtT?XC9}L<9lfaeqAL03hui_-Ky9sX!9wYJPCzZle6yiXa)qgvK`=9Up#LMF zFFB8VQ2fPuOP)@14ZeWVeoZluqmG^Gg`hf%bI{~0%%=)c#^OMj`iSsjNhRT%xYh|1 zOY*FO01a6X)kUq}AHUO94D-jNd0$pxH$e`m0NMDk>$OCzF$FoWk_(YWnaS1DEv-n3 z32)_L?omx6*U0Wt4X*0NYH}0n z6n;0$H;S}2dSSJhe*lo^rg;of3(w68n%g;-45tfO*Yy_5CW?*3}ay10F$(?;KXhEI~C}7)u$! z?60iQpqJz_&QtWp`Of)lg+)Nn_+yPu+3jgqzT`GR{ow3%>acPIupAaN;yK_QzpV;) z!YhqfkK}OJ99Q@?@|gRI+LhGS#7OB^`D9lq>w2<^M|$f#5< z;WB@EEv|v3;WHS#h3NrARZ1qlWSC!wzW(|7S%3@--I?vb(+W4>?9^V#mC1F*RD0Ap5L2gyq5B}xglF8T1} zwoO7_#BA;3S`hM;?(o|w^`@-fV|W^}oH-1b|KgT%YnO-jAYTOSLT2ge#-_s>PbKJ} zf)PEG;HN6Kjc>0b>(p0ifT|JFaG7xJP7_2d3yY1Hz5>{V8g8tn5adJx9ED;!C686JtAF%`8^tQ zIJmnVE{!>$MatqK&OZrOZoD!uHK?{-VF1_Rs}j#=wFwBKeuoT-dGB`3vWU-p-cMUK z1R9p-ijyfc51&NXBMzpqnc1g*DZl^X(F0b9^4BoU=Scr)j2ByuB~6POo_#Nc;S6y$ zE#VCsx*^eSw?#g+_jPxC>gyRq0{OuY24{hf1un)5m92ub>9PNzPdS&fmHMRA(<_Nw zNpT;#^;4=M@v$voKe)a;Y_CS+p)zy;u&N&qP|i)K?^_y;$bB1P?BPSKhq}8I*lem3 zLO3@N67fUf901g~2r-u_R`pfioIH<^w+6^D`aF!5+^T@iF*C(IV9kBxy${qY*0Z%M zXb-4OjDs}IB+#+Dd0QE4Y@yHPAKuZoaHHqwu!`BfB2W<^a?CMORwn4LICrtCa+bzw z8UpeX^>BU_NWx24#tE@|7((|AsijV|!FJ=s!!_`Es;B}aPA^AKKv`=Q#_<(QB)t`h zwj@_>2~wy@NDN_x>H?AR&{=JfYSoK^M<@!)#!(|)Mu_ymi9f#hZzK%KSM!TkhoPzM zikf9MMaqG*ajN8&X+3hH%Cw>A-(wX)=*@gc3#TtJi`MQIYVU=ZyYu|bs|Q%QFr&G* z7jLQV9e|iOX!f@o?K*y9hF4*$ng~$mPR|ifGLDsV5nzOrGE!li!LCO*eg1E8cHz8L z z29hmFbmWTq3KwArYK)h4P|a`1F)@Fc367|JuFCU#{3MV;q5Jgtpt|j65=QQ`!69nIQtDN zWtG8{*z<~R*1x2rc}rXFcqBVcPsrj)@W><6Wvo2*{tQ0`qSsK?={aGK%uCs_k3(>% z{>q;~I`AC!$m|ihGtY%nlv=Ydc>7DHRGmPBT_S1B=@9!HQAbVJKpb9Bn_ttb4y7qb#%z*~LKhl1EOwa; zcc68IfV+ty*na4y^o!H44}_Z5auG1dvrST3)1>)ze(e9WuNj&dOHnB&uy)3XuQkfdT^_mY$ql9)nUFU+DW(KypQLEZ#)3J12Ss4{ zqZb^560z)rxc4Ej;N3f*+VyPD&fPja_SXUBpxMr^;wLymz)u>kg+vEI z4JFUYFt^8VJuk}J0jXEbYTF`?4}bQDcb26!o`R_Dqhln#P%GV59eZV9y@(zo%+#wV zG}9%yZzg32ZDQ=b=F$X&Gc~M~Nf&N5DpuS#Nr^r4W0`cA{gf1mFs+N_7JXG5OkTo7 z5>g~ky$zolSq*IJXb85bO}ODJdC5A2Rv84Q9hk%0k--eU`pqHaZ$k}_2aD&}^h|vR zF}%MW>!^M9Mkw3!VlE{L>)JgP-bVidObc!`dw$p}9}`tq=X_H0_^F?qB0IK@X+z22 z7Jgs62?y|tO0$|RO$uCGH}N|BYMkzg`f%~w>!RFW1^unB+!6lld81uwA81x9S5-6H zS77|NHN2w{o5PF<5fRzM7CxBnwSlALV+MDv-PLd$OifqRh4Ji9z7rKZ(^4uEF4YSq z!V~+Cuq@=d>-!DWWNPmz<*Q3w<_@*tFLEM6r>b2hv&hfyb>D=>&Wb~$X(jf}tXyBC z9+C@wH&EfWOKoF(ST+6#g&I~tn8;qfjk6!A-#g`nydfNFr`7%7ZnOYHU-M0Xc2dbK zi8MaJBP`?J|7aFX<{W8akF8`AP+=b~z#DNB{2J74``gtogSx@c5YEkHN7R;2!ln(1 zdrg-Gh4M8%CsGlXLK&hQ69&s zYc0`#h?#J;P@(2Vn|F(T?D^NOFRXuQ$9<}eQ5x97KJ(2`kzkdHxaLcLK|CJ`gbEn{)_zR90}GoKv!x?Ffo ztO*b{juP#Qkl~<%S4{Z^qRA8#44W##o-I!ixe1a7my0}>?tW)DB)0lv)g$Qu z!D0qAd_J+70U*gKCKoH!!x>#*yP@~03>&N0)F zSE{~|=h5i+JJ)Y&a@bmS7+WmTer}ov&r5jB6;a?FC@7ABkJ z5KF^@Hq%)irFH)$U?_bRW(5R?n0>NK&G?JkT3gvz+YN4)VT=&6DBn#^Zh;n68`gQs( zJa3GRnSnbRzC0IZWx1Jb)jvEGv$OSj1%F4?Mk;yJXH7+yVt6OqA$k7s&W zzhshe{_V+^u02rumhsWkY6;tq2D*omyzOhZjajDz`fxSFef%a>91UP9Q(q-@f;rQw zcEhzaid^yEyO$9WqA(opF+0bbf=LlhU)_!MPBe=tmWO8g7n+slrJph~hL94-4VbDE ze3QVd4~w&DFoSZ0WGvJAz_-u}G}!$w9K~ASr_6&-+q}>v$>B*)1t)3S0%sSTTye+qpc0N=|*OP;#(J5=v zok+v3EJ!~Nr?WR~L|`YeiHWJrrG}&L3{kh1MFi-?WC5;?yt`jaK4lf@7GY@wG-;5j zxg1~$2{b+1>BNPNkkjBmTV-P)`A^0q3HaN4-^IFrbj6uI#NmRT|3Ykt$eC{vo|sfS zZuKKmRg)&u!xb-#{T)9yesbS;{H9^xc~eoC_6dll9xN_B<G_vlBsM- zC1l_v;er=Lm2FHz&HM-oh3NaA^*ky4)9w|?uCO%!I;t13jMON!GD-a^ko_3n4Bw6< zkPqU*hznqbyM$-z?#fA%*zYz2r~uE$7tJokvBhos03*={-k5WNd5gzaY@u;F1Z8Ih zf|}0t~uRK9~gjNmy-8Z8(?;D{AodWBLHObtANq?`|d6S+TOKu zfKTS%XhUyb^0^;TtY7u8oFZV8K3iLg5mthhr(}$M08R=wXshK5%Z41<29|ucz7~-? zZPowkqrZ`+(CH+|H4;I;sgcehLFzth2upg{<|9=p=!>`t<%J)(JIUvBxhF>8q-)|EgLmJ+5~9yCa7lg6ll&%WZZglhOMe3?oHIC0fm8 z?l2U;Y5I-zGJjK^Eyjl@i^7Cga@SV@2Vzd6Qq+6{@xmqFE;7^#vYWsa*lF+dl|8N^ z87rFqqt_A#Jrz8xFA|2TpUbQ_xe=vS)RixuTKkfcP&r_^9u*{OSh2T1_OCa_Dkijk z;^wpXN^=5(i-vP1R30jC`-Cv^pusbXu78GKv#&I&koL)^$SSfr%TEmdQOr_?9)P19 z7jI`yUv;X2NN|~9ol%ya$1rEiB+TU}5ua<&pRXl>L?|4F`UD?tBb>+ymlFOk<2kXt z?C-F0*ZT?ksd4!wXlJOw^iuat8(G4IO@nMz2SMf=RKdbrzEoH4?nCd)q$>(72SO5O zFU{oQ9pxw4ljYpjnS=y$^{!Pz`uHgY$|NXDlm2#M`|(U{JprdF_+d_~SwfGJKCnDh z4GB6HPKb`h!~(=mE;b3-`Jw;*cp+4aR8T*aC#cgnRs`Mt$gGO0Zkm4pzA7e0y>en6B>~kd-opTi1jR#*+uAao9F84>tiY4Gt50IpH;hh1XPzp)r zS?hNaxN4L4So+h4kOx>RAwCczSCgEkHj%6vPLDE~DzjO9if{{H$_IUy{Ca>wS&9C> z<`=|W0XwQPt!>s&+D`Ahm;_d)P=w=9a~E_tS`-n zY<z{M4XkPGozR=t)dRF|^Q@tVm>r;GA$x@vZd;z&lW^7WpF3&6xUT0Jc?}G$g1v|Te&^g`%Hz&-o zS#_bI(lbIF0lJz)837VRC!hBo5Mo?j>q0fO8@^>bE|~vFyUfRH`b-WfQ0Ahu+9T;i3DH1gvNlUAOIsgatd8TT-x$kJE(?rqr^O&AdRV_6mReeEj7%}#t&1Geyr%t5e zSN0ujY9*b)LhEC7kKOn6+BET6gJTSV(7lHHFJdllcoXEvu$L-=O+UGFTKww8SMXu| z0#-$6aqDHC)!(JHfKTjq#t&~V=zyz5?tV)#;$9nX7I0>?3xebM8=6?Dj}?k)&A1eI z2jxh{Wv@Hvo|zE+i^1g`H&h_BqkUHcb_e*Ih3iNDH?#~kT&$pqmOD^C8xY?CjapW9 z`fQQ$3Zl zPe@tO=Jja}LcmDB`QvX7I-v?@1}iJZz1?kpnBv!m1mk7yKWHJ`$I_PSH*c>jCY;#z zVHWf{Adu?6nIAB!ekRk^@tsfQg6)NbE{^C1*<07-4sMwKv`@)z+F_LBOGV7(>{m&c zljLNjEUUU@(|FWl|wllV06Q?hskikFn+vB&6HC><(D?IkZI;QB zyLjJc#TAMW)@C>H6pnH*h3T2%)G&D~?W4b+F6mda#p`5rsH<<{Zr}rw3#}*B399ki z8S1B#a%ijb!*(b8H!RnCA?ldI{+w(+#&6<8e~Xe-Goqka*Q2i)c0gNC3HS^;3&;2n zM|Ods*+w$uwxG`f(xw2oQBloXRUSQ{c?H1TMSy3Ol#mC<-?muOJ&hXc9SZ4#j{ief%>E#j7q)u z4zL%X^>?LV#CmgTl74h9tz+Y4qzpZzH2M={Q4)f=%ES=VL)37T*Ih95u$tPk>d5a{N$aVQSQ@#^s90`9)={y;3k$(M~V$^By`|V#8)`#2T9Q%-0L#O?V zbPM)m*!%2d%f|7ln6wMu<_(SR_U?GzNs9=`sR_w*FtZe<%j6dq95;^@y*F&T_*oX* zN&E2v(|>*}_}k>@;V28#gy2a#bGghnWk(oCR&5APsc4;7fEX3(gXQd&fGOg4s zxBRe?QXn8sEfFPMh!!F31R2arh`6Y`ltjtA(EbxM{>1xT`!Q&J4meH1ikUcbjA#hW z2(L;+z)duG2wJ~1x%g2=n5ex=m(`i5 zdH;?r)vNe^9iPmE#K>az71^c^UAmvz*RnLA$22-cqWyB9cVqFuF+vzp%20tOPpI_m zg{aj)Av$ZMHOxAz@`=%U^vcCA4hnG||Gb}kIA3H%I{YE9j{NVhr1-ZRO!oKx#J0vL z{stGwDRtJI|2+6LtmFO^W=Lj@=O?OZjF0D^18v3D@{*fH8ShWGBt`D#oDl+=zK4B8 zpyRt_K{I?IV_78riqOrRbQ!99iR{oYS#B&w$l{x;fUvaCS}fr&isjExv-uW#7PWI( zP*-E+SB+yAI)pi8=-T*<-1kQi3F{Q<486rUFzZEdw=m>;&^W2;{Q6R=$S&tO{Qexv z)VtLD7~QY;>+yneQ{V@~OLaVX`sCz0NQ?qD_)}qai2IuH*)ejG#7_bB zJ16+U!>{BVC^5^OYV&=M@l1Zb(heCf@=-DuLCWn-?+v&Vd{bgp<%qQVk*P0FzpZ{= zzSkYJr>*aUuPlxF@@)y_nis{BxII&8rNggm=9?XN=UK0t!rP~oiy*Do_!qlid>syv zWOTzHb<<>zqbeL-|H{G;HTNHxc@(^wa}dt8rHNWh?2?Mmb+9Pv+ZNz9SA!0|H}bs< z%dq*iR;ACK^e-9!vtBc#bL(vOk=Bdx4&w3AuL9bY^bdFes(GKR>>I+Eq4!`XZ}e`M z9_)itKbRUM4+`T8X^}Ty{8=F?x+y+XhB`>KDj@DYEA3DlxUpDz_}9J(#lM`$4fPX$ zujFs%rnZT6sbH1!!&r(%50{sBYfO3W!M0I4CY9%YUQ(c(CcFvA*JRDD8Ur^%cPS%F zB^)>@v^6#9?$*R}ncs3d38ryn%6XPxhOGnzGadzrP*uy3ma4O%fzKcN6m|o;EvK6J zb%og=R{5^C#WWWHc4G&N`kev}>;cjwhdB+?!R({N%Lr~x!mn>w71zf`^)*kmNE~5nY+IlP z!EP2uU?|K(Y>a3cHt+kl`YR42Uz93ChgJ1HxvWgZCFEy!AL%v3awYSd*!3E|XkSuIop$bIw?RyrDk=Psf*OH9@8{aYK%ltl_IS@BYCWqbTa zAbOvA5sMRWna4%TM)v><(Ua>^f63i!GLd8*WYHuA9>h`3I;=+>3(iVkHi)bvM>%o) z#B&S86_Xpu1bQgyoQcngt5Y7wQ%%8wONZ1=5;4wztsskE+Oy<-na;lcVX=FR_hb3T z?Y((lLtEeYequaKo4Ia6LAP2D-yRTWR4UddMULF!gwT|QE zyp{JMTIucuWFc&@T{13(mgt#uTr&4__=jsSbg0zVFnuINQ$LCejvsoasJKtGF)}tg zBd>vkk~qUR&ItD(d!f1YLg!|J+_~TPEMTNP*fUVCiQ7=92Gm&5M~!pGvshl{S{COp z!8nDQ9?VpI?lsOL5Ery(rELeo_{dqkqEyrU;Lb4w=8#ETgqLxgy;T38xD8m>Bll)H9ZagMM^E-YM)K1T2(qKF2)*n6sz7ug>uUIfRkN)4Qr;(v?#fxXg% z2Q^w*>+p8VM$7tl)-S0YW|l*6**I7 zVRd8#M;z;GH)fv~tG@O+f7?K4c#Vl)|1Z-BXt_tz6N&e*^cJC?XQxAijNg=blqMDx z$m9RIrJgpWQ4&{2m2H4IT=})`l&@;0x_sa*1+axk2a@d5nca@ zAEewhdfo(g65l9~FLsveox|8zub+S zV&lwfq0LSUh?DofqH@$VSUuBXP2+k%olqxBtHW-P&VKAD^6q#)OII#)$K( z6VJj0F8xz2)fMWZ7Vu+OZLe@<-OqH634z~)VqK7w6Fy%mamj%vUR_s>Qe{QEmmK4Z zK(FT`=-DZpo%b1-WW~-9EsK!-mUnwkX4&4tYe@u?RP;j5o~^7X%8xx%oRC8k9UsF? zjCvY3kuAt(gEMg0gFqR{sKp3UGr?TtFY+RXH2m;aF8WOw8Z@BaC7LF`-5{y33PE3s z5qa83cUfsJ%*SB4yDne z@!^C?$hEH6i-g~5G%-2JJW9J{S+0p#g$VZjiR;Y0{1+Xqpk5^3*R4`*TlXfAP1No^ z_w$ea&yE<4;U?dxq(<9dutMOn*Q;G9UQ5tu@Ai>JojZ^O3?Oli{^idl4Yv;zu{o5$ zn$p{G_~*x8?P`{GuD9{(A4hG@)mcM7K{L0Awcom@p?&BjJmCc;HdF|3OcJy*H~4&})O z_sqlF4?5%LA#4+|nRii;jGM;HOh?X(-xRH+p7oe_8k*`1>xzBLr_Nanar=rMEKY7g z>TmO-$hE_9aL*F7m9cRuphl$~!CtbL6GG*rFi%Jn{v#`gbKC{Nvr<6~O@>IsYx+7* zR_5>b_8h-kQo?j8M|4`65PossP2lT~gkR^VL5j$8f#S`#?&T@|oPkEXKK*c>5G8vS zP+QDyswV4X5*T`U_Bu*>sesa8fR}78{6f0$k)^`~wUU`diCRzdvLo0#-QUn-w(TOM zw?JR#?V7ePe`WvFW4tUU%%E!?7GJ)b;H1?|DE8KA|3OrLd*|;Qu~AQYXX; zKAeLQ8c+K=$g#`nQoNw8-ZK-kdEejv=OywRe_8kT>78%N$>56-T6nuWclAS@#XI2h z-kPqT*G^h|=dpu`rt$~q^ej<>()U{Pj{t_}Byg3aEp6_*Lzrs~9Ea)v0sAlJAAuMY zpn&gBR>5r5P-8J(rMfZE*jG)gMY8ZlNAua7EutA|El!^iY}RN1bPBl=eqd(t9`pM& z1afy$sB)QWweN#|K4T_-`13h;c@?XGhx%99R{t?1oO0A@)I>~6Te#b-n&KBc6T4n~ z$TlMJe2I>Pr#`s5OMs$=NFp1p!ZmGsE=uFf7WuhU($VitLf?I(`9?!1%w zy<7l@bhbB)-fY886ykwWfbvvv9Y1luiY{;Go~_|M)d`Kb0gBRf%CYKWv#wSLu1HTm zDyne;c1xZ)MiS<6NY}0I4UN}P84*_vp2NWYfa@n0eSy3wb?`L{eV+A3+}cG>i1Qf2 zt?bT|8umy9>zp#K0b+-;?~cYFe0)r}{mr9n`D&c{lip58P6c%vL+Ifh2$7rm<9BDo zUqO@ovCP$q{B|i^VSaCh9`o4(^41s{d&{~?HInm9^I6@`Yoo@>YVCz2Th0e5EAIIi zzd#O%><=aB+o<2qUDY=CIO8rR_hS@KH%EeMDe^00qA*^JJ)eBEOVu8fh>5$Wc7pkN z21@S&6u=U7mFGOL`)$bn_dsZEA4-}{)=7tG+0dUH{qfRKuQ)lxCAnlST9c8e47|kN zv0D@c>?{g@d@0xSOP9?_5+%b#4~iRJ-W-BO>`kW_8v=XYw}-HnKPL94@3`wQoK<+S zc5sx~fIZ&^1|7=2Pp#F}B0Rto+7=k~S))ef}SE>Cxnw&v(AU4DT$N;8xB`hZsuGqXd;&wD>ms;MHx*$pH*TE9;zz<5WJnLBf!xrUV!@(F~hr^_!=Q+r0!}?nn zYmZt&eo$&&TOWWx}wPm@WN8r)+_gHf+?T!Egz@j8rzc5tl4qp#>y<3eyw%nVP z^~BJdX|xX})!N~l?2mHrV?HClC4GPQGC>14SkW3^v84rJ4f&sjvJ^I($wk4nl~vK^ zVPA%QmTiIc;%`)I0+9|3nks)1`+MyVTO}vs8TR5{Lwr&dj zleTR^u=5(Y|6?%VxUid6Zn1ST8e!8(h*fPtKlvQ4+P`6t*YzcEva#>SM~9;8@gBKW zipA_j^ROBt7VTcM>E>W78t1XcFpk7gJjFaIhB~1u!CCe7#{Xsn^q!9BeCDi3oK!`i z9)JfvwWUWM^8To3xFt%!WAo+NdsN`K)MY*FTI`6=A1HEF?~|xmo*sP9vCNeSg7J&H z5ZuoK4ekx9yh85FgFj{)$Bu0&|9kOmm8R@?!I-fB!Om@Jh~@jhhusZnSHH=h!(zCs z_0}CjRdbaHGV$A~QY3|@jgdI#0>ybA;r?;udzUm1UD6!|0*NHGwyczUowvN1N3&Ox zTq9RJQQvnF|>ugcWu23y?C{vP09{YztBSH~XT1Or{2 z!M+*lYe=jRNeVP==r*QrK9inTnnI6H?t%xPA3}6cHz9_PBFOl?o_mYK9*tZ}@6o_P zysn&oeSM6xTaAjq_9-o*fQgy_=ae6%IM*BB+RRqzt4F24^xk*A`&8quIlNM(#oliT zs!PpzRDM#LkRYz~+(AeOI=(+(D)0qU+o6d$)SfCV{LPs6L0FPVOiSP#?@-RF%lZ1$ zNcm{C?-%DYP)WzLoA(h2eKqqS)_ZM~#WP6>jID$V`9JWaC|xQ5n)KwHWd9`1{0aH3 zU=HK!zZ5Y0+c}mDTJcHv8&48;4J4FM9zfbc0*7YKCz+d*6I+i~Qz zN#>167}h%nuP2~cQVc1M7K&V-4i+k!u8 zTnH)j6_i$SIJr+w_u!i+kQR&^)UZD9IiI~TbQ?jj5O1YYJJf_Whakfr(buZYsGJQe zd9-x@!?$zt9ITMeK9qO07Gx=E4F!Dl79h#rs5V4uMJpFBxB__s3`^D#X9Y!sz@#-R!Zs7b zpO24^neDo->-5qCeyyd$HPbc^cDuTxftmwXSiDbj!n{sSUzUIx4uCQz?$;h5w0=&{ z;|7n6bHm5RB@~_!U{u)$-K$hv&~8O!>8GsNVdX>Gl{pYKV>o7v?w{+Md9${7tQtEU z`HO;rK_Wk{jyEN@w<)cl9otW*5^vql;b95)qO_rEiiS{J@$qNm=X<0H zFOa$xTFKBpa2twy9NQcY;j=LzU3i9>@%xoOUoww8l3*p60Nt0*!*DC7j2;9cB?XB! z;hhH7(gab}2qTYQeHOsj1G%&5i9+Cl(hcGR72Ow~^*ny51oz~1x;1}VBWOpjei$p= zwMR`|s$UsU&YXwfh_Nzr2=Hc_&o5uc%+ocORbBbPRS3YCZ}Tyux>NRpVfr2)AGP1A z-c#tLBy^Q%J2oMk&?Nt`7|Pbj^MtVX3(zi-AjwJh2GJDYJb>4O*@v=d=x=?YddMXx zu8;<%P%lKC4`q>wXS@6^xGkTCMr=l&7I&c9I3}tAhP{i{Y9#l$%83(x_6Dox4KRe~ z^V`Ox&Nc~|0FR%1_sIR(iMfN?o{2x)VRnjj{(9 zCOQwRh#Wtn?SNb+5@K79pfVcfre2-BCIWQ%V2>nVxI~u%=tD!g$-VGR{=pOIRTU5f zG1e$A;X{Cf=IC?sh57(7)Wfesa6?SDB%xjW+%1yBQk#CjoWCe3*p{2fc^1?x=3H(r@VhuI@i_7j1=rjwkLGI2BDeRj#_jmoQbG z5+>DD)h_EXW_9kot>>zbCn>ZiWTgFq^+#<6LxwSfP{OHu3&wyRqnk`004>A=pgfcI zN1SFs)O9Gl-*f1{pmTyGE}F)GweP@PeytqAy@R&&)@&pY(&*8ISkxH8ZE^f`n+U3* z%FX&&#d&(Cn%Ui@H8p#Q>Rh?RmZyW6u7+vo*OZO~yg09K&O1$Qp9#>&w_TZwz36oR z_!s8lcj!&upa=xomLiVo*#iVTA)2)QiA*_O$}&E16_ZRI`}em^l9Rgp7s_z?2Zc>P z66Q6!A=NHpD9T4GBYs>b?>tG2J-!Y@2R?Czw$fh7gE6EbU8e#$1zQ^e*(~hO-+zw( z_+cl0-jaHw{66&;EfT@ybX@%p|6KxeH5TmZ=g9+i(3d?)bXe;pwg?@Id*Cz(`uylR zJjDW+&vZk$$u5ZLpevs0)61k8Rl=7xzK3NCeHy*sbs2-}d(8@rDDS$?Pu30!jR;1s zI7Drr7Nw3L<)~_=;8F&R+T;6ny0GV5H5P@F+xDz3=YwJN^Qwe^a3rXXZ7l8h;)+gh zZAzvmV3ZsVQnOYbIq=`U2eELk+o@3cp$~wt)SfC8h}VzW{q4%{{#twp2Rs#@~b>XR=V{kT5_& zC9NSqC1r860{+#+Wx8g*G$x|_U%45*+{nKd#u-K^b6&6?PK)b`Ee zR01kDPGx2;+LCiO?H)~A<{>gTPCBd-yT!`@7vWN^ahp%5+^dAg15b2?(vb`d{*#mJ zPJ+Z8z$OyY9=YmG5Twn%-{|jC;e*5m6*YK^OHrA&jbx-DNajNUj^s*G#L1=MuD9ekUe!gV_05wrg7UANwBD8xje ze>OK#kV}-XgfgO@&+&(23obCCks;D1)8)}ezu!&Onk%G)px#qrhaO#Fk!(jWpjr|> zq&nK+(DnC`n61Ar`4i?dmQ=3JOypiKIdl(C0pwtfv$J7_IGY64g|om-n3U!$0F22t z3!pbYq(wFzi~z9Ta}sOwUd-0&Ap|4;JTn2lLK7f;^>D^`#%Bn-b>?MNSD9(=?f@gk zuMThPTy|OywB`568Cd_Hn9N&M(2Pg%l0ZnU8bgV&o{#`>$cG{rxV&{q!sbT5E@8J# z_B4n<<)o6t*VG7hqb`q4ly`nm$8_QWw-_ek>Mi_>$yM*-w4;cmlhr$>#z(cCcI1H@ zcIYh4mAW_o_%>|Sg9{eXwkWHX{ubZHpAE&$ip+bbs@Y~xx_tW#TxtcOHRZnnFNC3Y zE}1iQfw-A;cMrS93Gw|V6p{X6&d}t+AVE&(q7F5rd=fI;O8Q)N`kV^rnHs6= zRFoK^q|Q?%Y!*}n>_S%_gfdc+R7cPwk6<`{11dJCLx5z7I|)+MH)E*F6H2pVA2<2R3g@1H(0f?$+1CVrz2B>`b|icI3i<`o%-#WE!nVTViz8pI zABQ9lU)E2#5Hy&(%v5nO6i;1VjJ{HIE1D2JVnL0AEPi|raT7z=_*#n7mgfa%KWAis zEir|$6k&5Iv6CXCGZS#sp7#(1z1b-$X8y;ue<_W@v zI!@^`34moGXVR^LM2wT`Dl4ePKY#c>#(VyQ_MfSD(4@W=`m0CyJ0c-TT(e?kUZtC1`$qfQtQeO%SF5qO+`i0LOvwb^bnfd&-ZhfuZ?_#WNJiZd|wK&6tyQ1|yUiUAM<`YRKFPi+Ev+W%MY ztEH4v7n=Bn_J{eUdaAsqsGlr&WdD!uX|(M`;;c3GTS5~CB>hZ>w8+D7C}2_fiS2Kd z!QBay_Avd{H~-s8006&<`E!q2bQ8}^Rqn@~vx{%(!V*{$dgC!jYa>~e z@cXVG5_Nnb>0in(Xx^rm5XW>{m7tf*|B(FuoofyQ4#e^|38)hOx6OdR9RSXu@Rfdz zerG0ug#94oW)5QqLl_$kFBU)~3{ZWRt@sZ2=hyF0H+kxOYH+QZEVv#Kr3`06W5(R_ zC4iS7MIbR;H)+K~K2>@n>eTk7+kB_B4_{o!d~WyR3KZuuDoQdqys8VWejK-Q!xp+8cQpA32ha0S+8F_u&SU}Aa^+-Ezggr!IouzKXK0Lj1hH55)IcQW<C&G1SCNfPQ^jiAWJm9yFZeDg!%uU$0BI|k4Kf0 z>O2U&@#x}sn4kQfO-R|t7(&<+Gbqm{>&s|Dxaa)pwl5VQUfR`?DeHd^{f1~`efQ#? zK_uGpjv$4^k$&9qXgSbAJP$ISn7B|#Eq--tD2t)*V0{-svP_cz_J4ytNMhbXOnS)R9-_``I`*d_$TCL-{?Vo{a6v*cubJdbnD+vVso$3gBMl* z;GEuDO-QuYqn9!BFs_*8-x)u`1wU9G+JPEp@ptC;|8q@l!wr=khRpbDIo6k@2u*7# zlOt%bF){IVh`A_7EN^ui67g?YfEr@t1A{%e1S4&3eTBmcFtGMVor(My*_?S0)iN5o zp+Fp@KVS(L(YadrSYE z5$3R$!@b3&Cb6JL;k(e$6G5oEHliko%u{}e=J{P@c) zZrw$3MyG<6uA4Fk5a?$fRY*RCKY9xZ=^N!eaI;QJ_;rnu2pPU}SCsVlLrATM1PLce z$M6X`4}H?>L`b8fs?mnYSd`29dz0bYIe6f2;`V9-2Bw_;Iw( zA&785tR4yavG;9pFc8!}LZQfCiY=@j&X=t~4CRE-7A-ZwA~u)#1eZK$u7(0KyeHb` z*?e zjIF(dX?32RbrS>(-GXy&{DmZ~uNe{!AAG#_w&>;KC>T4!`qeI=B}G=YX%-1qxSSN` z$Rl=asiJs8;K(BbsNte#rV&{vKYR#_t*!Mj2TY|}2ZONuXTPAEqa%#`Azml+E3_Y7 z+9+&^gwZzGHS*RCxw~ujCDoahA~B+*(TA)|Ik4`K2q=~#${9Ubqyu;l980MiW4;m> zQE{>FpLCNxZp_EzUo22J_t7e&U+2MK-^p0)BfvCIeC5hO5|Qlmy>UZZ(hUPkSF=~RPi&(TxG)L@m2lN zVYt-(zs^RHo@P{P7C}7@!lJgXSDJ{B+^b)H;0jQET859$YuhYFivgh~qNL~&(-iLK zT%RIE!v?bJhy;7CbqY(!E7DF2ZpB)vx_!d2w;Q=wT)8O+$hj z9fQC*FUf%u5w;z)-(hzB>Wtg>5){D#I}2(L#9&a^+VmW!de&k0)(Dtgu!$f%D7y8{ zd09MnpNS35=5S$acxn8%%Z-BVnE0@w(E|h23*SHStF0=C!y&SRxSPtZ;JS~QQ+EB$ zl4xziI^9X2ZzeUiU6K7kZ>$O07q|CNxiq$mvSmjMF*V4b!)!{yq2WV@+#B5;Rg$euacSZ!!PvU zs76)pZWosYA|I7HazzOEemaJrrg0y>q=P#hvF`vIXCnyMaQdy+1)@?RuJ}_;aE?BX zYYd!IIl~`(^eP!Dj$2{l-49GfcC^&%ovCG~F>Cc@fjGir2;(Ilj+y~q5*wy8cN7$u z+ml37h#Lij;HLhJ9_FY3U-@VPmp>rFiS;S&jP4FWU^f(KV;@BGAiTQ^FQF|wZ9t}X z&%u|}Zz?4v+bRUjwaw>}OU6L1H6DmPbY6fY2TB*+D&}JpoJK0=p)@;KfOG_jPFA4U z6NOz_TPJT15oKh>KR;&;sS?6I##2w(gOug8wK#IsrSGv(-#z1$#1uYr6ZDP9t;l`J zzFujo52%ETn>Q~*hTm2|DDOKHNY91yt_MB<46;>dAFk^+-YzQO*CyYoS#f@8%fYI< zztxd(QZghTP5+gnVm$Y0aKss^7Pl9`xLoPt#@>QT&3m^h>K!S}PWf!X+^i=TVUL_* zp|d+6{e==hV1qQe_zK~GiO>~Ue!OPNsR07OBkV~8Nk7F5I_?5Kkop^qme1;F`ImA^ zIK1f9ZBcd{3mx}cSSc;oY%V#DS}OC6_%!LnTFb|9LRo>k&1GW4|1gzea@b0{=)xE?=icS?8Y_eYZ)6Y2tR)ulE3eh#J2b+`#-2zz86_x+l1;#t%pJ<_5^jS_}C zB+flfeR^6I+myBdqc`m3lSW-zb2*hXSpRGZx$D}W$qEIafT&5m zAm~c8>(dnSe`%wlTYmBLQFuh^jL7~bA+#c8AGKiKsq^Z4Iw12tV9u9WDHojd{UNbI zfPyh(=PAx~x)=e$-j)!KY`k1f$|r1mYV`Q}<5!nI(LcCfV@xJJorhAAgvBKGBXP#} z#A1C5k+}3_4z^n2Dn_uKUG~2(p#|-qYlI{#8a^S<@^78P(7xPA5zY_a7b55ra33|W zKRvN|kG2K^m6e3E5f<`7Ty-NdlSOULMtS$Dn5pQ};KQ{uIjUy7s^$ihPZEl`ea9S! zpS!xon*TCJv9Yy#4{*C|EXmeV>LE|OF%Zlo0&LFAY?@9TrO!jSa>=)k<($aPt|EwQ z)1Q+wwGc80z$T|du1c5(4oCyduGYUHxIB*VMIoO1UGSE@)_FJZhVRGYzc+U2CU^XF zsjbmrnjAVu7qD?6Ds%Q~iK70VTuW-DupDiK1$6e) zx=x%?V7!BLWpqEadz91(>C1qa1hIT1U_2p=_%DtiBQ1byGKOBfL_9c{q&s3u)Qc$3@S6`qP-dGOYU>-`395<4 z$TPp@9O%^V2kX3j&-uzHUG;`v-|Ll<`*qWFot;JNLC`v_9>CNlSbP4Qv3p82W-yLZ z0R<$qi*Ekyr)Xh_(cgTXEk+LP;gL-e?Sb)B> z@It^THkrgGKgm4upIq>-G#F7uQk;3PJ?})R1}*`>MnMflcmTE&?~#}N^T;u~)4K`A zjJ7a>YH~KiQV7&jRRR4du{!qa%g;o%(6|Q+-^a$d`AY8gg-($MRaucmM%?=l+C@&@ zt#WeW2EVJ|zAe9-M!$EJH1spH2YoSJ0qLMjhGsSv7qzFGMVHzuX#Hw=tIiWghh8Hy zq5iX_E4V(1FnG5nMwK!Tabx5Lt!!1Cf%Ye2`CUsTaP}!X z&E2)p1FeY5nyvLA4Plld2=@x?G6oEk^<$c|Onh0!MPMA?dKC2pOWXfJ@>^%in>_Ke zn5_xL=?W)4c2b$G#^^k)^dVjLi)8dCsl5J}zhr4>y*FPh$%_w-FS+g3uOh(M^`49w zmviAPe;ogT-I_nI#^{&G;MxAA%1q!r61|ljPBw6p4)w5an0ttEB-Ct%!E2(Z$T^Zp zQYbd_bj7eSCp&r7jGQ@8PN8CVjL+EKQ+h=D4|i~oFGjn}Nc;pXGvSJ78Xed=0~ubb zpGKcyw+FnNO6gdF0G7zymyoWC6HoVq3i2z;8)cCz4Sth)4eT?lWp6(b&gbw~pSYK( zk$CSL>zi#K1+;H2B?V%aJ$!^`c2?4LQ0F2K4UVp8I>g602<9dTeN;5xLKEM^*n1j8 ziEmD)n!XwlRI3rw&n7OOeZXB&{CCNdV7E#d@@F+pv=9;PFRNx?$?*P8NLP62g(Fh$ zzL#HR0_O5v?`z~8N@$Ts)OT=zJ+Yzw6HS)isytf-h5hU--K`umuh+CK?Rfn}0ag5? zjS~d!qm9z>N2~8MB^){2E#Oc>C3Rtiyf7Ya58-u23xcXd&LJw-xum#P$(;dHlXEg3 z{Cm3Lat;dRM1qp2n6=GrNNEzW){Awa%DF*Mx%Y3S!cDY*jb6Jju|4pTki*ccsJ-lN zH)3Hz`!(a-1IfHbN!HmLL6F-9ri5eS3O@~23B9AUbz3@1mVUZXr-ZPg>7t5PX9Lp9 z<6aZ6i%C|8C<{tZ1G&tNSX}|$9ZwHus`@SepJ(Kr$B>!k|9v#aV%6skqWMQ+Qc-b1 zMY-`>@APefpg?&fVxH|hnm0@jLE?)@CB6m2u`AX9d{-#3D+cjYh?n|aB1NC;r{y1;RTokt{34c|O9 zHc()3So{hzPa;k2K<{?nr`9&!7;Bx(8U3C#9!Ub6OpEy|seBjj%dTNQj9D$~virTx zMwX2dulyzd?w6Wnv1#}0`fLLC)wb<{vz4pwsyt;G0)ZM~-apKyMGBR6IF)}xFgBSx zYW2mG6d@~Rh0Lsq-Kf2DC|7&}vztx*!DIQK%W`AI?PnStM<3=zOJcL)M6g+$<=rw> zeDl_=;diU{XHQzxT-E~OuLWzA1XWN8=obK1n(+C!N#BK?o{KlCZnS;et-2vM`y3r% z32ev(%YLVxws@|qc%43W%3nhnj&6T^qN+6jtKov69e5A9c?MKLm;wcgXp|PtBFFfi zsM_azexv%D)y3zA!8){k7t#v(y#9K$Gn+=Q8~jvguHCcS&4&XIV}_su|DsmBAz@ffvk2D-N z-?*Ul&@*InZ&4kb^hgBJJ|$tUppi7fd8HyyAq)v0Axc>#}gwe4QmBwO0cEUgyMp?seTtja<(9>dI#@rP-Ypr2ipb z&jQ^Jd$FX-?uMyP63qrrnKO_&Z-K9@hSf!DRJ|%#VD)B+uJRuSr)m5!C+q=C@;QZ1 zgfmV*4mA|w#V$Yi&L0!#Gb%92r7a4~%#7&{sfpYgqof)?OL3|1uQFlmtzX^*!o^vv?C;(ty8{%0Q6dR-RXI`D5E7iR+W*g-iS0l)1dIH%DOXN}|69v_v{ zSKVdk`MmxJFEmmu3+O;t%L zc1I$aRwHyDZ5YA9fX9_I2cI;j896hfvj?OX6f&n|C;qEhCHF?KYQJ~b&1cv(I> zIeR0&RsJ}LQ$hNb+OCcw{?~Jt7DA` zyIIB0j%C>5`67?gEO$b(uoh;$L_%-9NE@4H8${sv?um#L%2u`F9E0X!CXhleJBXy{ zduSViS>@Y*rByl~{a8KrP|m{D#|T<4(?H2WWyASNr?+C^ucK)5jrj-Y@Z$>TMi~({ zCkuV}XYB45CE$50ZqjNJR=IfH@edZ~MNss_yVo}0HtNX-Rq*&_2%_t+`~Ae>jvVnz zN)B)LE{PJDy~!}$iyXrmx?1McArGm{-M2dLyb0XDuQ1bDvn(}Y(0gSkl-V^KeQ zT6Rdxz8e=&Bf+0b{P{YwZ$Bi^SEocmsE@$TC$v83XyaS^%%>fkRAtTXfx{YZ-u{6H zeD-Ppf3oqWUAHBEvzmhPtbe$`EhKZf>;TK*03$2~iytWgJE48+d{trH{p!h!kHRsy z8PK)+n2^EwyJZ^D^aCSEV&VH({O76h8axo(XVVMAZNyyKzpK`}K+5?NrCcKW81WaG zLlmAsDQt`~w5ry??f3liVY~%&^(M)%;+ywXCcnomoB0$j<)U>W_&M-FN$BOT$n6)Z zpJxzs2cP;3z6g^38J}r*c(m?%mO}_4j>$Uq^RwmpS>e#E2 zj5hXiVjbolsVF$gdWGO`tbN&c&9GyyR~s1`*A}qW+ZM#FN`; z(>(ac3=IU|v|&8b`Dbm;S@C@;5>iY5Q9s4u@&zt_Ozcn>=LnwUT)8xl9G1=It9o)D zNJ(vcsDVpdb!pR1I}kK>>)D_`p*4)O*jodM8r=7MKYPAG=rlP`-s z6e72q{_7tNd>O&{eWdpWUM|)tVO$}{o0!+}gZbf-z@<}_LYy)56(ag>&WB_ z%hFklaI_{I#iAOU_?w^A_Pv`yPA#_CXo?M9i6yXI4Zf5A299)|8@O?&_^wUds{kU7hsaY4Bd>3int*$QMS{RZQ&QS$HVa0$|dq02}SF4lQ}1G-k3D+ z?7)9tF0Y#%l0_f%AjwsgOnd}OccXUPf{JPGZ9K7&+rL-oC?a(eO}vXlY38~fHDNfd zpN()-b%2)fAtV_wSU-$_y1@3c$NobXN^NOj!UP5?h?L4>K`S&J*+P zT~sIVT^oc|%acPYE+_y#_seoK9QQ{aIV86&XVFv9Fr(+^=W0 zT=(K-Cn1MP6=|V988sJVNsg^q3DAjwN zWq<$dhAd(ajr3Jy`FsjG*Ebu$_A&kZVp;c9C0w^2FXSk8n$pwmqSAv6TNyiY=D`N^ zX4*1TWa^J*M|wqw18nbhDWA|JZoOr4fB2NxIbqYE&%YRsh?3&V7Un$QZpB6dV5yR= zzK5seqPc>2m8hTjlJeXI(>Enes=L11=UYefJzy^j#nh*nqv&EhAL5g^k%j-G=q&u2 ze7i8djR70EjgV%eq@)C-V@wz&(%l0o5kVTs0i#EUilm}6BB>xHt%8)mk8YF(0TJK5 zf5G$F`8?0PbDwiv-|Oj?_4|OAv9B+EVagrX51O*~F<)&ze70-t>fQu8@%J*uP2Bgj z)KX%$RW^-J(pL=FgGJWidtrKuxw<8C@RG8|2MDSBl{624!1wdF@N(%gYe@BPV{|A~~TD7-)5A_E8`@$BgQw1o7NR!TXkdd9! zc&q4V^`e-l`XCzEY74WCwa-G=mVJ5LeS7gQXPW3Q+K^O5vEd-a;?#ITz|0{Lib~rc zhJ?r<7pzz#WQ41}5i7p#2DKwBM9(bgp#)rk^zF-H1nB2x-oiby4Eqyub55k8B`k!$ zOfkuHgJ^Rt8?(gultLZO+P)0A80RoD`1{BXG{6^bp@LSGu5g~-b8xvCbAEY;wHK68 z#bg^Fo7>i$H%GjeEbWe%OWlsO!6i%?kY&6Qj<<1g<(%rGY53$X+I-XPyz%0=zJPx% z)OO08EaNix35z3V?C)kIyO~g0jE%X_&-};{6^taul>evPB?uO+=0spR`a$?US z`C6;0gp%`^LEw4TPA-8BEY*F9+FU!e zR4Q(k3q;s3uncfI9n2H z0v^SI#y?Mo^@)fw-}K*XDEnRgF2mOB7T+wvUUNW&`&zn{Ijq;lgm+x*Kr+vQlHK$5 zN)eusFu?seFoNwcFcR`}!{IMAAU)U-{mlx9^jYXAJ0w0=YXYg}sh^i}zoqeI)&A%| z+=s8rxne|O&nd?gGgx60txp`p!T4Rr8?aHN zrKTSCr4GUFe2*O0BJu_K^v36~G?*V=AM4&|Av;%<&s}&1$af`ygCAlk#Yo0{)jBIF zLdVx8#`F#cN@~(F%*Q**G3Mf%4Ba+^TBA(a`#lg|WR(Ni8iT1rV=9g&pjV0nSni}IYUscsW8ye_e+fJ{bGWi)#k2Nr zR`JH^XHz7_YRA9Z)0>JZKU%fdkFfp0FX&Ud{I^}FA4ZvW9!`1(cJHS#=a{dF>6dl!KqL!;@W@^?-V8BmI6 zH65PM1id?BhA-P7EUr6IL=B)8J_|C)ipp2XVy4Ci4}6?z#bd!PT$S}lv|c~iT7t+p zV{Gv}{~m-fZ*s2+*||OFjt64?oH8OgfQ($Jq-c!2M;dc;+bTCC@fK%`u>&Omq&A&=OK1??Z2E9{?q@pAY z4E-g^9X*V=NNzojHP78&hFRveW>=?t=p2BdNq}v?J#8iz77Nk0l~LJ2a{0k@-tf~K zj$#4OUHuns%S_-`L4PNDoB`TBjw=m%IoITlIuX9#=jro5HiH4~fd)MMYA6qFAi7Cz z_)h_;2!z$1egkUhQq(QBZ7#<|<-H7zI0dVTB~{n< zvY;hGz^RX3+uuStKKO*WeHuRNg6<|)s4Vxf(9`_yX#*}#k7e6XeGaRngDaI&8Q3t< zOnv!Cdn~gZ=?MZ6E82IwPF#ySr7kGTZD&q4GCT$(zfdN<7e+A?tGS*~%MU=&j11p__=K}XGKe<7P}o7AxI~3qk?B1K zYEmBiom%GOaYRhgn;q07U zsF2Gaf3zs_wJ-6-RF{*wx5`A?5PBLhz<5SCA`jID6fX*+=0)$*=a%I*1Ih$|T`o+= z9X1_I;8zAp{Sh(F6{wZfV|mVj9&P?$i~yW7-??&oA2RA~GP_ib&d zw)3#IkYdgtvcIDjj6?JjwAJ}+1he$Q?I83^VAhK!#Q1v8ZeI<11_wU2v^wbDbXKiH z;g=pXN)r)C)%U^c%>Ng@ac0KqDVds8OE7ttMJ%iRAlgK&$Xnm+Z@gI2zr=f^6P0z*hxInOdIZyv z-!Jg@{L09Krdy(SY-E8yScrNWJIKA84PY4El{>Q$6PX_KEpZbLwxhmZ)cAu~|3H6b z6g{~2!;prG8k&c9)QTw?eF4yvR>@1jq0{BXfo0Xh`9p?&mnh>8?KWGsz~Yo=Rm9fr zmhIq&dkqtNC+jyfe?y_#zC_vByRX~66-P8nOcDbm%QNfwaiZ&}Ky+`%E6377RVsz! z?f~WU%8s7`o}K3+Jp_s~G(`#h;YjXpi*L!j_T*|M?>K%xWBgY@8`qQC$A$z!Z*Idn z74&r_%H%_tZiKs9`@N)YEr3Oq=|S_meKjmWy+`nIv)9~wJOh;1@88+d5>b5bjG|zm zQtpJqirI95?f1*#WRSe$>c6{mE^kpB9WwMn>Nn1(9Y}iqDk|iIo%0J~-~{Yn#*i*a zeaR-9&?(FoeMCXeHuC~sracf?Bla}9#q7_Vm4yZk59huZ;d!hQE0Io{+uBoY`-%AS zZ^tn&t6Wfp52n@KM2CN2;e0R zA49cmBBfq4Ubj4?z0y-bBhu&(9;3tQJeUt0pb0Tu?|_cqQD!2e6Y(z%=$VIWX{GV7 z(w=Gl!f;a!2XKqc`Pm$v#(pW8e#k;HJTIO2GM)o}Qc5-8OOf#(A6!;WJMg7O|5?<* zpx}XTB*)>pEi>K1&xnqu^MLuiN(zMUPmQm7*E~yK2Y-}gGnuuoQC0ozMa(Lq4Rbw3gWOC$88HL}r%m;7Sq zIgCT=ynv&OMa}gnN=(Fcry%Aktoqav?HW7~^M4!GQfX?Lrs|IB<>_(fqVEz1Ul_A3 zU=TsO+F*jdj@R-oDUF-ZJfIQpK6Hx2iB)PondbQx1Iib(@Wby zNaWL}nT!2j1?lB&ri|#0j^_-W+$=}cy|@t2DA^n=)P}?M46DfHQa=S7bXfkkQ!nvF z;=02h>X5klD5>|e|Nm*nYI@3YPK{mgjwy_O{aoF_=o-FrvE$0VrbdH5p`uNw;Lqu-0` zEc&NWvzRK6KT7Y(Uzk-{t*iN}(I`?Y;Bum~VL9M&ogP#L`_LHHYHYV)>ejw}rDN5;N=-4VR%hKjM5*|#)f zJPkQ%7ZHl^jZPbRz>jF??myr{^-8U8lD&)_U{Omfjd=#38vl#@_Ag9rUPP6mY*%v{ z5!Xuv(Z+^rt?}@9hl_xY_~SSKAiG?$@fnK~#4hUA8g88I?0B6aKJCS^-$(kRQDWI` z6SCY%x^Iz&;KP%>>;A(3{sjpm3yBW3&+!tdhc(mMg#4=0i;#^533?%WNqSZWBaBvt z9%nC(7>~7IASRd9mUm}729dHTMPJ64$fo+I(|;mch-cE9BBX4z&CZK)_De?J{xGC< z+j8u#6SvRH|2T;Zcpql8`V85`mlF><^zXX>V(l4HwJZ`t<=>FccJtlGx)V*$P&(8P z&(=>`-0Hs$X5;N0%0DmO5sO$E|6D_kne5)=+^gNovrbo6?{#8&_HthVTlV6?Zp^e` zD1QP>hFugXm425Eq#O9o+LyTV>b_)#C>ZD-+%`{1nkDl0Iv5pd4eV9!h6%FihBXp@ z9tvG2cFy#YIr$QY5=ssq?nQ+8-R}LziNiqMhhKvbAEpjh{fHTYkT7yLkxUL81BVyg zN+E17cB`7@u_f&TzA^_cGze7oO+0pmBG5l;Z8D3m*QYxef@|ofXv_Wvq67Kn#e{t~ z&=Yb{m~Iz!?O8+S`gid6P>r+i-yMH4T%p_g9NZh*1BSgf7e(1)buwa*GInZhQ!I^3 z1DAGHSnTWk|0ezO+X8-6EmHP%7U+fr!QGP`>JEE}4Td)}rHx@?d|$DAYl=mVzC;id zYduasPZ`XEyp29LPv(4L8AbH=>}VT(vpj5A^DmYdlc47nq*-|@X#;i6ZU_`czp|?h z->ci}(bBdv@NBpgW<6Z0j%3mf_QZJj+MO8a1pgPgm`*>a0kRIgzRnoHlC>xCY;dz8 zb-ifp-_#FMbv`Ff9Li~r)00cKLfHA94~&0(m5W4Sjk1WtQXY^-k8I*P2ksqbhT*__l*MkU}UrVt!vD8&5Fg zpL>(|)z1A~hi^)?p=7xmsE1<)xiF(kFKNT{5tr%~(Vf=d{+oT)WJc*D_6dDEt(4YP z0?3o^{SU82F9ZAehE)ZrS#iiohitK1e>!&Ocs0WB4Kmon?BD+?Xbdc0t2IUskTx;s z4p}Ngx;1f|-S6fFejGeUeW3#QtE$sS+@lbo>?LVRbz{pLytIZw`AC`lXZ3nI=tNwWXa zH)z7L?-hT&ca>RQG^T z!}60)CU{#!`S{BIRiQMOzB#e7yI-y+MEc0l9Kn*}X;#j9BG~@eGo^P-SuDMP<(*Og zdj7oQQ?2ZBsC2mAfE**2IPjWz;bLfgNZ1=l1WcT8C^Vy~G21=0W8K{y|rD`2+F z<@eUDG5O>4rT;HKCR$B-s{Bg_6`-}(e!FW6e&Ma{_tJOv&v+U0;-9V1<&iaSgX`5> zd2+G*qy*xCee+xY8FNt+czcHjSYX=fH2gBNtLB+fG&*TsYrKD3MB+#hBw`{h)uhfh z=QQWZwiM6V8MWm%sMWfH`-~&#eMIzYQl#XqLmsO3>$xwlp4&MH{*pu-?f?2mV zx_a^vumoQgNwXL0@M_OBq#e{qwZ2Pi?dR+0q^w*S-z_rFi+_S9y1FU1-fHn0BkB_f zAq^9!R#WgF!3i4fwv<0A3X>>ZO1XWUOY~1=Jtx7R+X=f zf$x8QYC$$%Zu>)=b*ffHO+)0-MKdpBi(z@SR#-mYEqwOiJ_d;sv;B$oV#%uMiI{Q|x_d z>4!7~@L{x4{{EJd(~(vNCOHtveOY{6+HIf&}%Oc-0R9+5JQN&zc4mhw1({M_X8BiQ@&CsE4LP93%@gl$n)0!|8Ecw zYW6cnPg4xE8g2bvg&uyiYZg1|s=a)1F6UI2GHauzHo@bs$pQ!W=sW1|MIZ&o%|=vC z*oytQsIgK%S=Caxd3n6*Q$w)hFNV9@V`(DSxF>NBozWaTd(W`T@$=ck>Q%$SpaE@i zd45#87a+d=`#)()$?J0Z1)ae|{oS}U-uXYTeMYPHNh*hk^4AaD4S$6CRkkYR(aVa^ z@zMi(=|ky=usbOF9~fPm3@#z{T&4@GkzWM&8WsALJjqb0euMVUQ}z$8M+KMW|19_q zA;mF0wKF+F+}-4!aSp3{)Fm^G&gWNGMA{1?jFdx~0N|hKR$K`p4#!Zft95D`AN~o7l+{CS% z-)782eA*w76cT@{1p+yUvWm_R;r{OUJTQ%aSMO6Hql02ej9p-VkwD@#Q*0@n-34P=S6l>a33G2*B)at)sh5Yj-0 zcsU&ulxvHh%l0Es+3M;&YpBG5YBr~wV1rtQZuR^AxYzZJSCjm2?<=CY8ayNSq`xXI z^{`eD;|JzzEZJza&jqKP6B3QSDzgx?h)^Uk8k|&~mG>LxuM$mYd<1TA8-NpG!f`MV zMkwEdU^p}L$kKlEiD&|Zw)Bg$Au~b>-mjIiG)~rexAAK)L+)LbDo#`f(;{$`|ZIDrVt9w{pcK-Ku|x;M}URqa{it zdT{Yjt=O7<0!?+!nkT%%_x`kncHRAGsC}I}V7gp+Sx-eEfzP=Vra!v1D`ka>g21)| z>oU62b^F-mZUzlM4#}bSZZ}Tj3|a33NV1`i)W4KV zaK=xLx5w&X3!LsJcCVzvq6xUnk7T)~%FgW}GFkk^m_N6Ec^x-EQE@26*=GF0pai-0Q+&9}0#YFB3CY=iP! z|DBvx^HYkdL3+mc>#ZuM$$BlIjaHL2#y)Uvja>f`x!6Fl0wrAL<&CEk9>x>0`pUv- zP^5J}pgU{6gtvG=R|yi;`}ijX(Zh)cDdPl%6}l$AY)jF`0{E2PEc_F!;GePwqIPNv zqVRlt+dP)X{|L}VpNg4vU)Kkzh>WV=*Ap}~S7d@brUD7nM3%U*l~_XfIBexEiT9Pw-tv$8 zQTMfh$5O`Y1&er@ZlG~L>)$$MxkZ*lV?Y*W-C(oxjopy8r~479WzFO_>t~$&v+?Sv z?~~EP850jJgH!H#K*~^Z-5I1zBi_T!_=S2FUt*JV{$(UgfPW-|b|d(XN&8foQ(^vn zE2rOCD?2?>zb)WI3NVPrtRrPK9hN-Tl$HMS_*nS}etdlL(fmmoOd1>xN%^eRF5DI# z(@zGJ*T(*4zac#1RBQ}Y?SC2XEmz3Yt^ zJ%rl^`EiF1Yn*X8^bz5ZWzt=BV1yikpcBz3i213k{hIsx6*aRz2NAe(c2e4T@AYps z1ZZjngjE+*)4lr2N9nHXigPw{dbO9D^?MtY0}l;sxH^egqi!g~z+U5~a^)0DXPiPqJ;Hau|lwfGK_NJ1-mRZ^fI#z7sXZ-B2cHx{TxE?`EIkLt38Z z)324VRs{mVT4S!at@4Vq4ItZ&BKaW$Li0~yU$j`b_`hVmGz&v<%#jSK;*aRCC(9+I zKD{vB0oRmwOM0Zama~ts?3%~nlFJc9o%6SHW$tLM0WQj1Wba4(4JwQOY?cy4sFODS zH{15xeQ}GZhxuV?H#+lP!D;OvNMDE2+wbk6I&4B6&qPy6W^#>+GS?k|qSA~{sEiMqH^ zh&!lTeb)~*IAM%txv6aR@y7VXTiK|(nUb-$<|a2F5D%@4_==qKlG>W5r`j6PIWVTJ z^d65XYNr=naLuaKGA#4qvt9fHL8vvchrR4kQH31)>(31JPim* z+TsBVQW}{D;aIP)Ad<6eEbqnboMy$p0@p-Q{-vKstX?ft2tFH}SpAst#efe#p0;|7 zd;&wS>awfDa;HMB%AEmnBt4PSj9{M@ZGL5{P82|t7zp4|_Lgtj-Be$SX{h3sm6yDr z%#$^$awlC>9@j{17l{1G>rg2Dizn(~(Fa@S&lrrfezT(F>|LOezcs?AsHdnpr^~9d z3h{#MPaBpMG^Z{j)HSN2iE#BF4r!Bp;F8-G-1pc$zSw5bNE^xhezvxm-7F#b=_slR zMu;EQqewo|Q`^G~SiP!gZlYWi&UM{r{4h^1ui~9evi{tav7HdBj1Xc^7*-&0C34AW z>*XPF1|zD8y~S(WjID_I)6nIUN@l=|`^}5PFkhHgH=k-+*p&j(yj4^LrKyRBWyTyR zD-)BC$~~673&Lzu&+cl;O`S_WFL&-ynx%belp3l%;6Z1k94APL&y2I|!csn>YZ$@Zjx+CAK^db0eKX5gEB_9noW7Udo35*p3rTFNx2zIA37 zK>rOs-jwr@%%15%uMh;uo~?oHM`D+$2xuTiP#A)ly4QrobS@f^W7ass5a7ERV; z^{1h*zu_dYcjh04XKsAH-{gzAA1aFn0{Q$cpSkqPF&!I!g#D?z*(^JzeKe}>?FG(t_VDZp~SUhtj8sjMC6{-{k>=wk8k^e*k^ zeJb!%Sr7XC$K30itehGVrgl_Bg~%i9P?;!vTq|I3!c!$0VU!1R*{oENEV*6K*FY#r z{U+o8+Eh)~o1AYFmWuy)93{4PlypRA)9)ZnQK?zi)%9TUr2T2a>B45m0pZ=1($O+M z82H24++BP&#BPrsOCS*2txBbM){<_Yj$ioLukA{xMKF(_L%BH9sfTvzO5~dEwTPn< z!RBI>Q~0|6?tGEWdaOJL1s7{JtDc5VCSvGouy^j9#x3$g2+t8EuoY$88CLBrdAnKc zaH|(WWB(yz)Jcd(E2(~69T~wyWK(5Czep|!mQxbMq%D8w4@lK+keQdQdItVj_)<3p zyH8BoYG^bb`a=e?KnlD?_JtPJ(sqo^ZlY3&zF{3XkOaRWq@uJ7*$hYLR+6^f5HTez!F3qUvq8P{cOZXo=Lwc;MXV|r5S7{vCGJQF-jg`pVvDxxptN(UTdU>V zdhM-cbN*w|=U?f&$EM*mJeGD~V%_m69up`tG_jf+(`L>2NjE*X^75A8QBBR6kIW-N z>_3X4&pMN;&hMIh6}fU7;p;z5DFI*Dy zV$w-;D;#nst8VKpiwU0&cHE_DEs_{1hY3*Urx{IXovpN*pgq|TdeD%%2Ig}$Z-2kL zsXF6)aiw$+6^#A+XuBTaYg7&oBQp0NCvQC=|*W!i-*D zrG_L7H*~4KMq1x_KIUrppeeBLr8pPGg z`nlFwC~;8^^x}S1OPLWPA&%sJHoI|7fF0_ zlFbZdrz@7&IAbY*^JuHT#y}qL4}*L!5vq`+7-nx86$B(kgY}v@Okr0q~6YKT?!< zPJa+VhF4!rP2EZv8C})5wL$;tHg`-f_-Xzt>APRvJNLA)UwvXW`tAw&umbhgp zY_R9Xez#iuWxxXiERQB%8veI5Z={kOS4PpmXxZnz3g(o$oOy-gqHp2^A>jHV2@%z4^iOQ{E#Vufljm zZk5-l3e(-)m8!SixE7Wex1K8J#bV>ph;&4Kkh0ncVgM@*E~P`$LyHZ5_XUcjN(I27 zFH4NofA|vIzL zj>oW{8O%*ozVCO&5jl$EXO zpDrCFFA1>{Cn7UlJ_t>%#fjZW7&fFX7_CthOpEO;_#OGs5{>r^BRcmC?vC{jc zul;FNQR&PU&DMWcB!u>DUi^Wy;nj~s zP)XFD$`0z$5-q@mY6YG!s-Kc$7j(sR`IwW4(G1aZ@7|r{HG}~{vY$D6KPq?Bb^0#0 zC#k+_6-8}y>jJVwRm%h!-y~1bceqK zlSY-g)7FLk&#J=Icj326?fL z3_V9sKFu_<1zGD}?=-+7nE1-710!6r_;x$OR;z5tNy>)gcLZmzm4-{VhtTxLD%Vfv zD+FKJW##Ls1K_Wbhop8r{{y(aD&2{(l*2<8K@N>-8xG+`hEKOw?=HT5Z$Ghh4ls?8 z)tVy=PgC})=mQ`7%xA*FGizsFah)7ZleB&oj%QhX^M8Xp8zv3PlLU0e^*{U#+wZi70mBJeUc?0a}@dLC-^V zsk?oM&db0OBqzNo%%N8p^3G@qy4>ieuWqG3CMbq6CWkXRr7M-0o%(UdTz&YNPA_HB z8je{H5>gvs;6>&S&+0Jiu#0VqQS*bL#TE4SMh_YB9eUwK$a3~uGUlEFNq-0s_+=u`9cNkGC^=|~H7(`xf6 z-%V)x;X+ssM}*HB9hDK>ko?UlIMb5}{}4k>;FLJrpt`T_XDZ6I$%tuMSCqJUc=J+Z zN#oQxD~8c76sp#3%!&NslCb@0eJUnVY1A?|cE#|?im);9->xo|uY9vGLDG@}zLQZ! z`D@q{#G6oNqRuDaM?Sc~@6;(X5&pfWq0@tByw=s=JSb4@XHm=cBThp~d_{IWejrE{ z9{;oY6K6IP`I~?F=~j1wU_4W9DOB;`F~q286vPuhVej$hV|4GcP;evy9w6KxJ4Np` zL!_6=@u(6NaQX1~%{&7MU`jzGu~5 zgZ)kogRGU{5NDaq!Q5;=1D(*BQk zuYa(n`rAkoOoubTQ%5pyhBo}|F!XuPmKK@wYqMd~-gM;2kO-M?dE64AZEV&Zt@ECg zodKo&tIKFi9`)cJQyF4~v6r>l1ta+Po4xXrs%Q&8rnp1SM>~c-lz@@fWVcXteIA5K z`nA;-sxdl~>~D%6nA;96k07K;xEF`014iu7ly9mA)k~W9gDLI|t%-4L2ECh(jy9pH ziUtT*({GtuNy#Q2Z%CiHrdROFYc+2H40LQ|W%WwzSU4rwR({$ucGs}9x32O0H43^F zuspf0sa7)vOJQd7`Qw{TG>Un~&v)VJal{qqC~)3fdk;hAn>4DeoMQ?rcRoSzxiLQG zkUq=kVH3oz@(U9nu`(7TQ61d%lcpL{zRPUgQb7D)v*_-Wuz5Oyax`fC4){hPLy1@= z65g+I%T>eYH-TR0bMAj#;gc1Ul_7iMW;>k=MxQ_?$5|mKC7*>`s(NwTa-8rtQ7ST) zd%HKVPV?r5`yXjoO1GM4a6Mr!Ab=x2#-=}@=_#SpVJq(Jd|nkpScTbmXE zd9hTJJgi_qAj3?@QSM6 zU@!z_Y9NSamvmMtDS$9hQyH}lBSi1tVH2iHN1~dBN2s4AVtsDDAliAFoM3H@h_)#v z5@zx|R=X+4aATEkhL)N`gp{z=A4fhU%%F+h%*$uz1q?Bnljzg@Xp=4@CSHCN z#=-Inz{$>%3Qoa?CB!+H!?Lm%2uu4s_s>;t OX_0WPMi?@|;_whfwLLXRMmJsD zb6WEK=P=XMv&`!*j{C${d>&OhuG0Y+kl4%#iJKetsc6Uw;p7>pE1{?xg#G#F8Gqlu z{H}XAXV9k!SJzA6@OhHr&R>B1W3M`07Y>=zsHXE@D0jmfy2sYzORolQs=O-U{o3v4Q>q{V<(8tj z3MSRaX1hTNWh~|@oPjb@W_(%kw-Wql#=+~O9gqFTGh;eZGcOeng8$fE*h@YrN~zo_ zzDc5a@6vFGB^=&3(Y1KtrDVNFHc$%b(kbtOmz+IVzWLX~u(`fR>%rS>&`}1UX4>tN zW`{YQ3#o`1Q$y<{p)qAFl#Of8%w+ISO`Xd<&7*SEhl|H+#-zGAPvm67hh6}*Q~f@> z(8`p>7x06d#M`SF=--;0et%nIdB~n$RYo1pgYNMX;A!jy+U5>-*Zr)e+KJogFO;4+ zI*bRjlA&b6YLFPt=~K7!7gfbRdr+N*Y@FZg8Q`v%lR+ht^*la^yh%0P&)DsSno=A7Rnu%|(l(1Jy&23B5JISt? zi0sE8bHUqH*7xES)-k^347l3F$!8kf;=$#9`Y<8soJ z$k{)poWVgaCp*hHs4foY#n%SZZBqbhgiV$HI2Za2=9AA8ju#ES&!i>AgDc{HSn*p` z7&gfD^ycbPBWDlu%ey$Qh5(D<(ucKGOXF)4%0~~Vq@Ff%N6%~(G<9p8m`6|&O=%s4 z^4qKKeU?IvI#mZ(;NqQM*GH4ws=0qNmOQ?H$uzeFI+{s6^T$GNjY=ESA4Nsu=%cGa z8+mCX_U@UxrojO^*PP8OWlXeEjq3l*8TDW0pGtqs zN|C<2R;_WF-`f6F3qQD!ELn)5Kh@HB_w*Z^?YjW*#QP7jNKgEENFz!_Y zuQ(Juz0^~$kgw?qadggnmAcW`*$1Ijjps9Sf=L?t z{4}8h>Q&5ai`G9kHarZ`76cmh)A?SiH{_z0ssTF`mLXJoBcBag6_Iz`20ppi>S9+K zu(Zi{Z4Jdp=8a}~&ElPPA)$Qv{m4xO8yjE%w29xX*jp*oiV z%5BQQyG%Bcqgs`gky%

oM5;*GHXXCY(>Vj#2XTf(Cx8JXW}Ip`WenyLePw&2;oG@bVnXN8j(ggF z^-3V(S`mV-kR`QJ5+%bE8dM+=&}H9NBq7tbLjT~t&^X1zzv~aEbLF?{ZP1L=EaB~M ziWO2%1@bC|rNIt&Wi^YOzC?ZY_{!#L&0UkJxcU{~8xuD%sizxBgr&Q4MLX7RlaCXD zjA@kYlE0Q-p>9OC1*Au?dPPRE%7+J7teVKh{n~T-5nO?dHO~AQL_xjYlUUs2JZMMD z-BPXuC(58BlgFKE1i9A1sLw9--ep6QjSqcJ%3QMHhZetqe4i~i8$&To3JI~CM+uQo zreB9aCvfh3Jd#@-bMN5rI&waWD63HeAJ z&hIF0%2k=DTWBlXNvQSE9sc~uD&=I=o)XMG09|G=G8#c>KGYh7_iVk$0+X~ZZrbXz z33{tV>raz_y{CEPy@G_E($BYgclFd$`BeqyBb%PgmBnZJy{6Gv74J+Y6}sk%Ty4() z%h8(&Nqx)>aIDRim+b;WE$Ui6ONhK74b{P#BjZm-+*WOeVgTA{?Xii|jM4o3c=D!6pOd9_LWJ?87PvA1jljDOSG7laG3nCy<6ign{r>6!y%H&NWk zX%vwYEKC$v2U^9>I=#>X_*^LVGiPsq|SK@xM-)rGPBk+sQ z$^iaG)w+eW(H2BK&U@hrp80cNSyL! zM0Zqg*~wUIyC{`GiVO7Qb{=u)xjBqT{Pg+5??9WM%hNJc8EjR{Cpn=KMwFWi^^&s# zQ$f_7mY;`wR{sw10)zQbC>DOHNymh^WWXCry~0&ST%>kqv?r`sE}=`>ga~W5HN=AA zXnvk!$OwvQo|wZhT%?7{HP`b z&@!gLM*@F5RwU~_Zf8(ogbr&Jtt|rAI`E9p_WE}BUB~%&PKpby5sfAL`&o@bGuOw2 zwbCl#!RTnYOjCYfK%FZ!eC($5mZpAj0GEFAvNqVuLz0_n?uyjYsi_= z15JYVyaiP@d~Rb__|HR(MB#SY%#3Asr3Znoh!(@0yRyvTt^s=$?3tBgyL2Dx8xF(Q z$0DC2!DbKeNFis=$7)$MsMHkNLQ%~TF7dmKUboTAZ7)aSv`R<1zmLcWkypX);ezOu z3+<2M7M7Sa<&Gj^QeKj}MQ@Vs6-`ps@TP5PQ>BR>hQL)BC#mXsn4<<;rxhz)Zk;-ExGMx<^?-^;{D|1thb>4WG;yT~+FB->>K2hkdO z*i#FWBA8HXm;?1C5u%AI3i8FWW2E?Bc`ZwGB8p3_X&Dvi_F7?4*nWvul#IJDF(ERD zv9PN@6VRGXQ^rq}M6<9~(C0NLox6)4 zgNHF$u*kl@0!nuAqM$rq4f0wcQUUfkPfrPa&!d zfUk+yCG0AzK|&4-eK4MrztQJ8p3*@t@c(B)259kawxx8tZ11r8M;k()d-RXT$2q!Q z)SUb>FeP%NWw_7_806jzD%zo+cp6Hjf?B>_pE(!$3SQI3ugaY~wq0mYgb`btd{kDaecxh_J9D7Lz! zVy6y1=%zLlAs+)?ryQ53$1!P+^nyzBn){^KSiINB+mJpMmA3JS{KAKeZL%Uu%ae`to;P!qJ47XfR< zc$QF&)e9zoC|l*vGC0u>?*{IxyG<6D!(-Dr!X0LRiC6|UW*h?nd2KHUOhCDE?!G-fmYjoKvVo6Z+U^nh*68pw zGB_$Mf3IwpasA96**fnDB2fAFit~N(56tMrk^}azD+Ds;`xgnxo;o<=>7Aaae>6xJ zU3wGTgCy+Z)8Ht;cLi_-?Kp6(0X7HTjb!?Z1i}PyK!FtwdXoe(J6J2WSY`Fr^h@OVP!bu^Ck$ymA-M z3nP~(DtTKg7=OGOxJRb)`N(lcBu%O9iVJCg!rv+6!@CjVR}K@EV&wA!Aq~pZV*c&2 z!}bG>ZZ=~GkVWo~tKk@`A7Lj0!#mdFOw7!CG4M zh91RXv6eAaaHyQJ%yh}hE~x_LS5ioN{xYu?$)qhlzh?H+$6upejM)_0Lqz3~DmvzX z{CX$ZS%rravf+SWFS03Hv-Fvmt;ze7Gw%UG!b;4C(!cvw|23pSemclX@j^v*>W$!! zHzTJEb^T;109R7%s_WCh)*C2}6f$&<42HXE_u1ZoN+e4X&N6?S{JnFo=9m2_Pvn_R+J^o0)96|nz5ZbP#a;YJxt zPuL%m@2)lM7>%uzZmAS%KU`SrZe0WMjFDUi^nY;Mr|a88CSPI{Z-n}r3*_5RFW6)TmhH5la6Q*MXIZBusbVL4t&aZnDw3%8j@`(;>$l+@+dz` z9`>duL{%;EMUK%Zi&9si@O7a?N%k`TKi^T7#8eC3hTYhLf+mN z9CMK|k&DM5_Fo5!US1B6XAwdX9pqt2L)_sWNywFeWU0gnk;#442axBN`x8=5?a3sNEUL|W6gA9@Zgk>++=+Kc~-feknyPf8iXvU$jq7ClaC-FntF`+Sg}?g zK<-^0MaWVfVgmmzImk-$kFpNPbvcOHR(BM)W^L6ee`V@gK7gDqk8)XynZx~dvdx|6 zA(IfhIS09VCwuyQqpxfE0CH`~8)zRnpNwc7wMBF$Y|O=I;;>@lbwh}lna8?HLq;ad z`n3~`3^q+4d7bj|fV==9Rvhk5$fP`-kBP@xR1Uvs`pD~)cLwAcgxE|Z&rv3apS68Z zW;sZiMnC7>l7&Y;{PF|H3zi3INC{EwRAc6~t_0+KzfW?IwlYXgY^a-!q8*NeRezOk z9Y69qN_xk|ys^vk1lo02+3M}sUpd94?a-+{0ET{Q-e!iu; zmJc9zFE2pIbty+qNxrd_iLGDC3TAa!%atfU$5LI(2avm$N97^I^uj*#iG*Ck$Ex2l zN6PDi(=FAtd;ob=Im?M`%&<8V^jlUxfr@RMDYJ>88zm$@eOW_H)BX$@oeJ<5H9Zr$fF3!c5xU>_%J^QFlpUlndHJJ^6`B20|=J41Z1t% zOT%>**K4FKO;z0!kTbJKA%{3QJG^$n#jt-zHh#)`0`fA1tZB$#WB$k>q^_F$y$+Ct z>Dvz=uUH;I$Tr`ePiUp9la1LUkL2Szzm=W)1ISC3#}l#^{7Wt?-#BHM!q0Y2C@4Uc0bt-RZ^4+XM2lgj{)Ba9ox=PL`TW z{R7BKd4E9eOGwG%{cQaKd|X4w>AQ=wvrQg3yVHx8KLE(f67m2>uHoYY$SvhB0CI0a zmfX}*-JkRNKKBuKPB2c)TAiIHxN4^tFMk4%OL?4-tnRTSAh&gS5_9)sw0|{cOO6=RsIwpk0)exOA>awy& zt~%0njc1qx~*oan0`U zTmB6|{w5(GK%P+k5kQ{ivTnaunUuANJUP*I`uNxG{=Vg30p#roxyw!L4{V&4c%4@&C zPb&}Bl;0{JKyEHC{rWz!{7phWfZSYO`Q?3P`I|251IW$gg%O?Z zT^`pn*B$wB5&L0bP(Fa9<+TX;@bNq414v$8ijWT=zf(Sd49Y7J@&V*`$_J29c_BhR zfc#GR0CK9ldv|?cI9uNR6@PYl@BaGmak{+s3;qk`tvl=k$m#OdulKK%ckZzdAg9Ya zzudo6-nh#?fSfLG{A&MNdEY+!0CKv#?~DDyaexX-CfIOyr0C`Ax zez$%Ac})2L@|g1Ue*FOQnDPPSLFL&U`vK%J Date: Sat, 14 Mar 2026 00:46:06 -0700 Subject: [PATCH 22/45] yoosh --- auto_sim/.vscode/settings.json | 3 +- auto_sim/README.md | 9 +- auto_sim/main.py | 14 ++- auto_sim/render.py | 34 +++-- auto_sim/test_controller_2/CMakeLists.txt | 5 + auto_sim/test_controller_2/main.cpp | 145 ++++++++++++---------- 6 files changed, 119 insertions(+), 91 deletions(-) create mode 100644 auto_sim/test_controller_2/CMakeLists.txt diff --git a/auto_sim/.vscode/settings.json b/auto_sim/.vscode/settings.json index 3212c3c..5535099 100644 --- a/auto_sim/.vscode/settings.json +++ b/auto_sim/.vscode/settings.json @@ -1,3 +1,4 @@ { - "python-envs.defaultEnvManager": "ms-python.python:pipenv" + "python-envs.defaultEnvManager": "ms-python.python:pipenv", + "cmake.sourceDirectory": "C:/Users/jessi/Documents/School/fsae/sim/auto_sim/test_controller_2" } \ No newline at end of file diff --git a/auto_sim/README.md b/auto_sim/README.md index 8addb73..4397b68 100644 --- a/auto_sim/README.md +++ b/auto_sim/README.md @@ -1,5 +1,6 @@ # TODO -- simulation loop -- basic boundary checking -- basic controller? -- full MPC \ No newline at end of file + +* basic boundary checking, triangulation +* basic controller? +* full MPC + diff --git a/auto_sim/main.py b/auto_sim/main.py index 746a526..ed6c4ea 100644 --- a/auto_sim/main.py +++ b/auto_sim/main.py @@ -37,6 +37,17 @@ # vehicle_state.theta = math.radians(23.54) vehicle_state.omega = -0.2 +def handle_key(key: int, state: VehicleState): + match key: + case pygame.K_d: + state.theta += 0.1 + case pygame.K_a: + state.theta -= 0.1 + case pygame.K_w: + state.x += 0.1 + case pygame.K_s: + state.x -= 0.1 + while running: # handle events # pygame.QUIT event means the user clicked X to close your window @@ -44,10 +55,11 @@ if event.type == pygame.QUIT: running = False + dt = clock.get_time() # fill the screen with a color to wipe away anything from last frame screen.fill("black") controls = compute(vehicle_state, CONE_POSITIONS) - sim_step(vehicle_state, controls, clock.get_time()) + sim_step(vehicle_state, controls, dt) render_world(vehicle_state, CONE_POSITIONS, screen) # flip() the display to put your work on screen diff --git a/auto_sim/render.py b/auto_sim/render.py index c7b49ff..4400bca 100644 --- a/auto_sim/render.py +++ b/auto_sim/render.py @@ -1,10 +1,9 @@ import pygame from scipy.spatial.transform import Rotation as R from constants import VEHICLE_WIDTH_M -from Controller import VehicleState +from Controller import VehicleState, Cone, ConeColor from math import ceil, degrees import numpy as np -from Controller import Cone, ConeColor PIXELS_PER_M = 50.0 w: int @@ -18,21 +17,8 @@ def init(): car = pygame.image.load('fsae.png').convert_alpha() vignette_image = pygame.image.load('vignette.png').convert_alpha() vignette_image = pygame.transform.scale(vignette_image, (1800,1800)) - - vignette = pygame.surface.Surface((3000, 3000), pygame.SRCALPHA) - vignette.fill((0,0,0,0)) - vignette.blit(vignette_image, vignette_image.get_rect(center=(1500, 1500)), special_flags=pygame.BLEND_RGBA_ADD) - -def handle_key(key: int, state: VehicleState): - match key: - case pygame.K_d: - state.theta += 0.1 - case pygame.K_a: - state.theta -= 0.1 - case pygame.K_w: - state.x += 0.1 - case pygame.K_s: - state.x -= 0.1 + vignette = pygame.Surface((3000,3000), pygame.SRCALPHA) + vignette.blit(vignette_image, vignette_image.get_rect(center=(1500,1500))) def transform(x: float, y: float, vehicle_state: VehicleState) -> tuple[int, int]: """ @@ -59,6 +45,7 @@ def create_grid_surface(l: int, spacing_px: int, color: str) -> pygame.Surface: old_grid_surf = None def drawGrid(screen: pygame.Surface, state: VehicleState, color=(45, 45, 45), spacing_m=1): global h, w, old_state, old_grid_surf, vignette + # create grid, cached spacing_px = int(spacing_m * PIXELS_PER_M) l: int= ceil(np.hypot(w/2, h/2) * 0.4 / spacing_m) * spacing_m if (spacing_m, color, l) != old_state or old_grid_surf is None: @@ -67,15 +54,22 @@ def drawGrid(screen: pygame.Surface, state: VehicleState, color=(45, 45, 45), sp old_grid_surf = create_grid_surface(2*l, spacing_px, color) grid_surf = old_grid_surf + # grid + screen_grid = pygame.surface.Surface((w, h), pygame.SRCALPHA) + # Calculate grid offset to align with world coordinates r = R.from_euler('z', -state.theta + np.pi/2, degrees=False) x, y = r.apply([state.x % spacing_m, state.y % spacing_m, 0])[:2] - rotated_surf = pygame.transform.rotate(grid_surf, degrees(-state.theta - np.pi/2)) - screen.blit(rotated_surf, rotated_surf.get_rect(center=( + rotated_surf: pygame.Surface = pygame.transform.rotate(grid_surf, degrees(-state.theta - np.pi/2)) + screen_grid.blit(rotated_surf, rotated_surf.get_rect(center=( w/2 - x * PIXELS_PER_M, 0.67 * h + y * PIXELS_PER_M )).topleft) - screen.blit(vignette, vignette.get_rect(center=(w/2, 0.67*h)), special_flags=pygame.BLEND_RGBA_MULT) + + # vignette + screen_grid.blit(vignette, vignette.get_rect(center=(w/2, 0.67*h)), special_flags=pygame.BLEND_RGBA_MULT) + + screen.blit(screen_grid, (0, 0)) def int_to_color(value: ConeColor) -> str: match value: diff --git a/auto_sim/test_controller_2/CMakeLists.txt b/auto_sim/test_controller_2/CMakeLists.txt new file mode 100644 index 0000000..d260db5 --- /dev/null +++ b/auto_sim/test_controller_2/CMakeLists.txt @@ -0,0 +1,5 @@ +cmake_minimum_required(VERSION 3.10) +set(CMAKE_CXX_STANDARD 20) +project(controller_intellisense) + +add_executable(controller_intellisense main.cpp) \ No newline at end of file diff --git a/auto_sim/test_controller_2/main.cpp b/auto_sim/test_controller_2/main.cpp index 08874f1..86daf5e 100644 --- a/auto_sim/test_controller_2/main.cpp +++ b/auto_sim/test_controller_2/main.cpp @@ -1,92 +1,107 @@ +#include +#include #include #include +#include #include -#include -#include struct ControlOutput { - // in local frame - double ax; - // no frame - double omega_dot; - ControlOutput(const double _ax, const double _omega_dot) : ax(_ax), omega_dot(_omega_dot) {} + // in local frame + double ax; + // no frame + double omega_dot; + ControlOutput(const double _ax, const double _omega_dot) + : ax(_ax), omega_dot(_omega_dot) {} }; struct VehicleState { - // in global frame - double x; - double y; - double theta; - // in local frame - double v_x; - double v_y; - // no frame - double omega; - VehicleState() : x(0), y(0), theta(0), v_x(0), v_y(0), omega(0) {} -}; -enum class ConeColor { - BLUE, - YELLOW + // in global frame + double x; + double y; + double theta; + // in local frame + double v_x; + double v_y; + // no frame + double omega; + VehicleState() : x(0), y(0), theta(0), v_x(0), v_y(0), omega(0) {} }; +enum class ConeColor { BLUE, YELLOW }; struct Cone { - double x; - double y; - ConeColor c; - Cone(double _x, double _y, ConeColor _c) : x(_x), y(_y), c(_c) {} + double x; + double y; + ConeColor c; + Cone(double _x, double _y, ConeColor _c) : x(_x), y(_y), c(_c) {} }; -void sim_step(VehicleState& state, const ControlOutput& control, const int dt) { - const double dt_s = dt / 1000.0; - state.v_x += (control.ax + state.omega * state.v_y) * dt_s; +void sim_step(VehicleState &state, const ControlOutput &control, const int dt) { + const double dt_s = dt / 1000.0; + state.v_x += (control.ax + state.omega * state.v_y) * dt_s; - const double beta = std::atan2(state.v_y, state.v_x); - const double tire_ay = -1 * beta; // simple tire model for lateral forces - state.v_y += (tire_ay - state.omega * state.v_x) * dt_s; + const double beta = std::atan2(state.v_y, state.v_x); + const double tire_ay = -1 * beta; // simple tire model for lateral forces + state.v_y += (tire_ay - state.omega * state.v_x) * dt_s; - state.theta += std::fmod(state.omega * dt_s, 2 * std::numbers::pi); - const double v_x_world = state.v_x * std::cos(state.theta) - state.v_y * std::sin(state.theta); - const double v_y_world = state.v_x * std::sin(state.theta) + state.v_y * std::cos(state.theta); - state.x += v_x_world * dt_s; - state.y += v_y_world * dt_s; + state.theta += std::fmod(state.omega * dt_s, 2 * std::numbers::pi); + const double v_x_world = state.v_x * std::cos(state.theta) - state.v_y * std::sin(state.theta); + const double v_y_world = state.v_x * std::sin(state.theta) + state.v_y * std::cos(state.theta); + state.x += v_x_world * dt_s; + state.y += v_y_world * dt_s; } -ControlOutput compute(const VehicleState& ve, const std::vector& cones) { - // Implementation for compute function - return {0,0}; +static std::unordered_map> adj{}; +/** + * Triangulates the cones. + * @param cones The vector of cones to triangulate. + * @note This function assumes that the input vector of cones is an add-only + * array whose order is preserved. + */ +void triangulate(const std::vector &cones) { + static size_t last_seen_cone = 0; + for (; last_seen_cone < cones.size(); ++last_seen_cone) { + const Cone &cone = cones[last_seen_cone]; + // process the new cone + } +} + +ControlOutput compute(const VehicleState &ve, const std::vector &cones) { + // Implementation for compute function + triangulate(cones); + return {0, 0}; } namespace py = pybind11; PYBIND11_MODULE(Controller, m, py::mod_gil_not_used()) { - py::class_(m, "ControlOutput") - .def(py::init()) - .def_readwrite("ax", &ControlOutput::ax) - .def_readwrite("omega_dot", &ControlOutput::omega_dot); - - py::class_(m, "VehicleState") - .def(py::init<>()) - .def_readwrite("x", &VehicleState::x) - .def_readwrite("y", &VehicleState::y) - .def_readwrite("theta", &VehicleState::theta) - .def_readwrite("v_x", &VehicleState::v_x) - .def_readwrite("v_y", &VehicleState::v_y) - .def_readwrite("omega", &VehicleState::omega); - - py::class_(m, "Cone") - .def(py::init()) - .def_readwrite("x", &Cone::x) - .def_readwrite("y", &Cone::y) - .def_readwrite("c", &Cone::c); - - py::enum_(m, "ConeColor") - .value("BLUE", ConeColor::BLUE) - .value("YELLOW", ConeColor::YELLOW) - .export_values(); + py::class_(m, "ControlOutput") + .def(py::init()) + .def_readwrite("ax", &ControlOutput::ax) + .def_readwrite("omega_dot", &ControlOutput::omega_dot); + + py::class_(m, "VehicleState") + .def(py::init<>()) + .def_readwrite("x", &VehicleState::x) + .def_readwrite("y", &VehicleState::y) + .def_readwrite("theta", &VehicleState::theta) + .def_readwrite("v_x", &VehicleState::v_x) + .def_readwrite("v_y", &VehicleState::v_y) + .def_readwrite("omega", &VehicleState::omega); + + py::class_(m, "Cone") + .def(py::init()) + .def_readwrite("x", &Cone::x) + .def_readwrite("y", &Cone::y) + .def_readwrite("c", &Cone::c); + + py::enum_(m, "ConeColor") + .value("BLUE", ConeColor::BLUE) + .value("YELLOW", ConeColor::YELLOW) + .export_values(); - m.def("compute", &compute, R"pbdoc( + m.def("compute", &compute, R"pbdoc( Compute control output )pbdoc"); - m.def("sim_step", &sim_step, R"pbdoc( + m.def("sim_step", &sim_step, R"pbdoc( Simulate one step of the vehicle dynamics )pbdoc"); } \ No newline at end of file From 31ef4828bef50fb5ee43aa621da23595c5458945 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Sat, 14 Mar 2026 00:48:52 -0700 Subject: [PATCH 23/45] remove math --- auto_sim/main.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/auto_sim/main.py b/auto_sim/main.py index ed6c4ea..2115c68 100644 --- a/auto_sim/main.py +++ b/auto_sim/main.py @@ -1,5 +1,3 @@ -import math - import pygame from Controller import compute, sim_step, Cone, ConeColor, VehicleState from render import init, render_world From 10cdb2c4797750153bf87cc1f761b262b77e36af Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Sun, 15 Mar 2026 13:27:34 -0700 Subject: [PATCH 24/45] poof --- auto_sim/.vscode/settings.json | 3 +- auto_sim/main.py | 4 +- auto_sim/test_controller_2/controller.cpp | 9 ++ auto_sim/test_controller_2/controller.hpp | 5 + auto_sim/test_controller_2/main.cpp | 107 ----------------- auto_sim/test_controller_2/pybind.cpp | 44 +++++++ auto_sim/test_controller_2/setup.py | 2 +- auto_sim/test_controller_2/sim.cpp | 19 +++ auto_sim/test_controller_2/sim.hpp | 4 + auto_sim/test_controller_2/triangulation.cpp | 117 +++++++++++++++++++ auto_sim/test_controller_2/triangulation.hpp | 65 +++++++++++ auto_sim/test_controller_2/types.hpp | 31 +++++ 12 files changed, 299 insertions(+), 111 deletions(-) create mode 100644 auto_sim/test_controller_2/controller.cpp create mode 100644 auto_sim/test_controller_2/controller.hpp delete mode 100644 auto_sim/test_controller_2/main.cpp create mode 100644 auto_sim/test_controller_2/pybind.cpp create mode 100644 auto_sim/test_controller_2/sim.cpp create mode 100644 auto_sim/test_controller_2/sim.hpp create mode 100644 auto_sim/test_controller_2/triangulation.cpp create mode 100644 auto_sim/test_controller_2/triangulation.hpp create mode 100644 auto_sim/test_controller_2/types.hpp diff --git a/auto_sim/.vscode/settings.json b/auto_sim/.vscode/settings.json index 5535099..82b6a2f 100644 --- a/auto_sim/.vscode/settings.json +++ b/auto_sim/.vscode/settings.json @@ -1,4 +1,5 @@ { "python-envs.defaultEnvManager": "ms-python.python:pipenv", - "cmake.sourceDirectory": "C:/Users/jessi/Documents/School/fsae/sim/auto_sim/test_controller_2" + "cmake.sourceDirectory": "C:/Users/jessi/Documents/School/fsae/sim/auto_sim/test_controller_2", + "editor.defaultFormatter": null } \ No newline at end of file diff --git a/auto_sim/main.py b/auto_sim/main.py index 2115c68..8457ec8 100644 --- a/auto_sim/main.py +++ b/auto_sim/main.py @@ -31,9 +31,9 @@ init() vehicle_state: VehicleState = VehicleState() -vehicle_state.v_x = 2.0 +# vehicle_state.v_y = 2.0 # vehicle_state.theta = math.radians(23.54) -vehicle_state.omega = -0.2 +# vehicle_state.omega = -0.2 def handle_key(key: int, state: VehicleState): match key: diff --git a/auto_sim/test_controller_2/controller.cpp b/auto_sim/test_controller_2/controller.cpp new file mode 100644 index 0000000..84180b3 --- /dev/null +++ b/auto_sim/test_controller_2/controller.cpp @@ -0,0 +1,9 @@ +#include "controller.hpp" +#include "triangulation.hpp" + +ControlOutput compute(const VehicleState& ve, const std::vector& cones) +{ + // Implementation for compute function + triangulate(cones); + return { 0, 0 }; +} \ No newline at end of file diff --git a/auto_sim/test_controller_2/controller.hpp b/auto_sim/test_controller_2/controller.hpp new file mode 100644 index 0000000..d805669 --- /dev/null +++ b/auto_sim/test_controller_2/controller.hpp @@ -0,0 +1,5 @@ +#pragma once +#include "types.hpp" +#include + +ControlOutput compute(const VehicleState& ve, const std::vector& cones); \ No newline at end of file diff --git a/auto_sim/test_controller_2/main.cpp b/auto_sim/test_controller_2/main.cpp deleted file mode 100644 index 86daf5e..0000000 --- a/auto_sim/test_controller_2/main.cpp +++ /dev/null @@ -1,107 +0,0 @@ -#include -#include -#include -#include -#include -#include - -struct ControlOutput { - // in local frame - double ax; - // no frame - double omega_dot; - ControlOutput(const double _ax, const double _omega_dot) - : ax(_ax), omega_dot(_omega_dot) {} -}; -struct VehicleState { - // in global frame - double x; - double y; - double theta; - // in local frame - double v_x; - double v_y; - // no frame - double omega; - VehicleState() : x(0), y(0), theta(0), v_x(0), v_y(0), omega(0) {} -}; -enum class ConeColor { BLUE, YELLOW }; -struct Cone { - double x; - double y; - ConeColor c; - Cone(double _x, double _y, ConeColor _c) : x(_x), y(_y), c(_c) {} -}; - -void sim_step(VehicleState &state, const ControlOutput &control, const int dt) { - const double dt_s = dt / 1000.0; - state.v_x += (control.ax + state.omega * state.v_y) * dt_s; - - const double beta = std::atan2(state.v_y, state.v_x); - const double tire_ay = -1 * beta; // simple tire model for lateral forces - state.v_y += (tire_ay - state.omega * state.v_x) * dt_s; - - state.theta += std::fmod(state.omega * dt_s, 2 * std::numbers::pi); - const double v_x_world = state.v_x * std::cos(state.theta) - state.v_y * std::sin(state.theta); - const double v_y_world = state.v_x * std::sin(state.theta) + state.v_y * std::cos(state.theta); - state.x += v_x_world * dt_s; - state.y += v_y_world * dt_s; -} - -static std::unordered_map> adj{}; -/** - * Triangulates the cones. - * @param cones The vector of cones to triangulate. - * @note This function assumes that the input vector of cones is an add-only - * array whose order is preserved. - */ -void triangulate(const std::vector &cones) { - static size_t last_seen_cone = 0; - for (; last_seen_cone < cones.size(); ++last_seen_cone) { - const Cone &cone = cones[last_seen_cone]; - // process the new cone - } -} - -ControlOutput compute(const VehicleState &ve, const std::vector &cones) { - // Implementation for compute function - triangulate(cones); - return {0, 0}; -} - -namespace py = pybind11; - -PYBIND11_MODULE(Controller, m, py::mod_gil_not_used()) { - py::class_(m, "ControlOutput") - .def(py::init()) - .def_readwrite("ax", &ControlOutput::ax) - .def_readwrite("omega_dot", &ControlOutput::omega_dot); - - py::class_(m, "VehicleState") - .def(py::init<>()) - .def_readwrite("x", &VehicleState::x) - .def_readwrite("y", &VehicleState::y) - .def_readwrite("theta", &VehicleState::theta) - .def_readwrite("v_x", &VehicleState::v_x) - .def_readwrite("v_y", &VehicleState::v_y) - .def_readwrite("omega", &VehicleState::omega); - - py::class_(m, "Cone") - .def(py::init()) - .def_readwrite("x", &Cone::x) - .def_readwrite("y", &Cone::y) - .def_readwrite("c", &Cone::c); - - py::enum_(m, "ConeColor") - .value("BLUE", ConeColor::BLUE) - .value("YELLOW", ConeColor::YELLOW) - .export_values(); - - m.def("compute", &compute, R"pbdoc( - Compute control output - )pbdoc"); - - m.def("sim_step", &sim_step, R"pbdoc( - Simulate one step of the vehicle dynamics - )pbdoc"); -} \ No newline at end of file diff --git a/auto_sim/test_controller_2/pybind.cpp b/auto_sim/test_controller_2/pybind.cpp new file mode 100644 index 0000000..fc50e73 --- /dev/null +++ b/auto_sim/test_controller_2/pybind.cpp @@ -0,0 +1,44 @@ +#include +#include + +#include "controller.hpp" +#include "sim.hpp" +#include "types.hpp" + +namespace py = pybind11; + +PYBIND11_MODULE(Controller, m, py::mod_gil_not_used()) +{ + py::class_(m, "ControlOutput") + .def(py::init()) + .def_readwrite("ax", &ControlOutput::ax) + .def_readwrite("omega_dot", &ControlOutput::omega_dot); + + py::class_(m, "VehicleState") + .def(py::init<>()) + .def_readwrite("x", &VehicleState::x) + .def_readwrite("y", &VehicleState::y) + .def_readwrite("theta", &VehicleState::theta) + .def_readwrite("v_x", &VehicleState::v_x) + .def_readwrite("v_y", &VehicleState::v_y) + .def_readwrite("omega", &VehicleState::omega); + + py::class_(m, "Cone") + .def(py::init()) + .def_readwrite("x", &Cone::x) + .def_readwrite("y", &Cone::y) + .def_readwrite("c", &Cone::c); + + py::enum_(m, "ConeColor") + .value("BLUE", ConeColor::BLUE) + .value("YELLOW", ConeColor::YELLOW) + .export_values(); + + m.def("compute", &compute, R"pbdoc( + Compute control output + )pbdoc"); + + m.def("sim_step", &sim_step, R"pbdoc( + Simulate one step of the vehicle dynamics + )pbdoc"); +} \ No newline at end of file diff --git a/auto_sim/test_controller_2/setup.py b/auto_sim/test_controller_2/setup.py index 6fc00b1..e258cc8 100644 --- a/auto_sim/test_controller_2/setup.py +++ b/auto_sim/test_controller_2/setup.py @@ -7,7 +7,7 @@ ext_modules = [ Pybind11Extension( "Controller", - ["main.cpp"], + ["pybind.cpp", "triangulation.cpp", "sim.cpp"], ), ] diff --git a/auto_sim/test_controller_2/sim.cpp b/auto_sim/test_controller_2/sim.cpp new file mode 100644 index 0000000..6978b08 --- /dev/null +++ b/auto_sim/test_controller_2/sim.cpp @@ -0,0 +1,19 @@ +#include "sim.hpp" +#include +#include + +void sim_step(VehicleState& state, const ControlOutput& control, const int dt) +{ + const double dt_s = dt / 1000.0; + state.v_x += (control.ax + state.omega * state.v_y) * dt_s; + + const double beta = std::atan2(state.v_y, state.v_x); + const double tire_ay = -1 * beta; // simple tire model for lateral forces + state.v_y += (tire_ay - state.omega * state.v_x) * dt_s; + + state.theta += std::fmod(state.omega * dt_s, 2 * std::numbers::pi); + const double v_x_world = state.v_x * std::cos(state.theta) - state.v_y * std::sin(state.theta); + const double v_y_world = state.v_x * std::sin(state.theta) + state.v_y * std::cos(state.theta); + state.x += v_x_world * dt_s; + state.y += v_y_world * dt_s; +} \ No newline at end of file diff --git a/auto_sim/test_controller_2/sim.hpp b/auto_sim/test_controller_2/sim.hpp new file mode 100644 index 0000000..e64c29e --- /dev/null +++ b/auto_sim/test_controller_2/sim.hpp @@ -0,0 +1,4 @@ +#pragma once +#include "types.hpp" + +void sim_step(VehicleState& state, const ControlOutput& control, const int dt); \ No newline at end of file diff --git a/auto_sim/test_controller_2/triangulation.cpp b/auto_sim/test_controller_2/triangulation.cpp new file mode 100644 index 0000000..808425a --- /dev/null +++ b/auto_sim/test_controller_2/triangulation.cpp @@ -0,0 +1,117 @@ +#include "triangulation.hpp" + +#include +#include +#include + +class ConeList { + const std::span cones; + +public: + ConeList(const std::span _cones) + : cones(_cones) + { + } + const Cone& operator[](const size_t cone_id) const + { + switch (cone_id) { + case 0: + return Cone { -100, -100, ConeColor::BLUE }; + case 1: + return Cone { 100, -100, ConeColor::YELLOW }; + case 2: + return Cone { 0, 100, ConeColor::BLUE }; + default: + return cones[cone_id - 3]; + } + } +}; + +void legalize_edge(const size_t new_cone, const size_t cone_a, const size_t cone_b) +{ +} + +bool on_edge(const Cone& cone, + const size_t a, + const size_t b, + const ConeList& existing_cones) +{ + // check if cone lies on the edge (a,b) + const Cone& cone_a = existing_cones[a]; + const Cone& cone_b = existing_cones[b]; + // check if cone is collinear with cone_a and cone_b + const double cross_product = (cone.y - cone_a.y) * (cone_b.x - cone_a.x) - (cone.x - cone_a.x) * (cone_b.y - cone_a.y); + if (std::abs(cross_product) > 1e-6) { + return false; + } + // check if cone is within the bounding box of cone_a and cone_b + const double dot_product = (cone.x - cone_a.x) * (cone_b.x - cone_a.x) + (cone.y - cone_a.y) * (cone_b.y - cone_a.y); + if (dot_product < 0) { + return false; + } + const double squared_length_ab = (cone_b.x - cone_a.x) * (cone_b.x - cone_a.x) + (cone_b.y - cone_a.y) * (cone_b.y - cone_a.y); + if (dot_product > squared_length_ab) { + return false; + } + return true; +} + +bool edge_detect(const Cone& cone, + size_t& a, + size_t& b, + size_t& c, + const ConeList& existing_cones) +{ + if (on_edge(cone, a, b, existing_cones)) { + return true; + } else if (on_edge(cone, b, c, existing_cones)) { + std::swap(a, c); + return true; + } else if (on_edge(cone, a, c, existing_cones)) { + std::swap(b, c); + return true; + } + return false; +} + +void insert(const Cone& cone, const size_t cone_id, const ConeList existing_cones) +{ + std::shared_ptr t = root.find_triangle(cone, existing_cones); // find smallest triangle which contains the cone + bool on_edge = edge_detect(cone, a, b, c, existing_cones); // check if cone lies on the edges of the triangle + if (on_edge) { // if on_edge, then cone is known to be on edge (a,b) + // very annoying + } else { // split triangle a,b,c into three triangles a,b,cone_id; + // b,c,cone_id; a,c,cone_id + adj[cone_id] = { a, b, c }; + adj[a].push_back(cone_id); + adj[b].push_back(cone_id); + adj[c].push_back(cone_id); + t.children.push_back(std::make_shared(a, b, cone_id)); + t.children.push_back(std::make_shared(b, c, cone_id)); + t.children.push_back(std::make_shared(a, c, cone_id)); + + // legalize the edges + legalize_edge(cone_id, a, b); + legalize_edge(cone_id, b, c); + legalize_edge(cone_id, a, c); + } +} + +/** + * Triangulates the cones. Populates the adj with the edges of the + * triangulation. + * @param cones The vector of cones to triangulate. + * @note This function assumes that the input vector of cones is an add-only + * array whose order is preserved. + */ +void triangulate(const std::vector& cones) +{ + static size_t last_seen_cone = 0; + for (; last_seen_cone < cones.size(); ++last_seen_cone) { + const Cone& cone = cones[last_seen_cone]; + // process the new cone + const size_t cone_id = last_seen_cone + 3; // cones 0,1,2 are reserved for the super triangle + const std::span s { cones }; + insert(cone, cone_id, s.first(last_seen_cone - 1)); + } +} \ No newline at end of file diff --git a/auto_sim/test_controller_2/triangulation.hpp b/auto_sim/test_controller_2/triangulation.hpp new file mode 100644 index 0000000..1df9f4a --- /dev/null +++ b/auto_sim/test_controller_2/triangulation.hpp @@ -0,0 +1,65 @@ +#pragma once +#include "types.hpp" +#include + +class Triangulation { + struct Triangle { + size_t a, b, c; // indices of the cones that form the triangle + Triangle(size_t _a, size_t _b, size_t _c) + : a(_a) + , b(_b) + , c(_c) + { + } + bool contains(const Cone& cone, const ConeList& existing_cones) const + { + // check if cone is inside the triangle formed by cones a, b, c + const Cone& cone_a = existing_cones[a]; + const Cone& cone_b = existing_cones[b]; + const Cone& cone_c = existing_cones[c]; + const double area_abc = 0.5 * std::abs(cone_a.x * (cone_b.y - cone_c.y) + cone_b.x * (cone_c.y - cone_a.y) + cone_c.x * (cone_a.y - cone_b.y)); + const double area_abp = 0.5 * std::abs(cone_a.x * (cone_b.y - cone.y) + cone_b.x * (cone.y - cone_a.y) + cone.x * (cone_a.y - cone_b.y)); + const double area_acp = 0.5 * std::abs(cone_a.x * (cone_c.y - cone.y) + cone_c.x * (cone.y - cone_a.y) + cone.x * (cone_a.y - cone_c.y)); + const double area_bcp = 0.5 * std::abs(cone_b.x * (cone_c.y - cone.y) + cone_c.x * (cone.y - cone_b.y) + cone.x * (cone_b.y - cone_c.y)); + return std::abs(area_abc - area_abp - area_acp - area_bcp) < 1e-6; + } + }; + + class TrianglePyramid { + std::vector> children {}; + Triangle t; + + public: + bool is_intact() { return children.empty(); } + std::shared_ptr + find_triangle(const Cone& cone, const ConeList& existing_cones) + { + for (const auto& child : children) { + if (child->t.contains(cone, existing_cones)) { + if (child->is_intact()) { + return child; + } + return child->find_triangle(cone, existing_cones); + } + } + throw std::runtime_error("Cone is not contained in any triangle"); + } + TrianglePyramid() = delete; + TrianglePyramid(const Triangle& _t) + : t(_t) + { + } + TrianglePyramid(size_t _a, size_t _b, size_t _c) + : t { _a, _b, _c } + { + } + }; + + TrianglePyramid root { 0, 1, 2 }; // super triangle that contains all cones + + static std::unordered_map> adj { + { 0, { 1, 2 } }, { 1, { 0, 2 } }, { 2, { 0, 1 } } + }; // adj of the edges +public: + void triangulate(const std::vector& cones); +} \ No newline at end of file diff --git a/auto_sim/test_controller_2/types.hpp b/auto_sim/test_controller_2/types.hpp new file mode 100644 index 0000000..46aa263 --- /dev/null +++ b/auto_sim/test_controller_2/types.hpp @@ -0,0 +1,31 @@ +#pragma once + +struct ControlOutput { + // in local frame + double ax; + // no frame + double omega_dot; + ControlOutput(const double _ax, const double _omega_dot) + : ax(_ax), omega_dot(_omega_dot) {} +}; + +struct VehicleState { + // in global frame + double x; + double y; + double theta; + // in local frame + double v_x; + double v_y; + // no frame + double omega; + VehicleState() : x(0), y(0), theta(0), v_x(0), v_y(0), omega(0) {} +}; + +enum class ConeColor { BLUE, YELLOW }; +struct Cone { + double x; + double y; + ConeColor c; + Cone(double _x, double _y, ConeColor _c) : x(_x), y(_y), c(_c) {} +}; \ No newline at end of file From ad1cf785299e23a3b1da606c98c08714ade0b967 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Sun, 15 Mar 2026 13:53:38 -0700 Subject: [PATCH 25/45] more connections --- auto_sim/test_controller_2/controller.cpp | 11 +++- auto_sim/test_controller_2/controller.hpp | 2 + auto_sim/test_controller_2/pybind.cpp | 10 ++++ auto_sim/test_controller_2/setup.py | 4 +- auto_sim/test_controller_2/triangulation.cpp | 57 +++++--------------- auto_sim/test_controller_2/triangulation.hpp | 52 ++++++++++++++++-- 6 files changed, 85 insertions(+), 51 deletions(-) diff --git a/auto_sim/test_controller_2/controller.cpp b/auto_sim/test_controller_2/controller.cpp index 84180b3..ff3c53f 100644 --- a/auto_sim/test_controller_2/controller.cpp +++ b/auto_sim/test_controller_2/controller.cpp @@ -1,9 +1,16 @@ #include "controller.hpp" -#include "triangulation.hpp" + +Triangulation t {}; + +Triangulation& get_triangulation() +{ + return t; +} ControlOutput compute(const VehicleState& ve, const std::vector& cones) { // Implementation for compute function - triangulate(cones); + t.triangulate(cones); + // t.adj will contain the adj list return { 0, 0 }; } \ No newline at end of file diff --git a/auto_sim/test_controller_2/controller.hpp b/auto_sim/test_controller_2/controller.hpp index d805669..f0a3e85 100644 --- a/auto_sim/test_controller_2/controller.hpp +++ b/auto_sim/test_controller_2/controller.hpp @@ -1,5 +1,7 @@ #pragma once +#include "triangulation.hpp" #include "types.hpp" #include +Triangulation& get_triangulation(); ControlOutput compute(const VehicleState& ve, const std::vector& cones); \ No newline at end of file diff --git a/auto_sim/test_controller_2/pybind.cpp b/auto_sim/test_controller_2/pybind.cpp index fc50e73..374d5e4 100644 --- a/auto_sim/test_controller_2/pybind.cpp +++ b/auto_sim/test_controller_2/pybind.cpp @@ -34,6 +34,16 @@ PYBIND11_MODULE(Controller, m, py::mod_gil_not_used()) .value("YELLOW", ConeColor::YELLOW) .export_values(); + // export object t (of type Triangulation) to python + py::class_(m, "Triangulation") + .def(py::init<>()) + .def("triangulate", &Triangulation::triangulate) + .def_readonly("adj", &Triangulation::adj); + + m.def("get_triangulation", &get_triangulation, R"pbdoc( + Get the current triangulation object) + )pbdoc"); + m.def("compute", &compute, R"pbdoc( Compute control output )pbdoc"); diff --git a/auto_sim/test_controller_2/setup.py b/auto_sim/test_controller_2/setup.py index e258cc8..886aa8c 100644 --- a/auto_sim/test_controller_2/setup.py +++ b/auto_sim/test_controller_2/setup.py @@ -7,7 +7,9 @@ ext_modules = [ Pybind11Extension( "Controller", - ["pybind.cpp", "triangulation.cpp", "sim.cpp"], + ["pybind.cpp", "triangulation.cpp", "sim.cpp", "controller.cpp"], + include_dirs=["."], + language="c++", ), ] diff --git a/auto_sim/test_controller_2/triangulation.cpp b/auto_sim/test_controller_2/triangulation.cpp index 808425a..a447f93 100644 --- a/auto_sim/test_controller_2/triangulation.cpp +++ b/auto_sim/test_controller_2/triangulation.cpp @@ -1,40 +1,10 @@ #include "triangulation.hpp" -#include -#include -#include - -class ConeList { - const std::span cones; - -public: - ConeList(const std::span _cones) - : cones(_cones) - { - } - const Cone& operator[](const size_t cone_id) const - { - switch (cone_id) { - case 0: - return Cone { -100, -100, ConeColor::BLUE }; - case 1: - return Cone { 100, -100, ConeColor::YELLOW }; - case 2: - return Cone { 0, 100, ConeColor::BLUE }; - default: - return cones[cone_id - 3]; - } - } -}; - -void legalize_edge(const size_t new_cone, const size_t cone_a, const size_t cone_b) +void Triangulation::legalize_edge(const size_t new_cone, const size_t cone_a, const size_t cone_b) { } -bool on_edge(const Cone& cone, - const size_t a, - const size_t b, - const ConeList& existing_cones) +bool Triangulation::on_edge(const Cone& cone, const size_t a, const size_t b) { // check if cone lies on the edge (a,b) const Cone& cone_a = existing_cones[a]; @@ -56,28 +26,24 @@ bool on_edge(const Cone& cone, return true; } -bool edge_detect(const Cone& cone, - size_t& a, - size_t& b, - size_t& c, - const ConeList& existing_cones) +bool Triangulation::edge_detect(const Cone& cone, size_t& a, size_t& b, size_t& c) { - if (on_edge(cone, a, b, existing_cones)) { + if (on_edge(cone, a, b)) { return true; - } else if (on_edge(cone, b, c, existing_cones)) { + } else if (on_edge(cone, b, c)) { std::swap(a, c); return true; - } else if (on_edge(cone, a, c, existing_cones)) { + } else if (on_edge(cone, a, c)) { std::swap(b, c); return true; } return false; } -void insert(const Cone& cone, const size_t cone_id, const ConeList existing_cones) +void Triangulation::insert(const Cone& cone, const size_t cone_id) { - std::shared_ptr t = root.find_triangle(cone, existing_cones); // find smallest triangle which contains the cone - bool on_edge = edge_detect(cone, a, b, c, existing_cones); // check if cone lies on the edges of the triangle + std::shared_ptr t = root.find_triangle(cone); // find smallest triangle which contains the cone + bool on_edge = edge_detect(cone, a, b, c); // check if cone lies on the edges of the triangle if (on_edge) { // if on_edge, then cone is known to be on edge (a,b) // very annoying } else { // split triangle a,b,c into three triangles a,b,cone_id; @@ -104,7 +70,7 @@ void insert(const Cone& cone, const size_t cone_id, const ConeList existing_cone * @note This function assumes that the input vector of cones is an add-only * array whose order is preserved. */ -void triangulate(const std::vector& cones) +void Triangulation::triangulate(const std::vector& cones) { static size_t last_seen_cone = 0; for (; last_seen_cone < cones.size(); ++last_seen_cone) { @@ -112,6 +78,7 @@ void triangulate(const std::vector& cones) // process the new cone const size_t cone_id = last_seen_cone + 3; // cones 0,1,2 are reserved for the super triangle const std::span s { cones }; - insert(cone, cone_id, s.first(last_seen_cone - 1)); + existing_cones.set_cones(s.first(last_seen_cone)); + insert(cone, cone_id); } } \ No newline at end of file diff --git a/auto_sim/test_controller_2/triangulation.hpp b/auto_sim/test_controller_2/triangulation.hpp index 1df9f4a..2609c38 100644 --- a/auto_sim/test_controller_2/triangulation.hpp +++ b/auto_sim/test_controller_2/triangulation.hpp @@ -1,7 +1,14 @@ #pragma once #include "types.hpp" +#include +#include +#include +#include +#include #include +// inspired by https://youtu.be/1TUUevxkvp4 + class Triangulation { struct Triangle { size_t a, b, c; // indices of the cones that form the triangle @@ -31,8 +38,8 @@ class Triangulation { public: bool is_intact() { return children.empty(); } - std::shared_ptr - find_triangle(const Cone& cone, const ConeList& existing_cones) + + std::shared_ptr find_triangle(const Cone& cone, const ConeList& existing_cones) { for (const auto& child : children) { if (child->t.contains(cone, existing_cones)) { @@ -55,11 +62,50 @@ class Triangulation { } }; + class ConeList { + const std::optional> cones; + + public: + ConeList() + : cones(std::nullopt) + { + } + + void set_cones(std::span _cones) + { + cones.emplace(_cones); + } + + const Cone& operator[](const size_t cone_id) const + { + switch (cone_id) { + case 0: + return Cone { -100, -100, ConeColor::BLUE }; + case 1: + return Cone { 100, -100, ConeColor::YELLOW }; + case 2: + return Cone { 0, 100, ConeColor::BLUE }; + default: + return cones.value()[cone_id - 3]; + } + } + }; + TrianglePyramid root { 0, 1, 2 }; // super triangle that contains all cones + ConeList existing_cones {}; + + /** + * helpers + */ + bool on_edge(const Cone& cone, const size_t a, const size_t b); + bool edge_detect(const Cone& cone, size_t& a, size_t& b, size_t& c); + void insert(const Cone& cone, const size_t cone_id); + void legalize_edge(const size_t new_cone, const size_t cone_a, const size_t cone_b); +public: + Triangulation() = default; static std::unordered_map> adj { { 0, { 1, 2 } }, { 1, { 0, 2 } }, { 2, { 0, 1 } } }; // adj of the edges -public: void triangulate(const std::vector& cones); } \ No newline at end of file From d08216d10c5b84a00516ebd81a7eb887316772b5 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Mon, 16 Mar 2026 16:46:26 -0700 Subject: [PATCH 26/45] cleanup --- auto_sim/test_controller_2/CMakeLists.txt | 23 ++++- auto_sim/test_controller_2/CMakePresets.json | 14 +++ auto_sim/test_controller_2/controller.cpp | 2 +- auto_sim/test_controller_2/triangulation.cpp | 61 +++++++------- auto_sim/test_controller_2/triangulation.hpp | 89 ++++++++++---------- 5 files changed, 113 insertions(+), 76 deletions(-) create mode 100644 auto_sim/test_controller_2/CMakePresets.json diff --git a/auto_sim/test_controller_2/CMakeLists.txt b/auto_sim/test_controller_2/CMakeLists.txt index d260db5..ae6b49c 100644 --- a/auto_sim/test_controller_2/CMakeLists.txt +++ b/auto_sim/test_controller_2/CMakeLists.txt @@ -1,5 +1,22 @@ cmake_minimum_required(VERSION 3.10) -set(CMAKE_CXX_STANDARD 20) -project(controller_intellisense) +set(CMAKE_CXX_STANDARD 23) +project(high_level_controller) -add_executable(controller_intellisense main.cpp) \ No newline at end of file +file( + DOWNLOAD + https://github.com/cpm-cmake/CPM.cmake/releases/download/v0.40.8/CPM.cmake + ${CMAKE_CURRENT_BINARY_DIR}/cmake/CPM.cmake + EXPECTED_HASH SHA256=78ba32abdf798bc616bab7c73aac32a17bbd7b06ad9e26a6add69de8f3ae4791 +) +include(${CMAKE_CURRENT_BINARY_DIR}/cmake/CPM.cmake) + +CPMAddPackage( + NAME pybind11 + GIT_REPOSITORY https://github.com/pybind/pybind11.git + VERSION 3.0.2 + DOWNLOAD_ONLY TRUE +) +add_subdirectory(${pybind11_SOURCE_DIR}) + +file(GLOB SOURCES *.cpp) +pybind11_add_module(controller ${SOURCES}) \ No newline at end of file diff --git a/auto_sim/test_controller_2/CMakePresets.json b/auto_sim/test_controller_2/CMakePresets.json new file mode 100644 index 0000000..7d04dd9 --- /dev/null +++ b/auto_sim/test_controller_2/CMakePresets.json @@ -0,0 +1,14 @@ +{ + "version": 10, + "configurePresets": [ + { + "name": "default", + "displayName": "", + "description": "", + "generator": "Visual Studio 17 2022", + "binaryDir": "${sourceDir}/build", + "cacheVariables": { + } + } + ] +} \ No newline at end of file diff --git a/auto_sim/test_controller_2/controller.cpp b/auto_sim/test_controller_2/controller.cpp index ff3c53f..714d9e9 100644 --- a/auto_sim/test_controller_2/controller.cpp +++ b/auto_sim/test_controller_2/controller.cpp @@ -10,7 +10,7 @@ Triangulation& get_triangulation() ControlOutput compute(const VehicleState& ve, const std::vector& cones) { // Implementation for compute function - t.triangulate(cones); + t.update(cones); // t.adj will contain the adj list return { 0, 0 }; } \ No newline at end of file diff --git a/auto_sim/test_controller_2/triangulation.cpp b/auto_sim/test_controller_2/triangulation.cpp index a447f93..867e2e1 100644 --- a/auto_sim/test_controller_2/triangulation.cpp +++ b/auto_sim/test_controller_2/triangulation.cpp @@ -4,14 +4,15 @@ void Triangulation::legalize_edge(const size_t new_cone, const size_t cone_a, co { } -bool Triangulation::on_edge(const Cone& cone, const size_t a, const size_t b) -{ +bool Triangulation::on_edge(const Cone& cone, const size_t a, const size_t b) const { // check if cone lies on the edge (a,b) const Cone& cone_a = existing_cones[a]; const Cone& cone_b = existing_cones[b]; // check if cone is collinear with cone_a and cone_b - const double cross_product = (cone.y - cone_a.y) * (cone_b.x - cone_a.x) - (cone.x - cone_a.x) * (cone_b.y - cone_a.y); - if (std::abs(cross_product) > 1e-6) { + if (const double cross_product = + (cone.y - cone_a.y) * (cone_b.x - cone_a.x) - + (cone.x - cone_a.x) * (cone_b.y - cone_a.y); + std::abs(cross_product) > 1e-6) { return false; } // check if cone is within the bounding box of cone_a and cone_b @@ -26,16 +27,17 @@ bool Triangulation::on_edge(const Cone& cone, const size_t a, const size_t b) return true; } -bool Triangulation::edge_detect(const Cone& cone, size_t& a, size_t& b, size_t& c) -{ +bool Triangulation::edge_detect(const Cone& cone, size_t& a, size_t& b, size_t& c) const { if (on_edge(cone, a, b)) { return true; - } else if (on_edge(cone, b, c)) { - std::swap(a, c); - return true; - } else if (on_edge(cone, a, c)) { - std::swap(b, c); - return true; + } + if (on_edge(cone, b, c)) { + std::swap(a, c); + return true; + } + if (on_edge(cone, a, c)) { + std::swap(b, c); + return true; } return false; } @@ -43,24 +45,25 @@ bool Triangulation::edge_detect(const Cone& cone, size_t& a, size_t& b, size_t& void Triangulation::insert(const Cone& cone, const size_t cone_id) { std::shared_ptr t = root.find_triangle(cone); // find smallest triangle which contains the cone - bool on_edge = edge_detect(cone, a, b, c); // check if cone lies on the edges of the triangle - if (on_edge) { // if on_edge, then cone is known to be on edge (a,b) + if (bool on_edge = edge_detect(cone, a, b, c)) { + // if on_edge, then cone is known to be on edge (a,b) // very annoying - } else { // split triangle a,b,c into three triangles a,b,cone_id; - // b,c,cone_id; a,c,cone_id - adj[cone_id] = { a, b, c }; - adj[a].push_back(cone_id); - adj[b].push_back(cone_id); - adj[c].push_back(cone_id); - t.children.push_back(std::make_shared(a, b, cone_id)); - t.children.push_back(std::make_shared(b, c, cone_id)); - t.children.push_back(std::make_shared(a, c, cone_id)); - - // legalize the edges - legalize_edge(cone_id, a, b); - legalize_edge(cone_id, b, c); - legalize_edge(cone_id, a, c); + throw std::runtime_error("Edge splitting not implemented yet"); // inshallah we will never need to implement this } + // split triangle a,b,c into three triangles a,b,cone_id; + // b,c,cone_id; a,c,cone_id + adj[cone_id] = {a, b, c}; + adj[a].push_back(cone_id); + adj[b].push_back(cone_id); + adj[c].push_back(cone_id); + t.children.push_back(std::make_shared(a, b, cone_id)); + t.children.push_back(std::make_shared(b, c, cone_id)); + t.children.push_back(std::make_shared(a, c, cone_id)); + + // legalize the edges + legalize_edge(cone_id, a, b); + legalize_edge(cone_id, b, c); + legalize_edge(cone_id, a, c); } /** @@ -70,7 +73,7 @@ void Triangulation::insert(const Cone& cone, const size_t cone_id) * @note This function assumes that the input vector of cones is an add-only * array whose order is preserved. */ -void Triangulation::triangulate(const std::vector& cones) +void Triangulation::update(const std::vector& cones) { static size_t last_seen_cone = 0; for (; last_seen_cone < cones.size(); ++last_seen_cone) { diff --git a/auto_sim/test_controller_2/triangulation.hpp b/auto_sim/test_controller_2/triangulation.hpp index 2609c38..d45ca8d 100644 --- a/auto_sim/test_controller_2/triangulation.hpp +++ b/auto_sim/test_controller_2/triangulation.hpp @@ -1,24 +1,56 @@ #pragma once #include "types.hpp" -#include #include #include #include +#include #include #include // inspired by https://youtu.be/1TUUevxkvp4 class Triangulation { + class ConeList { + std::optional> cones; + + public: + ConeList() + : cones(std::nullopt) + { + } + + void set_cones(std::span _cones) + { + cones.emplace(_cones); + } + + const Cone& operator[](const size_t cone_id) const + { + static const Cone cone0{ -100, -100, ConeColor::BLUE }, + cone1 = { 100, -100, ConeColor::YELLOW }, + cone2 = { 0, 100, ConeColor::BLUE }; + switch (cone_id) { + case 0: + return cone0; + case 1: + return cone1; + case 2: + return cone2; + default: + return cones.value()[cone_id - 3]; + } + } + }; + struct Triangle { size_t a, b, c; // indices of the cones that form the triangle - Triangle(size_t _a, size_t _b, size_t _c) + Triangle(const size_t _a, const size_t _b, const size_t _c) : a(_a) , b(_b) , c(_c) { } - bool contains(const Cone& cone, const ConeList& existing_cones) const + [[nodiscard]] bool contains(const Cone& cone) const { // check if cone is inside the triangle formed by cones a, b, c const Cone& cone_a = existing_cones[a]; @@ -37,9 +69,9 @@ class Triangulation { Triangle t; public: - bool is_intact() { return children.empty(); } + bool is_intact() const { return children.empty(); } - std::shared_ptr find_triangle(const Cone& cone, const ConeList& existing_cones) + std::shared_ptr find_triangle(const Cone& cone) { for (const auto& child : children) { if (child->t.contains(cone, existing_cones)) { @@ -52,60 +84,31 @@ class Triangulation { throw std::runtime_error("Cone is not contained in any triangle"); } TrianglePyramid() = delete; - TrianglePyramid(const Triangle& _t) + explicit TrianglePyramid(const Triangle& _t) : t(_t) { } - TrianglePyramid(size_t _a, size_t _b, size_t _c) + TrianglePyramid(const size_t _a, const size_t _b, const size_t _c) : t { _a, _b, _c } { } }; - class ConeList { - const std::optional> cones; - - public: - ConeList() - : cones(std::nullopt) - { - } - - void set_cones(std::span _cones) - { - cones.emplace(_cones); - } - - const Cone& operator[](const size_t cone_id) const - { - switch (cone_id) { - case 0: - return Cone { -100, -100, ConeColor::BLUE }; - case 1: - return Cone { 100, -100, ConeColor::YELLOW }; - case 2: - return Cone { 0, 100, ConeColor::BLUE }; - default: - return cones.value()[cone_id - 3]; - } - } - }; - TrianglePyramid root { 0, 1, 2 }; // super triangle that contains all cones ConeList existing_cones {}; /** * helpers */ - bool on_edge(const Cone& cone, const size_t a, const size_t b); - bool edge_detect(const Cone& cone, size_t& a, size_t& b, size_t& c); - void insert(const Cone& cone, const size_t cone_id); - void legalize_edge(const size_t new_cone, const size_t cone_a, const size_t cone_b); + [[nodiscard]] bool on_edge(const Cone& cone, size_t a, size_t b) const; + bool edge_detect(const Cone& cone, size_t& a, size_t& b, size_t& c) const; + void insert(const Cone& cone, size_t cone_id); + void legalize_edge(size_t new_cone, size_t cone_a, size_t cone_b); public: Triangulation() = default; - static std::unordered_map> adj { + std::unordered_map> adj { { 0, { 1, 2 } }, { 1, { 0, 2 } }, { 2, { 0, 1 } } }; // adj of the edges - void triangulate(const std::vector& cones); -} \ No newline at end of file + void update(const std::vector& cones); +}; From 597ccf66cdb3b1f96fe846e70813f4a70c7f6a59 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Mon, 16 Mar 2026 16:55:05 -0700 Subject: [PATCH 27/45] builds now --- auto_sim/test_controller_2/CMakePresets.json | 11 +- auto_sim/test_controller_2/pybind.cpp | 12 +- auto_sim/test_controller_2/triangulation.cpp | 123 ++++++++++--------- auto_sim/test_controller_2/triangulation.hpp | 19 ++- auto_sim/test_controller_2/types.hpp | 2 +- 5 files changed, 89 insertions(+), 78 deletions(-) diff --git a/auto_sim/test_controller_2/CMakePresets.json b/auto_sim/test_controller_2/CMakePresets.json index 7d04dd9..3276da1 100644 --- a/auto_sim/test_controller_2/CMakePresets.json +++ b/auto_sim/test_controller_2/CMakePresets.json @@ -2,13 +2,18 @@ "version": 10, "configurePresets": [ { - "name": "default", - "displayName": "", - "description": "", + "name": "default_win", "generator": "Visual Studio 17 2022", "binaryDir": "${sourceDir}/build", "cacheVariables": { } + }, + { + "name": "default_unix", + "generator": "Ninja", + "binaryDir": "${sourceDir}/build", + "cacheVariables": { + } } ] } \ No newline at end of file diff --git a/auto_sim/test_controller_2/pybind.cpp b/auto_sim/test_controller_2/pybind.cpp index 374d5e4..b4a8e00 100644 --- a/auto_sim/test_controller_2/pybind.cpp +++ b/auto_sim/test_controller_2/pybind.cpp @@ -23,21 +23,21 @@ PYBIND11_MODULE(Controller, m, py::mod_gil_not_used()) .def_readwrite("v_y", &VehicleState::v_y) .def_readwrite("omega", &VehicleState::omega); + py::enum_(m, "ConeColor") + .value("BLUE", ConeColor::BLUE) + .value("YELLOW", ConeColor::YELLOW) + .export_values(); + py::class_(m, "Cone") .def(py::init()) .def_readwrite("x", &Cone::x) .def_readwrite("y", &Cone::y) .def_readwrite("c", &Cone::c); - py::enum_(m, "ConeColor") - .value("BLUE", ConeColor::BLUE) - .value("YELLOW", ConeColor::YELLOW) - .export_values(); - // export object t (of type Triangulation) to python py::class_(m, "Triangulation") .def(py::init<>()) - .def("triangulate", &Triangulation::triangulate) + .def("triangulate", &Triangulation::update) .def_readonly("adj", &Triangulation::adj); m.def("get_triangulation", &get_triangulation, R"pbdoc( diff --git a/auto_sim/test_controller_2/triangulation.cpp b/auto_sim/test_controller_2/triangulation.cpp index 867e2e1..d68c126 100644 --- a/auto_sim/test_controller_2/triangulation.cpp +++ b/auto_sim/test_controller_2/triangulation.cpp @@ -1,64 +1,71 @@ #include "triangulation.hpp" -void Triangulation::legalize_edge(const size_t new_cone, const size_t cone_a, const size_t cone_b) -{ -} +void Triangulation::legalize_edge(const size_t new_cone, const size_t cone_a, + const size_t cone_b) {} -bool Triangulation::on_edge(const Cone& cone, const size_t a, const size_t b) const { - // check if cone lies on the edge (a,b) - const Cone& cone_a = existing_cones[a]; - const Cone& cone_b = existing_cones[b]; - // check if cone is collinear with cone_a and cone_b - if (const double cross_product = - (cone.y - cone_a.y) * (cone_b.x - cone_a.x) - - (cone.x - cone_a.x) * (cone_b.y - cone_a.y); - std::abs(cross_product) > 1e-6) { - return false; - } - // check if cone is within the bounding box of cone_a and cone_b - const double dot_product = (cone.x - cone_a.x) * (cone_b.x - cone_a.x) + (cone.y - cone_a.y) * (cone_b.y - cone_a.y); - if (dot_product < 0) { - return false; - } - const double squared_length_ab = (cone_b.x - cone_a.x) * (cone_b.x - cone_a.x) + (cone_b.y - cone_a.y) * (cone_b.y - cone_a.y); - if (dot_product > squared_length_ab) { - return false; - } - return true; +bool Triangulation::on_edge(const Cone &cone, const size_t a, + const size_t b) const { + // check if cone lies on the edge (a,b) + const Cone &cone_a = existing_cones[a]; + const Cone &cone_b = existing_cones[b]; + // check if cone is collinear with cone_a and cone_b + if (const double cross_product = (cone.y - cone_a.y) * (cone_b.x - cone_a.x) - + (cone.x - cone_a.x) * (cone_b.y - cone_a.y); + std::abs(cross_product) > 1e-6) { + return false; + } + // check if cone is within the bounding box of cone_a and cone_b + const double dot_product = (cone.x - cone_a.x) * (cone_b.x - cone_a.x) + + (cone.y - cone_a.y) * (cone_b.y - cone_a.y); + if (dot_product < 0) { + return false; + } + const double squared_length_ab = + (cone_b.x - cone_a.x) * (cone_b.x - cone_a.x) + + (cone_b.y - cone_a.y) * (cone_b.y - cone_a.y); + if (dot_product > squared_length_ab) { + return false; + } + return true; } -bool Triangulation::edge_detect(const Cone& cone, size_t& a, size_t& b, size_t& c) const { - if (on_edge(cone, a, b)) { - return true; - } - if (on_edge(cone, b, c)) { - std::swap(a, c); - return true; - } - if (on_edge(cone, a, c)) { - std::swap(b, c); - return true; - } - return false; +bool Triangulation::edge_detect(const Cone &cone, size_t &a, size_t &b, + size_t &c) const { + if (on_edge(cone, a, b)) { + return true; + } + if (on_edge(cone, b, c)) { + std::swap(a, c); + return true; + } + if (on_edge(cone, a, c)) { + std::swap(b, c); + return true; + } + return false; } -void Triangulation::insert(const Cone& cone, const size_t cone_id) -{ - std::shared_ptr t = root.find_triangle(cone); // find smallest triangle which contains the cone +void Triangulation::insert(const Cone &cone, const size_t cone_id) { + // find smallest triangle which contains the cone + const std::shared_ptr t = + root.find_triangle(cone, existing_cones); + size_t a = t->t.a, b = t->t.b, c = t->t.c; if (bool on_edge = edge_detect(cone, a, b, c)) { - // if on_edge, then cone is known to be on edge (a,b) - // very annoying - throw std::runtime_error("Edge splitting not implemented yet"); // inshallah we will never need to implement this - } + // if on_edge, then cone is known to be on edge (a,b) + // very annoying + throw std::runtime_error( + "Edge splitting not implemented yet"); // inshallah we will never need + // to implement this + } // split triangle a,b,c into three triangles a,b,cone_id; // b,c,cone_id; a,c,cone_id adj[cone_id] = {a, b, c}; adj[a].push_back(cone_id); adj[b].push_back(cone_id); adj[c].push_back(cone_id); - t.children.push_back(std::make_shared(a, b, cone_id)); - t.children.push_back(std::make_shared(b, c, cone_id)); - t.children.push_back(std::make_shared(a, c, cone_id)); + t->children.push_back(std::make_shared(a, b, cone_id)); + t->children.push_back(std::make_shared(b, c, cone_id)); + t->children.push_back(std::make_shared(a, c, cone_id)); // legalize the edges legalize_edge(cone_id, a, b); @@ -73,15 +80,15 @@ void Triangulation::insert(const Cone& cone, const size_t cone_id) * @note This function assumes that the input vector of cones is an add-only * array whose order is preserved. */ -void Triangulation::update(const std::vector& cones) -{ - static size_t last_seen_cone = 0; - for (; last_seen_cone < cones.size(); ++last_seen_cone) { - const Cone& cone = cones[last_seen_cone]; - // process the new cone - const size_t cone_id = last_seen_cone + 3; // cones 0,1,2 are reserved for the super triangle - const std::span s { cones }; - existing_cones.set_cones(s.first(last_seen_cone)); - insert(cone, cone_id); - } +void Triangulation::update(const std::vector &cones) { + static size_t last_seen_cone = 0; + for (; last_seen_cone < cones.size(); ++last_seen_cone) { + const Cone &cone = cones[last_seen_cone]; + // process the new cone + const size_t cone_id = + last_seen_cone + 3; // cones 0,1,2 are reserved for the super triangle + const std::span s{cones}; + existing_cones.set_cones(s.first(last_seen_cone)); + insert(cone, cone_id); + } } \ No newline at end of file diff --git a/auto_sim/test_controller_2/triangulation.hpp b/auto_sim/test_controller_2/triangulation.hpp index d45ca8d..370925e 100644 --- a/auto_sim/test_controller_2/triangulation.hpp +++ b/auto_sim/test_controller_2/triangulation.hpp @@ -50,12 +50,12 @@ class Triangulation { , c(_c) { } - [[nodiscard]] bool contains(const Cone& cone) const + [[nodiscard]] bool contains(const Cone& cone, const ConeList& cones) const { // check if cone is inside the triangle formed by cones a, b, c - const Cone& cone_a = existing_cones[a]; - const Cone& cone_b = existing_cones[b]; - const Cone& cone_c = existing_cones[c]; + const Cone& cone_a = cones[a]; + const Cone& cone_b = cones[b]; + const Cone& cone_c = cones[c]; const double area_abc = 0.5 * std::abs(cone_a.x * (cone_b.y - cone_c.y) + cone_b.x * (cone_c.y - cone_a.y) + cone_c.x * (cone_a.y - cone_b.y)); const double area_abp = 0.5 * std::abs(cone_a.x * (cone_b.y - cone.y) + cone_b.x * (cone.y - cone_a.y) + cone.x * (cone_a.y - cone_b.y)); const double area_acp = 0.5 * std::abs(cone_a.x * (cone_c.y - cone.y) + cone_c.x * (cone.y - cone_a.y) + cone.x * (cone_a.y - cone_c.y)); @@ -65,20 +65,19 @@ class Triangulation { }; class TrianglePyramid { + public: std::vector> children {}; Triangle t; + [[nodiscard]] bool is_intact() const { return children.empty(); } - public: - bool is_intact() const { return children.empty(); } - - std::shared_ptr find_triangle(const Cone& cone) + [[nodiscard]] std::shared_ptr find_triangle(const Cone& cone, const ConeList& cones) const { for (const auto& child : children) { - if (child->t.contains(cone, existing_cones)) { + if (child->t.contains(cone, cones)) { if (child->is_intact()) { return child; } - return child->find_triangle(cone, existing_cones); + return child->find_triangle(cone, cones); } } throw std::runtime_error("Cone is not contained in any triangle"); diff --git a/auto_sim/test_controller_2/types.hpp b/auto_sim/test_controller_2/types.hpp index 46aa263..d197a12 100644 --- a/auto_sim/test_controller_2/types.hpp +++ b/auto_sim/test_controller_2/types.hpp @@ -27,5 +27,5 @@ struct Cone { double x; double y; ConeColor c; - Cone(double _x, double _y, ConeColor _c) : x(_x), y(_y), c(_c) {} + Cone(const double _x, const double _y, const ConeColor _c) : x(_x), y(_y), c(_c) {} }; \ No newline at end of file From 663584e83493e5b9c19dccdd27ebb77efc2201a1 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Mon, 16 Mar 2026 16:56:29 -0700 Subject: [PATCH 28/45] tutorial --- auto_sim/test_controller_2/README.md | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100644 auto_sim/test_controller_2/README.md diff --git a/auto_sim/test_controller_2/README.md b/auto_sim/test_controller_2/README.md new file mode 100644 index 0000000..9a603c5 --- /dev/null +++ b/auto_sim/test_controller_2/README.md @@ -0,0 +1,9 @@ +# Development +To develop, set up as per usual as a CMake project. + +# Installation +```bash +pip install ./test_controller_2 +``` + +This will trigger setup.py which will automatically install the python package into the environment of your choosing \ No newline at end of file From 2d15059c9bdf10056a580ce8a68996a818eaea31 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Mon, 16 Mar 2026 16:57:12 -0700 Subject: [PATCH 29/45] wow --- auto_sim/.gitignore | 3 ++- auto_sim/test_controller_2/pybind.cpp | 1 - 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/auto_sim/.gitignore b/auto_sim/.gitignore index eb50f59..4c6891f 100644 --- a/auto_sim/.gitignore +++ b/auto_sim/.gitignore @@ -1,4 +1,5 @@ __pycache__ build *.egg-info -labels \ No newline at end of file +labels +.idea \ No newline at end of file diff --git a/auto_sim/test_controller_2/pybind.cpp b/auto_sim/test_controller_2/pybind.cpp index b4a8e00..a3c0d81 100644 --- a/auto_sim/test_controller_2/pybind.cpp +++ b/auto_sim/test_controller_2/pybind.cpp @@ -37,7 +37,6 @@ PYBIND11_MODULE(Controller, m, py::mod_gil_not_used()) // export object t (of type Triangulation) to python py::class_(m, "Triangulation") .def(py::init<>()) - .def("triangulate", &Triangulation::update) .def_readonly("adj", &Triangulation::adj); m.def("get_triangulation", &get_triangulation, R"pbdoc( From 83fe47ed8760a0842ccdab0aae5bf79809808223 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Mon, 16 Mar 2026 17:07:33 -0700 Subject: [PATCH 30/45] w formatting --- auto_sim/test_controller_2/.clang-format | 2 + auto_sim/test_controller_2/triangulation.cpp | 146 +++++++++---------- 2 files changed, 71 insertions(+), 77 deletions(-) create mode 100644 auto_sim/test_controller_2/.clang-format diff --git a/auto_sim/test_controller_2/.clang-format b/auto_sim/test_controller_2/.clang-format new file mode 100644 index 0000000..d1078a7 --- /dev/null +++ b/auto_sim/test_controller_2/.clang-format @@ -0,0 +1,2 @@ +BasedOnStyle: WebKit +BreakBeforeBraces: Attach diff --git a/auto_sim/test_controller_2/triangulation.cpp b/auto_sim/test_controller_2/triangulation.cpp index d68c126..c62505f 100644 --- a/auto_sim/test_controller_2/triangulation.cpp +++ b/auto_sim/test_controller_2/triangulation.cpp @@ -1,76 +1,69 @@ #include "triangulation.hpp" -void Triangulation::legalize_edge(const size_t new_cone, const size_t cone_a, - const size_t cone_b) {} - -bool Triangulation::on_edge(const Cone &cone, const size_t a, - const size_t b) const { - // check if cone lies on the edge (a,b) - const Cone &cone_a = existing_cones[a]; - const Cone &cone_b = existing_cones[b]; - // check if cone is collinear with cone_a and cone_b - if (const double cross_product = (cone.y - cone_a.y) * (cone_b.x - cone_a.x) - - (cone.x - cone_a.x) * (cone_b.y - cone_a.y); - std::abs(cross_product) > 1e-6) { - return false; - } - // check if cone is within the bounding box of cone_a and cone_b - const double dot_product = (cone.x - cone_a.x) * (cone_b.x - cone_a.x) + - (cone.y - cone_a.y) * (cone_b.y - cone_a.y); - if (dot_product < 0) { - return false; - } - const double squared_length_ab = - (cone_b.x - cone_a.x) * (cone_b.x - cone_a.x) + - (cone_b.y - cone_a.y) * (cone_b.y - cone_a.y); - if (dot_product > squared_length_ab) { - return false; - } - return true; +void Triangulation::legalize_edge(const size_t new_cone, const size_t cone_a, const size_t cone_b) { } -bool Triangulation::edge_detect(const Cone &cone, size_t &a, size_t &b, - size_t &c) const { - if (on_edge(cone, a, b)) { - return true; - } - if (on_edge(cone, b, c)) { - std::swap(a, c); +bool Triangulation::on_edge(const Cone& cone, const size_t a, const size_t b) const { + // check if cone lies on the edge (a,b) + const Cone& cone_a = existing_cones[a]; + const Cone& cone_b = existing_cones[b]; + // check if cone is collinear with cone_a and cone_b + if (const double cross_product = (cone.y - cone_a.y) * (cone_b.x - cone_a.x) - (cone.x - cone_a.x) * (cone_b.y - cone_a.y); + std::abs(cross_product) > 1e-6) { + return false; + } + // check if cone is within the bounding box of cone_a and cone_b + const double dot_product = (cone.x - cone_a.x) * (cone_b.x - cone_a.x) + (cone.y - cone_a.y) * (cone_b.y - cone_a.y); + if (dot_product < 0) { + return false; + } + const double squared_length_ab = (cone_b.x - cone_a.x) * (cone_b.x - cone_a.x) + (cone_b.y - cone_a.y) * (cone_b.y - cone_a.y); + if (dot_product > squared_length_ab) { + return false; + } return true; - } - if (on_edge(cone, a, c)) { - std::swap(b, c); - return true; - } - return false; } -void Triangulation::insert(const Cone &cone, const size_t cone_id) { - // find smallest triangle which contains the cone - const std::shared_ptr t = - root.find_triangle(cone, existing_cones); - size_t a = t->t.a, b = t->t.b, c = t->t.c; - if (bool on_edge = edge_detect(cone, a, b, c)) { - // if on_edge, then cone is known to be on edge (a,b) - // very annoying - throw std::runtime_error( - "Edge splitting not implemented yet"); // inshallah we will never need - // to implement this - } - // split triangle a,b,c into three triangles a,b,cone_id; - // b,c,cone_id; a,c,cone_id - adj[cone_id] = {a, b, c}; - adj[a].push_back(cone_id); - adj[b].push_back(cone_id); - adj[c].push_back(cone_id); - t->children.push_back(std::make_shared(a, b, cone_id)); - t->children.push_back(std::make_shared(b, c, cone_id)); - t->children.push_back(std::make_shared(a, c, cone_id)); +bool Triangulation::edge_detect(const Cone& cone, size_t& a, size_t& b, size_t& c) const { + if (on_edge(cone, a, b)) { + return true; + } + if (on_edge(cone, b, c)) { + std::swap(a, c); + return true; + } + if (on_edge(cone, a, c)) { + std::swap(b, c); + return true; + } + return false; +} - // legalize the edges - legalize_edge(cone_id, a, b); - legalize_edge(cone_id, b, c); - legalize_edge(cone_id, a, c); +void Triangulation::insert(const Cone& cone, const size_t cone_id) { + // find smallest triangle which contains the cone + const std::shared_ptr t = root.find_triangle(cone, existing_cones); + size_t a = t->t.a, b = t->t.b, c = t->t.c; + if (bool on_edge = edge_detect(cone, a, b, c)) { + // if on_edge, then cone is known to be on edge (a,b) + // very annoying + throw std::runtime_error( + "Edge splitting not implemented yet"); // inshallah we will never need + // to implement this + } + // split triangle a,b,c into three triangles a,b,cone_id; + // b,c,cone_id; a,c,cone_id + adj[cone_id] = { a, b, c }; + adj[a].push_back(cone_id); + adj[b].push_back(cone_id); + adj[c].push_back(cone_id); + t->children.push_back(std::make_shared(a, b, cone_id)); + t->children.push_back(std::make_shared(b, c, cone_id)); + t->children.push_back(std::make_shared(a, c, cone_id)); + + // legalize the edges + legalize_edge(cone_id, a, b); + legalize_edge(cone_id, b, c); + legalize_edge(cone_id, a, c); } /** @@ -80,15 +73,14 @@ void Triangulation::insert(const Cone &cone, const size_t cone_id) { * @note This function assumes that the input vector of cones is an add-only * array whose order is preserved. */ -void Triangulation::update(const std::vector &cones) { - static size_t last_seen_cone = 0; - for (; last_seen_cone < cones.size(); ++last_seen_cone) { - const Cone &cone = cones[last_seen_cone]; - // process the new cone - const size_t cone_id = - last_seen_cone + 3; // cones 0,1,2 are reserved for the super triangle - const std::span s{cones}; - existing_cones.set_cones(s.first(last_seen_cone)); - insert(cone, cone_id); - } -} \ No newline at end of file +void Triangulation::update(const std::vector& cones) { + static size_t last_seen_cone = 0; + for (; last_seen_cone < cones.size(); ++last_seen_cone) { + const Cone& cone = cones[last_seen_cone]; + // process the new cone + const size_t cone_id = last_seen_cone + 3; // cones 0,1,2 are reserved for the super triangle + const std::span s { cones }; + existing_cones.set_cones(s.first(last_seen_cone)); + insert(cone, cone_id); + } +} From 10bca0e61a34f986fc31f676abf50f241951c80b Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Mon, 16 Mar 2026 17:10:10 -0700 Subject: [PATCH 31/45] more format things --- auto_sim/test_controller_2/.clang-format | 1 + auto_sim/test_controller_2/triangulation.hpp | 37 ++++++-------------- 2 files changed, 12 insertions(+), 26 deletions(-) diff --git a/auto_sim/test_controller_2/.clang-format b/auto_sim/test_controller_2/.clang-format index d1078a7..8b0cf95 100644 --- a/auto_sim/test_controller_2/.clang-format +++ b/auto_sim/test_controller_2/.clang-format @@ -1,2 +1,3 @@ BasedOnStyle: WebKit BreakBeforeBraces: Attach +PackConstructorInitializers: NextLine diff --git a/auto_sim/test_controller_2/triangulation.hpp b/auto_sim/test_controller_2/triangulation.hpp index 370925e..9b35be9 100644 --- a/auto_sim/test_controller_2/triangulation.hpp +++ b/auto_sim/test_controller_2/triangulation.hpp @@ -14,19 +14,15 @@ class Triangulation { std::optional> cones; public: - ConeList() - : cones(std::nullopt) - { + ConeList() : cones(std::nullopt) { } - void set_cones(std::span _cones) - { + void set_cones(std::span _cones) { cones.emplace(_cones); } - const Cone& operator[](const size_t cone_id) const - { - static const Cone cone0{ -100, -100, ConeColor::BLUE }, + const Cone& operator[](const size_t cone_id) const { + static const Cone cone0 { -100, -100, ConeColor::BLUE }, cone1 = { 100, -100, ConeColor::YELLOW }, cone2 = { 0, 100, ConeColor::BLUE }; switch (cone_id) { @@ -44,14 +40,8 @@ class Triangulation { struct Triangle { size_t a, b, c; // indices of the cones that form the triangle - Triangle(const size_t _a, const size_t _b, const size_t _c) - : a(_a) - , b(_b) - , c(_c) - { - } - [[nodiscard]] bool contains(const Cone& cone, const ConeList& cones) const - { + Triangle(const size_t _a, const size_t _b, const size_t _c) : a(_a), b(_b), c(_c) { } + [[nodiscard]] bool contains(const Cone& cone, const ConeList& cones) const { // check if cone is inside the triangle formed by cones a, b, c const Cone& cone_a = cones[a]; const Cone& cone_b = cones[b]; @@ -66,12 +56,11 @@ class Triangulation { class TrianglePyramid { public: - std::vector> children {}; + std::vector> children { }; Triangle t; [[nodiscard]] bool is_intact() const { return children.empty(); } - [[nodiscard]] std::shared_ptr find_triangle(const Cone& cone, const ConeList& cones) const - { + [[nodiscard]] std::shared_ptr find_triangle(const Cone& cone, const ConeList& cones) const { for (const auto& child : children) { if (child->t.contains(cone, cones)) { if (child->is_intact()) { @@ -83,18 +72,14 @@ class Triangulation { throw std::runtime_error("Cone is not contained in any triangle"); } TrianglePyramid() = delete; - explicit TrianglePyramid(const Triangle& _t) - : t(_t) - { + explicit TrianglePyramid(const Triangle& _t) : t(_t) { } - TrianglePyramid(const size_t _a, const size_t _b, const size_t _c) - : t { _a, _b, _c } - { + TrianglePyramid(const size_t _a, const size_t _b, const size_t _c) : t { _a, _b, _c } { } }; TrianglePyramid root { 0, 1, 2 }; // super triangle that contains all cones - ConeList existing_cones {}; + ConeList existing_cones { }; /** * helpers From 00c88de373e81550afb366da780603aad03617a4 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 26 Mar 2026 22:08:12 -0700 Subject: [PATCH 32/45] W triangulation libary --- auto_sim/render.py | 9 +- auto_sim/test_controller_2/CMakeLists.txt | 26 ++++-- auto_sim/test_controller_2/controller.cpp | 31 +++++-- auto_sim/test_controller_2/controller.hpp | 4 +- auto_sim/test_controller_2/pybind.cpp | 14 ++- auto_sim/test_controller_2/setup.py | 6 +- auto_sim/test_controller_2/triangulation.cpp | 86 ----------------- auto_sim/test_controller_2/triangulation.hpp | 98 -------------------- auto_sim/test_controller_2/types.hpp | 48 +++++----- 9 files changed, 87 insertions(+), 235 deletions(-) delete mode 100644 auto_sim/test_controller_2/triangulation.cpp delete mode 100644 auto_sim/test_controller_2/triangulation.hpp diff --git a/auto_sim/render.py b/auto_sim/render.py index 4400bca..fd461da 100644 --- a/auto_sim/render.py +++ b/auto_sim/render.py @@ -1,7 +1,7 @@ import pygame from scipy.spatial.transform import Rotation as R from constants import VEHICLE_WIDTH_M -from Controller import VehicleState, Cone, ConeColor +from Controller import VehicleState, Cone, ConeColor, get_triangulation from math import ceil, degrees import numpy as np @@ -95,6 +95,13 @@ def render_world(vehicle_state: VehicleState, cones: list[Cone], screen: pygame. screen, int_to_color(cone.c), transform(cone.x, cone.y, vehicle_state), 0.2 * PIXELS_PER_M ) + + for edge in get_triangulation(): + v1, v2 = edge.v1(), edge.v2() + pygame.draw.line(screen, "white", + transform(cones[v1].x, cones[v1].y, vehicle_state), + transform(cones[v2].x, cones[v2].y, vehicle_state), 2 + ) car_rotated = pygame.transform.rotate(car,-90) scale_factor = VEHICLE_WIDTH_M / car_rotated.get_width() * PIXELS_PER_M diff --git a/auto_sim/test_controller_2/CMakeLists.txt b/auto_sim/test_controller_2/CMakeLists.txt index ae6b49c..d2012aa 100644 --- a/auto_sim/test_controller_2/CMakeLists.txt +++ b/auto_sim/test_controller_2/CMakeLists.txt @@ -3,20 +3,28 @@ set(CMAKE_CXX_STANDARD 23) project(high_level_controller) file( - DOWNLOAD - https://github.com/cpm-cmake/CPM.cmake/releases/download/v0.40.8/CPM.cmake - ${CMAKE_CURRENT_BINARY_DIR}/cmake/CPM.cmake - EXPECTED_HASH SHA256=78ba32abdf798bc616bab7c73aac32a17bbd7b06ad9e26a6add69de8f3ae4791 + DOWNLOAD + https://github.com/cpm-cmake/CPM.cmake/releases/download/v0.40.8/CPM.cmake + ${CMAKE_CURRENT_BINARY_DIR}/cmake/CPM.cmake + EXPECTED_HASH SHA256=78ba32abdf798bc616bab7c73aac32a17bbd7b06ad9e26a6add69de8f3ae4791 ) include(${CMAKE_CURRENT_BINARY_DIR}/cmake/CPM.cmake) CPMAddPackage( - NAME pybind11 - GIT_REPOSITORY https://github.com/pybind/pybind11.git - VERSION 3.0.2 - DOWNLOAD_ONLY TRUE + NAME pybind11 + GIT_REPOSITORY https://github.com/pybind/pybind11.git + VERSION 3.0.2 + DOWNLOAD_ONLY TRUE ) add_subdirectory(${pybind11_SOURCE_DIR}) +CPMAddPackage( + NAME CDT + GIT_REPOSITORY https://github.com/artem-ogre/CDT.git + GIT_TAG 1.4.4 + DOWNLOAD_ONLY TRUE +) + file(GLOB SOURCES *.cpp) -pybind11_add_module(controller ${SOURCES}) \ No newline at end of file +pybind11_add_module(controller ${SOURCES}) +target_include_directories(controller PRIVATE ${CDT_SOURCE_DIR}/CDT/include) \ No newline at end of file diff --git a/auto_sim/test_controller_2/controller.cpp b/auto_sim/test_controller_2/controller.cpp index 714d9e9..019603b 100644 --- a/auto_sim/test_controller_2/controller.cpp +++ b/auto_sim/test_controller_2/controller.cpp @@ -1,16 +1,33 @@ #include "controller.hpp" -Triangulation t {}; +#include +#include +#include -Triangulation& get_triangulation() -{ - return t; +static CDT::Triangulation cdt; +static CDT::EdgeUSet edges; + +const CDT::EdgeUSet& get_triangulation() { + return edges; } -ControlOutput compute(const VehicleState& ve, const std::vector& cones) -{ +ControlOutput compute(const VehicleState& ve, const std::vector& cones) { // Implementation for compute function - t.update(cones); + static size_t seen_points = 0; + if (seen_points < cones.size()) { + const auto start = std::next( + cones.begin(), + static_cast::difference_type>(std::min(seen_points, cones.size()))); + cdt.insertVertices(start, cones.end(), [](const Cone& c) { return c.x; }, [](const Cone& c) { return c.y; }); + // CDT::Triangulation cpy = cdt; + // cpy.eraseOuterTriangles(); + // std::cout << "number of vertices in cpy " << cpy.vertices.size() << std::endl; + cdt.eraseSuperTriangle(); + edges = CDT::extractEdgesFromTriangles(cdt.triangles); + std::cout << "number of edges " << edges.size() << std::endl; + seen_points = cones.size(); + } + // cdt.eraseSuperTriangle(); // t.adj will contain the adj list return { 0, 0 }; } \ No newline at end of file diff --git a/auto_sim/test_controller_2/controller.hpp b/auto_sim/test_controller_2/controller.hpp index f0a3e85..c6f9f31 100644 --- a/auto_sim/test_controller_2/controller.hpp +++ b/auto_sim/test_controller_2/controller.hpp @@ -1,7 +1,7 @@ #pragma once -#include "triangulation.hpp" +#include "CDT.h" #include "types.hpp" #include -Triangulation& get_triangulation(); +const CDT::EdgeUSet& get_triangulation(); ControlOutput compute(const VehicleState& ve, const std::vector& cones); \ No newline at end of file diff --git a/auto_sim/test_controller_2/pybind.cpp b/auto_sim/test_controller_2/pybind.cpp index a3c0d81..d1446a6 100644 --- a/auto_sim/test_controller_2/pybind.cpp +++ b/auto_sim/test_controller_2/pybind.cpp @@ -7,8 +7,7 @@ namespace py = pybind11; -PYBIND11_MODULE(Controller, m, py::mod_gil_not_used()) -{ +PYBIND11_MODULE(Controller, m, py::mod_gil_not_used()) { py::class_(m, "ControlOutput") .def(py::init()) .def_readwrite("ax", &ControlOutput::ax) @@ -34,15 +33,14 @@ PYBIND11_MODULE(Controller, m, py::mod_gil_not_used()) .def_readwrite("y", &Cone::y) .def_readwrite("c", &Cone::c); - // export object t (of type Triangulation) to python - py::class_(m, "Triangulation") - .def(py::init<>()) - .def_readonly("adj", &Triangulation::adj); - - m.def("get_triangulation", &get_triangulation, R"pbdoc( + m.def("get_triangulation", &get_triangulation, py::return_value_policy::reference, R"pbdoc( Get the current triangulation object) )pbdoc"); + py::class_(m, "Edge") + .def("v1", &CDT::Edge::v1) + .def("v2", &CDT::Edge::v2); + m.def("compute", &compute, R"pbdoc( Compute control output )pbdoc"); diff --git a/auto_sim/test_controller_2/setup.py b/auto_sim/test_controller_2/setup.py index 886aa8c..0fe06e2 100644 --- a/auto_sim/test_controller_2/setup.py +++ b/auto_sim/test_controller_2/setup.py @@ -7,8 +7,8 @@ ext_modules = [ Pybind11Extension( "Controller", - ["pybind.cpp", "triangulation.cpp", "sim.cpp", "controller.cpp"], - include_dirs=["."], + ["pybind.cpp", "sim.cpp", "controller.cpp"], + include_dirs=[".", "build/_deps/cdt-src/CDT/include"], language="c++", ), ] @@ -28,4 +28,4 @@ cmdclass={"build_ext": build_ext}, zip_safe=False, python_requires=">=3.9", -) \ No newline at end of file +) diff --git a/auto_sim/test_controller_2/triangulation.cpp b/auto_sim/test_controller_2/triangulation.cpp deleted file mode 100644 index c62505f..0000000 --- a/auto_sim/test_controller_2/triangulation.cpp +++ /dev/null @@ -1,86 +0,0 @@ -#include "triangulation.hpp" - -void Triangulation::legalize_edge(const size_t new_cone, const size_t cone_a, const size_t cone_b) { -} - -bool Triangulation::on_edge(const Cone& cone, const size_t a, const size_t b) const { - // check if cone lies on the edge (a,b) - const Cone& cone_a = existing_cones[a]; - const Cone& cone_b = existing_cones[b]; - // check if cone is collinear with cone_a and cone_b - if (const double cross_product = (cone.y - cone_a.y) * (cone_b.x - cone_a.x) - (cone.x - cone_a.x) * (cone_b.y - cone_a.y); - std::abs(cross_product) > 1e-6) { - return false; - } - // check if cone is within the bounding box of cone_a and cone_b - const double dot_product = (cone.x - cone_a.x) * (cone_b.x - cone_a.x) + (cone.y - cone_a.y) * (cone_b.y - cone_a.y); - if (dot_product < 0) { - return false; - } - const double squared_length_ab = (cone_b.x - cone_a.x) * (cone_b.x - cone_a.x) + (cone_b.y - cone_a.y) * (cone_b.y - cone_a.y); - if (dot_product > squared_length_ab) { - return false; - } - return true; -} - -bool Triangulation::edge_detect(const Cone& cone, size_t& a, size_t& b, size_t& c) const { - if (on_edge(cone, a, b)) { - return true; - } - if (on_edge(cone, b, c)) { - std::swap(a, c); - return true; - } - if (on_edge(cone, a, c)) { - std::swap(b, c); - return true; - } - return false; -} - -void Triangulation::insert(const Cone& cone, const size_t cone_id) { - // find smallest triangle which contains the cone - const std::shared_ptr t = root.find_triangle(cone, existing_cones); - size_t a = t->t.a, b = t->t.b, c = t->t.c; - if (bool on_edge = edge_detect(cone, a, b, c)) { - // if on_edge, then cone is known to be on edge (a,b) - // very annoying - throw std::runtime_error( - "Edge splitting not implemented yet"); // inshallah we will never need - // to implement this - } - // split triangle a,b,c into three triangles a,b,cone_id; - // b,c,cone_id; a,c,cone_id - adj[cone_id] = { a, b, c }; - adj[a].push_back(cone_id); - adj[b].push_back(cone_id); - adj[c].push_back(cone_id); - t->children.push_back(std::make_shared(a, b, cone_id)); - t->children.push_back(std::make_shared(b, c, cone_id)); - t->children.push_back(std::make_shared(a, c, cone_id)); - - // legalize the edges - legalize_edge(cone_id, a, b); - legalize_edge(cone_id, b, c); - legalize_edge(cone_id, a, c); -} - -/** - * Triangulates the cones. Populates the adj with the edges of the - * triangulation. - * @param cones The vector of cones to triangulate. - * @note This function assumes that the input vector of cones is an add-only - * array whose order is preserved. - */ -void Triangulation::update(const std::vector& cones) { - static size_t last_seen_cone = 0; - for (; last_seen_cone < cones.size(); ++last_seen_cone) { - const Cone& cone = cones[last_seen_cone]; - // process the new cone - const size_t cone_id = last_seen_cone + 3; // cones 0,1,2 are reserved for the super triangle - const std::span s { cones }; - existing_cones.set_cones(s.first(last_seen_cone)); - insert(cone, cone_id); - } -} diff --git a/auto_sim/test_controller_2/triangulation.hpp b/auto_sim/test_controller_2/triangulation.hpp deleted file mode 100644 index 9b35be9..0000000 --- a/auto_sim/test_controller_2/triangulation.hpp +++ /dev/null @@ -1,98 +0,0 @@ -#pragma once -#include "types.hpp" -#include -#include -#include -#include -#include -#include - -// inspired by https://youtu.be/1TUUevxkvp4 - -class Triangulation { - class ConeList { - std::optional> cones; - - public: - ConeList() : cones(std::nullopt) { - } - - void set_cones(std::span _cones) { - cones.emplace(_cones); - } - - const Cone& operator[](const size_t cone_id) const { - static const Cone cone0 { -100, -100, ConeColor::BLUE }, - cone1 = { 100, -100, ConeColor::YELLOW }, - cone2 = { 0, 100, ConeColor::BLUE }; - switch (cone_id) { - case 0: - return cone0; - case 1: - return cone1; - case 2: - return cone2; - default: - return cones.value()[cone_id - 3]; - } - } - }; - - struct Triangle { - size_t a, b, c; // indices of the cones that form the triangle - Triangle(const size_t _a, const size_t _b, const size_t _c) : a(_a), b(_b), c(_c) { } - [[nodiscard]] bool contains(const Cone& cone, const ConeList& cones) const { - // check if cone is inside the triangle formed by cones a, b, c - const Cone& cone_a = cones[a]; - const Cone& cone_b = cones[b]; - const Cone& cone_c = cones[c]; - const double area_abc = 0.5 * std::abs(cone_a.x * (cone_b.y - cone_c.y) + cone_b.x * (cone_c.y - cone_a.y) + cone_c.x * (cone_a.y - cone_b.y)); - const double area_abp = 0.5 * std::abs(cone_a.x * (cone_b.y - cone.y) + cone_b.x * (cone.y - cone_a.y) + cone.x * (cone_a.y - cone_b.y)); - const double area_acp = 0.5 * std::abs(cone_a.x * (cone_c.y - cone.y) + cone_c.x * (cone.y - cone_a.y) + cone.x * (cone_a.y - cone_c.y)); - const double area_bcp = 0.5 * std::abs(cone_b.x * (cone_c.y - cone.y) + cone_c.x * (cone.y - cone_b.y) + cone.x * (cone_b.y - cone_c.y)); - return std::abs(area_abc - area_abp - area_acp - area_bcp) < 1e-6; - } - }; - - class TrianglePyramid { - public: - std::vector> children { }; - Triangle t; - [[nodiscard]] bool is_intact() const { return children.empty(); } - - [[nodiscard]] std::shared_ptr find_triangle(const Cone& cone, const ConeList& cones) const { - for (const auto& child : children) { - if (child->t.contains(cone, cones)) { - if (child->is_intact()) { - return child; - } - return child->find_triangle(cone, cones); - } - } - throw std::runtime_error("Cone is not contained in any triangle"); - } - TrianglePyramid() = delete; - explicit TrianglePyramid(const Triangle& _t) : t(_t) { - } - TrianglePyramid(const size_t _a, const size_t _b, const size_t _c) : t { _a, _b, _c } { - } - }; - - TrianglePyramid root { 0, 1, 2 }; // super triangle that contains all cones - ConeList existing_cones { }; - - /** - * helpers - */ - [[nodiscard]] bool on_edge(const Cone& cone, size_t a, size_t b) const; - bool edge_detect(const Cone& cone, size_t& a, size_t& b, size_t& c) const; - void insert(const Cone& cone, size_t cone_id); - void legalize_edge(size_t new_cone, size_t cone_a, size_t cone_b); - -public: - Triangulation() = default; - std::unordered_map> adj { - { 0, { 1, 2 } }, { 1, { 0, 2 } }, { 2, { 0, 1 } } - }; // adj of the edges - void update(const std::vector& cones); -}; diff --git a/auto_sim/test_controller_2/types.hpp b/auto_sim/test_controller_2/types.hpp index d197a12..07b1669 100644 --- a/auto_sim/test_controller_2/types.hpp +++ b/auto_sim/test_controller_2/types.hpp @@ -1,31 +1,37 @@ #pragma once struct ControlOutput { - // in local frame - double ax; - // no frame - double omega_dot; - ControlOutput(const double _ax, const double _omega_dot) - : ax(_ax), omega_dot(_omega_dot) {} + // in local frame + double ax; + // no frame + double omega_dot; + ControlOutput(const double _ax, const double _omega_dot) + : ax(_ax), omega_dot(_omega_dot) { } }; struct VehicleState { - // in global frame - double x; - double y; - double theta; - // in local frame - double v_x; - double v_y; - // no frame - double omega; - VehicleState() : x(0), y(0), theta(0), v_x(0), v_y(0), omega(0) {} + // in global frame + double x; + double y; + double theta; + // in local frame + double v_x; + double v_y; + // no frame + double omega; + VehicleState() + : x(0), y(0), theta(0), v_x(0), v_y(0), omega(0) { } }; -enum class ConeColor { BLUE, YELLOW }; +enum class ConeColor { + BLUE, + YELLOW, + ORANGE, +}; struct Cone { - double x; - double y; - ConeColor c; - Cone(const double _x, const double _y, const ConeColor _c) : x(_x), y(_y), c(_c) {} + double x; + double y; + ConeColor c; + Cone(const double _x, const double _y, const ConeColor _c) + : x(_x), y(_y), c(_c) { } }; \ No newline at end of file From 93fd08fd457e1fc7051727e650f7ca820677534a Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 26 Mar 2026 22:09:43 -0700 Subject: [PATCH 33/45] 67 --- auto_sim/render.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/auto_sim/render.py b/auto_sim/render.py index fd461da..845ac01 100644 --- a/auto_sim/render.py +++ b/auto_sim/render.py @@ -90,18 +90,19 @@ def render_world(vehicle_state: VehicleState, cones: list[Cone], screen: pygame. p = transform(0, 0, vehicle_state) pygame.draw.circle(screen, "white",p , 5) - for cone in cones: - pygame.draw.circle( - screen, int_to_color(cone.c), - transform(cone.x, cone.y, vehicle_state), 0.2 * PIXELS_PER_M - ) for edge in get_triangulation(): v1, v2 = edge.v1(), edge.v2() - pygame.draw.line(screen, "white", + pygame.draw.line(screen, "#676767", transform(cones[v1].x, cones[v1].y, vehicle_state), transform(cones[v2].x, cones[v2].y, vehicle_state), 2 ) + + for cone in cones: + pygame.draw.circle( + screen, int_to_color(cone.c), + transform(cone.x, cone.y, vehicle_state), 0.2 * PIXELS_PER_M + ) car_rotated = pygame.transform.rotate(car,-90) scale_factor = VEHICLE_WIDTH_M / car_rotated.get_width() * PIXELS_PER_M From b3f99bdf175404a897244fae6428e3a69e1fdda6 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 26 Mar 2026 22:13:04 -0700 Subject: [PATCH 34/45] timing --- auto_sim/test_controller_2/controller.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/auto_sim/test_controller_2/controller.cpp b/auto_sim/test_controller_2/controller.cpp index 019603b..f1363a5 100644 --- a/auto_sim/test_controller_2/controller.cpp +++ b/auto_sim/test_controller_2/controller.cpp @@ -1,6 +1,7 @@ #include "controller.hpp" #include +#include #include #include @@ -15,6 +16,7 @@ ControlOutput compute(const VehicleState& ve, const std::vector& cones) { // Implementation for compute function static size_t seen_points = 0; if (seen_points < cones.size()) { + const auto start_time = std::chrono::high_resolution_clock::now(); const auto start = std::next( cones.begin(), static_cast::difference_type>(std::min(seen_points, cones.size()))); @@ -26,6 +28,9 @@ ControlOutput compute(const VehicleState& ve, const std::vector& cones) { edges = CDT::extractEdgesFromTriangles(cdt.triangles); std::cout << "number of edges " << edges.size() << std::endl; seen_points = cones.size(); + const auto end_time = std::chrono::high_resolution_clock::now(); + const auto duration = std::chrono::duration_cast(end_time - start_time).count(); + std::cout << "Time taken to insert vertices and extract edges: " << duration << " us" << std::endl; } // cdt.eraseSuperTriangle(); // t.adj will contain the adj list From 1425ee8f3f3e1d61eeb4f6331a169bca5d9c5ee5 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 26 Mar 2026 22:14:53 -0700 Subject: [PATCH 35/45] better --- auto_sim/test_controller_2/controller.cpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/auto_sim/test_controller_2/controller.cpp b/auto_sim/test_controller_2/controller.cpp index f1363a5..1a98535 100644 --- a/auto_sim/test_controller_2/controller.cpp +++ b/auto_sim/test_controller_2/controller.cpp @@ -21,16 +21,13 @@ ControlOutput compute(const VehicleState& ve, const std::vector& cones) { cones.begin(), static_cast::difference_type>(std::min(seen_points, cones.size()))); cdt.insertVertices(start, cones.end(), [](const Cone& c) { return c.x; }, [](const Cone& c) { return c.y; }); - // CDT::Triangulation cpy = cdt; - // cpy.eraseOuterTriangles(); - // std::cout << "number of vertices in cpy " << cpy.vertices.size() << std::endl; - cdt.eraseSuperTriangle(); - edges = CDT::extractEdgesFromTriangles(cdt.triangles); - std::cout << "number of edges " << edges.size() << std::endl; + CDT::Triangulation cpy = cdt; + cpy.eraseSuperTriangle(); + edges = CDT::extractEdgesFromTriangles(cpy.triangles); seen_points = cones.size(); const auto end_time = std::chrono::high_resolution_clock::now(); const auto duration = std::chrono::duration_cast(end_time - start_time).count(); - std::cout << "Time taken to insert vertices and extract edges: " << duration << " us" << std::endl; + std::cout << "triangulation time: " << duration << " us" << std::endl; } // cdt.eraseSuperTriangle(); // t.adj will contain the adj list From 6f5bedf15163c6e938a15a6aa41ff628e8c97690 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 26 Mar 2026 23:51:18 -0700 Subject: [PATCH 36/45] proper triangulation --- auto_sim/main.py | 22 ++++-- auto_sim/render.py | 33 +++++--- auto_sim/test_controller_2/controller.cpp | 95 +++++++++++++++++------ auto_sim/test_controller_2/controller.hpp | 31 +++++++- auto_sim/test_controller_2/pybind.cpp | 23 +++++- auto_sim/test_controller_2/scopetimer.hpp | 21 +++++ 6 files changed, 181 insertions(+), 44 deletions(-) create mode 100644 auto_sim/test_controller_2/scopetimer.hpp diff --git a/auto_sim/main.py b/auto_sim/main.py index 8457ec8..711df7d 100644 --- a/auto_sim/main.py +++ b/auto_sim/main.py @@ -1,5 +1,5 @@ import pygame -from Controller import compute, sim_step, Cone, ConeColor, VehicleState +from Controller import compute, sim_step, Cone, ConeColor, VehicleState, compute_path from render import init, render_world import ctypes import platform @@ -38,13 +38,20 @@ def handle_key(key: int, state: VehicleState): match key: case pygame.K_d: - state.theta += 0.1 + state.y -= 1 case pygame.K_a: - state.theta -= 0.1 + state.y += 1 case pygame.K_w: - state.x += 0.1 + state.x += 1 case pygame.K_s: - state.x -= 0.1 + state.x -= 1 + +ran = False +def simulate_cone_detection(state: VehicleState, cones: list[Cone]): + global ran + if not ran: + compute_path(cones) + ran = True while running: # handle events @@ -52,11 +59,14 @@ def handle_key(key: int, state: VehicleState): for event in pygame.event.get(): if event.type == pygame.QUIT: running = False + elif event.type == pygame.KEYDOWN: + handle_key(event.key, vehicle_state) dt = clock.get_time() # fill the screen with a color to wipe away anything from last frame screen.fill("black") - controls = compute(vehicle_state, CONE_POSITIONS) + simulate_cone_detection(vehicle_state, CONE_POSITIONS) + controls = compute(vehicle_state) sim_step(vehicle_state, controls, dt) render_world(vehicle_state, CONE_POSITIONS, screen) diff --git a/auto_sim/render.py b/auto_sim/render.py index 845ac01..8cd4a5b 100644 --- a/auto_sim/render.py +++ b/auto_sim/render.py @@ -1,11 +1,11 @@ import pygame from scipy.spatial.transform import Rotation as R from constants import VEHICLE_WIDTH_M -from Controller import VehicleState, Cone, ConeColor, get_triangulation +from Controller import VehicleState, Cone, ConeColor, get_offline_edges, calculate_boundary from math import ceil, degrees import numpy as np -PIXELS_PER_M = 50.0 +PIXELS_PER_M = 20.0 w: int h: int @@ -16,7 +16,7 @@ def init(): global car, vignette car = pygame.image.load('fsae.png').convert_alpha() vignette_image = pygame.image.load('vignette.png').convert_alpha() - vignette_image = pygame.transform.scale(vignette_image, (1800,1800)) + vignette_image = pygame.transform.scale(vignette_image, (30 * PIXELS_PER_M,30 * PIXELS_PER_M)) vignette = pygame.Surface((3000,3000), pygame.SRCALPHA) vignette.blit(vignette_image, vignette_image.get_rect(center=(1500,1500))) @@ -80,6 +80,8 @@ def int_to_color(value: ConeColor) -> str: case _: return "white" +# boundary = None + def render_world(vehicle_state: VehicleState, cones: list[Cone], screen: pygame.Surface): global w, h w, h = screen.get_width(), screen.get_height() @@ -90,12 +92,25 @@ def render_world(vehicle_state: VehicleState, cones: list[Cone], screen: pygame. p = transform(0, 0, vehicle_state) pygame.draw.circle(screen, "white",p , 5) - - for edge in get_triangulation(): - v1, v2 = edge.v1(), edge.v2() - pygame.draw.line(screen, "#676767", - transform(cones[v1].x, cones[v1].y, vehicle_state), - transform(cones[v2].x, cones[v2].y, vehicle_state), 2 + # if boundary is None: + # boundary = calculate_boundary(cones, ConeColor.YELLOW) + + for edge in get_offline_edges(): + v1, v2 = cones[edge.v1()], cones[edge.v2()] + # for i in range(len(boundary)): + # v1, v2 = cones[boundary[i]], cones[boundary[(i+1) % len(boundary)]] + default_colour = "#676767" + if v1.c == v2.c: + match v1.c: + case ConeColor.BLUE: + default_colour = "blue" + case ConeColor.YELLOW: + default_colour = "yellow" + case _: + default_colour = "red" + pygame.draw.line(screen, default_colour, + transform(v1.x, v1.y, vehicle_state), + transform(v2.x, v2.y, vehicle_state), 2 ) for cone in cones: diff --git a/auto_sim/test_controller_2/controller.cpp b/auto_sim/test_controller_2/controller.cpp index 1a98535..7671af8 100644 --- a/auto_sim/test_controller_2/controller.cpp +++ b/auto_sim/test_controller_2/controller.cpp @@ -1,35 +1,84 @@ #include "controller.hpp" +#include "scopetimer.hpp" -#include -#include -#include -#include +std::vector calculate_boundary(const std::vector& cones, const ConeColor c) { + size_t at = 0; + std::unordered_set unvisited_blue_cones { }; + for (size_t i = 0; i < cones.size(); ++i) { + if (cones[i].c == c) { + at = i; + unvisited_blue_cones.insert(i); + } + } + + std::vector path; + path.reserve(unvisited_blue_cones.size()); + path.push_back(at); + while (not unvisited_blue_cones.empty()) { + // find cone with shortest distance to cones[at] + size_t closest_cone = 0; + double closest_dist = std::numeric_limits::max(); + for (const size_t i : unvisited_blue_cones) { + if (const double dist = std::hypot(cones[at].x - cones[i].x, cones[at].y - cones[i].y); dist < closest_dist) { + closest_dist = dist; + closest_cone = i; + } + } + unvisited_blue_cones.erase(closest_cone); + path.push_back(closest_cone); + at = closest_cone; + } + + return path; +} + +static CDT::EdgeUSet offline_edges; +void compute_path(const std::vector& cones) { + ScopeTimer s { "offline triangulation timer" }; + CDT::Triangulation cdt; + cdt.insertVertices(cones.begin(), cones.end(), [](const Cone& c) { return c.x; }, [](const Cone& c) { return c.y; }); + + // calculate boundary edges and insert them into the triangulation + std::vector edges { }; + const std::vector blue_boundary = calculate_boundary(cones, ConeColor::BLUE); + for (size_t i = 0; i < blue_boundary.size(); ++i) { + edges.emplace_back(blue_boundary[i], blue_boundary[(i + 1) % blue_boundary.size()]); + } + const std::vector yellow_boundary = calculate_boundary(cones, ConeColor::YELLOW); + for (size_t i = 0; i < yellow_boundary.size(); ++i) { + edges.emplace_back(yellow_boundary[i], yellow_boundary[(i + 1) % yellow_boundary.size()]); + } + cdt.insertEdges(edges); + cdt.eraseOuterTrianglesAndHoles(); + std::cout << "Number of triangles: " << cdt.triangles.size() << std::endl; + offline_edges = CDT::extractEdgesFromTriangles(cdt.triangles); +} + +const CDT::EdgeUSet& get_offline_edges() { + return offline_edges; +} -static CDT::Triangulation cdt; static CDT::EdgeUSet edges; +void update_cone_positions(const std::vector& cones) { + static CDT::Triangulation cdt; + static size_t seen_cones = 0; + if (seen_cones == cones.size()) { + return; + } + + ScopeTimer s { "online triangulation timer" }; + const auto start = std::next(cones.begin(), static_cast::difference_type>(seen_cones)); + cdt.insertVertices(start, cones.end(), [](const Cone& c) { return c.x; }, [](const Cone& c) { return c.y; }); + CDT::Triangulation cpy = cdt; + cpy.eraseSuperTriangle(); + edges = CDT::extractEdgesFromTriangles(cpy.triangles); +} const CDT::EdgeUSet& get_triangulation() { return edges; } -ControlOutput compute(const VehicleState& ve, const std::vector& cones) { - // Implementation for compute function - static size_t seen_points = 0; - if (seen_points < cones.size()) { - const auto start_time = std::chrono::high_resolution_clock::now(); - const auto start = std::next( - cones.begin(), - static_cast::difference_type>(std::min(seen_points, cones.size()))); - cdt.insertVertices(start, cones.end(), [](const Cone& c) { return c.x; }, [](const Cone& c) { return c.y; }); - CDT::Triangulation cpy = cdt; - cpy.eraseSuperTriangle(); - edges = CDT::extractEdgesFromTriangles(cpy.triangles); - seen_points = cones.size(); - const auto end_time = std::chrono::high_resolution_clock::now(); - const auto duration = std::chrono::duration_cast(end_time - start_time).count(); - std::cout << "triangulation time: " << duration << " us" << std::endl; - } - // cdt.eraseSuperTriangle(); +ControlOutput compute(const VehicleState& ve) { // t.adj will contain the adj list return { 0, 0 }; } \ No newline at end of file diff --git a/auto_sim/test_controller_2/controller.hpp b/auto_sim/test_controller_2/controller.hpp index c6f9f31..584560f 100644 --- a/auto_sim/test_controller_2/controller.hpp +++ b/auto_sim/test_controller_2/controller.hpp @@ -3,5 +3,32 @@ #include "types.hpp" #include -const CDT::EdgeUSet& get_triangulation(); -ControlOutput compute(const VehicleState& ve, const std::vector& cones); \ No newline at end of file +ControlOutput compute(const VehicleState& ve); + +enum class DriverlessState { + IDLE, + ONLINE_MAPPING, // use for autocross, acceleration, skidpad, etc. + TRACKDRIVE_MAPPING, + TRACKDRIVE_RUNNING +}; + +// NOTE everything under this line should be private, but we are exposing it for testing purposes +/** + * @param cones cones to calculate the boundary from + * @param c the color of the cones to calculate the boundary for + * @return a vector of vertex indices that form the boundary of the given color. The order of the vertices is not guaranteed to be clockwise or counterclockwise. + */ +std::vector calculate_boundary(const std::vector& cones, ConeColor c); +/** + * This computes the path from the given ALL cones offline. + * @param cones + */ +void compute_path(const std::vector& cones); +/** + * @return offline edges + */ +const CDT::EdgeUSet& get_offline_edges(); + +// online path planning +void update_cone_positions(const std::vector& cones); +const CDT::EdgeUSet& get_triangulation(); \ No newline at end of file diff --git a/auto_sim/test_controller_2/pybind.cpp b/auto_sim/test_controller_2/pybind.cpp index d1446a6..b5fd645 100644 --- a/auto_sim/test_controller_2/pybind.cpp +++ b/auto_sim/test_controller_2/pybind.cpp @@ -33,10 +33,6 @@ PYBIND11_MODULE(Controller, m, py::mod_gil_not_used()) { .def_readwrite("y", &Cone::y) .def_readwrite("c", &Cone::c); - m.def("get_triangulation", &get_triangulation, py::return_value_policy::reference, R"pbdoc( - Get the current triangulation object) - )pbdoc"); - py::class_(m, "Edge") .def("v1", &CDT::Edge::v1) .def("v2", &CDT::Edge::v2); @@ -45,6 +41,25 @@ PYBIND11_MODULE(Controller, m, py::mod_gil_not_used()) { Compute control output )pbdoc"); + // offline path calculations + m.def("calculate_boundary", &calculate_boundary, R"pbdoc( + Calculate the boundary of the given color from the list of cones + )pbdoc"); + m.def("compute_path", &compute_path, R"pbdoc() + Compute the triangulation from a list of cones + )pbdoc"); + m.def("get_offline_edges", &get_offline_edges, py::return_value_policy::reference, R"pbdoc() + Get the offline edges calculated from compute_path() + )pbdoc"); + + // online path caclulations + m.def("update_cone_positions", &update_cone_positions, R"pbdoc() + Update the triangulation with new cone positions + )pbdoc"); + m.def("get_triangulation", &get_triangulation, py::return_value_policy::reference, R"pbdoc( + Get the current triangulation object) + )pbdoc"); + m.def("sim_step", &sim_step, R"pbdoc( Simulate one step of the vehicle dynamics )pbdoc"); diff --git a/auto_sim/test_controller_2/scopetimer.hpp b/auto_sim/test_controller_2/scopetimer.hpp new file mode 100644 index 0000000..38b583e --- /dev/null +++ b/auto_sim/test_controller_2/scopetimer.hpp @@ -0,0 +1,21 @@ +#pragma once +#include +#include + +class ScopeTimer { +public: + // Constructor: Starts the timer + explicit ScopeTimer(const std::string& name) + : m_name(name), m_start_time(std::chrono::steady_clock::now()) { } + + // Destructor: Stops the timer and prints elapsed time + ~ScopeTimer() { + const auto end_time = std::chrono::steady_clock::now(); + const auto duration_us = std::chrono::duration_cast(end_time - m_start_time).count(); + std::cout << "Timer [" << m_name << "]: " << duration_us << " us" << std::endl; + } + +private: + std::string m_name; + std::chrono::time_point m_start_time; +}; \ No newline at end of file From 6610487d41ebc46616a7c8625cf22054441cca84 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Fri, 27 Mar 2026 00:05:25 -0700 Subject: [PATCH 37/45] restructure --- auto_sim/main.py | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/auto_sim/main.py b/auto_sim/main.py index 711df7d..cbb1952 100644 --- a/auto_sim/main.py +++ b/auto_sim/main.py @@ -47,11 +47,12 @@ def handle_key(key: int, state: VehicleState): state.x -= 1 ran = False -def simulate_cone_detection(state: VehicleState, cones: list[Cone]): +def simulate_cone_detection(state: VehicleState): global ran if not ran: - compute_path(cones) + compute_path(CONE_POSITIONS) ran = True + return CONE_POSITIONS while running: # handle events @@ -62,11 +63,13 @@ def simulate_cone_detection(state: VehicleState, cones: list[Cone]): elif event.type == pygame.KEYDOWN: handle_key(event.key, vehicle_state) + # this is what is running on the + detected_cones = simulate_cone_detection(vehicle_state) + controls = compute(vehicle_state) + + # closing the SIL loop dt = clock.get_time() - # fill the screen with a color to wipe away anything from last frame screen.fill("black") - simulate_cone_detection(vehicle_state, CONE_POSITIONS) - controls = compute(vehicle_state) sim_step(vehicle_state, controls, dt) render_world(vehicle_state, CONE_POSITIONS, screen) From b74123270c2ff23b8d7cd84156ce21d3ba3c0ffa Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Fri, 27 Mar 2026 09:38:46 -0700 Subject: [PATCH 38/45] small fix --- auto_sim/test_controller_2/controller.cpp | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/auto_sim/test_controller_2/controller.cpp b/auto_sim/test_controller_2/controller.cpp index 7671af8..968475b 100644 --- a/auto_sim/test_controller_2/controller.cpp +++ b/auto_sim/test_controller_2/controller.cpp @@ -2,14 +2,15 @@ #include "scopetimer.hpp" std::vector calculate_boundary(const std::vector& cones, const ConeColor c) { - size_t at = 0; + size_t start = 0; std::unordered_set unvisited_blue_cones { }; for (size_t i = 0; i < cones.size(); ++i) { if (cones[i].c == c) { - at = i; + start = i; unvisited_blue_cones.insert(i); } } + size_t at = start; std::vector path; path.reserve(unvisited_blue_cones.size()); @@ -24,6 +25,9 @@ std::vector calculate_boundary(const std::vector& cones, con closest_cone = i; } } + if (closest_cone == start) { + break; + } unvisited_blue_cones.erase(closest_cone); path.push_back(closest_cone); at = closest_cone; @@ -32,11 +36,18 @@ std::vector calculate_boundary(const std::vector& cones, con return path; } +static double get_cone_x(const Cone& c) { + return c.x; +} +static double get_cone_y(const Cone& c) { + return c.y; +} + static CDT::EdgeUSet offline_edges; void compute_path(const std::vector& cones) { ScopeTimer s { "offline triangulation timer" }; CDT::Triangulation cdt; - cdt.insertVertices(cones.begin(), cones.end(), [](const Cone& c) { return c.x; }, [](const Cone& c) { return c.y; }); + cdt.insertVertices(cones.begin(), cones.end(), get_cone_x, get_cone_y); // calculate boundary edges and insert them into the triangulation std::vector edges { }; From 9ce85fd96591469bdeae3b6a1ba469a224431a85 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Fri, 27 Mar 2026 10:10:26 -0700 Subject: [PATCH 39/45] center line --- auto_sim/render.py | 34 +++++++++------- auto_sim/test_controller_2/controller.cpp | 47 ++++++++++++++++++----- auto_sim/test_controller_2/controller.hpp | 12 ++++++ auto_sim/test_controller_2/pybind.cpp | 6 +++ auto_sim/test_controller_2/types.hpp | 1 + 5 files changed, 77 insertions(+), 23 deletions(-) diff --git a/auto_sim/render.py b/auto_sim/render.py index 8cd4a5b..9358f37 100644 --- a/auto_sim/render.py +++ b/auto_sim/render.py @@ -1,7 +1,7 @@ import pygame from scipy.spatial.transform import Rotation as R from constants import VEHICLE_WIDTH_M -from Controller import VehicleState, Cone, ConeColor, get_offline_edges, calculate_boundary +from Controller import VehicleState, Cone, ConeColor, get_offline_edges, get_center_line, get_center_points from math import ceil, degrees import numpy as np @@ -95,19 +95,27 @@ def render_world(vehicle_state: VehicleState, cones: list[Cone], screen: pygame. # if boundary is None: # boundary = calculate_boundary(cones, ConeColor.YELLOW) - for edge in get_offline_edges(): - v1, v2 = cones[edge.v1()], cones[edge.v2()] - # for i in range(len(boundary)): - # v1, v2 = cones[boundary[i]], cones[boundary[(i+1) % len(boundary)]] + # for edge in get_offline_edges(): + # v1, v2 = cones[edge.v1()], cones[edge.v2()] + # default_colour = "#676767" + # if v1.c == v2.c: + # match v1.c: + # case ConeColor.BLUE: + # default_colour = "blue" + # case ConeColor.YELLOW: + # default_colour = "yellow" + # case _: + # default_colour = "red" + # pygame.draw.line(screen, default_colour, + # transform(v1.x, v1.y, vehicle_state), + # transform(v2.x, v2.y, vehicle_state), 2 + # ) + + center_points = get_center_points() + center_line = get_center_line() + for i in range(len(center_line)): + v1, v2 = center_points[center_line[i]], center_points[center_line[(i+1) % len(center_line)]] default_colour = "#676767" - if v1.c == v2.c: - match v1.c: - case ConeColor.BLUE: - default_colour = "blue" - case ConeColor.YELLOW: - default_colour = "yellow" - case _: - default_colour = "red" pygame.draw.line(screen, default_colour, transform(v1.x, v1.y, vehicle_state), transform(v2.x, v2.y, vehicle_state), 2 diff --git a/auto_sim/test_controller_2/controller.cpp b/auto_sim/test_controller_2/controller.cpp index 968475b..f70c248 100644 --- a/auto_sim/test_controller_2/controller.cpp +++ b/auto_sim/test_controller_2/controller.cpp @@ -3,23 +3,24 @@ std::vector calculate_boundary(const std::vector& cones, const ConeColor c) { size_t start = 0; - std::unordered_set unvisited_blue_cones { }; + std::unordered_set unvisited_cones { }; for (size_t i = 0; i < cones.size(); ++i) { if (cones[i].c == c) { start = i; - unvisited_blue_cones.insert(i); + unvisited_cones.insert(i); } } size_t at = start; std::vector path; - path.reserve(unvisited_blue_cones.size()); + path.reserve(unvisited_cones.size()); path.push_back(at); - while (not unvisited_blue_cones.empty()) { + unvisited_cones.erase(at); + while (not unvisited_cones.empty()) { // find cone with shortest distance to cones[at] size_t closest_cone = 0; double closest_dist = std::numeric_limits::max(); - for (const size_t i : unvisited_blue_cones) { + for (const size_t i : unvisited_cones) { if (const double dist = std::hypot(cones[at].x - cones[i].x, cones[at].y - cones[i].y); dist < closest_dist) { closest_dist = dist; closest_cone = i; @@ -28,7 +29,7 @@ std::vector calculate_boundary(const std::vector& cones, con if (closest_cone == start) { break; } - unvisited_blue_cones.erase(closest_cone); + unvisited_cones.erase(closest_cone); path.push_back(closest_cone); at = closest_cone; } @@ -43,7 +44,10 @@ static double get_cone_y(const Cone& c) { return c.y; } -static CDT::EdgeUSet offline_edges; +static CDT::EdgeUSet offline_inner_edges; +static CDT::EdgeUSet offline_boundary_edges; +static std::vector center_points; +static std::vector center_line; void compute_path(const std::vector& cones) { ScopeTimer s { "offline triangulation timer" }; CDT::Triangulation cdt; @@ -61,12 +65,35 @@ void compute_path(const std::vector& cones) { } cdt.insertEdges(edges); cdt.eraseOuterTrianglesAndHoles(); - std::cout << "Number of triangles: " << cdt.triangles.size() << std::endl; - offline_edges = CDT::extractEdgesFromTriangles(cdt.triangles); + offline_boundary_edges = cdt.fixedEdges; + offline_inner_edges = CDT::extractEdgesFromTriangles(cdt.triangles); + const size_t original_num_edges = offline_inner_edges.size(); + for (const auto e : offline_boundary_edges) { + offline_inner_edges.erase(e); + } + assert(offline_inner_edges.size() + offline_boundary_edges.size() == original_num_edges); + + center_points = { }; + for (const auto e : offline_inner_edges) { + // get center point of e + const Cone& c1 = cones[e.v1()]; + const Cone& c2 = cones[e.v2()]; + center_points.emplace_back((c1.x + c2.x) / 2, (c1.y + c2.y) / 2, ConeColor::CENTER); + } + center_line = calculate_boundary(center_points, ConeColor::CENTER); } const CDT::EdgeUSet& get_offline_edges() { - return offline_edges; + return offline_inner_edges; +} +const CDT::EdgeUSet& get_boundary_edges() { + return offline_inner_edges; +} +const std::vector& get_center_points() { + return center_points; +} +const std::vector& get_center_line() { + return center_line; } static CDT::EdgeUSet edges; diff --git a/auto_sim/test_controller_2/controller.hpp b/auto_sim/test_controller_2/controller.hpp index 584560f..86c575b 100644 --- a/auto_sim/test_controller_2/controller.hpp +++ b/auto_sim/test_controller_2/controller.hpp @@ -28,6 +28,18 @@ void compute_path(const std::vector& cones); * @return offline edges */ const CDT::EdgeUSet& get_offline_edges(); +/** + * @return boundary edges + */ +const CDT::EdgeUSet& get_boundary_edges(); +/** + * @return center points + */ +const std::vector& get_center_points(); +/** + * @return center line vertices + */ +const std::vector& get_center_line(); // online path planning void update_cone_positions(const std::vector& cones); diff --git a/auto_sim/test_controller_2/pybind.cpp b/auto_sim/test_controller_2/pybind.cpp index b5fd645..7fd3ae3 100644 --- a/auto_sim/test_controller_2/pybind.cpp +++ b/auto_sim/test_controller_2/pybind.cpp @@ -51,6 +51,12 @@ PYBIND11_MODULE(Controller, m, py::mod_gil_not_used()) { m.def("get_offline_edges", &get_offline_edges, py::return_value_policy::reference, R"pbdoc() Get the offline edges calculated from compute_path() )pbdoc"); + m.def("get_center_points", &get_center_points, py::return_value_policy::reference, R"pbdoc()) + Get the center points calculated from compute_path() + )pbdoc"); + m.def("get_center_line", get_center_line, py::return_value_policy::reference, R"pbdoc()) + Get the center line calculated from compute_path() + )pbdoc"); // online path caclulations m.def("update_cone_positions", &update_cone_positions, R"pbdoc() diff --git a/auto_sim/test_controller_2/types.hpp b/auto_sim/test_controller_2/types.hpp index 07b1669..a65f1ea 100644 --- a/auto_sim/test_controller_2/types.hpp +++ b/auto_sim/test_controller_2/types.hpp @@ -27,6 +27,7 @@ enum class ConeColor { BLUE, YELLOW, ORANGE, + CENTER }; struct Cone { double x; From 6aaefc4496614a171fac846142d98bc39e93c66a Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Thu, 2 Apr 2026 14:40:07 -0700 Subject: [PATCH 40/45] update pipfiles --- auto_sim/Pipfile | 2 + auto_sim/Pipfile.lock | 377 +++++++++++++++++++++++++++++++++--------- 2 files changed, 305 insertions(+), 74 deletions(-) diff --git a/auto_sim/Pipfile b/auto_sim/Pipfile index fdb120a..9acf776 100644 --- a/auto_sim/Pipfile +++ b/auto_sim/Pipfile @@ -9,6 +9,8 @@ numpy = "*" scipy = "*" setuptools = "*" pybind11 = "*" +requests = "*" +casadi = "==3.6.7" [dev-packages] diff --git a/auto_sim/Pipfile.lock b/auto_sim/Pipfile.lock index 52b1eea..1a2f9eb 100644 --- a/auto_sim/Pipfile.lock +++ b/auto_sim/Pipfile.lock @@ -1,7 +1,7 @@ { "_meta": { "hash": { - "sha256": "63ddba2de7dab27b129c0ff4adb1a6d5b6dc3b9212d0fc8202ab757e40506fcb" + "sha256": "99c98440216291c693070c2fd616fd9a9691013b1b74849cdfb39eeefffc7f78" }, "pipfile-spec": 6, "requires": { @@ -16,84 +16,296 @@ ] }, "default": { + "casadi": { + "hashes": [ + "sha256:0012c1951286013ab1c7ab79f21fec7374995085f9067d8cef7873dea062b426", + "sha256:045c1c7fc24bb27398f0ba8979eb6bf07b09525496dc83ee4e75ca037187a6bd", + "sha256:05ef1a87ac6e173cba3762f0ad0ba50c02154317a1135317a23c446a149a2e12", + "sha256:05fedada09ae4f1b8d53194168efa8d88ca878717a9a4e7a2465b6e2aacff805", + "sha256:16042ef095117704312be53389a0f22ab7a0968131db2bf785663d8003957a95", + "sha256:1a676437955fcda10488fea87f78ae64f495a7247825bda476199cdeb5ba0b4d", + "sha256:1a7122f3fd4dca0ed9af72bac398de06e225b291b45c3d89b73a19a3247eeeb5", + "sha256:1ab528f5ea04ef56e2676252cfc3a04023de5d1169578d1873ce403e23fb48d5", + "sha256:1b7467dd5e664a87e03ff118993587d154371a3fb7020ce705ee1045ab561399", + "sha256:1d52d8c11d0125552e10c458d8f33b38cd9a6eb9603405a1df87eb35aae4c93a", + "sha256:21cde87288afebb32a2a035bf6b6a91a025e24ee14aba7a0ae5515707b9887c1", + "sha256:21d5b4ce0f83cfc4ab3b9129bdd7501758457323518069f760cb13a0cefb7fc9", + "sha256:26fea7e80b104698d7c799556ec706ce29748e0e2a045a2ca7eb66032e428929", + "sha256:290e92a7e2f4cecffc229dc196a85855e7dbedf6929eed1850b27e4ad8ad67f0", + "sha256:32ba1335bb0928fd340adc9e21d481d918025072a1f0540cf94093b95f781951", + "sha256:3a51b7731126ca1767accbe6747df7ba0087fc32537ab7e5843db88ab42454e3", + "sha256:4e98668fe6ea0fc8bd9e4189a8f71725acd13c5e748af62b6796bc8fcb71af1d", + "sha256:514b5d2af59c3a60b2a955d9508d5f2c75f05bec1e2f5000043870856e3d41a3", + "sha256:53189e560bcc20e7b10714356c987ae98755e28d5103337fd96bc81632786591", + "sha256:5c252837354ca9da91bb2c9836921213d3972dad6edf516c9c8ef296e71a4f74", + "sha256:636746feeeef755a003b6b13a7fca275a8ad3e1be21f5d78a60e72ecdca9c841", + "sha256:641efda22adc1fab2faeb8cb1eb866619f2240f32fb6a02ae82b7399746cdc2e", + "sha256:65976cabb6f8b5eecd72b346edbc4211cf0c8743c1ab9109ddc0eb0407da5a7e", + "sha256:6b6240fcd207aa154867a09bb830d3067d0e55808fbaef8c4c0e24c55b6905b4", + "sha256:743b6d0070bb0262ec1fd4f840244991ebed2435b3104a5ae6a3bc928d0b31f7", + "sha256:783976f9016ed2e882f00f3dc81a72abd8424fc6e8d322793d4bd3782a37f96d", + "sha256:7d7997a9d9ded13fb974fec62c1b5921c1360910ee01784e5a421e288d10adad", + "sha256:7da27dc03524d56d450880db2cc7f45f075524bc434b0b317e2d80dd76b45d75", + "sha256:7ebcbb566f2a01118bc6db5453876d2a69a2ab56bdcd45f9b5487b6b45cbb1f8", + "sha256:7f65e7444ddb15f521e0dc5d61b2557054e264de1dea8d9990235336d3e80247", + "sha256:8c4b4d0443a07452e7b6e0717ec504ed6a82d5da8bca6798809fb0524499c552", + "sha256:8cefef3e30c36e89df94b94ba19eb3588b960ff713d7631233d3402d34544f47", + "sha256:971cefbe41e6bde88547c0591ab9424ad1efbe81edc6b0c5a54f125ddffadbf1", + "sha256:a5e2bf2311225970c06000c78d9945cb6e2142fb35981426a8eb86ef947a2483", + "sha256:a6c549826a0f46e0ee120581a8ed9b802100040274eb23c3e41ca7e4b5f92555", + "sha256:acddbf6bb05e357c898bcb3253ed106c3be6a0007957ec0231f84cae873a3def", + "sha256:adb46ef41749db9bc55a6a1677ba4a43f0ae6a2c1a6cbfc056681d907cf1e3be", + "sha256:b8665e49b6cf7e40f969f0cb3ed87468cb14c64d845438185ff9c289c5f79381", + "sha256:bb9f3c2e9ab4a1fb28a7ad7e5012e133720656b606b8f560bc1bc55a8154e2d9", + "sha256:c02a304f5e8b147bca343e84b4de39629e2b761901c1fe99ecffa66c99875b80", + "sha256:c30904baf2de259979e0e13b99c605cd18c4523aa86170de5940edeb5dd348fa", + "sha256:c58a7d0a2365e28f065c68c0c958e1cc18f9c1dddbf9407842215eaf02f03011", + "sha256:c7b5bf2d05140db9609a8fa1716936e7e00d78d42e599167fc682d3b88dd05b9", + "sha256:c7c1502a042efb321da1556e94cc8ef6a035785b9ebcb5ddd4676b32e58d09f8", + "sha256:cacf78a5e3c5590bf281a1606a213ac5d6d9e0be71986449b7e523044abb2ea0", + "sha256:ce8c104cfc459cea32966f0ffbf189cbea379f69cff32aab33852d4a9cf0b532", + "sha256:d4b175792cd86c2ae2875f20f24e62f4ffe9d8ddea4c2e7ab1f0c43c9d8fc1e6", + "sha256:ea85757c9914a7f3b0ea1f8b1f9635afce1b96c89a043af5fc5778438a90fefc", + "sha256:ed274c0636c811800e0e2f834a616f1ee9ee6d14e21d3ae3e5281b77f9f66603", + "sha256:ee407148f57cedf49928a027b3d628d5fc005fec9da2af27eec34dc7b8da10da", + "sha256:f2959bb834090491a324938454aa424cc1ab3f80cadac6fd8e2133b179a638e4", + "sha256:f3c2d013bcb969b8a9b2e8aa9b6fcfaf9990d29eef496d1eee7d44a889b0f9b6", + "sha256:f780e496157ab1caeb6fe19079e4508c2d7a9f5489b298e04c40438a3ba0caf4", + "sha256:f9783247f4675a660f584c785ef44f637ddc0b2ee0112c6eacb3a8cec5be4021", + "sha256:fa8da902f2e29893e71394c6bf1d3ccb6df52d063d06982a3e8b4ec5118f1827" + ], + "index": "pypi", + "version": "==3.6.7" + }, + "certifi": { + "hashes": [ + "sha256:027692e4402ad994f1c42e52a4997a9763c646b73e4096e4d5d6db8af1d6f0fa", + "sha256:e887ab5cee78ea814d3472169153c2d12cd43b14bd03329a39a9c6e2e80bfba7" + ], + "markers": "python_version >= '3.7'", + "version": "==2026.2.25" + }, + "charset-normalizer": { + "hashes": [ + "sha256:007d05ec7321d12a40227aae9e2bc6dca73f3cb21058999a1df9e193555a9dcc", + "sha256:03853ed82eeebbce3c2abfdbc98c96dc205f32a79627688ac9a27370ea61a49c", + "sha256:07d9e39b01743c3717745f4c530a6349eadbfa043c7577eef86c502c15df2c67", + "sha256:08e721811161356f97b4059a9ba7bafb23ea5ee2255402c42881c214e173c6b4", + "sha256:0c96c3b819b5c3e9e165495db84d41914d6894d55181d2d108cc1a69bfc9cce0", + "sha256:0ea948db76d31190bf08bd371623927ee1339d5f2a0b4b1b4a4439a65298703c", + "sha256:0f7eb884681e3938906ed0434f20c63046eacd0111c4ba96f27b76084cd679f5", + "sha256:12a6fff75f6bc66711b73a2f0addfc4c8c15a20e805146a02d147a318962c444", + "sha256:12d8baf840cc7889b37c7c770f478adea7adce3dcb3944d02ec87508e2dcf153", + "sha256:14265bfe1f09498b9d8ec91e9ec9fa52775edf90fcbde092b25f4a33d444fea9", + "sha256:16d971e29578a5e97d7117866d15889a4a07befe0e87e703ed63cd90cb348c01", + "sha256:177a0ba5f0211d488e295aaf82707237e331c24788d8d76c96c5a41594723217", + "sha256:1a87ca9d5df6fe460483d9a5bbf2b18f620cbed41b432e2bddb686228282d10b", + "sha256:1c2a768fdd44ee4a9339a9b0b130049139b8ce3c01d2ce09f67f5a68048d477c", + "sha256:1c2aed2e5e41f24ea8ef1590b8e848a79b56f3a5564a65ceec43c9d692dc7d8a", + "sha256:1dc8b0ea451d6e69735094606991f32867807881400f808a106ee1d963c46a83", + "sha256:1efde3cae86c8c273f1eb3b287be7d8499420cf2fe7585c41d370d3e790054a5", + "sha256:202389074300232baeb53ae2569a60901f7efadd4245cf3a3bf0617d60b439d7", + "sha256:203104ed3e428044fd943bc4bf45fa73c0730391f9621e37fe39ecf477b128cb", + "sha256:2257141f39fe65a3fdf38aeccae4b953e5f3b3324f4ff0daf9f15b8518666a2c", + "sha256:298930cec56029e05497a76988377cbd7457ba864beeea92ad7e844fe74cd1f1", + "sha256:2cd4a60d0e2fb04537162c62bbbb4182f53541fe0ede35cdf270a1c1e723cc42", + "sha256:2d6eb928e13016cea4f1f21d1e10c1cebd5a421bc57ddf5b1142ae3f86824fab", + "sha256:2fe249cb4651fd12605b7288b24751d8bfd46d35f12a20b1ba33dea122e690df", + "sha256:30b8d1d8c52a48c2c5690e152c169b673487a2a58de1ec7393196753063fcd5e", + "sha256:320ade88cfb846b8cd6b4ddf5ee9e80ee0c1f52401f2456b84ae1ae6a1a5f207", + "sha256:3534e7dcbdcf757da6b85a0bbf5b6868786d5982dd959b065e65481644817a18", + "sha256:36836d6ff945a00b88ba1e4572d721e60b5b8c98c155d465f56ad19d68f23734", + "sha256:38c0109396c4cfc574d502df99742a45c72c08eff0a36158b6f04000043dbf38", + "sha256:3946fa46a0cf3e4c8cb1cc52f56bb536310d34f25f01ca9b6c16afa767dab110", + "sha256:3bec022aec2c514d9cf199522a802bd007cd588ab17ab2525f20f9c34d067c18", + "sha256:3c9a494bc5ec77d43cea229c4f6db1e4d8fe7e1bbffa8b6f0f0032430ff8ab44", + "sha256:3dce51d0f5e7951f8bb4900c257dad282f49190fdbebecd4ba99bcc41fef404d", + "sha256:3dedcc22d73ec993f42055eff4fcfed9318d1eeb9a6606c55892a26964964e48", + "sha256:4042d5c8f957e15221d423ba781e85d553722fc4113f523f2feb7b188cc34c5e", + "sha256:481551899c856c704d58119b5025793fa6730adda3571971af568f66d2424bb5", + "sha256:4dc1e73c36828f982bfe79fadf5919923f8a6f4df2860804db9a98c48824ce8d", + "sha256:4e5163c14bffd570ef2affbfdd77bba66383890797df43dc8b4cc7d6f500bf53", + "sha256:511ef87c8aec0783e08ac18565a16d435372bc1ac25a91e6ac7f5ef2b0bff790", + "sha256:532bc9bf33a68613fd7d65e4b1c71a6a38d7d42604ecf239c77392e9b4e8998c", + "sha256:54523e136b8948060c0fa0bc7b1b50c32c186f2fceee897a495406bb6e311d2b", + "sha256:5649fd1c7bade02f320a462fdefd0b4bd3ce036065836d4f42e0de958038e116", + "sha256:56be790f86bfb2c98fb742ce566dfb4816e5a83384616ab59c49e0604d49c51d", + "sha256:5b77459df20e08151cd6f8b9ef8ef1f961ef73d85c21a555c7eed5b79410ec10", + "sha256:5ed6ab538499c8644b8a3e18debabcd7ce684f3fa91cf867521a7a0279cab2d6", + "sha256:6178f72c5508bfc5fd446a5905e698c6212932f25bcdd4b47a757a50605a90e2", + "sha256:6370e8686f662e6a3941ee48ed4742317cafbe5707e36406e9df792cdb535776", + "sha256:64f02c6841d7d83f832cd97ccf8eb8a906d06eb95d5276069175c696b024b60a", + "sha256:65bcd23054beab4d166035cabbc868a09c1a49d1efe458fe8e4361215df40265", + "sha256:66671f93accb62ed07da56613636f3641f1a12c13046ce91ffc923721f23c008", + "sha256:6696b7688f54f5af4462118f0bfa7c1621eeb87154f77fa04b9295ce7a8f2943", + "sha256:6785f414ae0f3c733c437e0f3929197934f526d19dfaa75e18fdb4f94c6fb374", + "sha256:67f6279d125ca0046a7fd386d01b311c6363844deac3e5b069b514ba3e63c246", + "sha256:6c114670c45346afedc0d947faf3c7f701051d2518b943679c8ff88befe14f8e", + "sha256:6e0d51f618228538a3e8f46bd246f87a6cd030565e015803691603f55e12afb5", + "sha256:6ed74185b2db44f41ef35fd1617c5888e59792da9bbc9190d6c7300617182616", + "sha256:708838739abf24b2ceb208d0e22403dd018faeef86ddac04319a62ae884c4f15", + "sha256:715479b9a2802ecac752a3b0efa2b0b60285cf962ee38414211abdfccc233b41", + "sha256:733784b6d6def852c814bce5f318d25da2ee65dd4839a0718641c696e09a2960", + "sha256:750e02e074872a3fad7f233b47734166440af3cdea0add3e95163110816d6752", + "sha256:752a45dc4a6934060b3b0dab47e04edc3326575f82be64bc4fc293914566503e", + "sha256:7579e913a5339fb8fa133f6bbcfd8e6749696206cf05acdbdca71a1b436d8e72", + "sha256:7641bb8895e77f921102f72833904dcd9901df5d6d72a2ab8f31d04b7e51e4e7", + "sha256:7804338df6fcc08105c7745f1502ba68d900f45fd770d5bdd5288ddccb8a42d8", + "sha256:80d04837f55fc81da168b98de4f4b797ef007fc8a79ab71c6ec9bc4dd662b15b", + "sha256:813c0e0132266c08eb87469a642cb30aaff57c5f426255419572aaeceeaa7bf4", + "sha256:82b271f5137d07749f7bf32f70b17ab6eaabedd297e75dce75081a24f76eb545", + "sha256:84c018e49c3bf790f9c2771c45e9313a08c2c2a6342b162cd650258b57817706", + "sha256:8751d2787c9131302398b11e6c8068053dcb55d5a8964e114b6e196cf16cb366", + "sha256:8778f0c7a52e56f75d12dae53ae320fae900a8b9b4164b981b9c5ce059cd1fcb", + "sha256:87fad7d9ba98c86bcb41b2dc8dbb326619be2562af1f8ff50776a39e55721c5a", + "sha256:8d828b6667a32a728a1ad1d93957cdf37489c57b97ae6c4de2860fa749b8fc1e", + "sha256:8e385e4267ab76874ae30db04c627faaaf0b509e1ccc11a95b3fc3e83f855c00", + "sha256:92a0a01ead5e668468e952e4238cccd7c537364eb7d851ab144ab6627dbbe12f", + "sha256:94e1885b270625a9a828c9793b4d52a64445299baa1fea5a173bf1d3dd9a1a5a", + "sha256:a180c5e59792af262bf263b21a3c49353f25945d8d9f70628e73de370d55e1e1", + "sha256:a277ab8928b9f299723bc1a2dabb1265911b1a76341f90a510368ca44ad9ab66", + "sha256:a5fe03b42827c13cdccd08e6c0247b6a6d4b5e3cdc53fd1749f5896adcdc2356", + "sha256:a6c5863edfbe888d9eff9c8b8087354e27618d9da76425c119293f11712a6319", + "sha256:a89c23ef8d2c6b27fd200a42aa4ac72786e7c60d40efdc76e6011260b6e949c4", + "sha256:adb2597b428735679446b46c8badf467b4ca5f5056aae4d51a19f9570301b1ad", + "sha256:ae196f021b5e7c78e918242d217db021ed2a6ace2bc6ae94c0fc596221c7f58d", + "sha256:ae89db9e5f98a11a4bf50407d4363e7b09b31e55bc117b4f7d80aab97ba009e5", + "sha256:aed52fea0513bac0ccde438c188c8a471c4e0f457c2dd20cdbf6ea7a450046c7", + "sha256:aef65cd602a6d0e0ff6f9930fcb1c8fec60dd2cfcb6facaf4bdb0e5873042db0", + "sha256:af21eb4409a119e365397b2adbaca4c9ccab56543a65d5dbd9f920d6ac29f686", + "sha256:b14b2d9dac08e28bb8046a1a0434b1750eb221c8f5b87a68f4fa11a6f97b5e34", + "sha256:bb6d88045545b26da47aa879dd4a89a71d1dce0f0e549b1abcb31dfe4a8eac49", + "sha256:bb8cc7534f51d9a017b93e3e85b260924f909601c3df002bcdb58ddb4dc41a5c", + "sha256:bc17a677b21b3502a21f66a8cc64f5bfad4df8a0b8434d661666f8ce90ac3af1", + "sha256:bd6c2a1c7573c64738d716488d2cdd3c00e340e4835707d8fdb8dc1a66ef164e", + "sha256:bd9b23791fe793e4968dba0c447e12f78e425c59fc0e3b97f6450f4781f3ee60", + "sha256:c03a41a8784091e67a39648f70c5f97b5b6a37f216896d44d2cdcb82615339a0", + "sha256:c0f081d69a6e58272819b70288d3221a6ee64b98df852631c80f293514d3b274", + "sha256:c35abb8bfff0185efac5878da64c45dafd2b37fb0383add1be155a763c1f083d", + "sha256:c36c333c39be2dbca264d7803333c896ab8fa7d4d6f0ab7edb7dfd7aea6e98c0", + "sha256:c45e9440fb78f8ddabcf714b68f936737a121355bf59f3907f4e17721b9d1aae", + "sha256:c593052c465475e64bbfe5dbd81680f64a67fdc752c56d7a0ae205dc8aeefe0f", + "sha256:cdd68a1fb318e290a2077696b7eb7a21a49163c455979c639bf5a5dcdc46617d", + "sha256:ce3412fbe1e31eb81ea42f4169ed94861c56e643189e1e75f0041f3fe7020abe", + "sha256:cf1493cd8607bec4d8a7b9b004e699fcf8f9103a9284cc94962cb73d20f9d4a3", + "sha256:cf29836da5119f3c8a8a70667b0ef5fdca3bb12f80fd06487cfa575b3909b393", + "sha256:d4a48e5b3c2a489fae013b7589308a40146ee081f6f509e047e0e096084ceca1", + "sha256:d560742f3c0d62afaccf9f41fe485ed69bd7661a241f86a3ef0f0fb8b1a397af", + "sha256:d6038d37043bced98a66e68d3aa2b6a35505dc01328cd65217cefe82f25def44", + "sha256:d61f00a0869d77422d9b2aba989e2d24afa6ffd552af442e0e58de4f35ea6d00", + "sha256:d635aab80466bc95771bb78d5370e74d36d1fe31467b6b29b8b57b2a3cd7d22c", + "sha256:dca4bbc466a95ba9c0234ef56d7dd9509f63da22274589ebd4ed7f1f4d4c54e3", + "sha256:dd915403e231e6b1809fe9b6d9fc55cf8fb5e02765ac625d9cd623342a7905d7", + "sha256:e044c39e41b92c845bc815e5ae4230804e8e7bc29e399b0437d64222d92809dd", + "sha256:e060d01aec0a910bdccb8be71faf34e7799ce36950f8294c8bf612cba65a2c9e", + "sha256:e1421b502d83040e6d7fb2fb18dff63957f720da3d77b2fbd3187ceb63755d7b", + "sha256:e17b8d5d6a8c47c85e68ca8379def1303fd360c3e22093a807cd34a71cd082b8", + "sha256:e5f4d355f0a2b1a31bc3edec6795b46324349c9cb25eed068049e4f472fb4259", + "sha256:e712b419df8ba5e42b226c510472b37bd57b38e897d3eca5e8cfd410a29fa859", + "sha256:e74327fb75de8986940def6e8dee4f127cc9752bee7355bb323cc5b2659b6d46", + "sha256:e80c8378d8f3d83cd3164da1ad2df9e37a666cdde7b1cb2298ed0b558064be30", + "sha256:e8ac484bf18ce6975760921bb6148041faa8fef0547200386ea0b52b5d27bf7b", + "sha256:eca9705049ad3c7345d574e3510665cb2cf844c2f2dcfe675332677f081cbd46", + "sha256:ed065083d0898c9d5b4bbec7b026fd755ff7454e6e8b73a67f8c744b13986e24", + "sha256:edac0f1ab77644605be2cbba52e6b7f630731fc42b34cb0f634be1a6eface56a", + "sha256:effc3f449787117233702311a1b7d8f59cba9ced946ba727bdc329ec69028e24", + "sha256:f22dec1690b584cea26fade98b2435c132c1b5f68e39f5a0b7627cd7ae31f1dc", + "sha256:f495a1652cf3fbab2eb0639776dad966c2fb874d79d87ca07f9d5f059b8bd215", + "sha256:f496c9c3cc02230093d8330875c4c3cdfc3b73612a5fd921c65d39cbcef08063", + "sha256:f59099f9b66f0d7145115e6f80dd8b1d847176df89b234a5a6b3f00437aa0832", + "sha256:f59ad4c0e8f6bba240a9bb85504faa1ab438237199d4cce5f622761507b8f6a6", + "sha256:fbccdc05410c9ee21bbf16a35f4c1d16123dcdeb8a1d38f33654fa21d0234f79", + "sha256:fea24543955a6a729c45a73fe90e08c743f0b3334bbf3201e6c4bc1b0c7fa464" + ], + "markers": "python_version >= '3.7'", + "version": "==3.4.7" + }, + "idna": { + "hashes": [ + "sha256:771a87f49d9defaf64091e6e6fe9c18d4833f140bd19464795bc32d966ca37ea", + "sha256:795dafcc9c04ed0c1fb032c2aa73654d8e8c5023a7df64a53f39190ada629902" + ], + "markers": "python_version >= '3.8'", + "version": "==3.11" + }, "numpy": { "hashes": [ - "sha256:0200b25c687033316fb39f0ff4e3e690e8957a2c3c8d22499891ec58c37a3eb5", - "sha256:0448e7f9caefb34b4b7dd2b77f21e8906e5d6f0365ad525f9f4f530b13df2afc", - "sha256:0a195f4216be9305a73c0e91c9b026a35f2161237cf1c6de9b681637772ea657", - "sha256:0a60e17a14d640f49146cb38e3f105f571318db7826d9b6fef7e4dce758faecd", - "sha256:120df8c0a81ebbf5b9020c91439fccd85f5e018a927a39f624845be194a2be02", - "sha256:148d59127ac95979d6f07e4d460f934ebdd6eed641db9c0db6c73026f2b2101a", - "sha256:1ec84fd7c8e652b0f4aaaf2e6e9cc8eaa9b1b80a537e06b2e3a2fb176eedcb26", - "sha256:22654fe6be0e5206f553a9250762c653d3698e46686eee53b399ab90da59bd92", - "sha256:22c31dc07025123aedf7f2db9e91783df13f1776dc52c6b22c620870dc0fab22", - "sha256:23b46bb6d8ecb68b58c09944483c135ae5f0e9b8d8858ece5e4ead783771d2a9", - "sha256:2629289168f4897a3c4e23dc98d6f1731f0fc0fe52fb9db19f974041e4cc12b9", - "sha256:26952e18d82a1dbbc2f008d402021baa8d6fc8e84347a2072a25e08b46d698b9", - "sha256:29363fbfa6f8ee855d7569c96ce524845e3d726d6c19b29eceec7dd555dab152", - "sha256:297837823f5bc572c5f9379b0c9f3a3365f08492cbdc33bcc3af174372ebb168", - "sha256:2abad5c7fef172b3377502bde47892439bae394a71bc329f31df0fd829b41a9e", - "sha256:2b3f8d2c4589b1a2028d2a770b0fc4d1f332fb5e01521f4de3199a896d158ddd", - "sha256:2ddb7919366ee468342b91dea2352824c25b55814a987847b6c52003a7c97f15", - "sha256:2e03c05abaee1f672e9d67bc858f300b5ccba1c21397211e8d77d98350972093", - "sha256:32e3bef222ad6b052280311d1d60db8e259e4947052c3ae7dd6817451fc8a4c5", - "sha256:33b3bf58ee84b172c067f56aeadc7ee9ab6de69c5e800ab5b10295d54c581adb", - "sha256:45f003dbdffb997a03da2d1d0cb41fbd24a87507fb41605c0420a3db5bd4667b", - "sha256:483a201202b73495f00dbc83796c6ae63137a9bdade074f7648b3e32613412dd", - "sha256:48da3a4ee1336454b07497ff7ec83903efa5505792c4e6d9bf83d99dc07a1e18", - "sha256:4b42639cdde6d24e732ff823a3fa5b701d8acad89c4142bc1d0bd6dc85200ba5", - "sha256:4bd4741a6a676770e0e97fe9ab2e51de01183df3dcbcec591d26d331a40de950", - "sha256:4d382735cecd7bcf090172489a525cd7d4087bc331f7df9f60ddc9a296cf208e", - "sha256:52077feedeff7c76ed7c9f1a0428558e50825347b7545bbb8523da2cd55c547a", - "sha256:54f29b877279d51e210e0c80709ee14ccbbad647810e8f3d375561c45ef613dd", - "sha256:5884ce5c7acfae1e4e1b6fde43797d10aa506074d25b531b4f54bde33c0c31d4", - "sha256:5e10da9e93247e554bb1d22f8edc51847ddd7dde52d85ce31024c1b4312bfba0", - "sha256:61b0cbabbb6126c8df63b9a3a0c4b1f44ebca5e12ff6997b80fcf267fb3150ef", - "sha256:65f3c2455188f09678355f5cae1f959a06b778bc66d535da07bf2ef20cd319d5", - "sha256:679f2a834bae9020f81534671c56fd0cc76dd7e5182f57131478e23d0dc59e24", - "sha256:6bd06731541f89cdc01b261ba2c9e037f1543df7472517836b78dfb15bd6e476", - "sha256:715de7f82e192e8cae5a507a347d97ad17598f8e026152ca97233e3666daaa71", - "sha256:737f630a337364665aba3b5a77e56a68cc42d350edd010c345d65a3efa3addcc", - "sha256:7395e69ff32526710748f92cd8c9849b361830968ea3e24a676f272653e8983e", - "sha256:76dbb9d4e43c16cf9aa711fcd8de1e2eeb27539dcefb60a1d5e9f12fae1d1ed8", - "sha256:76f0f283506c28b12bba319c0fab98217e9f9b54e6160e9c79e9f7348ba32e9c", - "sha256:77e76d932c49a75617c6d13464e41203cd410956614d0a0e999b25e9e8d27eec", - "sha256:7aa4e54f6469300ebca1d9eb80acd5253cdfa36f2c03d79a35883687da430875", - "sha256:7d1ce23cce91fcea443320a9d0ece9b9305d4368875bab09538f7a5b4131938a", - "sha256:7e58765ad74dcebd3ef0208a5078fba32dc8ec3578fe84a604432950cd043d79", - "sha256:7f3408ff897f8ab07a07fbe2823d7aee6ff644c097cc1f90382511fe982f647f", - "sha256:8ba7b51e71c05aa1f9bc3641463cd82308eab40ce0d5c7e1fd4038cbf9938147", - "sha256:8e236dbda4e1d319d681afcbb136c0c4a8e0f1a5c58ceec2adebb547357fe857", - "sha256:94f3c4a151a2e529adf49c1d54f0f57ff8f9b233ee4d44af623a81553ab86368", - "sha256:9684823a78a6cd6ad7511fc5e25b07947d1d5b5e2812c93fe99d7d4195130720", - "sha256:a016db5c5dba78fa8fe9f5d80d6708f9c42ab087a739803c0ac83a43d686a470", - "sha256:a111698b4a3f8dcbe54c64a7708f049355abd603e619013c346553c1fd4ca90b", - "sha256:a1988292870c7cb9d0ebb4cc96b4d447513a9644801de54606dc7aabf2b7d920", - "sha256:a315e5234d88067f2d97e1f2ef670a7569df445d55400f1e33d117418d008d52", - "sha256:a749547700de0a20a6718293396ec237bb38218049cfce788e08fcb716e8cf73", - "sha256:a97cbf7e905c435865c2d939af3d93f99d18eaaa3cabe4256f4304fb51604349", - "sha256:abdce0f71dcb4a00e4e77f3faf05e4616ceccfe72ccaa07f47ee79cda3b7b0f4", - "sha256:b346845443716c8e542d54112966383b448f4a3ba5c66409771b8c0889485dd3", - "sha256:b44fd60341c4d9783039598efadd03617fa28d041fc37d22b62d08f2027fa0e7", - "sha256:bb2e3cf95854233799013779216c57e153c1ee67a0bf92138acca0e429aefaee", - "sha256:bc71942c789ef415a37f0d4eab90341425a00d538cd0642445d30b41023d3395", - "sha256:be3b8487d725a77acccc9924f65fd8bce9af7fac8c9820df1049424a2115af6c", - "sha256:c59020932feb24ed49ffd03704fbab89f22aa9c0d4b180ff45542fe8918f5611", - "sha256:c6b124bfcafb9e8d3ed09130dbee44848c20b3e758b6bbf006e641778927c028", - "sha256:c9619741e9da2059cd9c3f206110b97583c7152c1dc9f8aafd4beb450ac1c89d", - "sha256:cd32fbacb9fd1bf041bf8e89e4576b6f00b895f06d00914820ae06a616bdfef7", - "sha256:d1b90d840b25874cf5cd20c219af10bac3667db3876d9a495609273ebe679070", - "sha256:d213c7e6e8d211888cc359bab7199670a00f5b82c0978b9d1c75baf1eddbeac0", - "sha256:d5f51900414fc9204a0e0da158ba2ac52b75656e7dce7e77fb9f84bfa343b4cc", - "sha256:d71e379452a2f670ccb689ec801b1218cd3983e253105d6e83780967e899d687", - "sha256:d84f0f881cb2225c2dfd7f78a10a5645d487a496c6668d6cc39f0f114164f3d0", - "sha256:decb0eb8a53c3b009b0962378065589685d66b23467ef5dac16cbe818afde27f", - "sha256:e7dd01a46700b1967487141a66ac1a3cf0dd8ebf1f08db37d46389401512ca97", - "sha256:eb610595dd91560905c132c709412b512135a60f1851ccbd2c959e136431ff67" + "sha256:07077278157d02f65c43b1b26a3886bce886f95d20aabd11f87932750dfb14ed", + "sha256:08f2e31ed5e6f04b118e49821397f12767934cfdd12a1ce86a058f91e004ee50", + "sha256:0aec54fd785890ecca25a6003fd9a5aed47ad607bbac5cd64f836ad8666f4959", + "sha256:0d35aea54ad1d420c812bfa0385c71cd7cc5bcf7c65fed95fc2cd02fe8c79827", + "sha256:0d4e437e295f18ec29bc79daf55e8a47a9113df44d66f702f02a293d93a2d6dd", + "sha256:0dfd3f9d3adbe2920b68b5cd3d51444e13a10792ec7154cd0a2f6e74d4ab3233", + "sha256:1378871da56ca8943c2ba674530924bb8ca40cd228358a3b5f302ad60cf875fc", + "sha256:15716cfef24d3a9762e3acdf87e27f58dc823d1348f765bbea6bef8c639bfa1b", + "sha256:19710a9ca9992d7174e9c52f643d4272dcd1558c5f7af7f6f8190f633bd651a7", + "sha256:23cbfd4c17357c81021f21540da84ee282b9c8fba38a03b7b9d09ba6b951421e", + "sha256:2483e4584a1cb3092da4470b38866634bafb223cbcd551ee047633fd2584599a", + "sha256:27a8d92cd10f1382a67d7cf4db7ce18341b66438bdd9f691d7b0e48d104c2a9d", + "sha256:28a650663f7314afc3e6ec620f44f333c386aad9f6fc472030865dc0ebb26ee3", + "sha256:2aa0613a5177c264ff5921051a5719d20095ea586ca88cc802c5c218d1c67d3e", + "sha256:2c194dd721e54ecad9ad387c1d35e63dce5c4450c6dc7dd5611283dda239aabb", + "sha256:2d19e6e2095506d1736b7d80595e0f252d76b89f5e715c35e06e937679ea7d7a", + "sha256:2d390634c5182175533585cc89f3608a4682ccb173cc9bb940b2881c8d6f8fa0", + "sha256:30caa73029a225b2d40d9fae193e008e24b2026b7ee1a867b7ee8d96ca1a448e", + "sha256:42c16925aa5a02362f986765f9ebabf20de75cdefdca827d14315c568dcab113", + "sha256:45dbed2ab436a9e826e302fcdcbe9133f9b0006e5af7168afb8963a6520da103", + "sha256:4636de7fd195197b7535f231b5de9e4b36d2c440b6e566d2e4e4746e6af0ca93", + "sha256:4a19d9dba1a76618dd86b164d608566f393f8ec6ac7c44f0cc879011c45e65af", + "sha256:4bbc7f303d125971f60ec0aaad5e12c62d0d2c925f0ab1273debd0e4ba37aba5", + "sha256:4d6d57903571f86180eb98f8f0c839fa9ebbfb031356d87f1361be91e433f5b7", + "sha256:4e874c976154687c1f71715b034739b45c7711bec81db01914770373d125e392", + "sha256:51fc224f7ca4d92656d5a5eb315f12eb5fe2c97a66249aa7b5f562528a3be38c", + "sha256:58c8b5929fcb8287cbd6f0a3fae19c6e03a5c48402ae792962ac465224a629a4", + "sha256:5a285b3b96f951841799528cd1f4f01cd70e7e0204b4abebac9463eecfcf2a40", + "sha256:5c70f1cc1c4efbe316a572e2d8b9b9cc44e89b95f79ca3331553fbb63716e2bf", + "sha256:62d6b0f03b694173f9fcb1fb317f7222fd0b0b103e784c6549f5e53a27718c44", + "sha256:6a246d5914aa1c820c9443ddcee9c02bec3e203b0c080349533fae17727dfd1b", + "sha256:6aa3236c78803afbcb255045fbef97a9e25a1f6c9888357d205ddc42f4d6eba5", + "sha256:6bbe4eb67390b0a0265a2c25458f6b90a409d5d069f1041e6aff1e27e3d9a79e", + "sha256:715d1c092715954784bc79e1174fc2a90093dc4dc84ea15eb14dad8abdcdeb74", + "sha256:72944b19f2324114e9dc86a159787333b77874143efcf89a5167ef83cfee8af0", + "sha256:81f4a14bee47aec54f883e0cad2d73986640c1590eb9bfaaba7ad17394481e6e", + "sha256:846300f379b5b12cc769334464656bc882e0735d27d9726568bc932fdc49d5ec", + "sha256:86b6f55f5a352b48d7fbfd2dbc3d5b780b2d79f4d3c121f33eb6efb22e9a2015", + "sha256:874f200b2a981c647340f841730fc3a2b54c9d940566a3c4149099591e2c4c3d", + "sha256:8a87ec22c87be071b6bdbd27920b129b94f2fc964358ce38f3822635a3e2e03d", + "sha256:8b3b60bb7cba2c8c81837661c488637eee696f59a877788a396d33150c35d842", + "sha256:8e3ed142f2728df44263aaf5fb1f5b0b99f4070c553a0d7f033be65338329150", + "sha256:93e15038125dc1e5345d9b5b68aa7f996ec33b98118d18c6ca0d0b7d6198b7e8", + "sha256:989824e9faf85f96ec9c7761cd8d29c531ad857bfa1daa930cba85baaecf1a9a", + "sha256:99d838547ace2c4aace6c4f76e879ddfe02bb58a80c1549928477862b7a6d6ed", + "sha256:9b2aec6af35c113b05695ebb5749a787acd63cafc83086a05771d1e1cd1e555f", + "sha256:9c585a1790d5436a5374bac930dad6ed244c046ed91b2b2a3634eb2971d21008", + "sha256:a7164afb23be6e37ad90b2f10426149fd75aee07ca55653d2aa41e66c4ef697e", + "sha256:ac6b31e35612a26483e20750126d30d0941f949426974cace8e6b5c58a3657b0", + "sha256:ad2e2ef14e0b04e544ea2fa0a36463f847f113d314aa02e5b402fdf910ef309e", + "sha256:b268594bccac7d7cf5844c7732e3f20c50921d94e36d7ec9b79e9857694b1b2f", + "sha256:b5f0362dc928a6ecd9db58868fca5e48485205e3855957bdedea308f8672ea4a", + "sha256:ba1f4fc670ed79f876f70082eff4f9583c15fb9a4b89d6188412de4d18ae2f40", + "sha256:ba203255017337d39f89bdd58417f03c4426f12beed0440cfd933cb15f8669c7", + "sha256:c901b15172510173f5cb310eae652908340f8dede90fff9e3bf6c0d8dfd92f83", + "sha256:c9b39d38a9bd2ae1becd7eac1303d031c5c110ad31f2b319c6e7d98b135c934d", + "sha256:d2a8490669bfe99a233298348acc2d824d496dee0e66e31b66a6022c2ad74a5c", + "sha256:dddbbd259598d7240b18c9d87c56a9d2fb3b02fe266f49a7c101532e78c1d871", + "sha256:df3775294accfdd75f32c74ae39fcba920c9a378a2fc18a12b6820aa8c1fb502", + "sha256:e44319a2953c738205bf3354537979eaa3998ed673395b964c1176083dd46252", + "sha256:e4a010c27ff6f210ff4c6ef34394cd61470d01014439b192ec22552ee867f2a8", + "sha256:e823b8b6edc81e747526f70f71a9c0a07ac4e7ad13020aa736bb7c9d67196115", + "sha256:e892aff75639bbef0d2a2cfd55535510df26ff92f63c92cd84ef8d4ba5a5557f", + "sha256:eea7ac5d2dce4189771cedb559c738a71512768210dc4e4753b107a2048b3d0e", + "sha256:ef4059d6e5152fa1a39f888e344c73fdc926e1b2dd58c771d67b0acfbf2aa67d", + "sha256:f169b9a863d34f5d11b8698ead99febeaa17a13ca044961aa8e2662a6c7766a0", + "sha256:f2cf083b324a467e1ab358c105f6cad5ea950f50524668a80c486ff1db24e119", + "sha256:f8474c4241bc18b750be2abea9d7a9ec84f46ef861dbacf86a4f6e043401f79e", + "sha256:f983334aea213c99992053ede6168500e5f086ce74fbc4acc3f2b00f5762e9db", + "sha256:f9e75681b59ddaa5e659898085ae0eaea229d054f2ac0c7e563a62205a700121", + "sha256:fbc356aae7adf9e6336d336b9c8111d390a05df88f1805573ebb0807bd06fd1d", + "sha256:fcfe2045fd2e8f3cb0ce9d4ba6dba6333b8fa05bb8a4939c908cd43322d14c7e" ], "index": "pypi", "markers": "python_version >= '3.11'", - "version": "==2.4.3" + "version": "==2.4.4" }, "pybind11": { "hashes": [ @@ -171,6 +383,15 @@ "markers": "python_version >= '3.6'", "version": "==2.6.1" }, + "requests": { + "hashes": [ + "sha256:18817f8c57c6263968bc123d237e3b8b08ac046f5456bd1e307ee8f4250d3517", + "sha256:4e6d1ef462f3626a1f0a0a9c42dd93c63bad33f9f1c1937509b8c5c8718ab56a" + ], + "index": "pypi", + "markers": "python_version >= '3.10'", + "version": "==2.33.1" + }, "scipy": { "hashes": [ "sha256:010f4333c96c9bb1a4516269e33cb5917b08ef2166d5556ca2fd9f082a9e6ea0", @@ -247,6 +468,14 @@ "index": "pypi", "markers": "python_version >= '3.9'", "version": "==82.0.1" + }, + "urllib3": { + "hashes": [ + "sha256:1b62b6884944a57dbe321509ab94fd4d3b307075e0c2eae991ac71ee15ad38ed", + "sha256:bf272323e553dfb2e87d9bfd225ca7b0f467b919d7bbd355436d3fd37cb0acd4" + ], + "markers": "python_version >= '3.9'", + "version": "==2.6.3" } }, "develop": {} From d9d75b5af6c561618d0ab4168a5835dc73e464c2 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Sun, 5 Apr 2026 20:41:07 -0700 Subject: [PATCH 41/45] draw --- auto_sim/render.py | 32 ++++++++++++++--------- auto_sim/test_controller_2/controller.cpp | 11 ++++---- auto_sim/test_controller_2/pybind.cpp | 3 +++ 3 files changed, 28 insertions(+), 18 deletions(-) diff --git a/auto_sim/render.py b/auto_sim/render.py index 9358f37..04d03d6 100644 --- a/auto_sim/render.py +++ b/auto_sim/render.py @@ -1,7 +1,7 @@ import pygame from scipy.spatial.transform import Rotation as R from constants import VEHICLE_WIDTH_M -from Controller import VehicleState, Cone, ConeColor, get_offline_edges, get_center_line, get_center_points +from Controller import VehicleState, Cone, ConeColor, get_offline_edges, get_center_line, get_center_points, get_boundary_edges from math import ceil, degrees import numpy as np @@ -98,28 +98,34 @@ def render_world(vehicle_state: VehicleState, cones: list[Cone], screen: pygame. # for edge in get_offline_edges(): # v1, v2 = cones[edge.v1()], cones[edge.v2()] # default_colour = "#676767" + # pygame.draw.line(screen, default_colour, + # transform(v1.x, v1.y, vehicle_state), + # transform(v2.x, v2.y, vehicle_state), 2 + # ) + + # for edge in get_boundary_edges(): + # v1, v2 = cones[edge.v1()], cones[edge.v2()] + # default_colour = "#FF0000" # if v1.c == v2.c: # match v1.c: # case ConeColor.BLUE: # default_colour = "blue" # case ConeColor.YELLOW: # default_colour = "yellow" - # case _: - # default_colour = "red" # pygame.draw.line(screen, default_colour, # transform(v1.x, v1.y, vehicle_state), - # transform(v2.x, v2.y, vehicle_state), 2 + # transform(v2.x, v2.y, vehicle_state), 1 # ) - center_points = get_center_points() - center_line = get_center_line() - for i in range(len(center_line)): - v1, v2 = center_points[center_line[i]], center_points[center_line[(i+1) % len(center_line)]] - default_colour = "#676767" - pygame.draw.line(screen, default_colour, - transform(v1.x, v1.y, vehicle_state), - transform(v2.x, v2.y, vehicle_state), 2 - ) + # center_points = get_center_points() + # center_line = get_center_line() + # for i in range(len(center_line)): + # v1, v2 = center_points[center_line[i]], center_points[center_line[(i+1) % len(center_line)]] + # default_colour = "#676767" + # pygame.draw.line(screen, default_colour, + # transform(v1.x, v1.y, vehicle_state), + # transform(v2.x, v2.y, vehicle_state), 2 + # ) for cone in cones: pygame.draw.circle( diff --git a/auto_sim/test_controller_2/controller.cpp b/auto_sim/test_controller_2/controller.cpp index f70c248..0d0e2dc 100644 --- a/auto_sim/test_controller_2/controller.cpp +++ b/auto_sim/test_controller_2/controller.cpp @@ -3,7 +3,7 @@ std::vector calculate_boundary(const std::vector& cones, const ConeColor c) { size_t start = 0; - std::unordered_set unvisited_cones { }; + std::unordered_set unvisited_cones {}; for (size_t i = 0; i < cones.size(); ++i) { if (cones[i].c == c) { start = i; @@ -54,7 +54,7 @@ void compute_path(const std::vector& cones) { cdt.insertVertices(cones.begin(), cones.end(), get_cone_x, get_cone_y); // calculate boundary edges and insert them into the triangulation - std::vector edges { }; + std::vector edges {}; const std::vector blue_boundary = calculate_boundary(cones, ConeColor::BLUE); for (size_t i = 0; i < blue_boundary.size(); ++i) { edges.emplace_back(blue_boundary[i], blue_boundary[(i + 1) % blue_boundary.size()]); @@ -73,7 +73,7 @@ void compute_path(const std::vector& cones) { } assert(offline_inner_edges.size() + offline_boundary_edges.size() == original_num_edges); - center_points = { }; + center_points = {}; for (const auto e : offline_inner_edges) { // get center point of e const Cone& c1 = cones[e.v1()]; @@ -87,7 +87,7 @@ const CDT::EdgeUSet& get_offline_edges() { return offline_inner_edges; } const CDT::EdgeUSet& get_boundary_edges() { - return offline_inner_edges; + return offline_boundary_edges; } const std::vector& get_center_points() { return center_points; @@ -106,7 +106,8 @@ void update_cone_positions(const std::vector& cones) { ScopeTimer s { "online triangulation timer" }; const auto start = std::next(cones.begin(), static_cast::difference_type>(seen_cones)); - cdt.insertVertices(start, cones.end(), [](const Cone& c) { return c.x; }, [](const Cone& c) { return c.y; }); + cdt.insertVertices( + start, cones.end(), [](const Cone& c) { return c.x; }, [](const Cone& c) { return c.y; }); CDT::Triangulation cpy = cdt; cpy.eraseSuperTriangle(); edges = CDT::extractEdgesFromTriangles(cpy.triangles); diff --git a/auto_sim/test_controller_2/pybind.cpp b/auto_sim/test_controller_2/pybind.cpp index 7fd3ae3..d6532e6 100644 --- a/auto_sim/test_controller_2/pybind.cpp +++ b/auto_sim/test_controller_2/pybind.cpp @@ -51,6 +51,9 @@ PYBIND11_MODULE(Controller, m, py::mod_gil_not_used()) { m.def("get_offline_edges", &get_offline_edges, py::return_value_policy::reference, R"pbdoc() Get the offline edges calculated from compute_path() )pbdoc"); + m.def("get_boundary_edges", &get_boundary_edges, py::return_value_policy::reference, R"pbdoc() + Get the boundary edges calculated from compute_path() + )pbdoc"); m.def("get_center_points", &get_center_points, py::return_value_policy::reference, R"pbdoc()) Get the center points calculated from compute_path() )pbdoc"); From e7526791f056d1b43759818a9cba8e76617996f2 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Mon, 6 Apr 2026 17:33:56 -0700 Subject: [PATCH 42/45] waow --- auto_sim/render.py | 4 +- auto_sim/test_controller_2/controller.cpp | 117 ------------------- auto_sim/test_controller_2/controller.hpp | 37 +----- auto_sim/test_controller_2/path.cpp | 131 ++++++++++++++++++++++ auto_sim/test_controller_2/path.hpp | 32 ++++++ auto_sim/test_controller_2/pybind.cpp | 4 + auto_sim/test_controller_2/setup.py | 2 +- 7 files changed, 171 insertions(+), 156 deletions(-) create mode 100644 auto_sim/test_controller_2/path.cpp create mode 100644 auto_sim/test_controller_2/path.hpp diff --git a/auto_sim/render.py b/auto_sim/render.py index 04d03d6..c709418 100644 --- a/auto_sim/render.py +++ b/auto_sim/render.py @@ -1,7 +1,7 @@ import pygame from scipy.spatial.transform import Rotation as R from constants import VEHICLE_WIDTH_M -from Controller import VehicleState, Cone, ConeColor, get_offline_edges, get_center_line, get_center_points, get_boundary_edges +from Controller import VehicleState, Cone, ConeColor, get_offline_edges, get_center_line, get_center_points, get_boundary_edges, project from math import ceil, degrees import numpy as np @@ -120,7 +120,7 @@ def render_world(vehicle_state: VehicleState, cones: list[Cone], screen: pygame. # center_points = get_center_points() # center_line = get_center_line() # for i in range(len(center_line)): - # v1, v2 = center_points[center_line[i]], center_points[center_line[(i+1) % len(center_line)]] + # v1, v2 = center_points[center_line[i]], center_points[center_line[(i+1) % len(center_line)]] # returns cone objects # default_colour = "#676767" # pygame.draw.line(screen, default_colour, # transform(v1.x, v1.y, vehicle_state), diff --git a/auto_sim/test_controller_2/controller.cpp b/auto_sim/test_controller_2/controller.cpp index 0d0e2dc..2c6caa3 100644 --- a/auto_sim/test_controller_2/controller.cpp +++ b/auto_sim/test_controller_2/controller.cpp @@ -1,121 +1,4 @@ #include "controller.hpp" -#include "scopetimer.hpp" - -std::vector calculate_boundary(const std::vector& cones, const ConeColor c) { - size_t start = 0; - std::unordered_set unvisited_cones {}; - for (size_t i = 0; i < cones.size(); ++i) { - if (cones[i].c == c) { - start = i; - unvisited_cones.insert(i); - } - } - size_t at = start; - - std::vector path; - path.reserve(unvisited_cones.size()); - path.push_back(at); - unvisited_cones.erase(at); - while (not unvisited_cones.empty()) { - // find cone with shortest distance to cones[at] - size_t closest_cone = 0; - double closest_dist = std::numeric_limits::max(); - for (const size_t i : unvisited_cones) { - if (const double dist = std::hypot(cones[at].x - cones[i].x, cones[at].y - cones[i].y); dist < closest_dist) { - closest_dist = dist; - closest_cone = i; - } - } - if (closest_cone == start) { - break; - } - unvisited_cones.erase(closest_cone); - path.push_back(closest_cone); - at = closest_cone; - } - - return path; -} - -static double get_cone_x(const Cone& c) { - return c.x; -} -static double get_cone_y(const Cone& c) { - return c.y; -} - -static CDT::EdgeUSet offline_inner_edges; -static CDT::EdgeUSet offline_boundary_edges; -static std::vector center_points; -static std::vector center_line; -void compute_path(const std::vector& cones) { - ScopeTimer s { "offline triangulation timer" }; - CDT::Triangulation cdt; - cdt.insertVertices(cones.begin(), cones.end(), get_cone_x, get_cone_y); - - // calculate boundary edges and insert them into the triangulation - std::vector edges {}; - const std::vector blue_boundary = calculate_boundary(cones, ConeColor::BLUE); - for (size_t i = 0; i < blue_boundary.size(); ++i) { - edges.emplace_back(blue_boundary[i], blue_boundary[(i + 1) % blue_boundary.size()]); - } - const std::vector yellow_boundary = calculate_boundary(cones, ConeColor::YELLOW); - for (size_t i = 0; i < yellow_boundary.size(); ++i) { - edges.emplace_back(yellow_boundary[i], yellow_boundary[(i + 1) % yellow_boundary.size()]); - } - cdt.insertEdges(edges); - cdt.eraseOuterTrianglesAndHoles(); - offline_boundary_edges = cdt.fixedEdges; - offline_inner_edges = CDT::extractEdgesFromTriangles(cdt.triangles); - const size_t original_num_edges = offline_inner_edges.size(); - for (const auto e : offline_boundary_edges) { - offline_inner_edges.erase(e); - } - assert(offline_inner_edges.size() + offline_boundary_edges.size() == original_num_edges); - - center_points = {}; - for (const auto e : offline_inner_edges) { - // get center point of e - const Cone& c1 = cones[e.v1()]; - const Cone& c2 = cones[e.v2()]; - center_points.emplace_back((c1.x + c2.x) / 2, (c1.y + c2.y) / 2, ConeColor::CENTER); - } - center_line = calculate_boundary(center_points, ConeColor::CENTER); -} - -const CDT::EdgeUSet& get_offline_edges() { - return offline_inner_edges; -} -const CDT::EdgeUSet& get_boundary_edges() { - return offline_boundary_edges; -} -const std::vector& get_center_points() { - return center_points; -} -const std::vector& get_center_line() { - return center_line; -} - -static CDT::EdgeUSet edges; -void update_cone_positions(const std::vector& cones) { - static CDT::Triangulation cdt; - static size_t seen_cones = 0; - if (seen_cones == cones.size()) { - return; - } - - ScopeTimer s { "online triangulation timer" }; - const auto start = std::next(cones.begin(), static_cast::difference_type>(seen_cones)); - cdt.insertVertices( - start, cones.end(), [](const Cone& c) { return c.x; }, [](const Cone& c) { return c.y; }); - CDT::Triangulation cpy = cdt; - cpy.eraseSuperTriangle(); - edges = CDT::extractEdgesFromTriangles(cpy.triangles); -} - -const CDT::EdgeUSet& get_triangulation() { - return edges; -} ControlOutput compute(const VehicleState& ve) { // t.adj will contain the adj list diff --git a/auto_sim/test_controller_2/controller.hpp b/auto_sim/test_controller_2/controller.hpp index 86c575b..cdf4921 100644 --- a/auto_sim/test_controller_2/controller.hpp +++ b/auto_sim/test_controller_2/controller.hpp @@ -1,7 +1,5 @@ #pragma once -#include "CDT.h" #include "types.hpp" -#include ControlOutput compute(const VehicleState& ve); @@ -10,37 +8,4 @@ enum class DriverlessState { ONLINE_MAPPING, // use for autocross, acceleration, skidpad, etc. TRACKDRIVE_MAPPING, TRACKDRIVE_RUNNING -}; - -// NOTE everything under this line should be private, but we are exposing it for testing purposes -/** - * @param cones cones to calculate the boundary from - * @param c the color of the cones to calculate the boundary for - * @return a vector of vertex indices that form the boundary of the given color. The order of the vertices is not guaranteed to be clockwise or counterclockwise. - */ -std::vector calculate_boundary(const std::vector& cones, ConeColor c); -/** - * This computes the path from the given ALL cones offline. - * @param cones - */ -void compute_path(const std::vector& cones); -/** - * @return offline edges - */ -const CDT::EdgeUSet& get_offline_edges(); -/** - * @return boundary edges - */ -const CDT::EdgeUSet& get_boundary_edges(); -/** - * @return center points - */ -const std::vector& get_center_points(); -/** - * @return center line vertices - */ -const std::vector& get_center_line(); - -// online path planning -void update_cone_positions(const std::vector& cones); -const CDT::EdgeUSet& get_triangulation(); \ No newline at end of file +}; \ No newline at end of file diff --git a/auto_sim/test_controller_2/path.cpp b/auto_sim/test_controller_2/path.cpp new file mode 100644 index 0000000..f00a7c3 --- /dev/null +++ b/auto_sim/test_controller_2/path.cpp @@ -0,0 +1,131 @@ +#include "path.hpp" +#include "scopetimer.hpp" + +std::vector calculate_boundary(const std::vector& cones, const ConeColor c) { + size_t start = 0; + std::unordered_set unvisited_cones {}; + for (size_t i = 0; i < cones.size(); ++i) { + if (cones[i].c == c) { + start = i; + unvisited_cones.insert(i); + } + } + size_t at = start; + + std::vector path; + path.reserve(unvisited_cones.size()); + path.push_back(at); + unvisited_cones.erase(at); + while (not unvisited_cones.empty()) { + // find cone with shortest distance to cones[at] + size_t closest_cone = 0; + double closest_dist = std::numeric_limits::max(); + for (const size_t i : unvisited_cones) { + if (const double dist = std::hypot(cones[at].x - cones[i].x, cones[at].y - cones[i].y); dist < closest_dist) { + closest_dist = dist; + closest_cone = i; + } + } + if (closest_cone == start) { + break; + } + unvisited_cones.erase(closest_cone); + path.push_back(closest_cone); + at = closest_cone; + } + + return path; +} + +static CDT::EdgeUSet offline_inner_edges; +static CDT::EdgeUSet offline_boundary_edges; +static std::vector center_points; +static std::vector center_line; +static double get_cone_x(const Cone& c) { + return c.x; +} +static double get_cone_y(const Cone& c) { + return c.y; +} +void compute_path(const std::vector& cones) { + ScopeTimer s { "offline triangulation timer" }; + CDT::Triangulation cdt; + cdt.insertVertices(cones.begin(), cones.end(), get_cone_x, get_cone_y); + + // calculate boundary edges and insert them into the triangulation + std::vector edges {}; + const std::vector blue_boundary = calculate_boundary(cones, ConeColor::BLUE); + for (size_t i = 0; i < blue_boundary.size(); ++i) { + edges.emplace_back(blue_boundary[i], blue_boundary[(i + 1) % blue_boundary.size()]); + } + const std::vector yellow_boundary = calculate_boundary(cones, ConeColor::YELLOW); + for (size_t i = 0; i < yellow_boundary.size(); ++i) { + edges.emplace_back(yellow_boundary[i], yellow_boundary[(i + 1) % yellow_boundary.size()]); + } + cdt.insertEdges(edges); + cdt.eraseOuterTrianglesAndHoles(); + offline_boundary_edges = cdt.fixedEdges; + offline_inner_edges = CDT::extractEdgesFromTriangles(cdt.triangles); + const size_t original_num_edges = offline_inner_edges.size(); + for (const auto e : offline_boundary_edges) { + offline_inner_edges.erase(e); + } + assert(offline_inner_edges.size() + offline_boundary_edges.size() == original_num_edges); + + center_points = {}; + for (const auto e : offline_inner_edges) { + // get center point of e + const Cone& c1 = cones[e.v1()]; + const Cone& c2 = cones[e.v2()]; + center_points.emplace_back((c1.x + c2.x) / 2, (c1.y + c2.y) / 2, ConeColor::CENTER); + } + center_line = calculate_boundary(center_points, ConeColor::CENTER); +} + +const CDT::EdgeUSet& get_offline_edges() { + return offline_inner_edges; +} +const CDT::EdgeUSet& get_boundary_edges() { + return offline_boundary_edges; +} +const std::vector& get_center_points() { + return center_points; +} +const std::vector& get_center_line() { + return center_line; +} + +static CDT::EdgeUSet edges; +void update_cone_positions(const std::vector& cones) { + static CDT::Triangulation cdt; + static size_t seen_cones = 0; + if (seen_cones == cones.size()) { + return; + } + + ScopeTimer s { "online triangulation timer" }; + const auto start = std::next(cones.begin(), static_cast::difference_type>(seen_cones)); + cdt.insertVertices( + start, cones.end(), [](const Cone& c) { return c.x; }, [](const Cone& c) { return c.y; }); + CDT::Triangulation cpy = cdt; + cpy.eraseSuperTriangle(); + edges = CDT::extractEdgesFromTriangles(cpy.triangles); +} + +const CDT::EdgeUSet& get_triangulation() { + return edges; +} + +/** + * Projects point at (x,y) to the line defined by c0 and c1. + * @param x Point to project x coordinate + * @param y Point to project y coordinate + * @param c0 First point defining the line to project onto + * @param c1 Second point defining the line to project onto + * @return the parameter t such that the projected point is at (1-t)*c0 + t*c1 + */ +double project(const double x, const double y, const Cone& c0, const Cone& c1) { + const double x_min_x0 = x - c0.x; + const double y_min_y0 = y - c0.y; + return x_min_x0 * (c1.x - c0.x) + y_min_y0 * (c1.y - c0.y); +} \ No newline at end of file diff --git a/auto_sim/test_controller_2/path.hpp b/auto_sim/test_controller_2/path.hpp new file mode 100644 index 0000000..0639ae7 --- /dev/null +++ b/auto_sim/test_controller_2/path.hpp @@ -0,0 +1,32 @@ +#pragma once + +#include "CDT.h" +#include "types.hpp" +#include + +/** + * @param cones cones to calculate the boundary from + * @param c the color of the cones to calculate the boundary for + * @return a vector of vertex indices that form the boundary of the given color. The order of the vertices is not guaranteed to be clockwise or counterclockwise. + */ +std::vector calculate_boundary(const std::vector& cones, ConeColor c); + +/** + * This computes the path from the given ALL cones offline. + * @param cones + */ +void compute_path(const std::vector& cones); + +/** + * @param cones + */ +void update_cone_positions(const std::vector& cones); + +// NOTE everything under this line should be private, but we are exposing it for testing purposes +const CDT::EdgeUSet& get_offline_edges(); +const CDT::EdgeUSet& get_boundary_edges(); +const std::vector& get_center_points(); +const std::vector& get_center_line(); +const CDT::EdgeUSet& get_triangulation(); + +double project(double x, double y, const Cone& c0, const Cone& c1); \ No newline at end of file diff --git a/auto_sim/test_controller_2/pybind.cpp b/auto_sim/test_controller_2/pybind.cpp index d6532e6..8b775ad 100644 --- a/auto_sim/test_controller_2/pybind.cpp +++ b/auto_sim/test_controller_2/pybind.cpp @@ -2,6 +2,7 @@ #include #include "controller.hpp" +#include "path.hpp" #include "sim.hpp" #include "types.hpp" @@ -60,6 +61,9 @@ PYBIND11_MODULE(Controller, m, py::mod_gil_not_used()) { m.def("get_center_line", get_center_line, py::return_value_policy::reference, R"pbdoc()) Get the center line calculated from compute_path() )pbdoc"); + m.def("project", &project, R"pbdoc()) + Project a point onto the center line + )pbdoc"); // online path caclulations m.def("update_cone_positions", &update_cone_positions, R"pbdoc() diff --git a/auto_sim/test_controller_2/setup.py b/auto_sim/test_controller_2/setup.py index 0fe06e2..c0fa05e 100644 --- a/auto_sim/test_controller_2/setup.py +++ b/auto_sim/test_controller_2/setup.py @@ -7,7 +7,7 @@ ext_modules = [ Pybind11Extension( "Controller", - ["pybind.cpp", "sim.cpp", "controller.cpp"], + ["pybind.cpp", "sim.cpp", "controller.cpp", "path.cpp"], include_dirs=[".", "build/_deps/cdt-src/CDT/include"], language="c++", ), From 242e9d6246d511ad65cdeaa6fcffabddb0a1f1a1 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Tue, 7 Apr 2026 18:58:16 -0700 Subject: [PATCH 43/45] wow --- auto_sim/test_controller_2/CMakeLists.txt | 25 +++- auto_sim/test_controller_2/path.cpp | 142 +++++++++++++--------- auto_sim/test_controller_2/path.hpp | 42 ++++--- auto_sim/test_controller_2/perception.cpp | 1 + auto_sim/test_controller_2/perception.hpp | 9 ++ auto_sim/test_controller_2/pybind.cpp | 44 ++++--- auto_sim/test_controller_2/setup.py | 2 +- 7 files changed, 166 insertions(+), 99 deletions(-) create mode 100644 auto_sim/test_controller_2/perception.cpp create mode 100644 auto_sim/test_controller_2/perception.hpp diff --git a/auto_sim/test_controller_2/CMakeLists.txt b/auto_sim/test_controller_2/CMakeLists.txt index d2012aa..bdc680d 100644 --- a/auto_sim/test_controller_2/CMakeLists.txt +++ b/auto_sim/test_controller_2/CMakeLists.txt @@ -24,7 +24,30 @@ CPMAddPackage( GIT_TAG 1.4.4 DOWNLOAD_ONLY TRUE ) +CPMAddPackage( + NAME EIGEN + GIT_REPOSITORY https://gitlab.com/libeigen/eigen + GIT_TAG 3147391d946bb4b6c68edd901f2add6ac1f31f8c + GIT_SHALLOW TRUE + DOWNLOAD_ONLY TRUE +) +set(EIGEN_INCLUDE_DIRS ${EIGEN_SOURCE_DIR}) +add_library(eigen_interface INTERFACE) +target_include_directories(eigen_interface SYSTEM INTERFACE ${EIGEN_INCLUDE_DIRS}) +target_compile_definitions(eigen_interface INTERFACE + EIGEN_STACK_ALLOCATION_LIMIT=0 + EIGEN_MAX_STATIC_ALIGN_BYTES=0 +) + +CPMAddPackage( + NAME alglib + URL https://www.alglib.net/translator/re/alglib-4.07.0.cpp.gpl.zip + DOWNLOAD_ONLY TRUE +) +add_library(alglib_interface INTERFACE) +target_include_directories(alglib_interface INTERFACE ${alglib_SOURCE_DIR}/src) file(GLOB SOURCES *.cpp) pybind11_add_module(controller ${SOURCES}) -target_include_directories(controller PRIVATE ${CDT_SOURCE_DIR}/CDT/include) \ No newline at end of file +target_include_directories(controller PRIVATE ${CDT_SOURCE_DIR}/CDT/include) +target_link_libraries(controller PRIVATE eigen_interface alglib_interface) \ No newline at end of file diff --git a/auto_sim/test_controller_2/path.cpp b/auto_sim/test_controller_2/path.cpp index f00a7c3..0e5a720 100644 --- a/auto_sim/test_controller_2/path.cpp +++ b/auto_sim/test_controller_2/path.cpp @@ -1,9 +1,11 @@ #include "path.hpp" +#include "perception.hpp" #include "scopetimer.hpp" +#include -std::vector calculate_boundary(const std::vector& cones, const ConeColor c) { +static std::vector calculate_boundary(const std::vector& cones, const ConeColor c) { size_t start = 0; - std::unordered_set unvisited_cones {}; + std::unordered_set unvisited_cones { }; for (size_t i = 0; i < cones.size(); ++i) { if (cones[i].c == c) { start = i; @@ -37,23 +39,26 @@ std::vector calculate_boundary(const std::vector& cones, con return path; } -static CDT::EdgeUSet offline_inner_edges; -static CDT::EdgeUSet offline_boundary_edges; -static std::vector center_points; -static std::vector center_line; -static double get_cone_x(const Cone& c) { - return c.x; +static float mod(const float a, const float b) { + return a - b * std::floor(a / b); } -static double get_cone_y(const Cone& c) { - return c.y; +static double mod(const double a, const double b) { + return a - b * std::floor(a / b); } -void compute_path(const std::vector& cones) { - ScopeTimer s { "offline triangulation timer" }; - CDT::Triangulation cdt; - cdt.insertVertices(cones.begin(), cones.end(), get_cone_x, get_cone_y); + +void compute_path_from_percepted_cones() { + ScopeTimer s { "compute_path timer" }; + static CDT::Triangulation cdt_cache; + static size_t seen_cones = 0; + if (seen_cones == cones.size()) { + return; + } + const auto start = std::next(cones.begin(), static_cast::difference_type>(seen_cones)); + cdt_cache.insertVertices( + start, cones.end(), [](const Cone& c) { return c.x; }, [](const Cone& c) { return c.y; }); // calculate boundary edges and insert them into the triangulation - std::vector edges {}; + std::vector edges { }; const std::vector blue_boundary = calculate_boundary(cones, ConeColor::BLUE); for (size_t i = 0; i < blue_boundary.size(); ++i) { edges.emplace_back(blue_boundary[i], blue_boundary[(i + 1) % blue_boundary.size()]); @@ -62,6 +67,8 @@ void compute_path(const std::vector& cones) { for (size_t i = 0; i < yellow_boundary.size(); ++i) { edges.emplace_back(yellow_boundary[i], yellow_boundary[(i + 1) % yellow_boundary.size()]); } + // compute corresponding triangulation + CDT::Triangulation cdt = cdt_cache; cdt.insertEdges(edges); cdt.eraseOuterTrianglesAndHoles(); offline_boundary_edges = cdt.fixedEdges; @@ -72,60 +79,79 @@ void compute_path(const std::vector& cones) { } assert(offline_inner_edges.size() + offline_boundary_edges.size() == original_num_edges); - center_points = {}; + // construct center line + center_points = { }; for (const auto e : offline_inner_edges) { // get center point of e const Cone& c1 = cones[e.v1()]; const Cone& c2 = cones[e.v2()]; center_points.emplace_back((c1.x + c2.x) / 2, (c1.y + c2.y) / 2, ConeColor::CENTER); } - center_line = calculate_boundary(center_points, ConeColor::CENTER); -} + center_line_idxs = calculate_boundary(center_points, ConeColor::CENTER); -const CDT::EdgeUSet& get_offline_edges() { - return offline_inner_edges; -} -const CDT::EdgeUSet& get_boundary_edges() { - return offline_boundary_edges; -} -const std::vector& get_center_points() { - return center_points; -} -const std::vector& get_center_line() { - return center_line; -} + // center line parameterization prefix + alglib::real_1d_array center_line_len_prefix; + center_line_len_prefix.setlength(center_line_idxs.size() + 1); + for (size_t i = 1; i <= center_line_idxs.size(); ++i) { + const Cone& c1 = center_points[center_line_idxs[i - 1]]; + const Cone& c2 = center_points[center_line_idxs[i]]; + center_line_len_prefix[i] = center_line_len_prefix[i - 1] + std::hypot(c1.x - c2.x, c1.y - c2.y); + } + center_line_length = center_line_len_prefix[center_line_idxs.size()]; -static CDT::EdgeUSet edges; -void update_cone_positions(const std::vector& cones) { - static CDT::Triangulation cdt; - static size_t seen_cones = 0; - if (seen_cones == cones.size()) { - return; + // draw a spline between all the center points + alglib::real_1d_array xs, ys { }; + xs.setlength(center_line_idxs.size() + 1); + ys.setlength(center_line_idxs.size() + 1); + for (size_t i = 0; i < center_line_idxs.size(); ++i) { + const Cone& c = center_points[center_line_idxs[i]]; + xs[i] = c.x; + ys[i] = c.y; } + // cycle around + xs[center_line_idxs.size()] = center_points[center_line_idxs[0]].x; + ys[center_line_idxs.size()] = center_points[center_line_idxs[0]].y; - ScopeTimer s { "online triangulation timer" }; - const auto start = std::next(cones.begin(), static_cast::difference_type>(seen_cones)); - cdt.insertVertices( - start, cones.end(), [](const Cone& c) { return c.x; }, [](const Cone& c) { return c.y; }); - CDT::Triangulation cpy = cdt; - cpy.eraseSuperTriangle(); - edges = CDT::extractEdgesFromTriangles(cpy.triangles); + alglib::spline1dbuildcubic(xs, center_line_len_prefix, x_spline); + alglib::spline1dbuildcubic(ys, center_line_len_prefix, y_spline); } -const CDT::EdgeUSet& get_triangulation() { - return edges; +double project(const double x, const double y) { + static constexpr uint32_t samples = 10; + double best_dist = std::numeric_limits::max(), best_t = 0; + for (double at_t = 0; at_t <= center_line_length; at_t += center_line_length / samples) { // NOLINT(*-flp30-c) + if (const double dist = std::hypot( + alglib::spline1dcalc(x_spline, at_t) - x, + alglib::spline1dcalc(y_spline, at_t) - y); + dist < best_dist) { + best_t = project(x, y, at_t); + best_dist = std::hypot( + alglib::spline1dcalc(x_spline, best_t) - x, + alglib::spline1dcalc(y_spline, best_t) - y); + } + } + return best_t; } +double project(const double x, const double y, double at_t) { + // just good to double check + assert(0 <= at_t); + assert(at_t <= center_line_length); + + double dd; + double at_x, at_dx; + alglib::spline1ddiff(x_spline, at_t, at_x, at_dx, dd); + double at_y, at_dy; + alglib::spline1ddiff(y_spline, at_t, at_y, at_dy, dd); -/** - * Projects point at (x,y) to the line defined by c0 and c1. - * @param x Point to project x coordinate - * @param y Point to project y coordinate - * @param c0 First point defining the line to project onto - * @param c1 Second point defining the line to project onto - * @return the parameter t such that the projected point is at (1-t)*c0 + t*c1 - */ -double project(const double x, const double y, const Cone& c0, const Cone& c1) { - const double x_min_x0 = x - c0.x; - const double y_min_y0 = y - c0.y; - return x_min_x0 * (c1.x - c0.x) + y_min_y0 * (c1.y - c0.y); -} \ No newline at end of file + // newton stepping ???? + for (uint32_t i = 0; i < 20; i++) { + const double f_prime = 2 * (at_x * at_dy + at_y * at_dx) - (x * at_dy + y * at_dx); + const double f = (at_x * at_dy + at_y * at_dx) - (x * at_dy + y * at_dx); + const double step = f / f_prime; + at_t -= step; + if (step < 1e-6) { + return at_t; + } + } + throw std::runtime_error("project did not converge"); +} diff --git a/auto_sim/test_controller_2/path.hpp b/auto_sim/test_controller_2/path.hpp index 0639ae7..0b52c2d 100644 --- a/auto_sim/test_controller_2/path.hpp +++ b/auto_sim/test_controller_2/path.hpp @@ -1,32 +1,36 @@ #pragma once #include "CDT.h" +#include "interpolation.h" #include "types.hpp" + #include /** - * @param cones cones to calculate the boundary from - * @param c the color of the cones to calculate the boundary for - * @return a vector of vertex indices that form the boundary of the given color. The order of the vertices is not guaranteed to be clockwise or counterclockwise. + * This computes the path from the given ALL cones offline. */ -std::vector calculate_boundary(const std::vector& cones, ConeColor c); - +void compute_path_from_percepted_cones(); /** - * This computes the path from the given ALL cones offline. - * @param cones + * Note this overload is MUCH faster than without at_t + * @param x x position + * @param y y position + * @param at_t please use this to seed the optimization algorithm + * @return t s.t. the point on the center line that is closest to (x, y) in terms of Euclidean distance */ -void compute_path(const std::vector& cones); - +double project(double x, double y, double at_t = 0); /** - * @param cones + * this variant tries a bunch of values of at_t, between [0, center_line_length], and returns the best one. + * this is more expensive but can be more robust if the center line is very curvy and the optimization algorithm gets stuck in a local minimum. + * @param x x position + * @param y y position + * @return */ -void update_cone_positions(const std::vector& cones); - -// NOTE everything under this line should be private, but we are exposing it for testing purposes -const CDT::EdgeUSet& get_offline_edges(); -const CDT::EdgeUSet& get_boundary_edges(); -const std::vector& get_center_points(); -const std::vector& get_center_line(); -const CDT::EdgeUSet& get_triangulation(); +double project(double x, double y); -double project(double x, double y, const Cone& c0, const Cone& c1); \ No newline at end of file +// for debugging purposes only +inline CDT::EdgeUSet offline_boundary_edges; +inline CDT::EdgeUSet offline_inner_edges; +inline std::vector center_points; +inline std::vector center_line_idxs; +inline alglib::spline1dinterpolant x_spline, y_spline; +inline double center_line_length; \ No newline at end of file diff --git a/auto_sim/test_controller_2/perception.cpp b/auto_sim/test_controller_2/perception.cpp new file mode 100644 index 0000000..f90e7de --- /dev/null +++ b/auto_sim/test_controller_2/perception.cpp @@ -0,0 +1 @@ +#include "perception.hpp" diff --git a/auto_sim/test_controller_2/perception.hpp b/auto_sim/test_controller_2/perception.hpp new file mode 100644 index 0000000..d60fd76 --- /dev/null +++ b/auto_sim/test_controller_2/perception.hpp @@ -0,0 +1,9 @@ +#pragma once + +#include "types.hpp" +#include + +inline std::vector cones {}; +inline void mock_perception(std::vector&& new_cones) { + cones = std::move(new_cones); +} \ No newline at end of file diff --git a/auto_sim/test_controller_2/pybind.cpp b/auto_sim/test_controller_2/pybind.cpp index 8b775ad..8da229f 100644 --- a/auto_sim/test_controller_2/pybind.cpp +++ b/auto_sim/test_controller_2/pybind.cpp @@ -1,6 +1,7 @@ #include #include +#include "CDT.h" #include "controller.hpp" #include "path.hpp" #include "sim.hpp" @@ -42,36 +43,39 @@ PYBIND11_MODULE(Controller, m, py::mod_gil_not_used()) { Compute control output )pbdoc"); - // offline path calculations - m.def("calculate_boundary", &calculate_boundary, R"pbdoc( - Calculate the boundary of the given color from the list of cones - )pbdoc"); - m.def("compute_path", &compute_path, R"pbdoc() + m.def("compute_path", &compute_path_from_percepted_cones, R"pbdoc() Compute the triangulation from a list of cones )pbdoc"); - m.def("get_offline_edges", &get_offline_edges, py::return_value_policy::reference, R"pbdoc() + + // debugging visualization purposes + m.def( + "get_offline_edges", [] -> CDT::EdgeUSet& { + return offline_inner_edges; + }, + py::return_value_policy::reference, R"pbdoc() Get the offline edges calculated from compute_path() )pbdoc"); - m.def("get_boundary_edges", &get_boundary_edges, py::return_value_policy::reference, R"pbdoc() + m.def( + "get_boundary_edges", [] -> CDT::EdgeUSet& { + return offline_boundary_edges; + }, + py::return_value_policy::reference, R"pbdoc() Get the boundary edges calculated from compute_path() )pbdoc"); - m.def("get_center_points", &get_center_points, py::return_value_policy::reference, R"pbdoc()) + m.def( + "get_center_points", [] -> std::vector& { + return center_points; + }, + py::return_value_policy::reference, R"pbdoc()) Get the center points calculated from compute_path() )pbdoc"); - m.def("get_center_line", get_center_line, py::return_value_policy::reference, R"pbdoc()) + m.def( + "get_center_line", [] -> std::vector& { + return center_line_idxs; + }, + py::return_value_policy::reference, R"pbdoc()) Get the center line calculated from compute_path() )pbdoc"); - m.def("project", &project, R"pbdoc()) - Project a point onto the center line - )pbdoc"); - - // online path caclulations - m.def("update_cone_positions", &update_cone_positions, R"pbdoc() - Update the triangulation with new cone positions - )pbdoc"); - m.def("get_triangulation", &get_triangulation, py::return_value_policy::reference, R"pbdoc( - Get the current triangulation object) - )pbdoc"); m.def("sim_step", &sim_step, R"pbdoc( Simulate one step of the vehicle dynamics diff --git a/auto_sim/test_controller_2/setup.py b/auto_sim/test_controller_2/setup.py index c0fa05e..b604625 100644 --- a/auto_sim/test_controller_2/setup.py +++ b/auto_sim/test_controller_2/setup.py @@ -7,7 +7,7 @@ ext_modules = [ Pybind11Extension( "Controller", - ["pybind.cpp", "sim.cpp", "controller.cpp", "path.cpp"], + ["pybind.cpp", "sim.cpp", "controller.cpp", "path.cpp", "perception.cpp"], include_dirs=[".", "build/_deps/cdt-src/CDT/include"], language="c++", ), From 6301414638214835e29f60854e3a9aaa11d1b446 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Tue, 7 Apr 2026 22:22:43 -0700 Subject: [PATCH 44/45] p --- auto_sim/main.py | 5 +- auto_sim/render.py | 23 ++++++- auto_sim/test_controller_2/CMakeLists.txt | 2 +- auto_sim/test_controller_2/path.cpp | 71 ++++++++++++---------- auto_sim/test_controller_2/path.hpp | 14 +++-- auto_sim/test_controller_2/perception.hpp | 4 +- auto_sim/test_controller_2/pybind.cpp | 74 +++++++++++++++++------ auto_sim/test_controller_2/setup.py | 14 ++++- 8 files changed, 141 insertions(+), 66 deletions(-) diff --git a/auto_sim/main.py b/auto_sim/main.py index cbb1952..8e12cf8 100644 --- a/auto_sim/main.py +++ b/auto_sim/main.py @@ -1,5 +1,5 @@ import pygame -from Controller import compute, sim_step, Cone, ConeColor, VehicleState, compute_path +from Controller import compute, sim_step, Cone, ConeColor, VehicleState, mock_perception, compute_path from render import init, render_world import ctypes import platform @@ -50,7 +50,8 @@ def handle_key(key: int, state: VehicleState): def simulate_cone_detection(state: VehicleState): global ran if not ran: - compute_path(CONE_POSITIONS) + mock_perception(CONE_POSITIONS) + compute_path() ran = True return CONE_POSITIONS diff --git a/auto_sim/render.py b/auto_sim/render.py index c709418..cb0ad33 100644 --- a/auto_sim/render.py +++ b/auto_sim/render.py @@ -1,7 +1,7 @@ import pygame from scipy.spatial.transform import Rotation as R from constants import VEHICLE_WIDTH_M -from Controller import VehicleState, Cone, ConeColor, get_offline_edges, get_center_line, get_center_points, get_boundary_edges, project +from Controller import VehicleState, Cone, ConeColor, spline_t, get_center_line_length, project, project_seeded from math import ceil, degrees import numpy as np @@ -81,9 +81,9 @@ def int_to_color(value: ConeColor) -> str: return "white" # boundary = None - +at_t = None def render_world(vehicle_state: VehicleState, cones: list[Cone], screen: pygame.Surface): - global w, h + global w, h, at_t w, h = screen.get_width(), screen.get_height() # grid for context @@ -127,6 +127,23 @@ def render_world(vehicle_state: VehicleState, cones: list[Cone], screen: pygame. # transform(v2.x, v2.y, vehicle_state), 2 # ) + # if at_t is None: + # else: + # at_t = project_seeded(vehicle_state.x, vehicle_state.y, at_t) + at_t = project(vehicle_state.x, vehicle_state.y) + x = spline_t(at_t) + pygame.draw.circle(screen, "red", transform(x.x, x.y, vehicle_state), 5) + + # DRAW SPLINE + # steps = 10 + # dt = get_center_line_length() / steps + # for t in range(steps): + # x1 = spline_t(t * dt) + # x2= spline_t((t + 1) * dt) + # pygame.draw.line(screen, "green", + # transform(x1.x, x1.y, vehicle_state), + # transform(x2.x, x2.y, vehicle_state), 2) + for cone in cones: pygame.draw.circle( screen, int_to_color(cone.c), diff --git a/auto_sim/test_controller_2/CMakeLists.txt b/auto_sim/test_controller_2/CMakeLists.txt index bdc680d..104c72a 100644 --- a/auto_sim/test_controller_2/CMakeLists.txt +++ b/auto_sim/test_controller_2/CMakeLists.txt @@ -47,7 +47,7 @@ CPMAddPackage( add_library(alglib_interface INTERFACE) target_include_directories(alglib_interface INTERFACE ${alglib_SOURCE_DIR}/src) -file(GLOB SOURCES *.cpp) +file(GLOB SOURCES *.cpp ${alglib_SOURCE_DIR}/src/*.cpp) pybind11_add_module(controller ${SOURCES}) target_include_directories(controller PRIVATE ${CDT_SOURCE_DIR}/CDT/include) target_link_libraries(controller PRIVATE eigen_interface alglib_interface) \ No newline at end of file diff --git a/auto_sim/test_controller_2/path.cpp b/auto_sim/test_controller_2/path.cpp index 0e5a720..d8b40f7 100644 --- a/auto_sim/test_controller_2/path.cpp +++ b/auto_sim/test_controller_2/path.cpp @@ -39,11 +39,9 @@ static std::vector calculate_boundary(const std::vector& con return path; } -static float mod(const float a, const float b) { - return a - b * std::floor(a / b); -} static double mod(const double a, const double b) { - return a - b * std::floor(a / b); + const double result = std::fmod(a, b); + return result >= 0 ? result : result + b; } void compute_path_from_percepted_cones() { @@ -92,15 +90,16 @@ void compute_path_from_percepted_cones() { // center line parameterization prefix alglib::real_1d_array center_line_len_prefix; center_line_len_prefix.setlength(center_line_idxs.size() + 1); + center_line_len_prefix[0] = 0; for (size_t i = 1; i <= center_line_idxs.size(); ++i) { const Cone& c1 = center_points[center_line_idxs[i - 1]]; - const Cone& c2 = center_points[center_line_idxs[i]]; + const Cone& c2 = center_points[center_line_idxs[i % center_line_idxs.size()]]; center_line_len_prefix[i] = center_line_len_prefix[i - 1] + std::hypot(c1.x - c2.x, c1.y - c2.y); } center_line_length = center_line_len_prefix[center_line_idxs.size()]; // draw a spline between all the center points - alglib::real_1d_array xs, ys { }; + alglib::real_1d_array xs, ys; xs.setlength(center_line_idxs.size() + 1); ys.setlength(center_line_idxs.size() + 1); for (size_t i = 0; i < center_line_idxs.size(); ++i) { @@ -112,46 +111,54 @@ void compute_path_from_percepted_cones() { xs[center_line_idxs.size()] = center_points[center_line_idxs[0]].x; ys[center_line_idxs.size()] = center_points[center_line_idxs[0]].y; - alglib::spline1dbuildcubic(xs, center_line_len_prefix, x_spline); - alglib::spline1dbuildcubic(ys, center_line_len_prefix, y_spline); + alglib::spline1dbuildcubic(center_line_len_prefix, xs, x_spline); + alglib::spline1dbuildcubic(center_line_len_prefix, ys, y_spline); } double project(const double x, const double y) { - static constexpr uint32_t samples = 10; + const ScopeTimer s { "project timer" }; + static constexpr uint32_t samples = 50; + static constexpr double eps = 0; + double best_dist = std::numeric_limits::max(), best_t = 0; - for (double at_t = 0; at_t <= center_line_length; at_t += center_line_length / samples) { // NOLINT(*-flp30-c) - if (const double dist = std::hypot( - alglib::spline1dcalc(x_spline, at_t) - x, - alglib::spline1dcalc(y_spline, at_t) - y); - dist < best_dist) { - best_t = project(x, y, at_t); - best_dist = std::hypot( - alglib::spline1dcalc(x_spline, best_t) - x, - alglib::spline1dcalc(y_spline, best_t) - y); + const double step_size = center_line_length / samples - eps; + assert(step_size > 0); + for (double at_t = eps; at_t < center_line_length; at_t += step_size) { // NOLINT(*-flp30-c) + const double t = project(x, y, at_t); + const double d = std::hypot( + alglib::spline1dcalc(x_spline, t) - x, + alglib::spline1dcalc(y_spline, t) - y); + if (d < best_dist) { + best_dist = d; + best_t = t; } } return best_t; } -double project(const double x, const double y, double at_t) { +double project(const double x0, const double y0, double t) { // just good to double check - assert(0 <= at_t); - assert(at_t <= center_line_length); - - double dd; - double at_x, at_dx; - alglib::spline1ddiff(x_spline, at_t, at_x, at_dx, dd); - double at_y, at_dy; - alglib::spline1ddiff(y_spline, at_t, at_y, at_dy, dd); + assert(0 <= t); + assert(t <= center_line_length); // newton stepping ???? for (uint32_t i = 0; i < 20; i++) { - const double f_prime = 2 * (at_x * at_dy + at_y * at_dx) - (x * at_dy + y * at_dx); - const double f = (at_x * at_dy + at_y * at_dx) - (x * at_dy + y * at_dx); + double l_x, l_dx, l_ddx; + alglib::spline1ddiff(x_spline, t, l_x, l_dx, l_ddx); + double l_y, l_dy, l_ddy; + alglib::spline1ddiff(y_spline, t, l_y, l_dy, l_ddy); + // ||l'(t)||_2 + (l(t) - x) dot l''(t) + const double f_prime = (l_dx * l_dx + l_dy * l_dy) + ((l_x - x0) * l_ddx + (l_y - y0) * l_ddy); + // (l(t) - x) dot l'(t) + const double f = (l_x - x0) * l_dx + (l_y - y0) * l_dy; const double step = f / f_prime; - at_t -= step; + t = mod(t - step, center_line_length); if (step < 1e-6) { - return at_t; + return t; } } - throw std::runtime_error("project did not converge"); + throw std::exception("project did not converge"); +} + +Location spline_t(const double t) { + return { alglib::spline1dcalc(x_spline, t), alglib::spline1dcalc(y_spline, t) }; } diff --git a/auto_sim/test_controller_2/path.hpp b/auto_sim/test_controller_2/path.hpp index 0b52c2d..3a7f3f2 100644 --- a/auto_sim/test_controller_2/path.hpp +++ b/auto_sim/test_controller_2/path.hpp @@ -12,12 +12,12 @@ void compute_path_from_percepted_cones(); /** * Note this overload is MUCH faster than without at_t - * @param x x position - * @param y y position - * @param at_t please use this to seed the optimization algorithm + * @param x0 x position + * @param y0 y position + * @param t please use this to seed the optimization algorithm * @return t s.t. the point on the center line that is closest to (x, y) in terms of Euclidean distance */ -double project(double x, double y, double at_t = 0); +double project(double x0, double y0, double t); /** * this variant tries a bunch of values of at_t, between [0, center_line_length], and returns the best one. * this is more expensive but can be more robust if the center line is very curvy and the optimization algorithm gets stuck in a local minimum. @@ -27,6 +27,12 @@ double project(double x, double y, double at_t = 0); */ double project(double x, double y); +struct Location { + double x; + double y; +}; +Location spline_t(double t); + // for debugging purposes only inline CDT::EdgeUSet offline_boundary_edges; inline CDT::EdgeUSet offline_inner_edges; diff --git a/auto_sim/test_controller_2/perception.hpp b/auto_sim/test_controller_2/perception.hpp index d60fd76..377c27c 100644 --- a/auto_sim/test_controller_2/perception.hpp +++ b/auto_sim/test_controller_2/perception.hpp @@ -3,7 +3,7 @@ #include "types.hpp" #include -inline std::vector cones {}; -inline void mock_perception(std::vector&& new_cones) { +inline std::vector cones { }; +inline void mock_perception(std::vector new_cones) { cones = std::move(new_cones); } \ No newline at end of file diff --git a/auto_sim/test_controller_2/pybind.cpp b/auto_sim/test_controller_2/pybind.cpp index 8da229f..d7e0068 100644 --- a/auto_sim/test_controller_2/pybind.cpp +++ b/auto_sim/test_controller_2/pybind.cpp @@ -4,11 +4,28 @@ #include "CDT.h" #include "controller.hpp" #include "path.hpp" +#include "perception.hpp" #include "sim.hpp" #include "types.hpp" namespace py = pybind11; +static CDT::EdgeUSet& get_offline_edges() { + return offline_inner_edges; +} +static CDT::EdgeUSet& get_boundary_edges() { + return offline_boundary_edges; +} +static std::vector& get_center_points() { + return center_points; +} +static std::vector& get_center_line() { + return center_line_idxs; +} +static double get_center_line_length() { + return center_line_length; +} + PYBIND11_MODULE(Controller, m, py::mod_gil_not_used()) { py::class_(m, "ControlOutput") .def(py::init()) @@ -39,6 +56,11 @@ PYBIND11_MODULE(Controller, m, py::mod_gil_not_used()) { .def("v1", &CDT::Edge::v1) .def("v2", &CDT::Edge::v2); + py::class_(m, "Location") + .def(py::init()) + .def_readwrite("x", &Location::x) + .def_readwrite("y", &Location::y); + m.def("compute", &compute, R"pbdoc( Compute control output )pbdoc"); @@ -48,36 +70,48 @@ PYBIND11_MODULE(Controller, m, py::mod_gil_not_used()) { )pbdoc"); // debugging visualization purposes - m.def( - "get_offline_edges", [] -> CDT::EdgeUSet& { - return offline_inner_edges; - }, - py::return_value_policy::reference, R"pbdoc() + m.def("get_offline_edges", &get_offline_edges, py::return_value_policy::reference, R"pbdoc() Get the offline edges calculated from compute_path() )pbdoc"); - m.def( - "get_boundary_edges", [] -> CDT::EdgeUSet& { - return offline_boundary_edges; - }, - py::return_value_policy::reference, R"pbdoc() + m.def("get_boundary_edges", &get_boundary_edges, py::return_value_policy::reference, R"pbdoc( Get the boundary edges calculated from compute_path() )pbdoc"); - m.def( - "get_center_points", [] -> std::vector& { - return center_points; - }, - py::return_value_policy::reference, R"pbdoc()) + m.def("get_center_points", &get_center_points, py::return_value_policy::reference, R"pbdoc( Get the center points calculated from compute_path() )pbdoc"); - m.def( - "get_center_line", [] -> std::vector& { - return center_line_idxs; - }, - py::return_value_policy::reference, R"pbdoc()) + m.def("get_center_line", &get_center_line, py::return_value_policy::reference, R"pbdoc( Get the center line calculated from compute_path() )pbdoc"); + m.def("get_center_line_length", &get_center_line_length, R"pbdoc( + Get the length of the center line calculated from compute_path() + )pbdoc"); m.def("sim_step", &sim_step, R"pbdoc( Simulate one step of the vehicle dynamics )pbdoc"); + + m.def("project", [](const double a, const double b) { return project(a, b); }, R"pbdoc( + Project a point onto a line defined by two points + )pbdoc"); + m.def("project_seeded", [](const double a, const double b, const double c) { return project(a, b, c); }, R"pbdoc() + Project a point onto a line defined by two points, with a seed for the optimization algorithm + )pbdoc"); + m.def("spline_t", &spline_t, R"pbdoc() + Get the point on the center line corresponding to a given t + )pbdoc"); + + m.def("mock_perception", &mock_perception, R"pbdoc( + Mock the perception module by directly setting the list of cones + )pbdoc"); + + static py::exception ex(m, "Alglib::ApError"); + py::register_exception_translator([](std::exception_ptr p) { + try { + if (p) + std::rethrow_exception(p); + } catch (const alglib::ap_error& e) { + // Set the Python error using a custom field instead of .what() + ex(e.msg.c_str()); + } + }); } \ No newline at end of file diff --git a/auto_sim/test_controller_2/setup.py b/auto_sim/test_controller_2/setup.py index b604625..5179022 100644 --- a/auto_sim/test_controller_2/setup.py +++ b/auto_sim/test_controller_2/setup.py @@ -4,11 +4,21 @@ __version__ = "0.0.1" +# get all sources in the build/_deps/alglib-src/alglib/src directory +import os + +alglib_srcs = [] +for root, dirs, files in os.walk("build/_deps/alglib-src/src"): + for file in files: + if file.endswith(".cpp"): + alglib_srcs.append(os.path.join(root, file)) +print(alglib_srcs) + ext_modules = [ Pybind11Extension( "Controller", - ["pybind.cpp", "sim.cpp", "controller.cpp", "path.cpp", "perception.cpp"], - include_dirs=[".", "build/_deps/cdt-src/CDT/include"], + ["pybind.cpp", "sim.cpp", "controller.cpp", "path.cpp", "perception.cpp"] + alglib_srcs, + include_dirs=[".", "build/_deps/cdt-src/CDT/include", "build/_deps/alglib-src/src"], language="c++", ), ] From 2652e74bd3d2f7206f5857578e63d0487cd2da23 Mon Sep 17 00:00:00 2001 From: Edwin <20777515+Lucien950@users.noreply.github.com> Date: Wed, 8 Apr 2026 01:13:58 -0700 Subject: [PATCH 45/45] fix :) --- auto_sim/main.py | 14 +++---- auto_sim/render.py | 54 +++++++++++++++++---------- auto_sim/test_controller_2/path.cpp | 36 ++++++++++-------- auto_sim/test_controller_2/path.hpp | 2 +- auto_sim/test_controller_2/pybind.cpp | 6 --- 5 files changed, 62 insertions(+), 50 deletions(-) diff --git a/auto_sim/main.py b/auto_sim/main.py index 8e12cf8..bb1faaf 100644 --- a/auto_sim/main.py +++ b/auto_sim/main.py @@ -36,15 +36,11 @@ # vehicle_state.omega = -0.2 def handle_key(key: int, state: VehicleState): - match key: - case pygame.K_d: - state.y -= 1 - case pygame.K_a: - state.y += 1 - case pygame.K_w: - state.x += 1 - case pygame.K_s: - state.x -= 1 + match key: + case pygame.K_d: state.y -= 1 + case pygame.K_a: state.y += 1 + case pygame.K_w: state.x += 1 + case pygame.K_s: state.x -= 1 ran = False def simulate_cone_detection(state: VehicleState): diff --git a/auto_sim/render.py b/auto_sim/render.py index cb0ad33..9f366ee 100644 --- a/auto_sim/render.py +++ b/auto_sim/render.py @@ -1,9 +1,12 @@ +from typing import Tuple + import pygame from scipy.spatial.transform import Rotation as R from constants import VEHICLE_WIDTH_M -from Controller import VehicleState, Cone, ConeColor, spline_t, get_center_line_length, project, project_seeded +from Controller import VehicleState, Cone, ConeColor, spline_t, project, get_center_line_length, get_center_points from math import ceil, degrees import numpy as np +from colorsys import hsv_to_rgb PIXELS_PER_M = 20.0 w: int @@ -71,6 +74,25 @@ def drawGrid(screen: pygame.Surface, state: VehicleState, color=(45, 45, 45), sp screen.blit(screen_grid, (0, 0)) + +# def create_spline_surface(): +# surf = pygame.Surface((77 * PIXELS_PER_M, 47 * PIXELS_PER_M), pygame.SRCALPHA) +# length = get_center_line_length() +# for x in range(int(77 * PIXELS_PER_M)): +# for y in range(int(47 * PIXELS_PER_M)): +# k = hsv_to_rgb(project(22-x/PIXELS_PER_M, -39+y/PIXELS_PER_M)/length, 1, 1) +# surf.set_at((x, y), (int(k[0]*255), int(k[1]*255), int(k[2]*255))) +# return surf + +# spline_surface = None +# def drawSplintSurface(screen: pygame.Surface, vehicle_state: VehicleState): +# global spline_surface +# if spline_surface is None: +# spline_surface = create_spline_surface() +# rotated_surf: pygame.Surface = pygame.transform.rotate(spline_surface, degrees(-vehicle_state.theta - np.pi/2)) +# screen.blit(rotated_surf, rotated_surf.get_rect(center=transform(-16.5, -15.5, vehicle_state))) + + def int_to_color(value: ConeColor) -> str: match value: case ConeColor.BLUE: @@ -80,7 +102,6 @@ def int_to_color(value: ConeColor) -> str: case _: return "white" -# boundary = None at_t = None def render_world(vehicle_state: VehicleState, cones: list[Cone], screen: pygame.Surface): global w, h, at_t @@ -88,13 +109,11 @@ def render_world(vehicle_state: VehicleState, cones: list[Cone], screen: pygame. # grid for context drawGrid(screen, vehicle_state) + # drawSplintSurface(screen, vehicle_state) - p = transform(0, 0, vehicle_state) - pygame.draw.circle(screen, "white",p , 5) - - # if boundary is None: - # boundary = calculate_boundary(cones, ConeColor.YELLOW) + # pygame.draw.circle(screen, "white", transform(0, 0, vehicle_state), 5) + # DRAW TRIANGULATION # for edge in get_offline_edges(): # v1, v2 = cones[edge.v1()], cones[edge.v2()] # default_colour = "#676767" @@ -102,7 +121,6 @@ def render_world(vehicle_state: VehicleState, cones: list[Cone], screen: pygame. # transform(v1.x, v1.y, vehicle_state), # transform(v2.x, v2.y, vehicle_state), 2 # ) - # for edge in get_boundary_edges(): # v1, v2 = cones[edge.v1()], cones[edge.v2()] # default_colour = "#FF0000" @@ -117,30 +135,28 @@ def render_world(vehicle_state: VehicleState, cones: list[Cone], screen: pygame. # transform(v2.x, v2.y, vehicle_state), 1 # ) + # DRAW CENTER LINE # center_points = get_center_points() - # center_line = get_center_line() - # for i in range(len(center_line)): - # v1, v2 = center_points[center_line[i]], center_points[center_line[(i+1) % len(center_line)]] # returns cone objects + # for i in range(len(center_points)): + # v1, v2 = center_points[i], center_points[(i+1) % len(center_points)] # returns cone objects # default_colour = "#676767" # pygame.draw.line(screen, default_colour, # transform(v1.x, v1.y, vehicle_state), # transform(v2.x, v2.y, vehicle_state), 2 # ) - # if at_t is None: - # else: - # at_t = project_seeded(vehicle_state.x, vehicle_state.y, at_t) - at_t = project(vehicle_state.x, vehicle_state.y) - x = spline_t(at_t) - pygame.draw.circle(screen, "red", transform(x.x, x.y, vehicle_state), 5) + # DRAW PATH PROJECTION + # at_t = project(vehicle_state.x, vehicle_state.y) + # x = spline_t(at_t) + # pygame.draw.circle(screen, "red", transform(x.x, x.y, vehicle_state), 5) # DRAW SPLINE - # steps = 10 + # steps = 100 # dt = get_center_line_length() / steps # for t in range(steps): # x1 = spline_t(t * dt) # x2= spline_t((t + 1) * dt) - # pygame.draw.line(screen, "green", + # pygame.draw.line(screen, "#676767", # transform(x1.x, x1.y, vehicle_state), # transform(x2.x, x2.y, vehicle_state), 2) diff --git a/auto_sim/test_controller_2/path.cpp b/auto_sim/test_controller_2/path.cpp index d8b40f7..6c39b83 100644 --- a/auto_sim/test_controller_2/path.cpp +++ b/auto_sim/test_controller_2/path.cpp @@ -78,45 +78,50 @@ void compute_path_from_percepted_cones() { assert(offline_inner_edges.size() + offline_boundary_edges.size() == original_num_edges); // construct center line - center_points = { }; + std::vector _centers = { }; for (const auto e : offline_inner_edges) { // get center point of e const Cone& c1 = cones[e.v1()]; const Cone& c2 = cones[e.v2()]; - center_points.emplace_back((c1.x + c2.x) / 2, (c1.y + c2.y) / 2, ConeColor::CENTER); + _centers.emplace_back((c1.x + c2.x) / 2, (c1.y + c2.y) / 2, ConeColor::CENTER); + } + const std::vector _center_idxs = calculate_boundary(_centers, ConeColor::CENTER); + center_points.clear(); + center_points.reserve(_center_idxs.size()); + for (const size_t i : _center_idxs) { + center_points.push_back(_centers[i]); } - center_line_idxs = calculate_boundary(center_points, ConeColor::CENTER); // center line parameterization prefix alglib::real_1d_array center_line_len_prefix; - center_line_len_prefix.setlength(center_line_idxs.size() + 1); + center_line_len_prefix.setlength(center_points.size() + 1); center_line_len_prefix[0] = 0; - for (size_t i = 1; i <= center_line_idxs.size(); ++i) { - const Cone& c1 = center_points[center_line_idxs[i - 1]]; - const Cone& c2 = center_points[center_line_idxs[i % center_line_idxs.size()]]; + for (size_t i = 1; i <= center_points.size(); ++i) { + const Cone& c1 = center_points[i - 1]; + const Cone& c2 = center_points[i % center_points.size()]; center_line_len_prefix[i] = center_line_len_prefix[i - 1] + std::hypot(c1.x - c2.x, c1.y - c2.y); } - center_line_length = center_line_len_prefix[center_line_idxs.size()]; + center_line_length = center_line_len_prefix[center_points.size()]; // draw a spline between all the center points alglib::real_1d_array xs, ys; - xs.setlength(center_line_idxs.size() + 1); - ys.setlength(center_line_idxs.size() + 1); - for (size_t i = 0; i < center_line_idxs.size(); ++i) { - const Cone& c = center_points[center_line_idxs[i]]; + xs.setlength(center_points.size() + 1); + ys.setlength(center_points.size() + 1); + for (size_t i = 0; i < center_points.size(); ++i) { + const Cone& c = center_points[i]; xs[i] = c.x; ys[i] = c.y; } // cycle around - xs[center_line_idxs.size()] = center_points[center_line_idxs[0]].x; - ys[center_line_idxs.size()] = center_points[center_line_idxs[0]].y; + xs[center_points.size()] = center_points[0].x; + ys[center_points.size()] = center_points[0].y; alglib::spline1dbuildcubic(center_line_len_prefix, xs, x_spline); alglib::spline1dbuildcubic(center_line_len_prefix, ys, y_spline); } double project(const double x, const double y) { - const ScopeTimer s { "project timer" }; + // const ScopeTimer s { "project timer" }; static constexpr uint32_t samples = 50; static constexpr double eps = 0; @@ -156,6 +161,7 @@ double project(const double x0, const double y0, double t) { return t; } } + std::cout << "project did not converge at x=" << x0 << "and y=" << y0 << std::endl; throw std::exception("project did not converge"); } diff --git a/auto_sim/test_controller_2/path.hpp b/auto_sim/test_controller_2/path.hpp index 3a7f3f2..466c2d0 100644 --- a/auto_sim/test_controller_2/path.hpp +++ b/auto_sim/test_controller_2/path.hpp @@ -37,6 +37,6 @@ Location spline_t(double t); inline CDT::EdgeUSet offline_boundary_edges; inline CDT::EdgeUSet offline_inner_edges; inline std::vector center_points; -inline std::vector center_line_idxs; +// inline std::vector center_line_idxs; inline alglib::spline1dinterpolant x_spline, y_spline; inline double center_line_length; \ No newline at end of file diff --git a/auto_sim/test_controller_2/pybind.cpp b/auto_sim/test_controller_2/pybind.cpp index d7e0068..8c2c864 100644 --- a/auto_sim/test_controller_2/pybind.cpp +++ b/auto_sim/test_controller_2/pybind.cpp @@ -19,9 +19,6 @@ static CDT::EdgeUSet& get_boundary_edges() { static std::vector& get_center_points() { return center_points; } -static std::vector& get_center_line() { - return center_line_idxs; -} static double get_center_line_length() { return center_line_length; } @@ -79,9 +76,6 @@ PYBIND11_MODULE(Controller, m, py::mod_gil_not_used()) { m.def("get_center_points", &get_center_points, py::return_value_policy::reference, R"pbdoc( Get the center points calculated from compute_path() )pbdoc"); - m.def("get_center_line", &get_center_line, py::return_value_policy::reference, R"pbdoc( - Get the center line calculated from compute_path() - )pbdoc"); m.def("get_center_line_length", &get_center_line_length, R"pbdoc( Get the length of the center line calculated from compute_path() )pbdoc");