development/thales/ocaml/sphere.ml

   1 (* extracted from sphere.hl *)
   2
   3 let sqrt8 = sqrt(8.0);;
   4 let sqrt2 = sqrt(2.0);;
   5 let pi = 4.0*.atan(1.0);;
   6
   7 let delta_x x1 x2 x3 x4 x5 x6 =
   8         x1*.x4*.(-.  x1 +.  x2 +.  x3 -. x4 +.  x5 +.  x6) +.
   9         x2*.x5*.(x1 -.  x2 +.  x3 +.  x4 -. x5 +.  x6) +.
  10         x3*.x6*.(x1 +.  x2 -.  x3 +.  x4 +.  x5 -.  x6)
  11         -. x2*.x3*.x4 -.  x1*.x3*.x5 -.  x1*.x2*.x6 -. x4*.x5*.x6;;
  12
  13 let delta_y y1 y2 y3 y4 y5 y6 =
  14     delta_x (y1*.y1) (y2*.y2) (y3*.y3) (y4*.y4) (y5*.y5) (y6*.y6);;
  15
  16 let delta_x4 x1 x2 x3 x4 x5 x6
  17         =  -.   x2*. x3 -.   x1*. x4 +.  x2*. x5
  18         +.  x3*. x6 -.   x5*. x6 +.  x1*. (-.   x1 +.   x2 +.   x3 -.   x4 +.   x5 +.   x6);;
  19
  20 let delta_x6 x1 x2 x3 x4 x5 x6
  21         = -.   x1 *. x2 -.  x3*.x6 +.  x1 *. x4
  22         +.  x2*. x5 -.  x4*. x5 +.  x3*.(-.   x3 +.  x1 +.  x2 -.  x6 +.  x4 +.  x5);;
  23
  24 let ups_x x1 x2 x6 =
  25     -.  x1*.x1 -.  x2*.x2 -.  x6*.x6
  26     +.  2.0 *.x1*.x6 +.  2.0 *.x1*.x2 +.  2.0 *.x2*.x6;;
  27
  28 let eta_x x1 x2 x3 =
  29         (sqrt ((x1*.x2*.x3)/.(ups_x x1 x2 x3)))
  30         ;;
  31
  32 let eta_y y1 y2 y3 =
  33                 let x1 = y1*.y1 in
  34                 let x2 = y2*.y2 in
  35                 let x3 = y3*.y3 in
  36                 eta_x x1 x2 x3;;
  37
  38 let atn2 (x,y) = atan(y/. x);;
  39
  40 let dih_x x1 x2 x3 x4 x5 x6 =
  41        let d_x4 = delta_x4 x1 x2 x3 x4 x5 x6 in
  42        let d = delta_x x1 x2 x3 x4 x5 x6 in
  43        pi/.  (2.0) +.   atn2( (sqrt ((4.0) *.  x1 *.  d)),-.  d_x4);;
  44
  45 let dih_y y1 y2 y3 y4 y5 y6 =
  46        let (x1,x2,x3,x4,x5,x6)= (y1*. y1,y2*. y2,y3*. y3,y4*. y4,y5*. y5,y6*. y6) in
  47        dih_x x1 x2 x3 x4 x5 x6;;
  48
  49 let dih2_y y1 y2 y3 y4 y5 y6 = dih_y y2 y3 y1 y5 y6 y4;;
  50
  51 let sol_x  x1 x2 x3 x4 x5 x6 =
  52         dih_x x1 x2 x3 x4 x5 x6 +.
  53         dih_x x2 x3 x1 x5 x6 x4 +.   dih_x x3 x1 x2 x6 x4 x5 -.  pi;;
  54
  55 let sol_y  y1 y2 y3 y4 y5 y6 =
  56         dih_y y1 y2 y3 y4 y5 y6 +.
  57         dih_y y2 y3 y1 y5 y6 y4 +.   dih_y y3 y1 y2 y6 y4 y5 -.  pi;;
  58
  59 let interp x1 y1 x2 y2 x = y1 +.  (x -. x1) *.  (y2-. y1)/. (x2-.x1);;
  60
  61 (*  c1 in lp2009.c *)
  62 let const1 = sol_y (2.0) (2.0) (2.0) (2.0) (2.0) (2.0) /.  pi;;
  63
  64 let ly y = interp (2.0) (1.0) (2.52) (0.0) y;;
  65
  66 let rho y = 1.0 +.  const1 -. const1*.  ly y;;
  67
  68 let rhazim y1 y2 y3 y4 y5 y6 = rho y1 *.  dih_y y1 y2 y3 y4 y5 y6;;
  69
  70 let lnazim y1 y2 y3 y4 y5 y6 = ly y1 *.  dih_y y1 y2 y3 y4 y5 y6;;
  71
  72 let taum y1 y2 y3 y4 y5 y6 = sol_y y1 y2 y3 y4 y5 y6 *.  (1.0 +.  const1) -. const1 *.  (lnazim y1 y2 y3 y4 y5 y6 +.  lnazim y2 y3 y1 y5 y6 y4 +.  lnazim y3 y1 y2 y6 y4 y5);;
  73