Implement LegoPathController::FUN_10048310 (#1192)

* WIP

* Rename

* Fix

* More WIP

* WIP

* WIP

* Fix

* Annotations

* Add more annotations, improve match

* Raise max functions

.github/workflows/build.yml

@@ -139,7 +139,7 @@ jobs:
       run: |
         reccmp-reccmp -S CONFIGPROGRESS.SVG --svg-icon assets/config.png --target CONFIG | tee CONFIGPROGRESS.TXT
         reccmp-reccmp -S ISLEPROGRESS.SVG --svg-icon assets/isle.png --target ISLE | tee ISLEPROGRESS.TXT
-        reccmp-reccmp -S LEGO1PROGRESS.SVG -T 4252 --svg-icon assets/lego1.png --target LEGO1 | tee LEGO1PROGRESS.TXT
+        reccmp-reccmp -S LEGO1PROGRESS.SVG -T 4302 --svg-icon assets/lego1.png --target LEGO1 | tee LEGO1PROGRESS.TXT
 
     - name: Compare Accuracy With Current Master
       shell: bash

LEGO1/lego/legoomni/include/act2actor.h

@@ -11,10 +11,10 @@
 class Act2Actor : public LegoAnimActor {
 public:
 	struct UnknownListStructure {
-		MxFloat m_unk0x00[3];  // 0x00
+		MxFloat m_position[3];  // 0x00
-		MxFloat m_unk0x0c[3];  // 0x0c
+		MxFloat m_direction[3]; // 0x0c
-		const char* m_unk0x18; // 0x18
+		const char* m_boundary; // 0x18
-		undefined m_unk0x1c;   // 0x1c
+		undefined m_unk0x1c;    // 0x1c
 	};
 
 	Act2Actor();

LEGO1/lego/legoomni/include/legopathcontroller.h

@@ -22,7 +22,9 @@ class Vector3;
 
 // VTABLE: LEGO1 0x100d7da8
 // SIZE 0x40
-struct LegoPathCtrlEdge : public LegoUnknown100db7f4 {};
+struct LegoPathCtrlEdge : public LegoUnknown100db7f4 {
+	undefined4 FUN_10048c40(const Vector3&);
+};
 
 struct LegoPathCtrlEdgeCompare {
 	MxU32 operator()(const LegoPathCtrlEdge* p_lhs, const LegoPathCtrlEdge* p_rhs) const
@@ -119,13 +121,13 @@ class LegoPathController : public MxCore {
 	);
 	MxResult FUN_10048310(
 		LegoPathEdgeContainer* p_grec,
-		const Vector3& p_position,
+		const Vector3& p_oldPosition,
-		const Vector3& p_direction,
+		const Vector3& p_oldDirection,
-		LegoPathBoundary* p_boundary1,
+		LegoPathBoundary* p_oldBoundary,
-		const Vector3& p_param5,
+		const Vector3& p_newPosition,
-		const Vector3& p_param6,
+		const Vector3& p_newDirection,
-		LegoPathBoundary* p_boundary2,
+		LegoPathBoundary* p_newBoundary,
-		MxBool p_param8,
+		LegoU8 p_mask,
 		MxFloat* p_param9
 	);
 
@@ -182,6 +184,15 @@ class LegoPathController : public MxCore {
 // TEMPLATE: LEGO1 0x100451a0
 // _Tree<LegoPathCtrlEdge *,LegoPathCtrlEdge *,set<LegoPathCtrlEdge *,LegoPathCtrlEdgeCompare,allocator<LegoPathCtrlEdge *> >::_Kfn,LegoPathCtrlEdgeCompare,allocator<LegoPathCtrlEdge *> >::~_Tree<LegoPathCtrlEdge *,LegoPathCtrlEdge *,set<LegoPathCtrlEdge *,LegoPathControl
 
+// TEMPLATE: LEGO1 0x10045270
+// _Tree<LegoPathCtrlEdge *,LegoPathCtrlEdge *,set<LegoPathCtrlEdge *,LegoPathCtrlEdgeCompare,allocator<LegoPathCtrlEdge *> >::_Kfn,LegoPathCtrlEdgeCompare,allocator<LegoPathCtrlEdge *> >::iterator::_Inc
+
+// TEMPLATE: LEGO1 0x100452b0
+// _Tree<LegoPathCtrlEdge *,LegoPathCtrlEdge *,set<LegoPathCtrlEdge *,LegoPathCtrlEdgeCompare,allocator<LegoPathCtrlEdge *> >::_Kfn,LegoPathCtrlEdgeCompare,allocator<LegoPathCtrlEdge *> >::erase
+
+// TEMPLATE: LEGO1 0x10045700
+// _Tree<LegoPathCtrlEdge *,LegoPathCtrlEdge *,set<LegoPathCtrlEdge *,LegoPathCtrlEdgeCompare,allocator<LegoPathCtrlEdge *> >::_Kfn,LegoPathCtrlEdgeCompare,allocator<LegoPathCtrlEdge *> >::_Erase
+
 // TEMPLATE: LEGO1 0x100457e0
 // Set<LegoPathCtrlEdge *,LegoPathCtrlEdgeCompare>::~Set<LegoPathCtrlEdge *,LegoPathCtrlEdgeCompare>
 
@@ -221,14 +232,74 @@ class LegoPathController : public MxCore {
 // SYNTHETIC: LEGO1 0x10047ae0
 // LegoUnknown100db7f4::~LegoUnknown100db7f4
 
+// TEMPLATE: LEGO1 0x10048f00
+// list<LegoBoundaryEdge,allocator<LegoBoundaryEdge> >::begin
+
+// TEMPLATE: LEGO1 0x10048f10
+// list<LegoBoundaryEdge,allocator<LegoBoundaryEdge> >::insert
+
 // TEMPLATE: LEGO1 0x10048f70
 // list<LegoBoundaryEdge,allocator<LegoBoundaryEdge> >::erase
 
+// TEMPLATE: LEGO1 0x10048fc0
+// _Tree<LegoPathCtrlEdge *,LegoPathCtrlEdge *,set<LegoPathCtrlEdge *,LegoPathCtrlEdgeCompare,allocator<LegoPathCtrlEdge *> >::_Kfn,LegoPathCtrlEdgeCompare,allocator<LegoPathCtrlEdge *> >::_Tree<LegoPathCtrlEdge *,LegoPathCtrlEdge *,set<LegoPathCtrlEdge *,Le
+
+// TEMPLATE: LEGO1 0x10049160
+// ?erase@?$_Tree@PAULegoPathCtrlEdge@@PAU1@U_Kfn@?$set@PAULegoPathCtrlEdge@@ULegoPathCtrlEdgeCompare@@V?$allocator@PAULegoPathCtrlEdge@@@@@@ULegoPathCtrlEdgeCompare@@V?$allocator@PAULegoPathCtrlEdge@@@@@@QAEIABQAULegoPathCtrlEdge@@@Z
+
+// TEMPLATE: LEGO1 0x10049290
+// _Tree<LegoPathCtrlEdge *,LegoPathCtrlEdge *,set<LegoPathCtrlEdge *,LegoPathCtrlEdgeCompare,allocator<LegoPathCtrlEdge *> >::_Kfn,LegoPathCtrlEdgeCompare,allocator<LegoPathCtrlEdge *> >::find
+
+// TEMPLATE: LEGO1 0x100492f0
+// _Tree<LegoPathCtrlEdge *,LegoPathCtrlEdge *,set<LegoPathCtrlEdge *,LegoPathCtrlEdgeCompare,allocator<LegoPathCtrlEdge *> >::_Kfn,LegoPathCtrlEdgeCompare,allocator<LegoPathCtrlEdge *> >::_Copy
+
+// TEMPLATE: LEGO1 0x10049370
+// _Tree<LegoPathCtrlEdge *,LegoPathCtrlEdge *,set<LegoPathCtrlEdge *,LegoPathCtrlEdgeCompare,allocator<LegoPathCtrlEdge *> >::_Kfn,LegoPathCtrlEdgeCompare,allocator<LegoPathCtrlEdge *> >::_Ubound
+
+// TEMPLATE: LEGO1 0x10049410
+// list<LegoBEWithFloat,allocator<LegoBEWithFloat> >::insert
+
+// TEMPLATE: LEGO1 0x10049470
+// list<LegoBEWithFloat,allocator<LegoBEWithFloat> >::_Buynode
+
+// TEMPLATE: LEGO1 0x100494a0
+// _Tree<LegoBEWithFloat *,LegoBEWithFloat *,multiset<LegoBEWithFloat *,LegoBEWithFloatComparator,allocator<LegoBEWithFloat *> >::_Kfn,LegoBEWithFloatComparator,allocator<LegoBEWithFloat *> >::iterator::_Inc
+
+// TEMPLATE: LEGO1 0x100495b0
+// _Tree<LegoBEWithFloat *,LegoBEWithFloat *,multiset<LegoBEWithFloat *,LegoBEWithFloatComparator,allocator<LegoBEWithFloat *> >::_Kfn,LegoBEWithFloatComparator,allocator<LegoBEWithFloat *> >::insert
+
+// TEMPLATE: LEGO1 0x10049840
+// _Tree<LegoBEWithFloat *,LegoBEWithFloat *,multiset<LegoBEWithFloat *,LegoBEWithFloatComparator,allocator<LegoBEWithFloat *> >::_Kfn,LegoBEWithFloatComparator,allocator<LegoBEWithFloat *> >::iterator::_Dec
+
+// TEMPLATE: LEGO1 0x10049890
+// _Tree<LegoBEWithFloat *,LegoBEWithFloat *,multiset<LegoBEWithFloat *,LegoBEWithFloatComparator,allocator<LegoBEWithFloat *> >::_Kfn,LegoBEWithFloatComparator,allocator<LegoBEWithFloat *> >::erase
+
+// TEMPLATE: LEGO1 0x10049cf0
+// _Tree<LegoBEWithFloat *,LegoBEWithFloat *,multiset<LegoBEWithFloat *,LegoBEWithFloatComparator,allocator<LegoBEWithFloat *> >::_Kfn,LegoBEWithFloatComparator,allocator<LegoBEWithFloat *> >::_Buynode
+
+// TEMPLATE: LEGO1 0x10049d50
+// _Tree<LegoBEWithFloat *,LegoBEWithFloat *,multiset<LegoBEWithFloat *,LegoBEWithFloatComparator,allocator<LegoBEWithFloat *> >::_Kfn,LegoBEWithFloatComparator,allocator<LegoBEWithFloat *> >::_Init
+
+// TEMPLATE: LEGO1 0x10049e00
+// _Tree<LegoBEWithFloat *,LegoBEWithFloat *,multiset<LegoBEWithFloat *,LegoBEWithFloatComparator,allocator<LegoBEWithFloat *> >::_Kfn,LegoBEWithFloatComparator,allocator<LegoBEWithFloat *> >::_Insert
+
+// TEMPLATE: LEGO1 0x1004a090
+// _Tree<LegoBEWithFloat *,LegoBEWithFloat *,multiset<LegoBEWithFloat *,LegoBEWithFloatComparator,allocator<LegoBEWithFloat *> >::_Kfn,LegoBEWithFloatComparator,allocator<LegoBEWithFloat *> >::_Lrotate
+
+// TEMPLATE: LEGO1 0x1004a0f0
+// _Tree<LegoBEWithFloat *,LegoBEWithFloat *,multiset<LegoBEWithFloat *,LegoBEWithFloatComparator,allocator<LegoBEWithFloat *> >::_Kfn,LegoBEWithFloatComparator,allocator<LegoBEWithFloat *> >::_Rrotate
+
+// TEMPLATE: LEGO1 0x1004a760
+// _Construct
+
 // TEMPLATE: LEGO1 0x1004a780
 // _Construct
 
 // GLOBAL: LEGO1 0x100f4360
 // _Tree<LegoPathCtrlEdge *,LegoPathCtrlEdge *,set<LegoPathCtrlEdge *,LegoPathCtrlEdgeCompare,allocator<LegoPathCtrlEdge *> >::_Kfn,LegoPathCtrlEdgeCompare,allocator<LegoPathCtrlEdge *> >::_Nil
+
+// GLOBAL: LEGO1 0x100f4364
+// _Tree<LegoBEWithFloat *,LegoBEWithFloat *,multiset<LegoBEWithFloat *,LegoBEWithFloatComparator,allocator<LegoBEWithFloat *> >::_Kfn,LegoBEWithFloatComparator,allocator<LegoBEWithFloat *> >::_Nil
 // clang-format on
 
 #endif // LEGOPATHCONTROLLER_H

LEGO1/lego/legoomni/include/legopathedgecontainer.h

@@ -6,17 +6,73 @@
 #include "mxtypes.h"
 
 class LegoPathBoundary;
-struct LegoUnknown100db7f4;
+struct LegoPathCtrlEdge;
 
 // SIZE 0x08
 struct LegoBoundaryEdge {
-	LegoUnknown100db7f4* m_edge;  // 0x00
+	LegoBoundaryEdge() {}
+
+	// FUNCTION: BETA10 0x100bd620
+	LegoBoundaryEdge(LegoPathCtrlEdge* p_edge, LegoPathBoundary* p_boundary)
+	{
+		m_edge = p_edge;
+		m_boundary = p_boundary;
+	}
+
+	LegoPathCtrlEdge* m_edge;     // 0x00
 	LegoPathBoundary* m_boundary; // 0x04
 
 	int operator==(LegoBoundaryEdge) const { return 0; }
 	int operator<(LegoBoundaryEdge) const { return 0; }
 };
 
+// SIZE 0x10
+struct LegoBEWithFloat {
+	LegoBEWithFloat()
+	{
+		m_edge = NULL;
+		m_boundary = NULL;
+		m_next = NULL;
+		m_unk0x0c = 0.0f;
+	}
+
+	// FUNCTION: BETA10 0x100bd9a0
+	LegoBEWithFloat(LegoPathCtrlEdge* p_edge, LegoPathBoundary* p_boundary, MxFloat p_unk0x0c)
+	{
+		m_edge = p_edge;
+		m_boundary = p_boundary;
+		m_next = NULL;
+		m_unk0x0c = p_unk0x0c;
+	}
+
+	// FUNCTION: BETA10 0x100bd9f0
+	LegoBEWithFloat(LegoPathCtrlEdge* p_edge, LegoPathBoundary* p_boundary, LegoBEWithFloat* p_next, MxFloat p_unk0x0c)
+	{
+		m_edge = p_edge;
+		m_boundary = p_boundary;
+		m_next = p_next;
+		m_unk0x0c = p_unk0x0c;
+	}
+
+	LegoPathCtrlEdge* m_edge;     // 0x00
+	LegoPathBoundary* m_boundary; // 0x04
+	LegoBEWithFloat* m_next;      // 0x08
+	MxFloat m_unk0x0c;            // 0x0c
+
+	int operator==(LegoBEWithFloat) const { return 0; }
+	int operator<(LegoBEWithFloat) const { return 0; }
+};
+
+struct LegoBEWithFloatComparator {
+	// FUNCTION: BETA10 0x100bef80
+	bool operator()(LegoBEWithFloat* const& p_a, LegoBEWithFloat* const& p_b) const
+	{
+		return p_a->m_unk0x0c < p_b->m_unk0x0c;
+	}
+};
+
+typedef multiset<LegoBEWithFloat*, LegoBEWithFloatComparator> LegoBEWithFloatSet;
+
 // SIZE 0x3c
 struct LegoPathEdgeContainer : public list<LegoBoundaryEdge> {
 	enum {
@@ -30,9 +86,10 @@ struct LegoPathEdgeContainer : public list<LegoBoundaryEdge> {
 		m_flags = 0;
 	}
 
-	void SetBit1(MxU32 p_flag)
+	// FUNCTION: BETA10 0x100bd660
+	void SetBit1(MxU32 p_set)
 	{
-		if (p_flag) {
+		if (p_set) {
 			m_flags |= c_bit1;
 		}
 		else {
@@ -40,10 +97,11 @@ struct LegoPathEdgeContainer : public list<LegoBoundaryEdge> {
 		}
 	}
 
+	// FUNCTION: BETA10 0x1001cb50
 	MxU32 GetBit1() { return m_flags & c_bit1; }
 
-	Mx3DPointFloat m_unk0x0c;     // 0x0c
+	Mx3DPointFloat m_position;    // 0x0c
-	Mx3DPointFloat m_unk0x20;     // 0x20
+	Mx3DPointFloat m_direction;   // 0x20
 	LegoPathBoundary* m_boundary; // 0x34
 	MxU8 m_flags;                 // 0x38
 };

LEGO1/lego/legoomni/src/actors/act2actor.cpp

@@ -134,8 +134,8 @@ void Act2Actor::SetWorldSpeed(MxFloat p_worldSpeed)
 // FUNCTION: BETA10 0x1000d4d6
 void Act2Actor::FUN_100192a0(undefined4 p_param)
 {
-	Mx3DPointFloat local38(0.0, 0.0, 0.0);
+	Mx3DPointFloat newPosition(0.0, 0.0, 0.0);
-	Mx3DPointFloat local4c(0.0, 0.0, 0.0);
+	Mx3DPointFloat newDirection(0.0, 0.0, 0.0);
 
 	if (m_grec) {
 		delete m_grec;
@@ -144,26 +144,25 @@ void Act2Actor::FUN_100192a0(undefined4 p_param)
 	m_grec = new LegoPathEdgeContainer();
 	assert(m_grec);
 
-	local38 = g_unk0x100f0db8[p_param].m_unk0x00;
+	newPosition = g_unk0x100f0db8[p_param].m_position;
-	local4c = g_unk0x100f0db8[p_param].m_unk0x0c;
+	newDirection = g_unk0x100f0db8[p_param].m_direction;
-
+	LegoPathBoundary* newBoundary = m_controller->GetPathBoundary(g_unk0x100f0db8[p_param].m_boundary);
-	LegoPathBoundary* otherBoundary = m_controller->GetPathBoundary(g_unk0x100f0db8[p_param].m_unk0x18);
 
 	MxResult sts = m_controller->FUN_10048310(
 		m_grec,
 		m_roi->GetWorldPosition(),
 		m_roi->GetWorldDirection(),
 		m_boundary,
-		local38,
+		newPosition,
-		local4c,
+		newDirection,
-		otherBoundary,
+		newBoundary,
-		TRUE,
+		LegoUnknown100db7f4::c_bit1,
 		NULL
 	);
 
-	assert(!sts);
+	assert(!sts); // == SUCCESS
 
-	if (sts) {
+	if (sts != SUCCESS) {
 		delete m_grec;
 		m_grec = NULL;
 	}

LEGO1/lego/legoomni/src/common/legoanimationmanager.cpp

@@ -2553,7 +2553,7 @@ MxBool LegoAnimationManager::FUN_10064120(LegoLocation::Boundary* p_boundary, Mx
 
 	while (local2c--) {
 		if (local34 != NULL) {
-			if (local34->Unknown(*boundary, LegoWEGEdge::c_bit1) && FUN_10064010(boundary, local34, destScale) &&
+			if (local34->BETA_1004a830(*boundary, LegoWEGEdge::c_bit1) && FUN_10064010(boundary, local34, destScale) &&
 				(!p_bool2 || FUN_10064010(boundary, local8, destScale))) {
 				p_boundary->m_srcScale = p_boundary->m_destScale = destScale;
 				p_boundary->m_name = boundary->GetName();

LEGO1/lego/legoomni/src/paths/legopathboundary.cpp

@@ -92,7 +92,7 @@ void LegoPathBoundary::SwitchBoundary(
 {
 	LegoUnknown100db7f4* e = p_edge;
 
-	if (p_edge->Unknown2(*p_boundary)) {
+	if (p_edge->BETA_100b53b0(*p_boundary)) {
 		LegoPathBoundary* newBoundary = (LegoPathBoundary*) p_edge->OtherFace(p_boundary);
 
 		if (newBoundary == NULL) {
@@ -102,7 +102,7 @@ void LegoPathBoundary::SwitchBoundary(
 		MxS32 local10 = 0;
 		MxU8 userNavFlag;
 
-		if (e->Unknown(*newBoundary, 1)) {
+		if (e->BETA_1004a830(*newBoundary, 1)) {
 			userNavFlag = p_actor->GetUserNavFlag();
 		}
 		else {
@@ -113,7 +113,7 @@ void LegoPathBoundary::SwitchBoundary(
 			p_edge = (LegoUnknown100db7f4*) p_edge->GetCounterclockwiseEdge(*newBoundary);
 			LegoPathBoundary* local20 = (LegoPathBoundary*) p_edge->OtherFace(newBoundary);
 
-			if (p_edge->GetMask0x03() && (userNavFlag || p_edge->Unknown(*local20, 1))) {
+			if (p_edge->GetMask0x03() && (userNavFlag || p_edge->BETA_1004a830(*local20, 1))) {
 				local10++;
 			}
 		} while (p_edge != e);
@@ -141,7 +141,7 @@ void LegoPathBoundary::SwitchBoundary(
 
 			LegoPathBoundary* local20 = (LegoPathBoundary*) p_edge->OtherFace(newBoundary);
 
-			if (p_edge->GetMask0x03() && (userNavFlag || p_edge->Unknown(*local20, 1))) {
+			if (p_edge->GetMask0x03() && (userNavFlag || p_edge->BETA_1004a830(*local20, 1))) {
 				local8--;
 			}
 		}

LEGO1/lego/legoomni/src/paths/legopathcontroller.cpp

@@ -733,22 +733,196 @@ MxResult LegoPathController::ReadVector(LegoStorage* p_storage, Mx4DPointFloat&
 	return SUCCESS;
 }
 
-// STUB: LEGO1 0x10048310
+// FUNCTION: LEGO1 0x10048310
-// STUB: BETA10 0x100b8911
+// FUNCTION: BETA10 0x100b8911
 MxResult LegoPathController::FUN_10048310(
 	LegoPathEdgeContainer* p_grec,
-	const Vector3& p_position,
+	const Vector3& p_oldPosition,
-	const Vector3& p_direction,
+	const Vector3& p_oldDirection,
-	LegoPathBoundary* p_boundary1,
+	LegoPathBoundary* p_oldBoundary,
-	const Vector3& p_param5,
+	const Vector3& p_newPosition,
-	const Vector3& p_param6,
+	const Vector3& p_newDirection,
-	LegoPathBoundary* p_boundary2,
+	LegoPathBoundary* p_newBoundary,
-	MxBool p_param8,
+	LegoU8 p_mask,
 	MxFloat* p_param9
 )
+{
+	p_grec->m_position = p_newPosition;
+	p_grec->m_direction = p_newDirection;
+	p_grec->m_boundary = p_newBoundary;
+
+	if (p_newBoundary == p_oldBoundary) {
+		p_grec->SetBit1(TRUE);
+		return SUCCESS;
+	}
+
+	list<LegoBEWithFloat> boundaryList;
+	list<LegoBEWithFloat>::iterator boundaryListIt;
+
+	LegoBEWithFloatSet boundarySet;
+	LegoBEWithFloatSet::iterator boundarySetItA;
+	LegoBEWithFloatSet::iterator boundarySetItB;
+
+	LegoPathCtrlEdgeSet pathCtrlEdgeSet(m_pfsE);
+
+	MxFloat local14 = 999999.0f;
+
+	p_grec->SetBit1(FALSE);
+
+	for (MxS32 i = 0; i < p_oldBoundary->GetNumEdges(); i++) {
+		LegoPathCtrlEdge* edge = (LegoPathCtrlEdge*) p_oldBoundary->GetEdges()[i];
+
+		if (edge->GetMask0x03()) {
+			LegoPathBoundary* otherFace = (LegoPathBoundary*) edge->OtherFace(p_oldBoundary);
+
+			if (otherFace != NULL && edge->BETA_1004a830(*otherFace, p_mask)) {
+				if (p_newBoundary == otherFace) {
+					float dist;
+					if ((dist = edge->DistanceToMidpoint(p_oldPosition) + edge->DistanceToMidpoint(p_newPosition)) <
+						local14) {
+						local14 = dist;
+						p_grec->erase(p_grec->begin(), p_grec->end());
+						p_grec->SetBit1(TRUE);
+						p_grec->push_back(LegoBoundaryEdge(edge, p_oldBoundary));
+					}
+				}
+				else {
+					boundaryList.push_back(LegoBEWithFloat(edge, p_oldBoundary, edge->DistanceToMidpoint(p_oldPosition))
+					);
+					boundarySet.insert(&boundaryList.back());
+				}
+			}
+		}
+
+		pathCtrlEdgeSet.erase(edge);
+	}
+
+	if (!p_grec->GetBit1()) {
+		while (pathCtrlEdgeSet.size() > 0) {
+			LegoBEWithFloat edgeWithFloat;
+			MxFloat local70 = 999999.0f;
+
+			boundarySetItA = boundarySetItB = boundarySet.begin();
+
+			if (boundarySetItB != boundarySet.end()) {
+				boundarySetItB++;
+			}
+
+			while (boundarySetItA != boundarySet.end()) {
+				MxU32 shouldRemove = TRUE;
+
+				LegoUnknown100db7f4* e = (*boundarySetItA)->m_edge;
+				LegoPathBoundary* b = (*boundarySetItA)->m_boundary;
+				assert(e && b);
+
+				LegoPathBoundary* bOther = (LegoPathBoundary*) e->OtherFace(b);
+				assert(bOther);
+
+				if (e->BETA_1004a830(*bOther, p_mask)) {
+					if (bOther == p_newBoundary) {
+						shouldRemove = FALSE;
+
+						LegoBEWithFloat* pfs = *boundarySetItA;
+						assert(pfs);
+
+						float dist;
+						if ((dist = pfs->m_edge->DistanceToMidpoint(p_newPosition) + pfs->m_unk0x0c) < local70) {
+							local70 = dist;
+							edgeWithFloat.m_edge = NULL;
+
+							if (dist < local14) {
+								local14 = dist;
+								p_grec->erase(p_grec->begin(), p_grec->end());
+								p_grec->SetBit1(TRUE);
+
+								do {
+									p_grec->push_front(LegoBoundaryEdge(pfs->m_edge, pfs->m_boundary));
+									pfs = pfs->m_next;
+								} while (pfs != NULL);
+							}
+						}
+					}
+					else {
+						for (MxS32 i = 0; i < bOther->GetNumEdges(); i++) {
+							LegoPathCtrlEdge* edge = (LegoPathCtrlEdge*) bOther->GetEdges()[i];
+
+							if (edge->GetMask0x03()) {
+								if (pathCtrlEdgeSet.find(edge) != pathCtrlEdgeSet.end()) {
+									shouldRemove = FALSE;
+
+									float dist;
+									if ((dist = edge->DistanceBetweenMidpoints(*e) + (*boundarySetItA)->m_unk0x0c) <
+										local70) {
+										local70 = dist;
+										edgeWithFloat = LegoBEWithFloat(edge, bOther, *boundarySetItA, dist);
+									}
+								}
+							}
+						}
+					}
+				}
+
+				if (shouldRemove) {
+					boundarySet.erase(boundarySetItA);
+				}
+
+				if (boundarySetItB != boundarySet.end()) {
+					boundarySetItA = boundarySetItB;
+					boundarySetItB++;
+				}
+				else {
+					break;
+				}
+			}
+
+			if (edgeWithFloat.m_edge != NULL) {
+				pathCtrlEdgeSet.erase(edgeWithFloat.m_edge);
+				boundaryList.push_back(edgeWithFloat);
+				boundarySet.insert(&boundaryList.back());
+			}
+			else {
+				break;
+			}
+		}
+	}
+
+	if (p_grec->GetBit1()) {
+		if (p_grec->size() > 0) {
+			LegoPathCtrlEdge* edge = p_grec->front().m_edge;
+
+			if (edge->FUN_10048c40(p_oldPosition)) {
+				p_grec->pop_front();
+			}
+		}
+
+		if (p_grec->size() > 0) {
+			LegoPathCtrlEdge* edge = p_grec->back().m_edge;
+
+			if (edge->FUN_10048c40(p_newPosition)) {
+				if (edge->OtherFace(p_grec->back().m_boundary) != NULL &&
+					edge->OtherFace(p_grec->back().m_boundary)->IsEqual(p_newBoundary)) {
+					p_grec->m_boundary = p_grec->back().m_boundary;
+					p_grec->pop_back();
+				}
+			}
+		}
+
+		if (p_param9 != NULL) {
+			*p_param9 = local14;
+		}
+
+		return SUCCESS;
+	}
+
+	return FAILURE;
+}
+
+// STUB: LEGO1 0x10048c40
+// STUB: BETA10 0x1001cc90
+undefined4 LegoPathCtrlEdge::FUN_10048c40(const Vector3&)
 {
 	// TODO
-	return SUCCESS;
+	return 0;
 }
 
 // FUNCTION: LEGO1 0x1004a240
@@ -763,8 +937,8 @@ MxS32 LegoPathController::FUN_1004a240(
 )
 {
 	if (p_grec.size() == 0) {
-		p_v1 = p_grec.m_unk0x0c;
+		p_v1 = p_grec.m_position;
-		p_v2 = p_grec.m_unk0x20;
+		p_v2 = p_grec.m_direction;
 		p_boundary = p_grec.m_boundary;
 		p_grec.SetBit1(FALSE);
 		return 1;

LEGO1/lego/sources/geom/legounkown100db7f4.h

@@ -24,15 +24,14 @@ struct LegoUnknown100db7f4 : public LegoEdge {
 	// FUNCTION: BETA10 0x100372a0
 	LegoResult FUN_1002ddc0(LegoWEEdge& p_f, Vector3& p_point)
 	{
-		if (p_f.IsEqual(*m_faceA)) {
+		if (p_f.IsEqual(m_faceA)) {
 			p_point[0] = -m_unk0x28.index_operator(0);
 			p_point[1] = -m_unk0x28.index_operator(1);
 			p_point[2] = -m_unk0x28.index_operator(2);
 		}
 		else {
 			// clang-format off
-			// FIXME: There is no * dereference in the original assertion
+			assert(p_f.IsEqual( m_faceB ));
-			assert(p_f.IsEqual( *m_faceB ));
 			// clang-format on
 			p_point = m_unk0x28;
 		}
@@ -41,16 +40,20 @@ struct LegoUnknown100db7f4 : public LegoEdge {
 	}
 
 	// FUNCTION: BETA10 0x1004a830
-	LegoU32 Unknown(LegoWEGEdge& p_face, LegoU8 p_mask)
+	LegoU32 BETA_1004a830(LegoWEGEdge& p_face, LegoU8 p_mask)
 	{
-		return (p_face.IsEqual(*m_faceB) && (m_flags & c_bit1) && (p_face.GetMask0x03() & p_mask) == p_mask) ||
+		assert(p_face.IsEqual(m_faceA) || p_face.IsEqual(m_faceB));
-			   (p_face.IsEqual(*m_faceA) && (m_flags & c_bit2) && (p_face.GetMask0x03() & p_mask) == p_mask);
+		return (p_face.IsEqual(m_faceB) && (m_flags & c_bit1) && (p_face.GetMask0x03() & p_mask) == p_mask) ||
+			   (p_face.IsEqual(m_faceA) && (m_flags & c_bit2) && (p_face.GetMask0x03() & p_mask) == p_mask);
 	}
 
 	// FUNCTION: BETA10 0x100b53b0
-	LegoU32 Unknown2(LegoWEGEdge& p_face)
+	LegoU32 BETA_100b53b0(LegoWEGEdge& p_face)
 	{
-		return (p_face.IsEqual(*m_faceA) && (m_flags & c_bit1)) || (p_face.IsEqual(*m_faceB) && (m_flags & c_bit2));
+		// clang-format off
+		assert(p_face.IsEqual( m_faceA ) || p_face.IsEqual( m_faceB ));
+		// clang-format on
+		return (p_face.IsEqual(m_faceA) && (m_flags & c_bit1)) || (p_face.IsEqual(m_faceB) && (m_flags & c_bit2));
 	}
 
 	// FUNCTION: BETA10 0x1001cbe0
@@ -64,6 +67,30 @@ struct LegoUnknown100db7f4 : public LegoEdge {
 		}
 	}
 
+	// FUNCTION: BETA10 0x100bd4a0
+	LegoFloat DistanceToMidpoint(const Vector3& p_vec)
+	{
+		Mx3DPointFloat point(*m_pointA);
+		((Vector3&) point).Add(*m_pointB);
+		((Vector3&) point).Mul(0.5f);
+		((Vector3&) point).Sub(p_vec);
+		return sqrt(point.LenSquared());
+	}
+
+	// FUNCTION: BETA10 0x100bd540
+	LegoFloat DistanceBetweenMidpoints(const LegoUnknown100db7f4& p_other)
+	{
+		Mx3DPointFloat point1(*m_pointA);
+		Mx3DPointFloat point2(*p_other.m_pointA);
+		((Vector3&) point1).Add(*m_pointB);
+		((Vector3&) point1).Mul(0.5f);
+		((Vector3&) point2).Add(*p_other.m_pointB);
+		((Vector3&) point2).Mul(0.5f);
+		((Vector3&) point1).Sub(point2);
+		return sqrt(point1.LenSquared());
+	}
+
+	// FUNCTION: BETA10 0x1001cc60
 	LegoU32 GetMask0x03() { return m_flags & (c_bit1 | c_bit2); }
 
 	// SYNTHETIC: LEGO1 0x1009a6c0

LEGO1/lego/sources/geom/legoweedge.h

@@ -22,9 +22,8 @@ class LegoWEEdge {
 	// FUNCTION: BETA10 0x1001cc30
 	LegoUnknown100db7f4** GetEdges() { return m_edges; }
 
-	// TODO: The assertion at BETA10 0x10037352 suggests that this function might take a pointer instead of a reference
 	// FUNCTION: BETA10 0x100373f0
-	LegoU32 IsEqual(LegoWEEdge& p_other) { return this == &p_other; }
+	LegoU32 IsEqual(LegoWEEdge* p_other) { return this == p_other; }
 
 	void SetEdges(LegoUnknown100db7f4** p_edges, LegoU8 p_numEdges)
 	{

LEGO1/lego/sources/geom/legowegedge.h

@@ -62,6 +62,7 @@ class LegoWEGEdge : public LegoWEEdge {
 		}
 	}
 
+	// FUNCTION: BETA10 0x1004a980
 	LegoU8 GetMask0x03() { return m_flags & (c_bit1 | c_bit2); }
 
 	// SYNTHETIC: LEGO1 0x1009a7e0

LEGO1/mxgeometry/mxgeometry3d.h

@@ -26,6 +26,7 @@ class Mx3DPointFloat : public Vector3 {
 	// FUNCTION: BETA10 0x10011600
 	Mx3DPointFloat(const Mx3DPointFloat& p_other) : Vector3(m_elements) { EqualsImpl(p_other.m_data); }
 
+	// FUNCTION: LEGO1 0x10048ed0
 	// FUNCTION: BETA10 0x100151e0
 	Mx3DPointFloat(const Vector3& p_other) : Vector3(m_elements) { EqualsImpl(p_other.m_data); }
 

LEGO1/mxstl/stlcompat.h

@@ -16,6 +16,7 @@
 #include <vector>
 using std::list;
 using std::map;
+using std::multiset;
 using std::pair;
 using std::set;
 using std::vector;

LEGO1/realtime/vector.h

@@ -286,6 +286,7 @@ class Vector3 : public Vector2 {
 	void Clear() override { memset(m_data, 0, sizeof(float) * 3); } // vtable+0x2c
 
 	// FUNCTION: LEGO1 0x10003bd0
+	// FUNCTION: BETA10 0x10011530
 	float LenSquared() const override
 	{
 		return m_data[0] * m_data[0] + m_data[1] * m_data[1] + m_data[2] * m_data[2];