• API
• FAQ
• Tools
• Archive
SHARE
TWEET Untitled a guest Sep 15th, 2019 80 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
1. // import a hash map implementation
2. use fnv::FnvHashMap;
3. use std::f64;
4.
5. // these are multiplied by epsilon to obtain
6. // smaller values of espilon. Both should be
7. // between 0 and 1, and CHECK_FACTOR should be greater
8. // than EPS_FACTOR. There is an optimal value of EPS_FACTOR
9. // for any function, and increasing CHECK_FACTOR produces slightly
10. // better results but causes the algorithm to take longer.
11. const EPS_FACTOR: f64 = 1.0 - 1.0 / 4.0;
12. const CHECK_FACTOR: f64 = 1.0 - 1.0 / 8.0;
13.
14. // whether to display the entire output or just the number
15. // of triangles
16. const SHOW_OUTPUT: bool = true;
17.
18. // tolerance used for numerical methods
19. const TOL: f64 = 1.0e-15;
20.
21. fn main() {
22.     // the function and its first two derivatives
23.     let f = |x: f64, y: f64| x.sin() + y.sin();
24.     let d = |x: f64, y: f64| [x.cos(), y.cos()];
25.     // [df/dxdx, df/dydy, df/dxdy]
26.     let dd = |x: f64, y: f64| [-x.sin(), -y.sin(), 0.0];
27.     let mut v = Vec::new();
28.     // the points that form the initial two triangles
29.     let mut points = vec![[-8.0, -8.0], [8.0, -8.0], [8.0, 8.0], [-8.0, 8.0]];
30.     // generate the approximation
31.     generate_approx_2d(
32.         // the function will add more values to points
33.         &mut points,
34.         // the indices of the triangles to generate
35.         // we can't generate the two triangles seperately
36.         // because the seam between them wouldn't line up
37.         &[[0, 1, 2], [2, 3, 0]],
38.         // epsilon
39.         1.0 / 8.0,
40.         // the step size. For any function g(x)=f(x0+ax, y0+bx), where
41.         // a^2+b^2=1, g can be decomposed into [p_i, q_i] intervals where
42.         // either g or -g is convex on [p_i, q_i] with q_i - p_i > step.
43.         // I am not sure if there is an easy way to find this constant
44.         1.0,
45.         // a value that should be larger than the lipschitz constant
46.         // for the function (used to create an oracle)
47.         1.1,
48.         // the function will add sets of indices to this vector
49.         &mut v,
50.         // pass the lambdas for the function
51.         f,
52.         d,
53.         dd,
54.     );
55.     // display the number of triangles generated
56.     println!("{}", v.len());
57.     if SHOW_OUTPUT {
58.         print!("[");
59.         for p in points {
60.             for val in &p {
61.                 print!("{:.12}, ", val);
62.             }
63.         }
64.         println!("]");
65.         print!("[");
66.         for tri in v {
67.             for index in &tri {
68.                 print!("{}, ", index);
69.             }
70.         }
71.         println!("]");
72.     }
73. }
74.
75. // generate a piecewise linear approximation of
76. // a function of two variables. Note that the
77. // vertices of the simplices generated lie on the function
78. fn generate_approx_2d<
79.     F: Fn(f64, f64) -> f64 + Copy,
80.     D: Fn(f64, f64) -> [f64; 2] + Copy,
81.     // [df/dxdx, df/dydy, df/dxdy]
82.     D2: Fn(f64, f64) -> [f64; 3] + Copy,
83. >(
84.     points: &mut Vec<[f64; 2]>,
85.     tris: &[[usize; 3]],
86.     epsilon: f64,
87.     step: f64,
88.     k: f64,
89.     v: &mut Vec<[usize; 3]>,
90.     f: F,
91.     d: D,
92.     dd: D2,
93. ) {
94.     let small_epsilon = epsilon * EPS_FACTOR;
95.     let len = tris.len();
96.     // initialize a hash map
97.     let mut edges_map = FnvHashMap::with_capacity_and_hasher(len * 2, Default::default());
98.
99.     // a lambda for processing the edges. This makes sure that edges between
100.     // triangles line up
101.     let mut get_edge = |n1: usize, n2: usize| {
102.         let (p1, p2, ccw) = if n1 > n2 {
103.             (n2, n1, false)
104.         } else {
105.             (n1, n2, true)
106.         };
107.         let (line, dist) = norm_line(points[p1], points[p1], points[p2], points[p2]);
108.         if let Some((start, end)) = edges_map.get(&(p1, p2)) {
109.             ((*start, *end), ccw, line)
110.         } else {
111.             let start = points.len();
112.
113.             let vec = generate_approx_1d(
114.                 0.0,
115.                 dist,
116.                 small_epsilon,
117.                 step,
118.                 |x| line.val(x, f),
119.                 |x| line.der(x, d),
120.                 |x| line.der_2(x, dd),
121.             );
122.
123.             for t in &vec {
124.                 let point = line.pos(*t);
125.                 points.push([point.0, point.1]);
126.             }
127.
128.             let end = start + vec.len();
129.             edges_map.insert((p1, p2), (start, end));
130.             ((start, end), ccw, line)
131.         }
132.     };
133.
134.     let mut calls = Vec::with_capacity(len);
135.
136.     for i in 0..len {
137.         let [n1, n2, n3] = tris[i];
138.         let (i1, c1, _) = get_edge(n1, n2);
139.         let (i2, c2, _) = get_edge(n2, n3);
140.         let (i3, c3, _) = get_edge(n3, n1);
141.         calls.push(([n1, n2, n3], [i1, i2, i3], [c1, c2, c3]));
142.     }
143.     generate_2d_approx_loop(epsilon, step, k, points, v, &mut calls, f, d, dd);
144. }
145.
146. // originally I implemented a recursive algorithm, but I was
147. // getting stack overflows so I changed it to use a heap-allocated
148. // list of calls
149. fn generate_2d_approx_loop<
150.     F: Fn(f64, f64) -> f64 + Copy,
151.     D: Fn(f64, f64) -> [f64; 2] + Copy,
152.     // [df/dxdx, df/dydy, df/dxdy]
153.     D2: Fn(f64, f64) -> [f64; 3] + Copy,
154. >(
155.     epsilon: f64,
156.     step: f64,
157.     k: f64,
158.     points: &mut Vec<[f64; 2]>,
159.     // the working vector of triangles
160.     v: &mut Vec<[usize; 3]>,
161.     // this will contain the initial calls needed
162.     stack: &mut Vec<([usize; 3], [(usize, usize); 3], [bool; 3])>,
163.     f: F,
164.     d: D,
165.     dd: D2,
166. ) {
167.     while let Some((bound, edges, ccw)) = stack.pop() {
168.         generate_2d_approx_division(
169.             bound,
170.             edges,
171.             ccw,
172.             epsilon,
173.             step,
174.             k,
175.             v,
176.             points,
177.             stack,
178.             EPS_FACTOR,
179.             CHECK_FACTOR,
180.             f,
181.             d,
182.             dd,
183.         );
184.     }
185. }
186.
187. fn generate_2d_approx_division<
188.     F: Fn(f64, f64) -> f64 + Copy,
189.     D: Fn(f64, f64) -> [f64; 2] + Copy,
190.     // [df/dxdx, df/dydy, df/dxdy]
191.     D2: Fn(f64, f64) -> [f64; 3] + Copy,
192. >(
193.     // bound will always be in CCW order
194.     bound: [usize; 3],
195.     // the positions of points along each edge. This stores
196.     // a start and end index into points.
197.     edges: [(usize, usize); 3],
198.     // the order of points on an edge might be backward since
199.     // neighboring triangles share edges
200.     edge_ccw: [bool; 3],
201.     epsilon: f64,
202.     step: f64,
203.     k: f64,
204.     v: &mut Vec<[usize; 3]>,
205.     points: &mut Vec<[f64; 2]>,
206.     stack: &mut Vec<([usize; 3], [(usize, usize); 3], [bool; 3])>,
207.     factor: f64,
208.     check_factor: f64,
209.     f: F,
210.     d: D,
211.     dd: D2,
212. ) {
213.     let small_epsilon = epsilon * factor;
214.
215.     // get the total length of all the edges
216.     let edges_len = edges.1 - edges.0 + edges.1 - edges.0 + edges.1 - edges.0;
217.
218.     if edges_len == 0 {
219.         // in this case there is no further subdivision neccesary,
220.         // so we check if the triangle described by bound
221.         // is entirely within the epsilon bound.
222.         let check_epsilon = epsilon * check_factor;
223.         let [x1, y1] = points[bound];
224.         let [x2, y2] = points[bound];
225.         let [x3, y3] = points[bound];
226.         let (l1, d1) = norm_line(x1, y1, x2, y2);
227.         let (l2, d2) = norm_line(x2, y2, x3, y3);
228.         let (l3, d3) = norm_line(x3, y3, x1, y1);
229.         let lines = [l1, l2, l3];
230.         let dists = [d1, d2, d3];
231.         let mut high_dist = d1;
232.         let mut high_pos = 0;
233.         for i in 1..3 {
234.             if dists[i] > high_dist {
235.                 high_dist = dists[i];
236.                 high_pos = i;
237.             }
238.         }
239.         let lines = [
240.             lines[high_pos],
241.             lines[(high_pos + 1) % 3],
242.             lines[(high_pos + 2) % 3],
243.         ];
244.         let point = lines.pos(0.0);
245.
246.         let z1 = lines.val(0.0, f);
247.         let z2 = lines.val(0.0, f);
248.         let z_slope = (z2 - z1) / high_dist;
249.         // the lipschitz constant gives us a range for where
250.         // the triangle is within the bound given that a line
251.         // from one vertex to a point on the opposite edge
252.         // is within a smaller bound
253.         let first_step_len = 2.0 * (1.0 - EPS_FACTOR) * epsilon / (k + z_slope.abs());
254.         let other_step_len = 2.0 * (1.0 - CHECK_FACTOR) * epsilon / (k + z_slope.abs());
255.         let mut check_pos = first_step_len;
256.         let mut last = true;
257.
258.         while check_pos < high_dist {
259.             let check_point = lines.pos(check_pos);
260.             let (line, len) = norm_line(point.0, point.1, check_point.0, check_point.1);
261.             // check if the line is within the smaller bound
262.             let check = max_from_point(
263.                 (0.0, line.val(0.0, f)),
264.                 0.0,
265.                 check_epsilon,
266.                 step,
267.                 len,
268.                 |x| line.val(x, f),
269.                 |x| line.der(x, d),
270.                 |x| line.der_2(x, dd),
271.             );
272.
273.             if let Err((min_slope, max_slope)) = check {
274.                 let end_z = z1 + check_pos * z_slope;
275.                 let end_slope = (end_z - line.val(0.0, f)) / len;
276.                 if min_slope <= end_slope && end_slope <= max_slope {
277.                     check_pos += other_step_len;
278.                     continue;
279.                 }
280.             }
281.             // if we reach this point then we need to further subdivide
282.             // the triangle. This is generally inneficient, and the optimal
283.             // value of check_factor will generally cause this case to
284.             // only occur a few times
285.             last = false;
286.             let factor = 0.5;
287.             let center_point = line.pos(len * factor);
288.             let center_pos = points.len();
289.             points.push([center_point.0, center_point.1]);
290.             let (line_0, len_0) = norm_line(lines.x, lines.y, center_point.0, center_point.1);
291.             let (line_1, len_1) = norm_line(lines.x, lines.y, center_point.0, center_point.1);
292.             let div_lens = [len_0, len_1, len * factor];
293.             let div_lines = [line_0, line_1, line];
294.             let mut line_points = [(0, 0); 3];
295.             for i in 0..3 {
296.                 // it is possible that the lines to the new point
297.                 // aren't within the small_epsilon bound
298.                 let approx = generate_approx_1d(
299.                     0.0,
300.                     div_lens[i],
301.                     small_epsilon,
302.                     step,
303.                     |x| div_lines[i].val(x, f),
304.                     |x| div_lines[i].der(x, d),
305.                     |x| div_lines[i].der_2(x, dd),
306.                 );
307.                 let index = points.len();
308.                 line_points[i] = (index, index + approx.len());
309.                 for t in &approx {
310.                     let point = div_lines[i].pos(*t);
311.                     points.push([point.0, point.1]);
312.                 }
313.             }
314.             for i in 0..3 {
315.                 let next = (i + 1) % 3;
316.                 stack.push((
317.                     [center_pos, bound[i], bound[next]],
318.                     // the index 3 is an empty vec
319.                     [line_points[i], (0, 0), line_points[next]],
320.                     [false, true, true],
321.                 ));
322.             }
323.             break;
324.         }
325.
326.         if last {
327.             v.push(bound);
328.         }
329.         return;
330.     }
331.     // generate a list to help process triangles
332.     let mut index_vec = Vec::with_capacity(3 + edges_len);
333.     let mut edge_boundaries = [0; 4];
334.     for i in 0..3 {
335.         index_vec.push(bound[i]);
336.         let edge_len = edges[i].1 - edges[i].0;
337.         for k in 0..edge_len {
338.             let n = if edge_ccw[i] {
339.                 edges[i].0 + k
340.             } else {
341.                 edges[i].1 - 1 - k
342.             };
343.             index_vec.push(n);
344.         }
345.         edge_boundaries[i + 1] = index_vec.len();
346.     }
347.     let triangulation = get_triangulation(edge_boundaries);
348.     let len = triangulation.len() / 3;
349.     let mut edges_map = FnvHashMap::with_capacity_and_hasher((3 * len) / 2, Default::default());
350.
351.     let get_edge =
352.         |edges_map: &mut FnvHashMap<_, _>, points: &mut Vec<[f64; 2]>, n1: usize, n2: usize| -> ((usize, usize), [f64; 2], f64, bool) {
353.             let (p1, p2, ccw) = if n1 > n2 {
354.                 (n2, n1, false)
355.             } else {
356.                 (n1, n2, true)
357.             };
358.
359.             // check if the points lie on the same edge
360.             let mut p1_edge = 0;
361.             let mut p2_edge = 0;
362.
363.             for i in 0..3 {
364.                 if p1 < edge_boundaries[3 - i] {
365.                     p1_edge = 2 - i;
366.                 }
367.                 if p2 > edge_boundaries[i] {
368.                     p2_edge = i;
369.                 }
370.             }
371.
372.             let point1 = points[index_vec[p1]];
373.             let point2 = points[index_vec[p2]];
374.
375.             let (edge_line, dist) = norm_line(point1, point1, point2, point2);
376.             let mid = [(point1 + point2) / 2.0, (point1 + point2) / 2.0];
377.
378.             // if both points lie on the same edge, then we don't want to
379.             // generate a new approx, as that would cause point aliasing
380.             if (p1_edge == p2_edge) || (p1 == 0 && p2 >= edge_boundaries) {
381.                 if p2 - p1 == 1 || (p1 == 0 && edge_boundaries - p2 == 1) {
382.                     if let Some((start, end)) = edges_map.get(&(p1, p2)) {
383.                         // this case occurs when an edge of an obtuse triangle
384.                         // is subdivided
385.                         return ((*start, *end), mid, dist, ccw);
386.                     }
387.                     return ((0, 0), mid, dist, ccw);
388.                 } else {
389.                     let (edge, start, end) = if p1 == 0 && p2 >= edge_boundaries {
390.                         (
391.                             2,
392.                             p2 - edge_boundaries,
393.                             edge_boundaries - edge_boundaries,
394.                         )
395.                     } else {
396.                         (
397.                             p1_edge,
398.                             p1 - edge_boundaries[p1_edge],
399.                             p2 - edge_boundaries[p1_edge],
400.                         )
401.                     };
402.                     let ccw = edge_ccw[edge];
403.                     let (index_start, index_end) = if ccw {
404.                         (edges[edge].0 + start, edges[edge].0 + end - 1)
405.                     } else {
406.                         (edges[edge].1 - end + 1, edges[edge].1 - start)
407.                     };
408.                     return ((index_start, index_end), mid, dist, ccw);
409.                 }
410.             }
411.
412.             // check if the edge has already been processed
413.             if let Some((start, end)) = edges_map.get(&(p1, p2)) {
414.                 return ((*start, *end), mid, dist, ccw);
415.             } else {
416.                 let approx = generate_approx_1d(
417.                     0.0,
418.                     dist,
419.                     small_epsilon,
420.                     step,
421.                     |x| edge_line.val(x, f),
422.                     |x| edge_line.der(x, d),
423.                     |x| edge_line.der_2(x, dd),
424.                 );
425.                 let index = points.len();
426.                 let end = index + approx.len();
427.                 edges_map.insert((p1, p2), (index, end));
428.
429.                 for t in &approx {
430.                     let point = edge_line.pos(*t);
431.                     points.push([point.0, point.1]);
432.                 }
433.
434.                 ((index, end), mid, dist, ccw)
435.             }
436.         };
437.
438.     let insert_mid = |edges_map: &mut FnvHashMap<_, _>, n1, n2, index| {
439.         let (p1, p2) = if n1 > n2 { (n2, n1) } else { (n1, n2) };
440.         edges_map.insert((p1, p2), (index, index + 1));
441.     };
442.
443.     // the first time through is used to check for obtuse triangles
444.     // which can cause an infinite loop. The adjustments made to
445.     // fix this need to also change the edges on neighboring triangles
446.     for i in 0..len {
447.         let tri = &triangulation[i * 3..(i * 3) + 3];
448.         let edges = [
449.             get_edge(&mut edges_map, points, tri, tri),
450.             get_edge(&mut edges_map, points, tri, tri),
451.             get_edge(&mut edges_map, points, tri, tri),
452.         ];
453.         for k in 0..3 {
454.             let edgek = edges[k].0;
455.             if edgek.1 - edgek.0 == 0 {
456.                 let next1 = (k + 1) % 3;
457.                 let next2 = (k + 2) % 3;
458.                 let (i1, mid1, d1, _) = edges[next1];
459.                 let (i2, mid2, d2, _) = edges[next2];
460.                 let mid0 = edges[k].1;
461.                 let d0 = edges[k].2;
462.                 let len1 = i1.1 - i1.0;
463.                 let len2 = i2.1 - i2.0;
464.                 if d1 * d1 + d2 * d2 <= d0 * d0 {
465.                     if len1 > 0 || len2 > 0 {
466.                         let index = points.len();
467.                         points.push(mid0);
468.                         insert_mid(&mut edges_map, tri[k], tri[next1], index);
469.                         if len1 == 0 {
470.                             points.push(mid1);
471.                             insert_mid(&mut edges_map, tri[next1], tri[next2], index + 1);
472.                         }
473.                         if len2 == 0 {
474.                             points.push(mid2);
475.                             insert_mid(&mut edges_map, tri[next2], tri[k], index + 1);
476.                         }
477.                     }
478.                     break;
479.                 }
480.             }
481.         }
482.     }
483.
484.     // now we run through to actually add the calls to the stack,
485.     // with the adjustments made to the neccesary edges
486.     for i in 0..len {
487.         let tri = &triangulation[i * 3..(i * 3) + 3];
488.         let edges = [
489.             get_edge(&mut edges_map, points, tri, tri),
490.             get_edge(&mut edges_map, points, tri, tri),
491.             get_edge(&mut edges_map, points, tri, tri),
492.         ];
493.
494.         let bound = [index_vec[tri], index_vec[tri], index_vec[tri]];
495.         let (edge1, _, _, ccw1) = edges;
496.         let (edge2, _, _, ccw2) = edges;
497.         let (edge3, _, _, ccw3) = edges;
498.         stack.push((bound, [edge1, edge2, edge3], [ccw1, ccw2, ccw3]));
499.     }
500. }
501.
502. // generates the indices for a triangulation given the number of
503. // points on each edge
504. fn get_triangulation(edges: [usize; 4]) -> Vec<usize> {
505.     let mut v = Vec::with_capacity(3);
506.     for i in 0..3 {
507.         v.push((edges[i] + edges[i + 1]) / 2);
508.     }
509.     for i in 0..3 {
510.         let prev = (i + 2) % 3;
511.         if edges[i + 1] - edges[i] > 1 {
512.             v.extend_from_slice(&[edges[i], v[i], v[prev]]);
513.         }
514.     }
515.     v
516. }
517.
518. // generate a 1 dimensional approximation where the endpoints
519. // lie on the function. Note that this is not an optimal approximation
520. fn generate_approx_1d<
521.     F: Fn(f64) -> f64 + Copy,
522.     D: Fn(f64) -> f64 + Copy,
523.     D2: Fn(f64) -> f64 + Copy,
524. >(
525.     start: f64,
526.     end: f64,
527.     epsilon: f64,
528.     step: f64,
529.     f: F,
530.     d: D,
531.     dd: D2,
532.     // the output does not include the first and last points
533. ) -> Vec<f64> {
534.     // create the list to hold the results
535.     let mut v = Vec::new();
536.
537.     let mut prev = max_from_pos(start, start, epsilon, step, end, f, d, dd);
538.
539.     // loop until we reach the max
540.     while let Some((_, x, _, _)) = prev {
541.         if x == end {
542.             break;
543.         }
544.         v.push(x);
545.         // the end of the previous line tells us the value of s_epsilon to use here
546.         prev = max_from_pos(x, x, epsilon, step, end, f, d, dd);
547.     }
548.     // we go back through the approximation to cause it to be
549.     // spaces more evenly if possible
550.     let n = v.len();
551.     let equal_len = (end - start) / (n + 1) as f64;
552.     let mut back = max_from_pos(
553.         -end,
554.         -end,
555.         epsilon,
556.         step,
557.         -start,
558.         |x| f(-x),
559.         |x| -d(-x),
560.         |x| dd(-x),
561.     );
562.     for i in 0..n {
563.         let min = if let Some((_, neg_x, _, _)) = back {
564.             -neg_x
565.         } else {
566.             start
567.         };
568.         let dist = start + (n - i) as f64 * equal_len;
569.         let mut new_point = v[n - i - 1];
570.         if dist < new_point {
571.             new_point = if min < dist { dist } else { min };
572.         } else {
573.             break;
574.         }
575.         v[n - i - 1] = new_point;
576.         back = max_from_pos(
577.             -new_point,
578.             -new_point,
579.             epsilon,
580.             step,
581.             -start,
582.             |x| f(-x),
583.             |x| -d(-x),
584.             |x| dd(-x),
585.         );
586.     }
587.     return v;
588. }
589.
590. // find a line segment within the epsilon bound with endpoints
591. // on the function
592. fn max_from_pos<F: Fn(f64) -> f64, D: Fn(f64) -> f64, D2: Fn(f64) -> f64>(
593.     point: f64,
594.     start: f64,
595.     epsilon: f64,
596.     step: f64,
597.     max_x: f64,
598.     f: F,
599.     d: D,
600.     dd: D2,
601. ) -> Option<(f64, f64, f64, bool)> {
602.     let mut test_x = start;
603.     let mut prev_x;
604.     let mut steps = 1.0;
605.     let point_y = f(point);
606.     let get_slope = |x, y| (y - point_y) / (x - point);
607.     let mut low_upper = f64::INFINITY;
608.     let mut upper_point = start;
609.     let mut upper_rdir = low_upper - d(start);
610.     let mut high_lower = -f64::INFINITY;
611.     let upper_start = low_upper;
612.     let lower_start = high_lower;
613.     let mut lower_point = start;
614.     let mut lower_rdir = high_lower - d(start);
615.     let mut prev_dd = dd(start);
616.     let mut rdir = 0.0;
617.
618.     loop {
619.         prev_x = test_x;
620.         test_x = start + steps * step;
621.
622.         if test_x > max_x {
623.             test_x = max_x;
624.         }
625.
626.         let mut new_dd = dd(test_x);
627.
628.         let mut should_step = true;
629.
630.         if prev_dd * new_dd < 0.0 {
631.             test_x = bisect(prev_x, test_x, |x| dd(x), TOL);
632.             new_dd = 0.0;
633.             should_step = false;
634.         }
635.
636.         let mut test_y = f(test_x);
637.         let mut slope = get_slope(test_x, test_y);
638.         let mut new_rdir = slope - d(test_x);
639.         if new_rdir * rdir < 0.0 {
640.             test_x = find_root(
641.                 prev_x,
642.                 test_x,
643.                 None,
644.                 |x| get_slope(x, f(x)) - d(x),
645.                 |x| {
646.                     let y = f(x);
647.                     let bottom = x - point;
648.                     (bottom * d(x) - (y - point_y)) / (bottom * bottom) - dd(x)
649.                 },
650.                 TOL,
651.             );
652.             test_y = f(test_x);
653.             new_rdir = 0.0;
654.             should_step = false;
655.             slope = get_slope(test_x, test_y);
656.         }
657.
658.         if should_step {
659.             steps += 1.0;
660.         }
661.         rdir = new_rdir;
662.
663.         let upper_slope = get_slope(test_x, test_y + epsilon);
664.         let lower_slope = get_slope(test_x, test_y - epsilon);
665.
666.         let dir = d(test_x);
667.         let new_upper_rdir = upper_slope - dir;
668.         let new_lower_rdir = lower_slope - dir;
669.
670.         let mut new_low_upper = low_upper;
671.         let mut new_high_lower = high_lower;
672.         let mut new_lower_point = lower_point;
673.         let mut new_upper_point = upper_point;
674.
675.         if upper_rdir > 0.0 && new_upper_rdir < 0.0 {
676.             let find = bisect(
677.                 prev_x,
678.                 test_x,
679.                 |x| {
680.                     if x == point {
681.                         upper_start - d(x)
682.                     } else {
683.                         get_slope(x, f(x) + epsilon) - d(x)
684.                     }
685.                 },
686.                 TOL,
687.             );
688.             let find_dir = get_slope(find, f(find) + epsilon);
689.             if find_dir <= low_upper {
690.                 new_low_upper = find_dir;
691.                 new_upper_point = find;
692.             }
693.         }
694.
695.         if lower_rdir < 0.0 && new_lower_rdir > 0.0 {
696.             let find = bisect(
697.                 prev_x,
698.                 test_x,
699.                 |x| {
700.                     if x == point {
701.                         lower_start - d(x)
702.                     } else {
703.                         get_slope(x, f(x) - epsilon) - d(x)
704.                     }
705.                 },
706.                 TOL,
707.             );
708.             let find_dir = get_slope(find, f(find) - epsilon);
709.             if find_dir >= high_lower {
710.                 new_high_lower = find_dir;
711.                 new_lower_point = find;
712.             }
713.         }
714.
715.         low_upper = new_low_upper;
716.         high_lower = new_high_lower;
717.         lower_point = new_lower_point;
718.         upper_point = new_upper_point;
719.
720.         if slope == low_upper {
721.             return Some((low_upper, test_x, upper_point, true));
722.         }
723.
724.         if slope == high_lower {
725.             return Some((high_lower, test_x, lower_point, false));
726.         }
727.
728.         if slope > low_upper {
729.             let find_x = find_root(
730.                 prev_x,
731.                 test_x,
732.                 None,
733.                 |x| {
734.                     if x == start {
735.                         d(x) - low_upper
736.                     } else {
737.                         get_slope(x, f(x)) - low_upper
738.                     }
739.                 },
740.                 |x| {
741.                     let y = f(x);
742.                     let bottom = x - point;
743.                     (bottom * d(x) - (y - point_y)) / (bottom * bottom)
744.                 },
745.                 TOL,
746.             );
747.             return Some((low_upper, find_x, upper_point, true));
748.         }
749.
750.         if slope < high_lower {
751.             let find_x = find_root(
752.                 prev_x,
753.                 test_x,
754.                 None,
755.                 |x| {
756.                     if x == start {
757.                         d(x) - high_lower
758.                     } else {
759.                         get_slope(x, f(x)) - high_lower
760.                     }
761.                 },
762.                 |x| {
763.                     let y = f(x);
764.                     let bottom = x - point;
765.                     (bottom * d(x) - (y - point_y)) / (bottom * bottom)
766.                 },
767.                 TOL,
768.             );
769.             return Some((high_lower, find_x, lower_point, false));
770.         }
771.
772.         if test_x == max_x {
773.             return None;
774.         }
775.
776.         upper_rdir = new_upper_rdir;
777.         lower_rdir = new_lower_rdir;
778.         prev_dd = new_dd;
779.     }
780. }
781.
782. // generate a line and normalize the vector
783. fn norm_line(x1: f64, y1: f64, x2: f64, y2: f64) -> (Line, f64) {
784.     let dx = x2 - x1;
785.     let dy = y2 - y1;
786.     let dist = (dx * dx + dy * dy).sqrt();
787.     let l = Line {
788.         x: x1,
789.         y: y1,
790.         dir_x: dx / dist,
791.         dir_y: dy / dist,
792.     };
793.     (l, dist)
794. }
795.
796. // used in the oracle
797. fn max_from_point<F: Fn(f64) -> f64, D: Fn(f64) -> f64, D2: Fn(f64) -> f64>(
798.     point: (f64, f64),
799.     start: f64,
800.     epsilon: f64,
801.     step: f64,
802.     max_x: f64,
803.     f: F,
804.     d: D,
805.     dd: D2,
806. ) -> Result<(f64, f64, f64, bool), (f64, f64)> {
807.     let mut test_x = start;
808.     let mut prev_x;
809.     let mut steps = 1.0;
810.     let get_slope = |x, y| (y - point.1) / (x - point.0);
811.     let mut low_upper = if start == point.0 {
812.         if (f(start) + epsilon) == point.1 {
813.             d(start)
814.         } else {
815.             f64::INFINITY
816.         }
817.     } else {
818.         get_slope(start, f(start) + epsilon)
819.     };
820.     let mut upper_point = start;
821.     let mut upper_rdir = low_upper - d(start);
822.     let mut high_lower = if start == point.0 {
823.         if (f(start) - epsilon) == point.1 {
824.             d(start)
825.         } else {
826.             -f64::INFINITY
827.         }
828.     } else {
829.         get_slope(start, f(start) - epsilon)
830.     };
831.     let upper_start = low_upper;
832.     let lower_start = high_lower;
833.     let mut lower_point = start;
834.     let mut lower_rdir = high_lower - d(start);
835.     let mut prev_dd = dd(start);
836.
837.     let hit_upper;
838.
839.     loop {
840.         prev_x = test_x;
841.         test_x = start + steps * step;
842.
843.         if test_x > max_x {
844.             test_x = max_x;
845.         }
846.
847.         let mut new_dd = dd(test_x);
848.
849.         if prev_dd * new_dd < 0.0 {
850.             test_x = bisect(prev_x, test_x, |x| dd(x), TOL);
851.             new_dd = 0.0;
852.         } else {
853.             steps += 1.0;
854.         }
855.
856.         let test_y = f(test_x);
857.         let upper_slope = get_slope(test_x, test_y + epsilon);
858.         let lower_slope = get_slope(test_x, test_y - epsilon);
859.         let dir = d(test_x);
860.         let new_upper_rdir = upper_slope - dir;
861.         let new_lower_rdir = lower_slope - dir;
862.
863.         let mut new_low_upper = low_upper;
864.         let mut new_high_lower = high_lower;
865.         let mut new_lower_point = lower_point;
866.         let mut new_upper_point = upper_point;
867.
868.         let mut found_upper = false;
869.         let mut found_lower = false;
870.
871.         if upper_slope <= low_upper {
872.             new_low_upper = upper_slope;
873.             new_upper_point = test_x;
874.         }
875.         if upper_rdir > 0.0 && new_upper_rdir < 0.0 {
876.             let find = bisect(
877.                 prev_x,
878.                 test_x,
879.                 |x| {
880.                     if x == point.0 {
881.                         upper_start
882.                     } else {
883.                         get_slope(x, f(x) + epsilon) - d(x)
884.                     }
885.                 },
886.                 TOL,
887.             );
888.             let find_dir = get_slope(find, f(find) + epsilon); // d(find);
889.             if find_dir <= low_upper {
890.                 new_low_upper = find_dir;
891.                 new_upper_point = find;
892.                 found_upper = true;
893.             }
894.         }
895.
896.         if lower_slope >= high_lower {
897.             new_high_lower = lower_slope;
898.             new_lower_point = test_x;
899.         }
900.
901.         if lower_rdir < 0.0 && new_lower_rdir > 0.0 {
902.             let find = bisect(
903.                 prev_x,
904.                 test_x,
905.                 |x| {
906.                     if x == point.0 {
907.                         lower_start
908.                     } else {
909.                         get_slope(x, f(x) - epsilon) - d(x)
910.                     }
911.                 },
912.                 TOL,
913.             );
914.             let find_dir = get_slope(find, f(find) - epsilon); // d(find);
915.             if find_dir >= high_lower {
916.                 new_high_lower = find_dir;
917.                 new_lower_point = find;
918.                 found_lower = true;
919.             }
920.         }
921.
922.         if found_upper {
923.             if new_low_upper < high_lower {
924.                 upper_point = new_upper_point;
925.                 hit_upper = true;
926.                 break;
927.             }
928.         }
929.
930.         if found_lower {
931.             if low_upper < new_high_lower {
932.                 lower_point = new_lower_point;
933.                 hit_upper = false;
934.                 break;
935.             }
936.         }
937.
938.         low_upper = new_low_upper;
939.         high_lower = new_high_lower;
940.         lower_point = new_lower_point;
941.         upper_point = new_upper_point;
942.
943.         // when this occurs, found_lower and found_upper are both true
944.         if low_upper < high_lower {
945.             hit_upper = upper_point > lower_point;
946.             break;
947.         }
948.
949.         if test_x == max_x {
950.             return Err((high_lower, low_upper));
951.         }
952.
953.         upper_rdir = new_upper_rdir;
954.         lower_rdir = new_lower_rdir;
955.         prev_dd = new_dd;
956.     }
957.
958.     if hit_upper {
959.         let find_x = find_root(
960.             prev_x,
961.             upper_point,
962.             None,
963.             |x| get_slope(x, f(x) + epsilon) - high_lower,
964.             |x| {
965.                 let y = f(x) + epsilon;
966.                 let bottom = x - point.0;
967.                 (bottom * d(x) - (y - point.1)) / (bottom * bottom)
968.             },
969.             TOL,
970.         );
971.         return Ok((high_lower, find_x, lower_point, true));
972.     } else {
973.         let find_x = find_root(
974.             prev_x,
975.             lower_point,
976.             None,
977.             |x| get_slope(x, f(x) - epsilon) - low_upper,
978.             |x| {
979.                 let y = f(x) - epsilon;
980.                 let bottom = x - point.0;
981.                 (bottom * d(x) - (y - point.1)) / (bottom * bottom)
982.             },
983.             TOL,
984.         );
985.
986.         return Ok((low_upper, find_x, upper_point, false));
987.     }
988. }
989.
990. // a struct to help with directional first and second derivatives
991. #[derive(Clone, Copy, Debug)]
992. pub struct Line {
993.     x: f64,
994.     y: f64,
995.     dir_x: f64,
996.     dir_y: f64,
997. }
998.
999. impl Line {
1000.     #[inline]
1001.     fn pos(self, t: f64) -> (f64, f64) {
1002.         (self.x + t * self.dir_x, self.y + t * self.dir_y)
1003.     }
1004.
1005.     #[inline]
1006.     fn val<F: Fn(f64, f64) -> f64>(self, t: f64, f: F) -> f64 {
1007.         let x = self.x + t * self.dir_x;
1008.         let y = self.y + t * self.dir_y;
1009.         f(x, y)
1010.     }
1011.
1012.     #[inline]
1013.     fn der<D: Fn(f64, f64) -> [f64; 2]>(self, t: f64, d: D) -> f64 {
1014.         let x = self.x + t * self.dir_x;
1015.         let y = self.y + t * self.dir_y;
1016.         let [dfdx, dfdy] = d(x, y);
1017.         self.dir_x * dfdx + self.dir_y * dfdy
1018.     }
1019.
1020.     #[inline]
1021.     fn der_2<D2: Fn(f64, f64) -> [f64; 3]>(self, t: f64, dd: D2) -> f64 {
1022.         let x = self.x + t * self.dir_x;
1023.         let y = self.y + t * self.dir_y;
1024.         let [dxdx, dydy, dxdy] = dd(x, y);
1025.         let x_part = self.dir_x * dxdx + self.dir_y * dxdy;
1026.         let y_part = self.dir_x * dxdy + self.dir_y * dydy;
1027.         self.dir_x * x_part + self.dir_y * y_part
1028.     }
1029. }
1030.
1031. fn bisect<F: Fn(f64) -> f64>(mut start: f64, mut end: f64, f: F, tolerance: f64) -> f64 {
1032.     let mut f_start = f(start);
1033.     let f_end = f(end);
1034.     if f_start == 0.0 {
1035.         return f_start;
1036.     }
1037.     if f_end == 0.0 {
1038.         return f_end;
1039.     }
1040.     while end - start > tolerance {
1041.         let mid = (start + end) / 2.0;
1042.         if mid == start || mid == end {
1043.             return mid;
1044.         }
1045.         let f_mid = f(mid);
1046.         if f_mid == 0.0 {
1047.             return mid;
1048.         }
1049.         if (f_mid > 0.0) != (f_start > 0.0) {
1050.             end = mid;
1051.         } else {
1052.             start = mid;
1053.             f_start = f_mid;
1054.         }
1055.     }
1056.     return (start + end) / 2.0;
1057. }
1058.
1059. // attempts to use newton's method, but switches to bisection
1060. // if it doesn't converge (or converge to the right root).
1061. fn find_root<F: Fn(f64) -> f64, D: Fn(f64) -> f64>(
1062.     mut start: f64,
1063.     mut end: f64,
1064.     mut initial: Option<f64>,
1065.     f: F,
1066.     d: D,
1067.     tolerance: f64,
1068. ) -> f64 {
1069.     // every iteration of this loop causes the range to get smaller
1070.     // so it will eventually be smaller than tol
1071.     // (assuming the tolerance is positive and numerical coarseness doesn't
1072.     // cause problems)
1073.     loop {
1074.         let mut x = if let Some(x) = initial {
1075.             x
1076.         } else {
1077.             (start + end) / 2.0
1078.         };
1079.         let mut n = 0;
1080.         // newton's method
1081.         // in theory this could get stuck in a cycle, so
1082.         // we need a condition to prevent the loop from going
1083.         // infinite. In most cases the break will terminate
1084.         // this loop before the while condition does
1085.         while n < 8 {
1086.             n += 1;
1087.             let offset = f(x) / d(x);
1088.             if offset.abs() < tolerance {
1089.                 return x - offset;
1090.             } else {
1091.                 x = x - offset;
1092.             }
1093.             // < and >= are not exact negations of eachother
1094.             // because of NaN. This expression will trigger
1095.             // if x is NaN, which makes this more stable.
1096.             // this condition will also work if d(x) is 0.0,
1097.             // which will cause offset to be +-infinity
1098.             if !(end < x && x < start) {
1099.                 break;
1100.             }
1101.         }
1102.         let mid = (start + end) / 2.0;
1103.         let f_mid = f(mid);
1104.         // bisection
1105.         if mid == start || mid == end {
1106.             return mid;
1107.         }
1108.         if (f_mid < 0.0) != (f(end) < 0.0) {
1109.             start = mid;
1110.         } else {
1111.             end = mid;
1112.         }
1113.         initial = None;
1114.         if (end - start) < tolerance {
1115.             return mid;
1116.         }
1117.     }
1118. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.

Top