src/cylinder.rs

/*!
Implementation of the surface of a 3D cylinder as a [`ManifoldPoint`].
*/

use alg_tools::euclidean::{Euclidean, Dot};
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 {
    /// Convert to cartesian coordinates.
    #[inline]
    pub fn to_cartesian(&self) -> Loc<f64, 3> {
        let &CylCoords{r, angle, z} = self;
        [r * angle.cos(), r * angle.sin(), z].into()
    }

    /// Form cylindrical coordinates from cartesian coordinates.
    #[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
}

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 to_cartesian(&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 = normalise_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.to_cartesian() - self.to_cartesian()
    }

    #[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>) {
        // |p + s t| <= r
        // |p|^2 + s^2 |t|^2 + 2s<p,t> = r^2
        let p = self.to_cartesian();
        let np2 = p.norm2_squared();
        let nt2 = t.norm2_squared();
        let r2 = r * r;
        let d = p.dot(t);
        assert!(np2 <= r2 + f64::EPSILON, "‖{p}‖ = {} > {r}", np2.sqrt());
        let s = (-d + (d*d + nt2 * (r2 - np2)).sqrt()) / nt2;
        if s < 1.0 {
            (Self::from_cartesian(p + s * t), Some((1.0-s) * t))
        } else {
            (Self::from_cartesian(p + t), None)
        }
    }

    #[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.
            t.rotate(self.angle+f64::PI/2.0)
        } else {
            // The angle is such that up would be rotated to self.angle, clockwise
            t.reflect_y().rotate(self.angle+f64::PI/2.0)
        }
    }

    #[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
            t.rotate(-self.angle-f64::PI/2.0)
        } else {
            // The angle is such that self.angle would be rotated to up, counterclockwise
            t.rotate(-self.angle-f64::PI/2.0).reflect_y()
        }
    }
}

/// Coordinates on a side
#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
pub struct SidePoint {
    pub z : f64,
    pub angle : Angle
}

/// Calculates the difference between two angles, normalised to [–π, π].
#[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 * π + α
        }
    }
}

/// Normalises an angle to [0, 2π].
#[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 some [`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,
}

/// Point on a specific [`Cylinder`]
#[derive(Clone, Debug, PartialEq)]
pub struct OnCylinder<'a> {
    /// Face and coordinates of the point
    point : Point,
    /// The specific cylinder the `point` lies on.
    cylinder : &'a Cylinder,
}


/// Cylinder configuration
#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
pub struct CylinderConfig {
    /// Number of Newton iterates two take to solve geodesics.
    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 {
    /// Returns the face of the 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>;

/// Goes through list of potential tangents (corresponding to several geodesics taking
/// different paths), and retuns the shortest tangent vector and the corresponding length.
#[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)
}

/// Indicates whether the `a` and `b` are a distance of less than [`f64::EPSILON`].
#[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) },
        }
    }

    /// Returns the z coordinate of the top (cap) of the cylinder.
    #[inline]
    pub fn top_z(&self) -> f64 {
        self.height / 2.0
    }

    /// Returns the z coordinate of the bottom (cap) of the cylinder.
    #[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))
    }

    /// Calculates the log between points on a `cap` and a `side` of the cylinder, in this direction.
    /// The `z` coordinate of the cap and an indication whether it is a `top` or bottom cap must
    /// also be given.
    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.
            // The first one avoids division, but seems numerically more unstable due to
            // sines and cosines.
            //t1 + capedge.tangent_from_side(top, t2)
            let n = t1.norm(L2);
            if n < f64::EPSILON {
                capedge.tangent_from_side(top, t2)
            } else {
                (t1/n)*(n + t2.norm(L2))
            }
        }
    }

    /// Calculates the log between points on a `side` and a `cap` of the cylinder, in this direction.
    /// The `z` coordinate of the cap and an indication whether it is a `top` or bottom cap must
    /// also be given.
    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.
            // The first one avoids division, but seems numerically more unstable due to
            // sines and cosines.
            // t1 + capedge.tangent_to_side(top, t2)
            let n = t1.norm(L2);
            if n < f64::EPSILON {
                capedge.tangent_to_side(top, t2)
            } else {
                (t1/n)*(n + t2.norm(L2))
            }
        }
    }

    /// Calculates the log between points on two sides of the cylinder, assuming the corresonding
    /// geodesic crosses a cap at height `z`. The `top` parameter indicates whether this is a
    /// top cap.
    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);
            // Either option should give the same result.
            // The first one avoids division, but seems numerically more unstable due to
            // sines and cosines.
            //
            // t1 + capedge1.tangent_to_side(top, t2 + capedge2.tangent_from_side(top, t3))
            let n = t1.norm(L2);
            if n < f64::EPSILON {
                let n2 = t2.norm(L2);
                capedge1.tangent_to_side(top,
                    if n2 < f64::EPSILON {
                        capedge2.tangent_from_side(top, t3)
                    } else {
                        (t2/n2)*(n2 + t3.norm(L2))
                    }
                )
            } else {
                (t1/n)*(n + t2.norm(L2) + t3.norm(L2))
            }
        }
    }

    /// Calculates the log between points on two caps of the cylinder, assuming the corresonding
    /// geodesic crosses the side. The heights of the caps and indications whether they are
    /// top caps, must also be given.
    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.
            // The first one avoids division, but seems numerically more unstable due to
            // sines and cosines.
            // t1 + capedge1.tangent_from_side(top1, t2 + capedge2.tangent_to_side(top2, t3))
            let n = t1.norm(L2);
            if n < f64::EPSILON {
                let n2 = t2.norm(L2);
                capedge1.tangent_from_side(top1,
                    if n2 < f64::EPSILON {
                        capedge2.tangent_to_side(top2, t3)
                    } else {
                        (t2/n2)*(n2 + t3.norm(L2))
                    }
                )
            } else {
                (t1/n)*(n + t2.norm(L2) + t3.norm(L2))
            }
        }
    }

    /// Calculates both the logarithmic map and distance between two points.
    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
                    ),
                ])
            },
        }
    }

    /// Calculates the exponential map of `point` in the direction `t` until the next edge.
    /// If `t` was fully exhausted before reaching an edge, the second return value is [`None`],
    /// otherwise it is a [`Some`] of the remaining tangent, translated at the returned point.
    #[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 if side_new.z <= self.bottom_z() + f64::EPSILON  {
                            let cap_new = CapPoint { angle : side_new.angle, r : self.radius };
                            (Point::Bottom(cap_new), Some(cap_new.tangent_from_side(false, t_new)))
                        } else {
                            (Point::Side(side_new), None)
                        }
                    }
                }
            }
        }
    }

    /// Calculates the exponential map from `point` in the direction of `tangent`.
    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 TOL : f64 = 1e-7;

    macro_rules! check_distance {
        ($distance : expr, $expected : expr) => {
            assert!(
                ($distance-$expected).abs() < TOL,
                "{} = {}, expected = {}",
                stringify!($distance),
                $distance,
                $expected,
            )
        }
    }

    macro_rules! check_vec_eq {
        ($left : expr, $right : expr) => {
            let err = ($left-$right).norm(L2);
            if err > TOL {
                panic!("{:?} != {:?} [error {err}]", $left, $right);
            }
        }
    }

    macro_rules! check_point_eq {
        ($left : expr, $right : expr) => {
            match ($left, $right) {
                (Point::Top(a), Point::Top(b)) | (Point::Bottom(a), Point::Bottom(b))=> {
                    if (a.angle-b.angle).abs() > TOL || (a.r-b.r).abs() > TOL {
                        panic!("{:?} != {:?}", a, b)
                    }
                },
                (Point::Side(a), Point::Side(b)) => {
                    if (a.angle-b.angle).abs() > TOL || (a.z-b.z).abs() > TOL {
                        panic!("{:?} != {:?}", a, b)
                    }
                },
                (a, b) => {
                    panic!("{:?} != {:?}", a, b)
                }
            }
        }
    }

    static CYL : Cylinder = Cylinder {
        radius : 1.0,
        height : 1.0,
        config : CylinderConfig { newton_iters : 20 },
    };

    /// Tests for correct distance calculation beween two points on the same cap.
    #[test]
    fn intra_cap_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());
    }

    /// Tests for correct distance calculation beween two points on the side.
    #[test]
    fn intra_side_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);
    }

    /// Tests for correct distance calculation beween two points on the side, when the corresponding
    /// minimal geodesic has to cross a cap.
    #[test]
    fn intra_side_over_cap_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));
    }

    /// Tests for correct distance calculation between points on top and bottom caps.
    #[test]
    fn top_bottom_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 for correct distance calculation between points on the side and a cap.
    #[test]
    fn top_side_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);
    }

    /// Tests for correct tangent calculation from a cap to the side.
    #[test]
    fn cap_side_partial_exp() {
        let π = f64::PI;
        let angles = [0.0, π/2.0, π*5.0/6.0, π*7.0/5.0];

        for φ in angles {
            let p = CYL.on_top(φ, CYL.radius / 2.0);
            let q_target = CYL.on_side(φ, CYL.height / 2.0);
            let t = Loc([CYL.radius, 0.0]).rotate(φ);
            let t_target = Loc([0.0, -CYL.radius / 2.0]);
            let (q, t_left) = CYL.partial_exp(p.point, t);
            check_point_eq!(q, q_target.point);
            if let Some(t_left_) = t_left {
                check_vec_eq!(t_left_, t_target);
            } else {
                panic!("No tangent left");
            }
        }

        for φ in angles {
            let p = CYL.on_top(φ + π/2.0, CYL.radius);
            let q_target = CYL.on_side(φ, CYL.height / 2.0);
            let t = 2.0 * Loc([CYL.radius, -CYL.radius]).rotate(φ);
            let t_target = Loc([-CYL.radius, -CYL.radius]);
            let (q, t_left) = CYL.partial_exp(p.point, t);
            check_point_eq!(q, q_target.point);
            if let Some(t_left_) = t_left {
                check_vec_eq!(t_left_, t_target);
            } else {
                panic!("No tangent left");
            }
        }
    }

    /// Tests for correct tangent calculation from the side to a cap.
    #[test]
    fn side_top_exp() {
        let π = f64::PI;
        let angles = [0.0, π/2.0, π*5.0/6.0, π*7.0/5.0];

        for φ in angles {
            let pt = CYL.on_top(φ, CYL.radius);
            let t = Loc([0.1, 0.3]).rotate(φ);
            let b = pt.clone().exp(&t);
            let a = pt.exp(&(-t));
            check_point_eq!(a.exp(&(2.0*t)).point, b.point);
        }
    }

    /// Tests that tangent “returned” on edge from cap to top, correctly
    /// sums with a top tangent.
    #[test]
    fn cap_side_log_exp() {
        let π = f64::PI;
        let angles = [0.0, π/2.0, π*5.0/6.0, π*7.0/5.0];

        for top in [true, false] {
            for (&φ, &ψ) in angles.iter().zip(angles.iter().rev()) {
                let a = if top {
                    CYL.on_top(φ, CYL.radius/2.0)
                } else {
                    CYL.on_bottom(φ, CYL.radius/2.0)
                };
                let b = CYL.on_side(ψ, 0.0);
                let t = a.log(&b);
                let (p@Point::Side(side), Some(tpb)) = CYL.partial_exp(a.point, t) else {
                    panic!("No tangent or point not on side");
                };
                let pp = CYL.point_on(p);
                let tpb2 = pp.log(&b);
                let tap = a.log(&pp);
                check_vec_eq!(tpb, tpb2);
                if top {
                    assert!(indistinguishable(side.z, CYL.top_z()));
                } else {
                    assert!(indistinguishable(side.z, CYL.bottom_z()));
                }
                let cap = CapPoint{ angle : side.angle, r : CYL.radius };
                let tbpcap = cap.tangent_from_side(top, tpb);
                check_vec_eq!(tap + tbpcap, t);
            }
        }
    }


    /// Tests that tangent “returned” on edge from top to side, correctly
    /// sums with a side tangent.
    #[test]
    fn side_cap_log_exp() {
        let π = f64::PI;
        let angles = [0.0, π/2.0, π*5.0/6.0, π*7.0/5.0];

        for top in [true, false] {
            for (&φ, &ψ) in angles.iter().zip(angles.iter().rev()) {
                let a = CYL.on_side(ψ, 0.0);
                let b = if top {
                    CYL.on_top(φ, CYL.radius/2.0)
                } else {
                    CYL.on_bottom(φ, CYL.radius/2.0)
                };
                let t = a.log(&b);
                let (p, cap, tpb) = match CYL.partial_exp(a.point, t) {
                    (p@Point::Top(cap), Some(tpb)) if top => (p, cap, tpb),
                    (p@Point::Bottom(cap), Some(tpb)) if !top => (p, cap, tpb),
                    _ =>  panic!("No tangent or point not on correct cap"),
                };
                let pp = CYL.point_on(p);
                let tpb2 = pp.log(&b);
                let tap = a.log(&pp);
                check_vec_eq!(tpb, tpb2);
                let tbpside = cap.tangent_to_side(top, tpb);
                check_vec_eq!(tap + tbpside, t);
            }
        }
    }
}