diff --git a/src/core/re3.cpp b/src/core/re3.cpp
index 672cca98..b9cfc34d 100644
--- a/src/core/re3.cpp
+++ b/src/core/re3.cpp
@@ -47,6 +47,7 @@
 #include "CarCtrl.h"
 #include "Population.h"
 #include "IniFile.h"
+#include "Zones.h"
 
 #include "crossplatform.h"
 
@@ -775,14 +776,14 @@ FixCar(void)
 static void
 TeleportToWaypoint(void)
 {
-	CStreaming::LoadScene(CRadar::TargetMarkerPos);
-	CStreaming::LoadSceneCollision(CRadar::TargetMarkerPos);
-	if (FindPlayerVehicle()) {
-		if (CRadar::TargetMarkerId != -1)
-			FindPlayerVehicle()->Teleport(CRadar::TargetMarkerPos + CVector(0.0f, 0.0f, CWorld::FindGroundZForCoord(CRadar::TargetMarkerPos.x, CRadar::TargetMarkerPos.y) + FindPlayerVehicle()->GetColModel()->boundingSphere.radius));
-	} else
-		if(CRadar::TargetMarkerId != -1)
-			FindPlayerPed()->Teleport(CRadar::TargetMarkerPos + CVector(0.0f, 0.0f, CWorld::FindGroundZForCoord(CRadar::TargetMarkerPos.x, CRadar::TargetMarkerPos.y) + FEET_OFFSET));
+	if (CRadar::TargetMarkerId == -1)
+		return;
+	CEntity* pEntityToTeleport = FindPlayerEntity();
+	CVector vNewPos = CRadar::TargetMarkerPos;
+	CStreaming::LoadScene(vNewPos);
+	CStreaming::LoadSceneCollision(vNewPos);
+	vNewPos.z = CWorld::FindGroundZForCoord(vNewPos.x, vNewPos.y) + pEntityToTeleport->GetDistanceFromCentreOfMassToBaseOfModel();
+	pEntityToTeleport->Teleport(vNewPos);
 }
 #endif