void AiRoutine::ResumeSchedule() { Transform* trans = this->GetObject()->GetComponent<Transform>(); AiMarker prevMark; prevMark.WaitTime = this->waitTimer; prevMark.Position = trans->GetPositionWorld(); prevMark.Orientation = trans->GetOrientationWorld(); AiMarker newMark = this->markers[this->currentMarker]; glm::vec3 oldForward = QuaternionForwardVector(prevMark.Orientation); glm::vec3 newForward = QuaternionForwardVector(newMark.Orientation); float dist = glm::distance(prevMark.Position, newMark.Position); // Don't use glm's vector angle function because it never returns an angle greater than PI/2 float angle = acos(glm::clamp(glm::dot(oldForward, newForward), -1.0f, 1.0f)); GridNavigator* gNav = this->GetObject()->GetComponent<GridNavigator>(); if (dist > ROUNDING_ERROR) { this->currentMarkerType = AI_MARKER_WALK; gNav->SetDestination(newMark.Position); this->unit->SwitchActionState(UNIT_ACTION_STATE_WALKING); } else if (angle > ROUNDING_ERROR) { this->currentMarkerType = AI_MARKER_LOOK; gNav->SetLookTarget(newMark.Orientation); this->unit->SwitchActionState(UNIT_ACTION_STATE_IDLE); } else { this->currentMarkerType = AI_MARKER_WAIT; this->waitTimer = newMark.WaitTime; this->unit->SwitchActionState(UNIT_ACTION_STATE_IDLE); } }