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

comparison: src/cylinder.rs

src/cylinder.rs

changeset 37
d7cd14b8ccc0
child 38
63318d1b4f00
equal deleted inserted replaced
34:aa6129697116 37:d7cd14b8ccc0
1 /*!
2 Implementation of the surface of a 3D cylinder as a [`ManifoldPoint`].
3 */
4
5 use alg_tools::euclidean::Euclidean;
6 use serde_repr::*;
7 use serde::{Serialize, Deserialize};
8 use alg_tools::loc::Loc;
9 use alg_tools::norms::{Norm, L2};
10 use alg_tools::types::Float;
11 use crate::manifold::{ManifoldPoint, EmbeddedManifoldPoint, FacedManifoldPoint};
12 use crate::newton::{newton_sym1x1, newton_sym2x2};
13
14 /// Angle
15 pub type Angle = f64;
16
17 /// Cylindrical coordinates in ℝ^3
18 #[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
19 pub struct CylCoords {
20 pub r : f64,
21 pub angle : Angle,
22 pub z : f64
23 }
24
25 impl CylCoords {
26 #[inline]
27 pub fn to_cartesian(&self) -> Loc<f64, 3> {
28 let &CylCoords{r, angle, z} = self;
29 [r * angle.cos(), r * angle.sin(), z].into()
30 }
31
32 #[inline]
33 #[allow(dead_code)]
34 pub fn from_cartesian(coords : Loc<f64, 3>) -> Self {
35 let [x, y, z] = coords.into();
36 let r = (x*x + y*y).sqrt();
37 let angle = y.atan2(x);
38 CylCoords {r, angle, z}
39 }
40 }
41
42 /// Coordinates on a cap
43 #[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
44 pub struct CapPoint {
45 pub r : f64,
46 pub angle : Angle
47 }
48
49 #[inline]
50 fn rotate(φ : f64, Loc([x, y]) : Loc<f64, 2>) -> Loc<f64, 2> {
51 let sin_φ = φ.sin();
52 let cos_φ = φ.cos();
53 [cos_φ * x - sin_φ * y, sin_φ * x + cos_φ * y].into()
54 }
55
56 impl CapPoint {
57 #[inline]
58 /// Convert to cylindrical coordinates given z coordinate
59 fn cyl_coords(&self, z : f64) -> CylCoords {
60 let CapPoint { r, angle } = *self;
61 CylCoords { r, angle, z }
62 }
63
64 #[inline]
65 /// Convert to cylindrical coordinates given z coordinate
66 fn cartesian_coords(&self) -> Loc<f64, 2> {
67 let CapPoint { r, angle } = *self;
68 [r * angle.cos(), r * angle.sin()].into()
69 }
70
71 #[inline]
72 #[allow(dead_code)]
73 /// Convert to cylindrical coordinates given z coordinate
74 fn from_cartesian(coords : Loc<f64, 2>) -> Self {
75 let [x, y] = coords.into();
76 let r = (x*x + y*y).sqrt();
77 let angle = y.atan2(x);
78 CapPoint { r, angle }
79 }
80
81 #[inline]
82 /// Calculate the vector between two points on the cap
83 fn log(&self, other : &CapPoint) -> Loc<f64, 2> {
84 other.cartesian_coords() - self.cartesian_coords()
85 }
86
87 #[inline]
88 /// Calculate partial exponential map until boundary.
89 /// 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.
91 fn partial_exp(&self, r : f64, t : &Tangent) -> (CapPoint, Option<Tangent>) {
92 let q = self.cartesian_coords() + t;
93 let n = q.norm2();
94 if n <= r {
95 (Self::from_cartesian(q), None)
96 } else {
97 let p = q * r / n;
98 (Self::from_cartesian(p), Some(q - p))
99 }
100 }
101
102 #[inline]
103 /// Convert tangent from side tangent to cap tangent
104 fn tangent_from_side(&self, top : bool, t : Tangent) -> Tangent {
105 if top {
106 // The angle is such that down would be rotated to self.angle, counterclockwise
107 // The new tangent is R[down + t] - R[down] = Rt.
108 rotate(self.angle+f64::PI/2.0, t)
109 } else {
110 // The angle is such that up would be rotated to self.angle, clockwise
111 rotate(self.angle-f64::PI/2.0, t)
112 }
113 }
114
115 #[inline]
116 /// Convert tangent from cap tangent to tangent tangent
117 fn tangent_to_side(&self, top : bool, t : Tangent) -> Tangent {
118 if top {
119 // The angle is such that self.angle would be rotated to down, clockwise
120 rotate(-self.angle-f64::PI/2.0, t)
121 } else {
122 // The angle is such that self.angle would be rotated to up, counterclockwise
123 rotate(f64::PI/2.0-self.angle, t)
124 }
125 }
126 }
127
128 /// Coordinates on a side
129 #[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
130 pub struct SidePoint {
131 pub z : f64,
132 pub angle : Angle
133 }
134
135 #[inline]
136 fn anglediff(mut φ1 : f64, mut φ2 : f64) -> f64 {
137 let π = f64::PI;
138 φ1 = normalise_angle(φ1);
139 φ2 = normalise_angle(φ2);
140 let α = φ2 - φ1;
141 if α >= 0.0 {
142 if α <= π {
143 α
144 } else {
145 α - 2.0 * π
146 }
147 } else {
148 if α >= -π {
149 α
150 } else {
151 2.0 * π + α
152 }
153 }
154 }
155
156 #[inline]
157 pub fn normalise_angle(φ : f64) -> f64 {
158 let π = f64::PI;
159 φ.rem_euclid(2.0 * π)
160 }
161
162 impl SidePoint {
163 #[inline]
164 /// Convert to cylindrical coordinates given radius
165 fn cyl_coords(&self, r : f64) -> CylCoords {
166 let SidePoint { z, angle } = *self;
167 CylCoords { r, angle, z }
168 }
169
170 #[inline]
171 /// Calculate tangent vector between two points on the side, given radius
172 fn log(&self, r : f64, other : &SidePoint) -> Loc<f64, 2> {
173 let SidePoint{ z : z1, angle : angle1 } = *self;
174 let SidePoint{ z : z2, angle : angle2 } = *other;
175 let φ = anglediff(angle1, angle2);
176 // TODO: is this correct?
177 [r*φ, z2-z1].into()
178 }
179
180 #[inline]
181 /// Calculate partial exponential map under boundary
182 /// Returns a point on the next face, as well as a remaining tangent on
183 /// the side of the cylinder, if `t` wasn't fully used.
184 fn partial_exp(&self, r : f64, (a, b) : (f64, f64), t : &Tangent)
185 -> (SidePoint, Option<Tangent>)
186 {
187 assert!(a <= self.z && self.z <= b);
188 let Loc([_, h]) = *t;
189 let s = if h > 0.0 {
190 ((b - self.z)/h).min(1.0)
191 } else if h < 0.0 {
192 ((a - self.z)/h).min(1.0)
193 } else {
194 1.0
195 };
196 let d = t * s;
197 let p = self.unflatten(r, d);
198 if s < 1.0 {
199 (p, Some(t - d))
200 } else {
201 (p, None)
202 }
203 }
204
205 #[inline]
206 /// Unflattens another point in the local coordinate system of self
207 fn unflatten(&self, r : f64, Loc([v, h]) : Loc<f64, 2>) -> Self {
208 SidePoint{ z : self.z + h, angle : normalise_angle(self.angle + v / r) }
209 }
210
211 }
212
213 /// Point on a [`Cylinder`]
214 #[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
215 pub enum Point {
216 Top(CapPoint),
217 Bottom(CapPoint),
218 Side(SidePoint),
219 }
220
221 /// Face on a [`Cylinder`]
222 #[derive(Copy, Clone, Debug, Eq, PartialEq, Serialize_repr, Deserialize_repr)]
223 #[repr(u8)]
224 pub enum Face {
225 Top,
226 Bottom,
227 Side,
228 }
229
230 #[derive(Clone, Debug, PartialEq)]
231 pub struct OnCylinder<'a> {
232 cylinder : &'a Cylinder,
233 point : Point,
234 }
235
236
237 /// Cylinder configuration
238 #[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
239 pub struct CylinderConfig {
240 pub newton_iters : usize,
241 }
242
243 impl Default for CylinderConfig {
244 fn default() -> Self {
245 CylinderConfig { newton_iters : 10 }
246 }
247 }
248
249 /// A cylinder
250 #[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
251 pub struct Cylinder {
252 /// Radius of the cylinder
253 pub radius : f64,
254 /// Height of the cylinder
255 pub height : f64,
256 /// Configuration for numerical methods
257 pub config : CylinderConfig
258 }
259
260
261 impl std::fmt::Display for Face {
262 fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
263 use Face::*;
264 let s = match *self {
265 Top => "Top",
266 Bottom => "Bottom",
267 Side => "Side",
268 };
269 write!(f, "{}", s)
270 }
271 }
272
273 impl Point {
274 fn face(&self) -> Face {
275 match *self {
276 Point::Top(..) => Face::Top,
277 Point::Bottom(_) => Face::Bottom,
278 Point::Side(_) => Face::Side,
279 }
280 }
281 }
282
283 impl<'a> FacedManifoldPoint for OnCylinder<'a> {
284 type Face = Face;
285 /// Returns the face of this point.
286 fn face(&self) -> Face {
287 self.point.face()
288 }
289 }
290
291 // Tangent vector
292 type Tangent = Loc<f64, 2>;
293
294 #[inline]
295 fn best_tangent<I>(tangents : I) -> (Tangent, f64)
296 where I : IntoIterator<Item = Tangent> {
297 tangents.into_iter()
298 .map(|t| (t, t.norm(L2)))
299 .reduce(|a@(_, la), b@(_, lb)| if la < lb { a } else { b })
300 .unwrap()
301 }
302
303 /// Swap the elements of a two-tuple
304 #[inline]
305 fn swap<A>((a, b) : (A, A)) -> (A, A) {
306 (b, a)
307 }
308
309 #[inline]
310 fn indistinguishable(a : f64, b : f64) -> bool {
311 a > b - f64::EPSILON && a < b + f64::EPSILON
312 }
313
314 impl Cylinder {
315 /// Return the cylindrical coordinates of `point` on this cylinder
316 fn cyl_coords(&self, point : &Point) -> CylCoords {
317 match *point {
318 Point::Top(cap) => { cap.cyl_coords(self.top_z()) },
319 Point::Bottom(cap) => { cap.cyl_coords(self.bottom_z()) },
320 Point::Side(side) => { side.cyl_coords(self.radius) },
321 }
322 }
323
324 #[inline]
325 pub fn top_z(&self) -> f64 {
326 self.height / 2.0
327 }
328
329 #[inline]
330 pub fn bottom_z(&self) -> f64 {
331 -self.height / 2.0
332 }
333
334 /// Find angle where a geodesic from `side` to `(cap, z)` crosses the cap edge.
335 ///
336 /// Uses `newton_sym1x1`.
337 fn side_cap_crossing(
338 &self,
339 side : &SidePoint,
340 cap : &CapPoint, z : f64, // `z` is the z coordinate of cap
341 ) -> Angle {
342 let &SidePoint { z : z_1, angle : φ_1 } = side;
343 let &CapPoint { r : r_2, angle : φ_2 } = cap;
344 assert_ne!(z, z_1);
345 let d_1 = z - z_1;
346 let d2_1 = d_1 * d_1;
347 let r = self.radius;
348 let r2 = r * r;
349 let r2_2 = r_2 * r_2;
350 let rr_2 = r * r_2;
351
352 let g = |α_1 : f64| {
353 let ψ = φ_2 - φ_1 - α_1;
354 let ψ_cos = ψ.cos();
355 let ψ_sin = ψ.sin();
356 let ψ_sin2 = ψ_sin * ψ_sin;
357 let ψ_cos2 = ψ_cos * ψ_cos;
358 let α2_1 = α_1 * α_1;
359 let c = d2_1 + r2 * α2_1;
360 let e = r2 + r2_2 - 2.0 * rr_2 * ψ_cos;
361 let g = r2 * α_1 / c.sqrt() - rr_2 * ψ_sin / e.sqrt();
362 let h = r2 * d2_1 / c.powf(3.0/2.0)
363 + r * r_2 * ((r2 + r2_2) * ψ_cos - rr_2 * (ψ_sin2 - 2.0 * ψ_cos2))
364 / e.powf(3.0/2.0);
365 (h, g)
366 };
367
368 let α_1 = newton_sym1x1(g, 0.0, self.config.newton_iters);
369 normalise_angle(φ_1 + α_1)
370
371 }
372
373 /// Find angles where the geodesic passing through a cap at height `z` from `side1` to `side2`
374 /// crosses the cap edge. **Panics if `side2.angle < side1.angle`.**
375 ///
376 /// Uses `newton_sym2x2`.
377 fn side_cap_side_crossing(
378 &self,
379 side1 : &SidePoint,
380 z : f64,
381 side2 : &SidePoint
382 ) -> (Angle, Angle) {
383 let &SidePoint { z : z_1, angle : φ_1 } = side1;
384 let &SidePoint { z : z_2, angle : φ_2 } = side2;
385 assert!(φ_2 >= φ_1);
386 assert_ne!(z_1, z);
387 assert_ne!(z_2, z);
388 let r = self.radius;
389 let r2 = r * r;
390 let d_1 = z - z_1;
391 let d_2 = z - z_2;
392 let d2_1 = d_1 * d_1;
393 let d2_2 = d_2 * d_2;
394 let g = |α_1 : f64, α_2 : f64| {
395 let α2_1 = α_1 * α_1;
396 let α2_2 = α_2 * α_2;
397 let ψ = (α_1 + α_2 + φ_1 - φ_2) / 2.0;
398 let ψ_sin = ψ.sin();
399 let ψ_cos = ψ.cos();
400 let c_1 = d2_1 + r2 * α2_1;
401 let c_2 = d2_2 + r2 * α2_2;
402 let g_1 = r2 * α_1 / c_1.sqrt() - r * ψ_cos;
403 let g_2 = r2 * α_2 / c_2.sqrt() - r * ψ_cos;
404 let h_12 = (r / 2.0) * ψ_sin;
405 let h_11 = r2 * d2_1 / c_1.powf(3.0 / 2.0) + h_12;
406 let h_22 = r2 * d2_2 / c_2.powf(3.0 / 2.0) + h_12;
407 ([h_11, h_12, h_22], [g_1, g_2])
408 };
409
410 let [α_1, α_2] = newton_sym2x2(g, [0.0, 0.0], self.config.newton_iters);
411 (normalise_angle(φ_1 + α_1), normalise_angle(φ_2 + α_2))
412
413 }
414
415 /// Find angles where the geodesic passing through the side from `cap1` at height `z_1`
416 /// to `cap2` at height `z_2` crosses the cap edges.
417 /// **Panics if `cap2.angle < cap1.angle`.**
418 ///
419 /// Uses `newton_sym2x2`.
420 fn cap_side_cap_crossing(
421 &self,
422 cap1 : &CapPoint, z_1 : f64,
423 cap2 : &CapPoint, z_2 : f64,
424 init_by_cap2 : bool
425 ) -> (Angle, Angle) {
426 let r = self.radius;
427 let &CapPoint { r : r_1, angle : φ_1 } = cap1;
428 let &CapPoint { r : r_2, angle : φ_2 } = cap2;
429 assert!(φ_2 >= φ_1);
430 assert_ne!(r_1, r);
431 assert_ne!(r_2, r);
432 assert_ne!(z_2, z_1);
433 if r_1 == 0.0 && r_2 == 0.0 {
434 // Singular case: both points are in the middle of the caps.
435 return (φ_1, φ_1)
436 }
437 let r2 = r * r;
438 let d = (z_2 - z_1).abs();
439 let d2 = d * d;
440 let r2_1 = r_1 * r_1;
441 let r2_2 = r_2 * r_2;
442 let rr_1 = r * r_1;
443 let rr_2 = r * r_2;
444 let f = |α_1 : f64, α_2 : f64| {
445 let cos_α1 = α_1.cos();
446 let sin_α1 = α_1.sin();
447 let cos_α2 = α_2.cos();
448 let sin_α2 = α_2.sin();
449 //let cos2_α1 = cos_α1 * cos_α1;
450 let sin2_α1 = sin_α1 * sin_α1;
451 //let cos2_α2 = cos_α2 * cos_α2;
452 let sin2_α2 = sin_α2 * sin_α2;
453 let ψ = φ_2 - φ_1 - α_1 - α_2;
454 let ψ2 = ψ * ψ;
455 let ψ2r2 = ψ2 * r2;
456 //let r4 = r2 * r2;
457 let b = d2 + ψ2r2;
458 let c = r2 * ψ / b.sqrt();
459 let e_1 = r2 + r2_1 - 2.0 * rr_1 * cos_α1;
460 let e_2 = r2 + r2_2 - 2.0 * rr_2 * cos_α2;
461 let g_1 = rr_1 * sin_α1 / e_1.sqrt() - c;
462 let g_2 = rr_2 * sin_α2 / e_2.sqrt() - c;
463 let h_12 = r2 * (1.0 - ψ2r2 / b) / b.sqrt();
464 // let h_11 = rr_1 * ( (r2 + r2_1) * cos_α1 - rr_1 * ( sin2_α1 + 2.0 * cos2_α1) )
465 // / e_1.powf(3.0/2.0) + h_12;
466 // let h_22 = rr_2 * ( (r2 + r2_2) * cos_α2 - rr_2 * ( sin2_α2 + 2.0 * cos2_α2) )
467 // / e_2.powf(3.0/2.0) + h_12;
468 // 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;
469 // 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;
470 let h_11 = rr_1 * (cos_α1 - rr_1 * sin2_α1 / e_1) / e_1.sqrt() + h_12;
471 let h_22 = rr_2 * (cos_α2 - rr_2 * sin2_α2 / e_2) / e_2.sqrt() + h_12;
472 ([h_11, h_12, h_22], [g_1, g_2])
473 };
474
475 let α_init = if init_by_cap2 {
476 [φ_2 - φ_1, 0.0]
477 } else {
478 [0.0, φ_2 - φ_1]
479 };
480 let [α_1, α_2] = newton_sym2x2(f, α_init, self.config.newton_iters);
481 (normalise_angle(φ_1 + α_1), normalise_angle(φ_2 - α_2))
482 }
483
484 fn cap_side_log(
485 &self,
486 cap : &CapPoint, (z, top) : (f64, bool),
487 side : &SidePoint
488 ) -> Tangent {
489 let r = self.radius;
490 if indistinguishable(side.z, z) {
491 // Degenerate case
492 let capedge = CapPoint{ angle : side.angle, r };
493 cap.log(&capedge)
494 } else if indistinguishable(r, cap.r)
495 && anglediff(side.angle, cap.angle).abs() < f64::PI/2.0 {
496 // Degenerate case
497 let sideedge = SidePoint{ angle : cap.angle, z};
498 cap.tangent_from_side(top, sideedge.log(r, side))
499 } else {
500 let φ = self.side_cap_crossing(side, cap, z);
501 let capedge = CapPoint{ angle : φ, r };
502 let sideedge = SidePoint{ angle : φ, z };
503 let t1 = cap.log(&capedge);
504 let t2 = sideedge.log(r, side);
505 // Either option should give the same result, but the first one avoids division.
506 t1 + capedge.tangent_from_side(top, t2)
507 // let n = t1.norm(L2);
508 // (t1/n)*(n + t2.norm(L2))
509 }
510 }
511
512 fn side_cap_log(
513 &self,
514 side : &SidePoint,
515 cap : &CapPoint, (z, top) : (f64, bool),
516 ) -> Tangent {
517 let r = self.radius;
518 if indistinguishable(side.z, z) {
519 // Degenerate case
520 let capedge = CapPoint{ angle : side.angle, r };
521 capedge.tangent_to_side(top, capedge.log(cap))
522 } else if indistinguishable(r, cap.r)
523 && anglediff(side.angle, cap.angle).abs() < f64::PI/2.0 {
524 // Degenerate case
525 side.log(r, &SidePoint{ z, angle : cap.angle })
526 } else {
527 let φ = self.side_cap_crossing(side, cap, z);
528 let capedge = CapPoint{ angle : φ, r };
529 let sideedge = SidePoint{ angle : φ, z };
530 let t1 = side.log(r, &sideedge);
531 let t2 = capedge.log(cap);
532 // Either option should give the same result, but the first one avoids division.
533 t1 + capedge.tangent_to_side(top, t2)
534 // let n = t1.norm(L2);
535 // (t1/n)*(n + t2.norm(L2))
536 }
537 }
538
539 fn side_cap_side_log(
540 &self,
541 side1 : &SidePoint,
542 (z, top) : (f64, bool),
543 side2 : &SidePoint
544 ) -> Tangent {
545 let r = self.radius;
546 if indistinguishable(side1.z, z) {
547 // Degenerate case
548 let capedge1 = CapPoint{ angle : side1.angle, r };
549 capedge1.tangent_to_side(top, self.cap_side_log(&capedge1, (z, top), side2))
550 } else if indistinguishable(side2.z, z) {
551 // Degenerate case
552 let capedge2 = CapPoint{ angle : side2.angle, r };
553 self.side_cap_log(side1, &capedge2, (z, top))
554 } else {
555 let (φ1, φ2) = if side2.angle >= side1.angle {
556 self.side_cap_side_crossing(side1, z, side2)
557 } else {
558 swap(self.side_cap_side_crossing(side2, z, side1))
559 };
560 let capedge1 = CapPoint{ angle : φ1, r };
561 let sideedge1 = SidePoint{ angle : φ1, z };
562 let capedge2 = CapPoint{ angle : φ2, r };
563 let sideedge2 = SidePoint{ angle : φ2, z };
564 let t1 = side1.log(r, &sideedge1);
565 let t2 = capedge1.log(&capedge2);
566 let t3 = sideedge2.log(r, &side2);
567 // Any option should give the same result, but the first one avoids division.
568 // t1 + capedge1.tangent_to_side(top, t2 + capedge2.tangent_from_side(top, t3))
569 //
570 // let n = t2.norm(L2);
571 // t1 + capedge1.tangent_to_side(top, (t2/n)*(n + t3.norm(L2)))
572 //
573 let n = t1.norm(L2);
574 (t1/n)*(n + t2.norm(L2) + t3.norm(L2))
575 //
576 // let n = t1.norm(L2);
577 // let t23 = t2 + capedge2.tangent_from_side(top, t3);
578 // (t1/n)*(n + t23.norm(L2))
579 }
580 }
581
582 fn cap_side_cap_log(
583 &self,
584 cap1 : &CapPoint, (z1, top1) : (f64, bool),
585 cap2 : &CapPoint, (z2, top2) : (f64, bool),
586 init_by_cap2 : bool,
587 ) -> Tangent {
588 let r = self.radius;
589 if indistinguishable(cap1.r, r) {
590 // Degenerate case
591 let sideedge1 = SidePoint{ angle : cap1.angle, z : z1 };
592 cap1.tangent_from_side(top1, self.side_cap_log(&sideedge1, cap2, (z2, top2)))
593 } else if indistinguishable(cap2.r, r) {
594 // Degenerate case
595 let sideedge2 = SidePoint{ angle : cap2.angle, z : z2 };
596 self.cap_side_log(cap1, (z1, top1), &sideedge2)
597 } else {
598 let (φ1, φ2) = if cap2.angle >= cap1.angle {
599 self.cap_side_cap_crossing(cap1, z1, cap2, z2, init_by_cap2)
600 } else {
601 swap(self.cap_side_cap_crossing(cap2, z2, cap1, z1, !init_by_cap2))
602 };
603 let sideedge1 = SidePoint{ angle : φ1, z : z1 };
604 let capedge1 = CapPoint{ angle : φ1, r };
605 let sideedge2 = SidePoint{ angle : φ2, z : z2};
606 let capedge2 = CapPoint{ angle : φ2, r };
607 let t1 = cap1.log(&capedge1);
608 let t2 = sideedge1.log(r, &sideedge2);
609 let t3 = capedge2.log(cap2);
610 // Either option should give the same result, but the first one avoids division.
611 t1 + capedge1.tangent_from_side(top1, t2 + capedge2.tangent_to_side(top2, t3))
612 //let n = t1.norm(L2);
613 //(t1/n)*(n + t2.norm(L2) + t3.norm(L2))
614 }
615 }
616
617 /// Calculates both the logarithmic map and distance to another point
618 fn log_dist(&self, source : &Point, destination : &Point) -> (Tangent, f64) {
619 use Point::*;
620 match (source, destination) {
621 (Top(cap1), Top(cap2)) => {
622 best_tangent([cap1.log(cap2)])
623 },
624 (Bottom(cap1), Bottom(cap2)) => {
625 best_tangent([cap1.log(cap2)])
626 },
627 (Bottom(cap), Side(side)) => {
628 best_tangent([self.cap_side_log(cap, (self.bottom_z(), false), side)])
629 },
630 (Top(cap), Side(side)) => {
631 best_tangent([self.cap_side_log(cap, (self.top_z(), true), side)])
632 },
633 (Side(side), Bottom(cap)) => {
634 best_tangent([self.side_cap_log(side, cap, (self.bottom_z(), false))])
635 },
636 (Side(side), Top(cap)) => {
637 best_tangent([self.side_cap_log(side, cap, (self.top_z(), true))])
638 },
639 (Side(side1), Side(side2)) => {
640 best_tangent([
641 side1.log(self.radius, side2),
642 self.side_cap_side_log(side1, (self.top_z(), true), side2),
643 self.side_cap_side_log(side1, (self.bottom_z(), false), side2),
644 ])
645 },
646 (Top(cap1), Bottom(cap2)) => {
647 best_tangent([
648 // We try a few possible initialisations for Newton
649 self.cap_side_cap_log(
650 cap1, (self.top_z(), true),
651 cap2, (self.bottom_z(), false),
652 false
653 ),
654 self.cap_side_cap_log(
655 cap1, (self.top_z(), true),
656 cap2, (self.bottom_z(), false),
657 true
658 ),
659 ])
660 },
661 (Bottom(cap1), Top(cap2)) => {
662 best_tangent([
663 // We try a few possible initialisations for Newton
664 self.cap_side_cap_log(
665 cap1, (self.bottom_z(), false),
666 cap2, (self.top_z(), true),
667 false
668 ),
669 self.cap_side_cap_log(
670 cap1, (self.bottom_z(), false),
671 cap2, (self.top_z(), true),
672 true
673 ),
674 ])
675 },
676 }
677 }
678
679 #[allow(unreachable_code)]
680 #[allow(unused_variables)]
681 fn partial_exp(&self, point : Point, t : Tangent) -> (Point, Option<Tangent>) {
682 match point {
683 Point::Top(cap) => {
684 let (cap_new, t_new_basis) = cap.partial_exp(self.radius, &t);
685 match t_new_basis {
686 None => (Point::Top(cap_new), None),
687 Some(t_new) => {
688 let side_new = SidePoint{ angle : cap_new.angle, z : self.top_z() };
689 (Point::Side(side_new), Some(cap_new.tangent_to_side(true, t_new)))
690 }
691 }
692 },
693 Point::Bottom(cap) => {
694 let (cap_new, t_new_basis) = cap.partial_exp(self.radius, &t);
695 match t_new_basis {
696 None => (Point::Bottom(cap_new), None),
697 Some(t_new) => {
698 let side_new = SidePoint{ angle : cap_new.angle, z : self.bottom_z() };
699 (Point::Side(side_new), Some(cap_new.tangent_to_side(false, t_new)))
700 }
701 }
702 },
703 Point::Side(side) => {
704 let lims = (self.bottom_z(), self.top_z());
705 let (side_new, t_new_basis) = side.partial_exp(self.radius, lims, &t);
706 match t_new_basis {
707 None => (Point::Side(side_new), None),
708 Some(t_new) => {
709 if side_new.z >= self.top_z() - f64::EPSILON {
710 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)))
712 } else {
713 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)))
715 }
716 }
717 }
718 }
719 }
720 }
721
722 fn exp(&self, point : &Point, tangent : &Tangent) -> Point {
723 let mut p = *point;
724 let mut t = *tangent;
725 loop {
726 (p, t) = match self.partial_exp(p, t) {
727 (p, None) => break p,
728 (p, Some(t)) => (p, t),
729 };
730 }
731 }
732
733 /// Check that `point` has valid coordinates, and normalise angles
734 pub fn normalise(&self, point : Point) -> Option<Point> {
735 match point {
736 Point::Side(side) => {
737 let a = self.bottom_z();
738 let b = self.top_z();
739 (a <= side.z && side.z <= b).then(|| {
740 Point::Side(SidePoint{ angle : normalise_angle(side.angle), .. side })
741 })
742 },
743 Point::Bottom(cap) => {
744 (cap.r <= self.radius).then(|| {
745 Point::Bottom(CapPoint{ angle : normalise_angle(cap.angle), .. cap })
746 })
747 },
748 Point::Top(cap) => {
749 (cap.r <= self.radius).then(|| {
750 Point::Top(CapPoint{ angle : normalise_angle(cap.angle), .. cap })
751 })
752 },
753 }
754 }
755
756 /// Convert `p` into a a point associated with the cylinder.
757 ///
758 /// May panic if the coordinates are invalid.
759 pub fn point_on(&self, point : Point) -> OnCylinder<'_> {
760 match self.normalise(point) {
761 None => panic!("{point:?} not on cylinder {self:?}"),
762 Some(point) => OnCylinder { cylinder : self, point }
763 }
764 }
765
766 /// Convert `p` into a a point on side associated with the cylinder.
767 ///
768 /// May panic if the coordinates are invalid.
769 pub fn point_on_side(&self, side : SidePoint) -> OnCylinder<'_> {
770 self.point_on(Point::Side(side))
771 }
772
773 /// Convert `p` into a a point on top associated with the cylinder.
774 ///
775 /// May panic if the coordinates are invalid.
776 pub fn point_on_top(&self, cap : CapPoint) -> OnCylinder<'_> {
777 self.point_on(Point::Top(cap))
778 }
779
780 /// Convert `p` into a a point on bottom associated with the cylinder.
781 ///
782 /// May panic if the coordinates are invalid.
783 pub fn point_on_bottom(&self, cap : CapPoint) -> OnCylinder<'_> {
784 self.point_on(Point::Bottom(cap))
785 }
786
787 /// Convert `p` into a a point on side associated with the cylinder.
788 ///
789 /// May panic if the coordinates are invalid.
790 pub fn on_side(&self, angle : Angle, z : f64) -> OnCylinder<'_> {
791 self.point_on_side(SidePoint{ angle, z })
792 }
793
794 /// Convert `p` into a a point on top associated with the cylinder.
795 ///
796 /// May panic if the coordinates are invalid.
797 pub fn on_top(&self, angle : Angle, r : f64) -> OnCylinder<'_> {
798 self.point_on_top(CapPoint{ angle, r })
799 }
800
801 /// Convert `p` into a a point on bottom associated with the cylinder.
802 ///
803 /// May panic if the coordinates are invalid.
804 pub fn on_bottom(&self, angle : Angle, r : f64) -> OnCylinder<'_> {
805 self.point_on_bottom(CapPoint{ angle, r })
806 }
807 }
808
809 impl<'a> OnCylinder<'a> {
810 /// Return the cylindrical coordinates of this point
811 pub fn cyl_coords(&self) -> CylCoords {
812 self.cylinder.cyl_coords(&self.point)
813 }
814 }
815
816 impl<'a> EmbeddedManifoldPoint for OnCylinder<'a> {
817 type EmbeddedCoords = Loc<f64, 3>;
818
819 /// Get embedded 3D coordinates
820 fn embedded_coords(&self) -> Loc<f64, 3> {
821 self.cyl_coords().to_cartesian()
822 }
823 }
824
825 impl<'a> ManifoldPoint for OnCylinder<'a> {
826 type Tangent = Tangent;
827
828 fn exp(self, tangent : &Self::Tangent) -> Self {
829 let cylinder = self.cylinder;
830 let point = cylinder.exp(&self.point, tangent);
831 OnCylinder { cylinder, point }
832 }
833
834 fn log(&self, other : &Self) -> Self::Tangent {
835 assert!(self.cylinder == other.cylinder);
836 self.cylinder.log_dist(&self.point, &other.point).0
837 }
838
839 fn dist_to(&self, other : &Self) -> f64 {
840 assert!(self.cylinder == other.cylinder);
841 self.cylinder.log_dist(&self.point, &other.point).1
842 }
843
844 fn tangent_origin(&self) -> Self::Tangent {
845 Loc([0.0, 0.0])
846 }
847 }
848
849 #[cfg(test)]
850 mod tests {
851 use super::*;
852
853 static CYL : Cylinder = Cylinder {
854 radius : 1.0,
855 height : 1.0,
856 config : CylinderConfig { newton_iters : 20 },
857 };
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
875 #[test]
876 fn intra_cap_log_dist() {
877 let π = f64::PI;
878 let p1 = CYL.on_top(0.0, 0.5);
879 let p2 = CYL.on_top(π, 0.5);
880 let p3 = CYL.on_top(π/2.0, 0.5);
881
882 check_distance(p1.dist_to(&p2), 1.0);
883 check_distance(p2.dist_to(&p3), 0.5_f64.sqrt());
884 check_distance(p3.dist_to(&p1), 0.5_f64.sqrt());
885 }
886
887 #[test]
888 fn intra_side_log_dist() {
889 let π = f64::PI;
890 let p1 = CYL.on_side(0.0, 0.0);
891 let p2 = CYL.on_side(0.0, 0.4);
892 let p3 = CYL.on_side(π/2.0, 0.0);
893
894 check_distance(p1.dist_to(&p2), 0.4);
895 check_distance(p1.dist_to(&p3), π/2.0*CYL.radius);
896 }
897
898 #[test]
899 fn intra_side_over_cap_log_dist() {
900 let π = f64::PI;
901 let off = 0.05;
902 let z = CYL.top_z() - off;
903 let p1 = CYL.on_side(0.0, z);
904 let p2 = CYL.on_side(π, z);
905
906 check_distance(p1.dist_to(&p2), 2.0 * (CYL.radius + off));
907 }
908
909 #[test]
910 fn top_bottom_log_dist() {
911 let π = f64::PI;
912 let p1 = CYL.on_top(0.0, 0.0);
913 let p2 = CYL.on_bottom(0.0, 0.0);
914
915 check_distance(p1.dist_to(&p2), 2.0 * CYL.radius + CYL.height);
916
917 let p1 = CYL.on_top(0.0, CYL.radius / 2.0);
918 let p2 = CYL.on_bottom(0.0, CYL.radius / 2.0);
919 let p3 = CYL.on_bottom(π, CYL.radius / 2.0);
920
921 check_distance(p1.dist_to(&p2), CYL.radius + CYL.height);
922 check_distance(p1.dist_to(&p3), 2.0 * CYL.radius + CYL.height);
923 }
924
925 #[test]
926 fn top_side_log_dist() {
927 let p1 = CYL.on_top(0.0, 0.0);
928 let p2 = CYL.on_side(0.0, 0.0);
929
930 check_distance(p1.dist_to(&p2), CYL.radius + CYL.height / 2.0);
931 }
932 }

Mercurial Repository: non-riemannian-opt

mercurial