33#include <libFreeWRL.h>
35#include "../vrml_parser/Structs.h"
36#include "../vrml_parser/CRoutes.h"
37#include "../main/headers.h"
39#include "../world_script/fieldSet.h"
40#include "../x3d_parser/Bindable.h"
42#include "quaternion.h"
44#include "../opengl/Frustum.h"
45#include "../opengl/Material.h"
46#include "../opengl/OpenGL_Utils.h"
47#include "../input/EAIHelpers.h"
50#include "LinearAlgebra.h"
58void *Component_Followers_constructor(){
63void Component_Followers_init(
struct tComponent_Followers *t){
66 t->prv = Component_Followers_constructor();
72void Component_Followers_clear(
struct tComponent_Followers *t){
80void do_ColorChaserTick(
void * ptr);
81void do_ColorDamperTick(
void * ptr);
82void do_CoordinateChaserTick(
void * ptr);
83void do_CoordinateDamperTick(
void * ptr);
84void do_OrientationChaserTick(
void * ptr);
85void do_OrientationDamperTick(
void * ptr);
86void do_PositionChaserTick(
void * ptr);
87void do_ColorDamperTick(
void * ptr);
88void do_PositionChaserTick(
void * ptr);
89void do_PositionDamperTick(
void * ptr);
90void do_PositionChaser2DTick(
void * ptr);
91void do_PositionDamper2DTick(
void * ptr);
92void do_ScalarChaserTick(
void * ptr);
93void do_ScalarDamperTick(
void * ptr);
94void do_TexCoordChaser2DTick(
void * ptr);
95void do_TexCoordDamper2DTick(
void * ptr);
107static int Buffer_length = 10;
167 void *initialDestination;
169 void *set_destination;
173 void *_previousValue;
179 void *initialDestination;
181 void *set_destination;
191#define VOIDFN (void(*))
192#define FLOATFN (float (*)(void *))
193#define INTFN (int (*) (void *, void *))
194#define VOIDPTR (void *)
197 void* (*copy)(
void *T,
void *A);
198 void* (*add)(
void *T,
void* A,
void* B);
199 void* (*dif)(
void *T,
void* A,
void* B);
200 void* (*scale)(
void *T,
void* A,
float S);
201 void* (*lerp)(
void *T,
void *A,
void *B,
float alpha);
202 float (*dist)(
void* A);
203 int (*same)(
void* A,
void* B);
204 int (*approx)(
void* A,
void* B);
205 void* (*arr)(
void* A,
int i);
212float *arr3f(
float *A,
int i){
218void *tmp3f [] = {&tmp3f[0],&tmp3f[1],&tmp3f[2],&tmp3f[3],&tmp3f[4],&tmp3f[5]};
236 veccopy3f(T->c,A->c);
240 vecadd3f(T->c,A->c,B->c);
244 vecdif3f(T->c,A->c,B->c);
248 vecscale3f(T->c,A->c,S);
252 veclerp3f(T->c,A->c,B->c,S);
255float sfvec3f_dist(
struct SFVec3f* A){
256 return veclength3f(A->c);
259 return vecsame3f(A->c,B->c);
265void *sfvec3f_tmp [] = {&sfvec3f_tmps[0],&sfvec3f_tmps[1],&sfvec3f_tmps[2],&sfvec3f_tmps[3],&sfvec3f_tmps[4],&sfvec3f_tmps[5]};
266ftype ftype_sfvec3f = {
271 VOIDFN sfvec3f_scale,
273 FLOATFN sfvec3f_dist,
289 t->copy(p->_destination,p->initialDestination);
291 t->copy(t->arr(p->_buffer,0),p->initialDestination);
292 for(C= 1; C<Buffer_length; C++ )
294 t->copy(t->arr(p->_buffer,C),p->initialValue);
296 t->copy(p->_previousValue,p->initialValue);
297 node->_steptime= node->duration / (double) Buffer_length;
307 Frac = (Now - node->_bufferendtime) / node->_steptime;
315 int NumToShift= (int)floor(Frac);
316 Frac-= (double) NumToShift;
317 if(NumToShift < Buffer_length)
321 t->copy(p->_previousValue,t->arr(p->_buffer,Buffer_length - NumToShift));
322 for( C= Buffer_length - 1; C>=NumToShift; C-- )
324 t->copy(t->arr(p->_buffer,C),t->arr(p->_buffer,C - NumToShift));
325 for( C= 0; C<NumToShift; C++ )
331 float Alpha= (float)C / (
float)NumToShift;
336 t->lerp(t->arr(p->_buffer,C),p->_destination,t->arr(p->_buffer,NumToShift),Alpha);
352 if(NumToShift == Buffer_length)
353 t->copy(p->_previousValue,t->arr(p->_buffer,0));
355 t->copy(p->_previousValue,p->_destination);
356 for( C= 0; C<Buffer_length; C++ )
358 t->copy(t->arr(p->_buffer,C),p->_destination);
360 node->_bufferendtime+= NumToShift * node->_steptime;
373 t->copy(p->_destination,p->set_destination);
377 chaser_UpdateBuffer(node, Now);
387double chaser_StepResponseCore(
double T)
389 return .5 - .5 * cos(T * PI);
395 if(t > node->duration)
399 return chaser_StepResponseCore(t / node->duration);
409 void *Output, *DeltaIn, *DeltaOut;
416 DeltaOut = t->tmp[5];
420 if(!node->_bufferendtime)
422 node->_bufferendtime= Now;
424 t->copy(p->value_changed,p->initialValue);
427 Frac= chaser_UpdateBuffer(node, Now);
441 if(t->type == FIELDTYPE_SFRotation){
446 t->copy(Output,p->_previousValue);
449 t->dif(DeltaIn,t->arr(p->_buffer,Buffer_length -1),p->_previousValue);
450 Alpha = chaser_StepResponse(node,((
double)(Buffer_length - 1) + Frac) * node->_steptime);
452 t->lerp(Output,Output,t->add(t->tmp[0],Output,DeltaIn),(
float)Alpha);
453 for(C= Buffer_length - 2; C>=0; C-- )
456 t->dif(DeltaIn,t->arr(p->_buffer,C),t->arr(p->_buffer,C+1));
458 Alpha = chaser_StepResponse(node,((
double)C + Frac) * node->_steptime);
460 t->lerp(Output,Output,t->add(t->tmp[0],Output,DeltaIn),(
float)Alpha);
466 t->copy(Output,p->_previousValue);
469 t->dif(DeltaIn,t->arr(p->_buffer,Buffer_length -1),p->_previousValue);
471 Alpha = chaser_StepResponse(node,((
double)(Buffer_length - 1) + Frac) * node->_steptime);
475 t->scale(DeltaOut,DeltaIn,(
float)Alpha);
479 t->add(Output,Output,DeltaOut);
481 for(C= Buffer_length - 2; C>=0; C-- )
485 t->dif(DeltaIn,t->arr(p->_buffer,C),t->arr(p->_buffer,C+1));
487 Alpha = chaser_StepResponse(node,((
double)C + Frac) * node->_steptime);
491 t->scale(DeltaOut,DeltaIn,(
float)Alpha);
494 t->add(Output,Output,DeltaOut);
498 if(!t->same(Output,p->value_changed)){
499 t->copy(p->value_changed,Output);
509 t->copy(p->value_changed,p->set_value);
511 t->copy(p->initialValue,p->set_value);
513 node->isActive = TRUE;
518void do_PositionChaserTick(
void * ptr){
525 _node->_buffer = REALLOCN(node,_node->_buffer,Buffer_length *
sizeof(
struct SFVec3f));
526 node->_t = &ftype_sfvec3f;
528 p->initialDestination = &_node->initialDestination;
529 p->initialValue = &_node->initialValue;
530 p->set_destination = &_node->set_destination;
531 p->set_value = &_node->set_value;
532 p->value_changed = &_node->value_changed;
533 p->_buffer = _node->_buffer;
534 p->_destination = &_node->_destination;
535 p->_previousValue = &_node->_previousvalue;
539 if(NODE_NEEDS_COMPILING){
543 node->isActive = TRUE;
547 if(!t->same(p->set_destination,p->_destination))
548 chaser_set_destination(node, Now);
550 else if(!t->same(p->set_value,p->initialValue))
551 chaser_set_value(node);
555 chaser_tick(node,Now);
580 node->_takefirstinput = TRUE;
582 damper_set_value(node,p->initialValue);
583 node->isActive = TRUE;
597 dist = t->dist(t->dif(t->tmp[0],t->arr(p->_values,0),p->_input));
602 float dist2 = t->dist(t->dif(t->tmp[0],t->arr(p->_values,1),t->arr(p->_values,0)));
603 if( dist2 > dist) dist= dist2;
609 float dist3 = t->dist(t->dif(t->tmp[0],t->arr(p->_values,2),t->arr(p->_values,1)));
610 if( dist3 > dist) dist= dist3;
616 float dist4 = t->dist(t->dif(t->tmp[0],t->arr(p->_values,3),t->arr(p->_values,2)));
617 if( dist4 > dist) dist= dist4;
623 float dist5 = t->dist(t->dif(t->tmp[0],t->arr(p->_values,4),t->arr(p->_values,3)));
624 if( dist5 > dist) dist= dist5;
635 node->_takefirstinput = FALSE;
638 t->copy(t->arr(p->_values,i),opos);
640 t->copy(p->value_changed, opos);
642 t->copy(p->initialValue,opos);
644 node->isActive = TRUE;
653 if(node->_takefirstinput)
655 node->_takefirstinput = FALSE;
656 damper_set_value(node,ipos);
659 if(!t->same(ipos,p->_input))
662 t->copy(p->_input,ipos);
663 node->isActive = TRUE;
701 node->_lasttick= now;
704 delta= now - node->_lasttick;
705 node->_lasttick= now;
706 alpha= exp(-delta / node->tau);
707 if(node->_takefirstinput)
715 if(node->order > 0 && node->tau != 0.0)
717 t->lerp(t->arr(p->_values,0),p->_input,t->arr(p->_values,0),(
float)alpha);
719 t->copy(t->arr(p->_values,0),p->_input);
725 if(node->order > 1 && node->tau != 0.0)
727 t->lerp(t->arr(p->_values,1),t->arr(p->_values,0),t->arr(p->_values,1),(
float)alpha);
729 t->copy(t->arr(p->_values,1),t->arr(p->_values,0));
735 if(node->order > 2 && node->tau != 0.0)
737 t->lerp(t->arr(p->_values,2),t->arr(p->_values,1),t->arr(p->_values,2),(
float)alpha);
739 t->copy(t->arr(p->_values,2),t->arr(p->_values,1));
746 if(node->order > 3 && node->tau != 0.0)
748 t->lerp(t->arr(p->_values,3),t->arr(p->_values,2),t->arr(p->_values,3),(
float)alpha);
750 t->copy(t->arr(p->_values,3),t->arr(p->_values,2));
756 if(node->order > 4 && node->tau != 0.0)
758 t->lerp(t->arr(p->_values,4),t->arr(p->_values,3),t->arr(p->_values,4),(
float)alpha);
760 t->copy(t->arr(p->_values,4),t->arr(p->_values,3));
762 dist= damper_GetDist(node);
764 if(dist < max(node->tolerance,.001f))
769 t->copy(t->arr(p->_values,i),p->_input);
772 t->copy(p->value_changed,p->_input);
774 node->isActive = FALSE;
779 t->copy(p->value_changed,t->arr(p->_values,4));
787void do_PositionDamperTick(
void * ptr){
793 node->_t = &ftype_sfvec3f;
795 _node->_values = REALLOCN(node,_node->_values,5 *
sizeof(
struct SFVec3f));
796 p->initialDestination = &_node->initialDestination;
797 p->initialValue = &_node->initialValue;
798 p->set_destination = &_node->set_destination;
799 p->set_value = &_node->set_value;
800 p->value_changed = &_node->value_changed;
801 p->_input = &_node->_input;
802 p->_values = _node->_values;
807 if(NODE_NEEDS_COMPILING){
813 if(!t->same(p->set_destination,p->_input))
815 damper_set_destination(node, p->set_destination);
817 if(node->tau != node->_tau)
818 damper_set_tau(node,node->tau);
821 if(!t->same(p->initialValue,p->set_value))
823 damper_set_value(node,p->set_value);
827 tick_damper(node,TickTime());
834 node->_destination = node->initialDestination;
835 buffer[0]= node->initialDestination;
836 for(C= 1; C<Buffer_length; C++ )
837 buffer[C]= node->initialValue;
838 node->_previousvalue= node->initialValue;
839 node->_steptime= node->duration / (double) Buffer_length;
847 Frac = (Now - node->_bufferendtime) / node->_steptime;
855 int NumToShift= (int)floor(Frac);
856 Frac-= (double) NumToShift;
857 if(NumToShift < Buffer_length)
860 node->_previousvalue= buffer[Buffer_length - NumToShift];
861 for( C= Buffer_length - 1; C>=NumToShift; C-- )
862 buffer[C]= buffer[C - NumToShift];
863 for( C= 0; C<NumToShift; C++ )
868 float tmp1[3],tmp2[3];
869 float Alpha= (float)C / (
float)NumToShift;
880 vecscale3f(tmp1,buffer[NumToShift].c,Alpha);
881 vecscale3f(tmp2,node->_destination.c,1.0f - Alpha);
882 vecadd3f(tmp3,tmp1,tmp2);
883 veccopy3f(buffer[C].c,tmp3);
885 vecadd3f(buffer[C].c,vecscale3f(tmp1,buffer[NumToShift].c,Alpha),vecscale3f(tmp2,node->_destination.c,1.0f - Alpha));
899 node->_previousvalue= NumToShift == Buffer_length? buffer[0] : node->_destination;
901 for( C= 0; C<Buffer_length; C++ )
902 buffer[C]= node->_destination;
904 node->_bufferendtime+= NumToShift * node->_steptime;
913 node->_destination= Dest;
917 chaser_UpdateBuffer(node, Now);
927double chaser_StepResponseCore(
double T)
929 return .5 - .5 * cos(T * PI);
935 if(t > node->duration)
939 return chaser_StepResponseCore(t / node->duration);
952 if(!node->_bufferendtime)
954 node->_bufferendtime= Now;
955 node->value_changed= node->initialValue;
958 Frac= chaser_UpdateBuffer(node, Now);
972 Output= node->_previousvalue;
974 vecdif3f(DeltaIn.c,buffer[Buffer_length - 1].c,node->_previousvalue.c);
977 vecscale3f(DeltaOut.c,DeltaIn.c,(
float)chaser_StepResponse(node,((
double)Buffer_length - 1.0 + Frac) * node->_steptime));
979 vecadd3f(Output.c,Output.c,DeltaOut.c);
980 for(C= Buffer_length - 2; C>=0; C-- )
983 vecdif3f(DeltaIn.c,buffer[C].c,buffer[C+1].c);
985 vecscale3f(DeltaOut.c,DeltaIn.c,(
float)chaser_StepResponse(node,((
double)C + Frac) * node->_steptime));
987 vecadd3f(Output.c,Output.c,DeltaOut.c);
989 if(!vecsame3f(Output.c,node->value_changed.c)){
990 node->value_changed= Output;
996 node->value_changed= opos;
997 node->initialValue = opos;
999 node->isActive = TRUE;
1004void do_PositionChaserTick(
void * ptr){
1009 node->_buffer = realloc(node->_buffer,Buffer_length *
sizeof(
struct SFVec3f));
1013 if(NODE_NEEDS_COMPILING){
1014 node->isActive = TRUE;
1017 if(!vecsame3f(node->set_destination.c,node->_destination.c))
1018 chaser_set_destination(node, node->set_destination,Now);
1019 else if(!vecsame3f(node->set_value.c,node->initialValue.c))
1020 chaser_set_value(node,node->set_value);
1024 chaser_tick(node,Now);
1046 node->_takefirstinput = TRUE;
1047 damper_set_value(node,node->initialValue);
1048 node->isActive = TRUE;
1058 dist = veclength3f(vecdif3f(tmp,values[0].c,node->_input.c));
1062 float dist2 = veclength3f(vecdif3f(tmp,values[1].c,values[0].c));
1063 if( dist2 > dist) dist= dist2;
1068 float dist3 = veclength3f(vecdif3f(tmp,values[2].c,values[1].c));
1069 if( dist3 > dist) dist= dist3;
1074 float dist4 = veclength3f(vecdif3f(tmp,values[3].c,values[2].c));
1075 if( dist4 > dist) dist= dist4;
1080 float dist5 = veclength3f(vecdif3f(tmp,values[4].c, values[3].c));
1081 if( dist5 > dist) dist= dist5;
1088 node->_takefirstinput = FALSE;
1089 values[0]= values[1]= values[2]= values[3]= values[4]= opos;
1090 node->value_changed= opos;
1091 node->initialValue = opos;
1093 node->isActive = TRUE;
1099 if(node->_takefirstinput)
1101 node->_takefirstinput = FALSE;
1102 damper_set_value(node,ipos);
1104 if(!vecsame3f(ipos.c,node->_input.c))
1106 node->_input = ipos;
1107 node->isActive = TRUE;
1114 float tmp[3], tmp2[3];
1117 vecdif3f(tmp,b.c,a.c);
1118 vecscale3f(tmp2,tmp,(
float)alpha);
1119 vecadd3f(ret.c,a.c,tmp2);
1121 vecadd3f(ret.c,a.c,vecscale3f(tmp2,vecdif3f(tmp,b.c,a.c),(
float)alpha));
1132 if(!node->_lasttick)
1134 node->_lasttick= now;
1137 delta= now - node->_lasttick;
1138 node->_lasttick= now;
1139 alpha= exp(-delta / node->tau);
1140 if(node->_takefirstinput)
1143 values[0]= node->order > 0 && node->tau != 0.0
1145 ? damper_diftimes(node->_input,values[0],alpha)
1148 values[1]= node->order > 1 && node->tau != 0.0
1150 ? damper_diftimes(values[0],values[1],alpha)
1153 values[2]= node->order > 2 && node->tau != 0.0
1155 ? damper_diftimes(values[1],values[2],alpha)
1158 values[3]= node->order > 3 && node->tau != 0.0
1160 ? damper_diftimes(values[2],values[3],alpha)
1163 values[4]= node->order > 4 && node->tau != 0.0
1165 ? damper_diftimes(values[3],values[4],alpha)
1168 dist= damper_GetDist(node);
1170 if(dist < max(node->tolerance,.001f))
1172 values[0]= values[1]= values[2]= values[3]= values[4]= node->_input;
1173 node->value_changed= node->_input;
1175 node->isActive = FALSE;
1179 node->value_changed= values[4];
1186void do_PositionDamperTick(
void * ptr){
1190 node->_values = realloc(node->_values,5 *
sizeof(
struct SFVec3f));
1195 if(NODE_NEEDS_COMPILING){
1197 if(!vecsame3f(node->set_destination.c,node->_input.c))
1198 damper_set_destination(node, node->set_destination);
1200 if(node->tau != node->_tau)
1201 damper_set_tau(node,node->tau);
1203 if(!vecsame3f(node->initialValue.c,node->set_value.c))
1204 damper_set_value(node,node->set_value);
1208 tick_positiondamper(node,TickTime());
1213void do_ColorChaserTick_default(
void * ptr){
1216 if(NODE_NEEDS_COMPILING){
1218 veccopy3f(node->value_changed.c, node->set_destination.c);
1223void do_ColorDamperTick_default(
void * ptr){
1226 if(NODE_NEEDS_COMPILING){
1228 veccopy3f(node->value_changed.c, node->set_destination.c);
1233void do_ColorChaserTick(
void * ptr){
1238 if(!_node->_buffer){
1240 _node->_buffer = REALLOCN(node,_node->_buffer,Buffer_length *
sizeof(
struct SFColor));
1241 node->_t = &ftype_sfvec3f;
1243 p->initialDestination = &_node->initialDestination;
1244 p->initialValue = &_node->initialValue;
1245 p->set_destination = &_node->set_destination;
1246 p->set_value = &_node->set_value;
1247 p->value_changed = &_node->value_changed;
1248 p->_buffer = _node->_buffer;
1249 p->_destination = &_node->_destination;
1250 p->_previousValue = &_node->_previousvalue;
1254 if(NODE_NEEDS_COMPILING){
1256 ftype *t = node->_t;
1258 node->isActive = TRUE;
1262 if(!t->same(p->set_destination,p->_destination))
1263 chaser_set_destination(node, Now);
1265 else if(!t->same(p->set_value,p->initialValue))
1266 chaser_set_value(node);
1270 chaser_tick(node,Now);
1273void do_ColorDamperTick(
void * ptr){
1277 if(!_node->_values){
1279 node->_t = &ftype_sfvec3f;
1281 _node->_values = REALLOCN(node,_node->_values,5 *
sizeof(
struct SFColor));
1282 p->initialDestination = &_node->initialDestination;
1283 p->initialValue = &_node->initialValue;
1284 p->set_destination = &_node->set_destination;
1285 p->set_value = &_node->set_value;
1286 p->value_changed = &_node->value_changed;
1287 p->_input = &_node->_input;
1288 p->_values = _node->_values;
1293 if(NODE_NEEDS_COMPILING){
1296 ftype *t = node->_t;
1299 if(!t->same(p->set_destination,p->_input))
1301 damper_set_destination(node, p->set_destination);
1303 if(node->tau != node->_tau)
1304 damper_set_tau(node,node->tau);
1307 if(!t->same(p->initialValue,p->set_value))
1309 damper_set_value(node,p->set_value);
1313 tick_damper(node,TickTime());
1316void do_CoordinateChaserTick_default(
void * ptr){
1319 if(NODE_NEEDS_COMPILING){
1322 n = node->set_destination.n;
1323 node->value_changed.n = n;
1324 node->value_changed.p = REALLOC(node->value_changed.p,n *
sizeof(
struct SFVec3f));
1325 memcpy(node->value_changed.p,node->set_destination.p,n*
sizeof(
struct SFVec3f));
1330void do_CoordinateDamperTick_default(
void * ptr){
1333 if(NODE_NEEDS_COMPILING){
1336 n = node->set_destination.n;
1337 node->value_changed.n = n;
1338 node->value_changed.p = REALLOC(node->value_changed.p,n *
sizeof(
struct SFVec3f));
1339 memcpy(node->value_changed.p,node->set_destination.p,n*
sizeof(
struct SFVec3f));
1346 T->p = REALLOC(T->p,A->n *
sizeof(
struct SFVec3f));
1348 memcpy(T->p,A->p,A->n *
sizeof(
struct SFVec3f));
1353 T->n = min(A->n,B->n);
1354 T->p = REALLOC(T->p,T->n *
sizeof(
struct SFVec3f));
1356 sfvec3f_add(&T->p[i],&A->p[i],&B->p[i]);
1361 T->n = min(A->n,B->n);
1362 T->p = REALLOC(T->p,T->n *
sizeof(
struct SFVec3f));
1364 sfvec3f_dif(&T->p[i],&A->p[i],&B->p[i]);
1370 T->p = REALLOC(T->p,T->n *
sizeof(
struct SFVec3f));
1372 sfvec3f_scale(&T->p[i],&A->p[i],S);
1378 T->p = REALLOC(T->p,T->n *
sizeof(
struct SFVec3f));
1380 sfvec3f_lerp(&T->p[i],&A->p[i],&B->p[i],alpha);
1387 dist += sfvec3f_dist(&A->p[i]);
1392 if(A->n != B->n)
return FALSE;
1395 isame = isame && sfvec3f_same(&A->p[i],&B->p[i]);
1402void *mfvec3f_tmp [] = {&mfvec3f_tmps[0],&mfvec3f_tmps[1],&mfvec3f_tmps[2],&mfvec3f_tmps[3],&mfvec3f_tmps[4],&mfvec3f_tmps[5]};
1403ftype ftype_mfvec3f = {
1405 VOIDFN mfvec3f_copy,
1408 VOIDFN mfvec3f_scale,
1409 VOIDFN mfvec3f_lerp,
1410 FLOATFN mfvec3f_dist,
1414 VOIDPTR mfvec3f_tmp,
1424 memcpy(T->c, A->c,
sizeof(
struct SFRotation));
1427 vrmlrot_to_quaternion(&qA, (
double) A->c[0],
1428 (
double) A->c[1], (
double) A->c[2], (
double) A->c[3]);
1431 quaternion_inverse(&qT,&qA);
1434 quaternion_to_vrmlrot(&qT, &x, &y, &z, &a);
1436 T->c[0] = (float) x;
1437 T->c[1] = (float) y;
1438 T->c[2] = (float) z;
1439 T->c[3] = (float) a;
1447 vrmlrot_to_quaternion(&qA, (
double) A->c[0],
1448 (
double) A->c[1], (
double) A->c[2], (
double) A->c[3]);
1450 vrmlrot_to_quaternion(&qB, (
double) B->c[0],
1451 (
double) B->c[1], (
double) B->c[2], (
double) B->c[3]);
1454 quaternion_multiply(&qT,&qA,&qB);
1457 quaternion_to_vrmlrot(&qT, &x, &y, &z, &a);
1459 T->c[0] = (float) x;
1460 T->c[1] = (float) y;
1461 T->c[2] = (float) z;
1462 T->c[3] = (float) a;
1467 memcpy(T->c, A->c,
sizeof(
struct SFRotation));
1472 sfrotation_multiply(T,A,B);
1482 vrmlrot_to_quaternion(&qA, (
double) A->c[0],
1483 (
double) A->c[1], (
double) A->c[2], (
double) A->c[3]);
1485 vrmlrot_to_quaternion(&qB, (
double) B->c[0],
1486 (
double) B->c[1], (
double) B->c[2], (
double) B->c[3]);
1489 quaternion_inverse(&qBI,&qB);
1491 quaternion_multiply(&qT,&qBI,&qA);
1494 quaternion_to_vrmlrot(&qT, &x, &y, &z, &a);
1496 T->c[0] = (float) x;
1497 T->c[1] = (float) y;
1498 T->c[2] = (float) z;
1499 T->c[3] = (float) a;
1516 sfrotation_copy(T,A);
1521 if (APPROX(alpha, 0.0f)) {
1522 memcpy(T->c,A->c,4*
sizeof(
float));
1523 }
else if (APPROX(alpha, 1.0f)) {
1524 memcpy(T->c,B->c,4*
sizeof(
float));
1528 vrmlrot_to_quaternion(&quatA,
1534 vrmlrot_to_quaternion(&quatB,
1540 quaternion_slerp(&quatT, &quatA, &quatB, (
double)alpha);
1541 quaternion_to_vrmlrot(&quatT,&x,&y,&z,&a);
1543 T->c[0] = (float) x;
1544 T->c[1] = (float) y;
1545 T->c[2] = (float) z;
1546 T->c[3] = (float) a;
1557 isame = isame && A->c[i] == B->c[i];
1564void *sfrotation_tmp [] = {&sfrotation_tmps[0],&sfrotation_tmps[1],&sfrotation_tmps[2],&sfrotation_tmps[3],&sfrotation_tmps[4],&sfrotation_tmps[5]};
1565ftype ftype_sfrotation = {
1566 FIELDTYPE_SFRotation,
1567 VOIDFN sfrotation_copy,
1568 VOIDFN sfrotation_add,
1569 VOIDFN sfrotation_dif,
1570 VOIDFN sfrotation_scale,
1571 VOIDFN sfrotation_slerp,
1572 FLOATFN sfrotation_dist,
1573 INTFN sfrotation_same,
1574 INTFN sfrotation_same,
1575 VOIDFN sfrotation_arr,
1576 VOIDPTR sfrotation_tmp,
1579void do_OrientationChaserTick(
void * ptr){
1584 if(!_node->_buffer){
1586 _node->_buffer = REALLOCN(node,_node->_buffer,Buffer_length *
sizeof(
struct SFRotation));
1587 node->_t = &ftype_sfrotation;
1589 p->initialDestination = &_node->initialDestination;
1590 p->initialValue = &_node->initialValue;
1591 p->set_destination = &_node->set_destination;
1592 p->set_value = &_node->set_value;
1593 p->value_changed = &_node->value_changed;
1594 p->_buffer = _node->_buffer;
1595 p->_destination = &_node->_destination;
1596 p->_previousValue = &_node->_previousvalue;
1600 if(NODE_NEEDS_COMPILING){
1602 ftype *t = node->_t;
1603 static int count = 0;
1604 node->isActive = TRUE;
1608 if(!t->same(p->set_destination,p->_destination))
1609 chaser_set_destination(node, Now);
1611 else if(!t->same(p->set_value,p->initialValue))
1612 chaser_set_value(node);
1617 chaser_tick(node,Now);
1620void do_OrientationDamperTick(
void * ptr){
1624 if(!_node->_values){
1626 node->_t = &ftype_sfrotation;
1628 _node->_values = REALLOCN(node,_node->_values,5 *
sizeof(
struct SFRotation));
1629 p->initialDestination = &_node->initialDestination;
1630 p->initialValue = &_node->initialValue;
1631 p->set_destination = &_node->set_destination;
1632 p->set_value = &_node->set_value;
1633 p->value_changed = &_node->value_changed;
1634 p->_input = &_node->_input;
1635 p->_values = _node->_values;
1640 if(NODE_NEEDS_COMPILING){
1643 ftype *t = node->_t;
1646 if(!t->same(p->set_destination,p->_input))
1648 damper_set_destination(node, p->set_destination);
1650 if(node->tau != node->_tau)
1651 damper_set_tau(node,node->tau);
1654 if(!t->same(p->initialValue,p->set_value))
1656 damper_set_value(node,p->set_value);
1660 tick_damper(node,TickTime());
1664void do_OrientationChaserTick_default(
void * ptr){
1667 if(NODE_NEEDS_COMPILING){
1669 veccopy3f(node->value_changed.c, node->set_destination.c);
1670 node->value_changed.c[3] = node->set_destination.c[3];
1675void do_OrientationDamperTick_default(
void * ptr){
1678 if(NODE_NEEDS_COMPILING){
1680 veccopy3f(node->value_changed.c, node->set_destination.c);
1681 node->value_changed.c[3] = node->set_destination.c[3];
1692 node->_destination = node->initialDestination;
1693 buffer[0]= node->initialDestination;
1694 for(C= 1; C<Buffer_length; C++ )
1695 buffer[C]= node->initialValue;
1696 node->_previousvalue= node->initialValue;
1697 node->_steptime= node->duration / (double) Buffer_length;
1705 Frac = (Now - node->_bufferendtime) / node->_steptime;
1713 int NumToShift= (int)floor(Frac);
1714 Frac-= (double) NumToShift;
1715 if(NumToShift < Buffer_length)
1718 node->_previousvalue= buffer[Buffer_length - NumToShift];
1719 for( C= Buffer_length - 1; C>=NumToShift; C-- )
1720 buffer[C]= buffer[C - NumToShift];
1721 for( C= 0; C<NumToShift; C++ )
1726 float Alpha= (float)C / (
float)NumToShift;
1729 if(1)sfrotation_slerp(&buffer[C],&node->_destination,&buffer[NumToShift],Alpha);
1730 else sfrotation_slerp(&buffer[C],&buffer[NumToShift],&node->_destination,Alpha);
1743 node->_previousvalue= NumToShift == Buffer_length? buffer[0] : node->_destination;
1745 for( C= 0; C<Buffer_length; C++ )
1746 buffer[C]= node->_destination;
1748 node->_bufferendtime+= NumToShift * node->_steptime;
1757 node->_destination= Dest;
1761 orichaser_UpdateBuffer(node, Now);
1770double orichaser_StepResponseCore(
double T)
1772 return .5 - .5 * cos(T * PI);
1778 if(t > node->duration)
1782 return orichaser_StepResponseCore(t / node->duration);
1796 if(!node->_bufferendtime)
1798 node->_bufferendtime= Now;
1799 node->value_changed= node->initialValue;
1802 Frac= orichaser_UpdateBuffer(node, Now);
1817 Output= node->_previousvalue;
1820 sfrotation_inverse(&tmp0,&node->_previousvalue);
1821 sfrotation_multiply(&DeltaIn,&tmp0,&buffer[Buffer_length -1]);
1823 sfrotation_dif(&DeltaIn,&buffer[Buffer_length -1],&node->_previousvalue);
1826 Alpha = orichaser_StepResponse(node,((
double)Buffer_length - 1.0 + Frac) * node->_steptime);
1829 sfrotation_multiply(&tmp0,&Output,&DeltaIn);
1831 sfrotation_add(&tmp0,&Output,&DeltaIn);
1833 sfrotation_slerp(&Output,&Output,&tmp0,(
float)Alpha);
1834 for(C= Buffer_length - 2; C>=0; C-- )
1838 sfrotation_inverse(&tmp0,&buffer[C + 1]);
1839 sfrotation_multiply(&DeltaIn,&tmp0,&buffer[C]);
1841 sfrotation_dif(&DeltaIn,&buffer[C],&buffer[C+1]);
1843 Alpha = orichaser_StepResponse(node,((
float)C + Frac) * node->_steptime);
1846 sfrotation_multiply(&tmp0,&Output,&DeltaIn);
1848 sfrotation_add(&tmp0,&Output,&DeltaIn);
1850 sfrotation_slerp(&Output,&Output,&tmp0,(
float)Alpha);
1852 if(!sfrotation_same(&Output,&node->value_changed)){
1853 node->value_changed= Output;
1859 node->value_changed= opos;
1860 node->initialValue = opos;
1862 node->isActive = TRUE;
1867void do_OrientationChaserTick_oldway_works(
void * ptr){
1872 node->_buffer = REALLOCN(node,node->_buffer,Buffer_length *
sizeof(
struct SFRotation));
1873 orichaser_init(node);
1876 if(NODE_NEEDS_COMPILING){
1878 node->isActive = TRUE;
1881 if(!sfrotation_same(&node->set_destination,&node->_destination))
1882 orichaser_set_destination(node, node->set_destination,Now);
1883 else if(!sfrotation_same(&node->set_value,&node->initialValue))
1884 orichaser_set_value(node,node->set_value);
1888 orichaser_tick(node,Now);
1895void do_CoordinateChaserTick(
void * ptr){
1900 if(!_node->_buffer){
1902 _node->_buffer = REALLOCN(node,_node->_buffer,Buffer_length *
sizeof(
struct Multi_Vec3f));
1903 node->_t = &ftype_mfvec3f;
1905 p->initialDestination = &_node->initialDestination;
1906 p->initialValue = &_node->initialValue;
1907 p->set_destination = &_node->set_destination;
1908 p->set_value = &_node->set_value;
1909 p->value_changed = &_node->value_changed;
1910 p->_buffer = _node->_buffer;
1911 p->_destination = &_node->_destination;
1912 p->_previousValue = &_node->_previousvalue;
1916 if(NODE_NEEDS_COMPILING){
1918 ftype *t = node->_t;
1920 node->isActive = TRUE;
1924 if(!t->same(p->set_destination,p->_destination))
1925 chaser_set_destination(node, Now);
1927 else if(!t->same(p->set_value,p->initialValue))
1928 chaser_set_value(node);
1932 chaser_tick(node,Now);
1935void do_CoordinateDamperTick(
void * ptr){
1939 if(!_node->_values){
1941 node->_t = &ftype_sfvec3f;
1943 _node->_values = REALLOCN(node,_node->_values,5 *
sizeof(
struct Multi_Vec3f));
1944 p->initialDestination = &_node->initialDestination;
1945 p->initialValue = &_node->initialValue;
1946 p->set_destination = &_node->set_destination;
1947 p->set_value = &_node->set_value;
1948 p->value_changed = &_node->value_changed;
1949 p->_input = &_node->_input;
1950 p->_values = _node->_values;
1955 if(NODE_NEEDS_COMPILING){
1958 ftype *t = node->_t;
1961 if(!t->same(p->set_destination,p->_input))
1963 damper_set_destination(node, p->set_destination);
1965 if(node->tau != node->_tau)
1966 damper_set_tau(node,node->tau);
1969 if(!t->same(p->initialValue,p->set_value))
1971 damper_set_value(node,p->set_value);
1975 tick_damper(node,TickTime());
1982void do_PositionChaser2DTick_default(
void * ptr){
1985 if(NODE_NEEDS_COMPILING){
1987 veccopy2f(node->value_changed.c, node->set_destination.c);
1993void do_PositionDamper2DTick_default(
void * ptr){
1996 if(NODE_NEEDS_COMPILING){
1998 veccopy2f(node->value_changed.c, node->set_destination.c);
2006 veccopy2f(T->c,A->c);
2010 vecadd2f(T->c,A->c,B->c);
2014 vecdif2f(T->c,A->c,B->c);
2018 vecscale2f(T->c,A->c,S);
2022 veclerp2f(T->c,A->c,B->c,alpha);
2025float sfvec2f_dist(
struct SFVec2f* A){
2026 return veclength2f(A->c);
2029 return vecsame2f(A->c,B->c);
2034struct SFVec2f sfvec2f_tmps[6];
2035void *sfvec2f_tmp [] = {&sfvec2f_tmps[0],&sfvec2f_tmps[1],&sfvec2f_tmps[2],&sfvec2f_tmps[3],&sfvec2f_tmps[4],&sfvec2f_tmps[5]};
2036ftype ftype_sfvec2f = {
2038 VOIDFN sfvec2f_copy,
2041 VOIDFN sfvec2f_scale,
2042 VOIDFN sfvec2f_lerp,
2043 FLOATFN sfvec2f_dist,
2047 VOIDPTR sfvec2f_tmp,
2050void do_PositionChaser2DTick(
void * ptr){
2055 if(!_node->_buffer){
2057 _node->_buffer = REALLOCN(node,_node->_buffer,Buffer_length *
sizeof(
struct SFVec2f));
2058 node->_t = &ftype_sfvec2f;
2060 p->initialDestination = &_node->initialDestination;
2061 p->initialValue = &_node->initialValue;
2062 p->set_destination = &_node->set_destination;
2063 p->set_value = &_node->set_value;
2064 p->value_changed = &_node->value_changed;
2065 p->_buffer = _node->_buffer;
2066 p->_destination = &_node->_destination;
2067 p->_previousValue = &_node->_previousvalue;
2071 if(NODE_NEEDS_COMPILING){
2073 ftype *t = node->_t;
2075 node->isActive = TRUE;
2079 if(!t->same(p->set_destination,p->_destination))
2080 chaser_set_destination(node, Now);
2082 else if(!t->same(p->set_value,p->initialValue))
2083 chaser_set_value(node);
2087 chaser_tick(node,Now);
2089void do_PositionDamper2DTick(
void * ptr){
2093 if(!_node->_values){
2095 node->_t = &ftype_sfvec2f;
2097 _node->_values = REALLOCN(node,_node->_values,5 *
sizeof(
struct SFVec2f));
2098 p->initialDestination = &_node->initialDestination;
2099 p->initialValue = &_node->initialValue;
2100 p->set_destination = &_node->set_destination;
2101 p->set_value = &_node->set_value;
2102 p->value_changed = &_node->value_changed;
2103 p->_input = &_node->_input;
2104 p->_values = _node->_values;
2109 if(NODE_NEEDS_COMPILING){
2112 ftype *t = node->_t;
2115 if(!t->same(p->set_destination,p->_input))
2117 damper_set_destination(node, p->set_destination);
2119 if(node->tau != node->_tau)
2120 damper_set_tau(node,node->tau);
2123 if(!t->same(p->initialValue,p->set_value))
2125 damper_set_value(node,p->set_value);
2129 tick_damper(node,TickTime());
2133void do_ScalarChaserTick_default(
void * ptr){
2136 if(NODE_NEEDS_COMPILING){
2138 node->value_changed = node->set_destination;
2143void do_ScalarDamperTick_default(
void * ptr){
2146 if(NODE_NEEDS_COMPILING){
2148 node->value_changed = node->set_destination;
2153float *scalar_copy(
float* T,
float *A){
2157float *scalar_add(
float* T,
float *A,
float *B){
2161float *scalar_dif(
float* T,
float *A,
float *B){
2165float *scalar_scale(
float* T,
float *A,
float S){
2169float *scalar_lerp(
float* T,
float *A,
float *B,
float alpha){
2170 *T = (1.0f -alpha)*(*A) + alpha*(*B);
2173float scalar_dist(
float* A){
2174 return (
float)fabs(*A);
2176int scalar_same(
float *A,
float *B){
2177 return *A == *B ? TRUE : FALSE;
2179float *scalar_arr(
float *A,
int i){
2182float scalar_tmps[6];
2183void *scalar_tmp [] = {&scalar_tmps[0],&scalar_tmps[1],&scalar_tmps[2],&scalar_tmps[3],&scalar_tmps[4],&scalar_tmps[5]};
2184ftype ftype_scalar = {
2189 VOIDFN scalar_scale,
2191 FLOATFN scalar_dist,
2198void do_ScalarChaserTick(
void * ptr){
2203 if(!_node->_buffer){
2205 _node->_buffer = REALLOCN(node,_node->_buffer,Buffer_length *
sizeof(
float));
2206 node->_t = &ftype_scalar;
2208 p->initialDestination = &_node->initialDestination;
2209 p->initialValue = &_node->initialValue;
2210 p->set_destination = &_node->set_destination;
2211 p->set_value = &_node->set_value;
2212 p->value_changed = &_node->value_changed;
2213 p->_buffer = _node->_buffer;
2214 p->_destination = &_node->_destination;
2215 p->_previousValue = &_node->_previousvalue;
2219 if(NODE_NEEDS_COMPILING){
2221 ftype *t = node->_t;
2223 node->isActive = TRUE;
2227 if(!t->same(p->set_destination,p->_destination))
2228 chaser_set_destination(node, Now);
2230 else if(!t->same(p->set_value,p->initialValue))
2231 chaser_set_value(node);
2235 chaser_tick(node,Now);
2237void do_ScalarDamperTick(
void * ptr){
2241 if(!_node->_values){
2243 node->_t = &ftype_scalar;
2245 _node->_values = REALLOCN(node,_node->_values,5 *
sizeof(
float));
2246 p->initialDestination = &_node->initialDestination;
2247 p->initialValue = &_node->initialValue;
2248 p->set_destination = &_node->set_destination;
2249 p->set_value = &_node->set_value;
2250 p->value_changed = &_node->value_changed;
2251 p->_input = &_node->_input;
2252 p->_values = _node->_values;
2257 if(NODE_NEEDS_COMPILING){
2260 ftype *t = node->_t;
2263 if(!t->same(p->set_destination,p->_input))
2265 damper_set_destination(node, p->set_destination);
2267 if(node->tau != node->_tau)
2268 damper_set_tau(node,node->tau);
2271 if(!t->same(p->initialValue,p->set_value))
2273 damper_set_value(node,p->set_value);
2277 tick_damper(node,TickTime());
2280void do_TexCoordChaser2DTick_default(
void * ptr){
2283 if(NODE_NEEDS_COMPILING){
2286 n = node->set_destination.n;
2287 node->value_changed.n = n;
2288 node->value_changed.p = REALLOC(node->value_changed.p,n *
sizeof(
struct SFVec2f));
2289 memcpy(node->value_changed.p,node->set_destination.p,n*
sizeof(
struct SFVec2f));
2294void do_TexCoordDamper2DTick_default(
void * ptr){
2297 if(NODE_NEEDS_COMPILING){
2300 n = node->set_destination.n;
2301 node->value_changed.n = n;
2302 node->value_changed.p = REALLOC(node->value_changed.p,n *
sizeof(
struct SFVec2f));
2303 memcpy(node->value_changed.p,node->set_destination.p,n*
sizeof(
struct SFVec2f));
2310 T->p = REALLOC(T->p,A->n *
sizeof(
struct SFVec2f));
2312 memcpy(T->p,A->p,A->n *
sizeof(
struct SFVec2f));
2317 T->n = min(A->n,B->n);
2318 T->p = REALLOC(T->p,T->n *
sizeof(
struct SFVec2f));
2320 sfvec2f_add(&T->p[i],&A->p[i],&B->p[i]);
2325 T->n = min(A->n,B->n);
2326 T->p = REALLOC(T->p,T->n *
sizeof(
struct SFVec2f));
2328 sfvec2f_dif(&T->p[i],&A->p[i],&B->p[i]);
2334 T->p = REALLOC(T->p,T->n *
sizeof(
struct SFVec2f));
2336 sfvec2f_scale(&T->p[i],&A->p[i],S);
2341 T->n = min(A->n,B->n);
2342 T->p = REALLOC(T->p,T->n *
sizeof(
struct SFVec2f));
2344 sfvec2f_lerp(&T->p[i],&A->p[i],&B->p[i],alpha);
2352 dist += sfvec2f_dist(&A->p[i]);
2357 if(A->n != B->n)
return FALSE;
2360 isame = isame && sfvec2f_same(&A->p[i],&B->p[i]);
2367void *mfvec2f_tmp [] = {&mfvec2f_tmps[0],&mfvec2f_tmps[1],&mfvec2f_tmps[2],&mfvec2f_tmps[3],&mfvec2f_tmps[4],&mfvec2f_tmps[5]};
2368ftype ftype_mfvec2f = {
2370 VOIDFN mfvec2f_copy,
2373 VOIDFN mfvec2f_scale,
2374 VOIDFN mfvec2f_lerp,
2375 FLOATFN mfvec2f_dist,
2379 VOIDPTR mfvec2f_tmp,
2381void do_TexCoordChaser2DTick(
void * ptr){
2386 if(!_node->_buffer){
2388 _node->_buffer = REALLOCN(node,_node->_buffer,Buffer_length *
sizeof(
struct Multi_Vec2f));
2389 node->_t = &ftype_mfvec2f;
2391 p->initialDestination = &_node->initialDestination;
2392 p->initialValue = &_node->initialValue;
2393 p->set_destination = &_node->set_destination;
2394 p->set_value = &_node->set_value;
2395 p->value_changed = &_node->value_changed;
2396 p->_buffer = _node->_buffer;
2397 p->_destination = &_node->_destination;
2398 p->_previousValue = &_node->_previousvalue;
2402 if(NODE_NEEDS_COMPILING){
2404 ftype *t = node->_t;
2406 node->isActive = TRUE;
2410 if(!t->same(p->set_destination,p->_destination))
2411 chaser_set_destination(node, Now);
2413 else if(!t->same(p->set_value,p->initialValue))
2414 chaser_set_value(node);
2418 chaser_tick(node,Now);
2421void do_TexCoordDamper2DTick(
void * ptr){
2425 if(!_node->_values){
2427 node->_t = &ftype_sfvec2f;
2429 _node->_values = REALLOCN(node,_node->_values,5 *
sizeof(
struct Multi_Vec2f));
2430 p->initialDestination = &_node->initialDestination;
2431 p->initialValue = &_node->initialValue;
2432 p->set_destination = &_node->set_destination;
2433 p->set_value = &_node->set_value;
2434 p->value_changed = &_node->value_changed;
2435 p->_input = &_node->_input;
2436 p->_values = _node->_values;
2441 if(NODE_NEEDS_COMPILING){
2444 ftype *t = node->_t;
2447 if(!t->same(p->set_destination,p->_input))
2449 damper_set_destination(node, p->set_destination);
2451 if(node->tau != node->_tau)
2452 damper_set_tau(node,node->tau);
2455 if(!t->same(p->initialValue,p->set_value))
2457 damper_set_value(node,p->set_value);
2461 tick_damper(node,TickTime());