Two-dimensional Proportional Navigation in Netlogo

Question

I am trying to implement a two-dimensional proportional navigation in Netlogo as it is defined by these formulas:

where v_r is the difference in velocities, r is the line of sight and phi is the angle. Theta is supposed to be the change in line of sight and N is the navigation constant. I am using the formula with the sin since I am two-dimensional.

And I am a bit lost because the two moving objects don't collide at all in my current implementation.

So, currently I am calculating v_r and r like this, and I'm fairly sure this is correct since testing it with predefined positions/orientations yields the desired results:

;; line of sight
let rx [xcor] of target - xcor
let ry [ycor] of target - ycor
; difference in velocity components
let vx ([dx] of target * speed) - dx * predator-speed
let vy ([dy] of target * speed) - dy * predator-speed

The angle is calculated like this and should also be correct:

let dot  (vx * rx + vy * ry)
let det  (vx * ry - vy * rx)
set angle atan det dot

And putting it all together, theta is this:

let theta (sqrt (vx ^ 2 + vy ^ 2) * sin angle) / sqrt(rx ^ 2 + ry ^ 2)

And then I calculate the difference to the previously calculated theta since the first formula uses its differential, multiply it with the constant and convert it to degrees:

let delta-theta theta - theta-old
set theta-old theta
let turn-rate (delta-theta * N * 180 / pi)
turn-at-most turn-rate max-hunt-turn

When I run it, the following happens (both turtles have the same speed, one moves right, one up) (it doesn't change much for most values of N between 3 and 5)

I assume I have some mistake in understanding the last step, since I do think that the components should be fine. To put it into a question: What do I do with theta (or delta theta) in order to get a change in heading?

Edit: here is where the actual turn happens:

to turn-at-most [turn max-turn]  ;; turtle procedure
  ifelse abs turn > max-turn
    [ ifelse turn > 0
        [ rt max-turn ]
        [ lt max-turn ] ]
    [ rt turn ]
end

Edit 2: current status is that I think the formula given for theta is actually the derivative of theta already, because using theta instead of delta-theta gives me the desired behavior (or at least it looks like it). I still need confirmation for this, but I'll keep the thread updated.

Answer 1

It has been confirmed that theta is already the difference, so the calculation of delta-theta is unnecessary. Furthermore, the calculation of coordinates did not take into account the torus shape of the world, which caused additional confusion.

; calculation of LOS using shortest distance in a torus world
let dist-target [distance myself] of target
let angle-target towards target
let rx sin angle-target * dist-target
let ry cos angle-target * dist-target
; difference in velocity components
let vx ([dx] of target * speed) - dx * predator-speed
let vy ([dy] of target * speed) - dy * predator-speed

; angle
let dot  (vx * rx + vy * ry)
let det  (vx * ry - vy * rx)
let angle 0
set angle atan det dot
; finally, theta
let theta (sqrt (vx ^ 2 + vy ^ 2) * sin angle) / sqrt(rx ^ 2 + ry ^ 2) / pi * 180
turn-at-most theta * interceptN max-hunt-turn

Answer 2

This is not an answer but it's way too long for comments. Below is a complete model using your code. It seems to me that the calculation of angle is fine, but are you sure about the formula for theta? I am getting very small numbers for theta (printed with a radians to degree correction) regardless of the starting angle for the predator.

globals [N]

breed [predators predator]
breed [preys prey]
turtles-own [speed]
predators-own [angle theta-old]

to setup
  clear-all
  create-preys 1
  [ setxy 0 10
    set heading 90
    set speed 1
    set color blue
  ]
  create-predators 1
  [ setxy 0 0
    set heading 0         ; experiment with this
    set speed 1
    set color red
  ]
  set N 4
  reset-ticks
end

to go
  ask one-of predators
  [ let target one-of preys
    ;; line of sight
    let rx [xcor] of target - xcor
    let ry [ycor] of target - ycor
    ; difference in velocity components
    let vx ([dx] of target * [speed] of target) - dx * speed
    let vy ([dy] of target * [speed] of target) - dy * speed
    ; calculate change in direction
    let dot  (vx * rx + vy * ry)
    let det  (vx * ry - vy * rx)
    set angle atan det dot
    type "Angle is: " print angle
    let theta (sqrt (vx ^ 2 + vy ^ 2) * sin angle) / sqrt (rx ^ 2 + ry ^ 2)
    type "Theta is: " print round (180 * theta / pi)
    let delta-theta theta - theta-old
    type "change in theta is: " print round (180 * delta-theta / pi)
    set theta-old theta
    let turn-rate (delta-theta * N * 180 / pi)
    turn-at-most turn-rate 360
  ]
  ask turtles [forward speed]
end

to turn-at-most [turn max-turn]  ;; turtle procedure
  ifelse abs turn > max-turn
    [ ifelse turn > 0
        [ rt max-turn ]
        [ lt max-turn ] ]
    [ rt turn ]
end

Instead of using the formulas, NetLogo has some good primitives for dealing with direction. This seems like a job for subtract-headings.