fp: First draft swarming on entity

feely_pona_game.cpp

@@ -630,3 +630,48 @@ static Dqn_Slice<Dqn_V2I> FP_Game_AStarPathFind(FP_Game            *game,
     DQN_ASSERT(result.size == slice_size);
     return result;
 }
+
+static Dqn_V2 FP_Game_CalcWaypointWorldPos(FP_Game *game, FP_GameWaypoint const *waypoint)
+{
+    Dqn_V2 result = {};
+    if (!game || !waypoint)
+        return result;
+
+    FP_GameEntity *waypoint_entity = FP_Game_GetEntity(game, waypoint->entity);
+    if (FP_Game_IsNilEntity(waypoint_entity))
+        return result;
+
+    // NOTE: We found a waypoint that is valid to move towards
+    switch (waypoint->type) {
+        case FP_GameWaypointType_At: {
+            result = FP_Game_CalcEntityWorldPos(game, waypoint_entity->handle);
+        } break;
+
+        case FP_GameWaypointType_Side: {
+            Dqn_Rect entity_rect = FP_Game_CalcEntityWorldHitBox(game, waypoint_entity->handle);
+            switch (waypoint->type_direction) {
+                case FP_GameDirection_Up: {
+                    result = Dqn_V2_InitNx2(entity_rect.pos.x + entity_rect.size.w * .5f, entity_rect.pos.y - entity_rect.size.h * .5f);
+                } break;
+
+                case FP_GameDirection_Down: {
+                    result = Dqn_V2_InitNx2(entity_rect.pos.x + entity_rect.size.w * .5f, entity_rect.pos.y + entity_rect.size.h + (entity_rect.size.h * .5f));
+                } break;
+
+                case FP_GameDirection_Left: {
+                    result = Dqn_V2_InitNx2(entity_rect.pos.x - entity_rect.size.w * .5f, entity_rect.pos.y + entity_rect.size.h * .5f);
+                } break;
+
+                case FP_GameDirection_Right: {
+                    result = Dqn_V2_InitNx2(entity_rect.pos.x + entity_rect.size.w + entity_rect.size.w * .5f, entity_rect.pos.y + entity_rect.size.h * .5f);
+                } break;
+
+                case FP_GameDirection_Count: DQN_INVALID_CODE_PATH; break;
+            }
+        } break;
+    }
+
+    return result;
+}
+
+