veloren_world/civ/
airship_travel.rs

1use crate::{
2    sim::WorldSim,
3    site::{self, Site},
4    site2::plot::PlotKindMeta,
5    util::{DHashMap, DHashSet, seed_expan},
6};
7use common::{
8    store::{Id, Store},
9    terrain::CoordinateConversions,
10    util::Dir,
11};
12use rand::prelude::*;
13use rand_chacha::ChaChaRng;
14use std::{fs::OpenOptions, io::Write};
15use tracing::{debug, warn};
16use vek::*;
17
18const AIRSHIP_TRAVEL_DEBUG: bool = false;
19
20macro_rules! debug_airships {
21    ($($arg:tt)*) => {
22        if AIRSHIP_TRAVEL_DEBUG {
23            debug!($($arg)*);
24        }
25    }
26}
27
28/// A docking position (id, position). The docking position id is
29/// an index of all docking positions in the world.
30#[derive(Clone, Copy, Debug, Default, PartialEq)]
31pub struct AirshipDockingPosition(pub u32, pub Vec3<f32>);
32
33/// An airship can dock with its port or starboard side facing the dock.
34#[derive(Debug, Copy, Clone, PartialEq, Default)]
35pub enum AirshipDockingSide {
36    #[default]
37    Port,
38    Starboard,
39}
40
41/// An approach for the airship provides the data needed to fly to a docking
42/// position and stop at the docking position. The approach provides a target
43/// 'final' postion that is offset from the docking postion such that
44/// when the airship flys from the final position to the docking position, the
45/// airship will be naturally aligned with the direction of the docking
46/// position, and only very small orientation adjustments will be needed
47/// before docking. The approach final position is selected to minimize the
48/// change of direction when flying from the takeoff location to the target
49/// docking position.
50#[derive(Clone, Debug, PartialEq)]
51pub struct AirshipDockingApproach {
52    pub dock_pos: AirshipDockingPosition,
53    /// The position of the airship when docked.
54    /// This is different from dock_pos because the airship is offset to align
55    /// the ramp with the dock.
56    pub airship_pos: Vec3<f32>,
57    /// The direction the airship is facing when docked.
58    pub airship_direction: Dir,
59    /// Then center of the AirshipDock Plot.
60    pub dock_center: Vec2<f32>,
61    /// The height above terrain the airship cruises at.
62    pub height: f32,
63    /// A 3D position that is offset from the direct line between the dock sites
64    /// to allow for a transition to the direction the airship will be
65    /// facing when it is docked.
66    pub approach_initial_pos: Vec2<f32>,
67    /// Intermediate position from the initial position to smooth out the
68    /// directional changes.
69    pub approach_final_pos: Vec2<f32>,
70    /// There are ramps on both the port and starboard sides of the airship.
71    /// This gives the side that the airship will dock on.
72    pub side: AirshipDockingSide,
73    /// The site name where the airship will be docked at the end of the
74    /// approach.
75    pub site_id: Id<Site>,
76}
77
78/// A route that an airship flies round-trip between two sites.
79#[derive(Clone, Debug)]
80pub struct AirshipRoute {
81    /// site[0] is the home site, site[1] is the away site.
82    pub sites: [Id<site::Site>; 2],
83    /// approaches[0] is flying from the home site to the away site.
84    /// approaches[1] is flying from the away site to the home site.
85    pub approaches: [AirshipDockingApproach; 2],
86    /// The distance between the two sites.
87    pub distance: u32,
88}
89
90impl AirshipRoute {
91    fn new(
92        site1: Id<site::Site>,
93        site2: Id<site::Site>,
94        approaches: [AirshipDockingApproach; 2],
95        distance: u32,
96    ) -> Self {
97        Self {
98            sites: [site1, site2],
99            approaches,
100            distance,
101        }
102    }
103}
104
105/// Airship routes are identified by a unique serial number starting from zero.
106type AirshipRouteId = u32;
107
108/// Data for airship operations. This is generated world data.
109#[derive(Clone, Default)]
110pub struct Airships {
111    /// The airship routes between sites.
112    pub routes: DHashMap<AirshipRouteId, AirshipRoute>,
113}
114
115// Internal data structures
116
117/// The docking postions at an AirshipDock plot.
118/// The center is the center of the plot. The docking_positions
119/// are the positions where the airship can dock.
120#[derive(Clone, Debug)]
121struct AirshipDockPositions {
122    pub center: Vec2<f32>,
123    pub docking_positions: Vec<AirshipDockingPosition>,
124    pub site_id: Id<site::Site>,
125}
126
127impl AirshipDockPositions {
128    fn from_plot_meta(
129        first_id: u32,
130        center: Vec2<i32>,
131        docking_positions: &[Vec3<i32>],
132        site_id: Id<site::Site>,
133    ) -> Self {
134        let mut dock_pos_id = first_id;
135        Self {
136            center: center.map(|i| i as f32),
137            docking_positions: docking_positions
138                .iter()
139                .map(|pos: &Vec3<i32>| {
140                    let docking_position =
141                        AirshipDockingPosition(dock_pos_id, pos.map(|i| i as f32));
142                    dock_pos_id += 1;
143                    docking_position
144                })
145                .collect(),
146            site_id,
147        }
148    }
149}
150
151/// Used while generating the airship routes to connect the airship docks.
152/// Encapsulates the connection between two airship docks, including the angle
153/// and distance.
154#[derive(Clone, Debug)]
155struct AirRouteConnection<'a> {
156    pub dock1: &'a AirshipDockPositions,
157    pub dock2: &'a AirshipDockPositions,
158    pub angle: f32,    // angle from dock1 to dock2, from dock2 the angle is -angle
159    pub distance: i64, // distance squared between dock1 and dock2
160}
161
162impl<'a> AirRouteConnection<'a> {
163    fn new(dock1: &'a AirshipDockPositions, dock2: &'a AirshipDockPositions) -> Self {
164        let angle = Airships::angle_between_vectors_ccw(
165            Airships::ROUTES_NORTH,
166            dock2.center - dock1.center,
167        );
168        let distance = dock1.center.distance_squared(dock2.center) as i64;
169        Self {
170            dock1,
171            dock2,
172            angle,
173            distance,
174        }
175    }
176}
177
178/// Dock connnections are a hash map (DHashMap) of DockConnectionHashKey to
179/// AirRouteConnection. The hash map is used internally during the generation of
180/// the airship routes.
181#[derive(Eq, PartialEq, Hash, Debug)]
182struct DockConnectionHashKey(Id<site::Site>, Id<site::Site>);
183
184/// Represents potential connections between two airship docks. Used during the
185/// generation of the airship routes.
186#[derive(Clone, Debug)]
187struct DockConnection<'a> {
188    pub dock: &'a AirshipDockPositions,
189    pub available_connections: usize,
190    pub connections: Vec<&'a AirRouteConnection<'a>>,
191}
192
193impl<'a> DockConnection<'a> {
194    fn new(dock: &'a AirshipDockPositions) -> Self {
195        Self {
196            dock,
197            available_connections: dock.docking_positions.len(),
198            connections: Vec::new(),
199        }
200    }
201
202    fn add_connection(&mut self, connection: &'a AirRouteConnection<'a>) {
203        self.connections.push(connection);
204        self.available_connections -= 1;
205    }
206}
207
208impl Airships {
209    /// The Z offset between the docking alignment point and the AirshipDock
210    /// plot docking position.
211    const AIRSHIP_TO_DOCK_Z_OFFSET: f32 = -1.0;
212    /// The absolute offset from the captain's position to the docking alignment
213    /// point on the X axis. The airship is assumed to be facing positive Y.
214    /// This is absolute assuming that the captain is in the center of the
215    /// airship. If the captain is not in the center, the
216    /// DOCK_ALIGN_POS_TO_CAPT_STARBOARD and DOCK_ALIGN_POS_TO_CAPT_PORT
217    /// need to be adjusted.
218    const CAPT_TO_DOCK_ALIGN_X: f32 = 19.0;
219    // const CAPT_TO_DOCK_ALIGN_X: f32 = 15.0;
220    /// The offset from the captain's position to the docking alignment point on
221    /// the Y axis. The airship is assumed to be facing positive Y.
222    /// This is positive if the docking alignment point is above (positive Y)
223    /// the captain's position.
224    // const CAPT_TO_DOCK_ALIGN_Y: f32 = -3.0;
225    const CAPT_TO_DOCK_ALIGN_Y: f32 = 8.0;
226    // the generated docking positions in world gen are a little low
227    const DEFAULT_DOCK_DURATION: f32 = 90.0;
228    /// The vector from the dock alignment point on the airship to the captain's
229    /// position when the airship is docked on the port side.
230    const DOCK_ALIGN_POS_TO_CAPT_PORT: Vec2<f32> = Vec2::new(
231        Airships::CAPT_TO_DOCK_ALIGN_X,
232        -Airships::CAPT_TO_DOCK_ALIGN_Y,
233    );
234    /// The vector from the dock alignment point on the airship to the captain's
235    /// position when the airship is docked on the starboard side.
236    const DOCK_ALIGN_POS_TO_CAPT_STARBOARD: Vec2<f32> = Vec2::new(
237        -Airships::CAPT_TO_DOCK_ALIGN_X,
238        -Airships::CAPT_TO_DOCK_ALIGN_Y,
239    );
240    const ROUTES_NORTH: Vec2<f32> = Vec2::new(0.0, 15000.0);
241    const STD_CRUISE_HEIGHT: f32 = 400.0;
242    const TAKEOFF_ASCENT_ALT: f32 = 150.0;
243
244    #[inline(always)]
245    pub fn docking_duration() -> f32 { Airships::DEFAULT_DOCK_DURATION }
246
247    #[inline(always)]
248    pub fn takeoff_ascent_height() -> f32 { Airships::TAKEOFF_ASCENT_ALT }
249
250    /// Get all the airship docking positions from the world sites.
251    fn all_airshipdock_positions(sites: &mut Store<Site>) -> Vec<AirshipDockPositions> {
252        let mut dock_pos_id = 0;
253        sites
254            .iter()
255            .flat_map(|(site_id, site)| site.site2().map(|site2| (site_id, site2)))
256            .flat_map(|(site_id, site2)| {
257                site2.plots().flat_map(move |plot| {
258                    if let Some(PlotKindMeta::AirshipDock {
259                        center,
260                        docking_positions,
261                        ..
262                    }) = plot.kind().meta()
263                    {
264                        Some((center, docking_positions, site_id))
265                    } else {
266                        None
267                    }
268                })
269            })
270            .map(|(center, docking_positions, site_id)| {
271                let positions = AirshipDockPositions::from_plot_meta(
272                    dock_pos_id,
273                    center,
274                    docking_positions,
275                    site_id,
276                );
277
278                dock_pos_id += positions.docking_positions.len() as u32;
279                positions
280            })
281            .collect::<Vec<_>>()
282    }
283
284    /// Generate the network of airship routes between all the sites with
285    /// airship docks. This is called only from the world generation code.
286    ///
287    /// After world sites are generated, the airship operations center creates a
288    /// network of airship routes between all the sites containing an
289    /// airship dock plot, and there are airships placed at each docking
290    /// position that will be used for an airship route. Each airship travels
291    /// between two sites. This is the airship's route (out and back).  When
292    /// an airship is created, the ops center internally assigns the airship
293    /// a route based on the airship's home docking position and the airship
294    /// routing network. Since a route is between two sites, and therefore
295    /// between two docking positions, there are two airships flying in opposite
296    /// directions.
297    ///
298    /// Todo: On longer routes, it should be possible to determine the flight
299    /// time and add airships to the route to maintain a schedule. The
300    /// airships would be spawned midair so that they don't appear our of
301    /// nowhere near the ground.
302    ///
303    /// Airships are assigned a flying height based on the direction of
304    /// travel to deconflict as much as possible.
305    pub fn generate_airship_routes(
306        &mut self,
307        sites: &mut Store<Site>,
308        world_sim: &mut WorldSim,
309        seed: u32,
310    ) {
311        let all_docking_positions = Airships::all_airshipdock_positions(sites);
312        // Create a map of all possible dock to dock connections.
313        let mut rng = ChaChaRng::from_seed(seed_expan::rng_state(seed));
314        let mut routes = DHashMap::<DockConnectionHashKey, AirRouteConnection>::default();
315        all_docking_positions.iter().for_each(|from_dock| {
316            all_docking_positions
317                .iter()
318                .filter(|to_dock| to_dock.site_id != from_dock.site_id)
319                .for_each(|to_dock| {
320                    routes.insert(
321                        DockConnectionHashKey(from_dock.site_id, to_dock.site_id),
322                        AirRouteConnection::new(from_dock, to_dock),
323                    );
324                });
325        });
326
327        // Do four rounds of connections.
328        // In each round, attempt to connect each dock to another dock that has at least
329        // one connection remaining. Assign scores to each candidate route based
330        // on:
331        // 1. How close the candidate route angle is to the optimal angle. The optimal
332        //    angle is calculated as follows:
333        //    - 0 existing connections - any angle, all angles are equally good.
334        //    - 1 existing connection - the angle for the vector opposite to the vector
335        //      for the existing connection.
336        //    - 2 existing connections - calculate the triangle formed by the first two
337        //      route endpoints and the dock center. Find the centroid of the triangle
338        //      and calculate the angle from the dock center to the centroid. The
339        //      optimal angle is the angle opposite to the vector from the dock center
340        //      to the centroid.
341        //    - 3 existing connections - calculate the triangle formed by the first
342        //      three connection endpoints. Find the centroid of the triangle and
343        //      calculate the angle from the dock center to the centroid. The optimal
344        //      angle is the angle opposite to the vector from the dock center to the
345        //      centroid.
346        // 2. The distance from the dock center to the connection endpoint. Generally,
347        //    the further the better, but the score function should be logarithmic to
348        //    not favor so much amongst the longer distances.
349
350        let mut dock_connections = all_docking_positions
351            .iter()
352            .map(DockConnection::new)
353            .collect::<Vec<_>>();
354
355        // The simple angle score is how close a2 is to the opposite of a1. E.g. if a1
356        // is 0.0, the the best score is when a2 is PI.
357        let angle_score_fn = |a1: f32, a2: f32| {
358            let optimal_angle = (a1 + std::f32::consts::PI).rem_euclid(std::f32::consts::TAU);
359            let angle_diff = (optimal_angle - a2)
360                .abs()
361                .min(std::f32::consts::TAU - (optimal_angle - a2).abs());
362            1.0 - (angle_diff / std::f32::consts::PI)
363        };
364
365        // The centroid angle score function calculates the angle from the dock center
366        // to the given 'centroid' vector, then applies the angle score function
367        // with a1 being the centroid angle and a2 being the route angle.
368        let centroid_angle_score_fn =
369            |centroid: Vec2<f32>, dock_center: Vec2<f32>, rt: &AirRouteConnection| {
370                let centroid_dir = centroid - dock_center;
371                if centroid_dir.is_approx_zero() {
372                    return 0.0;
373                }
374                let centroid_angle =
375                    Airships::angle_between_vectors_ccw(Airships::ROUTES_NORTH, centroid_dir);
376                angle_score_fn(centroid_angle, rt.angle)
377            };
378
379        // The distance score function is logarithmic and favors long distances (but not
380        // by much). The lower cutoff is so that docks within 5000 blocks of
381        // each other are not connected unless there are no other options. The final
382        // division and subtraction are used to scale the score so that middle
383        // distances have a score of around 1.0.
384        let distance_score_fn = |distance: i64| {
385            // Note the distance argument is the square of the actual distance.
386            if distance > 25000000 {
387                (((distance - 24999000) / 1000) as f32).ln() / 8.0 - 0.5
388            } else {
389                0.0
390            }
391        };
392
393        // overall score function
394        let score_fn = |con: &DockConnection, rt: &AirRouteConnection| {
395            let mut angle_score = match con.connections.len() {
396                // Anything goes
397                0 => 1.0,
398                // Opposite angle
399                1 => angle_score_fn(con.connections[0].angle, rt.angle),
400                // Centroid angle of triangle formed by the first two connections and the dock
401                // center
402                2 => {
403                    let centroid = (con.connections[0].dock2.center
404                        + con.connections[1].dock2.center
405                        + con.dock.center)
406                        / 3.0;
407                    centroid_angle_score_fn(centroid, con.dock.center, rt)
408                },
409                // Centroid angle of triangle formed by the first three connections
410                3 => {
411                    let centroid = (con.connections[0].dock2.center
412                        + con.connections[1].dock2.center
413                        + con.connections[2].dock2.center)
414                        / 3.0;
415                    centroid_angle_score_fn(centroid, con.dock.center, rt)
416                },
417                _ => 0.0,
418            };
419            let distance_score = distance_score_fn(rt.distance);
420            // The 5.0 multiplier was established by trial and error. Without the
421            // multiplier, the routes tend to have a long distance bias. Giving
422            // the angle score more weight helps to balance the resulting route
423            // network.
424            angle_score *= 5.0;
425            (angle_score, distance_score, angle_score + distance_score)
426        };
427
428        for _ in 0..4 {
429            let mut best_trial: Option<(Vec<(Id<site::Site>, Id<site::Site>)>, f32)> = None;
430            // 100 loops to shuffle the dock connections and try different combinations is
431            // enough. Using 1000 loops doesn't improve the results.
432            for _ in 0..100 {
433                dock_connections.shuffle(&mut rng);
434                let candidates = dock_connections
435                    .iter()
436                    .filter(|con| con.available_connections > 0)
437                    .collect::<Vec<_>>();
438                let mut trial = Vec::new();
439                let mut trial_score = 0f32;
440                for chunk in candidates.chunks(2) {
441                    if let [con1, con2] = chunk {
442                        let dock1_id = con1.dock.site_id;
443                        let dock2_id = con2.dock.site_id;
444                        let dock1_route = routes
445                            .get(&DockConnectionHashKey(dock1_id, dock2_id))
446                            .unwrap();
447                        let dock2_route = routes
448                            .get(&DockConnectionHashKey(dock2_id, dock1_id))
449                            .unwrap();
450                        let score1 = score_fn(con1, dock1_route);
451                        let score2 = score_fn(con2, dock2_route);
452                        trial_score += score1.2 + score2.2;
453                        trial.push((dock1_id, dock2_id));
454                    }
455                }
456                if let Some(current_best_trial) = best_trial.as_mut() {
457                    if trial_score > current_best_trial.1 {
458                        *current_best_trial = (trial, trial_score);
459                    }
460                } else {
461                    best_trial = Some((trial, trial_score));
462                }
463            }
464            if let Some(ref final_best_trial) = best_trial {
465                for (site1, site2) in final_best_trial.0.iter() {
466                    let dock1_route = routes.get(&DockConnectionHashKey(*site1, *site2)).unwrap();
467                    let dock2_route = routes.get(&DockConnectionHashKey(*site2, *site1)).unwrap();
468                    let con1 = dock_connections
469                        .iter_mut()
470                        .find(|con| con.dock.site_id == *site1)
471                        .unwrap();
472                    if con1.available_connections > 0 {
473                        con1.add_connection(dock1_route);
474                    }
475                    let con2 = dock_connections
476                        .iter_mut()
477                        .find(|con| con.dock.site_id == *site2)
478                        .unwrap();
479                    if con2.available_connections > 0 {
480                        con2.add_connection(dock2_route);
481                    }
482                }
483            }
484        }
485
486        // The dock connections are now set.
487        // At this point, we now have a network of airship routes between all the sites
488        // with airship docks, and we have a list of docking positions for each
489        // site. As airships are generated, they can be assigned a route based
490        // on their home docking position and the airship routing network.
491        // The number of airships per dock is determined by the number of connections at
492        // the dock. The docking positions used at the dock can be random. Each
493        // airship will have a route assigned that it will fly, out and back,
494        // round trip between two sites. This needs to remain constant so that
495        // travelers can know where the airship is going. The routes can be generated
496        // before the airships, and when an airship is generated, the appropriate route
497        // can be found by finding the docking position id for the docking position with
498        // the wpos closest to the airship position. When an airship is is loaded from
499        // saved RTSim data, the assigned routes will already be available. The airship
500        // routes will be persisted in the rtsim data.
501
502        let mut routes_added = DHashSet::<DockConnectionHashKey>::default();
503        // keep track of the docking positions that have been used on either end of the
504        // route.
505        let mut used_docking_positions = DHashSet::<u32>::default();
506
507        let mut random_dock_pos_fn =
508            |dock: &AirshipDockPositions, used_positions: &DHashSet<u32>| {
509                let mut dock_pos_index = rng.gen_range(0..dock.docking_positions.len());
510                let begin = dock_pos_index;
511                while used_positions.contains(&dock.docking_positions[dock_pos_index].0) {
512                    dock_pos_index = (dock_pos_index + 1) % dock.docking_positions.len();
513                    if dock_pos_index == begin {
514                        return None;
515                    }
516                }
517                Some(dock_pos_index)
518            };
519
520        let mut airship_route_id: u32 = 0;
521        dock_connections.iter().for_each(|con| {
522            con.connections.iter().for_each(|rt| {
523                if !routes_added
524                    .contains(&DockConnectionHashKey(rt.dock1.site_id, rt.dock2.site_id))
525                {
526                    if let Some(from_dock_pos_index) =
527                        random_dock_pos_fn(rt.dock1, &used_docking_positions)
528                    {
529                        if let Some(to_dock_pos_index) =
530                            random_dock_pos_fn(rt.dock2, &used_docking_positions)
531                        {
532                            let from_dock_pos_id =
533                                rt.dock1.docking_positions[from_dock_pos_index].0;
534                            let to_dock_pos_id = rt.dock2.docking_positions[to_dock_pos_index].0;
535                            let approaches = Airships::airship_approaches_for_route(
536                                world_sim,
537                                rt,
538                                from_dock_pos_id,
539                                to_dock_pos_id,
540                            );
541                            let distance = rt.dock1.docking_positions[from_dock_pos_index]
542                                .1
543                                .xy()
544                                .distance(rt.dock2.docking_positions[to_dock_pos_index].1.xy())
545                                as u32;
546
547                            self.routes.insert(
548                                airship_route_id,
549                                AirshipRoute::new(
550                                    rt.dock1.site_id,
551                                    rt.dock2.site_id,
552                                    approaches,
553                                    distance,
554                                ),
555                            );
556                            airship_route_id += 1;
557
558                            used_docking_positions.insert(from_dock_pos_id);
559                            used_docking_positions.insert(to_dock_pos_id);
560                            routes_added
561                                .insert(DockConnectionHashKey(rt.dock1.site_id, rt.dock2.site_id));
562                            routes_added
563                                .insert(DockConnectionHashKey(rt.dock2.site_id, rt.dock1.site_id));
564                        }
565                    }
566                }
567            });
568        });
569    }
570
571    /// Given a docking position, find the airship route and approach index
572    /// where the approach endpoint is closest to the docking position.
573    /// Return the route id (u32) and the approach index (0 or 1).
574    pub fn airship_route_for_docking_pos(
575        &self,
576        docking_pos: Vec3<f32>,
577    ) -> Option<(AirshipRouteId, usize)> {
578        // Find the route where where either approach.dock_pos is equal (very close to)
579        // the given docking_pos.
580        if let Some((route_id, min_index, _)) = self
581            .routes
582            .iter()
583            .flat_map(|(rt_id, rt)| {
584                rt.approaches
585                    .iter()
586                    .enumerate()
587                    .map(move |(index, approach)| {
588                        let distance =
589                            approach.dock_pos.1.xy().distance_squared(docking_pos.xy()) as i64;
590                        (rt_id, index, distance)
591                    })
592            })
593            .min_by_key(|(_, _, distance)| *distance)
594        {
595            Some((*route_id, min_index))
596        } else {
597            // It should be impossible to get here if
598            // should_spawn_airship_at_docking_position is working correctly.
599            warn!(
600                "No airship route has a docking postion near {:?}",
601                docking_pos
602            );
603            None
604        }
605    }
606
607    /// Given a airship dock docking position, determine if an airship should be
608    /// spawned at the docking position. Some airship docks will not have
609    /// the docking positions completely filled because some docks are not
610    /// connected to the maximum number of sites. E.g., if there are an odd
611    /// number of sites with airship docks. Another reason is the way the
612    /// routes are generated.
613    pub fn should_spawn_airship_at_docking_position(
614        &self,
615        docking_pos: &Vec3<i32>,
616        site_name: &str,
617    ) -> bool {
618        let use_docking_pos = self.routes.iter().any(|(_, rt)| {
619            rt.approaches.iter().any(|approach| {
620                approach
621                    .dock_pos
622                    .1
623                    .xy()
624                    .distance_squared(docking_pos.map(|i| i as f32).xy())
625                    < 10.0
626            })
627        });
628        if !use_docking_pos {
629            debug_airships!(
630                "Skipping docking position {:?} for site {}",
631                docking_pos,
632                site_name
633            );
634        }
635        use_docking_pos
636    }
637
638    /// Get the position and direction for the airship to dock at the given
639    /// docking position. If use_starboard_boarding is None, the side for
640    /// boarding is randomly chosen. The center of the airship position with
641    /// respect to the docking position is an asymmetrical offset depending on
642    /// which side of the airship will be used for boarding and where the
643    /// captain is located on the airship. The returned position is the
644    /// position where the captain will be when the airship is docked
645    /// (because the captain NPC is the one that is positioned in the agent
646    /// or rtsim code).
647    pub fn airship_vec_for_docking_pos(
648        docking_pos: Vec3<f32>,
649        airship_dock_center: Vec2<f32>,
650        docking_side: Option<AirshipDockingSide>,
651    ) -> (Vec3<f32>, Dir) {
652        // choose a random side for docking if not specified
653        let dock_side = docking_side.unwrap_or_else(|| {
654            if thread_rng().gen::<bool>() {
655                AirshipDockingSide::Starboard
656            } else {
657                AirshipDockingSide::Port
658            }
659        });
660        // Get the vector from the dock alignment position on the airship to the
661        // captain's position and the rotation angle for the ship to dock on the
662        // specified side. The dock alignment position is where the airship
663        // should touch or come closest to the dock. The side_rotation is the
664        // angle the ship needs to rotate from to be perpendicular to the vector
665        // from the dock center to the docking position. For example, if the docking
666        // position is directly north (0 degrees, or aligned with the unit_y
667        // vector), the ship needs to rotate 90 degrees CCW to dock on the port
668        // side or 270 degrees CCW to dock on the starboard side.
669        let (dock_align_to_captain, side_rotation) = if dock_side == AirshipDockingSide::Starboard {
670            (
671                Airships::DOCK_ALIGN_POS_TO_CAPT_STARBOARD,
672                3.0 * std::f32::consts::FRAC_PI_2,
673            )
674        } else {
675            (
676                Airships::DOCK_ALIGN_POS_TO_CAPT_PORT,
677                std::f32::consts::FRAC_PI_2,
678            )
679        };
680        // get the vector from the dock center to the docking platform point where the
681        // airship should touch or come closest to.
682        let dock_pos_offset = (docking_pos - airship_dock_center).xy();
683        // The airship direction when docked is the dock_pos_offset rotated by the
684        // side_rotation angle.
685        let airship_dir =
686            Dir::from_unnormalized(dock_pos_offset.rotated_z(side_rotation).with_z(0.0))
687                .unwrap_or_default();
688        // The dock_align_to_captain vector is rotated by the angle between unit_y and
689        // the airship direction.
690        let ship_dock_rotation =
691            Airships::angle_between_vectors_ccw(Vec2::unit_y(), airship_dir.vec().xy());
692        let captain_offset = dock_align_to_captain
693            .rotated_z(ship_dock_rotation)
694            .with_z(Airships::AIRSHIP_TO_DOCK_Z_OFFSET);
695
696        // To get the location of the pilot when the ship is docked, add the
697        // captain_offset to the docking position.
698        (docking_pos + captain_offset, airship_dir)
699    }
700
701    // Get the docking approach for the given docking position.
702    fn docking_approach_for(
703        depart_center: Vec2<f32>,
704        dest_center: Vec2<f32>,
705        docking_pos: &AirshipDockingPosition,
706        depart_to_dest_angle: f32,
707        map_center: Vec2<f32>,
708        max_dims: Vec2<f32>,
709        site_id: Id<Site>,
710    ) -> AirshipDockingApproach {
711        let (airship_pos, airship_direction) = Airships::airship_vec_for_docking_pos(
712            docking_pos.1,
713            dest_center,
714            Some(AirshipDockingSide::Starboard),
715        );
716        // calculate port final point. It is a 500 block extension from the docking
717        // position in the direction of the docking direction.
718        let port_final_pos = docking_pos.1.xy() + airship_direction.to_vec().xy() * 500.0;
719        let starboard_final_pos = docking_pos.1.xy() - airship_direction.to_vec().xy() * 500.0;
720        // calculate the turn angle required to align with the port. The port final
721        // point is the origin. One vector is the reverse of the vector from the
722        // port final point to the departure center. The other vector is from
723        // the port final point to the docking position.
724        let port_final_angle =
725            (airship_pos.xy() - port_final_pos).angle_between(-(depart_center - port_final_pos));
726        // The starboard angle is calculated similarly.
727        let starboard_final_angle = (airship_pos.xy() - starboard_final_pos)
728            .angle_between(-(depart_center - starboard_final_pos));
729
730        // If the angles are approximately equal, it means the departure position and
731        // the docking position are on the same line (angle near zero) or are
732        // perpendicular to each other (angle near 90). If perpendicular, pick
733        // the side where the final approach point is furthest from the edge of the map.
734        // If on the same line, pick the side where the final approach point is closest
735        // to the departure position.
736        let side = if (port_final_angle - starboard_final_angle).abs() < 0.1 {
737            // equal angles
738            if port_final_angle < std::f32::consts::FRAC_PI_4 {
739                // same line
740                if port_final_pos.distance_squared(depart_center)
741                    < starboard_final_pos.distance_squared(depart_center)
742                {
743                    // dock on port side
744                    AirshipDockingSide::Port
745                } else {
746                    // dock on starboard side
747                    AirshipDockingSide::Starboard
748                }
749            } else {
750                // perpendicular
751                // Use the final point closest to the center of the map.
752                if port_final_pos.distance_squared(map_center)
753                    < starboard_final_pos.distance_squared(map_center)
754                {
755                    // dock on port side
756                    AirshipDockingSide::Port
757                } else {
758                    // dock on starboard side
759                    AirshipDockingSide::Starboard
760                }
761            }
762        } else {
763            // pick the side with the least turn angle.
764            if port_final_angle < starboard_final_angle {
765                // port side
766                AirshipDockingSide::Port
767            } else {
768                // starboard side
769                AirshipDockingSide::Starboard
770            }
771        };
772
773        let height = if depart_to_dest_angle < std::f32::consts::PI {
774            Airships::STD_CRUISE_HEIGHT
775        } else {
776            Airships::STD_CRUISE_HEIGHT + 100.0
777        };
778
779        let check_pos_fn = |pos: Vec2<f32>, what: &str| {
780            if pos.x < 0.0 || pos.y < 0.0 || pos.x > max_dims.x || pos.y > max_dims.y {
781                warn!("{} pos out of bounds: {:?}", what, pos);
782            }
783        };
784
785        let initial_pos_fn = |final_pos: Vec2<f32>| {
786            // Get the angle between a line (1) connecting the final_pos and the
787            // depart_center and line (2) from the final_pos to the docking
788            // position. divide the angle in half then rotate line 1 CCW by that
789            // angle + 270 degrees. The initial approach point is on this
790            // rotated line 1, 500 blocks from the final_pos.
791            let line1 = (depart_center - final_pos).normalized();
792            let angle = line1.angle_between((airship_pos.xy() - final_pos).normalized());
793            let initial_pos_line = line1.rotated_z(angle / 2.0 + 3.0 * std::f32::consts::FRAC_PI_2);
794            let initial_pos = final_pos + initial_pos_line * 500.0;
795            check_pos_fn(final_pos, "final_pos");
796            check_pos_fn(initial_pos, "initial_pos");
797            initial_pos
798        };
799
800        if side == AirshipDockingSide::Starboard {
801            AirshipDockingApproach {
802                dock_pos: *docking_pos,
803                airship_pos,
804                airship_direction,
805                dock_center: dest_center,
806                height,
807                approach_initial_pos: initial_pos_fn(starboard_final_pos),
808                approach_final_pos: starboard_final_pos,
809                side,
810                site_id,
811            }
812        } else {
813            // recalculate the actual airship position and direction for the port side.
814            let (airship_pos, airship_direction) = Airships::airship_vec_for_docking_pos(
815                docking_pos.1,
816                dest_center,
817                Some(AirshipDockingSide::Port),
818            );
819            AirshipDockingApproach {
820                dock_pos: *docking_pos,
821                airship_pos,
822                airship_direction,
823                dock_center: dest_center,
824                height,
825                approach_initial_pos: initial_pos_fn(port_final_pos),
826                approach_final_pos: port_final_pos,
827                side,
828                site_id,
829            }
830        }
831    }
832
833    /// Builds approaches for the given route connection.
834    /// Each docking position has two possible approaches, based on the
835    /// port and starboard sides of the airship. The approaches are aligned
836    /// with the docking position direction, which is always perpendicular
837    /// to the vector from the airship dock plot center to the docking position.
838    /// The airship can pivot around the z axis, but it does so slowly. To
839    /// ensure that the airship is oriented in the correct direction for
840    /// landing, and to make it more realistic, the airship approaches
841    /// the docking position pre-aligned with the landing direction. The
842    /// approach consists of two positions, the initial point where the
843    /// airship will turn toward the final point, at the final point it will
844    /// turn toward the docking position and will be aligned with the docking
845    /// direction.
846    fn airship_approaches_for_route(
847        world_sim: &mut WorldSim,
848        route: &AirRouteConnection,
849        dock1_position_id: u32,
850        dock2_position_id: u32,
851    ) -> [AirshipDockingApproach; 2] {
852        /*  o Pick the docking side with the least rotation angle from the departure position.
853              If the angles are approximately equal, it means the departure position and
854              the docking position are on the same line (angle near zero) or are perpendicular to
855              each other (angle near 90). If perpendicular, pick the side where the final approach
856              point is furthest from the edge of the map. If on the same line, pick the side where
857              the final approach point is closest to the departure position.
858            o The cruising height above terrain is based on the angle between North and the
859              line between the docking positions.
860        */
861
862        let map_size_chunks = world_sim.get_size().map(|u| u as i32);
863        let max_dims = map_size_chunks.cpos_to_wpos().map(|u| u as f32);
864        let map_center = Vec2::new(max_dims.x / 2.0, max_dims.y / 2.0);
865
866        let dock1_positions = &route.dock1;
867        let dock2_positions = &route.dock2;
868        let dock1_center = dock1_positions.center;
869        let dock2_center = dock2_positions.center;
870        let docking_pos1 = dock1_positions
871            .docking_positions
872            .iter()
873            .find(|dp| dp.0 == dock1_position_id)
874            .unwrap();
875        let docking_pos2 = dock2_positions
876            .docking_positions
877            .iter()
878            .find(|dp| dp.0 == dock2_position_id)
879            .unwrap();
880        let dock1_to_dock2_angle = Airships::angle_between_vectors_ccw(
881            Airships::ROUTES_NORTH,
882            docking_pos2.1.xy() - docking_pos1.1.xy(),
883        );
884        let dock2_to_dock1_angle = std::f32::consts::TAU - dock1_to_dock2_angle;
885        debug_airships!(
886            "airship_approaches_for_route - dock1_pos:{:?}, dock2_pos:{:?}, \
887             dock1_to_dock2_angle:{}, dock2_to_dock1_angle:{}",
888            docking_pos1,
889            docking_pos2,
890            dock1_to_dock2_angle,
891            dock2_to_dock1_angle
892        );
893
894        [
895            Airships::docking_approach_for(
896                dock1_center,
897                dock2_center,
898                docking_pos2,
899                dock1_to_dock2_angle,
900                map_center,
901                max_dims,
902                dock2_positions.site_id,
903            ),
904            Airships::docking_approach_for(
905                dock2_center,
906                dock1_center,
907                docking_pos1,
908                dock2_to_dock1_angle,
909                map_center,
910                max_dims,
911                dock1_positions.site_id,
912            ),
913        ]
914    }
915
916    /// Returns the angle from vec v1 to vec v2 in the CCW direction.
917    fn angle_between_vectors_ccw(v1: Vec2<f32>, v2: Vec2<f32>) -> f32 {
918        let dot_product = v1.dot(v2);
919        let det = v1.x * v2.y - v1.y * v2.x; // determinant
920        let angle = det.atan2(dot_product); // atan2(det, dot_product) gives the CCW angle
921        if angle < 0.0 {
922            angle + std::f32::consts::TAU
923        } else {
924            angle
925        }
926    }
927
928    /// Returns the angle from vec v1 to vec v2 in the CW direction.
929    fn angle_between_vectors_cw(v1: Vec2<f32>, v2: Vec2<f32>) -> f32 {
930        let ccw_angle = Airships::angle_between_vectors_ccw(v1, v2);
931        std::f32::consts::TAU - ccw_angle
932    }
933}
934
935/// For debuging the airship routes. Writes the airship routes to a json file.
936fn write_airship_routes_log(file_path: &str, jsonstr: &str) -> std::io::Result<()> {
937    let mut file = OpenOptions::new()
938        .write(true)
939        .create(true)
940        .truncate(true)
941        .open(file_path)?;
942    file.write_all(jsonstr.as_bytes())?;
943    Ok(())
944}
945
946#[cfg(test)]
947mod tests {
948    use super::{AirshipDockingSide, Airships, approx::assert_relative_eq};
949    use vek::{Quaternion, Vec2, Vec3};
950
951    #[test]
952    fn basic_vec_test() {
953        let vec1 = Vec3::new(0.0f32, 10.0, 0.0);
954        let vec2 = Vec3::new(10.0, 0.0, 0.0);
955        let a12 = vec2.angle_between(vec1);
956        assert_relative_eq!(a12, std::f32::consts::FRAC_PI_2, epsilon = 0.00001);
957
958        let rotc2 = Quaternion::rotation_z(a12);
959        let vec3 = rotc2 * vec2;
960        assert!(vec3 == vec1);
961    }
962
963    #[test]
964    fn std_vec_angles_test() {
965        let refvec = Vec2::new(0.0f32, 10.0);
966
967        let vec1 = Vec2::new(0.0f32, 10.0);
968        let vec2 = Vec2::new(10.0f32, 0.0);
969        let vec3 = Vec2::new(0.0f32, -10.0);
970        let vec4 = Vec2::new(-10.0f32, 0.0);
971
972        let a1r = vec1.angle_between(refvec);
973        assert!(a1r == 0.0f32);
974
975        let a2r = vec2.angle_between(refvec);
976        assert_relative_eq!(a2r, std::f32::consts::FRAC_PI_2, epsilon = 0.00001);
977
978        let a3r: f32 = vec3.angle_between(refvec);
979        assert_relative_eq!(a3r, std::f32::consts::PI, epsilon = 0.00001);
980
981        let a4r = vec4.angle_between(refvec);
982        assert_relative_eq!(a4r, std::f32::consts::FRAC_PI_2, epsilon = 0.00001);
983    }
984
985    #[test]
986    fn vec_angles_test() {
987        let refvec = Vec3::new(0.0f32, 10.0, 0.0);
988
989        let vec1 = Vec3::new(0.0f32, 10.0, 0.0);
990        let vec2 = Vec3::new(10.0f32, 0.0, 0.0);
991        let vec3 = Vec3::new(0.0f32, -10.0, 0.0);
992        let vec4 = Vec3::new(-10.0f32, 0.0, 0.0);
993
994        let a1r = vec1.angle_between(refvec);
995        let a1r3 = Airships::angle_between_vectors_ccw(vec1.xy(), refvec.xy());
996        assert!(a1r == 0.0f32);
997        assert!(a1r3 == 0.0f32);
998
999        let a2r = vec2.angle_between(refvec);
1000        let a2r3 = Airships::angle_between_vectors_ccw(vec2.xy(), refvec.xy());
1001        assert_relative_eq!(a2r, std::f32::consts::FRAC_PI_2, epsilon = 0.00001);
1002        assert_relative_eq!(a2r3, std::f32::consts::FRAC_PI_2, epsilon = 0.00001);
1003
1004        let a3r: f32 = vec3.angle_between(refvec);
1005        let a3r3 = Airships::angle_between_vectors_ccw(vec3.xy(), refvec.xy());
1006        assert_relative_eq!(a3r, std::f32::consts::PI, epsilon = 0.00001);
1007        assert_relative_eq!(a3r3, std::f32::consts::PI, epsilon = 0.00001);
1008
1009        let a4r = vec4.angle_between(refvec);
1010        let a4r3 = Airships::angle_between_vectors_ccw(vec4.xy(), refvec.xy());
1011        assert_relative_eq!(a4r, std::f32::consts::FRAC_PI_2, epsilon = 0.00001);
1012        assert_relative_eq!(a4r3, std::f32::consts::FRAC_PI_2 * 3.0, epsilon = 0.00001);
1013    }
1014
1015    #[test]
1016    fn airship_angles_test() {
1017        let refvec = Vec2::new(0.0f32, 37.0);
1018        let ovec = Vec2::new(-4.0f32, -14.0);
1019        let oveccw0 = Vec2::new(-4, -14);
1020        let oveccw90 = Vec2::new(-14, 4);
1021        let oveccw180 = Vec2::new(4, 14);
1022        let oveccw270 = Vec2::new(14, -4);
1023        let ovecccw0 = Vec2::new(-4, -14);
1024        let ovecccw90 = Vec2::new(14, -4);
1025        let ovecccw180 = Vec2::new(4, 14);
1026        let ovecccw270 = Vec2::new(-14, 4);
1027
1028        let vec1 = Vec2::new(0.0f32, 37.0);
1029        let vec2 = Vec2::new(37.0f32, 0.0);
1030        let vec3 = Vec2::new(0.0f32, -37.0);
1031        let vec4 = Vec2::new(-37.0f32, 0.0);
1032
1033        assert!(
1034            ovec.rotated_z(Airships::angle_between_vectors_cw(vec1, refvec))
1035                .map(|x| x.round() as i32)
1036                == oveccw0
1037        );
1038        assert!(
1039            ovec.rotated_z(Airships::angle_between_vectors_cw(vec2, refvec))
1040                .map(|x| x.round() as i32)
1041                == oveccw90
1042        );
1043        assert!(
1044            ovec.rotated_z(Airships::angle_between_vectors_cw(vec3, refvec))
1045                .map(|x| x.round() as i32)
1046                == oveccw180
1047        );
1048        assert!(
1049            ovec.rotated_z(Airships::angle_between_vectors_cw(vec4, refvec))
1050                .map(|x| x.round() as i32)
1051                == oveccw270
1052        );
1053
1054        assert!(
1055            ovec.rotated_z(Airships::angle_between_vectors_ccw(vec1, refvec))
1056                .map(|x| x.round() as i32)
1057                == ovecccw0
1058        );
1059        assert!(
1060            ovec.rotated_z(Airships::angle_between_vectors_ccw(vec2, refvec))
1061                .map(|x| x.round() as i32)
1062                == ovecccw90
1063        );
1064        assert!(
1065            ovec.rotated_z(Airships::angle_between_vectors_ccw(vec3, refvec))
1066                .map(|x| x.round() as i32)
1067                == ovecccw180
1068        );
1069        assert!(
1070            ovec.rotated_z(Airships::angle_between_vectors_ccw(vec4, refvec))
1071                .map(|x| x.round() as i32)
1072                == ovecccw270
1073        );
1074    }
1075
1076    #[test]
1077    fn airship_vec_test() {
1078        {
1079            let dock_pos = Vec3::new(10.0f32, 10.0, 0.0);
1080            let airship_dock_center = Vec2::new(0.0, 0.0);
1081            let mut left_tested = false;
1082            let mut right_tested = false;
1083            {
1084                for _ in 0..1000 {
1085                    let (airship_pos, airship_dir) =
1086                        Airships::airship_vec_for_docking_pos(dock_pos, airship_dock_center, None);
1087                    if airship_pos.x > 21.0 {
1088                        assert_relative_eq!(
1089                            airship_pos,
1090                            Vec3 {
1091                                x: 29.091883,
1092                                y: 17.778175,
1093                                z: -1.0
1094                            },
1095                            epsilon = 0.00001
1096                        );
1097                        assert_relative_eq!(
1098                            airship_dir.to_vec(),
1099                            Vec3 {
1100                                x: -0.70710677,
1101                                y: 0.70710677,
1102                                z: 0.0
1103                            },
1104                            epsilon = 0.00001
1105                        );
1106                        left_tested = true;
1107                    } else {
1108                        assert_relative_eq!(
1109                            airship_pos,
1110                            Vec3 {
1111                                x: 17.778172,
1112                                y: 29.091885,
1113                                z: -1.0
1114                            },
1115                            epsilon = 0.00001
1116                        );
1117                        assert_relative_eq!(
1118                            airship_dir.to_vec(),
1119                            Vec3 {
1120                                x: 0.70710677,
1121                                y: -0.70710677,
1122                                z: 0.0
1123                            },
1124                            epsilon = 0.00001
1125                        );
1126                        right_tested = true;
1127                    }
1128                    if left_tested && right_tested {
1129                        break;
1130                    }
1131                }
1132            }
1133            {
1134                let (airship_pos, airship_dir) = Airships::airship_vec_for_docking_pos(
1135                    dock_pos,
1136                    airship_dock_center,
1137                    Some(AirshipDockingSide::Port),
1138                );
1139                assert_relative_eq!(
1140                    airship_pos,
1141                    Vec3 {
1142                        x: 29.091883,
1143                        y: 17.778175,
1144                        z: -1.0
1145                    },
1146                    epsilon = 0.00001
1147                );
1148                assert_relative_eq!(
1149                    airship_dir.to_vec(),
1150                    Vec3 {
1151                        x: -0.70710677,
1152                        y: 0.70710677,
1153                        z: 0.0
1154                    },
1155                    epsilon = 0.00001
1156                );
1157            }
1158            {
1159                let (airship_pos, airship_dir) = Airships::airship_vec_for_docking_pos(
1160                    dock_pos,
1161                    airship_dock_center,
1162                    Some(AirshipDockingSide::Starboard),
1163                );
1164                assert_relative_eq!(
1165                    airship_pos,
1166                    Vec3 {
1167                        x: 17.778172,
1168                        y: 29.091885,
1169                        z: -1.0
1170                    },
1171                    epsilon = 0.00001
1172                );
1173                assert_relative_eq!(
1174                    airship_dir.to_vec(),
1175                    Vec3 {
1176                        x: 0.70710677,
1177                        y: -0.70710677,
1178                        z: 0.0
1179                    },
1180                    epsilon = 0.00001
1181                );
1182            }
1183        }
1184        {
1185            let dock_pos = Vec3::new(28874.0, 18561.0, 0.0);
1186            let airship_dock_center = Vec2::new(28911.0, 18561.0);
1187            {
1188                let (airship_pos, airship_dir) = Airships::airship_vec_for_docking_pos(
1189                    dock_pos,
1190                    airship_dock_center,
1191                    Some(AirshipDockingSide::Port),
1192                );
1193                assert_relative_eq!(
1194                    airship_pos,
1195                    Vec3 {
1196                        x: 28855.0,
1197                        y: 18569.0,
1198                        z: -1.0
1199                    },
1200                    epsilon = 0.00001
1201                );
1202                assert_relative_eq!(
1203                    airship_dir.to_vec(),
1204                    Vec3 {
1205                        x: 4.371139e-8,
1206                        y: -1.0,
1207                        z: 0.0
1208                    },
1209                    epsilon = 0.00001
1210                );
1211            }
1212            {
1213                let (airship_pos, airship_dir) = Airships::airship_vec_for_docking_pos(
1214                    dock_pos,
1215                    airship_dock_center,
1216                    Some(AirshipDockingSide::Starboard),
1217                );
1218                assert_relative_eq!(
1219                    airship_pos,
1220                    Vec3 {
1221                        x: 28855.0,
1222                        y: 18553.0,
1223                        z: -1.0
1224                    },
1225                    epsilon = 0.00001
1226                );
1227                assert_relative_eq!(
1228                    airship_dir.to_vec(),
1229                    Vec3 {
1230                        x: -1.1924881e-8,
1231                        y: 1.0,
1232                        z: 0.0
1233                    },
1234                    epsilon = 0.00001
1235                );
1236            }
1237        }
1238    }
1239
1240    #[test]
1241    fn angle_score_test() {
1242        let rt_angles = [
1243            0.0,
1244            std::f32::consts::FRAC_PI_2,
1245            std::f32::consts::PI,
1246            std::f32::consts::FRAC_PI_2 * 3.0,
1247        ];
1248        let con_angles = [
1249            0.0,
1250            std::f32::consts::FRAC_PI_2,
1251            std::f32::consts::PI,
1252            std::f32::consts::FRAC_PI_2 * 3.0,
1253        ];
1254        let scores = [
1255            [0.0, 2.5, 5.0, 2.5],
1256            [2.5, 0.0, 2.5, 5.0],
1257            [5.0, 2.5, 0.0, 2.5],
1258            [2.5, 5.0, 2.5, 0.0],
1259        ];
1260        let score_fn2 = |a1: f32, a2: f32| {
1261            let optimal_angle = (a1 + std::f32::consts::PI).rem_euclid(std::f32::consts::TAU);
1262            let angle_diff = (optimal_angle - a2)
1263                .abs()
1264                .min(std::f32::consts::TAU - (optimal_angle - a2).abs());
1265            (1.0 - (angle_diff / std::f32::consts::PI)) * 5.0
1266        };
1267        let mut i = 0;
1268        let mut j = 0;
1269        rt_angles.iter().for_each(|rt_angle| {
1270            j = 0;
1271            con_angles.iter().for_each(|con_angle| {
1272                let score = score_fn2(*con_angle, *rt_angle);
1273                assert_relative_eq!(score, scores[i][j], epsilon = 0.00001);
1274                j += 1;
1275            });
1276            i += 1;
1277        });
1278    }
1279
1280    #[test]
1281    fn distance_score_test() {
1282        let distances = [1, 1000, 5001, 6000, 15000, 30000, 48000];
1283        let scores = [
1284            0.0,
1285            0.0,
1286            -0.20026308,
1287            0.66321766,
1288            1.0257597,
1289            1.2102475,
1290            1.329906,
1291        ];
1292        let score_fn = |distance: i64| {
1293            if distance > 25000000 {
1294                (((distance - 24999000) / 1000) as f32).ln() / 8.0 - 0.5
1295            } else {
1296                0.0
1297            }
1298        };
1299        let mut i = 0;
1300        distances.iter().for_each(|distance| {
1301            let dist2 = *distance * *distance;
1302            let score = score_fn(dist2);
1303            assert_relative_eq!(score, scores[i], epsilon = 0.00001);
1304            i += 1;
1305        });
1306    }
1307}