what wrong here? i just want to make smooth turn when point is behind (or just not in frond of agent): void VictoriaPhysics::UpdateMainAgent(float dt) { static glm::vec3 m_AgentOrientation = glm::vec3(1, 0, 0); static float stuckTime = 0; static bool m_IsTurning = false; static glm::vec3 m_RealDestination; static glm::vec3 m_TurnPoint; static const float m_TurnRadius = 4.0f; static const float m_TurnAngleThreshold = 90.0f; static const float m_TurnDegreeStep = 120.0f; if (m_Crowd && m_IsMoving) { dtCrowdAgent* ag = m_Crowd->getEditableAgent(m_MainActorId); if (ag && ag->active) { glm::vec3 currentPos(ag->npos[0], ag->npos[1], ag->npos[2]); glm::vec3 currentVel(ag->vel[0], ag->vel[1], ag->vel[2]); float polyPickExt[3] = { 0.5f, 2.0f, 0.5f }; if (glm::length2(currentVel) > 0.001f) { m_AgentOrientation = glm::normalize(currentVel); } ag->params.separationWeight = 0.01f; ag->params.maxSpeed = 5.0f; ag->params.maxAcceleration = 40.0f; ag->params.collisionQueryRange = ag->params.radius * 1.0f; dtQueryFilter filter; filter.setIncludeFlags(SAMPLE_POLYFLAGS_WALK ^ SAMPLE_POLYFLAGS_JUMP); filter.setExcludeFlags(SAMPLE_POLYFLAGS_SWIM); filter.setAreaCost(SAMPLE_POLYAREA_WATER, 65534); if (glm::length2(m_PathFindingEnd) > 0.001f) { glm::vec3 targetPos(m_PathFindingEnd); glm::vec3 toTarget = targetPos - currentPos; toTarget.y = 0; // Ignore vertical difference float angle = glm::degrees(std::acos(glm::dot(m_AgentOrientation, glm::normalize(toTarget)))); auto findTurnPoint = [&](const glm::vec3& realDest) -> glm::vec3 { glm::vec3 direction = realDest - currentPos; glm::vec3 cross = glm::cross(m_AgentOrientation, direction); glm::vec3 targetPos; if (cross.y < 0) { // Turn left targetPos = currentPos - glm::cross(m_AgentOrientation, glm::vec3(0, 1, 0)) * m_TurnRadius - glm::cross(m_AgentOrientation, glm::vec3(0, 1, 0)) * (m_TurnRadius * std::cos(glm::radians(180.0f - m_TurnDegreeStep))) + m_AgentOrientation * (m_TurnRadius * std::sin(glm::radians(180.0f - m_TurnDegreeStep))); } else { // Turn right targetPos = currentPos + glm::cross(m_AgentOrientation, glm::vec3(0, 1, 0)) * m_TurnRadius + glm::cross(m_AgentOrientation, glm::vec3(0, 1, 0)) * (m_TurnRadius * std::cos(glm::radians(m_TurnDegreeStep))) + m_AgentOrientation * (m_TurnRadius * std::sin(glm::radians(m_TurnDegreeStep))); } // Find nearest valid position on navigation mesh dtPolyRef nearestRef; float nearestPos[3]; m_NavMeshBuilder->GetNavMeshQuery()->findNearestPoly(&targetPos.x, polyPickExt, &filter, &nearestRef, nearestPos); return glm::vec3(nearestPos[0], nearestPos[1], nearestPos[2]); }; auto handleTurning = [&]() { if (glm::distance(currentPos, m_TurnPoint) <= ag->params.radius * 1.2f) { glm::vec3 toRealDest = m_RealDestination - currentPos; toRealDest.y = 0; float angleToReal = glm::degrees(std::acos(glm::dot(m_AgentOrientation, glm::normalize(toRealDest)))); if (angleToReal > m_TurnAngleThreshold) { m_TurnPoint = findTurnPoint(m_RealDestination); } else { m_IsTurning = false; float pos[3] = { m_RealDestination.x, m_RealDestination.y, m_RealDestination.z }; dtPolyRef nearestRef; float nearestPos[3]; m_NavMeshBuilder->GetNavMeshQuery()->findNearestPoly(pos, polyPickExt, &filter, &nearestRef, nearestPos); m_Crowd->requestMoveTarget(m_MainActorId, nearestRef, nearestPos); return; } } float pos[3] = { m_TurnPoint.x, m_TurnPoint.y, m_TurnPoint.z }; dtPolyRef nearestRef; float nearestPos[3]; m_NavMeshBuilder->GetNavMeshQuery()->findNearestPoly(pos, polyPickExt, &filter, &nearestRef, nearestPos); m_Crowd->requestMoveTarget(m_MainActorId, nearestRef, nearestPos); }; if (angle > m_TurnAngleThreshold && !m_IsTurning) { m_RealDestination = targetPos; m_IsTurning = true; m_TurnPoint = findTurnPoint(m_RealDestination); } if (m_IsTurning) { handleTurning(); } else { // Normal movement float pos[3] = { targetPos.x, targetPos.y, targetPos.z }; dtPolyRef nearestRef; float nearestPos[3]; m_NavMeshBuilder->GetNavMeshQuery()->findNearestPoly(pos, polyPickExt, &filter, &nearestRef, nearestPos); if (stuckTime > 1.0f) { float randomOffset = 0.5f; nearestPos[0] += ((float)rand() / RAND_MAX * 2 - 1) * randomOffset; nearestPos[2] += ((float)rand() / RAND_MAX * 2 - 1) * randomOffset; } if (m_Crowd->requestMoveTarget(m_MainActorId, nearestRef, nearestPos)) { // Movement request successful } } } else { m_IsMoving = false; } } } if (m_Crowd) { m_Crowd->update(dt / 1000.0f, nullptr); } }