Statistics
| Branch: | Tag: | Revision:

hlrc / server / src / Animation.cpp @ 10e164da

History | View | Annotate | Download (8.482 KB)

1 0c286af0 Simon Schulz
/*
2
* This file is part of hlrc_server
3
*
4
* Copyright(c) sschulz <AT> techfak.uni-bielefeld.de
5
* http://opensource.cit-ec.de/projects/hlrc_server
6
*
7
* This file may be licensed under the terms of the
8
* GNU General Public License Version 3 (the ``GPL''),
9
* or (at your option) any later version.
10
*
11
* Software distributed under the License is distributed
12
* on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either
13
* express or implied. See the GPL for the specific language
14
* governing rights and limitations.
15
*
16
* You should have received a copy of the GPL along with this
17
* program. If not, go to http://www.gnu.org/licenses/gpl.html
18
* or write to the Free Software Foundation, Inc.,
19
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
20
*
21
* The development of this software was supported by the
22
* Excellence Cluster EXC 277 Cognitive Interaction Technology.
23
* The Excellence Cluster EXC 277 is a grant of the Deutsche
24
* Forschungsgemeinschaft (DFG) in the context of the German
25
* Excellence Initiative.
26
*
27
*/
28
29
#include "Animation.h"
30
using namespace std;
31
using namespace boost;
32
33
Animation::Animation(){
34
    active = false;
35
    repetitions = 1;
36
    duration_each = 1000.0; //in ms
37
    scale = 1.0;
38
}
39
40
Animation::~Animation(){
41
}
42
43
void Animation::start(){
44
    if (active){
45
        printf("> Animation already active. ignored play() call\n");
46
        return;
47
    }
48
49
    active = true;
50
    repetition_now = 0;
51
52
    //Set up end time
53
    int duration = duration_each * repetitions;
54
    start_time = get_system_time(); //posix_time::microsec_clock::local_time();
55
    end_time   = start_time + boost::posix_time::milliseconds(duration);
56
}
57
58
bool Animation::is_active(){
59
    return active;
60
}
61
62
//! calculate motion profile based on constant acceleration
63
//! \param  tn time now (normalized to [0...1[)
64
//! \return value [0...1], f[tn=0] = 0
65
float Animation::apply_motion_profile_positive(float tn){
66
    float result;
67
68
    if ((tn < 0.0) || (tn > 1.0)){
69
        printf("> Animation::apply_motion_profile(%4.2f) called with invalid input (allowed is [0..1])\n",tn);
70
        return 0.0;
71
    }
72
73
    if (tn <= 0.5){
74
        //accelerated motion
75
        result = (2.0 * pow(tn, 2));
76
    }else if (tn <= 1.0){
77
        //deaccelerate
78
        result = (1.0 - 2.0 * pow(1.0 - tn, 2));
79
    }
80
    return result;
81
}
82
83
//start from 0, go to +1, go to -1, ... , -1 to 0
84
float Animation::apply_motion_profile_symmetric(float tn, unsigned int rep){
85
    unsigned int phases = 2*rep + 1;
86
    float fstep = 1.0 / phases;
87
    float result = 0.0;
88
89
    if (tn > 1.0){
90
        printf("> apply_motion_profile_symmetric(%f, %d) -> invalid tn > 1.0!\n",tn,rep);
91
        exit(1);
92
    }else if (tn <= fstep){
93
        //first slow advance to +1.0
94
        result = apply_motion_profile_positive(tn / (fstep));
95
    }else if (tn >= 1.0-fstep){
96
        //last slow return from -1 to 0
97
        result = -1.0 + apply_motion_profile_positive((tn-(1.0-fstep)) / (fstep));
98
    }else{
99
        //motion in between, alternating between +1,-1,+1,...,-1
100
        //convert to ]0,phases-fstep]
101
        float tnn = tn - fstep;
102
        //convert to (multiples) ]0,fstep]
103
        tnn = fmod(tnn, 2*fstep);
104
        //result has to be from+1 to -1
105
        if (tnn <= fstep){
106
            //+1 -> -1
107
            result = 1.0 - 2.0 * apply_motion_profile_positive(tnn/fstep);
108
        }else{
109
            //-1 -> +1
110
            result = -1.0 + 2.0 * apply_motion_profile_positive((tnn-fstep)/fstep);
111
        }
112
    }
113
    return result;
114
}
115
116
//start from 0, go to +1, goto 0, ... , +1 to 0
117
float Animation::apply_motion_profile_asymmetric(float tn, unsigned int rep){
118
    unsigned int phases = 2*rep;
119
    float fstep = 1.0 / phases;
120
    float result = 0.0;
121
122
    //motion in between, alternating between 0,1,0,1...
123
    //convert to (multiples) ]0,2*fstep]
124
    float tnn = fmod(tn, 2*fstep);
125
126
    //result has to be from+1 to -1
127
    if (tnn <= fstep){
128
        //0->1
129
        result = apply_motion_profile_positive(tnn/fstep);
130
    }else{
131
        //1->0
132
        result = 1.0 - apply_motion_profile_positive((tnn-fstep)/fstep);
133
    }
134
    return result;
135
}
136
137
void Animation::apply_on_gazestate(humotion::GazeState *gaze){
138
    //first check: active?
139
    if (!active){
140
        return;
141
    }
142
143
    //now check if this animation ended:
144
    system_time now = get_system_time(); //posix_time::microsec_clock::local_time();
145
    if (now > end_time){
146
        active = false;
147
        return;
148
    }
149
150
    //milliseconds from the beginning of this ani:
151
    posix_time::time_duration diff = now - start_time;
152
    float elapsed_ms = diff.total_milliseconds();
153
    //printf("mili %f\n",elapsed_ms);
154
155
    float max_elongation = 0.0;
156
    float profile = 0.0;
157
158
159
    //TODO: animation subclasses instead of switch() !!!
160
    switch((int)target){
161
        case(IDLE):
162
            //nothing to do!
163
            break;
164
165
        case(HEAD_SHAKE):
166
            max_elongation = scale * 15.0;
167
            profile = apply_motion_profile_symmetric(elapsed_ms / (repetitions * duration_each), repetitions);
168
            gaze->pan_offset = 0.5 * max_elongation * profile;
169
            //printf("%f %f %f %f DDD\n",elapsed_ms,elapsed_ms / (repetitions * duration_each), profile, gaze->pan_offset);fflush(stdout);
170
            break;
171
172
173
        case(HEAD_NOD):
174
            max_elongation = scale * 10.0;
175
            profile = apply_motion_profile_asymmetric(elapsed_ms / (repetitions * duration_each), repetitions);
176
            gaze->tilt_offset = - max_elongation * profile;
177
            break;
178
179
        case(EYEBLINK_L):
180
            if(elapsed_ms >= repetition_now*duration_each){
181
                //NOTE: duration/2 because we close the eye for 0.5x the time and keep it open for 0.5x time ;)
182
                //      scale can influence this 0.5/0.5 divider. i.e. scale = 0.5 -> 0.25*t closed & 0.75*t open
183
                gaze->eyeblink_request_left = scale * duration_each/2;
184
                repetition_now++;
185
            }
186
            break;
187
188
        case(EYEBLINK_R):
189
            if(elapsed_ms >= repetition_now*duration_each){
190
                //NOTE: see EYEBLINK_L for formula description
191
                gaze->eyeblink_request_right = scale * duration_each/2;
192
                repetition_now++;
193
            }
194
            break;
195
196
        case(EYEBLINK_BOTH):
197
            if(elapsed_ms >= repetition_now*duration_each){
198
                //NOTE: see EYEBLINK_L for formula description
199
                gaze->eyeblink_request_left  = scale * duration_each/2;
200
                gaze->eyeblink_request_right = scale * duration_each/2;
201
                repetition_now++;
202
            }
203
            break;
204
205
        case(ENGAGEMENT_LEFT):
206
            //TODO: use smooth animation instead of step like change
207
            gaze->pan_offset = scale * -10.0;
208
            break;
209
210
        case(ENGAGEMENT_RIGHT):
211
            //TODO: use smooth animation instead of step like change
212
            gaze->pan_offset = scale * 10.0;
213
            break;
214
215
        case(EYEBROWS_RAISE):
216
            profile = 15.0 * scale * apply_motion_profile_asymmetric(elapsed_ms / (repetitions * duration_each), repetitions);
217
            gaze->eyebrow_left   += -profile;
218
            gaze->eyebrow_right  +=  profile;
219
            break;
220
221
        case(EYEBROWS_LOWER):
222
            profile = 15.0 * scale * apply_motion_profile_asymmetric(elapsed_ms / (repetitions * duration_each), repetitions);
223
            gaze->eyebrow_left   +=  profile;
224
            gaze->eyebrow_right  += -profile;
225
            break;
226
227
228
229
        default:
230
            printf("> invalid animation target id (%d)\n",(int)target);
231
            active = false;
232
            throw runtime_error("unsupported animation id requested");
233
            break;
234
    }
235
236
237
    //ok, currently active. overwrite values:
238
    //gaze->tilt_offset = 20;
239
    //gaze->roll_offset = 20;
240
241
}
242
243
244
bool Animation::collides_with(Animation *ani){
245
    //same type -> always bad
246
    if (ani->target == target) return true;
247
248
    switch((int)target){
249
        case(IDLE):
250
            break;
251
252
        default:
253
            throw runtime_error("missing collides_with() case for animation");
254
            break;
255
256
        case(HEAD_SHAKE):
257
            break;
258
259
260
        case(HEAD_NOD):
261
            break;
262
263
        case(EYEBLINK_L):
264
            if (ani->target == EYEBLINK_BOTH) return true;
265
            break;
266
267
        case(EYEBLINK_R):
268
            if (ani->target == EYEBLINK_BOTH) return true;
269
            break;
270
271
        case(EYEBLINK_BOTH):
272
            if (ani->target == EYEBLINK_L) return true;
273
            if (ani->target == EYEBLINK_R) return true;
274
            break;
275
276
        case(EYEBROWS_RAISE):
277
            break;
278
279
        case(EYEBROWS_LOWER):
280
            break;
281
    }
282
283
    //fine
284
    return false;
285
286
}