diff --git a/src/core/Radar.cpp b/src/core/Radar.cpp
index 7e3a75b3..4e15559d 100644
--- a/src/core/Radar.cpp
+++ b/src/core/Radar.cpp
@@ -1504,7 +1504,7 @@ CRadar::ToggleTargetMarker(float x, float y)
 		ms_RadarTrace[nextBlip].m_bDim = 0;
 		ms_RadarTrace[nextBlip].m_bInUse = 1;
 		ms_RadarTrace[nextBlip].m_Radius = 1.0f;
-		CVector pos(x, y, 0.0f/*CWorld::FindGroundZForCoord(x,y)*/);
+		CVector pos(x, y, 0.0f);
 		TargetMarkerPos = pos;
 		ms_RadarTrace[nextBlip].m_vec2DPos = pos;
 		ms_RadarTrace[nextBlip].m_vecPos = pos;
diff --git a/src/core/re3.cpp b/src/core/re3.cpp
index d3b1f266..672cca98 100644
--- a/src/core/re3.cpp
+++ b/src/core/re3.cpp
@@ -775,12 +775,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, FindPlayerVehicle()->GetColModel()->boundingSphere.center.z));
+			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, FEET_OFFSET));
+			FindPlayerPed()->Teleport(CRadar::TargetMarkerPos + CVector(0.0f, 0.0f, CWorld::FindGroundZForCoord(CRadar::TargetMarkerPos.x, CRadar::TargetMarkerPos.y) + FEET_OFFSET));
 }
 #endif