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;

}