Add YCylinder

src/tube/mod.rs

@@ -1,4 +1,4 @@
-use glam::{bool, f32, Mat3, Vec3};
+use glam::{bool, f32, Mat3, Vec3, Vec3Swizzles};
 
 use crate::ifaces::{DebugTraceable, RayPath, Traceable};
 use coords::{FlatCoordinateSystem, InnerCS, OuterCS};
@@ -266,10 +266,91 @@ impl Rect {
 	}
 }
 
+fn solve_quadratic(a: f32, half_b: f32, c: f32) -> Option<(f32, f32)> {
+	let base = -half_b / a;
+	let d = base * base - c / a;
+	if d < 0. {
+		None
+	} else {
+		let δ = d.sqrt();
+		Some((base - δ, base + δ))
+	}
+}
+
+/// Цилиндр с центром в начале координат и осью вдоль оси Y.
+struct YCylinder {
+	pub half_length: f32,
+	pub radius: f32,
+}
+
+impl YCylinder {
+	/// Отражает луч, чтобы все координаты направления были положительны (допустимо благодаря симметрии YCylinder).
+	fn flip_ray(ray: Ray) -> Ray {
+		Ray {
+			pos: ray.pos * ray.dir.signum(),
+			dir: ray.dir.abs(),
+		}
+	}
+
+	fn is_inside(&self, pt: Vec3) -> bool {
+		let r = f32::hypot(pt.x, pt.z);
+		pt.y.abs() < self.half_length && r < self.radius
+	}
+
+	fn trace_into(&self, ray: Ray) -> Option<f32> {
+		let ray = Self::flip_ray(ray);
+
+		// 1. ray.pos.y + t * ray.dir.y = −half_length
+		let t_cap_in = (-self.half_length - ray.pos.y) / ray.dir.y;
+		let t_cap_out = (self.half_length - ray.pos.y) / ray.dir.y;
+
+		// 2. (ray.pos.x + t * ray.dir.x)² + (ray.pos.z + t * ray.dir.z)² = radius²
+		let pos = ray.pos.xz();
+		let dir = ray.dir.xz();
+		if dir.length_squared() < 1e-3 {
+			if pos.length_squared() >= self.radius.powi(2) {
+				return None;
+			}
+			return Some(t_cap_in).filter(|&t| t > 0.);
+		}
+		let (t_side_in, t_side_out) = solve_quadratic(
+			dir.length_squared(),
+			pos.dot(dir),
+			pos.length_squared() - self.radius.powi(2),
+		)?;
+		let t = f32::max(t_cap_in, t_side_in);
+		if t < 0. {
+			return None;
+		}
+		if t >= t_cap_out || t >= t_side_out {
+			return None;
+		}
+		Some(t)
+	}
+
+	fn trace_out_of(&self, ray: Ray) -> Option<f32> {
+		let ray = Self::flip_ray(ray);
+		let t_cap_out = (self.half_length - ray.pos.y) / ray.dir.y;
+		let pos = ray.pos.xz();
+		let dir = ray.dir.xz();
+		if dir.length_squared() < 1e-3 {
+			return Some(t_cap_out);
+		}
+		let (_t_side_in, t_side_out) = solve_quadratic(
+			dir.length_squared(),
+			pos.dot(dir),
+			pos.length_squared() - self.radius.powi(2),
+		)
+		.expect("the ray starts inside and is not along the axis so *has* to cross the side");
+		Some(t_side_out)
+	}
+}
+
 #[cfg(test)]
 mod tests {
 	use super::*;
 	use crate::types::ray;
+	use approx::assert_abs_diff_eq;
 	use glam::vec3;
 
 	#[test]
@@ -310,4 +391,67 @@ mod tests {
 		assert_eq!(r.trace_out_of(ray(vec3(1., 1., 1.), vec3(0., -1., 0.))), Some(4.));
 		assert_eq!(r.trace_out_of(ray(vec3(2., 3., 2.), vec3(1., 1., 1.))), Some(0.));
 	}
+
+	#[test]
+	fn test_cylinder() {
+		assert_eq!(
+			YCylinder::flip_ray(ray(vec3(2., 3., 2.), vec3(4., 5., 4.))),
+			ray(vec3(2., 3., 2.), vec3(4., 5., 4.)),
+		);
+		assert_eq!(
+			YCylinder::flip_ray(ray(vec3(2., 3., 2.), vec3(-4., 5., -4.))),
+			ray(vec3(-2., 3., -2.), vec3(4., 5., 4.)),
+		);
+		assert_eq!(
+			YCylinder::flip_ray(ray(vec3(2., 3., 2.), vec3(4., -5., 4.))),
+			ray(vec3(2., -3., 2.), vec3(4., 5., 4.)),
+		);
+		assert_eq!(
+			YCylinder::flip_ray(ray(vec3(2., 3., 2.), vec3(4., 0., 4.))),
+			ray(vec3(2., 3., 2.), vec3(4., 0., 4.)),
+		);
+
+		let r = YCylinder {
+			half_length: 3.,
+			radius: 2.,
+		};
+
+		assert_eq!(r.trace_into(ray(vec3(3., 4., 3.), vec3(0., -1., 0.))), None);
+		assert_eq!(r.trace_into(ray(vec3(1., 4., 1.), vec3(0., -1., 0.))), Some(1.));
+		assert_eq!(r.trace_into(ray(vec3(3., 3., 3.), vec3(1., 1., 1.))), None);
+		assert_abs_diff_eq!(
+			r.trace_into(ray(vec3(-3., 2., -3.), vec3(1., 0., 1.))).unwrap(),
+			1.5857864
+		);
+		assert_eq!(r.trace_into(ray(vec3(-3., 2., -3.), vec3(-1., 0., -1.))), None);
+		assert_abs_diff_eq!(
+			r.trace_into(ray(vec3(-3., 1., -3.), vec3(2., 2., 2.))).unwrap(),
+			0.7928932
+		);
+		assert_eq!(r.trace_into(ray(vec3(-3., 2.1, -3.), vec3(2., 2., 2.))), None);
+
+		assert_eq!(r.trace_into(ray(vec3(2., 3., 2.), vec3(1., 1., 1.))), None);
+		assert_eq!(r.trace_into(ray(vec3(-2., 3., -2.), vec3(-1., 1., -1.))), None);
+		assert_eq!(
+			r.trace_into(ray(vec3(1.4142135, 3., 1.4142135), vec3(-1., -1., -1.))),
+			Some(0.)
+		);
+		assert_eq!(
+			r.trace_into(ray(vec3(1.4142135, -3., 1.4142135), vec3(-1., 1., -1.))),
+			Some(0.)
+		);
+
+		assert_abs_diff_eq!(
+			r.trace_out_of(ray(vec3(0., 0., 0.), vec3(1., 1., 1.))).unwrap(),
+			1.4142135
+		);
+		assert_eq!(r.trace_out_of(ray(vec3(0., 0., 0.), vec3(0., 1., 0.))), Some(3.));
+		assert_eq!(r.trace_out_of(ray(vec3(0., 1., 0.), vec3(0., -1., 0.))), Some(4.));
+		assert_eq!(r.trace_out_of(ray(vec3(1., 1., 1.), vec3(0., -1., 0.))), Some(4.));
+		assert_abs_diff_eq!(
+			r.trace_out_of(ray(vec3(1.4142135, 3., 1.4142135), vec3(1., 1., 1.)))
+				.unwrap(),
+			0.
+		);
+	}
 }