35#include <libFreeWRL.h>
37#include "../vrml_parser/Structs.h"
38#include "../vrml_parser/CRoutes.h"
39#include "../main/headers.h"
41#include "LinearAlgebra.h"
42#include "Component_Geospatial.h"
43#include "../opengl/Frustum.h"
44#include "../opengl/OpenGL_Utils.h"
45#include "../scenegraph/Component_Shape.h"
46#include "../scenegraph/RenderFuncs.h"
56static void *Component_EnvironSensor_constructor(){
63void Component_EnvironSensor_init(
struct tComponent_EnvironSensor *t){
66 t->prv = Component_EnvironSensor_constructor();
70 p->candoVisibility = TRUE;
75#ifdef VISIBILITYOCCLUSION
86 if(renderstate()->render_boxes) extent6f_draw(node->_extent);
96 static const struct point_XYZ yvec = {0,0.05,0};
97 static const struct point_XYZ zvec = {0,0,-0.05};
98 static const struct point_XYZ zpvec = {0,0,0.05};
99 static const struct point_XYZ orig = {0,0,0};
100 struct point_XYZ t_zvec, t_yvec, t_orig, t_center;
101 GLDOUBLE modelMatrix[16];
102 GLDOUBLE projMatrix[16];
103 GLDOUBLE view2prox[16];
105 if(!((node->enabled)))
return;
115 FW_GL_GETDOUBLEV(GL_MODELVIEW_MATRIX, modelMatrix);
116 FW_GL_GETDOUBLEV(GL_PROJECTION_MATRIX, projMatrix);
117 FW_GLU_UNPROJECT(orig.x,orig.y,orig.z,modelMatrix,projMatrix,viewport,
118 &t_orig.x,&t_orig.y,&t_orig.z);
119 FW_GLU_UNPROJECT(zvec.x,zvec.y,zvec.z,modelMatrix,projMatrix,viewport,
120 &t_zvec.x,&t_zvec.y,&t_zvec.z);
121 FW_GLU_UNPROJECT(yvec.x,yvec.y,yvec.z,modelMatrix,projMatrix,viewport,
122 &t_yvec.x,&t_yvec.y,&t_yvec.z);
123 matinverse(view2prox,modelMatrix);
124 transform(&t_center,&orig, view2prox);
132 cx = t_center.x - ((node->center ).c[0]);
133 cy = t_center.y - ((node->center ).c[1]);
134 cz = t_center.z - ((node->center ).c[2]);
136 if(((node->size).c[0]) == 0 || ((node->size).c[1]) == 0 || ((node->size).c[2]) == 0)
return;
139 vecscale3f(cc,node->size.c,.5);
140 extent6f_constructor(node->_extent,-cc[0],cc[0],-cc[1],cc[1],-cc[2],cc[2]);
142 if(fabs(cx) > ((node->size).c[0])/2 ||
143 fabs(cy) > ((node->size).c[1])/2 ||
144 fabs(cz) > ((node->size).c[2])/2) {
154 ((node->__t1).c[0]) = (float)t_center.x;
155 ((node->__t1).c[1]) = (float)t_center.y;
156 ((node->__t1).c[2]) = (float)t_center.z;
158 VECDIFF(t_zvec,t_orig,dr1r2);
159 VECDIFF(t_yvec,t_orig,dr2r3);
165 len = sqrt(VECSQ(dr1r2)); VECSCALE(dr1r2,1/len);
166 len = sqrt(VECSQ(dr2r3)); VECSCALE(dr2r3,1/len);
182 if(fabs(VECPT(dr1r2, dr2r3)) > 0.001) {
183 printf (
"Sorry, can't handle unevenly scaled ProximitySensors yet :("
184 "dp: %f v: (%f %f %f) (%f %f %f)\n", VECPT(dr1r2, dr2r3),
185 dr1r2.x,dr1r2.y,dr1r2.z,
186 dr2r3.x,dr2r3.y,dr2r3.z
192 if(APPROX(dr1r2.z,1.0)) {
194 ((node->__t2).c[0]) = (float) 0;
195 ((node->__t2).c[1]) = (float) 0;
196 ((node->__t2).c[2]) = (float) 1;
197 ((node->__t2).c[3]) = (float) atan2(-dr2r3.x,dr2r3.y);
198 }
else if(APPROX(dr2r3.y,1.0)) {
200 ((node->__t2).c[0]) = (float) 0;
201 ((node->__t2).c[1]) = (float) 1;
202 ((node->__t2).c[2]) = (float) 0;
203 ((node->__t2).c[3]) = (float) atan2(dr1r2.x,dr1r2.z);
212 VECCP(nor1,nor2,ins);
214 len = sqrt(VECSQ(ins)); VECSCALE(ins,1/len);
217 VECCP(dr1r2,ins, nor1);
218 VECCP(zpvec, ins, nor2);
219 len = sqrt(VECSQ(nor1)); VECSCALE(nor1,1/len);
220 len = sqrt(VECSQ(nor2)); VECSCALE(nor2,1/len);
221 VECCP(nor1,nor2,ins);
223 ((node->__t2).c[3]) = (float) -atan2(sqrt(VECSQ(ins)), VECPT(nor1,nor2));
226 ((node->__t2).c[0]) = (float) ins.x;
227 ((node->__t2).c[1]) = (float) ins.y;
228 ((node->__t2).c[2]) = (float) ins.z;
243void do_ProximitySensorTick(
void *ptr) {
248 if (node->__oldEnabled != node->enabled) {
249 node->__oldEnabled = node->enabled;
252 if (!node->enabled)
return;
257 if (!node->isActive) {
259 printf (
"PROX - initial defaults\n");
262 node->isActive = TRUE;
263 node->enterTime = TickTime();
269 if (memcmp ((
void *) &node->position_changed,(
void *) &node->__t1,
sizeof(
struct SFColor))) {
271 printf (
"PROX - position changed!!! \n");
274 memcpy ((
void *) &node->position_changed,
275 (
void *) &node->__t1,
sizeof(
struct SFColor));
278 if (memcmp ((
void *) &node->orientation_changed, (
void *) &node->__t2,
sizeof(
struct SFRotation))) {
280 printf (
"PROX - orientation changed!!!\n ");
283 memcpy ((
void *) &node->orientation_changed,
284 (
void *) &node->__t2,
sizeof(
struct SFRotation));
289 if (node->isActive) {
291 printf (
"PROX - stopping\n");
294 node->isActive = FALSE;
295 node->exitTime = TickTime();
306void transformMBB(GLDOUBLE *rMBBmin, GLDOUBLE *rMBBmax, GLDOUBLE *matTransform, GLDOUBLE* inMBBmin, GLDOUBLE* inMBBmax);
307int transformMBB4d(GLDOUBLE *rMBBmin, GLDOUBLE *rMBBmax, GLDOUBLE *matTransform, GLDOUBLE* inMBBmin, GLDOUBLE* inMBBmax,
int isAffine);
308int __gluInvertMatrixd(
const GLDOUBLE m[16], GLDOUBLE invOut[16]);
309void __gluMultMatrixVecd(
const GLDOUBLE matrix[16],
const GLDOUBLE in[4], GLDOUBLE out[4]);
312static void twoPoints2RayMatrix(
double *ptnear,
double* ptfar,
double* rayMatrix){
313 double R1[16], R2[16], R3[16], T[16], rayMatrixInverse[16];
320 mattranslate(T,A[0],A[1],A[2]);
323 if(0) printf(
"Cdif %f %f %f\n",C[0],C[1],C[2]);
324 yaw = atan2(C[0],-C[2]);
325 matrixFromAxisAngle4d(R1, -yaw, 0.0, 1.0, 0.0);
326 transformAFFINEd(C,C,R1);
327 if(0) printf(
"Yawed Cdif %f %f %f\n",C[0],C[1],C[2]);
328 pitch = atan2(C[1],-C[2]);
329 if(0) printf(
"atan2 yaw=%f pitch=%f\n",yaw,pitch);
331 if(0) printf(
"[yaw=%f pitch=%f\n",yaw,pitch);
333 matrixFromAxisAngle4d(R1, pitch, 1.0, 0.0, 0.0);
334 if(0) printmatrix2(R1,
"pure R1");
335 matrixFromAxisAngle4d(R2, yaw, 0.0, 1.0, 0.0);
336 if(0) printmatrix2(R2,
"pure R2");
337 matmultiplyAFFINE(R3,R1,R2);
338 if(0) printmatrix2(R3,
"R3=R1*R2");
339 matmultiplyAFFINE(rayMatrixInverse,R3, T);
340 matinverseAFFINE(rayMatrix,rayMatrixInverse);
344static int frustumHitsMBB(
float *extent){
349 GLDOUBLE modelMatrix[16], projectionMatrix[16];
351 GLDOUBLE smin[3], smax[3], shapeMBBmin[3], shapeMBBmax[3];
353 FW_GL_GETDOUBLEV(GL_MODELVIEW_MATRIX, modelMatrix);
354 FW_GL_GETDOUBLEV(GL_PROJECTION_MATRIX, projectionMatrix);
359 shapeMBBmin[i] = extent[i*2 + 1];
360 shapeMBBmax[i] = extent[i*2];
363 transformMBB(smin,smax,modelMatrix,shapeMBBmin,shapeMBBmax);
365 isIn = smin[2] < 0.0;
391 double rayMatrix[16], modelMatrixPlus[16], projInverse[16], nearplane, farplane;
392 double A[4], B[4], a[4], b[4];
405 a[0] = a[1] = b[0] = b[1] = xy;
410 __gluMultMatrixVecd(projInverse, a, A);
411 vecscaled(A,A,1.0/A[3]);
416 __gluMultMatrixVecd(projInverse, b, B);
417 vecscaled(B,B,1.0/B[3]);
420 twoPoints2RayMatrix(A,B,rayMatrix);
422 matmultiplyAFFINE(modelMatrixPlus,modelMatrix,rayMatrix);
425 transformMBB(smin,smax,modelMatrixPlus,shapeMBBmin,shapeMBBmax);
429 isIn = isIn && smax[i] > 0.0;
431 isIn = isIn && smin[i] < 0.0;
436 transformMBB(smin,smax,modelMatrix,shapeMBBmin,shapeMBBmax);
437 isIn = isIn && smin[2] < nearplane;
438 isIn = isIn && smax[2] > farplane;
456 if (!p->candoVisibility)
return;
467 float emin[3], emax[3];
468 vecadd3f(emax, node->center.c, node->size.c);
469 vecdif3f(emin, node->center.c, node->size.c);
472 node->_extent[i*2 + 1] = emin[i];
473 node->_extent[i*2] = emax[i];
475 ihit = frustumHitsMBB(node->_extent);
485#ifdef VISIBILITYOCCLUSION
487 if (tg->Frustum.OccFailed) {
488 p->candoVisibility = FALSE;
489 ConsoleMessage(
"VisibilitySensor: OpenGL on this machine does not support GL_ARB_occlusion_query");
496 if (rs->render_blend) {
499 beginOcclusionQuery(node,renderstate()->render_geom);
503 rendVisibilityBox(node);
509 endOcclusionQuery(node,renderstate()->render_geom);
516#ifdef VISIBILITYOCCLUSION
519#ifdef HAVE_TO_REIMPLEMENT
520 extern GLfloat boxnorms[];
522 float x = ((node->size).c[0])/2;
523 float y = ((node->size).c[1])/2;
524 float z = ((node->size).c[2])/2;
525 float cx = node->center.c[0];
526 float cy = node->center.c[1];
527 float cz = node->center.c[2];
530 if ((x < 0) || (y < 0) || (z < 0))
return;
533 setExtent(cx+x, cx-x, cx+y, cx-y, cx+z, cx-z,X3D_NODE(node));
539 if NODE_NEEDS_COMPILING {
545 if (!node->__points.p) node->__points.p = MALLOC (
struct SFVec3f*,
sizeof(
struct SFVec3f)*(36));
549 pt = (
float *) node->__points.p;
551 #define PTF0 *pt++ = cx+x; *pt++ = cy+y; *pt++ = cz+z;
552 #define PTF1 *pt++ = cx-x; *pt++ = cy+y; *pt++ = cz+z;
553 #define PTF2 *pt++ = cx-x; *pt++ = cy-y; *pt++ = cz+z;
554 #define PTF3 *pt++ = cx+x; *pt++ = cy-y; *pt++ = cz+z;
555 #define PTR0 *pt++ = cx+x; *pt++ = cy+y; *pt++ = cz-z;
556 #define PTR1 *pt++ = cx-x; *pt++ = cy+y; *pt++ = cz-z;
557 #define PTR2 *pt++ = cx-x; *pt++ = cy-y; *pt++ = cz-z;
558 #define PTR3 *pt++ = cx+x; *pt++ = cy-y; *pt++ = cz-z;
561 PTF0 PTF1 PTF2 PTF0 PTF2 PTF3
562 PTR2 PTR1 PTR0 PTR3 PTR2 PTR0
563 PTF0 PTR0 PTR1 PTF0 PTR1 PTF1
564 PTF3 PTF2 PTR2 PTF3 PTR2 PTR3
565 PTF0 PTF3 PTR3 PTF0 PTR3 PTR0
566 PTF1 PTR1 PTR2 PTF1 PTR2 PTF2
571 FW_GL_DEPTHMASK(FALSE);
577 FW_GL_VERTEX_POINTER(3,GL_FLOAT,0,(GLfloat *)node->__points.p);
578 FW_GL_NORMAL_POINTER(GL_FLOAT,0,boxnorms);
581 sendArraysToGPU (GL_TRIANGLES, 0, 36);
582 FW_GL_DEPTHMASK(TRUE);
588void do_VisibilitySensorTick (
void *ptr) {
593 if (node->__oldEnabled != node->enabled) {
594 node->__oldEnabled = node->enabled;
597 if (!node->enabled)
return;
601 printf (
"do_VisibilitySensorTick, samples %d\n",node->__samples);
604 if (node->__Samples > 0) {
605 if (!node->isActive) {
607 printf (
"visibilitysensor - now active\n");
611 node->enterTime = TickTime();
616 if (node->isActive) {
618 printf (
"visibilitysensor - going inactive\n");
622 node->exitTime = TickTime();
660 int overlapMBBs(GLDOUBLE *MBBmin1, GLDOUBLE *MBBmax1, GLDOUBLE *MBBmin2, GLDOUBLE* MBBmax2);
685void do_TransformSensorTick (
void *ptr) {
693 if (node->__oldEnabled != node->enabled) {
694 node->__oldEnabled = node->enabled;
698 if (!node->enabled)
return;
700 if(((node->size).c[0]) <= 0.0f || ((node->size).c[1]) <= 0.0f || ((node->size).c[2]) <= 0.0f)
return;
703 printf (
"do_TransformSensorTick enabled\n");
709 unode = node->targetObject;
714 while((mehit = usehit_next(menode,mehit))){
716 double meinv[16],memin[3],memax[3];
717 float emin[3], emax[3], halfsize[3];
719 matinverseAFFINE(meinv,mehit->mvm);
726 matmultiplyAFFINE(ident,meinv,mehit->mvm);
728 printf(
"inverse check do_TransformSensor\n");
730 for(j=0;j<4;j++) printf(
"%lf ",ident[i*3+j]);
736 vecscale3f(halfsize,node->size.c,.5f);
737 vecadd3f(emax, node->center.c, halfsize);
738 vecdif3f(emin, node->center.c, halfsize);
741 node->_extent[i*2 + 1] = emin[i];
742 node->_extent[i*2] = emax[i];
746 memin[i] = node->_extent[i*2 + 1];
747 memax[i] = node->_extent[i*2];
752 while((uhit = usehit_next(unode,uhit))){
755 double u2me[16], umin[3],umax[3],uumin[3],uumax[3];
756 matmultiplyAFFINE(u2me,uhit->mvm,meinv);
760 umin[i] = unode->_extent[i*2 + 1];
761 umax[i] = unode->_extent[i*2];
763 transformMBB(uumin,uumax,u2me,umin,umax);
765 if( overlapMBBs(memin, memax, uumin, uumax) ){
767 static const struct point_XYZ yvec = {0,0.05,0};
768 static const struct point_XYZ zvec = {0,0,-0.05};
769 static const struct point_XYZ zpvec = {0,0,0.05};
770 static const struct point_XYZ orig = {0,0,0};
777 double t1u[3], t1me[3];
780 if (!node->isActive) {
782 printf (
"transformensor - now active\n");
786 node->enterTime = TickTime();
793 for(i=0;i<3;i++) t1u[i] = (umin[i] + umax[i])*.5;
794 transformAFFINEd(t1me,t1u,u2me);
795 for(i=0;i<3;i++) node->__t1.c[i] = (
float)t1me[i] - node->center.c[i];
796 if (memcmp ((
void *) &node->position_changed,(
void *) &node->__t1,
sizeof(
struct SFColor))) {
798 printf (
"PROX - position changed!!! \n");
801 memcpy ((
void *) &node->position_changed,
802 (
void *) &node->__t1,
sizeof(
struct SFColor));
806 transformAFFINE(&t_yvec,&yvec,u2me);
807 transformAFFINE(&t_zvec,&zvec,u2me);
808 transformAFFINE(&t_orig,&orig,u2me);
809 VECDIFF(t_zvec,t_orig,dr1r2);
810 VECDIFF(t_yvec,t_orig,dr2r3);
816 len = sqrt(VECSQ(dr1r2)); VECSCALE(dr1r2,1/len);
817 len = sqrt(VECSQ(dr2r3)); VECSCALE(dr2r3,1/len);
833 if(fabs(VECPT(dr1r2, dr2r3)) > 0.001) {
834 printf (
"Sorry, can't handle unevenly scaled TransformSensors yet :("
835 "dp: %f v: (%f %f %f) (%f %f %f)\n", VECPT(dr1r2, dr2r3),
836 dr1r2.x,dr1r2.y,dr1r2.z,
837 dr2r3.x,dr2r3.y,dr2r3.z
843 if(APPROX(dr1r2.z,1.0)) {
845 ((node->__t2).c[0]) = (float) 0;
846 ((node->__t2).c[1]) = (float) 0;
847 ((node->__t2).c[2]) = (float) 1;
848 ((node->__t2).c[3]) = (float) atan2(-dr2r3.x,dr2r3.y);
849 }
else if(APPROX(dr2r3.y,1.0)) {
851 ((node->__t2).c[0]) = (float) 0;
852 ((node->__t2).c[1]) = (float) 1;
853 ((node->__t2).c[2]) = (float) 0;
854 ((node->__t2).c[3]) = (float) atan2(dr1r2.x,dr1r2.z);
863 VECCP(nor1,nor2,ins);
865 len = sqrt(VECSQ(ins)); VECSCALE(ins,1/len);
868 VECCP(dr1r2,ins, nor1);
869 VECCP(zpvec, ins, nor2);
870 len = sqrt(VECSQ(nor1)); VECSCALE(nor1,1/len);
871 len = sqrt(VECSQ(nor2)); VECSCALE(nor2,1/len);
872 VECCP(nor1,nor2,ins);
874 ((node->__t2).c[3]) = (float) -atan2(sqrt(VECSQ(ins)), VECPT(nor1,nor2));
877 ((node->__t2).c[0]) = (float) ins.x;
878 ((node->__t2).c[1]) = (float) ins.y;
879 ((node->__t2).c[2]) = (float) ins.z;
882 if (memcmp ((
void *) &node->orientation_changed, (
void *) &node->__t2,
sizeof(
struct SFRotation))) {
884 printf (
"PROX - orientation changed!!!\n ");
887 memcpy ((
void *) &node->orientation_changed,
888 (
void *) &node->__t2,
sizeof(
struct SFRotation));
896 if (node->isActive) {
898 printf (
"transformsensor - going inactive\n");
902 node->exitTime = TickTime();
909 node->targetObject->_renderFlags |= VF_USE;
911 node->_renderFlags |= VF_USE;