FreeWRL / FreeX3D 4.3.0
Component_Followers.c
1/*
2
3
4X3D Followers Component
5
6*/
7
8
9/****************************************************************************
10 This file is part of the FreeWRL/FreeX3D Distribution.
11
12 Copyright 2009 CRC Canada. (http://www.crc.gc.ca)
13
14 FreeWRL/FreeX3D is free software: you can redistribute it and/or modify
15 it under the terms of the GNU Lesser Public License as published by
16 the Free Software Foundation, either version 3 of the License, or
17 (at your option) any later version.
18
19 FreeWRL/FreeX3D is distributed in the hope that it will be useful,
20 but WITHOUT ANY WARRANTY; without even the implied warranty of
21 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
22 GNU General Public License for more details.
23
24 You should have received a copy of the GNU General Public License
25 along with FreeWRL/FreeX3D. If not, see <http://www.gnu.org/licenses/>.
26****************************************************************************/
27
28#include <config.h>
29#include <system.h>
30#include <display.h>
31#include <internal.h>
32#include <iglobal.h>
33#include <libFreeWRL.h>
34
35#include "../vrml_parser/Structs.h"
36#include "../vrml_parser/CRoutes.h"
37#include "../main/headers.h"
38
39#include "../world_script/fieldSet.h"
40#include "../x3d_parser/Bindable.h"
41#include "Collision.h"
42#include "quaternion.h"
43#include "Viewer.h"
44#include "../opengl/Frustum.h"
45#include "../opengl/Material.h"
46#include "../opengl/OpenGL_Utils.h"
47#include "../input/EAIHelpers.h" /* for newASCIIString() */
48
49#include "Polyrep.h"
50#include "LinearAlgebra.h"
51//#include "Component_Followers.h"
52#include "Children.h"
53
54
55typedef struct pComponent_Followers{
56 int something;
57}* ppComponent_Followers;
58void *Component_Followers_constructor(){
59 void *v = MALLOCV(sizeof(struct pComponent_Followers));
60 memset(v,0,sizeof(struct pComponent_Followers));
61 return v;
62}
63void Component_Followers_init(struct tComponent_Followers *t){
64 //public
65 //private
66 t->prv = Component_Followers_constructor();
67 {
68 ppComponent_Followers p = (ppComponent_Followers)t->prv;
69 p->something = 0;
70 }
71}
72void Component_Followers_clear(struct tComponent_Followers *t){
73 //public
74}
75
76
77
78//ppComponent_Followers p = (ppComponent_Followers)gglobal()->Component_Followers.prv;
79
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);
96
97
98
99/*
100 adapted from
101 http://www.web3d.org/x3d/content/examples/Basic/Followers/index.html
102*/
103
104
105//for now we'll put the private variables static,
106// but they would go into a _private struct field in the node
107static int Buffer_length = 10;
108//static int cNumSupports = 10;
109//static int bInitialized = 0;
110//static double BufferEndTime = 0.0;
111//static double cStepTime = 0.0;
112//static struct SFVec3f previousValue = { 0.0f, 0.0f, 0.0f};
113//static struct SFVec3f destination = {0.0f,0.0f,0.0f};
114//static struct SFVec3f Buffer[10];
115
116/*
117//in theory you could have something like this,
118// and some generalized functions that delegate to some
119// function pointers or switch_case on type of element.
120//global constants
121 static int Buffer_length = 10;
122 static int cNumSupports = 10;
123
124//common, can initialize to common
125typedef struct chaser_struct {
126 int bInitialized;
127 double BufferEndTime;
128 double cStepTime;
129 //offsetofs
130 void *value_changed;
131 void *isActive;
132 void *set_destination;
133 void *set_value;
134 //privates
135 void *previousValue;
136 void *destination;
137 void *Buffer;
138}chaser_struct;
139typedef struct chaser_data {
140struct SFVec3f previousValue;
141struct SFVec3f destination;
142struct SFVec3f Buffer[10];
143}chaser_data;
144
145There are a number of ways to abstract types for generic algorithms in C
146a. ## macroization - used frequently in freewrl, however MF and SF still need different treatement
147 can go SFa = SFb, but MFa = MFb won't deep copy the p*
148 similar to templates in C++, generates separate code for each type during compilation
149b. low-level functions like ADD have a switch-case on fieldtype (need to pass in field type)
150c. function pointers that know the type, to abstract handling of an opaque type
151I'll try a combination here, as an experiment.
152A. keeping a certain order to the fields:
153- non-field-type-specific fields come first -int, bool, time- then can use a generic node->fieldname
154- value_changed the first field-type-specific field so offsetof(,value_changed) is the same for all chasers (and dampers)
155B. using c. above, generic functions for handling opaque types, with functions knowing the type
156
157*/
158
159//goal: prepare the absolute addresses of field-type-specific fields
160// for chaser and damper to generisize algos without using ## macros and offsetof
161// which compiles to more code
162// the following _ptrs structs would be populated in do_ on initialization
163// for each node instance (since absolute pointers vary with instance)
164typedef struct chaser_ptrs {
165 //public
166 void *value_changed;
167 void *initialDestination;
168 void *initialValue;
169 void *set_destination;
170 void *set_value;
171 //private
172 void *_buffer;
173 void *_previousValue;
174 void *_destination;
176typedef struct damper_ptrs {
177 //public
178 void *value_changed;
179 void *initialDestination;
180 void *initialValue;
181 void *set_destination;
182 void *set_value;
183 //private
184 void *_values;
185 void *_input;
187
188//goal: abstract a few numerical field types without using ## macroization
189//- including SF and MF in same interface, so algos can be generic
190// this should be static for a fieldtype
191#define VOIDFN (void(*))
192#define FLOATFN (float (*)(void *))
193#define INTFN (int (*) (void *, void *))
194#define VOIDPTR (void *)
195typedef struct ftype {
196 int type;
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); //for sf = array[i]
206 //void (*mfi)(void* A,int i); //for sf = mf.p[i]
207 void **tmp;
208 //lerp vs slerp for orientationChaser?
209}ftype;
210
211//example for float, but I need SFVec3f versions for position
212float *arr3f(float *A, int i){
213 //memcpy(T,&A[3*i],3*sizeof(float));
214 return &A[3*i];
215}
216
217float tmp3f1[6][3];
218void *tmp3f [] = {&tmp3f[0],&tmp3f[1],&tmp3f[2],&tmp3f[3],&tmp3f[4],&tmp3f[5]};
219
220
221ftype ftype_vec3f = {
222 -1, //not a real type, just for warm-up
223 VOIDFN veccopy3f,
224 VOIDFN vecadd3f,
225 VOIDFN vecdif3f,
226 VOIDFN vecscale3f,
227 VOIDFN veclerp3f,
228 FLOATFN veclength3f,
229 INTFN vecsame3f,
230 INTFN vecsame3f,
231 VOIDFN arr3f,
232 VOIDPTR tmp3f,
233};
234
235struct SFVec3f *sfvec3f_copy(struct SFVec3f* T, struct SFVec3f *A){
236 veccopy3f(T->c,A->c);
237 return T;
238}
239struct SFVec3f *sfvec3f_add(struct SFVec3f* T, struct SFVec3f *A, struct SFVec3f *B){
240 vecadd3f(T->c,A->c,B->c);
241 return T;
242}
243struct SFVec3f *sfvec3f_dif(struct SFVec3f* T, struct SFVec3f *A, struct SFVec3f *B){
244 vecdif3f(T->c,A->c,B->c);
245 return T;
246}
247struct SFVec3f *sfvec3f_scale(struct SFVec3f* T, struct SFVec3f *A, float S){
248 vecscale3f(T->c,A->c,S);
249 return T;
250}
251struct SFVec3f *sfvec3f_lerp(struct SFVec3f* T, struct SFVec3f *A, struct SFVec3f *B, float S){
252 veclerp3f(T->c,A->c,B->c,S);
253 return T;
254}
255float sfvec3f_dist(struct SFVec3f* A){
256 return veclength3f(A->c);
257}
258int sfvec3f_same(struct SFVec3f *A, struct SFVec3f *B){
259 return vecsame3f(A->c,B->c);
260}
261struct SFVec3f *sfvec3f_arr(struct SFVec3f *A, int i){
262 return &A[i];
263}
264struct SFVec3f sfvec3f_tmps[6];
265void *sfvec3f_tmp [] = {&sfvec3f_tmps[0],&sfvec3f_tmps[1],&sfvec3f_tmps[2],&sfvec3f_tmps[3],&sfvec3f_tmps[4],&sfvec3f_tmps[5]};
266ftype ftype_sfvec3f = {
267 FIELDTYPE_SFVec3f,
268 VOIDFN sfvec3f_copy,
269 VOIDFN sfvec3f_add,
270 VOIDFN sfvec3f_dif,
271 VOIDFN sfvec3f_scale,
272 VOIDFN sfvec3f_lerp,
273 FLOATFN sfvec3f_dist,
274 INTFN sfvec3f_same,
275 INTFN sfvec3f_same,
276 VOIDFN sfvec3f_arr,
277 VOIDPTR sfvec3f_tmp,
278};
279#define NEWWAY 1
280#ifdef NEWWAY
281void chaser_init(struct X3D_PositionChaser *node)
282{
283 int C;
284 chaser_ptrs *p = node->_p;
285 ftype *t = node->_t;
286
287 //struct SFVec3f *buffer = (struct SFVec3f*)node->_buffer;
288 //node->_destination = node->initialDestination;
289 t->copy(p->_destination,p->initialDestination);
290 //buffer[0]= node->initialDestination; //initial_destination;
291 t->copy(t->arr(p->_buffer,0),p->initialDestination);
292 for(C= 1; C<Buffer_length; C++ )
293 //buffer[C]= node->initialValue; //initial_value;
294 t->copy(t->arr(p->_buffer,C),p->initialValue);
295 // node->_previousvalue= node->initialValue; //initial_value;
296 t->copy(p->_previousValue,p->initialValue);
297 node->_steptime= node->duration / (double) Buffer_length; //cNumSupports;
298}
299double chaser_UpdateBuffer(struct X3D_PositionChaser *node, double Now)
300{
301 int C;
302 double Frac;
303 //struct SFVec3f *buffer = (struct SFVec3f*)node->_buffer;
304 chaser_ptrs *p = node->_p;
305 ftype *t = node->_t;
306
307 Frac = (Now - node->_bufferendtime) / node->_steptime;
308 // is normally < 1. When it has grown to be larger than 1, we have to shift the array because the step response
309 // of the oldest entry has already reached its destination, and it's time for a newer entry.
310 // has already reached it
311 // In the case of a very low frame rate, or a very short cStepTime we may need to shift by more than one entry.
312
313 if(Frac >= 1.0)
314 {
315 int NumToShift= (int)floor(Frac);
316 Frac-= (double) NumToShift;
317 if(NumToShift < Buffer_length)
318 { // normal case.
319
320 //node->_previousvalue= buffer[Buffer_length - NumToShift];
321 t->copy(p->_previousValue,t->arr(p->_buffer,Buffer_length - NumToShift));
322 for( C= Buffer_length - 1; C>=NumToShift; C-- )
323 //buffer[C]= buffer[C - NumToShift];
324 t->copy(t->arr(p->_buffer,C),t->arr(p->_buffer,C - NumToShift));
325 for( C= 0; C<NumToShift; C++ )
326 {
327 // Hmm, we have a destination value, but don't know how it has
328 // reached the current state.
329 // Therefore we do a linear interpolation from the latest value in the buffer to destination.
330 //float tmp1[3],tmp2[3];
331 float Alpha= (float)C / (float)NumToShift;
332 // might need to chain functions like this backward:
333 // float *vecadd3f(float *c, float *a, float *b)
334 // and feed it temps in the *c variable
335 // and in the last step use Buffer[] as *c
336 t->lerp(t->arr(p->_buffer,C),p->_destination,t->arr(p->_buffer,NumToShift),Alpha);
337 // vecadd3f(buffer[C].c,vecscale3f(tmp1,buffer[NumToShift].c,Alpha),vecscale3f(tmp2,node->_destination.c,1.0f - Alpha));
338 //}
339 }
340 }else
341 {
342 // degenerated case:
343 //
344 // We have a _VERY_ low frame rate...
345 // we can only guess how we should fill the array.
346 // Maybe we could write part of a linear interpolation
347 // from Buffer[0] to destination, that goes from BufferEndTime to Now
348 // (possibly only the end of the interpolation is to be written),
349 // but if we rech here we are in a very degenerate case...
350 // Thus we just write destination to the buffer.
351 //node->_previousvalue= NumToShift == Buffer_length? buffer[0] : node->_destination;
352 if(NumToShift == Buffer_length)
353 t->copy(p->_previousValue,t->arr(p->_buffer,0));
354 else
355 t->copy(p->_previousValue,p->_destination);
356 for( C= 0; C<Buffer_length; C++ )
357 //buffer[C]= node->_destination;
358 t->copy(t->arr(p->_buffer,C),p->_destination);
359 }
360 node->_bufferendtime+= NumToShift * node->_steptime;
361 }
362 return Frac;
363}
364//when a route toNode.toField is PositionChaser.set_destination
365//we need to call this function (somehow) much like a script?
366//
367void chaser_set_destination(struct X3D_PositionChaser *node, double Now)
368{
369 chaser_ptrs *p = node->_p;
370 ftype *t = node->_t;
371
372 // node->_destination= Dest;
373 t->copy(p->_destination,p->set_destination);
374 // Somehow we assign to Buffer[-1] and wait untill this gets shifted into the real buffer.
375 // Would we assign to Buffer[0] instead, we'd have no delay, but this would create a jump in the
376 // output because Buffer[0] is associated with a value in the past.
377 chaser_UpdateBuffer(node, Now);
378}
379// This function defines the shape of how the output responds to the input.
380// It must accept values for T in the range 0 <= T <= 1.
381// In order to create a smooth animation, it should return 0 for T == 0,
382// 1 for T == 1 and be sufficient smooth in the range 0 <= T <= 1.
383
384// It should be optimized for speed, in order for high performance. It's
385// executed Buffer.length + 1 times each simulation tick.
386
387double chaser_StepResponseCore(double T)
388{
389 return .5 - .5 * cos(T * PI);
390}
391double chaser_StepResponse(struct X3D_PositionChaser *node, double t)
392{
393 if(t < 0.0)
394 return 0.0;
395 if(t > node->duration)
396 return 1.0;
397 // When optimizing for speed, the above two if(.) cases can be omitted,
398 // as this funciton will not be called for values outside of 0..duration.
399 return chaser_StepResponseCore(t / node->duration);
400}
401
402void chaser_tick(struct X3D_PositionChaser *node, double Now)
403{
404 int C;
405 double Frac, Alpha;
406 //struct SFVec3f Output;
407 //struct SFVec3f DeltaIn;
408 //struct SFVec3f DeltaOut;
409 void *Output, *DeltaIn, *DeltaOut;
410 // OLDCODE UNUSED struct SFVec3f *buffer = (struct SFVec3f*)node->_buffer;
411 chaser_ptrs *p = node->_p;
412 ftype *t = node->_t;
413
414 Output = t->tmp[3];
415 DeltaIn = t->tmp[4];
416 DeltaOut = t->tmp[5];
417
418
419 //chaser_CheckInit(node);
420 if(!node->_bufferendtime)
421 {
422 node->_bufferendtime= Now; // first event we received, so we are in the initialization phase.
423 //node->value_changed= node->initialValue; //initial_value;
424 t->copy(p->value_changed,p->initialValue);
425 return;
426 }
427 Frac= chaser_UpdateBuffer(node, Now);
428 // Frac is a value in 0 <= Frac < 1.
429
430 // Now we can calculate the output.
431 // This means we calculate the delta between each entry in Buffer and its previous
432 // entries, calculate the step response of each such step and add it to form the output.
433
434 // The oldest vaule Buffer[Buffer.length - 1] needs some extra thought, because it has
435 // no previous value. More exactly, we haven't stored a previous value anymore.
436 // However, the step response of that missing previous value has already reached its
437 // destination, so we can - would we have that previous value - use this as a start point
438 // for adding the step responses.
439 // Actually UpdateBuffer(.) maintains this value in
440
441 if(t->type == FIELDTYPE_SFRotation){
442 //SFRotation - I think I have the SFRotation working below in the regular section
443 // - but to prove with type abstractions you can still do special cases, here's the more
444 // explicitly SFRotation version adapted from prototype example code in javascript
445 //var Output= previousValue;
446 t->copy(Output,p->_previousValue);
447
448 //var DeltaIn= previousValue.inverse().multiply(Buffer[Buffer.length - 1]);
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);
451 //Output= Output.slerp(Output.multiply(DeltaIn), StepResponse((Buffer.length - 1 + Frac) * cStepTime));
452 t->lerp(Output,Output,t->add(t->tmp[0],Output,DeltaIn),(float)Alpha);
453 for(C= Buffer_length - 2; C>=0; C-- )
454 {
455 // var DeltaIn= Buffer[C + 1].inverse().multiply(Buffer[C]);
456 t->dif(DeltaIn,t->arr(p->_buffer,C),t->arr(p->_buffer,C+1));
457
458 Alpha = chaser_StepResponse(node,((double)C + Frac) * node->_steptime);
459 // Output= Output.slerp(Output.multiply(DeltaIn), StepResponse((C + Frac) * cStepTime));
460 t->lerp(Output,Output,t->add(t->tmp[0],Output,DeltaIn),(float)Alpha);
461 }
462
463 }else{
464 //everything else
465 //Output= node->_previousvalue;
466 t->copy(Output,p->_previousValue);
467 //DeltaIn= Buffer[Buffer_length - 1].subtract(previousValue);
468 //vecdif3f(DeltaIn.c,buffer[Buffer_length - 1].c,node->_previousvalue.c);
469 t->dif(DeltaIn,t->arr(p->_buffer,Buffer_length -1),p->_previousValue);
470
471 Alpha = chaser_StepResponse(node,((double)(Buffer_length - 1) + Frac) * node->_steptime);
472
473 //DeltaOut= DeltaIn.multiply(StepResponse((Buffer_length - 1 + Frac) * cStepTime));
474 //vecscale3f(DeltaOut.c,DeltaIn.c,(float)chaser_StepResponse(node,((double)Buffer_length - 1.0 + Frac) * node->_steptime));
475 t->scale(DeltaOut,DeltaIn,(float)Alpha);
476
477 //Output= Output.add(DeltaOut);
478 //vecadd3f(Output.c,Output.c,DeltaOut.c);
479 t->add(Output,Output,DeltaOut);
480
481 for(C= Buffer_length - 2; C>=0; C-- )
482 {
483 //DeltaIn= Buffer[C].subtract(Buffer[C + 1]);
484 //vecdif3f(DeltaIn.c,buffer[C].c,buffer[C+1].c);
485 t->dif(DeltaIn,t->arr(p->_buffer,C),t->arr(p->_buffer,C+1));
486
487 Alpha = chaser_StepResponse(node,((double)C + Frac) * node->_steptime);
488
489 //DeltaOut= DeltaIn.multiply(StepResponse((C + Frac) * cStepTime));
490 //vecscale3f(DeltaOut.c,DeltaIn.c,(float)chaser_StepResponse(node,((double)C + Frac) * node->_steptime));
491 t->scale(DeltaOut,DeltaIn,(float)Alpha);
492 //Output= Output.add(DeltaOut);
493 //vecadd3f(Output.c,Output.c,DeltaOut.c);
494 t->add(Output,Output,DeltaOut);
495 }
496 }
497 //if(!vecsame3f(Output.c,node->value_changed.c)){
498 if(!t->same(Output,p->value_changed)){
499 t->copy(p->value_changed,Output);
500 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionChaser, value_changed));
501 }
502}
503void chaser_set_value(struct X3D_PositionChaser *node)
504{
505 chaser_ptrs *p = node->_p;
506 ftype *t = node->_t;
507
508 //node->value_changed= opos;
509 t->copy(p->value_changed,p->set_value);
510 //node->initialValue = opos;
511 t->copy(p->initialValue,p->set_value);
512 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionChaser, value_changed));
513 node->isActive = TRUE;
514 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionChaser, isActive));
515}
516
517
518void do_PositionChaserTick(void * ptr){
519 double Now;
520 struct X3D_PositionChaser *_node = (struct X3D_PositionChaser *)ptr;
521 struct X3D_PositionChaser *node = (struct X3D_PositionChaser *)ptr;
522 if(!node) return;
523 if(!_node->_buffer){
524 chaser_ptrs *p = MALLOCN(node,sizeof(chaser_ptrs));
525 _node->_buffer = REALLOCN(node,_node->_buffer,Buffer_length * sizeof(struct SFVec3f));
526 node->_t = &ftype_sfvec3f;
527 node->_p = p;
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();
537 chaser_init(node);
538 }
539 Now = TickTime();
540 if(NODE_NEEDS_COMPILING){
541 chaser_ptrs *p = node->_p;
542 ftype *t = node->_t;
543
544 node->isActive = TRUE;
545 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionChaser, isActive));
546 //Q how to tell which set_ was set: set_destination or set_value?
547 //if(!vecsame3f(node->set_destination.c,node->_destination.c))
548 if(!t->same(p->set_destination,p->_destination))
549 chaser_set_destination(node, Now);
550 //else if(!vecsame3f(node->set_value.c,node->initialValue.c)) //not sure I have the right idea here
551 else if(!t->same(p->set_value,p->initialValue))
552 chaser_set_value(node);
553 MARK_NODE_COMPILED
554 }
555 if(node->isActive)
556 chaser_tick(node,Now);
557}
558
559/*
560 //positiondamper
561
562*/
563//static int bInitializedD = FALSE;
564//static double lastTick = 0.0;
565//static int bNeedToTakeFirstInput = TRUE;
566//static struct SFVec3f value5 = {0.0f,0.0f,0.0f};
567//static struct SFVec3f value4 = {0.0f,0.0f,0.0f};
568//static struct SFVec3f value3 = {0.0f,0.0f,0.0f};
569//static struct SFVec3f value2 = {0.0f,0.0f,0.0f};
570//static struct SFVec3f value1 = {0.0f,0.0f,0.0f};
571//static struct SFVec3f input = {0.0f,0.0f,0.0f};
572//
573
574
575void damper_set_value(struct X3D_PositionDamper *node, void *opos);
576void damper_Init(struct X3D_PositionDamper *node)
577{
578 damper_ptrs *p = node->_p;
579 // OLDCODE UNUSED ftype *t = node->_t;
580
581 node->_takefirstinput = TRUE;
582 //damper_set_value(node,node->initialValue);
583 damper_set_value(node,p->initialValue);
584 node->isActive = TRUE;
585 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionDamper, isActive));
586}
587
588float damper_GetDist(struct X3D_PositionDamper *node)
589{
590 float dist;
591 damper_ptrs *p = node->_p;
592 ftype *t = node->_t;
593
594 //struct SFVec3f *values = (struct SFVec3f *)node->_values;
595
596 //double dist= value1.subtract(node->initialDestination).length();
597 //dist = veclength3f(vecdif3f(tmp,values[0].c,node->_input.c));
598 dist = t->dist(t->dif(t->tmp[0],t->arr(p->_values,0),p->_input));
599 if(node->order > 1)
600 {
601 //double dist2= value2.subtract(value1).length();
602 //float dist2 = veclength3f(vecdif3f(tmp,values[1].c,values[0].c));
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;
605 }
606 if(node->order > 2)
607 {
608 //double dist3= value3.subtract(value2).length();
609 //float dist3 = veclength3f(vecdif3f(tmp,values[2].c,values[1].c));
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;
612 }
613 if(node->order > 3)
614 {
615 //double dist4= value4.subtract(value3).length();
616 //float dist4 = veclength3f(vecdif3f(tmp,values[3].c,values[2].c));
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;
619 }
620 if(node->order > 4)
621 {
622 //double dist5= value5.subtract(value4).length();
623 //float dist5 = veclength3f(vecdif3f(tmp,values[4].c, values[3].c));
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;
626 }
627 return dist;
628}
629void damper_set_value(struct X3D_PositionDamper *node, void *opos)
630{
631 int i;
632 damper_ptrs *p = node->_p;
633 ftype *t = node->_t;
634
635 //struct SFVec3f *values = (struct SFVec3f *)node->_values;
636 node->_takefirstinput = FALSE;
637 //values[0]= values[1]= values[2]= values[3]= values[4]= opos;
638 for(i=0;i<5;i++)
639 t->copy(t->arr(p->_values,i),opos);
640 //node->value_changed= opos;
641 t->copy(p->value_changed, opos);
642 //node->initialValue = opos;
643 t->copy(p->initialValue,opos);
644 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionDamper, value_changed));
645 node->isActive = TRUE;
646 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionDamper, isActive));
647}
648
649void damper_set_destination(struct X3D_PositionDamper *node, void *ipos)
650{
651 damper_ptrs *p = node->_p;
652 ftype *t = node->_t;
653
654 if(node->_takefirstinput)
655 {
656 node->_takefirstinput = FALSE;
657 damper_set_value(node,ipos);
658 }
659 //if(!vecsame3f(ipos.c,node->_input.c))
660 if(!t->same(ipos,p->_input))
661 {
662 //node->_input = ipos;
663 t->copy(p->_input,ipos);
664 node->isActive = TRUE;
665 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionDamper, isActive));
666 }
667}
668
669//struct SFVec3f damper_diftimes(struct SFVec3f a, struct SFVec3f b, double alpha){
670//void* damper_diftimes(struct X3D_PositionDamper *node, void *T, void *A, void *B, double alpha){
671// struct SFVec3f ret;
672// float tmp[3], tmp2[3];
673// damper_ptrs *p = node->_p;
674// ftype *t = node->_t;
675//
676// //input .add(value1.subtract(input ).multiply(alpha))
677// if(1){
678// //vecdif3f(tmp,b.c,a.c);
679// t->dif(t->tmp[0],B,A);
680// //vecscale3f(tmp2,tmp,(float)alpha);
681// t->scale(t->tmp[1],t->tmp[0],(float)alpha);
682// //vecadd3f(ret.c,a.c,tmp2);
683// t->add(T,A,t->tmp[1]);
684// }else{
685// //vecadd3f(ret.c,a.c,vecscale3f(tmp2,vecdif3f(tmp,b.c,a.c),(float)alpha));
686// t->add(T,A,t->scale(t->tmp[1],t->dif(t->tmp[0],B,A),(float)alpha));
687// }
688// return T;
689//}
690
691void tick_damper(struct X3D_PositionDamper *node, double now)
692{
693 double delta,alpha;
694 float dist;
695 damper_ptrs *p = node->_p;
696 ftype *t = node->_t;
697
698 //struct SFVec3f *values = (struct SFVec3f *)node->_values;
699
700 if(!node->_lasttick)
701 {
702 node->_lasttick= now;
703 return;
704 }
705 delta= now - node->_lasttick;
706 node->_lasttick= now;
707 alpha= exp(-delta / node->tau);
708 if(node->_takefirstinput) // then don't do any processing.
709 return;
710
711 //values[0]= node->order > 0 && node->tau != 0.0
712 // //? input .add(value1.subtract(input ).multiply(alpha))
713 // ? damper_diftimes(node->_input,values[0],alpha)
714 // : node->_input;
715
716 if(node->order > 0 && node->tau != 0.0)
717 //damper_diftimes(node,t->arr(p->_values,0),p->_input,t->arr(p->_values,0),alpha);
718 t->lerp(t->arr(p->_values,0),p->_input,t->arr(p->_values,0),(float)alpha);
719 else
720 t->copy(t->arr(p->_values,0),p->_input);
721
722 //values[1]= node->order > 1 && node->tau != 0.0
723 // //? value1.add(value2.subtract(value1).multiply(alpha))
724 // ? damper_diftimes(values[0],values[1],alpha)
725 // : values[0];
726 if(node->order > 1 && node->tau != 0.0)
727 //damper_diftimes(node,t->arr(p->_values,1),t->arr(p->_values,0),t->arr(p->_values,1),alpha);
728 t->lerp(t->arr(p->_values,1),t->arr(p->_values,0),t->arr(p->_values,1),(float)alpha);
729 else
730 t->copy(t->arr(p->_values,1),t->arr(p->_values,0));
731
732 //values[2]= node->order > 2 && node->tau != 0.0
733 // //? value2.add(value3.subtract(value2).multiply(alpha))
734 // ? damper_diftimes(values[1],values[2],alpha)
735 // : values[1];
736 if(node->order > 2 && node->tau != 0.0)
737 //damper_diftimes(node,t->arr(p->_values,2),t->arr(p->_values,1),t->arr(p->_values,2),alpha);
738 t->lerp(t->arr(p->_values,2),t->arr(p->_values,1),t->arr(p->_values,2),(float)alpha);
739 else
740 t->copy(t->arr(p->_values,2),t->arr(p->_values,1));
741
742
743 //values[3]= node->order > 3 && node->tau != 0.0
744 // //? value3.add(value4.subtract(value3).multiply(alpha))
745 // ? damper_diftimes(values[2],values[3],alpha)
746 // : values[2];
747 if(node->order > 3 && node->tau != 0.0)
748 //damper_diftimes(node,t->arr(p->_values,3),t->arr(p->_values,2),t->arr(p->_values,3),alpha);
749 t->lerp(t->arr(p->_values,3),t->arr(p->_values,2),t->arr(p->_values,3),(float)alpha);
750 else
751 t->copy(t->arr(p->_values,3),t->arr(p->_values,2));
752
753 //values[4]= node->order > 4 && node->tau != 0.0
754 // //? value4.add(value5.subtract(value4).multiply(alpha))
755 // ? damper_diftimes(values[3],values[4],alpha)
756 // : values[3];
757 if(node->order > 4 && node->tau != 0.0)
758 //damper_diftimes(node,t->arr(p->_values,4),t->arr(p->_values,3),t->arr(p->_values,4),alpha);
759 t->lerp(t->arr(p->_values,4),t->arr(p->_values,3),t->arr(p->_values,4),(float)alpha);
760 else
761 t->copy(t->arr(p->_values,4),t->arr(p->_values,3));
762
763 dist= damper_GetDist(node);
764
765 if(dist < max(node->tolerance,.001f)) //eps)
766 {
767 int i;
768 //values[0]= values[1]= values[2]= values[3]= values[4]= node->_input;
769 for(i=0;i<5;i++)
770 t->copy(t->arr(p->_values,i),p->_input);
771
772 //node->value_changed= node->_input;
773 t->copy(p->value_changed,p->_input);
774 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionDamper, value_changed));
775 node->isActive = FALSE;
776 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionDamper, isActive));
777 return;
778 }
779 //node->value_changed= values[4];
780 t->copy(p->value_changed,t->arr(p->_values,4));
781 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionDamper, value_changed));
782}
783void damper_set_tau(struct X3D_PositionDamper *node, double tau){
784 node->_tau = tau;
785 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionDamper, tau));
786}
787
788void do_PositionDamperTick(void * ptr){
789 struct X3D_PositionDamper *_node = (struct X3D_PositionDamper *)ptr;
790 struct X3D_PositionDamper *node = (struct X3D_PositionDamper *)ptr;
791 if(!node)return;
792 if(!_node->_values){
793 damper_ptrs *p = MALLOCN(node,sizeof(damper_ptrs));
794 node->_t = &ftype_sfvec3f;
795 node->_p = p;
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;
804 //damper_CheckInit(node);
805 damper_Init(node);
806 }
807
808 if(NODE_NEEDS_COMPILING){
809 //node->isActive = TRUE;
810 damper_ptrs *p = node->_p;
811 ftype *t = node->_t;
812
813 //if(!vecsame3f(node->set_destination.c,node->_input.c)) //not sure i have the right idea
814 if(!t->same(p->set_destination,p->_input))
815 //damper_set_destination(node, node->set_destination);
816 damper_set_destination(node, p->set_destination);
817 //set_tau
818 if(node->tau != node->_tau)
819 damper_set_tau(node,node->tau);
820 //set_value
821 //if(!vecsame3f(node->initialValue.c,node->set_value.c))
822 if(!t->same(p->initialValue,p->set_value))
823 //damper_set_value(node,node->set_value);
824 damper_set_value(node,p->set_value);
825 MARK_NODE_COMPILED
826 }
827 if(node->isActive)
828 tick_damper(node,TickTime());
829}
830#else //NEWWAY
831void chaser_init(struct X3D_PositionChaser *node)
832{
833 int C;
834 struct SFVec3f *buffer = (struct SFVec3f*)node->_buffer;
835 node->_destination = node->initialDestination;
836 buffer[0]= node->initialDestination; //initial_destination;
837 for(C= 1; C<Buffer_length; C++ )
838 buffer[C]= node->initialValue; //initial_value;
839 node->_previousvalue= node->initialValue; //initial_value;
840 node->_steptime= node->duration / (double) Buffer_length; //cNumSupports;
841}
842double chaser_UpdateBuffer(struct X3D_PositionChaser *node, double Now)
843{
844 int C;
845 double Frac;
846 struct SFVec3f *buffer = (struct SFVec3f*)node->_buffer;
847
848 Frac = (Now - node->_bufferendtime) / node->_steptime;
849 // is normally < 1. When it has grown to be larger than 1, we have to shift the array because the step response
850 // of the oldest entry has already reached its destination, and it's time for a newer entry.
851 // has already reached it
852 // In the case of a very low frame rate, or a very short cStepTime we may need to shift by more than one entry.
853
854 if(Frac >= 1.0)
855 {
856 int NumToShift= (int)floor(Frac);
857 Frac-= (double) NumToShift;
858 if(NumToShift < Buffer_length)
859 { // normal case.
860
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++ )
865 {
866 // Hmm, we have a destination value, but don't know how it has
867 // reached the current state.
868 // Therefore we do a linear interpolation from the latest value in the buffer to destination.
869 float tmp1[3],tmp2[3];
870 float Alpha= (float)C / (float)NumToShift;
871 // might need to chain functions like this backward:
872 // float *vecadd3f(float *c, float *a, float *b)
873 // and feed it temps in the *c variable
874 // and in the last step use Buffer[] as *c
875 //Buffer[C]= Buffer[NumToShift].multiply(Alpha).add(destination.multiply((1 - Alpha)));
876 //vecadd3f(Buffer[C].c,vecscale3f(Buffer[NumToShift].c,(float)Alpha),vecscale3f(tmp2,destination.c,(1.0f-Alpha));
877 //printf("alf %f ",Alpha);
878 // buff[C] = alpha*buff[NumToShift] + (1-alpha)*destination;
879 if(1){
880 float tmp3[3];
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);
885 }else{
886 vecadd3f(buffer[C].c,vecscale3f(tmp1,buffer[NumToShift].c,Alpha),vecscale3f(tmp2,node->_destination.c,1.0f - Alpha));
887 }
888 }
889 }else
890 {
891 // degenerated case:
892 //
893 // We have a _VERY_ low frame rate...
894 // we can only guess how we should fill the array.
895 // Maybe we could write part of a linear interpolation
896 // from Buffer[0] to destination, that goes from BufferEndTime to Now
897 // (possibly only the end of the interpolation is to be written),
898 // but if we rech here we are in a very degenerate case...
899 // Thus we just write destination to the buffer.
900 node->_previousvalue= NumToShift == Buffer_length? buffer[0] : node->_destination;
901
902 for( C= 0; C<Buffer_length; C++ )
903 buffer[C]= node->_destination;
904 }
905 node->_bufferendtime+= NumToShift * node->_steptime;
906 }
907 return Frac;
908}
909//when a route toNode.toField is PositionChaser.set_destination
910//we need to call this function (somehow) much like a script?
911//
912void chaser_set_destination(struct X3D_PositionChaser *node, struct SFVec3f Dest, double Now)
913{
914 node->_destination= Dest;
915 // Somehow we assign to Buffer[-1] and wait untill this gets shifted into the real buffer.
916 // Would we assign to Buffer[0] instead, we'd have no delay, but this would create a jump in the
917 // output because Buffer[0] is associated with a value in the past.
918 chaser_UpdateBuffer(node, Now);
919}
920// This function defines the shape of how the output responds to the input.
921// It must accept values for T in the range 0 <= T <= 1.
922// In order to create a smooth animation, it should return 0 for T == 0,
923// 1 for T == 1 and be sufficient smooth in the range 0 <= T <= 1.
924
925// It should be optimized for speed, in order for high performance. It's
926// executed Buffer.length + 1 times each simulation tick.
927
928double chaser_StepResponseCore(double T)
929{
930 return .5 - .5 * cos(T * PI);
931}
932double chaser_StepResponse(struct X3D_PositionChaser *node, double t)
933{
934 if(t < 0.0)
935 return 0.0;
936 if(t > node->duration)
937 return 1.0;
938 // When optimizing for speed, the above two if(.) cases can be omitted,
939 // as this funciton will not be called for values outside of 0..duration.
940 return chaser_StepResponseCore(t / node->duration);
941}
942
943void chaser_tick(struct X3D_PositionChaser *node, double Now)
944{
945 int C;
946 double Frac;
947 struct SFVec3f Output;
948 struct SFVec3f DeltaIn;
949 struct SFVec3f DeltaOut;
950 struct SFVec3f *buffer = (struct SFVec3f*)node->_buffer;
951
952 //chaser_CheckInit(node);
953 if(!node->_bufferendtime)
954 {
955 node->_bufferendtime= Now; // first event we received, so we are in the initialization phase.
956 node->value_changed= node->initialValue; //initial_value;
957 return;
958 }
959 Frac= chaser_UpdateBuffer(node, Now);
960 // Frac is a value in 0 <= Frac < 1.
961
962 // Now we can calculate the output.
963 // This means we calculate the delta between each entry in Buffer and its previous
964 // entries, calculate the step response of each such step and add it to form the output.
965
966 // The oldest vaule Buffer[Buffer.length - 1] needs some extra thought, because it has
967 // no previous value. More exactly, we haven't stored a previous value anymore.
968 // However, the step response of that missing previous value has already reached its
969 // destination, so we can - would we have that previous value - use this as a start point
970 // for adding the step responses.
971 // Actually UpdateBuffer(.) maintains this value in
972
973 Output= node->_previousvalue;
974 //DeltaIn= Buffer[Buffer_length - 1].subtract(previousValue);
975 vecdif3f(DeltaIn.c,buffer[Buffer_length - 1].c,node->_previousvalue.c);
976
977 //DeltaOut= DeltaIn.multiply(StepResponse((Buffer_length - 1 + Frac) * cStepTime));
978 vecscale3f(DeltaOut.c,DeltaIn.c,(float)chaser_StepResponse(node,((double)Buffer_length - 1.0 + Frac) * node->_steptime));
979 //Output= Output.add(DeltaOut);
980 vecadd3f(Output.c,Output.c,DeltaOut.c);
981 for(C= Buffer_length - 2; C>=0; C-- )
982 {
983 //DeltaIn= Buffer[C].subtract(Buffer[C + 1]);
984 vecdif3f(DeltaIn.c,buffer[C].c,buffer[C+1].c);
985 //DeltaOut= DeltaIn.multiply(StepResponse((C + Frac) * cStepTime));
986 vecscale3f(DeltaOut.c,DeltaIn.c,(float)chaser_StepResponse(node,((double)C + Frac) * node->_steptime));
987 //Output= Output.add(DeltaOut);
988 vecadd3f(Output.c,Output.c,DeltaOut.c);
989 }
990 if(!vecsame3f(Output.c,node->value_changed.c)){
991 node->value_changed= Output;
992 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionChaser, value_changed));
993 }
994}
995void chaser_set_value(struct X3D_PositionChaser *node, struct SFVec3f opos)
996{
997 node->value_changed= opos;
998 node->initialValue = opos;
999 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionChaser, value_changed));
1000 node->isActive = TRUE;
1001 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionChaser, isActive));
1002}
1003
1004
1005void do_PositionChaserTick(void * ptr){
1006 double Now;
1007 struct X3D_PositionChaser *node = (struct X3D_PositionChaser *)ptr;
1008 if(!node) return;
1009 if(!node->_buffer){
1010 node->_buffer = realloc(node->_buffer,Buffer_length * sizeof(struct SFVec3f));
1011 chaser_init(node);
1012 }
1013 Now = TickTime();
1014 if(NODE_NEEDS_COMPILING){
1015 node->isActive = TRUE;
1016 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionChaser, isActive));
1017 //Q how to tell which set_ was set: set_destination or set_value?
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)) //not sure I have the right idea here
1021 chaser_set_value(node,node->set_value);
1022 MARK_NODE_COMPILED
1023 }
1024 if(node->isActive)
1025 chaser_tick(node,Now);
1026}
1027
1028/*
1029 //positiondamper
1030
1031*/
1032//static int bInitializedD = FALSE;
1033//static double lastTick = 0.0;
1034//static int bNeedToTakeFirstInput = TRUE;
1035//static struct SFVec3f value5 = {0.0f,0.0f,0.0f};
1036//static struct SFVec3f value4 = {0.0f,0.0f,0.0f};
1037//static struct SFVec3f value3 = {0.0f,0.0f,0.0f};
1038//static struct SFVec3f value2 = {0.0f,0.0f,0.0f};
1039//static struct SFVec3f value1 = {0.0f,0.0f,0.0f};
1040//static struct SFVec3f input = {0.0f,0.0f,0.0f};
1041//
1042
1043
1044void damper_set_value(struct X3D_PositionDamper *node, struct SFVec3f opos);
1045void damper_Init(struct X3D_PositionDamper *node)
1046{
1047 node->_takefirstinput = TRUE;
1048 damper_set_value(node,node->initialValue);
1049 node->isActive = TRUE;
1050 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionDamper, isActive));
1051}
1052
1053float damper_GetDist(struct X3D_PositionDamper *node)
1054{
1055 float tmp[3], dist;
1056 struct SFVec3f *values = (struct SFVec3f *)node->_values;
1057
1058 //double dist= value1.subtract(node->initialDestination).length();
1059 dist = veclength3f(vecdif3f(tmp,values[0].c,node->_input.c));
1060 if(node->order > 1)
1061 {
1062 //double dist2= value2.subtract(value1).length();
1063 float dist2 = veclength3f(vecdif3f(tmp,values[1].c,values[0].c));
1064 if( dist2 > dist) dist= dist2;
1065 }
1066 if(node->order > 2)
1067 {
1068 //double dist3= value3.subtract(value2).length();
1069 float dist3 = veclength3f(vecdif3f(tmp,values[2].c,values[1].c));
1070 if( dist3 > dist) dist= dist3;
1071 }
1072 if(node->order > 3)
1073 {
1074 //double dist4= value4.subtract(value3).length();
1075 float dist4 = veclength3f(vecdif3f(tmp,values[3].c,values[2].c));
1076 if( dist4 > dist) dist= dist4;
1077 }
1078 if(node->order > 4)
1079 {
1080 //double dist5= value5.subtract(value4).length();
1081 float dist5 = veclength3f(vecdif3f(tmp,values[4].c, values[3].c));
1082 if( dist5 > dist) dist= dist5;
1083 }
1084 return dist;
1085}
1086void damper_set_value(struct X3D_PositionDamper *node, struct SFVec3f opos)
1087{
1088 struct SFVec3f *values = (struct SFVec3f *)node->_values;
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;
1093 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionDamper, value_changed));
1094 node->isActive = TRUE;
1095 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionDamper, isActive));
1096}
1097
1098void damper_set_destination(struct X3D_PositionDamper *node, struct SFVec3f ipos)
1099{
1100 if(node->_takefirstinput)
1101 {
1102 node->_takefirstinput = FALSE;
1103 damper_set_value(node,ipos);
1104 }
1105 if(!vecsame3f(ipos.c,node->_input.c))
1106 {
1107 node->_input = ipos;
1108 node->isActive = TRUE;
1109 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionDamper, isActive));
1110 }
1111}
1112
1113struct SFVec3f damper_diftimes(struct SFVec3f a, struct SFVec3f b, double alpha){
1114 struct SFVec3f ret;
1115 float tmp[3], tmp2[3];
1116 //input .add(value1.subtract(input ).multiply(alpha))
1117 if(1){
1118 vecdif3f(tmp,b.c,a.c);
1119 vecscale3f(tmp2,tmp,(float)alpha);
1120 vecadd3f(ret.c,a.c,tmp2);
1121 }else{
1122 vecadd3f(ret.c,a.c,vecscale3f(tmp2,vecdif3f(tmp,b.c,a.c),(float)alpha));
1123 }
1124 return ret;
1125}
1126
1127void tick_positiondamper(struct X3D_PositionDamper *node, double now)
1128{
1129 double delta,alpha;
1130 float dist;
1131 struct SFVec3f *values = (struct SFVec3f *)node->_values;
1132
1133 if(!node->_lasttick)
1134 {
1135 node->_lasttick= now;
1136 return;
1137 }
1138 delta= now - node->_lasttick;
1139 node->_lasttick= now;
1140 alpha= exp(-delta / node->tau);
1141 if(node->_takefirstinput) // then don't do any processing.
1142 return;
1143
1144 values[0]= node->order > 0 && node->tau != 0.0
1145 //? input .add(value1.subtract(input ).multiply(alpha))
1146 ? damper_diftimes(node->_input,values[0],alpha)
1147 : node->_input;
1148
1149 values[1]= node->order > 1 && node->tau != 0.0
1150 //? value1.add(value2.subtract(value1).multiply(alpha))
1151 ? damper_diftimes(values[0],values[1],alpha)
1152 : values[0];
1153
1154 values[2]= node->order > 2 && node->tau != 0.0
1155 //? value2.add(value3.subtract(value2).multiply(alpha))
1156 ? damper_diftimes(values[1],values[2],alpha)
1157 : values[1];
1158
1159 values[3]= node->order > 3 && node->tau != 0.0
1160 //? value3.add(value4.subtract(value3).multiply(alpha))
1161 ? damper_diftimes(values[2],values[3],alpha)
1162 : values[2];
1163
1164 values[4]= node->order > 4 && node->tau != 0.0
1165 //? value4.add(value5.subtract(value4).multiply(alpha))
1166 ? damper_diftimes(values[3],values[4],alpha)
1167 : values[3];
1168
1169 dist= damper_GetDist(node);
1170
1171 if(dist < max(node->tolerance,.001f)) //eps)
1172 {
1173 values[0]= values[1]= values[2]= values[3]= values[4]= node->_input;
1174 node->value_changed= node->_input;
1175 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionDamper, value_changed));
1176 node->isActive = FALSE;
1177 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionDamper, isActive));
1178 return;
1179 }
1180 node->value_changed= values[4];
1181 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionDamper, value_changed));
1182}
1183void damper_set_tau(struct X3D_PositionDamper *node, double tau){
1184 node->_tau = tau;
1185 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionDamper, tau));
1186}
1187void do_PositionDamperTick(void * ptr){
1188 struct X3D_PositionDamper *node = (struct X3D_PositionDamper *)ptr;
1189 if(!node)return;
1190 if(!node->_values){
1191 node->_values = realloc(node->_values,5 * sizeof(struct SFVec3f));
1192 //damper_CheckInit(node);
1193 damper_Init(node);
1194 }
1195
1196 if(NODE_NEEDS_COMPILING){
1197 //node->isActive = TRUE;
1198 if(!vecsame3f(node->set_destination.c,node->_input.c)) //not sure i have the right idea
1199 damper_set_destination(node, node->set_destination);
1200 //set_tau
1201 if(node->tau != node->_tau)
1202 damper_set_tau(node,node->tau);
1203 //set_value
1204 if(!vecsame3f(node->initialValue.c,node->set_value.c))
1205 damper_set_value(node,node->set_value);
1206 MARK_NODE_COMPILED
1207 }
1208 if(node->isActive)
1209 tick_positiondamper(node,TickTime());
1210}
1211#endif //NEWWAY
1212
1213
1214void do_ColorChaserTick_default(void * ptr){
1215 struct X3D_ColorChaser *node = (struct X3D_ColorChaser *)ptr;
1216 if(!node)return;
1217 if(NODE_NEEDS_COMPILING){
1218 //default action copy input to output when not implemented
1219 veccopy3f(node->value_changed.c, node->set_destination.c);
1220 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_ColorChaser, value_changed));
1221 MARK_NODE_COMPILED
1222 }
1223}
1224void do_ColorDamperTick_default(void * ptr){
1225 struct X3D_ColorDamper *node = (struct X3D_ColorDamper *)ptr;
1226 if(!node)return;
1227 if(NODE_NEEDS_COMPILING){
1228 //default action copy input to output when not implemented
1229 veccopy3f(node->value_changed.c, node->set_destination.c);
1230 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_ColorDamper, value_changed));
1231 MARK_NODE_COMPILED
1232 }
1233}
1234void do_ColorChaserTick(void * ptr){
1235 double Now;
1236 struct X3D_ColorChaser *_node = (struct X3D_ColorChaser *)ptr;
1237 struct X3D_PositionChaser *node = (struct X3D_PositionChaser *)ptr; //abstract interface
1238 if(!node) return;
1239 if(!_node->_buffer){
1240 chaser_ptrs *p = MALLOCN(node,sizeof(chaser_ptrs));
1241 _node->_buffer = REALLOCN(node,_node->_buffer,Buffer_length * sizeof(struct SFColor)); //**changes with field type
1242 node->_t = &ftype_sfvec3f; //**changes with field type
1243 node->_p = p;
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();
1253 chaser_init(node);
1254 }
1255 Now = TickTime();
1256 if(NODE_NEEDS_COMPILING){
1257 chaser_ptrs *p = node->_p;
1258 ftype *t = node->_t;
1259
1260 node->isActive = TRUE;
1261 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionChaser, isActive));
1262 //Q how to tell which set_ was set: set_destination or set_value?
1263 //if(!vecsame3f(node->set_destination.c,node->_destination.c))
1264 if(!t->same(p->set_destination,p->_destination))
1265 chaser_set_destination(node, Now);
1266 //else if(!vecsame3f(node->set_value.c,node->initialValue.c)) //not sure I have the right idea here
1267 else if(!t->same(p->set_value,p->initialValue))
1268 chaser_set_value(node);
1269 MARK_NODE_COMPILED
1270 }
1271 if(node->isActive)
1272 chaser_tick(node,Now);
1273}
1274
1275void do_ColorDamperTick(void * ptr){
1276 struct X3D_ColorDamper *_node = (struct X3D_ColorDamper *)ptr;
1277 struct X3D_PositionDamper *node = (struct X3D_PositionDamper *)ptr; //abstract type
1278 if(!node)return;
1279 if(!_node->_values){
1280 damper_ptrs *p = MALLOCN(node,sizeof(damper_ptrs));
1281 node->_t = &ftype_sfvec3f; //**changes with field type
1282 node->_p = p;
1283 _node->_values = REALLOCN(node,_node->_values,5 * sizeof(struct SFColor)); //**changes with field type
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;
1291 //damper_CheckInit(node);
1292 damper_Init(node);
1293 }
1294
1295 if(NODE_NEEDS_COMPILING){
1296 //node->isActive = TRUE;
1297 damper_ptrs *p = node->_p;
1298 ftype *t = node->_t;
1299
1300 //if(!vecsame3f(node->set_destination.c,node->_input.c)) //not sure i have the right idea
1301 if(!t->same(p->set_destination,p->_input))
1302 //damper_set_destination(node, node->set_destination);
1303 damper_set_destination(node, p->set_destination);
1304 //set_tau
1305 if(node->tau != node->_tau)
1306 damper_set_tau(node,node->tau);
1307 //set_value
1308 //if(!vecsame3f(node->initialValue.c,node->set_value.c))
1309 if(!t->same(p->initialValue,p->set_value))
1310 //damper_set_value(node,node->set_value);
1311 damper_set_value(node,p->set_value);
1312 MARK_NODE_COMPILED
1313 }
1314 if(node->isActive)
1315 tick_damper(node,TickTime());
1316}
1317
1318void do_CoordinateChaserTick_default(void * ptr){
1319 struct X3D_CoordinateChaser *node = (struct X3D_CoordinateChaser *)ptr;
1320 if(!node)return;
1321 if(NODE_NEEDS_COMPILING){
1322 //default action copy input to output when not implemented
1323 int n;
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));
1328 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_CoordinateChaser, value_changed));
1329 MARK_NODE_COMPILED
1330 }
1331}
1332void do_CoordinateDamperTick_default(void * ptr){
1333 struct X3D_CoordinateDamper *node = (struct X3D_CoordinateDamper *)ptr;
1334 if(!node)return;
1335 if(NODE_NEEDS_COMPILING){
1336 //default action copy input to output when not implemented
1337 int n;
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));
1342 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_CoordinateDamper, value_changed));
1343 MARK_NODE_COMPILED
1344 }
1345}
1346
1347struct Multi_Vec3f *mfvec3f_copy(struct Multi_Vec3f* T, struct Multi_Vec3f *A){
1348 T->p = REALLOC(T->p,A->n * sizeof(struct SFVec3f));
1349 T->n = A->n;
1350 memcpy(T->p,A->p,A->n * sizeof(struct SFVec3f));
1351 return T;
1352}
1353struct Multi_Vec3f *mfvec3f_add(struct Multi_Vec3f* T, struct Multi_Vec3f *A, struct Multi_Vec3f *B){
1354 int i;
1355 T->n = min(A->n,B->n);
1356 T->p = REALLOC(T->p,T->n * sizeof(struct SFVec3f));
1357 for(i=0;i<T->n;i++)
1358 sfvec3f_add(&T->p[i],&A->p[i],&B->p[i]);
1359 return T;
1360}
1361struct Multi_Vec3f *mfvec3f_dif(struct Multi_Vec3f* T, struct Multi_Vec3f *A, struct Multi_Vec3f *B){
1362 int i;
1363 T->n = min(A->n,B->n);
1364 T->p = REALLOC(T->p,T->n * sizeof(struct SFVec3f));
1365 for(i=0;i<T->n;i++)
1366 sfvec3f_dif(&T->p[i],&A->p[i],&B->p[i]);
1367 return T;
1368}
1369struct Multi_Vec3f *mfvec3f_scale(struct Multi_Vec3f* T, struct Multi_Vec3f *A, float S){
1370 int i;
1371 T->n = A->n;
1372 T->p = REALLOC(T->p,T->n * sizeof(struct SFVec3f));
1373 for(i=0;i<T->n;i++)
1374 sfvec3f_scale(&T->p[i],&A->p[i],S);
1375 return T;
1376}
1377struct Multi_Vec3f *mfvec3f_lerp(struct Multi_Vec3f* T, struct Multi_Vec3f *A, struct Multi_Vec3f *B, float alpha){
1378 int i;
1379 T->n = A->n;
1380 T->p = REALLOC(T->p,T->n * sizeof(struct SFVec3f));
1381 for(i=0;i<T->n;i++)
1382 sfvec3f_lerp(&T->p[i],&A->p[i],&B->p[i],alpha);
1383 return T;
1384}
1385float mfvec3f_dist(struct Multi_Vec3f* A){
1386 int i;
1387 float dist = 0.0f;
1388 for(i=0;i<A->n;i++)
1389 dist += sfvec3f_dist(&A->p[i]);
1390 return dist;
1391}
1392int mfvec3f_same(struct Multi_Vec3f *A, struct Multi_Vec3f *B){
1393 int i, isame;
1394 if(A->n != B->n) return FALSE;
1395 isame = TRUE;
1396 for(i=0;i<A->n;i++)
1397 isame = isame && sfvec3f_same(&A->p[i],&B->p[i]);
1398 return isame;
1399}
1400struct Multi_Vec3f *mfvec3f_arr(struct Multi_Vec3f *A, int i){
1401 return &A[i];
1402}
1403struct Multi_Vec3f mfvec3f_tmps[6];
1404void *mfvec3f_tmp [] = {&mfvec3f_tmps[0],&mfvec3f_tmps[1],&mfvec3f_tmps[2],&mfvec3f_tmps[3],&mfvec3f_tmps[4],&mfvec3f_tmps[5]};
1405ftype ftype_mfvec3f = {
1406 FIELDTYPE_MFVec3f,
1407 VOIDFN mfvec3f_copy,
1408 VOIDFN mfvec3f_add,
1409 VOIDFN mfvec3f_dif,
1410 VOIDFN mfvec3f_scale,
1411 VOIDFN mfvec3f_lerp,
1412 FLOATFN mfvec3f_dist,
1413 INTFN mfvec3f_same,
1414 INTFN mfvec3f_same,
1415 VOIDFN mfvec3f_arr,
1416 VOIDPTR mfvec3f_tmp,
1417};
1418
1419
1420
1421
1422
1423struct SFRotation *sfrotation_inverse(struct SFRotation* T, struct SFRotation *A){
1424 Quaternion qA,qT;
1425 double x,y,z,a;
1426 memcpy(T->c, A->c, sizeof(struct SFRotation));
1427
1428 /* convert both rotation to quaternion */
1429 vrmlrot_to_quaternion(&qA, (double) A->c[0],
1430 (double) A->c[1], (double) A->c[2], (double) A->c[3]);
1431
1432 /* invert it */
1433 quaternion_inverse(&qT,&qA);
1434
1435 /* and return the resultant, as a vrml rotation */
1436 quaternion_to_vrmlrot(&qT, &x, &y, &z, &a);
1437 /* double to floats, can not use pointers... */
1438 T->c[0] = (float) x;
1439 T->c[1] = (float) y;
1440 T->c[2] = (float) z;
1441 T->c[3] = (float) a;
1442 return T;
1443}
1444struct SFRotation *sfrotation_multiply(struct SFRotation* T, struct SFRotation *A, struct SFRotation *B){
1445 Quaternion qA,qB,qT;
1446 double x,y,z,a;
1447
1448 /* convert both rotation to quaternion */
1449 vrmlrot_to_quaternion(&qA, (double) A->c[0],
1450 (double) A->c[1], (double) A->c[2], (double) A->c[3]);
1451
1452 vrmlrot_to_quaternion(&qB, (double) B->c[0],
1453 (double) B->c[1], (double) B->c[2], (double) B->c[3]);
1454
1455 /* multiply them */
1456 quaternion_multiply(&qT,&qA,&qB);
1457
1458 /* and return the resultant, as a vrml rotation */
1459 quaternion_to_vrmlrot(&qT, &x, &y, &z, &a);
1460 /* double to floats, can not use pointers... */
1461 T->c[0] = (float) x;
1462 T->c[1] = (float) y;
1463 T->c[2] = (float) z;
1464 T->c[3] = (float) a;
1465 return T;
1466}
1467
1468struct SFRotation *sfrotation_copy(struct SFRotation* T, struct SFRotation *A){
1469 memcpy(T->c, A->c, sizeof(struct SFRotation));
1470 return T;
1471}
1472struct SFRotation *sfrotation_add(struct SFRotation* T, struct SFRotation *A, struct SFRotation *B){
1473 //rotate a rotation by a dif rotation
1474 sfrotation_multiply(T,A,B);
1475 return T;
1476}
1477struct SFRotation *sfrotation_dif(struct SFRotation* T, struct SFRotation *A, struct SFRotation *B){
1478 //find the difference between 2 rotations, return the dif as rotation
1479 //T= inverse(B)*A
1480 Quaternion qA,qB,qBI,qT;
1481 double x,y,z,a;
1482
1483 /* convert both rotation to quaternion */
1484 vrmlrot_to_quaternion(&qA, (double) A->c[0],
1485 (double) A->c[1], (double) A->c[2], (double) A->c[3]);
1486
1487 vrmlrot_to_quaternion(&qB, (double) B->c[0],
1488 (double) B->c[1], (double) B->c[2], (double) B->c[3]);
1489
1490
1491 quaternion_inverse(&qBI,&qB);
1492 /* multiply them */
1493 quaternion_multiply(&qT,&qBI,&qA);
1494
1495 /* and return the resultant, as a vrml rotation */
1496 quaternion_to_vrmlrot(&qT, &x, &y, &z, &a);
1497 /* double to floats, can not use pointers... */
1498 T->c[0] = (float) x;
1499 T->c[1] = (float) y;
1500 T->c[2] = (float) z;
1501 T->c[3] = (float) a;
1502 return T;
1503}
1504struct SFRotation *sfrotation_scale(struct SFRotation* T, struct SFRotation *A, float S){
1505 //vecscale4f(T->c,A->c,S); // doesn't make sense
1506 //I think scale makes sense in linear world when you scale something that starts at the origin
1507 //In the spherical world, 0 would need to be such that if you take a difference rotation
1508 // delta=A-B
1509 // and scale it,
1510 // delta' = delta * scale
1511 //and add it back
1512 //A' = B + delta'
1513 //then A' would be on the same arc as A and B
1514 //to scale that way for an arbitrary scale -like 2.1- with:
1515 //a) quaternions: q^2.1 where ^ means power of:
1516 // do integer multiples then slerp: q2 = q*q; q3=q*q2; q21 = q2.slerp(q3,.1);
1517 //b) axis angle: sfrotation.angle *= 2.1;
1518 sfrotation_copy(T,A);
1519 T->c[3] *= S; //angle *= S;
1520 return T;
1521}
1522struct SFRotation *sfrotation_slerp(struct SFRotation* T, struct SFRotation *A, struct SFRotation *B, float alpha){
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));
1527 } else {
1528 Quaternion quatA, quatB, quatT;
1529 double x,y,z,a;
1530 vrmlrot_to_quaternion(&quatA,
1531 A->c[0],
1532 A->c[1],
1533 A->c[2],
1534 A->c[3]);
1535
1536 vrmlrot_to_quaternion(&quatB,
1537 B->c[0],
1538 B->c[1],
1539 B->c[2],
1540 B->c[3]);
1541
1542 quaternion_slerp(&quatT, &quatA, &quatB, (double)alpha);
1543 quaternion_to_vrmlrot(&quatT,&x,&y,&z,&a);
1544 /* double to floats, can not use pointers... */
1545 T->c[0] = (float) x;
1546 T->c[1] = (float) y;
1547 T->c[2] = (float) z;
1548 T->c[3] = (float) a;
1549 }
1550 return T;
1551}
1552float sfrotation_dist(struct SFRotation* A){
1553 //from a dif rotation, return the angle
1554 return A->c[4]; //just the angle?
1555}
1556int sfrotation_same(struct SFRotation *A, struct SFRotation *B){
1557 int i,isame = TRUE;
1558 for(i=0;i<4;i++)
1559 isame = isame && A->c[i] == B->c[i];
1560 return isame;
1561}
1562struct SFRotation *sfrotation_arr(struct SFRotation *A, int i){
1563 return &A[i];
1564}
1565struct SFRotation sfrotation_tmps[6];
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,
1579};
1580
1581void do_OrientationChaserTick(void * ptr){
1582 double Now;
1583 struct X3D_OrientationChaser *_node = (struct X3D_OrientationChaser *)ptr;
1584 struct X3D_PositionChaser *node = (struct X3D_PositionChaser *)ptr; //abstract interface
1585 if(!node) return;
1586 if(!_node->_buffer){
1587 chaser_ptrs *p = MALLOCN(node,sizeof(chaser_ptrs));
1588 _node->_buffer = REALLOCN(node,_node->_buffer,Buffer_length * sizeof(struct SFRotation)); //**changes with field type
1589 node->_t = &ftype_sfrotation; //**changes with field type
1590 node->_p = p;
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();
1600 chaser_init(node);
1601 }
1602 Now = TickTime();
1603 if(NODE_NEEDS_COMPILING){
1604 chaser_ptrs *p = node->_p;
1605 ftype *t = node->_t;
1606 static int count = 0;
1607 node->isActive = TRUE;
1608 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionChaser, isActive));
1609 //Q how to tell which set_ was set: set_destination or set_value?
1610 //if(!vecsame3f(node->set_destination.c,node->_destination.c))
1611 if(!t->same(p->set_destination,p->_destination))
1612 chaser_set_destination(node, Now);
1613 //else if(!vecsame3f(node->set_value.c,node->initialValue.c)) //not sure I have the right idea here
1614 else if(!t->same(p->set_value,p->initialValue))
1615 chaser_set_value(node);
1616 MARK_NODE_COMPILED
1617 count++;
1618 }
1619 if(node->isActive)
1620 chaser_tick(node,Now);
1621}
1622
1623void do_OrientationDamperTick(void * ptr){
1624 struct X3D_OrientationDamper *_node = (struct X3D_OrientationDamper *)ptr;
1625 struct X3D_PositionDamper *node = (struct X3D_PositionDamper *)ptr; //abstract type
1626 if(!node)return;
1627 if(!_node->_values){
1628 damper_ptrs *p = MALLOCN(node,sizeof(damper_ptrs));
1629 node->_t = &ftype_sfrotation; //**changes with field type
1630 node->_p = p;
1631 _node->_values = REALLOCN(node,_node->_values,5 * sizeof(struct SFRotation)); //**changes with field type
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;
1639 //damper_CheckInit(node);
1640 damper_Init(node);
1641 }
1642
1643 if(NODE_NEEDS_COMPILING){
1644 //node->isActive = TRUE;
1645 damper_ptrs *p = node->_p;
1646 ftype *t = node->_t;
1647
1648 //if(!vecsame3f(node->set_destination.c,node->_input.c)) //not sure i have the right idea
1649 if(!t->same(p->set_destination,p->_input))
1650 //damper_set_destination(node, node->set_destination);
1651 damper_set_destination(node, p->set_destination);
1652 //set_tau
1653 if(node->tau != node->_tau)
1654 damper_set_tau(node,node->tau);
1655 //set_value
1656 //if(!vecsame3f(node->initialValue.c,node->set_value.c))
1657 if(!t->same(p->initialValue,p->set_value))
1658 //damper_set_value(node,node->set_value);
1659 damper_set_value(node,p->set_value);
1660 MARK_NODE_COMPILED
1661 }
1662 if(node->isActive)
1663 tick_damper(node,TickTime());
1664}
1665
1666
1667void do_OrientationChaserTick_default(void * ptr){
1668 struct X3D_OrientationChaser *node = (struct X3D_OrientationChaser *)ptr;
1669 if(!node)return;
1670 if(NODE_NEEDS_COMPILING){
1671 //default action copy input to output when not implemented
1672 veccopy3f(node->value_changed.c, node->set_destination.c);
1673 node->value_changed.c[3] = node->set_destination.c[3];
1674 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_OrientationChaser, value_changed));
1675 MARK_NODE_COMPILED
1676 }
1677}
1678void do_OrientationDamperTick_default(void * ptr){
1679 struct X3D_OrientationDamper *node = (struct X3D_OrientationDamper *)ptr;
1680 if(!node)return;
1681 if(NODE_NEEDS_COMPILING){
1682 //default action copy input to output when not implemented
1683 veccopy3f(node->value_changed.c, node->set_destination.c);
1684 node->value_changed.c[3] = node->set_destination.c[3];
1685 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_OrientationDamper, value_changed));
1686 MARK_NODE_COMPILED
1687 }
1688}
1689
1690//>> orientation chaser old way works
1691void orichaser_init(struct X3D_OrientationChaser *node)
1692{
1693 int C;
1694 struct SFRotation *buffer = (struct SFRotation*)node->_buffer;
1695 node->_destination = node->initialDestination;
1696 buffer[0]= node->initialDestination; //initial_destination;
1697 for(C= 1; C<Buffer_length; C++ )
1698 buffer[C]= node->initialValue; //initial_value;
1699 node->_previousvalue= node->initialValue; //initial_value;
1700 node->_steptime= node->duration / (double) Buffer_length; //cNumSupports;
1701}
1702double orichaser_UpdateBuffer(struct X3D_OrientationChaser *node, double Now)
1703{
1704 int C;
1705 double Frac;
1706 struct SFRotation *buffer = (struct SFRotation*)node->_buffer;
1707
1708 Frac = (Now - node->_bufferendtime) / node->_steptime;
1709 // is normally < 1. When it has grown to be larger than 1, we have to shift the array because the step response
1710 // of the oldest entry has already reached its destination, and it's time for a newer entry.
1711 // has already reached it
1712 // In the case of a very low frame rate, or a very short cStepTime we may need to shift by more than one entry.
1713
1714 if(Frac >= 1.0)
1715 {
1716 int NumToShift= (int)floor(Frac);
1717 Frac-= (double) NumToShift;
1718 if(NumToShift < Buffer_length)
1719 { // normal case.
1720
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++ )
1725 {
1726 // Hmm, we have a destination value, but don't know how it has
1727 // reached the current state.
1728 // Therefore we do a linear interpolation from the latest value in the buffer to destination.
1729 float Alpha= (float)C / (float)NumToShift;
1730
1731 //Buffer[C]= destination.slerp(Buffer[NumToShift], Alpha);
1732 if(1)sfrotation_slerp(&buffer[C],&node->_destination,&buffer[NumToShift],Alpha); //Q. order of slerp params?
1733 else sfrotation_slerp(&buffer[C],&buffer[NumToShift],&node->_destination,Alpha); //Q. order of slerp params?
1734 }
1735 }else
1736 {
1737 // degenerated case:
1738 //
1739 // We have a _VERY_ low frame rate...
1740 // we can only guess how we should fill the array.
1741 // Maybe we could write part of a linear interpolation
1742 // from Buffer[0] to destination, that goes from BufferEndTime to Now
1743 // (possibly only the end of the interpolation is to be written),
1744 // but if we rech here we are in a very degenerate case...
1745 // Thus we just write destination to the buffer.
1746 node->_previousvalue= NumToShift == Buffer_length? buffer[0] : node->_destination;
1747
1748 for( C= 0; C<Buffer_length; C++ )
1749 buffer[C]= node->_destination;
1750 }
1751 node->_bufferendtime+= NumToShift * node->_steptime;
1752 }
1753 return Frac;
1754}
1755//when a route toNode.toField is PositionChaser.set_destination
1756//we need to call this function (somehow) much like a script?
1757//
1758void orichaser_set_destination(struct X3D_OrientationChaser *node, struct SFRotation Dest, double Now)
1759{
1760 node->_destination= Dest;
1761 // Somehow we assign to Buffer[-1] and wait untill this gets shifted into the real buffer.
1762 // Would we assign to Buffer[0] instead, we'd have no delay, but this would create a jump in the
1763 // output because Buffer[0] is associated with a value in the past.
1764 orichaser_UpdateBuffer(node, Now);
1765}
1766// This function defines the shape of how the output responds to the input.
1767// It must accept values for T in the range 0 <= T <= 1.
1768// In order to create a smooth animation, it should return 0 for T == 0,
1769// 1 for T == 1 and be sufficient smooth in the range 0 <= T <= 1.
1770
1771// It should be optimized for speed, in order for high performance. It's
1772// executed Buffer.length + 1 times each simulation tick.
1773double orichaser_StepResponseCore(double T)
1774{
1775 return .5 - .5 * cos(T * PI);
1776}
1777double orichaser_StepResponse(struct X3D_OrientationChaser *node, double t)
1778{
1779 if(t < 0.0)
1780 return 0.0;
1781 if(t > node->duration)
1782 return 1.0;
1783 // When optimizing for speed, the above two if(.) cases can be omitted,
1784 // as this funciton will not be called for values outside of 0..duration.
1785 return orichaser_StepResponseCore(t / node->duration);
1786}
1787
1788void orichaser_tick(struct X3D_OrientationChaser *node, double Now)
1789{
1790 int C;
1791 double Frac, Alpha;
1792 struct SFRotation Output;
1793 struct SFRotation DeltaIn;
1794 //struct SFRotation DeltaOut;
1795 struct SFRotation tmp0; //, tmp1;
1796 struct SFRotation *buffer = (struct SFRotation*)node->_buffer;
1797
1798 //orichaser_CheckInit(node);
1799 if(!node->_bufferendtime)
1800 {
1801 node->_bufferendtime= Now; // first event we received, so we are in the initialization phase.
1802 node->value_changed= node->initialValue; //initial_value;
1803 return;
1804 }
1805 Frac= orichaser_UpdateBuffer(node, Now);
1806 // Frac is a value in 0 <= Frac < 1.
1807
1808 // Now we can calculate the output.
1809 // This means we calculate the delta between each entry in Buffer and its previous
1810 // entries, calculate the step response of each such step and add it to form the output.
1811
1812 // The oldest vaule Buffer[Buffer.length - 1] needs some extra thought, because it has
1813 // no previous value. More exactly, we haven't stored a previous value anymore.
1814 // However, the step response of that missing previous value has already reached its
1815 // destination, so we can - would we have that previous value - use this as a start point
1816 // for adding the step responses.
1817 // Actually UpdateBuffer(.) maintains this value in
1818
1819 //var Output= previousValue;
1820 Output= node->_previousvalue;
1821 //var DeltaIn= previousValue.inverse().multiply(Buffer[Buffer.length - 1]);
1822 if(0){
1823 sfrotation_inverse(&tmp0,&node->_previousvalue);
1824 sfrotation_multiply(&DeltaIn,&tmp0,&buffer[Buffer_length -1]);
1825 }else{
1826 sfrotation_dif(&DeltaIn,&buffer[Buffer_length -1],&node->_previousvalue); //A - B same as B.inverse x A
1827 }
1828
1829 Alpha = orichaser_StepResponse(node,((double)Buffer_length - 1.0 + Frac) * node->_steptime);
1830 //Output= Output.slerp(Output.multiply(DeltaIn), StepResponse((Buffer.length - 1 + Frac) * cStepTime));
1831 if(0)
1832 sfrotation_multiply(&tmp0,&Output,&DeltaIn);
1833 else
1834 sfrotation_add(&tmp0,&Output,&DeltaIn); //same as multipley
1835
1836 sfrotation_slerp(&Output,&Output,&tmp0,(float)Alpha);
1837 for(C= Buffer_length - 2; C>=0; C-- )
1838 {
1839 //var DeltaIn= Buffer[C + 1].inverse().multiply(Buffer[C]);
1840 if(0){
1841 sfrotation_inverse(&tmp0,&buffer[C + 1]);
1842 sfrotation_multiply(&DeltaIn,&tmp0,&buffer[C]);
1843 }else{
1844 sfrotation_dif(&DeltaIn,&buffer[C],&buffer[C+1]);
1845 }
1846 Alpha = orichaser_StepResponse(node,((float)C + Frac) * node->_steptime);
1847 //Output= Output.slerp(Output.multiply(DeltaIn), StepResponse((C + Frac) * cStepTime));
1848 if(0)
1849 sfrotation_multiply(&tmp0,&Output,&DeltaIn);
1850 else
1851 sfrotation_add(&tmp0,&Output,&DeltaIn);
1852
1853 sfrotation_slerp(&Output,&Output,&tmp0,(float)Alpha); //order of slerp?
1854 }
1855 if(!sfrotation_same(&Output,&node->value_changed)){
1856 node->value_changed= Output;
1857 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_OrientationChaser, value_changed));
1858 }
1859}
1860void orichaser_set_value(struct X3D_OrientationChaser *node, struct SFRotation opos)
1861{
1862 node->value_changed= opos;
1863 node->initialValue = opos;
1864 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_OrientationChaser, value_changed));
1865 node->isActive = TRUE;
1866 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_OrientationChaser, isActive));
1867}
1868
1869
1870void do_OrientationChaserTick_oldway_works(void * ptr){
1871 double Now;
1872 struct X3D_OrientationChaser *node = (struct X3D_OrientationChaser *)ptr;
1873 if(!node) return;
1874 if(!node->_buffer){
1875 node->_buffer = REALLOCN(node,node->_buffer,Buffer_length * sizeof(struct SFRotation));
1876 orichaser_init(node);
1877 }
1878 Now = TickTime();
1879 if(NODE_NEEDS_COMPILING){
1880 //printf("node_needs_compiling\n");
1881 node->isActive = TRUE;
1882 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_OrientationChaser, isActive));
1883 //Q how to tell which set_ was set: set_destination or set_value?
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)) //not sure I have the right idea here
1887 orichaser_set_value(node,node->set_value);
1888 MARK_NODE_COMPILED
1889 }
1890 if(node->isActive)
1891 orichaser_tick(node,Now);
1892}
1893
1894//<<< oldway orientationchaser
1895
1896
1897
1898void do_CoordinateChaserTick(void * ptr){
1899 double Now;
1900 struct X3D_CoordinateChaser *_node = (struct X3D_CoordinateChaser *)ptr;
1901 struct X3D_PositionChaser *node = (struct X3D_PositionChaser *)ptr; //abstract interface
1902 if(!node) return;
1903 if(!_node->_buffer){
1904 chaser_ptrs *p = MALLOCN(node,sizeof(chaser_ptrs));
1905 _node->_buffer = REALLOCN(node,_node->_buffer,Buffer_length * sizeof(struct Multi_Vec3f)); //**changes with field type
1906 memset(_node->_buffer,0,Buffer_length * sizeof(struct Multi_Vec3f));
1907 node->_t = &ftype_mfvec3f; //**changes with field type
1908 node->_p = p;
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();
1918 chaser_init(node);
1919 }
1920 Now = TickTime();
1921 if(NODE_NEEDS_COMPILING){
1922 chaser_ptrs *p = node->_p;
1923 ftype *t = node->_t;
1924
1925 node->isActive = TRUE;
1926 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionChaser, isActive));
1927 //Q how to tell which set_ was set: set_destination or set_value?
1928 //if(!vecsame3f(node->set_destination.c,node->_destination.c))
1929 if(!t->same(p->set_destination,p->_destination))
1930 chaser_set_destination(node, Now);
1931 //else if(!vecsame3f(node->set_value.c,node->initialValue.c)) //not sure I have the right idea here
1932 else if(!t->same(p->set_value,p->initialValue))
1933 chaser_set_value(node);
1934 MARK_NODE_COMPILED
1935 }
1936 if(node->isActive)
1937 chaser_tick(node,Now);
1938}
1939
1940void do_CoordinateDamperTick(void * ptr){
1941 struct X3D_CoordinateDamper *_node = (struct X3D_CoordinateDamper *)ptr;
1942 struct X3D_PositionDamper *node = (struct X3D_PositionDamper *)ptr; //abstract type
1943 if(!node)return;
1944 if(!_node->_values){
1945 damper_ptrs *p = MALLOCN(node,sizeof(damper_ptrs));
1946 node->_t = &ftype_mfvec3f; //**changes with field type
1947 node->_p = p;
1948 _node->_values = REALLOCN(node,_node->_values,5 * sizeof(struct Multi_Vec3f)); //**changes with field type
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;
1957 //damper_CheckInit(node);
1958 damper_Init(node);
1959 }
1960
1961 if(NODE_NEEDS_COMPILING){
1962 //node->isActive = TRUE;
1963 damper_ptrs *p = node->_p;
1964 ftype *t = node->_t;
1965
1966 //if(!vecsame3f(node->set_destination.c,node->_input.c)) //not sure i have the right idea
1967 if(!t->same(p->set_destination,p->_input))
1968 //damper_set_destination(node, node->set_destination);
1969 damper_set_destination(node, p->set_destination);
1970 //set_tau
1971 if(node->tau != node->_tau)
1972 damper_set_tau(node,node->tau);
1973 //set_value
1974 //if(!vecsame3f(node->initialValue.c,node->set_value.c))
1975 if(!t->same(p->initialValue,p->set_value))
1976 //damper_set_value(node,node->set_value);
1977 damper_set_value(node,p->set_value);
1978 MARK_NODE_COMPILED
1979 }
1980 if(node->isActive)
1981 tick_damper(node,TickTime());
1982}
1983
1984
1985
1986
1987
1988void do_PositionChaser2DTick_default(void * ptr){
1989 struct X3D_PositionChaser2D *node = (struct X3D_PositionChaser2D *)ptr;
1990 if(!node)return;
1991 if(NODE_NEEDS_COMPILING){
1992 //default action copy input to output when not implemented
1993 veccopy2f(node->value_changed.c, node->set_destination.c);
1994 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionChaser2D, value_changed));
1995 MARK_NODE_COMPILED
1996 }
1997
1998}
1999void do_PositionDamper2DTick_default(void * ptr){
2000 struct X3D_PositionDamper2D *node = (struct X3D_PositionDamper2D *)ptr;
2001 if(!node)return;
2002 if(NODE_NEEDS_COMPILING){
2003 //default action copy input to output when not implemented
2004 veccopy2f(node->value_changed.c, node->set_destination.c);
2005 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionDamper2D, value_changed));
2006 MARK_NODE_COMPILED
2007 }
2008
2009}
2010
2011struct SFVec2f *sfvec2f_copy(struct SFVec2f* T, struct SFVec2f *A){
2012 veccopy2f(T->c,A->c);
2013 return T;
2014}
2015struct SFVec2f *sfvec2f_add(struct SFVec2f* T, struct SFVec2f *A, struct SFVec2f *B){
2016 vecadd2f(T->c,A->c,B->c);
2017 return T;
2018}
2019struct SFVec2f *sfvec2f_dif(struct SFVec2f* T, struct SFVec2f *A, struct SFVec2f *B){
2020 vecdif2f(T->c,A->c,B->c);
2021 return T;
2022}
2023struct SFVec2f *sfvec2f_scale(struct SFVec2f* T, struct SFVec2f *A, float S){
2024 vecscale2f(T->c,A->c,S);
2025 return T;
2026}
2027struct SFVec2f *sfvec2f_lerp(struct SFVec2f* T, struct SFVec2f *A, struct SFVec2f *B, float alpha){
2028 veclerp2f(T->c,A->c,B->c,alpha);
2029 return T;
2030}
2031float sfvec2f_dist(struct SFVec2f* A){
2032 return veclength2f(A->c);
2033}
2034int sfvec2f_same(struct SFVec2f *A, struct SFVec2f *B){
2035 return vecsame2f(A->c,B->c);
2036}
2037struct SFVec2f *sfvec2f_arr(struct SFVec2f *A, int i){
2038 return &A[i];
2039}
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 = {
2043 FIELDTYPE_SFVec2f,
2044 VOIDFN sfvec2f_copy,
2045 VOIDFN sfvec2f_add,
2046 VOIDFN sfvec2f_dif,
2047 VOIDFN sfvec2f_scale,
2048 VOIDFN sfvec2f_lerp,
2049 FLOATFN sfvec2f_dist,
2050 INTFN sfvec2f_same,
2051 INTFN sfvec2f_same,
2052 VOIDFN sfvec2f_arr,
2053 VOIDPTR sfvec2f_tmp,
2054};
2055
2056void do_PositionChaser2DTick(void * ptr){
2057 double Now;
2058 struct X3D_PositionChaser2D *_node = (struct X3D_PositionChaser2D *)ptr;
2059 struct X3D_PositionChaser *node = (struct X3D_PositionChaser *)ptr; //abstract interface
2060 if(!node) return;
2061 if(!_node->_buffer){
2062 chaser_ptrs *p = MALLOCN(node,sizeof(chaser_ptrs));
2063 _node->_buffer = REALLOCN(node,_node->_buffer,Buffer_length * sizeof(struct SFVec2f)); //**changes with ftype
2064 node->_t = &ftype_sfvec2f; //***changes with ftype
2065 node->_p = p;
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();
2075 chaser_init(node);
2076 }
2077 Now = TickTime();
2078 if(NODE_NEEDS_COMPILING){
2079 chaser_ptrs *p = node->_p;
2080 ftype *t = node->_t;
2081
2082 node->isActive = TRUE;
2083 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionChaser, isActive));
2084 //Q how to tell which set_ was set: set_destination or set_value?
2085 //if(!vecsame3f(node->set_destination.c,node->_destination.c))
2086 if(!t->same(p->set_destination,p->_destination))
2087 chaser_set_destination(node, Now);
2088 //else if(!vecsame3f(node->set_value.c,node->initialValue.c)) //not sure I have the right idea here
2089 else if(!t->same(p->set_value,p->initialValue))
2090 chaser_set_value(node);
2091 MARK_NODE_COMPILED
2092 }
2093 if(node->isActive)
2094 chaser_tick(node,Now);
2095}
2096void do_PositionDamper2DTick(void * ptr){
2097 struct X3D_PositionDamper2D *_node = (struct X3D_PositionDamper2D *)ptr;
2098 struct X3D_PositionDamper *node = (struct X3D_PositionDamper *)ptr; //abstract type
2099 if(!node)return;
2100 if(!_node->_values){
2101 damper_ptrs *p = MALLOCN(node,sizeof(damper_ptrs));
2102 node->_t = &ftype_sfvec2f; //** changes with ftype
2103 node->_p = p;
2104 _node->_values = REALLOCN(node,_node->_values,5 * sizeof(struct SFVec2f)); //** changes with ftype
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;
2112 //damper_CheckInit(node);
2113 damper_Init(node);
2114 }
2115
2116 if(NODE_NEEDS_COMPILING){
2117 //node->isActive = TRUE;
2118 damper_ptrs *p = node->_p;
2119 ftype *t = node->_t;
2120
2121 //if(!vecsame3f(node->set_destination.c,node->_input.c)) //not sure i have the right idea
2122 if(!t->same(p->set_destination,p->_input))
2123 //damper_set_destination(node, node->set_destination);
2124 damper_set_destination(node, p->set_destination);
2125 //set_tau
2126 if(node->tau != node->_tau)
2127 damper_set_tau(node,node->tau);
2128 //set_value
2129 //if(!vecsame3f(node->initialValue.c,node->set_value.c))
2130 if(!t->same(p->initialValue,p->set_value))
2131 //damper_set_value(node,node->set_value);
2132 damper_set_value(node,p->set_value);
2133 MARK_NODE_COMPILED
2134 }
2135 if(node->isActive)
2136 tick_damper(node,TickTime());
2137}
2138
2139
2140void do_ScalarChaserTick_default(void * ptr){
2141 struct X3D_ScalarChaser *node = (struct X3D_ScalarChaser *)ptr;
2142 if(!node)return;
2143 if(NODE_NEEDS_COMPILING){
2144 //default action copy input to output when not implemented
2145 node->value_changed = node->set_destination;
2146 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_ScalarChaser, value_changed));
2147 MARK_NODE_COMPILED
2148 }
2149}
2150void do_ScalarDamperTick_default(void * ptr){
2151 struct X3D_ScalarDamper *node = (struct X3D_ScalarDamper *)ptr;
2152 if(!node)return;
2153 if(NODE_NEEDS_COMPILING){
2154 //default action copy input to output when not implemented
2155 node->value_changed = node->set_destination;
2156 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_ScalarDamper, value_changed));
2157 MARK_NODE_COMPILED
2158 }
2159}
2160float *scalar_copy(float* T, float *A){
2161 *T = *A;
2162 return T;
2163}
2164float *scalar_add(float* T, float *A, float *B){
2165 *T = *A + *B;
2166 return T;
2167}
2168float *scalar_dif(float* T, float *A, float *B){
2169 *T = *A - *B;
2170 return T;
2171}
2172float *scalar_scale(float* T, float *A, float S){
2173 *T = *A *S;
2174 return T;
2175}
2176float *scalar_lerp(float* T, float *A, float *B, float alpha){
2177 *T = (1.0f -alpha)*(*A) + alpha*(*B);
2178 return T;
2179}
2180float scalar_dist(float* A){
2181 return (float)fabs(*A);
2182}
2183int scalar_same(float *A, float *B){
2184 return *A == *B ? TRUE : FALSE;
2185}
2186float *scalar_arr(float *A, int i){
2187 return &A[i];
2188}
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 = {
2192 FIELDTYPE_SFFloat,
2193 VOIDFN scalar_copy,
2194 VOIDFN scalar_add,
2195 VOIDFN scalar_dif,
2196 VOIDFN scalar_scale,
2197 VOIDFN scalar_lerp,
2198 FLOATFN scalar_dist,
2199 INTFN scalar_same,
2200 INTFN scalar_same,
2201 VOIDFN scalar_arr,
2202 VOIDPTR scalar_tmp,
2203};
2204
2205void do_ScalarChaserTick(void * ptr){
2206 double Now;
2207 struct X3D_ScalarChaser *_node = (struct X3D_ScalarChaser *)ptr;
2208 struct X3D_PositionChaser *node = (struct X3D_PositionChaser *)ptr; //abstract interface
2209 if(!node) return;
2210 if(!_node->_buffer){
2211 chaser_ptrs *p = MALLOCN(node,sizeof(chaser_ptrs));
2212 _node->_buffer = REALLOCN(node,_node->_buffer,Buffer_length * sizeof(float)); //**changes with ftype
2213 node->_t = &ftype_scalar; //***changes with ftype
2214 node->_p = p;
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();
2224 chaser_init(node);
2225 }
2226 Now = TickTime();
2227 if(NODE_NEEDS_COMPILING){
2228 chaser_ptrs *p = node->_p;
2229 ftype *t = node->_t;
2230
2231 node->isActive = TRUE;
2232 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionChaser, isActive));
2233 //Q how to tell which set_ was set: set_destination or set_value?
2234 //if(!vecsame3f(node->set_destination.c,node->_destination.c))
2235 if(!t->same(p->set_destination,p->_destination))
2236 chaser_set_destination(node, Now);
2237 //else if(!vecsame3f(node->set_value.c,node->initialValue.c)) //not sure I have the right idea here
2238 else if(!t->same(p->set_value,p->initialValue))
2239 chaser_set_value(node);
2240 MARK_NODE_COMPILED
2241 }
2242 if(node->isActive)
2243 chaser_tick(node,Now);
2244}
2245void do_ScalarDamperTick(void * ptr){
2246 struct X3D_ScalarDamper *_node = (struct X3D_ScalarDamper *)ptr;
2247 struct X3D_PositionDamper *node = (struct X3D_PositionDamper *)ptr; //abstract type
2248 if(!node)return;
2249 if(!_node->_values){
2250 damper_ptrs *p = MALLOCN(node,sizeof(damper_ptrs));
2251 node->_t = &ftype_scalar; //** changes with ftype
2252 node->_p = p;
2253 _node->_values = REALLOCN(node,_node->_values,5 * sizeof(float)); //** changes with ftype
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;
2261 //damper_CheckInit(node);
2262 damper_Init(node);
2263 }
2264
2265 if(NODE_NEEDS_COMPILING){
2266 //node->isActive = TRUE;
2267 damper_ptrs *p = node->_p;
2268 ftype *t = node->_t;
2269
2270 //if(!vecsame3f(node->set_destination.c,node->_input.c)) //not sure i have the right idea
2271 if(!t->same(p->set_destination,p->_input))
2272 //damper_set_destination(node, node->set_destination);
2273 damper_set_destination(node, p->set_destination);
2274 //set_tau
2275 if(node->tau != node->_tau)
2276 damper_set_tau(node,node->tau);
2277 //set_value
2278 //if(!vecsame3f(node->initialValue.c,node->set_value.c))
2279 if(!t->same(p->initialValue,p->set_value))
2280 //damper_set_value(node,node->set_value);
2281 damper_set_value(node,p->set_value);
2282 MARK_NODE_COMPILED
2283 }
2284 if(node->isActive)
2285 tick_damper(node,TickTime());
2286}
2287
2288void do_TexCoordChaser2DTick_default(void * ptr){
2289 struct X3D_TexCoordChaser2D *node = (struct X3D_TexCoordChaser2D *)ptr;
2290 if(!node)return;
2291 if(NODE_NEEDS_COMPILING){
2292 //default action copy input to output when not implemented
2293 int n;
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));
2298 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_TexCoordChaser2D, value_changed));
2299 MARK_NODE_COMPILED
2300 }
2301}
2302void do_TexCoordDamper2DTick_default(void * ptr){
2303 struct X3D_TexCoordDamper2D *node = (struct X3D_TexCoordDamper2D *)ptr;
2304 if(!node)return;
2305 if(NODE_NEEDS_COMPILING){
2306 //default action copy input to output when not implemented
2307 int n;
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));
2312 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_TexCoordDamper2D, value_changed));
2313 MARK_NODE_COMPILED
2314 }
2315}
2316
2317struct Multi_Vec2f *mfvec2f_copy(struct Multi_Vec2f* T, struct Multi_Vec2f *A){
2318 T->p = REALLOC(T->p,A->n * sizeof(struct SFVec2f));
2319 T->n = A->n;
2320 memcpy(T->p,A->p,A->n * sizeof(struct SFVec2f));
2321 return T;
2322}
2323struct Multi_Vec2f *mfvec2f_add(struct Multi_Vec2f* T, struct Multi_Vec2f *A, struct Multi_Vec2f *B){
2324 int i;
2325 T->n = min(A->n,B->n);
2326 T->p = REALLOC(T->p,T->n * sizeof(struct SFVec2f));
2327 for(i=0;i<T->n;i++)
2328 sfvec2f_add(&T->p[i],&A->p[i],&B->p[i]);
2329 return T;
2330}
2331struct Multi_Vec2f *mfvec2f_dif(struct Multi_Vec2f* T, struct Multi_Vec2f *A, struct Multi_Vec2f *B){
2332 int i;
2333 T->n = min(A->n,B->n);
2334 T->p = REALLOC(T->p,T->n * sizeof(struct SFVec2f));
2335 for(i=0;i<T->n;i++)
2336 sfvec2f_dif(&T->p[i],&A->p[i],&B->p[i]);
2337 return T;
2338}
2339struct Multi_Vec2f *mfvec2f_scale(struct Multi_Vec2f* T, struct Multi_Vec2f *A, float S){
2340 int i;
2341 T->n = A->n;
2342 T->p = REALLOC(T->p,T->n * sizeof(struct SFVec2f));
2343 for(i=0;i<T->n;i++)
2344 sfvec2f_scale(&T->p[i],&A->p[i],S);
2345 return T;
2346}
2347struct Multi_Vec2f *mfvec2f_lerp(struct Multi_Vec2f* T, struct Multi_Vec2f *A, struct Multi_Vec2f *B, float alpha){
2348 int i;
2349 T->n = min(A->n,B->n);
2350 T->p = REALLOC(T->p,T->n * sizeof(struct SFVec2f));
2351 for(i=0;i<T->n;i++)
2352 sfvec2f_lerp(&T->p[i],&A->p[i],&B->p[i],alpha);
2353 return T;
2354}
2355
2356float mfvec2f_dist(struct Multi_Vec2f* A){
2357 int i;
2358 float dist = 0.0f;
2359 for(i=0;i<A->n;i++)
2360 dist += sfvec2f_dist(&A->p[i]);
2361 return dist;
2362}
2363int mfvec2f_same(struct Multi_Vec2f *A, struct Multi_Vec2f *B){
2364 int i, isame;
2365 if(A->n != B->n) return FALSE;
2366 isame = TRUE;
2367 for(i=0;i<A->n;i++)
2368 isame = isame && sfvec2f_same(&A->p[i],&B->p[i]);
2369 return isame;
2370}
2371struct Multi_Vec2f *mfvec2f_arr(struct Multi_Vec2f *A, int i){
2372 return &A[i];
2373}
2374struct Multi_Vec2f mfvec2f_tmps[6];
2375void *mfvec2f_tmp [] = {&mfvec2f_tmps[0],&mfvec2f_tmps[1],&mfvec2f_tmps[2],&mfvec2f_tmps[3],&mfvec2f_tmps[4],&mfvec2f_tmps[5]};
2376ftype ftype_mfvec2f = {
2377 FIELDTYPE_MFVec2f,
2378 VOIDFN mfvec2f_copy,
2379 VOIDFN mfvec2f_add,
2380 VOIDFN mfvec2f_dif,
2381 VOIDFN mfvec2f_scale,
2382 VOIDFN mfvec2f_lerp,
2383 FLOATFN mfvec2f_dist,
2384 INTFN mfvec2f_same,
2385 INTFN mfvec2f_same,
2386 VOIDFN mfvec2f_arr,
2387 VOIDPTR mfvec2f_tmp,
2388};
2389void do_TexCoordChaser2DTick(void * ptr){
2390 double Now;
2391 struct X3D_TexCoordChaser2D * _node = (struct X3D_TexCoordChaser2D *)ptr;
2392 struct X3D_PositionChaser *node = (struct X3D_PositionChaser *)ptr; //abstract interface
2393 if(!node) return;
2394 if(!_node->_buffer){
2395 chaser_ptrs *p = MALLOCN(node,sizeof(chaser_ptrs));
2396 _node->_buffer = REALLOCN(node,_node->_buffer,Buffer_length * sizeof(struct Multi_Vec2f)); //**changes with field type
2397 memset(_node->_buffer,0,Buffer_length * sizeof(struct Multi_Vec2f));
2398 node->_t = &ftype_mfvec2f; //**changes with field type
2399 node->_p = p;
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();
2409 chaser_init(node);
2410 }
2411 Now = TickTime();
2412 if(NODE_NEEDS_COMPILING){
2413 chaser_ptrs *p = node->_p;
2414 ftype *t = node->_t;
2415
2416 node->isActive = TRUE;
2417 MARK_EVENT ((struct X3D_Node*)node, offsetof(struct X3D_PositionChaser, isActive));
2418 //Q how to tell which set_ was set: set_destination or set_value?
2419 //if(!vecsame3f(node->set_destination.c,node->_destination.c))
2420 if(!t->same(p->set_destination,p->_destination))
2421 chaser_set_destination(node, Now);
2422 //else if(!vecsame3f(node->set_value.c,node->initialValue.c)) //not sure I have the right idea here
2423 else if(!t->same(p->set_value,p->initialValue))
2424 chaser_set_value(node);
2425 MARK_NODE_COMPILED
2426 }
2427 if(node->isActive)
2428 chaser_tick(node,Now);
2429}
2430
2431void do_TexCoordDamper2DTick(void * ptr){
2432 struct X3D_TexCoordDamper2D *_node = (struct X3D_TexCoordDamper2D *)ptr;
2433 struct X3D_PositionDamper *node = (struct X3D_PositionDamper *)ptr; //abstract type
2434 if(!node)return;
2435 if(!_node->_values){
2436 damper_ptrs *p = MALLOCN(node,sizeof(damper_ptrs));
2437 node->_t = &ftype_mfvec2f; //**changes with field type
2438 node->_p = p;
2439 _node->_values = REALLOCN(node,_node->_values,5 * sizeof(struct Multi_Vec2f)); //**changes with field type
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;
2448 //damper_CheckInit(node);
2449 damper_Init(node);
2450 }
2451
2452 if(NODE_NEEDS_COMPILING){
2453 //node->isActive = TRUE;
2454 damper_ptrs *p = node->_p;
2455 ftype *t = node->_t;
2456
2457 //if(!vecsame3f(node->set_destination.c,node->_input.c)) //not sure i have the right idea
2458 if(!t->same(p->set_destination,p->_input))
2459 //damper_set_destination(node, node->set_destination);
2460 damper_set_destination(node, p->set_destination);
2461 //set_tau
2462 if(node->tau != node->_tau)
2463 damper_set_tau(node,node->tau);
2464 //set_value
2465 //if(!vecsame3f(node->initialValue.c,node->set_value.c))
2466 if(!t->same(p->initialValue,p->set_value))
2467 //damper_set_value(node,node->set_value);
2468 damper_set_value(node,p->set_value);
2469 MARK_NODE_COMPILED
2470 }
2471 if(node->isActive)
2472 tick_damper(node,TickTime());
2473}
2474//============================
2475
2476