/
vehicle_autodrive.cpp
1447 lines (1355 loc) · 58.9 KB
/
vehicle_autodrive.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
#include "vehicle.h" // IWYU pragma: associated
#include <algorithm>
#include <array>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <functional>
#include <map>
#include <memory>
#include <optional>
#include <queue>
#include <string>
#include <unordered_set>
#include <utility>
#include <vector>
#include "avatar.h"
#include "character.h"
#include "coordinates.h"
#include "creature_tracker.h"
#include "cuboid_rectangle.h"
#include "debug.h"
#include "enums.h"
#include "flood_fill.h"
#include "hash_utils.h"
#include "map.h"
#include "map_iterator.h"
#include "map_memory.h"
#include "mapdata.h"
#include "messages.h"
#include "point.h"
#include "tileray.h"
#include "translations.h"
#include "type_id.h"
#include "veh_type.h"
#include "vpart_position.h"
/*
* This file contains code that allows a vehicle to be driven by an in-game character (most
* likely the player character) to some remote destination, following a path of adjacent overmap
* tile (OMT) coordinates, while avoiding any obstacles along the way.
*
* The main entry point is do_autodrive(), which is intended to be called from the do_turn() of
* a long-running activity. This will cause the driver character to perform one or more driving
* actions (as long as they have enough moves), which could be steering and/or changing the
* desired speed. The driver may also do nothing if the vehicle is going in the right
* direction.
*
* Most of the logic is inside the private implementation class autodrive_controller, which is
* instantiated at the beginning of an autodrive activity and cleaned up at the end. This class
* caches information about the vehicle, driver and the local terrain in order to minimize the
* amount of computation required on each invocation of do_autodrive().
*
* Upon reaching a new overmap tile, as well as once initially, the autodrive algorithm will
* try to find a path that takes the vehicle from its current position through the current
* and next OMT in order to reach the 2nd next OMT in the overall path to destination (which
* is obtained from driver.omt_path). Thus we always plan ahead 2 OMTs worth of navigation,
* but we only follow the first half of the route, then recompute a new route for the next
* 2 OMTs. This ensures that we don't drive up to the edge of an OMT only to get stuck due
* to a large obstacle that can't be driven around from that position (a dead end) and instead
* we always choose to cross OMT boundaries in a position from which we are guaranteed to be
* able to continue onward.
*
* The pathfinding algorithm being employed is A*. The graph being searched is made up of nodes
* denoted by x and y coordinates as well as a direction of travel. Since steering changes the
* vehicle's direction in increments of 15 degrees there are 360/15 possible orientations. By
* limiting navigation to 2 OMTs at a time we have a maximum search space of 2*24*24*24 nodes.
* In practice, A* finds a good path after visiting fewer than 50 nodes due to the heuristic
* function that estimates which nodes are most likely to lead to the destination (which is
* based on proximity to the "exit zone" leading to the next OMT and whether the vehicle is
* pointing towards the middle of that zone). Although there can be many paths that we could
* take to get through to the next OMT, we just take the first one we find since it tends to
* be the most direct and is usually near-optimal. When the way is blocked we'll either give
* up after exhausting the search space or after we hit a limit on the search length.
*
* In order to build the search graph for A*, we consider every possible map tile and orientation
* in the search zone and we figure out whether the vehicle's pivot point could be placed there
* without the vehicle overlapping some obstacle (a collision-free placement). The pivot point
* is the part of the vehicle that moves the least while steering and is usually between the
* wheels of the vehicle, closer to the non-steerable ones. The pathfinding problem reduces to
* finding a path for the pivot point to follow through the set of possible positions and
* orientations (nodes). In order to figure out the possible connections between nodes (the
* edges of the graph) we consider a limited set of actions that the driver can perform at
* each location: nothing, steer left or steer right (possibly more than once). Given a current
* node, current speed and amount of steering we can predict where the vehicle's pivot point
* will end up in 1 turn and connect the 2 nodes.
*
* In order to keep the search space small we only consider paths traveled at constant speed
* (otherwise we'd have to add speed as an extra attribute in every node). When we need to
* compute a new path we first try to use the maximum safe autodrive speed (which is typically
* much less than the vehicle's max speed). If we can't find a path we cut the speed in half and
* try again, repeating until we succeed or reach the minimum speed. When the target speed is
* different from the current speed of the vehicle we try to estimate how long it will take to
* reach that speed and take that into account when simulating the first few edges (they'll be
* shorter or longer than the rest).
*
* Once we have generated a sequence of nodes to follow, we do not invoke A* again until we reach
* the next OMT. We simply cache the path and on every subsequent call to do_autodrive() we
* check where we are on that path, what node we're supposed to reach next and what action we
* should use to get there. However, sometimes the simplified simulation used to compute the
* search graph fails to accurately predict where the vehicle will end up. This could be because
* there was too much error in an approximation (such as time to accelerate) or the driving
* conditions changed (ran over a mound of dirt that interfered with steering). When such a
* discrepancy is detected, autodrive will fall back to "safe mode", cutting the speed to the
* minimum (1 tile per second) and recomputing the path.
*
* Since the navigation graph is cached and not updated in response to dynamic obstacles (such
* as animals) autodrive will also perform collision detection at every turn, before taking
* any action that may end the turn. If a possible collision is detected autodrive will enter
* "safe mode", reducing speed and recomputing. If the speed is already at minimum it will
* abort instead with a warning message.
*
* In most cases when autodrive aborts it will not automatically stop the vehicle. Stopping
* may be undesirable since nearby enemies could get into the vehicle. It is up to the player
* to either stop or continue driving manually for a while. However, when aborting due to a
* collision (which happened despite collision detection, i.e. there was a simulation discrepancy
* near an obstacle) the brakes will be engaged.
*
* In order to prevent exploiting the system to navigate in the dark, autodrive will also
* perform visibility checks. Not being able to see anything in front of the vehicle will
* immediately cancel (or fail to start) autodrive. If the driver has only partial visibility
* of the front of the vehicle, safe mode will engage limiting the speed. Furthermore, the
* driver needs to either see or have map memory for the map tiles in the current OMT; all
* unseen tiles will be treated as obstacles during pathfinding. In practice, this means
* that driving at night should only be possible through familiar terrain with limited
* lights / night vision or through unfamiliar terrain but with good headlights / excellent
* night vision.
*
* In order to keep the memory footprint small and constant, this implementation uses a different
* coordinate system for the navigation caches than on the game map. The "nav map" is always
* allocated to the size of two OMTs wide and one OMT high. Picture it like having one OMT `a` on
* the left and one OMT `b` on the right. The coordinates of the nav map are translated and
* rotated so that `a` is always the current OMT and `b` is the next OMT we are trying to reach.
* So even if, for example, we are planning a route south, the nav map is still allocated as two
* OMTs wide, but its coordinates are instead rotated when we want to deal with actual game map
* coordinates.
*/
static const ter_str_id ter_t_open_air( "t_open_air" );
static constexpr int OMT_SIZE = coords::map_squares_per( coords::omt );
static constexpr int NAV_MAP_NUM_OMT = 2;
static constexpr int NAV_MAP_SIZE_X = NAV_MAP_NUM_OMT * OMT_SIZE;
static constexpr int NAV_MAP_SIZE_Y = OMT_SIZE;
static constexpr int NAV_VIEW_PADDING = OMT_SIZE;
static constexpr int NAV_VIEW_SIZE_X = NAV_MAP_SIZE_X + 2 * NAV_VIEW_PADDING;
static constexpr int NAV_VIEW_SIZE_Y = NAV_MAP_SIZE_Y + 2 * NAV_VIEW_PADDING;
static constexpr int TURNING_INCREMENT = 15;
static constexpr int NUM_ORIENTATIONS = 360 / TURNING_INCREMENT;
// min and max speed in tiles/s
static constexpr int MIN_SPEED_TPS = 1;
static constexpr int MAX_SPEED_TPS = 3;
static constexpr int VMIPH_PER_TPS = static_cast<int>( vehicles::vmiph_per_tile );
/**
* Data type representing a vehicle orientation, which corresponds to an angle that is
* a multiple of TURNING_INCREMENT degrees, measured from the x axis.
* Range: [0, NUM_ORIENTATIONS)
*/
enum class orientation : int8_t {
d0 = 0,
};
/*
* Rotation amount which is a multiple of 90 degrees.
*/
enum class quad_rotation : int {
d0 = 0, d90, d180, d270,
};
/**
* Data type representing the convex hull of a vehicle under a particular rotation.
*/
struct vehicle_profile {
tileray tdir;
// Points occupied by the convex hull of the vehicle. Coordinates relative to pivot.
std::vector<point> occupied_zone;
// Points to check for collision when moving one step in this direction.
// Coordinates relative to pivot.
std::vector<point> collision_points;
};
/**
* Data type describing how what driving actions to perform at a given location
* in order to follow the current path to destination.
*/
struct navigation_step {
tripoint_abs_ms pos;
orientation steering_dir;
int target_speed_tps;
};
/**
* The address of a navigation node, i.e. a position and orientation on the nav map.
*/
// NOLINTNEXTLINE(cata-xy)
struct node_address {
int16_t x;
int16_t y;
orientation facing_dir;
bool operator== ( const node_address &other ) const {
return x == other.x && y == other.y && facing_dir == other.facing_dir;
}
point get_point() const {
return {x, y};
}
};
struct node_address_hasher {
std::size_t operator()( const node_address &addr ) const {
std::uint64_t val = addr.x;
val = ( val << 16 ) + addr.y;
val = ( val << 16 ) + static_cast<int>( addr.facing_dir );
return cata::hash64( val );
}
};
/*
* A node address annotated with its heuristic score, an approximation of how
* much it would cost to reach the goal from this node.
*/
struct scored_address {
struct node_address addr;
int16_t score;
bool operator> ( const scored_address &other ) const {
return score > other.score;
}
};
/*
* Data structure representing a navigation node that is known to be reachable. Contains
* information about the path to get there and enough information about the state of
* the vehicle and driver to predict which nodes can be reached on the next turn.
*/
struct navigation_node {
node_address prev;
int16_t cost;
int16_t tileray_steps;
int16_t speed;
int8_t target_speed_tps;
bool is_goal;
};
/*
* Data type describing a point transformation via translation and rotation.
*/
struct coord_transformation {
point pre_offset;
quad_rotation rotation;
point post_offset;
point transform( const point &p ) const;
tripoint transform( const point &p, int z ) const;
orientation transform( orientation dir ) const;
node_address transform( const point &p, orientation dir ) const;
coord_transformation inverse() const;
};
/*
* Collection of points for which there is still some check left to do, and a list of points
* that have already been visited. Used for remembering which points are up/down ramps when
* evaluating obstacles. Ramps will lead to obstacle-detection on another z-level
*/
struct point_queue {
std::queue<tripoint_bub_ms> to_check;
std::unordered_set<tripoint_bub_ms> visited;
void enqueue( const tripoint_bub_ms &p ) {
if( visited.find( p ) == visited.end() ) {
to_check.push( p );
}
}
};
/**
* Data structure that caches all the data needed in order to navigate from one
* OMT to the next OMT along the path to destination. Main components:
* - the view map (precomputed obstacle positions)
* - the nav map (valid positions and orientations where the vehicle pivot may be placed)
* - the navigation path found by A* search
*/
struct auto_navigation_data {
tripoint_abs_omt current_omt;
tripoint_abs_omt next_omt;
tripoint_abs_omt next_next_omt;
// boundaries of the view map
half_open_rectangle<point> view_bounds;
// boundaries of the nav map
half_open_rectangle<point> nav_bounds;
// boundaries of the next omt in nav map coords
half_open_rectangle<point> next_omt_bounds;
// transformation from nav map to game map coords
coord_transformation nav_to_map;
// transformation from view map to game map coords
coord_transformation view_to_map;
// transformation from nav map to view map coords
coord_transformation nav_to_view;
bool land_ok;
bool water_ok;
bool air_ok;
// the maximum speed to consider driving at, in tiles/s
int max_speed_tps;
// max acceleration
std::vector<int> acceleration;
// max amount of steering actions per turn
int max_steer;
std::array<vehicle_profile, NUM_ORIENTATIONS> profiles;
// known obstacles on the view map
cata::mdarray<bool, point, NAV_VIEW_SIZE_X, NAV_VIEW_SIZE_Y> is_obstacle;
// z-level of where the ground is per point on the view map
// Almost always same as the OMT's z, but might differ per mapsquare if we are driving up or down ramps
cata::mdarray<int, point, NAV_VIEW_SIZE_X, NAV_VIEW_SIZE_Y> ground_z;
// where on the nav map the vehicle pivot may be placed
std::array<cata::mdarray<bool, point, NAV_VIEW_SIZE_X, NAV_VIEW_SIZE_Y>, NUM_ORIENTATIONS>
valid_positions;
// node addresses that are valid end positions
std::unordered_set<node_address, node_address_hasher> goal_zone;
// the middle of the goal zone, in nav map coords
std::array<point, NAV_MAP_NUM_OMT> goal_points;
// computed path to next OMT
std::vector<navigation_step> path;
void clear() {
current_omt = { 0, 0, -100 };
path.clear();
}
vehicle_profile &profile( orientation dir ) {
return profiles.at( static_cast<int>( dir ) );
}
const vehicle_profile &profile( orientation dir ) const {
return profiles.at( static_cast<int>( dir ) );
}
bool &valid_position( orientation dir, point p ) {
return valid_positions[static_cast<int>( dir )][p.x][p.y];
}
bool valid_position( orientation dir, point p ) const {
return valid_positions[static_cast<int>( dir )][p.x][p.y];
}
bool valid_position( const node_address &addr ) const {
return valid_position( addr.facing_dir, point( addr.x, addr.y ) );
}
// transforms a point from map coords into view map coords
point to_view( const tripoint_abs_ms &p ) const {
return view_to_map.inverse().transform( p.raw().xy() );
}
// transforms a point from map bub coords into view map coords
point to_view( const tripoint_bub_ms &p ) const {
return to_view( get_map().getglobal( p ) );
}
// returns `p` adjusted so that the z-level is placed on the ground
template<typename Tripoint>
Tripoint adjust_z( const Tripoint &p ) const;
};
enum class collision_check_result : int {
ok,
no_visibility,
close_obstacle,
slow_down
};
class vehicle::autodrive_controller
{
public:
explicit autodrive_controller( const vehicle &driven_veh, const Character &driver );
const Character &get_driver() {
return driver;
}
const auto_navigation_data &get_data() {
return data;
}
void check_safe_speed();
std::optional<navigation_step> compute_next_step();
collision_check_result check_collision_zone( orientation turn_dir );
void reduce_speed();
private:
const vehicle &driven_veh;
const Character &driver;
auto_navigation_data data;
void compute_coordinates();
bool check_drivable( const tripoint_bub_ms &pt ) const;
void compute_obstacles();
void enqueue_if_ramp( point_queue &ramp_points, const map &here, const tripoint_bub_ms &p ) const;
void compute_obstacles_from_enqueued_ramp_points( point_queue &ramp_points, const map &here );
vehicle_profile compute_profile( orientation facing ) const;
void compute_valid_positions();
void compute_goal_zone();
void precompute_data();
scored_address compute_node_score( const node_address &addr, const navigation_node &node ) const;
void compute_next_nodes( const node_address &addr, const navigation_node &node,
int target_speed_tps,
std::vector<std::pair<node_address, navigation_node>> &next_nodes ) const;
std::optional<std::vector<navigation_step>> compute_path( int speed_tps ) const;
};
static const std::array<orientation, NUM_ORIENTATIONS> &all_orientations()
{
static const auto orientations_array = [] {
std::array<orientation, NUM_ORIENTATIONS> ret;
for( int i = 0; i < NUM_ORIENTATIONS; i++ )
{
ret[i] = static_cast<orientation>( i );
}
return ret;
}();
return orientations_array;
}
/*
* Normalize an orientation value to the range [0, NUM_ORIENTATIONS-1] using modular arithmetic.
*/
static orientation normalize_orientation( int steering_turns )
{
steering_turns %= NUM_ORIENTATIONS;
// beware that x % N < 0 when x < 0
if( steering_turns < 0 ) {
steering_turns += NUM_ORIENTATIONS;
}
return static_cast<orientation>( steering_turns );
}
static orientation to_orientation( units::angle angle )
{
return normalize_orientation( std::lround( units::to_degrees(
angle ) / TURNING_INCREMENT ) );
}
static std::string to_string( orientation dir )
{
return std::to_string( static_cast<int>( dir ) );
}
static orientation operator- ( const orientation &dir )
{
return static_cast<orientation>( ( NUM_ORIENTATIONS - static_cast<int>
( dir ) ) % NUM_ORIENTATIONS );
}
static orientation operator+ ( const orientation &dir1, const orientation &dir2 )
{
return static_cast<orientation>( ( static_cast<int>( dir1 ) + static_cast<int>
( dir2 ) ) % NUM_ORIENTATIONS );
}
static orientation operator- ( const orientation &dir1, const orientation &dir2 )
{
return dir1 + -dir2;
}
static orientation operator+ ( const orientation &dir, int steering_turns )
{
return dir + normalize_orientation( steering_turns );
}
static quad_rotation operator- ( const quad_rotation &rot )
{
return static_cast<quad_rotation>( ( 4 - static_cast<int>( rot ) ) % 4 );
}
static quad_rotation operator+ ( const quad_rotation &rot1, const quad_rotation &rot2 )
{
return static_cast<quad_rotation>( ( static_cast<int>( rot1 ) + static_cast<int>( rot2 ) ) % 4 );
}
static quad_rotation operator- ( const quad_rotation &rot1, const quad_rotation &rot2 )
{
return rot1 + -rot2;
}
static orientation operator+ ( const orientation &dir, const quad_rotation &rot )
{
return dir + static_cast<orientation>( static_cast<int>( rot ) * NUM_ORIENTATIONS / 4 );
}
static orientation &operator+= ( orientation &dir, const quad_rotation &rot )
{
return dir = dir + rot;
}
static units::angle to_angle( const orientation &dir )
{
return units::from_degrees( static_cast<int>( dir ) * TURNING_INCREMENT );
}
/*
* Returns the difference between two orientation values, normalized to the range
* [-NUM_ORIENTATIONS/2, NUM_ORIENTATIONS/2) via modular arithmetic.
*/
static int orientation_diff( const orientation &dir1, const orientation &dir2 )
{
return static_cast<int>( dir1 - dir2 + quad_rotation::d180 ) - NUM_ORIENTATIONS / 2;
}
/*
* Returns the angle of the given point, which must lie on either axis
*/
static quad_rotation to_quad_rotation( const point &pt )
{
if( pt.x > 0 ) {
return quad_rotation::d0;
} else if( pt.x < 0 ) {
return quad_rotation::d180;
} else if( pt.y > 0 ) {
return quad_rotation::d90;
} else {
return quad_rotation::d270;
}
}
static node_address make_node_address( point pos, orientation dir )
{
return { static_cast<int16_t>( pos.x ), static_cast<int16_t>( pos.y ), dir };
}
/*
* Computes atan2(dy, dx) in "orientation" units. Optimized to use only integer
* arithmetic and table lookups since it's used in the inner loop of A*.
*/
// NOLINTNEXTLINE(cata-xy)
static orientation approx_orientation( int dx, int dy )
{
orientation ret = orientation::d0;
if( dy < 0 ) {
dx *= -1;
dy *= -1;
ret += quad_rotation::d180;
}
if( dx < 0 ) {
std::swap( dx, dy );
dy *= -1;
ret += quad_rotation::d90;
}
if( dy > dx ) {
ret = ret + quad_rotation::d90 - approx_orientation( dy, dx );
} else if( dy > 0 ) {
static const auto atan_table = [] {
constexpr int table_size = 101;
std::array<orientation, table_size> table;
for( int i = 0; i < table_size; i++ )
{
table[i] = to_orientation( units::from_radians( std::atan( 1.0 * i /
( table_size - 1 ) ) ) );
}
return table;
}();
ret = ret + atan_table[ dy * 100 / dx ];
}
return ret;
}
static point rotate( point p, quad_rotation rotation )
{
switch( rotation ) {
case quad_rotation::d0:
return p;
case quad_rotation::d90:
return { -p.y - 1, p.x };
case quad_rotation::d180:
return { -p.x - 1, -p.y - 1 }; // NOLINT(cata-use-point-arithmetic)
case quad_rotation::d270:
return { p.y, -p.x - 1 };
}
return p;
}
point coord_transformation::transform( const point &p ) const
{
return rotate( p - pre_offset, rotation ) + post_offset;
}
tripoint coord_transformation::transform( const point &p, int z ) const
{
return tripoint( transform( p ), z );
}
orientation coord_transformation::transform( orientation dir ) const
{
return dir + rotation;
}
node_address coord_transformation::transform( const point &p, orientation dir ) const
{
return make_node_address( transform( p ), transform( dir ) );
}
coord_transformation coord_transformation::inverse() const
{
return { post_offset, -rotation, pre_offset };
}
static int signum( int val )
{
return ( 0 < val ) - ( val < 0 );
}
template<typename Tripoint>
Tripoint auto_navigation_data::adjust_z( const Tripoint &p ) const
{
if( !land_ok ) {
return p;
}
const point pt_view = to_view( p );
if( !view_bounds.contains( pt_view ) ) {
debugmsg( "Autodrive tried to adjust zlevel on out-of-bounds point p=%s", p.to_string() );
return p; // shouldn't happen, but who knows.
}
return { p.xy(), ground_z[pt_view] };
}
void vehicle::autodrive_controller::compute_coordinates()
{
data.view_bounds = { point_zero, {NAV_VIEW_SIZE_X, NAV_VIEW_SIZE_Y} };
data.nav_bounds = { point_zero, {NAV_MAP_SIZE_X, NAV_MAP_SIZE_Y} };
data.next_omt_bounds = { {OMT_SIZE, 0}, {OMT_SIZE * 2, OMT_SIZE} };
const tripoint_rel_omt omt_diff = data.next_omt - data.current_omt;
quad_rotation next_dir = to_quad_rotation( omt_diff.raw().xy() );
point mid_omt( OMT_SIZE / 2, OMT_SIZE / 2 );
const tripoint_abs_ms abs_mid_omt = project_to<coords::ms>( data.current_omt ) + mid_omt;
data.nav_to_view = { point_zero, quad_rotation::d0, {NAV_VIEW_PADDING, NAV_VIEW_PADDING} } ;
data.nav_to_map = { mid_omt, next_dir, abs_mid_omt.raw().xy() };
data.view_to_map = data.nav_to_map;
data.view_to_map.pre_offset += data.nav_to_view.post_offset;
}
vehicle_profile vehicle::autodrive_controller::compute_profile( orientation facing ) const
{
vehicle_profile ret;
tileray tdir( to_angle( facing ) );
ret.tdir = tdir;
std::map<int, std::pair<int, int>> extent_map;
const point pivot = driven_veh.pivot_point();
for( const vehicle_part &part : driven_veh.parts ) {
if( part.removed ) {
continue;
}
tripoint pos;
driven_veh.coord_translate( tdir, pivot, part.mount, pos );
if( extent_map.find( pos.y ) == extent_map.end() ) {
extent_map[pos.y] = { pos.x, pos.x };
} else {
auto &extent = extent_map[pos.y];
extent.first = std::min( extent.first, pos.x );
extent.second = std::max( extent.second, pos.x );
}
}
for( const auto &extent : extent_map ) {
const int y = extent.first;
const int x_min = extent.second.first;
const int x_max = extent.second.second;
for( int x = x_min; x <= x_max; x++ ) {
ret.occupied_zone.emplace_back( x, y );
}
}
for( int part_num : driven_veh.rotors ) {
const vehicle_part &part = driven_veh.part( part_num );
const int diameter = part.info().rotor_info->rotor_diameter;
const int radius = ( diameter + 1 ) / 2;
if( radius > 0 ) {
tripoint pos;
driven_veh.coord_translate( tdir, pivot, part.mount, pos );
for( tripoint pt : points_in_radius( pos, radius ) ) {
ret.occupied_zone.emplace_back( pt.xy() );
}
}
}
// figure out the maximum amount of displacement that can happen from advancing the
// tileray a single step by advancing it a bunch of times and reducing the x and y
// components of the displacement to at most 1
constexpr int far_advance = 100;
tdir.advance( far_advance );
const point increment( signum( tdir.dx() ), signum( tdir.dy() ) );
// compute the subset of vehicle points that would move to previously unoccupied spots
// (a.k.a. "moving first") when the vehicle moves one step; these are the only points
// we need to check for collision when the vehicle is moving in this direction
const std::unordered_set<point> occupied_set( ret.occupied_zone.begin(), ret.occupied_zone.end() );
for( const point &pt : ret.occupied_zone ) {
if( occupied_set.find( pt + increment ) == occupied_set.end() ) {
ret.collision_points.emplace_back( pt );
}
}
return ret;
}
// Return true if the map tile at the given position (in map coordinates)
// can be driven on (not an obstacle).
// The logic should match what is in vehicle::part_collision().
bool vehicle::autodrive_controller::check_drivable( const tripoint_bub_ms &pt ) const
{
const map &here = get_map();
// check if another vehicle is there; tiles occupied by the current
// vehicle are evidently drivable
const optional_vpart_position ovp = here.veh_at( pt );
if( ovp ) {
// Known corner case: some furniture can be driven over, but will collide with
// wheel parts; if the vehicle starts over such a furniture we'll mark that tile
// as safe and may collide with it by turning; however if we mark it unsafe
// we'll have no viable paths away from the starting point.
return &ovp->vehicle() == &driven_veh;
}
const tripoint_abs_ms pt_abs = here.getglobal( pt );
const tripoint_abs_omt pt_omt = project_to<coords::omt>( pt_abs );
// only check visibility for the current OMT, we'll check other OMTs when
// we reach them
if( pt_omt == data.current_omt ) {
// driver must see the tile or have seen it before in order to plan a route over it
if( !driver.sees( pt ) ) {
if( !driver.is_avatar() ) {
return false;
}
const avatar &avatar = *driver.as_avatar();
if( !avatar.is_map_memory_valid() ) {
debugmsg( "autodrive querying uninitialized map memory at %s", pt_abs.to_string() );
return false;
}
if( avatar.get_memorized_tile( pt_abs ) == mm_submap::default_tile ) {
// apparently open air doesn't get memorized, so pretend it is or else
// we can't fly helicopters due to the many unseen tiles behind the driver
if( !( data.air_ok && here.ter( pt ) == ter_t_open_air ) ) {
return false;
}
}
}
}
// check for creatures
// TODO: padding around monsters
Creature *critter = get_creature_tracker().creature_at( pt, true );
if( critter && driver.sees( *critter ) ) {
return false;
}
// don't drive over visible traps
if( here.can_see_trap_at( pt.raw(), driver ) ) {
return false;
}
// check for furniture that hinders movement; furniture with 0 move cost
// can be driven on
const furn_id furniture = here.furn( pt );
if( furniture != furn_str_id::NULL_ID() && furniture.obj().movecost != 0 ) {
return false;
}
const ter_id terrain = here.ter( pt );
if( terrain == ter_str_id::NULL_ID() ) {
return false;
}
// open air is an obstacle to non-flying vehicles; it is drivable
// for flying vehicles
if( terrain == ter_t_open_air ) {
return data.air_ok;
}
const ter_t &terrain_type = terrain.obj();
// watercraft can drive on water
if( data.water_ok && terrain_type.has_flag( ter_furn_flag::TFLAG_SWIMMABLE ) ) {
return true;
}
// remaining checks are for land-based navigation
if( !data.land_ok ) {
return false;
}
// NOLINTNEXTLINE(bugprone-branch-clone)
if( terrain_type.movecost <= 0 ) {
// walls and other impassable terrain
return false;
} else if( terrain_type.movecost == 2 || terrain_type.has_flag( ter_furn_flag::TFLAG_NOCOLLIDE ) ) {
// terrain with neutral move cost or tagged with NOCOLLIDE will never cause
// collisions
return true;
} else if( terrain_type.bash.str_max >= 0 && !terrain_type.bash.bash_below ) {
// bashable terrain (but not bashable floors) will cause collisions
return false;
} else if( terrain_type.has_flag( ter_furn_flag::TFLAG_LIQUID ) ) {
// water and lava
return false;
}
return true;
}
void vehicle::autodrive_controller::compute_obstacles()
{
const map &here = get_map();
const int z = data.current_omt.z();
point_queue ramp_points;
for( int dx = 0; dx < NAV_VIEW_SIZE_X; dx++ ) {
for( int dy = 0; dy < NAV_VIEW_SIZE_Y; dy++ ) {
const tripoint abs_map_pt = data.view_to_map.transform( point( dx, dy ), z );
const tripoint_bub_ms p = here.bub_from_abs( abs_map_pt );
data.is_obstacle[dx][dy] = !check_drivable( p );
data.ground_z[dx][dy] = z;
enqueue_if_ramp( ramp_points, here, p );
}
}
compute_obstacles_from_enqueued_ramp_points( ramp_points, here );
}
// Checks whether `p` is a drivable ramp up or down,
// and if so adds the ramp's destination tripoint to `ramp_points`
void vehicle::autodrive_controller::enqueue_if_ramp( point_queue &ramp_points,
const map &here, const tripoint_bub_ms &p ) const
{
if( !data.land_ok ) {
return;
}
ramp_points.visited.emplace( p );
if( p.z() < OVERMAP_HEIGHT && here.has_flag( ter_furn_flag::TFLAG_RAMP_UP, p ) ) {
ramp_points.enqueue( p + tripoint_above );
}
if( p.z() > -OVERMAP_DEPTH && here.has_flag( ter_furn_flag::TFLAG_RAMP_DOWN, p ) ) {
ramp_points.enqueue( p + tripoint_below );
}
}
// Flood-fills from all enqueued points of up or down ramps. For each connected point, we
// set `is_obstacle` and `ground_z` again, based on whether they are an obstacle or not on
// this zlevel.
void vehicle::autodrive_controller::compute_obstacles_from_enqueued_ramp_points(
point_queue &ramp_points, const map &here )
{
auto is_drivable = [this, &here]( const tripoint_bub_ms & p ) {
return here.inbounds( p ) && check_drivable( p );
};
while( !ramp_points.to_check.empty() ) {
const tripoint_bub_ms ramp_point = ramp_points.to_check.front();
ramp_points.to_check.pop();
for( const tripoint_bub_ms &p : ff::point_flood_fill_4_connected( ramp_point, ramp_points.visited,
is_drivable ) ) {
const point pt_view = data.to_view( p );
if( !data.view_bounds.contains( pt_view ) ) {
continue;
}
// We now know that point p is drivable (and not an obstacle) on this zlevel, since
// it passed the is_drivable check.
if( data.is_obstacle[pt_view] ) {
// We have examined this point previously on a different zlevel but conluded
// that it was an obstacle at that zlevel. But on this zlevel it is apparently
// not an obstacle, so this must be our groundlevel.
data.ground_z[pt_view] = p.z();
} else {
data.ground_z[pt_view] = std::min( data.ground_z[pt_view], p.z() );
}
data.is_obstacle[pt_view] = false;
enqueue_if_ramp( ramp_points, here, p );
}
};
}
void vehicle::autodrive_controller::compute_valid_positions()
{
const coord_transformation veh_rot = {point_zero, -data.nav_to_map.rotation, point_zero};
for( orientation facing : all_orientations() ) {
const vehicle_profile &profile = data.profile( data.nav_to_map.transform( facing ) );
for( int mx = 0; mx < NAV_MAP_SIZE_X; mx++ ) {
for( int my = 0; my < NAV_MAP_SIZE_Y; my++ ) {
const point nav_pt( mx, my );
bool valid = true;
for( const point &veh_pt : profile.occupied_zone ) {
const point view_pt = data.nav_to_view.transform( nav_pt ) + veh_rot.transform(
veh_pt ) - veh_rot.transform( point_zero );
if( !data.view_bounds.contains( view_pt ) || data.is_obstacle[view_pt.x][view_pt.y] ) {
valid = false;
break;
}
}
data.valid_position( facing, nav_pt ) = valid;
}
}
}
}
void vehicle::autodrive_controller::compute_goal_zone()
{
data.goal_zone.clear();
coord_transformation goal_transform;
if( data.next_next_omt.xy() != data.next_omt.xy() ) {
// set the goal at the edge of next_omt and next_next_omt (in next_omt
// space, pointing towards next_next_omt)
const point next_omt_middle( OMT_SIZE + OMT_SIZE / 2, OMT_SIZE / 2 );
const tripoint_rel_omt omt_diff = data.next_next_omt - data.next_omt;
const quad_rotation rotation = to_quad_rotation( omt_diff.raw().xy() ) - data.nav_to_map.rotation;
goal_transform = {next_omt_middle, rotation, next_omt_middle};
} else {
// set the goal at the edge of cur_omt and next_omt (in next_omt space,
// pointing away from cur_omt)
goal_transform = { point( OMT_SIZE - 1, 0 ), quad_rotation::d0, point_zero };
}
constexpr int max_turns = NUM_ORIENTATIONS / 8 + 1;
const int x = 2 * OMT_SIZE - 1;
for( int turns = -max_turns; turns <= max_turns; turns++ ) {
const orientation dir = orientation::d0 + turns;
static_assert( NAV_MAP_SIZE_Y == OMT_SIZE, "Unexpected nav map size" );
for( int y = 0; y < OMT_SIZE; y++ ) {
const point pt( x, y );
const node_address addr = goal_transform.transform( pt, dir );
if( data.valid_position( addr ) ) {
data.goal_zone.insert( addr );
}
}
}
data.goal_points[0] = point( OMT_SIZE, OMT_SIZE / 2 - 1 );
data.goal_points[1] = goal_transform.transform( point( x, OMT_SIZE / 2 - 1 ) );
}
void vehicle::autodrive_controller::precompute_data()
{
const tripoint_abs_omt current_omt = driven_veh.global_omt_location();
const tripoint_abs_omt next_omt = driver.omt_path.back();
const tripoint_abs_omt next_next_omt = driver.omt_path.size() >= 2 ?
driver.omt_path[driver.omt_path.size() - 2] : next_omt;
if( current_omt != data.current_omt || next_omt != data.next_omt ||
next_next_omt != data.next_next_omt ) {
data.current_omt = current_omt;
data.next_omt = next_omt;
data.next_next_omt = next_next_omt;
// initialize car and driver properties
data.land_ok = driven_veh.valid_wheel_config();
data.water_ok = driven_veh.can_float();
data.air_ok = driven_veh.has_sufficient_rotorlift();
data.max_speed_tps = std::min( MAX_SPEED_TPS, driven_veh.safe_velocity() / VMIPH_PER_TPS );
data.acceleration.resize( data.max_speed_tps );
for( int speed_tps = 0; speed_tps < data.max_speed_tps; speed_tps++ ) {
data.acceleration[speed_tps] = driven_veh.acceleration( true, speed_tps * VMIPH_PER_TPS );
}
// TODO: compute from driver's skill and speed stat
// TODO: change it during simulation based on vehicle speed and terrain
// or maybe just keep track of player moves?
data.max_steer = 1;
for( orientation dir : all_orientations() ) {
data.profile( dir ) = compute_profile( dir );
}
// initialize navigation data
compute_coordinates();
compute_obstacles();
compute_valid_positions();
compute_goal_zone();
data.path.clear();
}
}
static navigation_node make_start_node( const node_address &start, const vehicle &driven_veh )
{
navigation_node ret;
ret.prev = start;
ret.cost = 0;
ret.tileray_steps = driven_veh.face.get_steps();
ret.speed = driven_veh.velocity;
ret.target_speed_tps = 0;
ret.is_goal = false;
return ret;
}
scored_address vehicle::autodrive_controller::compute_node_score( const node_address &addr,
const navigation_node &node ) const
{
// TODO: tweak this
constexpr int cost_mult = 1;
constexpr int forward_dist_mult = 10;
constexpr int side_dist_mult = 8;
constexpr int angle_mult = 2;
constexpr int nearness_penalty = 15;
scored_address ret{ addr, 0 };
ret.score += cost_mult * node.cost;
if( node.is_goal ) {
return ret;
}
static constexpr std::array<point, 4> neighbor_deltas = {
point_east, point_south, point_west, point_north
};
for( const point &neighbor_delta : neighbor_deltas ) {
const point p = addr.get_point() + neighbor_delta;
if( !data.nav_bounds.contains( p ) || !data.valid_position( addr.facing_dir, p ) ) {
ret.score += nearness_penalty;
}
}
const int omt_index = std::min( std::max( addr.x / OMT_SIZE, 0 ),
static_cast<int>( data.goal_points.size() ) - 1 );
const point waypoint_delta = data.goal_points[omt_index] - addr.get_point();
const point goal_delta = data.goal_points.back() - addr.get_point();
const orientation waypoint_dir = approx_orientation( waypoint_delta.x, waypoint_delta.y );
ret.score += forward_dist_mult * std::abs( goal_delta.x );
ret.score += side_dist_mult * std::abs( goal_delta.y );
if( waypoint_delta.x > OMT_SIZE / 4 ) {
ret.score += angle_mult * std::abs( orientation_diff( addr.facing_dir, waypoint_dir ) );
}
return ret;
}
void vehicle::autodrive_controller::compute_next_nodes( const node_address &addr,