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
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
// 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::*;
use glam_det::{Cross, Dot, Mat3x4, UnitQuat, UnitQuatx4, Vec3, Vec3x4};
use crate::collision_tasks::common::{NormalizeExt, EPS_10, EPS_15};
use crate::collision_tasks::{ShapeTester, ShapeWideTester};
use crate::convex_contact_manifold::Convex4ContactManifoldWide;
use crate::shapes::CapsuleWide;
use crate::traits::{ContactContext, ContactManifoldWide, CreateShapeWide, PairTest, PairWideTest};
use crate::{Capsule, ConvexContactManifold, ShapeContainer};
impl PairWideTest<CapsuleWide, CapsuleWide> for ShapeWideTester {
// 2 manifold in Convex4ContactManifoldWide
#[inline]
fn test(
a: &CapsuleWide,
b: &CapsuleWide,
contact_context: &ContactContext,
manifold: &mut Convex4ContactManifoldWide,
) {
const LOWER_THRESHOLD_ANGLE: f32 = 0.01;
const UPPER_THRESHOLD_ANGLE: f32 = 0.05;
const LOWER_THRESHOLD: f32 = LOWER_THRESHOLD_ANGLE * LOWER_THRESHOLD_ANGLE;
const UPPER_THRESHOLD: f32 = UPPER_THRESHOLD_ANGLE * UPPER_THRESHOLD_ANGLE;
// 1. Consider two line segments as two lines, and find the closest points.
// The rotation ||ra|| == ||rb|| == 1 == ra^2 == rb^2
// n = ra x rb, where n is the normal of the plane defined by the two lines.
// ||n|| = ||ra|| * ||rb| * sin(θ) = sin(θ)
//
// la: the length from the closest point on `a` segment to `a` center.
// Project b-a to the normal of rb x n / ||n||, it equals to la * sin(θ)
// la = (b - a) * (rb x n) / ||n|| / sin(θ) = (b - a) * (rb x n) / ||n||^2
// lb = la * (ra * rb) - rb * (b - a)
//
// rb x n = rb x (ra x rb) = (rb * rb) * ra - (rb * ra) * rb = ra - rb * (ra * rb)
// ||n||^2 = ||ra x rb||^2 = ||ra||^2 * ||rb||^2 - (ra * rb)^2 = 1 - (ra * rb)^2
//
// la = ((b - a) * ra - (b - a) * rb * (ra * rb)) / (1 - (ra * rb)^2)
let orientation_a_matrix = Mat3x4::from_quat(*contact_context.orientation_a);
let ra_y_axis = orientation_a_matrix.y_axis;
let rb_y_axis = Mat3x4::from_quat(*contact_context.orientation_b).y_axis;
let ra_offset_b = ra_y_axis.dot(contact_context.offset_b);
let rb_offset_b = rb_y_axis.dot(contact_context.offset_b);
let rarb = ra_y_axis.dot(rb_y_axis);
// Note potential division by zero.
let mut la =
(ra_offset_b - rb_offset_b * rarb) * EPS_15.max(f32x4::ONE - rarb * rarb).recip();
let mut lb = la * rarb - rb_offset_b;
// 2. Clamp closest points to the line segment.
// Project segmenmt lines to each other to clamp la lb.
let scale_rarb = rarb.absf();
let b_half_height_on_a = b.half_height * scale_rarb;
let a_half_height_on_b = a.half_height * scale_rarb;
let b_on_a_min = ra_offset_b - b_half_height_on_a;
let b_on_a_max = ra_offset_b + b_half_height_on_a;
let a_on_b_min = -a_half_height_on_b - rb_offset_b;
let a_on_b_max = a_half_height_on_b - rb_offset_b;
let mut a_min = b_on_a_min.clamp(-a.half_height, a.half_height);
let mut a_max = b_on_a_max.clamp(-a.half_height, a.half_height);
let b_min = a_on_b_min.clamp(-b.half_height, b.half_height);
let b_max = a_on_b_max.clamp(-b.half_height, b.half_height);
la = la.clamp(a_min, a_max);
lb = lb.clamp(b_min, b_max);
let closest_point_on_a = ra_y_axis * la;
let closest_point_on_b = rb_y_axis * lb + contact_context.offset_b;
let closest_b_to_a = closest_point_on_a - closest_point_on_b;
let (distance, normal_closest_b_to_a) =
closest_b_to_a.normalize_and_length_or(orientation_a_matrix.x_axis, f32x4::EPSILON);
// 3. Compute closest point range for coplanar.
// Get the plane_normal of the plane defined b and closest_b_to_a.
// Only when the angle is small enough, we consider the two capsules are almost coplanar,
// and interaction now could be a line(diff min,max_closest_point_on_a), otherwise they are
// same points.
// plane_normal = (rb x closest_b_to_a)/||rb x closest_b_to_a||
// The angle between the plane and the capsule axis: angle ~= sin(angle) = ra *
// plane_normal.
let plane_normal = rb_y_axis.cross(normal_closest_b_to_a);
let plane_normal_length_squared = plane_normal.length_squared();
let numerator_unsquared = ra_y_axis.dot(plane_normal);
let squared_angle = f32x4::ZERO.select(
plane_normal_length_squared.lt(EPS_10),
numerator_unsquared * numerator_unsquared * plane_normal_length_squared.recip(),
);
// Angle close to UPPER is close to 0, and angle close to LOWER is close to 1,
// which means the smaller angle has larger contact line.
let interval_weight = ((f32x4::splat(UPPER_THRESHOLD) - squared_angle)
* f32x4::splat((UPPER_THRESHOLD - LOWER_THRESHOLD).recip()))
.clamp(f32x4::ZERO, f32x4::ONE);
// When squared_angle >= UPPER, interval_weight = 0, a_min == a_max == la.
let weighted_la = la - la * interval_weight;
a_min = interval_weight * a_min + weighted_la;
a_max = interval_weight * a_max + weighted_la;
let min_closest_point_on_a = ra_y_axis * a_min;
let max_closest_point_on_a = ra_y_axis * a_max;
// 4. Compute depth.
//In the coplanar case, there are two points. We need a method of computing depth which
// gives a reasonable result to the second contact. Note that one of the two
// contacts should end up with a distance equal to the previously computed segment distance,
// so we're doing some redundant work here. It's just easier to do that extra work
// than it would be to track which endpoint contributed the lower distance.
// Unproject the final interval endpoints from a back onto b.
//dot(offset_b + rb * tb0, ra) = ta0
//tb0 = (ta0 - daOffsetB) / rarb
//distance0 = dot(a0 - (offset_b + tb0 * rb), normal)
//distance1 = dot(a1 - (offset_b + tb1 * rb), normal)
// depth = a.radius + b.radius - distance
// distance = closest points distance project on normal.
let offset_b0 = min_closest_point_on_a - contact_context.offset_b;
let offset_b1 = max_closest_point_on_a - contact_context.offset_b;
//Note potential division by zero. In that case, treat both projected points as the closest
// point. (Handled by the conditional select that chooses the previously computed distance.)
let inverse_rarb = rarb.recip();
// Project a_min on b segment line.
let projected_lb0 = ((a_min - ra_offset_b) * inverse_rarb).clamp(b_min, b_max);
let projected_lb1 = ((a_max - ra_offset_b) * inverse_rarb).clamp(b_min, b_max);
// b0_normal: Closest point of a's segment to b center, and project it on normal.
let b0_normal = offset_b0.dot(normal_closest_b_to_a);
let b1_normal = offset_b1.dot(normal_closest_b_to_a);
let capsules_are_perpendicular = scale_rarb.lt(f32x4::EPSILON);
// `rb_normal * projected_lb0` is a_min projected on normal.
let rb_normal = rb_y_axis.dot(normal_closest_b_to_a);
let distance0 = distance.select(
capsules_are_perpendicular,
b0_normal - rb_normal * projected_lb0,
);
let distance1 = distance.select(
capsules_are_perpendicular,
b1_normal - rb_normal * projected_lb1,
);
let combined_radius = a.radius + b.radius;
manifold.depth[0] = combined_radius - distance0;
manifold.depth[1] = combined_radius - distance1;
// 5. Compute contact on surface.
// Surface contact = radius_on_normal + closest_point_on_a
let negative_radius = -a.radius;
let radius_on_normal = normal_closest_b_to_a * negative_radius;
manifold.offset_a[0] = min_closest_point_on_a + radius_on_normal;
manifold.offset_a[1] = max_closest_point_on_a + radius_on_normal;
manifold.normal = normal_closest_b_to_a.as_unit_vec3x4_unchecked();
manifold.feature_id[0] = u32x4::ZERO;
manifold.feature_id[1] = u32x4::ONE;
let minimum_accepted_depth = -contact_context.speculative_margin;
manifold.contact_exists[0] = manifold.depth[0].ge(minimum_accepted_depth);
manifold.contact_exists[1] = manifold.depth[1].ge(minimum_accepted_depth)
& (a_max - a_min).gt(f32x4::EPSILON * a.half_height);
}
#[inline]
fn should_reset_manifold_before_test() -> bool {
false
}
}
impl_pair_narrowphase!(Capsule, Capsule, CapsuleWide, CapsuleWide, 2);