// Copyright 2025 International Digital Economy Academy
//
// 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.
///|
pub struct ContactPair3D {
manifolds : Array[ContactPoint3D]
impulses : Array[@core.Real]
}
///|
pub fn ContactPair3D::manifolds_len(self : ContactPair3D) -> Int {
self.manifolds.length()
}
///|
pub fn ContactPair3D::manifolds(self : ContactPair3D) -> Array[ContactPoint3D] {
self.manifolds
}
///|
pub fn ContactPair3D::set_normal_impulse_at(
self : ContactPair3D,
idx : Int,
impulse : @core.Real,
) -> Unit {
if idx >= 0 && idx < self.impulses.length() {
self.impulses[idx] = impulse
}
}
///|
pub fn ContactPair3D::total_impulse(self : ContactPair3D) -> @core.Vec3 {
// Matches Rapier: sum(manifold_total_impulse * manifold_normal).
let mut out = @core.Vec3::zero()
let n = self.manifolds.length()
for i in 0.. @core.Real {
// Matches Rapier: sum(manifold_total_impulse) (not |total_impulse|).
let mut out = 0.0F
for i in 0.. (@core.Real, @core.Vec3) {
// Matches Rapier: strongest manifold impulse and its normal direction.
let mut best = 0.0F
let mut dir = @core.Vec3::zero()
let n = self.manifolds.length()
let m = self.impulses.length()
let k = if n < m { n } else { m }
for i in 0.. best {
best = imp
dir = self.manifolds[i].normal
}
}
(best, dir)
}
///|
pub struct IntersectionPair3D {
collider1 : ColliderHandle3D
collider2 : ColliderHandle3D
intersecting : Bool
}
///|
pub fn IntersectionPair3D::collider1(
self : IntersectionPair3D,
) -> ColliderHandle3D {
self.collider1
}
///|
pub fn IntersectionPair3D::collider2(
self : IntersectionPair3D,
) -> ColliderHandle3D {
self.collider2
}
///|
pub fn IntersectionPair3D::intersecting(self : IntersectionPair3D) -> Bool {
self.intersecting
}
///|
fn np_pair_key(
a : ColliderHandle3D,
b : ColliderHandle3D,
) -> (Int, Int, Int, Int) {
let (ai, ag) = a.into_raw_parts()
let (bi, bg) = b.into_raw_parts()
if ai < bi || (ai == bi && ag <= bg) {
(ai, ag, bi, bg)
} else {
(bi, bg, ai, ag)
}
}
///|
fn canonical_pair(
a : ColliderHandle3D,
b : ColliderHandle3D,
) -> (ColliderHandle3D, ColliderHandle3D) {
let (ai, ag, bi, bg) = np_pair_key(a, b)
(
ColliderHandle3D::from_raw_parts(ai, ag),
ColliderHandle3D::from_raw_parts(bi, bg),
)
}
///|
fn clamp_vec3_length_max(v : @core.Vec3, max_len : @core.Real) -> @core.Vec3 {
if max_len <= 0.0F {
return @core.Vec3::zero()
}
let len2 = v.length_squared()
let max2 = max_len * max_len
if len2 <= max2 {
return v
}
if len2 <= 1.0e-12F {
return @core.Vec3::zero()
}
let len = Float::sqrt(len2)
v.scale(max_len / len)
}
///|
fn intersects_moving_aabb3(
aabb1 : @core.Aabb3,
aabb2 : @core.Aabb3,
movement : @core.Vec3,
) -> Bool {
if aabb1.intersects(aabb2) {
return true
}
let eps = 1.0e-12F
let mut t_enter = 0.0F
let mut t_exit = 1.0F
fn update_axis_interval(
a_min : @core.Real,
a_max : @core.Real,
b_min : @core.Real,
b_max : @core.Real,
velocity : @core.Real,
eps : @core.Real,
t_enter : @core.Real,
t_exit : @core.Real,
) -> (@core.Real, @core.Real, Bool) {
if @core.abs(velocity) <= eps {
if a_max < b_min || a_min > b_max {
return (t_enter, t_exit, false)
}
return (t_enter, t_exit, true)
}
let inv = 1.0F / velocity
let t1 = (b_min - a_max) * inv
let t2 = (b_max - a_min) * inv
let axis_enter = if t1 < t2 { t1 } else { t2 }
let axis_exit = if t1 > t2 { t1 } else { t2 }
let next_enter = if axis_enter > t_enter { axis_enter } else { t_enter }
let next_exit = if axis_exit < t_exit { axis_exit } else { t_exit }
(next_enter, next_exit, next_enter <= next_exit)
}
let (tx_enter, tx_exit, ok_x) = update_axis_interval(
aabb1.mins.x,
aabb1.maxs.x,
aabb2.mins.x,
aabb2.maxs.x,
movement.x,
eps,
t_enter,
t_exit,
)
if !ok_x {
return false
}
t_enter = tx_enter
t_exit = tx_exit
let (ty_enter, ty_exit, ok_y) = update_axis_interval(
aabb1.mins.y,
aabb1.maxs.y,
aabb2.mins.y,
aabb2.maxs.y,
movement.y,
eps,
t_enter,
t_exit,
)
if !ok_y {
return false
}
t_enter = ty_enter
t_exit = ty_exit
let (tz_enter, tz_exit, ok_z) = update_axis_interval(
aabb1.mins.z,
aabb1.maxs.z,
aabb2.mins.z,
aabb2.maxs.z,
movement.z,
eps,
t_enter,
t_exit,
)
if !ok_z {
return false
}
t_enter = tz_enter
t_exit = tz_exit
t_exit >= 0.0F && t_enter <= 1.0F
}
///|
pub struct NarrowPhase3D {
contact_pairs : Array[((ColliderHandle3D, ColliderHandle3D), ContactPair3D)]
intersection_pairs : Array[
((ColliderHandle3D, ColliderHandle3D), IntersectionPair3D),
]
}
///|
pub fn NarrowPhase3D::NarrowPhase3D() -> NarrowPhase3D {
{ contact_pairs: [], intersection_pairs: [] }
}
///|
pub fn NarrowPhase3D::clear(self : NarrowPhase3D) -> Unit {
self.contact_pairs.clear()
self.intersection_pairs.clear()
}
///|
pub fn NarrowPhase3D::contact_pair(
self : NarrowPhase3D,
a : ColliderHandle3D,
b : ColliderHandle3D,
) -> ContactPair3D? {
let key = canonical_pair(a, b)
for i in 0.. Unit {
let key = canonical_pair(a, b)
for i in 0.. IntersectionPair3D? {
let key = canonical_pair(a, b)
for i in 0.. Unit {
self.clear()
for i in 0.. 0.0F || soft_ccd_prediction2 > 0.0F {
let inv_dt = if dt > 1.0e-12F { 1.0F / dt } else { 0.0F }
let max_vel1 = soft_ccd_prediction1 * inv_dt
let max_vel2 = soft_ccd_prediction2 * inv_dt
let clamped_linvel1 = clamp_vec3_length_max(linvel1, max_vel1)
let clamped_linvel2 = clamp_vec3_length_max(linvel2, max_vel2)
let aabb1 = co1.compute_collision_aabb(0.0F)
let aabb2 = co2.compute_collision_aabb(0.0F)
let relative_movement = clamped_linvel2.sub(clamped_linvel1)
if !intersects_moving_aabb3(aabb1, aabb2, relative_movement) {
continue
}
let soft_distance = dt * relative_movement.length()
if soft_distance > effective_prediction_distance {
effective_prediction_distance = soft_distance
}
}
let prediction_aabb1 = co1.compute_collision_aabb(
effective_prediction_distance,
)
let prediction_aabb2 = co2.compute_collision_aabb(
effective_prediction_distance,
)
if !prediction_aabb1.intersects(prediction_aabb2) {
continue
}
if !co1.collision_groups.test_groups(co2.collision_groups) {
self.contact_pairs.push(((a, b), { manifolds: [], impulses: [] }))
self.intersection_pairs.push(
((a, b), { collider1: a, collider2: b, intersecting: false }),
)
continue
}
// Keep the pair, but avoid generating contacts between colliders attached to the same body.
if co1.parent() is Some(p1) && co2.parent() is Some(p2) {
if p1.equals(p2) {
self.contact_pairs.push(((a, b), { manifolds: [], impulses: [] }))
self.intersection_pairs.push(
((a, b), { collider1: a, collider2: b, intersecting: false }),
)
continue
}
}
let p1 = co1.position()
let p2 = co2.position()
let s1 = co1.shape()
let s2 = co2.shape()
let contacts : Array[ContactPoint3D] = []
fn push_contacts_for_pair(
p1 : @core.Isometry3,
s1 : Shape3D,
p2 : @core.Isometry3,
s2 : Shape3D,
contacts : Array[ContactPoint3D],
) -> Unit {
match (s1, s2) {
(Compound(parts), _) =>
for i in 0..
for i in 0..
match (s1, s2) {
(HalfSpace(n), other) =>
if compute_halfspace_contact(p1, n, p2, other) is Some(cp) {
contacts.push(cp)
}
(other, HalfSpace(n)) =>
if compute_halfspace_contact(p2, n, p1, other) is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(Ball(r1), Ball(r2)) =>
if compute_ball_ball_contact(
p1.translation,
r1,
p2.translation,
r2,
)
is Some(cp) {
contacts.push(cp)
}
(Ball(r), Cuboid(he)) =>
if compute_ball_cuboid_contact(p1.translation, r, p2, he)
is Some(cp) {
contacts.push(cp)
}
(Cuboid(he), Ball(r)) =>
if compute_ball_cuboid_contact(p2.translation, r, p1, he)
is Some(cp) {
// Flip the contact so it matches the (cuboid, ball) ordering.
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(Cuboid(he1), Cuboid(he2)) =>
for
cp in compute_cuboid_cuboid_contacts(
p1, he1, p2, he2, effective_prediction_distance,
) {
contacts.push(cp)
}
(Ball(r), CapsuleY(cr, ch)) =>
if compute_ball_capsule_contact(p1.translation, r, p2, cr, ch)
is Some(cp) {
contacts.push(cp)
}
(CapsuleY(cr, ch), Ball(r)) =>
if compute_ball_capsule_contact(p2.translation, r, p1, cr, ch)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(CapsuleY(r1, h1), CapsuleY(r2, h2)) =>
if compute_capsule_capsule_contact(p1, r1, h1, p2, r2, h2)
is Some(cp) {
contacts.push(cp)
}
(CapsuleY(r, h), Cuboid(he)) =>
if compute_capsule_cuboid_contact(p1, r, h, p2, he) is Some(cp) {
contacts.push(cp)
}
(Cuboid(he), CapsuleY(r, h)) =>
if compute_capsule_cuboid_contact(p2, r, h, p1, he) is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(Cylinder(cr, ch), Cuboid(he)) =>
if compute_cylinder_cuboid_contact(p1, cr, ch, p2, he)
is Some(cp) {
contacts.push(cp)
}
(Cuboid(he), Cylinder(cr, ch)) =>
if compute_cylinder_cuboid_contact(p2, cr, ch, p1, he)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(RoundCylinder(cr, ch, br), Cuboid(he)) =>
if compute_cylinder_cuboid_contact(p1, cr + br, ch + br, p2, he)
is Some(cp) {
contacts.push(cp)
}
(Cuboid(he), RoundCylinder(cr, ch, br)) =>
if compute_cylinder_cuboid_contact(p2, cr + br, ch + br, p1, he)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(Cylinder(r1, h1), Cylinder(r2, h2)) =>
if compute_cylinder_cylinder_contact(p1, r1, h1, p2, r2, h2)
is Some(cp) {
contacts.push(cp)
}
(Cylinder(r1, h1), RoundCylinder(r2, h2, br2)) =>
if compute_cylinder_cylinder_contact(
p1,
r1,
h1,
p2,
r2 + br2,
h2 + br2,
)
is Some(cp) {
contacts.push(cp)
}
(RoundCylinder(r1, h1, br1), Cylinder(r2, h2)) =>
if compute_cylinder_cylinder_contact(
p1,
r1 + br1,
h1 + br1,
p2,
r2,
h2,
)
is Some(cp) {
contacts.push(cp)
}
(RoundCylinder(r1, h1, br1), RoundCylinder(r2, h2, br2)) =>
if compute_cylinder_cylinder_contact(
p1,
r1 + br1,
h1 + br1,
p2,
r2 + br2,
h2 + br2,
)
is Some(cp) {
contacts.push(cp)
}
(Cone(cr, ch), Cuboid(he)) =>
if compute_cone_cuboid_contact(p1, cr, ch, p2, he) is Some(cp) {
contacts.push(cp)
}
(Cuboid(he), Cone(cr, ch)) =>
if compute_cone_cuboid_contact(p2, cr, ch, p1, he) is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(Cone(r1, h1), Cylinder(r2, h2)) =>
if compute_cone_cylinder_contact(p1, r1, h1, p2, r2, h2)
is Some(cp) {
contacts.push(cp)
}
(Cone(r1, h1), RoundCylinder(r2, h2, br2)) =>
if compute_cone_cylinder_contact(
p1,
r1,
h1,
p2,
r2 + br2,
h2 + br2,
)
is Some(cp) {
contacts.push(cp)
}
(Cylinder(r1, h1), Cone(r2, h2)) =>
if compute_cone_cylinder_contact(p2, r2, h2, p1, r1, h1)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(RoundCylinder(r1, h1, br1), Cone(r2, h2)) =>
if compute_cone_cylinder_contact(
p2,
r2,
h2,
p1,
r1 + br1,
h1 + br1,
)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(Cone(r1, h1), Cone(r2, h2)) =>
if compute_cone_cone_contact(p1, r1, h1, p2, r2, h2) is Some(cp) {
contacts.push(cp)
}
(Ball(r), Cylinder(cr, ch)) =>
if compute_ball_cylinder_contact(p1.translation, r, p2, cr, ch)
is Some(cp) {
contacts.push(cp)
}
(Cylinder(cr, ch), Ball(r)) =>
if compute_ball_cylinder_contact(p2.translation, r, p1, cr, ch)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(Ball(r), RoundCylinder(cr, ch, br)) =>
if compute_ball_cylinder_contact(
p1.translation,
r,
p2,
cr + br,
ch + br,
)
is Some(cp) {
contacts.push(cp)
}
(RoundCylinder(cr, ch, br), Ball(r)) =>
if compute_ball_cylinder_contact(
p2.translation,
r,
p1,
cr + br,
ch + br,
)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(Ball(r), Cone(cr, ch)) =>
if compute_ball_cone_contact(p1.translation, r, p2, cr, ch)
is Some(cp) {
contacts.push(cp)
}
(Cone(cr, ch), Ball(r)) =>
if compute_ball_cone_contact(p2.translation, r, p1, cr, ch)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(Ball(r), TriMesh(vertices, indices)) =>
if compute_ball_trimesh_contact(
p1.translation,
r,
p2,
vertices,
indices,
)
is Some(cp) {
contacts.push(cp)
}
(Ball(r), Voxels(v)) =>
if compute_ball_voxels_contact(p1.translation, r, p2, v)
is Some(cp) {
contacts.push(cp)
}
(Ball(r), Triangle(a, b, c)) => {
let vtx = [a, b, c]
let idx = [(0, 1, 2)]
if compute_ball_trimesh_contact(p1.translation, r, p2, vtx, idx)
is Some(cp) {
contacts.push(cp)
}
}
(Ball(r), Heightfield(vertices, _, rows, cols, flags)) =>
if compute_ball_heightfield_contact_with_flags(
p1.translation,
r,
p2,
vertices,
rows,
cols,
flags,
)
is Some(cp) {
contacts.push(cp)
}
(TriMesh(vertices, indices), Ball(r)) =>
if compute_ball_trimesh_contact(
p2.translation,
r,
p1,
vertices,
indices,
)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(Voxels(v), Ball(r)) =>
if compute_ball_voxels_contact(p2.translation, r, p1, v)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(Triangle(a, b, c), Ball(r)) => {
let vtx = [a, b, c]
let idx = [(0, 1, 2)]
if compute_ball_trimesh_contact(p2.translation, r, p1, vtx, idx)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
}
(Heightfield(vertices, _, rows, cols, flags), Ball(r)) =>
if compute_ball_heightfield_contact_with_flags(
p2.translation,
r,
p1,
vertices,
rows,
cols,
flags,
)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(Cuboid(he), TriMesh(vertices, indices)) =>
if compute_cuboid_trimesh_contact(p1, he, p2, vertices, indices)
is Some(cp) {
contacts.push(cp)
}
(Cuboid(he), Voxels(v)) =>
if compute_cuboid_voxels_contact(p1, he, p2, v) is Some(cp) {
contacts.push(cp)
}
(Cuboid(he), Triangle(a, b, c)) => {
let cps = compute_cuboid_triangle_contacts(p1, he, p2, a, b, c)
for k in 0..
if compute_cuboid_heightfield_contact_with_flags(
p1, he, p2, vertices, rows, cols, flags,
)
is Some(cp) {
contacts.push(cp)
}
(TriMesh(vertices, indices), Cuboid(he)) =>
if compute_cuboid_trimesh_contact(p2, he, p1, vertices, indices)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(Voxels(v), Cuboid(he)) =>
if compute_cuboid_voxels_contact(p2, he, p1, v) is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(Triangle(a, b, c), Cuboid(he)) => {
let cps = compute_cuboid_triangle_contacts(p2, he, p1, a, b, c)
for k in 0..
if compute_cuboid_heightfield_contact_with_flags(
p2, he, p1, vertices, rows, cols, flags,
)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(CapsuleY(r, hh), TriMesh(vertices, indices)) =>
if compute_capsule_trimesh_contact(
p1, r, hh, p2, vertices, indices,
)
is Some(cp) {
contacts.push(cp)
}
(CapsuleY(r, hh), Voxels(v)) =>
if compute_capsule_voxels_contact(p1, r, hh, p2, v) is Some(cp) {
contacts.push(cp)
}
(CapsuleY(r, hh), Triangle(a, b, c)) => {
let vtx = [a, b, c]
let idx = [(0, 1, 2)]
if compute_capsule_trimesh_contact(p1, r, hh, p2, vtx, idx)
is Some(cp) {
contacts.push(cp)
}
}
(CapsuleY(r, hh), Heightfield(vertices, _, rows, cols, flags)) =>
if compute_capsule_heightfield_contact_with_flags(
p1, r, hh, p2, vertices, rows, cols, flags,
)
is Some(cp) {
contacts.push(cp)
}
(TriMesh(vertices, indices), CapsuleY(r, hh)) =>
if compute_capsule_trimesh_contact(
p2, r, hh, p1, vertices, indices,
)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(Voxels(v), CapsuleY(r, hh)) =>
if compute_capsule_voxels_contact(p2, r, hh, p1, v) is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(Triangle(a, b, c), CapsuleY(r, hh)) => {
let vtx = [a, b, c]
let idx = [(0, 1, 2)]
if compute_capsule_trimesh_contact(p2, r, hh, p1, vtx, idx)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
}
(Heightfield(vertices, _, rows, cols, flags), CapsuleY(r, hh)) =>
if compute_capsule_heightfield_contact_with_flags(
p2, r, hh, p1, vertices, rows, cols, flags,
)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(Cylinder(r, hh), TriMesh(vertices, indices)) =>
if compute_cylinder_trimesh_contact(
p1, r, hh, p2, vertices, indices,
)
is Some(cp) {
contacts.push(cp)
}
(Cylinder(r, hh), Voxels(v)) =>
if compute_cylinder_voxels_contact(p1, r, hh, p2, v) is Some(cp) {
contacts.push(cp)
}
(Cylinder(r, hh), Triangle(a, b, c)) => {
let vtx = [a, b, c]
let idx = [(0, 1, 2)]
if compute_cylinder_trimesh_contact(p1, r, hh, p2, vtx, idx)
is Some(cp) {
contacts.push(cp)
}
}
(Cylinder(r, hh), Heightfield(vertices, _, rows, cols, flags)) =>
if compute_cylinder_heightfield_contact_with_flags(
p1, r, hh, p2, vertices, rows, cols, flags,
)
is Some(cp) {
contacts.push(cp)
}
(TriMesh(vertices, indices), Cylinder(r, hh)) =>
if compute_cylinder_trimesh_contact(
p2, r, hh, p1, vertices, indices,
)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(Voxels(v), Cylinder(r, hh)) =>
if compute_cylinder_voxels_contact(p2, r, hh, p1, v) is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(Triangle(a, b, c), Cylinder(r, hh)) => {
let vtx = [a, b, c]
let idx = [(0, 1, 2)]
if compute_cylinder_trimesh_contact(p2, r, hh, p1, vtx, idx)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
}
(Heightfield(vertices, _, rows, cols, flags), Cylinder(r, hh)) =>
if compute_cylinder_heightfield_contact_with_flags(
p2, r, hh, p1, vertices, rows, cols, flags,
)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(RoundCylinder(r, hh, br), TriMesh(vertices, indices)) =>
if compute_round_cylinder_trimesh_contact(
p1, r, hh, br, p2, vertices, indices,
)
is Some(cp) {
contacts.push(cp)
}
(RoundCylinder(r, hh, br), Voxels(v)) =>
if compute_cylinder_voxels_contact(p1, r + br, hh + br, p2, v)
is Some(cp) {
contacts.push(cp)
}
(RoundCylinder(r, hh, br), Triangle(a, b, c)) => {
let vtx = [a, b, c]
let idx = [(0, 1, 2)]
if compute_round_cylinder_trimesh_contact(
p1, r, hh, br, p2, vtx, idx,
)
is Some(cp) {
contacts.push(cp)
}
}
(
RoundCylinder(r, hh, br),
Heightfield(vertices, _, rows, cols, flags),
) =>
if compute_round_cylinder_heightfield_contact_with_flags(
p1, r, hh, br, p2, vertices, rows, cols, flags,
)
is Some(cp) {
contacts.push(cp)
}
(TriMesh(vertices, indices), RoundCylinder(r, hh, br)) =>
if compute_round_cylinder_trimesh_contact(
p2, r, hh, br, p1, vertices, indices,
)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(Voxels(v), RoundCylinder(r, hh, br)) =>
if compute_cylinder_voxels_contact(p2, r + br, hh + br, p1, v)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(Triangle(a, b, c), RoundCylinder(r, hh, br)) => {
let vtx = [a, b, c]
let idx = [(0, 1, 2)]
if compute_round_cylinder_trimesh_contact(
p2, r, hh, br, p1, vtx, idx,
)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
}
(
Heightfield(vertices, _, rows, cols, flags),
RoundCylinder(r, hh, br),
) =>
if compute_round_cylinder_heightfield_contact_with_flags(
p2, r, hh, br, p1, vertices, rows, cols, flags,
)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(Cone(r, hh), TriMesh(vertices, indices)) =>
if compute_cone_trimesh_contact(
p1, r, hh, p2, vertices, indices,
)
is Some(cp) {
contacts.push(cp)
}
(Cone(r, hh), Voxels(v)) =>
if compute_cone_voxels_contact(p1, r, hh, p2, v) is Some(cp) {
contacts.push(cp)
}
(Cone(r, hh), Triangle(a, b, c)) => {
let vtx = [a, b, c]
let idx = [(0, 1, 2)]
if compute_cone_trimesh_contact(p1, r, hh, p2, vtx, idx)
is Some(cp) {
contacts.push(cp)
}
}
(Cone(r, hh), Heightfield(vertices, _, rows, cols, flags)) =>
if compute_cone_heightfield_contact_with_flags(
p1, r, hh, p2, vertices, rows, cols, flags,
)
is Some(cp) {
contacts.push(cp)
}
(TriMesh(vertices, indices), Cone(r, hh)) =>
if compute_cone_trimesh_contact(
p2, r, hh, p1, vertices, indices,
)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(Voxels(v), Cone(r, hh)) =>
if compute_cone_voxels_contact(p2, r, hh, p1, v) is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
(Triangle(a, b, c), Cone(r, hh)) => {
let vtx = [a, b, c]
let idx = [(0, 1, 2)]
if compute_cone_trimesh_contact(p2, r, hh, p1, vtx, idx)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
}
(Heightfield(vertices, _, rows, cols, flags), Cone(r, hh)) =>
if compute_cone_heightfield_contact_with_flags(
p2, r, hh, p1, vertices, rows, cols, flags,
)
is Some(cp) {
contacts.push({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
}
_ =>
if compute_convex_contact(p1, s1, p2, s2) is Some(cp) {
contacts.push(cp)
}
}
}
}
push_contacts_for_pair(p1, s1, p2, s2, contacts)
// Keep up to 4 contact points per pair (a lightweight manifold).
if contacts.length() > 4 {
// Sort by penetration descending (insertion sort for small N).
for i in 1..= 0 && contacts[j].penetration < key.penetration {
contacts[j + 1] = contacts[j]
j = j - 1
}
contacts[j + 1] = key
}
let unique_contacts : Array[ContactPoint3D] = []
let eps2 = 1.0e-8F
for c in contacts {
let mut dup = false
for k in 0..= 4 {
break
}
}
}
contacts.clear()
for c in unique_contacts {
contacts.push(c)
}
}
let impulses : Array[@core.Real] = []
for _ in 0.. 0 },
),
)
}
}
}
///|
pub fn NarrowPhase3D::all_contact_pairs(
self : NarrowPhase3D,
) -> Array[((ColliderHandle3D, ColliderHandle3D), ContactPair3D)] {
self.contact_pairs
}
///|
pub fn NarrowPhase3D::all_intersection_pairs(
self : NarrowPhase3D,
) -> Array[((ColliderHandle3D, ColliderHandle3D), IntersectionPair3D)] {
self.intersection_pairs
}