Core/Formations: update codestyle and fix crash

Closes #21288

(cherry picked from commit 73cc613dc86d7d72d78f22affe2faba6786bccc5)

8 files changed, 207 insertions, 146 deletions

diff --git a/src/server/game/AI/SmartScripts/SmartAI.cpp b/src/server/game/AI/SmartScripts/SmartAI.cpp
index 5769ea307ca..a90dc7e1b6b 100644
--- a/src/server/game/AI/SmartScripts/SmartAI.cpp
+++ b/src/server/game/AI/SmartScripts/SmartAI.cpp
@@ -548,14 +548,14 @@ void SmartAI::JustReachedHome()
     GetScript()->ProcessEventsFor(SMART_EVENT_REACHED_HOME);
 
     CreatureGroup* formation = me->GetFormation();
-    if (!formation || formation->getLeader() == me || !formation->isFormed())
+    if (!formation || formation->GetLeader() == me || !formation->IsFormed())
     {
         if (me->GetMotionMaster()->GetMotionSlotType(MOTION_SLOT_IDLE) != WAYPOINT_MOTION_TYPE && me->GetWaypointPath())
             me->GetMotionMaster()->MovePath(me->GetWaypointPath(), true);
         else
             me->ResumeMovement();
     }
-    else if (formation->isFormed())
+    else if (formation->IsFormed())
         me->GetMotionMaster()->MoveIdle(); // wait the order of leader
 }
 
diff --git a/src/server/game/Entities/Creature/Creature.cpp b/src/server/game/Entities/Creature/Creature.cpp
index 04752e739d1..8261a795915 100644
--- a/src/server/game/Entities/Creature/Creature.cpp
+++ b/src/server/game/Entities/Creature/Creature.cpp
@@ -374,9 +374,8 @@ void Creature::SearchFormation()
     if (!lowguid)
         return;
 
-    CreatureGroupInfoType::iterator frmdata = sFormationMgr->CreatureGroupMap.find(lowguid);
-    if (frmdata != sFormationMgr->CreatureGroupMap.end())
-        sFormationMgr->AddCreatureToGroup(frmdata->second->leaderGUID, this);
+    if (FormationInfo const* formationInfo = sFormationMgr->GetFormationInfo(lowguid))
+        sFormationMgr->AddCreatureToGroup(formationInfo->LeaderSpawnId, this);
 }
 
 bool Creature::IsFormationLeader() const
@@ -1066,17 +1065,18 @@ bool Creature::AIM_Initialize(CreatureAI* ai)
 
 void Creature::Motion_Initialize()
 {
-    if (!m_formation)
-        GetMotionMaster()->Initialize();
-    else if (m_formation->getLeader() == this)
+    if (m_formation)
     {
-        m_formation->FormationReset(false);
-        GetMotionMaster()->Initialize();
+        if (m_formation->GetLeader() == this)
+            m_formation->FormationReset(false);
+        else if (m_formation->IsFormed())
+        {
+            GetMotionMaster()->MoveIdle(); //wait the order of leader
+            return;
+        }
     }
-    else if (m_formation->isFormed())
-        GetMotionMaster()->MoveIdle(); //wait the order of leader
-    else
-        GetMotionMaster()->Initialize();
+
+    GetMotionMaster()->Initialize();
 }
 
 bool Creature::Create(ObjectGuid::LowType guidlow, Map* map, uint32 entry, Position const& pos, CreatureData const* data /*= nullptr*/, uint32 vehId /*= 0*/, bool dynamic)
@@ -2152,7 +2152,7 @@ void Creature::setDeathState(DeathState s)
         }
 
         //Dismiss group if is leader
-        if (m_formation && m_formation->getLeader() == this)
+        if (m_formation && m_formation->GetLeader() == this)
             m_formation->FormationReset(true);
 
         if ((CanFly() || IsFlying()))
diff --git a/src/server/game/Entities/Creature/CreatureGroups.cpp b/src/server/game/Entities/Creature/CreatureGroups.cpp
index 3f6cbd9279c..8e285f3e931 100644
--- a/src/server/game/Entities/Creature/CreatureGroups.cpp
+++ b/src/server/game/Entities/Creature/CreatureGroups.cpp
@@ -16,6 +16,7 @@
  */
 
 #include "CreatureGroups.h"
+#include "Containers.h"
 #include "Creature.h"
 #include "CreatureAI.h"
 #include "DatabaseEnv.h"
@@ -26,10 +27,12 @@
 
 #define MAX_DESYNC 5.0f
 
+FormationMgr::FormationMgr()
+{
+}
+
 FormationMgr::~FormationMgr()
 {
-    for (CreatureGroupInfoType::iterator itr = CreatureGroupMap.begin(); itr != CreatureGroupMap.end(); ++itr)
-        delete itr->second;
 }
 
 FormationMgr* FormationMgr::instance()
@@ -38,43 +41,53 @@ FormationMgr* FormationMgr::instance()
     return &instance;
 }
 
-void FormationMgr::AddCreatureToGroup(ObjectGuid::LowType leaderGuid, Creature* creature)
+void FormationMgr::AddCreatureToGroup(ObjectGuid::LowType leaderSpawnId, Creature* creature)
 {
-    Map* map = creature->FindMap();
-    if (!map)
-        return;
+    Map* map = creature->GetMap();
 
-    CreatureGroupHolderType::iterator itr = map->CreatureGroupHolder.find(leaderGuid);
-
-    //Add member to an existing group
+    auto itr = map->CreatureGroupHolder.find(leaderSpawnId);
     if (itr != map->CreatureGroupHolder.end())
     {
-        TC_LOG_DEBUG("entities.unit", "Group found: " UI64FMTD ", inserting %s, Group InstanceID %u", leaderGuid, creature->GetGUID().ToString().c_str(), creature->GetInstanceId());
-        itr->second->AddMember(creature);
+        //Add member to an existing group
+        TC_LOG_DEBUG("entities.unit", "Group found: " UI64FMTD ", inserting: %s, Group InstanceID %u", leaderSpawnId, creature->GetGUID().ToString().c_str(), creature->GetInstanceId());
+
+        // With dynamic spawn the creature may have just respawned
+        // we need to find previous instance of creature and delete it from the formation, as it'll be invalidated
+        auto bounds = Trinity::Containers::MapEqualRange(map->GetCreatureBySpawnIdStore(), creature->GetSpawnId());
+        for (auto const& pair : bounds)
+        {
+            Creature* other = pair.second;
+            if (other == creature)
+                continue;
+
+            if (itr->second->HasMember(other))
+                itr->second->RemoveMember(other);
+        }
     }
-    //Create new group
     else
     {
-        TC_LOG_DEBUG("entities.unit", "Group not found: " UI64FMTD ". Creating new group.", leaderGuid);
-        CreatureGroup* group = new CreatureGroup(leaderGuid);
-        map->CreatureGroupHolder[leaderGuid] = group;
-        group->AddMember(creature);
+        //Create new group
+        TC_LOG_DEBUG("entities.unit", "Group not found: " UI64FMTD ". Creating new group.", leaderSpawnId);
+        CreatureGroup* group = new CreatureGroup(leaderSpawnId);
+        std::tie(itr, std::ignore) = map->CreatureGroupHolder.emplace(leaderSpawnId, group);
     }
+
+    itr->second->AddMember(creature);
 }
 
 void FormationMgr::RemoveCreatureFromGroup(CreatureGroup* group, Creature* member)
 {
-    TC_LOG_DEBUG("entities.unit", "Deleting member pointer to GUID: " UI64FMTD " from group " UI64FMTD, group->GetId(), member->GetSpawnId());
+    TC_LOG_DEBUG("entities.unit", "Deleting member pointer to GUID: " UI64FMTD " from group " UI64FMTD, group->GetLeaderSpawnId(), member->GetSpawnId());
     group->RemoveMember(member);
 
-    if (group->isEmpty())
+    if (group->IsEmpty())
     {
-        Map* map = member->FindMap();
-        if (!map)
-            return;
+        Map* map = member->GetMap();
 
         TC_LOG_DEBUG("entities.unit", "Deleting group with InstanceID %u", member->GetInstanceId());
-        map->CreatureGroupHolder.erase(group->GetId());
+        auto itr = map->CreatureGroupHolder.find(group->GetLeaderSpawnId());
+        ASSERT(itr != map->CreatureGroupHolder.end(), "Not registered group " UI64FMTD " in map %u", group->GetLeaderSpawnId(), map->GetId());
+        map->CreatureGroupHolder.erase(itr);
         delete group;
     }
 }
@@ -83,13 +96,8 @@ void FormationMgr::LoadCreatureFormations()
 {
     uint32 oldMSTime = getMSTime();
 
-    for (CreatureGroupInfoType::iterator itr = CreatureGroupMap.begin(); itr != CreatureGroupMap.end(); ++itr) // for reload case
-        delete itr->second;
-    CreatureGroupMap.clear();
-
     //Get group data
     QueryResult result = WorldDatabase.Query("SELECT leaderGUID, memberGUID, dist, angle, groupAI, point_1, point_2 FROM creature_formations ORDER BY leaderGUID");
-
     if (!result)
     {
         TC_LOG_INFO("server.loading", ">>  Loaded 0 creatures in formations. DB table `creature_formations` is empty!");
@@ -97,88 +105,134 @@ void FormationMgr::LoadCreatureFormations()
     }
 
     uint32 count = 0;
-    Field* fields;
-    FormationInfo* group_member;
-
+    std::unordered_set<ObjectGuid::LowType> leaderSpawnIds;
     do
     {
-        fields = result->Fetch();
+        Field* fields = result->Fetch();
 
         //Load group member data
-        group_member                        = new FormationInfo();
-        group_member->leaderGUID            = fields[0].GetUInt64();
-        ObjectGuid::LowType memberGUID      = fields[1].GetUInt64();
-        group_member->groupAI               = fields[4].GetUInt32();
-        group_member->point_1               = fields[5].GetUInt16();
-        group_member->point_2               = fields[6].GetUInt16();
+        FormationInfo member;
+        member.LeaderSpawnId              = fields[0].GetUInt64();
+        ObjectGuid::LowType memberSpawnId = fields[1].GetUInt64();
+        member.FollowDist                 = 0.f;
+        member.FollowAngle                = 0.f;
+
         //If creature is group leader we may skip loading of dist/angle
-        if (group_member->leaderGUID != memberGUID)
+        if (member.LeaderSpawnId != memberSpawnId)
         {
-            group_member->follow_dist       = fields[2].GetFloat();
-            group_member->follow_angle      = fields[3].GetFloat() * float(M_PI) / 180;
-        }
-        else
-        {
-            group_member->follow_dist       = 0;
-            group_member->follow_angle      = 0;
+            member.FollowDist             = fields[2].GetFloat();
+            member.FollowAngle            = fields[3].GetFloat() * float(M_PI) / 180.0f;
         }
 
+        member.GroupAI                    = fields[4].GetUInt32();
+        for (uint8 i = 0; i < 2; ++i)
+            member.LeaderWaypointIDs[i]   = fields[5 + i].GetUInt16();
+
         // check data correctness
         {
-            if (!sObjectMgr->GetCreatureData(group_member->leaderGUID))
+            if (!sObjectMgr->GetCreatureData(member.LeaderSpawnId))
             {
-                TC_LOG_ERROR("sql.sql", "creature_formations table leader guid " UI64FMTD " incorrect (not exist)", group_member->leaderGUID);
-                delete group_member;
+                TC_LOG_ERROR("sql.sql", "creature_formations table leader guid " UI64FMTD " incorrect (not exist)", member.LeaderSpawnId);
                 continue;
             }
 
-            if (!sObjectMgr->GetCreatureData(memberGUID))
+            if (!sObjectMgr->GetCreatureData(memberSpawnId))
             {
-                TC_LOG_ERROR("sql.sql", "creature_formations table member guid " UI64FMTD " incorrect (not exist)", memberGUID);
-                delete group_member;
+                TC_LOG_ERROR("sql.sql", "creature_formations table member guid " UI64FMTD " incorrect (not exist)", memberSpawnId);
                 continue;
             }
+
+            leaderSpawnIds.insert(member.LeaderSpawnId);
         }
 
-        CreatureGroupMap[memberGUID] = group_member;
+        _creatureGroupMap.emplace(memberSpawnId, std::move(member));
         ++count;
+    } while (result->NextRow());
+
+    for (ObjectGuid::LowType leaderSpawnId : leaderSpawnIds)
+    {
+        if (!_creatureGroupMap.count(leaderSpawnId))
+        {
+            TC_LOG_ERROR("sql.sql", "creature_formation contains leader spawn " UI64FMTD " which is not included on its formation, removing", leaderSpawnId);
+            for (auto itr = _creatureGroupMap.begin(); itr != _creatureGroupMap.end();)
+            {
+                if (itr->second.LeaderSpawnId == leaderSpawnId)
+                {
+                    itr = _creatureGroupMap.erase(itr);
+                    continue;
+                }
+
+                ++itr;
+            }
+        }
     }
-    while (result->NextRow());
 
     TC_LOG_INFO("server.loading", ">> Loaded %u creatures in formations in %u ms", count, GetMSTimeDiffToNow(oldMSTime));
 }
 
+FormationInfo* FormationMgr::GetFormationInfo(ObjectGuid::LowType spawnId)
+{
+    return Trinity::Containers::MapGetValuePtr(_creatureGroupMap, spawnId);
+}
+
+void FormationMgr::AddFormationMember(ObjectGuid::LowType spawnId, float followAng, float followDist, ObjectGuid::LowType leaderSpawnId, uint32 groupAI)
+{
+    FormationInfo member;
+    member.LeaderSpawnId = leaderSpawnId;
+    member.FollowDist    = followDist;
+    member.FollowAngle   = followAng;
+    member.GroupAI       = groupAI;
+    for (uint8 i = 0; i < 2; ++i)
+        member.LeaderWaypointIDs[i] = 0;
+
+    _creatureGroupMap.emplace(spawnId, std::move(member));
+}
+
+CreatureGroup::CreatureGroup(ObjectGuid::LowType leaderSpawnId) : _leader(nullptr), _members(), _leaderSpawnId(leaderSpawnId), _formed(false), _engaging(false)
+{
+}
+
+CreatureGroup::~CreatureGroup()
+{
+}
+
 void CreatureGroup::AddMember(Creature* member)
 {
     TC_LOG_DEBUG("entities.unit", "CreatureGroup::AddMember: Adding %s.", member->GetGUID().ToString().c_str());
 
     //Check if it is a leader
-    if (member->GetSpawnId() == m_groupID)
+    if (member->GetSpawnId() == _leaderSpawnId)
     {
         TC_LOG_DEBUG("entities.unit", "%s is formation leader. Adding group.", member->GetGUID().ToString().c_str());
-        m_leader = member;
+        _leader = member;
     }
 
-    m_members[member] = sFormationMgr->CreatureGroupMap.find(member->GetSpawnId())->second;
+    // formation must be registered at this point
+    FormationInfo* formationInfo = ASSERT_NOTNULL(sFormationMgr->GetFormationInfo(member->GetSpawnId()));
+    _members.emplace(member, formationInfo);
     member->SetFormation(this);
 }
 
 void CreatureGroup::RemoveMember(Creature* member)
 {
-    if (m_leader == member)
-        m_leader = nullptr;
+    if (_leader == member)
+        _leader = nullptr;
 
-    m_members.erase(member);
+    _members.erase(member);
     member->SetFormation(nullptr);
 }
 
 void CreatureGroup::MemberEngagingTarget(Creature* member, Unit* target)
 {
-    uint8 groupAI = sFormationMgr->CreatureGroupMap[member->GetSpawnId()]->groupAI;
+    // used to prevent recursive calls
+    if (_engaging)
+        return;
+
+    uint8 groupAI = ASSERT_NOTNULL(sFormationMgr->GetFormationInfo(member->GetSpawnId()))->GroupAI;
     if (!groupAI)
         return;
 
-    if (member == m_leader)
+    if (member == _leader)
     {
         if (!(groupAI & FLAG_MEMBERS_ASSIST_LEADER))
             return;
@@ -186,64 +240,75 @@ void CreatureGroup::MemberEngagingTarget(Creature* member, Unit* target)
     else if (!(groupAI & FLAG_LEADER_ASSISTS_MEMBER))
         return;
 
-    for (CreatureGroupMemberType::iterator itr = m_members.begin(); itr != m_members.end(); ++itr)
+    _engaging = true;
+
+    for (auto const& pair : _members)
     {
-        Creature* other = itr->first;
-        // Skip self
+        Creature* other = pair.first;
         if (other == member)
             continue;
 
         if (!other->IsAlive())
             continue;
 
-        if (((other != m_leader && (groupAI & FLAG_MEMBERS_ASSIST_LEADER)) || (other == m_leader && (groupAI & FLAG_LEADER_ASSISTS_MEMBER))) && other->IsValidAttackTarget(target))
+        if (((other != _leader && (groupAI & FLAG_MEMBERS_ASSIST_LEADER)) || (other == _leader && (groupAI & FLAG_LEADER_ASSISTS_MEMBER))) && other->IsValidAttackTarget(target))
             other->EngageWithTarget(target);
     }
+
+    _engaging = false;
 }
 
 void CreatureGroup::FormationReset(bool dismiss)
 {
-    for (CreatureGroupMemberType::iterator itr = m_members.begin(); itr != m_members.end(); ++itr)
+    for (auto const& pair : _members)
     {
-        if (itr->first != m_leader && itr->first->IsAlive())
+        if (pair.first != _leader && pair.first->IsAlive())
         {
             if (dismiss)
-                itr->first->GetMotionMaster()->Initialize();
+                pair.first->GetMotionMaster()->Initialize();
             else
-                itr->first->GetMotionMaster()->MoveIdle();
-            TC_LOG_DEBUG("entities.unit", "Set %s movement for member %s", dismiss ? "default" : "idle", itr->first->GetGUID().ToString().c_str());
+                pair.first->GetMotionMaster()->MoveIdle();
+            TC_LOG_DEBUG("entities.unit", "Set %s movement for member: %s", dismiss ? "default" : "idle", pair.first->GetGUID().ToString().c_str());
         }
     }
-    m_Formed = !dismiss;
+
+    _formed = !dismiss;
 }
 
 void CreatureGroup::LeaderMoveTo(Position const& destination, uint32 id /*= 0*/, uint32 moveType /*= 0*/, bool orientation /*= false*/)
 {
     //! To do: This should probably get its own movement generator or use WaypointMovementGenerator.
     //! If the leader's path is known, member's path can be plotted as well using formation offsets.
-    if (!m_leader)
+    if (!_leader)
         return;
 
-    float x = destination.GetPositionX(), y = destination.GetPositionY(), z = destination.GetPositionZ();
-
-    float pathangle = std::atan2(m_leader->GetPositionY() - y, m_leader->GetPositionX() - x);
+    Position pos(destination);
+    float pathangle = std::atan2(_leader->GetPositionY() - pos.GetPositionY(), _leader->GetPositionX() - pos.GetPositionX());
 
-    for (CreatureGroupMemberType::iterator itr = m_members.begin(); itr != m_members.end(); ++itr)
+    for (auto const& pair : _members)
     {
-        Creature* member = itr->first;
-        if (member == m_leader || !member->IsAlive() || member->GetVictim() || !(itr->second->groupAI & FLAG_IDLE_IN_FORMATION))
+        Creature* member = pair.first;
+        if (member == _leader || !member->IsAlive() || member->IsEngaged() || !(pair.second->GroupAI & FLAG_IDLE_IN_FORMATION))
             continue;
 
-        if (itr->second->point_1)
-            if (m_leader->GetCurrentWaypointInfo().first == itr->second->point_1 || m_leader->GetCurrentWaypointInfo().first == itr->second->point_2)
-                itr->second->follow_angle = float(M_PI) * 2 - itr->second->follow_angle;
+        if (pair.second->LeaderWaypointIDs[0])
+        {
+            for (uint8 i = 0; i < 2; ++i)
+            {
+                if (_leader->GetCurrentWaypointInfo().first == pair.second->LeaderWaypointIDs[i])
+                {
+                    pair.second->FollowAngle = float(M_PI) * 2.f - pair.second->FollowAngle;
+                    break;
+                }
+            }
+        }
 
-        float angle = itr->second->follow_angle;
-        float dist = itr->second->follow_dist;
+        float angle = pair.second->FollowAngle;
+        float dist = pair.second->FollowDist;
 
-        float dx = x + std::cos(angle + pathangle) * dist;
-        float dy = y + std::sin(angle + pathangle) * dist;
-        float dz = z;
+        float dx = pos.GetPositionX() + std::cos(angle + pathangle) * dist;
+        float dy = pos.GetPositionY() + std::sin(angle + pathangle) * dist;
+        float dz = pos.GetPositionZ();
 
         Trinity::NormalizeMapCoord(dx);
         Trinity::NormalizeMapCoord(dy);
@@ -253,18 +318,18 @@ void CreatureGroup::LeaderMoveTo(Position const& destination, uint32 id /*= 0*/,
 
         Position point(dx, dy, dz, destination.GetOrientation());
 
-        member->GetMotionMaster()->MoveFormation(id, point, moveType, !member->IsWithinDist(m_leader, dist + MAX_DESYNC), orientation);
+        member->GetMotionMaster()->MoveFormation(id, point, moveType, !member->IsWithinDist(_leader, dist + MAX_DESYNC), orientation);
         member->SetHomePosition(dx, dy, dz, pathangle);
     }
 }
 
 bool CreatureGroup::CanLeaderStartMoving() const
 {
-    for (auto itr = m_members.begin(); itr != m_members.end(); ++itr)
+    for (auto const& pair : _members)
     {
-        if (itr->first != m_leader && itr->first->IsAlive())
+        if (pair.first != _leader && pair.first->IsAlive())
         {
-            if (itr->first->IsEngaged() || itr->first->IsReturningHome())
+            if (pair.first->IsEngaged() || pair.first->IsReturningHome())
                 return false;
         }
     }
diff --git a/src/server/game/Entities/Creature/CreatureGroups.h b/src/server/game/Entities/Creature/CreatureGroups.h
index a5735ca3f88..2b4924774ec 100644
--- a/src/server/game/Entities/Creature/CreatureGroups.h
+++ b/src/server/game/Entities/Creature/CreatureGroups.h
@@ -39,52 +39,55 @@ struct Position;
 
 struct FormationInfo
 {
-    ObjectGuid::LowType leaderGUID;
-    float follow_dist;
-    float follow_angle;
-    uint32 groupAI;
-    uint32 point_1;
-    uint32 point_2;
+    ObjectGuid::LowType LeaderSpawnId;
+    float FollowDist;
+    float FollowAngle;
+    uint32 GroupAI;
+    uint32 LeaderWaypointIDs[2];
 };
 
-typedef std::unordered_map<ObjectGuid::LowType/*memberDBGUID*/, FormationInfo*>   CreatureGroupInfoType;
-
 class TC_GAME_API FormationMgr
 {
     private:
-        FormationMgr() { }
+        FormationMgr();
         ~FormationMgr();
 
+        std::unordered_map<ObjectGuid::LowType /*spawnID*/, FormationInfo> _creatureGroupMap;
+
     public:
         static FormationMgr* instance();
 
-        void AddCreatureToGroup(ObjectGuid::LowType leaderGuid, Creature* creature);
+        void AddCreatureToGroup(ObjectGuid::LowType leaderSpawnId, Creature* creature);
         void RemoveCreatureFromGroup(CreatureGroup* group, Creature* creature);
+
         void LoadCreatureFormations();
-        CreatureGroupInfoType CreatureGroupMap;
+        FormationInfo* GetFormationInfo(ObjectGuid::LowType spawnId);
+
+        void AddFormationMember(ObjectGuid::LowType spawnId, float followAng, float followDist, ObjectGuid::LowType leaderSpawnId, uint32 groupAI);
 };
 
 class TC_GAME_API CreatureGroup
 {
     private:
-        Creature* m_leader; //Important do not forget sometimes to work with pointers instead synonims :D:D
-        typedef std::map<Creature*, FormationInfo*>  CreatureGroupMemberType;
-        CreatureGroupMemberType m_members;
+        Creature* _leader; //Important do not forget sometimes to work with pointers instead synonims :D:D
+        std::unordered_map<Creature*, FormationInfo*> _members;
 
-        ObjectGuid::LowType m_groupID;
-        bool m_Formed;
+        ObjectGuid::LowType _leaderSpawnId;
+        bool _formed;
+        bool _engaging;
 
     public:
         //Group cannot be created empty
-        explicit CreatureGroup(ObjectGuid::LowType id) : m_leader(nullptr), m_groupID(id), m_Formed(false) { }
-        ~CreatureGroup() { }
+        explicit CreatureGroup(ObjectGuid::LowType leaderSpawnId);
+        ~CreatureGroup();
 
-        Creature* getLeader() const { return m_leader; }
-        ObjectGuid::LowType GetId() const { return m_groupID; }
-        bool isEmpty() const { return m_members.empty(); }
-        bool isFormed() const { return m_Formed; }
-        bool IsLeader(Creature const* creature) const { return m_leader == creature; }
+        Creature* GetLeader() const { return _leader; }
+        ObjectGuid::LowType GetLeaderSpawnId() const { return _leaderSpawnId; }
+        bool IsEmpty() const { return _members.empty(); }
+        bool IsFormed() const { return _formed; }
+        bool IsLeader(Creature const* creature) const { return _leader == creature; }
 
+        bool HasMember(Creature* member) const { return _members.count(member) > 0; }
         void AddMember(Creature* member);
         void RemoveMember(Creature* member);
         void FormationReset(bool dismiss);
diff --git a/src/server/game/Maps/Map.h b/src/server/game/Maps/Map.h
index 174520be8a0..289fb4180b3 100644
--- a/src/server/game/Maps/Map.h
+++ b/src/server/game/Maps/Map.h
@@ -210,8 +210,6 @@ struct ZoneDynamicInfo
 #define MIN_UNLOAD_DELAY      1                             // immediate unload
 #define MAP_INVALID_ZONE      0xFFFFFFFF
 
-typedef std::map<ObjectGuid::LowType/*leaderDBGUID*/, CreatureGroup*> CreatureGroupHolderType;
-
 struct RespawnInfo; // forward declaration
 struct CompareRespawnInfo
 {
@@ -435,7 +433,7 @@ class TC_GAME_API Map : public GridRefManager<NGridType>
         void RemoveFromActive(T* obj);
 
         template<class T> void SwitchGridContainers(T* obj, bool on);
-        CreatureGroupHolderType CreatureGroupHolder;
+        std::unordered_map<ObjectGuid::LowType /*leaderSpawnId*/, CreatureGroup*> CreatureGroupHolder;
 
         void UpdateIteratorBack(Player* player);
 
diff --git a/src/server/scripts/Commands/cs_npc.cpp b/src/server/scripts/Commands/cs_npc.cpp
index 48c13f9bf8b..b33548acc68 100644
--- a/src/server/scripts/Commands/cs_npc.cpp
+++ b/src/server/scripts/Commands/cs_npc.cpp
@@ -1722,7 +1722,7 @@ public:
         ObjectGuid::LowType lowguid = creature->GetSpawnId();
         if (creature->GetFormation())
         {
-            handler->PSendSysMessage("Selected creature is already member of group " UI64FMTD, creature->GetFormation()->GetId());
+            handler->PSendSysMessage("Selected creature is already member of group " UI64FMTD, creature->GetFormation()->GetLeaderSpawnId());
             return false;
         }
 
@@ -1730,24 +1730,19 @@ public:
             return false;
 
         Player* chr = handler->GetSession()->GetPlayer();
-        FormationInfo* group_member;
 
-        group_member                 = new FormationInfo;
-        group_member->follow_angle   = (creature->GetAngle(chr) - chr->GetOrientation()) * 180 / float(M_PI);
-        group_member->follow_dist    = std::sqrt(std::pow(chr->GetPositionX() - creature->GetPositionX(), 2.f) + std::pow(chr->GetPositionY() - creature->GetPositionY(), 2.f));
-        group_member->leaderGUID     = leaderGUID;
-        group_member->groupAI        = 0;
-
-        sFormationMgr->CreatureGroupMap[lowguid] = group_member;
+        float  followAngle = (creature->GetAngle(chr) - chr->GetOrientation()) * 180.0f / float(M_PI);
+        float  followDist  = std::sqrt(std::pow(chr->GetPositionX() - creature->GetPositionX(), 2.f) + std::pow(chr->GetPositionY() - creature->GetPositionY(), 2.f));
+        uint32 groupAI     = 0;
+        sFormationMgr->AddFormationMember(lowguid, followAngle, followDist, leaderGUID, groupAI);
         creature->SearchFormation();
 
         WorldDatabasePreparedStatement* stmt = WorldDatabase.GetPreparedStatement(WORLD_INS_CREATURE_FORMATION);
-
         stmt->setUInt64(0, leaderGUID);
         stmt->setUInt64(1, lowguid);
-        stmt->setFloat(2, group_member->follow_dist);
-        stmt->setFloat(3, group_member->follow_angle);
-        stmt->setUInt32(4, uint32(group_member->groupAI));
+        stmt->setFloat (2, followAngle);
+        stmt->setFloat (3, followDist);
+        stmt->setUInt32(4, groupAI);
 
         WorldDatabase.Execute(stmt);
 
diff --git a/src/server/scripts/EasternKingdoms/zone_goldshire.cpp b/src/server/scripts/EasternKingdoms/zone_goldshire.cpp
index 3ba89e98f1f..fe6450cf4fe 100644
--- a/src/server/scripts/EasternKingdoms/zone_goldshire.cpp
+++ b/src/server/scripts/EasternKingdoms/zone_goldshire.cpp
@@ -252,7 +252,7 @@ struct npc_cameron : public ScriptedAI
                         _childrenGUIDs.push_back(jose->GetGUID());
 
                     // If Formation was disbanded, remake.
-                    if (!me->GetFormation()->isFormed())
+                    if (!me->GetFormation()->IsFormed())
                         for (ObjectGuid guid : _childrenGUIDs)
                             if (Creature* child = ObjectAccessor::GetCreature(*me, guid))
                                 child->SearchFormation();
diff --git a/src/server/scripts/Maelstrom/Stonecore/instance_stonecore.cpp b/src/server/scripts/Maelstrom/Stonecore/instance_stonecore.cpp
index c999435f051..77e8eabe75c 100644
--- a/src/server/scripts/Maelstrom/Stonecore/instance_stonecore.cpp
+++ b/src/server/scripts/Maelstrom/Stonecore/instance_stonecore.cpp
@@ -93,7 +93,7 @@ class instance_stonecore : public InstanceMapScript
                 creature->SearchFormation();
                 if (CreatureGroup* group = creature->GetFormation())
                 {
-                    switch (group->GetId())
+                    switch (group->GetLeaderSpawnId())
                     {
                         case CREATURE_FORMATION_MILLHOUSE_EVENT_TRASH:
                             millhouseTrashGUIDs.push_back(creature->GetGUID());