API tools faq
paste
Advertisement
matthewrmata

Strike Zone Optimization Algorithms

matthewrmata
Nov 1st, 2015
227
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
R 18.89 KB | None | 0 0
raw download clone embed print report
  1. ###########################
  2. # x-, y-, and z-Functions #
  3. ###########################
  4.  
  5. x_eval <- function(PFX,t){
  6.     return(0.5*PFX$ax*t^2 + PFX$vx0*t + PFX$x0);
  7. }
  8.  
  9. y_eval <- function(PFX,t){
  10.     return(0.5*PFX$ay*t^2 + PFX$vy0*t + PFX$y0);
  11. }
  12.  
  13. z_eval <- function(PFX,t){
  14.     return(0.5*PFX$az*t^2 + PFX$vz0*t + PFX$z0);
  15. }
  16.  
  17. ##############################
  18. # vx-, vy-, and vz-Functions #
  19. ##############################
  20.  
  21. vx_eval <- function(PFX,t){
  22.     return(PFX$ax*t + PFX$vx0);
  23. }
  24.  
  25. vy_eval <- function(PFX,t){
  26.     return(PFX$ay*t + PFX$vy0);
  27. }
  28.  
  29. vz_eval <- function(PFX,t){
  30.     return(PFX$az*t + PFX$vz0);
  31. }
  32.  
  33. ######################
  34. # Distance to a Face #
  35. ######################
  36.  
  37. min_face_x <- function(PFX,x_f,t_min,t_max){
  38.     dist_t_min <- dist_face_x(PFX,x_f,t_min);
  39.     dist_t_max <- dist_face_x(PFX,x_f,t_max);
  40.     dist_sz_min <- min(dist_t_min,dist_t_max);
  41.     disc <- PFX$vx0^2 - 2*PFX$ax*(PFX$x0-x_f);
  42.     if (PFX$ax != 0){
  43.         if(disc > 0){
  44.             root_1 <- (-PFX$vx0 - sqrt(disc))/PFX$ax;
  45.             if (root_1 > t_min && root_1 < t_max){
  46.                 dist_root_1 <- dist_face_x(PFX,x_f,root_1);
  47.                 if (dist_root_1 < dist_sz_min){
  48.                     dist_sz_min <- dist_root_1;
  49.                 }
  50.             }
  51.             root_2 <- (-PFX$vx0 + sqrt(disc))/PFX$ax;
  52.             if (root_2 > t_min && root_2 < t_max){
  53.                 dist_root_2 <- dist_face_x(PFX,x_f,root_2);
  54.                 if (dist_root_2 < dist_sz_min){
  55.                     dist_sz_min <- dist_root_2;
  56.                 }
  57.             }
  58.         }
  59.         root <- -PFX$vx0/PFX$ax;
  60.         if (root > t_min && root < t_max){
  61.             dist_root <- dist_face_x(PFX,x_f,root);
  62.             if (dist_root < dist_sz_min){
  63.                 dist_sz_min <- dist_root;
  64.             }
  65.         }
  66.     } else {
  67.         root <- -(PFX$x0-x_f)/PFX$vx0;
  68.         if (root > t_min && root < t_max){
  69.             dist_root <- dist_face_x(PFX,x_f,root);
  70.             if (dist_root < dist_sz_min){
  71.                 dist_sz_min <- dist_root;
  72.             }
  73.         }
  74.     }
  75.     return(dist_sz_min);
  76. }
  77.  
  78. dist_face_x <- function(PFX,x_f,t_f){
  79.     return(abs(x_eval(PFX,t_f) - x_f));
  80. }
  81.  
  82. min_face_y <- function(PFX,y_f,t_min,t_max){
  83.     dist_t_min <- dist_face_y(PFX,y_f,t_min);
  84.     dist_t_max <- dist_face_y(PFX,y_f,t_max);
  85.     dist_sz_min <- min(dist_t_min,dist_t_max);
  86.     disc <- PFX$vy0^2 - 2*PFX$ay*(PFX$y0-y_f);
  87.     if(PFX$ay != 0){
  88.         if(disc > 0){
  89.             root_1 <- (-PFX$vy0 - sqrt(disc))/PFX$ay;
  90.             if (root_1 > t_min && root_1 < t_max){
  91.                 dist_root_1 <- dist_face_y(PFX,y_f,root_1);
  92.                 if (dist_root_1 < dist_sz_min){
  93.                     dist_sz_min <- dist_root_1;
  94.                 }
  95.             }
  96.             root_2 <- (-PFX$vy0 + sqrt(disc))/PFX$ay;
  97.             if (root_2 > t_min && root_2 < t_max){
  98.                 dist_root_2 <- dist_face_y(PFX,y_f,root_2);
  99.                 if (dist_root_2 < dist_sz_min){
  100.                     dist_sz_min <- dist_root_2;
  101.                 }
  102.             }
  103.         }
  104.         root <- -PFX$vy0/PFX$ay;
  105.         if (root > t_min && root < t_max){
  106.             dist_root <- dist_face_y(PFX,y_f,root);
  107.             if (dist_root < dist_sz_min){
  108.                 dist_sz_min <- dist_root;
  109.             }
  110.         }
  111.     } else {
  112.         root <- -(PFX$y0-y_f)/PFX$vy0;
  113.         if (root > t_min && root < t_max){
  114.             dist_root <- dist_face_y(PFX,y_f,root);
  115.             if (dist_root < dist_sz_min){
  116.                 dist_sz_min <- dist_root;
  117.             }
  118.         }
  119.     }
  120.     return(dist_sz_min);
  121. }
  122.  
  123. dist_face_y <- function(PFX,y_f,t_f){
  124.     return(abs(y_eval(PFX,t_f) - y_f));
  125. }
  126.  
  127. min_face_z <- function(PFX,z_f,t_min,t_max){
  128.     dist_t_min <- dist_face_z(PFX,z_f,t_min);
  129.     dist_t_max <- dist_face_z(PFX,z_f,t_max);
  130.     dist_sz_min <- min(dist_t_min,dist_t_max);
  131.     disc <- PFX$vz0^2 - 2*PFX$az*(PFX$z0-z_f);
  132.     if(PFX$az != 0){
  133.         if(disc > 0){
  134.             root_1 <- (-PFX$vz0 - sqrt(disc))/PFX$az;
  135.             if (root_1 > t_min && root_1 < t_max){
  136.                 dist_root_1 <- dist_face_z(PFX,z_f,root_1);
  137.                 if (dist_root_1 < dist_sz_min){
  138.                     dist_sz_min <- dist_root_1;
  139.                 }
  140.             }
  141.             root_2 <- (-PFX$vz0 + sqrt(disc))/PFX$az;
  142.             if (root_2 > t_min && root_2 < t_max){
  143.                 dist_root_2 <- dist_face_z(PFX,z_f,root_2);
  144.                 if (dist_root_2 < dist_sz_min){
  145.                     dist_sz_min <- dist_root_2;
  146.                 }
  147.             }
  148.         }
  149.         root <- -PFX$vz0/PFX$az;
  150.         if (root > t_min && root < t_max){
  151.             dist_root <- dist_face_z(PFX,z_f,root);
  152.             if (dist_root < dist_sz_min){
  153.                 dist_sz_min <- dist_root;
  154.             }
  155.         }
  156.     } else {
  157.         root <- -(PFX$z0-z_f)/PFX$vz0;
  158.         if (root > t_min && root < t_max){
  159.             dist_root <- dist_face_z(PFX,z_f,root);
  160.             if (dist_root < dist_sz_min){
  161.                 dist_sz_min <- dist_root;
  162.             }
  163.         }
  164.     }
  165.     return(dist_sz_min);
  166. }
  167.  
  168. dist_face_z <- function(PFX,z_f,t_f){
  169.     return(abs(z_eval(PFX,t_f) - z_f));
  170. }
  171.    
  172. ###############################
  173. # Distance to a Diagonal Face #
  174. ###############################
  175.  
  176. min_diag_face_xy <- function(PFX,n_x,n_y,n_z,x_1,y_1,z_1,t_min,t_max){
  177.     n_c <- -(n_x*x_1 + n_y*y_1 + n_z*z_1);
  178.     p_2 <- 0.5*(n_x*PFX$ax + n_y*PFX$ay + n_z*PFX$az);
  179.     p_1 <- n_x*PFX$vx0 + n_y*PFX$vy0 + n_z*PFX$vz0;
  180.     p_0 <- n_x*PFX$x0 + n_y*PFX$y0 + n_z*PFX$z0 + n_c;
  181.     q_1 <- n_x*PFX$ax + n_y*PFX$ay + n_z*PFX$az;
  182.     q_0 <- n_x*PFX$vx0 + n_y*PFX$vy0 + n_z*PFX$vz0;
  183.     c_3 <- p_2*q_1;
  184.     c_2 <- p_2*q_0 + p_1*q_1;
  185.     c_1 <- p_1*q_0 + p_0*q_1;
  186.     c_0 <- p_0*q_0;
  187.     return(cubic_zero_diag_face_xy(c_2/c_3,c_1/c_3,c_0/c_3,PFX,n_x,n_y,n_z,n_c,x_1,y_1,z_1,t_min,t_max));
  188. }
  189.  
  190. dist_diag_face_xy <- function(PFX,n_x,n_y,n_z,n_c,t_f){
  191.     return(abs(n_x*x_eval(PFX,t_f) + n_y*y_eval(PFX,t_f) + n_z*z_eval(PFX,t_f) + n_c)/sqrt(n_x^2 + n_y^2 + n_z^2));
  192. }
  193.  
  194. #######################
  195. # Distance to an Edge #
  196. #######################
  197.  
  198. min_edge_xz <- function(PFX,x_e,z_e,t_min,t_max){
  199.     c_3 <- 0.5*(PFX$ax^2 + PFX$az^2);
  200.     c_2 <- 1.5*(PFX$ax*PFX$vx0^2 + PFX$az*PFX$vz0^2);
  201.     c_1 <- PFX$vx0^2 + PFX$vz0^2 + PFX$ax*(PFX$x0-x_e) + PFX$az*(PFX$z0-z_e);
  202.     c_0 <- PFX$vx0*(PFX$x0-x_e) + PFX$vz0*(PFX$z0-z_e);
  203.     return(cubic_zero_edge_xz(c_2/c_3,c_1/c_3,c_0/c_3,PFX,x_e,z_e,t_min,t_max));
  204. }
  205.  
  206. dist_edge_xz <- function(PFX,x_e,z_e,t_e){
  207.     return(sqrt((x_eval(PFX,t_e) - x_e)^2 + (z_eval(PFX,t_e) - z_e)^2));
  208. }
  209.  
  210. min_edge_yz <- function(PFX,y_e,z_e,t_min,t_max){
  211.     c_3 <- 0.5*(PFX$ay^2 + PFX$az^2);
  212.     c_2 <- 1.5*(PFX$ay*PFX$vy0^2 + PFX$az*PFX$vz0^2);
  213.     c_1 <- PFX$vy0^2 + PFX$vz0^2 + PFX$ay*(PFX$y0-y_e) + PFX$az*(PFX$z0-z_e);
  214.     c_0 <- PFX$vy0*(PFX$y0-y_e) + PFX$vz0*(PFX$z0-z_e);
  215.     return(cubic_zero_edge_yz(c_2/c_3,c_1/c_3,c_0/c_3,PFX,y_e,z_e,t_min,t_max));
  216. }
  217.  
  218. dist_edge_yz <- function(PFX,y_e,z_e,t_e){
  219.     return(sqrt((y_eval(PFX,t_e) - y_e)^2 + (z_eval(PFX,t_e) - z_e)^2));
  220. }
  221.  
  222. min_edge_xy <- function(PFX,x_e,y_e,t_min,t_max){
  223.     c_3 <- 0.5*(PFX$ax^2 + PFX$ay^2);
  224.     c_2 <- 1.5*(PFX$ax*PFX$vx0^2 + PFX$ay*PFX$vy0^2);
  225.     c_1 <- PFX$vx0^2 + PFX$vy0^2 + PFX$ax*(PFX$x0-x_e) + PFX$ay*(PFX$y0-y_e);
  226.     c_0 <- PFX$vx0*(PFX$x0-x_e) + PFX$vy0*(PFX$y0-y_e);
  227.     return(cubic_zero_edge_xy(c_2/c_3,c_1/c_3,c_0/c_3,PFX,x_e,y_e,t_min,t_max));
  228. }
  229.  
  230. dist_edge_xy <- function(PFX,x_e,y_e,t_e){
  231.     return(sqrt((x_eval(PFX,t_e) - x_e)^2 + (y_eval(PFX,t_e) - y_e)^2));
  232. }
  233.  
  234. ###############################
  235. # Distance to a Diagonal Edge #
  236. ###############################
  237.  
  238. min_diag_edge_xy <- function(PFX,x_1,y_1,z_1,x_2,y_2,t_min,t_max){
  239.     p_2 <- -0.5*(PFX$ax*(x_2 - x_1) + PFX$ay*(y_2 - y_1));
  240.     p_1 <- -PFX$vx0*(x_2 - x_1) - PFX$vy0*(y_2 - y_1);
  241.     p_0 <- (x_1 - PFX$x0)*(x_2 - x_1) + (y_1 - PFX$y0)*(y_2 - y_1);
  242.     q_1 <- PFX$ax*(x_2 - x_1) + PFX$ay*(y_2 - y_1);
  243.     q_0 <- PFX$vx0*(x_2 - x_1) + PFX$vy0*(y_2 - y_1);
  244.     c_3 <- 0.5*(PFX$ax^2 + PFX$ay^2 + PFX$az^2) + p_2*q_1;
  245.     c_2 <- 1.5*(PFX$ax*PFX$vx0 + PFX$ay*PFX$vy0 + PFX$az*PFX$vz0) + p_2*q_0 + p_1*q_1;
  246.     c_1 <- PFX$ax*(PFX$x0 - x_1) + PFX$vx0^2 + PFX$ay*(PFX$y0 - y_1) + PFX$vy0^2 + PFX$az*(PFX$z0 - z_1) + PFX$vz0^2 + p_1*q_0 + p_0*q_1;
  247.     c_0 <- PFX$vx0*(PFX$x0 - x_1) + PFX$vy0*(PFX$y0 - y_1) + PFX$vz0*(PFX$z0 - z_1) + p_0*q_0;
  248.     return(cubic_zero_diag_edge_xy(c_2/c_3,c_1/c_3,c_0/c_3,PFX,x_1,y_1,z_1,x_2,y_2,t_min,t_max));
  249. }
  250.    
  251. dist_diag_edge_xy <- function(PFX,x_1,y_1,z_1,x_2,y_2,t_e){
  252.     x2mx1 <- c(x_2 - x_1,y_2 - y_1,0);
  253.     x1mx0 <- c(x_1 - x_eval(PFX,t_e),y_1 - y_eval(PFX,t_e),z_1 - z_eval(PFX,t_e));
  254.     return(sqrt(((x1mx0%*%x1mx0)*(x2mx1%*%x2mx1) - (x1mx0%*%x2mx1)^2)/(x2mx1%*%x2mx1)));
  255. }
  256.  
  257. ########################
  258. # Distance to a Corner #
  259. ########################
  260.  
  261. min_corner <- function(PFX,x_c,y_c,z_c,t_min,t_max){
  262.     c_3 <- 0.5*(PFX$ax^2 + PFX$ay^2 + PFX$az^2);
  263.     c_2 <- 1.5*(PFX$ax*PFX$vx0 + PFX$ay*PFX$vy0 + PFX$az*PFX$vz0);
  264.     c_1 <- PFX$vx0^2 + PFX$vy0^2 + PFX$vz0^2 + PFX$ax*(PFX$x0-x_c) + PFX$ay*(PFX$y0-y_c) + PFX$az*(PFX$z0-z_c);
  265.     c_0 <- PFX$vx0*(PFX$x0-x_c) + PFX$vy0*(PFX$y0-y_c) + PFX$vz0*(PFX$z0-z_c);
  266.     return(cubic_zero_corner(c_2/c_3,c_1/c_3,c_0/c_3,PFX,x_c,y_c,z_c,t_min,t_max));
  267. }
  268.  
  269. dist_corner <- function(PFX,x_c,y_c,z_c,t_c){
  270.     return(sqrt((x_eval(PFX,t_c) - x_c)^2 + (y_eval(PFX,t_c) - y_c)^2 + (z_eval(PFX,t_c) - z_c)^2));
  271. }
  272.  
  273. ########################
  274. # Check the Pitch Zone #
  275. ########################
  276.  
  277. zone_check <- function(PFX,sz_left_x,sz_right_x,sz_top_z,sz_bottom_z){
  278.     # Check in the z-direction
  279.     if (PFX$z0 > sz_top_z){
  280.         zone_start <- 0;
  281.     } else if (PFX$z0 == sz_top_z) {
  282.         if (PFX$vz0 > 0){
  283.             zone_start <- 0;
  284.         } else {
  285.             zone_start <- 3;
  286.         }
  287.     } else if (PFX$z0 > sz_bottom_z){
  288.         zone_start <- 3;
  289.     } else if (PFX$z0 == sz_bottom_z){
  290.         if (PFX$vz0 > 0){
  291.             zone_start <- 3;
  292.         } else {
  293.             zone_start <- 6;
  294.         }
  295.     } else {
  296.         zone_start <- 6;
  297.     }
  298.     # Check in the x-direction
  299.     if (PFX$x0 < sz_left_x){
  300.         zone_start <- zone_start + 1;
  301.     } else if (PFX$x0 == sz_left_x) {
  302.         if (PFX$vx0 < 0){
  303.             zone_start <- zone_start + 1;
  304.         } else {
  305.             zone_start <- zone_start + 2;
  306.         }
  307.     } else if (PFX$x0 < sz_right_x){
  308.         zone_start <- zone_start + 2;
  309.     } else if (PFX$x0 == sz_right_x){
  310.         if (PFX$vx0 < 0){
  311.             zone_start <- zone_start + 2;
  312.         } else {
  313.             zone_start <- zone_start + 3;
  314.         }
  315.     } else {
  316.         zone_start <- zone_start + 3;
  317.     }
  318.     return(zone_start);
  319. }
  320.  
  321. zone_check_2D <- function(px,pz,vx,vz,sz_left_x,sz_right_x,sz_top_z,sz_bottom_z){
  322.     # Check in the z-direction
  323.     if (pz > sz_top_z){
  324.         zone_start <- 0;
  325.     } else if (pz == sz_top_z) {
  326.         if (vz > 0){
  327.             zone_start <- 0;
  328.         } else {
  329.             zone_start <- 3;
  330.         }
  331.     } else if (pz > sz_bottom_z){
  332.         zone_start <- 3;
  333.     } else if (pz == sz_bottom_z){
  334.         if (vz > 0){
  335.             zone_start <- 3;
  336.         } else {
  337.             zone_start <- 6;
  338.         }
  339.     } else {
  340.         zone_start <- 6;
  341.     }
  342.     # Check in the x-direction
  343.     if (px < sz_left_x){
  344.         zone_start <- zone_start + 1;
  345.     } else if (px == sz_left_x) {
  346.         if (vx < 0){
  347.             zone_start <- zone_start + 1;
  348.         } else {
  349.             zone_start <- zone_start + 2;
  350.         }
  351.     } else if (px < sz_right_x){
  352.         zone_start <- zone_start + 2;
  353.     } else if (px == sz_right_x){
  354.         if (vx < 0){
  355.             zone_start <- zone_start + 2;
  356.         } else {
  357.             zone_start <- zone_start + 3;
  358.         }
  359.     } else {
  360.         zone_start <- zone_start + 3;
  361.     }
  362.     return(zone_start);
  363. }
  364.    
  365. ##############################
  366. # Solution to Cubic Equation #
  367. ##############################
  368.  
  369. cubic_zero_corner <- function(a,b,c,PFX,x_c,y_c,z_c,t_min,t_max){
  370.     dist_t_min <- dist_corner(PFX,x_c,y_c,z_c,t_min);
  371.     dist_t_max <- dist_corner(PFX,x_c,y_c,z_c,t_max);
  372.     dist_sz_min <- min(dist_t_min,dist_t_max);
  373.     Q <- (a^2 - 3*b)/9;
  374.     R <- (2*a^3 - 9*a*b + 27*c)/54;
  375.     M <- R^2 - Q^3;
  376.     if (M > 0){
  377.         S <- -R + sqrt(M);
  378.         S <- sign(S)*abs(S)^(1/3);
  379.         T <- -R - sqrt(M);
  380.         T <- sign(T)*abs(T)^(1/3);
  381.         root <- -(a/3) + S + T;
  382.         if (root < t_max && root > t_min){
  383.             dist_root <- dist_corner(PFX,x_c,y_c,z_c,root);
  384.             if (dist_root < dist_sz_min){
  385.                 dist_sz_min <- dist_root;
  386.             }
  387.         }
  388.     } else if (M < 0) {
  389.         theta <- acos(sign(Q)*R/abs(Q)^(3/2));
  390.         root_1 <- -2*sqrt(Q)*cos(theta/3) - (a/3);
  391.         if (root_1 < t_max && root_1 > t_min){
  392.             dist_root_1 <- dist_corner(PFX,x_c,y_c,z_c,root_1);
  393.             if (dist_root_1 < dist_sz_min){
  394.                 dist_sz_min <- dist_root_1;
  395.             }
  396.         }
  397.         root_2 <- -2*sqrt(Q)*cos((theta+2*pi)/3) - (a/3);
  398.         if (root_2 < t_max && root_2 > t_min){
  399.             dist_root_2 <- dist_corner(PFX,x_c,y_c,z_c,root_2);
  400.             if (dist_root_2 < dist_sz_min){
  401.                 dist_sz_min <- dist_root_2;
  402.             }
  403.         }
  404.         root_3 <- -2*sqrt(Q)*cos((theta-2*pi)/3) - (a/3);
  405.         if (root_3 < t_max && root_3 > t_min){
  406.             dist_root_3 <- dist_corner(PFX,x_c,y_c,z_c,root_3);
  407.             if (dist_root_3 < dist_sz_min){
  408.                 dist_sz_min <- dist_root_3;
  409.             }
  410.         }
  411.     }
  412.     return(dist_sz_min);
  413. }
  414.  
  415. cubic_zero_edge_xz <- function(a,b,c,PFX,x_e,z_e,t_min,t_max){
  416.     dist_t_min <- dist_edge_xz(PFX,x_e,z_e,t_min);
  417.     dist_t_max <- dist_edge_xz(PFX,x_e,z_e,t_max);
  418.     dist_sz_min <- min(dist_t_min,dist_t_max);
  419.     Q <- (a^2 - 3*b)/9;
  420.     R <- (2*a^3 - 9*a*b + 27*c)/54;
  421.     M <- R^2 - Q^3;
  422.     if (M > 0){
  423.         S <- -R + sqrt(M);
  424.         S <- sign(S)*abs(S)^(1/3);
  425.         T <- -R - sqrt(M);
  426.         T <- sign(T)*abs(T)^(1/3);
  427.         root <- -(a/3) + S + T;
  428.         if(root < t_max && root > t_min){
  429.             dist_root <- dist_edge_xz(PFX,x_e,z_e,root);
  430.             if(dist_root < dist_sz_min){
  431.                 dist_sz_min <- dist_root;
  432.             }
  433.         }
  434.     } else if (M < 0) {
  435.         theta <- acos(sign(Q)*R/abs(Q)^(3/2));
  436.         root_1 <- -2*sqrt(Q)*cos(theta/3) - (a/3);
  437.         if(root_1 < t_max && root_1 > t_min){
  438.             dist_root_1 <- dist_edge_xz(PFX,x_e,z_e,root_1);
  439.             if(dist_root_1 < dist_sz_min){
  440.                 dist_sz_min <- dist_root_1;
  441.             }
  442.         }
  443.         root_2 <- -2*sqrt(Q)*cos((theta+2*pi)/3) - (a/3);
  444.         if(root_2 < t_max && root_2 > t_min){
  445.             dist_root_2 <- dist_edge_xz(PFX,x_e,z_e,root_2);
  446.             if(dist_root_2 < dist_sz_min){
  447.                 dist_sz_min <- dist_root_2;
  448.             }
  449.         }
  450.         root_3 <- -2*sqrt(Q)*cos((theta-2*pi)/3) - (a/3);
  451.         if(root_3 < t_max && root_3 > t_min){
  452.             dist_root_3 <- dist_edge_xz(PFX,x_e,z_e,root_3);
  453.             if(dist_root_3 < dist_sz_min){
  454.                 dist_sz_min <- dist_root_3;
  455.             }
  456.         }
  457.     }
  458.     return(dist_sz_min);
  459. }
  460.  
  461. cubic_zero_edge_yz <- function(a,b,c,PFX,y_e,z_e,t_min,t_max){
  462.     dist_t_min <- dist_edge_yz(PFX,y_e,z_e,t_min);
  463.     dist_t_max <- dist_edge_yz(PFX,y_e,z_e,t_max);
  464.     dist_sz_min <- min(dist_t_min,dist_t_max);
  465.     Q <- (a^2 - 3*b)/9;
  466.     R <- (2*a^3 - 9*a*b + 27*c)/54;
  467.     M <- R^2 - Q^3;
  468.     if (M > 0){
  469.         S <- -R + sqrt(M);
  470.         S <- sign(S)*abs(S)^(1/3);
  471.         T <- -R - sqrt(M);
  472.         T <- sign(T)*abs(T)^(1/3);
  473.         root <- -(a/3) + S + T;
  474.         if(root < t_max && root > t_min){
  475.             dist_root <- dist_edge_yz(PFX,y_e,z_e,root);
  476.             if(dist_root < dist_sz_min){
  477.                 dist_sz_min <- dist_root;
  478.             }
  479.         }
  480.     } else if (M < 0) {
  481.         theta <- acos(sign(Q)*R/abs(Q)^(3/2));
  482.         root_1 <- -2*sqrt(Q)*cos(theta/3) - (a/3);
  483.         if(root_1 < t_max && root_1 > t_min){
  484.             dist_root_1 <- dist_edge_yz(PFX,y_e,z_e,root_1);
  485.             if(dist_root_1 < dist_sz_min){
  486.                 dist_sz_min <- dist_root_1;
  487.             }
  488.         }
  489.         root_2 <- -2*sqrt(Q)*cos((theta+2*pi)/3) - (a/3);
  490.         if(root_2 < t_max && root_2 > t_min){
  491.             dist_root_2 <- dist_edge_yz(PFX,y_e,z_e,root_2);
  492.             if(dist_root_2 < dist_sz_min){
  493.                 dist_sz_min <- dist_root_2;
  494.             }
  495.         }
  496.         root_3 <- -2*sqrt(Q)*cos((theta-2*pi)/3) - (a/3);
  497.         if(root_3 < t_max && root_3 > t_min){
  498.             dist_root_3 <- dist_edge_yz(PFX,y_e,z_e,root_3);
  499.             if(dist_root_3 < dist_sz_min){
  500.                 dist_sz_min <- dist_root_3;
  501.             }
  502.         }
  503.     }
  504.     return(dist_sz_min);
  505. }
  506.  
  507. cubic_zero_edge_xy <- function(a,b,c,PFX,x_e,y_e,t_min,t_max){
  508.     dist_t_min <- dist_edge_xy(PFX,x_e,y_e,t_min);
  509.     dist_t_max <- dist_edge_xy(PFX,x_e,y_e,t_max);
  510.     dist_sz_min <- min(dist_t_min,dist_t_max);
  511.     Q <- (a^2 - 3*b)/9;
  512.     R <- (2*a^3 - 9*a*b + 27*c)/54;
  513.     M <- R^2 - Q^3;
  514.     if (M > 0){
  515.         S <- -R + sqrt(M);
  516.         S <- sign(S)*abs(S)^(1/3);
  517.         T <- -R - sqrt(M);
  518.         T <- sign(T)*abs(T)^(1/3);
  519.         root <- -(a/3) + S + T;
  520.         if(root < t_max && root > t_min){
  521.             dist_root <- dist_edge_xy(PFX,x_e,y_e,root);
  522.             if(dist_root < dist_sz_min){
  523.                 dist_sz_min <- dist_root;
  524.             }
  525.         }
  526.     } else if (M < 0) {
  527.         theta <- acos(sign(Q)*R/abs(Q)^(3/2));
  528.         root_1 <- -2*sqrt(Q)*cos(theta/3) - (a/3);
  529.         if(root_1 < t_max && root_1 > t_min){
  530.             dist_root_1 <- dist_edge_xy(PFX,x_e,y_e,root_1);
  531.             if(dist_root_1 < dist_sz_min){
  532.                 dist_sz_min <- dist_root_1;
  533.             }
  534.         }
  535.         root_2 <- -2*sqrt(Q)*cos((theta+2*pi)/3) - (a/3);
  536.         if(root_2 < t_max && root_2 > t_min){
  537.             dist_root_2 <- dist_edge_xy(PFX,x_e,y_e,root_2);
  538.             if(dist_root_2 < dist_sz_min){
  539.                 dist_sz_min <- dist_root_2;
  540.             }
  541.         }
  542.         root_3 <- -2*sqrt(Q)*cos((theta-2*pi)/3) - (a/3);
  543.         if(root_3 < t_max && root_3 > t_min){
  544.             dist_root_3 <- dist_edge_xy(PFX,x_e,y_e,root_3);
  545.             if(dist_root_3 < dist_sz_min){
  546.                 dist_sz_min <- dist_root_3;
  547.             }
  548.         }
  549.     }
  550.     return(dist_sz_min);
  551. }
  552.  
  553. cubic_zero_diag_edge_xy <- function(a,b,c,PFX,x_1,y_1,z_1,x_2,y_2,t_min,t_max){
  554.     dist_t_min <- dist_diag_edge_xy(PFX,x_1,y_1,z_1,x_2,y_2,t_min);
  555.     dist_t_max <- dist_diag_edge_xy(PFX,x_1,y_1,z_1,x_2,y_2,t_max);
  556.     dist_sz_min <- min(dist_t_min,dist_t_max);
  557.     Q <- (a^2 - 3*b)/9;
  558.     R <- (2*a^3 - 9*a*b + 27*c)/54;
  559.     M <- R^2 - Q^3;
  560.     if (M > 0){
  561.         S <- -R + sqrt(M);
  562.         S <- sign(S)*abs(S)^(1/3);
  563.         T <- -R - sqrt(M);
  564.         T <- sign(T)*abs(T)^(1/3);
  565.         root <- -(a/3) + S + T;
  566.         if(root < t_max && root > t_min){
  567.             dist_root <- dist_diag_edge_xy(PFX,x_1,y_1,z_1,x_2,y_2,root);
  568.             if(dist_root < dist_sz_min){
  569.                 dist_sz_min <- dist_root;
  570.             }
  571.         }
  572.     } else if (M < 0) {
  573.         theta <- acos(sign(Q)*R/abs(Q)^(3/2));
  574.         root_1 <- -2*sqrt(Q)*cos(theta/3) - (a/3);
  575.         if(root_1 < t_max && root_1 > t_min){
  576.             dist_root_1 <- dist_diag_edge_xy(PFX,x_1,y_1,z_1,x_2,y_2,root_1);
  577.             if(dist_root_1 < dist_sz_min){
  578.                 dist_sz_min <- dist_root_1;
  579.             }
  580.         }
  581.         root_2 <- -2*sqrt(Q)*cos((theta+2*pi)/3) - (a/3);
  582.         if(root_2 < t_max && root_2 > t_min){
  583.             dist_root_2 <- dist_diag_edge_xy(PFX,x_1,y_1,z_1,x_2,y_2,root_2);
  584.             if(dist_root_2 < dist_sz_min){
  585.                 dist_sz_min <- dist_root_2;
  586.             }
  587.         }
  588.         root_3 <- -2*sqrt(Q)*cos((theta-2*pi)/3) - (a/3);
  589.         if(root_3 < t_max && root_3 > t_min){
  590.             dist_root_3 <- dist_diag_edge_xy(PFX,x_1,y_1,z_1,x_2,y_2,root_3);
  591.             if(dist_root_3 < dist_sz_min){
  592.                 dist_sz_min <- dist_root_3;
  593.             }
  594.         }
  595.     }
  596.     return(dist_sz_min);
  597. }
  598.  
  599. cubic_zero_diag_face_xy <- function(a,b,c,PFX,n_x,n_y,n_z,n_c,x_1,y_1,z_1,t_min,t_max){
  600.     dist_t_min <- dist_diag_face_xy(PFX,n_x,n_y,n_z,n_c,t_min);
  601.     dist_t_max <- dist_diag_face_xy(PFX,n_x,n_y,n_z,n_c,t_max);
  602.     dist_sz_min <- min(dist_t_min,dist_t_max);
  603.     Q <- (a^2 - 3*b)/9;
  604.     R <- (2*a^3 - 9*a*b + 27*c)/54;
  605.     M <- R^2 - Q^3;
  606.     if (M > 0){
  607.         S <- -R + sqrt(M);
  608.         S <- sign(S)*abs(S)^(1/3);
  609.         T <- -R - sqrt(M);
  610.         T <- sign(T)*abs(T)^(1/3);
  611.         root <- -(a/3) + S + T;
  612.         if(root < t_max && root > t_min){
  613.             dist_root <- dist_diag_face_xy(PFX,n_x,n_y,n_z,n_c,root);
  614.             if(dist_root < dist_sz_min){
  615.                 dist_sz_min <- dist_root;
  616.             }
  617.         }
  618.     } else if (M < 0) {
  619.         theta <- acos(sign(Q)*R/abs(Q)^(3/2));
  620.         root_1 <- -2*sqrt(Q)*cos(theta/3) - (a/3);
  621.         if(root_1 < t_max && root_1 > t_min){
  622.             dist_root_1 <- dist_diag_face_xy(PFX,n_x,n_y,n_z,n_c,root_1);
  623.             if(dist_root_1 < dist_sz_min){
  624.                 dist_sz_min <- dist_root_1;
  625.             }
  626.         }
  627.         root_2 <- -2*sqrt(Q)*cos((theta+2*pi)/3) - (a/3);
  628.         if(root_2 < t_max && root_2 > t_min){
  629.             dist_root_2 <- dist_diag_face_xy(PFX,n_x,n_y,n_z,n_c,root_2);
  630.             if(dist_root_2 < dist_sz_min){
  631.                 dist_sz_min <- dist_root_2;
  632.             }
  633.         }
  634.         root_3 <- -2*sqrt(Q)*cos((theta-2*pi)/3) - (a/3);
  635.         if(root_3 < t_max && root_3 > t_min){
  636.             dist_root_3 <- dist_diag_face_xy(PFX,n_x,n_y,n_z,n_c,root_3);
  637.             if(dist_root_3 < dist_sz_min){
  638.                 dist_sz_min <- dist_root_3;
  639.             }
  640.         }
  641.     }
  642.     return(dist_sz_min);
  643. }
  644.  
  645. #############################
  646. # xy-Intersection of a Line #
  647. #############################
  648.  
  649. xy_intersect <- function(PFX,x_f,x_b,y_f,y_b){
  650.     M <- (y_f-y_b)/(x_f-x_b);
  651.     B <- y_f - M*x_f;
  652.     a <- 0.5*(PFX$ay - M*PFX$ax);
  653.     b <- PFX$vy0 - M*PFX$vx0;
  654.     c <- PFX$y0 - (M*PFX$x0 + B);
  655.     return(list(a = a, b = b, c = c, M = M));
  656. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
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
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!