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 glam_det::nums::{f32x4, u32x4, Bool, Num, PartialOrdEx, Signed};
use glam_det::{
    Cross, Dot, Isometry3, Mat3x4, Point3, Point3x4, UnitQuat, UnitQuatx4, Vec3, Vec3x4,
};
use smallvec::SmallVec;

use crate::collision_tasks::common::{NormalizeExt, EPS_5, EPS_8};
use crate::collision_tasks::manifold_candidate_helper::{
    CandidateScalarsReducer, ManifoldCandidateScalarSmallVec,
};
use crate::collision_tasks::traits::{
    Transform, TransformWide, Transformative, TransformativeWide,
};
use crate::collision_tasks::{tootbird, ShapeWideTester};
use crate::convex_contact_manifold::{Convex4ContactManifoldWide, Face, ManifoldCandidate};
use crate::shapes::{ConvexHullWide, GENERAL_FACE_VERTICES_RESTRICTIONS};
use crate::traits::{
    ContactContext, ContactManifoldWide, CreateShapeWide, OrientationWide, PairWideTest,
};
use crate::{
    ConvexContactManifold, ConvexHull, ConvexHullId, PairTest, ShapeContainer, ShapeTester,
};

impl PairWideTest<ConvexHullWide, ConvexHullWide> for ShapeWideTester {
    #[inline]
    fn should_reset_manifold_before_test() -> bool {
        true
    }

    // 4 manifold in Convex4ContactManifoldWide
    fn test(
        a: &ConvexHullWide,
        b: &ConvexHullWide,
        contact_context: &ContactContext,
        manifold_wide: &mut Convex4ContactManifoldWide,
    ) {
        const MAXIMUM_ITERRATIONS: usize = 25;
        let container = contact_context
            .complex_shape_container
            .expect("ShapeContainer is required for ConvexHullWide");

        let rotation_a = Mat3x4::from_quat(*contact_context.orientation_a);
        let rotation_b = Mat3x4::from_quat(*contact_context.orientation_b);
        let a2b_rotation = rotation_b.transpose() * rotation_a;
        let shift_a = a.get_convexhull_shift_wide(container, contact_context.pair_count);
        let shift_b = b.get_convexhull_shift_wide(container, contact_context.pair_count);
        let rotated_shift_b = rotation_b.mul_vec3(shift_b);
        let shift_a_in_b_local_space = a2b_rotation.mul_vec3(shift_a);
        let shift_gap_in_b_local_space = shift_a_in_b_local_space - shift_b;

        let center_a = a.get_center(container, contact_context.pair_count);
        let center_b = b.get_center(container, contact_context.pair_count);
        let center_a_in_b_local_space = a2b_rotation.mul_vec3(center_a);
        let center_gap_in_b_local_space = center_a_in_b_local_space - center_b;
        //let transform_b identity
        //let a_to_b_transforms = a_to_b_transform.split_soa();
        let speculative_margins: [f32; 4] = contact_context.speculative_margin.into();

        let local_offset_a = -rotation_b.transpose().mul_vec3(*contact_context.offset_b)
            + shift_gap_in_b_local_space;
        let normal = (local_offset_a - center_gap_in_b_local_space).normalize_or(Vec3x4::Y, EPS_8);
        let mut inactive_lanes =
            u32x4::splat(contact_context.pair_count as u32).le(u32x4::from([0, 1, 2, 3]));
        let hull_epsilon_scale = b.estimate_epsilon_scale(inactive_lanes, container);
        let hull_epsilon_scale = a
            .estimate_epsilon_scale(inactive_lanes, container)
            .min(hull_epsilon_scale);
        let minimum_accepted_depth = -contact_context.speculative_margin;

        let a2b_transform = TransformWide::new(local_offset_a, &a2b_rotation);
        let toot_bird_result = tootbird::find_minimum_depth(
            b,
            a,
            &a2b_transform,
            normal,
            &tootbird::IterContext::new(
                inactive_lanes,
                hull_epsilon_scale * EPS_5,
                minimum_accepted_depth,
                MAXIMUM_ITERRATIONS,
                contact_context.complex_shape_container,
                false,
            ),
        );
        let toot_bird_result_normal = toot_bird_result.normal;
        inactive_lanes = inactive_lanes | toot_bird_result.depth.le(minimum_accepted_depth);
        if inactive_lanes.all() {
            return;
        }
        let inactive: [bool; 4] = inactive_lanes.into();
        let contact_normals_in_b = toot_bird_result.normal.split_soa();
        let contact_normal_in_a = a2b_transform
            .orientation
            .inverse_mul_vec3(toot_bird_result.normal.as_vec3x4())
            .as_unit_vec3x4_unchecked();
        let contact_normals_in_a = contact_normal_in_a.split_soa();
        //let negatedLocalNormalInA = -contact_normal_in_a;
        let negated_offset_to_closest_on_a = toot_bird_result.normal * toot_bird_result.depth;
        let closest_on_a = toot_bird_result.closest_point_on_a - negated_offset_to_closest_on_a;
        let a_to_closest_on_a = closest_on_a - local_offset_a;
        let closest_on_ain_a = a2b_transform
            .orientation()
            .inverse_mul_vec3(a_to_closest_on_a);
        let closest_a_in_a_local_space = Point3x4::from_vec3x4(closest_on_ain_a).split_soa();
        let closest_a_in_b = Point3x4::from_vec3x4(toot_bird_result.closest_point_on_a).split_soa();

        for (i, (&shape_a, &shape_b)) in a
            .iter_take(contact_context.pair_count)
            .zip(b.iter_take(contact_context.pair_count))
            .enumerate()
        {
            if inactive[i] {
                continue;
            }
            let contact_normals_in_b = contact_normals_in_b[i];
            let closest_a_in_b = closest_a_in_b[i];
            let speculative_margins = speculative_margins[i];
            let a2b_rotation = a2b_rotation.extract_lane(i);
            let neg_local_normal_a = -contact_normals_in_a[i];
            let closest_a_in_a_local_space = closest_a_in_a_local_space[i];
            let a2b_transform = Transform::new(local_offset_a.extract_lane(i), &a2b_rotation);
            let shape_a = container
                .get::<ConvexHull>(shape_a)
                .expect("invalid shape id");
            let shape_b = container
                .get::<ConvexHull>(shape_b)
                .expect("invalid shape id");

            // the focus thing here is use every edge from face b to clip face a,and then check
            // every vertex from face a to clip face b like cuboid pair

            let epsilon_scale = shape_a
                .cal_estimate_epsilon_scale()
                .min(shape_b.cal_estimate_epsilon_scale());
            let bounding_plane_eps = epsilon_scale * 1e-5;
            let face_a = shape_a.pick_representative_face(
                neg_local_normal_a,
                closest_a_in_a_local_space,
                bounding_plane_eps,
            );
            let face_b = shape_b.pick_representative_face(
                contact_normals_in_b,
                closest_a_in_b,
                bounding_plane_eps,
            );
            let (face_b_tangent_x, face_b_tangent_y) = face_b.normal.any_orthogonal_pair();
            let face_a_vertex_indices = shape_a.get_vertex_indices(face_a.face_index);
            let face_b_vertex_indices = shape_b.get_vertex_indices(face_b.face_index);
            let mut cached_edges =
                SmallVec::<[CachedEdge; GENERAL_FACE_VERTICES_RESTRICTIONS]>::with_capacity(
                    face_a_vertex_indices.len(),
                );
            let pre_index_a = face_a_vertex_indices.last().expect("empty face");
            let mut pre_vertex_a = shape_a.get_point(*pre_index_a);
            pre_vertex_a = a2b_transform.transform_point(pre_vertex_a);
            for bundle_index in face_a_vertex_indices {
                let vertex_a = shape_a.get_point(*bundle_index);
                let vertex_a_2 = a2b_transform.transform_point(vertex_a);
                let edge_vec = vertex_a_2 - pre_vertex_a;
                let edge_plane_normal = contact_normals_in_b.as_vec3().cross(edge_vec);
                cached_edges.push(CachedEdge {
                    vertex: vertex_a_2,
                    edge_plane_normal,
                    max_dot: f32::MIN,
                });
                pre_vertex_a = vertex_a_2;
            }

            let vertices_len_a = face_a_vertex_indices.len();
            let vertices_len_b = face_b_vertex_indices.len();
            let max_candidate_count = *[vertices_len_a * 2, vertices_len_b * 2]
                .iter()
                .min()
                .max([vertices_len_a, vertices_len_b].iter().max())
                .expect("array is empty");

            let mut pre_index_b = *face_b_vertex_indices.last().expect("empty face");
            let face_b_origin = shape_b.get_point(pre_index_b);
            let mut pre_vertex_b = face_b_origin;
            let mut candidates =
                ManifoldCandidateScalarSmallVec::with_capacity(max_candidate_count);
            for bundle_index in face_b_vertex_indices {
                let vertex_b = shape_b.get_point(*bundle_index);
                let edge_offset_b = vertex_b - pre_vertex_b;
                let edge_plane_normal_b = edge_offset_b.cross(contact_normals_in_b.as_vec3());
                let mut latest_entry = f32::MIN;
                let mut earliest_exit = f32::MAX;
                for edge_a in cached_edges.iter_mut().take(face_a_vertex_indices.len()) {
                    let edge_b_to_edge_a = edge_a.vertex - pre_vertex_b;
                    let containment_dot = edge_b_to_edge_a.dot(edge_plane_normal_b);
                    if edge_a.max_dot < containment_dot {
                        edge_a.max_dot = containment_dot;
                    }
                    let numerator = edge_b_to_edge_a.dot(edge_a.edge_plane_normal);
                    let denominator = edge_a.edge_plane_normal.dot(edge_offset_b);
                    if denominator < 0f32 && numerator < latest_entry * denominator {
                        latest_entry = numerator / denominator;
                    } else if denominator > 0f32 && numerator < earliest_exit * denominator {
                        earliest_exit = numerator / denominator;
                    }
                }
                let entry_exit_epsilon = epsilon_scale * 2e-2;
                if latest_entry > 1.0f32 && (latest_entry - 1.0f32) < entry_exit_epsilon {
                    latest_entry = 1.0f32;
                }
                if earliest_exit < 0f32 && -earliest_exit < entry_exit_epsilon {
                    earliest_exit = 0f32;
                }

                latest_entry = latest_entry.max(0f32);
                earliest_exit = earliest_exit.min(1f32);

                let start_id = pre_index_b.index();
                let end_id = bundle_index.index();
                let base_feature_id = (start_id ^ end_id) << 8;
                if earliest_exit >= latest_entry && candidates.len() < max_candidate_count {
                    // create max contact
                    let point = edge_offset_b * earliest_exit + pre_vertex_b - face_b_origin;
                    candidates.push(ManifoldCandidate::new(
                        point.dot(face_b_tangent_x),
                        point.dot(face_b_tangent_y),
                        base_feature_id + end_id,
                    ));
                }
                if latest_entry < (earliest_exit + entry_exit_epsilon)
                    && latest_entry > 0f32
                    && candidates.len() < max_candidate_count
                {
                    //create min contact
                    let point = edge_offset_b * latest_entry + pre_vertex_b - face_b_origin;
                    candidates.push(ManifoldCandidate::new(
                        point.dot(face_b_tangent_x),
                        point.dot(face_b_tangent_y),
                        base_feature_id + end_id,
                    ));
                }

                pre_index_b = *bundle_index;
                pre_vertex_b = vertex_b;
            }

            let local_normal_a_dot_face_normal_b =
                contact_normals_in_b.as_vec3().dot(face_b.normal.as_vec3());
            assert!(
                local_normal_a_dot_face_normal_b.absf() > f32::EPSILON,
                "mesh data is invalid"
            );
            let inverse_local_normal_a_dot_face_normal_b = local_normal_a_dot_face_normal_b.recip();
            for (i, edge) in cached_edges.iter_mut().enumerate() {
                if candidates.len() >= max_candidate_count {
                    break;
                }
                if edge.max_dot <= 3e-6 {
                    let b_face_to_vertex_a = edge.vertex - face_b_origin;
                    let distance = b_face_to_vertex_a.dot(face_b.normal.as_vec3())
                        * inverse_local_normal_a_dot_face_normal_b;
                    let b_face_to_projected_vertex_a =
                        b_face_to_vertex_a - contact_normals_in_b.as_vec3() * distance;
                    candidates.push(ManifoldCandidate::new(
                        face_b_tangent_x.dot(b_face_to_projected_vertex_a),
                        face_b_tangent_y.dot(b_face_to_projected_vertex_a),
                        i as u32,
                    ));
                }
            }

            if !candidates.is_empty() {
                let face_a_normal_in_b_space = a2b_transform
                    .orientation()
                    .mul_vec3(face_a.normal.as_vec3());
                let _len_test = face_a_normal_in_b_space.length();
                let face_normal_a_dot_local_normal =
                    face_a_normal_in_b_space.dot(contact_normals_in_b.as_vec3());
                assert!(
                    face_normal_a_dot_local_normal.absf() > f32::EPSILON,
                    "face normal and contact normal should not ertical"
                );
                let inverse_face_normal_a_dot_local_normal = face_normal_a_dot_local_normal.recip();
                let dot_axis = face_a_normal_in_b_space * inverse_face_normal_a_dot_local_normal;
                let face_b = Face {
                    origin: face_b_origin,
                    tangent_x: face_b_tangent_x,
                    tangent_y: face_b_tangent_y,
                };
                let transform_b = Isometry3::from_rotation_translation(
                    contact_context.orientation_b.extract_lane(i),
                    contact_context.offset_b.extract_lane(i) + rotated_shift_b.extract_lane(i),
                );
                let mut reducer =
                    CandidateScalarsReducer::new(&mut candidates, manifold_wide, i, transform_b);
                let face_a_point = cached_edges[0].vertex;
                reducer.reduce(
                    dot_axis,
                    face_a_point,
                    &face_b,
                    epsilon_scale,
                    -speculative_margins,
                );
                candidates.clear();
            }
        }
        manifold_wide.normal = rotation_b
            .mul_vec3(toot_bird_result_normal.as_vec3x4())
            .as_unit_vec3x4_unchecked();
        for (offset_a, depth) in manifold_wide
            .offset_a
            .iter_mut()
            .zip(manifold_wide.depth.iter())
        {
            *offset_a -= manifold_wide.normal * depth;
        }
    }
}
impl_pair_narrowphase!(
    ConvexHullId,
    ConvexHullId,
    ConvexHullWide,
    ConvexHullWide,
    4
);

struct CachedEdge {
    pub vertex: Point3,
    pub edge_plane_normal: Vec3,
    pub max_dot: f32,
}

#[cfg(test)]
mod tests {
    use approx_det::assert_relative_eq;
    use glam_det::nums::{f32x4, Num};
    use glam_det::{Isometry3, UnitQuatx4, Vec3, Vec3x4};
    #[cfg(feature = "serde")]
    use glam_det::{Point3, Quat, UnitQuat};
    use wasm_bindgen_test::wasm_bindgen_test;

    use crate::collision_tasks::ShapeWideTester;
    use crate::convex_contact_manifold::Convex4ContactManifoldWide;
    use crate::shapes::{ConvexHullWide, CuboidExt};
    use crate::traits::{ContactContext, CreateShapeWide, PairWideTest};
    use crate::{ConvexHull, Cuboid, ShapeContainer};

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

        let a = Cuboid::new(Vec3::splat(1.0));
        let b = Cuboid::new(Vec3::splat(2.0));
        let points_a = (0..8).map(|i| a.get_vertex(i)).collect::<Vec<_>>();
        let points_b = (0..8).map(|i| b.get_vertex(i)).collect::<Vec<_>>();
        let a_convexhull = ConvexHull::new_unchecked(&points_a);
        let b_convexhull = ConvexHull::new_unchecked(&points_b);
        let mut container = ShapeContainer::default();
        let a = container.add(a_convexhull);
        let b = container.add(b_convexhull);
        let transform_a = Isometry3::IDENTITY;
        let transform_b = Isometry3::from_translation(Vec3::new(0.0, 0.0, 1.4));
        let a_wide = <ConvexHullWide as CreateShapeWide<1>>::create([&a].into_iter());
        let b_wide = <ConvexHullWide as CreateShapeWide<1>>::create([&b].into_iter());
        let speculative_margin = f32x4::splat(0.01);
        let offset_b = Vec3x4::splat_soa(transform_b.translation - transform_a.translation);
        let orientation_a = UnitQuatx4::splat_soa(transform_a.rotation);
        let orientation_b = UnitQuatx4::splat_soa(transform_b.rotation);
        let mut manifold = Convex4ContactManifoldWide::default();
        let contact_context = ContactContext {
            orientation_a: &orientation_a,
            orientation_b: &orientation_b,
            offset_b: &offset_b,
            speculative_margin,
            complex_shape_container: Some(&container),
            pair_count: 1,
        };

        ShapeWideTester::test(&a_wide, &b_wide, &contact_context, &mut manifold);
        assert_relative_eq!(manifold.offset_a[0].extract_lane(0).z, 0.5f32);
        assert_relative_eq!(manifold.offset_a[1].extract_lane(0).z, 0.5f32);
        assert_relative_eq!(manifold.offset_a[2].extract_lane(0).z, 0.5f32);
        assert_relative_eq!(manifold.offset_a[3].extract_lane(0).z, 0.5f32);
    }
    //qhull has no "shift to center" param
    #[cfg(not(feature = "qhull"))]
    mod no_qhull {
        use approx_det::assert_relative_eq;
        use glam::nums::{f32x4, Num};
        use glam::{Isometry3, UnitQuatx4, Vec3, Vec3x4};
        use wasm_bindgen_test::wasm_bindgen_test;

        use crate::shapes::BundleIndex;
        use crate::traits::{ContactContext, PairWideTest};
        use crate::{
            Convex4ContactManifoldWide, ConvexHull, ConvexHullWide, CreateShapeWide,
            ShapeContainer, ShapeWideTester,
        };

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

            let shift = Vec3::new(0., 0., 10.);
            let points_a_raw =
                ConvexHull::generate_displacement_cuboid_vertex(Vec3::splat(1.), Vec3::ZERO);
            let points_a = ConvexHull::generate_displacement_cuboid_vertex(Vec3::splat(1.), shift);
            let points_b = ConvexHull::generate_displacement_cuboid_vertex(Vec3::splat(2.), shift);

            let a = ConvexHull::new_unchecked_and_shift(&points_a);
            // make sure convexhull has been shifted.
            for (i, raw_point) in points_a_raw.iter().enumerate() {
                assert!(a
                    .get_point_raw(BundleIndex::new(i.try_into().unwrap()))
                    .abs_diff_eq(*raw_point, 1e-5));
            }

            let b = ConvexHull::new_unchecked_and_shift(&points_b);
            let mut container = ShapeContainer::default();
            let a = container.add(a);
            let b = container.add(b);
            let transform_a = Isometry3::IDENTITY;
            let transform_b = Isometry3::from_translation(Vec3::new(0.0, 0.0, 1.4));
            let a_wide = <ConvexHullWide as CreateShapeWide<1>>::create([&a].into_iter());
            let b_wide = <ConvexHullWide as CreateShapeWide<1>>::create([&b].into_iter());
            let speculative_margin = f32x4::splat(0.01);
            let offset_b = Vec3x4::splat_soa(transform_b.translation - transform_a.translation);
            let orientation_a = UnitQuatx4::splat_soa(transform_a.rotation);
            let orientation_b = UnitQuatx4::splat_soa(transform_b.rotation);
            let mut manifold = Convex4ContactManifoldWide::default();
            let contact_context = ContactContext {
                orientation_a: &orientation_a,
                orientation_b: &orientation_b,
                offset_b: &offset_b,
                speculative_margin,
                complex_shape_container: Some(&container),
                pair_count: 1,
            };

            ShapeWideTester::test(&a_wide, &b_wide, &contact_context, &mut manifold);
            assert!(manifold.contact_exists[0].extract(0));
            assert_relative_eq!(manifold.offset_a[0].extract_lane(0).z, 10.5f32);
            assert_relative_eq!(manifold.offset_a[1].extract_lane(0).z, 10.5f32);
            assert_relative_eq!(manifold.offset_a[2].extract_lane(0).z, 10.5f32);
            assert_relative_eq!(manifold.offset_a[3].extract_lane(0).z, 10.5f32);
        }
    }

    #[test]
    #[wasm_bindgen_test]
    #[cfg(feature = "serde")]
    fn test_latest_entry_1() {
        // latest_entry=1 offset = Vec3(-0.5515199, -0.1262393, -0.2773664) rotation
        // a=UnitQuat(-0.30320412, -0.04671629, -0.14460412, 0.94073087),rotation
        // b=UnitQuat(0.13640666, 0.896552, 0.4211716, -0.0142292455)
        let _ = env_logger::builder().is_test(true).try_init();
        let rotation_a = Quat::from_array([-0.30320412, -0.04671629, -0.14460412, 0.94073087])
            .as_unit_quat_unchecked();
        let rotation_b = Quat::from_array([0.13640666, 0.896552, 0.4211716, -0.0142292455])
            .as_unit_quat_unchecked();
        let offset_b = Vec3::new(-0.5515199, -0.1262393, -0.2773664);

        contact_test(rotation_a, rotation_b, offset_b);
    }

    #[cfg(feature = "serde")]
    fn contact_test(rotation_a: UnitQuat, rotation_b: UnitQuat, offset_b: Vec3) {
        let points: Vec<Point3> = serde_json::from_slice(include_bytes!(
            "tests/resource/convex_hull_create/triangular.json"
        ))
        .unwrap();
        let a = ConvexHull::new_unchecked(&points);
        let mut container = ShapeContainer::default();
        let a = container.add(a);
        let transform_a = Isometry3::from_quat(rotation_a);

        let transform_b = Isometry3::from_rotation_translation(rotation_b, offset_b);
        let a_wide = <ConvexHullWide as CreateShapeWide<1>>::create([&a].into_iter());
        let b_wide = <ConvexHullWide as CreateShapeWide<1>>::create([&a].into_iter());
        let speculative_margin = f32x4::splat(0.01);
        let offset_b = Vec3x4::splat_soa(transform_b.translation - transform_a.translation);
        let orientation_a = UnitQuatx4::splat_soa(transform_a.rotation);
        let orientation_b = UnitQuatx4::splat_soa(transform_b.rotation);
        let mut manifold = Convex4ContactManifoldWide::default();
        let contact_context = ContactContext {
            orientation_a: &orientation_a,
            orientation_b: &orientation_b,
            offset_b: &offset_b,
            speculative_margin,
            complex_shape_container: Some(&container),
            pair_count: 1,
        };

        ShapeWideTester::test(&a_wide, &b_wide, &contact_context, &mut manifold);
        assert!(manifold.contact_exists[0].extract(0));
    }

    #[test]
    #[wasm_bindgen_test]
    #[cfg(feature = "serde")]
    fn test_earliest_exit_0() {
        //latest_entry=0 offset = Vec3(0.07952821, -0.35760164,
        // -0.2799443) rotation a=UnitQuat(0.5306114, -0.74342275, -0.39264077, 0.10773814),rotation
        // b=UnitQuat(0.0806019, 0.18265624, -0.06265768, 0.97786194)
        let rotation_a = Quat::from_array([0.5306114, -0.74342275, -0.39264077, 0.10773814])
            .as_unit_quat_unchecked();
        let rotation_b = Quat::from_array([0.0806019, 0.18265624, -0.06265768, 0.97786194])
            .as_unit_quat_unchecked();
        let offset_b = Vec3::new(0.07952821, -0.35760164, -0.2799443);

        contact_test(rotation_a, rotation_b, offset_b);
    }
    #[test]
    #[wasm_bindgen_test]
    #[cfg(feature = "serde")]
    fn test_candidate_empty() {
        //candidates.is_empty offset =
        // Vec3(1.4415526, -0.012378693, -0.3213842) rotation a=UnitQuat(-0.61328185, -0.17735565,
        // 0.7692261, -0.026869163),rotation b=UnitQuat(0.58320856, 0.2171396, 0.7823491,
        // -0.025459152)

        let rotation_a = Quat::from_array([-0.61328185, -0.17735565, 0.7692261, -0.026869163])
            .as_unit_quat_unchecked();
        let rotation_b = Quat::from_array([0.58320856, 0.2171396, 0.7823491, -0.025459152])
            .as_unit_quat_unchecked();
        let offset_b = Vec3::new(1.4415526, -0.012378693, -0.3213842);
        contact_test(rotation_a, rotation_b, offset_b);
    }
}