44 lines
1.4 KiB
OCaml
44 lines
1.4 KiB
OCaml
|
|
open Hsim.Types
|
|
open Solvers.Zls
|
|
|
|
let of_array a = Bigarray.Array1.of_array Bigarray.Float64 Bigarray.c_layout a
|
|
|
|
type state = { si : bool; sx : carray }
|
|
|
|
let yd = cmake 3
|
|
let zout = cmake 1
|
|
let zsize = 1
|
|
|
|
let fzer _ _ _ = zout
|
|
let fout _ _ y = of_array [| y.{0}; y.{1}; y.{2} |]
|
|
let cget s = s.sx
|
|
let cset s lx = { s with sx=lx }
|
|
let zset s _ = s
|
|
let jump _ = true
|
|
let horizon _ = max_float
|
|
|
|
let sinus_cosinus theta0 omega =
|
|
let sin0 = Float.sin theta0 in
|
|
let cos0 = Float.cos theta0 in
|
|
let fder _ _ y =
|
|
yd.{0} <- omega *. y.{1}; yd.{1} <- -.omega *. y.{0}; yd.{2} <- 1.0; yd in
|
|
let step { si; sx } _ =
|
|
let sx = if si then of_array [| sin0; cos0; 0.0 |] else sx in
|
|
of_array [| sx.{0}; sx.{1}; sx.{2} |], { sx; si=false } in
|
|
let state = { sx=of_array [| sin0; cos0; 0.0 |]; si=true } in
|
|
let reset _ _ = state in
|
|
HNode {
|
|
state; fder; fzer; fout; step; horizon; cset; cget; zset; zsize; reset; jump
|
|
}
|
|
|
|
let errmsg_invalid = "Invalid arguments to model (needed: 2 floats)"
|
|
let errmsg_few = "Too few arguments to model (needed: 2 floats)"
|
|
let errmsg_many = "Too many arguments to model (needed: 2 floats)"
|
|
let sinus_cosinus = function
|
|
| [t0; om] ->
|
|
let t0, om = try float_of_string t0, float_of_string om
|
|
with Failure _ -> raise (Invalid_argument errmsg_invalid) in
|
|
sinus_cosinus t0 om
|
|
| [] | [_] -> raise (Invalid_argument errmsg_few)
|
|
| _ -> raise (Invalid_argument errmsg_many)
|