Implement LegoPathController::FUN_10048310 (#1192)

* WIP

* Rename

* Fix

* More WIP

* WIP

* WIP

* Fix

* Annotations

* Add more annotations, improve match

* Raise max functions

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 local4c(0.0, 0.0, 0.0);
+	Mx3DPointFloat newPosition(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;
-	local4c = g_unk0x100f0db8[p_param].m_unk0x0c;
-
-	LegoPathBoundary* otherBoundary = m_controller->GetPathBoundary(g_unk0x100f0db8[p_param].m_unk0x18);
+	newPosition = g_unk0x100f0db8[p_param].m_position;
+	newDirection = g_unk0x100f0db8[p_param].m_direction;
+	LegoPathBoundary* newBoundary = m_controller->GetPathBoundary(g_unk0x100f0db8[p_param].m_boundary);
 
 	MxResult sts = m_controller->FUN_10048310(
 		m_grec,
 		m_roi->GetWorldPosition(),
 		m_roi->GetWorldDirection(),
 		m_boundary,
-		local38,
-		local4c,
-		otherBoundary,
-		TRUE,
+		newPosition,
+		newDirection,
+		newBoundary,
+		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
-// STUB: BETA10 0x100b8911
+// FUNCTION: LEGO1 0x10048310
+// FUNCTION: BETA10 0x100b8911
 MxResult LegoPathController::FUN_10048310(
 	LegoPathEdgeContainer* p_grec,
-	const Vector3& p_position,
-	const Vector3& p_direction,
-	LegoPathBoundary* p_boundary1,
-	const Vector3& p_param5,
-	const Vector3& p_param6,
-	LegoPathBoundary* p_boundary2,
-	MxBool p_param8,
+	const Vector3& p_oldPosition,
+	const Vector3& p_oldDirection,
+	LegoPathBoundary* p_oldBoundary,
+	const Vector3& p_newPosition,
+	const Vector3& p_newDirection,
+	LegoPathBoundary* p_newBoundary,
+	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_v2 = p_grec.m_unk0x20;
+		p_v1 = p_grec.m_position;
+		p_v2 = p_grec.m_direction;
 		p_boundary = p_grec.m_boundary;
 		p_grec.SetBit1(FALSE);
 		return 1;