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);
}
}
