--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/src/cylinder.rs Wed Dec 04 23:19:46 2024 -0500
@@ -0,0 +1,932 @@
+/*!
+Implementation of the surface of a 3D cylinder as a [`ManifoldPoint`].
+*/
+
+use alg_tools::euclidean::Euclidean;
+use serde_repr::*;
+use serde::{Serialize, Deserialize};
+use alg_tools::loc::Loc;
+use alg_tools::norms::{Norm, L2};
+use alg_tools::types::Float;
+use crate::manifold::{ManifoldPoint, EmbeddedManifoldPoint, FacedManifoldPoint};
+use crate::newton::{newton_sym1x1, newton_sym2x2};
+
+/// Angle
+pub type Angle = f64;
+
+/// Cylindrical coordinates in ℝ^3
+#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
+pub struct CylCoords {
+ pub r : f64,
+ pub angle : Angle,
+ pub z : f64
+}
+
+impl CylCoords {
+ #[inline]
+ pub fn to_cartesian(&self) -> Loc<f64, 3> {
+ let &CylCoords{r, angle, z} = self;
+ [r * angle.cos(), r * angle.sin(), z].into()
+ }
+
+ #[inline]
+ #[allow(dead_code)]
+ pub fn from_cartesian(coords : Loc<f64, 3>) -> Self {
+ let [x, y, z] = coords.into();
+ let r = (x*x + y*y).sqrt();
+ let angle = y.atan2(x);
+ CylCoords {r, angle, z}
+ }
+}
+
+/// Coordinates on a cap
+#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
+pub struct CapPoint {
+ pub r : f64,
+ pub angle : Angle
+}
+
+#[inline]
+fn rotate(φ : f64, Loc([x, y]) : Loc<f64, 2>) -> Loc<f64, 2> {
+ let sin_φ = φ.sin();
+ let cos_φ = φ.cos();
+ [cos_φ * x - sin_φ * y, sin_φ * x + cos_φ * y].into()
+}
+
+impl CapPoint {
+ #[inline]
+ /// Convert to cylindrical coordinates given z coordinate
+ fn cyl_coords(&self, z : f64) -> CylCoords {
+ let CapPoint { r, angle } = *self;
+ CylCoords { r, angle, z }
+ }
+
+ #[inline]
+ /// Convert to cylindrical coordinates given z coordinate
+ fn cartesian_coords(&self) -> Loc<f64, 2> {
+ let CapPoint { r, angle } = *self;
+ [r * angle.cos(), r * angle.sin()].into()
+ }
+
+ #[inline]
+ #[allow(dead_code)]
+ /// Convert to cylindrical coordinates given z coordinate
+ fn from_cartesian(coords : Loc<f64, 2>) -> Self {
+ let [x, y] = coords.into();
+ let r = (x*x + y*y).sqrt();
+ let angle = y.atan2(x);
+ CapPoint { r, angle }
+ }
+
+ #[inline]
+ /// Calculate the vector between two points on the cap
+ fn log(&self, other : &CapPoint) -> Loc<f64, 2> {
+ other.cartesian_coords() - self.cartesian_coords()
+ }
+
+ #[inline]
+ /// Calculate partial exponential map until boundary.
+ /// Returns the final point within the cap as well as a remaining tangent on
+ /// the side of the cylinder, if `t` wasn't fully used.
+ fn partial_exp(&self, r : f64, t : &Tangent) -> (CapPoint, Option<Tangent>) {
+ let q = self.cartesian_coords() + t;
+ let n = q.norm2();
+ if n <= r {
+ (Self::from_cartesian(q), None)
+ } else {
+ let p = q * r / n;
+ (Self::from_cartesian(p), Some(q - p))
+ }
+ }
+
+ #[inline]
+ /// Convert tangent from side tangent to cap tangent
+ fn tangent_from_side(&self, top : bool, t : Tangent) -> Tangent {
+ if top {
+ // The angle is such that down would be rotated to self.angle, counterclockwise
+ // The new tangent is R[down + t] - R[down] = Rt.
+ rotate(self.angle+f64::PI/2.0, t)
+ } else {
+ // The angle is such that up would be rotated to self.angle, clockwise
+ rotate(self.angle-f64::PI/2.0, t)
+ }
+ }
+
+ #[inline]
+ /// Convert tangent from cap tangent to tangent tangent
+ fn tangent_to_side(&self, top : bool, t : Tangent) -> Tangent {
+ if top {
+ // The angle is such that self.angle would be rotated to down, clockwise
+ rotate(-self.angle-f64::PI/2.0, t)
+ } else {
+ // The angle is such that self.angle would be rotated to up, counterclockwise
+ rotate(f64::PI/2.0-self.angle, t)
+ }
+ }
+}
+
+/// Coordinates on a side
+#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
+pub struct SidePoint {
+ pub z : f64,
+ pub angle : Angle
+}
+
+#[inline]
+fn anglediff(mut φ1 : f64, mut φ2 : f64) -> f64 {
+ let π = f64::PI;
+ φ1 = normalise_angle(φ1);
+ φ2 = normalise_angle(φ2);
+ let α = φ2 - φ1;
+ if α >= 0.0 {
+ if α <= π {
+ α
+ } else {
+ α - 2.0 * π
+ }
+ } else {
+ if α >= -π {
+ α
+ } else {
+ 2.0 * π + α
+ }
+ }
+}
+
+#[inline]
+pub fn normalise_angle(φ : f64) -> f64 {
+ let π = f64::PI;
+ φ.rem_euclid(2.0 * π)
+}
+
+impl SidePoint {
+ #[inline]
+ /// Convert to cylindrical coordinates given radius
+ fn cyl_coords(&self, r : f64) -> CylCoords {
+ let SidePoint { z, angle } = *self;
+ CylCoords { r, angle, z }
+ }
+
+ #[inline]
+ /// Calculate tangent vector between two points on the side, given radius
+ fn log(&self, r : f64, other : &SidePoint) -> Loc<f64, 2> {
+ let SidePoint{ z : z1, angle : angle1 } = *self;
+ let SidePoint{ z : z2, angle : angle2 } = *other;
+ let φ = anglediff(angle1, angle2);
+ // TODO: is this correct?
+ [r*φ, z2-z1].into()
+ }
+
+ #[inline]
+ /// Calculate partial exponential map under boundary
+ /// Returns a point on the next face, as well as a remaining tangent on
+ /// the side of the cylinder, if `t` wasn't fully used.
+ fn partial_exp(&self, r : f64, (a, b) : (f64, f64), t : &Tangent)
+ -> (SidePoint, Option<Tangent>)
+ {
+ assert!(a <= self.z && self.z <= b);
+ let Loc([_, h]) = *t;
+ let s = if h > 0.0 {
+ ((b - self.z)/h).min(1.0)
+ } else if h < 0.0 {
+ ((a - self.z)/h).min(1.0)
+ } else {
+ 1.0
+ };
+ let d = t * s;
+ let p = self.unflatten(r, d);
+ if s < 1.0 {
+ (p, Some(t - d))
+ } else {
+ (p, None)
+ }
+ }
+
+ #[inline]
+ /// Unflattens another point in the local coordinate system of self
+ fn unflatten(&self, r : f64, Loc([v, h]) : Loc<f64, 2>) -> Self {
+ SidePoint{ z : self.z + h, angle : normalise_angle(self.angle + v / r) }
+ }
+
+}
+
+/// Point on a [`Cylinder`]
+#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
+pub enum Point {
+ Top(CapPoint),
+ Bottom(CapPoint),
+ Side(SidePoint),
+}
+
+/// Face on a [`Cylinder`]
+#[derive(Copy, Clone, Debug, Eq, PartialEq, Serialize_repr, Deserialize_repr)]
+#[repr(u8)]
+pub enum Face {
+ Top,
+ Bottom,
+ Side,
+}
+
+#[derive(Clone, Debug, PartialEq)]
+pub struct OnCylinder<'a> {
+ cylinder : &'a Cylinder,
+ point : Point,
+}
+
+
+/// Cylinder configuration
+#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
+pub struct CylinderConfig {
+ pub newton_iters : usize,
+}
+
+impl Default for CylinderConfig {
+ fn default() -> Self {
+ CylinderConfig { newton_iters : 10 }
+ }
+}
+
+/// A cylinder
+#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
+pub struct Cylinder {
+ /// Radius of the cylinder
+ pub radius : f64,
+ /// Height of the cylinder
+ pub height : f64,
+ /// Configuration for numerical methods
+ pub config : CylinderConfig
+}
+
+
+impl std::fmt::Display for Face {
+ fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+ use Face::*;
+ let s = match *self {
+ Top => "Top",
+ Bottom => "Bottom",
+ Side => "Side",
+ };
+ write!(f, "{}", s)
+ }
+}
+
+impl Point {
+ fn face(&self) -> Face {
+ match *self {
+ Point::Top(..) => Face::Top,
+ Point::Bottom(_) => Face::Bottom,
+ Point::Side(_) => Face::Side,
+ }
+ }
+}
+
+impl<'a> FacedManifoldPoint for OnCylinder<'a> {
+ type Face = Face;
+ /// Returns the face of this point.
+ fn face(&self) -> Face {
+ self.point.face()
+ }
+}
+
+// Tangent vector
+type Tangent = Loc<f64, 2>;
+
+#[inline]
+fn best_tangent<I>(tangents : I) -> (Tangent, f64)
+where I : IntoIterator<Item = Tangent> {
+ tangents.into_iter()
+ .map(|t| (t, t.norm(L2)))
+ .reduce(|a@(_, la), b@(_, lb)| if la < lb { a } else { b })
+ .unwrap()
+}
+
+/// Swap the elements of a two-tuple
+#[inline]
+fn swap<A>((a, b) : (A, A)) -> (A, A) {
+ (b, a)
+}
+
+#[inline]
+fn indistinguishable(a : f64, b : f64) -> bool {
+ a > b - f64::EPSILON && a < b + f64::EPSILON
+}
+
+impl Cylinder {
+ /// Return the cylindrical coordinates of `point` on this cylinder
+ fn cyl_coords(&self, point : &Point) -> CylCoords {
+ match *point {
+ Point::Top(cap) => { cap.cyl_coords(self.top_z()) },
+ Point::Bottom(cap) => { cap.cyl_coords(self.bottom_z()) },
+ Point::Side(side) => { side.cyl_coords(self.radius) },
+ }
+ }
+
+ #[inline]
+ pub fn top_z(&self) -> f64 {
+ self.height / 2.0
+ }
+
+ #[inline]
+ pub fn bottom_z(&self) -> f64 {
+ -self.height / 2.0
+ }
+
+ /// Find angle where a geodesic from `side` to `(cap, z)` crosses the cap edge.
+ ///
+ /// Uses `newton_sym1x1`.
+ fn side_cap_crossing(
+ &self,
+ side : &SidePoint,
+ cap : &CapPoint, z : f64, // `z` is the z coordinate of cap
+ ) -> Angle {
+ let &SidePoint { z : z_1, angle : φ_1 } = side;
+ let &CapPoint { r : r_2, angle : φ_2 } = cap;
+ assert_ne!(z, z_1);
+ let d_1 = z - z_1;
+ let d2_1 = d_1 * d_1;
+ let r = self.radius;
+ let r2 = r * r;
+ let r2_2 = r_2 * r_2;
+ let rr_2 = r * r_2;
+
+ let g = |α_1 : f64| {
+ let ψ = φ_2 - φ_1 - α_1;
+ let ψ_cos = ψ.cos();
+ let ψ_sin = ψ.sin();
+ let ψ_sin2 = ψ_sin * ψ_sin;
+ let ψ_cos2 = ψ_cos * ψ_cos;
+ let α2_1 = α_1 * α_1;
+ let c = d2_1 + r2 * α2_1;
+ let e = r2 + r2_2 - 2.0 * rr_2 * ψ_cos;
+ let g = r2 * α_1 / c.sqrt() - rr_2 * ψ_sin / e.sqrt();
+ let h = r2 * d2_1 / c.powf(3.0/2.0)
+ + r * r_2 * ((r2 + r2_2) * ψ_cos - rr_2 * (ψ_sin2 - 2.0 * ψ_cos2))
+ / e.powf(3.0/2.0);
+ (h, g)
+ };
+
+ let α_1 = newton_sym1x1(g, 0.0, self.config.newton_iters);
+ normalise_angle(φ_1 + α_1)
+
+ }
+
+ /// Find angles where the geodesic passing through a cap at height `z` from `side1` to `side2`
+ /// crosses the cap edge. **Panics if `side2.angle < side1.angle`.**
+ ///
+ /// Uses `newton_sym2x2`.
+ fn side_cap_side_crossing(
+ &self,
+ side1 : &SidePoint,
+ z : f64,
+ side2 : &SidePoint
+ ) -> (Angle, Angle) {
+ let &SidePoint { z : z_1, angle : φ_1 } = side1;
+ let &SidePoint { z : z_2, angle : φ_2 } = side2;
+ assert!(φ_2 >= φ_1);
+ assert_ne!(z_1, z);
+ assert_ne!(z_2, z);
+ let r = self.radius;
+ let r2 = r * r;
+ let d_1 = z - z_1;
+ let d_2 = z - z_2;
+ let d2_1 = d_1 * d_1;
+ let d2_2 = d_2 * d_2;
+ let g = |α_1 : f64, α_2 : f64| {
+ let α2_1 = α_1 * α_1;
+ let α2_2 = α_2 * α_2;
+ let ψ = (α_1 + α_2 + φ_1 - φ_2) / 2.0;
+ let ψ_sin = ψ.sin();
+ let ψ_cos = ψ.cos();
+ let c_1 = d2_1 + r2 * α2_1;
+ let c_2 = d2_2 + r2 * α2_2;
+ let g_1 = r2 * α_1 / c_1.sqrt() - r * ψ_cos;
+ let g_2 = r2 * α_2 / c_2.sqrt() - r * ψ_cos;
+ let h_12 = (r / 2.0) * ψ_sin;
+ let h_11 = r2 * d2_1 / c_1.powf(3.0 / 2.0) + h_12;
+ let h_22 = r2 * d2_2 / c_2.powf(3.0 / 2.0) + h_12;
+ ([h_11, h_12, h_22], [g_1, g_2])
+ };
+
+ let [α_1, α_2] = newton_sym2x2(g, [0.0, 0.0], self.config.newton_iters);
+ (normalise_angle(φ_1 + α_1), normalise_angle(φ_2 + α_2))
+
+ }
+
+ /// Find angles where the geodesic passing through the side from `cap1` at height `z_1`
+ /// to `cap2` at height `z_2` crosses the cap edges.
+ /// **Panics if `cap2.angle < cap1.angle`.**
+ ///
+ /// Uses `newton_sym2x2`.
+ fn cap_side_cap_crossing(
+ &self,
+ cap1 : &CapPoint, z_1 : f64,
+ cap2 : &CapPoint, z_2 : f64,
+ init_by_cap2 : bool
+ ) -> (Angle, Angle) {
+ let r = self.radius;
+ let &CapPoint { r : r_1, angle : φ_1 } = cap1;
+ let &CapPoint { r : r_2, angle : φ_2 } = cap2;
+ assert!(φ_2 >= φ_1);
+ assert_ne!(r_1, r);
+ assert_ne!(r_2, r);
+ assert_ne!(z_2, z_1);
+ if r_1 == 0.0 && r_2 == 0.0 {
+ // Singular case: both points are in the middle of the caps.
+ return (φ_1, φ_1)
+ }
+ let r2 = r * r;
+ let d = (z_2 - z_1).abs();
+ let d2 = d * d;
+ let r2_1 = r_1 * r_1;
+ let r2_2 = r_2 * r_2;
+ let rr_1 = r * r_1;
+ let rr_2 = r * r_2;
+ let f = |α_1 : f64, α_2 : f64| {
+ let cos_α1 = α_1.cos();
+ let sin_α1 = α_1.sin();
+ let cos_α2 = α_2.cos();
+ let sin_α2 = α_2.sin();
+ //let cos2_α1 = cos_α1 * cos_α1;
+ let sin2_α1 = sin_α1 * sin_α1;
+ //let cos2_α2 = cos_α2 * cos_α2;
+ let sin2_α2 = sin_α2 * sin_α2;
+ let ψ = φ_2 - φ_1 - α_1 - α_2;
+ let ψ2 = ψ * ψ;
+ let ψ2r2 = ψ2 * r2;
+ //let r4 = r2 * r2;
+ let b = d2 + ψ2r2;
+ let c = r2 * ψ / b.sqrt();
+ let e_1 = r2 + r2_1 - 2.0 * rr_1 * cos_α1;
+ let e_2 = r2 + r2_2 - 2.0 * rr_2 * cos_α2;
+ let g_1 = rr_1 * sin_α1 / e_1.sqrt() - c;
+ let g_2 = rr_2 * sin_α2 / e_2.sqrt() - c;
+ let h_12 = r2 * (1.0 - ψ2r2 / b) / b.sqrt();
+ // let h_11 = rr_1 * ( (r2 + r2_1) * cos_α1 - rr_1 * ( sin2_α1 + 2.0 * cos2_α1) )
+ // / e_1.powf(3.0/2.0) + h_12;
+ // let h_22 = rr_2 * ( (r2 + r2_2) * cos_α2 - rr_2 * ( sin2_α2 + 2.0 * cos2_α2) )
+ // / e_2.powf(3.0/2.0) + h_12;
+ // let h_11 = rr_1 * cos_α1 / e_1.sqrt() - rr_1*rr_1 * sin2_α1 / e_1.powf(3.0/2.0) + h_12;
+ // let h_22 = rr_2 * cos_α2 / e_2.sqrt() - rr_2*rr_2 * sin2_α2 / e_2.powf(3.0/2.0) + h_12;
+ let h_11 = rr_1 * (cos_α1 - rr_1 * sin2_α1 / e_1) / e_1.sqrt() + h_12;
+ let h_22 = rr_2 * (cos_α2 - rr_2 * sin2_α2 / e_2) / e_2.sqrt() + h_12;
+ ([h_11, h_12, h_22], [g_1, g_2])
+ };
+
+ let α_init = if init_by_cap2 {
+ [φ_2 - φ_1, 0.0]
+ } else {
+ [0.0, φ_2 - φ_1]
+ };
+ let [α_1, α_2] = newton_sym2x2(f, α_init, self.config.newton_iters);
+ (normalise_angle(φ_1 + α_1), normalise_angle(φ_2 - α_2))
+ }
+
+ fn cap_side_log(
+ &self,
+ cap : &CapPoint, (z, top) : (f64, bool),
+ side : &SidePoint
+ ) -> Tangent {
+ let r = self.radius;
+ if indistinguishable(side.z, z) {
+ // Degenerate case
+ let capedge = CapPoint{ angle : side.angle, r };
+ cap.log(&capedge)
+ } else if indistinguishable(r, cap.r)
+ && anglediff(side.angle, cap.angle).abs() < f64::PI/2.0 {
+ // Degenerate case
+ let sideedge = SidePoint{ angle : cap.angle, z};
+ cap.tangent_from_side(top, sideedge.log(r, side))
+ } else {
+ let φ = self.side_cap_crossing(side, cap, z);
+ let capedge = CapPoint{ angle : φ, r };
+ let sideedge = SidePoint{ angle : φ, z };
+ let t1 = cap.log(&capedge);
+ let t2 = sideedge.log(r, side);
+ // Either option should give the same result, but the first one avoids division.
+ t1 + capedge.tangent_from_side(top, t2)
+ // let n = t1.norm(L2);
+ // (t1/n)*(n + t2.norm(L2))
+ }
+ }
+
+ fn side_cap_log(
+ &self,
+ side : &SidePoint,
+ cap : &CapPoint, (z, top) : (f64, bool),
+ ) -> Tangent {
+ let r = self.radius;
+ if indistinguishable(side.z, z) {
+ // Degenerate case
+ let capedge = CapPoint{ angle : side.angle, r };
+ capedge.tangent_to_side(top, capedge.log(cap))
+ } else if indistinguishable(r, cap.r)
+ && anglediff(side.angle, cap.angle).abs() < f64::PI/2.0 {
+ // Degenerate case
+ side.log(r, &SidePoint{ z, angle : cap.angle })
+ } else {
+ let φ = self.side_cap_crossing(side, cap, z);
+ let capedge = CapPoint{ angle : φ, r };
+ let sideedge = SidePoint{ angle : φ, z };
+ let t1 = side.log(r, &sideedge);
+ let t2 = capedge.log(cap);
+ // Either option should give the same result, but the first one avoids division.
+ t1 + capedge.tangent_to_side(top, t2)
+ // let n = t1.norm(L2);
+ // (t1/n)*(n + t2.norm(L2))
+ }
+ }
+
+ fn side_cap_side_log(
+ &self,
+ side1 : &SidePoint,
+ (z, top) : (f64, bool),
+ side2 : &SidePoint
+ ) -> Tangent {
+ let r = self.radius;
+ if indistinguishable(side1.z, z) {
+ // Degenerate case
+ let capedge1 = CapPoint{ angle : side1.angle, r };
+ capedge1.tangent_to_side(top, self.cap_side_log(&capedge1, (z, top), side2))
+ } else if indistinguishable(side2.z, z) {
+ // Degenerate case
+ let capedge2 = CapPoint{ angle : side2.angle, r };
+ self.side_cap_log(side1, &capedge2, (z, top))
+ } else {
+ let (φ1, φ2) = if side2.angle >= side1.angle {
+ self.side_cap_side_crossing(side1, z, side2)
+ } else {
+ swap(self.side_cap_side_crossing(side2, z, side1))
+ };
+ let capedge1 = CapPoint{ angle : φ1, r };
+ let sideedge1 = SidePoint{ angle : φ1, z };
+ let capedge2 = CapPoint{ angle : φ2, r };
+ let sideedge2 = SidePoint{ angle : φ2, z };
+ let t1 = side1.log(r, &sideedge1);
+ let t2 = capedge1.log(&capedge2);
+ let t3 = sideedge2.log(r, &side2);
+ // Any option should give the same result, but the first one avoids division.
+ // t1 + capedge1.tangent_to_side(top, t2 + capedge2.tangent_from_side(top, t3))
+ //
+ // let n = t2.norm(L2);
+ // t1 + capedge1.tangent_to_side(top, (t2/n)*(n + t3.norm(L2)))
+ //
+ let n = t1.norm(L2);
+ (t1/n)*(n + t2.norm(L2) + t3.norm(L2))
+ //
+ // let n = t1.norm(L2);
+ // let t23 = t2 + capedge2.tangent_from_side(top, t3);
+ // (t1/n)*(n + t23.norm(L2))
+ }
+ }
+
+ fn cap_side_cap_log(
+ &self,
+ cap1 : &CapPoint, (z1, top1) : (f64, bool),
+ cap2 : &CapPoint, (z2, top2) : (f64, bool),
+ init_by_cap2 : bool,
+ ) -> Tangent {
+ let r = self.radius;
+ if indistinguishable(cap1.r, r) {
+ // Degenerate case
+ let sideedge1 = SidePoint{ angle : cap1.angle, z : z1 };
+ cap1.tangent_from_side(top1, self.side_cap_log(&sideedge1, cap2, (z2, top2)))
+ } else if indistinguishable(cap2.r, r) {
+ // Degenerate case
+ let sideedge2 = SidePoint{ angle : cap2.angle, z : z2 };
+ self.cap_side_log(cap1, (z1, top1), &sideedge2)
+ } else {
+ let (φ1, φ2) = if cap2.angle >= cap1.angle {
+ self.cap_side_cap_crossing(cap1, z1, cap2, z2, init_by_cap2)
+ } else {
+ swap(self.cap_side_cap_crossing(cap2, z2, cap1, z1, !init_by_cap2))
+ };
+ let sideedge1 = SidePoint{ angle : φ1, z : z1 };
+ let capedge1 = CapPoint{ angle : φ1, r };
+ let sideedge2 = SidePoint{ angle : φ2, z : z2};
+ let capedge2 = CapPoint{ angle : φ2, r };
+ let t1 = cap1.log(&capedge1);
+ let t2 = sideedge1.log(r, &sideedge2);
+ let t3 = capedge2.log(cap2);
+ // Either option should give the same result, but the first one avoids division.
+ t1 + capedge1.tangent_from_side(top1, t2 + capedge2.tangent_to_side(top2, t3))
+ //let n = t1.norm(L2);
+ //(t1/n)*(n + t2.norm(L2) + t3.norm(L2))
+ }
+ }
+
+ /// Calculates both the logarithmic map and distance to another point
+ fn log_dist(&self, source : &Point, destination : &Point) -> (Tangent, f64) {
+ use Point::*;
+ match (source, destination) {
+ (Top(cap1), Top(cap2)) => {
+ best_tangent([cap1.log(cap2)])
+ },
+ (Bottom(cap1), Bottom(cap2)) => {
+ best_tangent([cap1.log(cap2)])
+ },
+ (Bottom(cap), Side(side)) => {
+ best_tangent([self.cap_side_log(cap, (self.bottom_z(), false), side)])
+ },
+ (Top(cap), Side(side)) => {
+ best_tangent([self.cap_side_log(cap, (self.top_z(), true), side)])
+ },
+ (Side(side), Bottom(cap)) => {
+ best_tangent([self.side_cap_log(side, cap, (self.bottom_z(), false))])
+ },
+ (Side(side), Top(cap)) => {
+ best_tangent([self.side_cap_log(side, cap, (self.top_z(), true))])
+ },
+ (Side(side1), Side(side2)) => {
+ best_tangent([
+ side1.log(self.radius, side2),
+ self.side_cap_side_log(side1, (self.top_z(), true), side2),
+ self.side_cap_side_log(side1, (self.bottom_z(), false), side2),
+ ])
+ },
+ (Top(cap1), Bottom(cap2)) => {
+ best_tangent([
+ // We try a few possible initialisations for Newton
+ self.cap_side_cap_log(
+ cap1, (self.top_z(), true),
+ cap2, (self.bottom_z(), false),
+ false
+ ),
+ self.cap_side_cap_log(
+ cap1, (self.top_z(), true),
+ cap2, (self.bottom_z(), false),
+ true
+ ),
+ ])
+ },
+ (Bottom(cap1), Top(cap2)) => {
+ best_tangent([
+ // We try a few possible initialisations for Newton
+ self.cap_side_cap_log(
+ cap1, (self.bottom_z(), false),
+ cap2, (self.top_z(), true),
+ false
+ ),
+ self.cap_side_cap_log(
+ cap1, (self.bottom_z(), false),
+ cap2, (self.top_z(), true),
+ true
+ ),
+ ])
+ },
+ }
+ }
+
+ #[allow(unreachable_code)]
+ #[allow(unused_variables)]
+ fn partial_exp(&self, point : Point, t : Tangent) -> (Point, Option<Tangent>) {
+ match point {
+ Point::Top(cap) => {
+ let (cap_new, t_new_basis) = cap.partial_exp(self.radius, &t);
+ match t_new_basis {
+ None => (Point::Top(cap_new), None),
+ Some(t_new) => {
+ let side_new = SidePoint{ angle : cap_new.angle, z : self.top_z() };
+ (Point::Side(side_new), Some(cap_new.tangent_to_side(true, t_new)))
+ }
+ }
+ },
+ Point::Bottom(cap) => {
+ let (cap_new, t_new_basis) = cap.partial_exp(self.radius, &t);
+ match t_new_basis {
+ None => (Point::Bottom(cap_new), None),
+ Some(t_new) => {
+ let side_new = SidePoint{ angle : cap_new.angle, z : self.bottom_z() };
+ (Point::Side(side_new), Some(cap_new.tangent_to_side(false, t_new)))
+ }
+ }
+ },
+ Point::Side(side) => {
+ let lims = (self.bottom_z(), self.top_z());
+ let (side_new, t_new_basis) = side.partial_exp(self.radius, lims, &t);
+ match t_new_basis {
+ None => (Point::Side(side_new), None),
+ Some(t_new) => {
+ if side_new.z >= self.top_z() - f64::EPSILON {
+ let cap_new = CapPoint { angle : side_new.angle, r : self.radius };
+ (Point::Top(cap_new), Some(cap_new.tangent_from_side(true, t_new)))
+ } else {
+ let cap_new = CapPoint { angle : side_new.angle, r : self.radius };
+ (Point::Bottom(cap_new), Some(cap_new.tangent_from_side(false, t_new)))
+ }
+ }
+ }
+ }
+ }
+ }
+
+ fn exp(&self, point : &Point, tangent : &Tangent) -> Point {
+ let mut p = *point;
+ let mut t = *tangent;
+ loop {
+ (p, t) = match self.partial_exp(p, t) {
+ (p, None) => break p,
+ (p, Some(t)) => (p, t),
+ };
+ }
+ }
+
+ /// Check that `point` has valid coordinates, and normalise angles
+ pub fn normalise(&self, point : Point) -> Option<Point> {
+ match point {
+ Point::Side(side) => {
+ let a = self.bottom_z();
+ let b = self.top_z();
+ (a <= side.z && side.z <= b).then(|| {
+ Point::Side(SidePoint{ angle : normalise_angle(side.angle), .. side })
+ })
+ },
+ Point::Bottom(cap) => {
+ (cap.r <= self.radius).then(|| {
+ Point::Bottom(CapPoint{ angle : normalise_angle(cap.angle), .. cap })
+ })
+ },
+ Point::Top(cap) => {
+ (cap.r <= self.radius).then(|| {
+ Point::Top(CapPoint{ angle : normalise_angle(cap.angle), .. cap })
+ })
+ },
+ }
+ }
+
+ /// Convert `p` into a a point associated with the cylinder.
+ ///
+ /// May panic if the coordinates are invalid.
+ pub fn point_on(&self, point : Point) -> OnCylinder<'_> {
+ match self.normalise(point) {
+ None => panic!("{point:?} not on cylinder {self:?}"),
+ Some(point) => OnCylinder { cylinder : self, point }
+ }
+ }
+
+ /// Convert `p` into a a point on side associated with the cylinder.
+ ///
+ /// May panic if the coordinates are invalid.
+ pub fn point_on_side(&self, side : SidePoint) -> OnCylinder<'_> {
+ self.point_on(Point::Side(side))
+ }
+
+ /// Convert `p` into a a point on top associated with the cylinder.
+ ///
+ /// May panic if the coordinates are invalid.
+ pub fn point_on_top(&self, cap : CapPoint) -> OnCylinder<'_> {
+ self.point_on(Point::Top(cap))
+ }
+
+ /// Convert `p` into a a point on bottom associated with the cylinder.
+ ///
+ /// May panic if the coordinates are invalid.
+ pub fn point_on_bottom(&self, cap : CapPoint) -> OnCylinder<'_> {
+ self.point_on(Point::Bottom(cap))
+ }
+
+ /// Convert `p` into a a point on side associated with the cylinder.
+ ///
+ /// May panic if the coordinates are invalid.
+ pub fn on_side(&self, angle : Angle, z : f64) -> OnCylinder<'_> {
+ self.point_on_side(SidePoint{ angle, z })
+ }
+
+ /// Convert `p` into a a point on top associated with the cylinder.
+ ///
+ /// May panic if the coordinates are invalid.
+ pub fn on_top(&self, angle : Angle, r : f64) -> OnCylinder<'_> {
+ self.point_on_top(CapPoint{ angle, r })
+ }
+
+ /// Convert `p` into a a point on bottom associated with the cylinder.
+ ///
+ /// May panic if the coordinates are invalid.
+ pub fn on_bottom(&self, angle : Angle, r : f64) -> OnCylinder<'_> {
+ self.point_on_bottom(CapPoint{ angle, r })
+ }
+}
+
+impl<'a> OnCylinder<'a> {
+ /// Return the cylindrical coordinates of this point
+ pub fn cyl_coords(&self) -> CylCoords {
+ self.cylinder.cyl_coords(&self.point)
+ }
+}
+
+impl<'a> EmbeddedManifoldPoint for OnCylinder<'a> {
+ type EmbeddedCoords = Loc<f64, 3>;
+
+ /// Get embedded 3D coordinates
+ fn embedded_coords(&self) -> Loc<f64, 3> {
+ self.cyl_coords().to_cartesian()
+ }
+}
+
+impl<'a> ManifoldPoint for OnCylinder<'a> {
+ type Tangent = Tangent;
+
+ fn exp(self, tangent : &Self::Tangent) -> Self {
+ let cylinder = self.cylinder;
+ let point = cylinder.exp(&self.point, tangent);
+ OnCylinder { cylinder, point }
+ }
+
+ fn log(&self, other : &Self) -> Self::Tangent {
+ assert!(self.cylinder == other.cylinder);
+ self.cylinder.log_dist(&self.point, &other.point).0
+ }
+
+ fn dist_to(&self, other : &Self) -> f64 {
+ assert!(self.cylinder == other.cylinder);
+ self.cylinder.log_dist(&self.point, &other.point).1
+ }
+
+ fn tangent_origin(&self) -> Self::Tangent {
+ Loc([0.0, 0.0])
+ }
+}
+
+#[cfg(test)]
+mod tests {
+ use super::*;
+
+ static CYL : Cylinder = Cylinder {
+ radius : 1.0,
+ height : 1.0,
+ config : CylinderConfig { newton_iters : 20 },
+ };
+
+ fn check_distance(distance : f64, expected : f64) {
+ let tol = 1e-10;
+ assert!(
+ (distance-expected).abs() < tol,
+ "distance = {distance}, expected = {expected}"
+ );
+ }
+
+ // fn check_distance_less(distance : f64, expected : f64) {
+ // let tol = 1e-10;
+ // assert!(
+ // distance < expected + tol,
+ // "distance = {distance}, expected = {expected}"
+ // );
+ // }
+
+ #[test]
+ fn intra_cap_log_dist() {
+ let π = f64::PI;
+ let p1 = CYL.on_top(0.0, 0.5);
+ let p2 = CYL.on_top(π, 0.5);
+ let p3 = CYL.on_top(π/2.0, 0.5);
+
+ check_distance(p1.dist_to(&p2), 1.0);
+ check_distance(p2.dist_to(&p3), 0.5_f64.sqrt());
+ check_distance(p3.dist_to(&p1), 0.5_f64.sqrt());
+ }
+
+ #[test]
+ fn intra_side_log_dist() {
+ let π = f64::PI;
+ let p1 = CYL.on_side(0.0, 0.0);
+ let p2 = CYL.on_side(0.0, 0.4);
+ let p3 = CYL.on_side(π/2.0, 0.0);
+
+ check_distance(p1.dist_to(&p2), 0.4);
+ check_distance(p1.dist_to(&p3), π/2.0*CYL.radius);
+ }
+
+ #[test]
+ fn intra_side_over_cap_log_dist() {
+ let π = f64::PI;
+ let off = 0.05;
+ let z = CYL.top_z() - off;
+ let p1 = CYL.on_side(0.0, z);
+ let p2 = CYL.on_side(π, z);
+
+ check_distance(p1.dist_to(&p2), 2.0 * (CYL.radius + off));
+ }
+
+ #[test]
+ fn top_bottom_log_dist() {
+ let π = f64::PI;
+ let p1 = CYL.on_top(0.0, 0.0);
+ let p2 = CYL.on_bottom(0.0, 0.0);
+
+ check_distance(p1.dist_to(&p2), 2.0 * CYL.radius + CYL.height);
+
+ let p1 = CYL.on_top(0.0, CYL.radius / 2.0);
+ let p2 = CYL.on_bottom(0.0, CYL.radius / 2.0);
+ let p3 = CYL.on_bottom(π, CYL.radius / 2.0);
+
+ check_distance(p1.dist_to(&p2), CYL.radius + CYL.height);
+ check_distance(p1.dist_to(&p3), 2.0 * CYL.radius + CYL.height);
+ }
+
+ #[test]
+ fn top_side_log_dist() {
+ let p1 = CYL.on_top(0.0, 0.0);
+ let p2 = CYL.on_side(0.0, 0.0);
+
+ check_distance(p1.dist_to(&p2), CYL.radius + CYL.height / 2.0);
+ }
+}