rFactor2_vk_hud/src/overlay/elements/radar.rs

use anyhow::Result;
use cgmath::{ortho, vec2, vec3, vec4, Deg, InnerSpace, Matrix4, Rad, Vector2, Vector3, Vector4};
use rfactor_sm_reader::*;
use serde::{Deserialize, Serialize};
use vulkan_rs::prelude::*;

use std::sync::{Arc, Mutex};

use super::pipeline::SingleColorPipeline;

use crate::{
    overlay::{rendering::Rendering, rfactor_data::DataReceiver, UiOverlay},
    write_log,
};

#[derive(Clone)]
pub struct PositionOnlyVertex {
    pub position: Vector4<f32>,
}

impl PositionOnlyVertex {
    ///
    /// corners[0]  -  bottom left
    /// corners[1]  -  top left
    /// corners[2]  -  top right
    /// corners[3]  -  bottom right
    ///
    pub fn from_2d_corners(ortho: Matrix4<f32>, corners: [Vector2<f32>; 4]) -> [Self; 6] {
        [
            Self {
                position: ortho * corners[0].extend(0.0).extend(1.0),
            },
            Self {
                position: ortho * corners[1].extend(0.0).extend(1.0),
            },
            Self {
                position: ortho * corners[2].extend(0.0).extend(1.0),
            },
            Self {
                position: ortho * corners[2].extend(0.0).extend(1.0),
            },
            Self {
                position: ortho * corners[3].extend(0.0).extend(1.0),
            },
            Self {
                position: ortho * corners[0].extend(0.0).extend(1.0),
            },
        ]
    }
}

fn convert_vec(v: rF2Vec3) -> Vector3<f32> {
    vec3(v.x as f32, v.y as f32, v.z as f32)
}

#[derive(Deserialize, Serialize, Clone, Copy, Debug)]
pub struct RadarConfig {
    pub radar_scale: f32,
    pub radar_center_factor: f32,
    pub radar_transparency: f32,
    pub height_scale: f32,
    pub width_scale: f32,
    pub radar_car_distance: f32,
    pub safe_color: Vector3<f32>,
    pub danger_color: Vector3<f32>,
}

impl RadarConfig {
    pub const fn new() -> Self {
        Self {
            radar_scale: 1.0,
            radar_center_factor: 0.25,
            radar_transparency: 0.5,
            height_scale: 0.15,
            width_scale: 0.4,
            radar_car_distance: 20.0,
            safe_color: vec3(0.0, 0.75, 0.0),
            danger_color: vec3(0.75, 0.0, 0.0),
        }
    }
}

pub struct Radar {
    // config
    config: RadarConfig,

    // radar objects
    background: Option<RadarObject>,
    player_car: RadarObject,
    cars: Vec<RadarObject>,

    // buffer car objects, to prevent recreating them every update
    car_handles: Vec<RadarObject>,

    // math objects
    radar_center: Vector2<f32>,
    ortho: Matrix4<f32>,
    _window_width: u32,
    _window_height: u32,
    radar_extent: f32,
    car_width: f32,
    car_height: f32,

    device: Arc<Device>,
    queue: Arc<Mutex<Queue>>,

    pipeline: SingleColorPipeline,
    render_target: RenderTarget,
}

impl Radar {
    pub fn new(
        config: RadarConfig,
        device: Arc<Device>,
        queue: Arc<Mutex<Queue>>,
        rendering: &Rendering,
    ) -> Result<Self> {
        let radar_extent = rendering.swapchain().width() as f32 * 0.075 * config.radar_scale;
        let car_height = radar_extent * config.height_scale;
        let car_width = car_height * config.width_scale;
        let radar_center = vec2(
            rendering.swapchain().width() as f32 / 2.0,
            rendering.swapchain().height() as f32 / 2.0
                - rendering.swapchain().height() as f32 * config.radar_center_factor,
        );

        let flip_y = matrix4_from_diagonal(vec3(1.0, -1.0, 1.0));
        let ortho = flip_y
            * ortho(
                0.0,
                rendering.swapchain().width() as f32,
                0.0,
                rendering.swapchain().height() as f32,
                -1.0,
                1.0,
            );

        let render_target = RenderTarget::builder()
            .add_sub_pass(
                SubPass::builder(
                    rendering.swapchain().width(),
                    rendering.swapchain().height(),
                )
                .set_prepared_targets(&rendering.images(), 0, [0.0, 0.0, 0.0, 1.0], false)
                .build(&device, &queue)?,
            )
            .build(&device)?;

        let pipeline = SingleColorPipeline::new(
            device.clone(),
            render_target.render_pass(),
            rendering.swapchain().width(),
            rendering.swapchain().height(),
        )?;

        Ok(Self {
            config,

            background: if config.radar_transparency == 0.0 {
                None
            } else {
                Some(RadarObject::new(
                    device.clone(),
                    pipeline.descriptor_layout(),
                    PositionOnlyVertex::from_2d_corners(
                        ortho * Matrix4::from_translation(radar_center.extend(0.0)),
                        [
                            vec2(-radar_extent, -radar_extent),
                            vec2(-radar_extent, radar_extent),
                            vec2(radar_extent, radar_extent),
                            vec2(radar_extent, -radar_extent),
                        ],
                    ),
                    [0.5, 0.5, 0.5, config.radar_transparency],
                )?)
            },
            player_car: RadarObject::new(
                device.clone(),
                pipeline.descriptor_layout(),
                PositionOnlyVertex::from_2d_corners(
                    ortho * Matrix4::from_translation(radar_center.extend(0.0)),
                    [
                        vec2(-car_width, -car_height),
                        vec2(-car_width, car_height),
                        vec2(car_width, car_height),
                        vec2(car_width, -car_height),
                    ],
                ),
                [0.0, 0.9, 0.0, 0.9],
            )?,
            cars: Vec::new(),
            car_handles: Vec::new(),

            radar_center,
            ortho,
            _window_width: rendering.swapchain().width(),
            _window_height: rendering.swapchain().height(),
            radar_extent,
            car_width,
            car_height,

            device,
            queue,

            render_target,
            pipeline,
        })
    }

    fn create_car_object(&self, offset: Vector2<f32>, color: [f32; 4]) -> Result<RadarObject> {
        write_log!(" =================== create car object ===================");

        RadarObject::new(
            self.device.clone(),
            &self.pipeline.descriptor_layout(),
            Self::create_car_vertices(
                self.ortho
                    * Matrix4::from_translation(self.radar_center.extend(0.0))
                    * Matrix4::from_translation(offset.extend(0.0)),
                self.car_width,
                self.car_height,
            ),
            color,
        )
    }

    fn create_car_vertices(
        mvp: Matrix4<f32>,
        car_width: f32,
        car_height: f32,
    ) -> [PositionOnlyVertex; 6] {
        PositionOnlyVertex::from_2d_corners(
            mvp,
            [
                vec2(-car_width, -car_height),
                vec2(-car_width, car_height),
                vec2(car_width, car_height),
                vec2(car_width, -car_height),
            ],
        )
    }

    pub fn render(&self, image_index: u32) -> Result<Arc<CommandBuffer>> {
        let command_buffer =
            CommandBuffer::new_primary().build(self.device.clone(), self.queue.clone())?;

        {
            let mut recorder = command_buffer.begin(VkCommandBufferBeginInfo::new(
                VK_COMMAND_BUFFER_USAGE_ONE_TIME_SUBMIT_BIT
                    | VK_COMMAND_BUFFER_USAGE_SIMULTANEOUS_USE_BIT,
            ))?;

            self.render_target
                .begin(&recorder, VK_SUBPASS_CONTENTS_INLINE, image_index as usize);

            recorder.bind_pipeline(self.pipeline.pipeline())?;

            for object in self.objects() {
                let buffer = &object.position_buffer;

                recorder.bind_descriptor_sets_minimal(&[&object.descriptor_set]);
                recorder.bind_vertex_buffer(buffer);
                recorder.draw_complete_single_instance(buffer.size() as u32);
            }

            self.render_target.end(&recorder);
        }

        Ok(command_buffer)
    }

    fn objects(&self) -> Vec<&RadarObject> {
        write_log!(" =================== get objects of radar ===================");

        let mut objects = Vec::new();

        // only draw radar when any car is near enough
        if !self.cars.is_empty() {
            if let Some(background) = &self.background {
                objects.push(background);
            }

            for other_player_cars in &self.cars {
                objects.push(other_player_cars);
            }

            objects.push(&self.player_car);
        }

        write_log!(format!("obj count {}", objects.len()));

        objects
    }
}

impl UiOverlay for Radar {}

impl DataReceiver for Radar {
    fn scoring_update(&mut self, _vehicle_scoring: &[VehicleScoringInfoV01]) -> Result<()> {
        Ok(())
    }

    fn telemetry_update(
        &mut self,
        player_id: Option<i32>,
        telemetries: &[rF2VehicleTelemetry],
    ) -> Result<()> {
        write_log!(" ============================ Radar telemetry udpate ======================");
        write_log!(format!("player id {:?}", player_id));

        self.cars.clear();

        if let Some(player_id) = player_id {
            // make sure there are enough cars in buffer
            if self.car_handles.len() < telemetries.len() {
                let size_diff = telemetries.len() - self.car_handles.len();

                for _ in 0..size_diff {
                    self.car_handles
                        .push(self.create_car_object(vec2(0.0, 0.0), [0.0, 0.0, 0.0, 0.0])?);
                }
            }

            let mut player_position = CarPosition::default();
            let mut other_positions = Vec::new();

            for telemetry in telemetries {
                let car = CarPosition::new(
                    convert_vec(telemetry.position),
                    [
                        convert_vec(telemetry.orientation[0]),
                        convert_vec(telemetry.orientation[1]),
                        convert_vec(telemetry.orientation[2]),
                    ],
                );

                if telemetry.id == player_id {
                    player_position = car
                } else {
                    other_positions.push(car);
                }
            }

            // update radar objects
            let mut buffer_car_index = 0;

            for other_position in other_positions {
                let diff = player_position.position - other_position.position;
                let distance = diff.magnitude();

                // check if car is close enough to the players car
                if distance < self.config.radar_car_distance {
                    let offset = diff.xz() * (self.radar_extent / self.config.radar_car_distance);

                    let buffered_car = self.car_handles[buffer_car_index].clone();
                    buffer_car_index += 1;
                    buffered_car.update(
                        self.ortho,
                        offset,
                        player_position.rotation,
                        other_position.rotation,
                        self.radar_center,
                        self.car_width,
                        self.car_height,
                        [0.9, 0.9, 0.0, 0.9],
                    )?;

                    self.cars.push(buffered_car);
                }
            }
        }

        write_log!(format!("other cars: {:?}", self.cars.len()));

        Ok(())
    }
}

#[derive(Clone)]
struct RadarObject {
    descriptor_set: Arc<DescriptorSet>,

    // uniform buffer
    color_buffer: Arc<Buffer<f32>>,

    // vertex buffer
    position_buffer: Arc<Buffer<PositionOnlyVertex>>,
}

impl RadarObject {
    fn new(
        device: Arc<Device>,
        descriptor_layout: &Arc<DescriptorSetLayout>,
        positions: [PositionOnlyVertex; 6],
        color: [f32; 4],
    ) -> Result<Self> {
        let color_buffer = Buffer::builder()
            .set_usage(VK_BUFFER_USAGE_UNIFORM_BUFFER_BIT)
            .set_memory_usage(MemoryUsage::CpuOnly)
            .set_data(&color)
            .build(device.clone())?;

        let position_buffer = Buffer::builder()
            .set_usage(VK_BUFFER_USAGE_VERTEX_BUFFER_BIT)
            .set_memory_usage(MemoryUsage::CpuOnly)
            .set_data(&positions)
            .build(device.clone())?;

        let descriptor_pool = DescriptorPool::builder()
            .set_layout(descriptor_layout.clone())
            .build(device.clone())?;

        let descriptor_set = descriptor_pool.prepare_set().allocate()?;
        descriptor_set.update(&[DescriptorWrite::uniform_buffers(0, &[&color_buffer])])?;

        Ok(Self {
            descriptor_set,
            color_buffer,
            position_buffer,
        })
    }

    pub fn update(
        &self,

        ortho: Matrix4<f32>,
        offset: Vector2<f32>,
        player_rotation: impl Into<Deg<f32>>,
        rotation: impl Into<Deg<f32>>,
        radar_center: Vector2<f32>,
        car_width: f32,
        car_height: f32,
        color: [f32; 4],
    ) -> Result<()> {
        self.position_buffer.fill(&Radar::create_car_vertices(
            ortho
                * Matrix4::from_translation(radar_center.extend(0.0))
                * Matrix4::from_angle_z(-player_rotation.into())
                * Matrix4::from_translation(offset.extend(0.0))
                * Matrix4::from_angle_z(rotation.into()),
            car_width,
            car_height,
        ))?;
        self.color_buffer.fill(&color)
    }
}

struct CarPosition {
    pub position: Vector3<f32>,
    pub rotation: Rad<f32>,
}

impl CarPosition {
    fn new(position: Vector3<f32>, orientation: [Vector3<f32>; 3]) -> Self {
        Self {
            position,
            rotation: Rad(orientation[2].x.atan2(orientation[2].z)),
        }
    }
}

impl Default for CarPosition {
    fn default() -> Self {
        Self {
            position: vec3(0.0, 0.0, 0.0),
            rotation: Rad(0.0),
        }
    }
}

const fn matrix4_from_diagonal(diagonal: Vector3<f32>) -> Matrix4<f32> {
    Matrix4::from_cols(
        vec4(diagonal.x, 0.0, 0.0, 0.0),
        vec4(0.0, diagonal.y, 0.0, 0.0),
        vec4(0.0, 0.0, diagonal.z, 0.0),
        vec4(0.0, 0.0, 0.0, 1.0),
    )
}