Move into formation with priority

gistfile1.swift

func moveIntoFormation() {

    // New unit array. Starts empty, and once we assign a unit to a position,
    // we'll add to this array. 
    var positionedUnits : Array<CharacterNode> = []
    
    // Instead of looping over units, loop over unit count
    for var index = 0; index < self.units.count; ++index {
    
        // Get the vector from the formation class for that position. 
        // Just a simple accessor to the array.
        let vector = self.formation!.getVectorAtPosition(index)
        
        // Hard coded rotation by radian of 0.4.
        var newX  = vector.dx * cos(0.4) - vector.dy * sin(0.4);
        var newY = vector.dx * sin(0.4) + vector.dy * cos(0.4);
        let position = CGPoint(x:newX, y:newY)
        
        // The new magic is here. Make a function 
        let unit = self.findClosestUnitToPosition(position, positionedUnits: positionedUnits)
        positionedUnits.append(unit)
 
        // Walk the closest unit to the new position.
        unit.moveToPoint(position, callback: {
            // animation cleanup             
        })
    }
    
    // Finally, replace our old units array with the new one. This will keep the positions in
    // order for other actions that rely on positioning.
    self.units = positionedUnits
}