Mercurial > repos > non-riemannian-opt / file comparison

comparison: src/cylinder.rs

src/cylinder.rs

changeset 38
63318d1b4f00
parent 37
d7cd14b8ccc0
child 39
3d5c8ea1522c
equal deleted inserted replaced
37:d7cd14b8ccc0 38:63318d1b4f00
1 /*! 1 /*!
2 Implementation of the surface of a 3D cylinder as a [`ManifoldPoint`]. 2 Implementation of the surface of a 3D cylinder as a [`ManifoldPoint`].
3 */ 3 */
4 4
5 use alg_tools::euclidean::Euclidean; 5 use alg_tools::euclidean::{Euclidean, Dot};
6 use serde_repr::*; 6 use serde_repr::*;
7 use serde::{Serialize, Deserialize}; 7 use serde::{Serialize, Deserialize};
8 use alg_tools::loc::Loc; 8 use alg_tools::loc::Loc;
9 use alg_tools::norms::{Norm, L2}; 9 use alg_tools::norms::{Norm, L2};
10 use alg_tools::types::Float; 10 use alg_tools::types::Float;
44 pub struct CapPoint { 44 pub struct CapPoint {
45 pub r : f64, 45 pub r : f64,
46 pub angle : Angle 46 pub angle : Angle
47 } 47 }
48 48
49 /// Rotate input vector by angle φ.
49 #[inline] 50 #[inline]
50 fn rotate(φ : f64, Loc([x, y]) : Loc<f64, 2>) -> Loc<f64, 2> { 51 fn rotate(φ : f64, Loc([x, y]) : Loc<f64, 2>) -> Loc<f64, 2> {
51 let sin_φ = φ.sin(); 52 let sin_φ = φ.sin();
52 let cos_φ = φ.cos(); 53 let cos_φ = φ.cos();
53 [cos_φ * x - sin_φ * y, sin_φ * x + cos_φ * y].into() 54 [cos_φ * x - sin_φ * y, sin_φ * x + cos_φ * y].into()
54 } 55 }
55 56
57 /// Mirror y coordinate of input vector.
58 #[inline]
59 fn ymirror(Loc([x, y]) : Loc<f64, 2>) -> Loc<f64, 2> {
60 [x, -y].into()
61 }
62
63
56 impl CapPoint { 64 impl CapPoint {
57 #[inline] 65 #[inline]
58 /// Convert to cylindrical coordinates given z coordinate 66 /// Convert to cylindrical coordinates given z coordinate
59 fn cyl_coords(&self, z : f64) -> CylCoords { 67 fn cyl_coords(&self, z : f64) -> CylCoords {
60 let CapPoint { r, angle } = *self; 68 let CapPoint { r, angle } = *self;
61 CylCoords { r, angle, z } 69 CylCoords { r, angle, z }
62 } 70 }
63 71
64 #[inline] 72 #[inline]
65 /// Convert to cylindrical coordinates given z coordinate 73 /// Convert to cylindrical coordinates given z coordinate
66 fn cartesian_coords(&self) -> Loc<f64, 2> { 74 fn to_cartesian(&self) -> Loc<f64, 2> {
67 let CapPoint { r, angle } = *self; 75 let CapPoint { r, angle } = *self;
68 [r * angle.cos(), r * angle.sin()].into() 76 [r * angle.cos(), r * angle.sin()].into()
69 } 77 }
70 78
71 #[inline] 79 #[inline]
72 #[allow(dead_code)] 80 #[allow(dead_code)]
73 /// Convert to cylindrical coordinates given z coordinate 81 /// Convert to cylindrical coordinates given z coordinate
74 fn from_cartesian(coords : Loc<f64, 2>) -> Self { 82 fn from_cartesian(coords : Loc<f64, 2>) -> Self {
75 let [x, y] = coords.into(); 83 let [x, y] = coords.into();
76 let r = (x*x + y*y).sqrt(); 84 let r = (x*x + y*y).sqrt();
77 let angle = y.atan2(x); 85 let angle = normalise_angle(y.atan2(x));
78 CapPoint { r, angle } 86 CapPoint { r, angle }
79 } 87 }
80 88
81 #[inline] 89 #[inline]
82 /// Calculate the vector between two points on the cap 90 /// Calculate the vector between two points on the cap
83 fn log(&self, other : &CapPoint) -> Loc<f64, 2> { 91 fn log(&self, other : &CapPoint) -> Loc<f64, 2> {
84 other.cartesian_coords() - self.cartesian_coords() 92 other.to_cartesian() - self.to_cartesian()
85 } 93 }
86 94
87 #[inline] 95 #[inline]
88 /// Calculate partial exponential map until boundary. 96 /// Calculate partial exponential map until boundary.
89 /// Returns the final point within the cap as well as a remaining tangent on 97 /// Returns the final point within the cap as well as a remaining tangent on
90 /// the side of the cylinder, if `t` wasn't fully used. 98 /// the side of the cylinder, if `t` wasn't fully used.
91 fn partial_exp(&self, r : f64, t : &Tangent) -> (CapPoint, Option<Tangent>) { 99 fn partial_exp(&self, r : f64, t : &Tangent) -> (CapPoint, Option<Tangent>) {
92 let q = self.cartesian_coords() + t; 100 // |p + s t| <= r
93 let n = q.norm2(); 101 // |p|^2 + s^2 |t|^2 + 2s<p,t> = r^2
94 if n <= r { 102 let p = self.to_cartesian();
95 (Self::from_cartesian(q), None) 103 let np2 = p.norm2_squared();
96 } else { 104 let nt2 = t.norm2_squared();
97 let p = q * r / n; 105 let r2 = r * r;
98 (Self::from_cartesian(p), Some(q - p)) 106 let d = p.dot(t);
107 assert!(np2 <= r2);
108 let s = (-d + (d*d + nt2 * (r2 - np2)).sqrt()) / nt2;
109 if s < 1.0 {
110 (Self::from_cartesian(p + s * t), Some((1.0-s) * t))
111 } else {
112 (Self::from_cartesian(p + t), None)
99 } 113 }
100 } 114 }
101 115
102 #[inline] 116 #[inline]
103 /// Convert tangent from side tangent to cap tangent 117 /// Convert tangent from side tangent to cap tangent
106 // The angle is such that down would be rotated to self.angle, counterclockwise 120 // The angle is such that down would be rotated to self.angle, counterclockwise
107 // The new tangent is R[down + t] - R[down] = Rt. 121 // The new tangent is R[down + t] - R[down] = Rt.
108 rotate(self.angle+f64::PI/2.0, t) 122 rotate(self.angle+f64::PI/2.0, t)
109 } else { 123 } else {
110 // The angle is such that up would be rotated to self.angle, clockwise 124 // The angle is such that up would be rotated to self.angle, clockwise
111 rotate(self.angle-f64::PI/2.0, t) 125 rotate(self.angle+f64::PI/2.0, ymirror(t))
112 } 126 }
113 } 127 }
114 128
115 #[inline] 129 #[inline]
116 /// Convert tangent from cap tangent to tangent tangent 130 /// Convert tangent from cap tangent to tangent tangent
118 if top { 132 if top {
119 // The angle is such that self.angle would be rotated to down, clockwise 133 // The angle is such that self.angle would be rotated to down, clockwise
120 rotate(-self.angle-f64::PI/2.0, t) 134 rotate(-self.angle-f64::PI/2.0, t)
121 } else { 135 } else {
122 // The angle is such that self.angle would be rotated to up, counterclockwise 136 // The angle is such that self.angle would be rotated to up, counterclockwise
123 rotate(f64::PI/2.0-self.angle, t) 137 ymirror(rotate(-self.angle-f64::PI/2.0, t))
124 } 138 }
125 } 139 }
126 } 140 }
127 141
128 /// Coordinates on a side 142 /// Coordinates on a side
707 None => (Point::Side(side_new), None), 721 None => (Point::Side(side_new), None),
708 Some(t_new) => { 722 Some(t_new) => {
709 if side_new.z >= self.top_z() - f64::EPSILON { 723 if side_new.z >= self.top_z() - f64::EPSILON {
710 let cap_new = CapPoint { angle : side_new.angle, r : self.radius }; 724 let cap_new = CapPoint { angle : side_new.angle, r : self.radius };
711 (Point::Top(cap_new), Some(cap_new.tangent_from_side(true, t_new))) 725 (Point::Top(cap_new), Some(cap_new.tangent_from_side(true, t_new)))
712 } else { 726 } else if side_new.z <= self.bottom_z() + f64::EPSILON {
713 let cap_new = CapPoint { angle : side_new.angle, r : self.radius }; 727 let cap_new = CapPoint { angle : side_new.angle, r : self.radius };
714 (Point::Bottom(cap_new), Some(cap_new.tangent_from_side(false, t_new))) 728 (Point::Bottom(cap_new), Some(cap_new.tangent_from_side(false, t_new)))
729 } else {
730 (Point::Side(side_new), None)
715 } 731 }
716 } 732 }
717 } 733 }
718 } 734 }
719 } 735 }
848 864
849 #[cfg(test)] 865 #[cfg(test)]
850 mod tests { 866 mod tests {
851 use super::*; 867 use super::*;
852 868
869 static TOL : f64 = 1e-8;
870
871 macro_rules! check_distance {
872 ($distance : expr, $expected : expr) => {
873 assert!(
874 ($distance-$expected).abs() < TOL,
875 "{} = {}, expected = {}",
876 stringify!($distance),
877 $distance,
878 $expected,
879 )
880 }
881 }
882
883 macro_rules! check_vec_eq {
884 ($left : expr, $right : expr) => {
885 let err = ($left-$right).norm(L2);
886 if err > TOL {
887 panic!("{:?} != {:?} [error {err}]", $left, $right);
888 }
889 }
890 }
891
892 macro_rules! check_point_eq {
893 ($left : expr, $right : expr) => {
894 match ($left, $right) {
895 (Point::Top(a), Point::Top(b)) | (Point::Bottom(a), Point::Bottom(b))=> {
896 if (a.angle-b.angle).abs() > TOL || (a.r-b.r).abs() > TOL {
897 panic!("{:?} != {:?}", a, b)
898 }
899 },
900 (Point::Side(a), Point::Side(b)) => {
901 if (a.angle-b.angle).abs() > TOL || (a.z-b.z).abs() > TOL {
902 panic!("{:?} != {:?}", a, b)
903 }
904 },
905 (a, b) => {
906 panic!("{:?} != {:?}", a, b)
907 }
908 }
909 }
910 }
911
853 static CYL : Cylinder = Cylinder { 912 static CYL : Cylinder = Cylinder {
854 radius : 1.0, 913 radius : 1.0,
855 height : 1.0, 914 height : 1.0,
856 config : CylinderConfig { newton_iters : 20 }, 915 config : CylinderConfig { newton_iters : 20 },
857 }; 916 };
858
859 fn check_distance(distance : f64, expected : f64) {
860 let tol = 1e-10;
861 assert!(
862 (distance-expected).abs() < tol,
863 "distance = {distance}, expected = {expected}"
864 );
865 }
866
867 // fn check_distance_less(distance : f64, expected : f64) {
868 // let tol = 1e-10;
869 // assert!(
870 // distance < expected + tol,
871 // "distance = {distance}, expected = {expected}"
872 // );
873 // }
874 917
875 #[test] 918 #[test]
876 fn intra_cap_log_dist() { 919 fn intra_cap_log_dist() {
877 let π = f64::PI; 920 let π = f64::PI;
878 let p1 = CYL.on_top(0.0, 0.5); 921 let p1 = CYL.on_top(0.0, 0.5);
879 let p2 = CYL.on_top(π, 0.5); 922 let p2 = CYL.on_top(π, 0.5);
880 let p3 = CYL.on_top(π/2.0, 0.5); 923 let p3 = CYL.on_top(π/2.0, 0.5);
881 924
882 check_distance(p1.dist_to(&p2), 1.0); 925 check_distance!(p1.dist_to(&p2), 1.0);
883 check_distance(p2.dist_to(&p3), 0.5_f64.sqrt()); 926 check_distance!(p2.dist_to(&p3), 0.5_f64.sqrt());
884 check_distance(p3.dist_to(&p1), 0.5_f64.sqrt()); 927 check_distance!(p3.dist_to(&p1), 0.5_f64.sqrt());
885 } 928 }
886 929
887 #[test] 930 #[test]
888 fn intra_side_log_dist() { 931 fn intra_side_log_dist() {
889 let π = f64::PI; 932 let π = f64::PI;
890 let p1 = CYL.on_side(0.0, 0.0); 933 let p1 = CYL.on_side(0.0, 0.0);
891 let p2 = CYL.on_side(0.0, 0.4); 934 let p2 = CYL.on_side(0.0, 0.4);
892 let p3 = CYL.on_side(π/2.0, 0.0); 935 let p3 = CYL.on_side(π/2.0, 0.0);
893 936
894 check_distance(p1.dist_to(&p2), 0.4); 937 check_distance!(p1.dist_to(&p2), 0.4);
895 check_distance(p1.dist_to(&p3), π/2.0*CYL.radius); 938 check_distance!(p1.dist_to(&p3), π/2.0*CYL.radius);
896 } 939 }
897 940
898 #[test] 941 #[test]
899 fn intra_side_over_cap_log_dist() { 942 fn intra_side_over_cap_log_dist() {
900 let π = f64::PI; 943 let π = f64::PI;
901 let off = 0.05; 944 let off = 0.05;
902 let z = CYL.top_z() - off; 945 let z = CYL.top_z() - off;
903 let p1 = CYL.on_side(0.0, z); 946 let p1 = CYL.on_side(0.0, z);
904 let p2 = CYL.on_side(π, z); 947 let p2 = CYL.on_side(π, z);
905 948
906 check_distance(p1.dist_to(&p2), 2.0 * (CYL.radius + off)); 949 check_distance!(p1.dist_to(&p2), 2.0 * (CYL.radius + off));
907 } 950 }
908 951
909 #[test] 952 #[test]
910 fn top_bottom_log_dist() { 953 fn top_bottom_log_dist() {
911 let π = f64::PI; 954 let π = f64::PI;
912 let p1 = CYL.on_top(0.0, 0.0); 955 let p1 = CYL.on_top(0.0, 0.0);
913 let p2 = CYL.on_bottom(0.0, 0.0); 956 let p2 = CYL.on_bottom(0.0, 0.0);
914 957
915 check_distance(p1.dist_to(&p2), 2.0 * CYL.radius + CYL.height); 958 check_distance!(p1.dist_to(&p2), 2.0 * CYL.radius + CYL.height);
916 959
917 let p1 = CYL.on_top(0.0, CYL.radius / 2.0); 960 let p1 = CYL.on_top(0.0, CYL.radius / 2.0);
918 let p2 = CYL.on_bottom(0.0, CYL.radius / 2.0); 961 let p2 = CYL.on_bottom(0.0, CYL.radius / 2.0);
919 let p3 = CYL.on_bottom(π, CYL.radius / 2.0); 962 let p3 = CYL.on_bottom(π, CYL.radius / 2.0);
920 963
921 check_distance(p1.dist_to(&p2), CYL.radius + CYL.height); 964 check_distance!(p1.dist_to(&p2), CYL.radius + CYL.height);
922 check_distance(p1.dist_to(&p3), 2.0 * CYL.radius + CYL.height); 965 check_distance!(p1.dist_to(&p3), 2.0 * CYL.radius + CYL.height);
923 } 966 }
924 967
925 #[test] 968 #[test]
926 fn top_side_log_dist() { 969 fn top_side_log_dist() {
927 let p1 = CYL.on_top(0.0, 0.0); 970 let p1 = CYL.on_top(0.0, 0.0);
928 let p2 = CYL.on_side(0.0, 0.0); 971 let p2 = CYL.on_side(0.0, 0.0);
929 972
930 check_distance(p1.dist_to(&p2), CYL.radius + CYL.height / 2.0); 973 check_distance!(p1.dist_to(&p2), CYL.radius + CYL.height / 2.0);
931 } 974 }
932 } 975
976 #[test]
977 fn cap_side_partial_exp() {
978 let π = f64::PI;
979 let angles = [0.0, π/2.0, π*5.0/6.0, π*7.0/5.0];
980
981 for φ in angles {
982 let p = CYL.on_top(φ, CYL.radius / 2.0);
983 let q_target = CYL.on_side(φ, CYL.height / 2.0);
984 let t = rotate(φ, [CYL.radius, 0.0].into());
985 let t_target = Loc([0.0, -CYL.radius / 2.0]);
986 let (q, t_left) = CYL.partial_exp(p.point, t);
987 check_point_eq!(q, q_target.point);
988 if let Some(t_left_) = t_left {
989 check_vec_eq!(t_left_, t_target);
990 } else {
991 panic!("No tangent left");
992 }
993 }
994
995 for φ in angles {
996 let p = CYL.on_top(φ + π/2.0, CYL.radius);
997 let q_target = CYL.on_side(φ, CYL.height / 2.0);
998 let t = 2.0 * rotate(φ, Loc([CYL.radius, -CYL.radius]));
999 let t_target = Loc([-CYL.radius, -CYL.radius]);
1000 let (q, t_left) = CYL.partial_exp(p.point, t);
1001 check_point_eq!(q, q_target.point);
1002 if let Some(t_left_) = t_left {
1003 check_vec_eq!(t_left_, t_target);
1004 } else {
1005 panic!("No tangent left");
1006 }
1007 }
1008 }
1009
1010 #[test]
1011 fn side_top_exp() {
1012 let π = f64::PI;
1013 let angles = [0.0, π/2.0, π*5.0/6.0, π*7.0/5.0];
1014
1015 for φ in angles {
1016 let pt = CYL.on_top(φ, CYL.radius);
1017 let t = rotate(φ, Loc([0.1, 0.3]));
1018 let b = pt.clone().exp(&t);
1019 let a = pt.exp(&(-t));
1020 check_point_eq!(a.exp(&(2.0*t)).point, b.point);
1021 }
1022 }
1023
1024 /// Tests that tangent “returned” on edge from cap to top, correctly
1025 /// sums with a top tangent.
1026 #[test]
1027 fn cap_side_log_exp() {
1028 let π = f64::PI;
1029 let angles = [0.0, π/2.0, π*5.0/6.0, π*7.0/5.0];
1030
1031 for top in [true, false] {
1032 for (&φ, &ψ) in angles.iter().zip(angles.iter().rev()) {
1033 let a = if top {
1034 CYL.on_top(φ, CYL.radius/2.0)
1035 } else {
1036 CYL.on_bottom(φ, CYL.radius/2.0)
1037 };
1038 let b = CYL.on_side(ψ, 0.0);
1039 let t = a.log(&b);
1040 let (p@Point::Side(side), Some(tpb)) = CYL.partial_exp(a.point, t) else {
1041 panic!("No tangent or point not on side");
1042 };
1043 let pp = CYL.point_on(p);
1044 let tpb2 = pp.log(&b);
1045 let tap = a.log(&pp);
1046 check_vec_eq!(tpb, tpb2);
1047 if top {
1048 assert!(indistinguishable(side.z, CYL.top_z()));
1049 } else {
1050 assert!(indistinguishable(side.z, CYL.bottom_z()));
1051 }
1052 let cap = CapPoint{ angle : side.angle, r : CYL.radius };
1053 let tbpcap = cap.tangent_from_side(top, tpb);
1054 check_vec_eq!(tap + tbpcap, t);
1055 }
1056 }
1057 }
1058
1059
1060 /// Tests that tangent “returned” on edge from top to side, correctly
1061 /// sums with a side tangent.
1062 #[test]
1063 fn side_cap_log_exp() {
1064 let π = f64::PI;
1065 let angles = [0.0, π/2.0, π*5.0/6.0, π*7.0/5.0];
1066
1067 for top in [true, false] {
1068 for (&φ, &ψ) in angles.iter().zip(angles.iter().rev()) {
1069 let a = CYL.on_side(ψ, 0.0);
1070 let b = if top {
1071 CYL.on_top(φ, CYL.radius/2.0)
1072 } else {
1073 CYL.on_bottom(φ, CYL.radius/2.0)
1074 };
1075 let t = a.log(&b);
1076 let (p, cap, tpb) = match CYL.partial_exp(a.point, t) {
1077 (p@Point::Top(cap), Some(tpb)) if top => (p, cap, tpb),
1078 (p@Point::Bottom(cap), Some(tpb)) if !top => (p, cap, tpb),
1079 _ => panic!("No tangent or point not on correct cap"),
1080 };
1081 let pp = CYL.point_on(p);
1082 let tpb2 = pp.log(&b);
1083 let tap = a.log(&pp);
1084 check_vec_eq!(tpb, tpb2);
1085 let tbpside = cap.tangent_to_side(top, tpb);
1086 check_vec_eq!(tap + tbpside, t);
1087 }
1088 }
1089 }
1090 }

Mercurial Repository: non-riemannian-opt

mercurial