|
| 1 | +package boid |
| 2 | + |
| 3 | +import vecxt.all.* |
| 4 | +import vecxt.BoundsCheck.DoBoundsCheck.yes |
| 5 | + |
| 6 | +object BoidForces: |
| 7 | + import BoidConfig.* |
| 8 | + |
| 9 | + def limitForce(fx: Double, fy: Double, maxForce: Double): (Double, Double) = |
| 10 | + val magnitude = math.hypot(fx, fy) |
| 11 | + if magnitude > maxForce && magnitude > 0 then |
| 12 | + (fx * maxForce / magnitude, fy * maxForce / magnitude) |
| 13 | + else (fx, fy) |
| 14 | + |
| 15 | + def wrapBoundaries( |
| 16 | + positions: Matrix[Double], |
| 17 | + boidIndex: Int, |
| 18 | + width: Double, |
| 19 | + height: Double |
| 20 | + ): Unit = |
| 21 | + if positions((boidIndex, 0)) < 0 then positions((boidIndex, 0)) = width |
| 22 | + if positions((boidIndex, 0)) > width then positions((boidIndex, 0)) = 0 |
| 23 | + if positions((boidIndex, 1)) < 0 then positions((boidIndex, 1)) = height |
| 24 | + if positions((boidIndex, 1)) > height then positions((boidIndex, 1)) = 0 |
| 25 | + |
| 26 | + inline def calculateForces( |
| 27 | + positions: Matrix[Double], |
| 28 | + velocities: Matrix[Double], |
| 29 | + boidIndex: Int |
| 30 | + ): (Double, Double, Double, Double, Double, Double) = |
| 31 | + var sepX = 0.0 |
| 32 | + var sepY = 0.0 |
| 33 | + var sepCount = 0 |
| 34 | + |
| 35 | + var aliX = 0.0 |
| 36 | + var aliY = 0.0 |
| 37 | + var aliCount = 0 |
| 38 | + |
| 39 | + var cohX = 0.0 |
| 40 | + var cohY = 0.0 |
| 41 | + var cohCount = 0 |
| 42 | + |
| 43 | + val boidX = positions((boidIndex, 0)) |
| 44 | + val boidY = positions((boidIndex, 1)) |
| 45 | + val boidVelX = velocities((boidIndex, 0)) |
| 46 | + val boidVelY = velocities((boidIndex, 1)) |
| 47 | + |
| 48 | + var j = 0 |
| 49 | + while j < numBoids do |
| 50 | + if j != boidIndex then |
| 51 | + val dx = boidX - positions((j, 0)) |
| 52 | + val dy = boidY - positions((j, 1)) |
| 53 | + val distanceSquared = dx * dx + dy * dy |
| 54 | + |
| 55 | + if distanceSquared > 0 && distanceSquared < separationDistance * separationDistance then |
| 56 | + val distance = math.sqrt(distanceSquared) |
| 57 | + sepX += dx / distance |
| 58 | + sepY += dy / distance |
| 59 | + sepCount += 1 |
| 60 | + |
| 61 | + if distanceSquared > 0 && distanceSquared < alignmentDistance * alignmentDistance then |
| 62 | + aliX += velocities((j, 0)) |
| 63 | + aliY += velocities((j, 1)) |
| 64 | + aliCount += 1 |
| 65 | + |
| 66 | + if distanceSquared > 0 && distanceSquared < cohesionDistance * cohesionDistance then |
| 67 | + cohX += positions((j, 0)) |
| 68 | + cohY += positions((j, 1)) |
| 69 | + cohCount += 1 |
| 70 | + j += 1 |
| 71 | + |
| 72 | + // Process separation |
| 73 | + if sepCount > 0 then |
| 74 | + sepX /= sepCount |
| 75 | + sepY /= sepCount |
| 76 | + val magnitude = math.hypot(sepX, sepY) |
| 77 | + if magnitude > 0 then |
| 78 | + sepX = (sepX / magnitude) * maxSpeed - boidVelX |
| 79 | + sepY = (sepY / magnitude) * maxSpeed - boidVelY |
| 80 | + val (lx, ly) = limitForce(sepX, sepY, maxForce) |
| 81 | + sepX = lx |
| 82 | + sepY = ly |
| 83 | + |
| 84 | + // Process alignment |
| 85 | + if aliCount > 0 then |
| 86 | + aliX /= aliCount |
| 87 | + aliY /= aliCount |
| 88 | + val magnitude = math.hypot(aliX, aliY) |
| 89 | + if magnitude > 0 then |
| 90 | + aliX = (aliX / magnitude) * maxSpeed - boidVelX |
| 91 | + aliY = (aliY / magnitude) * maxSpeed - boidVelY |
| 92 | + val (lx, ly) = limitForce(aliX, aliY, maxForce) |
| 93 | + aliX = lx |
| 94 | + aliY = ly |
| 95 | + |
| 96 | + // Process cohesion |
| 97 | + if cohCount > 0 then |
| 98 | + cohX = cohX / cohCount - boidX |
| 99 | + cohY = cohY / cohCount - boidY |
| 100 | + val magnitude = math.hypot(cohX, cohY) |
| 101 | + if magnitude > 0 then |
| 102 | + cohX = (cohX / magnitude) * maxSpeed - boidVelX |
| 103 | + cohY = (cohY / magnitude) * maxSpeed - boidVelY |
| 104 | + val (lx, ly) = limitForce(cohX, cohY, maxForce) |
| 105 | + cohX = lx |
| 106 | + cohY = ly |
| 107 | + |
| 108 | + (sepX, sepY, aliX, aliY, cohX, cohY) |
0 commit comments