src/prove_parallel_slot.cc

   1 /*
   2  * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
   3  *
   4  * SPDX-License-Identifier: GPL-3.0-only
   5  */
   6
   7 #include <iostream>
   8 #include <vector>
   9 #include "pslot.hh"
  10
  11 #define CAR_CURB_TO_CURB 11.031078891255458
  12 #define CAR_WIDTH 1.771
  13 #define CAR_WHEELBASE 2.588
  14 #define CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT 3.427
  15 #define CAR_LENGTH 4.084
  16
  17 #define SLOT_MAX_WIDTH 2.2
  18 #define SLOT_STEP_LENGTH 0.01
  19 #define SLOT_STEP_WIDTH 0.01
  20
  21 #define PARKING_SPEED -0.001
  22 #define DELTA_ANGLE_TO_SLOT 0.0001
  23
  24 int main(int argc, char** argv)
  25 {
  26         if (argc != 2) {
  27                 std::cerr << "Number of direction changes needed." << std::endl;
  28                 exit(1);
  29         }
  30         int max_cusp = atoi(argv[1]);
  31         std::cout << std::fixed;
  32         std::cerr << std::fixed;
  33
  34         bcar::BicycleCar c;
  35         c.ctc(CAR_CURB_TO_CURB);
  36         c.w(CAR_WIDTH);
  37         c.wb(CAR_WHEELBASE);
  38         c.df(CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT);
  39         c.len(CAR_LENGTH);
  40
  41         bcar::Point zp(0.0, 0.0);
  42         double zh = 0.0;
  43         double w = c.w() + 4.5 * SLOT_STEP_WIDTH;
  44
  45         while (w < SLOT_MAX_WIDTH) {
  46                 double len = c.len() + SLOT_STEP_LENGTH;
  47                 while (true) {
  48                         bcar::ParkingSlot s(zp, zh, w, len);
  49                         s.set_parking_speed(PARKING_SPEED);
  50                         s.set_max_cusp(max_cusp);
  51                         s.set_delta_angle_to_slot(DELTA_ANGLE_TO_SLOT);
  52                         auto pr = s.fe(c);
  53                         if (!(pr.x() == 0.0 && pr.y() == 0.0 && pr.b() == 0.0
  54                                         && pr.e() == 0.0)) {
  55                                 std::cout << w << " " << len << " ";
  56                                 std::cout << max_cusp << " ";
  57                                 std::cout << pr.x() << " " << pr.y() << " ";
  58                                 std::cout << pr.b() << " " << pr.e() << " ";
  59                                 std::cout << std::endl;
  60                                 break;
  61                         }
  62                         len += SLOT_STEP_LENGTH;
  63                 }
  64                 w += SLOT_STEP_WIDTH;
  65         }
  66         return 0;
  67 }