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][0], points[p1][1], points[p2][0], points[p2][1]);
  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[0].1 - edges[0].0 + edges[1].1 - edges[1].0 + edges[2].1 - edges[2].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[0]];
  224.         let [x2, y2] = points[bound[1]];
  225.         let [x3, y3] = points[bound[2]];
  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[2].pos(0.0);
  245.  
  246.         let z1 = lines[0].val(0.0, f);
  247.         let z2 = lines[1].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[0].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[0].x, lines[0].y, center_point.0, center_point.1);
  291.             let (line_1, len_1) = norm_line(lines[1].x, lines[1].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[0], point1[1], point2[0], point2[1]);
  376.             let mid = [(point1[0] + point2[0]) / 2.0, (point1[1] + point2[1]) / 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[2]) {
  381.                 if p2 - p1 == 1 || (p1 == 0 && edge_boundaries[3] - 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[2] {
  390.                         (
  391.                             2,
  392.                             p2 - edge_boundaries[2],
  393.                             edge_boundaries[3] - edge_boundaries[2],
  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[0], tri[1]),
  450.             get_edge(&mut edges_map, points, tri[1], tri[2]),
  451.             get_edge(&mut edges_map, points, tri[2], tri[0]),
  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[0], tri[1]),
  490.             get_edge(&mut edges_map, points, tri[1], tri[2]),
  491.             get_edge(&mut edges_map, points, tri[2], tri[0]),
  492.         ];
  493.  
  494.         let bound = [index_vec[tri[0]], index_vec[tri[1]], index_vec[tri[2]]];
  495.         let (edge1, _, _, ccw1) = edges[0];
  496.         let (edge2, _, _, ccw2) = edges[1];
  497.         let (edge3, _, _, ccw3) = edges[2];
  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. OK, I Understand
 
Top