use std::sync::atomic;
use {math, glium, log, rand_xorshift};
use rand_xorshift::XorShiftRng;
use vec_map::VecMap;
use gl_utils::*;
use linear_sim::*;
use crate::program;
const DENSITY_MIN : f64 = 150.0; const DENSITY_MAX : f64 = 20000.0; const POSITION_MIN : [f64; 3] = [-5.0, -5.0, 2.0];
const POSITION_MAX : [f64; 3] = [ 5.0, 5.0, 10.0];
const CAPSULE_RADIUS_MIN : f64 = 0.05; const CAPSULE_RADIUS_MAX : f64 = 1.0; const CAPSULE_HALF_HEIGHT_MIN : f64 = 0.0; const CAPSULE_HALF_HEIGHT_MAX : f64 = 3.0;
const CUBOID_EXTENT_MIN : f64 = 0.01; const CUBOID_EXTENT_MAX : f64 = 4.0;
const TRIANGLE_SCALE : f64 = 5.0;
const HULL_MAX_POINTS : u32 = 50;
static EXAMPLE_COUNTER : atomic::AtomicU64 = atomic::AtomicU64::new (0);
pub fn init_capsule_capsule (
render_context : &mut Render <render::resource::Default>,
rng : &mut XorShiftRng
) {
use render::resource::draw3d;
init (render_context, rng);
let grid_vertex_data = render::resource::Default::debug_grid_vertices();
let capsule_vertex_data = next_capsule_capsule_vertices (rng);
let meshes = {
let mut v = VecMap::with_capacity (2);
assert!(v.insert (draw3d::MeshId::Grid as usize, &grid_vertex_data[..])
.is_none());
assert!(v.insert (draw3d::MeshId::Capsule as usize, &capsule_vertex_data[..])
.is_none());
v
};
render_context.resource.draw3d.instance_vertices_set (
&render_context.glium_display,
draw3d::InstancesInit {
billboards: VecMap::default(),
meshes,
.. Default::default()
}
);
program::print_testbed_modes_prompt();
println!("linear-sim penetration capsule v. capsule test");
println!(" press 'Tab' to generate test cases");
}
pub fn draw_capsule_capsule (
render_context : &Render <render::resource::Default>,
glium_frame : &mut glium::Frame,
center_indices_list : &glium::IndexBuffer <u32>
) {
use glium::Surface;
render::Resource::draw_3d (render_context, glium_frame);
let viewport = &render_context.viewports()[render::resource::MAIN_VIEWPORT];
let draw_parameters_viewport = glium::DrawParameters {
viewport: Some (*viewport.rect()),
.. Default::default()
};
let draw_parameters_write_depth = glium::DrawParameters {
depth: glium::Depth {
test: glium::DepthTest::IfLess,
write: true,
.. Default::default()
},
.. draw_parameters_viewport
};
let (transform_mat_world_to_view, projection_mat_perspective) =
viewport.camera3d().unwrap().view_mats();
let uniforms = glium::uniform!{
uni_transform_mat_view: transform_mat_world_to_view,
uni_projection_mat_perspective: projection_mat_perspective,
uni_color: color::rgba_u8_to_rgba_f32 (color::DEBUG_LIGHT_BLUE)
};
glium_frame.draw (
render_context.resource.draw3d.instance_vertices(),
center_indices_list,
&render_context.resource.shader_programs() [
shader::ProgramId::WorldSpace3dUniColor as usize
],
&uniforms,
&draw_parameters_write_depth
).unwrap();
}
pub fn next_capsule_capsule (
render_context : &mut Render <render::resource::Default>,
rng : &mut XorShiftRng
) {
use render::resource::draw3d::MeshId;
let capsule_vertex_data = next_capsule_capsule_vertices (rng);
render_context.resource.draw3d.instance_vertices_mesh_write (
MeshId::Capsule as usize, &capsule_vertex_data);
}
fn next_capsule_capsule_vertices (rng : &mut XorShiftRng)
-> [vertex::Vert3dOrientationScaleColor; 4]
{
let overlapping_capsules = generate_overlapping_capsule_capsule (rng);
let resolved_capsules =
resolve_overlapping_capsule_capsule (&overlapping_capsules);
let initial_capsules = [
overlapping_capsules[0].0.clone(), overlapping_capsules[1].0.clone()
];
vertices_capsule_capsule (initial_capsules, resolved_capsules)
}
fn generate_overlapping_capsule_capsule (rng : &mut XorShiftRng)
-> [(object::Static, f64); 2]
{
use rand::RngExt;
use geometry::shape;
use geometry::shape::Stereometric;
let radius_a = rng.random_range (CAPSULE_RADIUS_MIN..CAPSULE_RADIUS_MAX);
let radius_b = rng.random_range (CAPSULE_RADIUS_MIN..CAPSULE_RADIUS_MAX);
let half_height_a = rng.random_range (
CAPSULE_HALF_HEIGHT_MIN..CAPSULE_HALF_HEIGHT_MAX);
let half_height_b = rng.random_range (
CAPSULE_HALF_HEIGHT_MIN..CAPSULE_HALF_HEIGHT_MAX);
let density_a = rng.random_range (DENSITY_MIN..DENSITY_MAX);
let density_b = rng.random_range (DENSITY_MIN..DENSITY_MAX);
let shape_a = shape::Capsule::noisy (radius_a, half_height_a);
let shape_b = shape::Capsule::noisy (radius_b, half_height_b);
let volume_a = *shape_a.volume();
let volume_b = *shape_b.volume();
let mass_a = density_a * volume_a;
let mass_b = density_b * volume_b;
let min_distance = radius_a + radius_b;
let min_dim = f64::sqrt ((min_distance * min_distance) / 3.0);
let initial_aabb = geometry::Aabb3::<f64>::with_minmax (
POSITION_MIN.into(), POSITION_MAX.into()).unwrap();
let capsule_a = object::Static {
position: component::Position (initial_aabb.rand_point (rng)),
bound: component::Bound (shape::Bounded::from (shape_a).into()),
material: component::MATERIAL_STONE,
collidable: true
};
let within_min : math::Point3 <f64> = capsule_a.position.0
- math::Vector3::new (min_dim, min_dim, min_dim + half_height_a);
let within_max : math::Point3 <f64> = capsule_a.position.0
+ math::Vector3::new (min_dim, min_dim, min_dim + half_height_a);
let within_aabb = geometry::Aabb3::<f64>::with_minmax (within_min, within_max)
.unwrap();
let capsule_b = object::Static {
position: component::Position (within_aabb.rand_point (rng)),
bound: component::Bound (shape::Bounded::from (shape_b).into()),
material: component::MATERIAL_STONE,
collidable: true
};
[(capsule_a, mass_a), (capsule_b, mass_b)]
}
fn resolve_overlapping_capsule_capsule (
overlapping_capsules : &[(object::Static, f64); 2]
) -> [object::Static; 2] {
log::trace!("resolve overlapping capsule pair...");
let (capsule_a, mass_a) = &overlapping_capsules[0];
let (capsule_b, mass_b) = &overlapping_capsules[1];
let proximity = collision::Proximity::query (capsule_a, capsule_b);
log::trace!(" proximity: {proximity:?}");
debug_assert_eq!(proximity.relation(), collision::proximity::Relation::Intersect);
let total_mass = mass_a + mass_b;
let relative_mass_a = mass_a / total_mass;
let relative_mass_b = mass_b / total_mass;
let move_a = 1.0 - relative_mass_a;
let move_b = 1.0 - relative_mass_b;
let resolved_capsule_a = object::Static {
position: component::Position (capsule_a.position.0 +
2.0 * move_a * proximity.half_axis +
0.25 * collision::CONTACT_DISTANCE * *proximity.normal),
.. capsule_a.clone()
};
let resolved_capsule_b = object::Static {
position: component::Position (capsule_b.position.0 -
2.0 * move_b * proximity.half_axis -
0.25 * collision::CONTACT_DISTANCE * *proximity.normal),
.. capsule_b.clone()
};
let resolved_proximity =
collision::Proximity::query (&resolved_capsule_a, &resolved_capsule_b);
debug_assert_eq!(
resolved_proximity.relation(), collision::proximity::Relation::Contact);
log::trace!("...resolve overlapping capsule pair");
[
resolved_capsule_a, resolved_capsule_b
]
}
fn vertices_capsule_capsule (
overlapping_capsules : [object::Static; 2],
resolved_capsules : [object::Static; 2]
) -> [vertex::Vert3dOrientationScaleColor; 4] {
[
capsule_to_vertex (&overlapping_capsules[0], color::DEBUG_RED),
capsule_to_vertex (&overlapping_capsules[1], color::DEBUG_YELLOW),
capsule_to_vertex (&resolved_capsules[0], color::DEBUG_GREEN),
capsule_to_vertex (&resolved_capsules[1], color::DEBUG_BLUE)
]
}
pub fn init_cuboid_cuboid (
render_context : &mut Render <render::resource::Default>,
rng : &mut XorShiftRng
) {
use render::resource::draw3d;
init (render_context, rng);
let grid_vertex_data = render::resource::Default::debug_grid_vertices();
let cuboid_vertex_data = next_cuboid_cuboid_vertices (rng);
let aabb_lines = Some (&cuboid_vertex_data[..]);
let meshes = {
let mut v = VecMap::with_capacity (1);
assert!(v.insert (draw3d::MeshId::Grid as usize, &grid_vertex_data[..])
.is_none());
v
};
render_context.resource.draw3d.instance_vertices_set (
&render_context.glium_display,
draw3d::InstancesInit {
billboards: VecMap::default(),
aabb_lines,
meshes,
.. Default::default()
}
);
program::print_testbed_modes_prompt();
println!("linear-sim penetration cuboid v. cuboid test");
println!(" press 'Tab' to generate test cases");
}
pub fn draw_cuboid_cuboid (
render_context : &Render <render::resource::Default>,
glium_frame : &mut glium::Frame,
center_indices_list : &glium::IndexBuffer <u32>
) {
use glium::Surface;
render::Resource::draw_3d (render_context, glium_frame);
let viewport = &render_context.viewports()[render::resource::MAIN_VIEWPORT];
let draw_parameters_viewport = glium::DrawParameters {
viewport: Some (*viewport.rect()),
.. Default::default()
};
let draw_parameters_write_depth = glium::DrawParameters {
depth: glium::Depth {
test: glium::DepthTest::IfLess,
write: true,
.. Default::default()
},
.. draw_parameters_viewport
};
let (transform_mat_world_to_view, projection_mat_perspective) =
viewport.camera3d().unwrap().view_mats();
let uniforms = glium::uniform!{
uni_transform_mat_view: transform_mat_world_to_view,
uni_projection_mat_perspective: projection_mat_perspective,
uni_color: color::rgba_u8_to_rgba_f32 (color::DEBUG_LIGHT_BLUE)
};
glium_frame.draw (
render_context.resource.draw3d.instance_vertices(),
center_indices_list,
&render_context.resource.shader_programs() [
shader::ProgramId::WorldSpace3dUniColor as usize
],
&uniforms,
&draw_parameters_write_depth
).unwrap();
}
pub fn next_cuboid_cuboid (
render_context : &mut Render <render::resource::Default>,
rng : &mut XorShiftRng
) {
let cuboid_vertex_data = next_cuboid_cuboid_vertices (rng);
render_context.resource.draw3d
.instance_vertices_aabb_lines_write (&cuboid_vertex_data);
}
fn next_cuboid_cuboid_vertices (rng : &mut XorShiftRng)
-> [vertex::Vert3dOrientationScaleColor; 4]
{
let overlapping_cuboids = generate_overlapping_cuboid_cuboid (rng);
let resolved_cuboids =
resolve_overlapping_cuboid_cuboid (&overlapping_cuboids);
let initial_cuboids = [
overlapping_cuboids[0].0.clone(), overlapping_cuboids[1].0.clone()
];
vertices_cuboid_cuboid (initial_cuboids, resolved_cuboids)
}
fn generate_overlapping_cuboid_cuboid (rng : &mut XorShiftRng)
-> [(object::Static, f64); 2]
{
use rand::RngExt;
use geometry::shape;
use geometry::shape::Stereometric;
let extents_a = [
rng.random_range (CUBOID_EXTENT_MIN..CUBOID_EXTENT_MAX),
rng.random_range (CUBOID_EXTENT_MIN..CUBOID_EXTENT_MAX),
rng.random_range (CUBOID_EXTENT_MIN..CUBOID_EXTENT_MAX)
];
let extents_b = [
rng.random_range (CUBOID_EXTENT_MIN..CUBOID_EXTENT_MAX),
rng.random_range (CUBOID_EXTENT_MIN..CUBOID_EXTENT_MAX),
rng.random_range (CUBOID_EXTENT_MIN..CUBOID_EXTENT_MAX)
];
let density_a = rng.random_range (DENSITY_MIN..DENSITY_MAX);
let density_b = rng.random_range (DENSITY_MIN..DENSITY_MAX);
let shape_a = shape::Cuboid::noisy (extents_a);
let shape_b = shape::Cuboid::noisy (extents_b);
let volume_a = *shape_a.volume();
let volume_b = *shape_b.volume();
let mass_a = density_a * volume_a;
let mass_b = density_b * volume_b;
let min_distance = math::Vector3::new (
extents_a[0] + extents_b[0],
extents_a[1] + extents_b[1],
extents_a[2] + extents_b[2]
);
let initial_aabb = geometry::Aabb3::<f64>::with_minmax (
POSITION_MIN.into(), POSITION_MAX.into()).unwrap();
let cuboid_a = object::Static {
position: component::Position (initial_aabb.rand_point (rng)),
bound: component::Bound (shape::Bounded::from (shape_a).into()),
material: component::MATERIAL_STONE,
collidable: true
};
let within_min : math::Point3 <f64> = cuboid_a.position.0 - min_distance;
let within_max : math::Point3 <f64> = cuboid_a.position.0 + min_distance;
let within_aabb = geometry::Aabb3::<f64>::with_minmax (within_min, within_max)
.unwrap();
let cuboid_b = object::Static {
position: component::Position (within_aabb.rand_point (rng)),
bound: component::Bound (shape::Bounded::from (shape_b).into()),
material: component::MATERIAL_STONE,
collidable: true
};
[(cuboid_a, mass_a), (cuboid_b, mass_b)]
}
fn resolve_overlapping_cuboid_cuboid (
overlapping_cuboids : &[(object::Static, f64); 2]
) -> [object::Static; 2] {
log::trace!("resolve overlapping cuboid pair...");
let (cuboid_a, mass_a) = &overlapping_cuboids[0];
let (cuboid_b, mass_b) = &overlapping_cuboids[1];
let proximity = collision::Proximity::query (cuboid_a, cuboid_b);
log::trace!(" proximity: {proximity:?}");
debug_assert_eq!(
proximity.relation(), collision::proximity::Relation::Intersect);
let total_mass = mass_a + mass_b;
let relative_mass_a = mass_a / total_mass;
let relative_mass_b = mass_b / total_mass;
let move_a = 1.0 - relative_mass_a;
let move_b = 1.0 - relative_mass_b;
let resolved_cuboid_a = object::Static {
position: component::Position (cuboid_a.position.0 +
2.0 * move_a * proximity.half_axis +
0.25 * collision::CONTACT_DISTANCE * *proximity.normal),
.. cuboid_a.clone()
};
let resolved_cuboid_b = object::Static {
position: component::Position (cuboid_b.position.0 -
2.0 * move_b * proximity.half_axis -
0.25 * collision::CONTACT_DISTANCE * *proximity.normal),
.. cuboid_b.clone()
};
let resolved_proximity =
collision::Proximity::query (&resolved_cuboid_a, &resolved_cuboid_b);
debug_assert_eq!(
resolved_proximity.relation(),
collision::proximity::Relation::Contact
);
log::trace!("...resolve overlapping cuboid pair");
[
resolved_cuboid_a, resolved_cuboid_b
]
}
fn vertices_cuboid_cuboid (
overlapping_cuboids : [object::Static; 2],
resolved_cuboids : [object::Static; 2]
) -> [vertex::Vert3dOrientationScaleColor; 4] {
[
cuboid_to_vertex (&overlapping_cuboids[0], color::DEBUG_RED),
cuboid_to_vertex (&overlapping_cuboids[1], color::DEBUG_YELLOW),
cuboid_to_vertex (&resolved_cuboids[0], color::DEBUG_GREEN),
cuboid_to_vertex (&resolved_cuboids[1], color::DEBUG_BLUE)
]
}
pub fn init_capsule_cuboid (
render_context : &mut Render <render::resource::Default>,
rng : &mut XorShiftRng
) {
use render::resource::draw3d;
init (render_context, rng);
let grid_vertex_data = render::resource::Default::debug_grid_vertices();
let (capsule_vertex_data, cuboid_vertex_data) =
next_capsule_cuboid_vertices (rng);
let aabb_lines = Some (&cuboid_vertex_data[..]);
let meshes = {
let mut v = VecMap::with_capacity (2);
assert!(v.insert (draw3d::MeshId::Grid as usize, &grid_vertex_data[..])
.is_none());
assert!(v.insert (draw3d::MeshId::Capsule as usize, &capsule_vertex_data[..])
.is_none());
v
};
render_context.resource.draw3d.instance_vertices_set (
&render_context.glium_display,
draw3d::InstancesInit {
billboards: VecMap::default(),
aabb_lines,
meshes,
.. Default::default()
}
);
program::print_testbed_modes_prompt();
println!("linear-sim penetration capsule v. cuboid test");
println!(" press 'Tab' to generate test cases");
}
pub fn draw_capsule_cuboid (
render_context : &Render <render::resource::Default>,
glium_frame : &mut glium::Frame,
center_indices_list : &glium::IndexBuffer <u32>,
) {
use glium::Surface;
render::Resource::draw_3d (render_context, glium_frame);
let viewport = &render_context.viewports()[render::resource::MAIN_VIEWPORT];
let draw_parameters_viewport = glium::DrawParameters {
viewport: Some (*viewport.rect()),
.. Default::default()
};
let draw_parameters_write_depth = glium::DrawParameters {
depth: glium::Depth {
test: glium::DepthTest::IfLess,
write: true,
.. Default::default()
},
.. draw_parameters_viewport
};
let (transform_mat_world_to_view, projection_mat_perspective) =
viewport.camera3d().unwrap().view_mats();
let uniforms = glium::uniform!{
uni_transform_mat_view: transform_mat_world_to_view,
uni_projection_mat_perspective: projection_mat_perspective,
uni_color: color::rgba_u8_to_rgba_f32 (color::DEBUG_LIGHT_BLUE)
};
glium_frame.draw (
render_context.resource.draw3d.instance_vertices(),
center_indices_list,
&render_context.resource.shader_programs() [
shader::ProgramId::WorldSpace3dUniColor as usize
],
&uniforms,
&draw_parameters_write_depth
).unwrap();
}
pub fn next_capsule_cuboid (
render_context : &mut Render <render::resource::Default>,
rng : &mut XorShiftRng
) {
use render::resource::draw3d::MeshId;
let (capsule_vertex_data, cuboid_vertex_data) =
next_capsule_cuboid_vertices (rng);
render_context.resource.draw3d.instance_vertices_mesh_write (
MeshId::Capsule as usize, &capsule_vertex_data);
render_context.resource.draw3d
.instance_vertices_aabb_lines_write (&cuboid_vertex_data);
}
fn next_capsule_cuboid_vertices (rng : &mut XorShiftRng)
-> (
[vertex::Vert3dOrientationScaleColor; 2],
[vertex::Vert3dOrientationScaleColor; 2]
) {
let overlapping_capsule_cuboid = generate_overlapping_capsule_cuboid (rng);
let resolved_capsule_cuboid =
resolve_overlapping_capsule_cuboid (&overlapping_capsule_cuboid);
let initial_capsule_cuboid = [
overlapping_capsule_cuboid[0].0.clone(),
overlapping_capsule_cuboid[1].0.clone()
];
vertices_capsule_cuboid (initial_capsule_cuboid, resolved_capsule_cuboid)
}
fn generate_overlapping_capsule_cuboid (rng : &mut XorShiftRng)
-> [(object::Static, f64); 2]
{
use rand::RngExt;
use geometry::shape;
use geometry::shape::Stereometric;
let radius_a = rng.random_range (CAPSULE_RADIUS_MIN..CAPSULE_RADIUS_MAX);
let half_height_a = rng.random_range (
CAPSULE_HALF_HEIGHT_MIN..CAPSULE_HALF_HEIGHT_MAX);
let extents_b = [
rng.random_range (CUBOID_EXTENT_MIN..CUBOID_EXTENT_MAX),
rng.random_range (CUBOID_EXTENT_MIN..CUBOID_EXTENT_MAX),
rng.random_range (CUBOID_EXTENT_MIN..CUBOID_EXTENT_MAX)
];
let density_a = rng.random_range (DENSITY_MIN..DENSITY_MAX);
let density_b = rng.random_range (DENSITY_MIN..DENSITY_MAX);
let shape_a = shape::Capsule::noisy (radius_a, half_height_a);
let shape_b = shape::Cuboid::noisy (extents_b);
let volume_a = *shape_a.volume();
let volume_b = *shape_b.volume();
let mass_a = density_a * volume_a;
let mass_b = density_b * volume_b;
let min_distance = radius_a;
let min_dim = f64::sqrt ((min_distance * min_distance) / 3.0);
let initial_aabb = geometry::Aabb3::<f64>::with_minmax (
POSITION_MIN.into(), POSITION_MAX.into()).unwrap();
let capsule_a = object::Static {
position: component::Position (initial_aabb.rand_point (rng)),
bound: component::Bound (shape::Bounded::from (shape_a).into()),
material: component::MATERIAL_STONE,
collidable: true
};
let within_min : math::Point3 <f64> = capsule_a.position.0
- math::Vector3::new (min_dim, min_dim, min_dim + half_height_a);
let within_max : math::Point3 <f64> = capsule_a.position.0
+ math::Vector3::new (min_dim, min_dim, min_dim + half_height_a);
let within_aabb = geometry::Aabb3::<f64>::with_minmax (within_min, within_max)
.unwrap();
let cuboid_b = object::Static {
position: component::Position (within_aabb.rand_point (rng)),
bound: component::Bound (shape::Bounded::from (shape_b).into()),
material: component::MATERIAL_STONE,
collidable: true
};
[(capsule_a, mass_a), (cuboid_b, mass_b)]
}
fn resolve_overlapping_capsule_cuboid (
overlapping_capsule_cuboid : &[(object::Static, f64); 2]
) -> [object::Static; 2] {
log::trace!("resolve overlapping capsule pair...");
let (capsule_a, mass_a) = &overlapping_capsule_cuboid[0];
let (cuboid_b, mass_b) = &overlapping_capsule_cuboid[1];
let proximity = collision::Proximity::query (capsule_a, cuboid_b);
log::trace!(" proximity: {proximity:?}");
debug_assert_eq!(
proximity.relation(), collision::proximity::Relation::Intersect);
let total_mass = mass_a + mass_b;
let relative_mass_a = mass_a / total_mass;
let relative_mass_b = mass_b / total_mass;
let move_a = 1.0 - relative_mass_a;
let move_b = 1.0 - relative_mass_b;
let resolved_capsule_a = object::Static {
position: component::Position (capsule_a.position.0 +
2.0 * move_a * proximity.half_axis +
0.25 * collision::CONTACT_DISTANCE * *proximity.normal),
.. capsule_a.clone()
};
let resolved_cuboid_b = object::Static {
position: component::Position (cuboid_b.position.0 -
2.0 * move_b * proximity.half_axis -
0.25 * collision::CONTACT_DISTANCE * *proximity.normal),
.. cuboid_b.clone()
};
let resolved_proximity =
collision::Proximity::query (&resolved_capsule_a, &resolved_cuboid_b);
debug_assert_eq!(
resolved_proximity.relation(), collision::proximity::Relation::Contact);
log::trace!("...resolve overlapping capsule pair");
[ resolved_capsule_a, resolved_cuboid_b ]
}
fn vertices_capsule_cuboid (
overlapping_capsule_cuboid : [object::Static; 2],
resolved_capsule_cuboid : [object::Static; 2]
) -> (
[vertex::Vert3dOrientationScaleColor; 2],
[vertex::Vert3dOrientationScaleColor; 2]
) {
([
capsule_to_vertex (&overlapping_capsule_cuboid[0], color::DEBUG_RED),
capsule_to_vertex (&resolved_capsule_cuboid[0], color::DEBUG_GREEN)
], [
cuboid_to_vertex (&overlapping_capsule_cuboid[1], color::DEBUG_YELLOW),
cuboid_to_vertex (&resolved_capsule_cuboid[1], color::DEBUG_BLUE)
])
}
pub fn init_triangle_triangle (
render_context : &mut Render <render::resource::Default>,
rng : &mut XorShiftRng
) {
use render::resource::draw3d;
init (render_context, rng);
let grid_vertex_data = render::resource::Default::debug_grid_vertices();
let triangle_vertex_data = next_triangle_triangle_vertices (rng);
let buffer = render_context.resource.draw3d.user_buffers.entry (0).or_insert_with (||
glium::VertexBuffer::empty_dynamic (&render_context.glium_display, 16)
.unwrap());
buffer.write (triangle_vertex_data.as_slice());
assert!(render_context.resource.draw3d.user_draw.insert (0, draw3d::UserDraw {
buffer_key: draw3d::UserBufferKey (0),
range: 0..12,
primitive_type: glium::index::PrimitiveType::TrianglesList,
shader_program_id: shader::ProgramId::WorldSpace3dColor,
draw_pass: draw3d::DrawPass::Depth
}).is_none());
assert!(render_context.resource.draw3d.user_draw.insert (1, draw3d::UserDraw {
buffer_key: draw3d::UserBufferKey (0),
range: 12..16,
primitive_type: glium::index::PrimitiveType::LinesList,
shader_program_id: shader::ProgramId::WorldSpace3dColor,
draw_pass: draw3d::DrawPass::Depth
}).is_none());
let meshes = {
let mut v = VecMap::with_capacity (1);
assert!(v.insert (draw3d::MeshId::Grid as usize, &grid_vertex_data[..]).is_none());
v
};
render_context.resource.draw3d.instance_vertices_set (
&render_context.glium_display,
draw3d::InstancesInit {
billboards: VecMap::default(),
meshes,
.. Default::default()
}
);
program::print_testbed_modes_prompt();
println!("linear-sim penetration triangle v. triangle test");
println!(" press 'Tab' to generate test cases");
}
pub fn next_triangle_triangle (
render_context : &mut Render <render::resource::Default>,
rng : &mut XorShiftRng
) {
let triangle_vertex_data = next_triangle_triangle_vertices (rng);
render_context.resource.draw3d.user_buffers[0].write (triangle_vertex_data.as_slice());
}
fn next_triangle_triangle_vertices (rng : &mut XorShiftRng)
-> [vertex::Vert3dColor; 16]
{
println!("triangle/triangle example: {}",
EXAMPLE_COUNTER.fetch_add (1, atomic::Ordering::SeqCst));
let overlapping_triangles = generate_overlapping_triangle_triangle (rng);
let resolved_triangles =
resolve_overlapping_triangle_triangle (&overlapping_triangles);
vertices_triangle_triangle (overlapping_triangles, resolved_triangles)
}
fn generate_overlapping_triangle_triangle (rng : &mut XorShiftRng)
-> [geometry::Triangle3 <f64>; 2]
{
use rand::RngExt;
let within_aabb = geometry::Aabb3::<f64>::with_minmax (
POSITION_MIN.into(), POSITION_MAX.into()).unwrap();
let triangle_a = geometry::Triangle3::noisy (
within_aabb.rand_point (rng),
within_aabb.rand_point (rng),
within_aabb.rand_point (rng));
let p = {
let s = rng.random_range (0.0..1.0);
let t = rng.random_range (0.0..1.0 - s);
triangle_a.barycentric ([1.0 - s - t, s, t].into())
};
let triangle_b = if rng.random_bool (0.5) {
let q = {
let s = rng.random_range (0.0..1.0);
let t = rng.random_range (0.0..1.0 - s);
triangle_a.barycentric ([1.0 - s - t, s, t].into())
};
let a = within_aabb.rand_point (rng);
let b = {
let ap = p - a;
p + ap * rng.random_range (0.0..TRIANGLE_SCALE / 2.0)
};
let c = {
let aq = q - a;
q + aq * rng.random_range (0.0..TRIANGLE_SCALE / 2.0)
};
geometry::Triangle3::noisy (a, b, c)
} else {
let edge = triangle_a.edges()[rng.random_range (0usize..3)];
let q = edge.point (math::Normalized::noisy (rng.random_range (0.0..1.0)));
let dir_aabb = geometry::Aabb3::with_minmax (
math::Vector3::broadcast (-TRIANGLE_SCALE).into(),
math::Vector3::broadcast (TRIANGLE_SCALE).into()
).unwrap();
let pq = q - p;
let plane_normal = {
let rand_dir = dir_aabb.rand_point (rng).0;
pq.cross (rand_dir).normalized()
};
let a = {
let rand_dir = dir_aabb.rand_point (rng).0;
q + plane_normal.cross (rand_dir)
};
let ap = p - a;
let b = p + ap * rng.random_range (0.0..TRIANGLE_SCALE / 2.0);
let ab = b - a;
let c_base = a + ab * rng.random_range (0.0..1.0);
let ac_base = c_base - a;
let ap_len = ap.magnitude();
let pq_len = pq.magnitude();
let ac_base_len = ac_base.magnitude();
let min_c_height = if ac_base_len <= ap_len {
let bp_len = (p - b).magnitude();
let bc_base_len = (c_base - b).magnitude();
(pq_len * bc_base_len) / bp_len
} else {
(pq_len * ac_base_len) / ap_len
};
let c_height = rng.random_range (min_c_height..min_c_height + TRIANGLE_SCALE / 1.5);
let c = c_base + pq.normalized() * c_height;
geometry::Triangle3::noisy (a, b, c)
};
[triangle_a, triangle_b]
}
fn resolve_overlapping_triangle_triangle (
overlapping_triangles : &[geometry::Triangle3 <f64>; 2]
) -> [geometry::Triangle3 <f64>; 2] {
log::trace!("resolve overlapping triangle pair...");
let [mut triangle_a, mut triangle_b] = *overlapping_triangles;
let proximity =
collision::Proximity::query_triangle_triangle (&triangle_a, &triangle_b);
log::trace!(" proximity: {proximity:?}");
debug_assert_eq!(proximity.relation(), collision::proximity::Relation::Intersect);
let axis_normal = math::Unit3::normalize (proximity.half_axis);
triangle_a.translate (proximity.half_axis
+ *axis_normal * 0.25 * collision::CONTACT_DISTANCE);
triangle_b.translate (-proximity.half_axis
- *axis_normal * 0.25 * collision::CONTACT_DISTANCE);
if cfg!(debug_assertions) {
let proximity =
collision::Proximity::query_triangle_triangle (&triangle_a, &triangle_b);
debug_assert_eq!(proximity.relation(), collision::proximity::Relation::Contact);
}
[triangle_a, triangle_b]
}
fn vertices_triangle_triangle (
overlapping_triangles : [geometry::Triangle3 <f64>; 2],
resolved_triangles : [geometry::Triangle3 <f64>; 2]
) -> [vertex::Vert3dColor; 16] {
let barycenter_coords = math::Vector3::broadcast (1.0/3.0);
let [overlap_a, overlap_b] = overlapping_triangles;
let [resolved_a, resolved_b] = resolved_triangles;
let point_to_vertex = |point : math::Point3 <f64>, color| vertex::Vert3dColor {
position: point.0.numcast().unwrap().into_array(),
color: color::rgba_u8_to_rgba_f32 (color)
};
let overlap_a_points = overlap_a.points().map (|point|
point_to_vertex (point, color::DEBUG_RED));
let overlap_b_points = overlap_b.points().map (|point|
point_to_vertex (point, color::DEBUG_YELLOW));
let resolved_a_points = resolved_a.points().map (|point|
point_to_vertex (point, color::DEBUG_GREEN));
let resolved_b_points = resolved_b.points().map (|point|
point_to_vertex (point, color::DEBUG_BLUE));
let overlap_barycenter_a =
point_to_vertex (overlap_a.barycentric (barycenter_coords), color::DEBUG_LIGHT_BLUE);
let overlap_barycenter_b =
point_to_vertex (overlap_b.barycentric (barycenter_coords), color::DEBUG_LIGHT_BLUE);
let resolved_barycenter_a =
point_to_vertex (resolved_a.barycentric (barycenter_coords), color::DEBUG_LIGHT_BLUE);
let resolved_barycenter_b =
point_to_vertex (resolved_b.barycentric (barycenter_coords), color::DEBUG_LIGHT_BLUE);
[
overlap_a_points[0],
overlap_a_points[1],
overlap_a_points[2],
overlap_b_points[0],
overlap_b_points[1],
overlap_b_points[2],
resolved_a_points[0],
resolved_a_points[1],
resolved_a_points[2],
resolved_b_points[0],
resolved_b_points[1],
resolved_b_points[2],
overlap_barycenter_a,
resolved_barycenter_a,
overlap_barycenter_b,
resolved_barycenter_b
]
}
pub fn init_hull_hull (
render_context : &mut Render <render::resource::Default>,
rng : &mut XorShiftRng
) {
use render::resource::draw3d;
init (render_context, rng);
let grid_vertex_data = render::resource::Default::debug_grid_vertices();
let (triangle_vertex_data, line_vertex_data) = next_hull_hull_vertices (rng);
const MAX_POINTS_PER_HULL : usize = 3 * (HULL_MAX_POINTS * 2 - 4) as usize;
const MAX_TRIANGLE_POINTS : usize = 2 * MAX_POINTS_PER_HULL;
const MAX_LINE_POINTS : usize = 2 * MAX_POINTS_PER_HULL * 2;
let triangles_buffer = render_context.resource.draw3d.user_buffers.entry (0)
.or_insert_with (|| glium::VertexBuffer::empty_dynamic (
&render_context.glium_display, MAX_TRIANGLE_POINTS
).unwrap());
triangles_buffer.slice (..triangle_vertex_data.len()).unwrap()
.write (&triangle_vertex_data);
let lines_buffer = render_context.resource.draw3d.user_buffers.entry (1)
.or_insert_with (|| glium::VertexBuffer::empty_dynamic (
&render_context.glium_display, MAX_LINE_POINTS
).unwrap());
lines_buffer.slice (..line_vertex_data.len()).unwrap().write (&line_vertex_data);
assert!(render_context.resource.draw3d.user_draw.insert (0, draw3d::UserDraw {
buffer_key: draw3d::UserBufferKey (0),
range: 0..triangle_vertex_data.len() as u32,
primitive_type: glium::index::PrimitiveType::TrianglesList,
shader_program_id: shader::ProgramId::WorldSpace3dColor,
draw_pass: draw3d::DrawPass::Depth
}).is_none());
assert!(render_context.resource.draw3d.user_draw.insert (1, draw3d::UserDraw {
buffer_key: draw3d::UserBufferKey (1),
range: 0..line_vertex_data.len() as u32,
primitive_type: glium::index::PrimitiveType::LinesList,
shader_program_id: shader::ProgramId::WorldSpace3dColor,
draw_pass: draw3d::DrawPass::Depth
}).is_none());
let meshes = {
let mut v = VecMap::with_capacity (1);
assert!(v.insert (draw3d::MeshId::Grid as usize, &grid_vertex_data[..]).is_none());
v
};
render_context.resource.draw3d.instance_vertices_set (
&render_context.glium_display,
draw3d::InstancesInit {
billboards: VecMap::default(),
meshes,
.. Default::default()
}
);
program::print_testbed_modes_prompt();
println!("linear-sim penetration hull v. hull test");
println!(" press 'Tab' to generate test cases");
}
pub fn next_hull_hull (
render_context : &mut Render <render::resource::Default>,
rng : &mut XorShiftRng
) {
let (triangle_vertex_data, line_vertex_data) = next_hull_hull_vertices (rng);
render_context.resource.draw3d.user_buffers[0].slice (..triangle_vertex_data.len())
.unwrap().write (&triangle_vertex_data);
render_context.resource.draw3d.user_draw[0].range = 0..triangle_vertex_data.len()
as u32;
render_context.resource.draw3d.user_buffers[1].slice (..line_vertex_data.len())
.unwrap().write (&line_vertex_data);
render_context.resource.draw3d.user_draw[1].range = 0..line_vertex_data.len() as u32;
}
fn next_hull_hull_vertices (rng : &mut XorShiftRng)
-> (Vec <vertex::Vert3dColor>, Vec <vertex::Vert3dColor>)
{
log::debug!("hull/hull generation: {}",
EXAMPLE_COUNTER.fetch_add (1, atomic::Ordering::SeqCst));
let overlapping_hulls = generate_overlapping_hull_hull (rng);
let resolved_hulls = resolve_overlapping_hull_hull (overlapping_hulls.each_ref());
vertices_hull_hull (overlapping_hulls, resolved_hulls)
}
fn generate_overlapping_hull_hull (rng : &mut XorShiftRng)
-> [geometry::shape::Hull <f64>; 2]
{
use rand::RngExt;
use rand_distr::Distribution;
let [triangle_a, triangle_b] = loop {
let [triangle_a, triangle_b] = generate_overlapping_triangle_triangle (rng);
let aabb = geometry::Aabb3::with_minmax (
[-50.0, -50.0, 0.0].into(),
[ 50.0, 50.0, 10.0].into()).unwrap();
let triangle_a = triangle_a.points().map(|p| aabb.clamp (p)).try_into().unwrap();
let triangle_b = triangle_b.points().map(|p| aabb.clamp (p)).try_into().unwrap();
if geometry::intersect::discrete_triangle3 (triangle_a, triangle_b) {
break [triangle_a, triangle_b]
}
};
let mut gen_hull = |triangle : geometry::Triangle3 <f64>| {
let mut points = triangle.points().to_vec();
let aabb = geometry::Aabb3::with_minmax (
[-5.0, -5.0, 0.0].into(),
[ 5.0, 5.0, 10.0].into()).unwrap();
let npoints = rng.random_range (1..HULL_MAX_POINTS as usize - 3);
let std_normal = rand_distr::StandardNormal;
let randf = |rng : &mut XorShiftRng| {
let x : f64 = std_normal.sample (rng);
let y : f64 = std_normal.sample (rng);
x / y
};
let mut rand_vec = || math::vector3 (randf (rng), randf (rng), randf (rng));
for _ in 0..npoints {
points.push (aabb.clamp (points.last().unwrap() + rand_vec()));
}
geometry::Hull3::from_points_with_mesh (&points).unwrap()
};
[gen_hull (triangle_a), gen_hull (triangle_b)]
}
fn resolve_overlapping_hull_hull (overlapping_hulls : [&geometry::shape::Hull <f64>; 2])
-> [geometry::Hull3 <f64>; 2]
{
use geometry::Primitive;
type Object = (component::Position, component::Bound);
log::trace!("resolve overlapping hull pair...");
let [(hull_a, mesh_a), (hull_b, mesh_b)] = overlapping_hulls.map (Clone::clone);
let mk_object = |hull|
( component::Position::origin(),
geometry::shape::Variant::from (geometry::shape::Bounded::Hull (hull)).into()
);
let unmk_object = |object : Object| -> geometry::shape::Hull <f64> {
geometry::shape::Bounded::try_from (object.1.0).unwrap().try_into().unwrap()
};
let obj_a = mk_object ((hull_a, mesh_a));
let obj_b = mk_object ((hull_b, mesh_b));
let proximity = collision::Proximity::query_gjk (&obj_a, &obj_b);
let (mut hull_a, mesh_a) = unmk_object (obj_a);
let (mut hull_b, mesh_b) = unmk_object (obj_b);
log::trace!(" proximity: {proximity:?}");
debug_assert_eq!(proximity.relation(), collision::proximity::Relation::Intersect);
let axis_normal = math::Unit3::normalize (proximity.half_axis);
hull_a.translate (proximity.half_axis
+ *axis_normal * 0.25 * collision::CONTACT_DISTANCE);
hull_b.translate (-proximity.half_axis
- *axis_normal * 0.25 * collision::CONTACT_DISTANCE);
if cfg!(debug_assertions) {
let proximity = collision::Proximity::query_gjk (
&mk_object ((hull_a.clone(), mesh_a)),
&mk_object ((hull_b.clone(), mesh_b)));
debug_assert_eq!(proximity.relation(), collision::proximity::Relation::Contact);
}
[hull_a, hull_b]
}
fn vertices_hull_hull (
overlapping_hulls : [geometry::shape::Hull <f64>; 2],
resolved_hulls : [geometry::Hull3 <f64>; 2]
) -> (Vec <vertex::Vert3dColor>, Vec <vertex::Vert3dColor>) {
use math::VectorSpace;
let light_dir = math::Unit3::normalize ([1.0, 1.0, -2.0].into());
let point_to_vertex = |point : math::Point3 <f64>, color| vertex::Vert3dColor {
position: point.0.numcast().unwrap().into_array(),
color
};
let mut triangles = vec![];
let mut lines = vec![];
let mut do_triangles = |
hull : &geometry::Hull3 <f64>,
mesh : &geometry::mesh::VertexEdgeTriangleMesh,
base_color
| {
for triangle in mesh.triangles().values() {
let triangle = hull.triangle (triangle);
let color = {
let normal = triangle.unit_normal();
let dot = -light_dir.dot (*normal);
let normalized_dot = math::Normalized::noisy ((dot as f32 + 1.0) * 0.5);
let eigengrau =
math::Vector4::from (color::rgba_u8_to_rgba_f32 (color::EIGENGRAU));
let base_color = math::Vector4::from (color::rgba_u8_to_rgba_f32 (base_color));
math::Vector4::interpolate (eigengrau, base_color, normalized_dot).into_array()
};
for point in triangle.points() {
triangles.push (point_to_vertex (point, color));
}
}
};
let mut do_lines = |
hull : &geometry::Hull3 <f64>,
mesh : &geometry::mesh::VertexEdgeTriangleMesh,
color
| {
for edge in mesh.edges().values() {
let edge = hull.edge (edge);
let color = color::rgba_u8_to_rgba_f32 (color);
for point in edge.points() {
lines.push (point_to_vertex (point, color));
}
}
};
do_lines (&overlapping_hulls[0].0, &overlapping_hulls[0].1, color::RED);
do_lines (&overlapping_hulls[1].0, &overlapping_hulls[1].1, color::YELLOW);
do_triangles (&resolved_hulls[0], &overlapping_hulls[0].1, color::GREEN);
do_triangles (&resolved_hulls[1], &overlapping_hulls[1].1, color::BLUE);
(triangles, lines)
}
fn init (
render_context : &mut Render <render::resource::Default>,
rng : &mut XorShiftRng
) {
use rand::SeedableRng;
program::reset_render_context (render_context);
const HALF_GRID_DIMS : f32 = 0.5 * render::resource::draw3d::MESH_GRID_DIMS as f32;
render_context.camera3d_position_set ([0.0, -2.0*HALF_GRID_DIMS, 4.0].into());
*rng = XorShiftRng::seed_from_u64 (0);
EXAMPLE_COUNTER.store (0, atomic::Ordering::SeqCst);
}
fn capsule_to_vertex (object : &object::Static, color : [u8; 4])
-> vertex::Vert3dOrientationScaleColor
{
use geometry::shape;
let component::Position (position) = object.position;
let capsule = {
match object.bound {
component::Bound (shape::Variant::Bounded (shape::Bounded::Capsule (capsule)))
=> capsule,
_ => panic!("capsule to vertex called on a non-capsule object: {object:?}")
}
};
vertex::Vert3dOrientationScaleColor {
position: position.0.numcast().unwrap().into_array(),
orientation: math::Matrix3::identity().into_col_arrays(),
scale: [
*capsule.radius as f32,
*capsule.radius as f32,
*(capsule.radius + capsule.half_height) as f32
],
color: color::rgba_u8_to_rgba_f32 (color)
}
}
fn cuboid_to_vertex (object : &object::Static, color : [u8; 4])
-> vertex::Vert3dOrientationScaleColor
{
use geometry::shape;
let component::Position (position) = object.position;
let cuboid = {
match object.bound {
component::Bound (shape::Variant::Bounded (shape::Bounded::Cuboid (cuboid)))
=> cuboid,
_ => panic!("cuboid to vertex called on a non-cuboid object: {object:?}")
}
};
vertex::Vert3dOrientationScaleColor {
position: position.0.numcast().unwrap().into_array(),
orientation: math::Matrix3::identity().into_col_arrays(),
scale: cuboid.extents.map (|e| *e).numcast().unwrap().into_array(),
color: color::rgba_u8_to_rgba_f32 (color)
}
}