// 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 ContactPoint3D {
point1 : @core.Vec3
point2 : @core.Vec3
normal : @core.Vec3
penetration : @core.Real
}
///|
priv enum CuboidBestAxis {
A(Int)
B(Int)
Cross(Int, Int)
}
///|
fn local_sqrt(value : @core.Real) -> @core.Real {
if value <= 0.0F {
0.0F
} else {
let mut x = value
for _ in 0..<8 {
x = 0.5F * (x + value / x)
}
x
}
}
///|
fn local_max(a : @core.Real, b : @core.Real) -> @core.Real {
if a > b {
a
} else {
b
}
}
///|
fn local_min(a : @core.Real, b : @core.Real) -> @core.Real {
if a < b {
a
} else {
b
}
}
///|
fn closest_point_on_triangle3d(
p : @core.Vec3,
a : @core.Vec3,
b : @core.Vec3,
c : @core.Vec3,
) -> @core.Vec3 {
// Real-Time Collision Detection (Christer Ericson), 5.1.5.
let ab = b.sub(a)
let ac = c.sub(a)
let ap = p.sub(a)
let d1 = ab.dot(ap)
let d2 = ac.dot(ap)
if d1 <= 0.0F && d2 <= 0.0F {
return a
}
let bp = p.sub(b)
let d3 = ab.dot(bp)
let d4 = ac.dot(bp)
if d3 >= 0.0F && d4 <= d3 {
return b
}
let vc = d1 * d4 - d3 * d2
if vc <= 0.0F && d1 >= 0.0F && d3 <= 0.0F {
let v = d1 / (d1 - d3)
return a.add(ab.scale(v))
}
let cp = p.sub(c)
let d5 = ab.dot(cp)
let d6 = ac.dot(cp)
if d6 >= 0.0F && d5 <= d6 {
return c
}
let vb = d5 * d2 - d1 * d6
if vb <= 0.0F && d2 >= 0.0F && d6 <= 0.0F {
let w = d2 / (d2 - d6)
return a.add(ac.scale(w))
}
let va = d3 * d6 - d5 * d4
if va <= 0.0F && d4 - d3 >= 0.0F && d5 - d6 >= 0.0F {
let w = (d4 - d3) / (d4 - d3 + (d5 - d6))
return b.add(c.sub(b).scale(w))
}
let denom = 1.0F / (va + vb + vc)
let v = vb * denom
let w = vc * denom
a.add(ab.scale(v)).add(ac.scale(w))
}
///|
fn trimesh_closest_point(
p : @core.Vec3,
mesh_pos : @core.Isometry3,
vertices : Array[@core.Vec3],
indices : Array[(Int, Int, Int)],
) -> (@core.Vec3, @core.Vec3, @core.Real, Bool) {
if indices.length() == 0 || vertices.length() == 0 {
return (@core.Vec3::zero(), Vec3(0.0F, 1.0F, 0.0F), 0.0F, false)
}
let mut best_dist2 = 1.0e30F
let mut best_point = @core.Vec3::zero()
let mut best_normal = @core.Vec3(0.0F, 1.0F, 0.0F)
let mut best_normal_sum = @core.Vec3::zero()
let mut best_count = 0
let eps_d2 = 1.0e-6F
let eps_q2 = 1.0e-8F
for i in 0..= vertices.length() ||
i1 >= vertices.length() ||
i2 >= vertices.length() {
continue
}
let a = mesh_pos.transform_point(vertices[i0])
let b = mesh_pos.transform_point(vertices[i1])
let c = mesh_pos.transform_point(vertices[i2])
let q = closest_point_on_triangle3d(p, a, b, c)
let d2 = q.sub(p).length_squared()
let n = b.sub(a).cross(c.sub(a))
let n_norm = if n.length_squared() > 1.0e-12F {
n.normalize()
} else {
Vec3(0.0F, 1.0F, 0.0F)
}
if d2 + eps_d2 < best_dist2 {
best_dist2 = d2
best_point = q
best_normal_sum = n_norm
best_count = 1
} else if @core.abs(d2 - best_dist2) <= eps_d2 &&
q.sub(best_point).length_squared() <= eps_q2 {
// FIX_INTERNAL_EDGES-style smoothing: if multiple triangles are equally close at the same
// projection point (common for coplanar splits), average their normals.
best_normal_sum = best_normal_sum.add(n_norm)
best_count = best_count + 1
}
}
if best_count > 0 && best_normal_sum.length_squared() > 1.0e-12F {
best_normal = best_normal_sum.normalize()
}
(best_point, best_normal, best_dist2, true)
}
///|
/// Heightfield-specific closest-point query.
///
/// Unlike `trimesh_closest_point`, this exploits the regular grid structure:
/// it performs a best-first search over the heightfield cells, using each cell's AABB
/// as a lower-bound. This keeps performance reasonable for large heightfields while
/// still returning the exact closest point.
fn heightfield_closest_point(
p : @core.Vec3,
mesh_pos : @core.Isometry3,
vertices : Array[@core.Vec3],
nrows : Int,
ncols : Int,
flags : Int,
) -> (@core.Vec3, @core.Vec3, @core.Real, Bool) {
if nrows <= 1 || ncols <= 1 || vertices.length() != nrows * ncols {
return (@core.Vec3::zero(), Vec3(0.0F, 1.0F, 0.0F), 0.0F, false)
}
// Work in the heightfield local-space to avoid per-triangle transforms.
let inv = mesh_pos.inverse()
let p_local = inv.transform_point(p)
let origin_x = vertices[0].x
let origin_z = vertices[0].z
let dx0 = if nrows >= 2 { vertices[ncols].x - origin_x } else { 1.0F }
let dz0 = if ncols >= 2 { vertices[1].z - origin_z } else { 1.0F }
let dx = if dx0 < 0.0F { -dx0 } else { dx0 }
let dz = if dz0 < 0.0F { -dz0 } else { dz0 }
if dx <= 1.0e-12F || dz <= 1.0e-12F {
return (@core.Vec3::zero(), Vec3(0.0F, 1.0F, 0.0F), 0.0F, false)
}
let cells_x = nrows - 1
let cells_z = ncols - 1
fn clamp_int(v : Int, lo : Int, hi : Int) -> Int {
if v < lo {
lo
} else if v > hi {
hi
} else {
v
}
}
fn floor_to_int(x : @core.Real) -> Int {
// Matches Rust's `floor()` behavior for negative values.
let i = x.to_int()
if Float::from_int(i) > x {
i - 1
} else {
i
}
}
let fx = (p_local.x - origin_x) / dx
let fz = (p_local.z - origin_z) / dz
let start_i = clamp_int(floor_to_int(fx), 0, cells_x - 1)
let start_j = clamp_int(floor_to_int(fz), 0, cells_z - 1)
fn cell_id(i : Int, j : Int, cells_z : Int) -> Int {
i * cells_z + j
}
fn point_aabb_dist2(
p : @core.Vec3,
min : @core.Vec3,
max : @core.Vec3,
) -> @core.Real {
let dx = if p.x < min.x {
min.x - p.x
} else if p.x > max.x {
p.x - max.x
} else {
0.0F
}
let dy = if p.y < min.y {
min.y - p.y
} else if p.y > max.y {
p.y - max.y
} else {
0.0F
}
let dz = if p.z < min.z {
min.z - p.z
} else if p.z > max.z {
p.z - max.z
} else {
0.0F
}
dx * dx + dy * dy + dz * dz
}
fn cell_aabb(
vertices : Array[@core.Vec3],
ncols : Int,
origin_x : @core.Real,
origin_z : @core.Real,
dx : @core.Real,
dz : @core.Real,
i : Int,
j : Int,
) -> (@core.Vec3, @core.Vec3) {
let x0 = origin_x + Float::from_int(i) * dx
let x1 = origin_x + Float::from_int(i + 1) * dx
let z0 = origin_z + Float::from_int(j) * dz
let z1 = origin_z + Float::from_int(j + 1) * dz
let minx = if x0 < x1 { x0 } else { x1 }
let maxx = if x0 > x1 { x0 } else { x1 }
let minz = if z0 < z1 { z0 } else { z1 }
let maxz = if z0 > z1 { z0 } else { z1 }
let a = vertices[i * ncols + j]
let b = vertices[(i + 1) * ncols + j]
let c = vertices[(i + 1) * ncols + (j + 1)]
let d = vertices[i * ncols + (j + 1)]
let mut miny = a.y
let mut maxy = a.y
for v in [b, c, d] {
if v.y < miny {
miny = v.y
}
if v.y > maxy {
maxy = v.y
}
}
(Vec3(minx, miny, minz), Vec3(maxx, maxy, maxz))
}
// Min-heap of (lower_bound_dist2, i, j).
let heap : Array[(@core.Real, Int, Int)] = []
fn heap_push(
heap : Array[(@core.Real, Int, Int)],
item : (@core.Real, Int, Int),
) {
heap.push(item)
let mut k = heap.length() - 1
while k > 0 {
let p = (k - 1) / 2
if heap[p].0 <= heap[k].0 {
break
}
let tmp = heap[p]
heap[p] = heap[k]
heap[k] = tmp
k = p
}
}
fn heap_pop_min(
heap : Array[(@core.Real, Int, Int)],
) -> (@core.Real, Int, Int)? {
if heap.length() == 0 {
return None
}
let out = heap[0]
let last_opt = heap.pop()
if heap.length() == 0 {
return Some(out)
}
if last_opt is Some(last) {
heap[0] = last
} else {
return Some(out)
}
let mut k = 0
while true {
let l = 2 * k + 1
if l >= heap.length() {
break
}
let r = l + 1
let mut m = l
if r < heap.length() && heap[r].0 < heap[l].0 {
m = r
}
if heap[k].0 <= heap[m].0 {
break
}
let tmp = heap[k]
heap[k] = heap[m]
heap[m] = tmp
k = m
}
Some(out)
}
let visited : Array[Bool] = []
for _ in 0..<(cells_x * cells_z) {
visited.push(false)
}
let (bbmin0, bbmax0) = cell_aabb(
vertices, ncols, origin_x, origin_z, dx, dz, start_i, start_j,
)
heap_push(heap, (point_aabb_dist2(p_local, bbmin0, bbmax0), start_i, start_j))
let mut best_dist2 = 1.0e30F
let mut best_point_local = @core.Vec3::zero()
let mut best_normal_sum = @core.Vec3::zero()
let mut best_count = 0
let fix_internal_edges = (flags & 1) != 0
let eps_d2 = 1.0e-6F
let eps_q2 = 1.0e-8F
while heap_pop_min(heap) is Some((lb, i, j)) {
if lb > best_dist2 {
break
}
let id = cell_id(i, j, cells_z)
if visited[id] {
continue
}
visited[id] = true
// Test the two triangles of this cell in local-space.
let a = vertices[i * ncols + j]
let b = vertices[(i + 1) * ncols + j]
let c = vertices[(i + 1) * ncols + (j + 1)]
let d = vertices[i * ncols + (j + 1)]
fn update_best_for_triangle(
p_local : @core.Vec3,
a : @core.Vec3,
b : @core.Vec3,
c : @core.Vec3,
best_dist2 : @core.Real,
best_point : @core.Vec3,
best_normal_sum : @core.Vec3,
best_count : Int,
fix_internal_edges : Bool,
eps_d2 : @core.Real,
eps_q2 : @core.Real,
) -> (@core.Real, @core.Vec3, @core.Vec3, Int) {
let q = closest_point_on_triangle3d(p_local, a, b, c)
let d2 = q.sub(p_local).length_squared()
let n = b.sub(a).cross(c.sub(a))
let n_norm = if n.length_squared() > 1.0e-12F {
n.normalize()
} else {
Vec3(0.0F, 1.0F, 0.0F)
}
if d2 + eps_d2 < best_dist2 {
(d2, q, n_norm, 1)
} else if @core.abs(d2 - best_dist2) <= eps_d2 &&
q.sub(best_point).length_squared() <= eps_q2 {
if fix_internal_edges {
(best_dist2, best_point, best_normal_sum.add(n_norm), best_count + 1)
} else {
(best_dist2, best_point, best_normal_sum, best_count)
}
} else {
(best_dist2, best_point, best_normal_sum, best_count)
}
}
let (d2_0, q0, ns0, c0) = update_best_for_triangle(
p_local, a, c, b, best_dist2, best_point_local, best_normal_sum, best_count,
fix_internal_edges, eps_d2, eps_q2,
)
best_dist2 = d2_0
best_point_local = q0
best_normal_sum = ns0
best_count = c0
let (d2_1, q1, ns1, c1) = update_best_for_triangle(
p_local, a, d, c, best_dist2, best_point_local, best_normal_sum, best_count,
fix_internal_edges, eps_d2, eps_q2,
)
best_dist2 = d2_1
best_point_local = q1
best_normal_sum = ns1
best_count = c1
// Enqueue neighbors (4-neighborhood).
if i > 0 {
let ni = i - 1
let nj = j
let (bbmin, bbmax) = cell_aabb(
vertices, ncols, origin_x, origin_z, dx, dz, ni, nj,
)
heap_push(heap, (point_aabb_dist2(p_local, bbmin, bbmax), ni, nj))
}
if i + 1 < cells_x {
let ni = i + 1
let nj = j
let (bbmin, bbmax) = cell_aabb(
vertices, ncols, origin_x, origin_z, dx, dz, ni, nj,
)
heap_push(heap, (point_aabb_dist2(p_local, bbmin, bbmax), ni, nj))
}
if j > 0 {
let ni = i
let nj = j - 1
let (bbmin, bbmax) = cell_aabb(
vertices, ncols, origin_x, origin_z, dx, dz, ni, nj,
)
heap_push(heap, (point_aabb_dist2(p_local, bbmin, bbmax), ni, nj))
}
if j + 1 < cells_z {
let ni = i
let nj = j + 1
let (bbmin, bbmax) = cell_aabb(
vertices, ncols, origin_x, origin_z, dx, dz, ni, nj,
)
heap_push(heap, (point_aabb_dist2(p_local, bbmin, bbmax), ni, nj))
}
}
if best_count == 0 {
return (@core.Vec3::zero(), Vec3(0.0F, 1.0F, 0.0F), 0.0F, false)
}
let best_normal_local = if best_normal_sum.length_squared() > 1.0e-12F {
best_normal_sum.normalize()
} else {
Vec3(0.0F, 1.0F, 0.0F)
}
let best_point = mesh_pos.transform_point(best_point_local)
let best_normal = mesh_pos.rotation.rotate_vec3(best_normal_local)
(best_point, best_normal, best_dist2, true)
}
///|
/// Fast heightfield closest-point query for cases where we only care about hits within
/// a known maximum distance (typical for contact generation). This avoids allocating
/// a visited map / priority queue and instead scans only the cells whose xz-range
/// could contain a closest point within `max_dist`.
fn heightfield_closest_point_bounded(
p : @core.Vec3,
mesh_pos : @core.Isometry3,
vertices : Array[@core.Vec3],
nrows : Int,
ncols : Int,
flags : Int,
max_dist : @core.Real,
) -> (@core.Vec3, @core.Vec3, @core.Real, Bool) {
if nrows <= 1 || ncols <= 1 || vertices.length() != nrows * ncols {
return (@core.Vec3::zero(), Vec3(0.0F, 1.0F, 0.0F), 0.0F, false)
}
let inv = mesh_pos.inverse()
let p_local = inv.transform_point(p)
let origin_x = vertices[0].x
let origin_z = vertices[0].z
let dx0 = vertices[ncols].x - origin_x
let dz0 = vertices[1].z - origin_z
let dx = if dx0 < 0.0F { -dx0 } else { dx0 }
let dz = if dz0 < 0.0F { -dz0 } else { dz0 }
if dx <= 1.0e-12F || dz <= 1.0e-12F {
return (@core.Vec3::zero(), Vec3(0.0F, 1.0F, 0.0F), 0.0F, false)
}
let cells_x = nrows - 1
let cells_z = ncols - 1
fn floor_to_int(x : @core.Real) -> Int {
let i = x.to_int()
if Float::from_int(i) > x {
i - 1
} else {
i
}
}
fn clamp_int(v : Int, lo : Int, hi : Int) -> Int {
if v < lo {
lo
} else if v > hi {
hi
} else {
v
}
}
// Any point within `max_dist` implies |dx| <= max_dist and |dz| <= max_dist.
let r = if max_dist < 0.0F { 0.0F } else { max_dist }
let min_i = clamp_int(
floor_to_int((p_local.x - r - origin_x) / dx),
0,
cells_x - 1,
)
let max_i = clamp_int(
floor_to_int((p_local.x + r - origin_x) / dx),
0,
cells_x - 1,
)
let min_j = clamp_int(
floor_to_int((p_local.z - r - origin_z) / dz),
0,
cells_z - 1,
)
let max_j = clamp_int(
floor_to_int((p_local.z + r - origin_z) / dz),
0,
cells_z - 1,
)
let mut best_dist2 = 1.0e30F
let mut best_point_local = @core.Vec3::zero()
let mut best_normal_sum = @core.Vec3::zero()
let mut best_count = 0
let fix_internal_edges = (flags & 1) != 0
let eps_d2 = 1.0e-6F
let eps_q2 = 1.0e-8F
for i in min_i..<(max_i + 1) {
for j in min_j..<(max_j + 1) {
let a = vertices[i * ncols + j]
let b = vertices[(i + 1) * ncols + j]
let c = vertices[(i + 1) * ncols + (j + 1)]
let d = vertices[i * ncols + (j + 1)]
fn update_best_for_triangle(
p_local : @core.Vec3,
a : @core.Vec3,
b : @core.Vec3,
c : @core.Vec3,
best_dist2 : @core.Real,
best_point : @core.Vec3,
best_normal_sum : @core.Vec3,
best_count : Int,
fix_internal_edges : Bool,
eps_d2 : @core.Real,
eps_q2 : @core.Real,
) -> (@core.Real, @core.Vec3, @core.Vec3, Int) {
let q = closest_point_on_triangle3d(p_local, a, b, c)
let d2 = q.sub(p_local).length_squared()
let n = b.sub(a).cross(c.sub(a))
let n_norm = if n.length_squared() > 1.0e-12F {
n.normalize()
} else {
Vec3(0.0F, 1.0F, 0.0F)
}
if d2 + eps_d2 < best_dist2 {
(d2, q, n_norm, 1)
} else if @core.abs(d2 - best_dist2) <= eps_d2 &&
q.sub(best_point).length_squared() <= eps_q2 {
if fix_internal_edges {
(
best_dist2,
best_point,
best_normal_sum.add(n_norm),
best_count + 1,
)
} else {
(best_dist2, best_point, best_normal_sum, best_count)
}
} else {
(best_dist2, best_point, best_normal_sum, best_count)
}
}
let (d2_0, q0, ns0, c0) = update_best_for_triangle(
p_local, a, c, b, best_dist2, best_point_local, best_normal_sum, best_count,
fix_internal_edges, eps_d2, eps_q2,
)
best_dist2 = d2_0
best_point_local = q0
best_normal_sum = ns0
best_count = c0
let (d2_1, q1, ns1, c1) = update_best_for_triangle(
p_local, a, d, c, best_dist2, best_point_local, best_normal_sum, best_count,
fix_internal_edges, eps_d2, eps_q2,
)
best_dist2 = d2_1
best_point_local = q1
best_normal_sum = ns1
best_count = c1
}
}
if best_count == 0 {
return (@core.Vec3::zero(), Vec3(0.0F, 1.0F, 0.0F), 0.0F, false)
}
let best_normal_local = if best_normal_sum.length_squared() > 1.0e-12F {
best_normal_sum.normalize()
} else {
Vec3(0.0F, 1.0F, 0.0F)
}
(
mesh_pos.transform_point(best_point_local),
mesh_pos.rotation.rotate_vec3(best_normal_local),
best_dist2,
true,
)
}
///|
/// Returns the closest point on a triangle mesh to `p` (in world-space), along with the triangle
/// normal (world-space) and the squared distance.
///
/// This is a building block for query pipeline operations (point projection, swept-sphere
/// conservative advancement against meshes).
pub fn trimesh_closest_point_query(
p : @core.Vec3,
mesh_pos : @core.Isometry3,
vertices : Array[@core.Vec3],
indices : Array[(Int, Int, Int)],
) -> (@core.Vec3, @core.Vec3, @core.Real)? {
let (best_point, best_normal, best_dist2, ok) = trimesh_closest_point(
p, mesh_pos, vertices, indices,
)
if ok {
Some((best_point, best_normal, best_dist2))
} else {
None
}
}
///|
/// Heightfield variant of `trimesh_closest_point_query`.
pub fn heightfield_closest_point_query(
p : @core.Vec3,
mesh_pos : @core.Isometry3,
vertices : Array[@core.Vec3],
nrows : Int,
ncols : Int,
) -> (@core.Vec3, @core.Vec3, @core.Real)? {
let (best_point, best_normal, best_dist2, ok) = heightfield_closest_point(
p, mesh_pos, vertices, nrows, ncols, 0,
)
if ok {
Some((best_point, best_normal, best_dist2))
} else {
None
}
}
///|
/// Heightfield variant of `trimesh_closest_point_query` with explicit HeightFieldFlags bits.
pub fn heightfield_closest_point_query_with_flags(
p : @core.Vec3,
mesh_pos : @core.Isometry3,
vertices : Array[@core.Vec3],
nrows : Int,
ncols : Int,
flags : Int,
) -> (@core.Vec3, @core.Vec3, @core.Real)? {
let (best_point, best_normal, best_dist2, ok) = heightfield_closest_point(
p, mesh_pos, vertices, nrows, ncols, flags,
)
if ok {
Some((best_point, best_normal, best_dist2))
} else {
None
}
}
///|
fn voxels_floor_div_to_int(value : @core.Real, denom : @core.Real) -> Int {
if denom == 0.0F {
0
} else {
@math.floor((value / denom).to_double()).to_int()
}
}
///|
fn voxels_scan_range_local(
voxels : Voxels3DReal,
center_local : @core.Vec3,
radius : @core.Real,
) -> (Int, Int, Int, Int, Int, Int) {
let sx = voxels.voxel_size.x
let sy = voxels.voxel_size.y
let sz = voxels.voxel_size.z
let x0 = center_local.x - radius
let x1 = center_local.x + radius
let y0 = center_local.y - radius
let y1 = center_local.y + radius
let z0 = center_local.z - radius
let z1 = center_local.z + radius
(
voxels_floor_div_to_int(x0, sx),
voxels_floor_div_to_int(x1, sx),
voxels_floor_div_to_int(y0, sy),
voxels_floor_div_to_int(y1, sy),
voxels_floor_div_to_int(z0, sz),
voxels_floor_div_to_int(z1, sz),
)
}
///|
fn voxel_cell_aabb_local(
voxels : Voxels3DReal,
key : (Int, Int, Int),
) -> (@core.Vec3, @core.Vec3) {
let sx = voxels.voxel_size.x
let sy = voxels.voxel_size.y
let sz = voxels.voxel_size.z
let mins = @core.Vec3(
Float::from_int(key.0) * sx,
Float::from_int(key.1) * sy,
Float::from_int(key.2) * sz,
)
let maxs = mins.add(Vec3(sx, sy, sz))
(mins, maxs)
}
///|
fn compute_sphere_voxel_contact_local(
center_world : @core.Vec3,
center_local : @core.Vec3,
radius : @core.Real,
voxels_pos : @core.Isometry3,
voxels : Voxels3DReal,
key : (Int, Int, Int),
) -> ContactPoint3D? {
let (mins, maxs) = voxel_cell_aabb_local(voxels, key)
let clamped = @core.Vec3(
@core.clamp(center_local.x, mins.x, maxs.x),
@core.clamp(center_local.y, mins.y, maxs.y),
@core.clamp(center_local.z, mins.z, maxs.z),
)
let delta = center_local.sub(clamped)
let dist2 = delta.length_squared()
let r2 = radius * radius
if dist2 > r2 {
return None
}
let dist = local_sqrt(dist2)
let (normal_local, point2_local, penetration) = if dist > 1.0e-6F {
(delta.scale(-1.0F / dist), clamped, radius - dist)
} else {
let dx0 = center_local.x - mins.x
let dx1 = maxs.x - center_local.x
let dy0 = center_local.y - mins.y
let dy1 = maxs.y - center_local.y
let dz0 = center_local.z - mins.z
let dz1 = maxs.z - center_local.z
let mut best_d = dx0
let mut normal = @core.Vec3(-1.0F, 0.0F, 0.0F)
let mut point2 = @core.Vec3(mins.x, center_local.y, center_local.z)
if dx1 < best_d {
best_d = dx1
normal = Vec3(1.0F, 0.0F, 0.0F)
point2 = Vec3(maxs.x, center_local.y, center_local.z)
}
if dy0 < best_d {
best_d = dy0
normal = Vec3(0.0F, -1.0F, 0.0F)
point2 = Vec3(center_local.x, mins.y, center_local.z)
}
if dy1 < best_d {
best_d = dy1
normal = Vec3(0.0F, 1.0F, 0.0F)
point2 = Vec3(center_local.x, maxs.y, center_local.z)
}
if dz0 < best_d {
best_d = dz0
normal = Vec3(0.0F, 0.0F, -1.0F)
point2 = Vec3(center_local.x, center_local.y, mins.z)
}
if dz1 < best_d {
best_d = dz1
normal = Vec3(0.0F, 0.0F, 1.0F)
point2 = Vec3(center_local.x, center_local.y, maxs.z)
}
(normal, point2, radius + best_d)
}
let normal_world = voxels_pos.rotation.rotate_vec3(normal_local)
let point2_world = voxels_pos.transform_point(point2_local)
let point1_world = center_world.add(normal_world.scale(radius))
Some({
point1: point1_world,
point2: point2_world,
normal: normal_world,
penetration,
})
}
///|
fn compute_sphere_voxels_contact(
center_world : @core.Vec3,
center_local : @core.Vec3,
radius : @core.Real,
voxels_pos : @core.Isometry3,
voxels : Voxels3DReal,
) -> ContactPoint3D? {
let (ix0, ix1, iy0, iy1, iz0, iz1) = voxels_scan_range_local(
voxels, center_local, radius,
)
let mut best : ContactPoint3D? = None
let mut best_pen = -1.0F
for ix in ix0..<(ix1 + 1) {
for iz in iz0..<(iz1 + 1) {
if voxels.column_ys(ix, iz) is Some(ys) {
let mut start = 0
while start < ys.length() && ys[start] < iy0 {
start = start + 1
}
for i in start.. iy1 {
break
}
let key = (ix, iy, iz)
if compute_sphere_voxel_contact_local(
center_world, center_local, radius, voxels_pos, voxels, key,
)
is Some(cp) {
if cp.penetration > best_pen {
best_pen = cp.penetration
best = Some(cp)
}
}
}
}
}
}
best
}
///|
pub fn compute_ball_voxels_contact(
ball_center : @core.Vec3,
ball_radius : @core.Real,
voxels_pos : @core.Isometry3,
voxels : Voxels3DReal,
) -> ContactPoint3D? {
let inv = voxels_pos.inverse()
let center_local = inv.transform_point(ball_center)
compute_sphere_voxels_contact(
ball_center, center_local, ball_radius, voxels_pos, voxels,
)
}
///|
pub fn compute_cuboid_voxels_contact(
cuboid_pos : @core.Isometry3,
cuboid_half_extents : @core.Vec3,
voxels_pos : @core.Isometry3,
voxels : Voxels3DReal,
) -> ContactPoint3D? {
let inv = voxels_pos.inverse()
let mut best : ContactPoint3D? = None
let mut best_pen = -1.0F
let hx = cuboid_half_extents.x
let hy = cuboid_half_extents.y
let hz = cuboid_half_extents.z
let skin = 0.05F
let samples = [
@core.Vec3(-hx, -hy, -hz),
Vec3(-hx, -hy, hz),
Vec3(hx, -hy, -hz),
Vec3(hx, -hy, hz),
Vec3(0.0F, -hy, 0.0F),
]
for i in 0.. best_pen {
best_pen = cp.penetration
best = Some(cp)
}
}
}
best
}
///|
pub fn compute_capsule_voxels_contact(
capsule_pos : @core.Isometry3,
capsule_radius : @core.Real,
capsule_half_height : @core.Real,
voxels_pos : @core.Isometry3,
voxels : Voxels3DReal,
) -> ContactPoint3D? {
let inv = voxels_pos.inverse()
let mut best : ContactPoint3D? = None
let mut best_pen = -1.0F
let hh = capsule_half_height
let centers_local = [
@core.Vec3(0.0F, -hh, 0.0F),
Vec3(0.0F, -hh * 0.5F, 0.0F),
Vec3(0.0F, 0.0F, 0.0F),
Vec3(0.0F, hh * 0.5F, 0.0F),
Vec3(0.0F, hh, 0.0F),
]
for i in 0.. best_pen {
best_pen = cp.penetration
best = Some(cp)
}
}
}
best
}
///|
pub fn compute_cylinder_voxels_contact(
cyl_pos : @core.Isometry3,
cyl_radius : @core.Real,
cyl_half_height : @core.Real,
voxels_pos : @core.Isometry3,
voxels : Voxels3DReal,
) -> ContactPoint3D? {
let inv = voxels_pos.inverse()
let mut best : ContactPoint3D? = None
let mut best_pen = -1.0F
let skin = 0.05F
let hh = cyl_half_height
let r = cyl_radius
let a = 0.70710677F
let ring_dirs = [
@core.Vec3(1.0F, 0.0F, 0.0F),
Vec3(-1.0F, 0.0F, 0.0F),
Vec3(0.0F, 0.0F, 1.0F),
Vec3(0.0F, 0.0F, -1.0F),
Vec3(a, 0.0F, a),
Vec3(a, 0.0F, -a),
Vec3(-a, 0.0F, a),
Vec3(-a, 0.0F, -a),
]
// Bottom cap ring + center.
for i in 0.. best_pen {
best_pen = cp.penetration
best = Some(cp)
}
}
}
let pw0 = cyl_pos.transform_point(Vec3(0.0F, -hh, 0.0F))
let pl0 = inv.transform_point(pw0)
if compute_sphere_voxels_contact(pw0, pl0, skin, voxels_pos, voxels)
is Some(cp0) {
if cp0.penetration > best_pen {
best_pen = cp0.penetration
best = Some(cp0)
}
}
best
}
///|
pub fn compute_cone_voxels_contact(
cone_pos : @core.Isometry3,
cone_radius : @core.Real,
cone_half_height : @core.Real,
voxels_pos : @core.Isometry3,
voxels : Voxels3DReal,
) -> ContactPoint3D? {
let inv = voxels_pos.inverse()
let mut best : ContactPoint3D? = None
let mut best_pen = -1.0F
let skin = 0.05F
let hh = cone_half_height
let r = cone_radius
let a = 0.70710677F
let ring_dirs = [
@core.Vec3(1.0F, 0.0F, 0.0F),
Vec3(-1.0F, 0.0F, 0.0F),
Vec3(0.0F, 0.0F, 1.0F),
Vec3(0.0F, 0.0F, -1.0F),
Vec3(a, 0.0F, a),
Vec3(a, 0.0F, -a),
Vec3(-a, 0.0F, a),
Vec3(-a, 0.0F, -a),
]
// Base ring.
for i in 0.. best_pen {
best_pen = cp.penetration
best = Some(cp)
}
}
}
// Tip.
let pw_tip = cone_pos.transform_point(Vec3(0.0F, hh, 0.0F))
let pl_tip = inv.transform_point(pw_tip)
if compute_sphere_voxels_contact(pw_tip, pl_tip, skin, voxels_pos, voxels)
is Some(cp_tip) {
if cp_tip.penetration > best_pen {
best_pen = cp_tip.penetration
best = Some(cp_tip)
}
}
best
}
///|
pub fn compute_ball_trimesh_contact(
ball_center : @core.Vec3,
ball_radius : @core.Real,
mesh_pos : @core.Isometry3,
vertices : Array[@core.Vec3],
indices : Array[(Int, Int, Int)],
) -> ContactPoint3D? {
let (best_point, best_normal, best_dist2, ok) = trimesh_closest_point(
ball_center, mesh_pos, vertices, indices,
)
if !ok {
return None
}
let r2 = ball_radius * ball_radius
if best_dist2 > r2 {
return None
}
let dist = local_sqrt(best_dist2)
let normal = if dist > 1.0e-6F {
// Pointing from the ball center toward the mesh surface.
best_point.sub(ball_center).scale(1.0F / dist)
} else {
// Fallback to triangle normal.
best_normal
}
let point1 = ball_center.add(normal.scale(ball_radius))
let point2 = best_point
Some({ point1, point2, normal, penetration: ball_radius - dist })
}
///|
/// Cuboid-vs-triangle contact helper.
///
/// A triangle is treated as a surface (like a mesh triangle). This uses a standard
/// triangle-vs-OBB overlap test (SAT, Ericson 5.2.9) and produces a single contact point
/// suitable for the current solver.
pub fn compute_cuboid_triangle_contact(
cuboid_pos : @core.Isometry3,
cuboid_half_extents : @core.Vec3,
tri_pos : @core.Isometry3,
a_local : @core.Vec3,
b_local : @core.Vec3,
c_local : @core.Vec3,
) -> ContactPoint3D? {
// Triangle vertices in world-space.
let wa = tri_pos.transform_point(a_local)
let wb = tri_pos.transform_point(b_local)
let wc = tri_pos.transform_point(c_local)
// Transform triangle vertices into the cuboid local-space (OBB space).
let inv_box = cuboid_pos.inverse()
let a = inv_box.transform_point(wa)
let b = inv_box.transform_point(wb)
let c = inv_box.transform_point(wc)
let v0 = a
let v1 = b
let v2 = c
let e0 = v1.sub(v0)
let e1 = v2.sub(v1)
let e2 = v0.sub(v2)
let centroid = v0.add(v1).add(v2).scale(1.0F / 3.0F)
let eps = 1.0e-12F
let mut best_overlap = 1.0e30F
let mut best_axis = @core.Vec3(1.0F, 0.0F, 0.0F)
fn update_best(
axis : @core.Vec3,
overlap : @core.Real,
centroid : @core.Vec3,
best_overlap : @core.Real,
best_axis : @core.Vec3,
) -> (@core.Real, @core.Vec3) {
if overlap < best_overlap {
let signed = if axis.dot(centroid) >= 0.0F {
axis
} else {
axis.scale(-1.0F)
}
(overlap, signed)
} else {
(best_overlap, best_axis)
}
}
fn project_min_max(
axis : @core.Vec3,
v0 : @core.Vec3,
v1 : @core.Vec3,
v2 : @core.Vec3,
) -> (@core.Real, @core.Real) {
let p0 = axis.dot(v0)
let p1 = axis.dot(v1)
let p2 = axis.dot(v2)
let mut minp = p0
let mut maxp = p0
for p in [p1, p2] {
if p < minp {
minp = p
}
if p > maxp {
maxp = p
}
}
(minp, maxp)
}
fn overlap_box_interval(
minp : @core.Real,
maxp : @core.Real,
r : @core.Real,
) -> @core.Real? {
let lo = if -r > minp { -r } else { minp }
let hi = if r < maxp { r } else { maxp }
let o = hi - lo
if o < 0.0F {
None
} else {
Some(o)
}
}
fn box_radius_on_axis(he : @core.Vec3, axis : @core.Vec3) -> @core.Real {
he.x * @core.abs(axis.x) +
he.y * @core.abs(axis.y) +
he.z * @core.abs(axis.z)
}
// 1) Test the three box axes (x, y, z).
// Box interval is [-he, he] on each axis in box local-space.
{
let (minx, maxx) = (
local_min(v0.x, local_min(v1.x, v2.x)),
local_max(v0.x, local_max(v1.x, v2.x)),
)
if overlap_box_interval(minx, maxx, cuboid_half_extents.x) is Some(o) {
let (bo, ba) = update_best(
Vec3(1.0F, 0.0F, 0.0F),
o,
centroid,
best_overlap,
best_axis,
)
best_overlap = bo
best_axis = ba
} else {
return None
}
let (miny, maxy) = (
local_min(v0.y, local_min(v1.y, v2.y)),
local_max(v0.y, local_max(v1.y, v2.y)),
)
if overlap_box_interval(miny, maxy, cuboid_half_extents.y) is Some(o) {
let (bo, ba) = update_best(
Vec3(0.0F, 1.0F, 0.0F),
o,
centroid,
best_overlap,
best_axis,
)
best_overlap = bo
best_axis = ba
} else {
return None
}
let (minz, maxz) = (
local_min(v0.z, local_min(v1.z, v2.z)),
local_max(v0.z, local_max(v1.z, v2.z)),
)
if overlap_box_interval(minz, maxz, cuboid_half_extents.z) is Some(o) {
let (bo, ba) = update_best(
Vec3(0.0F, 0.0F, 1.0F),
o,
centroid,
best_overlap,
best_axis,
)
best_overlap = bo
best_axis = ba
} else {
return None
}
}
// 2) Test the triangle normal axis.
let n0 = e0.cross(v2.sub(v0))
let n2 = n0.length_squared()
if n2 > eps {
let inv = 1.0F / local_sqrt(n2)
let n = n0.scale(inv)
let (minp, maxp) = project_min_max(n, v0, v1, v2)
let r = box_radius_on_axis(cuboid_half_extents, n)
if overlap_box_interval(minp, maxp, r) is Some(o) {
let (bo, ba) = update_best(n, o, centroid, best_overlap, best_axis)
best_overlap = bo
best_axis = ba
} else {
return None
}
}
// 3) Test the 9 cross-product axes (triangle edges x box axes).
let edges = [e0, e1, e2]
let box_axes = [
@core.Vec3(1.0F, 0.0F, 0.0F),
Vec3(0.0F, 1.0F, 0.0F),
Vec3(0.0F, 0.0F, 1.0F),
]
for edge in edges {
for ax in box_axes {
let axis0 = edge.cross(ax)
let l2 = axis0.length_squared()
if l2 <= eps {
continue
}
let inv = 1.0F / local_sqrt(l2)
let axis = axis0.scale(inv)
let (minp, maxp) = project_min_max(axis, v0, v1, v2)
let r = box_radius_on_axis(cuboid_half_extents, axis)
if overlap_box_interval(minp, maxp, r) is Some(o) {
let (bo, ba) = update_best(axis, o, centroid, best_overlap, best_axis)
best_overlap = bo
best_axis = ba
} else {
return None
}
}
}
// Convert best axis to world-space.
let normal = cuboid_pos.rotation.rotate_vec3(best_axis)
fn local_clamp(
v : @core.Real,
lo : @core.Real,
hi : @core.Real,
) -> @core.Real {
if v < lo {
lo
} else if v > hi {
hi
} else {
v
}
}
// Choose a point on the cuboid face oriented along `best_axis`, but keep tangential
// coordinates near the triangle area under the box center. This avoids picking an arbitrary
// corner when a direction component is near zero, which can destabilize contact solving.
let dir_eps = 1.0e-6F
let tri_center_world = closest_point_on_triangle3d(
cuboid_pos.translation,
wa,
wb,
wc,
)
let tri_center_local = inv_box.transform_point(tri_center_world)
let sx = if best_axis.x > dir_eps {
cuboid_half_extents.x
} else if best_axis.x < -dir_eps {
-cuboid_half_extents.x
} else {
local_clamp(
tri_center_local.x,
-cuboid_half_extents.x,
cuboid_half_extents.x,
)
}
let sy = if best_axis.y > dir_eps {
cuboid_half_extents.y
} else if best_axis.y < -dir_eps {
-cuboid_half_extents.y
} else {
local_clamp(
tri_center_local.y,
-cuboid_half_extents.y,
cuboid_half_extents.y,
)
}
let sz = if best_axis.z > dir_eps {
cuboid_half_extents.z
} else if best_axis.z < -dir_eps {
-cuboid_half_extents.z
} else {
local_clamp(
tri_center_local.z,
-cuboid_half_extents.z,
cuboid_half_extents.z,
)
}
let point1 = cuboid_pos.transform_point(Vec3(sx, sy, sz))
let point2 = closest_point_on_triangle3d(point1, wa, wb, wc)
// Convert the witness points to a penetration depth along the chosen normal.
let pen = -normal.dot(point2.sub(point1))
let penetration = if pen < 0.0F { 0.0F } else { pen }
Some({ point1, point2, normal, penetration })
}
///|
/// Cuboid-vs-triangle contact manifold helper.
///
/// This clips the triangle by the cuboid OBB volume (in cuboid local-space) and generates
/// multiple contact points from the resulting intersection polygon.
pub fn compute_cuboid_triangle_contacts(
cuboid_pos : @core.Isometry3,
cuboid_half_extents : @core.Vec3,
tri_pos : @core.Isometry3,
a_local : @core.Vec3,
b_local : @core.Vec3,
c_local : @core.Vec3,
) -> Array[ContactPoint3D] {
// Triangle vertices in world-space.
let wa = tri_pos.transform_point(a_local)
let wb = tri_pos.transform_point(b_local)
let wc = tri_pos.transform_point(c_local)
let inv_box = cuboid_pos.inverse()
let v0 = inv_box.transform_point(wa)
let v1 = inv_box.transform_point(wb)
let v2 = inv_box.transform_point(wc)
let e0 = v1.sub(v0)
let n0 = e0.cross(v2.sub(v0))
let n2 = n0.length_squared()
let eps = 1.0e-12F
if n2 <= eps {
return []
}
let inv = 1.0F / local_sqrt(n2)
let mut normal_local = n0.scale(inv)
let dist = -normal_local.dot(v0)
if dist > 0.0F {
normal_local = normal_local.scale(-1.0F)
}
let normal = cuboid_pos.rotation.rotate_vec3(normal_local)
fn axis_value(v : @core.Vec3, axis : Int) -> @core.Real {
if axis == 0 {
v.x
} else if axis == 1 {
v.y
} else {
v.z
}
}
fn clip_poly(
poly : Array[@core.Vec3],
axis : Int,
sign : @core.Real,
limit : @core.Real,
) -> Array[@core.Vec3] {
let out : Array[@core.Vec3] = []
if poly.length() == 0 {
return out
}
let eps = 1.0e-6F
let mut prev = poly[poly.length() - 1]
let mut prev_in = sign * axis_value(prev, axis) <= limit + eps
for i in 0.. 1.0e-12F {
let t = (limit - pv) / denom
let inter = prev.add(curr.sub(prev).scale(t))
out.push(inter)
}
}
if curr_in {
out.push(curr)
}
prev = curr
prev_in = curr_in
}
out
}
let mut poly = [v0, v1, v2]
// x <= he.x and x >= -he.x.
poly = clip_poly(poly, 0, 1.0F, cuboid_half_extents.x)
poly = clip_poly(poly, 0, -1.0F, cuboid_half_extents.x)
// y <= he.y and y >= -he.y.
poly = clip_poly(poly, 1, 1.0F, cuboid_half_extents.y)
poly = clip_poly(poly, 1, -1.0F, cuboid_half_extents.y)
// z <= he.z and z >= -he.z.
poly = clip_poly(poly, 2, 1.0F, cuboid_half_extents.z)
poly = clip_poly(poly, 2, -1.0F, cuboid_half_extents.z)
if poly.length() == 0 {
return []
}
fn ray_box_exit(
origin : @core.Vec3,
dir : @core.Vec3,
he : @core.Vec3,
) -> @core.Vec3 {
let eps = 1.0e-12F
let mut t = 1.0e30F
if dir.x > eps {
let tx = (he.x - origin.x) / dir.x
if tx < t {
t = tx
}
} else if dir.x < -eps {
let tx = (-he.x - origin.x) / dir.x
if tx < t {
t = tx
}
}
if dir.y > eps {
let ty = (he.y - origin.y) / dir.y
if ty < t {
t = ty
}
} else if dir.y < -eps {
let ty = (-he.y - origin.y) / dir.y
if ty < t {
t = ty
}
}
if dir.z > eps {
let tz = (he.z - origin.z) / dir.z
if tz < t {
t = tz
}
} else if dir.z < -eps {
let tz = (-he.z - origin.z) / dir.z
if tz < t {
t = tz
}
}
if t == 1.0e30F {
origin
} else {
origin.add(dir.scale(t))
}
}
// Generate contacts from the clipped polygon vertices (dedup to avoid spurious duplicates).
let contacts : Array[ContactPoint3D] = []
let unique : Array[@core.Vec3] = []
let eps2 = 1.0e-10F
for i in 0.. ContactPoint3D? {
let mut best : ContactPoint3D? = None
let mut best_pen = -1.0F
for ti in 0.. best_pen {
best_pen = cp.penetration
best = Some(cp)
}
}
}
best
}
///|
pub fn compute_cuboid_trimesh_contact(
cuboid_pos : @core.Isometry3,
cuboid_half_extents : @core.Vec3,
mesh_pos : @core.Isometry3,
vertices : Array[@core.Vec3],
indices : Array[(Int, Int, Int)],
) -> ContactPoint3D? {
compute_convex_trimesh_contact(
cuboid_pos,
Cuboid(cuboid_half_extents),
mesh_pos,
vertices,
indices,
)
}
///|
pub fn compute_capsule_trimesh_contact(
capsule_pos : @core.Isometry3,
capsule_radius : @core.Real,
capsule_half_height : @core.Real,
mesh_pos : @core.Isometry3,
vertices : Array[@core.Vec3],
indices : Array[(Int, Int, Int)],
) -> ContactPoint3D? {
compute_convex_trimesh_contact(
capsule_pos,
CapsuleY(capsule_radius, capsule_half_height),
mesh_pos,
vertices,
indices,
)
}
///|
pub fn compute_cylinder_trimesh_contact(
cyl_pos : @core.Isometry3,
cyl_radius : @core.Real,
cyl_half_height : @core.Real,
mesh_pos : @core.Isometry3,
vertices : Array[@core.Vec3],
indices : Array[(Int, Int, Int)],
) -> ContactPoint3D? {
compute_convex_trimesh_contact(
cyl_pos,
Cylinder(cyl_radius, cyl_half_height),
mesh_pos,
vertices,
indices,
)
}
///|
pub fn compute_round_cylinder_trimesh_contact(
cyl_pos : @core.Isometry3,
cyl_radius : @core.Real,
cyl_half_height : @core.Real,
border_radius : @core.Real,
mesh_pos : @core.Isometry3,
vertices : Array[@core.Vec3],
indices : Array[(Int, Int, Int)],
) -> ContactPoint3D? {
compute_capsule_trimesh_contact(
cyl_pos,
cyl_radius + border_radius,
cyl_half_height + border_radius,
mesh_pos,
vertices,
indices,
)
}
///|
pub fn compute_cone_trimesh_contact(
cone_pos : @core.Isometry3,
cone_radius : @core.Real,
cone_half_height : @core.Real,
mesh_pos : @core.Isometry3,
vertices : Array[@core.Vec3],
indices : Array[(Int, Int, Int)],
) -> ContactPoint3D? {
compute_convex_trimesh_contact(
cone_pos,
Cone(cone_radius, cone_half_height),
mesh_pos,
vertices,
indices,
)
}
///|
fn compute_point_heightfield_contact_with_flags(
point1 : @core.Vec3,
radius : @core.Real,
heightfield_pos : @core.Isometry3,
vertices : Array[@core.Vec3],
nrows : Int,
ncols : Int,
flags : Int,
) -> ContactPoint3D? {
let (best_point, best_normal, best_dist2, ok) = heightfield_closest_point_bounded(
point1, heightfield_pos, vertices, nrows, ncols, flags, radius,
)
if !ok {
return None
}
let r2 = radius * radius
if best_dist2 > r2 {
return None
}
let dist = local_sqrt(best_dist2)
let normal0 = if dist > 1.0e-6F {
best_point.sub(point1).scale(1.0F / dist)
} else {
best_normal
}
// Keep the same terrain-oriented convention as trimeshes.
let normal = if normal0.y > 0.0F { normal0.scale(-1.0F) } else { normal0 }
Some({ point1, point2: best_point, normal, penetration: radius - dist })
}
///|
pub fn compute_ball_heightfield_contact(
ball_center : @core.Vec3,
ball_radius : @core.Real,
heightfield_pos : @core.Isometry3,
vertices : Array[@core.Vec3],
nrows : Int,
ncols : Int,
) -> ContactPoint3D? {
compute_ball_heightfield_contact_with_flags(
ball_center, ball_radius, heightfield_pos, vertices, nrows, ncols, 0,
)
}
///|
pub fn compute_ball_heightfield_contact_with_flags(
ball_center : @core.Vec3,
ball_radius : @core.Real,
heightfield_pos : @core.Isometry3,
vertices : Array[@core.Vec3],
nrows : Int,
ncols : Int,
flags : Int,
) -> ContactPoint3D? {
let (best_point, best_normal, best_dist2, ok) = heightfield_closest_point_bounded(
ball_center, heightfield_pos, vertices, nrows, ncols, flags, ball_radius,
)
if !ok {
return None
}
let r2 = ball_radius * ball_radius
if best_dist2 > r2 {
return None
}
let dist = local_sqrt(best_dist2)
let normal = if dist > 1.0e-6F {
best_point.sub(ball_center).scale(1.0F / dist)
} else {
best_normal
}
let point1 = ball_center.add(normal.scale(ball_radius))
let point2 = best_point
Some({ point1, point2, normal, penetration: ball_radius - dist })
}
///|
pub fn compute_cuboid_heightfield_contact(
cuboid_pos : @core.Isometry3,
cuboid_half_extents : @core.Vec3,
heightfield_pos : @core.Isometry3,
vertices : Array[@core.Vec3],
nrows : Int,
ncols : Int,
) -> ContactPoint3D? {
compute_cuboid_heightfield_contact_with_flags(
cuboid_pos, cuboid_half_extents, heightfield_pos, vertices, nrows, ncols, 0,
)
}
///|
pub fn compute_cuboid_heightfield_contact_with_flags(
cuboid_pos : @core.Isometry3,
cuboid_half_extents : @core.Vec3,
heightfield_pos : @core.Isometry3,
vertices : Array[@core.Vec3],
nrows : Int,
ncols : Int,
flags : Int,
) -> ContactPoint3D? {
let skin = 0.05F
let hx = cuboid_half_extents.x
let hy = cuboid_half_extents.y
let hz = cuboid_half_extents.z
let points = [
@core.Vec3(-hx, -hy, -hz),
Vec3(hx, -hy, -hz),
Vec3(-hx, hy, -hz),
Vec3(hx, hy, -hz),
Vec3(-hx, -hy, hz),
Vec3(hx, -hy, hz),
Vec3(-hx, hy, hz),
Vec3(hx, hy, hz),
Vec3(0.0F, -hy, 0.0F),
Vec3(0.0F, hy, 0.0F),
Vec3(-hx, 0.0F, 0.0F),
Vec3(hx, 0.0F, 0.0F),
Vec3(0.0F, 0.0F, -hz),
Vec3(0.0F, 0.0F, hz),
Vec3(0.0F, -hy, -hz),
Vec3(0.0F, -hy, hz),
Vec3(0.0F, hy, -hz),
Vec3(0.0F, hy, hz),
Vec3(-hx, 0.0F, -hz),
Vec3(-hx, 0.0F, hz),
Vec3(hx, 0.0F, -hz),
Vec3(hx, 0.0F, hz),
Vec3(-hx, -hy, 0.0F),
Vec3(hx, -hy, 0.0F),
Vec3(-hx, hy, 0.0F),
Vec3(hx, hy, 0.0F),
]
let mut best : ContactPoint3D? = None
let mut best_pen = -1.0F
for i in 0.. best_pen {
best_pen = cp.penetration
best = Some(cp)
}
}
}
best
}
///|
pub fn compute_capsule_heightfield_contact(
capsule_pos : @core.Isometry3,
capsule_radius : @core.Real,
capsule_half_height : @core.Real,
heightfield_pos : @core.Isometry3,
vertices : Array[@core.Vec3],
nrows : Int,
ncols : Int,
) -> ContactPoint3D? {
compute_capsule_heightfield_contact_with_flags(
capsule_pos, capsule_radius, capsule_half_height, heightfield_pos, vertices,
nrows, ncols, 0,
)
}
///|
pub fn compute_capsule_heightfield_contact_with_flags(
capsule_pos : @core.Isometry3,
capsule_radius : @core.Real,
capsule_half_height : @core.Real,
heightfield_pos : @core.Isometry3,
vertices : Array[@core.Vec3],
nrows : Int,
ncols : Int,
flags : Int,
) -> ContactPoint3D? {
let (a, b) = capsule_segment_endpoints(capsule_pos, capsule_half_height)
let centers = [
a,
b,
a.add(b).scale(0.5F),
a.scale(0.75F).add(b.scale(0.25F)),
a.scale(0.25F).add(b.scale(0.75F)),
]
let mut best : ContactPoint3D? = None
let mut best_pen = -1.0F
for i in 0.. best_pen {
best_pen = cp.penetration
best = Some(cp)
}
}
}
best
}
///|
pub fn compute_cylinder_heightfield_contact(
cyl_pos : @core.Isometry3,
cyl_radius : @core.Real,
cyl_half_height : @core.Real,
heightfield_pos : @core.Isometry3,
vertices : Array[@core.Vec3],
nrows : Int,
ncols : Int,
) -> ContactPoint3D? {
compute_cylinder_heightfield_contact_with_flags(
cyl_pos, cyl_radius, cyl_half_height, heightfield_pos, vertices, nrows, ncols,
0,
)
}
///|
pub fn compute_cylinder_heightfield_contact_with_flags(
cyl_pos : @core.Isometry3,
cyl_radius : @core.Real,
cyl_half_height : @core.Real,
heightfield_pos : @core.Isometry3,
vertices : Array[@core.Vec3],
nrows : Int,
ncols : Int,
flags : Int,
) -> ContactPoint3D? {
let skin = 0.05F
let r = cyl_radius
let h = cyl_half_height
let a = 0.70710677F
let ring = [
@core.Vec3(r, 0.0F, 0.0F),
Vec3(-r, 0.0F, 0.0F),
Vec3(0.0F, 0.0F, r),
Vec3(0.0F, 0.0F, -r),
Vec3(r * a, 0.0F, r * a),
Vec3(r * a, 0.0F, -r * a),
Vec3(-r * a, 0.0F, r * a),
Vec3(-r * a, 0.0F, -r * a),
]
let mut best : ContactPoint3D? = None
let mut best_pen = -1.0F
for i in 0.. best_pen {
best_pen = cp.penetration
best = Some(cp)
}
}
let p1 = cyl_pos.transform_point(ring[i].add(Vec3(0.0F, h, 0.0F)))
if compute_point_heightfield_contact_with_flags(
p1, skin, heightfield_pos, vertices, nrows, ncols, flags,
)
is Some(cp) {
if cp.penetration > best_pen {
best_pen = cp.penetration
best = Some(cp)
}
}
let pm = cyl_pos.transform_point(ring[i])
if compute_point_heightfield_contact_with_flags(
pm, skin, heightfield_pos, vertices, nrows, ncols, flags,
)
is Some(cp) {
if cp.penetration > best_pen {
best_pen = cp.penetration
best = Some(cp)
}
}
}
best
}
///|
pub fn compute_round_cylinder_heightfield_contact(
cyl_pos : @core.Isometry3,
cyl_radius : @core.Real,
cyl_half_height : @core.Real,
border_radius : @core.Real,
heightfield_pos : @core.Isometry3,
vertices : Array[@core.Vec3],
nrows : Int,
ncols : Int,
) -> ContactPoint3D? {
compute_round_cylinder_heightfield_contact_with_flags(
cyl_pos, cyl_radius, cyl_half_height, border_radius, heightfield_pos, vertices,
nrows, ncols, 0,
)
}
///|
pub fn compute_round_cylinder_heightfield_contact_with_flags(
cyl_pos : @core.Isometry3,
cyl_radius : @core.Real,
cyl_half_height : @core.Real,
border_radius : @core.Real,
heightfield_pos : @core.Isometry3,
vertices : Array[@core.Vec3],
nrows : Int,
ncols : Int,
flags : Int,
) -> ContactPoint3D? {
compute_capsule_heightfield_contact_with_flags(
cyl_pos,
cyl_radius + border_radius,
cyl_half_height + border_radius,
heightfield_pos,
vertices,
nrows,
ncols,
flags,
)
}
///|
pub fn compute_cone_heightfield_contact(
cone_pos : @core.Isometry3,
cone_radius : @core.Real,
cone_half_height : @core.Real,
heightfield_pos : @core.Isometry3,
vertices : Array[@core.Vec3],
nrows : Int,
ncols : Int,
) -> ContactPoint3D? {
compute_cone_heightfield_contact_with_flags(
cone_pos, cone_radius, cone_half_height, heightfield_pos, vertices, nrows, ncols,
0,
)
}
///|
pub fn compute_cone_heightfield_contact_with_flags(
cone_pos : @core.Isometry3,
cone_radius : @core.Real,
cone_half_height : @core.Real,
heightfield_pos : @core.Isometry3,
vertices : Array[@core.Vec3],
nrows : Int,
ncols : Int,
flags : Int,
) -> ContactPoint3D? {
let skin = 0.05F
let r = cone_radius
let hh = cone_half_height
let a = 0.70710677F
let ring_dirs = [
@core.Vec3(1.0F, 0.0F, 0.0F),
Vec3(-1.0F, 0.0F, 0.0F),
Vec3(0.0F, 0.0F, 1.0F),
Vec3(0.0F, 0.0F, -1.0F),
Vec3(a, 0.0F, a),
Vec3(a, 0.0F, -a),
Vec3(-a, 0.0F, a),
Vec3(-a, 0.0F, -a),
]
let slices = [1.0F, 0.66F, 0.33F]
let mut best : ContactPoint3D? = None
let mut best_pen = -1.0F
for s in slices {
let y = -hh + (1.0F - s) * (2.0F * hh)
let rr = r * s
for i in 0.. best_pen {
best_pen = cp.penetration
best = Some(cp)
}
}
}
}
best
}
///|
pub fn compute_halfspace_contact(
halfspace_pos : @core.Isometry3,
halfspace_normal_local : @core.Vec3,
other_pos : @core.Isometry3,
other_shape : Shape3D,
) -> ContactPoint3D? {
fn normalize_or_default(v : @core.Vec3) -> @core.Vec3 {
let n2 = v.length_squared()
if n2 <= 1.0e-12F {
Vec3(0.0F, 1.0F, 0.0F)
} else {
v.scale(1.0F / local_sqrt(n2))
}
}
// Support point of `shape` in world-space along `dir`.
fn support_world(
p : @core.Isometry3,
shape : Shape3D,
dir : @core.Vec3,
) -> @core.Vec3? {
match shape {
Ball(r) => Some(support_world_for_ball(p.translation, r, dir))
Cuboid(he) => Some(support_world_for_cuboid(p, he, dir))
CapsuleY(r, hh) => Some(support_world_for_capsule_y(p, r, hh, dir))
Cylinder(r, hh) => Some(support_world_for_cylinder_y(p, r, hh, dir))
RoundCylinder(r, hh, br) =>
Some(support_world_for_cylinder_y(p, r + br, hh + br, dir))
Cone(r, hh) => Some(support_world_for_cone_y(p, r, hh, dir))
Triangle(a, b, c) => {
let wa = p.transform_point(a)
let wb = p.transform_point(b)
let wc = p.transform_point(c)
let mut best = wa
let mut best_d = wa.dot(dir)
let db = wb.dot(dir)
if db > best_d {
best = wb
best_d = db
}
let dc = wc.dot(dir)
if dc > best_d {
best = wc
}
Some(best)
}
ConvexHull(points, br) =>
Some(support_world_for_convex(p, points, br, dir))
Compound(parts) => {
let mut best : @core.Vec3? = None
let mut best_d = -1.0e30F
for i in 0.. best_d {
best_d = d
best = Some(pt)
}
}
}
best
}
Voxels(v) => {
let (ix0, iy0, iz0) = v.min_key
let (ix1, iy1, iz1) = v.max_key
let sx = v.voxel_size.x
let sy = v.voxel_size.y
let sz = v.voxel_size.z
let local_dir = p.rotation.inverse().rotate_vec3(dir)
let x0 = Float::from_int(ix0) * sx
let y0 = Float::from_int(iy0) * sy
let z0 = Float::from_int(iz0) * sz
let x1 = Float::from_int(ix1 + 1) * sx
let y1 = Float::from_int(iy1 + 1) * sy
let z1 = Float::from_int(iz1 + 1) * sz
let corner = @core.Vec3(
if local_dir.x >= 0.0F {
x1
} else {
x0
},
if local_dir.y >= 0.0F {
y1
} else {
y0
},
if local_dir.z >= 0.0F {
z1
} else {
z0
},
)
Some(p.transform_point(corner))
}
TriMesh(vertices, _) =>
if vertices.length() == 0 {
None
} else {
let mut best = p.transform_point(vertices[0])
let mut best_d = best.dot(dir)
for i in 1.. best_d {
best_d = d
best = pt
}
}
Some(best)
}
Heightfield(vertices, _, _, _, _) =>
if vertices.length() == 0 {
None
} else {
let mut best = p.transform_point(vertices[0])
let mut best_d = best.dot(dir)
for i in 1.. best_d {
best_d = d
best = pt
}
}
Some(best)
}
HalfSpace(_) => None
}
}
let normal = normalize_or_default(
halfspace_pos.rotation.rotate_vec3(halfspace_normal_local),
)
// Choose the point with minimum projection on the plane normal.
if support_world(other_pos, other_shape, normal.scale(-1.0F)) is Some(p2) {
let plane_origin = halfspace_pos.translation
let dist = normal.dot(p2.sub(plane_origin))
if dist >= 0.0F {
return None
}
// Point on the plane directly "below" the witness point.
let p1 = p2.sub(normal.scale(dist))
Some({ point1: p1, point2: p2, normal, penetration: -dist })
} else {
None
}
}
///|
pub fn compute_ball_ball_contact(
center1 : @core.Vec3,
radius1 : @core.Real,
center2 : @core.Vec3,
radius2 : @core.Real,
) -> ContactPoint3D? {
let delta = center2.sub(center1)
let dist2 = delta.length_squared()
let r = radius1 + radius2
let r2 = r * r
if dist2 > r2 {
return None
}
let dist = local_sqrt(dist2)
let normal = if dist <= 1.0e-6F {
// Deterministic fallback to avoid NaNs.
@core.Vec3(1.0F, 0.0F, 0.0F)
} else {
delta.scale(1.0F / dist)
}
let point1 = center1.add(normal.scale(radius1))
let point2 = center2.sub(normal.scale(radius2))
Some({ point1, point2, normal, penetration: r - dist })
}
///|
pub fn compute_ball_cuboid_contact(
ball_center : @core.Vec3,
ball_radius : @core.Real,
cuboid_pos : @core.Isometry3,
cuboid_half_extents : @core.Vec3,
) -> ContactPoint3D? {
let inv = cuboid_pos.inverse()
let local_center = inv.transform_point(ball_center)
let he = cuboid_half_extents
let clamped = @core.Vec3(
@core.clamp(local_center.x, -he.x, he.x),
@core.clamp(local_center.y, -he.y, he.y),
@core.clamp(local_center.z, -he.z, he.z),
)
let delta = local_center.sub(clamped)
let dist2 = delta.length_squared()
if dist2 > ball_radius * ball_radius {
// Sphere center is outside the box and too far away.
// Note: when the center is inside, dist2 == 0 and this check doesn't trigger.
return None
}
let dist = local_sqrt(dist2)
let normal_local = if dist > 1.0e-6F {
// Outside: normal points from the sphere to the box (ball is shape1).
delta.scale(-1.0F / dist)
} else {
// Inside (or extremely close): pick the nearest face direction.
let dx = he.x - @core.abs(local_center.x)
let dy = he.y - @core.abs(local_center.y)
let dz = he.z - @core.abs(local_center.z)
if dx <= dy && dx <= dz {
Vec3(if local_center.x >= 0.0F { 1.0F } else { -1.0F }, 0.0F, 0.0F)
} else if dy <= dz {
Vec3(0.0F, if local_center.y >= 0.0F { 1.0F } else { -1.0F }, 0.0F)
} else {
Vec3(0.0F, 0.0F, if local_center.z >= 0.0F { 1.0F } else { -1.0F })
}
}
let normal = cuboid_pos.rotation.rotate_vec3(normal_local)
let point2_local = if dist > 1.0e-6F {
clamped
} else if @core.abs(normal_local.x) > 0.0F {
Vec3(normal_local.x * he.x, local_center.y, local_center.z)
} else if @core.abs(normal_local.y) > 0.0F {
Vec3(local_center.x, normal_local.y * he.y, local_center.z)
} else {
Vec3(local_center.x, local_center.y, normal_local.z * he.z)
}
let point2 = cuboid_pos.transform_point(point2_local)
let point1 = ball_center.add(normal.scale(ball_radius))
let penetration = if dist > 1.0e-6F {
ball_radius - dist
} else {
let dx = he.x - @core.abs(local_center.x)
let dy = he.y - @core.abs(local_center.y)
let dz = he.z - @core.abs(local_center.z)
let face_dist = if dx <= dy && dx <= dz {
dx
} else if dy <= dz {
dy
} else {
dz
}
ball_radius + face_dist
}
Some({ point1, point2, normal, penetration })
}
///|
pub fn compute_cylinder_cuboid_contact(
cyl_pos : @core.Isometry3,
cyl_radius : @core.Real,
cyl_half_height : @core.Real,
cuboid_pos : @core.Isometry3,
cuboid_half_extents : @core.Vec3,
) -> ContactPoint3D? {
compute_convex_contact(
cyl_pos,
Cylinder(cyl_radius, cyl_half_height),
cuboid_pos,
Cuboid(cuboid_half_extents),
)
}
///|
pub fn compute_cylinder_cylinder_contact(
cyl1_pos : @core.Isometry3,
cyl1_radius : @core.Real,
cyl1_half_height : @core.Real,
cyl2_pos : @core.Isometry3,
cyl2_radius : @core.Real,
cyl2_half_height : @core.Real,
) -> ContactPoint3D? {
compute_convex_contact(
cyl1_pos,
Cylinder(cyl1_radius, cyl1_half_height),
cyl2_pos,
Cylinder(cyl2_radius, cyl2_half_height),
)
}
///|
pub fn compute_cone_cylinder_contact(
cone_pos : @core.Isometry3,
cone_radius : @core.Real,
cone_half_height : @core.Real,
cyl_pos : @core.Isometry3,
cyl_radius : @core.Real,
cyl_half_height : @core.Real,
) -> ContactPoint3D? {
compute_convex_contact(
cone_pos,
Cone(cone_radius, cone_half_height),
cyl_pos,
Cylinder(cyl_radius, cyl_half_height),
)
}
///|
pub fn compute_cone_cone_contact(
cone1_pos : @core.Isometry3,
cone1_radius : @core.Real,
cone1_half_height : @core.Real,
cone2_pos : @core.Isometry3,
cone2_radius : @core.Real,
cone2_half_height : @core.Real,
) -> ContactPoint3D? {
compute_convex_contact(
cone1_pos,
Cone(cone1_radius, cone1_half_height),
cone2_pos,
Cone(cone2_radius, cone2_half_height),
)
}
///|
pub fn compute_cone_cuboid_contact(
cone_pos : @core.Isometry3,
cone_radius : @core.Real,
cone_half_height : @core.Real,
cuboid_pos : @core.Isometry3,
cuboid_half_extents : @core.Vec3,
) -> ContactPoint3D? {
compute_convex_contact(
cone_pos,
Cone(cone_radius, cone_half_height),
cuboid_pos,
Cuboid(cuboid_half_extents),
)
}
///|
fn closest_point_on_segment3d(
p : @core.Vec3,
a : @core.Vec3,
b : @core.Vec3,
) -> @core.Vec3 {
let ab = b.sub(a)
let denom = ab.length_squared()
if denom <= 1.0e-12F {
return a
}
let t = @core.clamp(p.sub(a).dot(ab) / denom, 0.0F, 1.0F)
a.add(ab.scale(t))
}
///|
fn capsule_segment_endpoints(
pos : @core.Isometry3,
half_height : @core.Real,
) -> (@core.Vec3, @core.Vec3) {
let a = pos.transform_point(Vec3(0.0F, -half_height, 0.0F))
let b = pos.transform_point(Vec3(0.0F, half_height, 0.0F))
(a, b)
}
///|
fn axis_interval(
p0 : @core.Real,
d : @core.Real,
he : @core.Real,
state : Int,
) -> (@core.Real, @core.Real, Bool) {
// state: 0=inside, 1=below, 2=above
let neg_inf = -1.0e30F
let pos_inf = 1.0e30F
if @core.abs(d) <= 1.0e-12F {
match state {
0 =>
if p0 >= -he && p0 <= he {
(neg_inf, pos_inf, true)
} else {
(0.0F, 0.0F, false)
}
1 =>
if p0 <= -he {
(neg_inf, pos_inf, true)
} else {
(0.0F, 0.0F, false)
}
_ => if p0 >= he { (neg_inf, pos_inf, true) } else { (0.0F, 0.0F, false) }
}
} else {
let t_neg = (-he - p0) / d
let t_pos = (he - p0) / d
let t_min = if t_neg < t_pos { t_neg } else { t_pos }
let t_max = if t_neg < t_pos { t_pos } else { t_neg }
match state {
0 => (t_min, t_max, true)
1 =>
if d > 0.0F {
(neg_inf, t_neg, true)
} else {
(t_neg, pos_inf, true)
}
_ =>
if d > 0.0F {
(t_pos, pos_inf, true)
} else {
(neg_inf, t_pos, true)
}
}
}
}
///|
fn segment_aabb_closest_points(
a : @core.Vec3,
b : @core.Vec3,
he : @core.Vec3,
) -> (@core.Vec3, @core.Vec3) {
let p0 = a
let d = b.sub(a)
let mut best_t = 0.0F
let mut best_dist2 = 1.0e30F
let mut best_p = a
let mut best_q = @core.Vec3(
@core.clamp(a.x, -he.x, he.x),
@core.clamp(a.y, -he.y, he.y),
@core.clamp(a.z, -he.z, he.z),
)
for sx in 0..<3 {
let (x_lo, x_hi, x_ok) = axis_interval(p0.x, d.x, he.x, sx)
if !x_ok {
continue
}
for sy in 0..<3 {
let (y_lo, y_hi, y_ok) = axis_interval(p0.y, d.y, he.y, sy)
if !y_ok {
continue
}
for sz in 0..<3 {
let (z_lo, z_hi, z_ok) = axis_interval(p0.z, d.z, he.z, sz)
if !z_ok {
continue
}
let lo = local_max(local_max(local_max(x_lo, y_lo), z_lo), 0.0F)
let hi = local_min(local_min(local_min(x_hi, y_hi), z_hi), 1.0F)
if lo > hi {
continue
}
let mut a2 = 0.0F
let mut b2 = 0.0F
let mut c2 = 0.0F
if sx != 0 {
let bound = if sx == 1 { -he.x } else { he.x }
let k0 = p0.x - bound
a2 = a2 + d.x * d.x
b2 = b2 + 2.0F * d.x * k0
c2 = c2 + k0 * k0
}
if sy != 0 {
let bound = if sy == 1 { -he.y } else { he.y }
let k0 = p0.y - bound
a2 = a2 + d.y * d.y
b2 = b2 + 2.0F * d.y * k0
c2 = c2 + k0 * k0
}
if sz != 0 {
let bound = if sz == 1 { -he.z } else { he.z }
let k0 = p0.z - bound
a2 = a2 + d.z * d.z
b2 = b2 + 2.0F * d.z * k0
c2 = c2 + k0 * k0
}
let mut t = lo
if a2 > 1.0e-12F {
t = @core.clamp(-b2 / (2.0F * a2), lo, hi)
} else if hi > lo {
let f_lo = b2 * lo + c2
let f_hi = b2 * hi + c2
t = if f_hi < f_lo { hi } else { lo }
}
let p = p0.add(d.scale(t))
let q = @core.Vec3(
@core.clamp(p.x, -he.x, he.x),
@core.clamp(p.y, -he.y, he.y),
@core.clamp(p.z, -he.z, he.z),
)
let dist2 = p.sub(q).length_squared()
if dist2 < best_dist2 - 1.0e-12F ||
(@core.abs(dist2 - best_dist2) <= 1.0e-12F && t < best_t) {
best_dist2 = dist2
best_t = t
best_p = p
best_q = q
}
}
}
}
(best_p, best_q)
}
///|
pub fn compute_ball_capsule_contact(
ball_center : @core.Vec3,
ball_radius : @core.Real,
capsule_pos : @core.Isometry3,
capsule_radius : @core.Real,
capsule_half_height : @core.Real,
) -> ContactPoint3D? {
let (a, b) = capsule_segment_endpoints(capsule_pos, capsule_half_height)
let axis_pt = closest_point_on_segment3d(ball_center, a, b)
let delta = axis_pt.sub(ball_center)
let dist2 = delta.length_squared()
let r = ball_radius + capsule_radius
if dist2 > r * r {
return None
}
let dist = local_sqrt(dist2)
let normal = if dist <= 1.0e-6F {
@core.Vec3(1.0F, 0.0F, 0.0F)
} else {
delta.scale(1.0F / dist)
}
let point1 = ball_center.add(normal.scale(ball_radius))
let point2 = axis_pt.sub(normal.scale(capsule_radius))
Some({ point1, point2, normal, penetration: r - dist })
}
///|
fn closest_points_segment_segment3d(
p1 : @core.Vec3,
q1 : @core.Vec3,
p2 : @core.Vec3,
q2 : @core.Vec3,
) -> (@core.Vec3, @core.Vec3) {
let d1 = q1.sub(p1)
let d2 = q2.sub(p2)
let r = p1.sub(p2)
let a = d1.dot(d1)
let e = d2.dot(d2)
let f = d2.dot(r)
let eps = 1.0e-12F
let mut s = 0.0F
let mut t = 0.0F
if a <= eps && e <= eps {
return (p1, p2)
}
if a <= eps {
s = 0.0F
t = @core.clamp(f / e, 0.0F, 1.0F)
} else {
let c = d1.dot(r)
if e <= eps {
t = 0.0F
s = @core.clamp(-c / a, 0.0F, 1.0F)
} else {
let b = d1.dot(d2)
let denom = a * e - b * b
if denom != 0.0F {
s = @core.clamp((b * f - c * e) / denom, 0.0F, 1.0F)
} else {
s = 0.0F
}
t = (b * s + f) / e
if t < 0.0F {
t = 0.0F
s = @core.clamp(-c / a, 0.0F, 1.0F)
} else if t > 1.0F {
t = 1.0F
s = @core.clamp((b - c) / a, 0.0F, 1.0F)
}
}
}
(p1.add(d1.scale(s)), p2.add(d2.scale(t)))
}
///|
pub fn compute_capsule_capsule_contact(
pos1 : @core.Isometry3,
r1 : @core.Real,
hh1 : @core.Real,
pos2 : @core.Isometry3,
r2 : @core.Real,
hh2 : @core.Real,
) -> ContactPoint3D? {
let (a1, b1) = capsule_segment_endpoints(pos1, hh1)
let (a2, b2) = capsule_segment_endpoints(pos2, hh2)
let (p1, p2) = closest_points_segment_segment3d(a1, b1, a2, b2)
let delta = p2.sub(p1)
let dist2 = delta.length_squared()
let r = r1 + r2
if dist2 > r * r {
return None
}
let dist = local_sqrt(dist2)
let normal = if dist <= 1.0e-6F {
@core.Vec3(1.0F, 0.0F, 0.0F)
} else {
delta.scale(1.0F / dist)
}
let point1 = p1.add(normal.scale(r1))
let point2 = p2.sub(normal.scale(r2))
Some({ point1, point2, normal, penetration: r - dist })
}
///|
pub fn compute_capsule_cuboid_contact(
capsule_pos : @core.Isometry3,
capsule_radius : @core.Real,
capsule_half_height : @core.Real,
cuboid_pos : @core.Isometry3,
cuboid_half_extents : @core.Vec3,
) -> ContactPoint3D? {
let (a_world, b_world) = capsule_segment_endpoints(
capsule_pos, capsule_half_height,
)
let inv = cuboid_pos.inverse()
let a_local = inv.transform_point(a_world)
let b_local = inv.transform_point(b_world)
let he = cuboid_half_extents
let (axis_local, box_local) = segment_aabb_closest_points(
a_local, b_local, he,
)
let delta = box_local.sub(axis_local)
let dist2 = delta.length_squared()
if dist2 > capsule_radius * capsule_radius {
return None
}
let dist = local_sqrt(dist2)
let mut normal_local = @core.Vec3::zero()
let mut point2_local = box_local
let penetration = if dist > 1.0e-6F {
normal_local = delta.scale(1.0F / dist)
capsule_radius - dist
} else {
// Segment passes through (or extremely close to) the box: pick a deterministic face normal.
let dx = he.x - @core.abs(axis_local.x)
let dy = he.y - @core.abs(axis_local.y)
let dz = he.z - @core.abs(axis_local.z)
if dx <= dy && dx <= dz {
normal_local = Vec3(
if axis_local.x >= 0.0F {
1.0F
} else {
-1.0F
},
0.0F,
0.0F,
)
point2_local = Vec3(normal_local.x * he.x, axis_local.y, axis_local.z)
capsule_radius + dx
} else if dy <= dz {
normal_local = Vec3(
0.0F,
if axis_local.y >= 0.0F {
1.0F
} else {
-1.0F
},
0.0F,
)
point2_local = Vec3(axis_local.x, normal_local.y * he.y, axis_local.z)
capsule_radius + dy
} else {
normal_local = Vec3(
0.0F,
0.0F,
if axis_local.z >= 0.0F {
1.0F
} else {
-1.0F
},
)
point2_local = Vec3(axis_local.x, axis_local.y, normal_local.z * he.z)
capsule_radius + dz
}
}
let normal = cuboid_pos.rotation.rotate_vec3(normal_local)
let axis_world = cuboid_pos.transform_point(axis_local)
let point2 = cuboid_pos.transform_point(point2_local)
let point1 = axis_world.add(normal.scale(capsule_radius))
Some({ point1, point2, normal, penetration })
}
///|
pub fn compute_ball_cylinder_contact(
ball_center : @core.Vec3,
ball_radius : @core.Real,
cyl_pos : @core.Isometry3,
cyl_radius : @core.Real,
cyl_half_height : @core.Real,
) -> ContactPoint3D? {
let inv = cyl_pos.inverse()
let local_center = inv.transform_point(ball_center)
let hh = cyl_half_height
let r = cyl_radius
let y = @core.clamp(local_center.y, -hh, hh)
let mut x = local_center.x
let mut z = local_center.z
let s2 = x * x + z * z
if s2 > 1.0e-12F {
let s = local_sqrt(s2)
if s > r {
let k = r / s
x = x * k
z = z * k
}
} else {
x = 0.0F
z = 0.0F
}
let point2_local = @core.Vec3(x, y, z)
let delta = local_center.sub(point2_local)
let dist2 = delta.length_squared()
if dist2 > ball_radius * ball_radius {
return None
}
let dist = local_sqrt(dist2)
let normal_local = if dist > 1.0e-6F {
delta.scale(-1.0F / dist)
} else {
// Inside: choose the nearest surface.
let radial = local_sqrt(s2)
let dr = r - radial
let dy = hh - @core.abs(local_center.y)
if dy <= dr {
Vec3(0.0F, if local_center.y >= 0.0F { 1.0F } else { -1.0F }, 0.0F)
} else if radial > 1.0e-6F {
Vec3(local_center.x / radial, 0.0F, local_center.z / radial)
} else {
Vec3(1.0F, 0.0F, 0.0F)
}
}
let normal = cyl_pos.rotation.rotate_vec3(normal_local)
let point2 = cyl_pos.transform_point(point2_local)
let point1 = ball_center.add(normal.scale(ball_radius))
let penetration = if dist > 1.0e-6F {
ball_radius - dist
} else {
let radial = local_sqrt(s2)
let dr = r - radial
let dy = hh - @core.abs(local_center.y)
let min_dist = if dy <= dr { dy } else { dr }
ball_radius + min_dist
}
Some({ point1, point2, normal, penetration })
}
///|
pub fn compute_ball_cone_contact(
ball_center : @core.Vec3,
ball_radius : @core.Real,
cone_pos : @core.Isometry3,
cone_radius : @core.Real,
cone_half_height : @core.Real,
) -> ContactPoint3D? {
let inv = cone_pos.inverse()
let local_center = inv.transform_point(ball_center)
let hh = cone_half_height
let r = cone_radius
let s2 = local_center.x * local_center.x + local_center.z * local_center.z
let s = local_sqrt(s2)
// Reduce to 2D in (s, y).
let ax = 0.0F
let ay = hh
let bx = r
let by = -hh
let px = s
let py = local_center.y
let abx = bx - ax
let aby = by - ay
let denom = abx * abx + aby * aby
let mut t = 0.0F
if denom > 1.0e-12F {
t = @core.clamp(((px - ax) * abx + (py - ay) * aby) / denom, 0.0F, 1.0F)
}
let qx = ax + abx * t
let qy = ay + aby * t
let base_x = if px > r { r } else { px }
let base_y = -hh
let dist2_lateral = (px - qx) * (px - qx) + (py - qy) * (py - qy)
let dist2_base = (px - base_x) * (px - base_x) + (py - base_y) * (py - base_y)
let use_base = dist2_base < dist2_lateral
let target_x = if use_base { base_x } else { qx }
let target_y = if use_base { base_y } else { qy }
// Map back to 3D.
let (tx, tz) = if s > 1.0e-6F {
let k = target_x / s
(local_center.x * k, local_center.z * k)
} else {
(target_x, 0.0F)
}
let point2_local = @core.Vec3(tx, target_y, tz)
let delta = local_center.sub(point2_local)
let dist2 = delta.length_squared()
if dist2 > ball_radius * ball_radius {
return None
}
let dist = local_sqrt(dist2)
let normal_local = if dist > 1.0e-6F {
delta.scale(-1.0F / dist)
} else {
// Inside: nearest between base and lateral surface (approx).
let dist_base = local_center.y + hh
let slope = r / (2.0F * hh)
let radius_y = (hh - local_center.y) * slope
let dist_side = radius_y - s
if dist_base <= dist_side {
Vec3(0.0F, -1.0F, 0.0F)
} else if s > 1.0e-6F {
@core.Vec3(local_center.x / s, slope, local_center.z / s).normalize()
} else {
Vec3(1.0F, 0.0F, 0.0F)
}
}
let normal = cone_pos.rotation.rotate_vec3(normal_local)
let point2 = cone_pos.transform_point(point2_local)
let point1 = ball_center.add(normal.scale(ball_radius))
let penetration = if dist > 1.0e-6F {
ball_radius - dist
} else {
let dist_base = local_center.y + hh
let slope = r / (2.0F * hh)
let radius_y = (hh - local_center.y) * slope
let dist_side = radius_y - s
let min_dist = if dist_base <= dist_side { dist_base } else { dist_side }
ball_radius + min_dist
}
Some({ point1, point2, normal, penetration })
}
///|
pub fn compute_cuboid_cuboid_contact(
pos1 : @core.Isometry3,
he1 : @core.Vec3,
pos2 : @core.Isometry3,
he2 : @core.Vec3,
) -> ContactPoint3D? {
let a = [he1.x, he1.y, he1.z]
let b = [he2.x, he2.y, he2.z]
let ra = pos1.rotation.to_mat3()
let rb = pos2.rotation.to_mat3()
fn col(m : @core.Mat3, i : Int) -> @core.Vec3 {
match i {
0 => Vec3(m.m00, m.m10, m.m20)
1 => Vec3(m.m01, m.m11, m.m21)
_ => Vec3(m.m02, m.m12, m.m22)
}
}
let a_axis = [col(ra, 0), col(ra, 1), col(ra, 2)]
let b_axis = [col(rb, 0), col(rb, 1), col(rb, 2)]
let d = pos2.translation.sub(pos1.translation)
let t = [d.dot(a_axis[0]), d.dot(a_axis[1]), d.dot(a_axis[2])]
// R = A^T B (in A's frame).
let r : Array[@core.Real] = []
let abs_r : Array[@core.Real] = []
let eps = 1.0e-6F
for i in 0..<3 {
for j in 0..<3 {
let rij = a_axis[i].dot(b_axis[j])
r.push(rij)
abs_r.push(@core.abs(rij) + eps)
}
}
fn r_at(r : Array[@core.Real], i : Int, j : Int) -> @core.Real {
r[i * 3 + j]
}
fn abs_r_at(r : Array[@core.Real], i : Int, j : Int) -> @core.Real {
r[i * 3 + j]
}
// Track the best (minimum) penetration axis.
let mut best = CuboidBestAxis::A(0)
let mut min_overlap = 1.0e30F
let min_extent1 = if a[0] < a[1] {
if a[0] < a[2] {
a[0]
} else {
a[2]
}
} else if a[1] < a[2] {
a[1]
} else {
a[2]
}
let min_extent2 = if b[0] < b[1] {
if b[0] < b[2] {
b[0]
} else {
b[2]
}
} else if b[1] < b[2] {
b[1]
} else {
b[2]
}
let edge_axis_bias = (if min_extent1 < min_extent2 {
min_extent1
} else {
min_extent2
}) *
0.05F
// Bias edge-edge axes slightly so face manifolds remain stable for nearly face-on stacks.
// Rapier/Parry keep persistent manifolds across frames; this SAT-only fallback needs a small
// preference for face axes to avoid degenerating into a single edge contact on tiny tilts.
// Test L = A_i.
for i in 0..<3 {
let ra_i = a[i]
let rb_i = b[0] * abs_r_at(abs_r, i, 0) +
b[1] * abs_r_at(abs_r, i, 1) +
b[2] * abs_r_at(abs_r, i, 2)
let dist = @core.abs(t[i])
let overlap = ra_i + rb_i - dist
if overlap < 0.0F {
return None
}
if overlap < min_overlap {
min_overlap = overlap
best = A(i)
}
}
// Test L = B_j.
for j in 0..<3 {
let ra_j = a[0] * abs_r_at(abs_r, 0, j) +
a[1] * abs_r_at(abs_r, 1, j) +
a[2] * abs_r_at(abs_r, 2, j)
let rb_j = b[j]
let proj = t[0] * r_at(r, 0, j) +
t[1] * r_at(r, 1, j) +
t[2] * r_at(r, 2, j)
let dist = @core.abs(proj)
let overlap = ra_j + rb_j - dist
if overlap < 0.0F {
return None
}
if overlap < min_overlap {
min_overlap = overlap
best = B(j)
}
}
// Test L = A_i x B_j.
for i in 0..<3 {
let i1 = (i + 1) % 3
let i2 = (i + 2) % 3
for j in 0..<3 {
let j1 = (j + 1) % 3
let j2 = (j + 2) % 3
let ra_c = a[i1] * abs_r_at(abs_r, i2, j) + a[i2] * abs_r_at(abs_r, i1, j)
let rb_c = b[j1] * abs_r_at(abs_r, i, j2) + b[j2] * abs_r_at(abs_r, i, j1)
let dist = @core.abs(t[i2] * r_at(r, i1, j) - t[i1] * r_at(r, i2, j))
let overlap = ra_c + rb_c - dist
if overlap < 0.0F {
return None
}
// Skip near-parallel axes when choosing the best axis.
let axis = a_axis[i].cross(b_axis[j])
if axis.length_squared() > 1.0e-12F {
if overlap + edge_axis_bias < min_overlap {
min_overlap = overlap
best = Cross(i, j)
}
}
}
}
// Contact normal pointing from box1 to box2.
let mut normal = match best {
A(i) => {
let s = if t[i] < 0.0F { -1.0F } else { 1.0F }
a_axis[i].scale(s)
}
B(j) => {
let proj = t[0] * r_at(r, 0, j) +
t[1] * r_at(r, 1, j) +
t[2] * r_at(r, 2, j)
let s = if proj < 0.0F { -1.0F } else { 1.0F }
b_axis[j].scale(s)
}
Cross(i, j) => {
let axis = a_axis[i].cross(b_axis[j])
let len = local_sqrt(axis.length_squared())
if len <= 1.0e-6F {
Vec3(1.0F, 0.0F, 0.0F)
} else {
axis.scale(1.0F / len)
}
}
}
if normal.dot(d) < 0.0F {
normal = normal.scale(-1.0F)
}
fn support(
pos : @core.Isometry3,
he : @core.Vec3,
dir : @core.Vec3,
) -> @core.Vec3 {
let local_dir = pos.rotation.inverse().rotate_vec3(dir)
let sx = if local_dir.x >= 0.0F { he.x } else { -he.x }
let sy = if local_dir.y >= 0.0F { he.y } else { -he.y }
let sz = if local_dir.z >= 0.0F { he.z } else { -he.z }
pos.transform_point(Vec3(sx, sy, sz))
}
let point1 = support(pos1, he1, normal)
let point2 = support(pos2, he2, normal.scale(-1.0F))
Some({ point1, point2, normal, penetration: min_overlap })
}
///|
priv struct CuboidFeature3D {
vertices : Array[@core.Vec3]
}
///|
fn cuboid_local_support_point(he : @core.Vec3, dir : @core.Vec3) -> @core.Vec3 {
Vec3(
if dir.x >= 0.0F {
he.x
} else {
-he.x
},
if dir.y >= 0.0F {
he.y
} else {
-he.y
},
if dir.z >= 0.0F {
he.z
} else {
-he.z
},
)
}
///|
fn vec3_component(v : @core.Vec3, i : Int) -> @core.Real {
match i {
0 => v.x
1 => v.y
_ => v.z
}
}
///|
fn vec3_axis(i : Int, sign : @core.Real) -> @core.Vec3 {
match i {
0 => Vec3(sign, 0.0F, 0.0F)
1 => Vec3(0.0F, sign, 0.0F)
_ => Vec3(0.0F, 0.0F, sign)
}
}
///|
fn cuboid_support_face_feature(
he : @core.Vec3,
local_dir : @core.Vec3,
) -> CuboidFeature3D {
let abs_x = @core.abs(local_dir.x)
let abs_y = @core.abs(local_dir.y)
let abs_z = @core.abs(local_dir.z)
let imax = if abs_x >= abs_y && abs_x >= abs_z {
0
} else if abs_y >= abs_z {
1
} else {
2
}
let sign = if vec3_component(local_dir, imax) >= 0.0F { 1.0F } else { -1.0F }
let vertices = match imax {
0 =>
[
@core.Vec3(he.x * sign, he.y, he.z),
Vec3(he.x * sign, -he.y, he.z),
Vec3(he.x * sign, -he.y, -he.z),
Vec3(he.x * sign, he.y, -he.z),
]
1 =>
[
Vec3(he.x, he.y * sign, he.z),
Vec3(-he.x, he.y * sign, he.z),
Vec3(-he.x, he.y * sign, -he.z),
Vec3(he.x, he.y * sign, -he.z),
]
_ =>
[
Vec3(he.x, he.y, he.z * sign),
Vec3(he.x, -he.y, he.z * sign),
Vec3(-he.x, -he.y, he.z * sign),
Vec3(-he.x, he.y, he.z * sign),
]
}
{ vertices, }
}
///|
fn cuboid_cuboid_compute_separation_wrt_local_line(
he1 : @core.Vec3,
he2 : @core.Vec3,
pos12 : @core.Isometry3,
axis1 : @core.Vec3,
) -> (@core.Real, @core.Vec3) {
let sign = if pos12.translation.dot(axis1) >= 0.0F { 1.0F } else { -1.0F }
let axis1_signed = axis1.scale(sign)
let axis2 = pos12.rotation.inverse().rotate_vec3(axis1_signed.scale(-1.0F))
let local_pt1 = cuboid_local_support_point(he1, axis1_signed)
let local_pt2 = cuboid_local_support_point(he2, axis2)
let pt2 = pos12.transform_point(local_pt2)
let separation = pt2.sub(local_pt1).dot(axis1_signed)
(separation, axis1_signed)
}
///|
fn cuboid_cuboid_find_local_separating_normal_oneway(
he1 : @core.Vec3,
he2 : @core.Vec3,
pos12 : @core.Isometry3,
) -> (@core.Real, @core.Vec3) {
let mut best_separation = -1.0e30F
let mut best_dir = @core.Vec3(1.0F, 0.0F, 0.0F)
for i in 0..<3 {
let sign = if vec3_component(pos12.translation, i) >= 0.0F {
1.0F
} else {
-1.0F
}
let axis1 = vec3_axis(i, sign)
let axis2 = pos12.rotation.inverse().rotate_vec3(axis1.scale(-1.0F))
let local_pt2 = cuboid_local_support_point(he2, axis2)
let pt2 = pos12.transform_point(local_pt2)
let separation = vec3_component(pt2, i) * sign - vec3_component(he1, i)
if separation > best_separation {
best_separation = separation
best_dir = axis1
}
}
(best_separation, best_dir)
}
///|
fn cuboid_cuboid_find_local_separating_edge_twoway(
he1 : @core.Vec3,
he2 : @core.Vec3,
pos12 : @core.Isometry3,
) -> (@core.Real, @core.Vec3) {
let x2 = pos12.rotation.rotate_vec3(Vec3(1.0F, 0.0F, 0.0F))
let y2 = pos12.rotation.rotate_vec3(Vec3(0.0F, 1.0F, 0.0F))
let z2 = pos12.rotation.rotate_vec3(Vec3(0.0F, 0.0F, 1.0F))
let axes = [
@core.Vec3(0.0F, -x2.z, x2.y),
Vec3(x2.z, 0.0F, -x2.x),
Vec3(-x2.y, x2.x, 0.0F),
Vec3(0.0F, -y2.z, y2.y),
Vec3(y2.z, 0.0F, -y2.x),
Vec3(-y2.y, y2.x, 0.0F),
Vec3(0.0F, -z2.z, z2.y),
Vec3(z2.z, 0.0F, -z2.x),
Vec3(-z2.y, z2.x, 0.0F),
]
let mut best_separation = -1.0e30F
let mut best_dir = @core.Vec3(1.0F, 0.0F, 0.0F)
for axis in axes {
let norm = axis.length()
if norm > 1.0e-6F {
let (separation, dir) = cuboid_cuboid_compute_separation_wrt_local_line(
he1,
he2,
pos12,
axis.scale(1.0F / norm),
)
if separation > best_separation {
best_separation = separation
best_dir = dir
}
}
}
(best_separation, best_dir)
}
///|
fn local_perp_dot_2d(a : @core.Vec2, b : @core.Vec2) -> @core.Real {
a.x * b.y - a.y * b.x
}
///|
fn local_orthonormal_basis(n : @core.Vec3) -> (@core.Vec3, @core.Vec3) {
if n.length_squared() <= 1.0e-12F {
return (Vec3(1.0F, 0.0F, 0.0F), Vec3(0.0F, 1.0F, 0.0F))
}
let base = if @core.abs(n.x) < 0.9F {
@core.Vec3(1.0F, 0.0F, 0.0F)
} else {
Vec3(0.0F, 1.0F, 0.0F)
}
let t0 = n.cross(base)
if t0.length_squared() <= 1.0e-12F {
return (Vec3(0.0F, 1.0F, 0.0F), Vec3(0.0F, 0.0F, 1.0F))
}
let u = t0.normalize()
let v = n.cross(u)
(u, v)
}
///|
fn local_point_in_convex_polygon2(
p : @core.Vec2,
poly : Array[@core.Vec2],
) -> Bool {
if poly.length() < 3 {
return false
}
let eps = 1.0e-8F
let mut sign = 0.0F
for i in 0.. (@core.Real, @core.Real)? {
let dir1 = edge1.1.sub(edge1.0)
let dir2 = edge2.1.sub(edge2.0)
let r = edge1.0.sub(edge2.0)
let a = dir1.dot(dir1)
let e = dir2.dot(dir2)
let f = dir2.dot(r)
let eps = 1.0e-12F
if a <= eps && e <= eps {
return Some((0.0F, 0.0F))
}
if a <= eps {
return Some((0.0F, f / e))
}
let c = dir1.dot(r)
if e <= eps {
return Some((-c / a, 0.0F))
}
let b = dir1.dot(dir2)
let ae = a * e
let bb = b * b
let denom = ae - bb
let scale = local_max(1.0F, local_max(@core.abs(ae), @core.abs(bb)))
let parallel = denom <= eps || @core.abs(denom) <= 1.0e-7F * scale
if parallel {
return None
}
let s = (b * f - c * e) / denom
Some((s, (b * s + f) / e))
}
///|
fn local_push_contact_if_unique(
contacts : Array[(@core.Vec3, @core.Vec3, @core.Real)],
p1 : @core.Vec3,
p2 : @core.Vec3,
dist : @core.Real,
) -> Unit {
let eps2 = 1.0e-8F
for i in 0.. Array[(@core.Vec3, @core.Vec3, @core.Real)] {
let contacts : Array[(@core.Vec3, @core.Vec3, @core.Real)] = []
let (basis_u, basis_v) = local_orthonormal_basis(sep_axis1)
let projected_face1 : Array[@core.Vec2] = []
for p in feature1.vertices {
projected_face1.push(Vec2(p.dot(basis_u), p.dot(basis_v)))
}
let vertices2_1 : Array[@core.Vec3] = []
for p in feature2.vertices {
vertices2_1.push(pos12.transform_point(p))
}
let projected_face2 : Array[@core.Vec2] = []
for p in vertices2_1 {
projected_face2.push(Vec2(p.dot(basis_u), p.dot(basis_v)))
}
if feature2.vertices.length() > 2 {
let normal2_1 = vertices2_1[2]
.sub(vertices2_1[1])
.cross(vertices2_1[0].sub(vertices2_1[1]))
let denom = normal2_1.dot(sep_axis1)
if @core.abs(denom) > 1.0e-10F {
for i in 0.. 2 {
let normal1 = feature1.vertices[2]
.sub(feature1.vertices[1])
.cross(feature1.vertices[0].sub(feature1.vertices[1]))
let denom = -normal1.dot(sep_axis1)
if @core.abs(denom) > 1.0e-10F {
for i in 0.. 0.0F && b1 < 1.0F && b2 > 0.0F && b2 < 1.0F {
let edge1a = feature1.vertices[i]
let edge1b = feature1.vertices[(i + 1) % feature1.vertices.length()]
let edge2a = vertices2_1[j]
let edge2b = vertices2_1[(j + 1) % vertices2_1.length()]
let local_p1 = edge1a.scale(1.0F - b1).add(edge1b.scale(b1))
let local_p2_1 = edge2a.scale(1.0F - b2).add(edge2b.scale(b2))
let dist = local_p2_1.sub(local_p1).dot(sep_axis1)
let local_p2 = pos21.transform_point(local_p2_1)
local_push_contact_if_unique(contacts, local_p1, local_p2, dist)
}
}
}
}
contacts
}
///|
fn compute_cuboid_cuboid_contacts(
pos1 : @core.Isometry3,
he1 : @core.Vec3,
pos2 : @core.Isometry3,
he2 : @core.Vec3,
prediction : @core.Real,
) -> Array[ContactPoint3D] {
let out : Array[ContactPoint3D] = []
let pos12 = pos1.inverse().mul(pos2)
let sep1 = cuboid_cuboid_find_local_separating_normal_oneway(he1, he2, pos12)
if sep1.0 > prediction {
return out
}
let pos21 = pos12.inverse()
let sep2 = cuboid_cuboid_find_local_separating_normal_oneway(he2, he1, pos21)
if sep2.0 > prediction {
return out
}
let sep3 = cuboid_cuboid_find_local_separating_edge_twoway(he1, he2, pos12)
if sep3.0 > prediction {
return out
}
let mut best_sep = sep1
if sep2.0 > sep1.0 && sep2.0 > sep3.0 {
best_sep = (sep2.0, pos12.rotation.rotate_vec3(sep2.1.scale(-1.0F)))
} else if sep3.0 > sep1.0 {
best_sep = sep3
}
let local_n1 = best_sep.1
let local_n2 = pos21.rotation.rotate_vec3(local_n1.scale(-1.0F))
let base_penetration = local_max(0.0F, -best_sep.0)
let feature1 = cuboid_support_face_feature(he1, local_n1)
let feature2 = cuboid_support_face_feature(he2, local_n2)
let local_contacts = local_compute_feature_contacts_face_face(
pos12, pos21, local_n1, feature1, feature2,
)
let normal_world = pos1.rotation.rotate_vec3(local_n1)
let eps = 1.0e-5F
for contact in local_contacts {
let local_p1 = contact.0
let local_p2 = contact.1
let dist = contact.2
if dist <= prediction + eps || base_penetration > 0.0F {
let penetration = base_penetration
out.push({
point1: pos1.transform_point(local_p1),
point2: pos2.transform_point(local_p2),
normal: normal_world,
penetration,
})
}
}
if out.length() == 0 {
if compute_cuboid_cuboid_contact(pos1, he1, pos2, he2) is Some(cp) {
out.push(cp)
}
}
out
}
///|
priv struct SupportVertex3D {
p : @core.Vec3 // Minkowski point (support1 - support2)
sup1 : @core.Vec3
sup2 : @core.Vec3
}
///|
fn support_world_for_convex(
pos : @core.Isometry3,
points : Array[@core.Vec3],
border_radius : @core.Real,
dir : @core.Vec3,
) -> @core.Vec3 {
if points.length() == 0 {
return pos.translation
}
let local_dir = pos.rotation.inverse().rotate_vec3(dir)
let mut best = points[0]
let mut best_dot = best.dot(local_dir)
for i in 1.. best_dot {
best_dot = d
best = p
}
}
let mut out = pos.transform_point(best)
let d2 = dir.length_squared()
if border_radius > 0.0F && d2 > 1.0e-12F {
out = out.add(dir.scale(border_radius / local_sqrt(d2)))
}
out
}
///|
fn support_world_for_ball(
center : @core.Vec3,
radius : @core.Real,
dir : @core.Vec3,
) -> @core.Vec3 {
let d2 = dir.length_squared()
if d2 <= 1.0e-12F {
center
} else {
center.add(dir.scale(radius / local_sqrt(d2)))
}
}
///|
fn support_world_for_cuboid(
pos : @core.Isometry3,
he : @core.Vec3,
dir : @core.Vec3,
) -> @core.Vec3 {
let local_dir = pos.rotation.inverse().rotate_vec3(dir)
let sx = if local_dir.x >= 0.0F { he.x } else { -he.x }
let sy = if local_dir.y >= 0.0F { he.y } else { -he.y }
let sz = if local_dir.z >= 0.0F { he.z } else { -he.z }
pos.transform_point(Vec3(sx, sy, sz))
}
///|
fn support_world_for_triangle(
pos : @core.Isometry3,
a : @core.Vec3,
b : @core.Vec3,
c : @core.Vec3,
dir : @core.Vec3,
) -> @core.Vec3 {
let local_dir = pos.rotation.inverse().rotate_vec3(dir)
let da = a.dot(local_dir)
let db = b.dot(local_dir)
let dc = c.dot(local_dir)
if db >= da && db >= dc {
pos.transform_point(b)
} else if dc >= da {
pos.transform_point(c)
} else {
pos.transform_point(a)
}
}
///|
fn support_world_for_cylinder_y(
pos : @core.Isometry3,
radius : @core.Real,
half_height : @core.Real,
dir : @core.Vec3,
) -> @core.Vec3 {
let local_dir = pos.rotation.inverse().rotate_vec3(dir)
let y = if local_dir.y >= 0.0F { half_height } else { -half_height }
let radial = @core.Vec3(local_dir.x, 0.0F, local_dir.z)
let rl2 = radial.length_squared()
let (x, z) = if rl2 <= 1.0e-12F {
// Any point on the cap is valid when the direction is purely along Y.
// Pick the center to avoid introducing a spurious tangential component
// that can cause GJK/EPA degeneracy.
(0.0F, 0.0F)
} else {
let inv = 1.0F / local_sqrt(rl2)
(radial.x * inv * radius, radial.z * inv * radius)
}
pos.transform_point(Vec3(x, y, z))
}
///|
fn support_world_for_capsule_y(
pos : @core.Isometry3,
radius : @core.Real,
half_height : @core.Real,
dir : @core.Vec3,
) -> @core.Vec3 {
let local_dir = pos.rotation.inverse().rotate_vec3(dir)
let cap_y = if local_dir.y >= 0.0F { half_height } else { -half_height }
let base = @core.Vec3(0.0F, cap_y, 0.0F)
let d2 = local_dir.length_squared()
let offset = if d2 <= 1.0e-12F {
@core.Vec3(radius, 0.0F, 0.0F)
} else {
local_dir.scale(radius / local_sqrt(d2))
}
pos.transform_point(base.add(offset))
}
///|
fn support_world_for_cone_y(
pos : @core.Isometry3,
radius : @core.Real,
half_height : @core.Real,
dir : @core.Vec3,
) -> @core.Vec3 {
let local_dir = pos.rotation.inverse().rotate_vec3(dir)
let apex = @core.Vec3(0.0F, half_height, 0.0F)
let radial = @core.Vec3(local_dir.x, 0.0F, local_dir.z)
let rl2 = radial.length_squared()
let base_pt = if rl2 <= 1.0e-12F {
// When the direction is purely along Y, any point on the base disk is valid.
// Pick the center to keep support points symmetric.
@core.Vec3(0.0F, -half_height, 0.0F)
} else {
let inv = 1.0F / local_sqrt(rl2)
Vec3(radial.x * inv * radius, -half_height, radial.z * inv * radius)
}
let da = apex.dot(local_dir)
let db = base_pt.dot(local_dir)
pos.transform_point(if da >= db { apex } else { base_pt })
}
///|
fn support_minkowski(
pos1 : @core.Isometry3,
s1 : Shape3D,
pos2 : @core.Isometry3,
s2 : Shape3D,
dir : @core.Vec3,
) -> SupportVertex3D? {
let sup1 = match s1 {
ConvexHull(points, br) => support_world_for_convex(pos1, points, br, dir)
Cuboid(he) => support_world_for_cuboid(pos1, he, dir)
Ball(r) => support_world_for_ball(pos1.translation, r, dir)
CapsuleY(r, hh) => support_world_for_capsule_y(pos1, r, hh, dir)
Cylinder(r, hh) => support_world_for_cylinder_y(pos1, r, hh, dir)
RoundCylinder(r, hh, br) =>
support_world_for_cylinder_y(pos1, r + br, hh + br, dir)
Cone(r, hh) => support_world_for_cone_y(pos1, r, hh, dir)
Triangle(a, b, c) => support_world_for_triangle(pos1, a, b, c, dir)
_ => return None
}
let neg_dir = dir.scale(-1.0F)
let sup2 = match s2 {
ConvexHull(points, br) =>
support_world_for_convex(pos2, points, br, neg_dir)
Cuboid(he) => support_world_for_cuboid(pos2, he, neg_dir)
Ball(r) => support_world_for_ball(pos2.translation, r, neg_dir)
CapsuleY(r, hh) => support_world_for_capsule_y(pos2, r, hh, neg_dir)
Cylinder(r, hh) => support_world_for_cylinder_y(pos2, r, hh, neg_dir)
RoundCylinder(r, hh, br) =>
support_world_for_cylinder_y(pos2, r + br, hh + br, neg_dir)
Cone(r, hh) => support_world_for_cone_y(pos2, r, hh, neg_dir)
Triangle(a, b, c) => support_world_for_triangle(pos2, a, b, c, neg_dir)
_ => return None
}
Some({ p: sup1.sub(sup2), sup1, sup2 })
}
///|
fn triple_product(a : @core.Vec3, b : @core.Vec3, c : @core.Vec3) -> @core.Vec3 {
// a x (b x c)
b.scale(a.dot(c)).sub(c.scale(a.dot(b)))
}
///|
fn gjk_update_simplex(
simplex : Array[SupportVertex3D],
dir : @core.Vec3,
) -> (@core.Vec3, Bool) {
// Newest point is always at the end of the array.
let a = simplex[simplex.length() - 1].p
let ao = a.scale(-1.0F)
if simplex.length() == 2 {
let v_b = simplex[0]
let v_a = simplex[1]
let b = v_b.p
let ab = b.sub(a)
if ab.dot(ao) > 0.0F {
let d = triple_product(ab, ao, ab)
if d.length_squared() > 1.0e-12F {
(d, false)
} else {
(ab.cross(Vec3(0.0F, 1.0F, 0.0F)), false)
}
} else {
simplex.clear()
simplex.push(v_a)
(ao, false)
}
} else if simplex.length() == 3 {
let b = simplex[1].p
let c = simplex[0].p
let ab = b.sub(a)
let ac = c.sub(a)
let abc = ab.cross(ac)
let ab_perp = abc.cross(ab)
if ab_perp.dot(ao) > 0.0F {
// Remove c, keep (b,a).
let v_b = simplex[1]
let v_a = simplex[2]
simplex.clear()
simplex.push(v_b)
simplex.push(v_a)
return (triple_product(ab, ao, ab), false)
}
let ac_perp = abc.cross(ac)
if ac_perp.dot(ao) > 0.0F {
// Remove b, keep (c,a).
let v_c = simplex[0]
let v_a = simplex[2]
simplex.clear()
simplex.push(v_c)
simplex.push(v_a)
return (triple_product(ac, ao, ac), false)
}
// Origin is either above or below the triangle.
if abc.dot(ao) > 0.0F {
(abc, false)
} else {
// Swap b and c to keep a consistent winding.
let v_c = simplex[0]
let v_b = simplex[1]
simplex[0] = v_b
simplex[1] = v_c
(abc.scale(-1.0F), false)
}
} else {
// Tetrahedron.
let b = simplex[2].p
let c = simplex[1].p
let d = simplex[0].p
let ab = b.sub(a)
let ac = c.sub(a)
let ad = d.sub(a)
let abc = ab.cross(ac)
let acd = ac.cross(ad)
let adb = ad.cross(ab)
if abc.dot(ao) > 0.0F {
// Remove d.
let v_c = simplex[1]
let v_b = simplex[2]
let v_a = simplex[3]
simplex.clear()
simplex.push(v_c)
simplex.push(v_b)
simplex.push(v_a)
return (abc, false)
}
if acd.dot(ao) > 0.0F {
// Remove b.
let v_d = simplex[0]
let v_c = simplex[1]
let v_a = simplex[3]
simplex.clear()
simplex.push(v_d)
simplex.push(v_c)
simplex.push(v_a)
return (acd, false)
}
if adb.dot(ao) > 0.0F {
// Remove c.
let v_d = simplex[0]
let v_b = simplex[2]
let v_a = simplex[3]
simplex.clear()
simplex.push(v_d)
simplex.push(v_b)
simplex.push(v_a)
return (adb, false)
}
(dir, true)
}
}
///|
fn gjk_intersect(
pos1 : @core.Isometry3,
s1 : Shape3D,
pos2 : @core.Isometry3,
s2 : Shape3D,
) -> Array[SupportVertex3D]? {
let mut dir = pos2.translation.sub(pos1.translation)
if dir.length_squared() <= 1.0e-12F {
dir = Vec3(1.0F, 0.0F, 0.0F)
}
let simplex : Array[SupportVertex3D] = []
if support_minkowski(pos1, s1, pos2, s2, dir) is Some(v0) {
simplex.push(v0)
dir = v0.p.scale(-1.0F)
} else {
return None
}
for _ in 0..<32 {
if support_minkowski(pos1, s1, pos2, s2, dir) is Some(a) {
if a.p.dot(dir) < 0.0F {
return None
}
simplex.push(a)
let (new_dir, contains) = gjk_update_simplex(simplex, dir)
if contains {
return Some(simplex)
}
dir = new_dir
if dir.length_squared() <= 1.0e-18F {
// Degenerate; treat as intersecting.
return Some(simplex)
}
} else {
return None
}
}
Some(simplex)
}
///|
priv struct EpaFace {
i0 : Int
i1 : Int
i2 : Int
normal : @core.Vec3
dist : @core.Real
}
///|
fn epa_make_face(
verts : Array[SupportVertex3D],
i0 : Int,
i1 : Int,
i2 : Int,
) -> EpaFace? {
let a = verts[i0].p
let b = verts[i1].p
let c = verts[i2].p
let n0 = b.sub(a).cross(c.sub(a))
let n2 = n0.length_squared()
if n2 <= 1.0e-18F {
return None
}
let inv = 1.0F / local_sqrt(n2)
let mut normal = n0.scale(inv)
let mut dist = normal.dot(a)
let mut i1 = i1
let mut i2 = i2
if dist < 0.0F {
normal = normal.scale(-1.0F)
dist = -dist
let tmp = i1
i1 = i2
i2 = tmp
}
Some({ i0, i1, i2, normal, dist })
}
///|
fn epa_make_face_with_opposite(
verts : Array[SupportVertex3D],
i0 : Int,
i1 : Int,
i2 : Int,
i_opp : Int,
) -> EpaFace? {
let a = verts[i0].p
let b = verts[i1].p
let c = verts[i2].p
let opp = verts[i_opp].p
let mut n0 = b.sub(a).cross(c.sub(a))
let n2 = n0.length_squared()
if n2 <= 1.0e-18F {
return None
}
let mut i1 = i1
let mut i2 = i2
// Ensure the normal points away from the opposite vertex (outward for tetrahedra).
if n0.dot(opp.sub(a)) > 0.0F {
n0 = n0.scale(-1.0F)
let tmp = i1
i1 = i2
i2 = tmp
}
let inv = 1.0F / local_sqrt(n2)
let mut normal = n0.scale(inv)
let mut dist = normal.dot(a)
if dist < 0.0F {
normal = normal.scale(-1.0F)
dist = -dist
let tmp = i1
i1 = i2
i2 = tmp
}
Some({ i0, i1, i2, normal, dist })
}
///|
fn barycentric_on_triangle(
a : @core.Vec3,
b : @core.Vec3,
c : @core.Vec3,
p : @core.Vec3,
) -> (@core.Real, @core.Real, @core.Real) {
let v0 = b.sub(a)
let v1 = c.sub(a)
let v2 = p.sub(a)
let d00 = v0.dot(v0)
let d01 = v0.dot(v1)
let d11 = v1.dot(v1)
let d20 = v2.dot(v0)
let d21 = v2.dot(v1)
let denom = d00 * d11 - d01 * d01
if @core.abs(denom) <= 1.0e-18F {
return (1.0F, 0.0F, 0.0F)
}
let inv = 1.0F / denom
let v = (d11 * d20 - d01 * d21) * inv
let w = (d00 * d21 - d01 * d20) * inv
let u = 1.0F - v - w
(u, v, w)
}
///|
fn epa_contact(
pos1 : @core.Isometry3,
s1 : Shape3D,
pos2 : @core.Isometry3,
s2 : Shape3D,
simplex : Array[SupportVertex3D],
) -> ContactPoint3D? {
// Gather vertices.
let verts : Array[SupportVertex3D] = []
for i in 0.. 1.0e-18F {
nvec.scale(1.0F / local_sqrt(n2))
} else {
dir
}
let delta = pos2.translation.sub(pos1.translation)
if delta.length_squared() > 1.0e-12F && normal.dot(delta) < 0.0F {
normal = normal.scale(-1.0F)
}
return Some({ point1: cp1, point2: cp2, normal, penetration: best.dist })
}
// Expand polytope.
let new_index = verts.length()
verts.push(v)
let edges : Array[(Int, Int)] = []
fn add_edge(edges : Array[(Int, Int)], a : Int, b : Int) -> Unit {
for i in 0.. ignore
return
}
}
edges.push((a, b))
}
let mut fi = 0
while fi < faces.length() {
let f = faces[fi]
// Visible if the new point is outside this face.
if f.normal.dot(verts[new_index].p) - f.dist > 1.0e-6F {
add_edge(edges, f.i0, f.i1)
add_edge(edges, f.i1, f.i2)
add_edge(edges, f.i2, f.i0)
faces[fi] = faces[faces.length() - 1]
faces.pop() |> ignore
continue
}
fi = fi + 1
}
for i in 0.. ContactPoint3D? {
// Compound handling.
match (s1, s2) {
(Compound(parts), other) => {
let mut best : ContactPoint3D? = None
let mut best_pen = -1.0F
for i in 0.. best_pen {
best_pen = cp.penetration
best = Some(cp)
}
}
}
best
}
(other, Compound(parts)) =>
if compute_convex_contact(pos2, Compound(parts), pos1, other) is Some(cp) {
Some({
point1: cp.point2,
point2: cp.point1,
normal: cp.normal.scale(-1.0F),
penetration: cp.penetration,
})
} else {
None
}
_ =>
// Supported convex pairs: ConvexHull/Cuboid/Ball.
if gjk_intersect(pos1, s1, pos2, s2) is Some(simplex) {
epa_contact(pos1, s1, pos2, s2, simplex)
} else {
None
}
}
}