F#. Flocks with k-d tree. Swarm Simulation Part 2.

Ich habe weiter mit der Schwarm Simulation gespielt.
Eine geeignete Datenstruktur zur Schwarm Simulation ist K-d tree, wie ich aus dem Beitrag "Functional flocks" erfahren habe.
Für unserer Zweck genügt es eigentlich zwei Dimensionen im k-d tree abzubilden.

// from Haskell Version https://github.com/mjsottile/publicstuff/tree/master/boids
namespace Flocks
open System

open Microsoft.FSharp.Math
open Microsoft.FSharp.Collections

module KDTree =

    type KDTreeNode<'a> =
        | Empty
        | Node of KDTreeNode<'a> * (float * float) * 'a * KDTreeNode<'a>
        //        left tree         position         data   Right tree
    
    let inline flatten tree =
        let s = System.Collections.Generic.Stack[tree]
        
        let rec loop (stack : Collections.Generic.Stack<KDTreeNode<'a>>) acc =
            match stack.Count>0 with
            | false -> acc
            | true ->
                match stack.Pop() with
                | Empty -> loop stack acc
                | Node(left, _, a, right) ->
                    stack.Push left
                    stack.Push right
                    loop stack (a :: acc)
        loop s


    let newKDTree = Empty

    let inline vecLessThan (a, b) (x, y)  = a < x && b < y
    let inline vecGreaterThan (a, b) (x, y)  = a > x && b > y

    let inline vecDimSelect (x, y) n =
        match n with
        | 0 -> x
        | 1 -> y
        | other -> failwith "invalid argument"  

    let inline kdtInBounds p bMin bMax =  (vecLessThan p bMax) && (vecGreaterThan p bMin)

    let inline kdtRangeSearch t bMin bMax =
        let rec inner t (current, next) acc =
            let nextfuncs = (next, current)
            match t with 
            | Empty -> acc
            | Node (left, npos, ndata, right) -> 
                match current npos < current bMin, 
                        current npos > current bMax with
                | true, _       -> inner right nextfuncs acc
                | _, true       -> inner left nextfuncs acc
                | false, false  ->
                    match kdtInBounds npos bMin bMax with
                    | true      -> inner right nextfuncs (inner left nextfuncs ((npos, ndata) :: acc))
                    | false     -> inner right nextfuncs (inner left nextfuncs acc)
        inner t (fst, snd) []

module Boids =
    open KDTree

    type Boid = { identifier : int; position : float * float; velocity : float * float; bounded : bool}

    // create KD Tree from boids list.
    let inline fromListWithDepth l =
        let rec loop boidPoints d =
            match Array.isEmpty boidPoints with
            | true -> Empty
            | false  ->
                let axis = d % 2 
                Array.sortInPlaceBy (fun boid -> vecDimSelect boid.position axis) boidPoints
                let index = Array.length boidPoints / 2
                if index = 0 then
                    let dataBoid =  boidPoints.[0]
                    Node (Empty, dataBoid.position, dataBoid, Empty)
                else
                    let lf, r =     boidPoints.[0..index - 1], boidPoints.[index..]
                    let dataBoid =  r.[0]
                    Node(loop lf  (d + 1) , dataBoid.position, dataBoid, loop r.[1..] (d + 1))
        loop (l |> Array.ofSeq) 0 

Meine Tests haben gezeigt, dass die explizite Verwendung vom Stack die schnellste Methode für das "flatten" einer Baumstruktur in einer Liste ist. Der umgekehrte Weg von einer Liste zu einem Baum ist die Funktion fromListWithDepth und sie ist die abgewandelte Version vom folgenden F# Snippet.
Der Rest ist ziehmlich identisch mit dem alten F# Boids Code. Der Schwarm ist in zwei Teilen geteilt - rote und blaue Teilchen - die einen bewegen sich in einer toroidalen Topologie (rote Teilchen), was komplett vom Matt Sottile Code übernommen ist, und die anderen (blaue) sind an die Grenzen des Formulars gebunden.

type Params = {velocity : float; cohesionParam : float;  separationParam : float;
                    sScale : float; alignmentParam : float;
                    vLimit : float; epsilon : float;
                    maxx : float;   maxy : float;
                    minx : float;   miny : float}
    let parms = 
        let maxx = 390.0
        let maxy = 390.0
        let minx = -8.0
        let miny = -8.0
        {velocity = 1.02; cohesionParam = 0.06; separationParam = 12.0; sScale = 0.2; 
         alignmentParam = 0.16; vLimit = 0.0025 * (max (maxx-minx) (maxy-miny));
         epsilon = 25.0; maxx = maxx; maxy = maxy;
         minx = minx; miny = miny}
    
    let vecZero = 0.0, 0.0

    let inline (<+>) (a, b) (a', b') = a + a', b + b'

    let inline (<->) (a, b) (a', b') = a - a', b - b'

    let inline (</>) (a, b) c = a/c, b/c

    let inline vecScale (s : float) (a, b) = s * a, s * b

    let inline sq x = x * x

    let inline vecNorm (x, y) = sqrt (sq x + sq y)
    //  sometimes we want to control runaway of vector scales, so this can
    // be used to enforce an upper bound
    let inline limiter boidVel speedLimit =
        match boidVel with
        |velX, velY when sq velX + sq velY > sq speedLimit ->
            let slowdown = (sq speedLimit) / (sq velX + sq velY)
            slowdown * velX, slowdown * velY
        |_ -> boidVel

    let inline findCentroid boids =
        match boids with
        | []      -> failwith "Bad centroid"
        | _   -> 
            let average f l = List.averageBy (fun boid -> boid.position |> f) l
            average fst boids, 
                average snd boids
            

// cohesion : go towards centroid.  parameter dictates fraction of
// distance from boid to centroid that contributes to velocity
    let inline cohesion b boids a  = 
        (findCentroid boids) <-> b.position
        |> vecScale a 
    
    //An acceleration to stop us hitting nearby boids
    let inline separation b boids a sScale =
        match boids with
        | [] -> vecZero
        | _ -> 
            boids
            |> List.map (fun boid -> boid.position <-> b.position)
            |> List.filter (fun i -> (vecNorm i) < a)       
            |> List.fold (<->) (0.0,0.0)   
            |> vecScale sScale
    
    //Boids try to match velocity with near boids.
    let inline alignment b boids a  =
        match boids with
        | [] -> vecZero
        | _ ->
            let avrg = List.averageBy (fun boid-> fst boid.velocity ) boids, List.averageBy (fun boid-> snd boid.velocity) boids
            vecScale a (avrg <-> b.velocity) 

    let inline wraparound parms  (x, y)  = 
        let w,h = parms.maxx - parms.minx, parms.maxy - parms.miny        
        let x' = if (x > parms.maxx) then x - w else (if x < parms.minx then x+w else x)     
        let y' = if (y > parms.maxy) then y - h else (if y < parms.miny then y+h else y) 
        (x', y')

    let inline boundPosition (boundMin,boundMax) boid =
        let bound coor =
            match coor > boundMax, coor<boundMin with
            |true, _ -> -1.0
            |_, true -> 1.0
            |_       -> 0.0
        bound <| vecDimSelect boid.position 0, bound <| vecDimSelect boid.position 1

    let inline oneboid parms b boids =  
        let c = cohesion b boids parms.cohesionParam       
        let s = separation b boids parms.separationParam parms.sScale 
        let a = alignment b boids parms.alignmentParam    

        //apply rules for current boid.
        let v' =  b.velocity <+> (vecScale 0.3 (c <+> s <+> a))
        match b.bounded with
        | true ->
            let vbound = v' <+> (boundPosition (parms.minx + 8.0, parms.maxx) b)
            let v'' = limiter (vecScale parms.velocity vbound) parms.vLimit      
            { b with identifier = b.identifier;  position = b.position <+> v'' ; velocity = v''}
        | false ->            
            let v'' = limiter (vecScale parms.velocity v') parms.vLimit      
            let p' =  b.position <+> v''
            { b with identifier = b.identifier;  position = wraparound parms p' ; velocity = v''}

    
    let inline splitBoxHoriz parms (lo, hi, ax, ay) =  
        let (lx, ly), (hx, hy) = lo, hi
        let w = parms.maxx - parms.minx
        if (hx-lx > w)   then 
            [( (parms.minx, ly), (parms.maxx, hy), ax, ay)]  
        else
            if (lx < parms.minx) then 
                [( (parms.minx, ly),  (hx, hy), ax, ay);
                 ( (parms.maxx - (parms.minx - lx), ly), (parms.maxx, hy), (ax - w), ay)]       
            else
                if (hx > parms.maxx)  then 
                    [((lx, ly),  (parms.maxx, hy), ax, ay);
                     ( (parms.minx, ly), (parms.minx + (hx - parms.maxx), hy), ax+w, ay)]            
                else [(lo,hi,ax,ay)]  
    
    let inline splitBoxVert parms (lo, hi, ax, ay) =
        let (lx, ly), (hx, hy) = lo, hi
        let h = parms.maxy - parms.miny
        if (hy-ly > h) then
            [( (lx, parms.miny), (hx, parms.maxy), ax, ay)]
        else 
            if (ly < parms.miny) then
               [((lx, parms.miny),  (hx, hy), ax, ay);
                ((lx, parms.maxy - (parms.miny - ly)), (hx, parms.maxy), ax, ay-h)]
            else 
                if (hy > parms.maxy) then
                    [((lx, ly), (hx, parms.maxy), ax, ay);
                     ((lx, parms.miny), (hx, parms.miny + (hy - parms.maxy)), ax, ay+h)]
                else [(lo,hi,ax,ay)]

    let inline findNeighbors parms tree b =           
        let epsvec = (parms.epsilon, parms.epsilon)
        let vlo, vhi = b.position <-> epsvec,  b.position <+> epsvec            
        
        // adjuster for wraparound      
        let adj1 ax ay (pos, theboid) = 
            (pos <+> (ax,ay), {theboid with position = theboid.position <+> (ax,ay) }) 
        
        let adjuster lo hi ax ay = 
            let neighbors = kdtRangeSearch tree lo hi                             
            List.map (adj1 ax ay) neighbors        
        
        let neighbors =
            match b.bounded with
            | false ->
                //split the boxes      
                let splith = splitBoxHoriz parms (vlo, vhi, 0.0, 0.0)      
                let splitv = List.collect (splitBoxVert parms) splith                      
        
                // do the sequence of range searches 
                List.collect (fun (lo,hi,ax,ay) -> adjuster lo hi ax ay) splitv            
            | true ->
                kdtRangeSearch tree vlo vhi
        // compute the distances from boid b to members 
        let dists = List.map (fun (_, boid) -> (vecNorm (b.position <-> boid.position), boid)) neighbors  
        
        b :: (List.map snd (List.filter (fun (d, _) -> d <= parms.epsilon) dists))

    //Updating the whole set of boids.
    let inline iterationkd fdraw parms tree =  
        let ftemp f g l = f l, g l
        flatten tree []  
        |> PSeq.map (fun boid -> oneboid parms boid (findNeighbors parms tree boid)) 
        |> ftemp fromListWithDepth (fdraw (int parms.epsilon))              

    let rndm = new Random()

    let inline makeboid i = 
        let x=rndm.NextDouble()
        let y=rndm.NextDouble()
        let m= (float i)%400.0
        match i % 3 with
        | 0 -> 
            {identifier = i; velocity = x - 1.5, y - 1.5; 
             position =  (m * (x - 0.5) , m * (y - 0.5) ); bounded = m > 150.0}
        | 1 -> 
            {identifier = i; velocity = - x , - y; 
             position = m * (x - 0.5) , m * (y - 0.5) + m; bounded = m > 150.0}
        | _ ->
            {identifier = i; velocity = 1.5 - x, y - 1.5; 
             position = m * (x - 0.5) + m, m * (y - 0.5) ; bounded = m > 150.0}

Wie man sieht, die Schwarm-Daten werden im K-d tree gespeichert und mit jeder GUI-Aktualisierung neu berechnet. Dies geschiet in der iterationkd-Funktion. Zuerst erstellen wir mit der flatten - Funktion aus dem Baum eine Liste von einzelnen Schwarmelementen. Dann errechnen wir parallel für jedes Element die neue Position (oneboid). Schließlich wird der neue Baum aus der Liste kreiert (fromListWithDepth).

Zusätzlich sind die Teilchen mit dem Kreis ihren "epsilon" Regionen dargestellt (man kann es als ihre Sichtweite bezeichnen).

//BoidDrawing.fs
namespace BoidForm

open System.Drawing
open System.Drawing
open System.Drawing.Drawing2D

open Flocks.Boids

module BoidDrawing =

    type Drawing =
        abstract Draw : Graphics -> unit

    let drawing f =
      { new Drawing with 
          member x.Draw(gr) = f(gr) }
      
    let emptyDrawing =
      { new Drawing with 
          member x.Draw(gr) = () }

    let pen = new Pen(Color.Black)

    let inline drawBoid (brush1, brush2) epsilon boids =
        drawing(fun g ->   
          boids
          |>Seq.iter (fun boid ->
              let (x,y) = boid.position
              g.TranslateTransform(float32 x, float32 y)
              if boid.bounded then
                g.FillEllipse(brush1, 0, 0, 6, 6)
              else
                g.FillEllipse(brush2, 0, 0, 6, 6)
              g.DrawEllipse(pen, (-epsilon / 2) + 3 , (-epsilon / 2) + 3, epsilon, epsilon)
              g.TranslateTransform(-(float32 x), -(float32 y))))

//BoidForm.fs
namespace BoidForm

open System
open System.Drawing
open System.Windows.Forms
open Flocks.KDTree
open BoidForm.BoidDrawing
open System.Drawing.Drawing2D

type public BoidForm() as form =
    inherit Form()
  
    do
        form.SuspendLayout();
         
        form.SetStyle(ControlStyles.AllPaintingInWmPaint ||| ControlStyles.OptimizedDoubleBuffer, true)
        form.FormBorderStyle <- FormBorderStyle.FixedToolWindow
        form.StartPosition <- FormStartPosition.CenterScreen;
    
        let tmr = new Timers.Timer(Interval = 40.0)
        tmr.Elapsed.Add(fun _ -> form.Invalidate() )
        tmr.Start()

        form.Text <- "F# Flock"

        // render the form
        form.ResumeLayout(false)
        form.PerformLayout()

    member x.guiRefresh (e:Graphics)  (swarm : Drawing) = 
        e.FillRectangle(Brushes.White, Rectangle(Point(0,0), Size(x.ClientSize.Width, x.ClientSize.Height-40)))
        e.SmoothingMode <- SmoothingMode.AntiAlias
        swarm.Draw(e)

Um das Ganze nicht so langweilig erscheinen zu lassen, sind diverse Parameter auf dem Formular platziert worden, damit die Änderungen sofort zu sehen sind.

//Programm.fs
namespace BoidForm

open System
open System.Drawing
open System.Windows.Forms
open Microsoft.FSharp.Collections

open Flocks.KDTree
open Flocks.Boids
open BoidDrawing

module Main =
    let synchronize f = 
        let ctx = System.Threading.SynchronizationContext.Current 
        f (fun g arg ->
            let nctx = System.Threading.SynchronizationContext.Current 
            if ctx <> null && ctx <> nctx then ctx.Post((fun _ -> g(arg)), null)
            else g(arg) )

    type Microsoft.FSharp.Control.Async with 
      static member AwaitObservable(ev1:IObservable<'a>) =
        synchronize (fun f ->
          Async.FromContinuations((fun (cont,econt,ccont) -> 
            let rec callback = (fun value ->
              remover.Dispose()
              f cont value )
            and remover : IDisposable  = ev1.Subscribe(callback) 
            () )))
  
      static member AwaitObservable(ev1:IObservable<'a>, ev2:IObservable<'b>) = 
        synchronize (fun f ->
          Async.FromContinuations((fun (cont,econt,ccont) -> 
            let rec callback1 = (fun value ->
              remover1.Dispose()
              remover2.Dispose()
              f cont (Choice1Of2(value)) )
            and callback2 = (fun value ->
              remover1.Dispose()
              remover2.Dispose()
              f cont (Choice2Of2(value)) )
            and remover1 : IDisposable  = ev1.Subscribe(callback1) 
            and remover2 : IDisposable  = ev2.Subscribe(callback2) 
            () )))

    type InputParams = | Cohesion of float | Alignment of float | Scale of float | Separation of float | Velocity of float | Epsilon of float
    let inline createParams input p =
        match input with
        | Cohesion v when v <> 0.0 ->     {p with cohesionParam = v} 
        | Alignment v when v <> 0.0->    {p with alignmentParam = v} 
        | Scale v when v <> 0.0->        {p with sScale = v} 
        | Separation v->    {p with separationParam = v} 
        | Velocity v->          {p with velocity = v}
        | Epsilon v -> {p with epsilon = v}
        | _ -> p

    let iter  = iterationkd (drawBoid (Brushes.Blue, Brushes.Red)) 

    let test =
        let boundMin,boundMax = 0.0, 400.0

        let af = new BoidForm(ClientSize = Size((int boundMax), (int boundMax)+50), Visible = true)
        let cohesionLabel = new Label(Text ="cohesion",Left = 8, Top = 410, Width = 60, Height = 15 )
        let cohesionTextBox = new TextBox(Text = parms.cohesionParam.ToString(), Left = 8, Top = 430, Width = 60)
        let alignmentLabel = new Label(Text ="alignment",Left = 70, Top = 410, Width = 60, Height = 15 )
        let alignmentTextBox = new TextBox(Text = parms.alignmentParam.ToString(), Left = 70, Top = 430, Width = 60)

        let scaleLabel = new Label(Text ="separation scale",Left = 135, Top = 410, Width = 90, Height = 15 )
        let scaleTextBox = new TextBox(Text = parms.sScale.ToString(), Left = 135, Top = 430, Width = 60)
        
        let separationLabel = new Label(Text ="separation",Left = 230, Top = 410, Width = 60, Height = 15 )
        let separationTextBox = new TextBox(Text = parms.separationParam.ToString(), Left = 230, Top = 430, Width = 60)
        
        let veloLabel = new Label(Text ="velocity",Left = 295, Top = 410, Width = 60, Height = 15 )
        let veloTextBox = new TextBox(Text = parms.velocity.ToString(), Left = 295, Top = 430, Width = 60)

        let epsilonLabel = new Label(Text ="epsilon",Left = 360, Top = 410, Width = 60, Height = 15 )
        let epsilonTextBox = new TextBox(Text = parms.epsilon.ToString(), Left = 360, Top = 430, Width = 60)

        let evtParamsChanged = 
            let parse f text = 
                let (ok, v) = System.Double.TryParse(text)
                if ok then Some(createParams (f v)) else None
            Event.merge (Event.map (fun _ -> parse Cohesion cohesionTextBox.Text) cohesionTextBox.TextChanged) (Event.map (fun _-> parse Alignment alignmentTextBox.Text) alignmentTextBox.TextChanged)
            |> Event.merge (Event.map (fun _ -> parse Scale scaleTextBox.Text) scaleTextBox.TextChanged)
            |> Event.merge (Event.map (fun _ -> parse Separation separationTextBox.Text) separationTextBox.TextChanged)
            |> Event.merge (Event.map (fun _ -> parse Velocity veloTextBox.Text) veloTextBox.TextChanged)
            |> Event.merge (Event.map (fun _ -> parse Epsilon epsilonTextBox.Text) epsilonTextBox.TextChanged)
            |> Event.choose id
        af.Controls.AddRange([| (cohesionTextBox:>Control); (alignmentTextBox:>Control);(scaleTextBox:>Control); (separationTextBox:>Control); (veloTextBox:>Control); (epsilonTextBox:>Control);
                                (cohesionLabel:>Control); (alignmentLabel:>Control);(scaleLabel:>Control); (separationLabel:>Control); (veloLabel:>Control); (epsilonLabel:>Control)|])
        let swarmInit = 
            List.map (fun i ->makeboid i ) [0..250] |> fromListWithDepth

        //Start swarm after 10 steps.
        let swarmStart  = 
            List.fold (fun (facc,_) _-> 
                    (iter parms facc)) (iter parms swarmInit) [0..10]

        let rec waiting swarm p = async {
            let! evnt = Async.AwaitObservable (af.Paint, evtParamsChanged)
            match evnt with
            | Choice1Of2(evntArg1) -> 
                let newSwarm, drawing = swarm 
                af.guiRefresh evntArg1.Graphics drawing
                do! waiting (iter p newSwarm) p
            | Choice2Of2(f) ->
                do! waiting swarm (f p)
                }
        (waiting swarmStart parms) |> Async.StartImmediate
        af

    [<STAThread>]
    do
        Application.EnableVisualStyles()
        Application.Run(test)

git repo