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 // Car dimensions taken from www.car.info
  12
  13 //// Opel Corsa
  14 //#define SLOT_START_WIDTH_MULTIPLIER 2.0
  15 //#define CAR_CURB_TO_CURB 9.900
  16 //#define CAR_FRONT_TRACK 1.320
  17 //#define CAR_WIDTH 1.532 // 1.944 // with mirrors
  18 //#define CAR_WHEELBASE 2.343
  19 //#define CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT 3.212
  20 //#define CAR_LENGTH 3.622
  21
  22 //// Wolkswagen transporter
  23 //#define SLOT_START_WIDTH_MULTIPLIER 12.4
  24 //#define CAR_CURB_TO_CURB 13.200
  25 //#define CAR_FRONT_TRACK 1.904
  26 //#define CAR_WIDTH 1.994 // 2.297 // with mirrors
  27 //#define CAR_WHEELBASE 3.400
  28 //#define CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT 4.308
  29 //#define CAR_LENGTH 5.500
  30
  31 //// Mercedes-Benz AMG GT
  32 //#define SLOT_START_WIDTH_MULTIPLIER 9.8
  33 //#define CAR_CURB_TO_CURB 11.470
  34 //#define CAR_FRONT_TRACK 1.678
  35 //#define CAR_WIDTH 1.939 // 2.075 // with mirrors
  36 //#define CAR_WHEELBASE 2.630
  37 //#define CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT 3.528
  38 //#define CAR_LENGTH 4.544
  39
  40 // Renault ZOE (vorobieva, computed ctc)
  41 #define SLOT_START_WIDTH_MULTIPLIER 4.5
  42 #define CAR_CURB_TO_CURB 10.802166641822163
  43 #define CAR_FRONT_TRACK 1.511
  44 #define CAR_WIDTH 1.771 // 1.945 // with mirrors
  45 #define CAR_WHEELBASE 2.588
  46 #define CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT 3.427
  47 #define CAR_LENGTH 4.084
  48
  49 #define SLOT_STEP_LENGTH 0.01
  50 #define SLOT_STEP_WIDTH 0.01
  51 #define SLOT_MAX_WIDTH (CAR_WIDTH + (SLOT_START_WIDTH_MULTIPLIER + 40) * SLOT_STEP_WIDTH)
  52
  53 #define PARKING_SPEED -0.001
  54 #define DELTA_ANGLE_TO_SLOT 0.0001
  55
  56 int main(int argc, char** argv)
  57 {
  58         if (argc != 2) {
  59                 std::cerr << "Number of direction changes needed." << std::endl;
  60                 exit(1);
  61         }
  62         int max_cusp = atoi(argv[1]);
  63         std::cout << std::fixed;
  64         std::cerr << std::fixed;
  65
  66         bcar::BicycleCar c;
  67         c.ctc(CAR_CURB_TO_CURB);
  68         c.ft(CAR_FRONT_TRACK);
  69         c.w(CAR_WIDTH);
  70         c.wb(CAR_WHEELBASE);
  71         c.df(CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT);
  72         c.len(CAR_LENGTH);
  73
  74         bcar::Point zp(0.0, 0.0);
  75         double zh = 0.0;
  76         double w = c.w() + SLOT_START_WIDTH_MULTIPLIER * SLOT_STEP_WIDTH;
  77
  78         while (w < SLOT_MAX_WIDTH) {
  79                 double len = c.len() + SLOT_STEP_LENGTH;
  80                 while (true) {
  81                         bcar::ParkingSlot s(zp, zh, w, len);
  82                         s.set_parking_speed(PARKING_SPEED);
  83                         s.set_max_cusp(max_cusp);
  84                         s.set_delta_angle_to_slot(DELTA_ANGLE_TO_SLOT);
  85                         auto pr = s.fe(c);
  86                         if (!(pr.x() == 0.0 && pr.y() == 0.0 && pr.b() == 0.0
  87                                         && pr.e() == 0.0)) {
  88                                 std::cout << w << " " << len << " ";
  89                                 std::cout << max_cusp << " ";
  90                                 std::cout << s._ispaths.size() << " ";
  91                                 auto f = s._ispaths.front().front();
  92                                 auto b = s._ispaths.back().front();
  93                                 std::cout << f.x() << " " << f.y() << " ";
  94                                 std::cout << f.h() << " ";
  95                                 std::cout << b.x() << " " << b.y() << " ";
  96                                 std::cout << b.h() << " ";
  97                                 std::cout << std::endl;
  98                                 break;
  99                         }
 100                         len += SLOT_STEP_LENGTH;
 101                 }
 102                 w += SLOT_STEP_WIDTH;
 103         }
 104         return 0;
 105 }