3 files changed, 268 insertions, 2 deletions

diff --git a/src/control/CarCtrl.cpp b/src/control/CarCtrl.cpp
index bcf94479..1f298fd5 100644
--- a/src/control/CarCtrl.cpp
+++ b/src/control/CarCtrl.cpp
@@ -38,6 +38,13 @@
 #define SAFE_DISTANCE_TO_PED 3.0f
 #define INFINITE_Z 1000000000.0f
 
+#define VEHICLE_HEIGHT_DIFF_TO_CONSIDER_WEAVING 4.0f
+#define PED_HEIGHT_DIFF_TO_CONSIDER_WEAVING 4.0f
+#define OBJECT_HEIGHT_DIFF_TO_CONSIDER_WEAVING 8.0f
+#define WIDTH_COEF_TO_WEAVE_SAFELY 1.2f
+#define OBJECT_WIDTH_TO_WEAVE 0.3f
+#define PED_WIDTH_TO_WEAVE 0.8f
+
 int &CCarCtrl::NumLawEnforcerCars = *(int*)0x8F1B38;
 int &CCarCtrl::NumAmbulancesOnDuty = *(int*)0x885BB0;
 int &CCarCtrl::NumFiretrucksOnDuty = *(int*)0x9411F0;
@@ -1173,6 +1180,245 @@ float CCarCtrl::TestCollisionBetween2MovingRects(CVehicle* pVehicleA, CVehicle*
 }
 #endif
 
+float CCarCtrl::FindAngleToWeaveThroughTraffic(CVehicle* pVehicle, CPhysical* pTarget, float angleToTarget, float angleForward)
+{
+	float distanceToTest = min(2.0f, pVehicle->GetMoveSpeed().Magnitude2D() * 2.5f + 1.0f) * 12.0f;
+	float left = pVehicle->GetPosition().x - distanceToTest;
+	float right = pVehicle->GetPosition().x + distanceToTest;
+	float top = pVehicle->GetPosition().y - distanceToTest;
+	float bottom = pVehicle->GetPosition().y + distanceToTest;
+	int xstart = max(0, CWorld::GetSectorIndexX(left));
+	int xend = min(NUMSECTORS_X - 1, CWorld::GetSectorIndexX(right));
+	int ystart = max(0, CWorld::GetSectorIndexY(top));
+	int yend = min(NUMSECTORS_Y - 1, CWorld::GetSectorIndexY(bottom));
+	assert(xstart <= xend);
+	assert(ystart <= yend);
+
+	float angleToWeaveLeft = angleToTarget;
+	float angleToWeaveRight = angleToTarget;
+
+	CWorld::AdvanceCurrentScanCode();
+
+	float angleToWeaveLeftLastIteration = -9999.9f;
+	float angleToWeaveRightLastIteration = -9999.9f;
+
+	while (angleToWeaveLeft != angleToWeaveLeftLastIteration ||
+		   angleToWeaveRight != angleToWeaveRightLastIteration){
+		angleToWeaveLeftLastIteration = angleToWeaveLeft;
+		angleToWeaveRightLastIteration = angleToWeaveRight;
+		for (int y = ystart; y <= yend; y++) {
+			for (int x = xstart; x <= xend; x++) {
+				CSector* s = CWorld::GetSector(x, y);
+				WeaveThroughCarsSectorList(s->m_lists[ENTITYLIST_VEHICLES], pVehicle, pTarget,
+					left, top, right, bottom, &angleToWeaveLeft, &angleToWeaveRight);
+				WeaveThroughCarsSectorList(s->m_lists[ENTITYLIST_VEHICLES_OVERLAP], pVehicle, pTarget,
+					left, top, right, bottom, &angleToWeaveLeft, &angleToWeaveRight);
+				WeaveThroughPedsSectorList(s->m_lists[ENTITYLIST_PEDS], pVehicle, pTarget,
+					left, top, right, bottom, &angleToWeaveLeft, &angleToWeaveRight);
+				WeaveThroughPedsSectorList(s->m_lists[ENTITYLIST_PEDS_OVERLAP], pVehicle, pTarget,
+					left, top, right, bottom, &angleToWeaveLeft, &angleToWeaveRight);
+				WeaveThroughObjectsSectorList(s->m_lists[ENTITYLIST_OBJECTS], pVehicle,
+					left, top, right, bottom, &angleToWeaveLeft, &angleToWeaveRight);
+				WeaveThroughObjectsSectorList(s->m_lists[ENTITYLIST_OBJECTS_OVERLAP], pVehicle,
+					left, top, right, bottom, &angleToWeaveLeft, &angleToWeaveRight);
+			}
+		}
+	}
+	float angleDiffFromActualToTarget = LimitRadianAngle(angleForward - angleToTarget);
+	float angleToBisectActualToTarget = LimitRadianAngle(angleToTarget + angleDiffFromActualToTarget / 2);
+	float angleDiffLeft = LimitRadianAngle(angleToWeaveLeft - angleToBisectActualToTarget);
+	angleDiffLeft = ABS(angleDiffLeft);
+	float angleDiffRight = LimitRadianAngle(angleToWeaveRight - angleToBisectActualToTarget);
+	angleDiffRight = ABS(angleDiffRight);
+	if (angleDiffLeft > HALFPI && angleDiffRight > HALFPI)
+		return angleToBisectActualToTarget;
+	if (ABS(angleDiffLeft - angleDiffRight) < 0.08f)
+		return angleToWeaveRight;
+	return angleDiffLeft < angleDiffRight ? angleToWeaveLeft : angleToWeaveRight;
+}
+
+void CCarCtrl::WeaveThroughCarsSectorList(CPtrList& lst, CVehicle* pVehicle, CPhysical* pTarget, float x_inf, float y_inf, float x_sup, float y_sup, float* pAngleToWeaveLeft, float* pAngleToWeaveRight)
+{
+	for (CPtrNode* pNode = lst.first; pNode != nil; pNode = pNode->next) {
+		CVehicle* pTestVehicle = (CVehicle*)pNode->item;
+		if (pTestVehicle->m_scanCode == CWorld::GetCurrentScanCode())
+			continue;
+		if (!pTestVehicle->bUsesCollision)
+			continue;
+		if (pTestVehicle == pTarget)
+			continue;
+		pTestVehicle->m_scanCode = CWorld::GetCurrentScanCode();
+		if (pTestVehicle->GetBoundCentre().x < x_inf || pTestVehicle->GetBoundCentre().x > x_sup)
+			continue;
+		if (pTestVehicle->GetBoundCentre().y < y_inf || pTestVehicle->GetBoundCentre().y > y_sup)
+			continue;
+		if (Abs(pTestVehicle->GetPosition().z - pVehicle->GetPosition().z) >= VEHICLE_HEIGHT_DIFF_TO_CONSIDER_WEAVING)
+			continue;
+		if (pTestVehicle != pVehicle)
+			WeaveForOtherCar(pTestVehicle, pVehicle, pAngleToWeaveLeft, pAngleToWeaveRight);
+	}
+}
+
+void CCarCtrl::WeaveForOtherCar(CEntity* pOtherEntity, CVehicle* pVehicle, float* pAngleToWeaveLeft, float* pAngleToWeaveRight)
+{
+	if (pVehicle->AutoPilot.m_nCarMission == MISSION_RAMPLAYER_CLOSE && pOtherEntity == FindPlayerVehicle())
+		return;
+	if (pVehicle->AutoPilot.m_nCarMission == MISSION_RAMCAR_CLOSE && pOtherEntity == pVehicle->AutoPilot.m_pTargetCar)
+		return;
+	CVehicle* pOtherCar = (CVehicle*)pOtherEntity;
+	CVector2D vecDiff = pOtherCar->GetPosition() - pVehicle->GetPosition();
+	float angleBetweenVehicles = CGeneral::GetATanOfXY(vecDiff.x, vecDiff.y);
+	float distance = vecDiff.Magnitude();
+	if (distance < 1.0f)
+		return;
+	if (DotProduct2D(pVehicle->GetMoveSpeed() - pOtherCar->GetMoveSpeed(), vecDiff) * 110.0f -
+	  pOtherCar->GetModelInfo()->GetColModel()->boundingSphere.radius -
+	  pVehicle->GetModelInfo()->GetColModel()->boundingSphere.radius < distance)
+		return;
+	CVector2D forward = pVehicle->GetForward();
+	forward.Normalise();
+	float forwardAngle = CGeneral::GetATanOfXY(forward.x, forward.y);
+	float angleDiff = angleBetweenVehicles - forwardAngle;
+	float lenProjection = ABS(pOtherCar->GetColModel()->boundingBox.max.y * sin(angleDiff));
+	float widthProjection = ABS(pOtherCar->GetColModel()->boundingBox.max.x * cos(angleDiff));
+	float lengthToEvade = (2 * (lenProjection + widthProjection) + WIDTH_COEF_TO_WEAVE_SAFELY * 2 * pVehicle->GetColModel()->boundingBox.max.x) / distance;
+	float diffToLeftAngle = LimitRadianAngle(angleBetweenVehicles - *pAngleToWeaveLeft);
+	diffToLeftAngle = ABS(diffToLeftAngle);
+	float angleToWeave = lengthToEvade / 2;
+	if (diffToLeftAngle < angleToWeave){
+		*pAngleToWeaveLeft = angleBetweenVehicles - angleToWeave;
+		while (*pAngleToWeaveLeft < -PI)
+			*pAngleToWeaveLeft += TWOPI;
+	}
+	float diffToRightAngle = LimitRadianAngle(angleBetweenVehicles - *pAngleToWeaveRight);
+	diffToRightAngle = ABS(diffToRightAngle);
+	if (diffToRightAngle < angleToWeave){
+		*pAngleToWeaveRight = angleBetweenVehicles + angleToWeave;
+		while (*pAngleToWeaveRight > PI)
+			*pAngleToWeaveRight -= TWOPI;
+	}
+}
+
+void CCarCtrl::WeaveThroughPedsSectorList(CPtrList& lst, CVehicle* pVehicle, CPhysical* pTarget, float x_inf, float y_inf, float x_sup, float y_sup, float* pAngleToWeaveLeft, float* pAngleToWeaveRight)
+{
+	for (CPtrNode* pNode = lst.first; pNode != nil; pNode = pNode->next) {
+		CPed* pPed = (CPed*)pNode->item;
+		if (pPed->m_scanCode == CWorld::GetCurrentScanCode())
+			continue;
+		if (!pPed->bUsesCollision)
+			continue;
+		if (pPed == pTarget)
+			continue;
+		pPed->m_scanCode = CWorld::GetCurrentScanCode();
+		if (pPed->GetPosition().x < x_inf || pPed->GetPosition().x > x_sup)
+			continue;
+		if (pPed->GetPosition().y < y_inf || pPed->GetPosition().y > y_sup)
+			continue;
+		if (Abs(pPed->GetPosition().z - pPed->GetPosition().z) >= PED_HEIGHT_DIFF_TO_CONSIDER_WEAVING)
+			continue;
+		if (pPed->m_pCurSurface != pVehicle)
+			WeaveForPed(pPed, pVehicle, pAngleToWeaveLeft, pAngleToWeaveRight);
+	}
+
+}
+void CCarCtrl::WeaveForPed(CEntity* pOtherEntity, CVehicle* pVehicle, float* pAngleToWeaveLeft, float* pAngleToWeaveRight)
+{
+	if (pVehicle->AutoPilot.m_nCarMission == MISSION_RAMPLAYER_CLOSE && pOtherEntity == FindPlayerPed())
+		return;
+	CPed* pPed = (CPed*)pOtherEntity;
+	CVector2D vecDiff = pPed->GetPosition() - pVehicle->GetPosition();
+	float angleBetweenVehicleAndPed = CGeneral::GetATanOfXY(vecDiff.x, vecDiff.y);
+	float distance = vecDiff.Magnitude();
+	float lengthToEvade = (WIDTH_COEF_TO_WEAVE_SAFELY * 2 * pVehicle->GetColModel()->boundingBox.max.x + PED_WIDTH_TO_WEAVE) / distance;
+	float diffToLeftAngle = LimitRadianAngle(angleBetweenVehicleAndPed - *pAngleToWeaveLeft);
+	diffToLeftAngle = ABS(diffToLeftAngle);
+	float angleToWeave = lengthToEvade / 2;
+	if (diffToLeftAngle < angleToWeave) {
+		*pAngleToWeaveLeft = angleBetweenVehicleAndPed - angleToWeave;
+		while (*pAngleToWeaveLeft < -PI)
+			*pAngleToWeaveLeft += TWOPI;
+	}
+	float diffToRightAngle = LimitRadianAngle(angleBetweenVehicleAndPed - *pAngleToWeaveRight);
+	diffToRightAngle = ABS(diffToRightAngle);
+	if (diffToRightAngle < angleToWeave) {
+		*pAngleToWeaveRight = angleBetweenVehicleAndPed + angleToWeave;
+		while (*pAngleToWeaveRight > PI)
+			*pAngleToWeaveRight -= TWOPI;
+	}
+}
+
+void CCarCtrl::WeaveThroughObjectsSectorList(CPtrList& lst, CVehicle* pVehicle, float x_inf, float y_inf, float x_sup, float y_sup, float* pAngleToWeaveLeft, float* pAngleToWeaveRight)
+{
+	for (CPtrNode* pNode = lst.first; pNode != nil; pNode = pNode->next) {
+		CObject* pObject = (CObject*)pNode->item;
+		if (pObject->m_scanCode == CWorld::GetCurrentScanCode())
+			continue;
+		if (!pObject->bUsesCollision)
+			continue;
+		pObject->m_scanCode = CWorld::GetCurrentScanCode();
+		if (pObject->GetPosition().x < x_inf || pObject->GetPosition().x > x_sup)
+			continue;
+		if (pObject->GetPosition().y < y_inf || pObject->GetPosition().y > y_sup)
+			continue;
+		if (Abs(pObject->GetPosition().z - pVehicle->GetPosition().z) >= OBJECT_HEIGHT_DIFF_TO_CONSIDER_WEAVING)
+			continue;
+		if (pObject->GetUp().z > 0.9f)
+			WeaveForObject(pObject, pVehicle, pAngleToWeaveLeft, pAngleToWeaveRight);
+	}
+}
+
+void CCarCtrl::WeaveForObject(CEntity* pOtherEntity, CVehicle* pVehicle, float* pAngleToWeaveLeft, float* pAngleToWeaveRight)
+{
+	float rightCoef;
+	float forwardCoef;
+	if (pOtherEntity->GetModelIndex() == MI_TRAFFICLIGHTS){
+		rightCoef = 2.957f;
+		forwardCoef = 0.147f;
+	}else if (pOtherEntity->GetModelIndex() == MI_SINGLESTREETLIGHTS1){
+		rightCoef = 0.744f;
+		forwardCoef = 0.0f;
+	}else if (pOtherEntity->GetModelIndex() == MI_SINGLESTREETLIGHTS2){
+		rightCoef = 0.043f;
+		forwardCoef = 0.0f;
+	}else if (pOtherEntity->GetModelIndex() == MI_SINGLESTREETLIGHTS3){
+		rightCoef = 1.143f;
+		forwardCoef = 0.145f;
+	}else if (pOtherEntity->GetModelIndex() == MI_DOUBLESTREETLIGHTS){
+		rightCoef = 0.0f;
+		forwardCoef = -0.048f;
+	}else if (IsTreeModel(pOtherEntity->GetModelIndex())){
+		rightCoef = 0.0f;
+		forwardCoef = 0.0f;
+	}else if (pOtherEntity->GetModelIndex() == MI_STREETLAMP1 || pOtherEntity->GetModelIndex() == MI_STREETLAMP2){
+		rightCoef = 0.0f;
+		forwardCoef = 0.0f;
+	}else
+		return;
+	CObject* pObject = (CObject*)pOtherEntity;
+	CVector2D vecDiff = pObject->GetPosition() +
+		rightCoef * pObject->GetRight() +
+		forwardCoef * pObject->GetForward() -
+		pVehicle->GetPosition();
+	float angleBetweenVehicleAndObject = CGeneral::GetATanOfXY(vecDiff.x, vecDiff.y);
+	float distance = vecDiff.Magnitude();
+	float lengthToEvade = (WIDTH_COEF_TO_WEAVE_SAFELY * 2 * pVehicle->GetColModel()->boundingBox.max.x + OBJECT_WIDTH_TO_WEAVE) / distance;
+	float diffToLeftAngle = LimitRadianAngle(angleBetweenVehicleAndObject - *pAngleToWeaveLeft);
+	diffToLeftAngle = ABS(diffToLeftAngle);
+	float angleToWeave = lengthToEvade / 2;
+	if (diffToLeftAngle < angleToWeave) {
+		*pAngleToWeaveLeft = angleBetweenVehicleAndObject - angleToWeave;
+		while (*pAngleToWeaveLeft < -PI)
+			*pAngleToWeaveLeft += TWOPI;
+	}
+	float diffToRightAngle = LimitRadianAngle(angleBetweenVehicleAndObject - *pAngleToWeaveRight);
+	diffToRightAngle = ABS(diffToRightAngle);
+	if (diffToRightAngle < angleToWeave) {
+		*pAngleToWeaveRight = angleBetweenVehicleAndObject + angleToWeave;
+		while (*pAngleToWeaveRight > PI)
+			*pAngleToWeaveRight -= TWOPI;
+	}
+}
+
 bool
 CCarCtrl::MapCouldMoveInThisArea(float x, float y)
 {
@@ -1187,4 +1433,5 @@ InjectHook(0x417EC0, &CCarCtrl::ChooseModel, PATCH_JUMP);
 InjectHook(0x418320, &CCarCtrl::RemoveDistantCars, PATCH_JUMP);
 InjectHook(0x418430, &CCarCtrl::PossiblyRemoveVehicle, PATCH_JUMP);
 InjectHook(0x418C10, &CCarCtrl::FindMaximumSpeedForThisCarInTraffic, PATCH_JUMP);
+InjectHook(0x41A590, &CCarCtrl::FindAngleToWeaveThroughTraffic, PATCH_JUMP);
 ENDPATCHES
diff --git a/src/control/CarCtrl.h b/src/control/CarCtrl.h
index faf78f00..77982ba6 100644
--- a/src/control/CarCtrl.h
+++ b/src/control/CarCtrl.h
@@ -64,6 +64,13 @@ public:
 	static void Init(void);
 	static void SlowCarDownForOtherCar(CEntity*, CVehicle*, float*, float);
 	static float TestCollisionBetween2MovingRects(CVehicle*, CVehicle*, float, float, CVector*, CVector*, uint8);
+	static float FindAngleToWeaveThroughTraffic(CVehicle*, CPhysical*, float, float);
+	static void WeaveThroughCarsSectorList(CPtrList&, CVehicle*, CPhysical*, float, float, float, float, float*, float*);
+	static void WeaveForOtherCar(CEntity*, CVehicle*, float*, float*);
+	static void WeaveThroughPedsSectorList(CPtrList&, CVehicle*, CPhysical*, float, float, float, float, float*, float*);
+	static void WeaveForPed(CEntity*, CVehicle*, float*, float*);
+	static void WeaveThroughObjectsSectorList(CPtrList&, CVehicle*, float, float, float, float, float*, float*);
+	static void WeaveForObject(CEntity*, CVehicle*, float*, float*);
 
 	static float GetOffsetOfLaneFromCenterOfRoad(int8 lane, CCarPathLink* pLink)
 	{
@@ -77,6 +84,15 @@ public:
 		return (float)timeInCurve / pVehicle->AutoPilot.m_nTimeToSpendOnCurrentCurve;
 	}
 
+	static float LimitRadianAngle(float angle)
+	{
+		while (angle < -PI)
+			angle += TWOPI;
+		while (angle > PI)
+			angle -= TWOPI;
+		return angle;
+	}
+
 	static int32 &NumLawEnforcerCars;
 	static int32 &NumAmbulancesOnDuty;
 	static int32 &NumFiretrucksOnDuty;
diff --git a/src/control/Script.cpp b/src/control/Script.cpp
index c81cd050..885d1d47 100644
--- a/src/control/Script.cpp
+++ b/src/control/Script.cpp
@@ -3493,12 +3493,15 @@ int8 CRunningScript::ProcessCommandsFrom300To399(int32 command)
 		default:
 			break;
 		}
+#ifdef FIX_BUGS
+		/* BUG: if audio is not initialized, this object will not be freed. */
+		if (!DMAudio.IsAudioInitialised())
+			return 0;
+#endif
 		cAudioScriptObject* obj = new cAudioScriptObject();
 		obj->Posn = *(CVector*)&ScriptParams[0];
 		obj->AudioId = ScriptParams[3];
 		obj->AudioEntity = AEHANDLE_NONE;
-		/* BUG: if audio is not initialized, this object will not be freed. */
-		/* Issue needs to be addressed in CreateOneShotScriptObject. */
 		DMAudio.CreateOneShotScriptObject(obj);
 		return 0;
 	}