src/bz/fx/agent_simulation.c

   1 #include "agent_simulation_internal.h"
   2
   3 #include <bz/math/math.h>
   4 #include <bz/math/random.h>
   5 #include <bz/memory/allocator.h>
   6
   7 struct BZAgent {
   8         size_t agentIdx;
   9         BZAgentDetails details;
  10         BZVector force;
  11         BZVector velocity;
  12 };
  13
  14 struct BZAgentSimulation {
  15         size_t maxAgents;
  16         size_t agentCount;
  17         BZAgent *agents;
  18 };
  19
  20 BZAgentSimulationID bzFXCreateAgentSimulation(BZMemoryArenaID arena, size_t maxAgents, const char *identifierFmt, ...) {
  21         BZAgentSimulation *simulation = bzMemoryAlloc(arena, sizeof(BZAgentSimulation));
  22
  23         simulation->maxAgents = maxAgents;
  24         simulation->agents = bzMemoryAlloc(arena, maxAgents * sizeof(BZAgent));
  25
  26         return simulation;
  27 }
  28
  29 static void agentSeekForce(BZVector *out, BZAgent *agent, const BZVector *seekTarget) {
  30         BZVector delta;
  31         bzVectorSubtract(&delta, seekTarget, &agent->details.position);
  32         if (bzVectorMagnitudeSquared(&delta) > 10.0f * 10.0f) { // FIXME
  33                 bzVectorNormalized(out, &delta);
  34                 bzVectorScale(out, out, agent->details.maxForce); // FIXME
  35         } else {
  36                 bzVectorSet(out, 0, 0);
  37         }
  38 }
  39
  40 static void agentSeparationForce(BZVector *out, BZAgentSimulationID simulation, BZAgent *agent) {
  41         bzVectorSet(out, 0, 0);
  42         for (size_t i = 0; i < simulation->agentCount; ++i) {
  43                 BZVector delta;
  44                 bzVectorSubtract(&delta, &agent->details.position, &simulation->agents[i].details.position);
  45                 float squareDistance = bzVectorMagnitudeSquared(&delta);
  46                 float radius = 50.0f; // FIXME
  47
  48                 if (squareDistance > 0 && squareDistance < radius * radius) {
  49                         BZVector force;
  50                         bzVectorScale(&force, &delta, (radius * radius) / squareDistance); // Square distance helps scale up/down avoidance
  51                         bzVectorAdd(out, out, &force);
  52                 }
  53         }
  54 }
  55
  56 static void agentAlignmentForce(BZVector *out, BZAgentSimulationID simulation, BZAgent *agent) {
  57         BZVector averageForward = bzVectorMake(0, 0);
  58         size_t neighbourCount = 0;
  59         for (size_t i = 0; i < simulation->agentCount; ++i) {
  60                 BZVector delta;
  61                 bzVectorSubtract(&delta, &agent->details.position, &simulation->agents[i].details.position);
  62                 float squareDistance = bzVectorMagnitudeSquared(&delta);
  63                 float radius = 150.0f; // FIXME
  64
  65                 if (squareDistance > 0 && squareDistance < radius * radius) {
  66                         BZVector forward;
  67                         bzVectorMakeAngle(&forward, simulation->agents[i].details.angle);
  68                         bzVectorAdd(&averageForward, &averageForward, &forward);
  69                         neighbourCount += 1;
  70                 }
  71         }
  72
  73         if (neighbourCount > 0) {
  74                 bzVectorNormalized(out, &averageForward); // FIXME, need to divide by number of agents? limit??
  75         } else {
  76                 bzVectorSet(out, 0, 0);
  77         }
  78 }
  79
  80 static void agentCohesionForce(BZVector *out, BZAgentSimulationID simulation, BZAgent *agent) {
  81         BZVector averagePosition = bzVectorMake(0, 0);
  82         size_t neighbourCount = 0;
  83         for (size_t i = 0; i < simulation->agentCount; ++i) {
  84                 BZVector delta;
  85                 bzVectorSubtract(&delta, &agent->details.position, &simulation->agents[i].details.position);
  86                 float squareDistance = bzVectorMagnitudeSquared(&delta);
  87                 float radius = 150.0f; // FIXME
  88
  89                 if (squareDistance > 0 && squareDistance < radius * radius) {
  90                         bzVectorAdd(&averagePosition, &averagePosition, &simulation->agents[i].details.position);
  91                         neighbourCount += 1;
  92                 }
  93         }
  94
  95         if (neighbourCount > 0) {
  96                 bzVectorScale(&averagePosition, &averagePosition, 1.0f / neighbourCount);
  97                 bzVectorSubtract(&averagePosition, &averagePosition, &agent->details.position);
  98                 bzVectorNormalized(out, &averagePosition);
  99         } else {
 100                 bzVectorSet(out, 0, 0);
 101         }
 102 }
 103
 104 void bzFXUpdateAgentSimulation(BZAgentSimulationID simulation, float deltaTime, BZCollisionSpaceID collisionSpace, BZVector *v) {
 105         for (size_t i = 0; i < simulation->agentCount; ++i) {
 106                 BZAgent *agent = &simulation->agents[i];
 107
 108                 BZVector seekForce;
 109                 agentSeekForce(&seekForce, agent, v);
 110
 111                 BZVector separationForce;
 112                 agentSeparationForce(&separationForce, simulation, agent);
 113
 114                 BZVector alignmentForce;
 115                 agentAlignmentForce(&alignmentForce, simulation, agent);
 116
 117                 BZVector cohesionForce;
 118                 agentCohesionForce(&cohesionForce, simulation, agent);
 119
 120                 bzVectorSet(&agent->force, 0, 0);
 121                 bzVectorScale(&seekForce, &seekForce, 40.0f);
 122                 bzVectorAdd(&agent->force, &agent->force, &seekForce);
 123                 bzVectorScale(&separationForce, &separationForce, 100.0f);
 124                 bzVectorAdd(&agent->force, &agent->force, &separationForce);
 125                 bzVectorScale(&alignmentForce, &alignmentForce, 10.0f);
 126                 bzVectorAdd(&agent->force, &agent->force, &alignmentForce);
 127                 bzVectorScale(&cohesionForce, &cohesionForce, 10.0f);
 128                 bzVectorAdd(&agent->force, &agent->force, &cohesionForce);
 129
 130 //              bzVectorScale(&agent->force, &agent->force, 36.0f);
 131
 132                 float forceMagnitude = bzVectorMagnitude(&agent->force);
 133                 forceMagnitude = bzMin(forceMagnitude, agent->details.maxForce);
 134                 if (forceMagnitude > 0.0f) {
 135                         bzVectorNormalized(&agent->force, &agent->force);
 136                         bzVectorScale(&agent->force, &agent->force, forceMagnitude);
 137                         bzVectorScale(&agent->force, &agent->force, 1.0f / agent->details.mass);
 138
 139                         BZVector force;
 140                         bzVectorScale(&force, &agent->force, deltaTime);
 141                         bzVectorAdd(&agent->velocity, &agent->velocity, &force);
 142 //                      bzVectorCopy(&agent->velocity, &agent->force);
 143                 }
 144
 145                 float velocityMagnitude = bzVectorMagnitude(&agent->velocity);
 146                 velocityMagnitude = bzMin(velocityMagnitude, agent->details.maxSpeed);
 147                 if (velocityMagnitude > 0.0f) {
 148                         bzVectorNormalized(&agent->velocity, &agent->velocity);
 149                         bzVectorScale(&agent->velocity, &agent->velocity, velocityMagnitude);
 150                 }
 151
 152                 BZVector velocity;
 153                 bzVectorScale(&velocity, &agent->velocity, deltaTime);
 154                 bzVectorAdd(&agent->details.position, &agent->details.position, &velocity);
 155
 156 //              bzCollisionResolve()
 157         }
 158 }
 159
 160 BZAgentID bzFXAgentSimulationAddAgent(BZAgentSimulationID simulation, const BZAgentDetails *details) {
 161         bzAssertMessage(simulation->agentCount < simulation->maxAgents, "No available agents");
 162
 163         BZAgent *agent = &simulation->agents[simulation->agentCount++];
 164         agent->agentIdx = 1;
 165         agent->details = *details;
 166         //bzVectorSet(&agent->force, 0, 0);
 167         bzVectorSet(&agent->velocity, bzRandomFloatRange(-10, 10), bzRandomFloatRange(-10, 10));
 168
 169         return agent;
 170 }
 171
 172 void bzFXAgentSimulationRemoveAgent(BZAgentSimulationID simulation, BZAgentID agent) {
 173         agent->agentIdx = 0; // FIXME
 174 }
 175
 176 void bzFXAgentSimulationGetAgentDetails(BZAgentDetails *detailsOut, BZAgentSimulationID simulation, BZAgentID agent) {
 177         *detailsOut = agent->details;
 178 }
 179
 180
 181
 182 #include <bz/gfx/gfx.h>
 183 void bzFXAgentSimulationDrawDebug(BZAgentSimulationID simulation) {
 184         for (size_t i = 0; i < simulation->agentCount; ++i) {
 185                 BZAgent *agent = &simulation->agents[i];
 186                 bzCirc(agent->details.position.x, agent->details.position.y, 2, 9);
 187
 188                 BZVector forward;
 189                 bzVectorMakeAngle(&forward, agent->details.angle);
 190                 //bzLine(agent->details.position.x, agent->details.position.y, agent->details.position.x + forward.x * 40, agent->details.position.y + forward.y * 40, 7);
 191
 192                 bzLine(
 193                         agent->details.position.x, agent->details.position.y,
 194                         agent->details.position.x + agent->velocity.x, agent->details.position.y + agent->velocity.y,
 195                         7);
 196
 197                 bzLine(
 198                         agent->details.position.x, agent->details.position.y,
 199                         agent->details.position.x + agent->force.x, agent->details.position.y + agent->force.y,
 200                         8);
 201         }
 202 }