1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
// 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::ops::Mul;
use glam_det::nums::*;
use glam_det::{Mat3x4, UnitQuat, UnitQuatx4, Vec3, Vec3x4};
use crate::collision_tasks::ShapeWideTester;
use crate::convex_contact_manifold::Convex4ContactManifoldWide;
use crate::shapes::{Cylinder, CylinderWide, Sphere, SphereWide};
use crate::traits::{ContactContext, ContactManifoldWide, CreateShapeWide, PairWideTest};
use crate::{ConvexContactManifold, PairTest, ShapeContainer, ShapeTester};
impl PairWideTest<SphereWide, CylinderWide> for ShapeWideTester {
#[inline]
fn should_reset_manifold_before_test() -> bool {
false
}
// 1 manifold in Convex4ContactManifoldWide
fn test(
a: &SphereWide,
b: &CylinderWide,
contact_context: &ContactContext,
manifold: &mut Convex4ContactManifoldWide,
) {
let orientation_b_matrix = Mat3x4::from_quat(*contact_context.orientation_b);
// step1: find closest point on cylinder to sphere center
// below point and vector is in cylinder local space:
// oa: sphere center
// ob: cylinder center
// closest_point_on_cylinder: the target closest point on cylinder to sphere center
// oa_ob: vector oa to ob
// ob_oa: vector ob to oa
// oa_to_closest: vector oa to closest_point_on_cylinder
let oa_ob = orientation_b_matrix
.transpose()
.mul_vec3(*contact_context.offset_b);
let ob_oa = -oa_ob; // sphere center in cylinder local space
// project ob_oa to xz-plane
let projected_center_distance = (ob_oa.x * ob_oa.x + ob_oa.z * ob_oa.z).sqrtf();
let inv_projected_center_distance = projected_center_distance.recip();
// if sphere center project point is out of cylinder circle, clamp it to the circle
// (x, y) -> (x * r / d, y * r / d) where d = |(x, y)| and r is cylinder radius
let clamp_to_cylinder_circle = projected_center_distance.gt(b.radius);
let r_d = b.radius * inv_projected_center_distance;
// else use the original point in cylinder circle
let closest_point_on_cylinder = Vec3x4::new(
(ob_oa.x * r_d).select(clamp_to_cylinder_circle, ob_oa.x),
ob_oa.y.clamp(-b.half_height, b.half_height),
(ob_oa.z * r_d).select(clamp_to_cylinder_circle, ob_oa.z),
);
let oa_to_closest = oa_ob + closest_point_on_cylinder;
// step2: decide which depth and normal to use
// vertical_depth: distance between oa and ob on y axis
// horizontal_depth: distance between oa and ob on xz plane
let vertical_depth = b.half_height - ob_oa.y.absf();
let horizontal_depth = b.radius - projected_center_distance;
let use_vertical_depth = vertical_depth.le(horizontal_depth);
// if use horizontal depth, normal is (0, 1, 0) or (0, -1, 0)
// if use vertical depth, normal is (x, 0, z), xz is from normalized projected ob_oa
// if sphere and cylinder center is very close, use (1, 0, 0)
let center_very_close_on_xz_plane =
projected_center_distance.le(b.radius * f32x4::splat(1e-5));
let default_normal = Vec3x4::new(
f32x4::ZERO.select(
use_vertical_depth,
f32x4::ONE.select(
center_very_close_on_xz_plane,
ob_oa.x * inv_projected_center_distance,
),
),
f32x4::ONE
.select(ob_oa.y.gt(f32x4::ZERO), f32x4::NEG_ONE)
.select(use_vertical_depth, f32x4::ZERO),
f32x4::ZERO.select(
use_vertical_depth,
f32x4::ZERO.select(
center_very_close_on_xz_plane,
ob_oa.z * inv_projected_center_distance,
),
),
);
// use oa to closest point on cylinder as normal direction
let oa_to_closest_distance = oa_to_closest.length();
let normal = -oa_to_closest * oa_to_closest_distance.recip();
// if oa is very close to cylinder, use default normal
let use_default_normal = oa_to_closest_distance.lt(f32x4::splat(1e-7));
manifold.normal = orientation_b_matrix
.mul(Vec3x4::lane_select(
use_default_normal,
default_normal,
normal,
))
.as_unit_vec3x4_unchecked();
manifold.depth[0] = vertical_depth
.select(use_vertical_depth, horizontal_depth)
.select(use_default_normal, -oa_to_closest_distance)
+ a.radius;
manifold.offset_a[0] =
orientation_b_matrix.mul_vec3(oa_to_closest) - manifold.normal * manifold.depth[0];
manifold.contact_exists[0] = manifold.depth[0].ge(-contact_context.speculative_margin);
manifold.feature_id[0] = u32x4::ZERO;
}
}
impl_pair_narrowphase!(Sphere, Cylinder, SphereWide, CylinderWide, 1);