FreeWRL / FreeX3D 4.3.0
SensInterps.c
1/*
2
3
4Do Sensors and Interpolators in C, not in perl.
5
6Interps are the "EventsProcessed" fields of interpolators.
7
8*/
9
10
11/****************************************************************************
12 This file is part of the FreeWRL/FreeX3D Distribution.
13
14 Copyright 2009 CRC Canada. (http://www.crc.gc.ca)
15
16 FreeWRL/FreeX3D is free software: you can redistribute it and/or modify
17 it under the terms of the GNU Lesser Public License as published by
18 the Free Software Foundation, either version 3 of the License, or
19 (at your option) any later version.
20
21 FreeWRL/FreeX3D is distributed in the hope that it will be useful,
22 but WITHOUT ANY WARRANTY; without even the implied warranty of
23 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
24 GNU General Public License for more details.
25
26 You should have received a copy of the GNU General Public License
27 along with FreeWRL/FreeX3D. If not, see <http://www.gnu.org/licenses/>.
28****************************************************************************/
29
30
31
32#include <config.h>
33#include <system.h>
34#include <display.h>
35#include <internal.h>
36
37#include <libFreeWRL.h>
38#include <list.h>
39
40#include "../vrml_parser/Structs.h"
41#include "../input/InputFunctions.h"
42#include "../opengl/LoadTextures.h" /* for finding a texture url in a multi url */
43
44
45#include "../main/headers.h"
46#include "../opengl/OpenGL_Utils.h"
47#include "../scenegraph/RenderFuncs.h"
48
49#include "../x3d_parser/Bindable.h"
50#include "../scenegraph/LinearAlgebra.h"
51#include "../scenegraph/Collision.h"
52#include "../scenegraph/quaternion.h"
53#include "../vrml_parser/CRoutes.h"
54#include "../opengl/OpenGL_Utils.h"
55#include "../opengl/Textures.h" /* for finding a texture url in a multi url */
56
57#include "SensInterps.h"
58
59
60
61/* when we get a new sound source, what is the number for this? */
62//int SoundSourceNumber = 0;
63typedef struct pSensInterps{
64 int stub;
65}* ppSensInterps;
66void *SensInterps_constructor(){
67 void *v = MALLOCV(sizeof(struct pSensInterps));
68 memset(v,0,sizeof(struct pSensInterps));
69 return v;
70}
71void SensInterps_init(struct tSensInterps *t)
72{
73 //public
74 //private
75 t->prv = SensInterps_constructor();
76 {
77 //ppSensInterps p = (ppSensInterps)t->prv;
78 }
79}
80
81
83//void locateAudioSource (struct X3D_AudioClip *node);
84
85
86/* time dependent sensor nodes- check/change activity state */
87void do_active_inactive (
88 int *act, /* pointer to are we active or not? */
89 double *inittime, /* pointer to nodes inittime */
90 double *startt, /* pointer to nodes startTime */
91 double *stopt, /* pointer to nodes stop time */
92 int loop, /* nodes loop field */
93 double myDuration, /* duration of cycle */
94 double speed, /* speed field */
95 double elapsedTime /* cumulative non-paused time */
96) {
97
98 /* what we do now depends on whether we are active or not */
99 /* gcc seemed to have problems mixing double* and floats in a function
100 call, so make them all doubles. Note duplicate printfs in
101 do_active_inactive call - uncomment and make sure all are identical
102 printf ("called ");
103 printf ("act %d ",*act);
104 printf ("initt %lf ",*inittime);
105 printf ("startt %lf ",*startt);
106 printf ("stopt %lf ",*stopt);
107 printf ("loop %d ",loop);
108 printf ("myDuration %lf ",myDuration);
109 printf ("speed %f\n",speed);
110 */
111 double ticktime = TickTime(); //changes once per frame, not in here
112
113 if (*act == 1) { /* active - should we stop? */
114 #ifdef SEVERBOSE
115 printf ("is active tick %f startt %f stopt %f\n",
116 TickTime(), *startt, *stopt);
117 #endif
118
119 if (ticktime > *stopt) {
120 if (*startt >= *stopt) {
121 /* cases 1 and 2 */
122 if (!(loop)) {
123 /*printf ("case 1 and 2, not loop md %f sp %f fabs %f\n",
124 myDuration, speed, fabs(myDuration/speed));
125 */
126
127 /* if (speed != 0) */
128 if (! APPROX(speed, 0)) {
129 //if (ticktime >= (*startt + fabs(myDuration/speed))) {
130 if (elapsedTime >= fabs(myDuration/speed) ) {
131 #ifdef SEVERBOSE
132 printf ("stopping case x\n");
133 printf ("TickTime() %f\n",ticktime);
134 printf ("startt %f\n",*startt);
135 printf ("myDuration %f\n",myDuration);
136 printf ("speed %f\n",speed);
137 #endif
138
139 *act = 0;
140 *stopt = ticktime;
141 }
142 }
143 }
144 } else {
145 #ifdef SEVERBOSE
146 printf ("stopping case z\n");
147 #endif
148
149 *act = 0;
150 *stopt = ticktime;
151 }
152 }
153 }
154
155 /* immediately process start events; as per spec. */
156 if (*act == 0) { /* active - should we start? */
157 /* printf ("is not active TickTime %f startt %f\n",TickTime(),*startt); */
158
159 if (ticktime >= *startt) {
160 /* We just might need to start running */
161
162 if (ticktime >= *stopt) {
163 /* lets look at the initial conditions; have not had a stoptime
164 event (yet) */
165
166 if (loop) {
167 if (*startt >= *stopt) {
168 /* VRML standards, table 4.2 case 2 */
169 /* printf ("CASE 2\n"); */
170 /* Umut Sezen's code: */
171 if (!(*startt > 0)) *startt = ticktime;
172 *act = 1;
173 }
174 } else if (*startt >= *stopt) {
175 if (*startt > *inittime) {
176 /* ie, we have an event */
177 /* printf ("case 1 here\n"); */
178 /* we should be running VRML standards, table 4.2 case 1 */
179 /* Umut Sezen's code: */
180 if (!(*startt > 0))
181 *startt = ticktime;
182 *act = 1;
183 }
184 }
185 } else {
186 /* printf ("case 3 here\n"); */
187 /* we should be running -
188 VRML standards, table 4.2 cases 1 and 2 and 3 */
189 /* Umut Sezen's code: */
190 if (!(*startt > 0)) *startt = ticktime;
191 *act = 1;
192 }
193 }
194 }
195}
196
197
198/* Interpolators - local routine, look for the appropriate key */
199int find_key (int kin, float frac, float *keys) {
200 int counter;
201
202 for (counter=1; counter <= kin; counter++) {
203 if (frac <keys[counter]) {
204 return counter;
205 }
206 }
207 return kin; /* huh? not found! */
208}
209
210
211/* ScalarInterpolators - return only one float */
212void do_OintScalar (void *node) {
213 /* ScalarInterpolator - store final value in px->value_changed */
214 struct X3D_ScalarInterpolator *px;
215 int kin, kvin;
216 float *kVs;
217 int counter;
218
219 if (!node) return;
220 px = (struct X3D_ScalarInterpolator *) node;
221 kin = px->key.n;
222 kvin = px->keyValue.n;
223 kVs = px->keyValue.p;
224
225 MARK_EVENT (node, offsetof (struct X3D_ScalarInterpolator, value_changed));
226
227 /* make sure we have the keys and keyValues */
228 if ((kvin == 0) || (kin == 0)) {
229 px->value_changed = (float) 0.0;
230 return;
231 }
232 if (kin>kvin) kin=kvin; /* means we don't use whole of keyValue, but... */
233
234 #ifdef SEVERBOSE
235 printf ("ScalarInterpolator, kin %d kvin %d, vc %f\n",kin,kvin,px->value_changed);
236 #endif
237
238 /* set_fraction less than or greater than keys */
239 if (px->set_fraction <= px->key.p[0]) {
240 px->value_changed = kVs[0];
241 } else if (px->set_fraction >= px->key.p[kin-1]) {
242 px->value_changed = kVs[kvin-1];
243 } else {
244 /* have to go through and find the key before */
245 counter=find_key(kin,(float)(px->set_fraction),px->key.p);
246 px->value_changed =
247 (px->set_fraction - px->key.p[counter-1]) /
248 (px->key.p[counter] - px->key.p[counter-1]) *
249 (kVs[counter] - kVs[counter-1]) +
250 kVs[counter-1];
251 }
252}
253
254/* VectorInterpolator - return MFFloat
255* proposed by Instant Player team, see 26_Hanim pdf
256*/
257void do_OintVector(void* node) {
258 /* VectorInterpolator - store final value in px->value_changed */
259 struct X3D_VectorInterpolator* px;
260 int kin, kvin, ksize;
261 float* kVs;
262 int counter;
263
264 if (!node) return;
265 px = (struct X3D_VectorInterpolator*)node;
266 kin = px->key.n;
267 kvin = px->keyValue.n;
268 ksize = kvin / kin; //should be an int
269 kVs = px->keyValue.p;
270 //ensure MF space
271 if (px->value_changed.n < ksize) {
272 px->value_changed.p = realloc(px->value_changed.p, ksize * sizeof(float));
273 px->value_changed.n = ksize;
274 }
275 MARK_EVENT(node, offsetof(struct X3D_VectorInterpolator, value_changed));
276
277 /* make sure we have the keys and keyValues */
278 if ((kvin == 0) || (kin == 0)) {
279 memset(px->value_changed.p,0,ksize*sizeof(float));
280 return;
281 }
282 if (kin > kvin) kin = kvin; /* means we don't use whole of keyValue, but... */
283
284#ifdef SEVERBOSE
285 printf("VectorInterpolator, kin %d kvin %d, vc %f\n", kin, kvin, px->value_changed);
286#endif
287
288 /* set_fraction less than or greater than keys */
289 if (px->set_fraction <= px->key.p[0]) {
290 memcpy(px->value_changed.p, &kVs[0*ksize], ksize*sizeof(float));
291 }
292 else if (px->set_fraction >= px->key.p[kin - 1]) {
293 //px->value_changed = kVs[kvin - 1];
294 memcpy(px->value_changed.p, &kVs[(kvin - 1) * ksize], ksize * sizeof(float));
295 }
296 else {
297 /* have to go through and find the key before */
298 counter = find_key(kin, (float)(px->set_fraction), px->key.p);
299 float incrementfactor = (px->set_fraction - px->key.p[counter - 1]) /
300 (px->key.p[counter] - px->key.p[counter - 1]);
301 for (int i = 0; i < ksize; i++) {
302 px->value_changed.p[i] =
303 incrementfactor * (kVs[counter*ksize +i] - kVs[(counter - 1)*ksize +i]) +
304 kVs[(counter - 1)*ksize +i];
305 }
306 }
307}
308
309
310
311
312/* from Instant Player paper - see 26_Hanim .pdf "The Morph Node", Alexa, Buhr, Muller / Fraunhofer.
313CoordinateMorpher {
314eventIn MFFloat set_weights
315exposedFieldMFVec3f keyValue []
316eventOut MFVec3f value_changed
317}
318The CoordinateMorpher node linearly interpolates among a
319set of MFVec3f values. Unlike the CoordinateInterpolator it does
320not interpolate two key frames but is able to blend any number of
321shapes. The number of coordinates in the keyValue shall be an
322integer multiple of the number of keyframes in the key field. That
323integer multiple defines how many coordinates will be contained in
324the value_changed eventout slot
325dug9: so instead of find_key, and interpolating between 2, _all_ keyvalues are used in weighted sum.
326out[i] = in[i,j]*weight[j]
327*/
328void do_CoordinateMorph(void* node) {
329 struct X3D_CoordinateMorpher* px;
330 int kin, kvin, ksize;
331 struct SFVec3f* kVs;
332
333 if (!node) return;
334 px = (struct X3D_CoordinateMorpher*)node;
335
336
337 MARK_EVENT(node, offsetof(struct X3D_CoordinateMorpher, value_changed));
338
339 kin = px->set_weights.n;
340 kvin = px->keyValue.n;
341 ksize = kvin / kin; //should be int
342 kVs = px->keyValue.p;
343
344 // ensure space
345 if (ksize != px->value_changed.n) {
346 px->value_changed.n = ksize;
347 px->value_changed.p = realloc(px->value_changed.p, sizeof(struct SFVec3f) * ksize);
348 }
349
350 /* make sure we have the keys and keyValues */
351 if ((kvin == 0) || (kin == 0)) {
352 memset(px->value_changed.p, 0, ksize * sizeof(struct SFVec3f));
353 return;
354 }
355 if (kin > kvin) kin = kvin; // means we don't use whole of keyValue
356 struct SFVec3f* vc = px->value_changed.p;
357 struct SFVec3f* kv = px->keyValue.p;
358 float* wt = px->set_weights.p;
359 //big loop do weighted sum
360 float wtvc[3];
361 for (int i = 0; i < ksize; i++) {
362 vecset3f(vc[i].c, 0.0f, 0.0f, 0.0f);
363 for (int j = 0; j < kin; j++)
364 {
365 vecscale3f(wtvc, kv[j*ksize +i].c, wt[j]);
366 vecadd3f(vc[i].c, vc[i].c, wtvc);
367 }
368 }
369}
370// NormalMorpher same as CoordinateMorpher above, except normalize summed value.
371void do_NormalMorph(void* node) {
372 struct X3D_NormalMorpher* px;
373 int kin, kvin, ksize;
374 struct SFVec3f* kVs;
375
376 if (!node) return;
377 px = (struct X3D_NormalMorpher*)node;
378
379
380 MARK_EVENT(node, offsetof(struct X3D_NormalMorpher, value_changed));
381
382 kin = px->set_weights.n;
383 kvin = px->keyValue.n;
384 ksize = kvin / kin; //should be int
385 kVs = px->keyValue.p;
386
387 // ensure space
388 if (ksize != px->value_changed.n) {
389 px->value_changed.n = ksize;
390 px->value_changed.p = realloc(px->value_changed.p, sizeof(struct SFVec3f) * ksize);
391 }
392
393 /* make sure we have the keys and keyValues */
394 if ((kvin == 0) || (kin == 0)) {
395 memset(px->value_changed.p, 0, ksize * sizeof(struct SFVec3f));
396 return;
397 }
398 if (kin > kvin) kin = kvin; // means we don't use whole of keyValue
399 struct SFVec3f* vc = px->value_changed.p;
400 struct SFVec3f* kv = px->keyValue.p;
401 float* wt = px->set_weights.p;
402 //big loop do weighted sum
403 float wtvc[3];
404 for (int i = 0; i < ksize; i++) {
405 vecset3f(vc[i].c, 0.0f, 0.0f, 0.0f);
406 for (int j = 0; j < kin; j++)
407 {
408 vecscale3f(wtvc, kv[j * ksize + i].c, wt[j]);
409 vecadd3f(vc[i].c, vc[i].c, wtvc);
410 }
411 vecnormalize3f(vc[i].c, vc[i].c);
412 }
413}
414
415void do_OintNormal(void *node) {
416 struct X3D_NormalInterpolator *px;
417 int kin, kvin/* , counter */;
418 struct SFVec3f *kVs;
419 struct SFVec3f *valchanged;
420
421 int thisone, prevone; /* which keyValues we are interpolating between */
422 int tmp;
423 float interval; /* where we are between 2 values */
424 struct point_XYZ normalval; /* different structures for normalization calls */
425 int kpkv; /* keys per key value */
426 int indx;
427 int myKey;
428
429 if (!node) return;
430 px = (struct X3D_NormalInterpolator *) node;
431
432
433 #ifdef SEVERBOSE
434 printf ("debugging OintCoord keys %d kv %d vc %d\n",px->keyValue.n, px->key.n,px->value_changed.n);
435 #endif
436
437 MARK_EVENT (node, offsetof (struct X3D_NormalInterpolator, value_changed));
438
439 kin = px->key.n;
440 kvin = px->keyValue.n;
441 kVs = px->keyValue.p;
442 kpkv = kvin/kin;
443
444 /* do we need to (re)allocate the value changed array? */
445 if (kpkv != px->value_changed.n) {
446 #ifdef SEVERBOSE
447 printf ("refactor valuechanged array. n %d sizeof p %d\n",
448 kpkv,sizeof (struct SFColor) * kpkv);
449 #endif
450 if (px->value_changed.n != 0) {
451 FREE_IF_NZ (px->value_changed.p);
452 }
453 px->value_changed.n = kpkv;
454 px->value_changed.p = MALLOC (struct SFVec3f*, sizeof (struct SFVec3f) * kpkv);
455 }
456
457 /* shortcut valchanged; have to put it here because might be reMALLOC'd */
458 valchanged = px->value_changed.p;
459
460
461 /* make sure we have the keys and keyValues */
462 if ((kvin == 0) || (kin == 0)) {
463 #ifdef SEVERBOSE
464 printf ("no keys or keyValues yet\n");
465 #endif
466
467 for (indx = 0; indx < kpkv; indx++) {
468 valchanged[indx].c[0] = (float) 0.0;
469 valchanged[indx].c[1] = (float) 0.0;
470 valchanged[indx].c[2] = (float) 0.0;
471 }
472 return;
473 }
474 if (kin>kvin) kin=kvin; /* means we don't use whole of keyValue, but... */
475
476
477 #ifdef SEVERBOSE
478 printf ("debugging, kpkv %d, px->value_changed.n %d\n", kpkv, px->value_changed.n);
479 printf ("NormalInterpolator, kpkv %d\n",kpkv);
480 #endif
481
482
483 /* set_fraction less than or greater than keys */
484 if (px->set_fraction <= px->key.p[0]) {
485 #ifdef SEVERBOSE
486 printf ("COINT out1\n");
487 #endif
488
489 for (indx = 0; indx < kpkv; indx++) {
490 memcpy ((void *)&valchanged[indx],
491 (void *)&kVs[indx], sizeof (struct SFColor));
492 }
493 #ifdef SEVERBOSE
494 printf ("COINT out1 copied\n");
495 #endif
496 } else if (px->set_fraction >= px->key.p[kin-1]) {
497 #ifdef SEVERBOSE
498 printf ("COINT out2\n");
499 #endif
500
501 for (indx = 0; indx < kpkv; indx++) {
502 memcpy ((void *)&valchanged[indx],
503 (void *)&kVs[kvin-kpkv+indx],
504 sizeof (struct SFColor));
505 }
506 #ifdef SEVERBOSE
507 printf ("COINT out2 finished\n");
508 #endif
509 } else {
510 #ifdef SEVERBOSE
511 printf ("COINT out3\n");
512 #endif
513
514 /* have to go through and find the key before */
515 #ifdef SEVERBOSE
516 printf ("indx=0, kin %d frac %f\n",kin,px->set_fraction);
517 #endif
518
519 myKey=find_key(kin,(float)(px->set_fraction),px->key.p);
520 #ifdef SEVERBOSE
521 printf ("working on key %d\n",myKey);
522 #endif
523
524 /* find the fraction between the 2 values */
525 interval = (px->set_fraction - px->key.p[myKey-1]) /
526 (px->key.p[myKey] - px->key.p[myKey-1]);
527
528 for (indx = 0; indx < kpkv; indx++) {
529 thisone = myKey * kpkv + indx;
530 prevone = (myKey-1) * kpkv + indx;
531
532 #ifdef SEVERBOSE
533 if (thisone >= kvin) {
534 printf ("CoordinateInterpolator error: thisone %d prevone %d indx %d kpkv %d kin %d kvin %d\n",thisone,prevone,
535 indx,kpkv,kin,kvin);
536 }
537 #endif
538
539 for (tmp=0; tmp<3; tmp++) {
540 valchanged[indx].c[tmp] = kVs[prevone].c[tmp] +
541 interval * (kVs[thisone].c[tmp] -
542 kVs[prevone].c[tmp]);
543 }
544 #ifdef SEVERBOSE
545 printf (" 1 %d interval %f prev %f this %f final %f\n",1,interval,kVs[prevone].c[1],kVs[thisone].c[1],valchanged[indx].c[1]);
546 #endif
547 }
548 #ifdef SEVERBOSE
549 printf ("COINT out3 finished\n");
550 #endif
551
552 }
553
554 /* if this is a NormalInterpolator... */
555 for (indx = 0; indx < kpkv; indx++) {
556 normalval.x = valchanged[indx].c[0];
557 normalval.y = valchanged[indx].c[1];
558 normalval.z = valchanged[indx].c[2];
559 normalize_vector(&normalval);
560 valchanged[indx].c[0] = (float) normalval.x;
561 valchanged[indx].c[1] = (float) normalval.y;
562 valchanged[indx].c[2] = (float) normalval.z;
563 }
564 #ifdef SEVERBOSE
565 printf ("Done CoordinateInterpolator\n");
566 #endif
567}
568
569
570void do_OintCoord(void *node) {
572 int kin, kvin/* , counter */;
573 struct SFVec3f *kVs;
574 struct SFVec3f *valchanged;
575
576 int thisone, prevone; /* which keyValues we are interpolating between */
577 int tmp;
578 float interval; /* where we are between 2 values */
579 int kpkv; /* keys per key value */
580 int indx;
581 int myKey;
582
583 if (!node) return;
584 px = (struct X3D_CoordinateInterpolator *) node;
585
586#ifdef SEVERBOSE
587 printf ("do_OintCoord, frac %f toGPU %d toCPU %d\n",px->set_fraction,px->_GPU_Routes_out, px->_CPU_Routes_out);
588 printf ("debugging OintCoord keys %d kv %d vc %d\n",px->keyValue.n, px->key.n,px->value_changed.n);
589 #endif
590
591 MARK_EVENT (node, offsetof (struct X3D_CoordinateInterpolator, value_changed));
592
593 // create the VBOs if required, for running on the GPU
594 if (px->_GPU_Routes_out > 0) {
595 if (px->_keyVBO==0) {
596 glGenBuffers(1,(GLuint *)&px->_keyValueVBO);
597 glGenBuffers(1,(GLuint *)&px->_keyVBO);
598 FW_GL_BINDBUFFER(GL_ARRAY_BUFFER,px->_keyValueVBO);
599 printf ("genning buffer data for %d keyValues, total floats %d\n",px->keyValue.n, px->keyValue.n*3);
600 glBufferData(GL_ARRAY_BUFFER,px->keyValue.n *sizeof(float)*3,px->keyValue.p, GL_STATIC_DRAW);
601
602 FW_GL_BINDBUFFER(GL_ARRAY_BUFFER,px->_keyVBO);
603 glBufferData(GL_ARRAY_BUFFER,px->key.n *sizeof(float),px->key.p, GL_STATIC_DRAW);
604 printf ("created VBOs for the CoordinateInterpolator, they are %d and %d\n",
605 px->_keyValueVBO, px->_keyVBO);
606 }
607 }
608
609
610
611 if (px->_CPU_Routes_out == 0) {
612 #ifdef SEVERBOSE
613 printf ("do_OintCoord, no CPU routes out, no need to do this work\n");
614 #endif
615 return;
616 }
617
618 //MARK_EVENT (node, offsetof (struct X3D_CoordinateInterpolator, value_changed));
619
620
621 kin = px->key.n;
622 kvin = px->keyValue.n;
623 kVs = px->keyValue.p;
624 kpkv = kvin/kin;
625
626 /* do we need to (re)allocate the value changed array? */
627 if (kpkv != px->value_changed.n) {
628 #ifdef SEVERBOSE
629 printf ("refactor valuechanged array. n %d sizeof p %d\n",
630 kpkv,sizeof (struct SFVec3f) * kpkv);
631 #endif
632 if (px->value_changed.n != 0) {
633 FREE_IF_NZ (px->value_changed.p);
634 }
635 px->value_changed.n = kpkv;
636 px->value_changed.p = MALLOC (struct SFVec3f*, sizeof (struct SFVec3f) * kpkv);
637 }
638
639 /* shortcut valchanged; have to put it here because might be reMALLOC'd */
640 valchanged = px->value_changed.p;
641
642
643 /* make sure we have the keys and keyValues */
644 if ((kvin == 0) || (kin == 0)) {
645 #ifdef SEVERBOSE
646 printf ("no keys or keyValues yet\n");
647 #endif
648
649 for (indx = 0; indx < kpkv; indx++) {
650 valchanged[indx].c[0] = (float) 0.0;
651 valchanged[indx].c[1] = (float) 0.0;
652 valchanged[indx].c[2] = (float) 0.0;
653 }
654 return;
655 }
656 if (kin>kvin) kin=kvin; /* means we don't use whole of keyValue, but... */
657
658
659 #ifdef SEVERBOSE
660 printf ("debugging, kpkv %d, px->value_changed.n %d\n", kpkv, px->value_changed.n);
661 printf ("CoordinateInterpolator, kpkv %d\n",kpkv);
662 #endif
663
664
665 /* set_fraction less than or greater than keys */
666 if (px->set_fraction <= px->key.p[0]) {
667 #ifdef SEVERBOSE
668 printf ("COINT out1\n");
669 #endif
670
671 for (indx = 0; indx < kpkv; indx++) {
672 memcpy ((void *)&valchanged[indx],
673 (void *)&kVs[indx], sizeof (struct SFVec3f));
674 /* JAS valchanged[indx].c[0] = kVs[indx].c[0]; */
675 /* JAS valchanged[indx].c[1] = kVs[indx].c[1]; */
676 /* JAS valchanged[indx].c[2] = kVs[indx].c[2]; */
677 }
678 #ifdef SEVERBOSE
679 printf ("COINT out1 copied\n");
680 #endif
681 } else if (px->set_fraction >= px->key.p[kin-1]) {
682 #ifdef SEVERBOSE
683 printf ("COINT out2\n");
684 #endif
685
686 for (indx = 0; indx < kpkv; indx++) {
687 memcpy ((void *)&valchanged[indx],
688 (void *)&kVs[kvin-kpkv+indx],
689 sizeof (struct SFVec3f));
690 }
691 #ifdef SEVERBOSE
692 printf ("COINT out2 finished\n");
693 #endif
694 } else {
695 #ifdef SEVERBOSE
696 printf ("COINT out3\n");
697 #endif
698
699 /* have to go through and find the key before */
700 #ifdef SEVERBOSE
701 printf ("indx=0, kin %d frac %f\n",kin,px->set_fraction);
702 #endif
703
704 myKey=find_key(kin,(float)(px->set_fraction),px->key.p);
705 #ifdef SEVERBOSE
706 printf ("working on key %d\n",myKey);
707 #endif
708
709 /* find the fraction between the 2 values */
710 interval = (px->set_fraction - px->key.p[myKey-1]) /
711 (px->key.p[myKey] - px->key.p[myKey-1]);
712
713 for (indx = 0; indx < kpkv; indx++) {
714 thisone = myKey * kpkv + indx;
715 prevone = (myKey-1) * kpkv + indx;
716
717 #ifdef SEVERBOSE
718 if (thisone >= kvin) {
719 printf ("CoordinateInterpolator error: thisone %d prevone %d indx %d kpkv %d kin %d kvin %d\n",thisone,prevone,
720 indx,kpkv,kin,kvin);
721 }
722 #endif
723
724 for (tmp=0; tmp<3; tmp++) {
725 valchanged[indx].c[tmp] = kVs[prevone].c[tmp] +
726 interval * (kVs[thisone].c[tmp] -
727 kVs[prevone].c[tmp]);
728 }
729 #ifdef SEVERBOSE
730 printf (" 1 %d interval %f prev %f this %f final %f\n",1,interval,kVs[prevone].c[1],kVs[thisone].c[1],valchanged[indx].c[1]);
731 #endif
732 }
733 #ifdef SEVERBOSE
734 printf ("COINT out3 finished\n");
735 #endif
736
737 }
738
739 #ifdef SEVERBOSE
740 printf ("Done CoordinateInterpolator\n");
741 #endif
742}
743
744void do_OintCoord2D(void *node) {
746 int kin, kvin/* , counter */;
747 struct SFVec2f *kVs;
748 struct SFVec2f *valchanged;
749
750 int thisone, prevone; /* which keyValues we are interpolating between */
751 int tmp;
752 float interval; /* where we are between 2 values */
753 int kpkv; /* keys per key value */
754 int indx;
755 int myKey;
756
757 if (!node) return;
758 px = (struct X3D_CoordinateInterpolator2D *) node;
759
760
761 #ifdef SEVERBOSE
762 printf ("debugging OintCoord keys %d kv %d vc %d\n",px->keyValue.n, px->key.n,px->value_changed.n);
763 #endif
764
765 MARK_EVENT (node, offsetof (struct X3D_CoordinateInterpolator2D, value_changed));
766
767 kin = px->key.n;
768 kvin = px->keyValue.n;
769 kVs = px->keyValue.p;
770 kpkv = kvin/kin;
771
772 /* do we need to (re)allocate the value changed array? */
773 if (kpkv != px->value_changed.n) {
774 #ifdef SEVERBOSE
775 printf ("refactor valuechanged array. n %d sizeof p %d\n",
776 kpkv,sizeof (struct SFVec2f) * kpkv);
777 #endif
778 if (px->value_changed.n != 0) {
779 FREE_IF_NZ (px->value_changed.p);
780 }
781 px->value_changed.n = kpkv;
782 px->value_changed.p = MALLOC (struct SFVec2f*, sizeof (struct SFVec2f) * kpkv);
783 }
784
785 /* shortcut valchanged; have to put it here because might be reMALLOC'd */
786 valchanged = px->value_changed.p;
787
788
789 /* make sure we have the keys and keyValues */
790 if ((kvin == 0) || (kin == 0)) {
791 #ifdef SEVERBOSE
792 printf ("no keys or keyValues yet\n");
793 #endif
794
795 for (indx = 0; indx < kpkv; indx++) {
796 valchanged[indx].c[0] = (float) 0.0;
797 valchanged[indx].c[1] = (float) 0.0;
798 }
799 return;
800 }
801 if (kin>kvin) kin=kvin; /* means we don't use whole of keyValue, but... */
802
803
804 #ifdef SEVERBOSE
805 printf ("debugging, kpkv %d, px->value_changed.n %d\n", kpkv, px->value_changed.n);
806 printf ("CoordinateInterpolator2D, kpkv %d\n",kpkv);
807 #endif
808
809
810 /* set_fraction less than or greater than keys */
811 if (px->set_fraction <= px->key.p[0]) {
812 #ifdef SEVERBOSE
813 printf ("COINT out1\n");
814 #endif
815
816 for (indx = 0; indx < kpkv; indx++) {
817 memcpy ((void *)&valchanged[indx],
818 (void *)&kVs[indx], sizeof (struct SFVec2f));
819 /* JAS valchanged[indx].c[0] = kVs[indx].c[0]; */
820 /* JAS valchanged[indx].c[1] = kVs[indx].c[1]; */
821 }
822 #ifdef SEVERBOSE
823 printf ("COINT out1 copied\n");
824 #endif
825 } else if (px->set_fraction >= px->key.p[kin-1]) {
826 #ifdef SEVERBOSE
827 printf ("COINT out2\n");
828 #endif
829
830 for (indx = 0; indx < kpkv; indx++) {
831 memcpy ((void *)&valchanged[indx],
832 (void *)&kVs[kvin-kpkv+indx],
833 sizeof (struct SFVec2f));
834 }
835 #ifdef SEVERBOSE
836 printf ("COINT out2 finished\n");
837 #endif
838 } else {
839 #ifdef SEVERBOSE
840 printf ("COINT out3\n");
841 #endif
842
843 /* have to go through and find the key before */
844 #ifdef SEVERBOSE
845 printf ("indx=0, kin %d frac %f\n",kin,px->set_fraction);
846 #endif
847
848 myKey=find_key(kin,(float)(px->set_fraction),px->key.p);
849 #ifdef SEVERBOSE
850 printf ("working on key %d\n",myKey);
851 #endif
852
853 /* find the fraction between the 2 values */
854 interval = (px->set_fraction - px->key.p[myKey-1]) /
855 (px->key.p[myKey] - px->key.p[myKey-1]);
856
857 for (indx = 0; indx < kpkv; indx++) {
858 thisone = myKey * kpkv + indx;
859 prevone = (myKey-1) * kpkv + indx;
860
861 #ifdef SEVERBOSE
862 if (thisone >= kvin) {
863 printf ("CoordinateInterpolator2D error: thisone %d prevone %d indx %d kpkv %d kin %d kvin %d\n",thisone,prevone,
864 indx,kpkv,kin,kvin);
865 }
866 #endif
867
868 for (tmp=0; tmp<2; tmp++) {
869 valchanged[indx].c[tmp] = kVs[prevone].c[tmp] +
870 interval * (kVs[thisone].c[tmp] -
871 kVs[prevone].c[tmp]);
872 }
873 }
874 #ifdef SEVERBOSE
875 printf ("COINT out3 finished\n");
876 #endif
877
878 }
879
880 #ifdef SEVERBOSE
881 printf ("Done CoordinateInterpolator2D\n");
882 #endif
883}
884
885void do_OintPos2D(void *node) {
886/* PositionInterpolator2D */
887/* Called during the "events_processed" section of the event loop, */
888/* so this is called ONLY when there is something required to do, thus */
889/* there is no need to look at whether it is active or not */
890
892 int kin, kvin, counter, tmp;
893 struct SFVec2f *kVs;
894
895 if (!node) return;
896 px = (struct X3D_PositionInterpolator2D *) node;
897
898 MARK_EVENT (node, offsetof (struct X3D_PositionInterpolator2D, value_changed));
899
900 kin = px->key.n;
901 kvin = px->keyValue.n;
902 kVs = px->keyValue.p;
903
904 #ifdef SEVERBOSE
905 printf("do_Oint2: Position interp2D, node %u kin %d kvin %d set_fraction %f\n",
906 node, kin, kvin, px->set_fraction);
907 #endif
908
909 /* make sure we have the keys and keyValues */
910 if ((kvin == 0) || (kin == 0)) {
911 px->value_changed.c[0] = (float) 0.0;
912 px->value_changed.c[1] = (float) 0.0;
913 return;
914 }
915 if (kin>kvin) kin=kvin; /* means we don't use whole of keyValue, but... */
916
917
918 /* set_fraction less than or greater than keys */
919 if (px->set_fraction <= ((px->key).p[0])) {
920 memcpy ((void *)&px->value_changed,
921 (void *)&kVs[0], sizeof (struct SFVec2f));
922 } else if (px->set_fraction >= px->key.p[kin-1]) {
923 memcpy ((void *)&px->value_changed,
924 (void *)&kVs[kvin-1], sizeof (struct SFVec2f));
925 } else {
926 /* have to go through and find the key before */
927 counter = find_key(kin,((float)(px->set_fraction)),px->key.p);
928 for (tmp=0; tmp<2; tmp++) {
929 px->value_changed.c[tmp] =
930 (px->set_fraction - px->key.p[counter-1]) /
931 (px->key.p[counter] - px->key.p[counter-1]) *
932 (kVs[counter].c[tmp] -
933 kVs[counter-1].c[tmp]) +
934 kVs[counter-1].c[tmp];
935 }
936 }
937 #ifdef SEVERBOSE
938 printf ("Pos/Col, new value (%f %f)\n",
939 px->value_changed.c[0],px->value_changed.c[1]);
940 #endif
941}
942
943/* PositionInterpolator, ColorInterpolator, GeoPositionInterpolator */
944/* Called during the "events_processed" section of the event loop, */
945/* so this is called ONLY when there is something required to do, thus */
946/* there is no need to look at whether it is active or not */
947
948/* GeoPositionInterpolator in the Component_Geospatial file */
949
950/* ColorInterpolator == PositionIterpolator */
951void do_ColorInterpolator (void *node) {
952 struct X3D_ColorInterpolator *px;
953 int kin, kvin, counter, tmp;
954 struct SFColor *kVs;
955
956 if (!node) return;
957 px = (struct X3D_ColorInterpolator *) node;
958
959 kvin = px->keyValue.n;
960 kVs = px->keyValue.p;
961 kin = px->key.n;
962
963 MARK_EVENT (node, offsetof (struct X3D_ColorInterpolator, value_changed));
964
965 #ifdef SEVERBOSE
966 printf("do_ColorInt: Position/Color interp, node %u kin %d kvin %d set_fraction %f\n",
967 node, kin, kvin, px->set_fraction);
968 #endif
969
970 /* make sure we have the keys and keyValues */
971 if ((kvin == 0) || (kin == 0)) {
972 px->value_changed.c[0] = (float) 0.0;
973 px->value_changed.c[1] = (float) 0.0;
974 px->value_changed.c[2] = (float) 0.0;
975 return;
976 }
977
978 if (kin>kvin) kin=kvin; /* means we don't use whole of keyValue, but... */
979
980 /* set_fraction less than or greater than keys */
981 if (px->set_fraction <= ((px->key).p[0])) {
982 memcpy ((void *)&px->value_changed, (void *)&kVs[0], sizeof (struct SFColor));
983 } else if (px->set_fraction >= px->key.p[kin-1]) {
984 memcpy ((void *)&px->value_changed, (void *)&kVs[kvin-1], sizeof (struct SFColor));
985 } else {
986 /* have to go through and find the key before */
987 counter = find_key(kin,((float)(px->set_fraction)),px->key.p);
988 for (tmp=0; tmp<3; tmp++) {
989 px->value_changed.c[tmp] =
990 (px->set_fraction - px->key.p[counter-1]) /
991 (px->key.p[counter] - px->key.p[counter-1]) *
992 (kVs[counter].c[tmp] - kVs[counter-1].c[tmp]) + kVs[counter-1].c[tmp];
993 }
994 }
995 #ifdef SEVERBOSE
996 printf ("Pos/Col, new value (%f %f %f)\n",
997 px->value_changed.c[0],px->value_changed.c[1],px->value_changed.c[2]);
998 #endif
999}
1000
1001
1002void do_PositionInterpolator (void *node) {
1003 struct X3D_PositionInterpolator *px;
1004 int kin, kvin, counter, tmp;
1005 struct SFVec3f *kVs;
1006
1007 if (!node) return;
1008 px = (struct X3D_PositionInterpolator *) node;
1009
1010 kvin = px->keyValue.n;
1011 kVs = px->keyValue.p;
1012 kin = px->key.n;
1013
1014 MARK_EVENT (node, offsetof (struct X3D_PositionInterpolator, value_changed));
1015
1016 #ifdef SEVERBOSE
1017 printf("do_PositionInt: Position/Vec3f interp, node %u kin %d kvin %d set_fraction %f\n",
1018 node, kin, kvin, px->set_fraction);
1019 #endif
1020
1021 /* make sure we have the keys and keyValues */
1022 if ((kvin == 0) || (kin == 0)) {
1023 px->value_changed.c[0] = (float) 0.0;
1024 px->value_changed.c[1] = (float) 0.0;
1025 px->value_changed.c[2] = (float) 0.0;
1026 return;
1027 }
1028
1029 if (kin>kvin) kin=kvin; /* means we don't use whole of keyValue, but... */
1030
1031 /* set_fraction less than or greater than keys */
1032 if (px->set_fraction <= ((px->key).p[0])) {
1033 memcpy ((void *)&px->value_changed, (void *)&kVs[0], sizeof (struct SFVec3f));
1034 } else if (px->set_fraction >= px->key.p[kin-1]) {
1035 memcpy ((void *)&px->value_changed, (void *)&kVs[kvin-1], sizeof (struct SFVec3f));
1036 } else {
1037 /* have to go through and find the key before */
1038 counter = find_key(kin,((float)(px->set_fraction)),px->key.p);
1039 for (tmp=0; tmp<3; tmp++) {
1040 px->value_changed.c[tmp] =
1041 (px->set_fraction - px->key.p[counter-1]) /
1042 (px->key.p[counter] - px->key.p[counter-1]) *
1043 (kVs[counter].c[tmp] - kVs[counter-1].c[tmp]) + kVs[counter-1].c[tmp];
1044 }
1045 }
1046 #ifdef SEVERBOSE
1047 printf ("Pos/Col, new value (%f %f %f)\n",
1048 px->value_changed.c[0],px->value_changed.c[1],px->value_changed.c[2]);
1049 #endif
1050}
1051
1052/* OrientationInterpolator */
1053/* Called during the "events_processed" section of the event loop, */
1054/* so this is called ONLY when there is something required to do, thus */
1055/* there is no need to look at whether it is active or not */
1056
1057void do_Oint4 (void *node) {
1058 struct X3D_OrientationInterpolator *px;
1059 int kin, kvin;
1060 struct SFRotation *kVs;
1061 int counter;
1062 float interval; /* where we are between 2 values */
1063 // UNUSED?? int stzero;
1064 // UNUSED?? int endzero; /* starting and/or ending angles zero? */
1065
1066 Quaternion st, fin, final;
1067 double x,y,z,a;
1068
1069 if (!node) return;
1070 px = (struct X3D_OrientationInterpolator *) node;
1071 kin = ((px->key).n);
1072 kvin = ((px->keyValue).n);
1073 kVs = ((px->keyValue).p);
1074
1075 #ifdef SEVERBOSE
1076 printf ("starting do_Oint4; keyValue count %d and key count %d\n",
1077 kvin, kin);
1078 #endif
1079
1080
1081 MARK_EVENT (node, offsetof (struct X3D_OrientationInterpolator, value_changed));
1082
1083 /* make sure we have the keys and keyValues */
1084 if ((kvin == 0) || (kin == 0)) {
1085 px->value_changed.c[0] = (float) 0.0;
1086 px->value_changed.c[1] = (float) 0.0;
1087 px->value_changed.c[2] = (float) 0.0;
1088 px->value_changed.c[3] = (float) 0.0;
1089 return;
1090 }
1091 if (kin>kvin) kin=kvin; /* means we don't use whole of keyValue, but... */
1092
1093
1094 /* set_fraction less than or greater than keys */
1095 if (px->set_fraction <= ((px->key).p[0])) {
1096 memcpy ((void *)&px->value_changed,
1097 (void *)&kVs[0], sizeof (struct SFRotation));
1098 } else if (px->set_fraction >= ((px->key).p[kin-1])) {
1099 memcpy ((void *)&px->value_changed,
1100 (void *)&kVs[kvin-1], sizeof (struct SFRotation));
1101 } else {
1102 counter = find_key(kin,(float)(px->set_fraction),px->key.p);
1103 interval = (px->set_fraction - px->key.p[counter-1]) /
1104 (px->key.p[counter] - px->key.p[counter-1]);
1105
1106
1107 /* are either the starting or ending angles zero? */
1108 // unused? stzero = APPROX(kVs[counter-1].c[3],0.0);
1109 // unused? endzero = APPROX(kVs[counter].c[3],0.0);
1110 #ifdef SEVERBOSE
1111 printf ("counter %d interval %f\n",counter,interval);
1112 printf ("angles %f %f %f %f, %f %f %f %f\n",
1113 kVs[counter-1].c[0],
1114 kVs[counter-1].c[1],
1115 kVs[counter-1].c[2],
1116 kVs[counter-1].c[3],
1117 kVs[counter].c[0],
1118 kVs[counter].c[1],
1119 kVs[counter].c[2],
1120 kVs[counter].c[3]);
1121 #endif
1122 vrmlrot_to_quaternion (&st, kVs[counter-1].c[0],
1123 kVs[counter-1].c[1], kVs[counter-1].c[2], kVs[counter-1].c[3]);
1124 vrmlrot_to_quaternion (&fin,kVs[counter].c[0],
1125 kVs[counter].c[1], kVs[counter].c[2], kVs[counter].c[3]);
1126
1127 quaternion_slerp(&final, &st, &fin, (double)interval);
1128 quaternion_to_vrmlrot(&final,&x, &y, &z, &a);
1129 px->value_changed.c[0] = (float) x;
1130 px->value_changed.c[1] = (float) y;
1131 px->value_changed.c[2] = (float) z;
1132 px->value_changed.c[3] = (float) a;
1133
1134 #ifdef SEVERBOSE
1135 printf ("Oint, new angle %f %f %f %f\n",px->value_changed.c[0],
1136 px->value_changed.c[1],px->value_changed.c[2], px->value_changed.c[3]);
1137 #endif
1138 }
1139}
1140
1141/* fired at start of event loop for every Collision */
1142/* void do_CollisionTick(struct X3D_Collision *cx) {*/
1143void do_CollisionTick( void *ptr) {
1144 struct X3D_Collision *cx = (struct X3D_Collision *)ptr;
1145 if (cx->__hit == 3) {
1146 /* printf ("COLLISION at %f\n",TickTime()); */
1147 cx->collideTime = TickTime();
1148 MARK_EVENT (ptr, offsetof(struct X3D_Collision, collideTime));
1149 }
1150}
1151
1152
1153/* Audio AudioClip sensor code */
1154/* void do_AudioTick(struct X3D_AudioClip *node) {*/
1155void do_AudioTick(void *ptr) {
1156 struct X3D_AudioClip *node = (struct X3D_AudioClip *)ptr;
1157 int oldstatus;
1158 double pitch, duration; /* gcc and params - make all doubles to do_active_inactive */
1159
1160 /* can we possibly have started yet? */
1161 if (!node) return;
1162
1163 if (node->__oldEnabled != node->enabled) {
1164 node->__oldEnabled = node->enabled;
1165 MARK_EVENT(X3D_NODE(node), offsetof(struct X3D_AudioClip, enabled));
1166 }
1167 if (!node->enabled) return;
1168
1169 if(node->__inittime == 0.0)
1170 node->__inittime = TickTime();
1171
1172 if(TickTime() < node->startTime) {
1173 return;
1174 }
1175
1176 oldstatus = node->isActive;
1177 pitch = node->pitch;
1178
1179 if(node->__sourceNumber < 0) return;
1181 //if (node->__sourceNumber == -1) {
1182 // locateAudioSource (node);
1183 // /* printf ("do_AudioTick, node %d sn %d\n", node, node->__sourceNumber); */
1184 //}
1185
1187 // * between 0 and infinity; if it is BADAUDIOSOURCE, bad source.
1188 // * check out locateAudioSource to find out reasons */
1189 //if (node->__sourceNumber == BADAUDIOSOURCE) return;
1190
1191 /* call common time sensor routine */
1192 //duration = return_Duration(node->__sourceNumber);
1193 duration = return_Duration(node);
1194 do_active_inactive (
1195 &node->isActive, &node->__inittime, &node->startTime,
1196 &node->stopTime,node->loop,duration,
1197 pitch,node->elapsedTime);
1198
1199 if (oldstatus != node->isActive) {
1200 /* push @e, [$t, "isActive", node->{isActive}]; */
1201 if (node->isActive == 1) {
1202 /* force code below to generate event */
1203 //node->__ctflag = 10.0;
1204 node->__lasttime = TickTime();
1205 node->elapsedTime = 0.0;
1206 }
1207 MARK_EVENT (X3D_NODE(node), offsetof(struct X3D_AudioClip, isActive));
1208 }
1209
1210 if(node->isActive){
1211 if(node->pauseTime > node->startTime){
1212 if( node->resumeTime < node->pauseTime && !node->isPaused){
1213 node->isPaused = TRUE;
1214 MARK_EVENT (X3D_NODE(node), offsetof(struct X3D_AudioClip, isPaused));
1215 }else if(node->resumeTime > node->pauseTime && node->isPaused){
1216 node->isPaused = FALSE;
1217 node->__lasttime = TickTime();
1218 MARK_EVENT (X3D_NODE(node), offsetof(struct X3D_AudioClip, isPaused));
1219 }
1220 }
1221 }
1222 if(node->isActive == 1 && node->isPaused == FALSE) {
1223 double dtime = TickTime();
1224 node->elapsedTime += dtime - node->__lasttime;
1225 node->__lasttime = dtime;
1226 //double myFrac = node->elapsedTime / duration;
1227 MARK_EVENT (ptr, offsetof(struct X3D_AudioClip, elapsedTime));
1228 }
1229}
1230void do_BufferAudioSourceTick(void* ptr) {
1231 struct X3D_BufferAudioSource* node = (struct X3D_BufferAudioSource*)ptr;
1232 int oldstatus;
1233 double duration; /* gcc and params - make all doubles to do_active_inactive */
1234 /* can we possibly have started yet? */
1235 if (!node) return;
1236
1237 if (node->__oldEnabled != node->enabled) {
1238 node->__oldEnabled = node->enabled;
1239 MARK_EVENT(X3D_NODE(node), offsetof(struct X3D_BufferAudioSource, enabled));
1240 }
1241 if (!node->enabled) return;
1242
1243 if (node->__inittime == 0.0)
1244 node->__inittime = TickTime();
1245
1246 if (TickTime() < node->startTime) {
1247 return;
1248 }
1249
1250 oldstatus = node->isActive;
1251
1252 if (node->__sourceNumber < 0) return;
1254 //if (node->__sourceNumber == -1) {
1255 // locateAudioSource (node);
1256 // /* printf ("do_AudioTick, node %d sn %d\n", node, node->__sourceNumber); */
1257 //}
1258
1260 // * between 0 and infinity; if it is BADAUDIOSOURCE, bad source.
1261 // * check out locateAudioSource to find out reasons */
1262 //if (node->__sourceNumber == BADAUDIOSOURCE) return;
1263
1264 /* call common time sensor routine */
1265 do_active_inactive(
1266 &node->isActive, &node->__inittime, &node->startTime,
1267 &node->stopTime, node->loop, 0.0,
1268 0.0, node->elapsedTime);
1269
1270 if (oldstatus != node->isActive) {
1271 /* push @e, [$t, "isActive", node->{isActive}]; */
1272 if (node->isActive == 1) {
1273 /* force code below to generate event */
1274 //node->__ctflag = 10.0;
1275 node->__lasttime = TickTime();
1276 node->elapsedTime = 0.0;
1277 }
1278 MARK_EVENT(X3D_NODE(node), offsetof(struct X3D_BufferAudioSource, isActive));
1279 }
1280
1281 if (node->isActive) {
1282 if (node->pauseTime > node->startTime) {
1283 if (node->resumeTime < node->pauseTime && !node->isPaused) {
1284 node->isPaused = TRUE;
1285 MARK_EVENT(X3D_NODE(node), offsetof(struct X3D_BufferAudioSource, isPaused));
1286 }
1287 else if (node->resumeTime > node->pauseTime && node->isPaused) {
1288 node->isPaused = FALSE;
1289 node->__lasttime = TickTime();
1290 MARK_EVENT(X3D_NODE(node), offsetof(struct X3D_BufferAudioSource, isPaused));
1291 }
1292 }
1293 }
1294 if (node->isActive == 1 && node->isPaused == FALSE) {
1295 double dtime = TickTime();
1296 node->elapsedTime += dtime - node->__lasttime;
1297 node->__lasttime = dtime;
1298 //double myFrac = node->elapsedTime / duration;
1299 MARK_EVENT(ptr, offsetof(struct X3D_BufferAudioSource, elapsedTime));
1300 }
1301}
1302
1303void do_OscillatorSourceTick(void* ptr) {
1304 struct X3D_OscillatorSource* node = (struct X3D_OscillatorSource*)ptr;
1305 int oldstatus, ichange;
1306 double duration; /* gcc and params - make all doubles to do_active_inactive */
1307 ichange = 0; //set this if any MARK_EVENTS in isActive, isPaused, as the render_OscillatorSource does math on these
1308
1309 /* can we possibly have started yet? */
1310 if (!node) return;
1311
1312 if (node->__oldEnabled != node->enabled) {
1313 node->__oldEnabled = node->enabled;
1314 MARK_EVENT(X3D_NODE(node), offsetof(struct X3D_OscillatorSource, enabled));
1315 ichange++;
1316 }
1317 if (!node->enabled) return;
1318
1319 if (node->__inittime == 0.0)
1320 node->__inittime = TickTime();
1321
1322 if (TickTime() < node->startTime) {
1323 return;
1324 }
1325
1326 oldstatus = node->isActive;
1327
1328
1329 /* call common time sensor routine */
1330 //duration = return_Duration(node->__sourceNumber);
1331 duration = 0.0;
1332 do_active_inactive(
1333 &node->isActive, &node->__inittime, &node->startTime,
1334 &node->stopTime, TRUE, duration,
1335 node->frequency, node->elapsedTime);
1336
1337 if (oldstatus != node->isActive) {
1338 /* push @e, [$t, "isActive", node->{isActive}]; */
1339 if (node->isActive == 1) {
1340 /* force code below to generate event */
1341 //node->__ctflag = 10.0;
1342 node->__lasttime = TickTime();
1343 node->elapsedTime = 0.0;
1344 }
1345 MARK_EVENT(X3D_NODE(node), offsetof(struct X3D_OscillatorSource, isActive));
1346 ichange++;
1347 }
1348
1349 if (node->isActive) {
1350 if (node->pauseTime > node->startTime) {
1351 if (node->resumeTime < node->pauseTime && !node->isPaused) {
1352 node->isPaused = TRUE;
1353 MARK_EVENT(X3D_NODE(node), offsetof(struct X3D_OscillatorSource, isPaused));
1354 ichange++;
1355 }
1356 else if (node->resumeTime > node->pauseTime && node->isPaused) {
1357 node->isPaused = FALSE;
1358 node->__lasttime = TickTime();
1359 MARK_EVENT(X3D_NODE(node), offsetof(struct X3D_OscillatorSource, isPaused));
1360 ichange++;
1361 }
1362 }
1363 }
1364 if (node->isActive == 1 && node->isPaused == FALSE) {
1365 double dtime = TickTime();
1366 node->elapsedTime += dtime - node->__lasttime;
1367 node->__lasttime = dtime;
1368 //double myFrac = node->elapsedTime / duration;
1369 MARK_EVENT(ptr, offsetof(struct X3D_OscillatorSource, elapsedTime));
1370 //ichange++;
1371 }
1372 if (ichange) node->_ichange++;
1373}
1374
1375
1376/* Similar to AudioClip, this is the Play, Pause, Stop, Resume code
1377*/
1378#define LOAD_STABLE 10 //from component_sound.c
1379unsigned char *movietexture_get_frame_by_fraction(struct X3D_Node* node, float fraction, int *width, int *height, int *nchan);
1380void do_MovieTextureTick( void *ptr) {
1381 struct X3D_MovieTexture *node = (struct X3D_MovieTexture *)ptr;
1382 //struct X3D_AudioClip *anode;
1383 int oldstatus;
1384 float frac; /* which texture to display */
1385 //int highest,lowest; /* selector variables */
1386 double myFrac;
1387 double speed;
1388 double duration;
1389 int tmpTrunc; /* used for timing for textures */
1390
1391 //anode = (struct X3D_AudioClip *)node;
1392 //do_AudioTick(ptr); //does play, pause, active, inactive part
1393
1394 /* can we possibly have started yet? */
1395 if (!node) return;
1396
1397 if (node->__oldEnabled != node->enabled) {
1398 node->__oldEnabled = node->enabled;
1399 MARK_EVENT(X3D_NODE(node), offsetof(struct X3D_MovieTexture, enabled));
1400 }
1401 if (!node->enabled) return;
1402
1403
1404 if(node->__init_time == 0.0)
1405 node->__init_time = TickTime();
1406
1407 if(TickTime() < node->startTime) {
1408 return;
1409 }
1410
1411// duration = (highest - lowest)/30.0;
1412 //highest = node->__highest;
1413 //lowest = node->__lowest;
1414 duration = node->duration_changed; //return_Duration(node);
1415 speed = node->speed;
1416
1417 oldstatus = node->isActive;
1418 do_active_inactive (
1419 &node->isActive, &node->__init_time, &node->startTime,
1420 &node->stopTime,node->loop,duration,
1421 speed,node->elapsedTime);
1422
1423 if (oldstatus != node->isActive) {
1424 if (node->isActive == 1) {
1425 /* force code below to generate event */
1426 //node->__ctflag = 10.0;
1427 node->__last_time = TickTime();
1428 node->elapsedTime = 0.0;
1429 }
1430 MARK_EVENT (X3D_NODE(node), offsetof(struct X3D_MovieTexture, isActive));
1431 }
1432
1433 if(node->isActive){
1434 if(node->pauseTime > node->startTime){
1435 if( node->resumeTime < node->pauseTime && !node->isPaused){
1436 node->isPaused = TRUE;
1437 MARK_EVENT (X3D_NODE(node), offsetof(struct X3D_MovieTexture, isPaused));
1438 }else if(node->resumeTime > node->pauseTime && node->isPaused){
1439 node->isPaused = FALSE;
1440 node->__last_time = TickTime();
1441 MARK_EVENT (X3D_NODE(node), offsetof(struct X3D_MovieTexture, isPaused));
1442 }
1443 }
1444 }
1445 if(node->isActive && node->isPaused == FALSE) {
1446 double dtime = TickTime();
1447 node->elapsedTime += dtime - node->__last_time;
1448 node->__last_time = dtime;
1449
1450 //frac = node->__ctex;
1451
1453 //if (node->__lowest >= node->__highest) {
1454 // node->__lowest = node->__highest-1;
1455 //}
1456 /* calculate what fraction we should be */
1457 // t = (now - startTime) modulo (duration/speed)
1458 myFrac = node->elapsedTime / duration;
1459 //myTime = (TickTime() - node->startTime) * speed/duration;
1460 tmpTrunc = (int) myFrac;
1461 frac = (float)myFrac - (float)tmpTrunc;
1462 /* negative speed? */
1463 if (speed < 0) {
1464 frac = 1.0f + frac; /* frac will be *negative* */
1465 /* else if (speed == 0) */
1466 } else if (APPROX(speed, 0.0f)) {
1467 frac = 0.0f;
1468 }
1469 node->__frac = frac;
1470 //clamp to last frame when not looping, so at end of show last frame sticks as per specs
1471 if(node->loop == FALSE && tmpTrunc > 0)
1472 node->__frac = 1.0f;
1473 //printf("tmptnk=%d frac=%f ",tmpTrunc,node->__frac);
1474 //node->elapsedTime = TickTime() - node->startTime;
1475 //printf("/ et %lf /",node->elapsedTime);
1476 MARK_EVENT (ptr, offsetof(struct X3D_MovieTexture, elapsedTime));
1477 }
1478 if(node->__loadstatus == LOAD_STABLE){
1479 //Nov 16, 2016 the following works with MPEG_Utils_ffmpeg.c on non-audio mpeg (vts.mpg)
1480 // x not tested with audio
1481 unsigned char* texdata;
1482 int width,height,nchan;
1483 textureTableIndexStruct_s *tti;
1484 texdata = movietexture_get_frame_by_fraction(X3D_NODE(node), node->__frac, &width, &height, &nchan);
1485 if(texdata){
1486 int thisTexture = node->__textureTableIndex;
1487 tti = getTableIndex(thisTexture);
1488 if(tti){
1489 static int once = 0;
1490 tti->x = width;
1491 tti->y = height;
1492 tti->z = 1;
1493 tti->channels = nchan;
1494 if(!once){
1495 //send it through textures.c once to get things like wrap set
1496 // textures.c likes to free texdata, so we'll deep copy
1497 tti->texdata = malloc(tti->x*tti->y*tti->channels);
1498 memcpy(tti->texdata,texdata,tti->x*tti->y*tti->channels);
1499 tti->status = TEX_NEEDSBINDING;
1500 once = 1;
1501 }else{
1502 tti->status = TEX_LOADED;
1503 glBindTexture(GL_TEXTURE_2D,tti->OpenGLTexture);
1504 //disable the mipmapping done on the once pass through textures.c above
1505 FW_GL_TEXPARAMETERI( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
1506 FW_GL_TEXPARAMETERI( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
1507 //replace the texture data every frame when we are isActive and not paused
1508 //we do this once per frame in startofloopnodeupdates call stack
1509 //(not per render call: we want the same texture to show in left/right or quad display viewports)
1510 if(nchan == 4)
1511 glTexImage2D(GL_TEXTURE_2D,0,GL_RGBA,width,height,0,GL_RGBA,GL_UNSIGNED_BYTE,texdata);
1512 if(nchan == 3)
1513 glTexImage2D(GL_TEXTURE_2D,0,GL_RGB,width,height,0,GL_RGB,GL_UNSIGNED_BYTE,texdata);
1514 glBindTexture(GL_TEXTURE_2D,0);
1515 }
1516 }
1517 }
1518
1519 }
1520}
1521
1522
1523/****************************************************************************
1524
1525 Sensitive nodes
1526
1527
1528*****************************************************************************/
1529
1530void do_TouchSensor ( void *ptr, int ev, int but1, int over) {
1531
1532 struct X3D_TouchSensor *node = (struct X3D_TouchSensor *)ptr;
1533 float normalval[3];
1534 ttglobal tg;
1535 #ifdef SENSVERBOSE
1536 printf ("%lf: TS ",TickTime());
1537 if (ev==ButtonPress) printf ("ButtonPress ");
1538 else if (ev==ButtonRelease) printf ("ButtonRelease ");
1539 else if (ev==KeyPress) printf ("KeyPress ");
1540 else if (ev==KeyRelease) printf ("KeyRelease ");
1541 else if (ev==MotionNotify) printf ("%lf MotionNotify ");
1542 else printf ("ev %d ",ev);
1543
1544 if (but1) printf ("but1 TRUE "); else printf ("but1 FALSE ");
1545 if (over) printf ("over TRUE "); else printf ("over FALSE ");
1546 printf ("\n");
1547 #endif
1548
1549
1550 /* if not enabled, do nothing */
1551 if (!node) return;
1552 if (node->__oldEnabled != node->enabled) {
1553 node->__oldEnabled = node->enabled;
1554 MARK_EVENT(X3D_NODE(node),offsetof (struct X3D_TouchSensor, enabled));
1555 }
1556 if (!node->enabled) return;
1557 tg = gglobal();
1558 /* isOver state */
1559 if ((ev == overMark) && (over != node->isOver)) {
1560 #ifdef SENSVERBOSE
1561 printf ("TS %u, isOver changed %d\n",node, over);
1562 #endif
1563 node->isOver = over;
1564 MARK_EVENT (ptr, offsetof (struct X3D_TouchSensor, isOver));
1565 }
1566
1567 /* active */
1568 /* button presses */
1569 if (ev == ButtonPress) {
1570 node->isActive=TRUE;
1571 MARK_EVENT (ptr, offsetof (struct X3D_TouchSensor, isActive));
1572 #ifdef SENSVERBOSE
1573 printf ("touchSens %u, butPress\n",node);
1574 #endif
1575
1576 node->touchTime = TickTime();
1577 MARK_EVENT(ptr, offsetof (struct X3D_TouchSensor, touchTime));
1578
1579 } else if (ev == ButtonRelease) {
1580 #ifdef SENSVERBOSE
1581 printf ("touchSens %u, butRelease\n",node);
1582 #endif
1583 node->isActive=FALSE;
1584 MARK_EVENT (ptr, offsetof (struct X3D_TouchSensor, isActive));
1585 }
1586
1587 /* hitPoint and hitNormal */
1588 /* save the current hitPoint for determining if this changes between runs */
1589 veccopy3f(node->_oldhitPoint.c,tg->RenderFuncs.ray_save_posn);
1590
1591 /* did the hitPoint change between runs? */
1592 if(!approx3f(node->_oldhitPoint.c,node->hitPoint_changed.c)){
1593 veccopy3f(node->hitPoint_changed.c,node->_oldhitPoint.c);
1594 MARK_EVENT(ptr, offsetof (struct X3D_TouchSensor, hitPoint_changed));
1595 }
1596
1597 /* have to normalize normal; change it from SFColor to struct point_XYZ. */
1598 veccopy3f(normalval,tg->RenderFuncs.hyp_save_norm);
1599 vecnormalize3f(normalval,normalval);
1600 veccopy3f(node->_oldhitNormal.c,normalval);
1601
1602 /* did the hitNormal change between runs? */
1603 if(!approx3f(node->_oldhitNormal.c,node->hitNormal_changed.c)) {
1604 //memcpy ((void *) &node->hitNormal_changed, (void *) &node->_oldhitNormal, sizeof(struct SFColor));
1605 veccopy3f(node->hitNormal_changed.c,node->_oldhitNormal.c);
1606 MARK_EVENT(ptr, offsetof (struct X3D_TouchSensor, hitNormal_changed));
1607 }
1608}
1609// see Mainloop.c get_hyperhit() for more explanation:
1610// in sensor-node-local coordinates (not quite sensor-local if sensor node has axisRotation):
1611// ray_save_posn - intersection point of pickray/bearing with sensitized geometry
1612// hyp_save_posn - point on camera/viewpoint nearplane on pickray/bearing, transformed to sensor-node-local
1613// hyp_save_norm - point on carmera/viewpoint farplane on pickray/bearing, transformed to sensor-node-local
1614
1615void do_LineSensor(void *ptr, int ev, int but1, int over) {
1616 /* There is no LineSensor node in the specs in April 2014. X3Dom guru Max Limper complained
1617 on X3DPublic about how PlaneSensor fails as an axis mover in the degenerate case of
1618 looking edge-on at the planeSensor. The solution we (dug9) came up with was LineSensor,
1619 which also has a degenerate case (when looking end-on at the Line), but that degerate
1620 case is more normal for users - more intuitive.
1621 LineSensor is the same as PlaneSensor, except minPosition and maxPosition are floats,
1622 and LineSensor uses a SFVec3f .direction field to say which way the Line is oriented
1623 in local-sensor coordinates. In April 2014, Paulo added LineSensor to the perl generator,
1624 and dug9 implemented it here.
1625 */
1626 struct X3D_LineSensor *node;
1627 float trackpoint[3], translation[3], xxx;
1628 //struct SFColor tr;
1629 //int tmp;
1630 ttglobal tg;
1631 UNUSED(over);
1632 node = (struct X3D_LineSensor *)ptr;
1633#ifdef SENSVERBOSE
1634 printf("%lf: TS ", TickTime());
1635 if (ev == ButtonPress) printf("ButtonPress ");
1636 else if (ev == ButtonRelease) printf("ButtonRelease ");
1637 else if (ev == KeyPress) printf("KeyPress ");
1638 else if (ev == KeyRelease) printf("KeyRelease ");
1639 else if (ev == MotionNotify) printf("%lf MotionNotify ");
1640 else printf("ev %d ", ev);
1641
1642 if (but1) printf("but1 TRUE "); else printf("but1 FALSE ");
1643 if (over) printf("over TRUE "); else printf("over FALSE ");
1644 printf("\n");
1645#endif
1646
1647 /* if not enabled, do nothing */
1648 if (!node) return;
1649
1650 if (node->__oldEnabled != node->enabled) {
1651 node->__oldEnabled = node->enabled;
1652 MARK_EVENT(X3D_NODE(node), offsetof(struct X3D_LineSensor, enabled));
1653 }
1654 if (!node->enabled) return;
1655 tg = gglobal();
1656
1657 /* only do something when button pressed */
1658 /* if (!but1) return; */
1659 if (but1){
1660 //pre-calculate for Press and Move
1661 /* hyperhit saved in render_hypersensitive phase */
1662 // bearing in sensor-local coordinates: (A=posn,B=norm)
1663 // B/norm is a point, so to get a direction vector: v = B - A
1664 float tt;
1665 float origin [] = { 0.0f, 0.0f, 0.0f };
1666 float footpoint2[3], footpoint1[3], v1[3]; //, temp[3], temp2[3];
1667 vecdif3f(v1, tg->RenderFuncs.hyp_save_norm, tg->RenderFuncs.hyp_save_posn);
1668 vecnormalize3f(v1, v1);
1669 if (!line_intersect_line_3f(tg->RenderFuncs.hyp_save_posn, v1,
1670 origin, node->direction.c, NULL, &tt, footpoint1, footpoint2))
1671 return; //no intersection, lines are parallel
1672 //footpoint1 - closest point of intersection on the A'B' bearing
1673 //footpoint2 - closest point of intersection on the Line (0,0,0)(LineSensor.direction)
1674 //tt is scale of unit vector from origin to footpoint2
1675 xxx = tt;
1676 veccopy3f(trackpoint,footpoint2); //unclamped intersection with Sensor geometry, for trackpoint
1677 }
1678 if ((ev == ButtonPress) && but1) {
1679 /* record the current position from the saved position */
1680#define LINESENSOR_FLOAT_OFFSET 1
1681#ifndef LINESENSOR_FLOAT_OFFSET
1682 struct SFColor op;
1683 veccopy3f(op.c, trackpoint);
1684 memcpy((void *)&node->_origPoint, (void *)&op,sizeof(struct SFColor));
1685 // (void *)&tg->RenderFuncs.ray_save_posn, sizeof(struct SFColor));
1686#else
1687 node->_origPoint.c[0] = xxx;
1688 //in case we go from mousedown to mouseup without mousemove:
1689 if (node->autoOffset)
1690 node->_origPoint.c[1] = node->offset;
1691 else
1692 node->_origPoint.c[1] = 0.0f;
1693#endif
1694 /* set isActive true */
1695 node->isActive = TRUE;
1696 MARK_EVENT(ptr, offsetof(struct X3D_LineSensor, isActive));
1697
1698 }
1699 else if (ev == ButtonRelease) {
1700 /* set isActive false */
1701 node->isActive = FALSE;
1702 MARK_EVENT(ptr, offsetof(struct X3D_LineSensor, isActive));
1703
1704 /* autoOffset? */
1705 if (node->autoOffset) {
1706#ifdef LINESENSOR_FLOAT_OFFSET
1707 node->offset = node->_origPoint.c[1];
1708#else
1709 veccopy3f(node->offset.c, node->translation_changed.c);
1710#endif
1711 MARK_EVENT(ptr, offsetof(struct X3D_LineSensor, offset));
1712 }
1713 }
1714 if ((ev == MotionNotify || ev == ButtonPress) && (node->isActive) && but1) {
1715 float xxxoffset, xxxorigin;
1716 //float diroffset[3], nondiroffset[3];
1717 /* trackpoint changed */
1718 veccopy3f(node->_oldtrackPoint.c,trackpoint);
1719
1720 if(!approx3f(node->_oldtrackPoint.c, node->trackPoint_changed.c) || ev == ButtonPress) {
1721 veccopy3f(node->trackPoint_changed.c, node->_oldtrackPoint.c);
1722 MARK_EVENT(ptr, offsetof(struct X3D_LineSensor, trackPoint_changed));
1723
1724 }
1725
1726 //clamp to min,max
1727#ifdef LINESENSOR_FLOAT_OFFSET
1728 xxxoffset = node->offset;
1729 xxxorigin = node->_origPoint.c[0];
1730#else
1731 //in theory the user can set a non-autoOffset sfvec3f offset that's not along .direction
1732 //- we accomodate that below^, so here we just use the part going along .direction
1733 xxxoffset = vecdot3f(node->direction.c,node->offset.c); //xxxoffset - like web3d specs offset, except just along direction vector
1734 xxxorigin = vecdot3f(node->direction.c,node->_origPoint.c); //mouse-down origin
1735#endif
1736 //xxx before: unclamped position from line origin
1737 xxx -= xxxorigin; //xxx after: net drag/delta along line since mouse-down
1738 xxx += xxxoffset; //xxx after: cumulative position along line (from line 0) after any/all mousedown/drag sequences
1739 if (node->maxPosition >= node->minPosition) {
1740 if (xxx < node->minPosition) {
1741 xxx = node->minPosition;
1742 }
1743 else if (xxx > node->maxPosition) {
1744 xxx = node->maxPosition;
1745 }
1746 }
1747 //translation clamped to LineSensor.minPosition/.maxPosition
1748 vecscale3f(translation, node->direction.c, xxx);
1749
1750#ifndef LINESENSOR_FLOAT_OFFSET
1751 //^add on any non-autoOffset non-.direction offset
1752 //a) part of offset going along direction
1753 vecscale3f(diroffset, node->direction.c, xxxoffset);
1754 //b) part of offset not going along direction
1755 vecdif3f(nondiroffset, node->offset.c, diroffset);
1756 //add non-direction part of offset
1757 vecadd3f(translation, translation, nondiroffset);
1758#endif
1759
1760 veccopy3f(node->_oldtranslation.c,translation);
1761
1762 if(!approx3f(node->_oldtranslation.c, node->translation_changed.c)) {
1763 veccopy3f(node->translation_changed.c, node->_oldtranslation.c);
1764 MARK_EVENT(ptr, offsetof(struct X3D_LineSensor, translation_changed));
1765 }
1766 //save current for use in mouse-up auto-offset
1767 node->_origPoint.c[1] = xxx;
1768 }
1769
1770
1771}
1772
1773
1774void do_PointSensor(void *ptr, int ev, int but1, int over) {
1775 /* Experimental node There is no PointSensor node in the specs in Dec 2017.
1776 Concept: you should be able to grab and drag something perpendicular to your ray.
1777 then if you move your viewpoint (ie with examine) you should be able to drag
1778 perpendicular to your new ray direction
1779 So the direction isn't in a field
1780 - its computed internally based on pickray/bearing direction
1781 - (in theory it could be an outputOnly)
1782 Trackpoint would start at ray/bearing distance from viewpoint
1783 */
1784 struct X3D_PointSensor *node;
1785 float trackpoint[3], translation[3], *posn, *rposn, *norm;
1786 ttglobal tg;
1787 UNUSED(over);
1788 node = (struct X3D_PointSensor *)ptr;
1789#ifdef SENSVERBOSE
1790 printf("%lf: TS ", TickTime());
1791 if (ev == ButtonPress) printf("ButtonPress ");
1792 else if (ev == ButtonRelease) printf("ButtonRelease ");
1793 else if (ev == KeyPress) printf("KeyPress ");
1794 else if (ev == KeyRelease) printf("KeyRelease ");
1795 else if (ev == MotionNotify) printf("%lf MotionNotify ");
1796 else printf("ev %d ", ev);
1797
1798 if (but1) printf("but1 TRUE "); else printf("but1 FALSE ");
1799 if (over) printf("over TRUE "); else printf("over FALSE ");
1800 printf("\n");
1801#endif
1802
1803 /* if not enabled, do nothing */
1804 if (!node) return;
1805
1806 if (node->__oldEnabled != node->enabled) {
1807 node->__oldEnabled = node->enabled;
1808 MARK_EVENT(X3D_NODE(node), offsetof(struct X3D_PointSensor, enabled));
1809 }
1810 if (!node->enabled) return;
1811 tg = gglobal();
1812
1813 /* only do something when button pressed */
1814 if (!but1) return;
1815 norm = tg->RenderFuncs.hyp_save_norm;
1816 posn = tg->RenderFuncs.hyp_save_posn;
1817 rposn = tg->RenderFuncs.ray_save_posn;
1818
1819 if ((ev == ButtonPress) && but1) {
1820 /* record the current position from the saved position */
1821 float tt[3];
1822 float distance = veclength3f(vecdif3f(tt,rposn,norm));
1823 //printf("dist0 = %f\n",distance);
1824 veccopy3f(trackpoint,rposn);
1825 //unconditionally send trackpoint_changed
1826 veccopy3f(node->_origPoint.c,trackpoint);
1827 veccopy3f(node->_oldtrackPoint.c, trackpoint);
1828 veccopy3f(node->trackPoint_changed.c, node->_oldtrackPoint.c);
1829 MARK_EVENT(ptr, offsetof(struct X3D_PointSensor, trackPoint_changed));
1830
1831 /* set isActive true */
1832 node->isActive = TRUE;
1833 MARK_EVENT(ptr, offsetof(struct X3D_PointSensor, isActive));
1834 }
1835 else if ((ev == MotionNotify) && (node->isActive) && but1) {
1836 /* trackpoint changed */
1837 float t1[3];
1838
1839 //pre-calculate for Press and Move
1840 /* hyperhit saved in render_hypersensitive phase */
1841 // bearing in sensor-local coordinates: (A=posn,B=norm)
1842 // B/norm is a point, so to get a direction vector: v = B - A
1843 float tt[3];
1844 //float N [] = { 0.0f, 0.0f, 1.0f };
1845 float v1[3];
1846
1847 veccopy3f(trackpoint,rposn);
1848
1849 veccopy3f(node->_oldtrackPoint.c,trackpoint);
1850 if(!approx3f(node->_oldtrackPoint.c, node->trackPoint_changed.c)) {
1851 veccopy3f(node->trackPoint_changed.c, node->_oldtrackPoint.c);
1852 MARK_EVENT(ptr, offsetof(struct X3D_PointSensor, trackPoint_changed));
1853 }
1854
1855 vecdif3f(v1, norm, posn);
1856 vecnormalize3f(v1, v1);
1857
1858 //IDEA intersect the pickray with a plane at distance to the mouse-down point
1859 // and drag in a plane perpendicular to the camera axis
1860 // ie plane = (mouse-down ray_posn, viewpoint axis)
1861 if (!line_intersect_plane_3f(posn,v1,tg->RenderFuncs.camera_axis,node->_origPoint.c,translation,NULL))
1862 return;
1863
1864 if (node->autoOffset){
1865 vecadd3f(translation,translation,node->offset.c);
1866 }
1867
1868
1869 //clamp to min,max
1870 vecclamp3f(translation,node->minPosition.c,node->maxPosition.c);
1871
1872 veccopy3f(node->_oldtranslation.c,translation);
1873
1874 if(!approx3f(node->_oldtranslation.c, node->translation_changed.c)) {
1875 veccopy3f(node->translation_changed.c, node->_oldtranslation.c);
1876 MARK_EVENT(ptr, offsetof(struct X3D_PointSensor, translation_changed));
1877 }
1878 }
1879 else if (ev == ButtonRelease) {
1880 /* set isActive false */
1881 node->isActive = FALSE;
1882 MARK_EVENT(ptr, offsetof(struct X3D_PointSensor, isActive));
1883 //unconditionally re-send trackpoint_changed
1884 MARK_EVENT(ptr, offsetof(struct X3D_PointSensor, trackPoint_changed));
1885
1886 /* autoOffset? */
1887 if (node->autoOffset) {
1888 veccopy3f(node->offset.c,node->translation_changed.c);
1889 MARK_EVENT(ptr, offsetof(struct X3D_PointSensor, offset));
1890 }
1891 }
1892
1893}
1894struct ID_point {
1895int ID;
1896float p[3];
1897int reset;
1898};
1899int lookup_ID(struct ID_point* idp, int n, int touchID){
1900 int j = -1;
1901 for(int i=0;i<n;i++){
1902 if(idp[i].ID == touchID) {
1903 j = i; break;
1904 }
1905 }
1906 return j;
1907}
1908/* void do_PlaneSensor (struct X3D_PlaneSensor *node, int ev, int over) {*/
1909void do_PlaneSensor ( void *ptr, int ev, int but1, int over) {
1910 struct X3D_PlaneSensor *node;
1911 float mult, nx, ny, trackpoint[3], inverserotation[4], *posn;
1912 float tr[3];
1913 int tmp, imethod;
1914 int touchID;
1915 struct ID_point drag_point, *dp, *op;
1916 ttglobal tg;
1917 UNUSED(over);
1918 node = (struct X3D_PlaneSensor *)ptr;
1919#ifdef SENSVERBOSE
1920 ConsoleMessage("%lf: TS ",TickTime());
1921 if (ev==ButtonPress) ConsoleMessage("ButtonPress ");
1922 else if (ev==ButtonRelease) ConsoleMessage("ButtonRelease ");
1923 else if (ev==KeyPress) ConsoleMessage("KeyPress ");
1924 else if (ev==KeyRelease) ConsoleMessage("KeyRelease ");
1925 else if (ev==MotionNotify) ConsoleMessage("MotionNotify ");
1926 else ConsoleMessage("ev %d ",ev);
1927
1928 if (but1) ConsoleMessage("but1 TRUE "); else ConsoleMessage("but1 FALSE ");
1929 if (over) ConsoleMessage("over TRUE "); else ConsoleMessage("over FALSE ");
1930 ConsoleMessage ("\n");
1931#endif
1932
1933 /* if not enabled, do nothing */
1934 if (!node) return;
1935
1936 if (node->__oldEnabled != node->enabled) {
1937 node->__oldEnabled = node->enabled;
1938 MARK_EVENT(X3D_NODE(node),offsetof (struct X3D_PlaneSensor, enabled));
1939 }
1940 if (!node->enabled) return;
1941 tg = gglobal();
1942
1943 tg = gglobal();
1944 if(!node->_orig_point){
1945 node->_orig_point = malloc(1 *sizeof(struct ID_point));
1946 memset(node->_orig_point,0,sizeof(struct ID_point));
1947 op = (struct ID_point*)node->_orig_point;
1948 op->reset = TRUE;
1949 }
1950 op = (struct ID_point*)node->_orig_point;
1951 dp = &drag_point;
1952 touchID = tg->RenderFuncs.touchID;
1953
1954 /* only do something when button pressed */
1955 /* if (!but1) return; */
1956 if (but1){
1957 float v[3], t1[3];
1958 float N[3] = { 0.0f, 0.0f, 1.0f }; //plane normal, in plane-local
1959 float NS[3]; //plane normal, in sensor-local after axisRotation
1960 //bearing (A,B) in sensor-local
1961 // A=posn, B=norm - norm is a point. To get a direction vector v = (B - A)
1962 //ConsoleMessage("hsp = %f %f %f \n", tg->RenderFuncs.hyp_save_posn[0], tg->RenderFuncs.hyp_save_posn[1], tg->RenderFuncs.hyp_save_posn[2]);
1963 vecnormalize3f(v, vecdif3f(t1, tg->RenderFuncs.hyp_save_norm, tg->RenderFuncs.hyp_save_posn));
1964 //rotate plane normal N, in plane-local to plane normal NS in sensor-local using axisRotation
1965 axisangle_rotate3f(NS,N, node->axisRotation.c);
1966 //a plane P dot N = d = const, for any point P on plane. Our plane is in plane-local coords,
1967 // so we could use P={0,0,0} and P dot N = d = 0
1968 posn = tg->RenderFuncs.hyp_save_posn;
1969 if (!line_intersect_planed_3f(posn, v, NS, 0.0f, trackpoint, NULL))
1970 return; //looking at plane edge-on / parallel, no intersection
1971 //is rotating the trackpoint/translation_changed opposite sense to rotating the virtual geometry?
1972 //-- we harmonize with x3dom and view3dscene
1973 veccopy4f(inverserotation,node->axisRotation.c);
1974 inverserotation[3] = -inverserotation[3];
1975 veccopy3f(dp->p,trackpoint);
1976 dp->ID = touchID;
1977 //axisangle_rotate3f(trackpoint, trackpoint, inverserotation);
1978 }
1979
1980 if ((ev==ButtonPress) && but1) {
1981 /* record the current position from the saved position */
1982 if(touchID == op->ID || op->reset == TRUE ){
1983 veccopy3f(op->p,trackpoint);
1984 op->ID = touchID;
1985 op->reset = TRUE; //FALSE;
1986 /* set isActive true */
1987 node->isActive=TRUE;
1988 MARK_EVENT (ptr, offsetof (struct X3D_PlaneSensor, isActive));
1989 }
1990
1991 }
1992 else if (ev == ButtonRelease) {
1993 /* set isActive false */
1994 if (touchID == op->ID) {
1995 //printf("release %d\n",touchID);
1996 node->isActive = FALSE;
1997 MARK_EVENT(ptr, offsetof(struct X3D_PlaneSensor, isActive));
1998 op->reset = TRUE;
1999 /* autoOffset? */
2000 if (node->autoOffset) {
2001 veccopy3f(node->offset.c, node->translation_changed.c);
2002
2003 MARK_EVENT(ptr, offsetof(struct X3D_PlaneSensor, offset));
2004 }
2005 }
2006 }
2007
2008
2009 if ((ev==MotionNotify || ev==ButtonPress) && (node->isActive) && but1) {
2010 /* hyperhit saved in render_hypersensitive phase */
2011 if(dp->ID == op->ID){
2012 /* trackpoint changed */
2013 if(!node->sensorLocalOutput){
2014 axisangle_rotate3f(trackpoint,trackpoint, inverserotation);
2015 }
2016
2017 veccopy3f(node->_oldtrackPoint.c, trackpoint);
2018 /*printf(">%f %f %f\n",nx,ny,node->_oldtrackPoint.c[2]); */
2019 if(!approx3f(node->_oldtrackPoint.c,node->trackPoint_changed.c) || ev==ButtonPress) {
2020 veccopy3f(node->trackPoint_changed.c, node->_oldtrackPoint.c);
2021 MARK_EVENT(ptr, offsetof (struct X3D_PlaneSensor, trackPoint_changed));
2022
2023 }
2024
2025 if(op->reset){
2026 veccopy3f(op->p,dp->p);
2027 op->reset = FALSE;
2028 //printf("reset %d\n",op->ID);
2029 }
2030 vecdif3f(tr,dp->p,op->p);
2031
2032 //translation
2033 vecadd3f(tr,tr,node->offset.c);
2034 /* clamp translation to max/min position */
2035 vecclamp2f(tr,node->minPosition.c,node->maxPosition.c);
2036 if(!node->sensorLocalOutput){
2037 axisangle_rotate3f(tr,tr, node->axisRotation.c);
2038 }
2039 veccopy3f(node->_oldtranslation.c,tr);
2040
2041 if(!approx3f(node->_oldtranslation.c,node->translation_changed.c)) {
2042 veccopy3f(node->translation_changed.c, (void *) node->_oldtranslation.c);
2043 MARK_EVENT(ptr, offsetof (struct X3D_PlaneSensor, translation_changed));
2044 }
2045 }
2046
2047 }
2048}
2049
2050//MIT AND EQUIVALENT PERMISSIVE LICENSE >>>>>>>>>
2051// original algos from LINPACK, typed in / transcribed from book LINPACK user's guide
2052// to C, then reworked to C++, then reworked back to C
2053// single precision float verion
2054// matrices - storage interpetation: elements are stored contiguously along each _row_
2055
2056void s_mat_multiply(float *r, float *a, int nra, int nca, float *b, int ncb){
2057 for(int i = 0; i< nra; i++){
2058 for(int j=0; j< ncb;j++){
2059 r[i*ncb +j] = 0.0f;
2060 for(int k= 0;k< nca;k++){
2061 r[i*ncb +j] += a[i*nca +k]* b[k*ncb +j];
2062 }
2063 }
2064 }
2065}
2066void s_mat_transpose(float *r, float *a, int nra, int nca){
2067 for(int i=0;i<nra;i++){
2068 for(int j=0;j<nca;j++){
2069 r[j*nra +i] = a[i*nca +j];
2070 }
2071 }
2072}
2073static void saxpy( int n, float *sa, float *sx, float *sy )
2074//constant times a vector plus a vector
2075{
2076 int i;
2077
2078 if( !n )return;
2079 if( (*sa) == 0.0 )return;
2080 for( i=0; i< n; i++ )
2081 {
2082 sy[i] = sy[i] + (*sa)*sx[i];
2083 }
2084}
2085
2086static float sdot( int n, float *sx, float *sy )
2087//dot product
2088{
2089 float stemp;
2090 int i;
2091 stemp = 0.0;
2092
2093 if( !n ) return( 0.0 );
2094 for( i=0; i< n; i++ )
2095 {
2096 stemp = stemp + sx[i]*sy[i];
2097 }
2098 return( stemp );
2099}
2100
2101static void sscal( int n, float *sa, float *sx )
2102//scales a vector by a constant
2103{
2104 int i;
2105
2106 if( !n )return;
2107 for( i=0; i< n; i++ )
2108 {
2109 sx[i] = (*sa)*sx[i];
2110 }
2111}
2112
2113void sprintnorm(float *N, float *B, int n)
2114{
2115 int i, j;
2116 float *a;
2117 printf(" NE.sz= %ld \n", n );
2118 for( i=0; i< n; i++ )
2119 {
2120 a = &N[i*n];
2121 for(j=0;j<n;j++) printf(" %4.0lf ", a[j] );
2122 printf("\n");
2123 }
2124 for(j=0;j< n;j++) printf(" %4.0lf ", B[j] );
2125 printf("\n");
2126
2127}
2128
2129int spofa(float *N, int n) {
2130//n - order of a
2131//returns - a is upper triangular
2132//info = 0 normal
2133// = k error at row k
2134
2135 float t, s, *a0, *a1;
2136 int j, k;
2137 int info;
2138
2139 for( j=0; j<n; j++ )
2140 {
2141 a0 = &N[j*n];
2142 info = j+1;
2143 s = 0.0f;
2144 // note - this for loop should not execute when j=0
2145 for( k=0; k<j-1; k++ )
2146 {
2147 a1 = &N[k*n];
2148 t = a0[k] - sdot( k, a1, a0 );
2149 t = t / a1[k];
2150 a0[k] = t;
2151 s = s + t*t;
2152 }
2153 s = a0[j] - s;
2154 if( s <= 0.0 ) return info;
2155 a0[j] = (float)sqrt( s );
2156 }
2157 info = 0;
2158 return info;
2159}
2160
2161void sposl(float *N, int n, float *B) {
2162//solves the system ax = b
2163//a[1][n] - matrix column
2164//b[n]
2165
2166 float t, *a;
2167 int k;
2168
2169 for( k=0; k<n; k++ )
2170 {
2171 a = &N[k*n];
2172 t= sdot( k, a, B );
2173 B[k] = ( B[k] - t )/a[k];
2174 }
2175
2176 for( k=n-1; k>(-1); k-- )
2177 {
2178 a = &N[k*n];
2179 B[k] = B[k]/a[k];
2180 t = -B[k];
2181 saxpy( k, &t, a, B );
2182 }
2183}
2184
2185void spodi(float *N, int n )
2186//computes inverse - don't need unless doing statistical analysis on fit of data
2187{
2188 float t, *a0, *a1;
2189 int j,k,nj,nk;
2190
2191 for( k=0; k<n; k++ )
2192 {
2193 a0 = &N[k*n];
2194 a0[k] = 1.0f / a0[k];
2195 t = -a0[k];
2196 sscal( k, &t, a0 );
2197 // note the following loop should not execute when k = n-1
2198 for( j=(k+1); j<n; j++ )
2199 {
2200 a1 = &N[j*n];
2201 t = a1[k];
2202 a1[k] = 0.0;
2203 nk = k+1; // takes acount of c starts on [0], and we want 1 elemnt
2204 saxpy( nk, &t, a0, a1 );
2205 }
2206 }
2207 // form inverse(r)*transpose(inverse(r))
2208 for( j=0L; j<n; j++ )
2209 {
2210 a0 = &N[j*n];
2211 // note the following loop should not execute when j =0
2212 for( k=0; k<j; k++ )
2213 {
2214 a1 = &N[k*n];
2215 t = a0[k];
2216 nk = k+1; // takes acount of c starts on [0], and we want 1 elemnt
2217 saxpy( nk, &t, a0, a1 );
2218 }
2219 t = a0[j];
2220 nj = j+1;
2221 sscal( nj, &t, a0 );
2222 }
2223}
2224
2225int least_squares_similarity2D_linpack(float *v0, float *v1, int np, float *param)
2226{
2227
2228 // chapter 2 p.46-48
2229 // solve Ax = b (via linear least sqaures)
2230 int n, nu;
2231 int noisy = 0;
2232 //allocate and populate your A and b
2233 //b is usually simple vector of x,y,x,y,x,y... however many points you have
2234 //A is usually some function of the other point source
2235 //x is an implicit vector of unknows - lets call them a,b,c,d,e,f
2236 // ie A[1]*x = f(x-,y') = a*x' + b*x'*y" + c*x'**2 + d*y'**2 or something like that
2237 // but since you don't know your x [] parameters yet, you leave them out
2238 // so that a row of A x column of x gives your formula
2239 // we know A, and b.
2240 // we want X
2241 // AtA*X = At*b
2242 // X = inverse(AtxA)*At*b
2243 // LU lower upper solvers don't do a full inverse, but they give you what you want X
2244
2245 // and we have some known points b[] = [x1' y1' x2' y2' x3' y3' x4' y4']
2246 // and we have some measured points [x1 y1 x2 y2 x3 y3 x4 y4 ...]
2247 // affine 2D - from textbooks
2248 //Digital Photogrammetry p.322 shows Affine as well - shows both similartiy and affine interpretation of 6 parameters
2249 //Similarity
2250 //a11 = scale*cose
2251 //a12 = -scale*sin
2252 //a21 = -a12
2253 //a22 = a11
2254 //Affine
2255 //a11 = sx*cos (skew mentioned)
2256 //a12 = -sy*sin (skew mentioned)
2257 //a21 = sx*sin
2258 //a22 = sy*cos
2259 // here the similarity is 4 parameters (equivalent to a scale, a rotation, and x,y translation)
2260 // let unknow parameters X = [a b c d]
2261 // A[i ]*X = x*a - y*b + 1*c + 0*d = b[0]
2262 // A[i+1}*X = y*a + x*b + 0*c + 1*d = b[1]
2263 // OR
2264 // A[i ] = [x -y 1 0]
2265 // A[i+1] = [y x 0 1]
2266 // with a = cos*scale, b = sin*scale
2267 // and if you have 2 points, you'd have 4 rows in A[]
2268
2269 n = 2*np; //number of observations = number of points, x xy 2 each
2270 nu = 4; //number of unknowns to solve: for a 2D similarity transform, its 4 unknows: 1 rotation, 1 scale, xy 2 translations
2271 static float *p1 = NULL;
2272 static float *N = NULL;
2273 static float *at = NULL;
2274 static float *a = NULL;
2275 static float *B = NULL;
2276
2277 p1 = realloc(p1, n*sizeof(float)); // 2 points, xy each
2278 a = realloc(a, nu*n*sizeof(float)); //4 unknowns, n = 2 x np observations
2279 at = realloc(at, n*nu*sizeof(float)); // A transpose
2280 N = realloc(N, nu*nu*sizeof(float)); //normal equation Nu = B, u = inverse(N)xB
2281 B = realloc(B, nu*sizeof(float)); // B = At x b
2282
2283 for(int i=0;i<np;i++){
2284 int j=2*i;
2285 p1[j+0] = v1[3*i +0];
2286 p1[j+1] = v1[3*i +1];
2287 }
2288 if(noisy){
2289 printf("b=\n");
2290 for(int i=0;i<n;i++){
2291 printf("[ %f ]\n",p1[i]);
2292 }
2293 }
2294 // [p1.x] = A[ x -y 1 0] [a]
2295 // [p1.y] [y x 0 1] [b]
2296 // [c]
2297 // [d]
2298 // b = Ax
2299 // x = inverse(AtA)*Atb
2300 //
2301 for(int i=0;i < np; i++){
2302 int ii,jj;
2303 ii = (2*i+0)*4;
2304 jj = (2*i+1)*4;
2305 a[ii + 0] = v0[i*3 +0];
2306 a[ii + 1] = -v0[i*3 +1];
2307 a[ii + 2] = 1.0f;
2308 a[ii + 3] = 0.0f;
2309 a[jj + 0] = v0[i*3 +1];
2310 a[jj + 1] = v0[i*3 +0];
2311 a[jj + 2] = 0.0f;
2312 a[jj + 3] = 1.0f;
2313 }
2314 if(noisy){
2315 printf("a=\n");
2316 for(int i=0; i<n; i++){
2317 printf("[ ");
2318 for(int j=0; j<nu; j++) printf("%f ",a[i*nu +j]);
2319 printf("]\n");
2320 }
2321 }
2322
2323 // now need to 'square up' for least squares
2324 // N = at x a
2325 // B = at x b
2326 s_mat_transpose(at,a,n,nu);
2327
2328
2329 if(noisy){
2330 printf("At\n");
2331 for(int i=0; i< nu;i++){
2332 printf("[ ");
2333 for(int j=0; j < n; j++) printf("%f ",at[i*n +j]);
2334 printf(" ]\n");
2335 }
2336 }
2337 s_mat_multiply(N,at,nu,n,a,nu);
2338 if(noisy){
2339 printf("N\n");
2340 for(int i=0;i<nu;i++){
2341 printf("[ ");
2342 for(int j=0;j<nu; j++) printf("%f ",N[i*nu +j]);
2343 printf(" ]\n");
2344 }
2345 }
2346 s_mat_multiply(B,at,nu,n,p1,1);
2347 if(noisy){
2348 printf("B=Atb\n");
2349 for(int i=0;i<nu;i++){
2350 printf("[ ");
2351 printf("%f ",B[i]);
2352 printf(" ]\n");
2353 }
2354 }
2355 int info = spofa(N,nu);
2356 if(info){
2357 printf("spofa info %d\n",info);
2358 return info;
2359 }
2360 if(noisy){
2361 printf("N after spofa factirung\n");
2362 for(int i=0;i<nu;i++){
2363 printf("[ ");
2364 for(int j=0;j<nu; j++) printf("%f ",N[i*nu +j]);
2365 printf(" ]\n");
2366 }
2367 }
2368
2369 sposl(N,nu,B);
2370
2371 if(noisy)printf("solved a=%f b=%f c=%f d=%f\n",B[0],B[1],B[2],B[3]);
2372 //if(noisy)printf("should be 1 0 1 1\n");
2373 float scale = sqrt(B[0]*B[0] + B[1]*B[1]);
2374 float anglerad = atan2(B[1]/scale,B[0]/scale);
2375 float angledeg = anglerad *180.0f/3.141596f;
2376 if(noisy)printf("scale=%f angle=%f\n",scale,angledeg);
2377
2378 if(noisy)printf("translation x= %f y= %f \n",B[2],B[3]);
2379 //x given back in output b, your original A is destroyed
2380 param[0] = scale; //x scale
2381 param[1] = scale; //y scale
2382 param[2] = anglerad;
2383 param[3] = B[2];
2384 param[4] = B[3];
2385
2386 return 0;
2387}
2388
2389// <<<< MIT AND EQUIVALENT PERMISSIVE LICENSE
2390int scale_constrained_2D(float *v00, float *v11, int np, float *param, float *minScale, float *maxScale){
2391 //v0 has origs, v1 has drags
2392 float p0[3], p1[3], delta_orig[3], delta_drag[3], angle, drag0[3], drag1[3], scale[2], iso_scale;
2393 float v0[6],v1[6];
2394 veccopy3f(p0,v00);
2395 for(int i=0;i<2;i++){
2396 vecdif3f(&v0[i*3],&v00[i*3],p0);
2397 vecdif3f(&v1[i*3],&v11[i*3],p0);
2398 }
2399 vecdif3f(delta_orig,&v0[3],v0);
2400 vecdif3f(delta_drag,&v1[3],v1);
2401 vecdif3f(drag0,v1,v0);
2402 vecdif3f(drag1,&v1[3],&v0[3]);
2403 {
2404 //isotropic scale
2405 float scale_orig, scale_drag, scale;
2406 scale_orig = veclength2f(delta_orig);
2407 scale_drag = veclength2f(delta_drag);
2408 iso_scale = scale_drag / scale_orig;
2409 }
2410 scale[0] = min(maxScale[0],max(minScale[0],iso_scale));
2411 scale[1] = min(maxScale[1],max(minScale[1],iso_scale));
2412 int need_aniso = scale[0] != scale[1]? TRUE : FALSE;
2413 if(need_aniso){
2414 //anisotropic scale
2415 float scale_orig[2], scale_drag[2], scale[2];
2416 scale[0] = delta_drag[0] / delta_orig[0];
2417 scale[1] = delta_drag[1] / delta_orig[1];
2418 scale[0] = min(maxScale[0],max(minScale[0],scale[0]));
2419 scale[1] = min(maxScale[1],max(minScale[1],scale[1]));
2420 }
2421 delta_drag[0] *= 1.0/scale[0];
2422 delta_drag[1] *= 1.0/scale[1];
2423 angle = vecangle2f(delta_orig,delta_drag);
2424 float x,y, xx, yy;
2425 xx = p0[0];
2426 yy = p0[1];
2427 xx = xx*scale[0];
2428 yy = yy*scale[1];
2429 x = (cos(angle)*xx - sin(angle)*yy);
2430 y = (sin(angle)*xx + cos(angle)*yy);
2431 xx = x; yy = y;
2432 xx -= v1[0];
2433 yy -= v1[1];
2434
2435 param[0] = scale[0];
2436 param[1] = scale[1];
2437 param[2] = angle;
2438 param[3] = -( xx- p0[0]);
2439 param[4] = -( yy- p0[1]);
2440
2441 return 1;
2442}
2443
2444void mainloop_update_touch_hyperhit_matrix(int touchID, double *netTao);
2445void mainloop_reset_touch_hyperhit(int touchID);
2446
2447#include "Decompose.h"
2448void do_MultiTouchSensor ( void *ptr, int ev, int but1, int over) {
2449 struct X3D_MultiTouchSensor *node;
2450 float nx, ny, trackpoint[3], inverserotation[4], *posn;
2451 float tr[3];
2452 int tmp, imethod, touchID;
2453
2454 ttglobal tg;
2455 UNUSED(over);
2456 node = (struct X3D_MultiTouchSensor *)ptr;
2457#ifdef SENSVERBOSE
2458 ConsoleMessage("%lf: TS ",TickTime());
2459 if (ev==ButtonPress) ConsoleMessage("ButtonPress ");
2460 else if (ev==ButtonRelease) ConsoleMessage("ButtonRelease ");
2461 else if (ev==KeyPress) ConsoleMessage("KeyPress ");
2462 else if (ev==KeyRelease) ConsoleMessage("KeyRelease ");
2463 else if (ev==MotionNotify) ConsoleMessage("MotionNotify ");
2464 else ConsoleMessage("ev %d ",ev);
2465
2466 if (but1) ConsoleMessage("but1 TRUE "); else ConsoleMessage("but1 FALSE ");
2467 if (over) ConsoleMessage("over TRUE "); else ConsoleMessage("over FALSE ");
2468 ConsoleMessage ("\n");
2469#endif
2470
2471 /* if not enabled, do nothing */
2472 if (!node) return;
2473
2474 if (node->__oldEnabled != node->enabled) {
2475 node->__oldEnabled = node->enabled;
2476 MARK_EVENT(X3D_NODE(node),offsetof (struct X3D_PlaneSensor, enabled));
2477 }
2478 if (!node->enabled)
2479 return;
2480 tg = gglobal();
2481 if(!node->_orig_points){
2482 node->_orig_points = malloc(32 *sizeof(struct ID_point));
2483 node->_drag_points = malloc(32 *sizeof(struct ID_point));
2484 }
2485 struct ID_point *op = (struct ID_point*)node->_orig_points;
2486 struct ID_point *dp = (struct ID_point*)node->_drag_points;
2487 touchID = tg->RenderFuncs.touchID;
2488
2489 /* only do something when button pressed */
2490 /* if (!but1) return; */
2491 if (but1){
2492 float v[3], t1[3];
2493 float N[3] = { 0.0f, 0.0f, 1.0f }; //plane normal, in plane-local
2494 float NS[3]; //plane normal, in sensor-local after axisRotation
2495
2496 //STRATEGY FOR MULTITOUCH:
2497 //MARK EVENT for a single touch, then over-write it with
2498 // multitouch results if a second touch shows up on the swame rendering frame/loop
2499 if(node->_drag_count > 31)
2500 return; //how many fingers you you have?
2501 if(tg->Mainloop.iframe != node->_lastframe){
2502 node->_drag_count = 0; //we re-count drags on every frame
2503 node->_lastframe = tg->Mainloop.iframe;
2504 }
2505 //bearing (A,B) in sensor-local
2506 // A=posn, B=norm - norm is a point. To get a direction vector v = (B - A)
2507 //ConsoleMessage("hsp = %f %f %f \n", tg->RenderFuncs.hyp_save_posn[0], tg->RenderFuncs.hyp_save_posn[1], tg->RenderFuncs.hyp_save_posn[2]);
2508 vecnormalize3f(v, vecdif3f(t1, tg->RenderFuncs.hyp_save_norm, tg->RenderFuncs.hyp_save_posn));
2509 //rotate plane normal N, in plane-local to plane normal NS in sensor-local using axisRotation
2510 axisangle_rotate3f(NS,N, node->axisRotation.c);
2511 //a plane P dot N = d = const, for any point P on plane. Our plane is in plane-local coords,
2512 // so we could use P={0,0,0} and P dot N = d = 0
2513 posn = tg->RenderFuncs.hyp_save_posn;
2514 //printf("%d %f %f \n",tg->RenderFuncs.touchID,posn[0],posn[1]);
2515 if (!line_intersect_planed_3f(posn, v, NS, 0.0f, trackpoint, NULL))
2516 return; //looking at plane edge-on / parallel, no intersection
2517 //is rotating the trackpoint/translation_changed opposite sense to rotating the virtual geometry?
2518 //-- we harmonize with x3dom and view3dscene
2519 veccopy4f(inverserotation,node->axisRotation.c);
2520 inverserotation[3] = -inverserotation[3];
2521 //axisangle_rotate3f(trackpoint, trackpoint, inverserotation);
2522 veccopy3f(dp[node->_drag_count].p,trackpoint);
2523 dp[node->_drag_count].ID = touchID;
2524 node->_drag_count++;
2525 }
2526 if ((ev==ButtonPress) && but1) {
2527 /* record the current position from the saved position */
2528 //struct SFColor op;
2529 float *posn;
2530 posn = tg->RenderFuncs.hyp_save_posn;
2531
2532 //op[node->_orig_count].reset = TRUE;
2533 //veccopy3f(ip[*touchpoin])
2534 //memcpy((void *)&node->_origPoint, (void *)&op,sizeof(struct SFColor));
2535 //if(node->_touchcount == 1)
2536 // veccopy3f(node->_origPoint.c,op.c);
2537 //else if(node->_touchcount == 2)
2538 // veccopy3f(node->_origPoint2.c,op.c);
2539
2540 /* autoOffset? */
2541 if (node->autoOffset) {
2542 veccopy3f(node->offset.c,node->translation_changed.c);
2543 MARK_EVENT (ptr, offsetof (struct X3D_MultiTouchSensor, offset));
2544 veccopy4f(node->rotationOffset.c,node->rotation_changed.c);
2545 MARK_EVENT (ptr, offsetof (struct X3D_MultiTouchSensor, rotationOffset));
2546 veccopy3f(node->scaleOffset.c,node->scale_changed.c);
2547 MARK_EVENT (ptr, offsetof (struct X3D_MultiTouchSensor, scaleOffset));
2548 if(1){
2549 //if have 2 multitouch drags, and lift one, and we write the offsets
2550 // then we need to update the hyperhit matrx for the remaining drag
2551 // so it remains in sync with the offsets
2552 double Tao[16], Tca[16], Tout[16], temp1[16], temp2[16], temp3[16], temp4[16], scaled[3], rotd[4], trand[3], dangle;
2553 //TautoOffset
2554 float2double(scaled,node->scaleOffset.c,3);
2555 matscale(temp1,scaled[0],scaled[1],scaled[2]);
2556 float2double(rotd,node->rotationOffset.c,4);
2557 matrotate(temp2,rotd[3],rotd[0],rotd[1],rotd[2]);
2558 float2double(trand,node->offset.c,3);
2559 mattranslate(temp3,trand[0],trand[1],trand[2]);
2560 matmultiplyAFFINE(temp4,temp1,temp2);
2561 matmultiplyAFFINE(Tao,temp4,temp3);
2562 if(node->_lastTao == NULL){
2563 node->_lastTao = malloc(16*sizeof(double));
2564 matidentity4d(node->_lastTao);
2565 }
2566 double lastTaoInv[16], netTao[16];
2567 matinverseAFFINE(lastTaoInv,node->_lastTao);
2568 matmultiplyAFFINE(netTao,lastTaoInv,Tao);
2569 for(int i=0;i<node->_orig_count;i++){
2570 mainloop_update_touch_hyperhit_matrix(op[i].ID,netTao);
2571 }
2572 memcpy(node->_lastTao,Tao,16*sizeof(double));
2573 if(1){
2574 double dd[3];
2575 float2double(dd,trackpoint,3);
2576 transformAFFINEd(dd,dd,netTao);
2577 double2float(trackpoint,dd,3);
2578 }
2579 }
2580 }
2581
2582 //veccopy3f(op.c, trackpoint);
2583 veccopy3f(op[node->_orig_count].p,trackpoint);
2584 op[node->_orig_count].ID = touchID;
2585 op[node->_orig_count].reset = FALSE; //TRUE;
2586 //printf("(A %d)",touchID);
2587 node->_orig_count++;
2588
2589 for(int k=0;k<node->_orig_count;k++){
2590 op[k].reset = TRUE;
2591 }
2592
2593
2594 //printf("but down\n");
2595 /* set isActive true */
2596 node->isActive=TRUE;
2597 MARK_EVENT (ptr, offsetof (struct X3D_MultiTouchSensor, isActive));
2598
2599 } else if ((ev==MotionNotify) && (node->isActive) && but1) {
2600 /* hyperhit saved in render_hypersensitive phase */
2601 //the calling function setup_picking() in mainloop is in a tight loop
2602 // over the number of touches in the current frame, and sends one touch at
2603 // at a time. So we wait and accumulate all the ongoing/current-frame drags/touches
2604 // before doing real work.
2605 if(node->_drag_count == node->_orig_count) {
2606 float rot4[4]= {0.0f, 0.0f, 1.0f, 0.0f};
2607 float scale3[3] = {1.0f,1.0f,1.0f};
2608
2609 nx = trackpoint[0]; ny = trackpoint[1];
2610 #ifdef SEVERBOSE
2611 ConsoleMessage ("now, nx %f ny %f op %f %f %f\n",nx,ny,
2612 node->_origPoint.c[0],node->_origPoint.c[1],
2613 node->_origPoint.c[2]);
2614 #endif
2615
2616 /* trackpoint changed */
2617 int ndrag = node->_drag_count;
2618 node->trackPoints_changed.p = realloc(node->trackPoints_changed.p, ndrag*(sizeof(struct SFVec3f)));
2619 node->trackPoints_changed.n = ndrag;
2620 node->touches_changed.p = realloc(node->touches_changed.p,ndrag*(sizeof(int)));
2621 node->touches_changed.n = ndrag;
2622
2623 for(int i=0; i<ndrag; i++){
2624 struct ID_point* dragp = &((struct ID_point*)node->_drag_points)[i];
2625 struct SFVec3f* p = (struct SFVec3f*)&node->trackPoints_changed.p[i];
2626 int *itouch = &(node->touches_changed.p[i]);
2627 if(!node->sensorLocalOutput){
2628 axisangle_rotate3f(p->c,dragp->p, inverserotation);
2629 }
2630 *itouch = dragp->ID;
2631 }
2632 /*printf(">%f %f %f\n",nx,ny,node->_oldtrackPoint.c[2]); */
2633 MARK_EVENT(ptr, offsetof (struct X3D_MultiTouchSensor, trackPoints_changed));
2634 MARK_EVENT(ptr, offsetof (struct X3D_MultiTouchSensor, touches_changed));
2635
2636
2637 //compute any translation, rotation, scaling
2638 switch(node->_drag_count){
2639 case 0: break;
2640 case 1: //translation only
2641 {
2642 //printf("case1 ");
2643 int j = lookup_ID(op,node->_orig_count,touchID);
2644 //printf("%d ",j);
2645 if(j > -1){
2646 if(op[j].reset){
2647 veccopy3f(op[j].p,dp[0].p);
2648 op[j].reset = FALSE;
2649 //printf("(Q %d)",op[j].ID);
2650 }
2651 vecdif3f(tr,dp[0].p,op[j].p);
2652 if(0){
2653 vecprint3fb("tr_comp",tr,"\n");
2654 vecprint3fb("sc_comp",scale3,"\n");
2655 vecprint4fb("rt_comp",rot4,"\n");
2656 }
2657
2658 }
2659 }
2660 break;
2661 case 2: //translation, rotation 1 scale (similarity)
2662 case 3: //translation, rotation, 2 scale (affine) maybe using least squares
2663 default:
2664 {
2665 //printf("case2\n");
2666 //we'll use the first 2 points and solve for single scale, rotation, xy translation (4 param)
2667 //but more gnerally you could use least squares, and solve a closest fit
2668 // affine (2 scalea, shear, rot, xytrans = 6param, needs 3+ touches)
2669 // to any number of points/touches
2670 int j0 = lookup_ID(op,node->_orig_count,dp[0].ID);
2671 int j1 = lookup_ID(op,node->_orig_count,dp[1].ID);
2672 //printf("j0,j1 %d %d ^ dp ID 0,1 %d %d $ op ID j01 %d %d ",j0,j1,dp[0].ID, dp[1].ID,op[j0].ID,op[j1].ID);
2673 if(j0 < 0 || j1 < 0){
2674 printf("ouch missing a touch point we should have \n");
2675 getchar();
2676 }
2677 if(j0 > -1 && j1 > -1){
2678 float dif0[3],dif1[3],dif[3];
2679 float *drag1,*drag0,*orig1,*orig0;
2680 drag1 = dp[1].p; drag0=dp[0].p;
2681 orig1 = op[j1].p; orig0 = op[j0].p;
2682 if(op[j0].reset){
2683 veccopy3f(op[j0].p,dp[0].p);
2684 op[j0].reset = FALSE;
2685 //printf("R1 ");
2686 }
2687 if(op[j1].reset){
2688 veccopy3f(op[j1].p,dp[1].p);
2689 op[j1].reset = FALSE;
2690 //printf("R2 ");
2691 }
2692
2693 vecdif3f(dif0,drag0,orig0);
2694 vecdif3f(dif1,drag1,orig1);
2695 vecadd3f(dif,dif0,dif1);
2696 vecscale3f(tr,dif,.5f);
2697
2698
2699 if(1) {
2700 // least squares
2701 float v0[6], v1[6], param[5];
2702 int np = 2;
2703 veccopy3f(&v0[0*3 +0],orig0);
2704 veccopy3f(&v0[1*3 +0],orig1);
2705 veccopy3f(&v1[0*3 +0],drag0);
2706 veccopy3f(&v1[1*3 +0],drag1);
2707 memset(param,0,5*sizeof(float));
2708 param[0] = param[1] = 1.0f;
2709 if(0) for(int k=0;k<2;k++){
2710 printf("%f %f | %f %f\n",v0[k*2],v0[k*2+1], v1[k*2],v1[k*2+1]);
2711 }
2712 if(1){
2713 least_squares_similarity2D_linpack(v0,v1,np,param);
2714 }else{
2715 scale_constrained_2D(v0,v1,np,param,node->minScale.c,node->maxScale.c);
2716 }
2717 rot4[3] = param[2];
2718 float scalex = param[0];
2719 float scaley = param[1];
2720 vecset3f(scale3,scalex,scaley,1.0f);
2721 veccopy2f(tr,&param[3]);
2722 if(0){
2723 vecprint3fb("tr_comp",tr,"\n");
2724 vecprint3fb("sc_comp",scale3,"\n");
2725 vecprint4fb("rt_comp",rot4,"\n");
2726 }
2727 if(0){
2728 // test using params computed above:
2729 // goal convert orig -> Tca -> drag using params
2730 double Tao[16], Tca[16], Tout[16], temp1[16], temp2[16], temp3[16], temp4[16];
2731 double dd[3], dd0[3], scaled[3], rotd[4], trand[3], dangle;
2732 float tca_orig[3], tr1[3];
2733 //test: can we create a transform and transform origs to drags?
2734 //Tcomputation_above
2735 float2double(scaled,scale3,3);
2736 matscale(temp1,scaled[0],scaled[1],scaled[2]);
2737 float2double(rotd,rot4,4);
2738 matrotate(temp2,rotd[3],rotd[0],rotd[1],rotd[2]);
2739 float2double(trand,tr,3);
2740 mattranslate(temp3,trand[0],trand[1],trand[2]);
2741
2742 matmultiplyAFFINE(temp4,temp1,temp2);
2743 matmultiplyAFFINE(Tca,temp4,temp3);
2744 // orign -> Tca -> drag
2745 float2double(dd0,orig0,3);
2746 transformAFFINEd(dd,dd0,Tca);
2747 double2float(tca_orig,dd,3);
2748 vecprint3fb("torig0 ",tca_orig,"\n");
2749 vecprint3fb("drag0 ",drag0,"\n");
2750
2751 float2double(dd0,orig1,3);
2752 transformAFFINEd(dd,dd0,Tca);
2753 double2float(tca_orig,dd,3);
2754 vecprint3fb("torig1 ",tca_orig,"\n");
2755 vecprint3fb("drag1 ",drag1,"\n");
2756 }
2757
2758 }else if(1) {
2759 // did not work properly.
2760 // what I should have done:
2761 // TRS = TCRS=C like x3d transform breakout
2762 // https://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/group.html#Transform
2763 // tjem tp get the summary transform T from the TCRS-C
2764 // would muliply all those to geterh as 4x4 transform
2765 // then use matrix_decompose - see below.
2766 // apply each value to intermediate coords before computing next
2767 //scale
2768 float dd00[3],dd01[3],dd10[3], dd11[3], dorig[3],ddrag[3], scale;
2769 vecdif3f(dd00,orig0,tr);
2770 vecdif3f(dd01,drag0,tr);
2771 vecdif3f(dd10,orig1,tr);
2772 vecdif3f(dd11,drag1,tr);
2773 vecdif3f(dorig,orig1,orig0);
2774 vecdif3f(ddrag,drag1,drag0);
2775 scale = veclength3f(ddrag)/veclength3f(dorig);
2776
2777 vecset3f(scale3,scale,scale,1.0f);
2778
2779 vecscale3f(dorig,dorig,scale);
2780 vecscale3f(ddrag,ddrag,scale);
2781
2782 //angle
2783 float angle00, angle01, angle10, angle11, angle0, angle1, angle;
2784 angle0 = atan2(dorig[1],dorig[0]);
2785 angle1 = atan2(ddrag[1],ddrag[0]);
2786 angle = angleNormalized(angle1 - angle0);
2787 rot4[3] = angle;
2788
2789 if(0){
2790 // TRS = TxCxRxSx(-C)
2791 // with C = (drag1 - drag0)/2 or (orig1 - orig0)/2
2792 // then T = T + {C - RxS(-C)}
2793 float center0[3], center[3], deltac[3];
2794 vecadd3f(center,drag1,drag0);
2795 //vecprint3fb("drag1",drag1," ");
2796 //vecprint3fb("drag0",drag0,"\n");
2797 //printf("scale %f ",scale);
2798 vecscale3f(center0,center,.5f*scale);
2799 //vecprint3fb("center scaled",center0," ");
2800 axisangle_rotate3f(center0,center0,rot4);
2801 //vecprint3fb("center rot4",center0,"\n");
2802 vecdif3f(deltac,center,center0);
2803 vecprint3fb("celtac",deltac,"\n");
2804 vecadd3f(tr,tr,deltac);
2805 }
2806
2807
2808 }
2809 }
2810 }
2811 break;
2812 }
2813
2814
2815 // apply auto-offsets from last buttonRelease to current outputs
2816 if(0){
2817 //not working well for multitouch with rotation and scale
2818 vecadd3f(tr,tr,node->offset.c);
2819 vecmult3f(scale3,scale3,node->scaleOffset.c);
2820 axisangle_rotate4f(rot4,rot4,node->rotationOffset.c);
2821 }
2822 else if(1){
2823 // Tout = Tcomputed_above X TautoOffset
2824 double Tao[16], Tca[16], Tout[16], temp1[16], temp2[16], temp3[16], temp4[16], scaled[3], rotd[4], trand[3], dangle;
2825 //TautoOffset
2826 float2double(scaled,node->scaleOffset.c,3);
2827 matscale(temp1,scaled[0],scaled[1],scaled[2]);
2828 float2double(rotd,node->rotationOffset.c,4);
2829 matrotate(temp2,rotd[3],rotd[0],rotd[1],rotd[2]);
2830 float2double(trand,node->offset.c,3);
2831 mattranslate(temp3,trand[0],trand[1],trand[2]);
2832 matmultiplyAFFINE(temp4,temp1,temp2);
2833 matmultiplyAFFINE(Tao,temp4,temp3);
2834
2835 //Tcomputation_above
2836 float2double(scaled,scale3,3);
2837 matscale(temp1,scaled[0],scaled[1],scaled[2]);
2838 float2double(rotd,rot4,4);
2839 matrotate(temp2,rotd[3],rotd[0],rotd[1],rotd[2]);
2840 float2double(trand,tr,3);
2841 mattranslate(temp3,trand[0],trand[1],trand[2]);
2842 matmultiplyAFFINE(temp4,temp1,temp2);
2843 matmultiplyAFFINE(Tca,temp4,temp3);
2844 //Tout
2845 matmultiply(Tout, Tca, Tao);
2846
2847 //break up / decompose matrix Tout into translation, rotation, scale
2848 if(1){
2849 //using Graphics Gems IV polar decomposition of affine matrices
2850 // https://webdocs.cs.ualberta.ca/~graphics/books/GraphicsGems/gemsiv/polar_decomp/
2851 HMatrix A;
2852 double ToutTranspose[16];
2853 mattranspose(ToutTranspose,Tout);
2854 double2float(A[0],ToutTranspose,16);
2855 AffineParts parts;
2856 decomp_affine(A, &parts);
2857 veccopy3f(tr,&parts.t.x);
2858 veccopy3f(scale3,&parts.k.x);
2859 Quaternion qq;
2860 Quat q = parts.q;
2861 qq.x = q.x; qq.y = q.y; qq.z = q.z; qq.w = q.w;
2862 quaternion_to_vrmlrot4f(&qq,rot4);
2863 rot4[3] = rot4[3];
2864 if(0){
2865 vecprint3fb("tr_Tout",tr,"\n");
2866 vecprint3fb("sc_Tout",scale3,"\n");
2867 vecprint4fb("rt_Tout",rot4,"\n");
2868 }
2869
2870 }
2871 else if(0){
2872 // using least squares; by transforming 2 arbitrary points using Tout,
2873 // then using least squares to solve for combined 2D similatrity transform param (like we do above)
2874 double d[3];
2875 float p0[9], p1[9];
2876 vecset3f(p0,0.0f,0.0f,0.0f);
2877 vecset3f(&p0[3],1.0f,0.0f,0.0f);
2878 vecset3f(&p0[6],0.0f,1.0f,0.0f);
2879 for(int k=0;k<3;k++){
2880 float2double(d,&p0[k*3],3);
2881 transformAFFINEd(d,d,Tout);
2882 double2float(&p1[k*3],d,3);
2883 }
2884 float param[5];
2885 least_squares_similarity2D_linpack(p0,p1,2,param);
2886 rot4[3] = param[2];
2887 float scalex = param[0];
2888 float scaley = param[1];
2889 //if(1) printf("Tout lsq scale %f angle %f tr %f %f\n",scale,param[1],param[2],param[3]);
2890 vecset3f(scale3,scalex,scaley,1.0f);
2891 veccopy2f(tr,&param[3]);
2892 if(0){
2893 vecprint3fb("tr_Tout",tr,"\n");
2894 vecprint3fb("sc_Tout",scale3,"\n");
2895 vecprint4fb("rt_Tout",rot4,"\n");
2896 }
2897
2898 }
2899 else if(1){
2900 // using direct method; by transforming 2 arbitrary points using Tout,
2901 // then direct metho to solve for combined 2D similatrity transform param (like we do above)
2902 // works a bit
2903 double d[3];
2904 float p0[9], p1[9];
2905 vecset3f(p0,0.0f,0.0f,0.0f);
2906 vecset3f(&p0[3],1.0f,0.0f,0.0f);
2907 vecset3f(&p0[6],0.0f,1.0f,0.0f);
2908 for(int k=0;k<3;k++){
2909 float2double(d,&p0[k*3],3);
2910 transformAFFINEd(d,d,Tout);
2911 double2float(&p1[k*3],d,3);
2912 }
2913 float param[5];
2914 scale_constrained_2D(p0,p1,2,param,node->minScale.c,node->maxScale.c);
2915
2916 rot4[3] = param[2];
2917 float scalex = param[0];
2918 float scaley = param[1];
2919 //if(1) printf("Tout lsq scale %f angle %f tr %f %f\n",scale,param[1],param[2],param[3]);
2920 vecset3f(scale3,scalex,scaley,1.0f);
2921 veccopy2f(tr,&param[3]);
2922 if(0){
2923 vecprint3fb("tr_Tout",tr,"\n");
2924 vecprint3fb("sc_Tout",scale3,"\n");
2925 vecprint4fb("rt_Tout",rot4,"\n");
2926 }
2927
2928 }
2929
2930
2931 }
2932
2933
2934 //translation
2935 //vecadd3f(tr,tr,node->offset.c);
2936 /* clamp translation to max/min position */
2937 vecclamp2f(tr,node->minPosition.c,node->maxPosition.c);
2938 if(!node->sensorLocalOutput){
2939 axisangle_rotate3f(tr,tr, node->axisRotation.c);
2940 }
2941 veccopy3f(node->_oldtranslation.c,tr);
2942
2943 if(!approx3f(node->_oldtranslation.c,node->translation_changed.c)) {
2944 veccopy3f(node->translation_changed.c, (void *) node->_oldtranslation.c);
2945 MARK_EVENT(ptr, offsetof (struct X3D_MultiTouchSensor, translation_changed));
2946 //vecprint3fb("tran_chng ",node->translation_changed.c,"\n");
2947
2948 }
2949
2950 //scale
2951 //vecmult3f(scale3,scale3,node->scaleOffset.c);
2952 /* clamp scale to max/min scale */
2953 vecclamp2f(scale3,node->minScale.c,node->maxScale.c);
2954 if(!node->sensorLocalOutput){
2955 axisangle_rotate3f(scale3,scale3, node->axisRotation.c);
2956 }
2957 veccopy3f(node->_oldscale.c,scale3);
2958
2959 if(!approx3f(node->_oldscale.c,node->scale_changed.c)) {
2960 veccopy3f(node->scale_changed.c, (void *) node->_oldscale.c);
2961 MARK_EVENT(ptr, offsetof (struct X3D_MultiTouchSensor, scale_changed));
2962 //vecprint3fb("sca_chng ",node->scale_changed.c,"\n");
2963 }
2964
2965 //rotation
2966 //axisangle_rotate4f(rot4,rot4,node->rotationOffset.c);
2967 veccopy4f(node->_oldrotation.c,rot4);
2968
2969 if(!approx4f(node->_oldrotation.c,node->rotation_changed.c)) {
2970 veccopy4f(node->rotation_changed.c, (void *) node->_oldrotation.c);
2971 MARK_EVENT(ptr, offsetof (struct X3D_MultiTouchSensor, rotation_changed));
2972 //vecprint4fb("rot_chg",node->rotation_changed.c,"\n");
2973 }
2974
2975 } //if drag_count == orig_count
2976 } else if (ev==ButtonRelease) {
2977
2978 //delete released touch from orig_points
2979 for(int i=0;i<node->_orig_count;i++){
2980 //if(op[i].ID != touchID) mainloop_reset_touch_hyperhit(op[i].ID);
2981
2982 if(op[i].ID == touchID){
2983 for(int j=i+1;j<node->_orig_count;j++)
2984 op[j-1] = op[j];
2985 node->_orig_count--;
2986 //printf("(D %d)",touchID);
2987 break;
2988 }
2989 }
2990 for(int i=0;i<node->_drag_count;i++){
2991 if(dp[i].ID == touchID){
2992 for(int j=i+1;j<node->_drag_count;j++)
2993 dp[j-1] = dp[j];
2994 node->_drag_count--;
2995 //printf("(D %d)",touchID);
2996 break;
2997 }
2998 }
2999 // reset orig_points = drag_points so they are 'starting over'
3000 // (otherwise you'll see a jump as drag averages change wildly)
3001 for(int i=0;i<node->_orig_count;i++){
3002 op[i].reset = TRUE;
3003 //mainloop_reset_touch_hyperhit(op[i].ID);
3004 //printf("(P %d)",op[i].ID);
3005 }
3006 //tg->RenderFuncs.hyperhit = FALSE;
3007
3008 /* set isActive false if no active touches left*/
3009 if(node->_orig_count < 1){
3010 node->isActive=FALSE;
3011 MARK_EVENT (ptr, offsetof (struct X3D_MultiTouchSensor, isActive));
3012 if(node->_lastTao == NULL)
3013 node->_lastTao = malloc(16*sizeof(double));
3014 matidentity4d(node->_lastTao);
3015 }
3016 /* autoOffset? */
3017 if (node->autoOffset) {
3018 veccopy3f(node->offset.c,node->translation_changed.c);
3019 MARK_EVENT (ptr, offsetof (struct X3D_MultiTouchSensor, offset));
3020 veccopy4f(node->rotationOffset.c,node->rotation_changed.c);
3021 MARK_EVENT (ptr, offsetof (struct X3D_MultiTouchSensor, rotationOffset));
3022 veccopy3f(node->scaleOffset.c,node->scale_changed.c);
3023 MARK_EVENT (ptr, offsetof (struct X3D_MultiTouchSensor, scaleOffset));
3024 if(1){
3025 //if have 2 multitouch drags, and lift one, and we write the offsets
3026 // then we need to update the hyperhit matrx for the remaining drag
3027 // so it remains in sync with the offsets
3028 double Tao[16], Tca[16], Tout[16], temp1[16], temp2[16], temp3[16], temp4[16], scaled[3], rotd[4], trand[3], dangle;
3029 //TautoOffset
3030 float2double(scaled,node->scaleOffset.c,3);
3031 matscale(temp1,scaled[0],scaled[1],scaled[2]);
3032 float2double(rotd,node->rotationOffset.c,4);
3033 matrotate(temp2,rotd[3],rotd[0],rotd[1],rotd[2]);
3034 float2double(trand,node->offset.c,3);
3035 mattranslate(temp3,trand[0],trand[1],trand[2]);
3036 matmultiplyAFFINE(temp4,temp1,temp2);
3037 matmultiplyAFFINE(Tao,temp4,temp3);
3038 if(node->_lastTao == NULL){
3039 node->_lastTao = malloc(16*sizeof(double));
3040 matidentity4d(node->_lastTao);
3041 }
3042 double lastTaoInv[16], netTao[16];
3043 matinverseAFFINE(lastTaoInv,node->_lastTao);
3044 matmultiplyAFFINE(netTao,lastTaoInv,Tao);
3045 for(int i=0;i<node->_orig_count;i++){
3046 mainloop_update_touch_hyperhit_matrix(op[i].ID,netTao);
3047 }
3048 memcpy(node->_lastTao,Tao,16*sizeof(double));
3049
3050 }
3051 }
3052 }
3053
3054}
3055//float *extent6f_translate3f(float *eout6, float *ein6, float *p3);
3056//float *extent6f_copy(float *eout6, float *ein6);
3057//static float testextent [] = {.05f, -.05f, .05f, -.05f, .05f, -.05f};
3058//static float testextent2 [] = {.15f, -.15f, .15f, -.15f, .15f, -.15f};
3059//void extent6f_draw(float *extent);
3060void draw_bbox(float *center, float *size);
3061void render_MultiTouchSensor(struct X3D_MultiTouchSensor *node){
3062 // how to 'see' a sensor> how about drawing its touch points in sensor-space?
3063 if(0){
3064 // draw small box for ButtonPress orig
3065 if(1) if(node->_orig_count > 0){
3066 float size[3]; //ee[6];
3067 struct ID_point *op = (struct ID_point*)node->_orig_points;
3068 for(int i=0;i< node->_orig_count; i++){
3069 //extent6f_translate3f(ee,testextent,op[i].p);
3070 //extent6f_draw(ee);
3071 draw_bbox(op[i].p,vecset3f(size,.2f,.2f,.2f));
3072 }
3073 }
3074 // draw bigger box for MotionNotify drag
3075 if(1) if(node->_drag_count > 0){
3076 float size[3]; //ee[6];
3077 struct ID_point *dp = (struct ID_point*)node->_drag_points;
3078 for(int i=0;i< node->_drag_count; i++){
3079 //extent6f_translate3f(ee,testextent2,dp[i].p);
3080 //extent6f_draw(ee);
3081 draw_bbox(dp[i].p,vecset3f(size,.8f,.8f,.2f));
3082 }
3083 }
3084 }
3085}
3086
3087/* void do_Anchor (struct X3D_Anchor *node, int ev, int over) {*/
3088void do_Anchor ( void *ptr, int ev, int but1, int over) {
3089 struct X3D_Anchor *node = (struct X3D_Anchor *)ptr;
3090 UNUSED(over);
3091 UNUSED(but1);
3092
3093 if (!node) return;
3094 /* try button release, so that we dont get worlds flashing past if
3095 the user keeps the finger down. :-) if (ev==ButtonPress) { */
3096 if (ev==ButtonRelease) {
3097 ttglobal tg = gglobal();
3098 /* no parameters in url field? */
3099 if (node->url.n < 1) return;
3100 setAnchorsAnchor( node );
3101 #ifdef OLDCODE
3102 OLDCODE FREE_IF_NZ(tg->RenderFuncs.OSX_replace_world_from_console);
3103 #endif // OLDCODE
3104
3105 tg->RenderFuncs.BrowserAction = TRUE;
3106 }
3107}
3108
3109//double angleAcuteDifferenced(double angle1, double angle2){
3110// //sometimes we cross over the -PI or PI barrier and get a jump
3111// //when really we want the small incremental difference
3112// double angledif = angle2 - angle1;
3113// if(angledif > PI) angledif -= 2*PI;
3114// if(angledif < -PI) angledif += 2*PI;
3115// return angledif;
3116//}
3117//double angleNormalized(double angle){
3118// return atan2(sin(angle),cos(angle));
3119//}
3120
3121void do_CylinderSensor ( void *ptr, int ev, int but1, int over) {
3122 //troubled
3123 struct X3D_CylinderSensor *node = (struct X3D_CylinderSensor *)ptr;
3124 double rot, radius, ang, length;
3125 double det, pos, neg, temp;
3126 double acute_angle, disk_angle, height;
3127 float Y[3] = { 0.0f, 1.0f, 0.0f }, ZERO[3] = { 0.0f, 0.0f, 0.0f };
3128 float aBearing[3], bBearing[3], dirBearing[3], posn[3], axisRotation[4];
3129 Quaternion bv, dir1, dir2, tempV;
3130 GLDOUBLE modelMatrix[16];
3131 ttglobal tg;
3132
3133 UNUSED(over);
3134
3135 /* if not enabled, do nothing */
3136 if (!node) return;
3137 if (node->__oldEnabled != node->enabled) {
3138 node->__oldEnabled = node->enabled;
3139 MARK_EVENT(X3D_NODE(node),offsetof (struct X3D_CylinderSensor, enabled));
3140 }
3141 if (!node->enabled) return;
3142
3143 /* only do something if the button is pressed */
3144 if (!but1) return;
3145 tg = gglobal();
3146
3147 /*precompute some values for mouse-down, mouse-move*/
3148 //convert all almost-sensor-local points into sensor-local
3149 //(the axisRotation never gets applied in the modelview transform stack - if that changes in the future, then don't need these)
3150 veccopy4f(axisRotation,node->axisRotation.c);
3151 axisRotation[3] = -axisRotation[3]; //harmonize rotation with view3dscene
3152 //Dec 2017 view3dscene only other browser that shares our interp of specs on axisRotation for CylinderSensor
3153 //- x3dom and view3dscene share our interpretation of axisRotation for Planesensor
3154 axisangle_rotate3f(aBearing, tg->RenderFuncs.hyp_save_posn, axisRotation);
3155 axisangle_rotate3f(bBearing, tg->RenderFuncs.hyp_save_norm, axisRotation);
3156 vecnormalize3f(dirBearing, vecdif3f(dirBearing, bBearing, aBearing));
3157 axisangle_rotate3f(posn,tg->RenderFuncs.ray_save_posn, axisRotation);
3158
3159 if (ev==ButtonPress) {
3160 /* record the current position from the saved position */
3161 /* on mouse-down we have to decide which sensor geometry to use: disk or cylinder, as per specs
3162 http://www.web3d.org/files/specifications/19775-1/V3.3/Part01/components/pointingsensor.html#CylinderSensor
3163 and that's determined by the angle between the bearing and the sensor Y axis, in sensor-local coords
3164 The bearing (A,B) where A=hyp_posn, B=hyp_norm and both are points in sensor-local coordinates
3165 To get a direction vector v = B - A
3166 */
3167 struct SFColor origPoint;
3168 /*ray_save_posn is the intersection with scene geometry, in sensor-local coordinates, for cylinder*/
3169 float dot, rs[3];
3170
3171 dot = vecdot3f(dirBearing, Y);
3172 dot = fclamp(dot,-1.0f,1.0f);
3173 acute_angle = acos(dot);
3174 ang = min(acute_angle,PI - acute_angle);
3175 //printf("ang= %f\n",(float)ang);
3176 veccopy3f(rs, posn); //posn: ray_posn (intersection with scene geometry) in sensor-local
3177 height = rs[1];
3178 rs[1] = 0.0f;
3179 //radius of ray_posn from cylinder axis,
3180 //for scaling the 'feel' of the rotations to what the user clicked
3181 radius = veclength3f(rs);
3182 vecnormalize3f(rs, rs);
3183 if (ang < node->diskAngle){
3184 //use end cap disks
3185 node->_usingDisk = TRUE;
3186 disk_angle = -atan2(rs[2], rs[0]);
3187 printf("using disk\n");
3188 }else{
3189 //use cylinder wall
3190 node->_usingDisk = FALSE;
3191 //printf("using cylinder\n");
3192 float travelled, cylpoint[3], axispoint[3], dif[3];
3193 line_intersect_line_3f(aBearing, dirBearing, ZERO, Y, NULL, NULL, cylpoint, axispoint);
3194 //travelled: closest distance of our bearing from cylinder axis
3195 travelled = veclength3f(vecdif3f(dif, cylpoint, axispoint));
3196 //which side of cylinder axis is our bearing on?
3197 //v x dif will point a different direction (up or down)
3198 //depending on which side, so dot with Y to get a sign
3199 if (det3f(dirBearing, dif, Y) > 0.0f) travelled = -travelled;
3200 disk_angle = travelled / (2.0f * PI * radius) * (2.0f * PI); //don't need the 2PI except to show how we converted to radians: travelled is a fraction of circumference, and circumference is 2PI
3201 }
3202 node->_radius = (float)radius; //store for later use on mouse-moves
3203 //printf("radius= %f\n",node->_radius);
3204 //origPoint - we get to store whatever we need later mouse-moves.
3205 //GOAL: be able to crank the disk, and keep going around in circles, accumulating angle, like s screw
3206 //printf("disk_angle=%f\n",(float)disk_angle);
3207 node->_origPoint.c[0] = disk_angle;
3208 node->_origPoint.c[1] = -height; //Q. why -height? don't know but it works
3209 //printf("rsp = %f %f %f\n",tg->RenderFuncs.ray_save_posn[0],tg->RenderFuncs.ray_save_posn[1],tg->RenderFuncs.ray_save_posn[2]);
3210 //printf("eqv = %f %f %f\n",cos(-disk_angle)*radius,height,sin(-disk_angle)*radius);
3211 /* set isActive true */
3212 node->isActive=TRUE;
3213 MARK_EVENT (ptr, offsetof (struct X3D_CylinderSensor, isActive));
3214
3215 }
3216 else if (ev == ButtonRelease) {
3217 /* set isActive false */
3218 node->isActive = FALSE;
3219 MARK_EVENT(ptr, offsetof(struct X3D_CylinderSensor, isActive));
3220 /* save auto offset of rotation */
3221 if (node->autoOffset) {
3222 node->offset = node->rotation_changed.c[3];
3223 }
3224 }
3225 if ((ev == MotionNotify || ev == ButtonPress) && (node->isActive)) {
3226 float trackpoint[3], rotation4f[4];
3227 //specs > cylsensor: "trackPoint_changed events represent the unclamped intersection points
3228 // on the surface of the invisible cylinder or disk"
3229 // Q. in sensor-local or sensor?
3230 veccopy3f(trackpoint,node->_oldtrackPoint.c); //a default, some non-junk value
3231
3232
3233 //compute delta rotation from drag
3234 //a plane P dot N = d = const, for any point P on plane. Our plane is in plane-local coords,
3235 // so we could use P={0,0,0} and P dot N = d = 0
3236 float diskpoint[3], orig_diskangle, height;
3237 height = node->_origPoint.c[1];
3238 radius = node->_radius;
3239 orig_diskangle = node->_origPoint.c[0];
3240 if (node->_usingDisk == TRUE) {
3241 //disk
3242 line_intersect_planed_3f(aBearing, dirBearing, Y, height, diskpoint, NULL);
3243 veccopy3f(trackpoint,diskpoint);
3244 vecnormalize3f(diskpoint, diskpoint);
3245 //for cylinder compute angle from intersection on cylinder of radius
3246 disk_angle = -atan2(diskpoint[2], diskpoint[0]);
3247 //printf("D1 %lf ",disk_angle);
3248 }else {
3249 float cylpoint[3]; //pi1[3],
3250 //cylinder wall
3251 //we want a drag off the cylinder to keep working even when mouse isn't over cylinder
3252 //on the cylinder
3253 //basically we try and do a linear drag perpendicular to both our bearing and the cylinder
3254 //axis, and convert that linear distance from cylinder axis from distance into rotations
3255 float travelled, axispoint[3], dif[3];
3256 line_intersect_line_3f(aBearing, dirBearing, ZERO, Y, NULL, NULL, cylpoint, axispoint);
3257 //cylpoint - closest point of approach of our bearing, on the bearing
3258 //axispoint - ditto, on the cyl axis
3259 //dif = cylpoint - axispoint //vector perpendicular to axis - our perpendicular 'travel' from the axis
3260 travelled = veclength3f(vecdif3f(dif, cylpoint, axispoint));
3261 if (det3f(dirBearing, dif, Y) > 0.0f) travelled = -travelled; // v x dif will be up or down the cyl axis, depending on which side of the axis we are on
3262 //convert from linear travel to rotation, using travel/circumference * 2PI
3263 disk_angle = travelled / (2.0f * PI * radius) * (2.0f * PI); //convert from distance to radians using ratio of circumference
3264 if(!line_intersect_cylinder_3f(aBearing,dirBearing,node->_radius,trackpoint))
3265 veccopy3f(trackpoint,cylpoint);
3266
3267 }
3268 rot = disk_angle - orig_diskangle;
3269 //printf(" D2 %lf ",rot);
3270 if (node->autoOffset) {
3271 //printf(" O %f ",node->offset);
3272 rot = node->offset + rot;
3273 }
3274 //printf(" D3 %lf ",rot);
3275 //printf(" N %lf ",rot);
3276 if (node->minAngle < node->maxAngle) {
3277 rot = fclamp(rot,node->minAngle,node->maxAngle);
3278 }
3279 //printf(" D4 %lf \n",rot);
3280
3281 vecset4f(rotation4f,0.0f,1.0f,0.0f,(float)rot);
3282
3283 if(!node->sensorLocalOutput){
3284 //this matches the octaga/instant/h3d technique
3285 axisangle_rotate3f(rotation4f,rotation4f,node->axisRotation.c); //rotate axis only
3286 }
3287 veccopy4f(node->_oldrotation.c,rotation4f);
3288 if(!approx4f(node->_oldrotation.c,node->rotation_changed.c)) {
3289 veccopy4f(node->rotation_changed.c, node->_oldrotation.c);
3290 MARK_EVENT(ptr, offsetof (struct X3D_CylinderSensor, rotation_changed));
3291 }
3292
3293 //the specs don't explicitly say if the trackpoint is in sensor-local (with axisRotation applied)
3294 // or node-local. But it seems easier to understand if in node-local
3295 if(!node->sensorLocalOutput)
3296 axisangle_rotate3f(trackpoint, trackpoint, node->axisRotation.c);
3297 veccopy3f(node->_oldtrackPoint.c,trackpoint);
3298 if(!approx3f(node->_oldtrackPoint.c, node->trackPoint_changed.c) || ev == ButtonPress) {
3299 veccopy3f(node->trackPoint_changed.c, node->_oldtrackPoint.c);
3300 MARK_EVENT(ptr, offsetof(struct X3D_CylinderSensor, trackPoint_changed));
3301 }
3302
3303 }
3304}
3305// see Mainloop.c get_hyperhit() for more explanation:
3306// in sensor-node-local coordinates (not quite sensor-local if sensor node has axisRotation):
3307// ray_save_posn - intersection point of pickray/bearing with sensitized geometry
3308// hyp_save_posn - point on camera/viewpoint nearplane on pickray/bearing, transformed to sensor-node-local
3309// hyp_save_norm - point on carmera/viewpoint farplane on pickray/bearing, transformed to sensor-node-local
3310
3311float fwfdsign(float x){ return x >= 0.0f ? 1.0f : -1.0f; }
3312
3313void do_CylinderSensor_simple ( void *ptr, int ev, int but1, int over) {
3314 //simplest cylinder case, no axisRotation, no disk, derived from SphereSensor
3315 //not used except for understanding
3316 struct X3D_CylinderSensor *node = (struct X3D_CylinderSensor *)ptr;
3317
3318 float *cur, *orig, onorm[3];
3319 ttglobal tg;
3320 UNUSED(over);
3321
3322 /* if not enabled, do nothing */
3323 if (!node)
3324 return;
3325 if (node->__oldEnabled != node->enabled) {
3326 node->__oldEnabled = node->enabled;
3327 MARK_EVENT(X3D_NODE(node),offsetof (struct X3D_CylinderSensor, enabled));
3328 }
3329 if (!node->enabled)
3330 return;
3331
3332 /* only do something if button1 is pressed */
3333 if (!but1) return;
3334 tg = gglobal();
3335
3336 cur = tg->RenderFuncs.ray_save_posn;
3337 orig = node->_origPoint.c;
3338 veccopy3f(onorm,orig);
3339 onorm[1] = 0.0f;
3340 vecnormalize3f(onorm,onorm);
3341 if (ev==ButtonPress) {
3342 /* record the current position from the saved position */
3343 float pcur[3], height;
3344 veccopy3f(orig,cur);
3345
3346 /* record the current Radius */
3347 veccopy3f(pcur,cur);
3348 height = pcur[1];
3349 pcur[1] = 0.0f;
3350 node->_radius = veclength3f(pcur);
3351 if (APPROX(node->_radius,0.0)) {
3352 printf ("warning, RADIUS %lf == 0, can not compute\n",node->_radius);
3353 return;
3354 }
3355
3356 /* save the initial norm here */
3357 //vecscale3f(onorm,cur,1.0f / node->_radius);
3358
3359 /* set isActive true */
3360 node->isActive=TRUE;
3361 MARK_EVENT (ptr, offsetof (struct X3D_CylinderSensor, isActive));
3362 } else if (ev==ButtonRelease) {
3363 /* set isActive false */
3364 node->isActive=FALSE;
3365 MARK_EVENT (ptr, offsetof (struct X3D_CylinderSensor, isActive));
3366
3367 if (node->autoOffset) {
3368 node->offset = node->rotation_changed.c[3];
3369 }
3370 } else if ((ev==MotionNotify) && (node->isActive)) {
3371
3372 float dotProd, sine, angle;
3373 float newRad;
3374 float cnorm[3];
3375 float newAxis[3];
3376 float pcur[3], height;
3377
3378 /* record the current Radius */
3379 height = cur[1];
3380 veccopy3f(pcur,cur);
3381 pcur[1] = 0.0f;
3382 newRad = veclength3f(pcur);
3383 /* bounds check... */
3384 if (APPROX(newRad,0.0)) {
3385 printf ("warning, newRad %lf == 0, can not compute\n",newRad);
3386 return;
3387 }
3388
3389 /* save the current norm here */
3390 //vecscale3f(cnorm,cur,1.0f/newRad);
3391 vecnormalize3f(cnorm,pcur);
3392
3393 /* find the cross-product between the initial and current points */
3394 veccross3f(newAxis,onorm,cnorm);
3395 sine = veclength3f(newAxis);
3396
3397 /* clamp the angle to |a| < 1.0 */
3398 /* remember A dot B = |A|*|B|*cos(theta_between) or theta_between = acos(A dot B/|A|*|B| ) */
3399 //dotProd = NORM_ORIG_X * NORM_CUR_X + NORM_ORIG_Y * NORM_CUR_Y + NORM_ORIG_Z * NORM_CUR_Z;
3400 dotProd = vecdot3f(onorm,cnorm);
3401 dotProd = fclamp(dotProd,-1.0f,1.0f);
3402 angle = acos(dotProd) * fwfdsign(newAxis[1]);
3403
3404 /* have axis-angle now */
3405 /*
3406 printf ("newRotation a %lf - rot -- %lf %lf %lf %lf\n",
3407 dotProd, newA.x,newA.y,newA.z,dotProd);
3408 */
3409 if(node->autoOffset)
3410 {
3411 angle += node->offset;
3412 angle = atan2(sin(angle),cos(angle));
3413 }
3414
3415
3416 //veccopy3f(node->rotation_changed.c,newAxis);
3417 vecset3f(node->rotation_changed.c,0.0f,1.0f,0.0f);
3418 node->rotation_changed.c[3] = angle;
3419 MARK_EVENT (ptr, offsetof (struct X3D_CylinderSensor, rotation_changed));
3420
3421 vecscale3f(node->trackPoint_changed.c,cnorm, node->_radius);
3422 node->trackPoint_changed.c[1] = height;
3423 MARK_EVENT (ptr, offsetof (struct X3D_CylinderSensor, trackPoint_changed));
3424 }
3425}
3426
3427
3428/********************************************************************************/
3429/* */
3430/* do the guts of a SphereSensor.... this has been changed considerably in Apr */
3431/* 2009 because the original, fast methods created by Tuomas Lukka failed in */
3432/* a boundary area (HUD, small transform scale, close to viewer) and I could */
3433/* not understand what *exactly* Tuomas' code did - I guess I don't have a */
3434/* doctorate in math like he does! I went to the old linear algebra text and */
3435/* created a simple but inelegant solution from that. J.A. Stewart. */
3436/* */
3437/********************************************************************************/
3438
3439void do_SphereSensor ( void *ptr, int ev, int but1, int over) {
3440 struct X3D_SphereSensor *node = (struct X3D_SphereSensor *)ptr;
3441
3442 float *cur, *orig, *onorm;
3443 ttglobal tg;
3444 UNUSED(over);
3445
3446 /* if not enabled, do nothing */
3447 if (!node)
3448 return;
3449 if (node->__oldEnabled != node->enabled) {
3450 node->__oldEnabled = node->enabled;
3451 MARK_EVENT(X3D_NODE(node),offsetof (struct X3D_SphereSensor, enabled));
3452 }
3453 if (!node->enabled)
3454 return;
3455
3456 /* only do something if button1 is pressed */
3457 if (!but1) return;
3458 tg = gglobal();
3459
3460 cur = tg->RenderFuncs.ray_save_posn;
3461 orig = node->_origPoint.c;
3462 onorm = node->_origNormalizedPoint.c;
3463 if (ev==ButtonPress) {
3464 /* record the current position from the saved position */
3465 veccopy3f(orig,cur);
3466
3467 /* record the current Radius */
3468 //RADIUS = (float) sqrt(CUR_X * CUR_X + CUR_Y * CUR_Y + CUR_Z * CUR_Z);
3469 node->_radius = veclength3f(cur);
3470 if (APPROX(node->_radius,0.0)) {
3471 printf ("warning, RADIUS %lf == 0, can not compute\n",node->_radius);
3472 return;
3473 }
3474
3475 /* save the initial norm here */
3476 vecscale3f(onorm,cur,1.0f / node->_radius);
3477
3478 /* norm(offset) ideally this would be done once during parsing
3479 of crazy SFRotation ie '1 1 -5 .6' in 10.wrl/10.x3d
3480 I might be getting rounding errors from repeated normalization */
3481 vrmlrot_normalize(node->offset.c);
3482
3483 /* set isActive true */
3484 node->isActive=TRUE;
3485 MARK_EVENT (ptr, offsetof (struct X3D_SphereSensor, isActive));
3486 } else if (ev==ButtonRelease) {
3487 /* set isActive false */
3488 node->isActive=FALSE;
3489 MARK_EVENT (ptr, offsetof (struct X3D_SphereSensor, isActive));
3490
3491 if (node->autoOffset) {
3492 veccopy4f(node->offset.c,node->rotation_changed.c);
3493 }
3494 }
3495 if ((ev==MotionNotify || ev==ButtonPress) && (node->isActive)) {
3496
3497 float dotProd;
3498 float newRad;
3499 float cnorm[3];
3500 float newA[4];
3501
3502 /* record the current Radius */
3503 newRad = veclength3f(cur);
3504 /* bounds check... */
3505 if (APPROX(newRad,0.0)) {
3506 printf ("warning, newRad %lf == 0, can not compute\n",newRad);
3507 return;
3508 }
3509
3510 /* save the current norm here */
3511 vecscale3f(cnorm,cur,1.0f/newRad);
3512
3513 /* find the cross-product between the initial and current points */
3514 veccross3f(newA,orig,cur);
3515 vecnormalize3f(newA,newA);
3516
3517 /* clamp the angle to |a| < 1.0 */
3518 /* remember A dot B = |A|*|B|*cos(theta_between) or theta_between = acos(A dot B/|A|*|B| ) */
3519 //dotProd = NORM_ORIG_X * NORM_CUR_X + NORM_ORIG_Y * NORM_CUR_Y + NORM_ORIG_Z * NORM_CUR_Z;
3520 dotProd = vecdot3f(onorm,cnorm);
3521 if (dotProd > 1.0)
3522 dotProd = 1.0;
3523 if (dotProd < -1.0)
3524 dotProd = -1.0;
3525 dotProd = acos(dotProd);
3526 newA[3] = dotProd;
3527
3528 /* have axis-angle now */
3529 /*
3530 printf ("newRotation a %lf - rot -- %lf %lf %lf %lf\n",
3531 dotProd, newA.x,newA.y,newA.z,dotProd);
3532 */
3533 if(node->autoOffset)
3534 {
3535
3536 /* copied from the javascript SFRotationMultiply */
3537 Quaternion q1, q2, qret;
3538 double newD[4];
3539 /* convert both rotations into quaternions */
3540 vrmlrot_to_quaternion(&q1, (double) newA[0],
3541 (double) newA[1], (double) newA[2], (double) dotProd);
3542 vrmlrot_to_quaternion(&q2, (double) node->offset.c[0],
3543 (double) node->offset.c[1], (double) node->offset.c[2], (double) node->offset.c[3]);
3544 /* multiply them */
3545 quaternion_multiply(&qret,&q1,&q2);
3546 /* and return the resultant, as a vrml rotation */
3547 quaternion_to_vrmlrot(&qret, &newD[0], &newD[1], &newD[2], &newD[3]);
3548 newA[0] = newD[0]; newA[1] = newD[1], newA[2] = newD[2], newA[3] = newD[3];
3549 dotProd = newD[3];
3550 /*}*/
3551 }
3552
3553
3554 veccopy4f(node->rotation_changed.c,newA);
3555 MARK_EVENT (ptr, offsetof (struct X3D_SphereSensor, rotation_changed));
3556
3557 vecscale3f(node->trackPoint_changed.c,cnorm, node->_radius);
3558 MARK_EVENT (ptr, offsetof (struct X3D_SphereSensor, trackPoint_changed));
3559 }
3560}