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"
54}* ppComponent_EnvironSensor;
56static void *Component_EnvironSensor_constructor(){
63void Component_EnvironSensor_init(
struct tComponent_EnvironSensor *t){
66 t->prv = Component_EnvironSensor_constructor();
68 ppComponent_EnvironSensor p = (ppComponent_EnvironSensor)t->prv;
70 p->candoVisibility = TRUE;
75#ifdef VISIBILITYOCCLUSION
87 bbox2extent6f(node->center.c,node->size.c,node->_extent);
88 if(renderstate()->render_geom && fwl_getDrawBoundingBoxes()) {
89 draw_bbox(node->center.c,node->size.c);
92 union_group_extent(node->_extent);
102 static const struct point_XYZ yvec = {.x=0,.y=0.05,.z=0};
103 static const struct point_XYZ zvec = {.x=0,.y=0,.z=-0.05};
104 static const struct point_XYZ zpvec = {.x=0,.y=0,.z=0.05};
105 static const struct point_XYZ orig = {.x=0,.y=0,.z=0};
106 struct point_XYZ t_zvec, t_yvec, t_orig, t_center;
107 GLDOUBLE modelMatrix[16];
108 GLDOUBLE projMatrix[16];
109 GLDOUBLE view2prox[16];
111 if(!((node->enabled)))
return;
121 FW_GL_GETDOUBLEV(GL_MODELVIEW_MATRIX, modelMatrix);
122 FW_GL_GETDOUBLEV(GL_PROJECTION_MATRIX, projMatrix);
123 FW_GLU_UNPROJECT(orig.x,orig.y,orig.z,modelMatrix,projMatrix,viewport,
124 &t_orig.x,&t_orig.y,&t_orig.z);
125 FW_GLU_UNPROJECT(zvec.x,zvec.y,zvec.z,modelMatrix,projMatrix,viewport,
126 &t_zvec.x,&t_zvec.y,&t_zvec.z);
127 FW_GLU_UNPROJECT(yvec.x,yvec.y,yvec.z,modelMatrix,projMatrix,viewport,
128 &t_yvec.x,&t_yvec.y,&t_yvec.z);
129 matinverse(view2prox,modelMatrix);
130 transform(&t_center,&orig, view2prox);
138 cx = t_center.x - ((node->center ).c[0]);
139 cy = t_center.y - ((node->center ).c[1]);
140 cz = t_center.z - ((node->center ).c[2]);
142 if(((node->size).c[0]) == 0 || ((node->size).c[1]) == 0 || ((node->size).c[2]) == 0)
return;
145 vecscale3f(cc,node->size.c,.5);
146 extent6f_constructor(node->_extent,-cc[0],cc[0],-cc[1],cc[1],-cc[2],cc[2]);
148 if(fabs(cx) > ((node->size).c[0])/2 ||
149 fabs(cy) > ((node->size).c[1])/2 ||
150 fabs(cz) > ((node->size).c[2])/2) {
160 ((node->__t1).c[0]) = (
float)t_center.x;
161 ((node->__t1).c[1]) = (
float)t_center.y;
162 ((node->__t1).c[2]) = (
float)t_center.z;
164 VECDIFF(t_zvec,t_orig,dr1r2);
165 VECDIFF(t_yvec,t_orig,dr2r3);
171 len = sqrt(VECSQ(dr1r2)); VECSCALE(dr1r2,1/len);
172 len = sqrt(VECSQ(dr2r3)); VECSCALE(dr2r3,1/len);
188 if(fabs(VECPT(dr1r2, dr2r3)) > 0.001) {
189 printf (
"Sorry, can't handle unevenly scaled ProximitySensors yet :("
190 "dp: %f v: (%f %f %f) (%f %f %f)\n", VECPT(dr1r2, dr2r3),
191 dr1r2.x,dr1r2.y,dr1r2.z,
192 dr2r3.x,dr2r3.y,dr2r3.z
198 if(APPROX(dr1r2.z,1.0)) {
200 ((node->__t2).c[0]) = (
float) 0;
201 ((node->__t2).c[1]) = (
float) 0;
202 ((node->__t2).c[2]) = (
float) 1;
203 ((node->__t2).c[3]) = (
float) atan2(-dr2r3.x,dr2r3.y);
204 }
else if(APPROX(dr2r3.y,1.0)) {
206 ((node->__t2).c[0]) = (
float) 0;
207 ((node->__t2).c[1]) = (
float) 1;
208 ((node->__t2).c[2]) = (
float) 0;
209 ((node->__t2).c[3]) = (
float) atan2(dr1r2.x,dr1r2.z);
218 VECCP(nor1,nor2,ins);
220 len = sqrt(VECSQ(ins)); VECSCALE(ins,1/len);
223 VECCP(dr1r2,ins, nor1);
224 VECCP(zpvec, ins, nor2);
225 len = sqrt(VECSQ(nor1)); VECSCALE(nor1,1/len);
226 len = sqrt(VECSQ(nor2)); VECSCALE(nor2,1/len);
227 VECCP(nor1,nor2,ins);
229 ((node->__t2).c[3]) = (
float) -atan2(sqrt(VECSQ(ins)), VECPT(nor1,nor2));
232 ((node->__t2).c[0]) = (
float) ins.x;
233 ((node->__t2).c[1]) = (
float) ins.y;
234 ((node->__t2).c[2]) = (
float) ins.z;
249void do_ProximitySensorTick(
void *ptr) {
254 if (node->__oldEnabled != node->enabled) {
255 node->__oldEnabled = node->enabled;
258 if (!node->enabled)
return;
263 if (!node->isActive) {
265 printf (
"PROX - initial defaults\n");
268 node->isActive = TRUE;
269 node->enterTime = TickTime();
275 if (memcmp ((
void *) &node->position_changed,(
void *) &node->__t1,
sizeof(
struct SFColor))) {
277 printf (
"PROX - position changed!!! \n");
280 memcpy ((
void *) &node->position_changed,
281 (
void *) &node->__t1,
sizeof(
struct SFColor));
284 if (memcmp ((
void *) &node->orientation_changed, (
void *) &node->__t2,
sizeof(
struct SFRotation))) {
286 printf (
"PROX - orientation changed!!!\n ");
289 memcpy ((
void *) &node->orientation_changed,
290 (
void *) &node->__t2,
sizeof(
struct SFRotation));
295 if (node->isActive) {
297 printf (
"PROX - stopping\n");
300 node->isActive = FALSE;
301 node->exitTime = TickTime();
312void transformMBB(GLDOUBLE *rMBBmin, GLDOUBLE *rMBBmax, GLDOUBLE *matTransform, GLDOUBLE* inMBBmin, GLDOUBLE* inMBBmax);
313int transformMBB4d(GLDOUBLE *rMBBmin, GLDOUBLE *rMBBmax, GLDOUBLE *matTransform, GLDOUBLE* inMBBmin, GLDOUBLE* inMBBmax,
int isAffine);
314int __gluInvertMatrixd(
const GLDOUBLE m[16], GLDOUBLE invOut[16]);
315void __gluMultMatrixVecd(
const GLDOUBLE matrix[16],
const GLDOUBLE in[4], GLDOUBLE out[4]);
318static void twoPoints2RayMatrix(
double *ptnear,
double* ptfar,
double* rayMatrix){
319 double R1[16], R2[16], R3[16], T[16], rayMatrixInverse[16];
326 mattranslate(T,A[0],A[1],A[2]);
329 if(0) printf(
"Cdif %f %f %f\n",C[0],C[1],C[2]);
330 yaw = atan2(C[0],-C[2]);
331 matrixFromAxisAngle4d(R1, -yaw, 0.0, 1.0, 0.0);
332 transformAFFINEd(C,C,R1);
333 if(0) printf(
"Yawed Cdif %f %f %f\n",C[0],C[1],C[2]);
334 pitch = atan2(C[1],-C[2]);
335 if(0) printf(
"atan2 yaw=%f pitch=%f\n",yaw,pitch);
337 if(0) printf(
"[yaw=%f pitch=%f\n",yaw,pitch);
339 matrixFromAxisAngle4d(R1, pitch, 1.0, 0.0, 0.0);
340 if(0) printmatrix2(R1,
"pure R1");
341 matrixFromAxisAngle4d(R2, yaw, 0.0, 1.0, 0.0);
342 if(0) printmatrix2(R2,
"pure R2");
343 matmultiplyAFFINE(R3,R1,R2);
344 if(0) printmatrix2(R3,
"R3=R1*R2");
345 matmultiplyAFFINE(rayMatrixInverse,R3, T);
346 matinverseAFFINE(rayMatrix,rayMatrixInverse);
350static int frustumHitsMBB(
float *extent){
355 GLDOUBLE modelMatrix[16], projectionMatrix[16];
357 GLDOUBLE smin[3], smax[3], shapeMBBmin[3], shapeMBBmax[3];
359 FW_GL_GETDOUBLEV(GL_MODELVIEW_MATRIX, modelMatrix);
360 FW_GL_GETDOUBLEV(GL_PROJECTION_MATRIX, projectionMatrix);
365 shapeMBBmin[i] = extent[i*2 + 1];
366 shapeMBBmax[i] = extent[i*2];
369 transformMBB(smin,smax,modelMatrix,shapeMBBmin,shapeMBBmax);
371 isIn = smin[2] < 0.0;
397 double rayMatrix[16], modelMatrixPlus[16], projInverse[16], nearplane, farplane;
398 double A[4], B[4], a[4], b[4];
411 a[0] = a[1] = b[0] = b[1] = xy;
416 __gluMultMatrixVecd(projInverse, a, A);
417 vecscaled(A,A,1.0/A[3]);
422 __gluMultMatrixVecd(projInverse, b, B);
423 vecscaled(B,B,1.0/B[3]);
426 twoPoints2RayMatrix(A,B,rayMatrix);
428 matmultiplyAFFINE(modelMatrixPlus,modelMatrix,rayMatrix);
431 transformMBB(smin,smax,modelMatrixPlus,shapeMBBmin,shapeMBBmax);
435 isIn = isIn && smax[i] > 0.0;
437 isIn = isIn && smin[i] < 0.0;
442 transformMBB(smin,smax,modelMatrix,shapeMBBmin,shapeMBBmax);
443 isIn = isIn && smin[2] < nearplane;
444 isIn = isIn && smax[2] > farplane;
455 ttglobal tg = gglobal();
461 ppComponent_EnvironSensor p = (ppComponent_EnvironSensor)tg->Component_EnvironSensor.prv;
462 if (!p->candoVisibility)
return;
473 float emin[3], emax[3];
474 vecadd3f(emax, node->center.c, node->size.c);
475 vecdif3f(emin, node->center.c, node->size.c);
478 node->_extent[i*2 + 1] = emin[i];
479 node->_extent[i*2] = emax[i];
481 ihit = frustumHitsMBB(node->_extent);
491#ifdef VISIBILITYOCCLUSION
493 if (tg->Frustum.OccFailed) {
494 p->candoVisibility = FALSE;
495 ConsoleMessage(
"VisibilitySensor: OpenGL on this machine does not support GL_ARB_occlusion_query");
501 if (rs->render_blend) {
504 beginOcclusionQuery(node,renderstate()->render_geom);
508 rendVisibilityBox(node);
514 endOcclusionQuery(node,renderstate()->render_geom);
521#ifdef VISIBILITYOCCLUSION
524#ifdef HAVE_TO_REIMPLEMENT
525 extern GLfloat boxnorms[];
527 float x = ((node->size).c[0])/2;
528 float y = ((node->size).c[1])/2;
529 float z = ((node->size).c[2])/2;
530 float cx = node->center.c[0];
531 float cy = node->center.c[1];
532 float cz = node->center.c[2];
535 if ((x < 0) || (y < 0) || (z < 0))
return;
538 setExtent(cx+x, cx-x, cx+y, cx-y, cx+z, cx-z,X3D_NODE(node));
544 if NODE_NEEDS_COMPILING {
550 if (!node->__points.p) node->__points.p = MALLOC (
struct SFVec3f*,
sizeof(
struct SFVec3f)*(36));
554 pt = (
float *) node->__points.p;
556 #define PTF0 *pt++ = cx+x; *pt++ = cy+y; *pt++ = cz+z;
557 #define PTF1 *pt++ = cx-x; *pt++ = cy+y; *pt++ = cz+z;
558 #define PTF2 *pt++ = cx-x; *pt++ = cy-y; *pt++ = cz+z;
559 #define PTF3 *pt++ = cx+x; *pt++ = cy-y; *pt++ = cz+z;
560 #define PTR0 *pt++ = cx+x; *pt++ = cy+y; *pt++ = cz-z;
561 #define PTR1 *pt++ = cx-x; *pt++ = cy+y; *pt++ = cz-z;
562 #define PTR2 *pt++ = cx-x; *pt++ = cy-y; *pt++ = cz-z;
563 #define PTR3 *pt++ = cx+x; *pt++ = cy-y; *pt++ = cz-z;
566 PTF0 PTF1 PTF2 PTF0 PTF2 PTF3
567 PTR2 PTR1 PTR0 PTR3 PTR2 PTR0
568 PTF0 PTR0 PTR1 PTF0 PTR1 PTF1
569 PTF3 PTF2 PTR2 PTF3 PTR2 PTR3
570 PTF0 PTF3 PTR3 PTF0 PTR3 PTR0
571 PTF1 PTR1 PTR2 PTF1 PTR2 PTF2
576 FW_GL_DEPTHMASK(FALSE);
582 FW_GL_VERTEX_POINTER(3,GL_FLOAT,0,(GLfloat *)node->__points.p);
583 FW_GL_NORMAL_POINTER(GL_FLOAT,0,boxnorms);
586 sendArraysToGPU (GL_TRIANGLES, 0, 36);
587 FW_GL_DEPTHMASK(TRUE);
593void do_VisibilitySensorTick (
void *ptr) {
598 if (node->__oldEnabled != node->enabled) {
599 node->__oldEnabled = node->enabled;
602 if (!node->enabled)
return;
606 printf (
"do_VisibilitySensorTick, samples %d\n",node->__samples);
609 if (node->__Samples > 0) {
610 if (!node->isActive) {
612 printf (
"visibilitysensor - now active\n");
616 node->enterTime = TickTime();
621 if (node->isActive) {
623 printf (
"visibilitysensor - going inactive\n");
627 node->exitTime = TickTime();
665 int overlapMBBs(GLDOUBLE *MBBmin1, GLDOUBLE *MBBmax1, GLDOUBLE *MBBmin2, GLDOUBLE* MBBmax2);
690void do_TransformSensorTick (
void *ptr) {
698 if (node->__oldEnabled != node->enabled) {
699 node->__oldEnabled = node->enabled;
703 if (!node->enabled)
return;
705 if(((node->size).c[0]) <= 0.0f || ((node->size).c[1]) <= 0.0f || ((node->size).c[2]) <= 0.0f)
return;
708 printf (
"do_TransformSensorTick enabled\n");
714 unode = node->targetObject;
719 while((mehit = usehit_next(menode,mehit))){
721 double meinv[16],memin[3],memax[3];
722 float emin[3], emax[3], halfsize[3];
724 matinverseAFFINE(meinv,mehit->mvm);
731 matmultiplyAFFINE(ident,meinv,mehit->mvm);
733 printf(
"inverse check do_TransformSensor\n");
735 for(j=0;j<4;j++) printf(
"%lf ",ident[i*3+j]);
741 vecscale3f(halfsize,node->size.c,.5f);
742 vecadd3f(emax, node->center.c, halfsize);
743 vecdif3f(emin, node->center.c, halfsize);
746 node->_extent[i*2 + 1] = emin[i];
747 node->_extent[i*2] = emax[i];
751 memin[i] = node->_extent[i*2 + 1];
752 memax[i] = node->_extent[i*2];
757 while((uhit = usehit_next(unode,uhit))){
760 double u2me[16], umin[3],umax[3],uumin[3],uumax[3];
761 matmultiplyAFFINE(u2me,uhit->mvm,meinv);
765 umin[i] = unode->_extent[i*2 + 1];
766 umax[i] = unode->_extent[i*2];
768 transformMBB(uumin,uumax,u2me,umin,umax);
770 if( overlapMBBs(memin, memax, uumin, uumax) ){
772 static const struct point_XYZ yvec = {.x=0,.y=0.05,.z=0};
773 static const struct point_XYZ zvec = {.x=0,.y=0,.z=-0.05};
774 static const struct point_XYZ zpvec = {.x=0,.y=0,.z=0.05};
775 static const struct point_XYZ orig = {.x=0,.y=0,.z=0};
782 double t1u[3], t1me[3];
785 if (!node->isActive) {
787 printf (
"transformensor - now active\n");
791 node->enterTime = TickTime();
798 for(i=0;i<3;i++) t1u[i] = (umin[i] + umax[i])*.5;
799 transformAFFINEd(t1me,t1u,u2me);
800 for(i=0;i<3;i++) node->__t1.c[i] = (
float)t1me[i] - node->center.c[i];
801 if (memcmp ((
void *) &node->position_changed,(
void *) &node->__t1,
sizeof(
struct SFColor))) {
803 printf (
"PROX - position changed!!! \n");
806 memcpy ((
void *) &node->position_changed,
807 (
void *) &node->__t1,
sizeof(
struct SFColor));
811 transformAFFINE(&t_yvec,&yvec,u2me);
812 transformAFFINE(&t_zvec,&zvec,u2me);
813 transformAFFINE(&t_orig,&orig,u2me);
814 VECDIFF(t_zvec,t_orig,dr1r2);
815 VECDIFF(t_yvec,t_orig,dr2r3);
821 len = sqrt(VECSQ(dr1r2)); VECSCALE(dr1r2,1/len);
822 len = sqrt(VECSQ(dr2r3)); VECSCALE(dr2r3,1/len);
838 if(fabs(VECPT(dr1r2, dr2r3)) > 0.001) {
839 printf (
"Sorry, can't handle unevenly scaled TransformSensors yet :("
840 "dp: %f v: (%f %f %f) (%f %f %f)\n", VECPT(dr1r2, dr2r3),
841 dr1r2.x,dr1r2.y,dr1r2.z,
842 dr2r3.x,dr2r3.y,dr2r3.z
848 if(APPROX(dr1r2.z,1.0)) {
850 ((node->__t2).c[0]) = (
float) 0;
851 ((node->__t2).c[1]) = (
float) 0;
852 ((node->__t2).c[2]) = (
float) 1;
853 ((node->__t2).c[3]) = (
float) atan2(-dr2r3.x,dr2r3.y);
854 }
else if(APPROX(dr2r3.y,1.0)) {
856 ((node->__t2).c[0]) = (
float) 0;
857 ((node->__t2).c[1]) = (
float) 1;
858 ((node->__t2).c[2]) = (
float) 0;
859 ((node->__t2).c[3]) = (
float) atan2(dr1r2.x,dr1r2.z);
868 VECCP(nor1,nor2,ins);
870 len = sqrt(VECSQ(ins)); VECSCALE(ins,1/len);
873 VECCP(dr1r2,ins, nor1);
874 VECCP(zpvec, ins, nor2);
875 len = sqrt(VECSQ(nor1)); VECSCALE(nor1,1/len);
876 len = sqrt(VECSQ(nor2)); VECSCALE(nor2,1/len);
877 VECCP(nor1,nor2,ins);
879 ((node->__t2).c[3]) = (
float) -atan2(sqrt(VECSQ(ins)), VECPT(nor1,nor2));
882 ((node->__t2).c[0]) = (
float) ins.x;
883 ((node->__t2).c[1]) = (
float) ins.y;
884 ((node->__t2).c[2]) = (
float) ins.z;
887 if (memcmp ((
void *) &node->orientation_changed, (
void *) &node->__t2,
sizeof(
struct SFRotation))) {
889 printf (
"PROX - orientation changed!!!\n ");
892 memcpy ((
void *) &node->orientation_changed,
893 (
void *) &node->__t2,
sizeof(
struct SFRotation));
901 if (node->isActive) {
903 printf (
"transformsensor - going inactive\n");
907 node->exitTime = TickTime();
914 node->targetObject->_renderFlags |= VF_USE;
916 node->_renderFlags |= VF_USE;