PositionPath.cpp

#include "PositionPath.h"

PositionPath::PositionPath ()
{
        Initialize ();
}

PositionPath::~PositionPath ()
{
        DeletePath ();
}

void PositionPath::Initialize ()
{
        is_constant = false;
        has_dynamic_rotation = false;
        has_static_rotation = false;
        first_node = NULL;
        current_node = NULL;
        not_complete = false;                   // empty path is considered to be complete
        node_pointer = NULL;

        constant_position.x = 0.0f;
        constant_position.y = 0.0f;
        constant_position.z = 0.0f;
        constant_position.roll = 0.0f;
        constant_position.pitch = 0.0f;
        constant_position.yaw = 0.0f;

        translation_box.xmin = 0.0f;
        translation_box.xmax = 0.0f;
        translation_box.ymin = 0.0f;
        translation_box.ymax = 0.0f;
        translation_box.zmin = 0.0f;
        translation_box.zmax = 0.0f;
}

void PositionPath::SetConstantPosition (float x, float y, float z, float roll, float pitch, float yaw)
{
        DeletePath ();

        constant_position.x = x;
        constant_position.y = y;
        constant_position.z = z;
        constant_position.roll = roll;
        constant_position.pitch = pitch;
        constant_position.yaw = yaw;

        translation_box.xmin = x;
        translation_box.xmax = x;
        translation_box.ymin = y;
        translation_box.ymax = y;
        translation_box.zmin = z;
        translation_box.zmax = z;

        is_constant = true;
        has_dynamic_rotation = false;
        if (roll == 0.0f && pitch == 0.0f && yaw == 0.0f)
        {has_static_rotation = false;}
        else
        {has_static_rotation = true;}
}

void PositionPath::AddCheckpoint (float x, float y, float z, float roll, float pitch, float yaw)
{
        PathNode * temp;

        if (not_complete)
        {
                // update end position and create new node with the same start position
                current_node->end_position.x = x;
                current_node->end_position.y = y;
                current_node->end_position.z = z;
                current_node->end_position.roll = roll;
                current_node->end_position.pitch = pitch;
                current_node->end_position.yaw = yaw;

                if (x > translation_box.xmax) translation_box.xmax = x;
                if (x < translation_box.xmin) translation_box.xmin = x;
                if (y > translation_box.ymax) translation_box.ymax = y;
                if (y < translation_box.ymin) translation_box.ymin = y;
                if (z > translation_box.zmax) translation_box.zmax = z;
                if (z < translation_box.zmin) translation_box.zmin = z;

                if (current_node->start_position.roll != current_node->end_position.roll ||
                        current_node->start_position.pitch != current_node->end_position.pitch ||
                        current_node->start_position.yaw != current_node->end_position.yaw)
                {
                        has_dynamic_rotation = true;
                        has_static_rotation = true;
                }

                // we may have to update the translation box under certain circumstances
                ConservativeUpdate (current_node);

                temp = current_node;

                current_node->next = new PathNode;
                current_node->next->start_time = current_node->end_time;
                current_node->next->start_position = current_node->end_position;
                current_node = current_node->next;
                current_node->next = NULL;
                current_node->previous = temp;
                current_node->end_time = current_node->start_time + 1000;// just make sure there is a value
        }
        else
        {printf ("Path Construction: You have to start a new path before adding a checkpoint (AddCheckpoint)\n"); return;}
}

void PositionPath::StartPath (int offset, float x, float y, float z, float roll, float pitch, float yaw)
{
        PathNode * temp;

        if (not_complete)
        {printf ("Path Construction: You have to complete the current path before starting a new one (StartPath)\n"); return;}

        not_complete = true;
        is_constant = false;

        if (first_node == NULL)
        {
                first_node = new PathNode;
                current_node = first_node;
                temp = NULL;

                translation_box.xmin = x;
                translation_box.xmax = x;
                translation_box.ymin = y;
                translation_box.ymax = y;
                translation_box.zmin = z;
                translation_box.zmax = z;
        }
        else
        {
                current_node->next = new PathNode;
                temp = current_node;
                current_node = current_node->next;

                if (x > translation_box.xmax) translation_box.xmax = x;
                if (x < translation_box.xmin) translation_box.xmin = x;
                if (y > translation_box.ymax) translation_box.ymax = y;
                if (y < translation_box.ymin) translation_box.ymin = y;
                if (z > translation_box.zmax) translation_box.zmax = z;
                if (z < translation_box.zmin) translation_box.zmin = z;
        }

        current_node->next = NULL;
        current_node->previous = temp;
        current_node->start_position.x = x;
        current_node->start_position.y = y;
        current_node->start_position.z = z;
        current_node->start_position.roll = roll;
        current_node->start_position.pitch = pitch;
        current_node->start_position.yaw = yaw;
        current_node->start_time = (current_node->previous == NULL ? offset : current_node->previous->end_time + offset);
        current_node->end_time = current_node->start_time + 1000;       // just to have no random value...

        if (roll != 0.0f || pitch != 0.0f || yaw != 0.0f)
        {has_static_rotation = true;}

        if (current_node->previous != NULL)
        {
                if (current_node->start_position.roll != current_node->previous->end_position.roll ||
                        current_node->start_position.pitch != current_node->previous->end_position.pitch ||
                        current_node->start_position.yaw != current_node->previous->end_position.yaw)
                {has_dynamic_rotation = true;}
        }
}

void PositionPath::CompletePath (float x, float y, float z, float roll, float pitch, float yaw)
{
        if (!not_complete)
        {printf ("Path Construction: You have to start a new path before it can be completed (CompletePath)\n"); return;}

        current_node->end_position.x = x;
        current_node->end_position.y = y;
        current_node->end_position.z = z;
        current_node->end_position.roll = roll;
        current_node->end_position.pitch = pitch;
        current_node->end_position.yaw = yaw;

        if (x > translation_box.xmax) translation_box.xmax = x;
        if (x < translation_box.xmin) translation_box.xmin = x;
        if (y > translation_box.ymax) translation_box.ymax = y;
        if (y < translation_box.ymin) translation_box.ymin = y;
        if (z > translation_box.zmax) translation_box.zmax = z;
        if (z < translation_box.zmin) translation_box.zmin = z;

        if (current_node->start_position.roll != current_node->end_position.roll ||
                current_node->start_position.pitch != current_node->end_position.pitch ||
                current_node->start_position.yaw != current_node->end_position.yaw)
        {
                has_dynamic_rotation = true;
                has_static_rotation = true;
        }

        // we may have to update the translation box under certain circumstances
        ConservativeUpdate (current_node);

        not_complete = false;
}

BoundingBox PositionPath::GetTranslationBox ()
{
        return translation_box;
}

void PositionPath::Shift (float x, float y, float z)
{
        if (is_constant)
        {
                constant_position.x += x;
                constant_position.y += y;
                constant_position.z += z;
        }
        else
        {
                PathNode * current_node;

                current_node = first_node;
                while (current_node != NULL)
                {
                        current_node->start_position.x += x;
                        current_node->start_position.y += y;
                        current_node->start_position.z += z;
                        current_node->end_position.x += x;
                        current_node->end_position.y += y;
                        current_node->end_position.z += z;
                        current_node = current_node->next;
                }
        }

        translation_box.xmax += x;
        translation_box.xmin += x;
        translation_box.ymax += y;
        translation_box.ymin += y;
        translation_box.zmax += z;
        translation_box.zmin += z;
}

void PositionPath::ConservativeUpdate (PathNode * node)
{
        float temp;
        bool update_x, update_y, update_z;

        update_x = false;
        update_y = false;
        update_z = false;

        // check which parts of the box has to be updated
        if (node->global_transition_evaluator)
        {
                if ((node->transition_evaluator[0] == PP_SINE && node->transition_parameter[0] != 0.0f) ||
                        (node->transition_evaluator[0] == PP_COSINE && node->transition_parameter[0] != 0.0f))
                {
                        update_x = true;
                        update_y = true;
                        update_z = true;
                }
        }
        else
        {
                if ((node->transition_evaluator[0] == PP_SINE && node->transition_parameter[0] != 0.0f) ||
                        (node->transition_evaluator[0] == PP_COSINE && node->transition_parameter[0] != 0.0f))
                {update_x = true;}

                if ((node->transition_evaluator[1] == PP_SINE && node->transition_parameter[1] != 0.0f) ||
                        (node->transition_evaluator[1] == PP_COSINE && node->transition_parameter[1] != 0.0f))
                {update_y = true;}

                if ((node->transition_evaluator[2] == PP_SINE && node->transition_parameter[2] != 0.0f) ||
                        (node->transition_evaluator[2] == PP_COSINE && node->transition_parameter[2] != 0.0f))
                {update_z = true;}
        }

        // and then update these coordinates if neccessary
        if (update_x)
        {
                temp = node->start_position.x - (node->end_position.x - node->start_position.x);
                if (translation_box.xmin > temp) translation_box.xmin = temp;
                if (translation_box.xmax < temp) translation_box.xmax = temp;
        }

        if (update_y)
        {
                temp = node->start_position.y - (node->end_position.y - node->start_position.y);
                if (translation_box.ymin > temp) translation_box.ymin = temp;
                if (translation_box.ymax < temp) translation_box.ymax = temp;
        }

        if (update_z)
        {
                temp = node->start_position.z - (node->end_position.z - node->start_position.z);
                if (translation_box.zmin > temp) translation_box.zmin = temp;
                if (translation_box.zmax < temp) translation_box.zmax = temp;
        }
}

void PositionPath::DeletePath ()
{
        PathNode * curr_node;
        PathNode * to_be_deleted;

        // delete nodes
        curr_node = first_node;
        while (curr_node != NULL)
        {
                to_be_deleted = curr_node;
                curr_node = curr_node->next;
                delete to_be_deleted;
        }

        // reset other members
        Initialize ();
}

void PositionPath::SetDuration (int time)
{
        if (!not_complete)
        {printf ("Path Construction: You have to start a new path first (SetDuration)\n"); return;}

        current_node->end_time = current_node->start_time + time;
}

void PositionPath::SetGlobalTransition (int mode, float parameter)
{
        if (!not_complete)
        {printf ("Path Construction: You have to start a new path before setting a transition. (SetGlobalTransition)\n"); return;}

        current_node->global_transition_evaluator = true;
        current_node->transition_evaluator[0] = mode;
        current_node->transition_parameter[0] = parameter;
}

void PositionPath::SetTransitionModes (int m_x, int m_y, int m_z, int m_r, int m_p, int m_yaw)
{
        if (!not_complete)
        {printf ("Path Construction: You have to start a new path before setting transition modes. (SetTransitionModes)\n"); return;}

        current_node->global_transition_evaluator = false;
        current_node->transition_evaluator[0] = m_x;
        current_node->transition_evaluator[1] = m_y;
        current_node->transition_evaluator[2] = m_z;
        current_node->transition_evaluator[3] = m_r;
        current_node->transition_evaluator[4] = m_p;
        current_node->transition_evaluator[5] = m_yaw;
}

void PositionPath::SetParameters (float p_x, float p_y, float p_z, float p_r, float p_p, float p_yaw)
{
        if (!not_complete)
        {printf ("Path Construction: You have to start a new path before setting transition parameters. (SetParameters)\n"); return;}

        current_node->transition_parameter[0] = p_x;
        current_node->transition_parameter[1] = p_y;
        current_node->transition_parameter[2] = p_z;
        current_node->transition_parameter[3] = p_r;
        current_node->transition_parameter[4] = p_p;
        current_node->transition_parameter[5] = p_yaw;
}

void PositionPath::SeekTime (int time)
{
        this->constant_position = this->constant_position;

        // special case 1: pointer already points to the correct node
        if (node_pointer != NULL && node_pointer->start_time >= time && node_pointer->end_time < time)
        {return;}

        // special case 2: first time that 'time' is bigger than the beginning of the path
        if (node_pointer == NULL && first_node != NULL && first_node->start_time <= time)
        {node_pointer = first_node;}
        else
        {
                // time still smaller than the beginning of the path
                if (node_pointer == NULL)
                {return;}
        }

        // now iterate either forward or backward
        if (node_pointer->start_time > time)
        {
                // backward
                while (node_pointer != NULL && time < node_pointer->start_time)
                {node_pointer = node_pointer->previous;}
                return;
        }

        if (node_pointer->next != NULL && node_pointer->next->start_time <= time)
        {
                // forward
                while (node_pointer->next != NULL && node_pointer->next->start_time <= time)
                {node_pointer = node_pointer->next;}
                return;
        }
}


Position PositionPath::GetPosition (int time)
{
        if (is_constant)
        {
                return constant_position;
        }
        else
        {
                // set node pointer
                SeekTime (time);

                // error (before)
                if (node_pointer == NULL)
                {return constant_position;}

                // error (after)
                if (node_pointer->end_time <= time)
                {return constant_position;}

                return EvaluateNode (node_pointer, (float)(time - node_pointer->start_time) / (float)(node_pointer->end_time - node_pointer->start_time));
        }
        return constant_position;
}

Position PositionPath::EvaluateNode (PathNode * node, float progress)
{
        Position result;
        float interpolation;

        if (node->global_transition_evaluator)
        {
                interpolation = EvaluateValue (node->transition_evaluator[0], progress, node->transition_parameter[0]);
                
                result.x = node->start_position.x * (1 - interpolation) + node->end_position.x * interpolation;
                result.y = node->start_position.y * (1 - interpolation) + node->end_position.y * interpolation;
                result.z = node->start_position.z * (1 - interpolation) + node->end_position.z * interpolation;
                result.roll = node->start_position.roll * (1 - interpolation) + node->end_position.roll * interpolation;
                result.pitch = node->start_position.pitch * (1 - interpolation) + node->end_position.pitch * interpolation;
                result.yaw = node->start_position.yaw * (1 - interpolation) + node->end_position.yaw * interpolation;
        }
        else
        {
                interpolation = EvaluateValue (node->transition_evaluator[0], progress, node->transition_parameter[0]);
                result.x = node->start_position.x * (1 - interpolation) + node->end_position.x * interpolation;
                
                interpolation = EvaluateValue (node->transition_evaluator[1], progress, node->transition_parameter[1]);
                result.y = node->start_position.y * (1 - interpolation) + node->end_position.y * interpolation;
                
                interpolation = EvaluateValue (node->transition_evaluator[2], progress, node->transition_parameter[2]);
                result.z = node->start_position.z * (1 - interpolation) + node->end_position.z * interpolation;
                
                interpolation = EvaluateValue (node->transition_evaluator[3], progress, node->transition_parameter[3]);
                result.roll = node->start_position.roll * (1 - interpolation) + node->end_position.roll * interpolation;
                
                interpolation = EvaluateValue (node->transition_evaluator[4], progress, node->transition_parameter[4]);
                result.pitch = node->start_position.pitch * (1 - interpolation) + node->end_position.pitch * interpolation;
                
                interpolation = EvaluateValue (node->transition_evaluator[5], progress, node->transition_parameter[5]);
                result.yaw = node->start_position.yaw * (1 - interpolation) + node->end_position.yaw * interpolation;
        }
        return result;
}

float PositionPath::EvaluateValue (int mode, float value, float parameter)
{
        // poor performance version
        switch (mode)
        {
                case    PP_LINEAR:      return value;
                case    PP_SINE:        return sin ((parameter + 0.25f) * value * 2 * MY_PI);
                case    PP_COSINE:      return 1 - cos ((parameter + 0.25f) * value * 2 * MY_PI);
                case    PP_JUMP:        return value < parameter ? 0.0f : 1.0f;
        }

        // unknown mode
        return 0.0f;
}

bool PositionPath::IsDefined (int time)
{
        if (is_constant)
        {
                return true;
        }
        else
        {
                SeekTime (time);

                // before beginning
                if (node_pointer == NULL) return false;

                // after current (but before next)
                if (node_pointer->end_time <= time) return false;

                return true;
        }
}

bool PositionPath::HasStaticRotation ()
{
        return has_static_rotation;
}

bool PositionPath::HasDynamicRotation ()
{
        return has_dynamic_rotation;
}

void PositionPath::PrintToConsole ()
{
        PathNode * current;

        printf ("-------------------- Printing Path --------------------\n");
        if (is_constant)
        {
                printf ("Constant Position:{ %g, %g, %g, %g, %g, %g}\n", constant_position.x, constant_position.y,
                                 constant_position.z, constant_position.roll, constant_position.pitch ,constant_position.yaw);
        }
        else
        {
                current = first_node;
                while (current != NULL)
                {
                        printf ("time: %d -> %d\n", current->start_time, current->end_time);
                        printf ("pos: {%g, %g, %g, %g, %g, %g} -> {%g, %g, %g, %g, %g, %g}\n", current->start_position.x, 
                                     current->start_position.y, current->start_position.z, current->start_position.roll, 
                                         current->start_position.pitch, current->start_position.yaw, current->end_position.x, 
                                         current->end_position.y, current->end_position.z, current->end_position.roll, 
                                         current->end_position.pitch, current->end_position.yaw);

                        if (current->global_transition_evaluator)
                        {
                                printf ("global transition: ");
                                switch (current->transition_evaluator[0])
                                {
                                        case PP_LINEAR:         printf ("LINEAR (%g)\n", current->transition_parameter [0]);
                                                                                break;
                                        case PP_SINE:           printf ("SINE (%g)\n", current->transition_parameter [0]);
                                                                                break;
                                        case PP_COSINE:         printf ("COSINE (%g)\n", current->transition_parameter [0]);
                                                                                break;
                                        case PP_JUMP:           printf ("JUMP (%g)\n", current->transition_parameter [0]);
                                                                                break;
                                        default:                        printf ("UNKNOWN TRANSITION\n");
                                                                                break;
                                }
                        }
                        else
                        {
                                printf ("local transitions (x, y, z, roll, pitch, yaw):\n");
                                for (int index = 0; index < 6; index++)
                                {
                                        switch (current->transition_evaluator[0])
                                        {
                                                case PP_LINEAR:         printf ("LINEAR (%g)\n", current->transition_parameter [index]);
                                                                                        break;
                                                case PP_SINE:           printf ("SINE (%g)\n", current->transition_parameter [index]);
                                                                                        break;
                                                case PP_COSINE:         printf ("COSINE (%g)\n", current->transition_parameter [index]);
                                                                                        break;
                                                case PP_JUMP:           printf ("JUMP (%g)\n", current->transition_parameter [index]);
                                                                                        break;
                                                default:                        printf ("UNKNOWN TRANSITION\n");
                                                                                        break;
                                        }
                                }
                        }

                        current = current->next;
                }
        }
        printf ("--------------------- Translation --------------------\n");
        printf ("xmin, xmax: {%g, %g}\n", translation_box.xmin, translation_box.xmax);
        printf ("ymin, ymax: {%g, %g}\n", translation_box.ymin, translation_box.ymax);
        printf ("zmin, zmax: {%g, %g}\n", translation_box.zmin, translation_box.zmax);
        printf ("-------------------- End of Path --------------------\n");
}

---