hlrc

/*

 * This file is part of hlrc_server

 *

 * Copyright(c) sschulz <AT> techfak.uni-bielefeld.de

 * http://opensource.cit-ec.de/projects/hlrc_server

 *

 * This file may be licensed under the terms of the

 * GNU General Public License Version 3 (the ``GPL''),

 * or (at your option) any later version.

 *

 * Software distributed under the License is distributed

 * on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either

 * express or implied. See the GPL for the specific language

 * governing rights and limitations.

 *

 * You should have received a copy of the GPL along with this

 * program. If not, go to http://www.gnu.org/licenses/gpl.html

 * or write to the Free Software Foundation, Inc.,

 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.

 *

 * The development of this software was supported by the

 * Excellence Cluster EXC 277 Cognitive Interaction Technology.

 * The Excellence Cluster EXC 277 is a grant of the Deutsche

 * Forschungsgemeinschaft (DFG) in the context of the German

 * Excellence Initiative.

 *

 */

#include "Animation.h"

using namespace std;

using namespace boost;

Animation::Animation() {

        active = false;

        repetitions = 1;

        duration_each = 1000.0; // in ms

        scale = 1.0;

}

Animation::~Animation() {

}

void Animation::start() {

        if (active) {

                printf("> Animation already active. ignored play() call\n");

                return;

        }

        active = true;

        repetition_now = 0;

        // Set up end time

        int duration = duration_each * repetitions;

        start_time = get_system_time(); // posix_time::microsec_clock::local_time();

        end_time = start_time + boost::posix_time::milliseconds(duration);

}

bool Animation::is_active() {

        return active;

}

//! calculate motion profile based on constant acceleration

//! \param  tn time now (normalized to [0...1[)

//! \return value [0...1], f[tn=0] = 0

float Animation::apply_motion_profile_positive(float tn) {

        float result = 0.0;

        if ((tn < 0.0) || (tn > 1.0)) {

                printf("> Animation::apply_motion_profile(%4.2f) called with invalid input (allowed is [0..1])\n", tn);

                return 0.0;

        }

        if (tn <= 0.5) {

                // accelerated motion

                result = (2.0 * pow(tn, 2));

        }

        else if (tn <= 1.0) {

                // deaccelerate

                result = (1.0 - 2.0 * pow(1.0 - tn, 2));

        }

        return result;

}

// start from 0, go to +1, go to -1, ... , -1 to 0

float Animation::apply_motion_profile_symmetric(float tn, unsigned int rep) {

        unsigned int phases = 2 * rep + 1;

        float fstep = 1.0 / phases;

        float result = 0.0;

        if (tn > 1.0) {

                printf("> apply_motion_profile_symmetric(%f, %d) -> invalid tn > 1.0!\n", tn, rep);

                exit(1);

        }

        else if (tn <= fstep) {

                // first slow advance to +1.0

                result = apply_motion_profile_positive(tn / (fstep));

        }

        else if (tn >= 1.0 - fstep) {

                // last slow return from -1 to 0

                result = -1.0 + apply_motion_profile_positive((tn - (1.0 - fstep)) / (fstep));

        }

        else {

                // motion in between, alternating between +1,-1,+1,...,-1

                // convert to ]0,phases-fstep]

                float tnn = tn - fstep;

                // convert to (multiples) ]0,fstep]

                tnn = fmod(tnn, 2 * fstep);

                // result has to be from+1 to -1

                if (tnn <= fstep) {

                        //+1 -> -1

                        result = 1.0 - 2.0 * apply_motion_profile_positive(tnn / fstep);

                }

                else {

                        //-1 -> +1

                        result = -1.0 + 2.0 * apply_motion_profile_positive((tnn - fstep) / fstep);

                }

        }

        return result;

}

// start from 0, go to +1, goto 0, ... , +1 to 0

float Animation::apply_motion_profile_asymmetric(float tn, unsigned int rep) {

        unsigned int phases = 2 * rep;

        float fstep = 1.0 / phases;

        float result = 0.0;

        // motion in between, alternating between 0,1,0,1...

        // convert to (multiples) ]0,2*fstep]

        float tnn = fmod(tn, 2 * fstep);

        // result has to be from+1 to -1

        if (tnn <= fstep) {

                // 0->1

                result = apply_motion_profile_positive(tnn / fstep);

        }

        else {

                // 1->0

                result = 1.0 - apply_motion_profile_positive((tnn - fstep) / fstep);

        }

        return result;

}

void Animation::apply_on_gazestate(humotion::GazeState* gaze) {

        // first check: active?

        if (!active) {

                return;

        }

        // now check if this animation ended:

        system_time now = get_system_time(); // posix_time::microsec_clock::local_time();

        if (now > end_time) {

                active = false;

                return;

        }

        // milliseconds from the beginning of this ani:

        posix_time::time_duration diff = now - start_time;

        float elapsed_ms = diff.total_milliseconds();

        // printf("mili %f\n",elapsed_ms);

        float max_elongation = 0.0;

        float profile = 0.0;

        // TODO: animation subclasses instead of switch() !!!

        switch ((int)target) {

                case (IDLE):

                        // nothing to do!

                        break;

                case (HEAD_SHAKE):

                        max_elongation = scale * 15.0;

                        profile = apply_motion_profile_symmetric(elapsed_ms / (repetitions * duration_each), repetitions);

                        gaze->pan_offset = 0.5 * max_elongation * profile;

                        // printf("%f %f %f %f DDD\n",elapsed_ms,elapsed_ms / (repetitions * duration_each), profile, gaze->pan_offset);fflush(stdout);

                        break;

                case (HEAD_NOD):

                        max_elongation = scale * 10.0;

                        profile = apply_motion_profile_asymmetric(elapsed_ms / (repetitions * duration_each), repetitions);

                        gaze->tilt_offset = -max_elongation * profile;

                        break;

                case (EYEBLINK_L):

                        if (elapsed_ms >= repetition_now * duration_each) {

                                // NOTE: duration/2 because we close the eye for 0.5x the time and keep it open for 0.5x time ;)

                                //      scale can influence this 0.5/0.5 divider. i.e. scale = 0.5 -> 0.25*t closed & 0.75*t open

                                gaze->eyeblink_request_left = scale * duration_each / 2;

                                repetition_now++;

                        }

                        break;

                case (EYEBLINK_R):

                        if (elapsed_ms >= repetition_now * duration_each) {

                                // NOTE: see EYEBLINK_L for formula description

                                gaze->eyeblink_request_right = scale * duration_each / 2;

                                repetition_now++;

                        }

                        break;

                case (EYEBLINK_BOTH):

                        if (elapsed_ms >= repetition_now * duration_each) {

                                // NOTE: see EYEBLINK_L for formula description

                                gaze->eyeblink_request_left = scale * duration_each / 2;

                                gaze->eyeblink_request_right = scale * duration_each / 2;

                                repetition_now++;

                        }

                        break;

                case (ENGAGEMENT_LEFT):

                        // TODO: use smooth animation instead of step like change

                        gaze->pan_offset = scale * -10.0;

                        break;

                case (ENGAGEMENT_RIGHT):

                        // TODO: use smooth animation instead of step like change

                        gaze->pan_offset = scale * 10.0;

                        break;

                case (EYEBROWS_RAISE):

                        profile = 15.0 * scale * apply_motion_profile_asymmetric(elapsed_ms / (repetitions * duration_each), repetitions);

                        gaze->eyebrow_left += -profile;

                        gaze->eyebrow_right += profile;

                        break;

                case (EYEBROWS_LOWER):

                        profile = 15.0 * scale * apply_motion_profile_asymmetric(elapsed_ms / (repetitions * duration_each), repetitions);

                        gaze->eyebrow_left += profile;

                        gaze->eyebrow_right += -profile;

                        break;

                default:

                        printf("> invalid animation target id (%d)\n", (int)target);

                        active = false;

                        throw runtime_error("unsupported animation id requested");

                        break;

        }

        // ok, currently active. overwrite values:

        // gaze->tilt_offset = 20;

        // gaze->roll_offset = 20;

}

bool Animation::collides_with(Animation* ani) {

        // same type -> always bad

        if (ani->target == target)

                return true;

        switch ((int)target) {

                case (IDLE):

                        break;

                default:

                        throw runtime_error("missing collides_with() case for animation");

                        break;

                case (HEAD_SHAKE):

                        break;

                case (HEAD_NOD):

                        break;

                case (EYEBLINK_L):

                        if (ani->target == EYEBLINK_BOTH)

                                return true;

                        break;

                case (EYEBLINK_R):

                        if (ani->target == EYEBLINK_BOTH)

                                return true;

                        break;

                case (EYEBLINK_BOTH):

                        if (ani->target == EYEBLINK_L)

                                return true;

                        if (ani->target == EYEBLINK_R)

                                return true;

                        break;

                case (EYEBROWS_RAISE):

                        break;

                case (EYEBROWS_LOWER):

                        break;

        }

        // fine

        return false;

}