<commit_before>#[macro_use]
extern crate serde_derive;
extern crate serde;
extern crate serde_xml_rs;

#[derive(Debug, Deserialize, Default)]
pub struct Mass {
    pub value: f64,
}

#[derive(Debug, Deserialize, Default)]
pub struct Inertia {
    pub ixx: f64,
    pub ixy: f64,
    pub ixz: f64,
    pub iyy: f64,
    pub iyz: f64,
    pub izz: f64,
}

#[derive(Debug, Deserialize, Default)]
pub struct Inertial {
    #[serde(default)]
    pub origin: Pose,
    pub mass: Mass,
    pub inertia: Inertia,
}

#[derive(Debug, Deserialize)]
#[serde(rename_all = "snake_case")]
pub enum Geometry {
    Box {
        #[serde(with = "urdf_vec3")]
        size: [f64; 3]
        //size: String
    },
    Cylinder { radius: f64, length: f64 },
    Sphere { radius: f64 },
    Mesh { filename: String, scale: f64 },
}

impl Default for Geometry {
    fn default() -> Geometry {
        Geometry::Box { size: [0.0f64, 0.0, 0.0] }
    }
}

#[derive(Debug, Deserialize, Default)]
pub struct Color {
    #[serde(with = "urdf_vec4")]
    #[serde(default="default_rgba")]
    pub rgba: [f64; 4],
}

#[derive(Debug, Deserialize)]
pub struct Texture {
    pub filename: String,
}

impl Default for Texture {
    fn default() -> Texture {
        Texture { filename: "".to_string() }
    }
}

#[derive(Debug, Deserialize, Default)]
pub struct Material {
    #[serde(default)]
    pub name: String,
    #[serde(default)]
    pub color: Color,
    #[serde(default)]
    pub texture: Texture,
}

#[derive(Debug, Deserialize, Default)]
pub struct Visual {
    #[serde(default)]
    pub name: String,
    #[serde(default)]
    pub origin: Pose,
    pub geometry: Geometry,
    #[serde(default)]
    pub material: Material,
}

#[derive(Debug, Deserialize, Default)]
pub struct Collision {
    #[serde(default)]
    pub name: String,
    pub origin: Pose,
    pub geometry: Geometry,
}

/// Urdf Link element
/// See http://wiki.ros.org/urdf/XML/link for more detail.
#[derive(Debug, Deserialize)]
pub struct Link {
    pub name: String,
    #[serde(default)]
    pub inertial: Inertial,
    #[serde(default)]
    pub visual: Visual,
    #[serde(default)]
    pub collision: Collision,
}

#[derive(Deserialize, Debug)]
pub struct Vec3 {
    #[serde(with = "urdf_vec3")]
    pub data: [f64; 3]
}

mod urdf_vec3 {
    use serde::{self, Deserialize, Deserializer};
    pub fn deserialize<D>(deserializer: D) -> Result<[f64; 3], D::Error>
    where D: Deserializer
{
        let s = String::deserialize(deserializer)?;
        let vec = s.split(" ").filter_map(|x| x.parse::<f64>().ok()).collect::<Vec<_>>();
        if vec.len() != 3 {
            return Err(serde::de::Error::custom(
                format!("failed to parse float array in {}", s)));
        }
        let mut arr = [0.0f64; 3];
        for i in 0..3 {
            arr[i] = vec[i];
        }
        Ok(arr)
    }
}

mod urdf_vec4 {
    use serde::{self, Deserialize, Deserializer};
    pub fn deserialize<D>(deserializer: D) -> Result<[f64; 4], D::Error>
    where D: Deserializer
{
        let s = String::deserialize(deserializer)?;
        let vec = s.split(" ").filter_map(|x| x.parse::<f64>().ok()).collect::<Vec<_>>();
        if vec.len() != 4 {
            return Err(serde::de::Error::custom(
                format!("failed to parse float array in {}", s)));
        }
        let mut arr = [0.0f64; 4];
        for i in 0..4 {
            arr[i] = vec[i];
        }
        Ok(arr)
    }
}

#[derive(Debug, Deserialize)]
pub struct Axis {
    #[serde(with = "urdf_vec3")]
    pub xyz: [f64; 3]
}

impl Default for Axis {
    fn default() -> Axis {
        Axis { xyz: [1.0f64, 0.0, 0.0,] }
    }
}

#[derive(Debug, Deserialize)]
pub struct Pose {
    #[serde(with = "urdf_vec3")]
    #[serde(default="default_zero3")]
    pub xyz: [f64; 3],
    #[serde(with = "urdf_vec3")]
    #[serde(default="default_zero3")]
    pub rpy: [f64; 3],
}

fn default_zero3() -> [f64; 3] {
    [0.0f64, 0.0, 0.0]
}

fn default_rgba() -> [f64; 4] {
    [1.0f64, 1.0, 1.0, 1.0]
}

impl Default for Pose {
    fn default() -> Pose {
        Pose {
            xyz: default_zero3(),
            rpy: default_zero3(),
        }
    }
}

#[derive(Debug, Deserialize)]
pub struct LinkName {
    pub link: String,
}

#[derive(Debug, Deserialize)]
#[serde(rename_all = "snake_case")]
pub enum JointType {
    Revolute,
    Continuous,
    Prismatic,
    Fixed,
    Floating,
    Planar,
}

#[derive(Debug, Deserialize)]
pub struct JointLimit {
    #[serde(default)]
    pub lower: f64,
    #[serde(default)]
    pub upper: f64,
    pub effort: f64,
    pub velocity: f64,
}

#[derive(Debug, Deserialize, Default)]
pub struct Mimic {
    joint: String,
    multiplier: f64,
    offset: f64,
}

#[derive(Debug, Deserialize, Default)]
pub struct SafetyController {
    soft_lower_limit: f64,
    soft_upper_limit: f64,
    k_position: f64,
    k_velocity: f64,
}

/// Urdf Joint element
/// See http://wiki.ros.org/urdf/XML/joint for more detail.
#[derive(Debug, Deserialize)]
pub struct Joint {
    pub name: String,
    #[serde(rename = "type")]
    pub joint_type: JointType,
    #[serde(default)]
    pub origin: Pose,
    pub parent: LinkName,
    pub child: LinkName,
    #[serde(default)]
    pub axis: Axis,
    pub limit: JointLimit,
    #[serde(default)]
    pub dynamics: Dynamics,
    #[serde(default)]
    pub mimic: Mimic,
    #[serde(default)]
    pub safety_controller: SafetyController,
}

#[derive(Debug, Deserialize, Default)]
pub struct Dynamics {
    damping: f64,
    friction: f64,
}

/// Top level struct to access urdf.
#[derive(Debug, Deserialize)]
pub struct Robot {
    pub name: String,

    #[serde(rename = "link", default)]
    pub links: Vec<Link>,

    #[serde(rename = "joint", default)]
    pub joints: Vec<Joint>,
}

use std::fs::File;
use std::io::prelude::*;
use std::path::Path;
use std::error::Error;
use std::fmt;

#[derive(Debug)]
pub enum UrdfError {
    File(std::io::Error),
    Xml(serde_xml_rs::Error),
    Parse(String),
}

impl fmt::Display for UrdfError {
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
        match *self {
            UrdfError::File(ref err) => err.fmt(f),
            UrdfError::Xml(ref err) => err.fmt(f),
            UrdfError::Parse(ref msg) => write!(f, "parse error {}", msg),
        }
    }
}

impl Error for UrdfError {
    fn description(&self) -> &str {
        match *self {
            UrdfError::File(ref err) => err.description(),
            UrdfError::Xml(ref err) => err.description(),
            UrdfError::Parse(_) => "parse error",
        }
    }
}

impl From<std::io::Error> for UrdfError {
    fn from(err: std::io::Error) -> UrdfError {
        UrdfError::File(err)
    }
}

impl From<serde_xml_rs::Error> for UrdfError {
    fn from(err: serde_xml_rs::Error) -> UrdfError {
        UrdfError::Xml(err)
    }
}

/// Read urdf file and create Robot instance
///
/// # Examples
///
/// ```
/// extern crate urdf_rs;
/// let urdf_robo = urdf_rs::read_file("sample.urdf").unwrap();
/// let links = urdf_robo.links;
/// println!("{:?}", links[0].visual.origin.xyz);
/// ```
pub fn read_file<P: AsRef<Path>>(path: P) -> Result<Robot, UrdfError> {
    let mut file = File::open(path)?;
    let mut contents = String::new();
    file.read_to_string(&mut contents)?;
    read_from_string(&contents)
}

/// Read from string instead of file.
///
///
/// # Examples
///
/// ```
/// let s = r##"
///     <robot name="robo">
///         <link name="shoulder1">
///             <inertial>
///                 <origin xyz="0 0 0.5" rpy="0 0 0"/>
///                 <mass value="1"/>
///                 <inertia ixx="100"  ixy="0"  ixz="0" iyy="100" iyz="0" izz="100" />
///             </inertial>
///             <visual>
///                 <origin xyz="0.1 0.2 0.3" rpy="-0.1 -0.2  -0.3" />
///                 <geometry>
///                     <box size="1.0 2.0 3.0" />
///                 </geometry>
///                 <material name="Cyan">
///                     <color rgba="0 1.0 1.0 1.0"/>
///                 </material>
///             </visual>
///             <collision>
///                 <origin xyz="0 0 0" rpy="0 0 0"/>
///                 <geometry>
///                     <cylinder radius="1" length="0.5"/>
///                 </geometry>
///             </collision>
///         </link>
///         <link name="elbow1" />
///         <link name="wrist1" />
///         <joint name="shoulder_pitch" type="revolute">
///             <origin xyz="0.0 0.0 0.1" />
///             <parent link="shoulder1" />
///             <child link="elbow1" />
///             <axis xyz="0 1 -1" />
///             <limit lower="-1" upper="1.0" effort="0" velocity="1.0"/>
///         </joint>
///         <joint name="shoulder_pitch" type="revolute">
///             <origin xyz="0.0 0.0 0.0" />
///             <parent link="elbow1" />
///             <child link="wrist1" />
///             <axis xyz="0 1 0" />
///             <limit lower="-2" upper="1.0" effort="0" velocity="1.0"/>
///         </joint>
///     </robot>
///    "##;
/// let urdf_robo = urdf_rs::read_from_string(s).unwrap();
/// println!("{:?}", urdf_robo.links[0].visual.origin.xyz);
/// ```

pub fn read_from_string(string: &str) -> Result<Robot, UrdfError> {
    serde_xml_rs::deserialize(string.as_bytes())
        .map_err(From::from)
}

#[test]
fn it_works() {
    let s = r##"
        <robot name="robo">
            <link name="shoulder1">
                <inertial>
                    <origin xyz="0 0 0.5" rpy="0 0 0"/>
                    <mass value="1"/>
                    <inertia ixx="100"  ixy="0"  ixz="0" iyy="100" iyz="0" izz="100" />
                </inertial>
                <visual>
                    <origin xyz="0.1 0.2 0.3" rpy="-0.1 -0.2  -0.3" />
                    <geometry>
                        <box size="1.0 2.0 3.0" />
                    </geometry>
                    <material name="Cyan">
                        <color rgba="0 1.0 1.0 1.0"/>
                    </material>
                </visual>
                <collision>
                    <origin xyz="0 0 0" rpy="0 0 0"/>
                    <geometry>
                        <cylinder radius="1" length="0.5"/>
                    </geometry>
                </collision>
            </link>
            <link name="elbow1" />
            <link name="wrist1" />
            <joint name="shoulder_pitch" type="revolute">
                <origin xyz="0.0 0.0 0.1" />
                <parent link="shoulder1" />
                <child link="elbow1" />
                <axis xyz="0 1 -1" />
                <limit lower="-1" upper="1.0" effort="0" velocity="1.0"/>
            </joint>
            <joint name="shoulder_pitch" type="revolute">
                <origin xyz="0.0 0.0 0.0" />
                <parent link="elbow1" />
                <child link="wrist1" />
                <axis xyz="0 1 0" />
                <limit lower="-2" upper="1.0" effort="0" velocity="1.0"/>
            </joint>
        </robot>
    "##;
    let robo = read_from_string(s).unwrap();

    assert_eq!(robo.name, "robo");
    assert_eq!(robo.links.len(), 3);
    assert_eq!(robo.joints.len(), 2);
    let xyz = robo.links[0].visual.origin.xyz;
    assert_eq!(xyz[0], 0.1);
    assert_eq!(xyz[1], 0.2);
    assert_eq!(xyz[2], 0.3);
    let rpy = robo.links[0].visual.origin.rpy;
    assert_eq!(rpy[0], -0.1);
    assert_eq!(rpy[1], -0.2);
    assert_eq!(rpy[2], -0.3);

    match robo.links[0].visual.geometry {
        Geometry::Box{size} => {
            assert_eq!(size[0], 1.0f64);
            assert_eq!(size[1], 2.0f64);
            assert_eq!(size[2], 3.0f64);
        },
        _ => { panic!("geometry error") },
    }

    assert_eq!(robo.joints[0].name, "shoulder_pitch");
    let xyz = robo.joints[0].axis.xyz;
    assert_eq!(xyz[0], 0.0f64);
    assert_eq!(xyz[1], 1.0f64);
    assert_eq!(xyz[2], -1.0f64);
    let xyz = robo.joints[0].axis.xyz;
    //"0 1 -1"
    assert_eq!(xyz[0], 0.0);
    assert_eq!(xyz[1], 1.0);
    assert_eq!(xyz[2], -1.0);

}
<commit_msg>Support default scale<commit_after>#[macro_use]
extern crate serde_derive;
extern crate serde;
extern crate serde_xml_rs;

#[derive(Debug, Deserialize, Default)]
pub struct Mass {
    pub value: f64,
}

#[derive(Debug, Deserialize, Default)]
pub struct Inertia {
    pub ixx: f64,
    pub ixy: f64,
    pub ixz: f64,
    pub iyy: f64,
    pub iyz: f64,
    pub izz: f64,
}

#[derive(Debug, Deserialize, Default)]
pub struct Inertial {
    #[serde(default)]
    pub origin: Pose,
    pub mass: Mass,
    pub inertia: Inertia,
}

#[derive(Debug, Deserialize)]
#[serde(rename_all = "snake_case")]
pub enum Geometry {
    Box {
        #[serde(with = "urdf_vec3")]
        size: [f64; 3]
    },
    Cylinder { radius: f64, length: f64 },
    Sphere { radius: f64 },
    Mesh {
        filename: String,
        #[serde(default = "default_scale")]
        scale: f64,
    },
}

fn default_scale() -> f64 {
    1.0f64
}

impl Default for Geometry {
    fn default() -> Geometry {
        Geometry::Box { size: [0.0f64, 0.0, 0.0] }
    }
}

#[derive(Debug, Deserialize, Default)]
pub struct Color {
    #[serde(with = "urdf_vec4")]
    #[serde(default="default_rgba")]
    pub rgba: [f64; 4],
}

#[derive(Debug, Deserialize)]
pub struct Texture {
    pub filename: String,
}

impl Default for Texture {
    fn default() -> Texture {
        Texture { filename: "".to_string() }
    }
}

#[derive(Debug, Deserialize, Default)]
pub struct Material {
    #[serde(default)]
    pub name: String,
    #[serde(default)]
    pub color: Color,
    #[serde(default)]
    pub texture: Texture,
}

#[derive(Debug, Deserialize, Default)]
pub struct Visual {
    #[serde(default)]
    pub name: String,
    #[serde(default)]
    pub origin: Pose,
    pub geometry: Geometry,
    #[serde(default)]
    pub material: Material,
}

#[derive(Debug, Deserialize, Default)]
pub struct Collision {
    #[serde(default)]
    pub name: String,
    pub origin: Pose,
    pub geometry: Geometry,
}

/// Urdf Link element
/// See http://wiki.ros.org/urdf/XML/link for more detail.
#[derive(Debug, Deserialize)]
pub struct Link {
    pub name: String,
    #[serde(default)]
    pub inertial: Inertial,
    #[serde(default)]
    pub visual: Visual,
    #[serde(default)]
    pub collision: Collision,
}

#[derive(Deserialize, Debug)]
pub struct Vec3 {
    #[serde(with = "urdf_vec3")]
    pub data: [f64; 3]
}

mod urdf_vec3 {
    use serde::{self, Deserialize, Deserializer};
    pub fn deserialize<D>(deserializer: D) -> Result<[f64; 3], D::Error>
    where D: Deserializer
{
        let s = String::deserialize(deserializer)?;
        let vec = s.split(" ").filter_map(|x| x.parse::<f64>().ok()).collect::<Vec<_>>();
        if vec.len() != 3 {
            return Err(serde::de::Error::custom(
                format!("failed to parse float array in {}", s)));
        }
        let mut arr = [0.0f64; 3];
        for i in 0..3 {
            arr[i] = vec[i];
        }
        Ok(arr)
    }
}

mod urdf_vec4 {
    use serde::{self, Deserialize, Deserializer};
    pub fn deserialize<D>(deserializer: D) -> Result<[f64; 4], D::Error>
    where D: Deserializer
{
        let s = String::deserialize(deserializer)?;
        let vec = s.split(" ").filter_map(|x| x.parse::<f64>().ok()).collect::<Vec<_>>();
        if vec.len() != 4 {
            return Err(serde::de::Error::custom(
                format!("failed to parse float array in {}", s)));
        }
        let mut arr = [0.0f64; 4];
        for i in 0..4 {
            arr[i] = vec[i];
        }
        Ok(arr)
    }
}

#[derive(Debug, Deserialize)]
pub struct Axis {
    #[serde(with = "urdf_vec3")]
    pub xyz: [f64; 3]
}

impl Default for Axis {
    fn default() -> Axis {
        Axis { xyz: [1.0f64, 0.0, 0.0,] }
    }
}

#[derive(Debug, Deserialize)]
pub struct Pose {
    #[serde(with = "urdf_vec3")]
    #[serde(default="default_zero3")]
    pub xyz: [f64; 3],
    #[serde(with = "urdf_vec3")]
    #[serde(default="default_zero3")]
    pub rpy: [f64; 3],
}

fn default_zero3() -> [f64; 3] {
    [0.0f64, 0.0, 0.0]
}

fn default_rgba() -> [f64; 4] {
    [1.0f64, 1.0, 1.0, 1.0]
}

impl Default for Pose {
    fn default() -> Pose {
        Pose {
            xyz: default_zero3(),
            rpy: default_zero3(),
        }
    }
}

#[derive(Debug, Deserialize)]
pub struct LinkName {
    pub link: String,
}

#[derive(Debug, Deserialize)]
#[serde(rename_all = "snake_case")]
pub enum JointType {
    Revolute,
    Continuous,
    Prismatic,
    Fixed,
    Floating,
    Planar,
}

#[derive(Debug, Deserialize)]
pub struct JointLimit {
    #[serde(default)]
    pub lower: f64,
    #[serde(default)]
    pub upper: f64,
    pub effort: f64,
    pub velocity: f64,
}

#[derive(Debug, Deserialize, Default)]
pub struct Mimic {
    joint: String,
    multiplier: f64,
    offset: f64,
}

#[derive(Debug, Deserialize, Default)]
pub struct SafetyController {
    soft_lower_limit: f64,
    soft_upper_limit: f64,
    k_position: f64,
    k_velocity: f64,
}

/// Urdf Joint element
/// See http://wiki.ros.org/urdf/XML/joint for more detail.
#[derive(Debug, Deserialize)]
pub struct Joint {
    pub name: String,
    #[serde(rename = "type")]
    pub joint_type: JointType,
    #[serde(default)]
    pub origin: Pose,
    pub parent: LinkName,
    pub child: LinkName,
    #[serde(default)]
    pub axis: Axis,
    pub limit: JointLimit,
    #[serde(default)]
    pub dynamics: Dynamics,
    #[serde(default)]
    pub mimic: Mimic,
    #[serde(default)]
    pub safety_controller: SafetyController,
}

#[derive(Debug, Deserialize, Default)]
pub struct Dynamics {
    damping: f64,
    friction: f64,
}

/// Top level struct to access urdf.
#[derive(Debug, Deserialize)]
pub struct Robot {
    pub name: String,

    #[serde(rename = "link", default)]
    pub links: Vec<Link>,

    #[serde(rename = "joint", default)]
    pub joints: Vec<Joint>,
}

use std::fs::File;
use std::io::prelude::*;
use std::path::Path;
use std::error::Error;
use std::fmt;

#[derive(Debug)]
pub enum UrdfError {
    File(std::io::Error),
    Xml(serde_xml_rs::Error),
    Parse(String),
}

impl fmt::Display for UrdfError {
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
        match *self {
            UrdfError::File(ref err) => err.fmt(f),
            UrdfError::Xml(ref err) => err.fmt(f),
            UrdfError::Parse(ref msg) => write!(f, "parse error {}", msg),
        }
    }
}

impl Error for UrdfError {
    fn description(&self) -> &str {
        match *self {
            UrdfError::File(ref err) => err.description(),
            UrdfError::Xml(ref err) => err.description(),
            UrdfError::Parse(_) => "parse error",
        }
    }
}

impl From<std::io::Error> for UrdfError {
    fn from(err: std::io::Error) -> UrdfError {
        UrdfError::File(err)
    }
}

impl From<serde_xml_rs::Error> for UrdfError {
    fn from(err: serde_xml_rs::Error) -> UrdfError {
        UrdfError::Xml(err)
    }
}

/// Read urdf file and create Robot instance
///
/// # Examples
///
/// ```
/// extern crate urdf_rs;
/// let urdf_robo = urdf_rs::read_file("sample.urdf").unwrap();
/// let links = urdf_robo.links;
/// println!("{:?}", links[0].visual.origin.xyz);
/// ```
pub fn read_file<P: AsRef<Path>>(path: P) -> Result<Robot, UrdfError> {
    let mut file = File::open(path)?;
    let mut contents = String::new();
    file.read_to_string(&mut contents)?;
    read_from_string(&contents)
}

/// Read from string instead of file.
///
///
/// # Examples
///
/// ```
/// let s = r##"
///     <robot name="robo">
///         <link name="shoulder1">
///             <inertial>
///                 <origin xyz="0 0 0.5" rpy="0 0 0"/>
///                 <mass value="1"/>
///                 <inertia ixx="100"  ixy="0"  ixz="0" iyy="100" iyz="0" izz="100" />
///             </inertial>
///             <visual>
///                 <origin xyz="0.1 0.2 0.3" rpy="-0.1 -0.2  -0.3" />
///                 <geometry>
///                     <box size="1.0 2.0 3.0" />
///                 </geometry>
///                 <material name="Cyan">
///                     <color rgba="0 1.0 1.0 1.0"/>
///                 </material>
///             </visual>
///             <collision>
///                 <origin xyz="0 0 0" rpy="0 0 0"/>
///                 <geometry>
///                     <cylinder radius="1" length="0.5"/>
///                 </geometry>
///             </collision>
///         </link>
///         <link name="elbow1" />
///         <link name="wrist1" />
///         <joint name="shoulder_pitch" type="revolute">
///             <origin xyz="0.0 0.0 0.1" />
///             <parent link="shoulder1" />
///             <child link="elbow1" />
///             <axis xyz="0 1 -1" />
///             <limit lower="-1" upper="1.0" effort="0" velocity="1.0"/>
///         </joint>
///         <joint name="shoulder_pitch" type="revolute">
///             <origin xyz="0.0 0.0 0.0" />
///             <parent link="elbow1" />
///             <child link="wrist1" />
///             <axis xyz="0 1 0" />
///             <limit lower="-2" upper="1.0" effort="0" velocity="1.0"/>
///         </joint>
///     </robot>
///    "##;
/// let urdf_robo = urdf_rs::read_from_string(s).unwrap();
/// println!("{:?}", urdf_robo.links[0].visual.origin.xyz);
/// ```

pub fn read_from_string(string: &str) -> Result<Robot, UrdfError> {
    serde_xml_rs::deserialize(string.as_bytes())
        .map_err(From::from)
}

#[test]
fn it_works() {
    let s = r##"
        <robot name="robo">
            <link name="shoulder1">
                <inertial>
                    <origin xyz="0 0 0.5" rpy="0 0 0"/>
                    <mass value="1"/>
                    <inertia ixx="100"  ixy="0"  ixz="0" iyy="100" iyz="0" izz="100" />
                </inertial>
                <visual>
                    <origin xyz="0.1 0.2 0.3" rpy="-0.1 -0.2  -0.3" />
                    <geometry>
                        <box size="1.0 2.0 3.0" />
                    </geometry>
                    <material name="Cyan">
                        <color rgba="0 1.0 1.0 1.0"/>
                    </material>
                </visual>
                <collision>
                    <origin xyz="0 0 0" rpy="0 0 0"/>
                    <geometry>
                        <cylinder radius="1" length="0.5"/>
                    </geometry>
                </collision>
            </link>
            <link name="elbow1" />
            <link name="wrist1" />
            <joint name="shoulder_pitch" type="revolute">
                <origin xyz="0.0 0.0 0.1" />
                <parent link="shoulder1" />
                <child link="elbow1" />
                <axis xyz="0 1 -1" />
                <limit lower="-1" upper="1.0" effort="0" velocity="1.0"/>
            </joint>
            <joint name="shoulder_pitch" type="revolute">
                <origin xyz="0.0 0.0 0.0" />
                <parent link="elbow1" />
                <child link="wrist1" />
                <axis xyz="0 1 0" />
                <limit lower="-2" upper="1.0" effort="0" velocity="1.0"/>
            </joint>
        </robot>
    "##;
    let robo = read_from_string(s).unwrap();

    assert_eq!(robo.name, "robo");
    assert_eq!(robo.links.len(), 3);
    assert_eq!(robo.joints.len(), 2);
    let xyz = robo.links[0].visual.origin.xyz;
    assert_eq!(xyz[0], 0.1);
    assert_eq!(xyz[1], 0.2);
    assert_eq!(xyz[2], 0.3);
    let rpy = robo.links[0].visual.origin.rpy;
    assert_eq!(rpy[0], -0.1);
    assert_eq!(rpy[1], -0.2);
    assert_eq!(rpy[2], -0.3);

    match robo.links[0].visual.geometry {
        Geometry::Box{size} => {
            assert_eq!(size[0], 1.0f64);
            assert_eq!(size[1], 2.0f64);
            assert_eq!(size[2], 3.0f64);
        },
        _ => { panic!("geometry error") },
    }

    assert_eq!(robo.joints[0].name, "shoulder_pitch");
    let xyz = robo.joints[0].axis.xyz;
    assert_eq!(xyz[0], 0.0f64);
    assert_eq!(xyz[1], 1.0f64);
    assert_eq!(xyz[2], -1.0f64);
    let xyz = robo.joints[0].axis.xyz;
    //"0 1 -1"
    assert_eq!(xyz[0], 0.0);
    assert_eq!(xyz[1], 1.0);
    assert_eq!(xyz[2], -1.0);
}
<|endoftext|>


<commit_before>//! Counting occurrences of a byte in a slice

#![no_std]

#[cfg(feature = "simd-accel")]
extern crate simd;

use core::{cmp, mem, ops, slice, usize};

#[cfg(feature = "simd-accel")]
use simd::u8x16;
#[cfg(feature = "avx-accel")]
use simd::x86::sse2::Sse2U8x16;
#[cfg(feature = "avx-accel")]
use simd::x86::avx::{LowHigh128, u8x32};

trait ByteChunk: Copy {
    type Splat: Copy;
    fn splat(byte: u8) -> Self::Splat;
    fn from_splat(splat: Self::Splat) -> Self;
    fn bytewise_equal(self, other: Self::Splat) -> Self;
    fn is_leading_utf8_byte(self) -> Self;
    fn increment(self, incr: Self) -> Self;
    fn sum(&self) -> usize;
}

impl ByteChunk for usize {
    type Splat = Self;

    fn splat(byte: u8) -> Self {
        let lo = usize::MAX / 0xFF;
        lo * byte as usize
    }

    fn from_splat(splat: Self) -> Self {
        splat
    }

    fn is_leading_utf8_byte(self) -> Self {
        // a leading UTF-8 byte is one which does not start with the bits 10.
        ((!self >> 7) | (self >> 6)) & Self::splat(1)
    }

    fn bytewise_equal(self, other: Self) -> Self {
        let lo = usize::MAX / 0xFF;
        let hi = lo << 7;

        let x = self ^ other;
        !((((x & !hi) + !hi) | x) >> 7) & lo
    }

    fn increment(self, incr: Self) -> Self {
        self + incr
    }

    fn sum(&self) -> usize {
        let every_other_byte_lo = usize::MAX / 0xFFFF;
        let every_other_byte = every_other_byte_lo * 0xFF;

        // Pairwise reduction to avoid overflow on next step.
        let pair_sum: usize = (self & every_other_byte) + ((self >> 8) & every_other_byte);

        // Multiplication results in top two bytes holding sum.
        pair_sum.wrapping_mul(every_other_byte_lo) >> ((mem::size_of::<usize>() - 2) * 8)
    }
}

#[cfg(feature = "simd-accel")]
impl ByteChunk for u8x16 {
    type Splat = Self;

    fn splat(byte: u8) -> Self {
        Self::splat(byte)
    }

    fn from_splat(splat: Self) -> Self {
        splat
    }

    fn is_leading_utf8_byte(self) -> Self {
        (self & Self::splat(0b1100_0000)).ne(Self::splat(0b1000_0000)).to_repr().to_u8()
    }

    fn bytewise_equal(self, other: Self) -> Self {
        self.eq(other).to_repr().to_u8()
    }

    fn increment(self, incr: Self) -> Self {
        // incr on -1
        self - incr
    }

    fn sum(&self) -> usize {
        let mut count = 0;
        for i in 0..16 {
            count += self.extract(i) as usize;
        }
        count
    }
}

#[cfg(feature = "avx-accel")]
impl ByteChunk for u8x32 {
    type Splat = Self;

    fn splat(byte: u8) -> Self {
        Self::splat(byte)
    }

    fn from_splat(splat: Self) -> Self {
        splat
    }

    fn is_leading_utf8_byte(self) -> Self {
        (self & Self::splat(0b1100_0000)).ne(Self::splat(0b1000_0000)).to_repr().to_u8()
    }

    fn bytewise_equal(self, other: Self) -> Self {
        self.eq(other).to_repr().to_u8()
    }

    fn increment(self, incr: Self) -> Self {
        // incr on -1
        self - incr
    }

    fn sum(&self) -> usize {
        let zero = u8x16::splat(0);
        let sad_lo = self.low().sad(zero);
        let sad_hi = self.high().sad(zero);

        let mut count = 0;
        count += (sad_lo.extract(0) + sad_lo.extract(1)) as usize;
        count += (sad_hi.extract(0) + sad_hi.extract(1)) as usize;
        count
    }
}

impl<T> ByteChunk for [T; 4]
    where T: ByteChunk<Splat = T>
{
    type Splat = T;

    fn splat(byte: u8) -> T {
        T::splat(byte)
    }

    fn from_splat(splat: T) -> Self {
        [splat, splat, splat, splat]
    }

    fn is_leading_utf8_byte(mut self) -> Self {
        for t in self[..].iter_mut() {
            *t = t.is_leading_utf8_byte();
        }
        self
    }

    fn bytewise_equal(mut self, needles: Self::Splat) -> Self {
        for t in self[..].iter_mut() {
            *t = t.bytewise_equal(needles);
        }
        self
    }

    fn increment(self, incr: Self) -> Self {
        [self[0].increment(incr[0]),
         self[1].increment(incr[1]),
         self[2].increment(incr[2]),
         self[3].increment(incr[3])]
    }

    fn sum(&self) -> usize {
        self[..].iter().map(ByteChunk::sum).fold(0, ops::Add::add)
    }
}

fn chunk_align<Chunk: ByteChunk>(x: &[u8]) -> (&[u8], &[Chunk], &[u8]) {
    let align = mem::size_of::<Chunk>();

    let offset_ptr = (x.as_ptr() as usize) % align;
    let offset_end = (x.as_ptr() as usize + x.len()) % align;

    let d2 = x.len().saturating_sub(offset_end);
    let d1 = cmp::min((align - offset_ptr) % align, d2);

    let (init, tail) = x.split_at(d2);
    let (init, mid) = init.split_at(d1);
    assert_eq!(mid.len() % align, 0);
    let mid = unsafe { slice::from_raw_parts(mid.as_ptr() as *const Chunk, mid.len() / align) };

    (init, mid, tail)
}

fn chunk_count<Chunk: ByteChunk>(haystack: &[Chunk], needle: u8) -> usize {
    let zero = Chunk::splat(0);
    let needles = Chunk::splat(needle);
    let mut count = 0;
    let mut i = 0;

    while i < haystack.len() {
        let mut counts = Chunk::from_splat(zero);

        let end = cmp::min(i + 255, haystack.len());
        for &chunk in &haystack[i..end] {
            counts = counts.increment(chunk.bytewise_equal(needles));
        }
        i = end;

        count += counts.sum();
    }

    count
}

fn count_generic<Chunk: ByteChunk<Splat = Chunk>>(naive_below: usize,
                                                  group_above: usize,
                                                  haystack: &[u8],
                                                  needle: u8)
                                                  -> usize {
    let mut count = 0;

    // Extract pre/post so naive_count is only inlined once.
    let len = haystack.len();
    let unchunked = if len < naive_below {
        [haystack, &haystack[0..0]]
    } else if len <= group_above {
        let (pre, mid, post) = chunk_align::<Chunk>(haystack);
        count += chunk_count(mid, needle);
        [pre, post]
    } else {
        let (pre, mid, post) = chunk_align::<[Chunk; 4]>(haystack);
        count += chunk_count(mid, needle);
        [pre, post]
    };

    for &slice in &unchunked {
        count += naive_count(slice, needle);
    }

    count
}

/// Count occurrences of a byte in a slice of bytes, fast
///
/// # Examples
///
/// ```
/// let s = b"This is a Text with spaces";
/// let number_of_spaces = bytecount::count(s, b' ');
/// assert_eq!(number_of_spaces, 5);
/// ```
#[cfg(not(feature = "simd-accel"))]
pub fn count(haystack: &[u8], needle: u8) -> usize {
    // Never use [usize; 4]
    count_generic::<usize>(32, usize::MAX, haystack, needle)
}

#[cfg(all(feature = "simd-accel", not(feature = "avx-accel")))]
pub fn count(haystack: &[u8], needle: u8) -> usize {
    count_generic::<u8x16>(32, 4096, haystack, needle)
}

#[cfg(feature = "avx-accel")]
pub fn count(haystack: &[u8], needle: u8) -> usize {
    count_generic::<u8x32>(64, 4096, haystack, needle)
}

/// Count up to `(2^32)-1` occurrences of a byte in a slice
/// of bytes, simple
///
/// # Example
///
/// ```
/// let s = b"This is yet another Text with spaces";
/// let number_of_spaces = bytecount::naive_count(s, b' ');
/// assert_eq!(number_of_spaces, 6);
/// ```
pub fn naive_count_32(haystack: &[u8], needle: u8) -> usize {
    haystack.iter().fold(0, |n, c| n + (*c == needle) as u32) as usize
}

/// Count occurrences of a byte in a slice of bytes, simple
///
/// # Example
///
/// ```
/// let s = b"This is yet another Text with spaces";
/// let number_of_spaces = bytecount::naive_count(s, b' ');
/// assert_eq!(number_of_spaces, 6);
/// ```
pub fn naive_count(haystack: &[u8], needle: u8) -> usize {
    haystack.iter().fold(0, |n, c| n + (*c == needle) as usize)
}

fn chunk_num_chars<Chunk: ByteChunk>(haystack: &[Chunk]) -> usize {
    let zero = Chunk::splat(0);
    let mut count = 0;
    let mut i = 0;
    while i < haystack.len() {
        let mut counts = Chunk::from_splat(zero);
        let end = cmp::min(i + 255, haystack.len());
        for &chunk in &haystack[i..end] {
            counts = counts.increment(chunk.is_leading_utf8_byte());
        }
        i = end;
        count += counts.sum();
    }
    count
}

fn num_chars_generic<Chunk: ByteChunk<Splat = Chunk>>(naive_below: usize,
                                                    group_above: usize,
                                                    haystack: &[u8])
                                                    -> usize {
    // Extract pre/post so naive_count is only inlined once.
    let len = haystack.len();
    let mut count = 0;
    let unchunked = if len < naive_below {
        [haystack, &haystack[0..0]]
    } else if len <= group_above {
        let (pre, mid, post) = chunk_align::<Chunk>(haystack);
        count += chunk_num_chars(mid);
        [pre, post]
    } else {
        let (pre, mid, post) = chunk_align::<[Chunk; 4]>(haystack);
        count += chunk_num_chars(mid);
        [pre, post]
    };
    for &slice in &unchunked {
        count += naive_num_chars(slice);
    }
    count
}

/// Count the number of UTF-8 encoded unicode codepoints in a slice of bytes, fast
///
/// This function is safe to use on any byte array, valid UTF-8 or not,
/// but the output is only meaningful for well-formed UTF-8.
///
/// # Example
///
/// ```
/// let swordfish = "メカジキ";
/// let char_count = bytecount::naive_num_chars(swordfish.as_bytes());
/// assert_eq!(char_count, 4);
/// ```
#[cfg(not(feature = "simd-accel"))]
pub fn num_chars(haystack: &[u8]) -> usize {
    // Never use [usize; 4]
    num_chars_generic::<usize>(32, usize::MAX, haystack)
}

#[cfg(all(feature = "simd-accel", not(feature = "avx-accel")))]
pub fn num_chars(haystack: &[u8]) -> usize {
    num_chars_generic::<u8x16>(32, 4096, haystack)
}

#[cfg(feature = "avx-accel")]
pub fn num_chars(haystack: &[u8]) -> usize {
    num_chars_generic::<u8x32>(64, 4096, haystack)
}

/// Count the number of UTF-8 encoded unicode codepoints in a slice of bytes, simple
///
/// This function is safe to use on any byte array, valid UTF-8 or not,
/// but the output is only meaningful for well-formed UTF-8.
///
/// # Example
///
/// ```
/// let swordfish = "メカジキ";
/// let char_count = bytecount::naive_num_chars(swordfish.as_bytes());
/// assert_eq!(char_count, 4);
/// ```
pub fn naive_num_chars(haystack: &[u8]) -> usize {
    haystack.iter().filter(|&&byte| (byte >> 6) != 0b10).count()
}
<commit_msg>Docstring called wrong functions<commit_after>//! Counting occurrences of a byte in a slice

#![no_std]

#[cfg(feature = "simd-accel")]
extern crate simd;

use core::{cmp, mem, ops, slice, usize};

#[cfg(feature = "simd-accel")]
use simd::u8x16;
#[cfg(feature = "avx-accel")]
use simd::x86::sse2::Sse2U8x16;
#[cfg(feature = "avx-accel")]
use simd::x86::avx::{LowHigh128, u8x32};

trait ByteChunk: Copy {
    type Splat: Copy;
    fn splat(byte: u8) -> Self::Splat;
    fn from_splat(splat: Self::Splat) -> Self;
    fn bytewise_equal(self, other: Self::Splat) -> Self;
    fn is_leading_utf8_byte(self) -> Self;
    fn increment(self, incr: Self) -> Self;
    fn sum(&self) -> usize;
}

impl ByteChunk for usize {
    type Splat = Self;

    fn splat(byte: u8) -> Self {
        let lo = usize::MAX / 0xFF;
        lo * byte as usize
    }

    fn from_splat(splat: Self) -> Self {
        splat
    }

    fn is_leading_utf8_byte(self) -> Self {
        // a leading UTF-8 byte is one which does not start with the bits 10.
        ((!self >> 7) | (self >> 6)) & Self::splat(1)
    }

    fn bytewise_equal(self, other: Self) -> Self {
        let lo = usize::MAX / 0xFF;
        let hi = lo << 7;

        let x = self ^ other;
        !((((x & !hi) + !hi) | x) >> 7) & lo
    }

    fn increment(self, incr: Self) -> Self {
        self + incr
    }

    fn sum(&self) -> usize {
        let every_other_byte_lo = usize::MAX / 0xFFFF;
        let every_other_byte = every_other_byte_lo * 0xFF;

        // Pairwise reduction to avoid overflow on next step.
        let pair_sum: usize = (self & every_other_byte) + ((self >> 8) & every_other_byte);

        // Multiplication results in top two bytes holding sum.
        pair_sum.wrapping_mul(every_other_byte_lo) >> ((mem::size_of::<usize>() - 2) * 8)
    }
}

#[cfg(feature = "simd-accel")]
impl ByteChunk for u8x16 {
    type Splat = Self;

    fn splat(byte: u8) -> Self {
        Self::splat(byte)
    }

    fn from_splat(splat: Self) -> Self {
        splat
    }

    fn is_leading_utf8_byte(self) -> Self {
        (self & Self::splat(0b1100_0000)).ne(Self::splat(0b1000_0000)).to_repr().to_u8()
    }

    fn bytewise_equal(self, other: Self) -> Self {
        self.eq(other).to_repr().to_u8()
    }

    fn increment(self, incr: Self) -> Self {
        // incr on -1
        self - incr
    }

    fn sum(&self) -> usize {
        let mut count = 0;
        for i in 0..16 {
            count += self.extract(i) as usize;
        }
        count
    }
}

#[cfg(feature = "avx-accel")]
impl ByteChunk for u8x32 {
    type Splat = Self;

    fn splat(byte: u8) -> Self {
        Self::splat(byte)
    }

    fn from_splat(splat: Self) -> Self {
        splat
    }

    fn is_leading_utf8_byte(self) -> Self {
        (self & Self::splat(0b1100_0000)).ne(Self::splat(0b1000_0000)).to_repr().to_u8()
    }

    fn bytewise_equal(self, other: Self) -> Self {
        self.eq(other).to_repr().to_u8()
    }

    fn increment(self, incr: Self) -> Self {
        // incr on -1
        self - incr
    }

    fn sum(&self) -> usize {
        let zero = u8x16::splat(0);
        let sad_lo = self.low().sad(zero);
        let sad_hi = self.high().sad(zero);

        let mut count = 0;
        count += (sad_lo.extract(0) + sad_lo.extract(1)) as usize;
        count += (sad_hi.extract(0) + sad_hi.extract(1)) as usize;
        count
    }
}

impl<T> ByteChunk for [T; 4]
    where T: ByteChunk<Splat = T>
{
    type Splat = T;

    fn splat(byte: u8) -> T {
        T::splat(byte)
    }

    fn from_splat(splat: T) -> Self {
        [splat, splat, splat, splat]
    }

    fn is_leading_utf8_byte(mut self) -> Self {
        for t in self[..].iter_mut() {
            *t = t.is_leading_utf8_byte();
        }
        self
    }

    fn bytewise_equal(mut self, needles: Self::Splat) -> Self {
        for t in self[..].iter_mut() {
            *t = t.bytewise_equal(needles);
        }
        self
    }

    fn increment(self, incr: Self) -> Self {
        [self[0].increment(incr[0]),
         self[1].increment(incr[1]),
         self[2].increment(incr[2]),
         self[3].increment(incr[3])]
    }

    fn sum(&self) -> usize {
        self[..].iter().map(ByteChunk::sum).fold(0, ops::Add::add)
    }
}

fn chunk_align<Chunk: ByteChunk>(x: &[u8]) -> (&[u8], &[Chunk], &[u8]) {
    let align = mem::size_of::<Chunk>();

    let offset_ptr = (x.as_ptr() as usize) % align;
    let offset_end = (x.as_ptr() as usize + x.len()) % align;

    let d2 = x.len().saturating_sub(offset_end);
    let d1 = cmp::min((align - offset_ptr) % align, d2);

    let (init, tail) = x.split_at(d2);
    let (init, mid) = init.split_at(d1);
    assert_eq!(mid.len() % align, 0);
    let mid = unsafe { slice::from_raw_parts(mid.as_ptr() as *const Chunk, mid.len() / align) };

    (init, mid, tail)
}

fn chunk_count<Chunk: ByteChunk>(haystack: &[Chunk], needle: u8) -> usize {
    let zero = Chunk::splat(0);
    let needles = Chunk::splat(needle);
    let mut count = 0;
    let mut i = 0;

    while i < haystack.len() {
        let mut counts = Chunk::from_splat(zero);

        let end = cmp::min(i + 255, haystack.len());
        for &chunk in &haystack[i..end] {
            counts = counts.increment(chunk.bytewise_equal(needles));
        }
        i = end;

        count += counts.sum();
    }

    count
}

fn count_generic<Chunk: ByteChunk<Splat = Chunk>>(naive_below: usize,
                                                  group_above: usize,
                                                  haystack: &[u8],
                                                  needle: u8)
                                                  -> usize {
    let mut count = 0;

    // Extract pre/post so naive_count is only inlined once.
    let len = haystack.len();
    let unchunked = if len < naive_below {
        [haystack, &haystack[0..0]]
    } else if len <= group_above {
        let (pre, mid, post) = chunk_align::<Chunk>(haystack);
        count += chunk_count(mid, needle);
        [pre, post]
    } else {
        let (pre, mid, post) = chunk_align::<[Chunk; 4]>(haystack);
        count += chunk_count(mid, needle);
        [pre, post]
    };

    for &slice in &unchunked {
        count += naive_count(slice, needle);
    }

    count
}

/// Count occurrences of a byte in a slice of bytes, fast
///
/// # Examples
///
/// ```
/// let s = b"This is a Text with spaces";
/// let number_of_spaces = bytecount::count(s, b' ');
/// assert_eq!(number_of_spaces, 5);
/// ```
#[cfg(not(feature = "simd-accel"))]
pub fn count(haystack: &[u8], needle: u8) -> usize {
    // Never use [usize; 4]
    count_generic::<usize>(32, usize::MAX, haystack, needle)
}

#[cfg(all(feature = "simd-accel", not(feature = "avx-accel")))]
pub fn count(haystack: &[u8], needle: u8) -> usize {
    count_generic::<u8x16>(32, 4096, haystack, needle)
}

#[cfg(feature = "avx-accel")]
pub fn count(haystack: &[u8], needle: u8) -> usize {
    count_generic::<u8x32>(64, 4096, haystack, needle)
}

/// Count up to `(2^32)-1` occurrences of a byte in a slice
/// of bytes, simple
///
/// # Example
///
/// ```
/// let s = b"This is yet another Text with spaces";
/// let number_of_spaces = bytecount::naive_count_32(s, b' ');
/// assert_eq!(number_of_spaces, 6);
/// ```
pub fn naive_count_32(haystack: &[u8], needle: u8) -> usize {
    haystack.iter().fold(0, |n, c| n + (*c == needle) as u32) as usize
}

/// Count occurrences of a byte in a slice of bytes, simple
///
/// # Example
///
/// ```
/// let s = b"This is yet another Text with spaces";
/// let number_of_spaces = bytecount::naive_count(s, b' ');
/// assert_eq!(number_of_spaces, 6);
/// ```
pub fn naive_count(haystack: &[u8], needle: u8) -> usize {
    haystack.iter().fold(0, |n, c| n + (*c == needle) as usize)
}

fn chunk_num_chars<Chunk: ByteChunk>(haystack: &[Chunk]) -> usize {
    let zero = Chunk::splat(0);
    let mut count = 0;
    let mut i = 0;
    while i < haystack.len() {
        let mut counts = Chunk::from_splat(zero);
        let end = cmp::min(i + 255, haystack.len());
        for &chunk in &haystack[i..end] {
            counts = counts.increment(chunk.is_leading_utf8_byte());
        }
        i = end;
        count += counts.sum();
    }
    count
}

fn num_chars_generic<Chunk: ByteChunk<Splat = Chunk>>(naive_below: usize,
                                                    group_above: usize,
                                                    haystack: &[u8])
                                                    -> usize {
    // Extract pre/post so naive_count is only inlined once.
    let len = haystack.len();
    let mut count = 0;
    let unchunked = if len < naive_below {
        [haystack, &haystack[0..0]]
    } else if len <= group_above {
        let (pre, mid, post) = chunk_align::<Chunk>(haystack);
        count += chunk_num_chars(mid);
        [pre, post]
    } else {
        let (pre, mid, post) = chunk_align::<[Chunk; 4]>(haystack);
        count += chunk_num_chars(mid);
        [pre, post]
    };
    for &slice in &unchunked {
        count += naive_num_chars(slice);
    }
    count
}

/// Count the number of UTF-8 encoded unicode codepoints in a slice of bytes, fast
///
/// This function is safe to use on any byte array, valid UTF-8 or not,
/// but the output is only meaningful for well-formed UTF-8.
///
/// # Example
///
/// ```
/// let swordfish = "メカジキ";
/// let char_count = bytecount::num_chars(swordfish.as_bytes());
/// assert_eq!(char_count, 4);
/// ```
#[cfg(not(feature = "simd-accel"))]
pub fn num_chars(haystack: &[u8]) -> usize {
    // Never use [usize; 4]
    num_chars_generic::<usize>(32, usize::MAX, haystack)
}

#[cfg(all(feature = "simd-accel", not(feature = "avx-accel")))]
pub fn num_chars(haystack: &[u8]) -> usize {
    num_chars_generic::<u8x16>(32, 4096, haystack)
}

#[cfg(feature = "avx-accel")]
pub fn num_chars(haystack: &[u8]) -> usize {
    num_chars_generic::<u8x32>(64, 4096, haystack)
}

/// Count the number of UTF-8 encoded unicode codepoints in a slice of bytes, simple
///
/// This function is safe to use on any byte array, valid UTF-8 or not,
/// but the output is only meaningful for well-formed UTF-8.
///
/// # Example
///
/// ```
/// let swordfish = "メカジキ";
/// let char_count = bytecount::naive_num_chars(swordfish.as_bytes());
/// assert_eq!(char_count, 4);
/// ```
pub fn naive_num_chars(haystack: &[u8]) -> usize {
    haystack.iter().filter(|&&byte| (byte >> 6) != 0b10).count()
}
<|endoftext|>


<commit_before>extern crate libc;

#[allow(dead_code,
        non_snake_case,
        non_camel_case_types,
        non_upper_case_globals)]
pub mod libpq;
<commit_msg>Add all the necessary callbacks<commit_after>extern crate libc;

#[allow(dead_code,
        non_snake_case,
        non_camel_case_types,
        non_upper_case_globals)]
pub mod libpq;

/// Symbols Postgres needs to find.

#[no_mangle]
pub unsafe extern fn _PG_init() { }

#[no_mangle]
pub unsafe extern fn
    _PG_output_plugin_init(cb: *mut libpq::OutputPluginCallbacks) { init(cb); }

/// Implementation of initialization and callbacks.

pub unsafe extern fn init(cb: *mut libpq::OutputPluginCallbacks) {
    (*cb).startup_cb = Some(startup);
    (*cb).begin_cb = Some(begin);
    (*cb).change_cb = Some(change);
    (*cb).commit_cb = Some(commit);
    (*cb).shutdown_cb = Some(shutdown);
}
/*
pub type LogicalOutputPluginInit =
    ::std::option::Option<extern "C" fn(cb: *mut Struct_OutputPluginCallbacks)
                              -> ()>;
*/

extern fn startup(ctx: *mut libpq::Struct_LogicalDecodingContext,
                  options: *mut libpq::OutputPluginOptions,
                  is_init: libpq::_bool) {

}
/*
pub type LogicalDecodeStartupCB =
    ::std::option::Option<extern "C" fn(ctx:
                                            *mut Struct_LogicalDecodingContext,
                                        options: *mut OutputPluginOptions,
                                        is_init: _bool) -> ()>;
 */

extern fn begin(ctx: *mut libpq::Struct_LogicalDecodingContext,
                txn: *mut libpq::ReorderBufferTXN) {

}
/*
pub type LogicalDecodeBeginCB =
    ::std::option::Option<extern "C" fn(arg1:
                                            *mut Struct_LogicalDecodingContext,
                                        txn: *mut ReorderBufferTXN) -> ()>;
 */

extern fn change(ctx: *mut libpq::Struct_LogicalDecodingContext,
                 txn: *mut libpq::ReorderBufferTXN,
                 relation: libpq::Relation,
                 change: *mut libpq::ReorderBufferChange) {

}
/*
pub type LogicalDecodeChangeCB =
    ::std::option::Option<extern "C" fn(arg1:
                                            *mut Struct_LogicalDecodingContext,
                                        txn: *mut ReorderBufferTXN,
                                        relation: Relation,
                                        change: *mut ReorderBufferChange)
                              -> ()>;
 */

extern fn commit(ctx: *mut libpq::Struct_LogicalDecodingContext,
                 txn: *mut libpq::ReorderBufferTXN,
                 lsn: libpq::XLogRecPtr) {

}
/*
pub type LogicalDecodeCommitCB =
    ::std::option::Option<extern "C" fn(arg1:
                                            *mut Struct_LogicalDecodingContext,
                                        txn: *mut ReorderBufferTXN,
                                        commit_lsn: XLogRecPtr) -> ()>;
 */

extern fn shutdown(ctx: *mut libpq::Struct_LogicalDecodingContext) {

}
/*
pub type LogicalDecodeShutdownCB =
    ::std::option::Option<extern "C" fn(arg1:
                                            *mut Struct_LogicalDecodingContext)
                              -> ()>;

 */
<|endoftext|>


<commit_before>pub extern crate crypto;
extern crate rand;

use rand::OsRng;
use rand::Rng;
use crypto::digest::Digest;

/// A one-time signing public key
pub struct PublicKey<T: Digest + Clone> {
    zero_values: Vec<Vec<u8>>,
    one_values:  Vec<Vec<u8>>,
    digest: T
}

/// A one-time signing private key
pub struct PrivateKey<T: Digest + Clone> {
    zero_values: Vec<Vec<u8>>, // For a n bits hash function: (n * n/8 bytes) for zero_values and one_values
    one_values:  Vec<Vec<u8>>,
    digest: T,
    used: bool
}

impl<T: Digest + Clone> PublicKey<T> {
    /// Verifies that the signature of the data is correctly signed with the given key
    pub fn verify_signature(    &self,
                                signature: &Vec<Vec<u8>>,
                                data:&[u8],
                                ) -> bool
    {
        let mut digest = self.digest.clone();
        digest.input(data);
        let mut data_hash = vec![0 as u8; digest.output_bytes()];
        digest.result(data_hash.as_mut_slice());
        digest.reset();

        for i in 0..data_hash.len() {
            let byte = data_hash[i];
            for j in 0..8 {
                let offset = i*8 + j;
                if (byte & (1<<j)) > 0 {
                    digest.input(signature[offset].as_slice());
                    let mut hashed_value = vec![0 as u8; digest.output_bytes()];
                    digest.result(hashed_value.as_mut_slice());
                    digest.reset();
                    if hashed_value != self.one_values[offset] {
                        return false;
                    }
                } else {
                    digest.input(signature[offset].as_slice());
                    let mut hashed_value = vec![0 as u8; digest.output_bytes()];
                    digest.result(hashed_value.as_mut_slice());
                    digest.reset();
                    if hashed_value != self.zero_values[offset] {
                        return false;
                    }
                }
            }
        }

        return true;
    }
}

impl <T: Digest + Clone> PrivateKey<T> {
    /// Generates a new random one-time signing key. This method can panic if OS RNG fails
    pub fn new(digest: T) -> PrivateKey<T> {
        let generate_bit_hash_values = |hasher: &T| -> Vec<Vec<u8>> {
            let mut rng = match OsRng::new() {
                Ok(g) => g,
                Err(e) => panic!("Failed to obtain OS RNG: {}", e)
            };
            let buffer_byte = vec![0 as u8; hasher.output_bytes()];
            let mut buffer  = vec![buffer_byte; hasher.output_bits()];

            for hash in buffer.iter_mut() {
                rng.fill_bytes(hash)
            }

            return buffer;
        };

        let zero_values = generate_bit_hash_values(&digest);
        let one_values  = generate_bit_hash_values(&digest);

        return PrivateKey { zero_values: zero_values,
                            one_values: one_values,
                            digest: digest,
                            used: false }
    }

    /// Returns the public key associated with this private key
    pub fn public_key(&self) -> PublicKey<T> {
        let mut digest = self.digest.clone();

        let hash_values = |x: &Vec<Vec<u8>>, hash_func: &mut Digest | -> Vec<Vec<u8>> {
            let buffer_byte = vec![0 as u8; hash_func.output_bytes()];
            let mut buffer  = vec![buffer_byte; hash_func.output_bits()];

            for i in 0..hash_func.output_bits(){
                hash_func.input(x[i].as_slice());
                hash_func.result(buffer[i].as_mut_slice());
                hash_func.reset();
            }

            return buffer;
        };

        let hashed_zero_values = hash_values(&self.zero_values, &mut digest);
        let hashed_one_values  = hash_values(&self.one_values, &mut digest);

        return PublicKey { zero_values: hashed_zero_values,
                            one_values: hashed_one_values,
                            digest: digest }
    }

    /// Signs the data with the private key and returns the result if successful.
    /// If unsuccesful, an explanation string is returned
    pub fn sign(&mut self, data: &[u8]) ->  Result<Vec<Vec<u8>>, &'static str> {
        if self.used {
            return Err("Attempting to sign more than once.");
        }
        self.digest.input(data);
        let mut data_hash = vec![0 as u8; self.digest.output_bytes()];
        self.digest.result(data_hash.as_mut_slice());
        self.digest.reset();

        let signature_len = data_hash.len() * 8;
        let mut signature = Vec::with_capacity(signature_len);

        for i in 0..data_hash.len() {
            let byte = data_hash[i];
            for j in 0..8 {
                let offset = i*8 + j;
                if (byte & (1<<j)) > 0 {
                    // Bit is 1
                    signature.push(self.one_values[offset].clone());
                } else {
                    // Bit is 0
                    signature.push(self.zero_values[offset].clone());
                }
            }
        }
        self.used = true;
        return Ok(signature);
    }
}

impl <T: Digest + Clone> Drop for PrivateKey<T> {
    fn drop(&mut self) {
        let zeroize_vector = |vector: &mut Vec<Vec<u8>>| {
            for v2 in vector.iter_mut() {
                for byte in v2.iter_mut() {
                    *byte = 0;
                }
            }
        };

        zeroize_vector(&mut self.zero_values);
        zeroize_vector(&mut self.one_values);
    }
}

impl<T: Digest + Clone> PartialEq for PrivateKey<T> {
    // ⚠ This is not a constant-time implementation
    fn eq(&self, other: &PrivateKey<T>) -> bool {
        if self.one_values.len() != other.one_values.len() {
            return false;
        }
        if self.zero_values.len() != other.zero_values.len() {
            return false;
        }

        for i in 0..self.zero_values.len() {
            if self.zero_values[i] != other.zero_values[i] || self.one_values[i] != other.one_values[i] {
                return false
            }
        }
        return true;
    }
}

#[cfg(test)]
use crypto::sha3::Sha3;
#[test]
fn test_public_key_length_256() {
    let pk = PrivateKey::new(Sha3::sha3_256());
    assert!(    pk.public_key().one_values.len() == 256 &&
                pk.public_key().zero_values.len() == 256);
}
#[test]
fn test_public_key_length_512() {
    let pk = PrivateKey::new(Sha3::sha3_512());
    assert!(    pk.public_key().one_values.len() == 512 &&
                pk.public_key().zero_values.len() == 512);
}

#[cfg(test)]
pub mod test;
<commit_msg>Remove re-export of crypto crate.<commit_after>//! *lamport* implements one-time hash-based signatures using the Lamport signature scheme.

extern crate crypto;
extern crate rand;

use rand::OsRng;
use rand::Rng;
use crypto::digest::Digest;

/// A one-time signing public key
pub struct PublicKey<T: Digest + Clone> {
    zero_values: Vec<Vec<u8>>,
    one_values:  Vec<Vec<u8>>,
    digest: T
}

/// A one-time signing private key
pub struct PrivateKey<T: Digest + Clone> {
    zero_values: Vec<Vec<u8>>, // For a n bits hash function: (n * n/8 bytes) for zero_values and one_values
    one_values:  Vec<Vec<u8>>,
    digest: T,
    used: bool
}

impl<T: Digest + Clone> PublicKey<T> {
    /// Verifies that the signature of the data is correctly signed with the given key
    pub fn verify_signature(    &self,
                                signature: &Vec<Vec<u8>>,
                                data:&[u8],
                                ) -> bool
    {
        let mut digest = self.digest.clone();
        digest.input(data);
        let mut data_hash = vec![0 as u8; digest.output_bytes()];
        digest.result(data_hash.as_mut_slice());
        digest.reset();

        for i in 0..data_hash.len() {
            let byte = data_hash[i];
            for j in 0..8 {
                let offset = i*8 + j;
                if (byte & (1<<j)) > 0 {
                    digest.input(signature[offset].as_slice());
                    let mut hashed_value = vec![0 as u8; digest.output_bytes()];
                    digest.result(hashed_value.as_mut_slice());
                    digest.reset();
                    if hashed_value != self.one_values[offset] {
                        return false;
                    }
                } else {
                    digest.input(signature[offset].as_slice());
                    let mut hashed_value = vec![0 as u8; digest.output_bytes()];
                    digest.result(hashed_value.as_mut_slice());
                    digest.reset();
                    if hashed_value != self.zero_values[offset] {
                        return false;
                    }
                }
            }
        }

        return true;
    }
}

impl <T: Digest + Clone> PrivateKey<T> {
    /// Generates a new random one-time signing key. This method can panic if OS RNG fails
    pub fn new(digest: T) -> PrivateKey<T> {
        let generate_bit_hash_values = |hasher: &T| -> Vec<Vec<u8>> {
            let mut rng = match OsRng::new() {
                Ok(g) => g,
                Err(e) => panic!("Failed to obtain OS RNG: {}", e)
            };
            let buffer_byte = vec![0 as u8; hasher.output_bytes()];
            let mut buffer  = vec![buffer_byte; hasher.output_bits()];

            for hash in buffer.iter_mut() {
                rng.fill_bytes(hash)
            }

            return buffer;
        };

        let zero_values = generate_bit_hash_values(&digest);
        let one_values  = generate_bit_hash_values(&digest);

        return PrivateKey { zero_values: zero_values,
                            one_values: one_values,
                            digest: digest,
                            used: false }
    }

    /// Returns the public key associated with this private key
    pub fn public_key(&self) -> PublicKey<T> {
        let mut digest = self.digest.clone();

        let hash_values = |x: &Vec<Vec<u8>>, hash_func: &mut Digest | -> Vec<Vec<u8>> {
            let buffer_byte = vec![0 as u8; hash_func.output_bytes()];
            let mut buffer  = vec![buffer_byte; hash_func.output_bits()];

            for i in 0..hash_func.output_bits(){
                hash_func.input(x[i].as_slice());
                hash_func.result(buffer[i].as_mut_slice());
                hash_func.reset();
            }

            return buffer;
        };

        let hashed_zero_values = hash_values(&self.zero_values, &mut digest);
        let hashed_one_values  = hash_values(&self.one_values, &mut digest);

        return PublicKey { zero_values: hashed_zero_values,
                            one_values: hashed_one_values,
                            digest: digest }
    }

    /// Signs the data with the private key and returns the result if successful.
    /// If unsuccesful, an explanation string is returned
    pub fn sign(&mut self, data: &[u8]) ->  Result<Vec<Vec<u8>>, &'static str> {
        if self.used {
            return Err("Attempting to sign more than once.");
        }
        self.digest.input(data);
        let mut data_hash = vec![0 as u8; self.digest.output_bytes()];
        self.digest.result(data_hash.as_mut_slice());
        self.digest.reset();

        let signature_len = data_hash.len() * 8;
        let mut signature = Vec::with_capacity(signature_len);

        for i in 0..data_hash.len() {
            let byte = data_hash[i];
            for j in 0..8 {
                let offset = i*8 + j;
                if (byte & (1<<j)) > 0 {
                    // Bit is 1
                    signature.push(self.one_values[offset].clone());
                } else {
                    // Bit is 0
                    signature.push(self.zero_values[offset].clone());
                }
            }
        }
        self.used = true;
        return Ok(signature);
    }
}

impl <T: Digest + Clone> Drop for PrivateKey<T> {
    fn drop(&mut self) {
        let zeroize_vector = |vector: &mut Vec<Vec<u8>>| {
            for v2 in vector.iter_mut() {
                for byte in v2.iter_mut() {
                    *byte = 0;
                }
            }
        };

        zeroize_vector(&mut self.zero_values);
        zeroize_vector(&mut self.one_values);
    }
}

impl<T: Digest + Clone> PartialEq for PrivateKey<T> {
    // ⚠ This is not a constant-time implementation
    fn eq(&self, other: &PrivateKey<T>) -> bool {
        if self.one_values.len() != other.one_values.len() {
            return false;
        }
        if self.zero_values.len() != other.zero_values.len() {
            return false;
        }

        for i in 0..self.zero_values.len() {
            if self.zero_values[i] != other.zero_values[i] || self.one_values[i] != other.one_values[i] {
                return false
            }
        }
        return true;
    }
}

#[cfg(test)]
use crypto::sha3::Sha3;
#[test]
fn test_public_key_length_256() {
    let pk = PrivateKey::new(Sha3::sha3_256());
    assert!(    pk.public_key().one_values.len() == 256 &&
                pk.public_key().zero_values.len() == 256);
}
#[test]
fn test_public_key_length_512() {
    let pk = PrivateKey::new(Sha3::sha3_512());
    assert!(    pk.public_key().one_values.len() == 512 &&
                pk.public_key().zero_values.len() == 512);
}

#[cfg(test)]
pub mod test;
<|endoftext|>


<commit_before>//! # Poisson-disk distribution generation
//!
//! Generates distribution of points where:
//!
//! * For each point there is disk of certain radius which doesn't intersect
//! with any other disk of other points
//! * Samples fill the space uniformly
//!
//! # Examples
//!
//! ```
//! extern crate poisson;
//! use poisson::{Builder, Type, algorithm};
//!
//! extern crate rand;
//!
//! extern crate nalgebra as na;
//! type Vec2 = na::Vec2<f64>;
//!
//! fn main() {
//!     let poisson =
//!         Builder::<_, Vec2>::with_radius(0.1, Type::Normal)
//!             .build(rand::weak_rng(), algorithm::Ebeida);
//!     let samples = poisson.generate();
//!     println!("{:?}", samples);
//! }
//! ```
extern crate modulo;

extern crate sphere;

extern crate rand;
use rand::{Rand, Rng};

extern crate num;
use num::NumCast;

extern crate nalgebra as na;
use na::{FloatVec, BaseFloat, Iterable, IterableMut};

#[macro_use]
extern crate lazy_static;

use std::marker::PhantomData;

use algorithm::{Creator, Algorithm};
use utils::math::calc_radius;

pub mod algorithm;
mod utils;

/// Describes what floats are.
pub trait Float:
    BaseFloat +
    Rand
{
    /// Casts usize to float.
    fn cast(n: usize) -> Self {
        NumCast::from(n).expect("Casting usize to float should always succeed.")
    }
}
impl<T> Float for T where T: BaseFloat + Rand
{}

/// Describes what vectors are.
pub trait Vector<F>:
    FloatVec<F> +
    IterableMut<F> +
    Iterable<F> +
    Rand +
    Clone
    where F: Float
{}
impl<T, F> Vector<F> for T
    where F: Float,
          T: FloatVec<F> + IterableMut<F> + Iterable<F> + Rand + Clone
{}

/// Enum for determining the type of poisson-disk distribution.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum Type {
    Normal,
    Perioditic,
}

impl Default for Type {
    fn default() -> Type {
        Type::Normal
    }
}

/// Builder for the generator.
#[derive(Default, Clone, Debug, PartialEq)]
pub struct Builder<F, V>
    where F: Float,
          V: Vector<F>
{
    radius: F,
    poisson_type: Type,
    _marker: PhantomData<V>,
}

impl<V, F> Builder<F, V>
    where F: Float,
          V: Vector<F>
{
    /// New Builder with type of distribution and radius specified.
    /// The radius should be ]0, √2 / 2]
    pub fn with_radius(radius: F, poisson_type: Type) -> Self {
        assert!(F::cast(0) < radius);
        assert!(radius <=
                NumCast::from(2f64.sqrt() / 2.).expect("Casting constant should always work."));
        Builder {
            radius: radius,
            poisson_type: poisson_type,
            _marker: PhantomData,
        }
    }

    /// New Builder with type of distribution and relative radius specified.
    /// The relative radius should be ]0, 1]
    pub fn with_relative_radius(relative: F, poisson_type: Type) -> Self {
        assert!(relative >= F::cast(0));
        assert!(relative <= F::cast(1));
        Builder {
            radius: relative *
                    NumCast::from(2f64.sqrt() / 2.).expect("Casting constant should always work."),
            poisson_type: poisson_type,
            _marker: PhantomData,
        }
    }

    /// New Builder with type of distribution, approximate amount of samples and relative radius specified.
    /// The amount of samples should be larger than 0.
    /// The relative radius should be [0, 1].
    /// For non-perioditic this is supported only for 2, 3 and 4 dimensional generation.
    pub fn with_samples(samples: usize, relative: F, poisson_type: Type) -> Self {
        Builder {
            radius: calc_radius::<F, V>(samples, relative, poisson_type),
            poisson_type: poisson_type,
            _marker: PhantomData,
        }
    }

    /// Returns the radius of the generator.
    pub fn radius(&self) -> F {
        self.radius
    }

    /// Returns the type of the generator.
    pub fn poisson_type(&self) -> Type {
        self.poisson_type
    }

    /// Builds generator with random number generator and algorithm specified.
    pub fn build<R, A>(self, rng: R, _algo: A) -> Generator<F, V, R, A>
        where R: Rng,
              A: Creator<F, V>
    {
        Generator::new(self, rng)
    }
}

/// Generates poisson-disk distribution for [0, 1]² area.
#[derive(Clone, Debug)]
pub struct Generator<F, V, R, A>
    where F: Float,
          V: Vector<F>,
          R: Rng,
          A: Creator<F, V>
{
    poisson: Builder<F, V>,
    rng: R,
    _algo: PhantomData<A>,
}

impl<F, V, R, A> Generator<F, V, R, A>
    where F: Float,
          V: Vector<F>,
          R: Rng,
          A: Creator<F, V>
{
    fn new(poisson: Builder<F, V>, rng: R) -> Self {
        Generator {
            rng: rng,
            poisson: poisson,
            _algo: PhantomData,
        }
    }

    /// Sets the radius of the generator.
    pub fn set_radius(&mut self, radius: F) {
        assert!(F::cast(0) < radius);
        assert!(radius <=
                NumCast::from(2f64.sqrt() / 2.).expect("Casting constant should always work."));
        self.poisson.radius = radius;
    }

    /// Returns the radius of the generator.
    pub fn radius(&self) -> F {
        self.poisson.radius
    }

    /// Returns the type of the generator.
    pub fn poisson_type(&self) -> Type {
        self.poisson.poisson_type
    }
}

impl<F, V, R, A> Generator<F, V, R, A>
    where F: Float,
          V: Vector<F>,
          R: Rng + Clone,
          A: Creator<F, V>
{
    /// Generates Poisson-disk distribution.
    pub fn generate(&self) -> Vec<V> {
        self.clone().into_iter().collect()
    }
}

impl<F, V, R, A> IntoIterator for Generator<F, V, R, A>
    where F: Float,
          V: Vector<F>,
          R: Rng,
          A: Creator<F, V>
{
    type IntoIter = PoissonIter<F, V, R, A::Algo>;
    type Item = V;

    fn into_iter(self) -> Self::IntoIter {
        PoissonIter {
            rng: self.rng,
            algo: A::create(&self.poisson),
            poisson: self.poisson,
        }
    }
}

/// Iterator for generating poisson-disk distribution.
#[derive(Clone)]
pub struct PoissonIter<F, V, R, A>
    where F: Float,
          V: Vector<F>,
          R: Rng,
          A: Algorithm<F, V>
{
    poisson: Builder<F, V>,
    rng: R,
    algo: A,
}

impl<F, V, R, A> Iterator for PoissonIter<F, V, R, A>
    where F: Float,
          V: Vector<F>,
          R: Rng,
          A: Algorithm<F, V>
{
    type Item = V;

    fn next(&mut self) -> Option<Self::Item> {
        self.algo.next(&mut self.poisson, &mut self.rng)
    }

    fn size_hint(&self) -> (usize, Option<usize>) {
        self.algo.size_hint(&self.poisson)
    }
}

impl<F, V, R, A> PoissonIter<F, V, R, A>
    where F: Float,
          V: Vector<F>,
          R: Rng,
          A: Algorithm<F, V>
{
    /// Returns the radius of the generator.
    pub fn radius(&self) -> F {
        self.poisson.radius
    }

    /// Returns the type of the generator.
    pub fn poisson_type(&self) -> Type {
        self.poisson.poisson_type
    }

    /// Restricts the poisson algorithm with arbitary sample.
    pub fn restrict(&mut self, value: V) {
        self.algo.restrict(value);
    }

    /// Checks legality of sample for currrent distribution.
    pub fn stays_legal(&self, value: V) -> bool {
        self.algo.stays_legal(&self.poisson, value)
    }
}
<commit_msg>Adds documentation on variants on Type enum. Fixes #6.<commit_after>//! # Poisson-disk distribution generation
//!
//! Generates distribution of points where:
//!
//! * For each point there is disk of certain radius which doesn't intersect
//! with any other disk of other points
//! * Samples fill the space uniformly
//!
//! # Examples
//!
//! ```
//! extern crate poisson;
//! use poisson::{Builder, Type, algorithm};
//!
//! extern crate rand;
//!
//! extern crate nalgebra as na;
//! type Vec2 = na::Vec2<f64>;
//!
//! fn main() {
//!     let poisson =
//!         Builder::<_, Vec2>::with_radius(0.1, Type::Normal)
//!             .build(rand::weak_rng(), algorithm::Ebeida);
//!     let samples = poisson.generate();
//!     println!("{:?}", samples);
//! }
//! ```
extern crate modulo;

extern crate sphere;

extern crate rand;
use rand::{Rand, Rng};

extern crate num;
use num::NumCast;

extern crate nalgebra as na;
use na::{FloatVec, BaseFloat, Iterable, IterableMut};

#[macro_use]
extern crate lazy_static;

use std::marker::PhantomData;

use algorithm::{Creator, Algorithm};
use utils::math::calc_radius;

pub mod algorithm;
mod utils;

/// Describes what floats are.
pub trait Float:
    BaseFloat +
    Rand
{
    /// Casts usize to float.
    fn cast(n: usize) -> Self {
        NumCast::from(n).expect("Casting usize to float should always succeed.")
    }
}
impl<T> Float for T where T: BaseFloat + Rand
{}

/// Describes what vectors are.
pub trait Vector<F>:
    FloatVec<F> +
    IterableMut<F> +
    Iterable<F> +
    Rand +
    Clone
    where F: Float
{}
impl<T, F> Vector<F> for T
    where F: Float,
          T: FloatVec<F> + IterableMut<F> + Iterable<F> + Rand + Clone
{}

/// Enum for determining the type of poisson-disk distribution.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum Type {
    /// Acts like there is void all around the space placing no restrictions to sides.
    Normal,
    /// Makes the space to wrap around on edges allowing tiling of poisson-disk distribution.
    /// This makes samples next to a edge restrict samples on opposite one.
    Perioditic,
}

impl Default for Type {
    fn default() -> Type {
        Type::Normal
    }
}

/// Builder for the generator.
#[derive(Default, Clone, Debug, PartialEq)]
pub struct Builder<F, V>
    where F: Float,
          V: Vector<F>
{
    radius: F,
    poisson_type: Type,
    _marker: PhantomData<V>,
}

impl<V, F> Builder<F, V>
    where F: Float,
          V: Vector<F>
{
    /// New Builder with type of distribution and radius specified.
    /// The radius should be ]0, √2 / 2]
    pub fn with_radius(radius: F, poisson_type: Type) -> Self {
        assert!(F::cast(0) < radius);
        assert!(radius <=
                NumCast::from(2f64.sqrt() / 2.).expect("Casting constant should always work."));
        Builder {
            radius: radius,
            poisson_type: poisson_type,
            _marker: PhantomData,
        }
    }

    /// New Builder with type of distribution and relative radius specified.
    /// The relative radius should be ]0, 1]
    pub fn with_relative_radius(relative: F, poisson_type: Type) -> Self {
        assert!(relative >= F::cast(0));
        assert!(relative <= F::cast(1));
        Builder {
            radius: relative *
                    NumCast::from(2f64.sqrt() / 2.).expect("Casting constant should always work."),
            poisson_type: poisson_type,
            _marker: PhantomData,
        }
    }

    /// New Builder with type of distribution, approximate amount of samples and relative radius specified.
    /// The amount of samples should be larger than 0.
    /// The relative radius should be [0, 1].
    /// For non-perioditic this is supported only for 2, 3 and 4 dimensional generation.
    pub fn with_samples(samples: usize, relative: F, poisson_type: Type) -> Self {
        Builder {
            radius: calc_radius::<F, V>(samples, relative, poisson_type),
            poisson_type: poisson_type,
            _marker: PhantomData,
        }
    }

    /// Returns the radius of the generator.
    pub fn radius(&self) -> F {
        self.radius
    }

    /// Returns the type of the generator.
    pub fn poisson_type(&self) -> Type {
        self.poisson_type
    }

    /// Builds generator with random number generator and algorithm specified.
    pub fn build<R, A>(self, rng: R, _algo: A) -> Generator<F, V, R, A>
        where R: Rng,
              A: Creator<F, V>
    {
        Generator::new(self, rng)
    }
}

/// Generates poisson-disk distribution for [0, 1]² area.
#[derive(Clone, Debug)]
pub struct Generator<F, V, R, A>
    where F: Float,
          V: Vector<F>,
          R: Rng,
          A: Creator<F, V>
{
    poisson: Builder<F, V>,
    rng: R,
    _algo: PhantomData<A>,
}

impl<F, V, R, A> Generator<F, V, R, A>
    where F: Float,
          V: Vector<F>,
          R: Rng,
          A: Creator<F, V>
{
    fn new(poisson: Builder<F, V>, rng: R) -> Self {
        Generator {
            rng: rng,
            poisson: poisson,
            _algo: PhantomData,
        }
    }

    /// Sets the radius of the generator.
    pub fn set_radius(&mut self, radius: F) {
        assert!(F::cast(0) < radius);
        assert!(radius <=
                NumCast::from(2f64.sqrt() / 2.).expect("Casting constant should always work."));
        self.poisson.radius = radius;
    }

    /// Returns the radius of the generator.
    pub fn radius(&self) -> F {
        self.poisson.radius
    }

    /// Returns the type of the generator.
    pub fn poisson_type(&self) -> Type {
        self.poisson.poisson_type
    }
}

impl<F, V, R, A> Generator<F, V, R, A>
    where F: Float,
          V: Vector<F>,
          R: Rng + Clone,
          A: Creator<F, V>
{
    /// Generates Poisson-disk distribution.
    pub fn generate(&self) -> Vec<V> {
        self.clone().into_iter().collect()
    }
}

impl<F, V, R, A> IntoIterator for Generator<F, V, R, A>
    where F: Float,
          V: Vector<F>,
          R: Rng,
          A: Creator<F, V>
{
    type IntoIter = PoissonIter<F, V, R, A::Algo>;
    type Item = V;

    fn into_iter(self) -> Self::IntoIter {
        PoissonIter {
            rng: self.rng,
            algo: A::create(&self.poisson),
            poisson: self.poisson,
        }
    }
}

/// Iterator for generating poisson-disk distribution.
#[derive(Clone)]
pub struct PoissonIter<F, V, R, A>
    where F: Float,
          V: Vector<F>,
          R: Rng,
          A: Algorithm<F, V>
{
    poisson: Builder<F, V>,
    rng: R,
    algo: A,
}

impl<F, V, R, A> Iterator for PoissonIter<F, V, R, A>
    where F: Float,
          V: Vector<F>,
          R: Rng,
          A: Algorithm<F, V>
{
    type Item = V;

    fn next(&mut self) -> Option<Self::Item> {
        self.algo.next(&mut self.poisson, &mut self.rng)
    }

    fn size_hint(&self) -> (usize, Option<usize>) {
        self.algo.size_hint(&self.poisson)
    }
}

impl<F, V, R, A> PoissonIter<F, V, R, A>
    where F: Float,
          V: Vector<F>,
          R: Rng,
          A: Algorithm<F, V>
{
    /// Returns the radius of the generator.
    pub fn radius(&self) -> F {
        self.poisson.radius
    }

    /// Returns the type of the generator.
    pub fn poisson_type(&self) -> Type {
        self.poisson.poisson_type
    }

    /// Restricts the poisson algorithm with arbitary sample.
    pub fn restrict(&mut self, value: V) {
        self.algo.restrict(value);
    }

    /// Checks legality of sample for currrent distribution.
    pub fn stays_legal(&self, value: V) -> bool {
        self.algo.stays_legal(&self.poisson, value)
    }
}
<|endoftext|>


<commit_before>use std::io;

pub mod newc;
pub use newc::Builder as NewcBuilder;

pub fn write_cpio<RS, W>(mut inputs: Vec<(NewcBuilder, RS)>, output: W) -> io::Result<W>
    where RS: io::Read + io::Seek,
          W: io::Write
{
    let output = try!(inputs.drain(..)
        .enumerate()
        .fold(Ok(output), |output, (idx, (builder, mut input))| {

            // If the output is valid, try to write the next input file
            output.and_then(move |output| {

                // Grab the length of the input file
                let len = try!(input.seek(io::SeekFrom::End(0)));
                try!(input.seek(io::SeekFrom::Start(0)));

                // Create our writer fp with a unique inode number
                let mut fp = builder.ino(idx as u32)
                    .write(output, len as u32);

                // Write out the file
                try!(io::copy(&mut input, &mut fp));

                // And finish off the input file
                fp.finish()
            })
        }));

    newc::trailer(output)
}

#[cfg(test)]
mod tests {
    use super::*;
    use std::io::Cursor;

    #[test]
    fn test_multi_file() {
        // Set up our input files
        let input = vec![
            (NewcBuilder::new("./hello_world")
                .uid(1000)
                .gid(1000)
                .mode(0o100644), Cursor::new("Hello, World".to_string())),
            (NewcBuilder::new("./hello_world2")
                .uid(1000)
                .gid(1000)
                .mode(0o100644), Cursor::new("Hello, World 2".to_string())),
        ];

        // Set up our output file
        let output = Cursor::new(vec![]);

        // Write out the CPIO archive
        let _ = write_cpio(input, output).unwrap();
    }
}
<commit_msg>Switch to using an iterator<commit_after>use std::io;
use std::iter::Iterator;

pub mod newc;
pub use newc::Builder as NewcBuilder;

pub fn write_cpio<I, RS, W>(inputs: I, output: W) -> io::Result<W>
    where I: Iterator<Item = (NewcBuilder, RS)> + Sized,
          RS: io::Read + io::Seek,
          W: io::Write
{
    let output = try!(inputs.enumerate()
        .fold(Ok(output), |output, (idx, (builder, mut input))| {

            // If the output is valid, try to write the next input file
            output.and_then(move |output| {

                // Grab the length of the input file
                let len = try!(input.seek(io::SeekFrom::End(0)));
                try!(input.seek(io::SeekFrom::Start(0)));

                // Create our writer fp with a unique inode number
                let mut fp = builder.ino(idx as u32)
                    .write(output, len as u32);

                // Write out the file
                try!(io::copy(&mut input, &mut fp));

                // And finish off the input file
                fp.finish()
            })
        }));

    newc::trailer(output)
}

#[cfg(test)]
mod tests {
    use super::*;
    use std::io::Cursor;

    #[test]
    fn test_multi_file() {
        // Set up our input files
        let mut input = vec![
            (NewcBuilder::new("./hello_world")
                .uid(1000)
                .gid(1000)
                .mode(0o100644), Cursor::new("Hello, World".to_string())),
            (NewcBuilder::new("./hello_world2")
                .uid(1000)
                .gid(1000)
                .mode(0o100644), Cursor::new("Hello, World 2".to_string())),
        ];

        // Set up our output file
        let output = Cursor::new(vec![]);

        // Write out the CPIO archive
        let _ = write_cpio(input.drain(..), output).unwrap();
    }
}
<|endoftext|>


<commit_before>// Copyright 2015 Cameron Hart
//
// Licensed under the Apache License, Version 2.0 <LICENSE-APACHE or
// http://www.apache.org/licenses/LICENSE-2.0> or the MIT license
// <LICENSE-MIT or http://opensource.org/licenses/MIT>, at your
// option. This file may not be copied, modified, or distributed
// except according to those terms.

use std::mem;
use std::ptr;

////////////////////////////////////////////////////////////////////////////////
// Insertion sort (based off libstd collections slice version)
////////////////////////////////////////////////////////////////////////////////

fn insertsort_impl<T, F>(ptr: *mut T, len: isize, lt: &F) where F: Fn(&T, &T) -> bool {
    // 1 <= i < len;
    for i in 1..len {
        // j satisfies: 0 <= j <= i;
        let mut j = i;
        unsafe {
            // `i` is in bounds.
            let read_ptr = ptr.offset(i) as *const T;

            // find where to insert, we need to do strict <,
            // rather than <=, to maintain stability.

            // 0 <= j - 1 < len, so .offset(j - 1) is in bounds.
            while j > 0 && lt(&*read_ptr, &*ptr.offset(j - 1)) {
                j -= 1;
            }

            // shift everything to the right, to make space to
            // insert this value.

            // j + 1 could be `len` (for the last `i`), but in
            // that case, `i == j` so we don't copy. The
            // `.offset(j)` is always in bounds.

            if i != j {
                let tmp = ptr::read(read_ptr);
                ptr::copy(ptr.offset(j), ptr.offset(j + 1), (i - j) as usize);
                ptr::copy_nonoverlapping(&tmp, ptr.offset(j), 1);
                mem::forget(tmp);
            }
        }
    }
}

pub fn insertsort_by<T: PartialOrd, F>(v: &mut[T], lt: F) where F: Fn(&T, &T) -> bool {
    insertsort_impl(v.as_mut_ptr(), v.len() as isize, &lt);
}

pub fn insertsort<T: PartialOrd>(v: &mut[T]) {
    insertsort_by(v, |a, b| a.lt(b));
}

////////////////////////////////////////////////////////////////////////////////
// Heap sort
////////////////////////////////////////////////////////////////////////////////

/// Builds a heap in the array so that the largest element is at the root.
/// Operates on data in-place.
fn heapify<T, F>(ptr: *mut T, len: isize, lt: &F) where F: Fn(&T, &T) -> bool {
    // start is assigned to the index of the last parent node
    let mut start = (len - 2) / 2;
    let end = len - 1;
    while start >= 0 {
        // shift down the node at index start such that all nodes below start
        // are in heap order
        shift_down(ptr, start, end, lt);
        // go up the next parent node
        start = start - 1;
    }
    // after shifting down the root all nodes are in heap order
}

/// Repair the heap whose root element is at index start.
/// Assumes a valid heap struture.
fn shift_down<T, F>(ptr: *mut T, start: isize, end: isize, lt: &F) where F: Fn(&T, &T) -> bool {
    let mut root = start;
    let mut next_root = root * 2;
    // while the root has at least one child
    while next_root < end {
        // left child
        let left_child = next_root + 1;
        // keep track of child to swap with
        let mut swap = root;
        unsafe {
            if lt(&*ptr.offset(swap), &*ptr.offset(left_child)) {
                swap = left_child;
            }
            // if there is a right child and it is greater
            let right_child = left_child + 1;
            if right_child <= end && lt(&*ptr.offset(swap), &*ptr.offset(right_child)) {
                swap = right_child;
            }
            if swap == root {
                // the root holds the largest element
                return;
            }
            ptr::swap(ptr.offset(root), ptr.offset(swap));
        }
        // repeat to continue shifting down the child
        root = swap;
        next_root = root * 2;
    }
}

/// Internal heapsort implementation
fn heapsort_impl<T, F>(ptr: *mut T, len: isize, lt: &F) where F: Fn(&T, &T) -> bool {
    // build the heap in-place so the largest value is at the root
    heapify(ptr, len, lt);
    let mut end = len - 1;
    while end > 0 {
        // ptr is the root and largest value, swap it to the end of the sorted elements
        unsafe { ptr::swap(ptr.offset(end), ptr); }
        // the heap size is reduced by one
        end = end - 1;
        // the swap invalidated the heap, so restore it
        shift_down(ptr, 0, end, lt);
    }
}

pub fn heapsort_by<T: PartialOrd, F>(v: &mut[T], lt: F) where F: Fn(&T, &T) -> bool {
    let len = v.len() as isize;
    if len > 0 {
        let ptr = v.as_mut_ptr();
        heapsort_impl(ptr, len, &lt);
    }
}

pub fn heapsort<T: PartialOrd>(v: &mut[T]) {
    heapsort_by(v, |a, b| a.lt(b));
}

////////////////////////////////////////////////////////////////////////////////
// Introspection sort
////////////////////////////////////////////////////////////////////////////////

#[inline]
fn lg(n: usize) -> usize {
    mem::size_of::<usize>() * 8 - 1 - n.leading_zeros() as usize
}

#[inline]
/// Calculates the number of elements between the first and last pointers
fn ptr_distance<T>(last: *const T, first: *const T) -> isize {
    ((last as usize - first as usize) / mem::size_of::<T>()) as isize
}

#[inline]
fn median_3<T, F>(a: *mut T, b: *mut T, c: *mut T, lt: &F) -> *mut T where F: Fn(&T, &T) -> bool {
    unsafe {
        if lt(&*a, &*b) {
            if lt(&*b, &*c) {
                b
            }
            else if lt(&*a, &*c) {
                c
            }
            else {
                a
            }
        }
        else if lt(&*a, &*c) {
            a
        }
        else if lt(&*b, &*c) {
            c
        }
        else {
            b
        }
    }
}

#[inline]
fn partition<T, F>(mut first: *mut T, mut last: *mut T, pivot: *mut T, lt: &F) -> *mut T
        where F: Fn(&T, &T) -> bool {
    unsafe {
        loop {
            // find first element greater than the pivot
            while lt(&*first, &*pivot) {
                first = first.offset(1);
            }
            // find last element smaller than the pivot
            last = last.offset(-1);
            while lt(&*pivot, &*last) {
                last = last.offset(-1);
            }
            // if first and last have met then partitioning is complete
            if !((first as usize) < (last as usize)) {
                return first;
            }
            // swap the first and last elements to be on the right side of the pivot
            ptr::swap(first, last);
            // move to the next element
            first = first.offset(1);
        }
    }
}

#[inline]
fn partition_pivot<T, F>(ptr: *mut T, len: isize, lt: &F) -> *mut T where F: Fn(&T, &T) -> bool {
    unsafe {
        // choose a pivot based on media of 3 elements
        let pivot = median_3(ptr.offset(1), ptr.offset(len / 2), ptr.offset(len - 1), lt);
        // swap the pivot with the first element so it's already partitioned
        ptr::swap(ptr, pivot);
        // partition elements on either side of the pivot
        return partition(ptr.offset(1), ptr.offset(len), ptr, lt);
    }
}

fn introsort_loop<T, F>(ptr: *mut T, mut last: *mut T, mut depth_limit: usize, lt: &F) where F: Fn(&T, &T) -> bool {
    // Threshold at which we stop and let the insertsort finish off
    const THRESHOLD: isize = 32;

    let mut len = ptr_distance(last, ptr);
    while len > THRESHOLD {
        // if the depth limit has been reached switch to heapsort
        if depth_limit == 0 {
            heapsort_impl(ptr, len, lt);
            return;
        }
        depth_limit -= 1;
        // choose partition and pivot
        let pivot = partition_pivot(ptr, len, lt);
        // introsort the elements after the pivot
        introsort_loop(pivot, last, depth_limit, lt);
        len = ptr_distance(pivot, ptr);
        last = pivot;
    }
}

#[inline]
fn introsort_impl<T: PartialOrd, F>(v: &mut[T], lt: F) where F: Fn(&T, &T) -> bool {
    let len = v.len() as isize;
    if len > 0 {
        let ptr = v.as_mut_ptr();
        unsafe {
            introsort_loop(ptr, ptr.offset(len), 2 * lg(len as usize), &lt);
        }
        // insertsort mostly sorted data
        insertsort_impl(ptr, len, &lt);
    }
}

///
/// Sorts the slice, in place, using `lt` to compare elements.
///
/// The order of equal elements is not guaranteed to be preserved.
///
/// This sort is `O(n log n)` worst-case and stable.
///
/// The sort is implemented using the Introsort algorithm. Introsort or
/// introspective sort is a hybrid sorting algorithm that provides both fast
/// average performance and (asymptotically) optimal worst-case performance.
/// It begins with quicksort and switches to heapsort when the recursion depth
/// exceeds a level based on (the logarithm of) the number of elements being
/// sorted. This combines the good parts of both algorithms, with practical
/// performance comparable to quicksort on typical data sets and worst-case
/// O(n log n) runtime due to the heap sort.
///
/// # Examples
///
/// ```rust
/// let mut v = [5, 4, 1, 3, 2];
/// sortrs::introsort_by(&mut v, |a, b| a.lt(b));
/// assert!(v == [1, 2, 3, 4, 5]);
///
/// // reverse sorting
/// sortrs::introsort_by(&mut v, |a, b| b.lt(a));
/// assert!(v == [5, 4, 3, 2, 1]);
/// ```
pub fn introsort_by<T: PartialOrd, F>(v: &mut[T], lt: F) where F: Fn(&T, &T) -> bool {
    introsort_impl(v, lt);
}

/// Sorts the slice, in place.
///
/// This is equivalent to `self.sort_by(|a, b| a.cmp(b))`.
///
/// # Examples
///
/// ```rust
/// let mut v = [-5, 4, 1, -3, 2];
///
/// sortrs::introsort(&mut v);
/// assert!(v == [-5, -3, 1, 2, 4]);
/// ```
pub fn introsort<T: PartialOrd>(v: &mut[T]) {
    introsort_impl(v, |a, b| a.lt(b))
}

<commit_msg>Ran rustfmt over lib.rs<commit_after>// Copyright 2015 Cameron Hart
//
// Licensed under the Apache License, Version 2.0 <LICENSE-APACHE or
// http://www.apache.org/licenses/LICENSE-2.0> or the MIT license
// <LICENSE-MIT or http://opensource.org/licenses/MIT>, at your
// option. This file may not be copied, modified, or distributed
// except according to those terms.

use std::mem;
use std::ptr;

///
/// Insertion sort (based off libstd collections slice version)
///

fn insertsort_impl<T, F>(ptr: *mut T, len: isize, lt: &F)
    where F: Fn(&T, &T) -> bool
{
    // 1 <= i < len;
    for i in 1..len {
        // j satisfies: 0 <= j <= i;
        let mut j = i;
        unsafe {
            // `i` is in bounds.
            let read_ptr = ptr.offset(i) as *const T;

            // find where to insert, we need to do strict <,
            // rather than <=, to maintain stability.

            // 0 <= j - 1 < len, so .offset(j - 1) is in bounds.
            while j > 0 && lt(&*read_ptr, &*ptr.offset(j - 1)) {
                j -= 1;
            }

            // shift everything to the right, to make space to
            // insert this value.

            // j + 1 could be `len` (for the last `i`), but in
            // that case, `i == j` so we don't copy. The
            // `.offset(j)` is always in bounds.

            if i != j {
                let tmp = ptr::read(read_ptr);
                ptr::copy(ptr.offset(j), ptr.offset(j + 1), (i - j) as usize);
                ptr::copy_nonoverlapping(&tmp, ptr.offset(j), 1);
                mem::forget(tmp);
            }
        }
    }
}

pub fn insertsort_by<T: PartialOrd, F>(v: &mut [T], lt: F)
    where F: Fn(&T, &T) -> bool
{
    insertsort_impl(v.as_mut_ptr(), v.len() as isize, &lt);
}

pub fn insertsort<T: PartialOrd>(v: &mut [T]) {
    insertsort_by(v, |a, b| a.lt(b));
}

///
/// Heap sort
///

/// Builds a heap in the array so that the largest element is at the root.
/// Operates on data in-place.
fn heapify<T, F>(ptr: *mut T, len: isize, lt: &F)
    where F: Fn(&T, &T) -> bool
{
    // start is assigned to the index of the last parent node
    let mut start = (len - 2) / 2;
    let end = len - 1;
    while start >= 0 {
        // shift down the node at index start such that all nodes below start
        // are in heap order
        shift_down(ptr, start, end, lt);
        // go up the next parent node
        start = start - 1;
    }
    // after shifting down the root all nodes are in heap order
}

/// Repair the heap whose root element is at index start.
/// Assumes a valid heap struture.
fn shift_down<T, F>(ptr: *mut T, start: isize, end: isize, lt: &F)
    where F: Fn(&T, &T) -> bool
{
    let mut root = start;
    let mut next_root = root * 2;
    // while the root has at least one child
    while next_root < end {
        // left child
        let left_child = next_root + 1;
        // keep track of child to swap with
        let mut swap = root;
        unsafe {
            if lt(&*ptr.offset(swap), &*ptr.offset(left_child)) {
                swap = left_child;
            }
            // if there is a right child and it is greater
            let right_child = left_child + 1;
            if right_child <= end && lt(&*ptr.offset(swap), &*ptr.offset(right_child)) {
                swap = right_child;
            }
            if swap == root {
                // the root holds the largest element
                return;
            }
            ptr::swap(ptr.offset(root), ptr.offset(swap));
        }
        // repeat to continue shifting down the child
        root = swap;
        next_root = root * 2;
    }
}

/// Internal heapsort implementation
fn heapsort_impl<T, F>(ptr: *mut T, len: isize, lt: &F)
    where F: Fn(&T, &T) -> bool
{
    // build the heap in-place so the largest value is at the root
    heapify(ptr, len, lt);
    let mut end = len - 1;
    while end > 0 {
        // ptr is the root and largest value, swap it to the end of the sorted elements
        unsafe {
            ptr::swap(ptr.offset(end), ptr);
        }
        // the heap size is reduced by one
        end = end - 1;
        // the swap invalidated the heap, so restore it
        shift_down(ptr, 0, end, lt);
    }
}

pub fn heapsort_by<T: PartialOrd, F>(v: &mut [T], lt: F)
    where F: Fn(&T, &T) -> bool
{
    let len = v.len() as isize;
    if len > 0 {
        let ptr = v.as_mut_ptr();
        heapsort_impl(ptr, len, &lt);
    }
}

pub fn heapsort<T: PartialOrd>(v: &mut [T]) {
    heapsort_by(v, |a, b| a.lt(b));
}

///
/// Introspection sort
///

#[inline]
fn lg(n: usize) -> usize {
    mem::size_of::<usize>() * 8 - 1 - n.leading_zeros() as usize
}

#[inline]
/// Calculates the number of elements between the first and last pointers
fn ptr_distance<T>(last: *const T, first: *const T) -> isize {
    ((last as usize - first as usize) / mem::size_of::<T>()) as isize
}

#[inline]
fn median_3<T, F>(a: *mut T, b: *mut T, c: *mut T, lt: &F) -> *mut T
    where F: Fn(&T, &T) -> bool
{
    unsafe {
        if lt(&*a, &*b) {
            if lt(&*b, &*c) {
                b
            } else if lt(&*a, &*c) {
                c
            } else {
                a
            }
        } else if lt(&*a, &*c) {
            a
        } else if lt(&*b, &*c) {
            c
        } else {
            b
        }
    }
}

#[inline]
fn partition<T, F>(mut first: *mut T, mut last: *mut T, pivot: *mut T, lt: &F) -> *mut T
    where F: Fn(&T, &T) -> bool
{
    unsafe {
        loop {
            // find first element greater than the pivot
            while lt(&*first, &*pivot) {
                first = first.offset(1);
            }
            // find last element smaller than the pivot
            last = last.offset(-1);
            while lt(&*pivot, &*last) {
                last = last.offset(-1);
            }
            // if first and last have met then partitioning is complete
            if !((first as usize) < (last as usize)) {
                return first;
            }
            // swap the first and last elements to be on the right side of the pivot
            ptr::swap(first, last);
            // move to the next element
            first = first.offset(1);
        }
    }
}

#[inline]
fn partition_pivot<T, F>(ptr: *mut T, len: isize, lt: &F) -> *mut T
    where F: Fn(&T, &T) -> bool
{
    unsafe {
        // choose a pivot based on media of 3 elements
        let pivot = median_3(ptr.offset(1), ptr.offset(len / 2), ptr.offset(len - 1), lt);
        // swap the pivot with the first element so it's already partitioned
        ptr::swap(ptr, pivot);
        // partition elements on either side of the pivot
        return partition(ptr.offset(1), ptr.offset(len), ptr, lt);
    }
}

fn introsort_loop<T, F>(ptr: *mut T, mut last: *mut T, mut depth_limit: usize, lt: &F)
    where F: Fn(&T, &T) -> bool
{
    // Threshold at which we stop and let the insertsort finish off
    const THRESHOLD: isize = 32;

    let mut len = ptr_distance(last, ptr);
    while len > THRESHOLD {
        // if the depth limit has been reached switch to heapsort
        if depth_limit == 0 {
            heapsort_impl(ptr, len, lt);
            return;
        }
        depth_limit -= 1;
        // choose partition and pivot
        let pivot = partition_pivot(ptr, len, lt);
        // introsort the elements after the pivot
        introsort_loop(pivot, last, depth_limit, lt);
        len = ptr_distance(pivot, ptr);
        last = pivot;
    }
}

#[inline]
fn introsort_impl<T: PartialOrd, F>(v: &mut [T], lt: F)
    where F: Fn(&T, &T) -> bool
{
    let len = v.len() as isize;
    if len > 0 {
        let ptr = v.as_mut_ptr();
        unsafe {
            introsort_loop(ptr, ptr.offset(len), 2 * lg(len as usize), &lt);
        }
        // insertsort mostly sorted data
        insertsort_impl(ptr, len, &lt);
    }
}

///
/// Sorts the slice, in place, using `lt` to compare elements.
///
/// The order of equal elements is not guaranteed to be preserved.
///
/// This sort is `O(n log n)` worst-case and stable.
///
/// The sort is implemented using the Introsort algorithm. Introsort or
/// introspective sort is a hybrid sorting algorithm that provides both fast
/// average performance and (asymptotically) optimal worst-case performance.
/// It begins with quicksort and switches to heapsort when the recursion depth
/// exceeds a level based on (the logarithm of) the number of elements being
/// sorted. This combines the good parts of both algorithms, with practical
/// performance comparable to quicksort on typical data sets and worst-case
/// O(n log n) runtime due to the heap sort.
///
/// # Examples
///
/// ```rust
/// let mut v = [5, 4, 1, 3, 2];
/// sortrs::introsort_by(&mut v, |a, b| a.lt(b));
/// assert!(v == [1, 2, 3, 4, 5]);
///
/// // reverse sorting
/// sortrs::introsort_by(&mut v, |a, b| b.lt(a));
/// assert!(v == [5, 4, 3, 2, 1]);
/// ```
pub fn introsort_by<T: PartialOrd, F>(v: &mut [T], lt: F)
    where F: Fn(&T, &T) -> bool
{
    introsort_impl(v, lt);
}

/// Sorts the slice, in place.
///
/// This is equivalent to `self.sort_by(|a, b| a.cmp(b))`.
///
/// # Examples
///
/// ```rust
/// let mut v = [-5, 4, 1, -3, 2];
///
/// sortrs::introsort(&mut v);
/// assert!(v == [-5, -3, 1, 2, 4]);
/// ```
pub fn introsort<T: PartialOrd>(v: &mut [T]) {
    introsort_impl(v, |a, b| a.lt(b))
}
<|endoftext|>