phys-collision 2.0.1-beta.0

// Copyright (C) 2020-2025 phys-collision authors. All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

use std::cmp::Ordering;

use glam_det::nums::{bool32x4, Num, Signed};
use glam_det::{Dot, Isometry3, Point3, Vec3};
use smallvec::SmallVec;

use crate::convex_contact_manifold::{
    Convex4ContactManifoldWide, Face, ManifoldCandidate, ManifoldCandidateWide,
};
use crate::shapes::GENERAL_FACE_VERTICES_RESTRICTIONS;

pub type ManifoldCandidateScalarSmallVec =
    SmallVec<[ManifoldCandidate; GENERAL_FACE_VERTICES_RESTRICTIONS * 2]>;

pub struct CandidateScalarsReducer<'a> {
    candidates: &'a mut ManifoldCandidateScalarSmallVec,
    manifold_wide: &'a mut Convex4ContactManifoldWide,
    slot_index: usize,
    transform_b: Isometry3,
}

impl<'a> CandidateScalarsReducer<'a> {
    /// Creates a new `CandidateScalarsReducer`.
    /// # Parameters
    /// - `candidates`: The candidates to reduce.
    /// - `manifold_wide`: The manifold to write the reduced candidates to.
    /// - `slot_index`: The slot index to write the reduced candidates to.
    /// - `transform_b`: The transform of the second shape.
    /// # Returns
    /// The created `CandidateScalarsReducer`.
    /// # Panics
    /// Panics if `candidates` is empty.
    pub fn new(
        candidates: &'a mut ManifoldCandidateScalarSmallVec,
        manifold_wide: &'a mut Convex4ContactManifoldWide,
        slot_index: usize,
        transform_b: Isometry3,
    ) -> Self {
        assert!(!candidates.is_empty());
        Self {
            candidates,
            manifold_wide,
            slot_index,
            transform_b,
        }
    }

    pub fn reduce(
        &mut self,
        dot_axis: Vec3,
        face_center_a: Point3,
        face_b: &Face,
        epsilon_scale: f32,
        min_depth: f32,
    ) {
        assert!(!self.candidates.is_empty());
        let face_center_a_to_face_center_b = face_b.origin - face_center_a;
        let base_dot = dot_axis.dot(face_center_a_to_face_center_b);
        let x_dot = dot_axis.dot(face_b.tangent_x);
        let y_dot = dot_axis.dot(face_b.tangent_y);
        self.candidates.retain(
            #[inline]
            |candidate| {
                let depth = base_dot + x_dot * candidate.x + y_dot * candidate.y;
                if depth <= min_depth {
                    false
                } else {
                    candidate.depth = depth;
                    true
                }
            },
        );
        if self.candidates.len() <= 4 {
            for (i, candidate) in self.candidates.iter().enumerate() {
                self.manifold_wide.write_slot_with_candidate_scalar(
                    *candidate,
                    self.slot_index,
                    i,
                    face_b,
                    self.transform_b,
                );
            }
            return;
        }
        let extremity_scale = epsilon_scale * 1e-2f32;
        let extremity_x = extremity_scale * 0.6_f32;
        let extremity_y = extremity_scale * 0.51_f32;

        let valid_iter = self.candidates.iter().enumerate();

        let (best_index0, &candidate0, _) = valid_iter
            .map(
                #[inline]
                |(i, candidate)| {
                    let extremity = candidate.x * extremity_x + candidate.y * extremity_y;
                    let mut score = candidate.depth;
                    if candidate.depth > 0_f32 {
                        score += extremity.absf();
                    }
                    (i, candidate, score)
                },
            )
            .max_by(|(_, _, left), (_, _, right)| left.partial_cmp(right).unwrap())
            .unwrap();
        self.manifold_wide.write_slot_with_candidate_scalar(
            candidate0,
            self.slot_index,
            0,
            face_b,
            self.transform_b,
        );
        remove_candidate_at(self.candidates, best_index0);
        let valid_iter = self.candidates.iter().enumerate();
        let (best_index1, &candidate1, max_distance_squared) = valid_iter
            .map(
                #[inline]
                |(i, candidate)| {
                    let dx = candidate.x - candidate0.x;
                    let dy = candidate.y - candidate0.y;
                    let distance_squared = dx * dx + dy * dy;
                    (i, candidate, distance_squared)
                },
            )
            .max_by(|(_, _, left), (_, _, right)| {
                if left > right {
                    Ordering::Greater
                } else {
                    Ordering::Less
                }
            })
            .unwrap();

        if max_distance_squared < 1e-6f32 * epsilon_scale * epsilon_scale {
            return;
        }
        self.manifold_wide.write_slot_with_candidate_scalar(
            candidate1,
            self.slot_index,
            1,
            face_b,
            self.transform_b,
        );
        remove_candidate_at(self.candidates, best_index1);

        let edge_offset_x = candidate1.x - candidate0.x;
        let edge_offset_y = candidate1.y - candidate0.y;
        let mut min_signed_area = 0f32;
        let mut max_signed_area = 0f32;
        let mut best_index2 = 0usize;
        let mut best_index3 = 0usize;

        for i in 0..self.candidates.len() {
            let candidate = self.candidates[i];
            let offset_x = candidate.x - candidate0.x;
            let offset_y = candidate.y - candidate0.y;
            let mut signed_area = offset_x * edge_offset_y - offset_y * edge_offset_x;
            if candidate.depth < 0f32 {
                signed_area *= 0.25f32;
            }
            if signed_area < min_signed_area {
                min_signed_area = signed_area;
                best_index2 = i;
            }
            if signed_area > max_signed_area {
                max_signed_area = signed_area;
                best_index3 = i;
            }
        }

        let area_epsilon = 1e-6f32 * max_distance_squared * max_distance_squared;
        if min_signed_area * min_signed_area > area_epsilon {
            let candidate2 = *self.candidates.get(best_index2).unwrap();
            self.manifold_wide.write_slot_with_candidate_scalar(
                candidate2,
                self.slot_index,
                2,
                face_b,
                self.transform_b,
            );
        }
        if max_signed_area * max_signed_area > area_epsilon {
            let candidate3 = *self.candidates.get(best_index3).unwrap();
            self.manifold_wide.write_slot_with_candidate_scalar(
                candidate3,
                self.slot_index,
                3,
                face_b,
                self.transform_b,
            );
        }
    }
}

fn remove_candidate_at(candidates: &mut ManifoldCandidateScalarSmallVec, index: usize) {
    let last_index = candidates.len() - 1;
    if index < last_index {
        candidates[index] = candidates[last_index];
    }
    candidates.pop();
}
#[inline]
pub fn conditional_select(
    cond: bool32x4,
    left: &ManifoldCandidateWide,
    right: &ManifoldCandidateWide,
) -> ManifoldCandidateWide {
    ManifoldCandidateWide {
        x: left.x.select(cond, right.x),
        y: left.y.select(cond, right.y),
        depth: left.depth.select(cond, right.depth),
        feature_id: left.feature_id.select(cond, right.feature_id),
    }
}