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 10.820
  12 #define CAR_WIDTH 1.625
  13 #define CAR_WHEELBASE 2.450
  14 #define CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT 3.105
  15 #define CAR_LENGTH 3.760
  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 MAX_CUSP 10
  23 #define DELTA_ANGLE_TO_SLOT 0.0001
  24
  25 int main()
  26 {
  27         std::cout << std::fixed;
  28         std::cerr << std::fixed;
  29
  30         bcar::BicycleCar c;
  31         c.ctc(CAR_CURB_TO_CURB);
  32         c.w(CAR_WIDTH);
  33         c.wb(CAR_WHEELBASE);
  34         c.df(CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT);
  35         c.len(CAR_LENGTH);
  36
  37         bcar::Point zp(0.0, 0.0);
  38         double zh = 0.0;
  39         double w = c.w() + 4.5 * SLOT_STEP_WIDTH;
  40
  41         while (w < SLOT_MAX_WIDTH) {
  42                 double len = c.len() + SLOT_STEP_LENGTH;
  43                 while (true) {
  44                         bcar::ParkingSlot s(zp, zh, w, len);
  45                         s.set_parking_speed(PARKING_SPEED);
  46                         s.set_max_cusp(MAX_CUSP);
  47                         s.set_delta_angle_to_slot(DELTA_ANGLE_TO_SLOT);
  48                         auto pr = s.fe(c);
  49                         if (!(pr.x() == 0.0 && pr.y() == 0.0 && pr.b() == 0.0
  50                                         && pr.e() == 0.0)) {
  51                                 std::cout << w << " " << len << std::endl;
  52                                 break;
  53                         }
  54                         len += SLOT_STEP_LENGTH;
  55                 }
  56                 w += SLOT_STEP_WIDTH;
  57         }
  58         return 0;
  59 }