```(* Approximation of zero *)
let leps l = (abs_float l) < epsilon_float

type point = float array

type vector = float array

let ( |+| ) a b =
[| a.(0) +. b.(0); a.(1) +. b.(1); a.(2) +. b.(2) |]

(* vector subtraction *)
let (|-|) a b =
[| a.(0) -. b.(0); a.(1) -. b.(1); a.(2) -. b.(2) |]

(* scalar-vector multiplication*)
let (|*|) k a =
[| k *. a.(0); k *. a.(1); k *. a.(2) |]

(* dot product *)
let (|**|) a b =
a.(0) *. b.(0) +. a.(1) *. b.(1) +. a.(2) *. b.(2)

(* cross product *)
let (|^|) v1 v2 =
[| v1.(1)*.v2.(2) -. v2.(1)*.v1.(2);  v1.(2)*.v2.(0) -. v2.(2)*.v1.(0);  v1.(0)*.v2.(1) -. v2.(0)*.v1.(1) |]

(* length of a vector *)
let vlength a =
sqrt (a |**| a)

(* normalize vector *)
let vnorm a =
(1.0 /. (vlength a)) |*| a

type state = { stpos: point; stvel: vector; staccel: vector}

type derivative = { dstpos: point; dstvel: vector }

let euler st dt =
{
stpos = st.stpos |+| (dt |*| st.stvel);
stvel = st.stvel |+| (dt |*| st.staccel);
staccel = st.staccel
}

let newton_stormer_verlet st dt =
let v = st.stvel |+|  (dt |*| st.staccel) in
{
stpos = st.stpos |+|  (dt |*| v);
stvel = v;
staccel = st.staccel
}

let runge_kutta_4th_order st dt =
let evaluate dt d =
let s = {
stpos = st.stpos |+| (dt |*| d.dstpos);
stvel = st.stvel |+| (dt |*| d.dstvel);
staccel = st.staccel
}
in
{
dstpos = s.stvel;
dstvel = s.staccel
}
in
let a = evaluate 0.0 { dstpos = st.stvel; dstvel = st.staccel} in
let b = evaluate (dt *. 0.5) a in
let c = evaluate (dt *. 0.5) b in
let d = evaluate dt c in
let dxdt = 1.0 /. 6.0 |*| (a.dstpos |+| (2.0 |*| (b.dstpos |+| c.dstpos)) |+| d.dstpos)
and dvdt = 1.0 /. 6.0 |*| (a.dstvel |+| (2.0 |*| (b.dstvel |+| c.dstvel)) |+| d.dstvel) in
{
stpos = st.stpos |+| (dt |*| dxdt);
stvel = st.stvel |+| (dt |*| dvdt);
staccel = st.staccel
}

let explicit_verlet st dt =
let x0 = st.stpos |-| (dt |*| st.stvel) and x1 = st.stpos in
let v = x1 |-| x0 |+| ((dt *. dt) |*| st.staccel) in
{
stpos = x1 |+| v;
stvel = (1.0 /. dt) |*| v;
staccel = st.staccel
}

let evaluate integrator initial time dt =
let rec eval st t =
if t >= time then st else
eval (integrator st dt) (t +. dt)
in
eval initial 0.

(* Constraints *)

type path_constraint =
{
get_nearest_valid_point : point -> point;
get_tangent_velocity : point -> vector -> vector
}

(* Bead on wire hoop *)

type hoop = { center : point; radius : float}

let bead1 = { stpos = [|100.; 0.; 0.;|]; stvel = [|0.; 30.; 0.;|]; staccel = [|-40.; -10.; 0.|] }

let print_bead p t x y =
let s = Printf.sprintf "%s speed= %f" t (vlength p.stvel) in
Graphics.moveto x y;
Graphics.draw_string s

let get_pt pt =
let l = pt |-| center in
let d = vlength l in
center |+| ((radius /. d) |*| l )
and get_tan pt v =
let speed = vlength v in
if (leps speed) then
v
else
let vel = vnorm v in
let l = vnorm (pt |-| center) in
let parallel = (vel |**| l) |*| l in
let perpendicular = vel |-| parallel in
(speed) |*| (vnorm perpendicular)
in
{
get_nearest_valid_point = get_pt;
get_tangent_velocity = get_tan
}

let update_system hp bead time dt =
let x1 = hp.get_nearest_valid_point x0' in
let v1 = hp.get_tangent_velocity x1 v0' in
let result = {stpos = x1; stvel = v1; staccel = bead.staccel} in
result

(* Test *)
let width = 500
let height = 500

let center =  [|0.; 0.; 0.;|]

let hoop1  = make_hoop center radius

Graphics.set_color Graphics.black;
let p = bead.stpos and i = truncate in
Graphics.fill_circle ((i p.(0)) + width/2) ((i p.(1)) + height/2) 3

let draw_hoop c r =
let i = truncate in
Graphics.draw_circle ((i c.(0)) + width/2) ((i c.(1)) + height/2) (i r)

let init_graphics () =
let init_string = (Printf.sprintf " %dx%d" width height) in
Graphics.open_graph init_string;

let rec main t0 st0 =
let _ = Graphics.auto_synchronize false in
let _ = Graphics.clear_graph () in
let t1 = Unix.gettimeofday () in
let dt = t1 -. t0 in
let st1 = update_system hoop1 st0 dt (dt /. 100.0) in