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.

wasm_bindgen_test_configure!(run_in_browser);

use approx_det::assert_relative_eq;
use glam::{Isometry3, UnitVec3};
use glam_det::nums::{f32x4, Num};
use glam_det::{ExtratLaneExternal, Point3, Quat, UnitQuat, UnitQuatx4, Vec3, Vec3x4};
use serde::{Deserialize, Serialize};
use wasm_bindgen_test::*;

use crate::collision_tasks::tests::common::{
    convex4manifold_wide2convex4contact_manifold, get_input_wide, Convex4ContactManifold,
    ConvexHullInput, Mvec3, TestInput,
};
use crate::collision_tasks::ShapeWideTester;
use crate::convex_contact_manifold::Convex4ContactManifoldWide;
use crate::shapes::{ConvexHullWide, CuboidWide};
use crate::traits::{CreateShapeWide, PairWideTest};
use crate::{ConvexHull, ConvexHullId, Cuboid, ShapeContainer};

type InputTypeData = TestInput<Cuboid, ConvexHullInput>;
type InputType = TestInput<Cuboid, ConvexHullId>;

#[test]
#[wasm_bindgen_test]
fn test_cuboid_convex_hull_tester() {
    let _ = env_logger::builder().is_test(true).try_init();

    let file_input = include_bytes!("resource/cuboid_convex_hull/contact1.json");

    let input0: InputTypeData =
        serde_json::from_slice(file_input).expect("file should be proper JSON");
    let convex_hull = ConvexHull::new_unchecked(&input0.b.vertices[..]);
    let mut container = ShapeContainer::default();
    let id = container.add(convex_hull);

    let input0 = InputType {
        a: input0.a,
        b: id,
        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.5,
            y: -0.19,
            z: 0.5,
        },
        OffsetA1: Mvec3 {
            x: -0.5,
            y: -0.19,
            z: -0.5,
        },
        OffsetA2: Mvec3 {
            x: 0.5,
            y: -0.19,
            z: -0.5,
        },
        OffsetA3: Mvec3 {
            x: 0.5,
            y: -0.19,
            z: 0.5,
        },
        Normal: Mvec3 {
            x: 0.0,
            y: -1.0,
            z: 0.0,
        },
        Depth0: 0.69,
        Depth1: 0.69,
        Depth2: 0.69,
        Depth3: 0.69,
        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!(
        CuboidWide,
        ConvexHullWide,
        array,
        pair_count,
        outputs,
        convex4manifold_wide2convex4contact_manifold,
        Some(&container)
    );
}

#[test]
#[wasm_bindgen_test]
fn test_cuboid_convex_hull_tester2() {
    let _ = env_logger::builder().is_test(true).try_init();

    let file_input = include_bytes!("resource/cuboid_convex_hull/contact2.json");

    let input0: InputTypeData =
        serde_json::from_slice(file_input).expect("file should be proper JSON");
    let convex_hull = ConvexHull::new_unchecked(&input0.b.vertices[..]);
    let mut container = ShapeContainer::default();
    let id = container.add(convex_hull);

    let input0 = InputType {
        a: input0.a,
        b: id,
        speculative_margin: input0.speculative_margin,
        offset_b: input0.offset_b,
        orientation_a: input0.orientation_a,
        orientation_b: input0.orientation_b,
    };

    let output0 = Convex4ContactManifold {
        OffsetA0: Mvec3 {
            x: 0.5,
            y: -0.24000001,
            z: -0.5,
        },
        OffsetA1: Mvec3 {
            x: -0.5,
            y: -0.24000001,
            z: -0.5,
        },
        OffsetA2: Mvec3 {
            x: 0.5,
            y: 0.5,
            z: -0.5,
        },
        OffsetA3: Mvec3 {
            x: -0.5,
            y: 0.5,
            z: -0.5,
        },
        Normal: Mvec3 {
            x: 0.0,
            y: 0.0,
            z: 1.0,
        },
        Depth0: 0.7,
        Depth1: 0.7,
        Depth2: 0.7,
        Depth3: 0.7,
        FeatureId0: 263,
        FeatureId1: 262,
        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!(
        CuboidWide,
        ConvexHullWide,
        array,
        pair_count,
        outputs,
        convex4manifold_wide2convex4contact_manifold,
        Some(&container)
    );
}

#[test]
#[wasm_bindgen_test]
fn test_cuboid_convex_hull_tester3() {
    let _ = env_logger::builder().is_test(true).try_init();

    let file_input = include_bytes!("resource/cuboid_convex_hull/contact3.json");

    let input0: InputTypeData =
        serde_json::from_slice(file_input).expect("file should be proper JSON");
    let convex_hull = ConvexHull::new_unchecked(&input0.b.vertices[..]);
    let mut container = ShapeContainer::default();
    let id = container.add(convex_hull);

    let input0 = InputType {
        a: input0.a,
        b: id,
        speculative_margin: input0.speculative_margin,
        offset_b: input0.offset_b,
        orientation_a: input0.orientation_a,
        orientation_b: input0.orientation_b,
    };

    let output0 = Convex4ContactManifold {
        OffsetA0: Mvec3 {
            x: 0.5282747,
            y: 0.299999,
            z: 2.8276744,
        },
        OffsetA1: Mvec3 {
            x: 0.53216404,
            y: 0.299999,
            z: 2.8307602,
        },
        OffsetA2: Mvec3 {
            x: 0.5251112,
            y: 0.299999,
            z: 2.8407032,
        },
        OffsetA3: Mvec3 {
            x: 0.5212218,
            y: 0.299999,
            z: 2.8376174,
        },
        Normal: Mvec3 {
            x: -2.9802322e-8,
            y: -0.9999998,
            z: -1.8626451e-8,
        },
        Depth0: 0.015750963,
        Depth1: 0.01450725,
        Depth2: 0.017121887,
        Depth3: 0.0183656,
        FeatureId0: 5891,
        FeatureId1: 2569,
        FeatureId2: 1551,
        FeatureId3: 6932,
        Contact0Exists: true,
        Contact1Exists: true,
        Contact2Exists: true,
        Contact3Exists: true,
    };
    let array = [input0];
    let outputs = [output0];
    let pair_count = array.len();
    TestWide!(
        CuboidWide,
        ConvexHullWide,
        array,
        pair_count,
        outputs,
        convex4manifold_wide2convex4contact_manifold,
        Some(&container)
    );
}

#[test]
#[wasm_bindgen_test]
fn test_different_lane() {
    let _ = env_logger::builder().is_test(true).try_init();
    let points: Vec<Point3> = serde_json::from_slice(include_bytes!(
        "resource/convex_hull_create/convex_hull_diamond.json"
    ))
    .unwrap();
    let a = Cuboid::new(Vec3::new(200.0, 0.6, 200.0));
    let b = ConvexHull::new_unchecked(&points);
    let mut container = ShapeContainer::default();
    let b = container.add(b);
    let a_wide = <CuboidWide as CreateShapeWide<4>>::create([&a; 4].into_iter());
    let b_wide = <ConvexHullWide as CreateShapeWide<4>>::create([&b; 4].into_iter());
    let speculative_margin = f32x4::splat(0.01);
    let offset_b0 = Vec3::new(-0.9715807, 1.6972294, 4.6107597);
    let orientation_a0 = Quat::from_array([-0.0, -0.0, -0.0, 1.0]).as_unit_quat_unchecked();
    let orientation_b0 = Quat::from_array([-0.95281816, 0.30315584, 0.014564567, -0.0046799295])
        .as_unit_quat_unchecked();

    let i = 3;
    let mut offset_b = [Vec3::ZERO, Vec3::ZERO, Vec3::ZERO, Vec3::ZERO];
    offset_b[i] = offset_b0;
    let mut orientation_a = [
        UnitQuat::IDENTITY,
        UnitQuat::IDENTITY,
        UnitQuat::IDENTITY,
        UnitQuat::IDENTITY,
    ];
    orientation_a[i] = orientation_a0;
    let mut orientation_b = [
        UnitQuat::IDENTITY,
        UnitQuat::IDENTITY,
        UnitQuat::IDENTITY,
        UnitQuat::IDENTITY,
    ];
    orientation_b[i] = orientation_b0;
    let manifold = collision_test(
        &mut container,
        &a_wide,
        &b_wide,
        speculative_margin,
        offset_b,
        orientation_a,
        orientation_b,
    );

    check(&manifold, i);
}

fn check(manifold: &Convex4ContactManifoldWide, i: usize) {
    assert_relative_eq!(
        manifold.normal.extract_lane(i),
        Vec3::new(3.3320475e-8, -0.9999998, 1.4959369e-8).as_unit_vec3_unchecked()
    );
    let depth = manifold.depth.extract_lane_ext(i);
    assert_relative_eq!(depth[0], -0.0015472593);
    assert_relative_eq!(depth[1], -0.0072477376);
    assert_relative_eq!(depth[2], 0.0f32);
    assert_relative_eq!(depth[3], 0.0f32);
}

fn collision_test(
    container: &mut ShapeContainer,
    a_wide: &CuboidWide,
    b_wide: &ConvexHullWide,
    speculative_margin: f32x4,
    offset_b: [Vec3; 4],
    orientation_a: [UnitQuat; 4],
    orientation_b: [UnitQuat; 4],
) -> Convex4ContactManifoldWide {
    let offset_b = Vec3x4::compose_soa(&offset_b);
    let orientation_a = UnitQuatx4::compose_soa(&orientation_a);
    let orientation_b = UnitQuatx4::compose_soa(&orientation_b);
    let mut manifold = Convex4ContactManifoldWide::default();
    let contact_context = crate::traits::ContactContext {
        offset_b: &offset_b,
        orientation_a: &orientation_a,
        orientation_b: &orientation_b,
        speculative_margin,
        complex_shape_container: Some(container),
        pair_count: 4,
    };

    ShapeWideTester::test(a_wide, b_wide, &contact_context, &mut manifold);
    manifold
}

#[derive(Serialize, Deserialize)]
struct QuatData {
    x: f32,
    y: f32,
    z: f32,
    w: f32,
}

#[derive(Serialize, Deserialize)]
struct CuboidData {
    half_length: Vec3,
    position: Vec3,
    orientation: QuatData,
}

#[derive(Serialize, Deserialize)]
struct ConvexHullData {
    vertices: Vec<Vec3>,
    position: Vec3,
    orientation: QuatData,
    #[serde(default)]
    triangles: Vec<[Vec3; 3]>,
}

#[derive(Serialize, Deserialize)]
struct CollisionData {
    cuboid: CuboidData,
    convex_hull: ConvexHullData,
    relative_position: Vec3,
    manifold_depths: [f32; 4],
    max_depth: f32,
    slot_index: usize,
    speculative_margin: f32,
}
#[test]
#[wasm_bindgen_test]
// small mesh and thin cuboid problem will be solved in other issue
// remove ignore in issue #2171
#[ignore]
fn test_small_mesh() {
    let _ = env_logger::builder().is_test(true).try_init();

    let json_content = include_bytes!("resource/cuboid_convex_hull/small_mesh.json");

    let collision_data: CollisionData =
        serde_json::from_slice(json_content).expect("Failed to parse JSON");
    let cuboid = Cuboid::new(collision_data.cuboid.half_length * 2.0);

    let hull_points: Vec<Point3> = collision_data
        .convex_hull
        .vertices
        .iter()
        .map(|&vertex| Point3::from_vec3(vertex))
        .collect();
    let convex_hull = ConvexHull::new_unchecked(&hull_points);

    let cuboid_transform = Isometry3::from_rotation_translation(
        UnitQuat::from_xyzw_unchecked(
            collision_data.cuboid.orientation.x,
            collision_data.cuboid.orientation.y,
            collision_data.cuboid.orientation.z,
            collision_data.cuboid.orientation.w,
        ),
        collision_data.cuboid.position,
    );

    let convex_hull_transform = Isometry3::from_rotation_translation(
        UnitQuat::from_xyzw_unchecked(
            collision_data.convex_hull.orientation.x,
            collision_data.convex_hull.orientation.y,
            collision_data.convex_hull.orientation.z,
            collision_data.convex_hull.orientation.w,
        ),
        collision_data.convex_hull.position * 10.0,
    );
    let mut container = ShapeContainer::default();
    let convex_hull_id = container.add(convex_hull);

    let a_wide = <CuboidWide as CreateShapeWide<1>>::create([&cuboid].into_iter());
    let b_wide = <ConvexHullWide as CreateShapeWide<1>>::create([&convex_hull_id].into_iter());

    let speculative_margin = f32x4::splat(collision_data.speculative_margin);
    let offset_b =
        Vec3x4::splat_soa(convex_hull_transform.translation - cuboid_transform.translation);
    let orientation_a = UnitQuatx4::splat_soa(cuboid_transform.rotation);
    let orientation_b = UnitQuatx4::splat_soa(convex_hull_transform.rotation);

    let mut manifold = Convex4ContactManifoldWide::default();
    let contact_context = crate::traits::ContactContext {
        offset_b: &offset_b,
        orientation_a: &orientation_a,
        orientation_b: &orientation_b,
        speculative_margin,
        complex_shape_container: Some(&container),
        pair_count: 1,
    };

    ShapeWideTester::test(&a_wide, &b_wide, &contact_context, &mut manifold);

    let max_depth = manifold
        .depth
        .iter()
        .map(|&depth| depth.extract_lane_ext(0).abs())
        .fold(0.0f32, f32::max);
    let normal = manifold.normal.first_lane();
    assert_relative_eq!(max_depth, 0.014562669);
    assert_relative_eq!(
        normal,
        UnitVec3::from_array_unchecked([-0.0058674314, -0.05829362, -0.9982822])
    );
}

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

    use crate::collision_tasks::tests::common::{
        build_displaced_convexhull_test, CollisionTestShape,
    };

    #[test]
    fn test_cuboid_convex_shift() {
        let shape_a = Cuboid::new_xyz(1., 2., 3.);
        let convexhull_cuboid_length = Vec3::new(4., 5., 6.);
        let convexhull_displacement_vec = Vec3::new(1e0, 1e4, 1e2);
        let desired_offset_b = Vec3::new(0., 4.1, 0.);
        let rotation_a = UnitQuat::from_euler_default(-1., -2., -3.);
        let rotation_b = UnitQuat::from_euler_default(1., 2., 3.);

        build_displaced_convexhull_test(
            &CollisionTestShape::Cuboid(shape_a),
            convexhull_cuboid_length,
            convexhull_displacement_vec,
            desired_offset_b,
            rotation_a,
            rotation_b,
        );
    }
}