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 approx_det::assert_relative_eq;
use wasm_bindgen_test::*;

use super::common::{get_input_wide, ConvexHullInput};
use crate::collision_tasks::tests::common::{
    convex4manifold_wide2convex4contact_manifold, Convex4ContactManifold, Mvec3, TestInput,
};
use crate::collision_tasks::ShapeWideTester;
use crate::shapes::{ConvexHullWide, InfinitePlaneWide};
use crate::traits::PairWideTest;
use crate::{ConvexHull, ConvexHullId, InfinitePlane, ShapeContainer};

wasm_bindgen_test_configure!(run_in_browser);
type InputTypeData = TestInput<ConvexHullInput, InfinitePlane>;
type InputType = TestInput<ConvexHullId, InfinitePlane>;

// the following tests use cube vertices as convex hull
#[test]
#[wasm_bindgen_test]
#[cfg(feature = "serde")]
fn test_convex_hull_infinite_plane_000() {
    let _ = env_logger::builder().is_test(true).try_init();

    let file_input =
        include_bytes!("resource/convex_hull_infinite_plane/000_no_rotate_4_contacts.json");
    let input0: InputTypeData =
        serde_json::from_slice(file_input).expect("file should be proper JSON");
    let convex_hull = ConvexHull::new_unchecked(&input0.a.vertices[..]);
    let mut container = ShapeContainer::default();
    let id = container.add(convex_hull);
    let input0 = InputType {
        a: id,
        b: input0.b,
        speculative_margin: input0.speculative_margin,
        offset_b: input0.offset_b,
        orientation_a: input0.orientation_a,
        orientation_b: input0.orientation_b,
    };
    let mut output0 = Convex4ContactManifold {
        OffsetA0: Mvec3 {
            x: -1.0,
            y: 0.0,
            z: -1.0,
        },
        OffsetA1: Mvec3 {
            x: 1.0,
            y: 0.0,
            z: -1.0,
        },
        OffsetA2: Mvec3 {
            x: 1.0,
            y: 0.0,
            z: 1.0,
        },
        OffsetA3: Mvec3 {
            x: -1.0,
            y: 0.0,
            z: 1.0,
        },
        Normal: Mvec3 {
            x: 0.0,
            y: 1.0,
            z: 0.0,
        },
        Depth0: 1.0,
        Depth1: 1.0,
        Depth2: 1.0,
        Depth3: 1.0,
        FeatureId0: 3,
        FeatureId1: 0,
        FeatureId2: 1,
        FeatureId3: 2,
        Contact0Exists: true,
        Contact1Exists: true,
        Contact2Exists: true,
        Contact3Exists: true,
    };
    output0.OffsetA0 -= output0.Normal * output0.Depth0;
    output0.OffsetA1 -= output0.Normal * output0.Depth1;
    output0.OffsetA2 -= output0.Normal * output0.Depth2;
    output0.OffsetA3 -= output0.Normal * output0.Depth3;
    let array = [input0];
    let outputs = [output0];
    let pair_count = array.len();
    TestWide!(
        ConvexHullWide,
        InfinitePlaneWide,
        array,
        pair_count,
        outputs,
        convex4manifold_wide2convex4contact_manifold,
        Some(&container)
    );
}

#[test]
#[wasm_bindgen_test]
#[cfg(feature = "serde")]
fn test_convex_hull_infinite_plane_001() {
    let _ = env_logger::builder().is_test(true).try_init();

    let file_input =
        include_bytes!("resource/convex_hull_infinite_plane/001_rotate_z_45_2_contacts.json");
    let input0: InputTypeData =
        serde_json::from_slice(file_input).expect("file should be proper JSON");
    let convex_hull = ConvexHull::new_unchecked(&input0.a.vertices[..]);
    let mut container = ShapeContainer::default();
    let id = container.add(convex_hull);
    let input0 = InputType {
        a: id,
        b: input0.b,
        speculative_margin: input0.speculative_margin,
        offset_b: input0.offset_b,
        orientation_a: input0.orientation_a,
        orientation_b: input0.orientation_b,
    };
    let mut output0 = Convex4ContactManifold {
        OffsetA0: Mvec3 {
            x: -0.0,
            y: -0.414,
            z: -1.0,
        },
        OffsetA1: Mvec3 {
            x: 0.0,
            y: -0.414,
            z: 1.0,
        },
        OffsetA2: Mvec3 {
            x: 0.0,
            y: 0.0,
            z: 0.0,
        },
        OffsetA3: Mvec3 {
            x: -0.0,
            y: 0.0,
            z: 0.0,
        },
        Normal: Mvec3 {
            x: 0.0,
            y: 1.0,
            z: 0.0,
        },
        Depth0: 1.0002,
        Depth1: 1.0002,
        Depth2: 0.0,
        Depth3: 0.0,
        FeatureId0: 1,
        FeatureId1: 0,
        FeatureId2: 0,
        FeatureId3: 0,
        Contact0Exists: true,
        Contact1Exists: true,
        Contact2Exists: false,
        Contact3Exists: false,
    };
    output0.OffsetA0 -= output0.Normal * output0.Depth0;
    output0.OffsetA1 -= output0.Normal * output0.Depth1;
    output0.OffsetA2 -= output0.Normal * output0.Depth2;
    output0.OffsetA3 -= output0.Normal * output0.Depth3;
    let array = [input0];
    let outputs = [output0];
    let pair_count = array.len();
    TestWide!(
        ConvexHullWide,
        InfinitePlaneWide,
        array,
        pair_count,
        outputs,
        convex4manifold_wide2convex4contact_manifold,
        Some(&container)
    );
}

#[test]
#[wasm_bindgen_test]
#[cfg(feature = "serde")]
fn test_convex_hull_infinite_plane_002() {
    let _ = env_logger::builder().is_test(true).try_init();

    let file_input =
        include_bytes!("resource/convex_hull_infinite_plane/002_rotate_x_45_z_45_1_contact.json");
    let input0: InputTypeData =
        serde_json::from_slice(file_input).expect("file should be proper JSON");
    let convex_hull = ConvexHull::new_unchecked(&input0.a.vertices[..]);
    let mut container = ShapeContainer::default();
    let id = container.add(convex_hull);
    let input0 = InputType {
        a: id,
        b: input0.b,
        speculative_margin: input0.speculative_margin,
        offset_b: input0.offset_b,
        orientation_a: input0.orientation_a,
        orientation_b: input0.orientation_b,
    };
    let mut output0 = Convex4ContactManifold {
        OffsetA0: Mvec3 {
            x: -0.0,
            y: -0.8,
            z: -0.29289,
        },
        OffsetA1: Mvec3 {
            x: 0.0,
            y: -0.0,
            z: 0.0,
        },
        OffsetA2: Mvec3 {
            x: 0.0,
            y: 0.0,
            z: 0.0,
        },
        OffsetA3: Mvec3 {
            x: -0.0,
            y: 0.0,
            z: 0.0,
        },
        Normal: Mvec3 {
            x: 0.0,
            y: 1.0,
            z: 0.0,
        },
        Depth0: 0.907106,
        Depth1: 0.0,
        Depth2: 0.0,
        Depth3: 0.0,
        FeatureId0: 0,
        FeatureId1: 0,
        FeatureId2: 0,
        FeatureId3: 0,
        Contact0Exists: true,
        Contact1Exists: false,
        Contact2Exists: false,
        Contact3Exists: false,
    };
    output0.OffsetA0 -= output0.Normal * output0.Depth0;
    output0.OffsetA1 -= output0.Normal * output0.Depth1;
    output0.OffsetA2 -= output0.Normal * output0.Depth2;
    output0.OffsetA3 -= output0.Normal * output0.Depth3;
    let array = [input0];
    let outputs = [output0];
    let pair_count = array.len();
    TestWide!(
        ConvexHullWide,
        InfinitePlaneWide,
        array,
        pair_count,
        outputs,
        convex4manifold_wide2convex4contact_manifold,
        Some(&container)
    );
}

#[test]
#[wasm_bindgen_test]
#[cfg(feature = "serde")]
fn test_convex_hull_infinite_plane_003() {
    let _ = env_logger::builder().is_test(true).try_init();

    let file_input = include_bytes!(
        "resource/convex_hull_infinite_plane/003_fully_penetrate_rotate_z_60_4_contacts.json"
    );
    let input0: InputTypeData =
        serde_json::from_slice(file_input).expect("file should be proper JSON");
    let convex_hull = ConvexHull::new_unchecked(&input0.a.vertices[..]);
    let mut container = ShapeContainer::default();
    let id = container.add(convex_hull);
    let input0 = InputType {
        a: id,
        b: input0.b,
        speculative_margin: input0.speculative_margin,
        offset_b: input0.offset_b,
        orientation_a: input0.orientation_a,
        orientation_b: input0.orientation_b,
    };
    let mut output0 = Convex4ContactManifold {
        OffsetA0: Mvec3 {
            x: -1.366025,
            y: 10.0,
            z: -1.0,
        },
        OffsetA1: Mvec3 {
            x: 0.3660254,
            y: 10.0,
            z: -1.0,
        },
        OffsetA2: Mvec3 {
            x: 0.3660254,
            y: 10.0,
            z: 1.0,
        },
        OffsetA3: Mvec3 {
            x: -1.366025,
            y: 10.0,
            z: 1.0,
        },
        Normal: Mvec3 {
            x: 0.0,
            y: 1.0,
            z: 0.0,
        },
        Depth0: 10.366025,
        Depth1: 11.366025,
        Depth2: 11.366025,
        Depth3: 10.366025,
        FeatureId0: 0,
        FeatureId1: 1,
        FeatureId2: 2,
        FeatureId3: 3,
        Contact0Exists: true,
        Contact1Exists: true,
        Contact2Exists: true,
        Contact3Exists: true,
    };
    output0.OffsetA0 -= output0.Normal * output0.Depth0;
    output0.OffsetA1 -= output0.Normal * output0.Depth1;
    output0.OffsetA2 -= output0.Normal * output0.Depth2;
    output0.OffsetA3 -= output0.Normal * output0.Depth3;
    let array = [input0];
    let outputs = [output0];
    let pair_count = array.len();
    TestWide!(
        ConvexHullWide,
        InfinitePlaneWide,
        array,
        pair_count,
        outputs,
        convex4manifold_wide2convex4contact_manifold,
        Some(&container)
    );
}

#[test]
#[wasm_bindgen_test]
#[cfg(feature = "serde")]
fn test_convex_hull_infinite_plane_004() {
    let _ = env_logger::builder().is_test(true).try_init();

    let file_input = include_bytes!("resource/convex_hull_infinite_plane/004_nocollide.json");
    let input0: InputTypeData =
        serde_json::from_slice(file_input).expect("file should be proper JSON");
    let convex_hull = ConvexHull::new_unchecked(&input0.a.vertices[..]);
    let mut container = ShapeContainer::default();
    let id = container.add(convex_hull);
    let input0 = InputType {
        a: id,
        b: input0.b,
        speculative_margin: input0.speculative_margin,
        offset_b: input0.offset_b,
        orientation_a: input0.orientation_a,
        orientation_b: input0.orientation_b,
    };
    let mut output0 = Convex4ContactManifold {
        OffsetA0: Mvec3 {
            x: 0.0,
            y: 0.0,
            z: 0.0,
        },
        OffsetA1: Mvec3 {
            x: 0.0,
            y: 0.0,
            z: 0.0,
        },
        OffsetA2: Mvec3 {
            x: 0.0,
            y: 0.0,
            z: 0.0,
        },
        OffsetA3: Mvec3 {
            x: 0.0,
            y: 0.0,
            z: 0.0,
        },
        Normal: Mvec3 {
            x: 0.0,
            y: 1.0,
            z: 0.0,
        },
        Depth0: 0.0,
        Depth1: 0.0,
        Depth2: 0.0,
        Depth3: 0.0,
        FeatureId0: 0,
        FeatureId1: 0,
        FeatureId2: 0,
        FeatureId3: 0,
        Contact0Exists: false,
        Contact1Exists: false,
        Contact2Exists: false,
        Contact3Exists: false,
    };
    output0.OffsetA0 -= output0.Normal * output0.Depth0;
    output0.OffsetA1 -= output0.Normal * output0.Depth1;
    output0.OffsetA2 -= output0.Normal * output0.Depth2;
    output0.OffsetA3 -= output0.Normal * output0.Depth3;
    let array = [input0];
    let outputs = [output0];
    let pair_count = array.len();
    TestWide!(
        ConvexHullWide,
        InfinitePlaneWide,
        array,
        pair_count,
        outputs,
        convex4manifold_wide2convex4contact_manifold,
        Some(&container)
    );
}

//qhull has no "shift to center" param
#[cfg(not(feature = "qhull"))]
mod shift_to_center {
    use glam::{UnitQuat, Vec3};
    use phys_geom::shape::InfinitePlane;

    use crate::collision_tasks::tests::common::compare_and_print_convex_contact_manifold;
    use crate::{ConvexContactManifold, ConvexHull, PairTest, ShapeContainer, ShapeTester};

    fn execute_displaced_convexhull_infinite_plane_test(
        convexhull_cuboid_length: Vec3,
        convexhull_displacement_vec: Vec3,
        shift_align_center: bool,
        offset_b: Vec3,
        orientation_a: UnitQuat,
        orientation_b: UnitQuat,
    ) -> ConvexContactManifold {
        let points = ConvexHull::generate_displacement_cuboid_vertex(
            convexhull_cuboid_length,
            convexhull_displacement_vec,
        );

        let convex_hull = if shift_align_center {
            ConvexHull::new_unchecked_and_shift(&points)
        } else {
            ConvexHull::new_unchecked(&points)
        };
        let mut container = ShapeContainer::default();
        let hull_id = container.add(convex_hull);

        ShapeTester::test(
            hull_id,
            InfinitePlane::default(),
            1e-5,
            offset_b,
            orientation_a,
            orientation_b,
            Some(&container),
        )
    }

    fn build_displaced_convexhull_infinite_plane_test(
        convexhull_cuboid_length: Vec3,
        convexhull_displacement_vec: Vec3,
        desired_offset_b: Vec3,
        rotation_a: UnitQuat,
        rotation_b: UnitQuat,
    ) {
        let _ = env_logger::builder().is_test(true).try_init();

        println!("convexhull_displacement_vec: {convexhull_displacement_vec}; rotation_a: {rotation_a}, rotation_b: {rotation_b}");

        let mut manifold_native_centering = execute_displaced_convexhull_infinite_plane_test(
            convexhull_cuboid_length,
            Vec3::ZERO,
            false,
            desired_offset_b,
            rotation_a,
            rotation_b,
        );
        // Matching offset_a to displacement manifold by adding convexhull_displacement_vec.
        let rotated_convexhull_displacement_vec = rotation_a.mul_vec3(convexhull_displacement_vec);
        for i in 0..manifold_native_centering.count {
            manifold_native_centering.contacts[i].offset_a += rotated_convexhull_displacement_vec;
        }

        let rotated_translate_vec = rotation_a * convexhull_displacement_vec;
        let adjusted_offset_b = desired_offset_b + rotated_translate_vec;

        let manifold_with_translated = execute_displaced_convexhull_infinite_plane_test(
            convexhull_cuboid_length,
            convexhull_displacement_vec,
            false,
            adjusted_offset_b,
            rotation_a,
            rotation_b,
        );
        let manifold_with_translated_and_shift_align_center =
            execute_displaced_convexhull_infinite_plane_test(
                convexhull_cuboid_length,
                convexhull_displacement_vec,
                true,
                adjusted_offset_b,
                rotation_a,
                rotation_b,
            );

        println!("shift_align_center OFF:");
        let err_no_align = compare_and_print_convex_contact_manifold(
            &manifold_native_centering,
            &manifold_with_translated,
        );
        println!("shift_align_center ON:");
        let err_with_align = compare_and_print_convex_contact_manifold(
            &manifold_native_centering,
            &manifold_with_translated_and_shift_align_center,
        );

        println!(
            "Max error when shift_align_center OFF: {err_no_align:.2}; Max error when shift_align_center ON: {err_with_align:.2}",
        );

        assert!(err_no_align >= err_with_align);
        assert_eq!(
            manifold_native_centering.count,
            manifold_with_translated_and_shift_align_center.count
        );
    }

    #[test]
    fn test_convex_infinite_plane_shift() {
        let convexhull_cuboid_length = Vec3::new(4., 5., 6.);
        let convexhull_displacement_vec = Vec3::new(10., 20., 30.);
        let desired_offset_b = Vec3::new(0., 2.4, 0.);
        let rotation_a = UnitQuat::from_euler_default(1., 2., 3.);
        let rotation_b = UnitQuat::from_euler_default(-1., -2., -3.);

        build_displaced_convexhull_infinite_plane_test(
            convexhull_cuboid_length,
            convexhull_displacement_vec,
            desired_offset_b,
            rotation_a,
            rotation_b,
        );
    }
}