use crate::b2_collision::*;
use crate::b2_math::*;
use crate::b2_settings::*;
use crate::shapes::b2_circle_shape::*;
use crate::shapes::b2_edge_shape::*;
use crate::shapes::b2_polygon_shape::*;
pub fn b2_collide_edge_and_circle(
manifold: &mut B2manifold,
edge_a: &B2edgeShape,
xf_a: &B2Transform,
circle_b: &B2circleShape,
xf_b: &B2Transform,
) {
manifold.point_count = 0;
let q: B2vec2 =
b2_mul_t_transform_by_vec2(*xf_a, b2_mul_transform_by_vec2(*xf_b, circle_b.m_p));
let (a, b) = (edge_a.m_vertex1, edge_a.m_vertex2);
let e: B2vec2 = b - a;
let mut n = B2vec2::new(e.y, -e.x);
let offset: f32 = b2_dot(n, q - a);
let one_sided: bool = edge_a.m_one_sided;
if one_sided && offset < 0.0 {
return;
}
let u: f32 = b2_dot(e, b - q);
let v: f32 = b2_dot(e, q - a);
let radius: f32 = edge_a.base.m_radius + circle_b.base.m_radius;
let mut cf = B2contactFeature::default();
cf.index_b = 0;
cf.type_b = B2contactFeatureType::EVertex as u8;
if v <= 0.0 {
let p: B2vec2 = a;
let d: B2vec2 = q - p;
let dd: f32 = b2_dot(d, d);
if dd > radius * radius {
return;
}
if edge_a.m_one_sided {
let a1: B2vec2 = edge_a.m_vertex0;
let b1: B2vec2 = a;
let e1: B2vec2 = b1 - a1;
let u1: f32 = b2_dot(e1, b1 - q);
if u1 > 0.0 {
return;
}
}
cf.index_a = 0;
cf.type_a = B2contactFeatureType::EVertex as u8;
manifold.point_count = 1;
manifold.manifold_type = B2manifoldType::ECircles;
manifold.local_normal.set_zero();
manifold.local_point = p;
manifold.points[0].id.cf = cf;
manifold.points[0].local_point = circle_b.m_p;
return;
}
if u <= 0.0 {
let p: B2vec2 = b;
let d: B2vec2 = q - p;
let dd: f32 = b2_dot(d, d);
if dd > radius * radius {
return;
}
if edge_a.m_one_sided {
let b2: B2vec2 = edge_a.m_vertex3;
let a2: B2vec2 = b;
let e2: B2vec2 = b2 - a2;
let v2: f32 = b2_dot(e2, q - a2);
if v2 > 0.0 {
return;
}
}
cf.index_a = 1;
cf.type_a = B2contactFeatureType::EVertex as u8;
manifold.point_count = 1;
manifold.manifold_type = B2manifoldType::ECircles;
manifold.local_normal.set_zero();
manifold.local_point = p;
manifold.points[0].id.cf = cf;
manifold.points[0].local_point = circle_b.m_p;
return;
}
let den: f32 = b2_dot(e, e);
b2_assert(den > 0.0);
let p: B2vec2 = (1.0 / den) * (u * a + v * b);
let d: B2vec2 = q - p;
let dd: f32 = b2_dot(d, d);
if dd > radius * radius {
return;
}
if offset < 0.0 {
n.set(-n.x, -n.y);
}
n.normalize();
cf.index_a = 0;
cf.type_a = B2contactFeatureType::EFace as u8;
manifold.point_count = 1;
manifold.manifold_type = B2manifoldType::EFaceA;
manifold.local_normal = n;
manifold.local_point = a;
manifold.points[0].id.cf = cf;
manifold.points[0].local_point = circle_b.m_p;
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
enum B2ePAxisType {
EUnknown,
EEdgeA,
EEdgeB,
}
impl Default for B2ePAxisType {
fn default() -> Self {
return B2ePAxisType::EUnknown;
}
}
#[derive(Clone, Default, Copy, Debug)]
struct B2epaxis {
normal: B2vec2,
axis_type: B2ePAxisType,
index: i32,
separation: f32,
}
#[derive(Clone, Default, Copy, Debug)]
struct B2tempPolygon {
vertices: [B2vec2; B2_MAX_POLYGON_VERTICES],
normals: [B2vec2; B2_MAX_POLYGON_VERTICES],
count: usize,
}
#[derive(Clone, Default, Copy, Debug)]
struct B2referenceFace {
i1: usize,
i2: usize,
v1: B2vec2,
v2: B2vec2,
normal: B2vec2,
side_normal1: B2vec2,
side_offset1: f32,
side_normal2: B2vec2,
side_offset2: f32,
}
fn b2_compute_edge_separation(polygon_b: B2tempPolygon, v1: B2vec2, normal1: B2vec2) -> B2epaxis {
let mut axis = B2epaxis {
axis_type: B2ePAxisType::EEdgeA,
index: -1,
separation: -B2_MAX_FLOAT,
normal: B2vec2::zero(),
};
let axes: [B2vec2; 2] = [normal1, -normal1];
for j in 0..2 {
let mut sj: f32 = B2_MAX_FLOAT;
for i in 0..polygon_b.count {
let si: f32 = b2_dot(axes[j], polygon_b.vertices[i] - v1);
if si < sj {
sj = si;
}
}
if sj > axis.separation {
axis.index = j as i32;
axis.separation = sj;
axis.normal = axes[j];
}
}
return axis;
}
fn b2_compute_polygon_separation(polygon_b: B2tempPolygon, v1: B2vec2, v2: B2vec2) -> B2epaxis {
let mut axis = B2epaxis {
axis_type: B2ePAxisType::EUnknown,
index: -1,
separation: -B2_MAX_FLOAT,
normal: B2vec2::zero(),
};
for i in 0..polygon_b.count {
let n: B2vec2 = -polygon_b.normals[i];
let s1: f32 = b2_dot(n, polygon_b.vertices[i] - v1);
let s2: f32 = b2_dot(n, polygon_b.vertices[i] - v2);
let s: f32 = b2_min(s1, s2);
if s > axis.separation {
axis.axis_type = B2ePAxisType::EEdgeB;
axis.index = i as i32;
axis.separation = s;
axis.normal = n;
}
}
return axis;
}
pub fn b2_collide_edge_and_polygon(
manifold: &mut B2manifold,
edge_a: &B2edgeShape,
xf_a: &B2Transform,
polygon_b: &B2polygonShape,
xf_b: &B2Transform,
) {
manifold.point_count = 0;
let xf: B2Transform = b2_mul_t_transform(*xf_a, *xf_b);
let centroid_b: B2vec2 = b2_mul_transform_by_vec2(xf, polygon_b.m_centroid);
let v1: B2vec2 = edge_a.m_vertex1;
let v2: B2vec2 = edge_a.m_vertex2;
let mut edge1: B2vec2 = v2 - v1;
edge1.normalize();
let normal1 = B2vec2::new(edge1.y, -edge1.x);
let offset1: f32 = b2_dot(normal1, centroid_b - v1);
let one_sided: bool = edge_a.m_one_sided;
if one_sided && offset1 < 0.0 {
return;
}
let mut temp_polygon_b = B2tempPolygon::default();
temp_polygon_b.count = polygon_b.m_count;
for i in 0..polygon_b.m_count {
temp_polygon_b.vertices[i] = b2_mul_transform_by_vec2(xf, polygon_b.m_vertices[i]);
temp_polygon_b.normals[i] = b2_mul_rot_by_vec2(xf.q, polygon_b.m_normals[i]);
}
let radius: f32 = polygon_b.base.m_radius + edge_a.base.m_radius;
let edge_axis: B2epaxis = b2_compute_edge_separation(temp_polygon_b, v1, normal1);
if edge_axis.separation > radius {
return;
}
let polygon_axis: B2epaxis = b2_compute_polygon_separation(temp_polygon_b, v1, v2);
if polygon_axis.separation > radius {
return;
}
const K_RELATIVE_TOL: f32 = 0.98;
const K_ABSOLUTE_TOL: f32 = 0.001;
let mut primary_axis: B2epaxis;
if polygon_axis.separation - radius
> K_RELATIVE_TOL * (edge_axis.separation - radius) + K_ABSOLUTE_TOL
{
primary_axis = polygon_axis;
} else {
primary_axis = edge_axis;
}
if one_sided {
let mut edge0: B2vec2 = v1 - edge_a.m_vertex0;
edge0.normalize();
let normal0 = B2vec2::new(edge0.y, -edge0.x);
let convex1: bool = b2_cross(edge0, edge1) >= 0.0;
let mut edge2: B2vec2 = edge_a.m_vertex3 - v2;
edge2.normalize();
let normal2 = B2vec2::new(edge2.y, -edge2.x);
let convex2: bool = b2_cross(edge1, edge2) >= 0.0;
const SIN_TOL: f32 = 0.1;
let side1: bool = b2_dot(primary_axis.normal, edge1) <= 0.0;
if side1 {
if convex1 {
if b2_cross(primary_axis.normal, normal0) > SIN_TOL {
return;
}
} else {
primary_axis = edge_axis;
}
} else {
if convex2 {
if b2_cross(normal2, primary_axis.normal) > SIN_TOL {
return;
}
} else {
primary_axis = edge_axis;
}
}
}
let mut clip_points = <[B2clipVertex; 2]>::default();
let mut rf = B2referenceFace::default();
if primary_axis.axis_type == B2ePAxisType::EEdgeA {
manifold.manifold_type = B2manifoldType::EFaceA;
let mut best_index: i32 = 0;
let mut best_value: f32 = b2_dot(primary_axis.normal, temp_polygon_b.normals[0]);
for i in 1..temp_polygon_b.count {
let value: f32 = b2_dot(primary_axis.normal, temp_polygon_b.normals[i]);
if value < best_value {
best_value = value;
best_index = i as i32;
}
}
let i1: i32 = best_index;
let i2: i32 = if i1 + 1 < temp_polygon_b.count as i32 {
i1 + 1
} else {
0
};
clip_points[0].v = temp_polygon_b.vertices[i1 as usize];
clip_points[0].id.cf.index_a = 0;
clip_points[0].id.cf.index_b = i1 as u8;
clip_points[0].id.cf.type_a = B2contactFeatureType::EFace as u8;
clip_points[0].id.cf.type_b = B2contactFeatureType::EVertex as u8;
clip_points[1].v = temp_polygon_b.vertices[i2 as usize];
clip_points[1].id.cf.index_a = 0;
clip_points[1].id.cf.index_b = i2 as u8;
clip_points[1].id.cf.type_a = B2contactFeatureType::EFace as u8;
clip_points[1].id.cf.type_b = B2contactFeatureType::EVertex as u8;
rf.i1 = 0;
rf.i2 = 1;
rf.v1 = v1;
rf.v2 = v2;
rf.normal = primary_axis.normal;
rf.side_normal1 = -edge1;
rf.side_normal2 = edge1;
} else {
manifold.manifold_type = B2manifoldType::EFaceB;
clip_points[0].v = v2;
clip_points[0].id.cf.index_a = 1;
clip_points[0].id.cf.index_b = primary_axis.index as u8;
clip_points[0].id.cf.type_a = B2contactFeatureType::EVertex as u8;
clip_points[0].id.cf.type_b = B2contactFeatureType::EFace as u8;
clip_points[1].v = v1;
clip_points[1].id.cf.index_a = 0;
clip_points[1].id.cf.index_b = primary_axis.index as u8;
clip_points[1].id.cf.type_a = B2contactFeatureType::EVertex as u8;
clip_points[1].id.cf.type_b = B2contactFeatureType::EFace as u8;
rf.i1 = primary_axis.index as usize;
rf.i2 = if rf.i1 + 1 < temp_polygon_b.count {
rf.i1 + 1
} else {
0
};
rf.v1 = temp_polygon_b.vertices[rf.i1];
rf.v2 = temp_polygon_b.vertices[rf.i2];
rf.normal = temp_polygon_b.normals[rf.i1];
rf.side_normal1.set(rf.normal.y, -rf.normal.x);
rf.side_normal2 = -rf.side_normal1;
}
rf.side_offset1 = b2_dot(rf.side_normal1, rf.v1);
rf.side_offset2 = b2_dot(rf.side_normal2, rf.v2);
let mut clip_points1 = <[B2clipVertex; 2]>::default();
let mut clip_points2 = <[B2clipVertex; 2]>::default();
let np: usize = b2_clip_segment_to_line(
&mut clip_points1,
clip_points,
rf.side_normal1,
rf.side_offset1,
rf.i1,
);
if np < B2_MAX_MANIFOLD_POINTS {
return;
}
let np: usize = b2_clip_segment_to_line(
&mut clip_points2,
clip_points1,
rf.side_normal2,
rf.side_offset2,
rf.i2,
);
if np < B2_MAX_MANIFOLD_POINTS {
return;
}
if primary_axis.axis_type == B2ePAxisType::EEdgeA {
manifold.local_normal = rf.normal;
manifold.local_point = rf.v1;
} else {
manifold.local_normal = polygon_b.m_normals[rf.i1];
manifold.local_point = polygon_b.m_vertices[rf.i1];
}
let mut point_count: usize = 0;
for i in 0..B2_MAX_MANIFOLD_POINTS {
let separation: f32;
separation = b2_dot(rf.normal, clip_points2[i].v - rf.v1);
if separation <= radius {
let cp: &mut B2manifoldPoint = &mut manifold.points[point_count];
if primary_axis.axis_type == B2ePAxisType::EEdgeA {
cp.local_point = b2_mul_t_transform_by_vec2(xf, clip_points2[i].v);
cp.id = clip_points2[i].id;
} else {
cp.local_point = clip_points2[i].v;
cp.id.cf.type_a = clip_points2[i].id.cf.type_b;
cp.id.cf.type_b = clip_points2[i].id.cf.type_a;
cp.id.cf.index_a = clip_points2[i].id.cf.index_b;
cp.id.cf.index_b = clip_points2[i].id.cf.index_a;
}
point_count += 1;
}
}
manifold.point_count = point_count;
}