chore: update
This commit is contained in:
parent
4776edc9db
commit
416d97c513
25 changed files with 1653 additions and 283 deletions
|
|
@ -1,5 +1,5 @@
|
||||||
|
|
||||||
let dt = 0.001
|
let dt = 0.01
|
||||||
let g = 9.81
|
let g = 9.81
|
||||||
|
|
||||||
let node f_integr (x0, x') = x where
|
let node f_integr (x0, x') = x where
|
||||||
17
exm/ball/dune
Normal file
17
exm/ball/dune
Normal file
|
|
@ -0,0 +1,17 @@
|
||||||
|
(env
|
||||||
|
(dev
|
||||||
|
(flags
|
||||||
|
(:standard -w -a))))
|
||||||
|
|
||||||
|
(rule
|
||||||
|
(targets exm_ball.ml ball.ml ball.zci)
|
||||||
|
(deps
|
||||||
|
(:zl ball.zls)
|
||||||
|
(package zelus))
|
||||||
|
(action
|
||||||
|
(run zeluc -s main -o exm_ball %{zl})))
|
||||||
|
|
||||||
|
(executable
|
||||||
|
(name exm_ball)
|
||||||
|
(public_name exm_ball)
|
||||||
|
(libraries zelus))
|
||||||
18
exm/ballc/ball.zls
Normal file
18
exm/ballc/ball.zls
Normal file
|
|
@ -0,0 +1,18 @@
|
||||||
|
let g = 9.81
|
||||||
|
let p0 = 5.0
|
||||||
|
let v0 = 0.0
|
||||||
|
|
||||||
|
let hybrid ball () = (p, z) where
|
||||||
|
rec der p = v init p0 reset z -> 0.0
|
||||||
|
and der v = -. g init v0 reset z -> -0.8 *. (last v)
|
||||||
|
and z = up (-. p)
|
||||||
|
|
||||||
|
let hybrid main () =
|
||||||
|
let der t = 1.0 init 0.0 in
|
||||||
|
let rec der p = 1.0 init -0.01 reset q -> -0.01
|
||||||
|
and q = up (last p) in
|
||||||
|
let (b, z) = ball () in
|
||||||
|
present q | z -> (
|
||||||
|
print_float t; print_string "\t";
|
||||||
|
print_float b; print_newline ()
|
||||||
|
); ()
|
||||||
16
exm/ballc/dune
Normal file
16
exm/ballc/dune
Normal file
|
|
@ -0,0 +1,16 @@
|
||||||
|
(env
|
||||||
|
(dev
|
||||||
|
(flags
|
||||||
|
(:standard -w -a))))
|
||||||
|
|
||||||
|
(rule
|
||||||
|
(targets ball.ml main.ml ball.zci)
|
||||||
|
(deps
|
||||||
|
(:zl ball.zls))
|
||||||
|
(action
|
||||||
|
(run zeluc -s main %{zl})))
|
||||||
|
|
||||||
|
(executable
|
||||||
|
(public_name ballc.exe)
|
||||||
|
(name main)
|
||||||
|
(libraries hsim))
|
||||||
10
exm/ballc/ztypes.ml
Normal file
10
exm/ballc/ztypes.ml
Normal file
|
|
@ -0,0 +1,10 @@
|
||||||
|
|
||||||
|
include Hsim
|
||||||
|
include Ztypes
|
||||||
|
|
||||||
|
module type IGNORE = sig end
|
||||||
|
module Defaultsolver : IGNORE = struct end
|
||||||
|
|
||||||
|
module Zlsrun = struct
|
||||||
|
module Make (S : IGNORE) = Runtime
|
||||||
|
end
|
||||||
17
exm/dune
17
exm/dune
|
|
@ -1,17 +0,0 @@
|
||||||
(env
|
|
||||||
(dev
|
|
||||||
(flags
|
|
||||||
(:standard -w -a))))
|
|
||||||
|
|
||||||
(rule
|
|
||||||
(targets exm_ball_discrete.ml ball_discrete.ml ball_discrete.zci)
|
|
||||||
(deps
|
|
||||||
(:zl ball_discrete.zls)
|
|
||||||
(package zelus))
|
|
||||||
(action
|
|
||||||
(run zeluc -s main -o exm_ball_discrete %{zl})))
|
|
||||||
|
|
||||||
(executable
|
|
||||||
(name exm_ball_discrete)
|
|
||||||
(public_name exm_ball_discrete)
|
|
||||||
(libraries zelus))
|
|
||||||
17
exm/fib/dune
Normal file
17
exm/fib/dune
Normal file
|
|
@ -0,0 +1,17 @@
|
||||||
|
(env
|
||||||
|
(dev
|
||||||
|
(flags
|
||||||
|
(:standard -w -a))))
|
||||||
|
|
||||||
|
(rule
|
||||||
|
(targets exm_fib.ml fib.ml fib.zci)
|
||||||
|
(deps
|
||||||
|
(:zl fib.zls)
|
||||||
|
(package zelus))
|
||||||
|
(action
|
||||||
|
(run zeluc -s main -o exm_fib %{zl})))
|
||||||
|
|
||||||
|
(executable
|
||||||
|
(name exm_fib)
|
||||||
|
(public_name exm_fib)
|
||||||
|
(libraries zelus))
|
||||||
7
exm/fib/fib.zls
Normal file
7
exm/fib/fib.zls
Normal file
|
|
@ -0,0 +1,7 @@
|
||||||
|
|
||||||
|
let node fib () = n where
|
||||||
|
rec n = 0 -> pre (1 -> (pre n) + n)
|
||||||
|
|
||||||
|
let node main () =
|
||||||
|
let f = fib () in
|
||||||
|
print_int f; print_newline ()
|
||||||
128
exm/main.zls
128
exm/main.zls
|
|
@ -1,9 +1,26 @@
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
(** Zélus
|
|
||||||
|
|
||||||
Synchronous language kernel _à la_ Lustre:
|
|
||||||
|
|
||||||
|
(** Zélus: Hybrid system programming language
|
||||||
|
|
||||||
|
- Model discrete systems and their continuous environment
|
||||||
|
- Research language, design space for hybrid system modelers
|
||||||
|
- Compilation to OCaml, execution with an off-the-shelf ODE solver
|
||||||
|
- Developed by the Inria PARKAS team *)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
(** Synchronous language kernel _à la_ Lustre:
|
||||||
- programs are Mealy machines (outputs on each transition)
|
- programs are Mealy machines (outputs on each transition)
|
||||||
- variables represent streams of values in time *)
|
- variables represent streams of values in time *)
|
||||||
|
|
||||||
|
|
@ -18,21 +35,22 @@ let node incr x = y where
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
(** - we can use values of the previous instants with [pre] and
|
||||||
(** - we can use values of the previous instants (using [pre]) and
|
initialize streams with [->] *)
|
||||||
initialize streams (using [->]) *)
|
|
||||||
|
|
||||||
let node accumulate x = z where
|
let node accumulate x = z where
|
||||||
rec w = pre x
|
rec z = x -> (pre z) + x
|
||||||
and y = 0 -> pre x
|
|
||||||
and z = x -> (pre z) + x
|
|
||||||
|
|
||||||
(* x │ 1 2 5 2 5 3 …
|
(* x │ 1 2 5 2 5 3 …
|
||||||
───┼─────────────────────
|
──────────────┼─────────────────────
|
||||||
w │ 1 2 5 2 5 …
|
pre x │ 1 2 5 2 5 …
|
||||||
y │ 0 1 2 5 2 5 …
|
0 -> x │ 0 2 5 2 5 3 …
|
||||||
z │ 1 3 8 10 15 18 … *)
|
accumulate x │ 1 3 8 10 15 18 … *)
|
||||||
|
|
||||||
|
let node fib () = n where
|
||||||
|
rec n = 0 -> pre (1 -> pre(n) + n)
|
||||||
|
|
||||||
|
(** - causality loops are forbidden ([rec x = x]) *)
|
||||||
|
|
||||||
|
|
||||||
(** - we can reset streams at will *)
|
(** - we can reset streams at will *)
|
||||||
|
|
@ -53,12 +71,32 @@ let node loop x = y where
|
||||||
loop x │ 0 1 2 0 1 2 … *)
|
loop x │ 0 1 2 0 1 2 … *)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
(** Math/physics reminder!
|
||||||
|
|
||||||
|
- Ordinary differential equations (ODEs), initial value problems
|
||||||
|
- Zero-crossing event basics
|
||||||
|
- Background on solvers *)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
(** Already able to model physical behaviours! *)
|
(** Already able to model physical behaviours! *)
|
||||||
|
|
||||||
let dt = 0.001 (* Integration step *)
|
let dt = 0.01 (* Integration step *)
|
||||||
let g = 9.81 (* Gravitational constant *)
|
let g = 9.81 (* Gravitational constant *)
|
||||||
let node f_integr (x0, x') = x where (* Forward Euler integrator *)
|
let node f_integr (x0, x') = x where (* Forward Euler integrator *)
|
||||||
rec x = x0 -> pre (x +. x' *. dt)
|
rec x = x0 -> pre (x +. x' *. dt)
|
||||||
|
|
||||||
let node b_integr (x0, x') = x where (* Backward Euler integrator *)
|
let node b_integr (x0, x') = x where (* Backward Euler integrator *)
|
||||||
rec x = x0 -> (pre x) +. x' *. dt
|
rec x = x0 -> (pre x) +. x' *. dt
|
||||||
|
|
||||||
|
|
@ -68,7 +106,21 @@ let node bouncing_ball (p0, v0) = p where
|
||||||
and q = p0 -> 0.0 and w = v0 -> -0.8 *. (pre v)
|
and q = p0 -> 0.0 and w = v0 -> -0.8 *. (pre v)
|
||||||
and z = false -> (pre p) < 0.0
|
and z = false -> (pre p) < 0.0
|
||||||
|
|
||||||
(** Quite cumbersome. *)
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
(** Cumbersome, and error-prone! *)
|
||||||
|
|
||||||
|
let node sincos () = (sin, cos) where
|
||||||
|
rec sin = f_integr(0.0, cos)
|
||||||
|
and cos = f_integr(1.0, -. sin)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -81,6 +133,9 @@ let node bouncing_ball (p0, v0) = p where
|
||||||
let hybrid integr (x0, x') = x where
|
let hybrid integr (x0, x') = x where
|
||||||
der x = x' init x0
|
der x = x' init x0
|
||||||
|
|
||||||
|
let hybrid time () = t where
|
||||||
|
der t = 1.0 init 0.0
|
||||||
|
|
||||||
let hybrid position (p0, v0, a) = p where
|
let hybrid position (p0, v0, a) = p where
|
||||||
rec der p = v init p0
|
rec der p = v init p0
|
||||||
and der v = a init v0
|
and der v = a init v0
|
||||||
|
|
@ -89,33 +144,28 @@ let hybrid position (p0, v0, a) = p where
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
(** - mix discrete and continuous code with [up], [present], [reset]
|
||||||
|
and [last] *)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
(** We can intermingle discrete and continuous behaviours: *)
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
(** We can now express physical systems much more precisely: *)
|
|
||||||
|
|
||||||
let hybrid bouncing_ball (p0, v0) = p where
|
let hybrid bouncing_ball (p0, v0) = p where
|
||||||
rec der p = v init p0 reset z -> 0.0
|
rec der p = v init p0 reset z -> 0.0
|
||||||
and der v = -. g init v0 reset z -> -0.8 *. last v
|
and der v = -. g init v0 reset z -> -0.8 *. last v
|
||||||
and z = up(-. p)
|
and z = up(-. p)
|
||||||
|
|
||||||
|
let hybrid time_bounces (p0, v0) = (p, b) where
|
||||||
|
rec p = bouncing_ball (p0, v0)
|
||||||
|
and der t = 1.0 init 0.0
|
||||||
|
and init b = 0.0
|
||||||
|
and present up(-. p) -> do
|
||||||
|
b = t -. last b
|
||||||
|
done
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
(** - w FIXME e can mix discrete and continuous behaviours *)
|
||||||
|
|
|
||||||
17
exm/sincos/dune
Normal file
17
exm/sincos/dune
Normal file
|
|
@ -0,0 +1,17 @@
|
||||||
|
(env
|
||||||
|
(dev
|
||||||
|
(flags
|
||||||
|
(:standard -w -a))))
|
||||||
|
|
||||||
|
(rule
|
||||||
|
(targets exm_sincos.ml sincos.ml sincos.zci)
|
||||||
|
(deps
|
||||||
|
(:zl sincos.zls)
|
||||||
|
(package zelus))
|
||||||
|
(action
|
||||||
|
(run zeluc -s main -o exm_sincos %{zl})))
|
||||||
|
|
||||||
|
(executable
|
||||||
|
(name exm_sincos)
|
||||||
|
(public_name exm_sincos)
|
||||||
|
(libraries zelus))
|
||||||
21
exm/sincos/sincos.zls
Normal file
21
exm/sincos/sincos.zls
Normal file
|
|
@ -0,0 +1,21 @@
|
||||||
|
|
||||||
|
let dt = 0.001 (* Integration step *)
|
||||||
|
|
||||||
|
let node f_integr (x0, x') = x where (* Forward Euler integrator *)
|
||||||
|
rec x = x0 -> pre (x +. x' *. dt)
|
||||||
|
let node b_integr (x0, x') = x where (* Backward Euler integrator *)
|
||||||
|
rec x = x0 -> (pre x) +. x' *. dt
|
||||||
|
|
||||||
|
let node sincos () = (sin, cos) where
|
||||||
|
rec sin = f_integr(0.0, cos)
|
||||||
|
and cos = b_integr(1.0, -. sin)
|
||||||
|
|
||||||
|
let node main () =
|
||||||
|
let rec t = 0.0 -> pre t +. dt in
|
||||||
|
let (sin, cos) = sincos () in
|
||||||
|
match t <= 500.0 with
|
||||||
|
| true ->
|
||||||
|
(print_float t; print_string "\t";
|
||||||
|
print_float sin; print_string "\t";
|
||||||
|
print_float cos; print_newline ())
|
||||||
|
| false -> ()
|
||||||
12
justfile
12
justfile
|
|
@ -1,3 +1,11 @@
|
||||||
exm1:
|
exm1:
|
||||||
timeout 1s dune exec exm_ball_discrete \
|
timeout 1s dune exec exm_ball | \
|
||||||
| feedgnuplot --stream --domain --lines
|
feedgnuplot --stream --domain --lines
|
||||||
|
|
||||||
|
exm2:
|
||||||
|
timeout 10s dune exec exm_sincos | \
|
||||||
|
feedgnuplot --stream --domain --lines
|
||||||
|
|
||||||
|
exm3:
|
||||||
|
dune exec ballc.exe | \
|
||||||
|
feedgnuplot --stream --domain --lines
|
||||||
|
|
|
||||||
41
lib/hsim/csolver.ml
Normal file
41
lib/hsim/csolver.ml
Normal file
|
|
@ -0,0 +1,41 @@
|
||||||
|
open Zls
|
||||||
|
open Odexx.Ode45
|
||||||
|
|
||||||
|
type state =
|
||||||
|
{ state: t; vec: carray; now: Full.time }
|
||||||
|
|
||||||
|
let make_full () : (carray, carray) Full.csolver =
|
||||||
|
let state =
|
||||||
|
let v = Zls.cmake 0 in
|
||||||
|
let state = initialize (fun _ _ _ -> ()) (vec v) in
|
||||||
|
set_stop_time state 1.0; { state; vec=v; now=0.0 } in
|
||||||
|
let step ({ state ; vec=v; now } as s) h =
|
||||||
|
let y_nv = vec v in
|
||||||
|
let h = step state h y_nv in
|
||||||
|
let state = copy state in
|
||||||
|
let f t = get_dky state y_nv (now +. t) 0; unvec y_nv in
|
||||||
|
{ s with now=now +. h }, { Full.h=h -. now; f } in
|
||||||
|
let reset _ Full.{ fder; y0; h } =
|
||||||
|
let fder t cvec dvec = Zls.blit (fder t cvec) dvec in
|
||||||
|
let state = initialize fder (vec y0) in
|
||||||
|
set_stop_time state h;
|
||||||
|
{ state; vec = y0; now=0.0 } in
|
||||||
|
Full.DNode { state; step; reset }
|
||||||
|
|
||||||
|
let make () : (carray, carray) Fill.csolver =
|
||||||
|
let state =
|
||||||
|
let v = Zls.cmake 0 in
|
||||||
|
let state = initialize (fun _ _ _ -> ()) (vec v) in
|
||||||
|
set_stop_time state 1.0; { state; vec=v; now=0.0 } in
|
||||||
|
let step ({ state ; vec=v; now } as s) h =
|
||||||
|
let y_nv = vec v in
|
||||||
|
let h = step state h y_nv in
|
||||||
|
let state = copy state in
|
||||||
|
let f t = get_dky state y_nv (now +. t) 0; unvec y_nv in
|
||||||
|
{ s with now=now +. h }, { Fill.h=h -. now; f } in
|
||||||
|
let reset _ Fill.{ fder; y0; h } =
|
||||||
|
let fder t cvec dvec = Zls.blit (fder t cvec) dvec in
|
||||||
|
let state = initialize fder (vec y0) in
|
||||||
|
set_stop_time state h;
|
||||||
|
{ state; vec = y0; now=0.0 } in
|
||||||
|
Fill.DNode { state; step; reset }
|
||||||
|
|
@ -4,4 +4,5 @@
|
||||||
(:standard -w -50))))
|
(:standard -w -50))))
|
||||||
|
|
||||||
(library
|
(library
|
||||||
(name hsim))
|
(name hsim)
|
||||||
|
(libraries zelus))
|
||||||
|
|
|
||||||
161
lib/hsim/fill.ml
161
lib/hsim/fill.ml
|
|
@ -1,34 +1,35 @@
|
||||||
[@@@warning "-27-50-69"]
|
[@@@warning "-27-50-69"]
|
||||||
let todo = assert false
|
|
||||||
|
|
||||||
(* Little OCaml reminder *)
|
|
||||||
|
|
||||||
type t = { foo : int; bar : int; }
|
|
||||||
|
|
||||||
let f () =
|
|
||||||
let baz = { foo = 0; bar = 1 } in
|
|
||||||
let qux = { baz with foo = 2 } in (* same as "baz", except field "foo" *)
|
|
||||||
assert (qux = { foo = 2; bar = 1 })
|
|
||||||
|
|
||||||
|
(* Little OCaml reminder: *)
|
||||||
|
type _s = A | B of int | C of float * string (* sum types *)
|
||||||
|
type _t = { a : int; b : int; } (* product types *)
|
||||||
|
|
||||||
|
let _f () =
|
||||||
|
let x = { a = 0; b = 1 } in
|
||||||
|
let y = { x with a = 2 } in (* same as "x", except field "a" *)
|
||||||
|
assert (y = { a = 2; b = 1 })
|
||||||
|
|
||||||
|
(* Everything is immutable, except explicitly declared record fields! *)
|
||||||
|
type _q = { c : int (* immutable *); mutable d : int; }
|
||||||
|
|
||||||
|
(* Types can be parameterized by other types: *)
|
||||||
|
type 'a _llist = Nil | Cons of { v : 'a; mutable next : 'a _llist }
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
(** Discrete-time node *)
|
(** Discrete-time node *)
|
||||||
type ('i, 'o, 'r, 's) dnode =
|
type ('i, 'o, 'r) dnode =
|
||||||
DNode of {
|
DNode : {
|
||||||
state : 's; (** current state *)
|
state : 's; (** current state *)
|
||||||
step : 's -> 'i -> 's * 'o; (** step function *)
|
step : 's -> 'i -> 's * 'o; (** step function *)
|
||||||
reset : 's -> 'r -> 's; (** reset function *)
|
reset : 's -> 'r -> 's; (** reset function *)
|
||||||
}
|
} -> ('i, 'o, 'r) dnode
|
||||||
|
|
||||||
|
|
||||||
(** Run a discrete node on a list of inputs *)
|
(** Run a discrete node on a list of inputs *)
|
||||||
let drun (DNode n : ('i, 'o, 'r, 's) dnode) (i : 'i list) : 'o list =
|
let drun (DNode n : ('i, 'o, 'r) dnode) (i : 'i list) : 'o list =
|
||||||
todo
|
snd (List.fold_left_map n.step n.state i)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -60,7 +61,7 @@ type 'a signal =
|
||||||
(** Initial value problem (IVP) *)
|
(** Initial value problem (IVP) *)
|
||||||
type ('y, 'yder) ivp =
|
type ('y, 'yder) ivp =
|
||||||
{ y0 : 'y; (** initial position *)
|
{ y0 : 'y; (** initial position *)
|
||||||
fder : time -> 'y -> 'yder; (** derivative function on [[0.0, h]] *)
|
fder : time -> 'y -> 'yder; (** derivative function *)
|
||||||
h : time; } (** maximal horizon *)
|
h : time; } (** maximal horizon *)
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -75,11 +76,11 @@ type ('y, 'yder) ivp =
|
||||||
|
|
||||||
|
|
||||||
(** ODE solver *)
|
(** ODE solver *)
|
||||||
type ('y, 'yder, 's) csolver =
|
type ('y, 'yder) csolver =
|
||||||
(time, (** requested horizon *)
|
(time, (** requested horizon *)
|
||||||
'y dense, (** solution approximation *)
|
'y dense, (** solution approximation *)
|
||||||
('y, 'yder) ivp, (** initial value problem *)
|
('y, 'yder) ivp) (** initial value problem *)
|
||||||
's) dnode
|
dnode
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -111,11 +112,11 @@ type ('y, 'zin) zcp =
|
||||||
|
|
||||||
|
|
||||||
(** Zero-crossing solver *)
|
(** Zero-crossing solver *)
|
||||||
type ('y, 'zin, 'zout, 's) zsolver =
|
type ('y, 'zin, 'zout) zsolver =
|
||||||
('y dense, (** input value *)
|
('y dense, (** input value *)
|
||||||
time * 'zout, (** horizon and zero-crossing events *)
|
time * 'zout, (** horizon and zero-crossing events *)
|
||||||
('y, 'zin) zcp, (** zero-crossing problem *)
|
('y, 'zin) zcp) (** zero-crossing problem *)
|
||||||
's) dnode
|
dnode
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -128,12 +129,12 @@ type ('y, 'zin, 'zout, 's) zsolver =
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
(** Full solver (composition of an ODE and zero-crossing solver) *)
|
(** Full solver (composition of an ODE and zero-crossing solver) *)
|
||||||
type ('y, 'yder, 'zin, 'zout, 's) solver =
|
type ('y, 'yder, 'zin, 'zout) solver =
|
||||||
(time, (** requested horizon *)
|
(time, (** requested horizon *)
|
||||||
'y dense * 'zout, (** output and zero-crossing events *)
|
'y dense * 'zout, (** output and zero-crossing events *)
|
||||||
('y, 'yder) ivp * ('y, 'zin) zcp, (** (re)initialization parameters *)
|
('y, 'yder) ivp * ('y, 'zin) zcp) (** (re)initialization parameters *)
|
||||||
's) dnode
|
dnode
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -143,98 +144,98 @@ type ('y, 'yder, 'zin, 'zout, 's) solver =
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
(** Compose an ODE solver and a zero-crossing solver *)
|
(** Compose an ODE solver and a zero-crossing solver. *)
|
||||||
let build_solver : ('y, 'yder, 'cs) csolver ->
|
let compose_solvers : ('y, 'yder) csolver ->
|
||||||
('y, 'zin, 'zout, 'zs) zsolver ->
|
('y, 'zin, 'zout) zsolver ->
|
||||||
('y, 'yder, 'zin, 'zout, 'cs * 'zs) solver
|
('y, 'yder, 'zin, 'zout) solver
|
||||||
= fun (DNode cs) (DNode zs) ->
|
= fun (DNode csolver) (DNode zsolver) ->
|
||||||
let state = (cs.state, zs.state) in
|
let state = (csolver.state, zsolver.state) in
|
||||||
let step (cstate, zstate) (h : time) =
|
let step (cstate, zstate) h =
|
||||||
todo in
|
let cstate, y = csolver.step cstate h in
|
||||||
|
let zstate, (h, z) = zsolver.step zstate y in
|
||||||
|
(cstate, zstate), ({ y with h }, z) in
|
||||||
let reset (cstate, zstate) (ivp, zcp) =
|
let reset (cstate, zstate) (ivp, zcp) =
|
||||||
(cs.reset cstate ivp, zs.reset zstate zcp) in
|
(csolver.reset cstate ivp, zsolver.reset zstate zcp) in
|
||||||
DNode { state; step; reset }
|
DNode { state; step; reset }
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
(** Hybrid (discrete-time and continuous-time) node *)
|
(** Hybrid (discrete-time and continuous-time) node *)
|
||||||
type ('i, 'o, 'r, 's, 'y, 'yder, 'zin, 'zout) hnode =
|
type ('i, 'o, 'r, 'y, 'yder, 'zin, 'zout) hnode =
|
||||||
HNode of {
|
HNode : {
|
||||||
state : 's; (** current state *)
|
state : 's; (** current state *)
|
||||||
step : 's -> 'i -> 's * 'o; (** discrete step function *)
|
step : 's -> time -> 'i -> 's * 'o; (** discrete step function *)
|
||||||
reset : 's -> 'r -> 's; (** reset function *)
|
reset : 's -> 'r -> 's; (** reset function *)
|
||||||
fder : 's -> 'i -> 'y -> 'yder; (** derivative function *)
|
fder : 's -> time -> 'i -> 'y -> 'yder; (** derivative function *)
|
||||||
fzer : 's -> 'i -> 'y -> 'zin; (** zero-crossing function *)
|
fzer : 's -> time -> 'i -> 'y -> 'zin; (** zero-crossing function *)
|
||||||
fout : 's -> 'i -> 'y -> 'o; (** continuous output function *)
|
fout : 's -> time -> 'i -> 'y -> 'o; (** continuous output function *)
|
||||||
cget : 's -> 'y; (** continuous state getter *)
|
cget : 's -> 'y; (** continuous state getter *)
|
||||||
cset : 's -> 'y -> 's; (** continuous state setter *)
|
cset : 's -> 'y -> 's; (** continuous state setter *)
|
||||||
zset : 's -> 'zout -> 's; (** zero-crossing information setter *)
|
zset : 's -> 'zout -> 's; (** zero-crossing information setter *)
|
||||||
jump : 's -> bool; (** discrete go-again indicator *)
|
jump : 's -> bool; (** discrete go-again function *)
|
||||||
}
|
} -> ('i, 'o, 'r, 'y, 'yder, 'zin, 'zout) hnode
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
(** Simulation mode (either discrete or continuous) *)
|
|
||||||
|
(** Simulation mode (either discrete ([D]) or continuous ([C])). *)
|
||||||
type mode = D | C
|
type mode = D | C
|
||||||
|
|
||||||
(** Simulation state *)
|
(** Simulation state *)
|
||||||
type ('i, 'o, 'r, 'y, 'yder, 'zin, 'zout, 'ms, 'ss) state =
|
type ('i, 'o, 'r, 'y) state =
|
||||||
State of {
|
State : {
|
||||||
solver : (** solver state *)
|
solver : ('y, 'yder, 'zin, 'zout) solver; (** solver state *)
|
||||||
('y, 'yder, 'zin, 'zout,'ss) solver;
|
model : ('i, 'o, 'r, 'y, 'yder, 'zin, 'zout) hnode; (** model state *)
|
||||||
model : (** model state *)
|
|
||||||
('i, 'o, 'r, 'ms, 'y, 'yder, 'zin, 'zout) hnode;
|
|
||||||
input : 'i signal; (** current input *)
|
input : 'i signal; (** current input *)
|
||||||
time : time; (** current time *)
|
time : time; (** current time *)
|
||||||
mode : mode; (** current step mode *)
|
mode : mode; (** current step mode *)
|
||||||
}
|
} -> ('i, 'o, 'r, 'y) state
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
(** Discrete simulation step *)
|
(** Discrete simulation step *)
|
||||||
let dstep (State ({ model = HNode m; solver = DNode s; _ } as state)) =
|
let dstep (State ({ model = HNode m; solver = DNode s; _ } as state)) =
|
||||||
let i = Option.get state.input in
|
let i = Option.get state.input in
|
||||||
let (ms, o) = m.step m.state (todo (* current input? *)) in
|
let ms, o = m.step m.state state.time (i.f state.time) in
|
||||||
let model = HNode { m with state = todo (* ? *) } in
|
let model = HNode { m with state = ms } in
|
||||||
let state =
|
let state = if m.jump ms then
|
||||||
if m.jump ms then State { state with model = todo (* ? *) }
|
State { state with model }
|
||||||
else if state.time >= i.h then
|
else if state.time >= i.h then
|
||||||
State { state with input = todo (* ? *); model; time = todo (* ? *) }
|
State { state with input = None; model; time = 0. }
|
||||||
else
|
else
|
||||||
let y0 = todo (* ? *) and h = i.h -. state.time in
|
let y0 = m.cget ms and h = i.h -. state.time and ofs = (+.) state.time in
|
||||||
let ivp = { h; y0; fder = fun t y -> m.fder ms (i.f todo (* ? *)) y } in
|
let ivp = { h; y0; fder = fun t y -> m.fder ms (ofs t) (i.f (ofs t)) y } in
|
||||||
let zcp = { h; y0; fzer = fun t y -> m.fzer ms (i.f todo (* ? *)) y } in
|
let zcp = { h; y0; fzer = fun t y -> m.fzer ms (ofs t) (i.f (ofs t)) y } in
|
||||||
let solver = DNode { s with state = s.reset s.state (ivp, zcp) } in
|
let solver = DNode { s with state = s.reset s.state (ivp, zcp) } in
|
||||||
State { state with model; solver; mode = todo (* ? *) } in
|
let input = Some { h; f = fun t -> i.f (ofs t) } in
|
||||||
(state, Some { h = 0.; f = fun _ -> o })
|
State { model; solver; mode = C; time = 0.; input } in
|
||||||
|
state, Some { h = 0.; f = fun _ -> o }
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
(** Continuous simulation step *)
|
(** Continuous simulation step *)
|
||||||
let cstep (State ({ model = HNode m; solver = DNode s; _ } as st)) =
|
let cstep (State ({ model = HNode m; solver = DNode s; _ } as state)) =
|
||||||
let i = Option.get st.input in
|
let i = Option.get state.input in
|
||||||
let (ss, (y, z)) = s.step s.state todo (* ? *) in
|
let ss, (y, z) = s.step s.state i.h in
|
||||||
let ms = m.zset (m.cset m.state (y.f y.h)) z in
|
|
||||||
let out = Some { y with f = fun t -> m.fout ms todo (* ? *) (y.f t) } in
|
|
||||||
let mode = if m.jump ms || st.time +. y.h >= i.h then D else C in
|
|
||||||
let model = HNode { m with state = ms } in
|
|
||||||
let solver = DNode { s with state = ss } in
|
let solver = DNode { s with state = ss } in
|
||||||
(State { st with model; solver; mode; time = todo (* ? *) }, out)
|
let ms = m.zset (m.cset m.state (y.f y.h)) z in
|
||||||
|
let model = HNode { m with state = ms } in
|
||||||
|
let ofs = (+.) state.time in
|
||||||
|
let out = { y with f = fun t -> m.fout ms (ofs t) (i.f (ofs t)) (y.f t) } in
|
||||||
|
let mode = if m.jump ms || state.time +. y.h >= i.h then D else C in
|
||||||
|
State { state with model; solver; mode; time = state.time +. y.h }, Some out
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
(** Simulate a hybrid model with a solver *)
|
(** Simulate a hybrid model with a solver *)
|
||||||
let hsim : ('i, 'o, 'r, 'ms, 'y, 'yder, 'zin, 'zout) hnode ->
|
let hsim : ('i, 'o, 'r, 'y, 'yder, 'zin, 'zout) hnode ->
|
||||||
('y, 'yder, 'zin, 'zout, 'ss) solver ->
|
('y, 'yder, 'zin, 'zout) solver ->
|
||||||
('i signal, 'o signal, 'r, _) dnode
|
('i signal, 'o signal, 'r) dnode
|
||||||
= fun model solver ->
|
= fun model solver ->
|
||||||
let state = State { model; solver; input = None; time = 0.; mode = D } in
|
let state = State { model; solver; input = None; time = 0.; mode = D } in
|
||||||
let step (State s as st) input = match (input, s.input, s.mode) with
|
let step (State s as st) input = match (input, s.input, s.mode) with
|
||||||
|
|
@ -253,8 +254,8 @@ let hsim : ('i, 'o, 'r, 'ms, 'y, 'yder, 'zin, 'zout) hnode ->
|
||||||
|
|
||||||
|
|
||||||
(** Run a simulation on a list of inputs *)
|
(** Run a simulation on a list of inputs *)
|
||||||
let hrun (model : ('i, 'o, 'r, 'ms, 'y, 'yder, 'zin, 'zout) hnode)
|
let hrun (model : ('i, 'o, 'r, 'y, 'yder, 'zin, 'zout) hnode)
|
||||||
(solver : ('y, 'yder, 'zin, 'zout, 'ss) solver)
|
(solver : ('y, 'yder, 'zin, 'zout) solver)
|
||||||
(i : 'i dense list) : 'o dense list
|
(i : 'i dense list) : 'o dense list
|
||||||
= let sim = hsim model solver and i = List.map Option.some i in
|
= let sim = hsim model solver and i = List.map Option.some i in
|
||||||
let rec step os (DNode sim) i =
|
let rec step os (DNode sim) i =
|
||||||
|
|
@ -263,7 +264,3 @@ let hrun (model : ('i, 'o, 'r, 'ms, 'y, 'yder, 'zin, 'zout) hnode)
|
||||||
if o = None then (sim, List.rev_map Option.get os)
|
if o = None then (sim, List.rev_map Option.get os)
|
||||||
else step (o :: os) sim None in
|
else step (o :: os) sim None in
|
||||||
List.fold_left_map (step []) sim i |> snd |> List.flatten
|
List.fold_left_map (step []) sim i |> snd |> List.flatten
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1,110 +0,0 @@
|
||||||
|
|
||||||
(** Discrete-time node *)
|
|
||||||
type ('i, 'o, 'r, 's) dnode =
|
|
||||||
DNode of {
|
|
||||||
state : 's; (** current state *)
|
|
||||||
step : 's -> 'i -> 's * 'o; (** step function *)
|
|
||||||
reset : 's -> 'r -> 's; (** reset function *)
|
|
||||||
}
|
|
||||||
|
|
||||||
(** Run a discrete node on a list of inputs *)
|
|
||||||
val drun : ('i, 'o, 'r, 's) dnode -> 'i list -> 'o list
|
|
||||||
|
|
||||||
type time =
|
|
||||||
float (** [≥ 0.0] *)
|
|
||||||
|
|
||||||
(** Interval-defined functions *)
|
|
||||||
type 'a dense =
|
|
||||||
{ h : time; (** horizon *)
|
|
||||||
f : time -> 'a } (** [f : [0, h] -> α] *)
|
|
||||||
|
|
||||||
(** Continuous-time signal *)
|
|
||||||
type 'a signal =
|
|
||||||
'a dense option
|
|
||||||
|
|
||||||
(** Initial value problem (IVP) *)
|
|
||||||
type ('y, 'yder) ivp =
|
|
||||||
{ y0 : 'y; (** initial position *)
|
|
||||||
fder : time -> 'y -> 'yder; (** derivative function on [[0.0, h]] *)
|
|
||||||
h : time; } (** maximal horizon *)
|
|
||||||
|
|
||||||
(** ODE solver *)
|
|
||||||
type ('y, 'yder, 's) csolver =
|
|
||||||
(time, (** requested horizon *)
|
|
||||||
'y dense, (** solution approximation *)
|
|
||||||
('y, 'yder) ivp, (** initial value problem *)
|
|
||||||
's) dnode
|
|
||||||
|
|
||||||
(** Zero-crossing problem (ZCP) *)
|
|
||||||
type ('y, 'zin) zcp =
|
|
||||||
{ y0 : 'y; (** initial position *)
|
|
||||||
fzer : time -> 'y -> 'zin; (** zero-crossing function *)
|
|
||||||
h : time; } (** maximal horizon *)
|
|
||||||
|
|
||||||
(** Zero-crossing solver *)
|
|
||||||
type ('y, 'zin, 'zout, 's) zsolver =
|
|
||||||
('y dense, (** input value *)
|
|
||||||
time * 'zout, (** horizon and zero-crossing events *)
|
|
||||||
('y, 'zin) zcp, (** zero-crossing problem *)
|
|
||||||
's) dnode
|
|
||||||
|
|
||||||
(** Full solver (composition of an ODE and zero-crossing solver) *)
|
|
||||||
type ('y, 'yder, 'zin, 'zout, 's) solver =
|
|
||||||
(time, (** requested horizon *)
|
|
||||||
'y dense * 'zout, (** output and zero-crossing events *)
|
|
||||||
('y, 'yder) ivp * ('y, 'zin) zcp, (** (re)initialization parameters *)
|
|
||||||
's) dnode
|
|
||||||
|
|
||||||
(** Compose an ODE solver and a zero-crossing solver *)
|
|
||||||
val build_solver : ('y, 'yder, 'cs) csolver ->
|
|
||||||
('y, 'zin, 'zout, 'zs) zsolver ->
|
|
||||||
('y, 'yder, 'zin, 'zout, 'cs * 'zs) solver
|
|
||||||
|
|
||||||
(** Hybrid (discrete-time and continuous-time) node *)
|
|
||||||
type ('i, 'o, 'r, 's, 'y, 'yder, 'zin, 'zout) hnode =
|
|
||||||
HNode of {
|
|
||||||
state : 's; (** current state *)
|
|
||||||
step : 's -> 'i -> 's * 'o; (** discrete step function *)
|
|
||||||
reset : 's -> 'r -> 's; (** reset function *)
|
|
||||||
fder : 's -> 'i -> 'y -> 'yder; (** derivative function *)
|
|
||||||
fzer : 's -> 'i -> 'y -> 'zin; (** zero-crossing function *)
|
|
||||||
fout : 's -> 'i -> 'y -> 'o; (** continuous output function *)
|
|
||||||
cget : 's -> 'y; (** continuous state getter *)
|
|
||||||
cset : 's -> 'y -> 's; (** continuous state setter *)
|
|
||||||
zset : 's -> 'zout -> 's; (** zero-crossing information setter *)
|
|
||||||
jump : 's -> bool; (** discrete go-again indicator *)
|
|
||||||
}
|
|
||||||
|
|
||||||
(** Simulation mode (either discrete or continuous) *)
|
|
||||||
type mode = D | C
|
|
||||||
|
|
||||||
(** Simulation state *)
|
|
||||||
type ('i, 'o, 'r, 'y, 'yder, 'zin, 'zout, 'ms, 'ss) state =
|
|
||||||
State of {
|
|
||||||
solver : (** solver state *)
|
|
||||||
('y, 'yder, 'zin, 'zout, 'ss) solver;
|
|
||||||
model : (** model state *)
|
|
||||||
('i, 'o, 'r, 'ms, 'y, 'yder, 'zin, 'zout) hnode;
|
|
||||||
input : 'i signal; (** current input *)
|
|
||||||
time : time; (** current time *)
|
|
||||||
mode : mode; (** current step mode *)
|
|
||||||
}
|
|
||||||
|
|
||||||
(** Discrete simulation step *)
|
|
||||||
val dstep : ('i, 'o, 'r, 'y, 'yder, 'zin, 'zout, 'ms, 'ss) state ->
|
|
||||||
('i, 'o, 'r, 'y, 'yder, 'zin, 'zout, 'ms, 'ss) state * 'o signal
|
|
||||||
|
|
||||||
(** Continuous simulation step *)
|
|
||||||
val cstep : ('i, 'o, 'r, 'y, 'yder, 'zin, 'zout, 'ms, 'ss) state ->
|
|
||||||
('i, 'o, 'r, 'y, 'yder, 'zin, 'zout, 'ms, 'ss) state * 'o signal
|
|
||||||
|
|
||||||
(** Simulate a hybrid model with a solver *)
|
|
||||||
val hsim : ('i, 'o, 'r, 'ms, 'y, 'yder, 'zin, 'zout) hnode ->
|
|
||||||
('y, 'yder, 'zin, 'zout, 'ss) solver ->
|
|
||||||
('i signal, 'o signal, 'r, ('i, 'o, 'r, 'y, 'yder, 'zin, 'zout, 'ms, 'ss) state) dnode
|
|
||||||
|
|
||||||
(** Run a simulation on a list of inputs *)
|
|
||||||
val hrun : ('i, 'o, 'r, 'ms, 'y, 'yder, 'zin, 'zout) hnode ->
|
|
||||||
('y, 'yder, 'zin, 'zout, 'ss) solver ->
|
|
||||||
'i dense list ->
|
|
||||||
'o dense list
|
|
||||||
|
|
@ -145,11 +145,11 @@ let compose_solvers :
|
||||||
type ('i, 'o, 'r, 'y, 'yder, 'zin, 'zout) hnode =
|
type ('i, 'o, 'r, 'y, 'yder, 'zin, 'zout) hnode =
|
||||||
HNode : {
|
HNode : {
|
||||||
state : 's; (** current state *)
|
state : 's; (** current state *)
|
||||||
step : 's -> 'i -> 's * 'o; (** discrete step function *)
|
step : 's -> time -> 'i -> 's * 'o; (** discrete step function *)
|
||||||
|
fder : 's -> time -> 'i -> 'y -> 'yder; (** derivative function *)
|
||||||
|
fzer : 's -> time -> 'i -> 'y -> 'zin; (** zero-crossing function *)
|
||||||
|
fout : 's -> time -> 'i -> 'y -> 'o; (** continuous output function *)
|
||||||
reset : 's -> 'r -> 's; (** reset function *)
|
reset : 's -> 'r -> 's; (** reset function *)
|
||||||
fder : 's -> 'i -> 'y -> 'yder; (** derivative function *)
|
|
||||||
fzer : 's -> 'i -> 'y -> 'zin; (** zero-crossing function *)
|
|
||||||
fout : 's -> 'i -> 'y -> 'o; (** continuous output function *)
|
|
||||||
cget : 's -> 'y; (** continuous state getter *)
|
cget : 's -> 'y; (** continuous state getter *)
|
||||||
cset : 's -> 'y -> 's; (** continuous state setter *)
|
cset : 's -> 'y -> 's; (** continuous state setter *)
|
||||||
zset : 's -> 'zout -> 's; (** zero-crossing information setter *)
|
zset : 's -> 'zout -> 's; (** zero-crossing information setter *)
|
||||||
|
|
@ -189,10 +189,8 @@ type mode = D | C
|
||||||
time, and step mode. *)
|
time, and step mode. *)
|
||||||
type ('i, 'o, 'r, 'y) state =
|
type ('i, 'o, 'r, 'y) state =
|
||||||
State : {
|
State : {
|
||||||
solver : (** solver state *)
|
solver : ('y, 'yder, 'zin, 'zout) solver; (** solver state *)
|
||||||
('y, 'yder, 'zin, 'zout) solver;
|
model : ('i, 'o, 'r, 'y, 'yder, 'zin, 'zout) hnode; (** model state *)
|
||||||
model : (** model state *)
|
|
||||||
('i, 'o, 'r, 'y, 'yder, 'zin, 'zout) hnode;
|
|
||||||
input : 'i signal; (** current input *)
|
input : 'i signal; (** current input *)
|
||||||
time : time; (** current time *)
|
time : time; (** current time *)
|
||||||
mode : mode; (** current step mode *)
|
mode : mode; (** current step mode *)
|
||||||
|
|
@ -206,7 +204,7 @@ type ('i, 'o, 'r, 'y) state =
|
||||||
let dstep (State ({ model = HNode m; solver = DNode s; _ } as st)) =
|
let dstep (State ({ model = HNode m; solver = DNode s; _ } as st)) =
|
||||||
let i = Option.get st.input in
|
let i = Option.get st.input in
|
||||||
(* Step the model *)
|
(* Step the model *)
|
||||||
let ms, o = m.step m.state (i.f st.time) in
|
let ms, o = m.step m.state st.time (i.f st.time) in
|
||||||
let model = HNode { m with state = ms } in
|
let model = HNode { m with state = ms } in
|
||||||
let st =
|
let st =
|
||||||
if m.jump ms then
|
if m.jump ms then
|
||||||
|
|
@ -217,11 +215,12 @@ let dstep (State ({ model = HNode m; solver = DNode s; _ } as st)) =
|
||||||
State { st with input = None; model; time = 0. }
|
State { st with input = None; model; time = 0. }
|
||||||
else
|
else
|
||||||
(* otherwise, reset the solver and switch to continuous mode. *)
|
(* otherwise, reset the solver and switch to continuous mode. *)
|
||||||
let y0 = m.cget ms and h = i.h -. st.time in
|
let y0 = m.cget ms and h = i.h -. st.time and ofs = (+.) st.time in
|
||||||
let ivp = { h; y0; fder = fun t -> m.fder ms (i.f (t +. st.time)) } in
|
let ivp = { h; y0; fder = fun t -> m.fder ms (ofs t) (i.f (ofs t)) } in
|
||||||
let zcp = { h; y0; fzer = fun t -> m.fzer ms (i.f (t +. st.time)) } in
|
let zcp = { h; y0; fzer = fun t -> m.fzer ms (ofs t) (i.f (ofs t)) } in
|
||||||
let solver = DNode { s with state = s.reset s.state (ivp, zcp) } in
|
let solver = DNode { s with state = s.reset s.state (ivp, zcp) } in
|
||||||
State { st with model; solver; mode = C } in
|
let input = Some { h = i.h -. st.time; f = fun t -> i.f (ofs t) } in
|
||||||
|
State { model; solver; mode = C; time = 0.; input } in
|
||||||
(* Return the state and the output function (defined only at [0.0]). *)
|
(* Return the state and the output function (defined only at [0.0]). *)
|
||||||
st, Some { h = 0.; f = fun _ -> o }
|
st, Some { h = 0.; f = fun _ -> o }
|
||||||
|
|
||||||
|
|
@ -233,14 +232,14 @@ let cstep (State ({ model = HNode m; solver = DNode s; _ } as st)) =
|
||||||
let i = Option.get st.input in
|
let i = Option.get st.input in
|
||||||
(* Step the solver. *)
|
(* Step the solver. *)
|
||||||
let ss, (y, z) = s.step s.state i.h in
|
let ss, (y, z) = s.step s.state i.h in
|
||||||
|
let solver = DNode { s with state = ss } in
|
||||||
(* Update the model's state. *)
|
(* Update the model's state. *)
|
||||||
let ms = m.zset (m.cset m.state (y.f y.h)) z in
|
let ms = m.zset (m.cset m.state (y.f y.h)) z in
|
||||||
|
let model = HNode { m with state = ms } in
|
||||||
(* Create the output function. *)
|
(* Create the output function. *)
|
||||||
let out = { y with f = fun t -> m.fout ms (i.f (t +. st.time)) (y.f t) } in
|
let out = { y with f = fun t -> m.fout ms (st.time +. t) (i.f (st.time +. t)) (y.f t) } in
|
||||||
(* Compute the mode for the next step. *)
|
(* Compute the mode for the next step. *)
|
||||||
let mode = if m.jump ms || st.time +. y.h >= i.h then D else C in
|
let mode = if m.jump ms || st.time +. y.h >= i.h then D else C in
|
||||||
let model = HNode { m with state = ms } in
|
|
||||||
let solver = DNode { s with state = ss } in
|
|
||||||
(* Return the state and the output function. *)
|
(* Return the state and the output function. *)
|
||||||
State { st with model; solver; mode; time = st.time +. y.h }, Some out
|
State { st with model; solver; mode; time = st.time +. y.h }, Some out
|
||||||
|
|
||||||
|
|
|
||||||
361
lib/hsim/illinois.ml
Normal file
361
lib/hsim/illinois.ml
Normal file
|
|
@ -0,0 +1,361 @@
|
||||||
|
(* This code was originally written by Timothy Bourke and Marc Pouzet and is *)
|
||||||
|
(* part of the Zelus standard library. *)
|
||||||
|
(* It is implemented with in-place modification of arrays. *)
|
||||||
|
|
||||||
|
let debug () =
|
||||||
|
false
|
||||||
|
|
||||||
|
let printf x = Format.printf x
|
||||||
|
|
||||||
|
type root_direction = Up | Down | Either | Ignore
|
||||||
|
|
||||||
|
let extra_precision = ref false
|
||||||
|
let set_precise_logging _ = (extra_precision := true)
|
||||||
|
|
||||||
|
let fold_zxzx f acc f0 f1 =
|
||||||
|
let n = Zls.length f0 in
|
||||||
|
let rec fold acc i =
|
||||||
|
if i = n then acc
|
||||||
|
else
|
||||||
|
let acc' = f i acc f0.{i} f1.{i} in
|
||||||
|
fold acc' (i + 1)
|
||||||
|
in fold acc 0
|
||||||
|
|
||||||
|
(* return a function that looks for zero-crossings *)
|
||||||
|
let get_check_root rdir =
|
||||||
|
let check_up x0 x1 = if x0 < 0.0 && x1 >= 0.0 then 1l else 0l in
|
||||||
|
let check_down x0 x1 = if x0 > 0.0 && x1 <= 0.0 then -1l else 0l in
|
||||||
|
let check_either x0 x1 = if x0 < 0.0 && x1 >= 0.0 then 1l else
|
||||||
|
if x0 > 0.0 && x1 <= 0.0 then -1l else 0l in
|
||||||
|
let no_check _x0 _x1 = 0l in
|
||||||
|
|
||||||
|
match rdir with
|
||||||
|
| Up -> check_up
|
||||||
|
| Down -> check_down
|
||||||
|
| Either -> check_either
|
||||||
|
| Ignore -> no_check
|
||||||
|
|
||||||
|
let up = Up
|
||||||
|
let down = Down
|
||||||
|
let either = Either
|
||||||
|
let ign = Ignore
|
||||||
|
|
||||||
|
(* returns true if a signal has moved from zero to a stritly positive value *)
|
||||||
|
let takeoff f0 f1 =
|
||||||
|
let n = Zls.length f0 in
|
||||||
|
let rec fold acc i =
|
||||||
|
if i = n then acc
|
||||||
|
else if acc then acc else fold ((f0.{i} = 0.0) && (f1.{i} > 0.0)) (i + 1)
|
||||||
|
in fold false 0
|
||||||
|
|
||||||
|
(* return a function that looks for zero-crossings between f0 and f1 *)
|
||||||
|
(** code inutile
|
||||||
|
let make_check_root rdir f0 f1 =
|
||||||
|
let check = get_check_root rdir in
|
||||||
|
(fun i -> check f0.{i} f1.{i})
|
||||||
|
**)
|
||||||
|
|
||||||
|
(* update roots and returns true if there was at least one root *)
|
||||||
|
(* between f0 and f1 for one component of index [i in [0..length f0 - 1]] *)
|
||||||
|
(* update [roots] *)
|
||||||
|
let update_roots calc_zc f0 f1 roots =
|
||||||
|
let update i found x0 x1 =
|
||||||
|
let zc = calc_zc x0 x1 in
|
||||||
|
roots.{i} <- zc;
|
||||||
|
found || (zc <> 0l)
|
||||||
|
in
|
||||||
|
fold_zxzx update false f0 f1
|
||||||
|
|
||||||
|
(* update [roots] *)
|
||||||
|
let clear_roots roots =
|
||||||
|
for i = 0 to Zls.length roots - 1 do
|
||||||
|
roots.{i} <- 0l
|
||||||
|
done
|
||||||
|
|
||||||
|
let log_limits f0 f1 =
|
||||||
|
let logf i _ = printf "z| g[% 2d]: % .24e --> % .24e@." i in
|
||||||
|
fold_zxzx logf () f0 f1
|
||||||
|
|
||||||
|
let log_limit f0 =
|
||||||
|
let logf i _ x _ = printf "z| g[% 2d]: % .24e@." i x in
|
||||||
|
fold_zxzx logf () f0 f0
|
||||||
|
|
||||||
|
(* the type signature of the zero-crossing function *)
|
||||||
|
type zcfn = float -> Zls.carray -> Zls.carray -> unit
|
||||||
|
|
||||||
|
(* type of a session with the solver *)
|
||||||
|
(* zx = g(t, c) yields the values of system zero-crossing expressions
|
||||||
|
|
||||||
|
f0/t0 are the zero-crossing expression values at the last mesh point
|
||||||
|
f1/t1 are the zero-crossing expression values at the next mesh point
|
||||||
|
|
||||||
|
bothf_valid is true when both f0/t0 and f1/t1 are valid and thus find
|
||||||
|
can check for zero-crossings between them.
|
||||||
|
|
||||||
|
roots is the array of booleans returned to callers to indicate on which
|
||||||
|
expressions zero-crossings have been detected.
|
||||||
|
|
||||||
|
calc_zc determines the kind of zero-crossings to seek and report.
|
||||||
|
|
||||||
|
fta and ftb are temporary arrays used when searching for zero-crossings.
|
||||||
|
They are kept in the session as an optimisation to avoid having to
|
||||||
|
continually create and destroy arrays.
|
||||||
|
*)
|
||||||
|
type t = {
|
||||||
|
g : zcfn;
|
||||||
|
mutable bothf_valid : bool;
|
||||||
|
|
||||||
|
mutable f0 : Zls.carray;
|
||||||
|
mutable t0 : float;
|
||||||
|
|
||||||
|
mutable f1 : Zls.carray;
|
||||||
|
mutable t1 : float;
|
||||||
|
|
||||||
|
mutable calc_zc : float -> float -> int32;
|
||||||
|
|
||||||
|
mutable fta : Zls.carray;
|
||||||
|
mutable ftb : Zls.carray;
|
||||||
|
}
|
||||||
|
|
||||||
|
(* Called from find when bothf_valid = false to initialise f1. *)
|
||||||
|
let reinitialize ({ g; f1 = f1; t1 = t1; _ } as s) t c =
|
||||||
|
s.t1 <- t;
|
||||||
|
g t1 c f1; (* fill f1, because it is immediately copied into f0 by next_mesh *)
|
||||||
|
if debug () then (printf "z|---------- init(%.24e, ... ----------@." t;
|
||||||
|
log_limit s.f1);
|
||||||
|
s.bothf_valid <- false
|
||||||
|
|
||||||
|
let initialize_only nroots g =
|
||||||
|
{
|
||||||
|
g = g;
|
||||||
|
bothf_valid = false;
|
||||||
|
|
||||||
|
f0 = Zls.cmake nroots;
|
||||||
|
t0 = 0.0;
|
||||||
|
|
||||||
|
f1 = Zls.cmake nroots;
|
||||||
|
t1 = 0.0;
|
||||||
|
|
||||||
|
fta = Zls.cmake nroots;
|
||||||
|
ftb = Zls.cmake nroots;
|
||||||
|
|
||||||
|
calc_zc = get_check_root Up;
|
||||||
|
}
|
||||||
|
|
||||||
|
let initialize nroots g c =
|
||||||
|
let s = initialize_only nroots g in
|
||||||
|
reinitialize s 0.0 c;
|
||||||
|
s
|
||||||
|
|
||||||
|
|
||||||
|
let num_roots { f0; _ } = Zls.length f0
|
||||||
|
|
||||||
|
(* f0/t0 take the previous values of f1/t1, f1/t1 are refreshed by g *)
|
||||||
|
let step ({ g; f0 = f0; f1 = f1; t1 = t1; _ } as s) t c =
|
||||||
|
(* swap f0 and f1; f0 takes the previous value of f1 *)
|
||||||
|
s.f0 <- f1;
|
||||||
|
s.t0 <- t1;
|
||||||
|
s.f1 <- f0;
|
||||||
|
s.t1 <- t;
|
||||||
|
|
||||||
|
(* calculate a new value for f1 *)
|
||||||
|
g t c s.f1;
|
||||||
|
s.bothf_valid <- true;
|
||||||
|
|
||||||
|
if debug () then
|
||||||
|
(printf "z|---------- step(%.24e, %.24e)----------@." s.t0 s.t1;
|
||||||
|
log_limits s.f0 s.f1)
|
||||||
|
|
||||||
|
type root_interval = SearchLeft | FoundMid | SearchRight
|
||||||
|
|
||||||
|
let resolve_intervals r1 r2 =
|
||||||
|
match r1, r2 with
|
||||||
|
| SearchLeft, _ | _, SearchLeft -> SearchLeft
|
||||||
|
| FoundMid, _ | _, FoundMid -> FoundMid
|
||||||
|
| SearchRight, _ -> SearchRight
|
||||||
|
|
||||||
|
(* Check for zero-crossings between f_left and f_mid, filling roots with the
|
||||||
|
intermediate results and returning:
|
||||||
|
|
||||||
|
SearchLeft zero-crossing in (f_left, f_mid)
|
||||||
|
FoundMid no zero-crossing in (f_left, f_mid)
|
||||||
|
zero-crossing in (f_left, f_mid]
|
||||||
|
SearchRight no zero-crossing in (f_left, f_mid]
|
||||||
|
(possible) zero-crossing in (f_mid, f_right]
|
||||||
|
*)
|
||||||
|
let check_interval calc_zc f_left f_mid =
|
||||||
|
let check _i r x0 x1 =
|
||||||
|
let rv = calc_zc x0 x1 in
|
||||||
|
let r' = if rv = 0l then SearchRight
|
||||||
|
else if x1 = 0.0 then FoundMid
|
||||||
|
else SearchLeft in
|
||||||
|
resolve_intervals r r' in
|
||||||
|
fold_zxzx check SearchRight f_left f_mid
|
||||||
|
|
||||||
|
(* locates the zero-crossing *)
|
||||||
|
(* [find s (dky, c) roots = time] *)
|
||||||
|
(* stores the zero-crossing into the vector [roots] and returns the *)
|
||||||
|
(* time [time] right after the instant one zero-crossing has been found between *)
|
||||||
|
(* time [t0] and [t1] *)
|
||||||
|
let find ({ g = g; bothf_valid = bothf_valid;
|
||||||
|
f0 = f0; t0 = t0; f1 = f1; t1 = t1;
|
||||||
|
fta = fta; ftb = ftb; calc_zc = calc_zc } as s)
|
||||||
|
(dky, c) roots =
|
||||||
|
let ttol = 100.0 *. epsilon_float *. max (abs_float t0) (abs_float t1) in
|
||||||
|
|
||||||
|
(* A small optimisation to avoid copying or overwriting f1 *)
|
||||||
|
let get_f_right ofr = match ofr with None -> f1 | Some f -> f in
|
||||||
|
let f_mid_from_f_right ofr = match ofr with None -> ftb | Some f -> f in
|
||||||
|
|
||||||
|
(* update roots and c; return (t, f0_valid, f0, fta, ftb) *)
|
||||||
|
let interval_too_small t_left t_right f_left f_mid f_right' =
|
||||||
|
dky t_right 0; (* c = dky_0(t_right); update state *)
|
||||||
|
ignore (update_roots calc_zc f_left (get_f_right f_right') roots);
|
||||||
|
|
||||||
|
if debug () then
|
||||||
|
(printf
|
||||||
|
"z|---------- stall(%.24e, %.24e) {interval < %.24e !}--@."
|
||||||
|
t_left t_right ttol;
|
||||||
|
log_limits f_left (get_f_right f_right'));
|
||||||
|
|
||||||
|
match f_right' with
|
||||||
|
| None -> (t_right, false, f_left, f_mid, ftb)
|
||||||
|
| Some f_right -> (t_right, true, f_right, f_mid, f_left) in
|
||||||
|
|
||||||
|
(* Searches between (t_left, f_left) and (t_right, f_right) to find the
|
||||||
|
leftmost (t_mid, f_mid):
|
||||||
|
|
||||||
|
|
|
||||||
|
| f_right
|
||||||
|
|
|
||||||
|
| f_mid
|
||||||
|
+--[t_left---------t_mid---------------t_right]--
|
||||||
|
|
|
||||||
|
| f_left
|
||||||
|
|
|
||||||
|
|
||||||
|
t_left and t_right are the times that bound the interval
|
||||||
|
f_left and f_right are the values at the end points
|
||||||
|
|
||||||
|
f_mid is an array to be filled within the function (if necessary)
|
||||||
|
f_right' is used in the optimisation to avoid copying or overwriting f1
|
||||||
|
|
||||||
|
alpha is a parameter of the Illinois method, and
|
||||||
|
i is used in its calculation
|
||||||
|
|
||||||
|
seek() returns either:
|
||||||
|
(t, false, f0', fta', ftb') - root found at original f_right
|
||||||
|
(i.e., t = original t_right)
|
||||||
|
or
|
||||||
|
(t, true, f0', fta', ftb') - root found at f0' (i.e., t < t_right)
|
||||||
|
*)
|
||||||
|
let rec seek (t_left, f_left, f_mid, t_right, f_right', alpha, i) =
|
||||||
|
let dt = t_right -. t_left in
|
||||||
|
let f_right = get_f_right f_right' in
|
||||||
|
|
||||||
|
let leftmost_midpoint default =
|
||||||
|
let check _ t_min x_left x_right =
|
||||||
|
if x_left = 0.0 then t_min (* ignore expressions equal to zero at LHS *)
|
||||||
|
else
|
||||||
|
let sn = (x_right /. alpha) /. x_left in
|
||||||
|
let sn_d = 1.0 -. sn in
|
||||||
|
(* refer Dahlquist and Bjorck, sec. 6.2.2
|
||||||
|
stop if sn_d is not "large enough" *)
|
||||||
|
let t' =
|
||||||
|
if sn_d <= ttol then t_left +. (dt /. 2.0)
|
||||||
|
else t_right +. (sn /. sn_d) *. dt in
|
||||||
|
min t_min t' in
|
||||||
|
fold_zxzx check default f_left f_right in
|
||||||
|
|
||||||
|
if dt <= ttol
|
||||||
|
then interval_too_small t_left t_right f_left f_mid f_right'
|
||||||
|
else
|
||||||
|
let t_mid = leftmost_midpoint t_right in
|
||||||
|
if t_mid = t_right
|
||||||
|
then interval_too_small t_left t_right f_left f_mid f_right'
|
||||||
|
else begin
|
||||||
|
|
||||||
|
dky t_mid 0; (* c = dky_0(t_mid); interpolate state *)
|
||||||
|
g t_mid c f_mid; (* f_mid = g(t_mid, c); compute zc expressions *)
|
||||||
|
|
||||||
|
match check_interval calc_zc f_left f_mid with
|
||||||
|
| SearchLeft ->
|
||||||
|
if debug () then printf "z| (%.24e -- %.24e] %.24e@."
|
||||||
|
t_left t_mid t_right;
|
||||||
|
let alpha = if i >= 1 then alpha *. 0.5 else alpha in
|
||||||
|
let n_mid = f_mid_from_f_right f_right' in
|
||||||
|
seek (t_left, f_left, n_mid, t_mid, Some f_mid, alpha, i + 1)
|
||||||
|
|
||||||
|
| SearchRight ->
|
||||||
|
if debug () then printf "z| %.24e (%.24e -- %.24e]@."
|
||||||
|
t_left t_mid t_right;
|
||||||
|
let alpha = if i >= 1 then alpha *. 2.0 else alpha in
|
||||||
|
seek (t_mid, f_mid, f_left, t_right, f_right', alpha, i + 1)
|
||||||
|
|
||||||
|
| FoundMid ->
|
||||||
|
if debug () then printf "z| %.24e [%.24e] %.24e@."
|
||||||
|
t_left t_mid t_right;
|
||||||
|
ignore (update_roots calc_zc f_left f_mid roots);
|
||||||
|
let f_tmp = f_mid_from_f_right f_right' in
|
||||||
|
(t_mid, true, f_mid, f_left, f_tmp)
|
||||||
|
end
|
||||||
|
in
|
||||||
|
|
||||||
|
if not bothf_valid then (clear_roots roots; assert false)
|
||||||
|
else begin
|
||||||
|
if debug () then
|
||||||
|
printf "z|\nz|---------- find(%.24e, %.24e)----------@." t0 t1;
|
||||||
|
|
||||||
|
match check_interval calc_zc f0 f1 with
|
||||||
|
| SearchRight -> begin
|
||||||
|
clear_roots roots;
|
||||||
|
s.bothf_valid <- false;
|
||||||
|
assert false
|
||||||
|
end
|
||||||
|
|
||||||
|
| FoundMid -> begin
|
||||||
|
if debug () then printf "z| zero-crossing at limit (%.24e)@." t1;
|
||||||
|
ignore (update_roots calc_zc f0 f1 roots);
|
||||||
|
s.bothf_valid <- false;
|
||||||
|
t1
|
||||||
|
end
|
||||||
|
|
||||||
|
| SearchLeft -> begin
|
||||||
|
let (t, v, f0', fta', ftb') =
|
||||||
|
seek (t0, f0, fta, t1, None, 1.0, 0) in
|
||||||
|
|
||||||
|
s.t0 <- t;
|
||||||
|
s.f0 <- f0';
|
||||||
|
s.bothf_valid <- v;
|
||||||
|
s.fta <- fta';
|
||||||
|
s.ftb <- ftb';
|
||||||
|
|
||||||
|
t
|
||||||
|
end
|
||||||
|
end
|
||||||
|
|
||||||
|
(* the main function of this module *)
|
||||||
|
(* locate a root *)
|
||||||
|
let find s (dky, c) roots = find s (dky, c) roots
|
||||||
|
|
||||||
|
(* is there a root? [has_root s: bool] is true is there is a change in sign *)
|
||||||
|
(* for one component [i in [0..length f0 - 1]] beetwen [f0.(i)] and [f1.(i)] *)
|
||||||
|
let has_roots { bothf_valid; f0; f1; calc_zc; _ } =
|
||||||
|
bothf_valid && (check_interval calc_zc f0 f1 <> SearchRight)
|
||||||
|
|
||||||
|
let takeoff { bothf_valid; f0; f1; _ } =
|
||||||
|
bothf_valid && (takeoff f0 f1)
|
||||||
|
|
||||||
|
(* returns true if a signal has moved from zero to a stritly positive value *)
|
||||||
|
(* Added by MP. Ask Tim if this code is necessary, that is, what happens *)
|
||||||
|
(* with function [find] when the signal is taking off from [0.0] to a *)
|
||||||
|
(* strictly positive value *)
|
||||||
|
let find_takeoff ({ f0; f1; _ } as s) roots =
|
||||||
|
let calc_zc x0 x1 =
|
||||||
|
if (x0 = 0.0) && (x1 > 0.0) then 1l else 0l in
|
||||||
|
let b = update_roots calc_zc f0 f1 roots in
|
||||||
|
if b then begin s.t1 <- s.t0; s.f1 <- s.f0; s.ftb <- s.fta end;
|
||||||
|
s.t0
|
||||||
|
|
||||||
|
let set_root_directions s rd = (s.calc_zc <- get_check_root rd)
|
||||||
|
|
||||||
60
lib/hsim/lift.ml
Normal file
60
lib/hsim/lift.ml
Normal file
|
|
@ -0,0 +1,60 @@
|
||||||
|
|
||||||
|
type ('s, 'a) state =
|
||||||
|
{ mutable state : 's;
|
||||||
|
mutable input : 'a option;
|
||||||
|
mutable time : Ztypes.time;
|
||||||
|
mutable jump : bool; }
|
||||||
|
|
||||||
|
let lift_hsim_full (n : unit Ztypes.hsimu)
|
||||||
|
: (unit, unit, unit, Ztypes.cvec, Ztypes.dvec, Ztypes.zoutvec, Ztypes.zinvec option) Full.hnode * int
|
||||||
|
= let Hsim { alloc; step; reset; derivative; crossings; maxsize; _ } = n in
|
||||||
|
let state = { state = alloc (); input = None; time = 0.0; jump = false } in
|
||||||
|
let csize, zsize = maxsize state.state in
|
||||||
|
let no_zin, no_zout = Zls.zmake zsize, Zls.cmake zsize in
|
||||||
|
let no_der, pos = Zls.cmake csize, Zls.cmake csize in
|
||||||
|
let no_time = -1.0 in reset state.state;
|
||||||
|
let fder { state; time; _ } offset () y =
|
||||||
|
derivative state y no_der no_zin no_zout (time +. offset);
|
||||||
|
no_der in
|
||||||
|
let fzer { state; time; _ } offset () y =
|
||||||
|
crossings state y no_zin no_zout (time +. offset); no_zout in
|
||||||
|
let fout _ _ () _ = () in
|
||||||
|
let step { state; time; _ } offset () =
|
||||||
|
{ state; time=time +. offset; input=Some (); jump = false },
|
||||||
|
step state pos no_der no_zin (time +. offset) in
|
||||||
|
let reset ({ state; _ } as st) () = reset state; st in
|
||||||
|
let jump s = s.jump in
|
||||||
|
let cset ({ state; _ } as st) _ =
|
||||||
|
derivative state pos no_der no_zin no_zout no_time; st in
|
||||||
|
let zset ({ state; _ } as st) = function None -> st | Some zinvec ->
|
||||||
|
derivative state pos no_der zinvec no_zout no_time; { st with jump = true } in
|
||||||
|
let cget { state; _ } =
|
||||||
|
derivative state pos no_der no_zin no_zout no_time; pos in
|
||||||
|
HNode { state; fder; fzer; fout; step; reset; jump; cget; cset; zset }, zsize
|
||||||
|
|
||||||
|
let lift_hsim (n : unit Ztypes.hsimu)
|
||||||
|
: (unit, unit, unit, Ztypes.cvec, Ztypes.dvec, Ztypes.zoutvec, Ztypes.zinvec option) Fill.hnode * int
|
||||||
|
= let Hsim { alloc; step; reset; derivative; crossings; maxsize; _ } = n in
|
||||||
|
let state = { state = alloc (); input = None; time = 0.0; jump = false } in
|
||||||
|
let csize, zsize = maxsize state.state in
|
||||||
|
let no_zin, no_zout = Zls.zmake zsize, Zls.cmake zsize in
|
||||||
|
let no_der, pos = Zls.cmake csize, Zls.cmake csize in
|
||||||
|
let no_time = -1.0 in reset state.state;
|
||||||
|
let fder { state; time; _ } offset () y =
|
||||||
|
derivative state y no_der no_zin no_zout (time +. offset);
|
||||||
|
no_der in
|
||||||
|
let fzer { state; time; _ } offset () y =
|
||||||
|
crossings state y no_zin no_zout (time +. offset); no_zout in
|
||||||
|
let fout _ _ () _ = () in
|
||||||
|
let step { state; time; _ } offset () =
|
||||||
|
{ state; time=time +. offset; input=Some (); jump = false },
|
||||||
|
step state pos no_der no_zin (time +. offset) in
|
||||||
|
let reset ({ state; _ } as st) () = reset state; st in
|
||||||
|
let jump s = s.jump in
|
||||||
|
let cset ({ state; _ } as st) _ =
|
||||||
|
derivative state pos no_der no_zin no_zout no_time; st in
|
||||||
|
let zset ({ state; _ } as st) = function None -> st | Some zinvec ->
|
||||||
|
derivative state pos no_der zinvec no_zout no_time; { st with jump = true } in
|
||||||
|
let cget { state; _ } =
|
||||||
|
derivative state pos no_der no_zin no_zout no_time; pos in
|
||||||
|
HNode { state; fder; fzer; fout; step; reset; jump; cget; cset; zset }, zsize
|
||||||
416
lib/hsim/odexx.ml
Normal file
416
lib/hsim/odexx.ml
Normal file
|
|
@ -0,0 +1,416 @@
|
||||||
|
|
||||||
|
(* This code was originally written by Timothy Bourke and Marc Pouzet and is *)
|
||||||
|
(* part of the Zelus standard library. *)
|
||||||
|
|
||||||
|
open Zls
|
||||||
|
|
||||||
|
module type BUTCHER_TABLEAU =
|
||||||
|
sig (* {{{ *)
|
||||||
|
|
||||||
|
val order : int (* solver order *)
|
||||||
|
|
||||||
|
val initial_reduction_limit_factor : float
|
||||||
|
(* factor limiting the reduction of h after a failed step *)
|
||||||
|
|
||||||
|
(* Butcher Tableau:
|
||||||
|
|
||||||
|
a(0) |
|
||||||
|
a(1) | b(1)
|
||||||
|
a(2) | b(2) b(3)
|
||||||
|
a(3) | b(4) b(5) b(6)
|
||||||
|
... | ...
|
||||||
|
-------+--------------
|
||||||
|
a(n) | b(~) b(~) b(~) ...
|
||||||
|
| b(+) b(+) b(+) ...
|
||||||
|
|
||||||
|
The b(~) values must be included in b.
|
||||||
|
The b(+) values are given indirectly via e.
|
||||||
|
|
||||||
|
e/h = y_n+1 - y*_n+1 = b(~)s - b(+)s
|
||||||
|
|
||||||
|
*)
|
||||||
|
|
||||||
|
val a : float array (* h coefficients; one per stage *)
|
||||||
|
val b : float array (* previous stage coefficients *)
|
||||||
|
val e : float array (* error estimation coefficients *)
|
||||||
|
val bi : float array (* interpolation coefficients *)
|
||||||
|
|
||||||
|
(* let ns be the number of stages, then:
|
||||||
|
size(a) = ns x 1
|
||||||
|
size(b) = ns x ns
|
||||||
|
(but only the lower strictly triangular entries)
|
||||||
|
size(e) = ns
|
||||||
|
size(bi) = ns x po
|
||||||
|
(where po is the order of the interpolating polynomial)
|
||||||
|
*)
|
||||||
|
|
||||||
|
|
||||||
|
end (* }}} *)
|
||||||
|
|
||||||
|
module GenericODE (Butcher : BUTCHER_TABLEAU) : STATE_ODE_SOLVER =
|
||||||
|
struct (* {{{1 *)
|
||||||
|
open Bigarray
|
||||||
|
|
||||||
|
let debug () =
|
||||||
|
false
|
||||||
|
(* !Common.Debug.debug *)
|
||||||
|
|
||||||
|
let pow = 1.0 /. float(Butcher.order)
|
||||||
|
|
||||||
|
let mA r = Butcher.a.(r)
|
||||||
|
let h_matB = Array.copy Butcher.b
|
||||||
|
let update_mhB h = for i = 0 to Array.length h_matB - 1 do
|
||||||
|
h_matB.(i) <- Butcher.b.(i) *. h
|
||||||
|
done
|
||||||
|
(* let mhB r c = if c >= r then 0.0 else h_matB.(((r-1)*r)/2 + c) *)
|
||||||
|
let mhB_row r = Array.sub h_matB (((r-1)*r)/2) r
|
||||||
|
|
||||||
|
let mE c = Butcher.e.(c)
|
||||||
|
|
||||||
|
let maxK = Array.length(Butcher.a) - 1
|
||||||
|
|
||||||
|
let rowsBI = Array.length(Butcher.a)
|
||||||
|
let colsBI = Array.length(Butcher.bi) / rowsBI
|
||||||
|
let maxBI = colsBI - 1
|
||||||
|
|
||||||
|
let h_matBI = Array.copy Butcher.bi
|
||||||
|
let update_mhBI h = for i = 0 to Array.length h_matBI - 1 do
|
||||||
|
h_matBI.(i) <- Butcher.bi.(i) *. h
|
||||||
|
done
|
||||||
|
let mhBI_row r = Array.sub h_matBI (r * colsBI) colsBI
|
||||||
|
|
||||||
|
let minmax minimum maximum x = min maximum (max minimum x)
|
||||||
|
|
||||||
|
let mapinto r f =
|
||||||
|
for i = 0 to Array1.dim r - 1 do
|
||||||
|
r.{i} <- f i
|
||||||
|
done
|
||||||
|
|
||||||
|
let fold2 f a v1 v2 =
|
||||||
|
let acc = ref a in
|
||||||
|
for i = 0 to min (length v1) (length v2) - 1 do
|
||||||
|
acc := f !acc (get v1 i) (get v2 i)
|
||||||
|
done;
|
||||||
|
!acc
|
||||||
|
|
||||||
|
let maxnorm2 f = fold2 (fun acc v1 v2 -> max acc (abs_float (f v1 v2))) 0.0
|
||||||
|
|
||||||
|
type rhsfn = float -> Zls.carray -> Zls.carray -> unit
|
||||||
|
type dkyfn = Zls.carray -> float -> int -> unit
|
||||||
|
|
||||||
|
(* dx = sysf(t, y) describes the system dynamics
|
||||||
|
|
||||||
|
y/time is the current mesh point
|
||||||
|
yold/last_time is the previous mesh point
|
||||||
|
(and also used for intermediate values during the
|
||||||
|
calculation of the next mesh point)
|
||||||
|
|
||||||
|
(y and yold are mutable because they are swapped after having calculated
|
||||||
|
the next mesh point yold)
|
||||||
|
|
||||||
|
h is the step size to be used for calculating the next mesh point.
|
||||||
|
|
||||||
|
k.(0) is the instantaneous derivative at the previous mesh point
|
||||||
|
k.(maxK) is the instantaneous derivative at the current mesh point
|
||||||
|
|
||||||
|
k.(1--maxK-1) track intermediate instantaneous derivatives during the
|
||||||
|
calculation of the next mesh point.
|
||||||
|
*)
|
||||||
|
type t = {
|
||||||
|
mutable sysf : float -> Zls.carray -> Zls.carray -> unit;
|
||||||
|
mutable y : Zls.carray;
|
||||||
|
mutable time : float;
|
||||||
|
mutable last_time : float;
|
||||||
|
mutable h : float;
|
||||||
|
mutable hmax : float;
|
||||||
|
|
||||||
|
k : Zls.carray array;
|
||||||
|
|
||||||
|
mutable yold : Zls.carray;
|
||||||
|
|
||||||
|
(* -- parameters -- *)
|
||||||
|
mutable stop_time : float;
|
||||||
|
|
||||||
|
(* bounds on small step sizes (mesh-points) *)
|
||||||
|
mutable min_step : float;
|
||||||
|
mutable max_step : float;
|
||||||
|
|
||||||
|
(* initial/fixed step size *)
|
||||||
|
initial_step_size : float option;
|
||||||
|
|
||||||
|
mutable rel_tol : float;
|
||||||
|
mutable abs_tol : float;
|
||||||
|
}
|
||||||
|
|
||||||
|
type nvec = Zls.carray
|
||||||
|
let cmake = Array1.create float64 c_layout
|
||||||
|
let unvec x = x
|
||||||
|
let vec x = x
|
||||||
|
|
||||||
|
let calculate_hmax tfinal min_step max_step =
|
||||||
|
(* [ensure hmax >= min_step] *)
|
||||||
|
let hmax =
|
||||||
|
if tfinal = infinity then max_step
|
||||||
|
else if max_step = infinity then 0.1 *. tfinal
|
||||||
|
else min max_step tfinal in
|
||||||
|
max min_step hmax
|
||||||
|
|
||||||
|
(* NB: y must be the initial state vector (y_0)
|
||||||
|
* k(0) must be the initial deriviatives vector (dy_0) *)
|
||||||
|
let initial_stepsize { initial_step_size; abs_tol; rel_tol; max_step;
|
||||||
|
time; y; hmax; k; _ } =
|
||||||
|
let hmin = 16.0 *. epsilon_float *. abs_float time in
|
||||||
|
match initial_step_size with
|
||||||
|
| Some h -> minmax hmin max_step h
|
||||||
|
| None ->
|
||||||
|
let threshold = abs_tol /. rel_tol in
|
||||||
|
let rh =
|
||||||
|
maxnorm2 (fun y dy -> dy /. (max (abs_float y) threshold)) y k.(0)
|
||||||
|
/. (0.8 *. rel_tol ** pow)
|
||||||
|
in
|
||||||
|
max hmin (if hmax *. rh > 1.0 then 1.0 /. rh else hmax)
|
||||||
|
|
||||||
|
let reinitialize
|
||||||
|
?rhsfn ({ stop_time; min_step; max_step; sysf; _ } as s) t ny =
|
||||||
|
Bigarray.Array1.blit ny s.y;
|
||||||
|
s.time <- t;
|
||||||
|
s.last_time <- t;
|
||||||
|
s.hmax <- calculate_hmax stop_time min_step max_step;
|
||||||
|
sysf t s.y s.k.(maxK); (* update initial derivatives;
|
||||||
|
to be FSAL swapped into k.(0) *)
|
||||||
|
s.h <- initial_stepsize s;
|
||||||
|
Option.iter (fun v -> s.sysf <- v) rhsfn
|
||||||
|
|
||||||
|
let initialize f ydata =
|
||||||
|
let y_len = Bigarray.Array1.dim ydata in
|
||||||
|
let s = {
|
||||||
|
sysf = f;
|
||||||
|
y = Zls.cmake y_len;
|
||||||
|
time = 0.0;
|
||||||
|
last_time = 0.0;
|
||||||
|
h = 0.0;
|
||||||
|
hmax = 0.0;
|
||||||
|
|
||||||
|
k = Array.init (maxK + 1) (fun _ -> Zls.cmake y_len);
|
||||||
|
yold = Zls.cmake y_len;
|
||||||
|
|
||||||
|
(* parameters *)
|
||||||
|
stop_time = infinity;
|
||||||
|
|
||||||
|
min_step = 16.0 *. epsilon_float;
|
||||||
|
max_step = infinity;
|
||||||
|
initial_step_size = None;
|
||||||
|
|
||||||
|
rel_tol = 1.0e-3;
|
||||||
|
abs_tol = 1.0e-6;
|
||||||
|
} in
|
||||||
|
Bigarray.Array1.blit ydata s.k.(0);
|
||||||
|
reinitialize s 0.0 ydata;
|
||||||
|
s
|
||||||
|
|
||||||
|
let set_stop_time t v =
|
||||||
|
if (v <= 0.0) then failwith "The stop time must be strictly positive.";
|
||||||
|
t.stop_time <- v
|
||||||
|
|
||||||
|
let set_min_step t v = t.min_step <- v
|
||||||
|
let set_max_step t v = t.max_step <- v
|
||||||
|
|
||||||
|
let set_tolerances t rel abs =
|
||||||
|
if (rel <= 0.0 || abs <= 0.0)
|
||||||
|
then failwith "Tolerance values must be strictly positive.";
|
||||||
|
(t.rel_tol <- rel; t.abs_tol <- abs)
|
||||||
|
|
||||||
|
let make_newval y k s =
|
||||||
|
let hB = mhB_row s in
|
||||||
|
let newval i =
|
||||||
|
let acc = ref y.{i} in
|
||||||
|
for si = 0 to s - 1 do
|
||||||
|
acc := !acc +. k.(si).{i} *. hB.(si)
|
||||||
|
done;
|
||||||
|
!acc in
|
||||||
|
newval
|
||||||
|
|
||||||
|
let calculate_error threshold k y ynew =
|
||||||
|
let maxerr = ref 0.0 in
|
||||||
|
for i = 0 to Bigarray.Array1.dim y - 1 do
|
||||||
|
let kE = ref 0.0 in
|
||||||
|
for s = 0 to maxK do
|
||||||
|
kE := !kE +. k.(s).{i} *. mE s
|
||||||
|
done;
|
||||||
|
let err = !kE /. (max threshold (max (abs_float y.{i})
|
||||||
|
(abs_float ynew.{i}))) in
|
||||||
|
maxerr := max !maxerr (abs_float err)
|
||||||
|
done;
|
||||||
|
!maxerr
|
||||||
|
|
||||||
|
let log_step t y dy t' y' dy' =
|
||||||
|
Printf.printf
|
||||||
|
"s| % .24e % .24e\n" t t';
|
||||||
|
for i = 0 to Array1.dim y - 1 do
|
||||||
|
Printf.printf "s| f[% 2d]: % .24e (% .24e) --> % .24e (% .24e)\n"
|
||||||
|
i (y.{i}) dy.{i} y'.{i} dy'.{i}
|
||||||
|
done
|
||||||
|
|
||||||
|
(* TODO: add stats: nfevals, nfailed, nsteps *)
|
||||||
|
let step s t_limit user_y =
|
||||||
|
let { stop_time; abs_tol; rel_tol;
|
||||||
|
sysf = f; time = t; h = h; hmax = hmax;
|
||||||
|
k = k; y = y; yold = ynew; _ } = s in
|
||||||
|
|
||||||
|
(* First Same As Last (FSAL) swap; doing it after the previous
|
||||||
|
step invalidates the interpolation routine. *)
|
||||||
|
let tmpK = k.(0) in
|
||||||
|
k.(0) <- k.(maxK);
|
||||||
|
k.(maxK) <- tmpK;
|
||||||
|
|
||||||
|
let hmin = 16.0 *. epsilon_float *. abs_float t in
|
||||||
|
let h = minmax hmin hmax h in
|
||||||
|
let max_t = min t_limit stop_time in
|
||||||
|
let h, finished =
|
||||||
|
if 1.1 *. h >= abs_float (max_t -. t)
|
||||||
|
then (max_t -. t, true)
|
||||||
|
else (h, false) in
|
||||||
|
|
||||||
|
if h < s.min_step then failwith
|
||||||
|
(Printf.sprintf
|
||||||
|
"odexx: step size < min step size (\n now=%.24e\n h=%.24e\n< min_step=%.24e)"
|
||||||
|
t h s.min_step);
|
||||||
|
|
||||||
|
if debug () then Printf.printf "s|\ns|----------step(%.24e)----------\n" max_t;
|
||||||
|
|
||||||
|
let rec onestep (alreadyfailed: bool) h =
|
||||||
|
|
||||||
|
(* approximate next state vector *)
|
||||||
|
update_mhB h;
|
||||||
|
for s = 1 to maxK - 1 do
|
||||||
|
mapinto ynew (make_newval y k s);
|
||||||
|
f (t +. h *. mA s) ynew k.(s)
|
||||||
|
done;
|
||||||
|
|
||||||
|
let tnew = if finished then max_t else t +. h *. (mA maxK) in
|
||||||
|
mapinto ynew (make_newval y k maxK);
|
||||||
|
f tnew ynew k.(maxK);
|
||||||
|
if debug () then log_step t y k.(0) tnew ynew k.(maxK);
|
||||||
|
|
||||||
|
let err = h *. calculate_error (abs_tol /. rel_tol) k y ynew in
|
||||||
|
if err > rel_tol then begin
|
||||||
|
if debug () then Printf.printf "s| error exceeds tolerance\n";
|
||||||
|
|
||||||
|
if h <= hmin then failwith
|
||||||
|
(Printf.sprintf "Error (%e) > relative tolerance (%e) at t=%e"
|
||||||
|
err rel_tol t);
|
||||||
|
|
||||||
|
let nexth =
|
||||||
|
if alreadyfailed then max hmin (0.5 *. h)
|
||||||
|
else max hmin (h *. max Butcher.initial_reduction_limit_factor
|
||||||
|
(0.8 *. (rel_tol /. err) ** pow)) in
|
||||||
|
onestep true nexth
|
||||||
|
end
|
||||||
|
else
|
||||||
|
let h = tnew -. t in
|
||||||
|
let nexth =
|
||||||
|
if alreadyfailed then h
|
||||||
|
else let f = 1.25 *. (err /. rel_tol) ** pow in
|
||||||
|
if f > 0.2 then h /. f else 5.0 *. h in
|
||||||
|
(tnew, nexth)
|
||||||
|
in
|
||||||
|
let nextt, nexth = onestep false h in
|
||||||
|
|
||||||
|
(* advance a step *)
|
||||||
|
s.y <- ynew;
|
||||||
|
s.yold <- y;
|
||||||
|
|
||||||
|
Bigarray.Array1.blit ynew user_y;
|
||||||
|
s.last_time <- t;
|
||||||
|
s.time <- nextt;
|
||||||
|
s.h <- nexth;
|
||||||
|
s.time
|
||||||
|
|
||||||
|
let get_dky { last_time = t; time = t'; yold = y; k; _ } yi ti kd =
|
||||||
|
|
||||||
|
if kd > 0 then
|
||||||
|
failwith
|
||||||
|
(Printf.sprintf
|
||||||
|
"get_dky: requested derivative of order %d \
|
||||||
|
cannot be interpolated at time %.24e" kd ti);
|
||||||
|
if ti < t || ti > t' then
|
||||||
|
failwith
|
||||||
|
(Printf.sprintf
|
||||||
|
"get_dky: requested time %.24e is out of range\n\ [%.24e,...,%.24e]"
|
||||||
|
ti t t');
|
||||||
|
|
||||||
|
let h = t' -. t in
|
||||||
|
let th = (ti -. t) /. h in
|
||||||
|
|
||||||
|
update_mhBI h;
|
||||||
|
for i = 0 to Bigarray.Array1.dim y - 1 do
|
||||||
|
let ya = ref y.{i} in
|
||||||
|
for s = 0 to maxK do
|
||||||
|
let k = k.(s).{i} in
|
||||||
|
let hbi = mhBI_row s in
|
||||||
|
let acc = ref 0.0 in
|
||||||
|
for j = maxBI downto 0 do
|
||||||
|
acc := (!acc +. k *. hbi.(j)) *. th
|
||||||
|
done;
|
||||||
|
ya := !ya +. !acc
|
||||||
|
done;
|
||||||
|
yi.{i} <- !ya
|
||||||
|
done
|
||||||
|
|
||||||
|
(* copy functions *)
|
||||||
|
let copy ({ last_time; time; h; yold; k; _ } as s) =
|
||||||
|
{ s with last_time; time; h; yold = Zls.copy yold; k = Zls.copy_matrix k }
|
||||||
|
|
||||||
|
let blit { last_time = l1; time = t1; yold = yhold1; k = k1; _ }
|
||||||
|
({ yold; k; _ } as s2) =
|
||||||
|
s2.last_time <- l1; s2.time <- t1;
|
||||||
|
Zls.blit yhold1 yold; Zls.blit_matrix k1 k
|
||||||
|
|
||||||
|
end (* }}} *)
|
||||||
|
|
||||||
|
module Ode23 = GenericODE (
|
||||||
|
struct
|
||||||
|
let order = 3
|
||||||
|
let initial_reduction_limit_factor = 0.5
|
||||||
|
|
||||||
|
let a = [| 0.0; 1.0/.2.0; 3.0/.4.0; 1.0 |]
|
||||||
|
|
||||||
|
let b = [| 1.0/.2.0;
|
||||||
|
0.0; 3.0/.4.0;
|
||||||
|
2.0/.9.0; 1.0/.3.0; 4.0/.9.0 |]
|
||||||
|
|
||||||
|
let e = [| -5.0/.72.0; 1.0/.12.0; 1.0/.9.0; -1.0/.8.0 |]
|
||||||
|
|
||||||
|
let bi = [| 1.0; -4.0/.3.0; 5.0/.9.0;
|
||||||
|
0.0; 1.0; -2.0/.3.0;
|
||||||
|
0.0; 4.0/.3.0; -8.0/.9.0;
|
||||||
|
0.0; -1.0; 1.0 |]
|
||||||
|
end)
|
||||||
|
|
||||||
|
module Ode45 = GenericODE (
|
||||||
|
struct
|
||||||
|
let order = 5
|
||||||
|
let initial_reduction_limit_factor = 0.1
|
||||||
|
|
||||||
|
let a = [| 0.0; 1.0/.5.0; 3.0/.10.0; 4.0/.5.0; 8.0/.9.0; 1.0; 1.0 |]
|
||||||
|
|
||||||
|
let b = [|
|
||||||
|
1.0/. 5.0;
|
||||||
|
3.0/.40.0; 9.0/.40.0;
|
||||||
|
44.0/.45.0; -56.0/.15.0; 32.0/.9.0;
|
||||||
|
19372.0/.6561.0; -25360.0/.2187.0; 64448.0/.6561.0; -212.0/.729.0;
|
||||||
|
9017.0/.3168.0; -355.0/.33.0; 46732.0/.5247.0; 49.0/.176.0; -5103.0/.18656.0;
|
||||||
|
35.0/.384.0; 0.0; 500.0/.1113.0; 125.0/.192.0; -2187.0/.6784.0; 11.0/.84.0;
|
||||||
|
|]
|
||||||
|
|
||||||
|
let e = [| 71.0/.57600.0; 0.0; -71.0/.16695.0; 71.0/.1920.0;
|
||||||
|
-17253.0/.339200.0; 22.0/.525.0; -1.0/.40.0 |]
|
||||||
|
|
||||||
|
let bi = [| 1.0; -183.0/.64.0; 37.0/.12.0; -145.0/.128.0;
|
||||||
|
0.0; 0.0; 0.0; 0.0;
|
||||||
|
0.0; 1500.0/.371.0; -1000.0/.159.0; 1000.0/.371.0;
|
||||||
|
0.0; -125.0/.32.0; 125.0/.12.0; -375.0/.64.0;
|
||||||
|
0.0; 9477.0/.3392.0; -729.0/.106.0; 25515.0/.6784.0;
|
||||||
|
0.0; -11.0/.7.0; 11.0/.3.0; -55.0/.28.0;
|
||||||
|
0.0; 3.0/.2.0; -4.0; 5.0/.2.0 |]
|
||||||
|
end)
|
||||||
10
lib/hsim/runtime.ml
Normal file
10
lib/hsim/runtime.ml
Normal file
|
|
@ -0,0 +1,10 @@
|
||||||
|
|
||||||
|
(* let go_full (Hsim main : unit Ztypes.hsimu) : unit = *)
|
||||||
|
(* let n, z = Lift.lift_hsim_full (Hsim main) in *)
|
||||||
|
(* let s = Full.compose_solvers (Csolver.make_full ()) (Zsolver.make_full z) in *)
|
||||||
|
(* ignore @@ Full.hrun n s [{h=10.0; f=fun _ -> ()}] *)
|
||||||
|
|
||||||
|
let go (Hsim main : unit Ztypes.hsimu) : unit =
|
||||||
|
let n, z = Lift.lift_hsim (Hsim main) in
|
||||||
|
let s = Fill.compose_solvers (Csolver.make ()) (Zsolver.make z) in
|
||||||
|
ignore @@ Fill.hrun n s [{h=10.0; f=fun _ -> ()}]
|
||||||
3
lib/hsim/solver.ml
Normal file
3
lib/hsim/solver.ml
Normal file
|
|
@ -0,0 +1,3 @@
|
||||||
|
|
||||||
|
module Sim = Full
|
||||||
|
|
||||||
216
lib/hsim/zls.ml
Normal file
216
lib/hsim/zls.ml
Normal file
|
|
@ -0,0 +1,216 @@
|
||||||
|
|
||||||
|
(* This code was originally written by Timothy Bourke and Marc Pouzet and is *)
|
||||||
|
(* part of the Zelus standard library. *)
|
||||||
|
|
||||||
|
(* open Ztypes *)
|
||||||
|
open Bigarray
|
||||||
|
|
||||||
|
(* Interfaces functions from within Zelus *)
|
||||||
|
|
||||||
|
type carray = (float, float64_elt, c_layout) Array1.t
|
||||||
|
type zarray = (int32, int32_elt, c_layout) Array1.t
|
||||||
|
|
||||||
|
let cmake (n: int) : carray =
|
||||||
|
let r = Array1.create float64 c_layout n in
|
||||||
|
Array1.fill r 0.0;
|
||||||
|
r
|
||||||
|
|
||||||
|
let zmake (n: int) : zarray =
|
||||||
|
let r = Array1.create int32 c_layout n in
|
||||||
|
Array1.fill r 0l;
|
||||||
|
r
|
||||||
|
|
||||||
|
let length = Array1.dim
|
||||||
|
|
||||||
|
let get = Array1.get
|
||||||
|
let set = Array1.set
|
||||||
|
let get_zin v i = Array1.get v i <> 0l
|
||||||
|
(* fill zinvec with zeros *)
|
||||||
|
let zzero zinvec length =
|
||||||
|
for i = 0 to length - 1 do
|
||||||
|
Array1.set zinvec i 0l
|
||||||
|
done
|
||||||
|
let czero c length =
|
||||||
|
for i = 0 to length - 1 do
|
||||||
|
Array1.set c i 0.0
|
||||||
|
done
|
||||||
|
|
||||||
|
(* copy functions *)
|
||||||
|
|
||||||
|
(* copy [c1] into [c2] *)
|
||||||
|
let blit c1 c2 = Array1.blit c1 c2
|
||||||
|
let copy c1 = let c2 = cmake (length c1) in blit c1 c2; c2
|
||||||
|
|
||||||
|
let blit_matrix m1 m2 = Array.iter2 blit m1 m2
|
||||||
|
let copy_matrix m = Array.map copy m
|
||||||
|
|
||||||
|
type 's f_alloc = unit -> 's
|
||||||
|
type 's f_maxsize = 's -> int * int
|
||||||
|
type 's f_csize = 's -> int
|
||||||
|
type 's f_zsize = 's -> int
|
||||||
|
type ('s, 'o) f_step = 's -> carray -> carray -> zarray -> float -> 'o
|
||||||
|
type 's f_ders = 's -> carray -> carray -> zarray -> carray -> float -> unit
|
||||||
|
type 's f_zero = 's -> carray -> zarray -> carray -> float -> unit
|
||||||
|
type 's f_reset = 's -> unit
|
||||||
|
type 's f_horizon = 's -> float
|
||||||
|
|
||||||
|
(* TODO: eliminate this ? *)
|
||||||
|
(* Compare two floats for equality, see:
|
||||||
|
* http://www.cygnus-software.com/papers/comparingfloats/comparingfloats.htm *)
|
||||||
|
let time_eq f1 f2 =
|
||||||
|
if abs_float (f1 -. f2) < min_float
|
||||||
|
then true (* absolute error check for numbers around to zero *)
|
||||||
|
else
|
||||||
|
let rel_error =
|
||||||
|
if abs_float f1 > abs_float f2
|
||||||
|
then abs_float ((f1 -. f2) /. f1)
|
||||||
|
else abs_float ((f1 -. f2) /. f2)
|
||||||
|
in
|
||||||
|
(rel_error <= 0.000001)
|
||||||
|
(* Compare times with 99.9999% accuracy. *)
|
||||||
|
|
||||||
|
let time_leq t1 t2 = t1 < t2 || time_eq t1 t2
|
||||||
|
let time_geq t1 t2 = t1 > t2 || time_eq t1 t2
|
||||||
|
|
||||||
|
(* TODO:
|
||||||
|
- adapt to the new sundials interface, rework, and simplify.
|
||||||
|
- take advantage of the final field.
|
||||||
|
*)
|
||||||
|
|
||||||
|
(* Interface of a stateful ODE solver *)
|
||||||
|
module type STATE_ODE_SOLVER =
|
||||||
|
sig
|
||||||
|
|
||||||
|
(* A session with the solver. *)
|
||||||
|
type t
|
||||||
|
|
||||||
|
(* The type of vectors used internally by the solver. *)
|
||||||
|
type nvec
|
||||||
|
|
||||||
|
(* Create a vector of the given size. *)
|
||||||
|
val cmake : int -> nvec
|
||||||
|
|
||||||
|
(* Unwrap a vector returning an array of continuous-state values. *)
|
||||||
|
val unvec : nvec -> carray
|
||||||
|
|
||||||
|
(* Wrap a vector of continuous-state values into an vector. *)
|
||||||
|
val vec : carray -> nvec
|
||||||
|
|
||||||
|
(* A right-hand-side function called by the solver to calculate the
|
||||||
|
instantaneous derivatives: [f t cvec dvec].
|
||||||
|
- t, the current simulation time (input)
|
||||||
|
- cvec, current values for continuous states (input)
|
||||||
|
- dvec, the vector of instantaneous derivatives (output) *)
|
||||||
|
type rhsfn = float -> carray -> carray -> unit
|
||||||
|
|
||||||
|
(* An interpolation function: [df cvec t k]
|
||||||
|
- cvec, a vector for storing the interpolated continuous states (output)
|
||||||
|
- t, the time to interpolate at,
|
||||||
|
- k, the derivative to interpolate *)
|
||||||
|
type dkyfn = nvec -> float -> int -> unit
|
||||||
|
|
||||||
|
(* [initialize f c] creates a solver session from a function [f] and
|
||||||
|
an initial state vector [c]. *)
|
||||||
|
val initialize : rhsfn -> nvec -> t
|
||||||
|
|
||||||
|
(* [reinitialize s t c] reinitializes the solver with the given time
|
||||||
|
[t] and vector of continuous states [c]. *)
|
||||||
|
(* warning. the size of [c] must be unchanged *)
|
||||||
|
val reinitialize : ?rhsfn:rhsfn -> t -> float -> nvec -> unit
|
||||||
|
|
||||||
|
(* [t' = step s tl c] given a state vector [c], takes a step to the next
|
||||||
|
'mesh-point', or the given time limit [tl] (whichever is sooner),
|
||||||
|
updating [c]. *)
|
||||||
|
val step : t -> float -> nvec -> float
|
||||||
|
|
||||||
|
(* Returns an interpolation function that can produce results for any
|
||||||
|
time [t] since the last mesh-point or the initial instant. *)
|
||||||
|
val get_dky : t -> dkyfn
|
||||||
|
|
||||||
|
|
||||||
|
(* generic solver parameters *)
|
||||||
|
val set_stop_time : t -> float -> unit
|
||||||
|
val set_min_step : t -> float -> unit
|
||||||
|
val set_max_step : t -> float -> unit
|
||||||
|
val set_tolerances : t -> float -> float -> unit
|
||||||
|
|
||||||
|
val copy : t -> t
|
||||||
|
|
||||||
|
val blit : t -> t -> unit
|
||||||
|
end
|
||||||
|
|
||||||
|
(* Interface of a stateful zero-crossing solver *)
|
||||||
|
module type STATE_ZEROC_SOLVER =
|
||||||
|
sig
|
||||||
|
(* A session with the solver. A zero-crossing solver has two internal
|
||||||
|
continuous-state vectors, called 'before' and 'now'. *)
|
||||||
|
type t
|
||||||
|
|
||||||
|
(* Zero-crossing function: [g t cvec zout]
|
||||||
|
- t, simulation time (input)
|
||||||
|
- cvec, vector of continuous states (input)
|
||||||
|
- zout, values of zero-crossing expressions (output) *)
|
||||||
|
type zcfn = float -> carray -> carray -> unit
|
||||||
|
|
||||||
|
(* Create a session with the zero-crossing solver:
|
||||||
|
[initialize nroots g cvec0]
|
||||||
|
- nroots, number of zero-crossing expressions
|
||||||
|
- g, function to calculate zero-crossing expressions
|
||||||
|
- cvec0, initial continuous state
|
||||||
|
|
||||||
|
Sets the 'now' vector to cvec0. *)
|
||||||
|
val initialize : int -> zcfn -> carray -> t
|
||||||
|
|
||||||
|
(* The same but does not run [g] at initialization time *)
|
||||||
|
val initialize_only : int -> zcfn -> t
|
||||||
|
|
||||||
|
(* Reinitialize the zero-crossing solver after a discrete step that
|
||||||
|
updates the continuous states directly: [reinitialize s t cvec].
|
||||||
|
- s, a session with the zero-crossing solver
|
||||||
|
- t, the current simultation time
|
||||||
|
- cvec, the current continuous state vector
|
||||||
|
|
||||||
|
Resets the 'now' vector to cvec. *)
|
||||||
|
val reinitialize : t -> float -> carray -> unit
|
||||||
|
|
||||||
|
(* Advance the zero-crossing solver after a continuous step:
|
||||||
|
[step s t cvec].
|
||||||
|
- s, a session with the zero-crossing solver
|
||||||
|
- t, the current simultation time
|
||||||
|
- cvec, the current continuous state vector
|
||||||
|
|
||||||
|
Moves the current 'now' vector to 'before', then sets 'now' to cvec. *)
|
||||||
|
val step : t -> float -> carray -> unit
|
||||||
|
|
||||||
|
val takeoff : t -> bool
|
||||||
|
(* Returns true if one zero-crossing signal moves from 0 to v > 0 *)
|
||||||
|
(* Compares the 'before' and 'now' vectors and returns true only if
|
||||||
|
there exists an i, such that before[i] < 0 and now[i] >= 0. *)
|
||||||
|
val has_roots : t -> bool
|
||||||
|
|
||||||
|
(* Locates the time of the zero-crossing closest to the 'before' vector.
|
||||||
|
Call after [has_roots] indicates the existence of a zero-crossing:
|
||||||
|
[t = find s (f, c) zin].
|
||||||
|
- The [get_dky] function [f] is provided by the state solver and is
|
||||||
|
expected to update the array [c] with the interpolated state.
|
||||||
|
- zin, is populated with the status of all zero-crossings
|
||||||
|
- the returned values is the simulation time of the earliest
|
||||||
|
zero-crossing that was found. *)
|
||||||
|
val find : t -> ((float -> int -> unit) * carray) -> zarray -> float
|
||||||
|
|
||||||
|
(* locate the fields for which there is a takeoff *)
|
||||||
|
val find_takeoff : t -> zarray -> float
|
||||||
|
end
|
||||||
|
|
||||||
|
(*
|
||||||
|
module type RUNTIME =
|
||||||
|
sig
|
||||||
|
val go : unit hsimu -> unit
|
||||||
|
val check : bool hsimu -> int -> unit
|
||||||
|
end
|
||||||
|
|
||||||
|
module type DISCRETE_RUNTIME =
|
||||||
|
sig
|
||||||
|
val go : float -> (unit -> unit) -> unit
|
||||||
|
end
|
||||||
|
*)
|
||||||
42
lib/hsim/zsolver.ml
Normal file
42
lib/hsim/zsolver.ml
Normal file
|
|
@ -0,0 +1,42 @@
|
||||||
|
open Zls
|
||||||
|
open Illinois
|
||||||
|
|
||||||
|
type state = { state: t; vec: zarray }
|
||||||
|
|
||||||
|
let make_full size : (carray, carray, zarray option) Full.zsolver =
|
||||||
|
let state =
|
||||||
|
{ state = initialize 0 (fun _ _ _ -> ()) (cmake 0);
|
||||||
|
vec = zmake 0 } in
|
||||||
|
let reset { vec; _ } Full.{ fzer; y0; _ } =
|
||||||
|
let fzer t cvec zout =
|
||||||
|
let zout' = fzer t cvec in blit zout' zout in
|
||||||
|
{ state = initialize size fzer y0;
|
||||||
|
vec = if length vec = size then vec else zmake size } in
|
||||||
|
let step ({ state; vec } as s) Full.{ h; f } =
|
||||||
|
let y1 = f h in
|
||||||
|
let fder h _ = let y = f h in blit y y1 in
|
||||||
|
step state h y1;
|
||||||
|
if has_roots state then
|
||||||
|
let h = find state (fder, y1) vec in
|
||||||
|
s, (h, Some vec)
|
||||||
|
else s, (h, None) in
|
||||||
|
Full.DNode { state; step; reset }
|
||||||
|
|
||||||
|
let make size : (carray, carray, zarray option) Fill.zsolver =
|
||||||
|
let state =
|
||||||
|
{ state = initialize 0 (fun _ _ _ -> ()) (cmake 0);
|
||||||
|
vec = zmake 0 } in
|
||||||
|
let reset { vec; _ } Fill.{ fzer; y0; _ } =
|
||||||
|
let fzer t cvec zout =
|
||||||
|
let zout' = fzer t cvec in blit zout' zout in
|
||||||
|
{ state = initialize size fzer y0;
|
||||||
|
vec = if length vec = size then vec else zmake size } in
|
||||||
|
let step ({ state; vec } as s) Fill.{ h; f } =
|
||||||
|
let y1 = f h in
|
||||||
|
let fder h _ = let y = f h in blit y y1 in
|
||||||
|
step state h y1;
|
||||||
|
if has_roots state then
|
||||||
|
let h = find state (fder, y1) vec in
|
||||||
|
s, (h, Some vec)
|
||||||
|
else s, (h, None) in
|
||||||
|
Fill.DNode { state; step; reset }
|
||||||
170
lib/hsim/ztypes.ml
Normal file
170
lib/hsim/ztypes.ml
Normal file
|
|
@ -0,0 +1,170 @@
|
||||||
|
(**************************************************************************)
|
||||||
|
(* *)
|
||||||
|
(* Zelus *)
|
||||||
|
(* A synchronous language for hybrid systems *)
|
||||||
|
(* http://zelus.di.ens.fr *)
|
||||||
|
(* *)
|
||||||
|
(* Marc Pouzet and Timothy Bourke *)
|
||||||
|
(* *)
|
||||||
|
(* Copyright 2012 - 2019. All rights reserved. *)
|
||||||
|
(* *)
|
||||||
|
(* This file is distributed under the terms of the CeCILL-C licence *)
|
||||||
|
(* *)
|
||||||
|
(* Zelus is developed in the INRIA PARKAS team. *)
|
||||||
|
(* *)
|
||||||
|
(**************************************************************************)
|
||||||
|
|
||||||
|
(* Type declarations and values that must be linked with *)
|
||||||
|
(* the generated code *)
|
||||||
|
type 'a continuous = { mutable pos: 'a; mutable der: 'a }
|
||||||
|
|
||||||
|
type ('a, 'b) zerocrossing = { mutable zin: 'a; mutable zout: 'b }
|
||||||
|
|
||||||
|
type 'a signal = 'a * bool
|
||||||
|
type zero = bool
|
||||||
|
|
||||||
|
(* a synchronous stream function with type 'a -D-> 'b *)
|
||||||
|
(* is represented by an OCaml value of type ('a, 'b) node *)
|
||||||
|
type ('s, 'a, 'b) node_rec = {
|
||||||
|
alloc : unit -> 's; (* allocate the state *)
|
||||||
|
step : 's -> 'a -> 'b; (* compute a step *)
|
||||||
|
reset : 's -> unit; (* reset/inialize the state *)
|
||||||
|
}
|
||||||
|
|
||||||
|
type ('a, 'b) node =
|
||||||
|
Node: ('s, 'a, 'b) node_rec -> ('a, 'b) node
|
||||||
|
|
||||||
|
(* the same with a method copy *)
|
||||||
|
type ('a, 'b) cnode =
|
||||||
|
Cnode:
|
||||||
|
{ alloc : unit -> 's; (* allocate the state *)
|
||||||
|
copy : 's -> 's -> unit; (* copy the source into the destination *)
|
||||||
|
step : 's -> 'a -> 'b; (* compute a step *)
|
||||||
|
reset : 's -> unit; (* reset/inialize the state *)
|
||||||
|
} -> ('a, 'b) cnode
|
||||||
|
|
||||||
|
open Bigarray
|
||||||
|
|
||||||
|
type time = float
|
||||||
|
type cvec = (float, float64_elt, c_layout) Array1.t
|
||||||
|
type dvec = (float, float64_elt, c_layout) Array1.t
|
||||||
|
type zinvec = (int32, int32_elt, c_layout) Array1.t
|
||||||
|
type zoutvec = (float, float64_elt, c_layout) Array1.t
|
||||||
|
|
||||||
|
(* The interface with the ODE solver *)
|
||||||
|
type cstate =
|
||||||
|
{ mutable dvec : dvec; (* the vector of derivatives *)
|
||||||
|
mutable cvec : cvec; (* the vector of positions *)
|
||||||
|
mutable zinvec : zinvec; (* the vector of boolean; true when the
|
||||||
|
solver has detected a zero-crossing *)
|
||||||
|
mutable zoutvec : zoutvec; (* the corresponding vector that define
|
||||||
|
zero-crossings *)
|
||||||
|
mutable cindex : int; (* the position in the vector of positions *)
|
||||||
|
mutable zindex : int; (* the position in the vector of zero-crossings *)
|
||||||
|
mutable cend : int; (* the end of the vector of positions *)
|
||||||
|
mutable zend : int; (* the end of the zero-crossing vector *)
|
||||||
|
mutable cmax : int; (* the maximum size of the vector of positions *)
|
||||||
|
mutable zmax : int; (* the maximum number of zero-crossings *)
|
||||||
|
mutable horizon : float; (* the next horizon *)
|
||||||
|
mutable major : bool; (* integration iff [major = false] *)
|
||||||
|
}
|
||||||
|
|
||||||
|
(* The interface with the ODE solver (new Zélus version). *)
|
||||||
|
type cstate_new =
|
||||||
|
{ mutable dvec : dvec; (* Derivative vector. *)
|
||||||
|
mutable cvec : cvec; (* Position vector. *)
|
||||||
|
mutable zinvec : zinvec; (* Zero-crossing result vector. *)
|
||||||
|
mutable zoutvec : zoutvec; (* Zero-crossing value vector. *)
|
||||||
|
mutable cindex : int; (* Position in position vector. *)
|
||||||
|
mutable zindex : int; (* Position in zero-crossing vector. *)
|
||||||
|
mutable cend : int; (* End of position vector. *)
|
||||||
|
mutable zend : int; (* End of zero-crossing vector. *)
|
||||||
|
mutable cmax : int; (* Maximum size of position vector. *)
|
||||||
|
mutable zmax : int; (* Maximum size of zero-crossing vector. *)
|
||||||
|
mutable horizon : float; (* Next horizon. *)
|
||||||
|
mutable major : bool; (* Step mode: major <-> discrete. *)
|
||||||
|
mutable time : float; (* Simulation time. *)
|
||||||
|
}
|
||||||
|
|
||||||
|
(* A hybrid node is a node that is parameterised by a continuous state *)
|
||||||
|
(* all instances points to this global parameter and read/write on it *)
|
||||||
|
type ('a, 'b) hnode = cstate -> (time * 'a, 'b) node
|
||||||
|
|
||||||
|
type 'b hsimu =
|
||||||
|
Hsim:
|
||||||
|
{ alloc : unit -> 's;
|
||||||
|
(* allocate the initial state *)
|
||||||
|
maxsize : 's -> int * int;
|
||||||
|
(* returns the max length of the *)
|
||||||
|
(* cvector and zvector *)
|
||||||
|
csize : 's -> int;
|
||||||
|
(* returns the current length of the continuous state vector *)
|
||||||
|
zsize : 's -> int;
|
||||||
|
(* returns the current length of the zero-crossing vector *)
|
||||||
|
step : 's -> cvec -> dvec -> zinvec -> time -> 'b;
|
||||||
|
(* computes a step *)
|
||||||
|
derivative : 's -> cvec -> dvec -> zinvec -> zoutvec -> time -> unit;
|
||||||
|
(* computes the derivative *)
|
||||||
|
crossings : 's -> cvec -> zinvec -> zoutvec -> time -> unit;
|
||||||
|
(* computes the zero-crossings *)
|
||||||
|
reset : 's -> unit;
|
||||||
|
(* resets the state *)
|
||||||
|
horizon : 's -> time;
|
||||||
|
(* gives the next time horizon *)
|
||||||
|
} -> 'b hsimu
|
||||||
|
|
||||||
|
(* a function with type 'a -C-> 'b, when given to a solver, is *)
|
||||||
|
(* represented by an OCaml value of type ('a, 'b) hsnode *)
|
||||||
|
type ('a, 'b) hsnode =
|
||||||
|
Hnode:
|
||||||
|
{ state : 's;
|
||||||
|
(* the discrete state *)
|
||||||
|
zsize : int;
|
||||||
|
(* the maximum size of the zero-crossing vector *)
|
||||||
|
csize : int;
|
||||||
|
(* the maximum size of the continuous state vector (positions) *)
|
||||||
|
derivative : 's -> 'a -> time -> cvec -> dvec -> unit;
|
||||||
|
(* computes the derivative *)
|
||||||
|
crossing : 's -> 'a -> time -> cvec -> zoutvec -> unit;
|
||||||
|
(* computes the derivative *)
|
||||||
|
output : 's -> 'a -> cvec -> 'b;
|
||||||
|
(* computes the zero-crossings *)
|
||||||
|
setroots : 's -> 'a -> cvec -> zinvec -> unit;
|
||||||
|
(* returns the zero-crossings *)
|
||||||
|
majorstep : 's -> time -> cvec -> 'a -> 'b;
|
||||||
|
(* computes a step *)
|
||||||
|
reset : 's -> unit;
|
||||||
|
(* resets the state *)
|
||||||
|
horizon : 's -> time;
|
||||||
|
(* gives the next time horizon *)
|
||||||
|
} -> ('a, 'b) hsnode
|
||||||
|
|
||||||
|
(* An idea suggested by Adrien Guatto, 26/04/2021 *)
|
||||||
|
(* provide a means to the type for input/outputs of nodes *)
|
||||||
|
(* express them with GADT to ensure type safety *)
|
||||||
|
(* type ('a, 'b) node =
|
||||||
|
| Fun : { step : 'a -> 'b;
|
||||||
|
typ_arg: 'a typ;
|
||||||
|
typ_return: 'b typ
|
||||||
|
} -> ('a, 'b) node
|
||||||
|
| Node :
|
||||||
|
{ state : 's; step : 's -> 'a -> 'b * 's;
|
||||||
|
typ_arg: 'a typ;
|
||||||
|
typ_state : 's typ;
|
||||||
|
typ_return: 'b typ } -> ('a, 'b) node
|
||||||
|
|
||||||
|
and 'a typ =
|
||||||
|
| Tunit : unit typ
|
||||||
|
| Tarrow : 'a typ * 'b typ -> ('a * 'b) typ
|
||||||
|
| Tint : int -> int typ
|
||||||
|
| Ttuple : 'a typlist -> 'a typ
|
||||||
|
| Tnode : 'a typ * 'b typ -> ('a,'b) node typ
|
||||||
|
|
||||||
|
and 'a typlist =
|
||||||
|
| Tnil : unit typlist
|
||||||
|
| Tpair : 'a typ * 'b typlist -> ('a * 'b) typlist
|
||||||
|
|
||||||
|
Q1: do it for records? sum types ? How?
|
||||||
|
Q2: provide a "type_of" function for every introduced type?
|
||||||
|
*)
|
||||||
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue