<commit_before>extern crate cgmath;
extern crate froggy;
#[macro_use]
extern crate gfx;
extern crate gfx_window_glutin;
extern crate glutin;

use std::{time};
use cgmath::{Angle, EuclideanSpace, One, Rotation3, Transform, Zero};
use gfx::traits::{Device, Factory, FactoryExt};

pub type ColorFormat = gfx::format::Srgba8;
pub type DepthFormat = gfx::format::DepthStencil;

gfx_defines!{
    vertex Vertex {
        pos: [f32; 4] = "a_Pos",
        normal: [i8; 4] = "a_Normal",
    }

    vertex Instance {
        offset_scale: [f32; 4] = "a_OffsetScale",
        rotation: [f32; 4] = "a_Rotation",
        color: [f32; 4] = "a_Color",
    }

    constant Globals {
        projection: [[f32; 4]; 4] = "u_Projection",
        camera_pos: [f32; 4] = "u_CameraPos",
        light_pos: [f32; 4] = "u_LightPos",
        light_color: [f32; 4] = "u_LightColor",
    }

    pipeline pipe {
        vert_buf: gfx::VertexBuffer<Vertex> = (),
        inst_buf: gfx::InstanceBuffer<Instance> = (),
        globals: gfx::ConstantBuffer<Globals> = "b_Globals",
        out_color: gfx::RenderTarget<ColorFormat> = "Target0",
        out_depth: gfx::DepthTarget<DepthFormat> = gfx::preset::depth::LESS_EQUAL_WRITE,
    }
}

fn vertex(x: i8, y: i8, z: i8, nx: i8, ny: i8, nz: i8) -> Vertex {
    Vertex {
        pos: [x as f32, y as f32, z as f32, 1.0],
        normal: [nx, ny, nz, 0],
    }
}

fn create_geometry<R: gfx::Resources, F: gfx::Factory<R>>(factory: &mut F)
                   -> (gfx::handle::Buffer<R, Vertex>, gfx::Slice<R>)
{
    let vertices = [
        // bottom
        vertex(1, 1, 0, 0, 0, -1),
        vertex(1, -1, 0, 0, 0, -1),
        vertex(-1, -1, 0, 0, 0, -1),
        vertex(-1, 1, 0, 0, 0, -1),
        // top
        vertex(1, 1, 2, 0, 0, 1),
        vertex(1, -1, 2, 0, 0, 1),
        vertex(-1, -1, 2, 0, 0, 1),
        vertex(-1, 1, 2, 0, 0, 1),
        // left
        vertex(-1, 1, 2, -1, 0, 0),
        vertex(-1, 1, 0, -1, 0, 0),
        vertex(-1, -1, 0, -1, 0, 0),
        vertex(-1, -1, 2, -1, 0, 0),
        // right
        vertex(1, 1, 2, 1, 0, 0),
        vertex(1, 1, 0, 1, 0, 0),
        vertex(1, -1, 0, 1, 0, 0),
        vertex(1, -1, 2, 1, 0, 0),
        // front
        vertex(1, -1, 2, 0, -1, 0),
        vertex(1, -1, 0, 0, -1, 0),
        vertex(-1, -1, 0, 0, -1, 0),
        vertex(-1, -1, 2, 0, -1, 0),
        // back
        vertex(1, 1, 2, 0, 1, 0),
        vertex(1, 1, 0, 0, 1, 0),
        vertex(-1, 1, 0, 0, 1, 0),
        vertex(-1, 1, 2, 0, 1, 0),
    ];

    let indices = [
        0u16, 1, 2, 0, 2, 3,
        4, 5, 6, 4, 6, 7,
        8, 9, 10, 8, 10, 11,
        12, 13, 14, 12, 14, 15,
        16, 17, 18, 16, 18, 19,
        20, 21, 22, 20, 22, 23,
    ];

    factory.create_vertex_buffer_with_slice(&vertices, &indices[..])
}

type Space = cgmath::Decomposed<cgmath::Vector3<f32>, cgmath::Quaternion<f32>>;

struct Level {
    speed: f32,
}

struct Material {
    color: [f32; 4],
}

struct Node {
    local: Space,
    world: Space,
    parent: Option<froggy::Pointer<Node>>,
}

struct Cube {
    node: froggy::Pointer<Node>,
    material: froggy::Pointer<Material>,
    level: froggy::Pointer<Level>,
}

fn create_cubes(nodes: &mut froggy::Storage<Node>,
                materials: &froggy::Storage<Material>,
                levels: &froggy::Storage<Level>)
                -> Vec<Cube>
{
    let root_mat = materials.iter().next().unwrap();
    let root_lev = levels.iter().next().unwrap();
    let mut list = vec![
        Cube {
            node: nodes.create(Node {
                local: Space {
                    disp: cgmath::Vector3::zero(),
                    rot: cgmath::Quaternion::one(),
                    scale: 2.0,
                },
                world: Space::one(),
                parent: None,
            }),
            material: materials.pin(&root_mat),
            level: levels.pin(&root_lev),
        }
    ];
    struct Stack {
        parent: froggy::Pointer<Node>,
        level_id: usize,
    }
    let mut stack = vec![
        Stack {
            parent: list[0].node.clone(),
            level_id: 1,
        }
    ];

    let axis = [cgmath::Vector3::unit_z(),
                cgmath::Vector3::unit_x(), -cgmath::Vector3::unit_x(),
                cgmath::Vector3::unit_y(), -cgmath::Vector3::unit_y()];
    let children: Vec<_> = axis.iter().map(|&axe| {
        Space {
            disp: cgmath::vec3(0.0, 0.0, 1.0),
            rot: cgmath::Quaternion::from_axis_angle(axe, cgmath::Rad::turn_div_4()),
            scale: 1.0,
        }.concat(&Space {
            disp: cgmath::vec3(0.0, 0.0, 1.0),
            rot: cgmath::Quaternion::one(),
            scale: 0.4,
        })
    }).collect();

    while let Some(next) = stack.pop() {
        //HACK: materials are indexed the same way as levels
        // it's fine for demostration purposes
        let material = match materials.iter().nth(next.level_id) {
            Some(item) => materials.pin(&item),
            None => continue,
        };
        let level = match levels.iter().nth(next.level_id) {
            Some(item) => levels.pin(&item),
            None => continue,
        };
        for child in &children {
            let cube = Cube {
                node: nodes.create(Node {
                    local: child.clone(),
                    world: Space::one(),
                    parent: Some(next.parent.clone()),
                }),
                material: material.clone(),
                level: level.clone(),
            };
            stack.push(Stack {
                parent: cube.node.clone(),
                level_id: next.level_id + 1,
            });
            list.push(cube);
        }
    }

    list
}

fn make_globals(camera_pos: cgmath::Point3<f32>, aspect: f32) -> Globals {
    let mx_proj = {
        let fovy = cgmath::Deg(60.0);
        let perspective = cgmath::perspective(fovy, aspect, 1.0, 100.0);
        let focus = cgmath::Point3::new(0.0, 0.0, 3.0);
        let view = cgmath::Matrix4::look_at(
            camera_pos, focus, cgmath::Vector3::unit_z());
        perspective * view
    };
    Globals {
        projection: mx_proj.into(),
        camera_pos: camera_pos.to_vec().extend(1.0).into(),
        light_pos: [0.0, -10.0, 10.0, 1.0],
        light_color: [1.0, 1.0, 1.0, 1.0],
    }
}

const COLORS: [[f32; 4]; 6] = [
    [1.0, 1.0, 0.5, 1.0],
    [0.5, 0.5, 1.0, 1.0],
    [0.5, 1.0, 0.5, 1.0],
    [1.0, 0.5, 0.5, 1.0],
    [0.5, 1.0, 1.0, 1.0],
    [1.0, 0.5, 1.0, 1.0],
];

const SPEEDS: [f32; 6] = [
    0.7, -1.0, 1.3, -1.6, 1.9, -2.2
];

fn main() {
    // feed Froggy
    let mut node_store = froggy::Storage::new();
    let material_store: froggy::Storage<_> =
        COLORS.iter().map(|&color| Material { color: color }).collect();
    let level_store: froggy::Storage<_> =
        SPEEDS.iter().map(|&speed| Level { speed: speed }).collect();

    //Note: we populated the storages, but the returned pointers are already dropped.
    // Thus, all will be lost if we lock for writing now, but locking for reading retains the
    // contents, and cube creation will add references to them, so they will stay alive.
    let mut cubes = create_cubes(&mut node_store, &material_store, &level_store);
    println!("Initialized {} cubes on {} levels", cubes.len(), SPEEDS.len());

    // init window and graphics
    let builder = glutin::WindowBuilder::new()
        .with_title("Froggy Cube-ception".to_string())
        .with_vsync();
    let event_loop = glutin::EventsLoop::new();
    let (window, mut device, mut factory, main_color, main_depth) =
        gfx_window_glutin::init::<ColorFormat, DepthFormat>(builder, &event_loop);
    let mut encoder = gfx::Encoder::from(factory.create_command_buffer());

    let (cube_vbuf, mut cube_slice) = create_geometry(&mut factory);
    let cube_ibuf = factory.create_buffer(cubes.len(),
        gfx::buffer::Role::Vertex, gfx::memory::Usage::Dynamic, gfx::Bind::empty()
        ).unwrap();

    // init global parameters
    let camera_pos = cgmath::Point3::new(-1.8, -8.0, 3.0);
    let globals = {
        let (w, h, _, _) = main_color.get_dimensions();
        make_globals(camera_pos, w as f32 / h as f32)
    };
    let global_buf = factory.create_constant_buffer(1);
    encoder.update_constant_buffer(&global_buf, &globals);

    // init pipeline states
    let pso = factory.create_pipeline_simple(
        include_bytes!("vert.glsl"),
        include_bytes!("frag.glsl"),
        pipe::new()
        ).unwrap();
    let mut data = pipe::Data {
        vert_buf: cube_vbuf,
        inst_buf: cube_ibuf,
        globals: global_buf,
        out_color: main_color,
        out_depth: main_depth,
    };

    let mut instances = Vec::new();
    let mut moment = time::Instant::now();
    let mut running = true;

    while running {
        // process events
        event_loop.poll_events(|glutin::Event::WindowEvent {event, ..}| {
            use glutin::WindowEvent as Event;
            match event {
                Event::KeyboardInput(_, _, Some(glutin::VirtualKeyCode::Escape), _) |
                Event::Closed => running = false,
                Event::Resized(width, height) => {
                    gfx_window_glutin::update_views(&window, &mut data.out_color, &mut data.out_depth);
                    let globals = make_globals(camera_pos, width as f32 / height as f32);
                    encoder.update_constant_buffer(&data.globals, &globals);
                },
                _ => (),
            }
        });

        // get time delta
        let duration = moment.elapsed();
        let delta = duration.as_secs() as f32 + (duration.subsec_nanos() as f32 * 1.0e-9);
        moment = time::Instant::now();

        // Note: the following 3 passes could be combined into one for efficiency.
        // This is not the goal of the demo though. It is made with an assuption that
        // the scenegraph and its logic is separate from the main game, and that
        // more processing is done in parallel.

        // animate local spaces
        for cube in cubes.iter_mut() {
            let level = &level_store[&cube.level];
            let angle = cgmath::Rad(delta * level.speed);
            node_store[&cube.node].local.concat_self(&Space {
                disp: cgmath::Vector3::zero(),
                rot: cgmath::Quaternion::from_angle_z(angle),
                scale: 1.0,
            });
        }

        // re-compute world spaces, using streaming iteration
        {
            let mut cursor = node_store.cursor();
            while let Some(mut item) = cursor.next() {
                item.world = match item.parent {
                    Some(ref parent) => item.look_back(parent).unwrap().world.concat(&item.local),
                    None => item.local,
                };
            }
        }

        // update instancing CPU info
        instances.clear();
        for cube in cubes.iter_mut() {
            let material = &material_store[&cube.material];
            let space = &node_store[&cube.node].world;
            instances.push(Instance {
                offset_scale: space.disp.extend(space.scale).into(),
                rotation: space.rot.v.extend(space.rot.s).into(),
                color: material.color,
            });
        }

        // update instancing GPU info
        cube_slice.instances = Some((instances.len() as gfx::InstanceCount, 0));
        encoder.update_buffer(&data.inst_buf, &instances, 0).unwrap();

        // draw -- start
        encoder.clear_depth(&data.out_depth, 1.0);
        encoder.clear(&data.out_color, [0.1, 0.2, 0.3, 1.0]);
        encoder.draw(&cube_slice, &pso, &data);
        // draw -- end
        encoder.flush(&mut device);
        window.swap_buffers().unwrap();
        device.cleanup();
    }
}
<commit_msg>Updated cube demo to work with iterators<commit_after>extern crate cgmath;
extern crate froggy;
#[macro_use]
extern crate gfx;
extern crate gfx_window_glutin;
extern crate glutin;

use std::{time};
use cgmath::{Angle, EuclideanSpace, One, Rotation3, Transform, Zero};
use gfx::traits::{Device, Factory, FactoryExt};

pub type ColorFormat = gfx::format::Srgba8;
pub type DepthFormat = gfx::format::DepthStencil;

gfx_defines!{
    vertex Vertex {
        pos: [f32; 4] = "a_Pos",
        normal: [i8; 4] = "a_Normal",
    }

    vertex Instance {
        offset_scale: [f32; 4] = "a_OffsetScale",
        rotation: [f32; 4] = "a_Rotation",
        color: [f32; 4] = "a_Color",
    }

    constant Globals {
        projection: [[f32; 4]; 4] = "u_Projection",
        camera_pos: [f32; 4] = "u_CameraPos",
        light_pos: [f32; 4] = "u_LightPos",
        light_color: [f32; 4] = "u_LightColor",
    }

    pipeline pipe {
        vert_buf: gfx::VertexBuffer<Vertex> = (),
        inst_buf: gfx::InstanceBuffer<Instance> = (),
        globals: gfx::ConstantBuffer<Globals> = "b_Globals",
        out_color: gfx::RenderTarget<ColorFormat> = "Target0",
        out_depth: gfx::DepthTarget<DepthFormat> = gfx::preset::depth::LESS_EQUAL_WRITE,
    }
}

fn vertex(x: i8, y: i8, z: i8, nx: i8, ny: i8, nz: i8) -> Vertex {
    Vertex {
        pos: [x as f32, y as f32, z as f32, 1.0],
        normal: [nx, ny, nz, 0],
    }
}

fn create_geometry<R: gfx::Resources, F: gfx::Factory<R>>(factory: &mut F)
                   -> (gfx::handle::Buffer<R, Vertex>, gfx::Slice<R>)
{
    let vertices = [
        // bottom
        vertex(1, 1, 0, 0, 0, -1),
        vertex(1, -1, 0, 0, 0, -1),
        vertex(-1, -1, 0, 0, 0, -1),
        vertex(-1, 1, 0, 0, 0, -1),
        // top
        vertex(1, 1, 2, 0, 0, 1),
        vertex(1, -1, 2, 0, 0, 1),
        vertex(-1, -1, 2, 0, 0, 1),
        vertex(-1, 1, 2, 0, 0, 1),
        // left
        vertex(-1, 1, 2, -1, 0, 0),
        vertex(-1, 1, 0, -1, 0, 0),
        vertex(-1, -1, 0, -1, 0, 0),
        vertex(-1, -1, 2, -1, 0, 0),
        // right
        vertex(1, 1, 2, 1, 0, 0),
        vertex(1, 1, 0, 1, 0, 0),
        vertex(1, -1, 0, 1, 0, 0),
        vertex(1, -1, 2, 1, 0, 0),
        // front
        vertex(1, -1, 2, 0, -1, 0),
        vertex(1, -1, 0, 0, -1, 0),
        vertex(-1, -1, 0, 0, -1, 0),
        vertex(-1, -1, 2, 0, -1, 0),
        // back
        vertex(1, 1, 2, 0, 1, 0),
        vertex(1, 1, 0, 0, 1, 0),
        vertex(-1, 1, 0, 0, 1, 0),
        vertex(-1, 1, 2, 0, 1, 0),
    ];

    let indices = [
        0u16, 1, 2, 0, 2, 3,
        4, 5, 6, 4, 6, 7,
        8, 9, 10, 8, 10, 11,
        12, 13, 14, 12, 14, 15,
        16, 17, 18, 16, 18, 19,
        20, 21, 22, 20, 22, 23,
    ];

    factory.create_vertex_buffer_with_slice(&vertices, &indices[..])
}

type Space = cgmath::Decomposed<cgmath::Vector3<f32>, cgmath::Quaternion<f32>>;

struct Level {
    speed: f32,
}

struct Material {
    color: [f32; 4],
}

struct Node {
    local: Space,
    world: Space,
    parent: Option<froggy::Pointer<Node>>,
}

struct Cube {
    node: froggy::Pointer<Node>,
    material: froggy::Pointer<Material>,
    level: froggy::Pointer<Level>,
}

fn create_cubes(nodes: &mut froggy::Storage<Node>,
                materials: &froggy::Storage<Material>,
                levels: &froggy::Storage<Level>)
                -> Vec<Cube>
{
    let mut mat_iter = materials.iter_all();
    let mut lev_iter = levels.iter_all();
    let root_mat = mat_iter.next().unwrap();
    let root_lev = lev_iter.next().unwrap();
    let mut list = vec![
        Cube {
            node: nodes.create(Node {
                local: Space {
                    disp: cgmath::Vector3::zero(),
                    rot: cgmath::Quaternion::one(),
                    scale: 2.0,
                },
                world: Space::one(),
                parent: None,
            }),
            material: materials.pin(&root_mat),
            level: levels.pin(&root_lev),
        }
    ];
    struct Stack<'a> {
        parent: froggy::Pointer<Node>,
        mat_iter: froggy::Iter<'a, Material>,
        lev_iter: froggy::Iter<'a, Level>,
    }
    let mut stack = vec![
        Stack {
            parent: list[0].node.clone(),
            mat_iter,
            lev_iter,
        }
    ];

    let axis = [cgmath::Vector3::unit_z(),
                cgmath::Vector3::unit_x(), -cgmath::Vector3::unit_x(),
                cgmath::Vector3::unit_y(), -cgmath::Vector3::unit_y()];
    let children: Vec<_> = axis.iter().map(|&axe| {
        Space {
            disp: cgmath::vec3(0.0, 0.0, 1.0),
            rot: cgmath::Quaternion::from_axis_angle(axe, cgmath::Rad::turn_div_4()),
            scale: 1.0,
        }.concat(&Space {
            disp: cgmath::vec3(0.0, 0.0, 1.0),
            rot: cgmath::Quaternion::one(),
            scale: 0.4,
        })
    }).collect();

    while let Some(mut next) = stack.pop() {
        //HACK: materials are indexed the same way as levels
        // it's fine for demostration purposes
        let material = match next.mat_iter.next() {
            Some(item) => materials.pin(&item),
            None => continue,
        };
        let level = match next.lev_iter.next() {
            Some(item) => levels.pin(&item),
            None => continue,
        };
        for child in &children {
            let cube = Cube {
                node: nodes.create(Node {
                    local: child.clone(),
                    world: Space::one(),
                    parent: Some(next.parent.clone()),
                }),
                material: material.clone(),
                level: level.clone(),
            };
            stack.push(Stack {
                parent: cube.node.clone(),
                mat_iter: next.mat_iter.clone(),
                lev_iter: next.lev_iter.clone(),
            });
            list.push(cube);
        }
    }

    list
}

fn make_globals(camera_pos: cgmath::Point3<f32>, aspect: f32) -> Globals {
    let mx_proj = {
        let fovy = cgmath::Deg(60.0);
        let perspective = cgmath::perspective(fovy, aspect, 1.0, 100.0);
        let focus = cgmath::Point3::new(0.0, 0.0, 3.0);
        let view = cgmath::Matrix4::look_at(
            camera_pos, focus, cgmath::Vector3::unit_z());
        perspective * view
    };
    Globals {
        projection: mx_proj.into(),
        camera_pos: camera_pos.to_vec().extend(1.0).into(),
        light_pos: [0.0, -10.0, 10.0, 1.0],
        light_color: [1.0, 1.0, 1.0, 1.0],
    }
}

const COLORS: [[f32; 4]; 6] = [
    [1.0, 1.0, 0.5, 1.0],
    [0.5, 0.5, 1.0, 1.0],
    [0.5, 1.0, 0.5, 1.0],
    [1.0, 0.5, 0.5, 1.0],
    [0.5, 1.0, 1.0, 1.0],
    [1.0, 0.5, 1.0, 1.0],
];

const SPEEDS: [f32; 6] = [
    0.7, -1.0, 1.3, -1.6, 1.9, -2.2
];

fn main() {
    // feed Froggy
    let mut node_store = froggy::Storage::new();
    let material_store: froggy::Storage<_> =
        COLORS.iter().map(|&color| Material { color: color }).collect();
    let level_store: froggy::Storage<_> =
        SPEEDS.iter().map(|&speed| Level { speed: speed }).collect();

    //Note: we populated the storages, but the returned pointers are already dropped.
    // Thus, all will be lost if we lock for writing now, but locking for reading retains the
    // contents, and cube creation will add references to them, so they will stay alive.
    let mut cubes = create_cubes(&mut node_store, &material_store, &level_store);
    println!("Initialized {} cubes on {} levels", cubes.len(), SPEEDS.len());

    // init window and graphics
    let builder = glutin::WindowBuilder::new()
        .with_title("Froggy Cube-ception".to_string())
        .with_vsync();
    let event_loop = glutin::EventsLoop::new();
    let (window, mut device, mut factory, main_color, main_depth) =
        gfx_window_glutin::init::<ColorFormat, DepthFormat>(builder, &event_loop);
    let mut encoder = gfx::Encoder::from(factory.create_command_buffer());

    let (cube_vbuf, mut cube_slice) = create_geometry(&mut factory);
    let cube_ibuf = factory.create_buffer(cubes.len(),
        gfx::buffer::Role::Vertex, gfx::memory::Usage::Dynamic, gfx::Bind::empty()
        ).unwrap();

    // init global parameters
    let camera_pos = cgmath::Point3::new(-1.8, -8.0, 3.0);
    let globals = {
        let (w, h, _, _) = main_color.get_dimensions();
        make_globals(camera_pos, w as f32 / h as f32)
    };
    let global_buf = factory.create_constant_buffer(1);
    encoder.update_constant_buffer(&global_buf, &globals);

    // init pipeline states
    let pso = factory.create_pipeline_simple(
        include_bytes!("vert.glsl"),
        include_bytes!("frag.glsl"),
        pipe::new()
        ).unwrap();
    let mut data = pipe::Data {
        vert_buf: cube_vbuf,
        inst_buf: cube_ibuf,
        globals: global_buf,
        out_color: main_color,
        out_depth: main_depth,
    };

    let mut instances = Vec::new();
    let mut moment = time::Instant::now();
    let mut running = true;

    while running {
        // process events
        event_loop.poll_events(|glutin::Event::WindowEvent {event, ..}| {
            use glutin::WindowEvent as Event;
            match event {
                Event::KeyboardInput(_, _, Some(glutin::VirtualKeyCode::Escape), _) |
                Event::Closed => running = false,
                Event::Resized(width, height) => {
                    gfx_window_glutin::update_views(&window, &mut data.out_color, &mut data.out_depth);
                    let globals = make_globals(camera_pos, width as f32 / height as f32);
                    encoder.update_constant_buffer(&data.globals, &globals);
                },
                _ => (),
            }
        });

        // get time delta
        let duration = moment.elapsed();
        let delta = duration.as_secs() as f32 + (duration.subsec_nanos() as f32 * 1.0e-9);
        moment = time::Instant::now();

        // Note: the following 3 passes could be combined into one for efficiency.
        // This is not the goal of the demo though. It is made with an assuption that
        // the scenegraph and its logic is separate from the main game, and that
        // more processing is done in parallel.

        // animate local spaces
        for cube in cubes.iter_mut() {
            let level = &level_store[&cube.level];
            let angle = cgmath::Rad(delta * level.speed);
            node_store[&cube.node].local.concat_self(&Space {
                disp: cgmath::Vector3::zero(),
                rot: cgmath::Quaternion::from_angle_z(angle),
                scale: 1.0,
            });
        }

        // re-compute world spaces, using streaming iteration
        {
            let mut cursor = node_store.cursor();
            while let Some(mut item) = cursor.next() {
                item.world = match item.parent {
                    Some(ref parent) => item.look_back(parent).unwrap().world.concat(&item.local),
                    None => item.local,
                };
            }
        }

        // update instancing CPU info
        instances.clear();
        for cube in cubes.iter_mut() {
            let material = &material_store[&cube.material];
            let space = &node_store[&cube.node].world;
            instances.push(Instance {
                offset_scale: space.disp.extend(space.scale).into(),
                rotation: space.rot.v.extend(space.rot.s).into(),
                color: material.color,
            });
        }

        // update instancing GPU info
        cube_slice.instances = Some((instances.len() as gfx::InstanceCount, 0));
        encoder.update_buffer(&data.inst_buf, &instances, 0).unwrap();

        // draw -- start
        encoder.clear_depth(&data.out_depth, 1.0);
        encoder.clear(&data.out_color, [0.1, 0.2, 0.3, 1.0]);
        encoder.draw(&cube_slice, &pso, &data);
        // draw -- end
        encoder.flush(&mut device);
        window.swap_buffers().unwrap();
        device.cleanup();
    }
}
<|endoftext|>


<commit_before>use super::{CellId, Mesh, PointId};
use crate::shapes::Shape;
use crate::StrError;
use russell_lab::{sort2, sort4};
use std::collections::HashMap;

/// Aliases (usize,usize) as the key of edges
///
/// # Note
///
/// Since the local numbering scheme runs over "corners" first, we can compare
/// edges using only two points; i.e., the middle points don't matter.
pub type EdgeKey = (usize, usize);

/// Aliases (usize,usize,usize,usize) as the key of faces
///
/// # Note
///
/// If a face has at most 3 points, the fourth entry in the key will be set to the total number of points.
/// In this way, we can compare 4-node (or more nodes) faces with each other. Since the local numbering
/// scheme runs over the "corners" first, the middle points don't matter.
pub type FaceKey = (usize, usize, usize, usize);

/// Holds the point ids of an edge (an entity belonging to a solid cell in 2D or a face in 3D)
#[derive(Clone, Debug)]
pub struct Edge {
    /// List of points defining this edge; in the right order (unsorted)
    pub points: Vec<PointId>,
}

/// Holds the point ids of a face (an entity belonging to a solid cell in 3D)
#[derive(Clone, Debug)]
pub struct Face {
    /// List of points defining this face; in the right order (unsorted)
    pub points: Vec<PointId>,
}

/// Extracts all edges (internal and boundary) in 2D
///
/// # Input
///
/// * `mesh` -- the Mesh
/// * `shapes` -- the shapes of cells (len == cells.len())
///
/// # Output
///
/// * Returns a map relating edge keys to `Vec<(cell_id, e)>` where:
///     - `cell_id` -- the id of the cell sharing the edge
///     - `e` -- is the cell's local edge index
pub fn all_edges_2d(mesh: &Mesh, shapes: &Vec<Shape>) -> Result<HashMap<EdgeKey, Vec<(CellId, usize)>>, StrError> {
    if mesh.space_ndim != 2 {
        return Err("this function works in 2D only");
    }
    let mut edges: HashMap<EdgeKey, Vec<(CellId, usize)>> = HashMap::new();
    mesh.cells.iter().zip(shapes).for_each(|(cell, shape)| {
        if shape.geo_ndim == 2 {
            for e in 0..shape.nedge {
                let mut edge_key: EdgeKey = (
                    cell.points[shape.edge_node_id(e, 0)],
                    cell.points[shape.edge_node_id(e, 1)],
                );
                sort2(&mut edge_key);
                let data = edges.entry(edge_key).or_insert(Vec::new());
                data.push((cell.id, e));
            }
        }
    });
    Ok(edges)
}

/// Extracts all faces (internal and boundary) in 3D
///
/// # Input
///
/// * `mesh` -- the Mesh
/// * `shapes` -- the shapes of cells (len == cells.len())
///
/// # Output
///
/// * Returns a map relating face keys to `Vec<(cell_id, f)>` where:
///     - `cell_id` -- the id of the cell sharing the face
///     - `f` -- is the cell's local face index
pub fn all_faces_3d(mesh: &Mesh, shapes: &Vec<Shape>) -> Result<HashMap<FaceKey, Vec<(CellId, usize)>>, StrError> {
    if mesh.space_ndim != 3 {
        return Err("this function works in 3D only");
    }
    let mut faces: HashMap<FaceKey, Vec<(CellId, usize)>> = HashMap::new();
    mesh.cells.iter().zip(shapes).for_each(|(cell, shape)| {
        if shape.geo_ndim == 3 {
            for f in 0..shape.nface {
                let mut face_key: FaceKey = if shape.face_nnode > 3 {
                    (
                        cell.points[shape.face_node_id(f, 0)],
                        cell.points[shape.face_node_id(f, 1)],
                        cell.points[shape.face_node_id(f, 2)],
                        cell.points[shape.face_node_id(f, 3)],
                    )
                } else {
                    (
                        cell.points[shape.face_node_id(f, 0)],
                        cell.points[shape.face_node_id(f, 1)],
                        cell.points[shape.face_node_id(f, 2)],
                        mesh.points.len(),
                    )
                };
                sort4(&mut face_key);
                let data = faces.entry(face_key).or_insert(Vec::new());
                data.push((cell.id, f));
            }
        }
    });
    Ok(faces)
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#[cfg(test)]
mod tests {
    use super::{all_edges_2d, all_faces_3d, Edge, Face};
    use crate::mesh::{Samples, Shapes};
    use crate::StrError;

    #[test]
    fn all_edges_2d_works() -> Result<(), StrError> {
        //  3---------2---------5
        //  |         |         |
        //  |   [0]   |   [1]   |
        //  |         |         |
        //  0---------1---------4
        let mesh = Samples::two_quads_horizontal();
        let shapes = Shapes::new(&mesh)?;
        let edges = all_edges_2d(&mesh, &shapes)?;
        let mut keys: Vec<_> = edges.keys().collect();
        keys.sort();
        assert_eq!(keys, [&(0, 1), &(0, 3), &(1, 2), &(1, 4), &(2, 3), &(2, 5), &(4, 5)]);
        assert_eq!(edges.get(&(0, 1)).unwrap(), &[(0, 0)]);
        assert_eq!(edges.get(&(0, 3)).unwrap(), &[(0, 3)]);
        assert_eq!(edges.get(&(1, 2)).unwrap(), &[(0, 1), (1, 3)]);
        assert_eq!(edges.get(&(1, 4)).unwrap(), &[(1, 0)]);
        assert_eq!(edges.get(&(2, 3)).unwrap(), &[(0, 2)]);
        assert_eq!(edges.get(&(2, 5)).unwrap(), &[(1, 2)]);
        assert_eq!(edges.get(&(4, 5)).unwrap(), &[(1, 1)]);
        Ok(())
    }

    #[test]
    fn all_edges_2d_mixed_works() -> Result<(), StrError> {
        //           4---------3
        //           |         |
        //           |   [1]   |
        //           |         |
        //  0--------1---------2
        let mesh = Samples::mixed_shapes_2d();
        let shapes = Shapes::new(&mesh)?;
        let edges = all_edges_2d(&mesh, &shapes)?;
        let mut keys: Vec<_> = edges.keys().collect();
        keys.sort();
        assert_eq!(keys, [&(1, 2), &(1, 4), &(2, 3), &(3, 4)]);
        assert_eq!(edges.get(&(1, 2)).unwrap(), &[(1, 0)]);
        assert_eq!(edges.get(&(1, 4)).unwrap(), &[(1, 3)]);
        assert_eq!(edges.get(&(2, 3)).unwrap(), &[(1, 1)]);
        assert_eq!(edges.get(&(3, 4)).unwrap(), &[(1, 2)]);
        Ok(())
    }

    #[test]
    fn all_faces_3d_works() -> Result<(), StrError> {
        //      8-------------11
        //     /.             /|
        //    / .            / |
        //   /  .           /  |
        //  /   .          /   |       id = 1
        // 9-------------10    |       attribute_id = 2
        // |    .         |    |
        // |    4---------|----7
        // |   /.         |   /|
        // |  / .         |  / |
        // | /  .         | /  |
        // |/   .         |/   |
        // 5--------------6    |       id = 0
        // |    .         |    |       attribute_id = 1
        // |    0---------|----3
        // |   /          |   /
        // |  /           |  /
        // | /            | /
        // |/             |/
        // 1--------------2
        let mesh = Samples::two_cubes_vertical();
        let shapes = Shapes::new(&mesh)?;
        let faces = all_faces_3d(&mesh, &shapes)?;
        let mut keys: Vec<_> = faces.keys().collect();
        keys.sort();
        assert_eq!(
            keys,
            [
                &(0, 1, 2, 3),
                &(0, 1, 4, 5),
                &(0, 3, 4, 7),
                &(1, 2, 5, 6),
                &(2, 3, 6, 7),
                &(4, 5, 6, 7),
                &(4, 5, 8, 9),
                &(4, 7, 8, 11),
                &(5, 6, 9, 10),
                &(6, 7, 10, 11),
                &(8, 9, 10, 11),
            ]
        );
        assert_eq!(faces.get(&(0, 1, 2, 3)).unwrap(), &[(0, 4)]);
        assert_eq!(faces.get(&(0, 1, 4, 5)).unwrap(), &[(0, 2)]);
        assert_eq!(faces.get(&(0, 3, 4, 7)).unwrap(), &[(0, 0)]);
        assert_eq!(faces.get(&(1, 2, 5, 6)).unwrap(), &[(0, 1)]);
        assert_eq!(faces.get(&(2, 3, 6, 7)).unwrap(), &[(0, 3)]);
        assert_eq!(faces.get(&(4, 5, 6, 7)).unwrap(), &[(0, 5), (1, 4)]);
        assert_eq!(faces.get(&(4, 5, 8, 9)).unwrap(), &[(1, 2)]);
        assert_eq!(faces.get(&(4, 7, 8, 11)).unwrap(), &[(1, 0)]);
        assert_eq!(faces.get(&(5, 6, 9, 10)).unwrap(), &[(1, 1)]);
        assert_eq!(faces.get(&(6, 7, 10, 11)).unwrap(), &[(1, 3)]);
        assert_eq!(faces.get(&(8, 9, 10, 11)).unwrap(), &[(1, 5)]);
        Ok(())
    }

    #[test]
    fn all_faces_3d_mixed_works() -> Result<(), StrError> {
        //                       4------------7-----------10
        //                      /.           /|            |
        //                     / .          / |            |
        //                    /  .         /  |            |
        //                   /   .        /   |            |
        //                  5------------6    |            |
        //                  |    .       |`.  |            |
        //                  |    0-------|--`.3------------9
        //                  |   /        |   /`.          /
        //                  |  /         |  /   `.       /
        //                  | /          | /      `.    /
        //                  |/           |/         `. /
        //  12-----11-------1------------2------------8
        //
        let mesh = Samples::mixed_shapes_3d();
        let shapes = Shapes::new(&mesh)?;
        let faces = all_faces_3d(&mesh, &shapes)?;
        let mut keys: Vec<_> = faces.keys().collect();
        keys.sort();
        assert_eq!(
            keys,
            [
                &(0, 1, 2, 3),
                &(0, 1, 4, 5),
                &(0, 3, 4, 7),
                &(1, 2, 5, 6),
                &(2, 3, 6, 7),
                &(2, 3, 6, 13),
                &(2, 3, 8, 13),
                &(2, 6, 8, 13),
                &(3, 6, 8, 13),
                &(4, 5, 6, 7),
            ]
        );
        assert_eq!(faces.get(&(0, 1, 2, 3)).unwrap(), &[(0, 4)]);
        assert_eq!(faces.get(&(0, 1, 4, 5)).unwrap(), &[(0, 2)]);
        assert_eq!(faces.get(&(0, 3, 4, 7)).unwrap(), &[(0, 0)]);
        assert_eq!(faces.get(&(1, 2, 5, 6)).unwrap(), &[(0, 1)]);
        assert_eq!(faces.get(&(2, 3, 6, 7)).unwrap(), &[(0, 3)]);
        assert_eq!(faces.get(&(2, 3, 6, 13)).unwrap(), &[(1, 0)]);
        assert_eq!(faces.get(&(2, 3, 8, 13)).unwrap(), &[(1, 2)]);
        assert_eq!(faces.get(&(2, 6, 8, 13)).unwrap(), &[(1, 1)]);
        assert_eq!(faces.get(&(3, 6, 8, 13)).unwrap(), &[(1, 3)]);
        assert_eq!(faces.get(&(4, 5, 6, 7)).unwrap(), &[(0, 5)]);
        Ok(())
    }

    #[test]
    fn derive_works() {
        let edge = Edge {
            points: vec![10, 20, 33],
        };
        let face = Face {
            points: vec![1, 2, 3, 4],
        };
        let edge_clone = edge.clone();
        let face_clone = face.clone();
        assert_eq!(format!("{:?}", edge), "Edge { points: [10, 20, 33] }");
        assert_eq!(format!("{:?}", face), "Face { points: [1, 2, 3, 4] }");
        assert_eq!(edge_clone.points.len(), 3);
        assert_eq!(face_clone.points.len(), 4);
    }
}
<commit_msg>Add test<commit_after>use super::{CellId, Mesh, PointId};
use crate::shapes::Shape;
use crate::StrError;
use russell_lab::{sort2, sort4};
use std::collections::HashMap;

/// Aliases (usize,usize) as the key of edges
///
/// # Note
///
/// Since the local numbering scheme runs over "corners" first, we can compare
/// edges using only two points; i.e., the middle points don't matter.
pub type EdgeKey = (usize, usize);

/// Aliases (usize,usize,usize,usize) as the key of faces
///
/// # Note
///
/// If a face has at most 3 points, the fourth entry in the key will be set to the total number of points.
/// In this way, we can compare 4-node (or more nodes) faces with each other. Since the local numbering
/// scheme runs over the "corners" first, the middle points don't matter.
pub type FaceKey = (usize, usize, usize, usize);

/// Holds the point ids of an edge (an entity belonging to a solid cell in 2D or a face in 3D)
#[derive(Clone, Debug)]
pub struct Edge {
    /// List of points defining this edge; in the right order (unsorted)
    pub points: Vec<PointId>,
}

/// Holds the point ids of a face (an entity belonging to a solid cell in 3D)
#[derive(Clone, Debug)]
pub struct Face {
    /// List of points defining this face; in the right order (unsorted)
    pub points: Vec<PointId>,
}

/// Extracts all edges (internal and boundary) in 2D
///
/// # Input
///
/// * `mesh` -- the Mesh
/// * `shapes` -- the shapes of cells (len == cells.len())
///
/// # Output
///
/// * Returns a map relating edge keys to `Vec<(cell_id, e)>` where:
///     - `cell_id` -- the id of the cell sharing the edge
///     - `e` -- is the cell's local edge index
pub fn all_edges_2d(mesh: &Mesh, shapes: &Vec<Shape>) -> Result<HashMap<EdgeKey, Vec<(CellId, usize)>>, StrError> {
    if mesh.space_ndim != 2 {
        return Err("this function works in 2D only");
    }
    let mut edges: HashMap<EdgeKey, Vec<(CellId, usize)>> = HashMap::new();
    mesh.cells.iter().zip(shapes).for_each(|(cell, shape)| {
        if shape.geo_ndim == 2 {
            for e in 0..shape.nedge {
                let mut edge_key: EdgeKey = (
                    cell.points[shape.edge_node_id(e, 0)],
                    cell.points[shape.edge_node_id(e, 1)],
                );
                sort2(&mut edge_key);
                let data = edges.entry(edge_key).or_insert(Vec::new());
                data.push((cell.id, e));
            }
        }
    });
    Ok(edges)
}

/// Extracts all faces (internal and boundary) in 3D
///
/// # Input
///
/// * `mesh` -- the Mesh
/// * `shapes` -- the shapes of cells (len == cells.len())
///
/// # Output
///
/// * Returns a map relating face keys to `Vec<(cell_id, f)>` where:
///     - `cell_id` -- the id of the cell sharing the face
///     - `f` -- is the cell's local face index
pub fn all_faces_3d(mesh: &Mesh, shapes: &Vec<Shape>) -> Result<HashMap<FaceKey, Vec<(CellId, usize)>>, StrError> {
    if mesh.space_ndim != 3 {
        return Err("this function works in 3D only");
    }
    let mut faces: HashMap<FaceKey, Vec<(CellId, usize)>> = HashMap::new();
    mesh.cells.iter().zip(shapes).for_each(|(cell, shape)| {
        if shape.geo_ndim == 3 {
            for f in 0..shape.nface {
                let mut face_key: FaceKey = if shape.face_nnode > 3 {
                    (
                        cell.points[shape.face_node_id(f, 0)],
                        cell.points[shape.face_node_id(f, 1)],
                        cell.points[shape.face_node_id(f, 2)],
                        cell.points[shape.face_node_id(f, 3)],
                    )
                } else {
                    (
                        cell.points[shape.face_node_id(f, 0)],
                        cell.points[shape.face_node_id(f, 1)],
                        cell.points[shape.face_node_id(f, 2)],
                        mesh.points.len(),
                    )
                };
                sort4(&mut face_key);
                let data = faces.entry(face_key).or_insert(Vec::new());
                data.push((cell.id, f));
            }
        }
    });
    Ok(faces)
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#[cfg(test)]
mod tests {
    use super::{all_edges_2d, all_faces_3d, Edge, Face};
    use crate::mesh::{Mesh, Samples, Shapes};
    use crate::StrError;

    #[test]
    fn capture_some_wrong_input() {
        let mesh = Mesh {
            space_ndim: 1,
            points: Vec::new(),
            cells: Vec::new(),
        };
        assert_eq!(
            all_edges_2d(&mesh, &Vec::new()).err(),
            Some("this function works in 2D only")
        );
        assert_eq!(
            all_faces_3d(&mesh, &Vec::new()).err(),
            Some("this function works in 3D only")
        );
    }

    #[test]
    fn all_edges_2d_works() -> Result<(), StrError> {
        //  3---------2---------5
        //  |         |         |
        //  |   [0]   |   [1]   |
        //  |         |         |
        //  0---------1---------4
        let mesh = Samples::two_quads_horizontal();
        let shapes = Shapes::new(&mesh)?;
        let edges = all_edges_2d(&mesh, &shapes)?;
        let mut keys: Vec<_> = edges.keys().collect();
        keys.sort();
        assert_eq!(keys, [&(0, 1), &(0, 3), &(1, 2), &(1, 4), &(2, 3), &(2, 5), &(4, 5)]);
        assert_eq!(edges.get(&(0, 1)).unwrap(), &[(0, 0)]);
        assert_eq!(edges.get(&(0, 3)).unwrap(), &[(0, 3)]);
        assert_eq!(edges.get(&(1, 2)).unwrap(), &[(0, 1), (1, 3)]);
        assert_eq!(edges.get(&(1, 4)).unwrap(), &[(1, 0)]);
        assert_eq!(edges.get(&(2, 3)).unwrap(), &[(0, 2)]);
        assert_eq!(edges.get(&(2, 5)).unwrap(), &[(1, 2)]);
        assert_eq!(edges.get(&(4, 5)).unwrap(), &[(1, 1)]);
        Ok(())
    }

    #[test]
    fn all_edges_2d_mixed_works() -> Result<(), StrError> {
        //           4---------3
        //           |         |
        //           |   [1]   |
        //           |         |
        //  0--------1---------2
        let mesh = Samples::mixed_shapes_2d();
        let shapes = Shapes::new(&mesh)?;
        let edges = all_edges_2d(&mesh, &shapes)?;
        let mut keys: Vec<_> = edges.keys().collect();
        keys.sort();
        assert_eq!(keys, [&(1, 2), &(1, 4), &(2, 3), &(3, 4)]);
        assert_eq!(edges.get(&(1, 2)).unwrap(), &[(1, 0)]);
        assert_eq!(edges.get(&(1, 4)).unwrap(), &[(1, 3)]);
        assert_eq!(edges.get(&(2, 3)).unwrap(), &[(1, 1)]);
        assert_eq!(edges.get(&(3, 4)).unwrap(), &[(1, 2)]);
        Ok(())
    }

    #[test]
    fn all_faces_3d_works() -> Result<(), StrError> {
        //      8-------------11
        //     /.             /|
        //    / .            / |
        //   /  .           /  |
        //  /   .          /   |       id = 1
        // 9-------------10    |       attribute_id = 2
        // |    .         |    |
        // |    4---------|----7
        // |   /.         |   /|
        // |  / .         |  / |
        // | /  .         | /  |
        // |/   .         |/   |
        // 5--------------6    |       id = 0
        // |    .         |    |       attribute_id = 1
        // |    0---------|----3
        // |   /          |   /
        // |  /           |  /
        // | /            | /
        // |/             |/
        // 1--------------2
        let mesh = Samples::two_cubes_vertical();
        let shapes = Shapes::new(&mesh)?;
        let faces = all_faces_3d(&mesh, &shapes)?;
        let mut keys: Vec<_> = faces.keys().collect();
        keys.sort();
        assert_eq!(
            keys,
            [
                &(0, 1, 2, 3),
                &(0, 1, 4, 5),
                &(0, 3, 4, 7),
                &(1, 2, 5, 6),
                &(2, 3, 6, 7),
                &(4, 5, 6, 7),
                &(4, 5, 8, 9),
                &(4, 7, 8, 11),
                &(5, 6, 9, 10),
                &(6, 7, 10, 11),
                &(8, 9, 10, 11),
            ]
        );
        assert_eq!(faces.get(&(0, 1, 2, 3)).unwrap(), &[(0, 4)]);
        assert_eq!(faces.get(&(0, 1, 4, 5)).unwrap(), &[(0, 2)]);
        assert_eq!(faces.get(&(0, 3, 4, 7)).unwrap(), &[(0, 0)]);
        assert_eq!(faces.get(&(1, 2, 5, 6)).unwrap(), &[(0, 1)]);
        assert_eq!(faces.get(&(2, 3, 6, 7)).unwrap(), &[(0, 3)]);
        assert_eq!(faces.get(&(4, 5, 6, 7)).unwrap(), &[(0, 5), (1, 4)]);
        assert_eq!(faces.get(&(4, 5, 8, 9)).unwrap(), &[(1, 2)]);
        assert_eq!(faces.get(&(4, 7, 8, 11)).unwrap(), &[(1, 0)]);
        assert_eq!(faces.get(&(5, 6, 9, 10)).unwrap(), &[(1, 1)]);
        assert_eq!(faces.get(&(6, 7, 10, 11)).unwrap(), &[(1, 3)]);
        assert_eq!(faces.get(&(8, 9, 10, 11)).unwrap(), &[(1, 5)]);
        Ok(())
    }

    #[test]
    fn all_faces_3d_mixed_works() -> Result<(), StrError> {
        //                       4------------7-----------10
        //                      /.           /|            |
        //                     / .          / |            |
        //                    /  .         /  |            |
        //                   /   .        /   |            |
        //                  5------------6    |            |
        //                  |    .       |`.  |            |
        //                  |    0-------|--`.3------------9
        //                  |   /        |   /`.          /
        //                  |  /         |  /   `.       /
        //                  | /          | /      `.    /
        //                  |/           |/         `. /
        //  12-----11-------1------------2------------8
        //
        let mesh = Samples::mixed_shapes_3d();
        let shapes = Shapes::new(&mesh)?;
        let faces = all_faces_3d(&mesh, &shapes)?;
        let mut keys: Vec<_> = faces.keys().collect();
        keys.sort();
        assert_eq!(
            keys,
            [
                &(0, 1, 2, 3),
                &(0, 1, 4, 5),
                &(0, 3, 4, 7),
                &(1, 2, 5, 6),
                &(2, 3, 6, 7),
                &(2, 3, 6, 13),
                &(2, 3, 8, 13),
                &(2, 6, 8, 13),
                &(3, 6, 8, 13),
                &(4, 5, 6, 7),
            ]
        );
        assert_eq!(faces.get(&(0, 1, 2, 3)).unwrap(), &[(0, 4)]);
        assert_eq!(faces.get(&(0, 1, 4, 5)).unwrap(), &[(0, 2)]);
        assert_eq!(faces.get(&(0, 3, 4, 7)).unwrap(), &[(0, 0)]);
        assert_eq!(faces.get(&(1, 2, 5, 6)).unwrap(), &[(0, 1)]);
        assert_eq!(faces.get(&(2, 3, 6, 7)).unwrap(), &[(0, 3)]);
        assert_eq!(faces.get(&(2, 3, 6, 13)).unwrap(), &[(1, 0)]);
        assert_eq!(faces.get(&(2, 3, 8, 13)).unwrap(), &[(1, 2)]);
        assert_eq!(faces.get(&(2, 6, 8, 13)).unwrap(), &[(1, 1)]);
        assert_eq!(faces.get(&(3, 6, 8, 13)).unwrap(), &[(1, 3)]);
        assert_eq!(faces.get(&(4, 5, 6, 7)).unwrap(), &[(0, 5)]);
        Ok(())
    }

    #[test]
    fn derive_works() {
        let edge = Edge {
            points: vec![10, 20, 33],
        };
        let face = Face {
            points: vec![1, 2, 3, 4],
        };
        let edge_clone = edge.clone();
        let face_clone = face.clone();
        assert_eq!(format!("{:?}", edge), "Edge { points: [10, 20, 33] }");
        assert_eq!(format!("{:?}", face), "Face { points: [1, 2, 3, 4] }");
        assert_eq!(edge_clone.points.len(), 3);
        assert_eq!(face_clone.points.len(), 4);
    }
}
<|endoftext|>


<commit_before>use anyhow::Result;
use std::collections::HashMap;
use std::io;
use std::process;
use std::sync::atomic::AtomicBool;
use std::sync::Mutex;
use std::time::{Duration, Instant};
use tide::Request;

use async_std::sync::{channel, Arc, Receiver, Sender};
use futures::sink::SinkExt;
use futures::stream::StreamExt;

use async_ctrlc::CtrlC;
use async_std::task;

use extendedmind_engine::{AsyncSender, Bytes, Engine};

#[derive(Clone, Copy)]
#[repr(u8)]
enum SystemCommand {
    WakeUp = 1,
    Disconnect = 2,
}

#[derive(Copy, Clone, Debug)]
enum ReceiverType {
    System,
    Hypercore,
    Client,
}

#[derive(Clone)]
struct State {
    // Engine
    engine: Engine,
    // System command receiver
    system_commands: Receiver<Result<Bytes, io::Error>>,
    // Stores the channels needed to make a protocol
    channels: HashMap<String, SplitChannels>,
}

#[derive(Clone)]
struct SplitChannels {
    engine_channel: (AsyncSender, Receiver<Result<Bytes, io::Error>>),
    socket_channel: (AsyncSender, Receiver<Result<Bytes, io::Error>>),
}

#[derive(Clone)]
struct WrappedData {
    data: Bytes,
    receiver_type: ReceiverType,
    discovery_key: Option<String>,
}

type WrappedBytesClosure =
    Box<dyn Fn(Result<Bytes, io::Error>) -> Result<WrappedData, io::Error> + Send + Sync>;

type ChannelSenderReceiver = (
    Sender<Result<Bytes, io::Error>>,
    Receiver<Result<Bytes, io::Error>>,
);

async fn async_main(initial_state: State) -> Result<()> {
    let mut app = tide::with_state(initial_state);

    app.at("/ws/:discovery_key")
        .get(|req: Request<State>| async move {
            tide::websocket::upgrade(req, |_req, handle| async move {
                // Get handle to async-tungstenite WebSocketStream
                let ws = handle.into_inner();
                let (mut ws_writer, mut ws_reader) = ws.split();

                // Get discovery key from the path
                let discovery_key: Arc<String> =
                    Arc::new(_req.param("discovery_key")?.parse().unwrap());
                dbg!(&discovery_key);

                // Launch a task for the protocol
                let (engine_sender, engine_receiver) = _req.state().channels
                    [discovery_key.as_ref()]
                .engine_channel
                .clone();
                let engine = _req.state().engine.clone();
                task::spawn(async move {
                    engine
                        .connect_passive(engine_receiver, engine_sender)
                        .await
                        .unwrap();
                });

                // Launch a second task for listening to receivers
                let (client_sender, client_receiver): ChannelSenderReceiver = channel(1000);
                task::spawn(async move {
                    let (receivers, hypercore_sender) =
                        collect_receivers_and_sender(_req.state(), client_receiver, discovery_key);
                    let mut merged_receivers = futures::stream::select_all(receivers);
                    let mut now = Instant::now();
                    while let Some(Ok(wrapped_value)) = merged_receivers.next().await {
                        match wrapped_value.receiver_type {
                            ReceiverType::System => {
                                if wrapped_value.data.as_ref().get(0)
                                    == Some(&(SystemCommand::Disconnect as u8))
                                {
                                    dbg!("got disconnect");
                                    ws_writer.close().await.unwrap();
                                    break;
                                }
                                // Send ping message if enough time has elapsed
                                if now.elapsed().as_secs() > 30 {
                                    dbg!("sending ping");
                                    ws_writer
                                        .send(tungstenite::Message::Ping(Vec::new()))
                                        .await
                                        .unwrap();
                                    now = Instant::now();
                                }
                            }
                            ReceiverType::Client => {
                                dbg!(
                                    "got client request, forwarding to hypercore {}",
                                    wrapped_value.data.len()
                                );
                                hypercore_sender.clone().send(wrapped_value.data).await;
                            }
                            ReceiverType::Hypercore => {
                                let msg = wrapped_value.data.as_ref().to_vec();
                                dbg!("got hypercore message, sending to client, {:?}", msg.len());
                                ws_writer
                                    .send(tungstenite::Message::Binary(msg))
                                    .await
                                    .unwrap();
                            }
                        }
                    }
                });

                // Block loop on incoming messages
                loop {
                    let incoming_msg = ws_reader.next().await;
                    let msg = incoming_msg.unwrap().unwrap();
                    dbg!("got incoming client message {:?}", msg.len());
                    client_sender.send(Ok(Bytes::from(msg.into_data()))).await;
                }
            })
        });

    app.listen("0.0.0.0:8080").await?;
    Ok(())
}

type CollectedReceiversAndSender = (
    Vec<futures::stream::Map<Receiver<Result<Bytes, io::Error>>, WrappedBytesClosure>>,
    AsyncSender,
);

fn collect_receivers_and_sender(
    state: &State,
    client_receiver: Receiver<Result<Bytes, io::Error>>,
    discovery_key: Arc<String>,
) -> CollectedReceiversAndSender {
    let (writer, reader) = state.channels[discovery_key.as_ref()]
        .socket_channel
        .clone();
    (
        vec![
            state
                .system_commands
                .clone()
                .map(Box::new(|read_result: Result<Bytes, io::Error>| {
                    read_result.map(|data| WrappedData {
                        data,
                        receiver_type: ReceiverType::System,
                        discovery_key: None,
                    })
                }) as WrappedBytesClosure),
            reader.map(Box::new(move |read_result: Result<Bytes, io::Error>| {
                read_result.map(|data| WrappedData {
                    data,
                    receiver_type: ReceiverType::Hypercore,
                    discovery_key: Some((*discovery_key).clone()),
                })
            }) as WrappedBytesClosure),
            client_receiver.map(Box::new(|read_result: Result<Bytes, io::Error>| {
                read_result.map(|data| WrappedData {
                    data,
                    receiver_type: ReceiverType::Client,
                    discovery_key: None,
                })
            }) as WrappedBytesClosure),
        ],
        writer,
    )
}

fn main() -> Result<()> {
    fern::Dispatch::new()
        .format(|out, message, record| {
            out.finish(format_args!(
                "{}[{}][{}] {}",
                chrono::Local::now().format("[%Y-%m-%d %H:%M:%S]"),
                record.target(),
                record.level(),
                message
            ))
        })
        .level(log::LevelFilter::Debug)
        .chain(std::io::stdout())
        .apply()?;

    // Initialize the engine blocking
    let engine = futures::executor::block_on(async move { Engine::new_disk(false, None).await });

    // Create channels
    let (ping_sender, system_command_receiver): ChannelSenderReceiver = channel(1000);
    let (incoming_sender, incoming_receiver): ChannelSenderReceiver = channel(1000);
    let (outgoing_sender, outgoing_receiver): ChannelSenderReceiver = channel(1000);
    let incoming_sender = AsyncSender::new(incoming_sender);
    let outgoing_sender = AsyncSender::new(outgoing_sender);

    // Create state for websocket
    let initial_state = State {
        engine,
        system_commands: system_command_receiver,
        channels: [(
            "demo".to_string(),
            SplitChannels {
                engine_channel: (incoming_sender, outgoing_receiver),
                socket_channel: (outgoing_sender, incoming_receiver),
            },
        )]
        .iter()
        .cloned()
        .collect(),
    };

    // Listen to ctrlc in a separate task
    let ctrlc = CtrlC::new().expect("cannot create Ctrl+C handler?");
    let disconnect_sender = ping_sender.clone();
    let abort: Arc<Mutex<AtomicBool>> = Arc::new(Mutex::new(AtomicBool::new(false)));
    let abort_writer = abort.clone();
    task::spawn(async move {
        ctrlc.await;
        disconnect_sender
            .send(Ok(Bytes::from_static(&[SystemCommand::Disconnect as u8])))
            .await;
        *abort_writer.as_ref().lock().unwrap() = AtomicBool::new(true);
        // Wait 2s before killing, to allow time for file saving and closing sockets
        task::sleep(Duration::from_millis(2000)).await;
        process::exit(0);
    });

    // Need to start a system executor to send the WakeUp command, we send it once per second so
    // that the listener will send a WS Ping when it wants to in a 1s delay.
    task::spawn(async move {
        let mut interval = async_std::stream::interval(Duration::from_secs(1));
        while interval.next().await.is_some() && !*abort.as_ref().lock().unwrap().get_mut() {
            task::sleep(Duration::from_millis(1000)).await;
            // dbg!("Sending WakeUp");
            ping_sender
                .send(Ok(Bytes::from_static(&[SystemCommand::WakeUp as u8])))
                .await;
        }
    });

    // Block server with initial state
    futures::executor::block_on(async_main(initial_state))?;

    Ok(())
}
<commit_msg>Much shorter sleep<commit_after>use anyhow::Result;
use std::collections::HashMap;
use std::io;
use std::process;
use std::sync::atomic::AtomicBool;
use std::sync::Mutex;
use std::time::{Duration, Instant};
use tide::Request;

use async_std::sync::{channel, Arc, Receiver, Sender};
use futures::sink::SinkExt;
use futures::stream::StreamExt;

use async_ctrlc::CtrlC;
use async_std::task;

use extendedmind_engine::{AsyncSender, Bytes, Engine};

#[derive(Clone, Copy)]
#[repr(u8)]
enum SystemCommand {
    WakeUp = 1,
    Disconnect = 2,
}

#[derive(Copy, Clone, Debug)]
enum ReceiverType {
    System,
    Hypercore,
    Client,
}

#[derive(Clone)]
struct State {
    // Engine
    engine: Engine,
    // System command receiver
    system_commands: Receiver<Result<Bytes, io::Error>>,
    // Stores the channels needed to make a protocol
    channels: HashMap<String, SplitChannels>,
}

#[derive(Clone)]
struct SplitChannels {
    engine_channel: (AsyncSender, Receiver<Result<Bytes, io::Error>>),
    socket_channel: (AsyncSender, Receiver<Result<Bytes, io::Error>>),
}

#[derive(Clone)]
struct WrappedData {
    data: Bytes,
    receiver_type: ReceiverType,
    discovery_key: Option<String>,
}

type WrappedBytesClosure =
    Box<dyn Fn(Result<Bytes, io::Error>) -> Result<WrappedData, io::Error> + Send + Sync>;

type ChannelSenderReceiver = (
    Sender<Result<Bytes, io::Error>>,
    Receiver<Result<Bytes, io::Error>>,
);

async fn async_main(initial_state: State) -> Result<()> {
    let mut app = tide::with_state(initial_state);

    app.at("/ws/:discovery_key")
        .get(|req: Request<State>| async move {
            tide::websocket::upgrade(req, |_req, handle| async move {
                // Get handle to async-tungstenite WebSocketStream
                let ws = handle.into_inner();
                let (mut ws_writer, mut ws_reader) = ws.split();

                // Get discovery key from the path
                let discovery_key: Arc<String> =
                    Arc::new(_req.param("discovery_key")?.parse().unwrap());
                dbg!(&discovery_key);

                // Launch a task for the protocol
                let (engine_sender, engine_receiver) = _req.state().channels
                    [discovery_key.as_ref()]
                .engine_channel
                .clone();
                let engine = _req.state().engine.clone();
                task::spawn(async move {
                    engine
                        .connect_passive(engine_receiver, engine_sender)
                        .await
                        .unwrap();
                });

                // Launch a second task for listening to receivers
                let (client_sender, client_receiver): ChannelSenderReceiver = channel(1000);
                task::spawn(async move {
                    let (receivers, hypercore_sender) =
                        collect_receivers_and_sender(_req.state(), client_receiver, discovery_key);
                    let mut merged_receivers = futures::stream::select_all(receivers);
                    let mut now = Instant::now();
                    while let Some(Ok(wrapped_value)) = merged_receivers.next().await {
                        match wrapped_value.receiver_type {
                            ReceiverType::System => {
                                if wrapped_value.data.as_ref().get(0)
                                    == Some(&(SystemCommand::Disconnect as u8))
                                {
                                    dbg!("got disconnect");
                                    ws_writer.close().await.unwrap();
                                    break;
                                }
                                // Send ping message if enough time has elapsed
                                if now.elapsed().as_secs() > 30 {
                                    dbg!("sending ping");
                                    ws_writer
                                        .send(tungstenite::Message::Ping(Vec::new()))
                                        .await
                                        .unwrap();
                                    now = Instant::now();
                                }
                            }
                            ReceiverType::Client => {
                                dbg!(
                                    "got client request, forwarding to hypercore {}",
                                    wrapped_value.data.len()
                                );
                                hypercore_sender.clone().send(wrapped_value.data).await;
                            }
                            ReceiverType::Hypercore => {
                                let msg = wrapped_value.data.as_ref().to_vec();
                                dbg!("got hypercore message, sending to client, {:?}", msg.len());
                                ws_writer
                                    .send(tungstenite::Message::Binary(msg))
                                    .await
                                    .unwrap();
                            }
                        }
                    }
                });

                // Block loop on incoming messages
                loop {
                    let incoming_msg = ws_reader.next().await;
                    let msg = incoming_msg.unwrap().unwrap();
                    dbg!("got incoming client message {:?}", msg.len());
                    client_sender.send(Ok(Bytes::from(msg.into_data()))).await;
                }
            })
        });

    app.listen("0.0.0.0:8080").await?;
    Ok(())
}

type CollectedReceiversAndSender = (
    Vec<futures::stream::Map<Receiver<Result<Bytes, io::Error>>, WrappedBytesClosure>>,
    AsyncSender,
);

fn collect_receivers_and_sender(
    state: &State,
    client_receiver: Receiver<Result<Bytes, io::Error>>,
    discovery_key: Arc<String>,
) -> CollectedReceiversAndSender {
    let (writer, reader) = state.channels[discovery_key.as_ref()]
        .socket_channel
        .clone();
    (
        vec![
            state
                .system_commands
                .clone()
                .map(Box::new(|read_result: Result<Bytes, io::Error>| {
                    read_result.map(|data| WrappedData {
                        data,
                        receiver_type: ReceiverType::System,
                        discovery_key: None,
                    })
                }) as WrappedBytesClosure),
            reader.map(Box::new(move |read_result: Result<Bytes, io::Error>| {
                read_result.map(|data| WrappedData {
                    data,
                    receiver_type: ReceiverType::Hypercore,
                    discovery_key: Some((*discovery_key).clone()),
                })
            }) as WrappedBytesClosure),
            client_receiver.map(Box::new(|read_result: Result<Bytes, io::Error>| {
                read_result.map(|data| WrappedData {
                    data,
                    receiver_type: ReceiverType::Client,
                    discovery_key: None,
                })
            }) as WrappedBytesClosure),
        ],
        writer,
    )
}

fn main() -> Result<()> {
    fern::Dispatch::new()
        .format(|out, message, record| {
            out.finish(format_args!(
                "{}[{}][{}] {}",
                chrono::Local::now().format("[%Y-%m-%d %H:%M:%S]"),
                record.target(),
                record.level(),
                message
            ))
        })
        .level(log::LevelFilter::Debug)
        .chain(std::io::stdout())
        .apply()?;

    // Initialize the engine blocking
    let engine = futures::executor::block_on(async move { Engine::new_disk(false, None).await });

    // Create channels
    let (ping_sender, system_command_receiver): ChannelSenderReceiver = channel(1000);
    let (incoming_sender, incoming_receiver): ChannelSenderReceiver = channel(1000);
    let (outgoing_sender, outgoing_receiver): ChannelSenderReceiver = channel(1000);
    let incoming_sender = AsyncSender::new(incoming_sender);
    let outgoing_sender = AsyncSender::new(outgoing_sender);

    // Create state for websocket
    let initial_state = State {
        engine,
        system_commands: system_command_receiver,
        channels: [(
            "demo".to_string(),
            SplitChannels {
                engine_channel: (incoming_sender, outgoing_receiver),
                socket_channel: (outgoing_sender, incoming_receiver),
            },
        )]
        .iter()
        .cloned()
        .collect(),
    };

    // Listen to ctrlc in a separate task
    let ctrlc = CtrlC::new().expect("cannot create Ctrl+C handler?");
    let disconnect_sender = ping_sender.clone();
    let abort: Arc<Mutex<AtomicBool>> = Arc::new(Mutex::new(AtomicBool::new(false)));
    let abort_writer = abort.clone();
    task::spawn(async move {
        ctrlc.await;
        disconnect_sender
            .send(Ok(Bytes::from_static(&[SystemCommand::Disconnect as u8])))
            .await;
        *abort_writer.as_ref().lock().unwrap() = AtomicBool::new(true);
        // Wait 200ms before killing, to allow time for file saving and closing sockets
        task::sleep(Duration::from_millis(200)).await;
        process::exit(0);
    });

    // Need to start a system executor to send the WakeUp command, we send it once per second so
    // that the listener will send a WS Ping when it wants to in a 1s delay.
    task::spawn(async move {
        let mut interval = async_std::stream::interval(Duration::from_secs(1));
        while interval.next().await.is_some() && !*abort.as_ref().lock().unwrap().get_mut() {
            task::sleep(Duration::from_millis(1000)).await;
            // dbg!("Sending WakeUp");
            ping_sender
                .send(Ok(Bytes::from_static(&[SystemCommand::WakeUp as u8])))
                .await;
        }
    });

    // Block server with initial state
    futures::executor::block_on(async_main(initial_state))?;

    Ok(())
}
<|endoftext|>


<commit_before>use error::RuntimeError;

use sys::x11;
use sys::xcb;

pub struct LinuxApplication {
   pub x11_display_handler: X11DisplayHandler,
   pub connection_handler: ConnectionHandler,
}

impl LinuxApplication {
   pub fn new() -> Result<Self, RuntimeError> {
      let x11_display_handler = try!(X11DisplayHandler::new());
      let connection_handler = try!(ConnectionHandler::new(&x11_display_handler));

      Ok(LinuxApplication {
         x11_display_handler: x11_display_handler,
         connection_handler: connection_handler,
      })
   }

   pub fn desktop_size(&self) -> (u32, u32) {
      (1200, 800)
   }
}

pub struct X11DisplayHandler {
   pub display: x11::Display,
}

impl X11DisplayHandler {
   #[inline]
   pub fn new() -> Result<Self, RuntimeError> {
      let display = try!(x11::Display::default());

      display.xcb_own_event_queue();

      Ok(X11DisplayHandler {
         display: display
      })
   }

   #[inline]
   pub fn connection(&self) -> Result<xcb::Connection, RuntimeError> {
      self.display.xcb_connection()
   }
}

pub struct ConnectionHandler {
   pub connection: xcb::Connection,
}

impl ConnectionHandler {
   #[inline]
   pub fn new(display_handler: &X11DisplayHandler) -> Result<Self, RuntimeError> {
      Ok(ConnectionHandler {
         connection: try!(display_handler.connection())
      })
   }
}
<commit_msg>Starting a XCB ScreenHandler implementation<commit_after>use error::RuntimeError;

use sys::x11;
use sys::xcb;

pub struct LinuxApplication {
   pub x11_display: X11DisplayHandler,
   pub connection: ConnectionHandler,
   pub screen: ScreenHandler,
}

impl LinuxApplication {
   pub fn new() -> Result<Self, RuntimeError> {
      let x11_display = try!(X11DisplayHandler::new());
      let connection = try!(ConnectionHandler::new(&x11_display));
      let screen = ScreenHandler::new(&x11_display, &connection);

      Ok(LinuxApplication {
         x11_display: x11_display,
         connection: connection,
         screen: screen,
      })
   }

   pub fn desktop_size(&self) -> (u32, u32) {
      (1200, 800)
   }
}

pub struct X11DisplayHandler {
   pub display: x11::Display,
}

impl X11DisplayHandler {
   #[inline]
   pub fn new() -> Result<Self, RuntimeError> {
      let display = try!(x11::Display::default());

      display.xcb_own_event_queue();

      Ok(X11DisplayHandler {
         display: display
      })
   }

   #[inline]
   pub fn connection(&self) -> Result<xcb::Connection, RuntimeError> {
      self.display.xcb_connection()
   }

   #[inline]
   pub fn screen_id(&self) -> x11::ScreenID {
      self.display.default_screen()
   }
}

pub struct ConnectionHandler {
   pub connection: xcb::Connection,
}

impl ConnectionHandler {
   #[inline]
   pub fn new(display: &X11DisplayHandler) -> Result<Self, RuntimeError> {
      Ok(ConnectionHandler {
         connection: try!(display.connection())
      })
   }

   #[inline]
   pub fn screen_of_display(&self, display: &X11DisplayHandler) -> xcb::Screen {
      let screen_id = display.screen_id();

      self.connection.screen_of_display(&screen_id)
   }
}

pub struct ScreenHandler {
   pub screen: xcb::Screen,
}

impl ScreenHandler {
   #[inline]
   pub fn new(display: &X11DisplayHandler, connection: &ConnectionHandler) -> Self {
      ScreenHandler {
         screen: connection.screen_of_display(display)
      }
   }
}<|endoftext|>


<commit_before>use super::cpu::interrupt;
use super::io;
use core::option::{Option, None};

pub unsafe fn init(table: interrupt::table) {
    table.enable(6, keypress as u32);
}

pub static mut keydown: Option<extern fn(char)> = None;

#[no_mangle]
pub unsafe fn keypress() {
    keydown.map(|f| {
        let x = *io::UART0 as u8 as char;
	if(x != '\r' && x != 22 as u8 as char) {f(x);}
	else if (x == 22 as u8 as char) { f(8 as u8 as char); }
	else {f('\n');}
    });

    asm!("pop {r11, lr}
          subs pc, lr, #4");
}
<commit_msg>Pseudo backspaces and newlines; less idiotic structure for checking for them<commit_after>use super::cpu::interrupt;
use super::io;
use core::option::{Option, None};

pub unsafe fn init(table: interrupt::table) {
    table.enable(6, keypress as u32);
}

pub static mut keydown: Option<extern fn(char)> = None;

#[no_mangle]
pub unsafe fn keypress() {
    keydown.map(|f| {
        let x = *io::UART0 as u8;
    	match x {
    		13			=>	{ f('\n'); }
    		127			=>	{ f(8 as u8 as char); f(' '); f(8 as u8 as char);}
    		_			=>	{ f(x as char); }
    	}
    }
	);
    asm!("pop {r11, lr}
          subs pc, lr, #4");
}
<|endoftext|>


<commit_before>//! The Gilbert–Johnson–Keerthi distance algorithm.

use na::{self, Unit};
use simba::scalar::RealField;

use crate::query::algorithms::{special_support_maps::ConstantOrigin, CSOPoint, VoronoiSimplex};
use crate::shape::SupportMap;
// use query::Proximity;
use crate::math::{Isometry, Point, Vector, DIM};
use crate::query::{self, Ray};

/// Results of the GJK algorithm.
#[derive(Clone, Debug, PartialEq)]
pub enum GJKResult<N: RealField> {
    /// Result of the GJK algorithm when the origin is inside of the polytope.
    Intersection,
    /// Result of the GJK algorithm when a projection of the origin on the polytope is found.
    ClosestPoints(Point<N>, Point<N>, Unit<Vector<N>>),
    /// Result of the GJK algorithm when the origin is too close to the polytope but not inside of it.
    Proximity(Unit<Vector<N>>),
    /// Result of the GJK algorithm when the origin is too far away from the polytope.
    NoIntersection(Unit<Vector<N>>),
}

/// The absolute tolerence used by the GJK algorithm.
pub fn eps_tol<N: RealField>() -> N {
    let _eps = N::default_epsilon();
    _eps * na::convert(10.0f64)
}

/// Projects the origin on the boundary of the given shape.
///
/// The origin is assumed to be outside of the shape. If it is inside,
/// use the EPA algorithm instead.
/// Return `None` if the origin is not inside of the shape or if
/// the EPA algorithm failed to compute the projection.
pub fn project_origin<N, G: ?Sized>(
    m: &Isometry<N>,
    g: &G,
    simplex: &mut VoronoiSimplex<N>,
) -> Option<Point<N>>
where
    N: RealField,
    G: SupportMap<N>,
{
    match closest_points(
        m,
        g,
        &Isometry::identity(),
        &ConstantOrigin,
        N::max_value(),
        true,
        simplex,
    ) {
        GJKResult::Intersection => None,
        GJKResult::ClosestPoints(p, _, _) => Some(p),
        _ => unreachable!(),
    }
}

/*
 * Separating Axis GJK
 */
/// Projects the origin on a shape using the Separating Axis GJK algorithm.
/// The algorithm will stop as soon as the polytope can be proven to be at least `max_dist` away
/// from the origin.
///
/// # Arguments:
/// * simplex - the simplex to be used by the GJK algorithm. It must be already initialized
///             with at least one point on the shape boundary.
/// * exact_dist - if `false`, the gjk will stop as soon as it can prove that the origin is at
/// a distance smaller than `max_dist` but not inside of `shape`. In that case, it returns a
/// `GJKResult::Proximity(sep_axis)` where `sep_axis` is a separating axis. If `false` the gjk will
/// compute the exact distance and return `GJKResult::Projection(point)` if the origin is closer
/// than `max_dist` but not inside `shape`.
pub fn closest_points<N, G1: ?Sized, G2: ?Sized>(
    m1: &Isometry<N>,
    g1: &G1,
    m2: &Isometry<N>,
    g2: &G2,
    max_dist: N,
    exact_dist: bool,
    simplex: &mut VoronoiSimplex<N>,
) -> GJKResult<N>
where
    N: RealField,
    G1: SupportMap<N>,
    G2: SupportMap<N>,
{
    let _eps = N::default_epsilon();
    let _eps_tol: N = eps_tol();
    let _eps_rel: N = _eps_tol.sqrt();

    // FIXME: reset the simplex if it is empty?
    let mut proj = simplex.project_origin_and_reduce();

    let mut old_dir;

    if let Some(proj_dir) = Unit::try_new(proj.coords, N::zero()) {
        old_dir = -proj_dir;
    } else {
        return GJKResult::Intersection;
    }

    let mut max_bound = N::max_value();
    let mut dir;
    let mut niter = 0;

    loop {
        let old_max_bound = max_bound;

        if let Some((new_dir, dist)) = Unit::try_new_and_get(-proj.coords, _eps_tol) {
            dir = new_dir;
            max_bound = dist;
        } else {
            // The origin is on the simplex.
            return GJKResult::Intersection;
        }

        if max_bound >= old_max_bound {
            if exact_dist {
                let (p1, p2) = result(simplex, true);
                return GJKResult::ClosestPoints(p1, p2, old_dir); // upper bounds inconsistencies
            } else {
                return GJKResult::Proximity(old_dir);
            }
        }

        let cso_point = CSOPoint::from_shapes(m1, g1, m2, g2, &dir);
        let min_bound = -dir.dot(&cso_point.point.coords);

        assert!(min_bound == min_bound);

        if min_bound > max_dist {
            return GJKResult::NoIntersection(dir);
        } else if !exact_dist && min_bound > na::zero() && max_bound <= max_dist {
            return GJKResult::Proximity(old_dir);
        } else if max_bound - min_bound <= _eps_rel * max_bound {
            if exact_dist {
                let (p1, p2) = result(simplex, false);
                return GJKResult::ClosestPoints(p1, p2, dir); // the distance found has a good enough precision
            } else {
                return GJKResult::Proximity(dir);
            }
        }

        if !simplex.add_point(cso_point) {
            if exact_dist {
                let (p1, p2) = result(simplex, false);
                return GJKResult::ClosestPoints(p1, p2, dir);
            } else {
                return GJKResult::Proximity(dir);
            }
        }

        old_dir = dir;
        proj = simplex.project_origin_and_reduce();

        if simplex.dimension() == DIM {
            if min_bound >= _eps_tol {
                if exact_dist {
                    let (p1, p2) = result(simplex, true);
                    return GJKResult::ClosestPoints(p1, p2, old_dir);
                } else {
                    // NOTE: previous implementation used old_proj here.
                    return GJKResult::Proximity(old_dir);
                }
            } else {
                return GJKResult::Intersection; // Point inside of the cso.
            }
        }
        niter += 1;
        if niter == 10000 {
            //            eprintln!("Error: GJK did not converge.");
            return GJKResult::NoIntersection(Vector::x_axis());
        }
    }
}

/// Casts a ray on a support map using the GJK algorithm.
pub fn cast_ray<N, G: ?Sized>(
    m: &Isometry<N>,
    shape: &G,
    simplex: &mut VoronoiSimplex<N>,
    ray: &Ray<N>,
    max_toi: N,
) -> Option<(N, Vector<N>)>
where
    N: RealField,
    G: SupportMap<N>,
{
    let m2 = Isometry::identity();
    let g2 = ConstantOrigin;
    minkowski_ray_cast(m, shape, &m2, &g2, ray, max_toi, simplex)
}

/// Compute the normal and the distance that can travel `g1` along the direction
/// `dir` so that `g1` and `g2` just touch.
pub fn directional_distance<N, G1: ?Sized, G2: ?Sized>(
    m1: &Isometry<N>,
    g1: &G1,
    m2: &Isometry<N>,
    g2: &G2,
    dir: &Vector<N>,
    simplex: &mut VoronoiSimplex<N>,
) -> Option<(N, Vector<N>, Point<N>, Point<N>)>
where
    N: RealField,
    G1: SupportMap<N>,
    G2: SupportMap<N>,
{
    let ray = Ray::new(Point::origin(), *dir);
    minkowski_ray_cast(m1, g1, m2, g2, &ray, N::max_value(), simplex).map(|(toi, normal)| {
        let witnesses = if !toi.is_zero() {
            result(simplex, simplex.dimension() == DIM)
        } else {
            // If there is penetration, the witness points
            // are undefined.
            (Point::origin(), Point::origin())
        };

        (toi, normal, witnesses.0, witnesses.1)
    })
}

// Ray-cast on the Minkowski Difference `m1 * g1 - m2 * g2`.
fn minkowski_ray_cast<N, G1: ?Sized, G2: ?Sized>(
    m1: &Isometry<N>,
    g1: &G1,
    m2: &Isometry<N>,
    g2: &G2,
    ray: &Ray<N>,
    max_toi: N,
    simplex: &mut VoronoiSimplex<N>,
) -> Option<(N, Vector<N>)>
where
    N: RealField,
    G1: SupportMap<N>,
    G2: SupportMap<N>,
{
    let _eps = N::default_epsilon();
    let _eps_tol: N = eps_tol();
    let _eps_rel: N = _eps_tol.sqrt();

    let ray_length = ray.dir.norm();

    if relative_eq!(ray_length, N::zero()) {
        return None;
    }

    let mut ltoi = N::zero();
    let mut curr_ray = Ray::new(ray.origin, ray.dir / ray_length);
    let max_toi = max_toi * ray_length;
    let dir = -curr_ray.dir;
    let mut ldir = dir;

    // Initialize the simplex.
    let support_point = CSOPoint::from_shapes(m1, g1, m2, g2, &dir);
    simplex.reset(support_point.translate(&-curr_ray.origin.coords));

    // FIXME: reset the simplex if it is empty?
    let mut proj = simplex.project_origin_and_reduce();
    let mut max_bound = N::max_value();
    let mut dir;
    let mut niter = 0;
    let mut last_chance = false;

    loop {
        let old_max_bound = max_bound;

        if let Some((new_dir, dist)) = Unit::try_new_and_get(-proj.coords, _eps_tol) {
            dir = new_dir;
            max_bound = dist;
        } else {
            return Some((ltoi / ray_length, ldir));
        }

        let support_point = if max_bound >= old_max_bound {
            // Upper bounds inconsistencies. Consider the projection as a valid support point.
            last_chance = true;
            CSOPoint::single_point(proj + curr_ray.origin.coords)
        } else {
            CSOPoint::from_shapes(m1, g1, m2, g2, &dir)
        };

        if last_chance && ltoi > N::zero() {
            // last_chance && ltoi > N::zero() && (support_point.point - curr_ray.origin).dot(&ldir) >= N::zero() {
            return Some((ltoi / ray_length, ldir));
        }

        // Clip the ray on the support plane (None <=> t < 0)
        // The configurations are:
        //   dir.dot(curr_ray.dir)  |   t   |               Action
        // −−−−−−−−−−−−−−−−−−−−-----+−−−−−−−+−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
        //          < 0             |  < 0  | Continue.
        //          < 0             |  > 0  | New lower bound, move the origin.
        //          > 0             |  < 0  | Miss. No intersection.
        //          > 0             |  > 0  | New higher bound.
        match query::ray_toi_with_plane(&support_point.point, &dir, &curr_ray) {
            Some(t) => {
                if dir.dot(&curr_ray.dir) < na::zero() && t > N::zero() {
                    // new lower bound
                    ldir = *dir;
                    ltoi += t;

                    if ltoi > max_toi {
                        return None;
                    }

                    let shift = curr_ray.dir * t;
                    curr_ray.origin += shift;
                    max_bound = N::max_value();
                    simplex.modify_pnts(&|pt| pt.translate_mut(&-shift));
                    last_chance = false;
                }
            }
            None => {
                if dir.dot(&curr_ray.dir) > N::default_epsilon() {
                    // miss
                    return None;
                }
            }
        }

        if last_chance {
            return None;
        }

        let min_bound = -dir.dot(&(support_point.point.coords - curr_ray.origin.coords));

        assert!(min_bound == min_bound);

        if max_bound - min_bound <= _eps_rel * max_bound {
            return None;
        }

        let _ = simplex.add_point(support_point.translate(&-curr_ray.origin.coords));
        proj = simplex.project_origin_and_reduce();

        if simplex.dimension() == DIM {
            if min_bound >= _eps_tol {
                return None;
            } else {
                return Some((ltoi / ray_length, ldir)); // Point inside of the cso.
            }
        }

        niter += 1;
        if niter == 10000 {
            //            eprintln!("Error: GJK did not converge.");
            return None;
        }
    }
}

fn result<N: RealField>(simplex: &VoronoiSimplex<N>, prev: bool) -> (Point<N>, Point<N>) {
    let mut res = (Point::origin(), Point::origin());
    if prev {
        for i in 0..simplex.prev_dimension() + 1 {
            let coord = simplex.prev_proj_coord(i);
            let point = simplex.prev_point(i);
            res.0 += point.orig1.coords * coord;
            res.1 += point.orig2.coords * coord;
        }

        res
    } else {
        for i in 0..simplex.dimension() + 1 {
            let coord = simplex.proj_coord(i);
            let point = simplex.point(i);
            res.0 += point.orig1.coords * coord;
            res.1 += point.orig2.coords * coord;
        }

        res
    }
}
<commit_msg>Improve numerical stability of GJK-ray-cast when using fixed-point numbers.<commit_after>//! The Gilbert–Johnson–Keerthi distance algorithm.

use na::{self, Unit};
use simba::scalar::RealField;

use crate::query::algorithms::{special_support_maps::ConstantOrigin, CSOPoint, VoronoiSimplex};
use crate::shape::SupportMap;
// use query::Proximity;
use crate::math::{Isometry, Point, Vector, DIM};
use crate::query::{self, Ray};

/// Results of the GJK algorithm.
#[derive(Clone, Debug, PartialEq)]
pub enum GJKResult<N: RealField> {
    /// Result of the GJK algorithm when the origin is inside of the polytope.
    Intersection,
    /// Result of the GJK algorithm when a projection of the origin on the polytope is found.
    ClosestPoints(Point<N>, Point<N>, Unit<Vector<N>>),
    /// Result of the GJK algorithm when the origin is too close to the polytope but not inside of it.
    Proximity(Unit<Vector<N>>),
    /// Result of the GJK algorithm when the origin is too far away from the polytope.
    NoIntersection(Unit<Vector<N>>),
}

/// The absolute tolerence used by the GJK algorithm.
pub fn eps_tol<N: RealField>() -> N {
    let _eps = N::default_epsilon();
    _eps * na::convert(10.0f64)
}

/// Projects the origin on the boundary of the given shape.
///
/// The origin is assumed to be outside of the shape. If it is inside,
/// use the EPA algorithm instead.
/// Return `None` if the origin is not inside of the shape or if
/// the EPA algorithm failed to compute the projection.
pub fn project_origin<N, G: ?Sized>(
    m: &Isometry<N>,
    g: &G,
    simplex: &mut VoronoiSimplex<N>,
) -> Option<Point<N>>
where
    N: RealField,
    G: SupportMap<N>,
{
    match closest_points(
        m,
        g,
        &Isometry::identity(),
        &ConstantOrigin,
        N::max_value(),
        true,
        simplex,
    ) {
        GJKResult::Intersection => None,
        GJKResult::ClosestPoints(p, _, _) => Some(p),
        _ => unreachable!(),
    }
}

/*
 * Separating Axis GJK
 */
/// Projects the origin on a shape using the Separating Axis GJK algorithm.
/// The algorithm will stop as soon as the polytope can be proven to be at least `max_dist` away
/// from the origin.
///
/// # Arguments:
/// * simplex - the simplex to be used by the GJK algorithm. It must be already initialized
///             with at least one point on the shape boundary.
/// * exact_dist - if `false`, the gjk will stop as soon as it can prove that the origin is at
/// a distance smaller than `max_dist` but not inside of `shape`. In that case, it returns a
/// `GJKResult::Proximity(sep_axis)` where `sep_axis` is a separating axis. If `false` the gjk will
/// compute the exact distance and return `GJKResult::Projection(point)` if the origin is closer
/// than `max_dist` but not inside `shape`.
pub fn closest_points<N, G1: ?Sized, G2: ?Sized>(
    m1: &Isometry<N>,
    g1: &G1,
    m2: &Isometry<N>,
    g2: &G2,
    max_dist: N,
    exact_dist: bool,
    simplex: &mut VoronoiSimplex<N>,
) -> GJKResult<N>
where
    N: RealField,
    G1: SupportMap<N>,
    G2: SupportMap<N>,
{
    let _eps = N::default_epsilon();
    let _eps_tol: N = eps_tol();
    let _eps_rel: N = _eps_tol.sqrt();

    // FIXME: reset the simplex if it is empty?
    let mut proj = simplex.project_origin_and_reduce();

    let mut old_dir;

    if let Some(proj_dir) = Unit::try_new(proj.coords, N::zero()) {
        old_dir = -proj_dir;
    } else {
        return GJKResult::Intersection;
    }

    let mut max_bound = N::max_value();
    let mut dir;
    let mut niter = 0;

    loop {
        let old_max_bound = max_bound;

        if let Some((new_dir, dist)) = Unit::try_new_and_get(-proj.coords, _eps_tol) {
            dir = new_dir;
            max_bound = dist;
        } else {
            // The origin is on the simplex.
            return GJKResult::Intersection;
        }

        if max_bound >= old_max_bound {
            if exact_dist {
                let (p1, p2) = result(simplex, true);
                return GJKResult::ClosestPoints(p1, p2, old_dir); // upper bounds inconsistencies
            } else {
                return GJKResult::Proximity(old_dir);
            }
        }

        let cso_point = CSOPoint::from_shapes(m1, g1, m2, g2, &dir);
        let min_bound = -dir.dot(&cso_point.point.coords);

        assert!(min_bound == min_bound);

        if min_bound > max_dist {
            return GJKResult::NoIntersection(dir);
        } else if !exact_dist && min_bound > na::zero() && max_bound <= max_dist {
            return GJKResult::Proximity(old_dir);
        } else if max_bound - min_bound <= _eps_rel * max_bound {
            if exact_dist {
                let (p1, p2) = result(simplex, false);
                return GJKResult::ClosestPoints(p1, p2, dir); // the distance found has a good enough precision
            } else {
                return GJKResult::Proximity(dir);
            }
        }

        if !simplex.add_point(cso_point) {
            if exact_dist {
                let (p1, p2) = result(simplex, false);
                return GJKResult::ClosestPoints(p1, p2, dir);
            } else {
                return GJKResult::Proximity(dir);
            }
        }

        old_dir = dir;
        proj = simplex.project_origin_and_reduce();

        if simplex.dimension() == DIM {
            if min_bound >= _eps_tol {
                if exact_dist {
                    let (p1, p2) = result(simplex, true);
                    return GJKResult::ClosestPoints(p1, p2, old_dir);
                } else {
                    // NOTE: previous implementation used old_proj here.
                    return GJKResult::Proximity(old_dir);
                }
            } else {
                return GJKResult::Intersection; // Point inside of the cso.
            }
        }
        niter += 1;
        if niter == 10000 {
            //            eprintln!("Error: GJK did not converge.");
            return GJKResult::NoIntersection(Vector::x_axis());
        }
    }
}

/// Casts a ray on a support map using the GJK algorithm.
pub fn cast_ray<N, G: ?Sized>(
    m: &Isometry<N>,
    shape: &G,
    simplex: &mut VoronoiSimplex<N>,
    ray: &Ray<N>,
    max_toi: N,
) -> Option<(N, Vector<N>)>
where
    N: RealField,
    G: SupportMap<N>,
{
    let m2 = Isometry::identity();
    let g2 = ConstantOrigin;
    minkowski_ray_cast(m, shape, &m2, &g2, ray, max_toi, simplex)
}

/// Compute the normal and the distance that can travel `g1` along the direction
/// `dir` so that `g1` and `g2` just touch.
pub fn directional_distance<N, G1: ?Sized, G2: ?Sized>(
    m1: &Isometry<N>,
    g1: &G1,
    m2: &Isometry<N>,
    g2: &G2,
    dir: &Vector<N>,
    simplex: &mut VoronoiSimplex<N>,
) -> Option<(N, Vector<N>, Point<N>, Point<N>)>
where
    N: RealField,
    G1: SupportMap<N>,
    G2: SupportMap<N>,
{
    let ray = Ray::new(Point::origin(), *dir);
    minkowski_ray_cast(m1, g1, m2, g2, &ray, N::max_value(), simplex).map(|(toi, normal)| {
        let witnesses = if !toi.is_zero() {
            result(simplex, simplex.dimension() == DIM)
        } else {
            // If there is penetration, the witness points
            // are undefined.
            (Point::origin(), Point::origin())
        };

        (toi, normal, witnesses.0, witnesses.1)
    })
}

// Ray-cast on the Minkowski Difference `m1 * g1 - m2 * g2`.
fn minkowski_ray_cast<N, G1: ?Sized, G2: ?Sized>(
    m1: &Isometry<N>,
    g1: &G1,
    m2: &Isometry<N>,
    g2: &G2,
    ray: &Ray<N>,
    max_toi: N,
    simplex: &mut VoronoiSimplex<N>,
) -> Option<(N, Vector<N>)>
where
    N: RealField,
    G1: SupportMap<N>,
    G2: SupportMap<N>,
{
    let _eps = N::default_epsilon();
    let _eps_tol: N = eps_tol();
    let _eps_rel: N = _eps_tol.sqrt();

    let ray_length = ray.dir.norm();

    if relative_eq!(ray_length, N::zero()) {
        return None;
    }

    let mut ltoi = N::zero();
    let mut curr_ray = Ray::new(ray.origin, ray.dir / ray_length);
    let dir = -curr_ray.dir;
    let mut ldir = dir;

    // Initialize the simplex.
    let support_point = CSOPoint::from_shapes(m1, g1, m2, g2, &dir);
    simplex.reset(support_point.translate(&-curr_ray.origin.coords));

    // FIXME: reset the simplex if it is empty?
    let mut proj = simplex.project_origin_and_reduce();
    let mut max_bound = N::max_value();
    let mut dir;
    let mut niter = 0;
    let mut last_chance = false;

    loop {
        let old_max_bound = max_bound;

        if let Some((new_dir, dist)) = Unit::try_new_and_get(-proj.coords, _eps_tol) {
            dir = new_dir;
            max_bound = dist;
        } else {
            return Some((ltoi / ray_length, ldir));
        }

        let support_point = if max_bound >= old_max_bound {
            // Upper bounds inconsistencies. Consider the projection as a valid support point.
            last_chance = true;
            CSOPoint::single_point(proj + curr_ray.origin.coords)
        } else {
            CSOPoint::from_shapes(m1, g1, m2, g2, &dir)
        };

        if last_chance && ltoi > N::zero() {
            // last_chance && ltoi > N::zero() && (support_point.point - curr_ray.origin).dot(&ldir) >= N::zero() {
            return Some((ltoi / ray_length, ldir));
        }

        // Clip the ray on the support plane (None <=> t < 0)
        // The configurations are:
        //   dir.dot(curr_ray.dir)  |   t   |               Action
        // −−−−−−−−−−−−−−−−−−−−-----+−−−−−−−+−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
        //          < 0             |  < 0  | Continue.
        //          < 0             |  > 0  | New lower bound, move the origin.
        //          > 0             |  < 0  | Miss. No intersection.
        //          > 0             |  > 0  | New higher bound.
        match query::ray_toi_with_plane(&support_point.point, &dir, &curr_ray) {
            Some(t) => {
                if dir.dot(&curr_ray.dir) < na::zero() && t > N::zero() {
                    // new lower bound
                    ldir = *dir;
                    ltoi += t;

                    // NOTE: we divide by ray_length instead of doing max_toi * ray_length
                    // because the multiplication may cause an overflow if max_toi is set
                    // to N::max_value() by users that want to have an infinite ray.
                    if ltoi / ray_length > max_toi {
                        return None;
                    }

                    let shift = curr_ray.dir * t;
                    curr_ray.origin += shift;
                    max_bound = N::max_value();
                    simplex.modify_pnts(&|pt| pt.translate_mut(&-shift));
                    last_chance = false;
                }
            }
            None => {
                if dir.dot(&curr_ray.dir) > _eps_tol {
                    // miss
                    return None;
                }
            }
        }

        if last_chance {
            return None;
        }

        let min_bound = -dir.dot(&(support_point.point.coords - curr_ray.origin.coords));

        assert!(min_bound == min_bound);

        if max_bound - min_bound <= _eps_rel * max_bound {
            // This is needed when using fixed-points to avoid missing
            // some castes.
            // FIXME: I feel like we should always return `Some` in
            // this case, even with floating-point numbers. Though it
            // has not been sufficinetly tested with floats yet to be sure.
            if cfg!(feature = "improved_fixed_point_support") {
                return Some((ltoi / ray_length, ldir));
            } else {
                return None;
            }
        }

        let _ = simplex.add_point(support_point.translate(&-curr_ray.origin.coords));
        proj = simplex.project_origin_and_reduce();

        if simplex.dimension() == DIM {
            if min_bound >= _eps_tol {
                return None;
            } else {
                return Some((ltoi / ray_length, ldir)); // Point inside of the cso.
            }
        }

        niter += 1;
        if niter == 10000 {
            //            eprintln!("Error: GJK did not converge.");
            return None;
        }
    }
}

fn result<N: RealField>(simplex: &VoronoiSimplex<N>, prev: bool) -> (Point<N>, Point<N>) {
    let mut res = (Point::origin(), Point::origin());
    if prev {
        for i in 0..simplex.prev_dimension() + 1 {
            let coord = simplex.prev_proj_coord(i);
            let point = simplex.prev_point(i);
            res.0 += point.orig1.coords * coord;
            res.1 += point.orig2.coords * coord;
        }

        res
    } else {
        for i in 0..simplex.dimension() + 1 {
            let coord = simplex.proj_coord(i);
            let point = simplex.point(i);
            res.0 += point.orig1.coords * coord;
            res.1 += point.orig2.coords * coord;
        }

        res
    }
}
<|endoftext|>