--- a/src/cylinder.rs Wed Dec 04 23:19:46 2024 -0500
+++ b/src/cylinder.rs Thu Dec 05 16:50:08 2024 -0500
@@ -2,7 +2,7 @@
Implementation of the surface of a 3D cylinder as a [`ManifoldPoint`].
*/
-use alg_tools::euclidean::Euclidean;
+use alg_tools::euclidean::{Euclidean, Dot};
use serde_repr::*;
use serde::{Serialize, Deserialize};
use alg_tools::loc::Loc;
@@ -46,6 +46,7 @@
pub angle : Angle
}
+/// Rotate input vector by angle φ.
#[inline]
fn rotate(φ : f64, Loc([x, y]) : Loc<f64, 2>) -> Loc<f64, 2> {
let sin_φ = φ.sin();
@@ -53,6 +54,13 @@
[cos_φ * x - sin_φ * y, sin_φ * x + cos_φ * y].into()
}
+/// Mirror y coordinate of input vector.
+#[inline]
+fn ymirror(Loc([x, y]) : Loc<f64, 2>) -> Loc<f64, 2> {
+ [x, -y].into()
+}
+
+
impl CapPoint {
#[inline]
/// Convert to cylindrical coordinates given z coordinate
@@ -63,7 +71,7 @@
#[inline]
/// Convert to cylindrical coordinates given z coordinate
- fn cartesian_coords(&self) -> Loc<f64, 2> {
+ fn to_cartesian(&self) -> Loc<f64, 2> {
let CapPoint { r, angle } = *self;
[r * angle.cos(), r * angle.sin()].into()
}
@@ -74,14 +82,14 @@
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);
+ 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.cartesian_coords() - self.cartesian_coords()
+ other.to_cartesian() - self.to_cartesian()
}
#[inline]
@@ -89,13 +97,19 @@
/// 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)
+ // |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);
+ 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 {
- let p = q * r / n;
- (Self::from_cartesian(p), Some(q - p))
+ (Self::from_cartesian(p + t), None)
}
}
@@ -108,7 +122,7 @@
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)
+ rotate(self.angle+f64::PI/2.0, ymirror(t))
}
}
@@ -120,7 +134,7 @@
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)
+ ymirror(rotate(-self.angle-f64::PI/2.0, t))
}
}
}
@@ -709,9 +723,11 @@
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 {
+ } 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)
}
}
}
@@ -850,28 +866,55 @@
mod tests {
use super::*;
+ static TOL : f64 = 1e-8;
+
+ 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 },
};
- 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;
@@ -879,9 +922,9 @@
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());
+ 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]
@@ -891,8 +934,8 @@
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);
+ check_distance!(p1.dist_to(&p2), 0.4);
+ check_distance!(p1.dist_to(&p3), π/2.0*CYL.radius);
}
#[test]
@@ -903,7 +946,7 @@
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));
+ check_distance!(p1.dist_to(&p2), 2.0 * (CYL.radius + off));
}
#[test]
@@ -912,14 +955,14 @@
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);
+ 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);
+ check_distance!(p1.dist_to(&p2), CYL.radius + CYL.height);
+ check_distance!(p1.dist_to(&p3), 2.0 * CYL.radius + CYL.height);
}
#[test]
@@ -927,6 +970,121 @@
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);
+ check_distance!(p1.dist_to(&p2), CYL.radius + CYL.height / 2.0);
+ }
+
+ #[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 = rotate(φ, [CYL.radius, 0.0].into());
+ 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 * rotate(φ, Loc([CYL.radius, -CYL.radius]));
+ 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");
+ }
+ }
+ }
+
+ #[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 = rotate(φ, Loc([0.1, 0.3]));
+ 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);
+ }
+ }
}
}