path-planning 0.1.0

/* Copyright (C) 2020 Dylan Staatz - All Rights Reserved. */

pub use parry3d_f64 as parry;

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

use std::collections::hash_map::IntoIter;
use std::collections::HashMap;
use std::iter::FromIterator;

use nalgebra::storage::Storage;
use nalgebra::{Const, Vector};
use parry::bounding_volume::{BoundingVolume, AABB};
use parry::math::{Isometry, Point, Real, DIM};
use parry::na::Unit;
use parry::query::intersection_test;
use parry::shape::{
  Ball, Capsule, Compound, Cone, ConvexPolyhedron, Cuboid, Cylinder, HalfSpace,
  RoundCone, RoundConvexPolyhedron, RoundCuboid, RoundCylinder, RoundTriangle,
  Segment, Shape, SharedShape, Triangle,
};
use serde::{Deserialize, Serialize};

use crate::error::{Error, Result};
use crate::params::FromParams;

use super::params::{Obstacle3dParams, Offset3dParams, Shape3dParams};
use super::{AppearingObstacle, Behavior, Obstacle, ObstacleId, ObstacleSpace};

pub type X = Real;
pub const D: usize = DIM;
pub type O = Obstacle3df64;
pub type OS = ObstacleSpace3df64;

/// A set of a bunch of obstacles
#[derive(Clone, Serialize, Deserialize)]
#[serde(
  from = "Vec<(ObstacleId, Obstacle3df64)>",
  into = "Vec<(ObstacleId, Obstacle3df64)>"
)]
pub struct ObstacleSpace3df64 {
  obstacles: HashMap<ObstacleId, Obstacle3df64>,
}

impl ObstacleSpace3df64 {
  fn internal_empty() -> Self {
    Self {
      obstacles: HashMap::new(),
    }
  }

  fn internal_add_obstacles<I>(&mut self, obstacles: I)
  where
    I: IntoIterator<Item = (ObstacleId, Obstacle3df64)>,
  {
    obstacles.into_iter().for_each(|(obs_id, obs)| {
      self.obstacles.insert(obs_id, obs);
    })
  }

  fn internal_get_obstacles(
    &self,
    obstacles: &[ObstacleId],
  ) -> Vec<&Obstacle3df64> {
    obstacles
      .iter()
      .filter_map(|obs_id| self.obstacles.get(obs_id))
      .collect()
  }

  fn internal_get_sensed_obstacles(&self) -> Vec<(ObstacleId, &Obstacle3df64)> {
    self
      .obstacles
      .iter()
      .filter(|(_id, obs)| obs.sensed)
      .map(|(id, obs)| (*id, obs))
      .collect()
  }

  fn internal_get_not_sensed_obstacles(
    &self,
  ) -> Vec<(ObstacleId, &Obstacle3df64)> {
    self
      .obstacles
      .iter()
      .filter(|(_id, obs)| !obs.sensed)
      .map(|(id, obs)| (*id, obs))
      .collect()
  }

  fn internal_get_all_obstacle_ids(&self) -> Vec<ObstacleId> {
    self.obstacles.keys().cloned().collect()
  }

  fn internal_remove_obstacles(&mut self, obstacles: &[ObstacleId]) {
    obstacles.iter().for_each(|obs_id| {
      self.obstacles.remove(obs_id);
    })
  }

  fn internal_count(&self) -> usize {
    self.obstacles.len()
  }

  fn internal_check_sensors<S>(
    &mut self,
    pose: &Vector<X, Const<D>, S>,
    radius: X,
  ) -> Vec<ObstacleId>
  where
    S: Storage<X, Const<D>>,
  {
    // TODO: Implement some kind of line of sight based sensing
    let ball = Ball::new(radius);
    let offset = Point::from(pose.clone_owned()).into();

    let mut added = Vec::new();
    for obs in self.obstacles.iter_mut() {
      if !obs.1.sensed {
        if obs.1.intersects_shape(&offset, &ball) {
          obs.1.sensed = true;
          added.push(*obs.0);
        }
      }
    }
    added
  }
}

impl FromParams for ObstacleSpace3df64 {
  type Params = Vec<<Obstacle3df64 as FromParams>::Params>;

  fn from_params(params: Self::Params) -> Result<Self> {
    let mut obs = Vec::new();
    for obs_params in params {
      obs.push(Obstacle3df64::from_params(obs_params)?);
    }

    Ok(obs.into_iter().collect())
  }
}

impl FromIterator<(ObstacleId, Obstacle3df64)> for ObstacleSpace3df64 {
  fn from_iter<I>(iter: I) -> Self
  where
    I: IntoIterator<Item = (ObstacleId, Obstacle3df64)>,
  {
    Self {
      obstacles: HashMap::from_iter(iter),
    }
  }
}

impl FromIterator<Obstacle3df64> for ObstacleSpace3df64 {
  fn from_iter<I>(iter: I) -> Self
  where
    I: IntoIterator<Item = Obstacle3df64>,
  {
    Self {
      obstacles: HashMap::from_iter(iter.into_iter().enumerate()),
    }
  }
}

impl IntoIterator for ObstacleSpace3df64 {
  type Item = (ObstacleId, Obstacle3df64);
  type IntoIter = IntoIter<ObstacleId, Obstacle3df64>;
  fn into_iter(self) -> Self::IntoIter {
    self.obstacles.into_iter()
  }
}

impl FromIterator<(Isometry<Real>, SharedShape)> for ObstacleSpace3df64 {
  fn from_iter<I>(iter: I) -> Self
  where
    I: IntoIterator<Item = (Isometry<Real>, SharedShape)>,
  {
    Self {
      obstacles: HashMap::from_iter(
        iter.into_iter().map(|obs| obs.into()).enumerate(),
      ),
    }
  }
}

impl From<Vec<(Isometry<Real>, SharedShape)>> for ObstacleSpace3df64 {
  fn from(static_obstacles: Vec<(Isometry<Real>, SharedShape)>) -> Self {
    static_obstacles.into_iter().collect()
  }
}

impl From<Vec<Obstacle3df64>> for ObstacleSpace3df64 {
  fn from(static_obstacles: Vec<Obstacle3df64>) -> Self {
    static_obstacles.into_iter().collect()
  }
}

impl From<Vec<(ObstacleId, Obstacle3df64)>> for ObstacleSpace3df64 {
  fn from(static_obstacles: Vec<(ObstacleId, Obstacle3df64)>) -> Self {
    static_obstacles.into_iter().collect()
  }
}

impl Into<Vec<(ObstacleId, Obstacle3df64)>> for ObstacleSpace3df64 {
  fn into(self) -> Vec<(ObstacleId, Obstacle3df64)> {
    self.into_iter().collect()
  }
}

mod euclidean {
  use crate::cspace::{CSpace, EuclideanSpace};
  use crate::trajectories::FullTrajectory;

  use super::*;

  const N: usize = D;
  type CS = EuclideanSpace<X, N>;

  impl Obstacle<X, CS, N> for ObstacleSpace3df64 {
    fn trajectory_free<TF, S1, S2>(&self, t: &TF) -> bool
    where
      TF: FullTrajectory<X, <CS as CSpace<X, N>>::Traj, S1, S2, N>,
      S1: Storage<X, Const<N>>,
      S2: Storage<X, Const<N>>,
    {
      let start = Point::from(t.start().clone_owned());
      let end = Point::from(t.end().clone_owned());
      let segment = Segment::new(start, end);
      let identity = Isometry::identity();

      self
        .obstacles
        .values()
        .filter(|obs| obs.sensed)
        .all(|obs| !obs.intersects_shape(&identity, &segment))
    }

    fn is_free<S>(&self, x: &Vector<X, Const<N>, S>) -> bool
    where
      S: Storage<X, Const<N>>,
    {
      let x = Point::from(x.clone_owned());
      self
        .obstacles
        .values()
        .filter(|obs| obs.sensed)
        .all(|obs| !obs.contains_point(&x))
    }
  }

  impl ObstacleSpace<X, CS, N> for ObstacleSpace3df64 {
    type Obs = Obstacle3df64;

    fn empty() -> Self {
      Self::internal_empty()
    }

    fn add_obstacles<I>(&mut self, obstacles: I)
    where
      I: IntoIterator<Item = (ObstacleId, Self::Obs)>,
    {
      self.internal_add_obstacles(obstacles)
    }

    fn get_obstacles(&self, obstacles: &[ObstacleId]) -> Vec<&Self::Obs> {
      self.internal_get_obstacles(obstacles)
    }

    fn get_sensed_obstacles(&self) -> Vec<(ObstacleId, &Self::Obs)> {
      self.internal_get_sensed_obstacles()
    }

    fn get_not_sensed_obstacles(&self) -> Vec<(ObstacleId, &Self::Obs)> {
      self.internal_get_not_sensed_obstacles()
    }

    fn get_all_obstacle_ids(&self) -> Vec<ObstacleId> {
      self.internal_get_all_obstacle_ids()
    }

    fn remove_obstacles(&mut self, obstacles: &[ObstacleId]) {
      self.internal_remove_obstacles(obstacles)
    }

    fn count(&self) -> usize {
      self.internal_count()
    }

    fn check_sensors<S>(
      &mut self,
      pose: &Vector<X, Const<N>, S>,
      radius: X,
    ) -> Vec<ObstacleId>
    where
      S: Storage<X, Const<N>>,
    {
      self.internal_check_sensors(pose, radius)
    }
  }

  impl Obstacle<X, CS, N> for Obstacle3df64 {
    fn trajectory_free<TF, S1, S2>(&self, t: &TF) -> bool
    where
      TF: FullTrajectory<X, <CS as CSpace<X, N>>::Traj, S1, S2, N>,
      S1: Storage<X, Const<N>>,
      S2: Storage<X, Const<N>>,
    {
      let start = Point::from(t.start().clone_owned());
      let end = Point::from(t.end().clone_owned());
      let segment = Segment::new(start, end);
      let identity = Isometry::identity();

      !self.intersects_shape(&identity, &segment)
    }

    fn is_free<S>(&self, x: &Vector<X, Const<N>, S>) -> bool
    where
      S: Storage<X, Const<N>>,
    {
      let x = Point::from(x.clone_owned());
      !self.contains_point(&x)
    }
  }

  impl AppearingObstacle<X, CS, N> for Obstacle3df64 {
    fn can_sense<S>(&self, pose: &Vector<X, Const<N>, S>, radius: X) -> bool
    where
      S: Storage<X, Const<N>>,
    {
      if self.sensed {
        return true;
      }

      let ball = Ball::new(radius);
      let offset = Point::from(pose.clone_owned()).into();

      self.intersects_shape(&offset, &ball)
    }
  }
}

mod leader_follower {
  use crate::cspace::{leader_follower::LeaderFollowerCSpace, CSpace};
  use crate::trajectories::FullTrajectory;

  use super::*;

  const N: usize = D * 2;

  impl<CS> Obstacle<X, CS, N> for ObstacleSpace3df64
  where
    CS: LeaderFollowerCSpace<X, D, N>,
  {
    fn trajectory_free<TF, S1, S2>(&self, t: &TF) -> bool
    where
      TF: FullTrajectory<X, CS::Traj, S1, S2, N>,
      S1: Storage<X, Const<N>>,
      S2: Storage<X, Const<N>>,
    {
      // Breakout
      let lead_start = CS::get_leader(t.start());
      let lead_end = CS::get_leader(t.end());
      let fol_start = CS::get_follower(t.start());
      let fol_end = CS::get_follower(t.end());

      let identity = Isometry::identity();

      // Segment implementation
      let lead_traj = Segment::new(
        Point::from(lead_start.clone_owned()),
        Point::from(lead_end.clone_owned()),
      );
      let fol_traj = Segment::new(
        Point::from(fol_start.clone_owned()),
        Point::from(fol_end.clone_owned()),
      );
      let los_start = Segment::new(
        Point::from(lead_start.clone_owned()),
        Point::from(fol_start.clone_owned()),
      );
      let los_end = Segment::new(
        Point::from(lead_end.clone_owned()),
        Point::from(fol_end.clone_owned()),
      );
      let segments = [lead_traj, fol_traj, los_start, los_end];
      self.obstacles.values().filter(|obs| obs.sensed).all(|obs| {
        segments
          .iter()
          .all(|seg| !obs.intersects_shape(&identity, seg))
      })
    }

    fn is_free<S>(&self, x: &Vector<X, Const<N>, S>) -> bool
    where
      S: Storage<X, Const<N>>,
    {
      // Breakout
      let leader = CS::get_leader(x);
      let follower = CS::get_follower(x);

      let identity = Isometry::identity();

      // Segment implementation
      let segment = Segment::new(
        Point::from(leader.clone_owned()),
        Point::from(follower.clone_owned()),
      );

      self
        .obstacles
        .values()
        .filter(|obs| obs.sensed)
        .all(|obs| !obs.intersects_shape(&identity, &segment))
    }
  }

  impl<CS> ObstacleSpace<X, CS, N> for ObstacleSpace3df64
  where
    CS: LeaderFollowerCSpace<X, D, N>,
  {
    type Obs = Obstacle3df64;

    fn empty() -> Self {
      Self::internal_empty()
    }

    fn add_obstacles<I>(&mut self, obstacles: I)
    where
      I: IntoIterator<Item = (ObstacleId, Self::Obs)>,
    {
      self.internal_add_obstacles(obstacles)
    }

    fn get_obstacles(&self, obstacles: &[ObstacleId]) -> Vec<&Self::Obs> {
      self.internal_get_obstacles(obstacles)
    }

    fn get_sensed_obstacles(&self) -> Vec<(ObstacleId, &Self::Obs)> {
      self.internal_get_sensed_obstacles()
    }

    fn get_not_sensed_obstacles(&self) -> Vec<(ObstacleId, &Self::Obs)> {
      self.internal_get_not_sensed_obstacles()
    }

    fn get_all_obstacle_ids(&self) -> Vec<ObstacleId> {
      self.internal_get_all_obstacle_ids()
    }

    fn remove_obstacles(&mut self, obstacles: &[ObstacleId]) {
      self.internal_remove_obstacles(obstacles)
    }

    fn count(&self) -> usize {
      self.internal_count()
    }

    fn check_sensors<S>(
      &mut self,
      pose: &Vector<X, Const<N>, S>,
      radius: X,
    ) -> Vec<ObstacleId>
    where
      S: Storage<X, Const<N>>,
    {
      let leader = CS::get_leader(pose);
      self.internal_check_sensors(&leader, radius)
    }
  }

  impl<CS> Obstacle<X, CS, N> for Obstacle3df64
  where
    CS: LeaderFollowerCSpace<X, D, N>,
  {
    fn trajectory_free<TF, S1, S2>(&self, t: &TF) -> bool
    where
      TF: FullTrajectory<X, <CS as CSpace<X, N>>::Traj, S1, S2, N>,
      S1: Storage<X, Const<N>>,
      S2: Storage<X, Const<N>>,
    {
      // Breakout
      let lead_start = CS::get_leader(t.start());
      let lead_end = CS::get_leader(t.end());
      let fol_start = CS::get_follower(t.start());
      let fol_end = CS::get_follower(t.end());

      let identity = Isometry::identity();

      // Segment implementation
      let lead_traj = Segment::new(
        Point::from(lead_start.clone_owned()),
        Point::from(lead_end.clone_owned()),
      );
      let fol_traj = Segment::new(
        Point::from(fol_start.clone_owned()),
        Point::from(fol_end.clone_owned()),
      );
      let los_start = Segment::new(
        Point::from(lead_start.clone_owned()),
        Point::from(fol_start.clone_owned()),
      );
      let los_end = Segment::new(
        Point::from(lead_end.clone_owned()),
        Point::from(fol_end.clone_owned()),
      );
      let segments = [lead_traj, fol_traj, los_start, los_end];
      segments
        .iter()
        .all(|seg| !self.intersects_shape(&identity, seg))
    }

    fn is_free<S>(&self, x: &Vector<X, Const<N>, S>) -> bool
    where
      S: Storage<X, Const<N>>,
    {
      // Breakout
      let leader = CS::get_leader(x);
      let follower = CS::get_follower(x);

      let identity = Isometry::identity();

      // Segment implementation
      let segment = Segment::new(
        Point::from(leader.clone_owned()),
        Point::from(follower.clone_owned()),
      );

      !self.intersects_shape(&identity, &segment)
    }
  }

  impl<CS> AppearingObstacle<X, CS, N> for Obstacle3df64
  where
    CS: LeaderFollowerCSpace<X, D, N>,
  {
    fn can_sense<S>(&self, pose: &Vector<X, Const<N>, S>, radius: X) -> bool
    where
      S: Storage<X, Const<N>>,
    {
      if self.sensed {
        return true;
      }

      let leader = CS::get_leader(pose);
      let ball = Ball::new(radius);
      let offset = Point::from(leader.clone_owned()).into();

      self.intersects_shape(&offset, &ball)
    }
  }
}

/// An obstacle that can be any of the 3d shapes supported by parry
/// except Compound, Polyline, or TriMesh
#[derive(Clone, Serialize, Deserialize)]
pub struct Obstacle3df64 {
  shape: SharedShape,
  offset: Isometry<X>,
  sensed: bool,
  aabb: AABB,
}

impl Obstacle3df64 {
  pub fn no_offset(shape: SharedShape) -> Self {
    Self::with_offset(shape, Isometry::identity())
  }

  pub fn with_offset(shape: SharedShape, offset: Isometry<X>) -> Self {
    Self::new(shape, offset, Behavior::default() == Behavior::Static)
  }

  pub fn new(shape: SharedShape, offset: Isometry<X>, sensed: bool) -> Self {
    let aabb = shape.compute_aabb(&offset);
    Self {
      shape,
      offset,
      sensed,
      aabb,
    }
  }

  pub fn shape(&self) -> &SharedShape {
    &self.shape
  }

  pub fn offset(&self) -> &Isometry<X> {
    &self.offset
  }

  pub fn sensed(&self) -> bool {
    self.sensed
  }

  pub fn aabb(&self) -> &AABB {
    &self.aabb
  }

  pub fn intersects(&self, other: &Self) -> bool {
    if !self.aabb.intersects(&other.aabb) {
      return false;
    }

    intersection_test(
      &self.offset,
      self.shape.0.as_ref(),
      &other.offset,
      other.shape.0.as_ref(),
    )
    .unwrap()
  }

  fn intersects_shape<S: Shape>(
    &self,
    offset: &Isometry<X>,
    shape: &S,
  ) -> bool {
    if !self.aabb.intersects(&shape.compute_aabb(offset)) {
      return false;
    }

    intersection_test(&self.offset, self.shape.0.as_ref(), offset, shape)
      .unwrap()
  }

  fn contains_point(&self, point: &Point<X>) -> bool {
    if !self.aabb.contains_local_point(point) {
      return false;
    }

    self.shape.contains_point(&self.offset, point)
  }
}

impl FromParams for Obstacle3df64 {
  type Params = Obstacle3dParams<X>;
  fn from_params(params: Self::Params) -> Result<Self> {
    let sensed = match params.behavior {
      Behavior::Static => true,
      Behavior::Appearing => false,
    };

    Ok(Obstacle3df64::new(
      SharedShape::from_params(params.shape)?,
      Isometry::from_params(params.offset)?,
      sensed,
    ))
  }
}

impl From<(Isometry<Real>, SharedShape)> for Obstacle3df64 {
  fn from(static_obstacle: (Isometry<Real>, SharedShape)) -> Self {
    Self::with_offset(static_obstacle.1, static_obstacle.0)
  }
}

impl FromParams for Isometry<X> {
  type Params = Offset3dParams<X>;
  fn from_params(params: Self::Params) -> Result<Self> {
    Ok(Isometry::new(
      params.translation,
      params.axis_angle.map(|x| x.to_radians()),
    ))
  }
}

impl FromParams for SharedShape {
  type Params = Shape3dParams<X>;
  fn from_params(params: Self::Params) -> Result<Self> {
    match params {
      Shape3dParams::Ball { radius } => Ok(SharedShape::new(Ball::new(radius))),
      Shape3dParams::Capsule { a, b, radius } => {
        Ok(SharedShape::new(Capsule::new(a.into(), b.into(), radius)))
      }
      Shape3dParams::Compound { shapes } => {
        let mut new_shapes = Vec::new();
        for (offset, shape) in shapes {
          new_shapes.push((
            Isometry::from_params(offset)?,
            SharedShape::from_params(shape)?,
          ))
        }
        Ok(SharedShape::new(Compound::new(new_shapes)))
      }
      Shape3dParams::ConvexPolyhedron { points } => {
        let points: Vec<_> = points.into_iter().map(|p| p.into()).collect();
        let poly = ConvexPolyhedron::from_convex_hull(&points)
          .ok_or(Error::ConvexHullError)?;
        Ok(SharedShape::new(poly))
      }
      Shape3dParams::Cone {
        half_height,
        radius,
      } => Ok(SharedShape::new(Cone::new(half_height, radius))),
      Shape3dParams::Cuboid { half_extents } => {
        Ok(SharedShape::new(Cuboid::new(half_extents)))
      }
      Shape3dParams::Cylinder {
        half_height,
        radius,
      } => Ok(SharedShape::new(Cylinder::new(half_height, radius))),
      Shape3dParams::HalfSpace { normal } => Ok(SharedShape::new(
        HalfSpace::new(Unit::new_normalize(normal)),
      )),
      Shape3dParams::RoundCone {
        half_height,
        radius,
        border_radius,
      } => {
        let cone = Cone::new(half_height, radius);
        let cone = RoundCone {
          base_shape: cone,
          border_radius: border_radius,
        };
        Ok(SharedShape::new(cone))
      }
      Shape3dParams::RoundConvexPolyhedron {
        points,
        border_radius,
      } => {
        let points: Vec<_> = points.into_iter().map(|p| p.into()).collect();
        let poly = ConvexPolyhedron::from_convex_hull(&points)
          .ok_or(Error::ConvexHullError)?;
        let poly = RoundConvexPolyhedron {
          base_shape: poly,
          border_radius: border_radius,
        };
        Ok(SharedShape::new(poly))
      }
      Shape3dParams::RoundCylinder {
        half_height,
        radius,
        border_radius,
      } => {
        let cylinder = Cylinder::new(half_height, radius);
        let cylinder = RoundCylinder {
          base_shape: cylinder,
          border_radius: border_radius,
        };
        Ok(SharedShape::new(cylinder))
      }
      Shape3dParams::RoundCuboid {
        half_extents,
        border_radius,
      } => {
        let cube = Cuboid::new(half_extents);
        let cube = RoundCuboid {
          base_shape: cube,
          border_radius: border_radius,
        };
        Ok(SharedShape::new(cube))
      }
      Shape3dParams::RoundTriangle {
        a,
        b,
        c,
        border_radius,
      } => {
        let tri = Triangle::new(a.into(), b.into(), c.into());
        let tri = RoundTriangle {
          base_shape: tri,
          border_radius: border_radius,
        };
        Ok(SharedShape::new(tri))
      }
      Shape3dParams::Segment { a, b } => {
        Ok(SharedShape::new(Segment::new(a.into(), b.into())))
      }
      Shape3dParams::Triangle { a, b, c } => Ok(SharedShape::new(
        Triangle::new(a.into(), b.into(), c.into()),
      )),
    }
  }
}

#[cfg(test)]
mod tests {

  use parry::na::base::Unit;
  use parry::shape::{
    Ball, Capsule, Cone, ConvexPolyhedron, Cuboid, Cylinder, HalfSpace,
    RoundCone, RoundConvexPolyhedron, RoundCuboid, RoundCylinder,
    RoundTriangle, Segment, SharedShape, Triangle,
  };

  use crate::trajectories::{EuclideanTrajectory, FullTrajOwned};

  use super::*;

  fn get_shapes() -> Vec<(Isometry<Real>, SharedShape)> {
    vec![
      SharedShape::new(Ball::new(0.5)),
      SharedShape::new(Capsule::new(
        [0.0, 0.0, 0.0].into(),
        [0.0, 1.0, 0.0].into(),
        0.5,
      )),
      // Deserialization seems to fail for Compound shape
      SharedShape::new(Cone::new(0.5, 1.0)),
      SharedShape::new(
        ConvexPolyhedron::from_convex_hull(&[
          [0.5, 0.0, 0.5].into(),
          [-0.5, 0.0, 0.5].into(),
          [0.0, 1.0, 0.0].into(),
          [0.0, 0.1, 0.0].into(),
          [0.0, 0.0, 1.0].into(),
        ])
        .unwrap(),
      ),
      SharedShape::new(Cuboid::new([0.5, 0.5, 0.5].into())),
      SharedShape::new(Cylinder::new(0.5, 1.0)),
      SharedShape::new(HalfSpace::new(Unit::new_normalize(
        [1.0, 1.0, 1.0].into(),
      ))),
      // Deserialization seems to fail for Polyline shape
      SharedShape::new(RoundCone {
        base_shape: Cone::new(0.5, 1.0),
        border_radius: 0.1,
      }),
      SharedShape::new(RoundConvexPolyhedron {
        base_shape: ConvexPolyhedron::from_convex_hull(&[
          [0.5, 0.0, 0.5].into(),
          [-0.5, 0.0, 0.5].into(),
          [0.0, 1.0, 0.0].into(),
          [0.0, 0.1, 0.0].into(),
          [0.0, 0.0, 1.0].into(),
        ])
        .unwrap(),
        border_radius: 0.1,
      }),
      SharedShape::new(RoundCuboid {
        base_shape: Cuboid::new([0.5, 0.5, 0.5].into()),
        border_radius: 0.1,
      }),
      SharedShape::new(RoundCylinder {
        base_shape: Cylinder::new(0.5, 1.0),
        border_radius: 0.1,
      }),
      SharedShape::new(RoundTriangle {
        base_shape: Triangle::new(
          [0.5, 0.0, 0.0].into(),
          [-0.5, 0.0, 0.0].into(),
          [0.0, 1.0, 1.0].into(),
        ),
        border_radius: 0.1,
      }),
      SharedShape::new(Segment::new(
        [0.0, 0.0, 0.0].into(),
        [1.0, 1.0, 1.0].into(),
      )),
      // Deserialization seems to fail for TriMesh shape
      SharedShape::new(Triangle::new(
        [0.5, 0.0, 0.0].into(),
        [-0.5, 0.0, 0.0].into(),
        [0.0, 1.0, 1.0].into(),
      )),
    ]
    .into_iter()
    .map(|i| (Isometry::<X>::identity(), i))
    .collect()
  }

  fn get_obstacle_space() -> ObstacleSpace3df64 {
    ObstacleSpace3df64::from(get_shapes())
  }

  fn get_obstacles() -> Vec<Obstacle3df64> {
    get_shapes().into_iter().map(Obstacle3df64::from).collect()
  }

  #[test]
  fn test_serialize_bincode_obstacles() {
    let obstacles = get_obstacles();
    let v = bincode::serialize(&obstacles).unwrap();
    let _: Vec<Obstacle3df64> = bincode::deserialize(&v).unwrap();
  }

  #[test]
  fn test_intersection_parry_obstacle() {
    let obstacle_space = get_obstacle_space();
    let t = FullTrajOwned::new(
      [0.0, 0.0, 0.0].into(),
      [0.0, 1.0, 0.0].into(),
      EuclideanTrajectory::new(),
    );
    assert_eq!(obstacle_space.trajectory_free(&t), false);

    let t = FullTrajOwned::new(
      [1.0, 10.0, 0.0].into(),
      [0.0, 10.0, 0.0].into(),
      EuclideanTrajectory::new(),
    );
    assert_eq!(obstacle_space.trajectory_free(&t), true);
  }

  #[test]
  fn test_intersection_ball() {
    let ball = Ball::new(0.75);
    let ball = Obstacle3df64::no_offset(SharedShape::new(ball));
    let t = FullTrajOwned::new(
      [0.0, 0.0, 0.0].into(),
      [0.0, 1.0, 0.0].into(),
      EuclideanTrajectory::new(),
    );
    assert_eq!(ball.trajectory_free(&t), false);
    let t = FullTrajOwned::new(
      [1.0, 0.0, 0.0].into(),
      [0.0, 1.0, 0.0].into(),
      EuclideanTrajectory::new(),
    );
    assert_eq!(ball.trajectory_free(&t), false);
    let t = FullTrajOwned::new(
      [-1.0, 0.0, 0.0].into(),
      [1.0, 0.0, 0.0].into(),
      EuclideanTrajectory::new(),
    );
    assert_eq!(ball.trajectory_free(&t), false);
    let t = FullTrajOwned::new(
      [1.0, -1.0, 0.0].into(),
      [1.0, 1.0, 0.0].into(),
      EuclideanTrajectory::new(),
    );
    assert_eq!(ball.trajectory_free(&t), true);
    let t = FullTrajOwned::new(
      [0.7500001, -1.0, 0.0].into(),
      [0.7500001, 1.0, 0.0].into(),
      EuclideanTrajectory::new(),
    );
    assert_eq!(ball.trajectory_free(&t), true);
  }
}