extern crate num_traits;
extern crate serde;
use num_traits::Float;
#[derive(Debug, Clone, Copy, Serialize, Deserialize)]
#[repr(C)]
pub struct Point<F: Float> {
pub x: F,
pub y: F,
pub z: F,
}
impl<F: Float> Point<F> {
pub fn from_coord(coord: [F; 3]) -> Self {
Point {
x: coord[0],
y: coord[1],
z: coord[2],
}
}
}
#[derive(Debug, Clone, Copy, Serialize, Deserialize)]
#[repr(C)]
pub struct Vector<F: Float> {
pub x: F,
pub y: F,
pub z: F,
}
impl<F: Float> std::ops::Index<usize> for Vector<F> {
type Output = F;
fn index(&self, i: usize) -> &F {
unsafe { &*(self as *const Vector<F> as *const [F; 3]) }.index(i)
}
}
impl<F: Float> std::ops::IndexMut<usize> for Vector<F> {
fn index_mut(&mut self, i: usize) -> &mut F {
unsafe { &mut *(self as *mut Vector<F> as *mut [F; 3]) }.index_mut(i)
}
}
impl<F: Float> Vector<F> {
pub fn zero() -> Self {
Vector {
x: F::zero(),
y: F::zero(),
z: F::zero(),
}
}
pub fn from_coord(coord: [F; 3]) -> Self {
Vector {
x: coord[0],
y: coord[1],
z: coord[2],
}
}
pub fn norm(&self) -> F {
self.norm2().sqrt()
}
pub fn norm2(&self) -> F {
self.x * self.x + self.y * self.y + self.z * self.z
}
pub fn cross_product(&self, other: &Vector<F>) -> Self {
Vector {
x: self.y * other.z - self.z * other.y,
y: self.z * other.x - self.x * other.z,
z: self.x * other.y - self.y * other.x,
}
}
pub fn dot(&self, other: &Vector<F>) -> F {
self.x * other.x + self.y * other.y + self.z * other.z
}
}
impl<F: Float> std::ops::Sub<Point<F>> for Point<F> {
type Output = Vector<F>;
fn sub(self, b: Point<F>) -> Self::Output {
Vector {
x: self.x - b.x,
y: self.y - b.y,
z: self.z - b.z,
}
}
}
impl<F: Float> std::ops::Add<Vector<F>> for Point<F> {
type Output = Point<F>;
fn add(self, b: Vector<F>) -> Self::Output {
Point {
x: self.x + b.x,
y: self.y + b.y,
z: self.z + b.z,
}
}
}
impl<F: Float> std::ops::AddAssign<Vector<F>> for Point<F> {
fn add_assign(&mut self, b: Vector<F>) {
self.x = b.x + self.x;
self.y = b.y + self.y;
self.z = b.z + self.z;
}
}
impl<F: Float> std::ops::Sub<Vector<F>> for Vector<F> {
type Output = Vector<F>;
fn sub(self, b: Vector<F>) -> Self::Output {
Vector {
x: self.x - b.x,
y: self.y - b.y,
z: self.z - b.z,
}
}
}
impl<F: Float> std::ops::Neg for Vector<F> {
type Output = Vector<F>;
fn neg(self) -> Self::Output {
Vector {
x: -self.x,
y: -self.y,
z: -self.z,
}
}
}
impl<F: Float> std::ops::Add<Vector<F>> for Vector<F> {
type Output = Vector<F>;
fn add(self, b: Vector<F>) -> Self::Output {
Vector {
x: self.x + b.x,
y: self.y + b.y,
z: self.z + b.z,
}
}
}
impl<F: Float> std::ops::SubAssign<Vector<F>> for Vector<F> {
fn sub_assign(&mut self, b: Vector<F>) {
self.x = b.x - self.x;
self.y = b.y - self.y;
self.z = b.z - self.z;
}
}
impl<F: Float> std::ops::AddAssign<Vector<F>> for Vector<F> {
fn add_assign(&mut self, b: Vector<F>) {
self.x = b.x + self.x;
self.y = b.y + self.y;
self.z = b.z + self.z;
}
}
impl<F: Float> std::ops::Mul<Vector<F>> for f32 {
type Output = Vector<F>;
fn mul(self, b: Vector<F>) -> Self::Output {
Vector {
x: b.x * F::from(self).unwrap(),
y: b.y * F::from(self).unwrap(),
z: b.z * F::from(self).unwrap(),
}
}
}
impl<F: Float> std::ops::Mul<Vector<F>> for f64 {
type Output = Vector<F>;
fn mul(self, b: Vector<F>) -> Self::Output {
Vector {
x: b.x * F::from(self).unwrap(),
y: b.y * F::from(self).unwrap(),
z: b.z * F::from(self).unwrap(),
}
}
}
impl<F: Float> std::ops::Mul<F> for Vector<F> {
type Output = Vector<F>;
fn mul(self, b: F) -> Self::Output {
Vector {
x: self.x * b,
y: self.y * b,
z: self.z * b,
}
}
}
impl<F: Float> std::ops::Div<F> for Vector<F> {
type Output = Vector<F>;
fn div(self, b: F) -> Self::Output {
Vector {
x: self.x / b,
y: self.y / b,
z: self.z / b,
}
}
}
pub fn distance_segment<F: Float>(
a: Point<F>,
b: Point<F>,
c: Point<F>,
d: Point<F>,
) -> (F, Vector<F>) {
let u = b - a;
let v = d - c;
let n = u.cross_product(&v);
if n.norm() < F::from(1e-5).unwrap() {
return ((a - c).norm(), (a - c));
}
let normalise = u.dot(&v) / u.norm2();
let mut mu =
(-((c - a).dot(&v)) - normalise * ((a - c).dot(&u))) / (v.norm2() - normalise * u.dot(&v));
let mut lambda = (-((a - c).dot(&u)) + mu * u.dot(&v)) / (u.norm2());
if F::zero() <= mu && mu <= F::one() && F::zero() <= lambda && lambda <= F::one() {
let vec = (a + u * lambda) - (c + v * mu);
(vec.norm(), vec)
} else {
let mut min_dist = F::infinity();
let mut min_vec = Vector::zero();
lambda = F::zero();
mu = -((c - a).dot(&v)) / v.norm2();
if F::zero() <= mu && mu <= F::one() {
let vec = (a + u * lambda) - (c + v * mu);
if min_dist > vec.norm() {
min_dist = vec.norm();
min_vec = vec.clone();
}
} else {
mu = F::zero();
let vec = (a + u * lambda) - (c + v * mu);
if min_dist > vec.norm() {
min_dist = vec.norm();
min_vec = vec.clone();
}
mu = F::one();
let vec = (a + u * lambda) - (c + v * mu);
if min_dist > vec.norm() {
min_dist = vec.norm();
min_vec = vec.clone();
}
}
lambda = F::one();
mu = (-(c - a).dot(&v) + u.dot(&v)) / v.norm2();
if F::zero() <= mu && mu <= F::one() {
min_dist = min_dist.min(((a + u * lambda) - (c + v * mu)).norm());
let vec = (a + u * lambda) - (c + v * mu);
if min_dist > vec.norm() {
min_dist = vec.norm();
min_vec = vec.clone();
}
} else {
mu = F::zero();
let vec = (a + u * lambda) - (c + v * mu);
if min_dist > vec.norm() {
min_dist = vec.norm();
min_vec = vec.clone();
}
mu = F::one();
let vec = (a + u * lambda) - (c + v * mu);
if min_dist > vec.norm() {
min_dist = vec.norm();
min_vec = vec.clone();
}
}
mu = F::zero();
lambda = (-((a - c).dot(&u)) + mu * u.dot(&v)) / (u.norm2());
if F::zero() <= lambda && F::one() >= lambda {
let vec = (a + u * lambda) - (c + v * mu);
if min_dist > vec.norm() {
min_dist = vec.norm();
min_vec = vec.clone();
}
} else {
lambda = F::zero();
let vec = (a + u * lambda) - (c + v * mu);
if min_dist > vec.norm() {
min_dist = vec.norm();
min_vec = vec.clone();
}
lambda = F::one();
let vec = (a + u * lambda) - (c + v * mu);
if min_dist > vec.norm() {
min_dist = vec.norm();
min_vec = vec.clone();
}
}
mu = F::one();
lambda = (-((a - c).dot(&u)) + mu * u.dot(&v)) / (u.norm2());
if F::zero() <= lambda && F::one() >= lambda {
let vec = (a + u * lambda) - (c + v * mu);
if min_dist > vec.norm() {
min_dist = vec.norm();
min_vec = vec.clone();
}
} else {
lambda = F::zero();
let vec = (a + u * lambda) - (c + v * mu);
if min_dist > vec.norm() {
min_dist = vec.norm();
min_vec = vec.clone();
}
lambda = F::one();
let vec = (a + u * lambda) - (c + v * mu);
if min_dist > vec.norm() {
min_dist = vec.norm();
min_vec = vec.clone();
}
}
(min_dist, min_vec)
}
}