37#include <libFreeWRL.h>
40#include "../vrml_parser/Structs.h"
41#include "../input/InputFunctions.h"
42#include "../opengl/LoadTextures.h"
45#include "../main/headers.h"
46#include "../opengl/OpenGL_Utils.h"
47#include "../scenegraph/RenderFuncs.h"
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"
57#include "SensInterps.h"
66void *SensInterps_constructor(){
71void SensInterps_init(
struct tSensInterps *t)
75 t->prv = SensInterps_constructor();
87void do_active_inactive (
111 double ticktime = TickTime();
115 printf (
"is active tick %f startt %f stopt %f\n",
116 TickTime(), *startt, *stopt);
119 if (ticktime > *stopt) {
120 if (*startt >= *stopt) {
128 if (! APPROX(speed, 0)) {
130 if (elapsedTime >= fabs(myDuration/speed) ) {
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);
146 printf (
"stopping case z\n");
159 if (ticktime >= *startt) {
162 if (ticktime >= *stopt) {
167 if (*startt >= *stopt) {
171 if (!(*startt > 0)) *startt = ticktime;
174 }
else if (*startt >= *stopt) {
175 if (*startt > *inittime) {
190 if (!(*startt > 0)) *startt = ticktime;
199int find_key (
int kin,
float frac,
float *keys) {
202 for (counter=1; counter <= kin; counter++) {
203 if (frac <keys[counter]) {
212void do_OintScalar (
void *node) {
222 kvin = px->keyValue.n;
223 kVs = px->keyValue.p;
228 if ((kvin == 0) || (kin == 0)) {
229 px->value_changed = (float) 0.0;
232 if (kin>kvin) kin=kvin;
235 printf (
"ScalarInterpolator, kin %d kvin %d, vc %f\n",kin,kvin,px->value_changed);
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];
245 counter=find_key(kin,(
float)(px->set_fraction),px->key.p);
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]) +
257void do_OintVector(
void* node) {
260 int kin, kvin, ksize;
267 kvin = px->keyValue.n;
269 kVs = px->keyValue.p;
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;
278 if ((kvin == 0) || (kin == 0)) {
279 memset(px->value_changed.p,0,ksize*
sizeof(
float));
282 if (kin > kvin) kin = kvin;
285 printf(
"VectorInterpolator, kin %d kvin %d, vc %f\n", kin, kvin, px->value_changed);
289 if (px->set_fraction <= px->key.p[0]) {
290 memcpy(px->value_changed.p, &kVs[0*ksize], ksize*
sizeof(
float));
292 else if (px->set_fraction >= px->key.p[kin - 1]) {
294 memcpy(px->value_changed.p, &kVs[(kvin - 1) * ksize], ksize *
sizeof(
float));
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];
328void do_CoordinateMorph(
void* node) {
330 int kin, kvin, ksize;
339 kin = px->set_weights.n;
340 kvin = px->keyValue.n;
342 kVs = px->keyValue.p;
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);
351 if ((kvin == 0) || (kin == 0)) {
352 memset(px->value_changed.p, 0, ksize *
sizeof(
struct SFVec3f));
355 if (kin > kvin) kin = kvin;
356 struct SFVec3f* vc = px->value_changed.p;
357 struct SFVec3f* kv = px->keyValue.p;
358 float* wt = px->set_weights.p;
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++)
365 vecscale3f(wtvc, kv[j*ksize +i].c, wt[j]);
366 vecadd3f(vc[i].c, vc[i].c, wtvc);
371void do_NormalMorph(
void* node) {
373 int kin, kvin, ksize;
382 kin = px->set_weights.n;
383 kvin = px->keyValue.n;
385 kVs = px->keyValue.p;
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);
394 if ((kvin == 0) || (kin == 0)) {
395 memset(px->value_changed.p, 0, ksize *
sizeof(
struct SFVec3f));
398 if (kin > kvin) kin = kvin;
399 struct SFVec3f* vc = px->value_changed.p;
400 struct SFVec3f* kv = px->keyValue.p;
401 float* wt = px->set_weights.p;
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++)
408 vecscale3f(wtvc, kv[j * ksize + i].c, wt[j]);
409 vecadd3f(vc[i].c, vc[i].c, wtvc);
411 vecnormalize3f(vc[i].c, vc[i].c);
415void do_OintNormal(
void *node) {
421 int thisone, prevone;
434 printf (
"debugging OintCoord keys %d kv %d vc %d\n",px->keyValue.n, px->key.n,px->value_changed.n);
440 kvin = px->keyValue.n;
441 kVs = px->keyValue.p;
445 if (kpkv != px->value_changed.n) {
447 printf (
"refactor valuechanged array. n %d sizeof p %d\n",
448 kpkv,
sizeof (
struct SFColor) * kpkv);
450 if (px->value_changed.n != 0) {
451 FREE_IF_NZ (px->value_changed.p);
453 px->value_changed.n = kpkv;
454 px->value_changed.p = MALLOC (
struct SFVec3f*,
sizeof (
struct SFVec3f) * kpkv);
458 valchanged = px->value_changed.p;
462 if ((kvin == 0) || (kin == 0)) {
464 printf (
"no keys or keyValues yet\n");
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;
474 if (kin>kvin) kin=kvin;
478 printf (
"debugging, kpkv %d, px->value_changed.n %d\n", kpkv, px->value_changed.n);
479 printf (
"NormalInterpolator, kpkv %d\n",kpkv);
484 if (px->set_fraction <= px->key.p[0]) {
486 printf (
"COINT out1\n");
489 for (indx = 0; indx < kpkv; indx++) {
490 memcpy ((
void *)&valchanged[indx],
491 (
void *)&kVs[indx],
sizeof (
struct SFColor));
494 printf (
"COINT out1 copied\n");
496 }
else if (px->set_fraction >= px->key.p[kin-1]) {
498 printf (
"COINT out2\n");
501 for (indx = 0; indx < kpkv; indx++) {
502 memcpy ((
void *)&valchanged[indx],
503 (
void *)&kVs[kvin-kpkv+indx],
507 printf (
"COINT out2 finished\n");
511 printf (
"COINT out3\n");
516 printf (
"indx=0, kin %d frac %f\n",kin,px->set_fraction);
519 myKey=find_key(kin,(
float)(px->set_fraction),px->key.p);
521 printf (
"working on key %d\n",myKey);
525 interval = (px->set_fraction - px->key.p[myKey-1]) /
526 (px->key.p[myKey] - px->key.p[myKey-1]);
528 for (indx = 0; indx < kpkv; indx++) {
529 thisone = myKey * kpkv + indx;
530 prevone = (myKey-1) * kpkv + indx;
533 if (thisone >= kvin) {
534 printf (
"CoordinateInterpolator error: thisone %d prevone %d indx %d kpkv %d kin %d kvin %d\n",thisone,prevone,
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]);
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]);
549 printf (
"COINT out3 finished\n");
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;
565 printf (
"Done CoordinateInterpolator\n");
570void do_OintCoord(
void *node) {
576 int thisone, prevone;
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);
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);
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);
611 if (px->_CPU_Routes_out == 0) {
613 printf (
"do_OintCoord, no CPU routes out, no need to do this work\n");
622 kvin = px->keyValue.n;
623 kVs = px->keyValue.p;
627 if (kpkv != px->value_changed.n) {
629 printf (
"refactor valuechanged array. n %d sizeof p %d\n",
630 kpkv,
sizeof (
struct SFVec3f) * kpkv);
632 if (px->value_changed.n != 0) {
633 FREE_IF_NZ (px->value_changed.p);
635 px->value_changed.n = kpkv;
636 px->value_changed.p = MALLOC (
struct SFVec3f*,
sizeof (
struct SFVec3f) * kpkv);
640 valchanged = px->value_changed.p;
644 if ((kvin == 0) || (kin == 0)) {
646 printf (
"no keys or keyValues yet\n");
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;
656 if (kin>kvin) kin=kvin;
660 printf (
"debugging, kpkv %d, px->value_changed.n %d\n", kpkv, px->value_changed.n);
661 printf (
"CoordinateInterpolator, kpkv %d\n",kpkv);
666 if (px->set_fraction <= px->key.p[0]) {
668 printf (
"COINT out1\n");
671 for (indx = 0; indx < kpkv; indx++) {
672 memcpy ((
void *)&valchanged[indx],
673 (
void *)&kVs[indx],
sizeof (
struct SFVec3f));
679 printf (
"COINT out1 copied\n");
681 }
else if (px->set_fraction >= px->key.p[kin-1]) {
683 printf (
"COINT out2\n");
686 for (indx = 0; indx < kpkv; indx++) {
687 memcpy ((
void *)&valchanged[indx],
688 (
void *)&kVs[kvin-kpkv+indx],
692 printf (
"COINT out2 finished\n");
696 printf (
"COINT out3\n");
701 printf (
"indx=0, kin %d frac %f\n",kin,px->set_fraction);
704 myKey=find_key(kin,(
float)(px->set_fraction),px->key.p);
706 printf (
"working on key %d\n",myKey);
710 interval = (px->set_fraction - px->key.p[myKey-1]) /
711 (px->key.p[myKey] - px->key.p[myKey-1]);
713 for (indx = 0; indx < kpkv; indx++) {
714 thisone = myKey * kpkv + indx;
715 prevone = (myKey-1) * kpkv + indx;
718 if (thisone >= kvin) {
719 printf (
"CoordinateInterpolator error: thisone %d prevone %d indx %d kpkv %d kin %d kvin %d\n",thisone,prevone,
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]);
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]);
734 printf (
"COINT out3 finished\n");
740 printf (
"Done CoordinateInterpolator\n");
744void do_OintCoord2D(
void *node) {
750 int thisone, prevone;
762 printf (
"debugging OintCoord keys %d kv %d vc %d\n",px->keyValue.n, px->key.n,px->value_changed.n);
768 kvin = px->keyValue.n;
769 kVs = px->keyValue.p;
773 if (kpkv != px->value_changed.n) {
775 printf (
"refactor valuechanged array. n %d sizeof p %d\n",
776 kpkv,
sizeof (
struct SFVec2f) * kpkv);
778 if (px->value_changed.n != 0) {
779 FREE_IF_NZ (px->value_changed.p);
781 px->value_changed.n = kpkv;
782 px->value_changed.p = MALLOC (
struct SFVec2f*,
sizeof (
struct SFVec2f) * kpkv);
786 valchanged = px->value_changed.p;
790 if ((kvin == 0) || (kin == 0)) {
792 printf (
"no keys or keyValues yet\n");
795 for (indx = 0; indx < kpkv; indx++) {
796 valchanged[indx].c[0] = (float) 0.0;
797 valchanged[indx].c[1] = (float) 0.0;
801 if (kin>kvin) kin=kvin;
805 printf (
"debugging, kpkv %d, px->value_changed.n %d\n", kpkv, px->value_changed.n);
806 printf (
"CoordinateInterpolator2D, kpkv %d\n",kpkv);
811 if (px->set_fraction <= px->key.p[0]) {
813 printf (
"COINT out1\n");
816 for (indx = 0; indx < kpkv; indx++) {
817 memcpy ((
void *)&valchanged[indx],
818 (
void *)&kVs[indx],
sizeof (
struct SFVec2f));
823 printf (
"COINT out1 copied\n");
825 }
else if (px->set_fraction >= px->key.p[kin-1]) {
827 printf (
"COINT out2\n");
830 for (indx = 0; indx < kpkv; indx++) {
831 memcpy ((
void *)&valchanged[indx],
832 (
void *)&kVs[kvin-kpkv+indx],
836 printf (
"COINT out2 finished\n");
840 printf (
"COINT out3\n");
845 printf (
"indx=0, kin %d frac %f\n",kin,px->set_fraction);
848 myKey=find_key(kin,(
float)(px->set_fraction),px->key.p);
850 printf (
"working on key %d\n",myKey);
854 interval = (px->set_fraction - px->key.p[myKey-1]) /
855 (px->key.p[myKey] - px->key.p[myKey-1]);
857 for (indx = 0; indx < kpkv; indx++) {
858 thisone = myKey * kpkv + indx;
859 prevone = (myKey-1) * kpkv + indx;
862 if (thisone >= kvin) {
863 printf (
"CoordinateInterpolator2D error: thisone %d prevone %d indx %d kpkv %d kin %d kvin %d\n",thisone,prevone,
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]);
875 printf (
"COINT out3 finished\n");
881 printf (
"Done CoordinateInterpolator2D\n");
885void do_OintPos2D(
void *node) {
892 int kin, kvin, counter, tmp;
901 kvin = px->keyValue.n;
902 kVs = px->keyValue.p;
905 printf(
"do_Oint2: Position interp2D, node %u kin %d kvin %d set_fraction %f\n",
906 node, kin, kvin, px->set_fraction);
910 if ((kvin == 0) || (kin == 0)) {
911 px->value_changed.c[0] = (float) 0.0;
912 px->value_changed.c[1] = (float) 0.0;
915 if (kin>kvin) kin=kvin;
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));
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];
938 printf (
"Pos/Col, new value (%f %f)\n",
939 px->value_changed.c[0],px->value_changed.c[1]);
951void do_ColorInterpolator (
void *node) {
953 int kin, kvin, counter, tmp;
959 kvin = px->keyValue.n;
960 kVs = px->keyValue.p;
966 printf(
"do_ColorInt: Position/Color interp, node %u kin %d kvin %d set_fraction %f\n",
967 node, kin, kvin, px->set_fraction);
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;
978 if (kin>kvin) kin=kvin;
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));
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];
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]);
1002void do_PositionInterpolator (
void *node) {
1004 int kin, kvin, counter, tmp;
1010 kvin = px->keyValue.n;
1011 kVs = px->keyValue.p;
1017 printf(
"do_PositionInt: Position/Vec3f interp, node %u kin %d kvin %d set_fraction %f\n",
1018 node, kin, kvin, px->set_fraction);
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;
1029 if (kin>kvin) kin=kvin;
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));
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];
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]);
1057void do_Oint4 (
void *node) {
1066 Quaternion st, fin,
final;
1071 kin = ((px->key).n);
1072 kvin = ((px->keyValue).n);
1073 kVs = ((px->keyValue).p);
1076 printf (
"starting do_Oint4; keyValue count %d and key count %d\n",
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;
1091 if (kin>kvin) kin=kvin;
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));
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]);
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],
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]);
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;
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]);
1143void do_CollisionTick(
void *ptr) {
1145 if (cx->__hit == 3) {
1147 cx->collideTime = TickTime();
1148 MARK_EVENT (ptr, offsetof(
struct X3D_Collision, collideTime));
1155void do_AudioTick(
void *ptr) {
1158 double pitch, duration;
1163 if (node->__oldEnabled != node->enabled) {
1164 node->__oldEnabled = node->enabled;
1165 MARK_EVENT(X3D_NODE(node), offsetof(
struct X3D_AudioClip, enabled));
1167 if (!node->enabled)
return;
1169 if(node->__inittime == 0.0)
1170 node->__inittime = TickTime();
1172 if(TickTime() < node->startTime) {
1176 oldstatus = node->isActive;
1177 pitch = node->pitch;
1179 if(node->__sourceNumber < 0)
return;
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);
1199 if (oldstatus != node->isActive) {
1201 if (node->isActive == 1) {
1204 node->__lasttime = TickTime();
1205 node->elapsedTime = 0.0;
1207 MARK_EVENT (X3D_NODE(node), offsetof(
struct X3D_AudioClip, 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));
1222 if(node->isActive == 1 && node->isPaused == FALSE) {
1223 double dtime = TickTime();
1224 node->elapsedTime += dtime - node->__lasttime;
1225 node->__lasttime = dtime;
1227 MARK_EVENT (ptr, offsetof(
struct X3D_AudioClip, elapsedTime));
1230void do_BufferAudioSourceTick(
void* ptr) {
1237 if (node->__oldEnabled != node->enabled) {
1238 node->__oldEnabled = node->enabled;
1241 if (!node->enabled)
return;
1243 if (node->__inittime == 0.0)
1244 node->__inittime = TickTime();
1246 if (TickTime() < node->startTime) {
1250 oldstatus = node->isActive;
1252 if (node->__sourceNumber < 0)
return;
1266 &node->isActive, &node->__inittime, &node->startTime,
1267 &node->stopTime, node->loop, 0.0,
1268 0.0, node->elapsedTime);
1270 if (oldstatus != node->isActive) {
1272 if (node->isActive == 1) {
1275 node->__lasttime = TickTime();
1276 node->elapsedTime = 0.0;
1281 if (node->isActive) {
1282 if (node->pauseTime > node->startTime) {
1283 if (node->resumeTime < node->pauseTime && !node->isPaused) {
1284 node->isPaused = TRUE;
1287 else if (node->resumeTime > node->pauseTime && node->isPaused) {
1288 node->isPaused = FALSE;
1289 node->__lasttime = TickTime();
1294 if (node->isActive == 1 && node->isPaused == FALSE) {
1295 double dtime = TickTime();
1296 node->elapsedTime += dtime - node->__lasttime;
1297 node->__lasttime = dtime;
1303void do_OscillatorSourceTick(
void* ptr) {
1305 int oldstatus, ichange;
1312 if (node->__oldEnabled != node->enabled) {
1313 node->__oldEnabled = node->enabled;
1317 if (!node->enabled)
return;
1319 if (node->__inittime == 0.0)
1320 node->__inittime = TickTime();
1322 if (TickTime() < node->startTime) {
1326 oldstatus = node->isActive;
1333 &node->isActive, &node->__inittime, &node->startTime,
1334 &node->stopTime, TRUE, duration,
1335 node->frequency, node->elapsedTime);
1337 if (oldstatus != node->isActive) {
1339 if (node->isActive == 1) {
1342 node->__lasttime = TickTime();
1343 node->elapsedTime = 0.0;
1349 if (node->isActive) {
1350 if (node->pauseTime > node->startTime) {
1351 if (node->resumeTime < node->pauseTime && !node->isPaused) {
1352 node->isPaused = TRUE;
1356 else if (node->resumeTime > node->pauseTime && node->isPaused) {
1357 node->isPaused = FALSE;
1358 node->__lasttime = TickTime();
1364 if (node->isActive == 1 && node->isPaused == FALSE) {
1365 double dtime = TickTime();
1366 node->elapsedTime += dtime - node->__lasttime;
1367 node->__lasttime = dtime;
1372 if (ichange) node->_ichange++;
1378#define LOAD_STABLE 10
1379unsigned char *movietexture_get_frame_by_fraction(
struct X3D_Node* node,
float fraction,
int *width,
int *height,
int *nchan);
1380void do_MovieTextureTick(
void *ptr) {
1397 if (node->__oldEnabled != node->enabled) {
1398 node->__oldEnabled = node->enabled;
1401 if (!node->enabled)
return;
1404 if(node->__init_time == 0.0)
1405 node->__init_time = TickTime();
1407 if(TickTime() < node->startTime) {
1414 duration = node->duration_changed;
1415 speed = node->speed;
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);
1423 if (oldstatus != node->isActive) {
1424 if (node->isActive == 1) {
1427 node->__last_time = TickTime();
1428 node->elapsedTime = 0.0;
1434 if(node->pauseTime > node->startTime){
1435 if( node->resumeTime < node->pauseTime && !node->isPaused){
1436 node->isPaused = TRUE;
1438 }
else if(node->resumeTime > node->pauseTime && node->isPaused){
1439 node->isPaused = FALSE;
1440 node->__last_time = TickTime();
1445 if(node->isActive && node->isPaused == FALSE) {
1446 double dtime = TickTime();
1447 node->elapsedTime += dtime - node->__last_time;
1448 node->__last_time = dtime;
1458 myFrac = node->elapsedTime / duration;
1460 tmpTrunc = (int) myFrac;
1461 frac = (float)myFrac - (
float)tmpTrunc;
1466 }
else if (APPROX(speed, 0.0f)) {
1469 node->__frac = frac;
1471 if(node->loop == FALSE && tmpTrunc > 0)
1472 node->__frac = 1.0f;
1478 if(node->__loadstatus == LOAD_STABLE){
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);
1486 int thisTexture = node->__textureTableIndex;
1487 tti = getTableIndex(thisTexture);
1489 static int once = 0;
1493 tti->channels = nchan;
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;
1502 tti->status = TEX_LOADED;
1503 glBindTexture(GL_TEXTURE_2D,tti->OpenGLTexture);
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);
1511 glTexImage2D(GL_TEXTURE_2D,0,GL_RGBA,width,height,0,GL_RGBA,GL_UNSIGNED_BYTE,texdata);
1513 glTexImage2D(GL_TEXTURE_2D,0,GL_RGB,width,height,0,GL_RGB,GL_UNSIGNED_BYTE,texdata);
1514 glBindTexture(GL_TEXTURE_2D,0);
1530void do_TouchSensor (
void *ptr,
int ev,
int but1,
int over) {
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);
1544 if (but1) printf (
"but1 TRUE ");
else printf (
"but1 FALSE ");
1545 if (over) printf (
"over TRUE ");
else printf (
"over FALSE ");
1552 if (node->__oldEnabled != node->enabled) {
1553 node->__oldEnabled = node->enabled;
1554 MARK_EVENT(X3D_NODE(node),offsetof (
struct X3D_TouchSensor, enabled));
1556 if (!node->enabled)
return;
1559 if ((ev == overMark) && (over != node->isOver)) {
1561 printf (
"TS %u, isOver changed %d\n",node, over);
1563 node->isOver = over;
1569 if (ev == ButtonPress) {
1570 node->isActive=TRUE;
1573 printf (
"touchSens %u, butPress\n",node);
1576 node->touchTime = TickTime();
1579 }
else if (ev == ButtonRelease) {
1581 printf (
"touchSens %u, butRelease\n",node);
1583 node->isActive=FALSE;
1589 veccopy3f(node->_oldhitPoint.c,tg->RenderFuncs.ray_save_posn);
1592 if(!approx3f(node->_oldhitPoint.c,node->hitPoint_changed.c)){
1593 veccopy3f(node->hitPoint_changed.c,node->_oldhitPoint.c);
1598 veccopy3f(normalval,tg->RenderFuncs.hyp_save_norm);
1599 vecnormalize3f(normalval,normalval);
1600 veccopy3f(node->_oldhitNormal.c,normalval);
1603 if(!approx3f(node->_oldhitNormal.c,node->hitNormal_changed.c)) {
1605 veccopy3f(node->hitNormal_changed.c,node->_oldhitNormal.c);
1606 MARK_EVENT(ptr, offsetof (
struct X3D_TouchSensor, hitNormal_changed));
1615void do_LineSensor(
void *ptr,
int ev,
int but1,
int over) {
1627 float trackpoint[3], translation[3], xxx;
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);
1642 if (but1) printf(
"but1 TRUE ");
else printf(
"but1 FALSE ");
1643 if (over) printf(
"over TRUE ");
else printf(
"over FALSE ");
1650 if (node->__oldEnabled != node->enabled) {
1651 node->__oldEnabled = node->enabled;
1652 MARK_EVENT(X3D_NODE(node), offsetof(
struct X3D_LineSensor, enabled));
1654 if (!node->enabled)
return;
1665 float origin [] = { 0.0f, 0.0f, 0.0f };
1666 float footpoint2[3], footpoint1[3], v1[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))
1676 veccopy3f(trackpoint,footpoint2);
1678 if ((ev == ButtonPress) && but1) {
1680#define LINESENSOR_FLOAT_OFFSET 1
1681#ifndef LINESENSOR_FLOAT_OFFSET
1683 veccopy3f(op.c, trackpoint);
1684 memcpy((
void *)&node->_origPoint, (
void *)&op,
sizeof(
struct SFColor));
1687 node->_origPoint.c[0] = xxx;
1689 if (node->autoOffset)
1690 node->_origPoint.c[1] = node->offset;
1692 node->_origPoint.c[1] = 0.0f;
1695 node->isActive = TRUE;
1699 else if (ev == ButtonRelease) {
1701 node->isActive = FALSE;
1705 if (node->autoOffset) {
1706#ifdef LINESENSOR_FLOAT_OFFSET
1707 node->offset = node->_origPoint.c[1];
1709 veccopy3f(node->offset.c, node->translation_changed.c);
1714 if ((ev == MotionNotify || ev == ButtonPress) && (node->isActive) && but1) {
1715 float xxxoffset, xxxorigin;
1718 veccopy3f(node->_oldtrackPoint.c,trackpoint);
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));
1727#ifdef LINESENSOR_FLOAT_OFFSET
1728 xxxoffset = node->offset;
1729 xxxorigin = node->_origPoint.c[0];
1733 xxxoffset = vecdot3f(node->direction.c,node->offset.c);
1734 xxxorigin = vecdot3f(node->direction.c,node->_origPoint.c);
1739 if (node->maxPosition >= node->minPosition) {
1740 if (xxx < node->minPosition) {
1741 xxx = node->minPosition;
1743 else if (xxx > node->maxPosition) {
1744 xxx = node->maxPosition;
1748 vecscale3f(translation, node->direction.c, xxx);
1750#ifndef LINESENSOR_FLOAT_OFFSET
1753 vecscale3f(diroffset, node->direction.c, xxxoffset);
1755 vecdif3f(nondiroffset, node->offset.c, diroffset);
1757 vecadd3f(translation, translation, nondiroffset);
1760 veccopy3f(node->_oldtranslation.c,translation);
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));
1767 node->_origPoint.c[1] = xxx;
1774void do_PointSensor(
void *ptr,
int ev,
int but1,
int over) {
1785 float trackpoint[3], translation[3], *posn, *rposn, *norm;
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);
1798 if (but1) printf(
"but1 TRUE ");
else printf(
"but1 FALSE ");
1799 if (over) printf(
"over TRUE ");
else printf(
"over FALSE ");
1806 if (node->__oldEnabled != node->enabled) {
1807 node->__oldEnabled = node->enabled;
1808 MARK_EVENT(X3D_NODE(node), offsetof(
struct X3D_PointSensor, enabled));
1810 if (!node->enabled)
return;
1815 norm = tg->RenderFuncs.hyp_save_norm;
1816 posn = tg->RenderFuncs.hyp_save_posn;
1817 rposn = tg->RenderFuncs.ray_save_posn;
1819 if ((ev == ButtonPress) && but1) {
1822 float distance = veclength3f(vecdif3f(tt,rposn,norm));
1824 veccopy3f(trackpoint,rposn);
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));
1832 node->isActive = TRUE;
1835 else if ((ev == MotionNotify) && (node->isActive) && but1) {
1847 veccopy3f(trackpoint,rposn);
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));
1855 vecdif3f(v1, norm, posn);
1856 vecnormalize3f(v1, v1);
1861 if (!line_intersect_plane_3f(posn,v1,tg->RenderFuncs.camera_axis,node->_origPoint.c,translation,NULL))
1864 if (node->autoOffset){
1865 vecadd3f(translation,translation,node->offset.c);
1870 vecclamp3f(translation,node->minPosition.c,node->maxPosition.c);
1872 veccopy3f(node->_oldtranslation.c,translation);
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));
1879 else if (ev == ButtonRelease) {
1881 node->isActive = FALSE;
1884 MARK_EVENT(ptr, offsetof(
struct X3D_PointSensor, trackPoint_changed));
1887 if (node->autoOffset) {
1888 veccopy3f(node->offset.c,node->translation_changed.c);
1899int lookup_ID(
struct ID_point* idp,
int n,
int touchID){
1901 for(
int i=0;i<n;i++){
1902 if(idp[i].ID == touchID) {
1909void do_PlaneSensor (
void *ptr,
int ev,
int but1,
int over) {
1911 float mult, nx, ny, trackpoint[3], inverserotation[4], *posn;
1915 struct ID_point drag_point, *dp, *op;
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);
1928 if (but1) ConsoleMessage(
"but1 TRUE ");
else ConsoleMessage(
"but1 FALSE ");
1929 if (over) ConsoleMessage(
"over TRUE ");
else ConsoleMessage(
"over FALSE ");
1930 ConsoleMessage (
"\n");
1936 if (node->__oldEnabled != node->enabled) {
1937 node->__oldEnabled = node->enabled;
1938 MARK_EVENT(X3D_NODE(node),offsetof (
struct X3D_PlaneSensor, enabled));
1940 if (!node->enabled)
return;
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;
1950 op = (
struct ID_point*)node->_orig_point;
1952 touchID = tg->RenderFuncs.touchID;
1958 float N[3] = { 0.0f, 0.0f, 1.0f };
1963 vecnormalize3f(v, vecdif3f(t1, tg->RenderFuncs.hyp_save_norm, tg->RenderFuncs.hyp_save_posn));
1965 axisangle_rotate3f(NS,N, node->axisRotation.c);
1968 posn = tg->RenderFuncs.hyp_save_posn;
1969 if (!line_intersect_planed_3f(posn, v, NS, 0.0f, trackpoint, NULL))
1973 veccopy4f(inverserotation,node->axisRotation.c);
1974 inverserotation[3] = -inverserotation[3];
1975 veccopy3f(dp->p,trackpoint);
1980 if ((ev==ButtonPress) && but1) {
1982 if(touchID == op->ID || op->reset == TRUE ){
1983 veccopy3f(op->p,trackpoint);
1987 node->isActive=TRUE;
1992 else if (ev == ButtonRelease) {
1994 if (touchID == op->ID) {
1996 node->isActive = FALSE;
2000 if (node->autoOffset) {
2001 veccopy3f(node->offset.c, node->translation_changed.c);
2009 if ((ev==MotionNotify || ev==ButtonPress) && (node->isActive) && but1) {
2011 if(dp->ID == op->ID){
2013 if(!node->sensorLocalOutput){
2014 axisangle_rotate3f(trackpoint,trackpoint, inverserotation);
2017 veccopy3f(node->_oldtrackPoint.c, trackpoint);
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));
2026 veccopy3f(op->p,dp->p);
2030 vecdif3f(tr,dp->p,op->p);
2033 vecadd3f(tr,tr,node->offset.c);
2035 vecclamp2f(tr,node->minPosition.c,node->maxPosition.c);
2036 if(!node->sensorLocalOutput){
2037 axisangle_rotate3f(tr,tr, node->axisRotation.c);
2039 veccopy3f(node->_oldtranslation.c,tr);
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));
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++){
2060 for(
int k= 0;k< nca;k++){
2061 r[i*ncb +j] += a[i*nca +k]* b[k*ncb +j];
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];
2073static void saxpy(
int n,
float *sa,
float *sx,
float *sy )
2079 if( (*sa) == 0.0 )
return;
2080 for( i=0; i< n; i++ )
2082 sy[i] = sy[i] + (*sa)*sx[i];
2086static float sdot(
int n,
float *sx,
float *sy )
2093 if( !n )
return( 0.0 );
2094 for( i=0; i< n; i++ )
2096 stemp = stemp + sx[i]*sy[i];
2101static void sscal(
int n,
float *sa,
float *sx )
2107 for( i=0; i< n; i++ )
2109 sx[i] = (*sa)*sx[i];
2113void sprintnorm(
float *N,
float *B,
int n)
2117 printf(
" NE.sz= %ld \n", n );
2118 for( i=0; i< n; i++ )
2121 for(j=0;j<n;j++) printf(
" %4.0lf ", a[j] );
2124 for(j=0;j< n;j++) printf(
" %4.0lf ", B[j] );
2129int spofa(
float *N,
int n) {
2135 float t, s, *a0, *a1;
2139 for( j=0; j<n; j++ )
2145 for( k=0; k<j-1; k++ )
2148 t = a0[k] - sdot( k, a1, a0 );
2154 if( s <= 0.0 )
return info;
2155 a0[j] = (float)sqrt( s );
2161void sposl(
float *N,
int n,
float *B) {
2169 for( k=0; k<n; k++ )
2173 B[k] = ( B[k] - t )/a[k];
2176 for( k=n-1; k>(-1); k-- )
2181 saxpy( k, &t, a, B );
2185void spodi(
float *N,
int n )
2191 for( k=0; k<n; k++ )
2194 a0[k] = 1.0f / a0[k];
2198 for( j=(k+1); j<n; j++ )
2204 saxpy( nk, &t, a0, a1 );
2208 for( j=0L; j<n; j++ )
2212 for( k=0; k<j; k++ )
2217 saxpy( nk, &t, a0, a1 );
2221 sscal( nj, &t, a0 );
2225int least_squares_similarity2D_linpack(
float *v0,
float *v1,
int np,
float *param)
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;
2277 p1 = realloc(p1, n*
sizeof(
float));
2278 a = realloc(a, nu*n*
sizeof(
float));
2279 at = realloc(at, n*nu*
sizeof(
float));
2280 N = realloc(N, nu*nu*
sizeof(
float));
2281 B = realloc(B, nu*
sizeof(
float));
2283 for(
int i=0;i<np;i++){
2285 p1[j+0] = v1[3*i +0];
2286 p1[j+1] = v1[3*i +1];
2290 for(
int i=0;i<n;i++){
2291 printf(
"[ %f ]\n",p1[i]);
2301 for(
int i=0;i < np; i++){
2305 a[ii + 0] = v0[i*3 +0];
2306 a[ii + 1] = -v0[i*3 +1];
2309 a[jj + 0] = v0[i*3 +1];
2310 a[jj + 1] = v0[i*3 +0];
2316 for(
int i=0; i<n; i++){
2318 for(
int j=0; j<nu; j++) printf(
"%f ",a[i*nu +j]);
2326 s_mat_transpose(at,a,n,nu);
2331 for(
int i=0; i< nu;i++){
2333 for(
int j=0; j < n; j++) printf(
"%f ",at[i*n +j]);
2337 s_mat_multiply(N,at,nu,n,a,nu);
2340 for(
int i=0;i<nu;i++){
2342 for(
int j=0;j<nu; j++) printf(
"%f ",N[i*nu +j]);
2346 s_mat_multiply(B,at,nu,n,p1,1);
2349 for(
int i=0;i<nu;i++){
2355 int info = spofa(N,nu);
2357 printf(
"spofa info %d\n",info);
2361 printf(
"N after spofa factirung\n");
2362 for(
int i=0;i<nu;i++){
2364 for(
int j=0;j<nu; j++) printf(
"%f ",N[i*nu +j]);
2371 if(noisy)printf(
"solved a=%f b=%f c=%f d=%f\n",B[0],B[1],B[2],B[3]);
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);
2378 if(noisy)printf(
"translation x= %f y= %f \n",B[2],B[3]);
2382 param[2] = anglerad;
2390int scale_constrained_2D(
float *v00,
float *v11,
int np,
float *param,
float *minScale,
float *maxScale){
2392 float p0[3], p1[3], delta_orig[3], delta_drag[3], angle, drag0[3], drag1[3], scale[2], iso_scale;
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);
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]);
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;
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;
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]));
2421 delta_drag[0] *= 1.0/scale[0];
2422 delta_drag[1] *= 1.0/scale[1];
2423 angle = vecangle2f(delta_orig,delta_drag);
2429 x = (cos(angle)*xx - sin(angle)*yy);
2430 y = (sin(angle)*xx + cos(angle)*yy);
2435 param[0] = scale[0];
2436 param[1] = scale[1];
2438 param[3] = -( xx- p0[0]);
2439 param[4] = -( yy- p0[1]);
2444void mainloop_update_touch_hyperhit_matrix(
int touchID,
double *netTao);
2445void mainloop_reset_touch_hyperhit(
int touchID);
2447#include "Decompose.h"
2448void do_MultiTouchSensor (
void *ptr,
int ev,
int but1,
int over) {
2450 float nx, ny, trackpoint[3], inverserotation[4], *posn;
2452 int tmp, imethod, touchID;
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);
2466 if (but1) ConsoleMessage(
"but1 TRUE ");
else ConsoleMessage(
"but1 FALSE ");
2467 if (over) ConsoleMessage(
"over TRUE ");
else ConsoleMessage(
"over FALSE ");
2468 ConsoleMessage (
"\n");
2474 if (node->__oldEnabled != node->enabled) {
2475 node->__oldEnabled = node->enabled;
2476 MARK_EVENT(X3D_NODE(node),offsetof (
struct X3D_PlaneSensor, enabled));
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));
2487 touchID = tg->RenderFuncs.touchID;
2493 float N[3] = { 0.0f, 0.0f, 1.0f };
2499 if(node->_drag_count > 31)
2501 if(tg->Mainloop.iframe != node->_lastframe){
2502 node->_drag_count = 0;
2503 node->_lastframe = tg->Mainloop.iframe;
2508 vecnormalize3f(v, vecdif3f(t1, tg->RenderFuncs.hyp_save_norm, tg->RenderFuncs.hyp_save_posn));
2510 axisangle_rotate3f(NS,N, node->axisRotation.c);
2513 posn = tg->RenderFuncs.hyp_save_posn;
2515 if (!line_intersect_planed_3f(posn, v, NS, 0.0f, trackpoint, NULL))
2519 veccopy4f(inverserotation,node->axisRotation.c);
2520 inverserotation[3] = -inverserotation[3];
2522 veccopy3f(dp[node->_drag_count].p,trackpoint);
2523 dp[node->_drag_count].ID = touchID;
2524 node->_drag_count++;
2526 if ((ev==ButtonPress) && but1) {
2530 posn = tg->RenderFuncs.hyp_save_posn;
2541 if (node->autoOffset) {
2542 veccopy3f(node->offset.c,node->translation_changed.c);
2544 veccopy4f(node->rotationOffset.c,node->rotation_changed.c);
2546 veccopy3f(node->scaleOffset.c,node->scale_changed.c);
2552 double Tao[16], Tca[16], Tout[16], temp1[16], temp2[16], temp3[16], temp4[16], scaled[3], rotd[4], trand[3], dangle;
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);
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);
2572 memcpy(node->_lastTao,Tao,16*
sizeof(
double));
2575 float2double(dd,trackpoint,3);
2576 transformAFFINEd(dd,dd,netTao);
2577 double2float(trackpoint,dd,3);
2583 veccopy3f(op[node->_orig_count].p,trackpoint);
2584 op[node->_orig_count].ID = touchID;
2585 op[node->_orig_count].reset = FALSE;
2587 node->_orig_count++;
2589 for(
int k=0;k<node->_orig_count;k++){
2596 node->isActive=TRUE;
2599 }
else if ((ev==MotionNotify) && (node->isActive) && but1) {
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};
2609 nx = trackpoint[0]; ny = trackpoint[1];
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]);
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;
2623 for(
int i=0; i<ndrag; 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);
2630 *itouch = dragp->ID;
2638 switch(node->_drag_count){
2643 int j = lookup_ID(op,node->_orig_count,touchID);
2647 veccopy3f(op[j].p,dp[0].p);
2648 op[j].reset = FALSE;
2651 vecdif3f(tr,dp[0].p,op[j].p);
2653 vecprint3fb(
"tr_comp",tr,
"\n");
2654 vecprint3fb(
"sc_comp",scale3,
"\n");
2655 vecprint4fb(
"rt_comp",rot4,
"\n");
2670 int j0 = lookup_ID(op,node->_orig_count,dp[0].ID);
2671 int j1 = lookup_ID(op,node->_orig_count,dp[1].ID);
2673 if(j0 < 0 || j1 < 0){
2674 printf(
"ouch missing a touch point we should have \n");
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;
2683 veccopy3f(op[j0].p,dp[0].p);
2684 op[j0].reset = FALSE;
2688 veccopy3f(op[j1].p,dp[1].p);
2689 op[j1].reset = FALSE;
2693 vecdif3f(dif0,drag0,orig0);
2694 vecdif3f(dif1,drag1,orig1);
2695 vecadd3f(dif,dif0,dif1);
2696 vecscale3f(tr,dif,.5f);
2701 float v0[6], v1[6], param[5];
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]);
2713 least_squares_similarity2D_linpack(v0,v1,np,param);
2715 scale_constrained_2D(v0,v1,np,param,node->minScale.c,node->maxScale.c);
2718 float scalex = param[0];
2719 float scaley = param[1];
2720 vecset3f(scale3,scalex,scaley,1.0f);
2721 veccopy2f(tr,¶m[3]);
2723 vecprint3fb(
"tr_comp",tr,
"\n");
2724 vecprint3fb(
"sc_comp",scale3,
"\n");
2725 vecprint4fb(
"rt_comp",rot4,
"\n");
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];
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]);
2742 matmultiplyAFFINE(temp4,temp1,temp2);
2743 matmultiplyAFFINE(Tca,temp4,temp3);
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");
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");
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);
2777 vecset3f(scale3,scale,scale,1.0f);
2779 vecscale3f(dorig,dorig,scale);
2780 vecscale3f(ddrag,ddrag,scale);
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);
2793 float center0[3], center[3], deltac[3];
2794 vecadd3f(center,drag1,drag0);
2798 vecscale3f(center0,center,.5f*scale);
2800 axisangle_rotate3f(center0,center0,rot4);
2802 vecdif3f(deltac,center,center0);
2803 vecprint3fb(
"celtac",deltac,
"\n");
2804 vecadd3f(tr,tr,deltac);
2818 vecadd3f(tr,tr,node->offset.c);
2819 vecmult3f(scale3,scale3,node->scaleOffset.c);
2820 axisangle_rotate4f(rot4,rot4,node->rotationOffset.c);
2824 double Tao[16], Tca[16], Tout[16], temp1[16], temp2[16], temp3[16], temp4[16], scaled[3], rotd[4], trand[3], dangle;
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);
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);
2845 matmultiply(Tout, Tca, Tao);
2852 double ToutTranspose[16];
2853 mattranspose(ToutTranspose,Tout);
2854 double2float(A[0],ToutTranspose,16);
2856 decomp_affine(A, &parts);
2857 veccopy3f(tr,&parts.t.x);
2858 veccopy3f(scale3,&parts.k.x);
2861 qq.x = q.x; qq.y = q.y; qq.z = q.z; qq.w = q.w;
2862 quaternion_to_vrmlrot4f(&qq,rot4);
2865 vecprint3fb(
"tr_Tout",tr,
"\n");
2866 vecprint3fb(
"sc_Tout",scale3,
"\n");
2867 vecprint4fb(
"rt_Tout",rot4,
"\n");
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);
2885 least_squares_similarity2D_linpack(p0,p1,2,param);
2887 float scalex = param[0];
2888 float scaley = param[1];
2890 vecset3f(scale3,scalex,scaley,1.0f);
2891 veccopy2f(tr,¶m[3]);
2893 vecprint3fb(
"tr_Tout",tr,
"\n");
2894 vecprint3fb(
"sc_Tout",scale3,
"\n");
2895 vecprint4fb(
"rt_Tout",rot4,
"\n");
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);
2914 scale_constrained_2D(p0,p1,2,param,node->minScale.c,node->maxScale.c);
2917 float scalex = param[0];
2918 float scaley = param[1];
2920 vecset3f(scale3,scalex,scaley,1.0f);
2921 veccopy2f(tr,¶m[3]);
2923 vecprint3fb(
"tr_Tout",tr,
"\n");
2924 vecprint3fb(
"sc_Tout",scale3,
"\n");
2925 vecprint4fb(
"rt_Tout",rot4,
"\n");
2937 vecclamp2f(tr,node->minPosition.c,node->maxPosition.c);
2938 if(!node->sensorLocalOutput){
2939 axisangle_rotate3f(tr,tr, node->axisRotation.c);
2941 veccopy3f(node->_oldtranslation.c,tr);
2943 if(!approx3f(node->_oldtranslation.c,node->translation_changed.c)) {
2944 veccopy3f(node->translation_changed.c, (
void *) node->_oldtranslation.c);
2953 vecclamp2f(scale3,node->minScale.c,node->maxScale.c);
2954 if(!node->sensorLocalOutput){
2955 axisangle_rotate3f(scale3,scale3, node->axisRotation.c);
2957 veccopy3f(node->_oldscale.c,scale3);
2959 if(!approx3f(node->_oldscale.c,node->scale_changed.c)) {
2960 veccopy3f(node->scale_changed.c, (
void *) node->_oldscale.c);
2967 veccopy4f(node->_oldrotation.c,rot4);
2969 if(!approx4f(node->_oldrotation.c,node->rotation_changed.c)) {
2970 veccopy4f(node->rotation_changed.c, (
void *) node->_oldrotation.c);
2976 }
else if (ev==ButtonRelease) {
2979 for(
int i=0;i<node->_orig_count;i++){
2982 if(op[i].ID == touchID){
2983 for(
int j=i+1;j<node->_orig_count;j++)
2985 node->_orig_count--;
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++)
2994 node->_drag_count--;
3001 for(
int i=0;i<node->_orig_count;i++){
3009 if(node->_orig_count < 1){
3010 node->isActive=FALSE;
3012 if(node->_lastTao == NULL)
3013 node->_lastTao = malloc(16*
sizeof(
double));
3014 matidentity4d(node->_lastTao);
3017 if (node->autoOffset) {
3018 veccopy3f(node->offset.c,node->translation_changed.c);
3020 veccopy4f(node->rotationOffset.c,node->rotation_changed.c);
3022 veccopy3f(node->scaleOffset.c,node->scale_changed.c);
3028 double Tao[16], Tca[16], Tout[16], temp1[16], temp2[16], temp3[16], temp4[16], scaled[3], rotd[4], trand[3], dangle;
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);
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);
3048 memcpy(node->_lastTao,Tao,16*
sizeof(
double));
3060void draw_bbox(
float *center,
float *size);
3065 if(1)
if(node->_orig_count > 0){
3068 for(
int i=0;i< node->_orig_count; i++){
3071 draw_bbox(op[i].p,vecset3f(size,.2f,.2f,.2f));
3075 if(1)
if(node->_drag_count > 0){
3078 for(
int i=0;i< node->_drag_count; i++){
3081 draw_bbox(dp[i].p,vecset3f(size,.8f,.8f,.2f));
3088void do_Anchor (
void *ptr,
int ev,
int but1,
int over) {
3096 if (ev==ButtonRelease) {
3097 ttglobal tg = gglobal();
3099 if (node->url.n < 1)
return;
3100 setAnchorsAnchor( node );
3102 OLDCODE FREE_IF_NZ(tg->RenderFuncs.OSX_replace_world_from_console);
3105 tg->RenderFuncs.BrowserAction = TRUE;
3121void do_CylinderSensor (
void *ptr,
int ev,
int but1,
int over) {
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];
3137 if (node->__oldEnabled != node->enabled) {
3138 node->__oldEnabled = node->enabled;
3141 if (!node->enabled)
return;
3150 veccopy4f(axisRotation,node->axisRotation.c);
3151 axisRotation[3] = -axisRotation[3];
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);
3159 if (ev==ButtonPress) {
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);
3176 veccopy3f(rs, posn);
3181 radius = veclength3f(rs);
3182 vecnormalize3f(rs, rs);
3183 if (ang < node->diskAngle){
3185 node->_usingDisk = TRUE;
3186 disk_angle = -atan2(rs[2], rs[0]);
3187 printf(
"using disk\n");
3190 node->_usingDisk = FALSE;
3192 float travelled, cylpoint[3], axispoint[3], dif[3];
3193 line_intersect_line_3f(aBearing, dirBearing, ZERO, Y, NULL, NULL, cylpoint, axispoint);
3195 travelled = veclength3f(vecdif3f(dif, cylpoint, axispoint));
3199 if (det3f(dirBearing, dif, Y) > 0.0f) travelled = -travelled;
3200 disk_angle = travelled / (2.0f * PI * radius) * (2.0f * PI);
3202 node->_radius = (float)radius;
3207 node->_origPoint.c[0] = disk_angle;
3208 node->_origPoint.c[1] = -height;
3212 node->isActive=TRUE;
3216 else if (ev == ButtonRelease) {
3218 node->isActive = FALSE;
3221 if (node->autoOffset) {
3222 node->offset = node->rotation_changed.c[3];
3225 if ((ev == MotionNotify || ev == ButtonPress) && (node->isActive)) {
3226 float trackpoint[3], rotation4f[4];
3230 veccopy3f(trackpoint,node->_oldtrackPoint.c);
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) {
3242 line_intersect_planed_3f(aBearing, dirBearing, Y, height, diskpoint, NULL);
3243 veccopy3f(trackpoint,diskpoint);
3244 vecnormalize3f(diskpoint, diskpoint);
3246 disk_angle = -atan2(diskpoint[2], diskpoint[0]);
3255 float travelled, axispoint[3], dif[3];
3256 line_intersect_line_3f(aBearing, dirBearing, ZERO, Y, NULL, NULL, cylpoint, axispoint);
3260 travelled = veclength3f(vecdif3f(dif, cylpoint, axispoint));
3261 if (det3f(dirBearing, dif, Y) > 0.0f) travelled = -travelled;
3263 disk_angle = travelled / (2.0f * PI * radius) * (2.0f * PI);
3264 if(!line_intersect_cylinder_3f(aBearing,dirBearing,node->_radius,trackpoint))
3265 veccopy3f(trackpoint,cylpoint);
3268 rot = disk_angle - orig_diskangle;
3270 if (node->autoOffset) {
3272 rot = node->offset + rot;
3276 if (node->minAngle < node->maxAngle) {
3277 rot = fclamp(rot,node->minAngle,node->maxAngle);
3281 vecset4f(rotation4f,0.0f,1.0f,0.0f,(
float)rot);
3283 if(!node->sensorLocalOutput){
3285 axisangle_rotate3f(rotation4f,rotation4f,node->axisRotation.c);
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);
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);
3311float fwfdsign(
float x){
return x >= 0.0f ? 1.0f : -1.0f; }
3313void do_CylinderSensor_simple (
void *ptr,
int ev,
int but1,
int over) {
3318 float *cur, *orig, onorm[3];
3325 if (node->__oldEnabled != node->enabled) {
3326 node->__oldEnabled = node->enabled;
3336 cur = tg->RenderFuncs.ray_save_posn;
3337 orig = node->_origPoint.c;
3338 veccopy3f(onorm,orig);
3340 vecnormalize3f(onorm,onorm);
3341 if (ev==ButtonPress) {
3343 float pcur[3], height;
3344 veccopy3f(orig,cur);
3347 veccopy3f(pcur,cur);
3350 node->_radius = veclength3f(pcur);
3351 if (APPROX(node->_radius,0.0)) {
3352 printf (
"warning, RADIUS %lf == 0, can not compute\n",node->_radius);
3360 node->isActive=TRUE;
3362 }
else if (ev==ButtonRelease) {
3364 node->isActive=FALSE;
3367 if (node->autoOffset) {
3368 node->offset = node->rotation_changed.c[3];
3370 }
else if ((ev==MotionNotify) && (node->isActive)) {
3372 float dotProd, sine, angle;
3376 float pcur[3], height;
3380 veccopy3f(pcur,cur);
3382 newRad = veclength3f(pcur);
3384 if (APPROX(newRad,0.0)) {
3385 printf (
"warning, newRad %lf == 0, can not compute\n",newRad);
3391 vecnormalize3f(cnorm,pcur);
3394 veccross3f(newAxis,onorm,cnorm);
3395 sine = veclength3f(newAxis);
3400 dotProd = vecdot3f(onorm,cnorm);
3401 dotProd = fclamp(dotProd,-1.0f,1.0f);
3402 angle = acos(dotProd) * fwfdsign(newAxis[1]);
3409 if(node->autoOffset)
3411 angle += node->offset;
3412 angle = atan2(sin(angle),cos(angle));
3417 vecset3f(node->rotation_changed.c,0.0f,1.0f,0.0f);
3418 node->rotation_changed.c[3] = angle;
3421 vecscale3f(node->trackPoint_changed.c,cnorm, node->_radius);
3422 node->trackPoint_changed.c[1] = height;
3439void do_SphereSensor (
void *ptr,
int ev,
int but1,
int over) {
3442 float *cur, *orig, *onorm;
3449 if (node->__oldEnabled != node->enabled) {
3450 node->__oldEnabled = node->enabled;
3460 cur = tg->RenderFuncs.ray_save_posn;
3461 orig = node->_origPoint.c;
3462 onorm = node->_origNormalizedPoint.c;
3463 if (ev==ButtonPress) {
3465 veccopy3f(orig,cur);
3469 node->_radius = veclength3f(cur);
3470 if (APPROX(node->_radius,0.0)) {
3471 printf (
"warning, RADIUS %lf == 0, can not compute\n",node->_radius);
3476 vecscale3f(onorm,cur,1.0f / node->_radius);
3481 vrmlrot_normalize(node->offset.c);
3484 node->isActive=TRUE;
3486 }
else if (ev==ButtonRelease) {
3488 node->isActive=FALSE;
3491 if (node->autoOffset) {
3492 veccopy4f(node->offset.c,node->rotation_changed.c);
3495 if ((ev==MotionNotify || ev==ButtonPress) && (node->isActive)) {
3503 newRad = veclength3f(cur);
3505 if (APPROX(newRad,0.0)) {
3506 printf (
"warning, newRad %lf == 0, can not compute\n",newRad);
3511 vecscale3f(cnorm,cur,1.0f/newRad);
3514 veccross3f(newA,orig,cur);
3515 vecnormalize3f(newA,newA);
3520 dotProd = vecdot3f(onorm,cnorm);
3525 dotProd = acos(dotProd);
3533 if(node->autoOffset)
3537 Quaternion q1, q2, qret;
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]);
3545 quaternion_multiply(&qret,&q1,&q2);
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];
3554 veccopy4f(node->rotation_changed.c,newA);
3557 vecscale3f(node->trackPoint_changed.c,cnorm, node->_radius);