Why do my boids rush to world origin when matching speed?

Question

I have a problem implementing Conrad Parker's boids pseudocode.

I'm implementing rule1, rule2 and rule3. The problem is that whenever rule3 is active (ie, matchSpeed in my code below), the boids rush to the centre of the world (0, 0, 0) and then flock around that spot. This happens regardless of where they start in the world.

But when rule3 doesn't run, the boids flock and drift as expected. What am I doing wrong?

My code is in Scala and I'm using the jMonkeyEngine, but I suspect the problem is a general one.

  val sepDistance = 10f
  val clumpFactor = 100f
  val avoidFactor = 3f
  val alignFactor = 800f

  val speedLimit = 2f

  def moveAgents(target: Node)
  {
    agents.foreach(a => {
      a.velocity.addLocal(clump(a))        //rule1
      a.velocity.addLocal(keepAway(a))     //rule2
      a.velocity.addLocal(matchSpeed(a))   //rule3
      a.velocity = limitSpeed(a.velocity)
      a.move(a.velocity)
      })
  }

  def clump (a: Agent): Vector3f = // rule1
  {
    val centre = Vector3f.ZERO.clone
    for (oA <- agents if oA != a) yield 
      centre.addLocal(oA.position)

    centre.divideLocal(agents.length.toFloat - 1f)
    centre.subtractLocal(a.position)
    centre.divideLocal(clumpFactor)
    return centre
  }

  def keepAway (a: Agent): Vector3f = // rule2
  {
    val keepAway = Vector3f.ZERO.clone
    for (oA <- agents if oA != a) {
      if (Math.abs(oA.position.distance(a.position)) < sepDistance) 
        keepAway.subtractLocal(oA.position.subtract(a.position))
    }

    return keepAway.divide(avoidFactor)
  }

  def matchSpeed (a: Agent): Vector3f = // rule3
  {
    val matchSpeed = Vector3f.ZERO.clone
    for (oA <- agents if oA != a)
      matchSpeed.addLocal(oA.velocity)

    matchSpeed.divideLocal(agents.length.toFloat - 1f)
    matchSpeed.subtractLocal(a.position)
    matchSpeed.divideLocal(alignFactor)

    return matchSpeed
  }

Answer 1

The problem is that the matchSpeed method subtracts the focal boid's position from the average velocity, rather than its velocity.

So:

matchSpeed.subtractLocal(a.position)

Should be:

matchSpeed.subtractLocal(a.velocity)