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"
57}* ppComponent_Followers;
58void *Component_Followers_constructor(){
63void Component_Followers_init(
struct tComponent_Followers *t){
66 t->prv = Component_Followers_constructor();
68 ppComponent_Followers p = (ppComponent_Followers)t->prv;
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;
536 node->_bufferendtime= TickTime();
540 if(NODE_NEEDS_COMPILING){
544 node->isActive = TRUE;
548 if(!t->same(p->set_destination,p->_destination))
549 chaser_set_destination(node, Now);
551 else if(!t->same(p->set_value,p->initialValue))
552 chaser_set_value(node);
556 chaser_tick(node,Now);
581 node->_takefirstinput = TRUE;
583 damper_set_value(node,p->initialValue);
584 node->isActive = TRUE;
598 dist = t->dist(t->dif(t->tmp[0],t->arr(p->_values,0),p->_input));
603 float dist2 = t->dist(t->dif(t->tmp[0],t->arr(p->_values,1),t->arr(p->_values,0)));
604 if( dist2 > dist) dist= dist2;
610 float dist3 = t->dist(t->dif(t->tmp[0],t->arr(p->_values,2),t->arr(p->_values,1)));
611 if( dist3 > dist) dist= dist3;
617 float dist4 = t->dist(t->dif(t->tmp[0],t->arr(p->_values,3),t->arr(p->_values,2)));
618 if( dist4 > dist) dist= dist4;
624 float dist5 = t->dist(t->dif(t->tmp[0],t->arr(p->_values,4),t->arr(p->_values,3)));
625 if( dist5 > dist) dist= dist5;
636 node->_takefirstinput = FALSE;
639 t->copy(t->arr(p->_values,i),opos);
641 t->copy(p->value_changed, opos);
643 t->copy(p->initialValue,opos);
645 node->isActive = TRUE;
654 if(node->_takefirstinput)
656 node->_takefirstinput = FALSE;
657 damper_set_value(node,ipos);
660 if(!t->same(ipos,p->_input))
663 t->copy(p->_input,ipos);
664 node->isActive = TRUE;
702 node->_lasttick= now;
705 delta= now - node->_lasttick;
706 node->_lasttick= now;
707 alpha= exp(-delta / node->tau);
708 if(node->_takefirstinput)
716 if(node->order > 0 && node->tau != 0.0)
718 t->lerp(t->arr(p->_values,0),p->_input,t->arr(p->_values,0),(
float)alpha);
720 t->copy(t->arr(p->_values,0),p->_input);
726 if(node->order > 1 && node->tau != 0.0)
728 t->lerp(t->arr(p->_values,1),t->arr(p->_values,0),t->arr(p->_values,1),(
float)alpha);
730 t->copy(t->arr(p->_values,1),t->arr(p->_values,0));
736 if(node->order > 2 && node->tau != 0.0)
738 t->lerp(t->arr(p->_values,2),t->arr(p->_values,1),t->arr(p->_values,2),(
float)alpha);
740 t->copy(t->arr(p->_values,2),t->arr(p->_values,1));
747 if(node->order > 3 && node->tau != 0.0)
749 t->lerp(t->arr(p->_values,3),t->arr(p->_values,2),t->arr(p->_values,3),(
float)alpha);
751 t->copy(t->arr(p->_values,3),t->arr(p->_values,2));
757 if(node->order > 4 && node->tau != 0.0)
759 t->lerp(t->arr(p->_values,4),t->arr(p->_values,3),t->arr(p->_values,4),(
float)alpha);
761 t->copy(t->arr(p->_values,4),t->arr(p->_values,3));
763 dist= damper_GetDist(node);
765 if(dist < max(node->tolerance,.001f))
770 t->copy(t->arr(p->_values,i),p->_input);
773 t->copy(p->value_changed,p->_input);
775 node->isActive = FALSE;
780 t->copy(p->value_changed,t->arr(p->_values,4));
788void do_PositionDamperTick(
void * ptr){
794 node->_t = &ftype_sfvec3f;
796 _node->_values = REALLOCN(node,_node->_values,5 *
sizeof(
struct SFVec3f));
797 p->initialDestination = &_node->initialDestination;
798 p->initialValue = &_node->initialValue;
799 p->set_destination = &_node->set_destination;
800 p->set_value = &_node->set_value;
801 p->value_changed = &_node->value_changed;
802 p->_input = &_node->_input;
803 p->_values = _node->_values;
808 if(NODE_NEEDS_COMPILING){
814 if(!t->same(p->set_destination,p->_input))
816 damper_set_destination(node, p->set_destination);
818 if(node->tau != node->_tau)
819 damper_set_tau(node,node->tau);
822 if(!t->same(p->initialValue,p->set_value))
824 damper_set_value(node,p->set_value);
828 tick_damper(node,TickTime());
835 node->_destination = node->initialDestination;
836 buffer[0]= node->initialDestination;
837 for(C= 1; C<Buffer_length; C++ )
838 buffer[C]= node->initialValue;
839 node->_previousvalue= node->initialValue;
840 node->_steptime= node->duration / (double) Buffer_length;
848 Frac = (Now - node->_bufferendtime) / node->_steptime;
856 int NumToShift= (int)floor(Frac);
857 Frac-= (double) NumToShift;
858 if(NumToShift < Buffer_length)
861 node->_previousvalue= buffer[Buffer_length - NumToShift];
862 for( C= Buffer_length - 1; C>=NumToShift; C-- )
863 buffer[C]= buffer[C - NumToShift];
864 for( C= 0; C<NumToShift; C++ )
869 float tmp1[3],tmp2[3];
870 float Alpha= (float)C / (
float)NumToShift;
881 vecscale3f(tmp1,buffer[NumToShift].c,Alpha);
882 vecscale3f(tmp2,node->_destination.c,1.0f - Alpha);
883 vecadd3f(tmp3,tmp1,tmp2);
884 veccopy3f(buffer[C].c,tmp3);
886 vecadd3f(buffer[C].c,vecscale3f(tmp1,buffer[NumToShift].c,Alpha),vecscale3f(tmp2,node->_destination.c,1.0f - Alpha));
900 node->_previousvalue= NumToShift == Buffer_length? buffer[0] : node->_destination;
902 for( C= 0; C<Buffer_length; C++ )
903 buffer[C]= node->_destination;
905 node->_bufferendtime+= NumToShift * node->_steptime;
914 node->_destination= Dest;
918 chaser_UpdateBuffer(node, Now);
928double chaser_StepResponseCore(
double T)
930 return .5 - .5 * cos(T * PI);
936 if(t > node->duration)
940 return chaser_StepResponseCore(t / node->duration);
953 if(!node->_bufferendtime)
955 node->_bufferendtime= Now;
956 node->value_changed= node->initialValue;
959 Frac= chaser_UpdateBuffer(node, Now);
973 Output= node->_previousvalue;
975 vecdif3f(DeltaIn.c,buffer[Buffer_length - 1].c,node->_previousvalue.c);
978 vecscale3f(DeltaOut.c,DeltaIn.c,(
float)chaser_StepResponse(node,((
double)Buffer_length - 1.0 + Frac) * node->_steptime));
980 vecadd3f(Output.c,Output.c,DeltaOut.c);
981 for(C= Buffer_length - 2; C>=0; C-- )
984 vecdif3f(DeltaIn.c,buffer[C].c,buffer[C+1].c);
986 vecscale3f(DeltaOut.c,DeltaIn.c,(
float)chaser_StepResponse(node,((
double)C + Frac) * node->_steptime));
988 vecadd3f(Output.c,Output.c,DeltaOut.c);
990 if(!vecsame3f(Output.c,node->value_changed.c)){
991 node->value_changed= Output;
997 node->value_changed= opos;
998 node->initialValue = opos;
1000 node->isActive = TRUE;
1005void do_PositionChaserTick(
void * ptr){
1010 node->_buffer = realloc(node->_buffer,Buffer_length *
sizeof(
struct SFVec3f));
1014 if(NODE_NEEDS_COMPILING){
1015 node->isActive = TRUE;
1018 if(!vecsame3f(node->set_destination.c,node->_destination.c))
1019 chaser_set_destination(node, node->set_destination,Now);
1020 else if(!vecsame3f(node->set_value.c,node->initialValue.c))
1021 chaser_set_value(node,node->set_value);
1025 chaser_tick(node,Now);
1047 node->_takefirstinput = TRUE;
1048 damper_set_value(node,node->initialValue);
1049 node->isActive = TRUE;
1059 dist = veclength3f(vecdif3f(tmp,values[0].c,node->_input.c));
1063 float dist2 = veclength3f(vecdif3f(tmp,values[1].c,values[0].c));
1064 if( dist2 > dist) dist= dist2;
1069 float dist3 = veclength3f(vecdif3f(tmp,values[2].c,values[1].c));
1070 if( dist3 > dist) dist= dist3;
1075 float dist4 = veclength3f(vecdif3f(tmp,values[3].c,values[2].c));
1076 if( dist4 > dist) dist= dist4;
1081 float dist5 = veclength3f(vecdif3f(tmp,values[4].c, values[3].c));
1082 if( dist5 > dist) dist= dist5;
1089 node->_takefirstinput = FALSE;
1090 values[0]= values[1]= values[2]= values[3]= values[4]= opos;
1091 node->value_changed= opos;
1092 node->initialValue = opos;
1094 node->isActive = TRUE;
1100 if(node->_takefirstinput)
1102 node->_takefirstinput = FALSE;
1103 damper_set_value(node,ipos);
1105 if(!vecsame3f(ipos.c,node->_input.c))
1107 node->_input = ipos;
1108 node->isActive = TRUE;
1115 float tmp[3], tmp2[3];
1118 vecdif3f(tmp,b.c,a.c);
1119 vecscale3f(tmp2,tmp,(
float)alpha);
1120 vecadd3f(ret.c,a.c,tmp2);
1122 vecadd3f(ret.c,a.c,vecscale3f(tmp2,vecdif3f(tmp,b.c,a.c),(
float)alpha));
1133 if(!node->_lasttick)
1135 node->_lasttick= now;
1138 delta= now - node->_lasttick;
1139 node->_lasttick= now;
1140 alpha= exp(-delta / node->tau);
1141 if(node->_takefirstinput)
1144 values[0]= node->order > 0 && node->tau != 0.0
1146 ? damper_diftimes(node->_input,values[0],alpha)
1149 values[1]= node->order > 1 && node->tau != 0.0
1151 ? damper_diftimes(values[0],values[1],alpha)
1154 values[2]= node->order > 2 && node->tau != 0.0
1156 ? damper_diftimes(values[1],values[2],alpha)
1159 values[3]= node->order > 3 && node->tau != 0.0
1161 ? damper_diftimes(values[2],values[3],alpha)
1164 values[4]= node->order > 4 && node->tau != 0.0
1166 ? damper_diftimes(values[3],values[4],alpha)
1169 dist= damper_GetDist(node);
1171 if(dist < max(node->tolerance,.001f))
1173 values[0]= values[1]= values[2]= values[3]= values[4]= node->_input;
1174 node->value_changed= node->_input;
1176 node->isActive = FALSE;
1180 node->value_changed= values[4];
1187void do_PositionDamperTick(
void * ptr){
1191 node->_values = realloc(node->_values,5 *
sizeof(
struct SFVec3f));
1196 if(NODE_NEEDS_COMPILING){
1198 if(!vecsame3f(node->set_destination.c,node->_input.c))
1199 damper_set_destination(node, node->set_destination);
1201 if(node->tau != node->_tau)
1202 damper_set_tau(node,node->tau);
1204 if(!vecsame3f(node->initialValue.c,node->set_value.c))
1205 damper_set_value(node,node->set_value);
1209 tick_positiondamper(node,TickTime());
1214void do_ColorChaserTick_default(
void * ptr){
1217 if(NODE_NEEDS_COMPILING){
1219 veccopy3f(node->value_changed.c, node->set_destination.c);
1224void do_ColorDamperTick_default(
void * ptr){
1227 if(NODE_NEEDS_COMPILING){
1229 veccopy3f(node->value_changed.c, node->set_destination.c);
1234void do_ColorChaserTick(
void * ptr){
1239 if(!_node->_buffer){
1241 _node->_buffer = REALLOCN(node,_node->_buffer,Buffer_length *
sizeof(
struct SFColor));
1242 node->_t = &ftype_sfvec3f;
1244 p->initialDestination = &_node->initialDestination;
1245 p->initialValue = &_node->initialValue;
1246 p->set_destination = &_node->set_destination;
1247 p->set_value = &_node->set_value;
1248 p->value_changed = &_node->value_changed;
1249 p->_buffer = _node->_buffer;
1250 p->_destination = &_node->_destination;
1251 p->_previousValue = &_node->_previousvalue;
1252 node->_bufferendtime= TickTime();
1256 if(NODE_NEEDS_COMPILING){
1258 ftype *t = node->_t;
1260 node->isActive = TRUE;
1264 if(!t->same(p->set_destination,p->_destination))
1265 chaser_set_destination(node, Now);
1267 else if(!t->same(p->set_value,p->initialValue))
1268 chaser_set_value(node);
1272 chaser_tick(node,Now);
1275void do_ColorDamperTick(
void * ptr){
1279 if(!_node->_values){
1281 node->_t = &ftype_sfvec3f;
1283 _node->_values = REALLOCN(node,_node->_values,5 *
sizeof(
struct SFColor));
1284 p->initialDestination = &_node->initialDestination;
1285 p->initialValue = &_node->initialValue;
1286 p->set_destination = &_node->set_destination;
1287 p->set_value = &_node->set_value;
1288 p->value_changed = &_node->value_changed;
1289 p->_input = &_node->_input;
1290 p->_values = _node->_values;
1295 if(NODE_NEEDS_COMPILING){
1298 ftype *t = node->_t;
1301 if(!t->same(p->set_destination,p->_input))
1303 damper_set_destination(node, p->set_destination);
1305 if(node->tau != node->_tau)
1306 damper_set_tau(node,node->tau);
1309 if(!t->same(p->initialValue,p->set_value))
1311 damper_set_value(node,p->set_value);
1315 tick_damper(node,TickTime());
1318void do_CoordinateChaserTick_default(
void * ptr){
1321 if(NODE_NEEDS_COMPILING){
1324 n = node->set_destination.n;
1325 node->value_changed.n = n;
1326 node->value_changed.p = REALLOC(node->value_changed.p,n *
sizeof(
struct SFVec3f));
1327 memcpy(node->value_changed.p,node->set_destination.p,n*
sizeof(
struct SFVec3f));
1332void do_CoordinateDamperTick_default(
void * ptr){
1335 if(NODE_NEEDS_COMPILING){
1338 n = node->set_destination.n;
1339 node->value_changed.n = n;
1340 node->value_changed.p = REALLOC(node->value_changed.p,n *
sizeof(
struct SFVec3f));
1341 memcpy(node->value_changed.p,node->set_destination.p,n*
sizeof(
struct SFVec3f));
1348 T->p = REALLOC(T->p,A->n *
sizeof(
struct SFVec3f));
1350 memcpy(T->p,A->p,A->n *
sizeof(
struct SFVec3f));
1355 T->n = min(A->n,B->n);
1356 T->p = REALLOC(T->p,T->n *
sizeof(
struct SFVec3f));
1358 sfvec3f_add(&T->p[i],&A->p[i],&B->p[i]);
1363 T->n = min(A->n,B->n);
1364 T->p = REALLOC(T->p,T->n *
sizeof(
struct SFVec3f));
1366 sfvec3f_dif(&T->p[i],&A->p[i],&B->p[i]);
1372 T->p = REALLOC(T->p,T->n *
sizeof(
struct SFVec3f));
1374 sfvec3f_scale(&T->p[i],&A->p[i],S);
1380 T->p = REALLOC(T->p,T->n *
sizeof(
struct SFVec3f));
1382 sfvec3f_lerp(&T->p[i],&A->p[i],&B->p[i],alpha);
1389 dist += sfvec3f_dist(&A->p[i]);
1394 if(A->n != B->n)
return FALSE;
1397 isame = isame && sfvec3f_same(&A->p[i],&B->p[i]);
1404void *mfvec3f_tmp [] = {&mfvec3f_tmps[0],&mfvec3f_tmps[1],&mfvec3f_tmps[2],&mfvec3f_tmps[3],&mfvec3f_tmps[4],&mfvec3f_tmps[5]};
1405ftype ftype_mfvec3f = {
1407 VOIDFN mfvec3f_copy,
1410 VOIDFN mfvec3f_scale,
1411 VOIDFN mfvec3f_lerp,
1412 FLOATFN mfvec3f_dist,
1416 VOIDPTR mfvec3f_tmp,
1426 memcpy(T->c, A->c,
sizeof(
struct SFRotation));
1429 vrmlrot_to_quaternion(&qA, (
double) A->c[0],
1430 (
double) A->c[1], (
double) A->c[2], (
double) A->c[3]);
1433 quaternion_inverse(&qT,&qA);
1436 quaternion_to_vrmlrot(&qT, &x, &y, &z, &a);
1438 T->c[0] = (float) x;
1439 T->c[1] = (float) y;
1440 T->c[2] = (float) z;
1441 T->c[3] = (float) a;
1445 Quaternion qA,qB,qT;
1449 vrmlrot_to_quaternion(&qA, (
double) A->c[0],
1450 (
double) A->c[1], (
double) A->c[2], (
double) A->c[3]);
1452 vrmlrot_to_quaternion(&qB, (
double) B->c[0],
1453 (
double) B->c[1], (
double) B->c[2], (
double) B->c[3]);
1456 quaternion_multiply(&qT,&qA,&qB);
1459 quaternion_to_vrmlrot(&qT, &x, &y, &z, &a);
1461 T->c[0] = (float) x;
1462 T->c[1] = (float) y;
1463 T->c[2] = (float) z;
1464 T->c[3] = (float) a;
1469 memcpy(T->c, A->c,
sizeof(
struct SFRotation));
1474 sfrotation_multiply(T,A,B);
1480 Quaternion qA,qB,qBI,qT;
1484 vrmlrot_to_quaternion(&qA, (
double) A->c[0],
1485 (
double) A->c[1], (
double) A->c[2], (
double) A->c[3]);
1487 vrmlrot_to_quaternion(&qB, (
double) B->c[0],
1488 (
double) B->c[1], (
double) B->c[2], (
double) B->c[3]);
1491 quaternion_inverse(&qBI,&qB);
1493 quaternion_multiply(&qT,&qBI,&qA);
1496 quaternion_to_vrmlrot(&qT, &x, &y, &z, &a);
1498 T->c[0] = (float) x;
1499 T->c[1] = (float) y;
1500 T->c[2] = (float) z;
1501 T->c[3] = (float) a;
1518 sfrotation_copy(T,A);
1523 if (APPROX(alpha, 0.0f)) {
1524 memcpy(T->c,A->c,4*
sizeof(
float));
1525 }
else if (APPROX(alpha, 1.0f)) {
1526 memcpy(T->c,B->c,4*
sizeof(
float));
1528 Quaternion quatA, quatB, quatT;
1530 vrmlrot_to_quaternion(&quatA,
1536 vrmlrot_to_quaternion(&quatB,
1542 quaternion_slerp(&quatT, &quatA, &quatB, (
double)alpha);
1543 quaternion_to_vrmlrot(&quatT,&x,&y,&z,&a);
1545 T->c[0] = (float) x;
1546 T->c[1] = (float) y;
1547 T->c[2] = (float) z;
1548 T->c[3] = (float) a;
1559 isame = isame && A->c[i] == B->c[i];
1566void *sfrotation_tmp [] = {&sfrotation_tmps[0],&sfrotation_tmps[1],&sfrotation_tmps[2],&sfrotation_tmps[3],&sfrotation_tmps[4],&sfrotation_tmps[5]};
1567ftype ftype_sfrotation = {
1568 FIELDTYPE_SFRotation,
1569 VOIDFN sfrotation_copy,
1570 VOIDFN sfrotation_add,
1571 VOIDFN sfrotation_dif,
1572 VOIDFN sfrotation_scale,
1573 VOIDFN sfrotation_slerp,
1574 FLOATFN sfrotation_dist,
1575 INTFN sfrotation_same,
1576 INTFN sfrotation_same,
1577 VOIDFN sfrotation_arr,
1578 VOIDPTR sfrotation_tmp,
1581void do_OrientationChaserTick(
void * ptr){
1586 if(!_node->_buffer){
1588 _node->_buffer = REALLOCN(node,_node->_buffer,Buffer_length *
sizeof(
struct SFRotation));
1589 node->_t = &ftype_sfrotation;
1591 p->initialDestination = &_node->initialDestination;
1592 p->initialValue = &_node->initialValue;
1593 p->set_destination = &_node->set_destination;
1594 p->set_value = &_node->set_value;
1595 p->value_changed = &_node->value_changed;
1596 p->_buffer = _node->_buffer;
1597 p->_destination = &_node->_destination;
1598 p->_previousValue = &_node->_previousvalue;
1599 node->_bufferendtime= TickTime();
1603 if(NODE_NEEDS_COMPILING){
1605 ftype *t = node->_t;
1606 static int count = 0;
1607 node->isActive = TRUE;
1611 if(!t->same(p->set_destination,p->_destination))
1612 chaser_set_destination(node, Now);
1614 else if(!t->same(p->set_value,p->initialValue))
1615 chaser_set_value(node);
1620 chaser_tick(node,Now);
1623void do_OrientationDamperTick(
void * ptr){
1627 if(!_node->_values){
1629 node->_t = &ftype_sfrotation;
1631 _node->_values = REALLOCN(node,_node->_values,5 *
sizeof(
struct SFRotation));
1632 p->initialDestination = &_node->initialDestination;
1633 p->initialValue = &_node->initialValue;
1634 p->set_destination = &_node->set_destination;
1635 p->set_value = &_node->set_value;
1636 p->value_changed = &_node->value_changed;
1637 p->_input = &_node->_input;
1638 p->_values = _node->_values;
1643 if(NODE_NEEDS_COMPILING){
1646 ftype *t = node->_t;
1649 if(!t->same(p->set_destination,p->_input))
1651 damper_set_destination(node, p->set_destination);
1653 if(node->tau != node->_tau)
1654 damper_set_tau(node,node->tau);
1657 if(!t->same(p->initialValue,p->set_value))
1659 damper_set_value(node,p->set_value);
1663 tick_damper(node,TickTime());
1667void do_OrientationChaserTick_default(
void * ptr){
1670 if(NODE_NEEDS_COMPILING){
1672 veccopy3f(node->value_changed.c, node->set_destination.c);
1673 node->value_changed.c[3] = node->set_destination.c[3];
1678void do_OrientationDamperTick_default(
void * ptr){
1681 if(NODE_NEEDS_COMPILING){
1683 veccopy3f(node->value_changed.c, node->set_destination.c);
1684 node->value_changed.c[3] = node->set_destination.c[3];
1695 node->_destination = node->initialDestination;
1696 buffer[0]= node->initialDestination;
1697 for(C= 1; C<Buffer_length; C++ )
1698 buffer[C]= node->initialValue;
1699 node->_previousvalue= node->initialValue;
1700 node->_steptime= node->duration / (double) Buffer_length;
1708 Frac = (Now - node->_bufferendtime) / node->_steptime;
1716 int NumToShift= (int)floor(Frac);
1717 Frac-= (double) NumToShift;
1718 if(NumToShift < Buffer_length)
1721 node->_previousvalue= buffer[Buffer_length - NumToShift];
1722 for( C= Buffer_length - 1; C>=NumToShift; C-- )
1723 buffer[C]= buffer[C - NumToShift];
1724 for( C= 0; C<NumToShift; C++ )
1729 float Alpha= (float)C / (
float)NumToShift;
1732 if(1)sfrotation_slerp(&buffer[C],&node->_destination,&buffer[NumToShift],Alpha);
1733 else sfrotation_slerp(&buffer[C],&buffer[NumToShift],&node->_destination,Alpha);
1746 node->_previousvalue= NumToShift == Buffer_length? buffer[0] : node->_destination;
1748 for( C= 0; C<Buffer_length; C++ )
1749 buffer[C]= node->_destination;
1751 node->_bufferendtime+= NumToShift * node->_steptime;
1760 node->_destination= Dest;
1764 orichaser_UpdateBuffer(node, Now);
1773double orichaser_StepResponseCore(
double T)
1775 return .5 - .5 * cos(T * PI);
1781 if(t > node->duration)
1785 return orichaser_StepResponseCore(t / node->duration);
1799 if(!node->_bufferendtime)
1801 node->_bufferendtime= Now;
1802 node->value_changed= node->initialValue;
1805 Frac= orichaser_UpdateBuffer(node, Now);
1820 Output= node->_previousvalue;
1823 sfrotation_inverse(&tmp0,&node->_previousvalue);
1824 sfrotation_multiply(&DeltaIn,&tmp0,&buffer[Buffer_length -1]);
1826 sfrotation_dif(&DeltaIn,&buffer[Buffer_length -1],&node->_previousvalue);
1829 Alpha = orichaser_StepResponse(node,((
double)Buffer_length - 1.0 + Frac) * node->_steptime);
1832 sfrotation_multiply(&tmp0,&Output,&DeltaIn);
1834 sfrotation_add(&tmp0,&Output,&DeltaIn);
1836 sfrotation_slerp(&Output,&Output,&tmp0,(
float)Alpha);
1837 for(C= Buffer_length - 2; C>=0; C-- )
1841 sfrotation_inverse(&tmp0,&buffer[C + 1]);
1842 sfrotation_multiply(&DeltaIn,&tmp0,&buffer[C]);
1844 sfrotation_dif(&DeltaIn,&buffer[C],&buffer[C+1]);
1846 Alpha = orichaser_StepResponse(node,((
float)C + Frac) * node->_steptime);
1849 sfrotation_multiply(&tmp0,&Output,&DeltaIn);
1851 sfrotation_add(&tmp0,&Output,&DeltaIn);
1853 sfrotation_slerp(&Output,&Output,&tmp0,(
float)Alpha);
1855 if(!sfrotation_same(&Output,&node->value_changed)){
1856 node->value_changed= Output;
1862 node->value_changed= opos;
1863 node->initialValue = opos;
1865 node->isActive = TRUE;
1870void do_OrientationChaserTick_oldway_works(
void * ptr){
1875 node->_buffer = REALLOCN(node,node->_buffer,Buffer_length *
sizeof(
struct SFRotation));
1876 orichaser_init(node);
1879 if(NODE_NEEDS_COMPILING){
1881 node->isActive = TRUE;
1884 if(!sfrotation_same(&node->set_destination,&node->_destination))
1885 orichaser_set_destination(node, node->set_destination,Now);
1886 else if(!sfrotation_same(&node->set_value,&node->initialValue))
1887 orichaser_set_value(node,node->set_value);
1891 orichaser_tick(node,Now);
1898void do_CoordinateChaserTick(
void * ptr){
1903 if(!_node->_buffer){
1905 _node->_buffer = REALLOCN(node,_node->_buffer,Buffer_length *
sizeof(
struct Multi_Vec3f));
1906 memset(_node->_buffer,0,Buffer_length *
sizeof(
struct Multi_Vec3f));
1907 node->_t = &ftype_mfvec3f;
1909 p->initialDestination = &_node->initialDestination;
1910 p->initialValue = &_node->initialValue;
1911 p->set_destination = &_node->set_destination;
1912 p->set_value = &_node->set_value;
1913 p->value_changed = &_node->value_changed;
1914 p->_buffer = _node->_buffer;
1915 p->_destination = &_node->_destination;
1916 p->_previousValue = &_node->_previousvalue;
1917 node->_bufferendtime= TickTime();
1921 if(NODE_NEEDS_COMPILING){
1923 ftype *t = node->_t;
1925 node->isActive = TRUE;
1929 if(!t->same(p->set_destination,p->_destination))
1930 chaser_set_destination(node, Now);
1932 else if(!t->same(p->set_value,p->initialValue))
1933 chaser_set_value(node);
1937 chaser_tick(node,Now);
1940void do_CoordinateDamperTick(
void * ptr){
1944 if(!_node->_values){
1946 node->_t = &ftype_mfvec3f;
1948 _node->_values = REALLOCN(node,_node->_values,5 *
sizeof(
struct Multi_Vec3f));
1949 memset(_node->_values,0,5*
sizeof(
struct Multi_Vec3f));
1950 p->initialDestination = &_node->initialDestination;
1951 p->initialValue = &_node->initialValue;
1952 p->set_destination = &_node->set_destination;
1953 p->set_value = &_node->set_value;
1954 p->value_changed = &_node->value_changed;
1955 p->_input = &_node->_input;
1956 p->_values = _node->_values;
1961 if(NODE_NEEDS_COMPILING){
1964 ftype *t = node->_t;
1967 if(!t->same(p->set_destination,p->_input))
1969 damper_set_destination(node, p->set_destination);
1971 if(node->tau != node->_tau)
1972 damper_set_tau(node,node->tau);
1975 if(!t->same(p->initialValue,p->set_value))
1977 damper_set_value(node,p->set_value);
1981 tick_damper(node,TickTime());
1988void do_PositionChaser2DTick_default(
void * ptr){
1991 if(NODE_NEEDS_COMPILING){
1993 veccopy2f(node->value_changed.c, node->set_destination.c);
1999void do_PositionDamper2DTick_default(
void * ptr){
2002 if(NODE_NEEDS_COMPILING){
2004 veccopy2f(node->value_changed.c, node->set_destination.c);
2012 veccopy2f(T->c,A->c);
2016 vecadd2f(T->c,A->c,B->c);
2020 vecdif2f(T->c,A->c,B->c);
2024 vecscale2f(T->c,A->c,S);
2028 veclerp2f(T->c,A->c,B->c,alpha);
2031float sfvec2f_dist(
struct SFVec2f* A){
2032 return veclength2f(A->c);
2035 return vecsame2f(A->c,B->c);
2040struct SFVec2f sfvec2f_tmps[6];
2041void *sfvec2f_tmp [] = {&sfvec2f_tmps[0],&sfvec2f_tmps[1],&sfvec2f_tmps[2],&sfvec2f_tmps[3],&sfvec2f_tmps[4],&sfvec2f_tmps[5]};
2042ftype ftype_sfvec2f = {
2044 VOIDFN sfvec2f_copy,
2047 VOIDFN sfvec2f_scale,
2048 VOIDFN sfvec2f_lerp,
2049 FLOATFN sfvec2f_dist,
2053 VOIDPTR sfvec2f_tmp,
2056void do_PositionChaser2DTick(
void * ptr){
2061 if(!_node->_buffer){
2063 _node->_buffer = REALLOCN(node,_node->_buffer,Buffer_length *
sizeof(
struct SFVec2f));
2064 node->_t = &ftype_sfvec2f;
2066 p->initialDestination = &_node->initialDestination;
2067 p->initialValue = &_node->initialValue;
2068 p->set_destination = &_node->set_destination;
2069 p->set_value = &_node->set_value;
2070 p->value_changed = &_node->value_changed;
2071 p->_buffer = _node->_buffer;
2072 p->_destination = &_node->_destination;
2073 p->_previousValue = &_node->_previousvalue;
2074 node->_bufferendtime= TickTime();
2078 if(NODE_NEEDS_COMPILING){
2080 ftype *t = node->_t;
2082 node->isActive = TRUE;
2086 if(!t->same(p->set_destination,p->_destination))
2087 chaser_set_destination(node, Now);
2089 else if(!t->same(p->set_value,p->initialValue))
2090 chaser_set_value(node);
2094 chaser_tick(node,Now);
2096void do_PositionDamper2DTick(
void * ptr){
2100 if(!_node->_values){
2102 node->_t = &ftype_sfvec2f;
2104 _node->_values = REALLOCN(node,_node->_values,5 *
sizeof(
struct SFVec2f));
2105 p->initialDestination = &_node->initialDestination;
2106 p->initialValue = &_node->initialValue;
2107 p->set_destination = &_node->set_destination;
2108 p->set_value = &_node->set_value;
2109 p->value_changed = &_node->value_changed;
2110 p->_input = &_node->_input;
2111 p->_values = _node->_values;
2116 if(NODE_NEEDS_COMPILING){
2119 ftype *t = node->_t;
2122 if(!t->same(p->set_destination,p->_input))
2124 damper_set_destination(node, p->set_destination);
2126 if(node->tau != node->_tau)
2127 damper_set_tau(node,node->tau);
2130 if(!t->same(p->initialValue,p->set_value))
2132 damper_set_value(node,p->set_value);
2136 tick_damper(node,TickTime());
2140void do_ScalarChaserTick_default(
void * ptr){
2143 if(NODE_NEEDS_COMPILING){
2145 node->value_changed = node->set_destination;
2150void do_ScalarDamperTick_default(
void * ptr){
2153 if(NODE_NEEDS_COMPILING){
2155 node->value_changed = node->set_destination;
2160float *scalar_copy(
float* T,
float *A){
2164float *scalar_add(
float* T,
float *A,
float *B){
2168float *scalar_dif(
float* T,
float *A,
float *B){
2172float *scalar_scale(
float* T,
float *A,
float S){
2176float *scalar_lerp(
float* T,
float *A,
float *B,
float alpha){
2177 *T = (1.0f -alpha)*(*A) + alpha*(*B);
2180float scalar_dist(
float* A){
2181 return (
float)fabs(*A);
2183int scalar_same(
float *A,
float *B){
2184 return *A == *B ? TRUE : FALSE;
2186float *scalar_arr(
float *A,
int i){
2189float scalar_tmps[6];
2190void *scalar_tmp [] = {&scalar_tmps[0],&scalar_tmps[1],&scalar_tmps[2],&scalar_tmps[3],&scalar_tmps[4],&scalar_tmps[5]};
2191ftype ftype_scalar = {
2196 VOIDFN scalar_scale,
2198 FLOATFN scalar_dist,
2205void do_ScalarChaserTick(
void * ptr){
2210 if(!_node->_buffer){
2212 _node->_buffer = REALLOCN(node,_node->_buffer,Buffer_length *
sizeof(
float));
2213 node->_t = &ftype_scalar;
2215 p->initialDestination = &_node->initialDestination;
2216 p->initialValue = &_node->initialValue;
2217 p->set_destination = &_node->set_destination;
2218 p->set_value = &_node->set_value;
2219 p->value_changed = &_node->value_changed;
2220 p->_buffer = _node->_buffer;
2221 p->_destination = &_node->_destination;
2222 p->_previousValue = &_node->_previousvalue;
2223 node->_bufferendtime= TickTime();
2227 if(NODE_NEEDS_COMPILING){
2229 ftype *t = node->_t;
2231 node->isActive = TRUE;
2235 if(!t->same(p->set_destination,p->_destination))
2236 chaser_set_destination(node, Now);
2238 else if(!t->same(p->set_value,p->initialValue))
2239 chaser_set_value(node);
2243 chaser_tick(node,Now);
2245void do_ScalarDamperTick(
void * ptr){
2249 if(!_node->_values){
2251 node->_t = &ftype_scalar;
2253 _node->_values = REALLOCN(node,_node->_values,5 *
sizeof(
float));
2254 p->initialDestination = &_node->initialDestination;
2255 p->initialValue = &_node->initialValue;
2256 p->set_destination = &_node->set_destination;
2257 p->set_value = &_node->set_value;
2258 p->value_changed = &_node->value_changed;
2259 p->_input = &_node->_input;
2260 p->_values = _node->_values;
2265 if(NODE_NEEDS_COMPILING){
2268 ftype *t = node->_t;
2271 if(!t->same(p->set_destination,p->_input))
2273 damper_set_destination(node, p->set_destination);
2275 if(node->tau != node->_tau)
2276 damper_set_tau(node,node->tau);
2279 if(!t->same(p->initialValue,p->set_value))
2281 damper_set_value(node,p->set_value);
2285 tick_damper(node,TickTime());
2288void do_TexCoordChaser2DTick_default(
void * ptr){
2291 if(NODE_NEEDS_COMPILING){
2294 n = node->set_destination.n;
2295 node->value_changed.n = n;
2296 node->value_changed.p = REALLOC(node->value_changed.p,n *
sizeof(
struct SFVec2f));
2297 memcpy(node->value_changed.p,node->set_destination.p,n*
sizeof(
struct SFVec2f));
2302void do_TexCoordDamper2DTick_default(
void * ptr){
2305 if(NODE_NEEDS_COMPILING){
2308 n = node->set_destination.n;
2309 node->value_changed.n = n;
2310 node->value_changed.p = REALLOC(node->value_changed.p,n *
sizeof(
struct SFVec2f));
2311 memcpy(node->value_changed.p,node->set_destination.p,n*
sizeof(
struct SFVec2f));
2318 T->p = REALLOC(T->p,A->n *
sizeof(
struct SFVec2f));
2320 memcpy(T->p,A->p,A->n *
sizeof(
struct SFVec2f));
2325 T->n = min(A->n,B->n);
2326 T->p = REALLOC(T->p,T->n *
sizeof(
struct SFVec2f));
2328 sfvec2f_add(&T->p[i],&A->p[i],&B->p[i]);
2333 T->n = min(A->n,B->n);
2334 T->p = REALLOC(T->p,T->n *
sizeof(
struct SFVec2f));
2336 sfvec2f_dif(&T->p[i],&A->p[i],&B->p[i]);
2342 T->p = REALLOC(T->p,T->n *
sizeof(
struct SFVec2f));
2344 sfvec2f_scale(&T->p[i],&A->p[i],S);
2349 T->n = min(A->n,B->n);
2350 T->p = REALLOC(T->p,T->n *
sizeof(
struct SFVec2f));
2352 sfvec2f_lerp(&T->p[i],&A->p[i],&B->p[i],alpha);
2360 dist += sfvec2f_dist(&A->p[i]);
2365 if(A->n != B->n)
return FALSE;
2368 isame = isame && sfvec2f_same(&A->p[i],&B->p[i]);
2375void *mfvec2f_tmp [] = {&mfvec2f_tmps[0],&mfvec2f_tmps[1],&mfvec2f_tmps[2],&mfvec2f_tmps[3],&mfvec2f_tmps[4],&mfvec2f_tmps[5]};
2376ftype ftype_mfvec2f = {
2378 VOIDFN mfvec2f_copy,
2381 VOIDFN mfvec2f_scale,
2382 VOIDFN mfvec2f_lerp,
2383 FLOATFN mfvec2f_dist,
2387 VOIDPTR mfvec2f_tmp,
2389void do_TexCoordChaser2DTick(
void * ptr){
2394 if(!_node->_buffer){
2396 _node->_buffer = REALLOCN(node,_node->_buffer,Buffer_length *
sizeof(
struct Multi_Vec2f));
2397 memset(_node->_buffer,0,Buffer_length *
sizeof(
struct Multi_Vec2f));
2398 node->_t = &ftype_mfvec2f;
2400 p->initialDestination = &_node->initialDestination;
2401 p->initialValue = &_node->initialValue;
2402 p->set_destination = &_node->set_destination;
2403 p->set_value = &_node->set_value;
2404 p->value_changed = &_node->value_changed;
2405 p->_buffer = _node->_buffer;
2406 p->_destination = &_node->_destination;
2407 p->_previousValue = &_node->_previousvalue;
2408 node->_bufferendtime= TickTime();
2412 if(NODE_NEEDS_COMPILING){
2414 ftype *t = node->_t;
2416 node->isActive = TRUE;
2420 if(!t->same(p->set_destination,p->_destination))
2421 chaser_set_destination(node, Now);
2423 else if(!t->same(p->set_value,p->initialValue))
2424 chaser_set_value(node);
2428 chaser_tick(node,Now);
2431void do_TexCoordDamper2DTick(
void * ptr){
2435 if(!_node->_values){
2437 node->_t = &ftype_mfvec2f;
2439 _node->_values = REALLOCN(node,_node->_values,5 *
sizeof(
struct Multi_Vec2f));
2440 memset(_node->_values,0,5*
sizeof(
struct Multi_Vec2f));
2441 p->initialDestination = &_node->initialDestination;
2442 p->initialValue = &_node->initialValue;
2443 p->set_destination = &_node->set_destination;
2444 p->set_value = &_node->set_value;
2445 p->value_changed = &_node->value_changed;
2446 p->_input = &_node->_input;
2447 p->_values = _node->_values;
2452 if(NODE_NEEDS_COMPILING){
2455 ftype *t = node->_t;
2458 if(!t->same(p->set_destination,p->_input))
2460 damper_set_destination(node, p->set_destination);
2462 if(node->tau != node->_tau)
2463 damper_set_tau(node,node->tau);
2466 if(!t->same(p->initialValue,p->set_value))
2468 damper_set_value(node,p->set_value);
2472 tick_damper(node,TickTime());