35#include <libFreeWRL.h>
37#include "../vrml_parser/Structs.h"
38#include "../vrml_parser/CRoutes.h"
39#include "../main/headers.h"
41#include "../world_script/fieldSet.h"
42#include "../x3d_parser/Bindable.h"
44#include "quaternion.h"
46#include "../opengl/Frustum.h"
47#include "../opengl/Material.h"
48#include "../opengl/OpenGL_Utils.h"
49#include "../input/EAIHelpers.h"
52#include "LinearAlgebra.h"
53#include "Component_Shape.h"
54#include "Component_Geospatial.h"
56#include "../scenegraph/RenderFuncs.h"
57#include "../ui/common.h"
87void push_planetId(
int planetId);
88int current_planetId();
95static int USE_GC_FOR_LCS = TRUE;
320 int xtm_northing_first;
321 int utm_northern_hemisphere;
322 int gd_latitude_first;
329int isNodetypeGeospatial(
int nodetype,
int specversion){
332 nodetype == NODE_GeoCoordinate || nodetype == NODE_GeoElevationGrid || nodetype == NODE_GeoLOD
333 || nodetype == NODE_GeoLocation || nodetype == NODE_GeoPositionInterpolator
334 || nodetype == NODE_GeoProximitySensor || nodetype == NODE_GeoTouchSensor
335 || nodetype == NODE_GeoTransform || nodetype == NODE_GeoViewpoint || nodetype == NODE_GeoOrigin ? TRUE : FALSE;
336 if(!iret && specversion > 320) {
338 nodetype == NODE_EspduTransform || nodetype == NODE_ReceiverPdu || nodetype == NODE_SignalPdu
339 || nodetype == NODE_TransmitterPdu ? TRUE : FALSE;
343int isNodeGeospatial(
struct X3D_Node* node){
344 return isNodetypeGeospatial(node->_nodeType, X3D_PROTO(node->_executionContext)->__specversion);
350#define MF_SF_TEMPS struct Multi_Vec3d mIN; struct Multi_Vec3d mOUT; struct Multi_Vec3d gdCoords;
351#define FREE_MF_SF_TEMPS FREE_IF_NZ(gdCoords.p); FREE_IF_NZ(mOUT.p); FREE_IF_NZ(mIN.p);
353#define COMPILE_GEOSYSTEM(me) compile_geoSystem (X3D_NODE(me), me->_nodeType, &me->geoSystem, &me->__geoSystem);
355#define RADIANS_PER_DEGREE (double)0.0174532925199432957692
356#define DEGREES_PER_RADIAN (double)57.2957795130823208768
360#define UTM_SCALE (double)0.9996
361#define UTM_FALSE_EASTING 500000.0
362#define UTM_FALSE_NORTHING 10000000.0
363#define UTM_ZONE_SIZE 6.0
365#define U3TM_SCALE (double)0.9999
366#define U3TM_FALSE_EASTING 0.0
367#define U3TM_FALSE_NORTHING 0.0
368#define U3TM_ZONE_SIZE 3.0
372#define GEOEL_AA_A (double)6377563.396
373#define GEOEL_AA_F (double)299.3249646
374#define GEOEL_AM_A (double)6377340.189
375#define GEOEL_AM_F (double)299.3249646
376#define GEOEL_AN_A (double)6378160
377#define GEOEL_AN_F (double)298.25
378#define GEOEL_BN_A (double)6377483.865
379#define GEOEL_BN_F (double)299.1528128
380#define GEOEL_BR_A (double)6377397.155
381#define GEOEL_BR_F (double)299.1528128
382#define GEOEL_CC_A (double)6378206.4
383#define GEOEL_CC_F (double)294.9786982
384#define GEOEL_CD_A (double)6378249.145
385#define GEOEL_CD_F (double)293.465
386#define GEOEL_EA_A (double)6377276.345
387#define GEOEL_EA_F (double)300.8017
388#define GEOEL_EB_A (double)6377298.556
389#define GEOEL_EB_F (double)300.8017
390#define GEOEL_EC_A (double)6377301.243
391#define GEOEL_EC_F (double)300.8017
392#define GEOEL_ED_A (double)6377295.664
393#define GEOEL_ED_F (double)300.8017
394#define GEOEL_EE_A (double)6377304.063
395#define GEOEL_EE_F (double)300.8017
396#define GEOEL_EF_A (double)6377309.613
397#define GEOEL_EF_F (double)300.8017
398#define GEOEL_FA_A (double)6378155
399#define GEOEL_FA_F (double)298.3
400#define GEOEL_HE_A (double)6378200
401#define GEOEL_HE_F (double)298.3
402#define GEOEL_HO_A (double)6378270
403#define GEOEL_HO_F (double)297
404#define GEOEL_ID_A (double)6378160
405#define GEOEL_ID_F (double)298.247
406#define GEOEL_IN_A (double)6378388
407#define GEOEL_IN_F (double)297
408#define GEOEL_KA_A (double)6378245
409#define GEOEL_KA_F (double)298.3
410#define GEOEL_RF_A (double)6378137
411#define GEOEL_RF_F (double)298.257222101
412#define GEOEL_SA_A (double)6378160
413#define GEOEL_SA_F (double)298.25
414#define GEOEL_WD_A (double)6378135
415#define GEOEL_WD_F (double)298.26
416#define GEOEL_WE_A (double)6378137
417#define GEOEL_WE_F (double)298.257223563
421#define ELLIPSOIDB(typ) \
422 case typ: *semimajor = typ##_A; *flattening = 1.0/typ##_F; break;
425int nextra_ellipsoid = 1;
427int getEllipsoidParams(
int etype,
double *semimajor,
double *flattening){
430 *semimajor = *flattening = 0.0;
433 *semimajor = extra_ellipsoid[-etype].a;
434 *flattening = extra_ellipsoid[-etype].f;
460 default: printf (
"unknown ellipsoid type: %s\n", stringGEOSPATIALType(etype));
463 if(*semimajor > 0.0) iret = 1;
469#define INITIALIZE_GEOSPATIAL(me) \
470 initializeGeospatial((struct X3D_GeoOrigin **) &me->geoOrigin);
473void CONVERT_BACK_TO_GD_OR_UTMB(Geosys *targetGeoSystem,
struct X3D_Node *GeoOrigin,
477static void gccToGdc (Geosys *geoSystem,
struct SFVec3d *gcc,
struct SFVec3d *gdc);
478void calculateViewingSpeed(
void);
480void pop_group_visible();
490void clear_planets(Stack *planet_stack){
495 for(i=0;i<vectorSize(planet_stack);i++){
496 planet = vector_get_ptr(
struct Planet,planet_stack,i);
497 if(planet && planet->gegs) deleteVector(
struct X3D_Node *, planet->gegs);
499 deleteVector(
struct Planet,planet_stack);
510 Stack *current_planet_stack;
515}* ppComponent_Geospatial;
516void *Component_Geospatial_constructor(){
521void Component_Geospatial_init(
struct tComponent_Geospatial *t){
524 t->prv = Component_Geospatial_constructor();
527 ppComponent_Geospatial p = (ppComponent_Geospatial)t->prv;
532 memset(&planet,0,
sizeof(
struct Planet));
533 vecset4d(planet.autoOrient.c,0.0,0.0,1.0,0.0);
534 vecsetd(planet.autoOrigin.c,0.0,0.0,0.0);
535 planet.autoOriginSet = USE_GC_FOR_LCS;
537 p->planet_stack = newStack(
struct Planet);
538 stack_push(
struct Planet,p->planet_stack,planet);
541 p->current_planet_stack = newStack(
int);
542 stack_push(
int,p->current_planet_stack,0);
543 memset(p->gcgdpars,0,50*
sizeof(
void*));
545 memset(p->fgeopars,0,50*
sizeof(
void*));
550void Component_Geospatial_clear(
struct tComponent_Geospatial *t){
553 ppComponent_Geospatial p = (ppComponent_Geospatial)t->prv;
555 clear_planets(p->planet_stack);
556 p->planet_stack = NULL;
558 if(p->current_planet_stack){
559 FREE_IF_NZ(p->current_planet_stack->data);
560 FREE_IF_NZ(p->current_planet_stack);
561 p->current_planet_stack = NULL;
567void push_planetId(
int planetId){
568 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
569 Stack *current_planet_stack = (Stack*)p->current_planet_stack;
570 stack_push(
int,current_planet_stack,planetId);
572int current_planetId(){
573 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
574 Stack *current_planet_stack = (Stack*)p->current_planet_stack;
575 return stack_top(
int,current_planet_stack);
578 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
579 Stack *current_planet_stack = (Stack*)p->current_planet_stack;
580 stack_pop(
int,current_planet_stack);
582struct Planet *add_planet(
int planetId){
583 struct Planet planet, *ppointer;
584 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
585 memset(&planet,0,
sizeof(
struct Planet));
586 vecset4d(planet.autoOrient.c,0.0,0.0,1.0,0.0);
587 vecsetd(planet.autoOrigin.c,0.0,0.0,0.0);
588 planet.ID = planetId;
589 planet.autoOriginSet = USE_GC_FOR_LCS;
590 stack_push(
struct Planet,p->planet_stack,planet);
591 ppointer = vector_get_ptr(
struct Planet,p->planet_stack,p->planet_stack->n-1);
594struct Planet *current_planet(){
597 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
598 planetId = stack_top(
int,p->current_planet_stack);
601 for(i=0;i<vectorSize(p->planet_stack);i++){
602 planet = vector_get_ptr(
struct Planet,p->planet_stack,i);
603 if(planetId == planet->ID){
614{13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13},
615{3,1,-2,-3,-3,-3,-1,3,1,5,9,11,19,27,31,34,33,34,33,34,28,23,17,13,9,4,4,1,-2,-2,0,2,3,2,1,1},
616{2,2,1,-1,-3,-7,-14,-24,-27,-25,-19,3,24,37,47,60,61,58,51,43,29,20,12,5,-2,-10,-14,-12,-10,-14,-12,-6,-2,3,6,4},
617{2,9,17,10,13,1,-14,-30,-39,-46,-42,-21,6,29,49,65,60,57,47,41,21,18,14,7,-3,-22,-29,-32,-32,-26,-15,-2,13,17,19,6},
618{-8,8,8,1,-11,-19,-16,-18,-22,-35,-40,-26,-12,24,45,63,62,59,47,48,42,28,12,-10,-19,-33,-43,-42,-43,-29,-2,17,23,22,6,2},
619{-12,-10,-13,-20,-31,-34,-21,-16,-26,-34,-33,-35,-26,2,33,59,52,51,52,48,35,40,33,-9,-28,-39,-48,-59,-50,-28,3,23,37,18,-1,-11},
620{-7,-5,-8,-15,-28,-40,-42,-29,-22,-26,-32,-51,-40,-17,17,31,34,44,36,28,29,17,12,-20,-15,-40,-33,-34,-34,-28,7,29,43,20,4,-6},
621{5,10,7,-7,-23,-39,-47,-34,-9,-10,-20,-45,-48,-32,-9,17,25,31,31,26,15,6,1,-29,-44,-61,-67,-59,-36,-11,21,39,49,39,22,10},
622{13,12,11,2,-11,-28,-38,-29,-10,3,1,-11,-41,-42,-16,3,17,33,22,23,2,-3,-7,-36,-59,-90,-95,-63,-24,12,53,60,58,46,36,26},
623{22,16,17,13,1,-12,-23,-20,-14,-3,14,10,-15,-27,-18,3,12,20,18,12,-13,-9,-28,-49,-62,-89,-102,-63,-9,33,58,73,74,63,50,32},
624{36,22,11,6,-1,-8,-10,-8,-11,-9,1,32,4,-18,-13,-9,4,14,12,13,-2,-14,-25,-32,-38,-60,-75,-63,-26,0,35,52,68,76,64,52},
625{51,27,10,0,-9,-11,-5,-2,-3,-1,9,35,20,-5,-6,-5,0,13,17,23,21,8,-9,-10,-11,-20,-40,-47,-45,-25,5,23,45,58,57,63},
626{46,22,5,-2,-8,-13,-10,-7,-4,1,9,32,16,4,-8,4,12,15,22,27,34,29,14,15,15,7,-9,-25,-37,-39,-23,-14,15,33,34,45},
627{21,6,1,-7,-12,-12,-12,-10,-7,-1,8,23,15,-2,-6,6,21,24,18,26,31,33,39,41,30,24,13,-2,-20,-32,-33,-27,-14,-2,5,20},
628{-15,-18,-18,-16,-17,-15,-10,-10,-8,-2,6,14,13,3,3,10,20,27,25,26,34,39,45,45,38,39,28,13,-1,-15,-22,-22,-18,-15,-14,-10},
629{-45,-43,-37,-32,-30,-26,-23,-22,-16,-10,-2,10,20,20,21,24,22,17,16,19,25,30,35,35,33,30,27,10,-2,-14,-23,-30,-33,-29,-35,-43},
630{-61,-60,-61,-55,-49,-44,-38,-31,-25,-16,-6,1,4,5,4,2,6,12,16,16,17,21,20,26,26,22,16,10,-1,-16,-29,-36,-46,-55,-54,-59},
631{-53,-54,-55,-52,-48,-42,-38,-38,-29,-26,-26,-24,-23,-21,-19,-16,-12,-8,-4,-1,1,4,4,6,5,4,2,-6,-15,-24,-33,-40,-48,-50,-53,-52},
632{-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30}
635static double geoidCorrection(
double latitudeDeg,
double longitudeDeg)
646 int il, ip, il1, ip1;
647 double dl, dp, d00, d01, d10, d11, d;
649 il = (int)(longitudeDeg/10.0) + 18 -1;
650 ip = 18 - ((int)(latitudeDeg/10.0) + 9);
653 if(il1 > 35) il1 = 0;
656 d00 = (float)geoid[ip][il];
657 d01 = (float)geoid[ip1][il];
658 d10 = (float)geoid[ip][il1];
659 d11 = (float)geoid[ip1][il1];
661 dl = longitudeDeg - (-180.0 + (float)(il)*10.0);
662 dp = latitudeDeg - (90.0 - (float)(ip)*10.0);
667 d = (1.0 - dl)*(1.0 - dp)*d00 + (dl)*(1.0 - dp)*d10 + (1.0-dl)*(dp)*d01 + (dl)*(dp)*d11;
677static void Gd_Gc3d_fw(Geosys *geoSystem,
struct SFVec3d *inc,
int n,
struct SFVec3d *outc) {
678 int geotype, lat_first, geoid;
679 double radius, flattening;
680 geotype = geoSystem->ellipsoid;
681 lat_first = geoSystem->gd_latitude_first;
682 geoid = geoSystem->geoid_height;
683 if(getEllipsoidParams(geotype,&radius,&flattening))
687 double A2 = radius*radius;
688 double F = flattening;
689 double C = A*((double)1.0 - F);
691 double Eps2 = F*((double)2.0 - F);
692 double Eps25 = (double) 0.25 * Eps2;
707 printf (
"Gd_Gc, NOT lat first\n");
708 latitude = 1; longitude = 0;
718 printf (
"Gd_Gc, have n of %d\n",inc->n);
721 for (i=0; i<n; i++) {
723 printf (
"Gd_Gc, ining lat %lf long %lf ele %lf ",LATITUDE_IN, LONGITUDE_IN, ELEVATION_IN);
726 if(geoSystem->gd_degrees == FALSE){
728 source_lat = inc[i].c[latitude];
729 source_lon = inc[i].c[longitude];
732 source_lat = RADIANS_PER_DEGREE * inc[i].c[latitude];
733 source_lon = RADIANS_PER_DEGREE * inc[i].c[longitude];
737 printf (
"Source Latitude %lf Source Longitude %lf\n",source_lat, source_lon);
740 slat = sin(source_lat);
742 clat = cos(source_lat);
745 printf (
"slat %lf slat2 %lf clat %lf\n",slat, slat2, clat);
750 Rn = A / ( (.25 - Eps25 * slat2 + .9999944354799/4) + (.25-Eps25 * slat2)/(.25 - Eps25 * slat2 + .9999944354799/4));
752 RnPh = Rn + inc[i].c[elevation];
756 double dlatin, dlongin;
757 dlatin = inc[i].c[latitude];
758 dlongin = inc[i].c[longitude];
759 if(geoSystem->gd_degrees == FALSE){
760 dlatin *= DEGREES_PER_RADIAN;
761 dlongin *= DEGREES_PER_RADIAN;
763 RnPh += geoidCorrection(dlatin,dlongin);
766 printf (
"Rn %lf RnPh %lf\n",Rn, RnPh);
769 outc[i].c[0] = RnPh * clat * cos(source_lon);
770 outc[i].c[1] = RnPh * clat * sin(source_lon);
771 outc[i].c[2] = ((C2 / A2) * Rn + inc[i].c[elevation]) * slat;
774 printf (
"Gd_Gc, outing x %lf y %lf z %lf\n", GC_X_OUT, GC_Y_OUT, GC_Z_OUT);
780double dclamp(
double fval,
double fstart,
double fend);
781static void* fwgeo_gc[50];
782static void Gd_Gc3d_geolib(Geosys *geoSystem,
struct SFVec3d *inc,
int n,
struct SFVec3d *outc){
784 double semimajor, flattening, gd[3], gc[3];
786 getEllipsoidParams(geoSystem->ellipsoid,&semimajor,&flattening);
787 if(FALSE && flattening == 0.0){
791 veccopyd(gd,inc[i].c);
792 if(!geoSystem->gd_latitude_first) vecswizzle2d(gd);
793 if(geoSystem->gd_degrees) vecscale2d(gd,gd,RADIANS_PER_DEGREE);
794 radius = semimajor + gd[2];
795 gc[0] = cos(gd[0])*cos(gd[1])*radius;
796 gc[1] = cos(gd[0])*sin(gd[1])*radius;
797 gc[2] = sin(gd[0])*radius;
798 veccopyd(outc[i].c,gc);
804 geotype = geoSystem->ellipsoid;
805 if(geotype < 0) geotype = -geotype + GEOELLIPSOID_COUNT;
806 if(!fwgeo_gc[geotype]){
807 fwgeo_gc[geotype] = fgeo_initializeGC(semimajor,flattening);
810 veccopyd(gd,inc[i].c);
811 if(!geoSystem->gd_latitude_first) vecswizzle2d(gd);
812 if(!geoSystem->gd_degrees) vecscale2d(gd,gd,DEGREES_PER_RADIAN);
816 gd[0] = dclamp(gd[0],-90.0,90.0);
817 fgeo_gd2gc(fwgeo_gc[geotype], gd[0], gd[1], gd[2], &gc[0], &gc[1], &gc[2]);
818 veccopyd(outc[i].c,gc);
827static void Gd_Gc3d_srm(Geosys *geoSystem,
struct SFVec3d *inc,
int n,
struct SFVec3d *outc){
832 SRM_Celestiodetic cd_srf;
833 SRM_Status_Code status;
835 SRM_ORM_Code src_orm = SRM_ORMCOD_WGS_1984;
836 SRM_RT_Code src_rt = SRM_RTCOD_WGS_1984_IDENTITY;
837 status = SRM_CD_Create(src_orm,src_rt,&cd_srf);
838 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 1 ");
841 SRM_Celestiocentric cc_srf;
842 SRM_ORM_Code tgt_orm = SRM_ORMCOD_WGS_1984;
843 SRM_RT_Code tgt_rt = SRM_RTCOD_WGS_1984_IDENTITY;
844 status = SRM_CC_Create(tgt_orm, tgt_rt, &cc_srf);
845 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 2 ");
849 SRM_Coordinate3D cd_3d_coord;
851 status = cd_srf.methods->CreateCoordinate3D(&cd_srf,
854 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 3 ");
857 SRM_Coordinate3D cc_3d_coord;
859 status = cc_srf.methods->CreateCoordinate3D(&cc_srf,
862 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 4 ");
864 for(
int i=0;i<n;i++){
867 veccopyd(gd,inc[i].c);
868 if(!geoSystem->gd_latitude_first) vecswizzle2d(gd);
869 if(geoSystem->gd_degrees) vecscale2d(gd,gd,RADIANS_PER_DEGREE);
871 SRM_Long_Float latitude = gd[0];
872 SRM_Long_Float longitude = gd[1];
873 SRM_Long_Float ellipsoidal_height = gd[2];
875 printf(
"swizzled and radians gd:\n");
876 printf(
"%d %lf %lf %lf\n",i,gd[0],gd[1],gd[2]);
877 status = cd_srf.methods->SetCoordinate3DValues(&cd_srf,&cd_3d_coord,
880 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 4 ");
885 SRM_Coordinate_Valid_Region valid_region;
887 status = cc_srf.methods->ChangeCoordinate3DSRF(&cc_srf,
892 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 5 ");
894 SRM_Long_Float tgt_ord[3];
895 vecsetd(tgt_ord,0.0,0.0,0.0);
896 status = cc_srf.methods->GetCoordinate3DValues(&cc_srf,
897 &cc_3d_coord, &tgt_ord[0], &tgt_ord[1], &tgt_ord[2]);
898 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 6 ");
900 veccopyd(outc[i].c,tgt_ord);
906static void Gd_Gc3d(Geosys *geoSystem,
struct SFVec3d *inc,
int n,
struct SFVec3d *outc){
910 Gd_Gc3d_geolib(geoSystem,inc,n,outc);
919 Gd_Gc3d_srm(geoSystem,inc,n,outc);
920 printf(
"srm gd2gc:\n");
921 for(i=0;i<min(200,n);i++){
922 printf(
"gd %d %lf %lf %lf\n",i,inc[i].c[0],inc[i].c[1],inc[i].c[2]);
923 printf(
"gc %d %lf %lf %lf\n",i,outc[i].c[0],outc[i].c[1],outc[i].c[2]);
928 double semimajor, flattening;
929 getEllipsoidParams(geoSystem->ellipsoid,&semimajor,&flattening);
930 if(flattening == 0.0){
933 double radius, gd[3], gc[3];
935 veccopyd(gd,inc[i].c);
936 if(!geoSystem->gd_latitude_first) vecswizzle2d(gd);
937 if(geoSystem->gd_degrees) vecscale2d(gd,gd,RADIANS_PER_DEGREE);
938 radius = semimajor + gd[2];
939 gc[0] = cos(gd[0])*cos(gd[1])*radius;
940 gc[1] = cos(gd[0])*sin(gd[1])*radius;
941 gc[2] = sin(gd[0])*radius;
942 veccopyd(outc[i].c,gc);
948 Gd_Gc3d_fw(geoSystem,inc,n,outc);
950 printf(
"regular gd2gc:\n");
951 for(i=0;i<min(5,n);i++){
952 printf(
"gd %d %lf %lf %lf\n",i,inc[i].c[0],inc[i].c[1],inc[i].c[2]);
953 printf(
"gc %d %lf %lf %lf\n",i,outc[i].c[0],outc[i].c[1],outc[i].c[2]);
965static void Xtm_Gd3d(Geosys *geoSystem,
struct SFVec3d *inc,
int n,
struct SFVec3d *outc,
double radius,
double flatten,
966 double scaleFactor,
double falseEasting,
double falseNorthing,
double zoneSize) {
968 int hemisphere_north,
zone, northing_first;
978 double Eccentricity = (F) * (2.0-F);
989 double longitudeOriginDeg;
990 double myeccPrimeSquared;
992 double northingDRCT1;
994 double calcConstantTerm1;
995 double calcConstantTerm2;
996 double calcConstantTerm3;
997 double calcConstantTerm4;
999 if(geoSystem->gd_latitude_first == FALSE){
1004 hemisphere_north = geoSystem->utm_northern_hemisphere;
1005 zone = geoSystem->xtm_zone;
1006 northing_first = geoSystem->xtm_northing_first;
1010 if (!northing_first) { northing = 1; easting = 0; }
1015 if (!northing_first) printf (
"UTM to GD, not northing first, flipping norhting and easting\n");
1019 if (northing_first) printf (
"Utm_Gd: northing first\n");
else printf (
"Utm_Gd: NOT northing_first\n");
1020 if (!hemisphere_north) printf (
"Utm_Gd: NOT hemisphere_north\n");
else printf (
"Utm_Gd: hemisphere_north\n");
1032 longitudeOriginDeg = (
zone -1) * zoneSize - 180. + zoneSize*.5;
1033 myeccPrimeSquared = Eccentricity/(((double) 1.0) - Eccentricity);
1034 eccRoot = (((double)1.0) - sqrt (((
double)1.0) - Eccentricity))/
1035 (((
double)1.0) + sqrt (((
double)1.0) - Eccentricity));
1037 calcConstantTerm1 = ((double)1.0) -Eccentricity/
1038 ((
double)4.0) - ((double)3.0) *Eccentricity*Eccentricity/
1039 ((
double)64.0) -((double)5.0) *Eccentricity*Eccentricity*Eccentricity/((
double)256.0);
1041 calcConstantTerm2 = ((double)3.0) * eccRoot/((
double)2.0) - ((double)27.0) *eccRoot*eccRoot*eccRoot/((
double)32.0);
1042 calcConstantTerm3 = ((double)21.0) * eccRoot*eccRoot/ ((
double)16.0) - ((double)55.0) *eccRoot*eccRoot*eccRoot*eccRoot/ ((
double)32.0);
1043 calcConstantTerm4 = ((double)151.0) *eccRoot*eccRoot*eccRoot/ ((
double)96.0);
1046 printf (
"zone %d\n",
zone);
1047 printf (
"longitudeOriginDeg %lf\n",longitudeOriginDeg);
1048 printf (
"myeccPrimeSquared %lf\n",myeccPrimeSquared);
1049 printf (
"eccRoot %lf\n",eccRoot);
1055 outc[i].c[elevation] = inc[i].c[elevation];
1057 myEasting = inc[i].c[easting] - falseEasting;
1058 if (hemisphere_north) myNorthing = inc[i].c[northing];
1059 else myNorthing = inc[i].c[northing] - falseNorthing;
1062 printf (
"myEasting %lf\n",myEasting);
1063 printf (
"myNorthing %lf\n",myNorthing);
1068 myNorthing= myNorthing / scaleFactor;
1069 northingDRCT1 = myNorthing /(radius * calcConstantTerm1);
1071 myphi1rad = northingDRCT1 +
1072 calcConstantTerm2 * sin(((
double)2.0) *northingDRCT1)+
1073 calcConstantTerm3 * sin(((
double)4.0) *northingDRCT1)+
1074 calcConstantTerm4 * sin(((
double)6.0) *northingDRCT1);
1076 myN1 = radius/sqrt(((
double)1.0) - Eccentricity * sin(myphi1rad) * sin (myphi1rad));
1077 myT1 = tan(myphi1rad) * tan(myphi1rad);
1078 myC1 = Eccentricity * cos(myphi1rad) * cos (myphi1rad);
1079 myR1 = radius * (((double)1.0) - Eccentricity) / pow(((
double)1.0) - Eccentricity * sin(myphi1rad) * sin (myphi1rad), 1.5);
1080 myD = myEasting/(myN1*scaleFactor);
1082 Latitude = myphi1rad-(myN1*tan(myphi1rad)/myR1)*
1083 (myD*myD/((
double)2.0) -
1084 (((
double)5.0) + ((
double)3.0) *myT1+ ((
double)10.0) *myC1-
1085 ((
double)4.0) *myC1*myC1- ((
double)9.0) *myeccPrimeSquared)*
1086 myD*myD*myD*myD/((
double)24.0) +(((
double)61.0) +((
double)90.0) *
1087 myT1+((
double)298.0) *myC1+ ((
double)45.0) *myT1*myT1-
1088 ((
double)252.0) * myeccPrimeSquared- ((
double)3.0) *myC1*myC1)*myD*myD*myD*myD*myD*myD/((
double)720.0));
1090 Longitude = (myD-(((double)1.0)+((double)2.0)*myT1+myC1)*myD*myD*myD/((double)6.0)+(((double)5.0) - ((double)2.0) *myC1+
1091 ((double)28.0) *myT1-((double)3.0) *myC1*myC1+
1092 ((double)8.0) *myeccPrimeSquared+((double)24.0) *myT1*myT1)*myD*myD*myD*myD*myD/120)/cos(myphi1rad);
1094 if(geoSystem->gd_degrees == FALSE){
1096 outc[i].c[latitude] = Latitude ;
1097 outc[i].c[longitude] = longitudeOriginDeg*RADIANS_PER_DEGREE + Longitude;
1100 outc[i].c[latitude] = Latitude * DEGREES_PER_RADIAN;
1101 outc[i].c[longitude] = longitudeOriginDeg + (Longitude * DEGREES_PER_RADIAN);
1117 printf (
"utmtogd\tnorthing %lf easting %lf ele %lf\n\tlat %lf long %lf ele %lf\n", NORTHING_IN, EASTING_IN, ELEVATION_IN, LATITUDE_OUT, LONGITUDE_OUT, ELEVATION_IN);
1123static void Xtm_Gd3d_geolib(Geosys *geoSystem,
struct SFVec3d *inc,
int n,
struct SFVec3d *outc,
1124 double radius,
double flatten,
double scaleFactor,
double falseEasting,
double falseNorthing,
1127 int hemisphere_north,
zone, northing_first, geotype;
1140 double dlongitudeOrigin;
1144 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
1146 if(geoSystem->gd_latitude_first == FALSE){
1151 hemisphere_north = geoSystem->utm_northern_hemisphere;
1152 zone = geoSystem->xtm_zone;
1153 northing_first = geoSystem->xtm_northing_first;
1154 geotype = geoSystem->ellipsoid;
1155 if(geotype < 0) geotype = -geotype + GEOELLIPSOID_COUNT;
1156 if(!p->fgeopars[geotype])
1157 p->fgeopars[geotype] = fgeo_initializeTM(radius, F, 1.0);
1158 fgeo = p->fgeopars[geotype];
1160 if (!northing_first) { northing = 1; easting = 0; }
1165 if (!northing_first) printf (
"UTM to GD, not northing first, flipping norhting and easting\n");
1169 if (northing_first) printf (
"Utm_Gd: northing first\n");
else printf (
"Utm_Gd: NOT northing_first\n");
1170 if (!hemisphere_north) printf (
"Utm_Gd: NOT hemisphere_north\n");
else printf (
"Utm_Gd: hemisphere_north\n");
1174 dlongitudeOrigin = (
zone -1) * zoneSize - 180. + zoneSize*.5;
1179 outc[i].c[elevation] = inc[i].c[elevation];
1181 myEasting = inc[i].c[easting] - falseEasting;
1182 if (hemisphere_north) myNorthing = inc[i].c[northing];
1183 else myNorthing = inc[i].c[northing] - falseNorthing;
1185 myEasting /= scaleFactor;
1186 myNorthing /= scaleFactor;
1189 printf (
"myEasting %lf\n",myEasting);
1190 printf (
"myNorthing %lf\n",myNorthing);
1195 fgeo_tm2gd(fgeo,myEasting, myNorthing, dlongitudeOrigin, &dLatitude, &dLongitude);
1197 if(geoSystem->gd_degrees == FALSE){
1199 outc[i].c[latitude] = dLatitude * RADIANS_PER_DEGREE ;
1200 outc[i].c[longitude] = dLongitude*RADIANS_PER_DEGREE ;
1203 outc[i].c[latitude] = dLatitude;
1204 outc[i].c[longitude] = dLongitude;
1210static void Xtm_Gd3d_srm(Geosys *geoSystem,
struct SFVec3d *inc,
int n,
struct SFVec3d *outc,
1211 double radius,
double flatten,
double scaleFactor,
double falseEasting,
double falseNorthing,
1217 SRM_Status_Code status;
1218 SRM_SRFS_Code_Info srfs_code_info;
1219 SRM_TransverseMercator *utm12_srf;
1221 srfs_code_info.srfs_code = SRM_SRFSCOD_UNIVERSAL_TRANSVERSE_MERCATOR;
1222 srfs_code_info.value.srfsm_utm = SRM_SRFSMUTMCOD_ZONE_12_NORTHERN_HEMISPHERE;
1224 status = SRM_CreateSRFSetMember(srfs_code_info,
1225 SRM_ORMCOD_WGS_1984,
1226 SRM_RTCOD_WGS_1984_IDENTITY,
1227 (SRM_Object_Reference *)&utm12_srf);
1228 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 1a ");
1231 SRM_Celestiodetic cd_srf;
1233 SRM_ORM_Code tgt_orm = SRM_ORMCOD_WGS_1984;
1234 SRM_RT_Code tgt_rt = SRM_RTCOD_WGS_1984_IDENTITY;
1235 status = SRM_CD_Create(tgt_orm,tgt_rt,&cd_srf);
1236 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 1b ");
1240 SRM_Coordinate3D xtm_3d_coord;
1241 status = utm12_srf->methods->CreateCoordinate3D(utm12_srf,
1244 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 2a ");
1247 SRM_Coordinate3D cd_3d_coord;
1248 status = cd_srf.methods->CreateCoordinate3D(&cd_srf,
1251 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 2b ");
1255 for(
int i=0;i<n;i++){
1257 status = utm12_srf->methods->SetCoordinate3DValues(utm12_srf,&xtm_3d_coord,
1258 inc[i].c[0], inc[i].c[1],inc[i].c[2]);
1259 printf(
"UTM inc[%d]= %lf %lf %lf\n",i,inc[i].c[0],inc[i].c[1],inc[i].c[2]);
1261 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 2c ");
1266 SRM_Coordinate_Valid_Region valid_region;
1268 status = cd_srf.methods->ChangeCoordinate3DSRF(&cd_srf,
1274 status = utm12_srf->methods->ChangeCoordinate3DSRF(&cd_srf,
1275 utm12_srf, &xtm_3d_coord, &cd_3d_coord, &valid_region);
1277 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 5 status=%d\n ",status);
1279 SRM_Long_Float tgt_ord[3];
1280 vecsetd(tgt_ord,0.0,0.0,0.0);
1281 status = cd_srf.methods->GetCoordinate3DValues(&cd_srf,
1282 &cd_3d_coord, &tgt_ord[0], &tgt_ord[1], &tgt_ord[2]);
1283 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 6 ");
1288 veccopyd(gd,tgt_ord);
1289 if(geoSystem->gd_latitude_first) vecswizzle2d(gd);
1290 if(geoSystem->gd_degrees) vecscale2d(gd,gd,DEGREES_PER_RADIAN);
1292 printf(
"UNswizzled and MAYBE degrees gd:\n");
1293 printf(
"%d %lf %lf %lf\n",i,gd[0],gd[1],gd[2]);
1295 veccopyd(outc[i].c,gd);
1311static void gdToWm3d(Geosys *geoSystem,
double *gdcoords,
double *wmcoords);
1312static void Wm_Gd3d(Geosys *geoSystem,
struct SFVec3d *inc,
int n,
struct SFVec3d *outc);
1314static void gdToUtm3d(Geosys *geoSystem,
double *gdcoords,
double *xtmcoords);
1315static void gdTo3tm3d(Geosys *geoSystem,
double *gdcoords,
double *xtmcoords);
1316static void Utm_Gd3d(Geosys *geoSystem,
struct SFVec3d *inc,
int n,
struct SFVec3d *outc) {
1317 double semimajor, flattening;
1318 getEllipsoidParams(geoSystem->ellipsoid,&semimajor,&flattening);
1321 Xtm_Gd3d_geolib(geoSystem, inc, n, outc, semimajor, flattening, UTM_SCALE, UTM_FALSE_EASTING, UTM_FALSE_NORTHING, UTM_ZONE_SIZE);
1326 Xtm_Gd3d_srm(geoSystem, inc, n, outc, semimajor, flattening, UTM_SCALE, UTM_FALSE_EASTING, UTM_FALSE_NORTHING, UTM_ZONE_SIZE);
1329 Xtm_Gd3d(geoSystem, inc, n, outc, semimajor, flattening, UTM_SCALE, UTM_FALSE_EASTING, UTM_FALSE_NORTHING, UTM_ZONE_SIZE);
1332 double xtm[3], neh[3];
1334 printf(
"in UTM_gd3d\n");
1335 printf(
"UTM y %lf x %lf h %lf zone %d\n",inc->c[0],inc->c[1],inc->c[2],geoSystem->xtm_zone);
1336 gdToUtm3d(geoSystem,outc->c, xtm);
1338 if(!geoSystem->xtm_northing_first) vecswizzle2d(neh);
1339 printf(
"UTM y %lf x %lf h %lf zone %d\n",neh[0],neh[1],neh[2],geoSystem->xtm_zone);
1341 gdTo3tm3d(geoSystem,outc->c, xtm);
1343 if(!geoSystem->xtm_northing_first) vecswizzle2d(neh);
1344 printf(
"3TM y %lf x %lf h %lf zone %d\n",neh[0],neh[1],neh[2],geoSystem->xtm_zone);
1347static void U3tm_Gd3d(Geosys *geoSystem,
struct SFVec3d *inc,
int n,
struct SFVec3d *outc) {
1348 double semimajor, flattening;
1349 getEllipsoidParams(geoSystem->ellipsoid,&semimajor,&flattening);
1352 Xtm_Gd3d_geolib(geoSystem, inc, n, outc, semimajor, flattening, U3TM_SCALE, U3TM_FALSE_EASTING, U3TM_FALSE_NORTHING, U3TM_ZONE_SIZE);
1355 Xtm_Gd3d(geoSystem, inc, n, outc, semimajor, flattening, U3TM_SCALE, U3TM_FALSE_EASTING, U3TM_FALSE_NORTHING, U3TM_ZONE_SIZE);
1358double getTerrainHeight(
int planetID, Geosys *geoSystem,
struct SFVec3d *gdCoord);
1359double userHeight2ellipsoidHeight(Geosys *geoSystem,
struct SFVec3d *gdCoord){
1360 double additionalHeight = 0.0;
1361 if(geoSystem->geoid_height == TRUE){
1365 double gddegrees[3];
1366 if(geoSystem->gd_degrees) veccopyd(gddegrees,gdCoord->c);
1367 else vecscaled(gddegrees,gdCoord->c,DEGREES_PER_RADIAN);
1368 if(!geoSystem->gd_latitude_first) vecswizzle2d(gddegrees);
1369 additionalHeight += geoidCorrection(gddegrees[0],gddegrees[1]);
1371 if(geoSystem->relativeHeight == TRUE){
1373 struct Planet *planet = current_planet();
1374 additionalHeight += getTerrainHeight( planet->ID, geoSystem, gdCoord);
1376 return additionalHeight;
1388static void moveCoords3d (Geosys * geoSystem,
struct SFVec3d *offset,
struct SFVec4d *yup,
1394 switch (geoSystem->spatial_system) {
1398 memcpy (gdCoords, inCoords,
sizeof (
struct SFVec3d) * n);
1399 if(geoSystem->geoid_height || geoSystem->relativeHeight)
1400 for(i=0; i < n; i++)
1401 gdCoords[i].c[2] += userHeight2ellipsoidHeight(geoSystem,&gdCoords[i]);
1404 Gd_Gc3d(geoSystem,gdCoords,n,outCoords);
1410 for (i=0; i< n; i++) {
1411 veccopyd(outCoords[i].c,inCoords[i].c);
1413 gccToGdc (geoSystem, &inCoords[i], &gdCoords[i]);
1422 Utm_Gd3d(geoSystem,inCoords,n, gdCoords);
1423 if(geoSystem->geoid_height || geoSystem->relativeHeight)
1424 for(i=0; i < n; i++)
1425 gdCoords[i].c[2] += userHeight2ellipsoidHeight(geoSystem,&gdCoords[i]);
1426 Gd_Gc3d(geoSystem,gdCoords,n,outCoords);
1434 U3tm_Gd3d(geoSystem,inCoords,n, gdCoords);
1435 if(geoSystem->geoid_height || geoSystem->relativeHeight)
1436 for(i=0; i < n; i++)
1437 gdCoords[i].c[2] += userHeight2ellipsoidHeight(geoSystem,&gdCoords[i]);
1438 Gd_Gc3d(geoSystem,gdCoords,n,outCoords);
1446 Wm_Gd3d(geoSystem,inCoords,n, gdCoords);
1447 if(geoSystem->geoid_height || geoSystem->relativeHeight)
1448 for(i=0; i < n; i++)
1449 gdCoords[i].c[2] += userHeight2ellipsoidHeight(geoSystem,&gdCoords[i]);
1450 Gd_Gc3d(geoSystem,gdCoords,n,outCoords);
1455 printf (
"incorrect geoSystem field, %s\n",stringGEOSPATIALType(geoSystem->spatial_system));
1463 vecdifd(outCoords[i].c,outCoords[i].c,offset->c);
1467 vrmlrot_to_quaternion(&qup,yup->c[0],yup->c[1],yup->c[2],-yup->c[3]);
1470 quaternion_rotationd(outCoords[i].c,&qup,outCoords[i].c);
1477static void initializeGeospatial (
struct X3D_GeoOrigin **nodeptr) {
1483 printf (
"\ninitializing GeoSpatial code nodeptr %u\n",*nodeptr);
1486 if (*nodeptr != NULL) {
1487 if (X3D_GEOORIGIN(*nodeptr)->_nodeType != NODE_GeoOrigin) {
1488 printf (
"expected a GeoOrigin node, but got a node of type %s\n",
1489 stringNodeType(X3D_GEOORIGIN(*nodeptr)->_nodeType));
1494 node = X3D_GEOORIGIN(*nodeptr);
1496 specversion = X3D_PROTO(node->_executionContext)->__specversion;
1499 if NODE_NEEDS_COMPILING {
1502 compile_geoSystem (X3D_NODE(node),node->_nodeType, &node->geoSystem, &node->__geoSystem);
1504 moveCoords3d(GEOSYS(node->__geoSystem), NULL, NULL,
1505 &node->geoCoords,1, &node->__movedCoords, &node->__movedgd);
1507 node->rotateYUp = FALSE;
1509 if(node->rotateYUp == TRUE)
1513 Quaternion qz,qx,qr;
1517 dangle = node->__movedgd.c[1];
1518 gs = GEOSYS(node->__geoSystem);
1521 dangle *= RADIANS_PER_DEGREE;
1522 dangle += RADIANS_PER_DEGREE*90.0;
1523 vrmlrot_to_quaternion (&qz,0.0, 0.0, 1.0, dangle);
1526 printf (
"GeoOrient qz angle (deg) %lf angle (rad) %lf quat: %lf %lf %lf %lf\n",dangle*DEGREES_PER_RADIAN,
1527 dangle,qz.x, qz.y, qz.z,qz.w);
1530 dangle = node->__movedgd.c[0];
1531 if(gs->gd_degrees == TRUE)
1532 dangle *= RADIANS_PER_DEGREE;
1533 dangle = RADIANS_PER_DEGREE*180.0 - dangle;
1534 vrmlrot_to_quaternion (&qx,1.0, 0.0, 0.0,dangle);
1537 printf (
"GeoOrient qx angle (deg) %lf angle (rad) %lf quat: %lf %lf %lf %lf\n",
1538 dangle*DEGREES_PER_RADIAN, dangle, qx.x, qx.y, qx.z,qx.w);
1542 quaternion_multiply(&qr,&qz,&qx);
1545 printf (
"GeoOrient qr %lf %lf %lf %lf\n",qr.x, qr.y, qr.z,qr.w);
1548 quaternion_to_vrmlrot(&qr, &orient.c[0], &orient.c[1], &orient.c[2], &orient.c[3]);
1550 node->__rotyup.c[i] = orient.c[i];
1554 node->__rotyup.c[i] = 0.0;
1555 node->__rotyup.c[1] = 1.0;
1559 printf (
"initializeGeospatial, __movedCoords %lf %lf %lf, ryup %d, geoSystem %d %d %d %d\n",
1560 node->__movedCoords.c[0],
1561 node->__movedCoords.c[1],
1562 node->__movedCoords.c[2],
1564 node->__geoSystem.p[0],
1565 node->__geoSystem.p[1],
1566 node->__geoSystem.p[2],
1567 node->__geoSystem.p[3]);
1568 node->__geoSystem.p[4]);
1569 node->__geoSystem.p[5]);
1570 node->__geoSystem.p[6]);
1571 node->__geoSystem.p[7]);
1572 printf (
"initializeGeospatial, done\n\n");
1583double A, F, C, A2, C2, Eps2, Eps21, Eps25, C254, C2DA, CEE,
1584 CE2, CEEps2, TwoCEE, tem, ARat1, ARat2, BRat1, BRat2, B1,B2,B3,B4,B5;
1588struct gcgd* initializeGcToGdParams(
int type,
double A,
double F) {
1590 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
1591 if(type < 0) type = -type + GEOELLIPSOID_COUNT;
1592 if(p->gcgdpars[type])
return p->gcgdpars[type];
1593 g = malloc(
sizeof(
struct gcgd));
1594 p->gcgdpars[type] = g;
1599 g->C =(A) * (1-g->F);
1600 g->C2 = g->C * g->C;
1601 g->Eps2 =(g->F) * (2.0-g->F);
1602 g->Eps21 =g->Eps2 - 1.0;
1603 g->Eps25 =.25 * (g->Eps2);
1604 g->C254 =54.0 * g->C2;
1606 g->C2DA = g->C2 / A;
1607 g->CE2 = g->A2 - g->C2;
1608 g->tem = g->CE2 / g->C2;
1609 g->CEE = g->Eps2 * g->Eps2;
1610 g->TwoCEE =2.0 * g->CEE;
1611 g->CEEps2 =g->Eps2 * g->CE2;
1616 g->ARat1 =pow((A + 50005.0),2);
1617 g->ARat2 =(g->ARat1) / pow((g->C+50005.0),2);
1621 g->BRat1 =pow((A-10005.0),2);
1622 g->BRat2 =(g->BRat1) / pow((g->C-10005.0),2);
1625 g->B1=0.100225438677758E+01;
1626 g->B2=-0.393246903633930E-04;
1627 g->B3=0.241216653453483E+12;
1628 g->B4=0.133733602228679E+14;
1629 g->B5=0.984537701867943E+00;
1634static void gccToGdc_fw (Geosys *geoSystem,
struct SFVec3d *gcc,
struct SFVec3d *gdc) {
1638 double GCC_X, GCC_Y, GCC_Z;
1640 double w2,w,z2,testu,testb,top,top2,rr,q,s12,rnn,s1,zp2,wp,wp2,cf,gee,alpha,cl,arg2,p,xarg,r2,r1,ro,
1644 printf (
"gccToGdc input %lf %lf %lf\n",GCC_X, GCC_Y, GCC_Z);
1649 if(geoSystem->gd_latitude_first == FALSE){
1650 latitude = 1; longitude = 0;
1652 getEllipsoidParams(geoSystem->ellipsoid,&A,&F);
1653 g = initializeGcToGdParams(geoSystem->ellipsoid,A,F);
1655 w2=GCC_X * GCC_X + GCC_Y * GCC_Y;
1659 testu=w2 + g->ARat2 * z2;
1660 testb=w2 + g->BRat2 * z2;
1662 if ((testb > g->BRat1) && (testu < g->ARat1))
1667 top= GCC_Z * (g->B1 + (g->B2 * w2 + g->B3) /
1668 (g->B4 + w2 * g->B5 + z2));
1684 rnn = g->A / ( (.25 - g->Eps25*s12 + .9999944354799/4) + (.25-g->Eps25*s12)/(.25 - g->Eps25*s12 + .9999944354799/4));
1692 gdc->c[elevation] = q-rnn;
1694 gdc->c[elevation] = GCC_Z / s1 + (g->Eps21 * rnn);
1695 gdc->c[latitude] = atan(top / w);
1696 gdc->c[longitude] = atan2(GCC_Y,GCC_X);
1701 wp2=GCC_X * GCC_X + GCC_Y * GCC_Y;
1705 gee=wp2 - (g->Eps21 * zp2) - g->CEEps2;
1706 alpha=cf / (gee*gee);
1707 cl=g->CEE * wp2 * alpha / gee;
1708 arg2=cl * (cl + 2.0);
1709 s1=1.0 + cl + sqrt(arg2);
1710 s=pow(s1,(1.0/3.0));
1711 p=alpha / (3.0 * pow(( s + (1.0/s) + 1.0),2));
1712 xarg= 1.0 + (g->TwoCEE * p);
1714 r2= -p * (2.0 * (1.0 - g->Eps2) * zp2 / ( q * ( 1.0 + q) ) + wp2);
1715 r1=(1.0 + (1.0 / q));
1721 ro = g->A * sqrt( .50 * (r1+r2));
1725 ro=ro - p * g->Eps2 * wp / ( 1.0 + q);
1728 arg = pow(( wp - roe),2) + zp2;
1729 v=sqrt(arg - g->Eps2 * zp2);
1730 zo=g->C2DA * GCC_Z / v;
1731 gdc->c[elevation] = sqrt(arg) * (1.0 - g->C2DA / v);
1732 top=GCC_Z+ g->tem*zo;
1733 gdc->c[latitude] = atan( top / wp );
1734 gdc->c[longitude] =atan2(GCC_Y,GCC_X);
1737 if(geoSystem->gd_degrees == TRUE){
1739 gdc->c[latitude] *= DEGREES_PER_RADIAN;
1740 gdc->c[longitude] *= DEGREES_PER_RADIAN;
1746static void gccToGdc_geolib (Geosys *geoSystem,
struct SFVec3d *gcc,
struct SFVec3d *gdc){
1748 double gd[3],gc[3], semimajor,flattening;
1750 getEllipsoidParams(geoSystem->ellipsoid,&semimajor,&flattening);
1751 if(FALSE && flattening == 0.0){
1753 double radius, horizontal_radius;
1754 veccopyd(gc,gcc->c);
1756 radius = veclengthd(gc);
1757 horizontal_radius = veclength2d(gc);
1758 gd[0] = atan2(gc[2],horizontal_radius);
1759 gd[1] = atan2(gc[1],gc[0]);
1760 gd[2] = radius - semimajor;
1762 if(!geoSystem->gd_latitude_first) vecswizzle2d(gd);
1763 if(geoSystem->gd_degrees) vecscale2d(gd,gd,DEGREES_PER_RADIAN);
1765 veccopyd(gdc->c,gd);
1769 geotype = geoSystem->ellipsoid;
1770 if(geotype < 0) geotype = -geotype + GEOELLIPSOID_COUNT;
1771 if(!fwgeo_gc[geotype]){
1772 fwgeo_gc[geotype] = fgeo_initializeGC(semimajor,flattening);
1774 veccopyd(gc,gcc->c);
1776 fgeo_gc2gd(fwgeo_gc[geotype],gc[0],gc[1],gc[2], &gd[0],&gd[1],&gd[2]);
1777 if(!geoSystem->gd_latitude_first) vecswizzle2d(gd);
1778 if(!geoSystem->gd_degrees) vecscale2d(gd,gd,RADIANS_PER_DEGREE);
1779 veccopyd(gdc->c,gd);
1786static void gccToGdc_srm (Geosys *geoSystem,
struct SFVec3d *gcc,
struct SFVec3d *gdc){
1787 double gd[3],gc[3], semimajor,flattening;
1792 SRM_Celestiocentric cc_srf;
1793 SRM_Status_Code status;
1795 SRM_ORM_Code src_orm = SRM_ORMCOD_WGS_1984;
1796 SRM_RT_Code src_rt = SRM_RTCOD_WGS_1984_IDENTITY;
1797 status = SRM_CC_Create(src_orm,src_rt,&cc_srf);
1798 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 1 ");
1801 SRM_Celestiodetic cd_srf;
1802 SRM_ORM_Code tgt_orm = SRM_ORMCOD_WGS_1984;
1803 SRM_RT_Code tgt_rt = SRM_RTCOD_WGS_1984_IDENTITY;
1804 status = SRM_CD_Create(tgt_orm, tgt_rt, &cd_srf);
1805 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 2 ");
1809 SRM_Coordinate3D cc_3d_coord;
1810 status = cc_srf.methods->CreateCoordinate3D(&cc_srf,
1813 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 4 ");
1816 SRM_Coordinate3D cd_3d_coord;
1817 status = cd_srf.methods->CreateCoordinate3D(&cd_srf,
1820 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 3 ");
1823 status = cc_srf.methods->SetCoordinate3DValues(&cc_srf,&cc_3d_coord,
1824 gcc->c[0],gcc->c[1],gcc->c[2]);
1826 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 4 ");
1831 SRM_Coordinate_Valid_Region valid_region;
1833 status = cd_srf.methods->ChangeCoordinate3DSRF(&cd_srf,
1838 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 5 ");
1840 SRM_Long_Float tgt_ord[3];
1841 vecsetd(tgt_ord,0.0,0.0,0.0);
1842 status = cd_srf.methods->GetCoordinate3DValues(&cd_srf,
1843 &cd_3d_coord, &tgt_ord[0], &tgt_ord[1], &tgt_ord[2]);
1844 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 6 ");
1846 veccopyd(gd,tgt_ord);
1847 if(geoSystem->gd_latitude_first) vecswizzle2d(gd);
1848 if(geoSystem->gd_degrees) vecscale2d(gd,gd,DEGREES_PER_RADIAN);
1850 SRM_Long_Float latitude = gd[0];
1851 SRM_Long_Float longitude = gd[1];
1852 SRM_Long_Float ellipsoidal_height = gd[2];
1854 printf(
"UNswizzled and maybe degrees gd:\n");
1855 printf(
" %lf %lf %lf\n",gd[0],gd[1],gd[2]);
1857 veccopyd(gdc->c,gd);
1861static void gccToGdc (Geosys *geoSystem,
struct SFVec3d *gcc,
struct SFVec3d *gdc){
1863 if(method_geolib()){
1864 gccToGdc_geolib(geoSystem,gcc,gdc);
1870 gccToGdc_srm(geoSystem,gcc,gdc);
1875 double semimajor, flattening;
1876 getEllipsoidParams(geoSystem->ellipsoid,&semimajor,&flattening);
1877 if(flattening == 0.0){
1880 double radius, horizontal_radius, gd[3], gc[3];
1881 veccopyd(gc,gcc->c);
1883 radius = veclengthd(gc);
1884 horizontal_radius = veclength2d(gc);
1885 gd[0] = atan2(gc[2],horizontal_radius);
1886 gd[1] = atan2(gc[1],gc[0]);
1887 gd[2] = radius - semimajor;
1889 if(!geoSystem->gd_latitude_first) vecswizzle2d(gd);
1890 if(geoSystem->gd_degrees) vecscale2d(gd,gd,DEGREES_PER_RADIAN);
1892 veccopyd(gdc->c,gd);
1896 gccToGdc_fw(geoSystem,gcc,gdc);
1902static void gdToXtm(
double radius,
double flattening,
double latitude,
double longitude,
double scaleFactor,
1903 double falseEasting,
double falseNorthing,
double zoneSize,
int *
zone,
double *easting,
double *northing)
1905#define DEG2RAD (PI/180.00)
1911 double longOriginradian, dlon;
1912 double eccentprime, e2, A, F;
1925 dlon = longitude * DEGREES_PER_RADIAN;
1927 *
zone = (int) (((dlon + 180.0)/zoneSize) + 1);
1929 lat_radian = latitude;
1930 long_radian = longitude;
1931 myScale = scaleFactor;
1932 longOrigin = (*
zone - 1)*zoneSize - 180.0 + zoneSize/2.0;
1933 longOriginradian = longOrigin * DEG2RAD;
1934 eccentprime = e2/(1.0-e2);
1942 NNN = A / sqrt(1.0-e2 * sin(lat_radian)*sin(lat_radian));
1943 TTT = tan(lat_radian) * tan(lat_radian);
1944 CCC = eccentprime * cos(lat_radian)*cos(lat_radian);
1945 AAA = cos(lat_radian) * (long_radian - longOriginradian);
1947 * ( ( 1. - e2/4 - 3. * e2 * e2/64
1948 - 5.0 * e2 * e2 * e2/256.0
1950 - ( 3. * e2/8 + 3. * e2 * e2/32.
1951 + 45. * e2 * e2 * e2/1024.
1952 ) * sin(2. * lat_radian)
1953 + ( 15. * e2 * e2/256. +
1954 45. * e2 * e2 * e2/1024.
1955 ) * sin(4. * lat_radian)
1956 - ( 35. * e2 * e2 * e2/3072.
1957 ) * sin(6. * lat_radian)
1962 *easting = myScale*NNN*(AAA+(1-TTT+CCC)*AAA*AAA*AAA/6
1963 + (5.-18.*TTT+TTT*TTT+72.*CCC-58.*eccentprime)*AAA*AAA*AAA*AAA*AAA/120.)
1966 *northing= myScale * ( MMM + NNN*tan(lat_radian) *
1967 ( AAA*AAA/2.+(5.-TTT+9.*CCC+4.*CCC*CCC)*AAA*AAA*AAA*AAA/24 + (61.-58.*TTT+TTT*TTT+600.*CCC-330.*eccentprime) * AAA*AAA*AAA*AAA*AAA*AAA/720.));
1969 if (latitude < 0) *northing += falseNorthing;
1972 printf (
"gdToUtm: lat %lf long %lf zone %d -> easting %lf northing %lf\n",latitude, longitude, *
zone,*easting, *northing);
1978static void gdToXtm_geolib(
int geotype,
double radius,
double flattening,
double latitude,
double longitude,
double scaleFactor,
1979 double falseEasting,
double falseNorthing,
double zoneSize,
int *
zone,
double *easting,
double *northing)
1981 double F, dlon0, dlat, dlon;
1983 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
1986 if(!p->fgeopars[geotype])
1987 p->fgeopars[geotype] = fgeo_initializeTM(radius, F, 1.0);
1988 fgeo = p->fgeopars[geotype];
1992 dlon = longitude * DEGREES_PER_RADIAN;
1994 *
zone = (int) (((dlon + 180.0)/zoneSize) + 1);
1996 dlon0 = (*
zone -1) * zoneSize - 180. + zoneSize*.5;
1997 dlat = latitude * DEGREES_PER_RADIAN;
1999 fgeo_gd2tm(fgeo,dlat,dlon,dlon0,easting, northing);
2000 *easting *= scaleFactor;
2001 *northing *= scaleFactor;
2003 if (latitude < 0.0) *northing += falseNorthing;
2004 *easting += falseEasting;
2007 printf (
"gdToUtm: lat %lf long %lf zone %d -> easting %lf northing %lf\n",latitude, longitude, *
zone,*easting, *northing);
2013static void gdToXtm_srm(
int geotype,
double radius,
double flattening,
double latitude,
double longitude,
double scaleFactor,
2014 double falseEasting,
double falseNorthing,
double zoneSize,
int *
zone,
double *easting,
double *northing)
2020 double dlon = longitude * DEGREES_PER_RADIAN;
2022 *
zone = (int) (((dlon + 180.0)/zoneSize) + 1);
2030 SRM_Status_Code status;
2031 SRM_Celestiodetic cd_srf;
2033 SRM_ORM_Code tgt_orm = SRM_ORMCOD_WGS_1984;
2034 SRM_RT_Code tgt_rt = SRM_RTCOD_WGS_1984_IDENTITY;
2035 status = SRM_CD_Create(tgt_orm,tgt_rt,&cd_srf);
2036 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 1b ");
2039 SRM_SRFS_Code_Info srfs_code_info;
2040 SRM_TransverseMercator *utm12_srf;
2042 srfs_code_info.srfs_code = SRM_SRFSCOD_UNIVERSAL_TRANSVERSE_MERCATOR;
2044 srfs_code_info.value.srfsm_utm = (SRM_SRFSM_UTM_Code) *
zone;
2046 status = SRM_CreateSRFSetMember(srfs_code_info,
2047 SRM_ORMCOD_WGS_1984,
2048 SRM_RTCOD_WGS_1984_IDENTITY,
2049 (SRM_Object_Reference *)&utm12_srf);
2050 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 1a ");
2053 SRM_Coordinate3D cd_3d_coord;
2054 status = cd_srf.methods->CreateCoordinate3D(&cd_srf,
2057 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 2b ");
2060 SRM_Coordinate3D xtm_3d_coord;
2061 status = utm12_srf->methods->CreateCoordinate3D(utm12_srf,
2064 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 2a ");
2067 status = cd_srf.methods->SetCoordinate3DValues(&cd_srf,&cd_3d_coord,
2068 longitude, latitude,0.0);
2069 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 2c ");
2072 SRM_Coordinate_Valid_Region valid_region;
2073 status = utm12_srf->methods->ChangeCoordinate3DSRF(utm12_srf,
2079 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 5 status=%d\n ",status);
2081 SRM_Long_Float tgt_ord[3];
2082 vecsetd(tgt_ord,0.0,0.0,0.0);
2083 status = utm12_srf->methods->GetCoordinate3DValues(utm12_srf,
2084 &xtm_3d_coord, &tgt_ord[0], &tgt_ord[1], &tgt_ord[2]);
2085 if(status != SRM_STATCOD_SUCCESS) printf(
"ouch 6 ");
2087 *easting = tgt_ord[0];
2088 *northing = tgt_ord[1];
2089 printf(
"easting = %lf northing = %lf \n",*easting, *northing);
2106static void gdToUtm3d(Geosys *geoSystem,
double *gdcoords,
double *xtmcoords) {
2107 double semimajor, flattening;
2108 double gdradians[3];
2109 int geotype, northing_first, latitude_first, is_degrees, *
zone;
2111 geotype = geoSystem->ellipsoid;
2112 northing_first = geoSystem->xtm_northing_first;
2113 latitude_first = geoSystem->gd_latitude_first;
2114 is_degrees = geoSystem->gd_degrees;
2116 if(is_degrees) vecscaled(gdradians,gdcoords,RADIANS_PER_DEGREE);
2117 else veccopyd(gdradians,gdcoords);
2118 if(!latitude_first) vecswizzle2d(gdradians);
2120 getEllipsoidParams(geotype,&semimajor,&flattening);
2121 zone = &geoSystem->xtm_zone;
2124 gdToXtm_geolib(geotype,semimajor,flattening,gdradians[0],gdradians[1], UTM_SCALE, UTM_FALSE_EASTING, UTM_FALSE_NORTHING, UTM_ZONE_SIZE,
zone, &xtmcoords[1], &xtmcoords[0]);
2129 gdToXtm_srm(geotype,semimajor,flattening,gdradians[0],gdradians[1], UTM_SCALE, UTM_FALSE_EASTING, UTM_FALSE_NORTHING, UTM_ZONE_SIZE,
zone, &xtmcoords[1], &xtmcoords[0]);
2132 gdToXtm(semimajor,flattening, gdradians[0],gdradians[1], UTM_SCALE, UTM_FALSE_EASTING, UTM_FALSE_NORTHING, UTM_ZONE_SIZE,
zone, &xtmcoords[1], &xtmcoords[0]);
2134 if(!northing_first) vecswizzle2d(xtmcoords);
2135 xtmcoords[2] = gdcoords[2];
2137static void gdTo3tm3d(Geosys *geoSystem,
double *gdcoords,
double *xtmcoords) {
2138 double semimajor, flattening;
2139 double gdradians[3];
2140 int geotype, northing_first, latitude_first, is_degrees, *
zone;
2142 geotype = geoSystem->ellipsoid;
2143 northing_first = geoSystem->xtm_northing_first;
2144 latitude_first = geoSystem->gd_latitude_first;
2145 is_degrees = geoSystem->gd_degrees;
2147 if(is_degrees) vecscaled(gdradians,gdcoords,RADIANS_PER_DEGREE);
2148 else veccopyd(gdradians,gdcoords);
2149 if(!latitude_first) vecswizzle2d(gdradians);
2151 getEllipsoidParams(geotype,&semimajor,&flattening);
2152 zone = &geoSystem->xtm_zone;
2155 gdToXtm_geolib(geotype,semimajor,flattening,gdradians[0],gdradians[1], U3TM_SCALE, U3TM_FALSE_EASTING, U3TM_FALSE_NORTHING, U3TM_ZONE_SIZE,
zone, &xtmcoords[1], &xtmcoords[0]);
2158 gdToXtm(semimajor, flattening, gdradians[0],gdradians[1], U3TM_SCALE, U3TM_FALSE_EASTING, U3TM_FALSE_NORTHING, U3TM_ZONE_SIZE,
zone, &xtmcoords[1], &xtmcoords[0]);
2160 if(!northing_first) vecswizzle2d(xtmcoords);
2161 xtmcoords[2] = gdcoords[2];
2186static void gdToWm3d(Geosys *geoSystem,
double *gdcoords,
double *wmcoords);
2187static void Wm_Gd3d(Geosys *geoSystem,
struct SFVec3d *inc,
int n,
struct SFVec3d *outc);
2188static void gdToWm(
double radius,
double latitudeRadians,
double longitudeRadians,
double *x,
double *y){
2189 *x = longitudeRadians * radius;
2190 *y = log(tan(.5*(PI*.5 + latitudeRadians)))*radius;
2193static void gdToWm3d(Geosys *geoSystem,
double *gdcoords,
double *wmcoords) {
2195 double semimajor, flattening;
2196 double gdradians[3];
2197 int geotype, northing_first, latitude_first, is_degrees, *
zone;
2199 geotype = geoSystem->ellipsoid;
2201 northing_first = FALSE;
2202 latitude_first = geoSystem->gd_latitude_first;
2203 is_degrees = geoSystem->gd_degrees;
2205 if(is_degrees) vecscaled(gdradians,gdcoords,RADIANS_PER_DEGREE);
2206 else veccopyd(gdradians,gdcoords);
2207 if(!latitude_first) vecswizzle2d(gdradians);
2209 getEllipsoidParams(geotype,&semimajor,&flattening);
2211 gdToWm(semimajor, gdradians[0],gdradians[1], &wmcoords[1], &wmcoords[0]);
2213 if(!northing_first) vecswizzle2d(wmcoords);
2214 wmcoords[2] = gdcoords[2];
2216static void Wm_Gd3d0(Geosys *geoSystem,
struct SFVec3d *inc,
int n,
struct SFVec3d *outc,
double radius){
2218 int latitude, longitude, elevation;
2219 latitude=0; longitude=1;
2220 if(geoSystem->gd_latitude_first == FALSE){
2225 for(
int i=0;i<n;i++){
2226 outc[i].c[elevation] = inc[i].c[2];
2231 lat = 2.0*atan(exp(lat)) - .5*PI;
2232 if(geoSystem->gd_degrees == FALSE){
2234 outc[i].c[latitude] = lat;
2235 outc[i].c[longitude] = lon;
2238 outc[i].c[latitude] = lat * DEGREES_PER_RADIAN;
2239 outc[i].c[longitude] = lon * DEGREES_PER_RADIAN;
2243static void Wm_Gd3d(Geosys *geoSystem,
struct SFVec3d *inc,
int n,
struct SFVec3d *outc) {
2244 double semimajor, flattening;
2245 getEllipsoidParams(geoSystem->ellipsoid,&semimajor,&flattening);
2246 Wm_Gd3d0(geoSystem, inc, n, outc, semimajor);
2255static void GeoOrient (
struct X3D_Node *geoOrigin, Geosys *geoSystem,
struct SFVec3d *gdCoords,
struct SFVec4d *orient) {
2259 double dangle, gdcoords[3];
2266 if (geoSystem != NULL) {
2267 if (geoSystem->spatial_system == GEOSP_GC) {
2269 printf (
"GeoOrient - simple GC, so no orient\n");
2276 if(((
struct X3D_GeoOrigin*)geoOrigin)->rotateYUp == TRUE)
return;
2280 printf (
"GeoOrient - gdCoords->c[0,1] is %f %f\n",gdCoords->c[0],gdCoords->c[1]);
2284 veccopyd(gdcoords,gdCoords->c);
2285 if(!geoSystem->gd_latitude_first) vecswizzle2d(gdcoords);
2286 dangle = gdcoords[1];
2287 if(geoSystem->gd_degrees == TRUE)
2288 dangle *= RADIANS_PER_DEGREE;
2289 dangle += RADIANS_PER_DEGREE*90.0;
2290 vrmlrot_to_quaternion (&qz,0.0, 0.0, 1.0, dangle);
2293 printf (
"GeoOrient qz angle (deg) %lf angle (rad) %lf quat: %lf %lf %lf %lf\n",((
double)90.0 + gdCoords->c[1]),
2294 RADIANS_PER_DEGREE*((
double)90.0 + gdCoords->c[1]),qz.x, qz.y, qz.z,qz.w);
2297 dangle = gdcoords[0];
2298 if(geoSystem->gd_degrees == TRUE)
2299 dangle *= RADIANS_PER_DEGREE;
2300 dangle = RADIANS_PER_DEGREE*180.0 - dangle;
2301 vrmlrot_to_quaternion (&qx,1.0, 0.0, 0.0, dangle);
2304 printf (
"GeoOrient qx angle (deg) %lf angle (rad) %lf quat: %lf %lf %lf %lf\n",
2305 ((
double)180.0 - gdCoords->c[0]), RADIANS_PER_DEGREE*((
double)180.0 - gdCoords->c[0]), qx.x, qx.y, qx.z,qx.w);
2310 quaternion_multiply(&qr, &qz, &qx);
2314 printf (
"GeoOrient qr %lf %lf %lf %lf\n",qr.x, qr.y, qr.z,qr.w);
2317 quaternion_to_vrmlrot(&qr, &orient->c[0], &orient->c[1], &orient->c[2], &orient->c[3]);
2318 vecnormald(orient->c,orient->c);
2320 printf (
"GeoOrient rotation %lf %lf %lf %lf\n",orient->c[0], orient->c[1], orient->c[2], orient->c[3]);
2338char * stringint_int2string(
struct stringint *table,
int itype){
2341 if(table[i].i == itype)
return table[i].c;
2346int stringint_string2int(
struct stringint *table,
const char *ctype){
2349 if(!strcmp(table[i].c,ctype))
return table[i].i;
2354struct stringint lookup_ellipsoids [] = {
2380struct stringint lookup_spatialreferencesys [] = {
2391 int i, specversion, nextra;
2392 indexT this_srf = INT_ID_UNDEFINED;
2393 indexT this_srf_ind = INT_ID_UNDEFINED;
2395 Geosys *srf = GEOSYS(*nodegeosys);
2398 printf (
"start of compile_geoSystem\n");
2403 srf = malloc(
sizeof(Geosys));
2404 register_node_gc(node,(
void*)srf);
2405 *nodegeosys = X3D_NODE(srf);
2409 srf->spatial_system = GEOSP_GD;
2410 srf->ellipsoid = GEOEL_WE;
2411 srf->xtm_zone = INT_ID_UNDEFINED;
2412 srf->xtm_northing_first = TRUE;
2413 srf->utm_northern_hemisphere = TRUE;
2414 srf->gd_latitude_first = TRUE;
2415 srf->geoid_height = FALSE;
2416 specversion = X3D_PROTO(node->_executionContext)->__specversion;
2417 if(specversion > 320 && STRICT33){
2419 srf->gd_degrees = FALSE;
2422 srf->gd_degrees = TRUE;
2424 srf->relativeHeight = FALSE;
2426 if (args->n==0)
return;
2430 ee.a = ee.b = ee.f = 0.0;
2433 for (i=0; i<args->n; i++) {
2434 int itype = stringint_string2int(lookup_spatialreferencesys,args->p[i]->strptr);
2442 if (this_srf == INT_ID_UNDEFINED) {
2443 ConsoleMessage (
"geoSystem in node %s, must have GC, GD or UTM",stringNodeType(nodeType));
2447 srf->spatial_system = (int) this_srf;
2449 if (this_srf == GEOSP_GC) {
2452 }
else if (this_srf == GEOSP_GD || this_srf == GEOSP_3TM || this_srf == GEOSP_UTM || this_srf == GEOSP_WM) {
2453 for (i=0; i<args->n; i++) {
2454 if (i != this_srf_ind) {
2456 char *str = args->p[i]->strptr;
2458 iellipse = stringint_string2int(lookup_ellipsoids,str);
2460 srf->ellipsoid = iellipse;
2463 if (strcmp(
"latitude_first", str) == 0) {
2464 srf->gd_latitude_first = TRUE;
2465 }
else if (strcmp(
"longitude_first", str) == 0) {
2466 srf->gd_latitude_first = FALSE;
2467 }
else if(strcmp (
"WGS84",str) == 0){
2468 srf->geoid_height = TRUE;
2474 sscanf(args->p[i]->strptr,
"R%lf",&radius);
2477 }
else if (str[0] ==
'A') {
2480 sscanf(str,
"A%lf",&a);
2483 }
else if (str[0] ==
'B') {
2486 sscanf(str,
"B%lf",&b);
2489 }
else if (!strncmp(str,
"IF",2)) {
2492 sscanf(str,
"IF%lf",&invf);
2495 }
else if (str[0] ==
'F') {
2498 sscanf(str,
"F%lf",&f);
2503 if (strcmp (
"S",str) == 0) {
2504 srf->utm_northern_hemisphere = FALSE;
2505 }
else if (strcmp (
"N",str) == 0) {
2506 srf->utm_northern_hemisphere = TRUE;
2507 }
else if (str[0] ==
'Z') {
2509 sscanf(str,
"Z%d",&
zone);
2511 srf->xtm_zone =
zone;
2512 }
else if (strcmp(
"northing_first",str) == 0) {
2513 srf->xtm_northing_first = TRUE;
2514 }
else if (strcmp(
"easting_first",str) == 0) {
2515 srf->xtm_northing_first = FALSE;
2519 ConsoleMessage(
"geoSystem parameter %s not handled, in node %s",str,stringNodeType(nodeType));
2531 a = extra_ellipsoid[nextra_ellipsoid].a;
2532 b = extra_ellipsoid[nextra_ellipsoid].b;
2534 if(ee.a != 0.0 && ee.b != 0.0){
2535 ee.f = (ee.a-ee.b)/ee.a;
2536 }
else if(ee.a != 0.0){
2544 ifound = nextra_ellipsoid;
2545 for(i=1;i<nextra_ellipsoid;i++){
2546 if(extra_ellipsoid[i].a == ee.a && extra_ellipsoid[i].f == ee.f){
2551 extra_ellipsoid[ifound] = ee;
2552 srf->ellipsoid = -ifound;
2553 if(ifound == nextra_ellipsoid)
2558 printf (
"printf done compileGeoSystem\n");
2622 specversion = X3D_PROTO(node->_executionContext)->__specversion;
2624 planet = current_planet();
2625 if(!planet->autoOriginSet){
2626 if(geoOrigin && specversion < 330 ){
2629 struct SFVec3d offset, *poffset;
2634 initializeGeospatial(&geoOrigin);
2635 veccopyd(planet->autoOrigin.c,geoOrigin->__movedCoords.c);
2636 GeoOrient(X3D_NODE(geoOrigin), GEOSYS(geoOrigin->__geoSystem), &geoOrigin->__movedgd, &planet->autoOrient);
2637 planet->autoOriginSet = TRUE;
2640 user2gc(geoSystem,userCoord,1,&planet->autoOrigin);
2641 gc2gd(geoSystem,&planet->autoOrigin,1,&gdCoord);
2642 GeoOrient(X3D_NODE(geoOrigin), geoSystem, &gdCoord, &planet->autoOrient);
2643 planet->autoOriginSet = TRUE;
2651 struct SFVec3d gcCoord, lcsCoord;
2653 COMPILE_GEOSYSTEM(node)
2654 gs = GEOSYS(node->__geoSystem);
2655 FREE_IF_NZ(node->__movedCoords.p);
2656 node->__movedCoords.p = MALLOC (
struct SFVec3f *,
sizeof (
struct SFVec3f) *node->point.n);
2658 for(i=0;i<node->point.n;i++){
2659 user2gc(gs,&node->point.p[i],1,&gcCoord);
2660 gc2lcs(gs,&gcCoord,1,&lcsCoord);
2661 double2float(node->__movedCoords.p[i].c,lcsCoord.c,3);
2663 node->__movedCoords.n = node->point.n;
2676int checkX3DGeoElevationGridFields (
struct X3D_GeoElevationGrid *node,
float **points,
int *npoints) {
2690 float *texcoord = NULL;
2695 nx = node->xDimension;
2696 xSp = node->xSpacing;
2697 nz = node->zDimension;
2698 zSp = node->zSpacing;
2699 height = node->height.p;
2700 nh = node->height.n;
2702 COMPILE_GEOSYSTEM(node)
2704 if (node->__geoSystem != NULL) {
2705 mySRF = GEOSYS(node->__geoSystem)->spatial_system;
2714 ntri = (nx && nz ? 2 * (nx-1) * (nz-1) : 0);
2720 printf (
"GeoElevationgrid: warning: x,y vs. height: %d * %d ne %d:\n", nx,nz,nh);
2722 printf (
"GeoElevationgrid: error: x,y vs. height: %d * %d ne %d:\n", nx,nz,nh);
2728 if ((nx < 2) || (nz < 2)) {
2729 printf (
"GeoElevationGrid: xDimension and zDimension less than 2 %d %d\n", nx,nz);
2737 if (!(node->texCoord)) {
2739 FREE_IF_NZ(rep->GeneratedTexCoords[0]);
2742 texcoord = rep->GeneratedTexCoords[0] = MALLOC (
float *,
sizeof (
float) * nquads * 12);
2749 newpoints = MALLOC (
float *,
sizeof (
float) * nz * nx * 3);
2751 FREE_IF_NZ(rep->actualCoord);
2752 rep->actualCoord = (
float *)newpoints;
2755 if (node->_coordIndex.n > 0) {FREE_IF_NZ(node->_coordIndex.p);}
2756 node->_coordIndex.p = MALLOC (
int *,
sizeof(
int) * nquads * 5);
2757 cindexptr = node->_coordIndex.p;
2759 node->_coordIndex.n = nquads * 5;
2761 *points = newpoints;
2762 *npoints = node->_coordIndex.n;
2765 printf (
"coordindex:\n");
2771 for (j = 0; j < (nz -1); j++) {
2772 for (i=0; i < (nx-1) ; i++) {
2774 printf (
" %d %d %d %d %d\n", j*nx+i, j*nx+i+nx, j*nx+i+nx+1, j*nx+i+1, -1);
2777#ifdef WINDING_ELEVATIONGRID
2778 *cindexptr = j*nx+i; cindexptr++;
2779 *cindexptr = j*nx+i+nx; cindexptr++;
2780 *cindexptr = j*nx+i+nx+1; cindexptr++;
2781 *cindexptr = j*nx+i+1; cindexptr++;
2782 *cindexptr = -1; cindexptr++;
2784 *cindexptr = j*nx+i; cindexptr++;
2785 *cindexptr = j*nx+i+1; cindexptr++;
2786 *cindexptr = j*nx+i+nx+1; cindexptr++;
2787 *cindexptr = j*nx+i+nx; cindexptr++;
2788 *cindexptr = -1; cindexptr++;
2796 if (!(node->texCoord)) {
2798 for (j = 0; j < (nz -1); j++) {
2799 for (i=0; i < (nx-1) ; i++) {
2801#ifdef WINDING_ELEVATIONGRID
2803 *texcoord = ((float) (i+0)/(nx-1)); texcoord++;
2804 *texcoord = ((float)(j+0)/(nz-1)); texcoord ++;
2806 *texcoord = ((float) (i+0)/(nx-1)); texcoord++;
2807 *texcoord = ((float)(j+1)/(nz-1)); texcoord ++;
2809 *texcoord = ((float) (i+1)/(nx-1)); texcoord++;
2810 *texcoord = ((float)(j+1)/(nz-1)); texcoord ++;
2813 *texcoord = ((float) (i+0)/(nx-1)); texcoord++;
2814 *texcoord = ((float)(j+0)/(nz-1)); texcoord ++;
2816 *texcoord = ((float) (i+1)/(nx-1)); texcoord++;
2817 *texcoord = ((float)(j+1)/(nz-1)); texcoord ++;
2819 *texcoord = ((float) (i+1)/(nx-1)); texcoord++;
2820 *texcoord = ((float)(j+0)/(nz-1)); texcoord ++;
2823 *texcoord = ((float) (i+0)/(nx-1)); texcoord++;
2824 *texcoord = ((float) (j+0)/(nz-1)); texcoord++;
2826 *texcoord = ((float) (i+1)/(nx-1)); texcoord++;
2827 *texcoord = ((float) (j+0)/(nz-1)); texcoord++;
2829 *texcoord = ((float) (i+1)/(nx-1)); texcoord++;
2830 *texcoord = ((float) (j+1)/(nz-1)); texcoord++;
2833 *texcoord = ((float) (i+0)/(nx-1)); texcoord++;
2834 *texcoord = ((float) (j+0)/(nz-1)); texcoord++;
2836 *texcoord = ((float) (i+1)/(nx-1)); texcoord++;
2837 *texcoord = ((float) (j+1)/(nz-1)); texcoord++;
2839 *texcoord = ((float) (i+0)/(nx-1)); texcoord++;
2840 *texcoord = ((float) (j+1)/(nz-1)); texcoord++;
2853 mIN.p = MALLOC (
struct SFVec3d *,
sizeof (
struct SFVec3d) * mIN.n);
2855 mOUT.n=0; mOUT.p = NULL;
2856 gdCoords.n=0; gdCoords.p = NULL;
2857 struct SFVec3d lastpoint, firstpoint;
2859 for (j=0; j<nz; j++) {
2860 for (i=0; i < nx; i++) {
2863 printf (
" %lf %lf %lf # (hei ind %d) point [%d, %d]\n",
2865 height[i+(j*nx)] * ((
double)node->yScale),
2873 if ((mySRF == GEOSP_GD) || (mySRF == GEOSP_UTM) || (mySRF == GEOSP_3TM) || (mySRF == GEOSP_WM)) {
2877 mIN.p[k].c[0] = zSp * j + node->geoGridOrigin.c[0];
2880 mIN.p[k].c[1] =xSp * i + node->geoGridOrigin.c[1];
2883 mIN.p[k].c[2] = (height[k] *(node->yScale)) + node->geoGridOrigin.c[2];
2884 veccopyd(lastpoint.c,mIN.p[k].c);
2885 if(i==0 && j==0) veccopyd(firstpoint.c,mIN.p[k].c);
2889 mIN.p[k].c[0] = zSp * j + node->geoGridOrigin.c[0];
2891 mIN.p[k].c[1] =xSp * i + node->geoGridOrigin.c[1];
2893 mIN.p[k].c[2] = (height[k] *(node->yScale)) + node->geoGridOrigin.c[2];
2901 vecprint3db(
"firstpoint",firstpoint.c,
"\n");
2902 vecprint3db(
" lastpoint",lastpoint.c,
"\n");
2903 vecprint3db(
"nx*nz",mIN.p[nx*nz -1].c,
"\n");
2904 printf (
"points before moving origin, lat, lon, height, index:\n");
2905 for (j=0; j<nz; j++) {
2906 for (i=0; i < nx; i++) {
2908 printf (
" %lf %lf %lf %d\n",mIN.p[k].c[0],
2909 mIN.p[k].c[1],mIN.p[k].c[2],k);
2919 compile_geoSystem(X3D_NODE(node),node->_nodeType,&node->geoSystem,&node->__geoSystem);
2920 gs = GEOSYS(node->__geoSystem);
2922 update_origin(gs, X3D_NODE(node), &node->geoGridOrigin, X3D_GEOORIGIN(node->geoOrigin));
2925 user2gc(gs,mIN.p,mIN.n,mOUT.p);
2927 printf (
"points in gc XYZ, index:\n");
2928 for (j=0; j<nz; j++) {
2929 for (i=0; i < nx; i++) {
2930 double ci[3], co[3];
2932 veccopyd(co,mOUT.p[k].c);
2933 printf (
" %lf %lf %lf %d\n",co[0],co[1],co[2],k);
2934 veccopyd(ci,mIN.p[k].c);
2935 printf (
" %lf %lf %lf %d\n",ci[0],ci[1],ci[2],k);
2942 gc2lcs(gs,mOUT.p,mOUT.n,mOUT.p);
2950 for (j=0; j<nz; j++) {
2951 for (i=0; i < nx; i++) {
2954 double2float(newpoints,mOUT.p[k].c,3);
2963 printf (
"points converted to mesh coords, xyz index:\n");
2964 newpoints = rep->actualCoord;
2965 for (j=0; j<nz; j++) {
2966 for (i=0; i < nx; i++) {
2969 printf (
" %f %f %f %d\n",newpoints[0],newpoints[1],newpoints[2],k);
2984int planetInPlanets(
int planet,
struct Multi_Int32 *planets){
2986 for(i=0;i<planets->n;i++)
2987 if(planets->p[i] == planet) ifound = i;
2990void RegisterGeoElevationGrid(
struct X3D_Node *node,
int planetID);
2992 if( extent6f_isSet(node->_extent)) {
2994 extent6f_rotate4d(ef6, node->_extent, node->__localOrient.c);
2995 extent6f_translate3d(ef6,ef6,node->__autoOffset.c);
2996 union_group_extent(ef6);
3009 initializeGeospatial((
struct X3D_GeoOrigin **) &node->geoOrigin);
3010 planetID = current_planetId();
3012 if (!compile_poly_if_required(node, NULL, NULL, node->color, node->normal, node->texCoord))
return;
3014 CULL_FACE(node->solid)
3015 render_polyrep(node);
3016 if(!planetInPlanets(planetID,&node->__planets)){
3019 RegisterGeoElevationGrid(X3D_NODE(node), planetID);
3020 node->__planets.p = realloc(node->__planets.p,(node->__planets.n+1)*
sizeof(
int));
3021 node->__planets.p[node->__planets.n] = planetID;
3022 node->__planets.n++;
3024 setExtentGeoElevationGrid(node);
3034 struct SFVec3d gdCoord, gcCoord;
3039 planet = current_planet();
3041 printf (
"compiling GeoLocation\n");
3044 compile_geoSystem(X3D_NODE(node),node->_nodeType,&node->geoSystem,&node->__geoSystem);
3045 gs = GEOSYS(node->__geoSystem);
3046 if(node->relativeHeight) gs->relativeHeight = TRUE;
3048 update_origin(gs, X3D_NODE(node), &node->geoCoords, X3D_GEOORIGIN(node->geoOrigin));
3051 MARK_SFVEC3D_INOUT_EVENT(node->geoCoords, node->__oldgeoCoords, offsetof (
struct X3D_GeoLocation, geoCoords))
3054 MARK_MFNODE_INOUT_EVENT(node->children, node->__oldChildren, offsetof (
struct X3D_GeoLocation, children))
3056 REINITIALIZE_SORTED_NODES_FIELD(node->children,node->_sortedChildren);
3065 printf (
"compiled GeoLocation\n\n");
3089 RETURN_FROM_CHILD_IF_NOT_FOR_ME
3092 prep_sibAffectors((
struct X3D_Node*)node,&node->__sibAffectors);
3094 prep_BBox((
struct BBoxFields*)&node->bboxCenter);
3095 normalChildren(node->children);
3097 fin_sibAffectors((
struct X3D_Node*)node,&node->__sibAffectors);
3114 if(!renderstate()->render_vp) {
3115 geoprep(GEOSYS(node->__geoSystem),&node->geoCoords);
3125 geofin(GEOSYS(node->__geoSystem),&node->geoCoords);
3131void add_node_to_broto_context(
struct X3D_Proto *currentContext,
struct X3D_Node *node);
3133void deleteMallocedFieldValue(
int type,
union anyVrml *fieldPtr);
3137 if (childUrl->n > 0) {
3139 if (*childNode == NULL) {
3140 *childNode = createNewX3DNode(NODE_Inline);
3141 if(node->_executionContext)
3142 add_node_to_broto_context(X3D_PROTO(node->_executionContext),X3D_NODE(*childNode));
3143 ADD_PARENT(X3D_NODE(*childNode), X3D_NODE(node));
3146 deleteMallocedFieldValue(FIELDTYPE_MFString,(
union anyVrml*)&X3D_INLINE(*childNode)->url);
3147 X3D_INLINE(*childNode)->url.p = MALLOC(
struct Uni_String **,
sizeof(
struct Uni_String)*childUrl->n);
3148 for (i=0; i<childUrl->n; i++) {
3150 X3D_INLINE(*childNode)->url.p[i] = newASCIIString(childUrl->p[i]->strptr);
3153 X3D_INLINE(*childNode)->url.n = childUrl->n;
3154 X3D_INLINE(*childNode)->load = TRUE;
3158#define UNLOAD_CHILD(childNode) \
3159 if (node->childNode != NULL) \
3160 X3D_INLINE(node->childNode)->load = FALSE;
3163static void GeoLODchildren (
struct X3D_GeoLOD *node) {
3164 int load = node->__inRange;
3167 if (((node->__childloadstatus)==0) && (load)) {
3169 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
3171 printf (
"GeoLODchildren - have to LOAD_CHILD for node %u (level %d)\n",node,p->geoLodLevel);
3174 LOAD_CHILD(node,&node->__child1Node,&node->child1Url);
3175 LOAD_CHILD(node,&node->__child2Node,&node->child2Url);
3176 LOAD_CHILD(node,&node->__child3Node,&node->child3Url);
3177 LOAD_CHILD(node,&node->__child4Node,&node->child4Url);
3183 node->__childloadstatus = 1;
3189static void GeoUnLODchildren (
struct X3D_GeoLOD *node) {
3190 int load = node->__inRange;
3192 if (!(load) && ((node->__childloadstatus) != 0)) {
3194 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
3195 printf (
"GeoLODloadChildren, removing children from node %u level %d\n",node,p->geoLodLevel);
3197 UNLOAD_CHILD(__child1Node)
3198 UNLOAD_CHILD(__child2Node)
3199 UNLOAD_CHILD(__child3Node)
3200 UNLOAD_CHILD(__child4Node)
3202 node->__childloadstatus = 0;
3207static void GeoLODrootUrl (
struct X3D_GeoLOD *node) {
3208 int load = node->__inRange == 0;
3211 if (((node->__rooturlloadstatus)==0) && (load)) {
3213 printf (
"GeoLODrootUrl - have to LOAD_CHILD for node %u\n",node);
3216 LOAD_CHILD(node,&node->__rootUrl, &node->rootUrl);
3219 node->__rooturlloadstatus = 1;
3224static void GeoUnLODrootUrl (
struct X3D_GeoLOD *node) {
3225 int load = node->__inRange;
3227 if (!(load) && ((node->__rooturlloadstatus) != 0)) {
3229 printf (
"GeoLODloadChildren, removing rootUrl\n");
3231 node->__childloadstatus = 0;
3237void compile_GeoLOD (
struct X3D_GeoLOD * node) {
3240 compile_geoSystem(X3D_NODE(node),node->_nodeType,&node->geoSystem,&node->__geoSystem);
3241 gs = GEOSYS(node->__geoSystem);
3243 update_origin(gs, X3D_NODE(node), &node->center, X3D_GEOORIGIN(node->geoOrigin));
3244 user2gc(gs,&node->center,1,&gcCoord);
3245 gc2lcs(gs,&gcCoord,1,&node->__movedCoords);
3254 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
3259 printf (
"child_GeoLOD %u (level %d), renderFlags %x render_hier vp %d geom %d light %d sens %d blend %d prox %d col %d\n",
3263 render_vp,render_geom,render_light,render_sensitive,render_blend,render_proximity,render_collision);
3267 if (node->__level == -1) node->__level = p->geoLodLevel;
3268 else if (node->__level != p->geoLodLevel) {
3269 printf (
"hmmm - GeoLOD %p was level %d, now %d\n",node,node->__level, p->geoLodLevel);
3273 prep_BBox((
struct BBoxFields*)&node->bboxCenter);
3277 if ( node->__inRange) {
3278 printf (
"GeoLOD %u (level %d) closer\n",node,p->geoLodLevel);
3280 printf (
"GeoLOD %u (level %d) farther away\n",node,p->geoLodLevel);
3286 if (!(node->__inRange)) {
3289 GeoUnLODchildren (node);
3291 if (node->rootNode.n != 0) {
3292 for (i=0; i<node->rootNode.n; i++) {
3294 printf (
"GeoLOD %u is rendering rootNode %u",node,node->rootNode.p[i]);
3295 if (node->rootNode.p[i]!=NULL) printf (
" (%s) ",stringNodeType(X3D_NODE(node->rootNode.p[i])->_nodeType));
3299 render_node (node->rootNode.p[i]);
3301 }
else if (node->rootUrl.n != 0) {
3304 GeoLODrootUrl (node);
3307 if (node->__rootUrl != NULL) {
3309 printf (
"GeoLOD %u is rendering rootUrl %u",node,node->__rootUrl);
3310 if (node->__rootUrl != NULL) printf (
" (%s) ", stringNodeType(X3D_NODE(node->__rootUrl)->_nodeType));
3314 render_node (node->__rootUrl);
3323 GeoLODchildren (node);
3326 GeoUnLODrootUrl (node);
3329 printf (
"rendering children at %d, they are: ",p->geoLodLevel);
3330 if (node->child1Url.n>0) printf (
" :%s: ",node->child1Url.p[0]->strptr);
3331 if (node->child2Url.n>0) printf (
" :%s: ",node->child2Url.p[0]->strptr);
3332 if (node->child3Url.n>0) printf (
" :%s: ",node->child3Url.p[0]->strptr);
3333 if (node->child4Url.n>0) printf (
" :%s: ",node->child4Url.p[0]->strptr);
3339 printf (
"GeoLOD %u is rendering children %u ", node, node->__child1Node);
3340 if (node->__child1Node != NULL) printf (
" (%s) ",stringNodeType(X3D_NODE(node->__child1Node)->_nodeType));
3341 printf (
" %u ", node->__child2Node);
3342 if (node->__child2Node != NULL) printf (
" (%s) ",stringNodeType(X3D_NODE(node->__child2Node)->_nodeType));
3343 printf (
" %u ", node->__child3Node);
3344 if (node->__child3Node != NULL) printf (
" (%s) ",stringNodeType(X3D_NODE(node->__child3Node)->_nodeType));
3345 printf (
" %u ", node->__child4Node);
3346 if (node->__child4Node != NULL) printf (
" (%s) ",stringNodeType(X3D_NODE(node->__child4Node)->_nodeType));
3350 if (node->__child1Node != NULL) render_node (node->__child1Node);
3351 if (node->__child2Node != NULL) render_node (node->__child2Node);
3352 if (node->__child3Node != NULL) render_node (node->__child3Node);
3353 if (node->__child4Node != NULL) render_node (node->__child4Node);
3368 printf (
"compiling GeoMetadata\n");
3381 printf (
"compiling GeoOrigin\n");
3384 ConsoleMessage (
"compiling GeoOrigin\n");
3386 COMPILE_GEOSYSTEM(node)
3390 node->__rotyup.c[i] = 0.0;
3391 node->__rotyup.c[1] = 1.0;
3397 MARK_SFVEC3D_INOUT_EVENT(node->geoCoords,node->__oldgeoCoords,offsetof (
struct X3D_GeoOrigin, geoCoords))
3409 printf (
"compiling GeoPositionInterpolator\n");
3414 struct SFVec3d gcCoord, lcsCoord;
3415 compile_geoSystem(X3D_NODE(node),node->_nodeType,&node->geoSystem,&node->__geoSystem);
3416 gs = GEOSYS(node->__geoSystem);
3417 FREE_IF_NZ(node->__movedValue.p);
3418 node->__movedValue.p = MALLOC(
struct SFVec3f*,node->keyValue.n *
sizeof(
struct SFVec3f));
3419 node->__movedValue.n = node->keyValue.n;
3420 for(i=0;i<node->keyValue.n;i++){
3421 user2gc(gs,&node->keyValue.p[i],1,&gcCoord);
3422 gc2lcs(gs,&gcCoord,1,&lcsCoord);
3423 double2float(node->__movedValue.p[i].c,lcsCoord.c,3);
3441void do_GeoPositionInterpolator (
void *innode) {
3444 int kin, kvin, counter, tmp;
3449 if (!innode)
return;
3452 if (NODE_NEEDS_COMPILING) compile_GeoPositionInterpolator(node);
3453 kvin = node->__movedValue.n;
3454 kVs_lcs = node->__movedValue.p;
3455 kVs_user = node->keyValue.p;
3461 if (node->__oldKeyValuePtr.p != node->keyValue.p) {
3463 node->__oldKeyValuePtr.p= node->keyValue.p;
3465 if (node->__oldKeyPtr.p != node->key.p) {
3467 node->__oldKeyPtr.p = node->key.p;
3472 printf(
"do_GeoPos: Position/Color interp, node %u kin %d kvin %d set_fraction %f\n",
3473 node, kin, kvin, node->set_fraction);
3477 if ((kvin == 0) || (kin == 0)) {
3478 node->value_changed.c[0] = (float) 0.0;
3479 node->value_changed.c[1] = (float) 0.0;
3480 node->value_changed.c[2] = (float) 0.0;
3481 node->geovalue_changed.c[0] = 0.0;
3482 node->geovalue_changed.c[1] = 0.0;
3483 node->geovalue_changed.c[2] = 0.0;
3487 if (kin>kvin) kin=kvin;
3490 if (node->set_fraction <= ((node->key).p[0])) {
3491 veccopyd(node->geovalue_changed.c,kVs_user[0].c);
3492 veccopy3f(node->value_changed.c,kVs_lcs[0].c);
3493 }
else if (node->set_fraction >= node->key.p[kin-1]) {
3494 memcpy ((
void *)&node->geovalue_changed, (
void *)&kVs_user[kvin-1], sizeof (
struct SFVec3d));
3495 veccopyd(node->geovalue_changed.c,kVs_user[kvin-1].c);
3496 veccopy3f(node->value_changed.c,kVs_lcs[kvin-1].c);
3499 float fpart, fdif[3];
3500 double dpart, ddif[3];
3501 counter = find_key(kin,((
float)(node->set_fraction)),node->key.p);
3504 fpart = (node->set_fraction - node->key.p[counter-1]) /
3505 (node->key.p[counter] - node->key.p[counter-1]);
3506 veclerp3f(node->value_changed.c,kVs_lcs[counter-1].c,kVs_lcs[counter].c,fpart);
3510 veclerpd(node->geovalue_changed.c,kVs_user[counter-1].c,kVs_user[counter].c,dpart);
3528 printf (
"Pos/Col, new value (%f %f %f)\n",
3529 node->value_changed.c[0],node->value_changed.c[1],node->value_changed.c[2]);
3530 printf (
"geovalue_changed %lf %lf %lf\n",node->geovalue_changed.c[0], node->geovalue_changed.c[1], node->geovalue_changed.c[2]);
3542 printf (
"compiling GeoProximitySensor\n");
3546 struct SFVec3d gcCoord, lcsCoord, gdCoord;
3547 specversion = X3D_PROTO(node->_executionContext)->__specversion;
3548 if(specversion < 330){
3551 if(!(veclengthd(node->geoCenter.c) == 0.0))
3552 veccopyd(node->center.c,node->geoCenter.c);
3555 compile_geoSystem(X3D_NODE(node),node->_nodeType,&node->geoSystem,&node->__geoSystem);
3556 gs = GEOSYS(node->__geoSystem);
3557 user2gc(gs,&node->center,1,&gcCoord);
3558 gc2lcs(gs,&gcCoord,1,&node->__movedCoords);
3559 gc2gd(gs,&gcCoord,1,&gdCoord);
3560 GeoOrient(node->geoOrigin, GEOSYS(node->__geoSystem), &gdCoord, &node->__localOrient);
3562 MARK_SFVEC3D_INOUT_EVENT(node->geoCenter, node->__oldGeoCenter,offsetof (
struct X3D_GeoProximitySensor, geoCenter))
3570 printf (
"compiled GeoProximitySensor\n\n");
3575void tcs2gcB_transform(Geosys *geoSystem,
struct SFVec3d *gcCoord,
struct SFVec3d* translate,
struct SFVec4d *rotate)
3579 Geosys *gs = geoSystem;
3581 getEllipsoidParams(gs->ellipsoid, &A, &F);
3582 if(gs->spatial_system == GEOSP_GC && veclengthd(gcCoord->c) < A/2.0){
3583 vecset4d(rotate->c,0.0,1.0,0.0,0.0);
3584 veccopyd(translate->c,gcCoord->c);
3586 gc2gd(gs,gcCoord,1, &gdCoord);
3587 tcs2gc_transform(gs,&gdCoord,rotate,translate);
3590void gc2tcsB_transform(Geosys *geoSystem,
struct SFVec3d *gcCoord,
struct SFVec3d* translate,
struct SFVec4d *rotate)
3594 Geosys *gs = geoSystem;
3596 getEllipsoidParams(gs->ellipsoid, &A, &F);
3597 if(gs->spatial_system == GEOSP_GC && veclengthd(gcCoord->c) < A/2.0){
3599 vecset4d(rotate->c,0.0,1.0,0.0,0.0);
3600 vecsetd(translate->c,-gcCoord->c[0],-gcCoord->c[1],-gcCoord->c[2]);
3602 gc2gd(gs,gcCoord,1, &gdCoord);
3603 gc2tcs_transform(gs,&gdCoord,translate,rotate);
3609void geoprep0(Geosys *geoSystem,
struct SFVec3d *userCoord){
3612 struct SFVec3d translation1, translation2, gcCoord, gdCoord;
3613 struct SFVec4d rotation1, rotation2;
3615 Geosys *gs = geoSystem;
3632 getEllipsoidParams(gs->ellipsoid, &A, &F);
3633 if(gs->spatial_system == GEOSP_GC && veclengthd(userCoord->c) < A/2.0){
3636 FW_GL_TRANSLATE_D(userCoord->c[0],userCoord->c[1],userCoord->c[2]);
3638 user2gc(gs,userCoord,1,&gcCoord);
3640 gc2lcs_transform(&translation1,&rotation1);
3641 FW_GL_ROTATE_RADIANS(rotation1.c[3],rotation1.c[0],rotation1.c[1],rotation1.c[2]);
3642 FW_GL_TRANSLATE_D(translation1.c[0],translation1.c[1],translation1.c[2]);
3643 gc2gd(gs,&gcCoord,1, &gdCoord);
3644 tcs2gc_transform(gs,&gdCoord,&rotation2,&translation2);
3645 FW_GL_TRANSLATE_D(translation2.c[0],translation2.c[1],translation2.c[2]);
3646 FW_GL_ROTATE_RADIANS(rotation2.c[3],rotation2.c[0],rotation2.c[1],rotation2.c[2]);
3649 user2gc(gs,userCoord,1,&gcCoord);
3650 gc2lcs_transform(&translation1,&rotation1);
3651 FW_GL_ROTATE_RADIANS(rotation1.c[3],rotation1.c[0],rotation1.c[1],rotation1.c[2]);
3652 FW_GL_TRANSLATE_D(translation1.c[0],translation1.c[1],translation1.c[2]);
3653 tcs2gcB_transform(gs,&gcCoord,&translation2,&rotation2);
3654 FW_GL_TRANSLATE_D(translation2.c[0],translation2.c[1],translation2.c[2]);
3655 FW_GL_ROTATE_RADIANS(rotation2.c[3],rotation2.c[0],rotation2.c[1],rotation2.c[2]);
3658void geoprep(Geosys *geoSystem,
struct SFVec3d *userCoord){
3660 if(!renderstate()->render_vp) {
3661 FW_GL_PUSH_MATRIX();
3662 push_transform_local_identity();
3663 FW_GL_PUSH_MATRIX();
3664 FW_GL_LOAD_IDENTITY();
3665 geoprep0(geoSystem,userCoord);
3669 FW_GL_GETDOUBLEV(GL_MODELVIEW_MATRIX,mat);
3671 FW_GL_TRANSFORM_D(mat);
3672 reset_transform_local(mat);
3678void geoprepT0(Geosys *geoSystem,
struct SFVec3d *userCoord);
3679void geofin(Geosys *geoSystem,
struct SFVec3d *userCoord){
3681 if(!renderstate()->render_vp) {
3682 pop_transform_local();
3685 geoprepT0(geoSystem,userCoord);
3691 if(renderstate()->render_geom && fwl_getDrawBoundingBoxes()) {
3693 geoprep(GEOSYS(node->__geoSystem),&node->center);
3695 double2float(center,node->center.c,3);
3696 bbox2extent6f(center,node->size.c,node->_extent);
3697 draw_bbox(center,node->size.c);
3699 extent6f_mattransform4d(node->_extent,node->_extent,peek_transform_local());
3700 union_group_extent(node->_extent);
3702 geofin(GEOSYS(node->__geoSystem),&node->center);
3714 static struct point_XYZ yvec = {.x=0,.y=0.05,.z=0};
3715 static struct point_XYZ zvec = {.x=0,.y=0,.z=-0.05};
3716 static struct point_XYZ zpvec = {.x=0,.y=0,.z=0.05};
3717 static struct point_XYZ orig = {.x=0,.y=0,.z=0};
3718 struct point_XYZ t_zvec, t_yvec, t_orig, t_center;
3719 GLDOUBLE modelMatrix[16];
3720 GLDOUBLE projMatrix[16];
3721 GLDOUBLE view2prox[16];
3723 if(!((node->enabled)))
return;
3726 geoprep(GEOSYS(node->__geoSystem),&node->center);
3727 FW_GL_GETDOUBLEV(GL_MODELVIEW_MATRIX, modelMatrix);
3728 geofin(GEOSYS(node->__geoSystem),&node->center);
3729 matinverseAFFINE(view2prox,modelMatrix);
3732 transform(&t_orig,&orig,view2prox);
3734 transform(&t_center,&orig, view2prox);
3750 vecscale3f(cc,node->size.c,.5);
3751 extent6f_constructor(node->_extent,-cc[0],cc[0],-cc[1],cc[1],-cc[2],cc[2]);
3754 if(((node->size).c[0]) == 0 || ((node->size).c[1]) == 0 || ((node->size).c[2]) == 0)
return;
3756 if(fabs(cx) > ((node->size).c[0])/2 ||
3757 fabs(cy) > ((node->size).c[1])/2 ||
3758 fabs(cz) > ((node->size).c[2])/2)
return;
3766 ((node->__t1).c[0]) = (
float)t_center.x;
3767 ((node->__t1).c[1]) = (
float)t_center.y;
3768 ((node->__t1).c[2]) = (
float)t_center.z;
3774 struct SFVec3d tcsCoord, gcCoord, userCoord;
3775 matrix_to_quaternion(&quat,modelMatrix);
3776 quaternion_normalize(&quat);
3777 quaternion_to_vrmlrot(&quat,&oo[0],&oo[1],&oo[2],&oo[3]);
3780 double2float(node->__t2.c,oo,4);
3781 float2double(tcsCoord.c,node->__t1.c,3);
3784 struct X3D_Node *boundvp = getActiveLayerBoundViewpoint();
3786 if(boundvp && boundvp->_nodeType == NODE_GeoViewpoint){
3787 struct SFVec3d gcCoord, geoCoord;
3789 user2gc(GEOSYS(gvp->__geoSystem),&gvp->position,1,&gcCoord);
3791 tcs2gc(GEOSYS(node->__geoSystem),&node->center,&tcsCoord,1,&gcCoord);
3793 gc2user(GEOSYS(node->__geoSystem),&gcCoord,1,&userCoord);
3794 veccopyd(node->__t3.c,userCoord.c);
3801void do_GeoProximitySensorTick(
void *ptr) {
3807 if (node->__oldEnabled != node->enabled) {
3808 node->__oldEnabled = node->enabled;
3811 if (!node->enabled)
return;
3816 if (!node->isActive) {
3818 printf (
"PROX - initial defaults\n");
3822 node->enterTime = TickTime();
3829 if (memcmp ((
void *) &node->position_changed,(
void *) &node->__t1,
sizeof(
struct SFColor))) {
3831 printf (
"PROX - position changed!!! \n");
3834 memcpy ((
void *) &node->position_changed,
3835 (
void *) &node->__t1,
sizeof(
struct SFColor));
3839 printf (
"do_GeoProximitySensorTick, position changed; it now is %lf %lf %lf\n",node->position_changed.c[0],
3840 node->position_changed.c[1], node->position_changed.c[2]);
3841 printf (
"nearPlane is %lf\n",Viewer.nearPlane);
3845 if(!vecsamed(node->geoCoord_changed.c,node->__t3.c)){
3846 veccopyd(node->geoCoord_changed.c,node->__t3.c);
3850 if (memcmp ((
void *) &node->orientation_changed, (
void *) &node->__t2,
sizeof(
struct SFRotation))) {
3852 printf (
"PROX - orientation changed!!!\n ");
3855 memcpy ((
void *) &node->orientation_changed,
3856 (
void *) &node->__t2,
sizeof(
struct SFRotation));
3860 if (node->isActive) {
3862 printf (
"PROX - stopping\n");
3866 node->exitTime = TickTime();
3882 printf (
"compiling GeoTouchSensor\n");
3884 compile_geoSystem(X3D_NODE(node),node->_nodeType,&node->geoSystem,&node->__geoSystem);
3892void do_GeoTouchSensor (
void *ptr,
int ev,
int but1,
int over) {
3901 printf (
"%lf: TS ",TickTime());
3902 if (ev==ButtonPress) printf (
"ButtonPress ");
3903 else if (ev==ButtonRelease) printf (
"ButtonRelease ");
3904 else if (ev==KeyPress) printf (
"KeyPress ");
3905 else if (ev==KeyRelease) printf (
"KeyRelease ");
3906 else if (ev==MotionNotify) printf (
"%lf MotionNotify ");
3907 else printf (
"ev %d ",ev);
3909 if (but1) printf (
"but1 TRUE ");
else printf (
"but1 FALSE ");
3910 if (over) printf (
"over TRUE ");
else printf (
"over FALSE ");
3916 if (node->__oldEnabled != node->enabled) {
3917 node->__oldEnabled = node->enabled;
3920 if (!node->enabled)
return;
3923 if ((ev == overMark) && (over != node->isOver)) {
3925 printf (
"TS %u, isOver changed %d\n",node, over);
3927 node->isOver = over;
3933 if (ev == ButtonPress) {
3937 printf (
"touchSens %u, butPress\n",node);
3940 node->touchTime = TickTime();
3943 }
else if (ev == ButtonRelease) {
3945 printf (
"touchSens %u, butRelease\n",node);
3953 memcpy ((
void *) &node->_oldhitPoint, (
void *) &tg->RenderFuncs.ray_save_posn,
sizeof(
struct SFColor));
3956 if ((APPROX(node->_oldhitPoint.c[0],node->hitPoint_changed.c[0])!= TRUE) ||
3957 (APPROX(node->_oldhitPoint.c[1],node->hitPoint_changed.c[1])!= TRUE) ||
3958 (APPROX(node->_oldhitPoint.c[2],node->hitPoint_changed.c[2])!= TRUE)) {
3961 printf (
"GeoTouchSens, hitPoint changed: %f %f %f\n",node->hitPoint_changed.c[0],
3962 node->hitPoint_changed.c[1], node->hitPoint_changed.c[2]);
3965 memcpy ((
void *) &node->hitPoint_changed, (
void *) &node->_oldhitPoint,
sizeof(
struct SFColor));
3970 node->hitGeoCoord_changed.c[0] = (double) node->hitPoint_changed.c[0];
3971 node->hitGeoCoord_changed.c[1] = (double) node->hitPoint_changed.c[1];
3972 node->hitGeoCoord_changed.c[2] = (double) node->hitPoint_changed.c[2];
3977 printf (
"\nhitGeoCoord_changed as a GCC, %lf %lf %lf\n",
3978 node->hitGeoCoord_changed.c[0],
3979 node->hitGeoCoord_changed.c[1],
3980 node->hitGeoCoord_changed.c[2]);
3983 Geosys *gs = GEOSYS(node->__geoSystem);
3984 lcs2gc(gs,&node->hitGeoCoord_changed,1,&gcCoord);
3985 gc2user(gs,&gcCoord,1,&node->hitGeoCoord_changed);
3990 normalval.x = tg->RenderFuncs.hyp_save_norm[0];
3991 normalval.y = tg->RenderFuncs.hyp_save_norm[1];
3992 normalval.z = tg->RenderFuncs.hyp_save_norm[2];
3993 normalize_vector(&normalval);
3994 node->_oldhitNormal.c[0] = (float) normalval.x;
3995 node->_oldhitNormal.c[1] = (float) normalval.y;
3996 node->_oldhitNormal.c[2] = (float) normalval.z;
3999 MARK_SFVEC3F_INOUT_EVENT(node->hitNormal_changed,node->_oldhitNormal,offsetof (
struct X3D_GeoTouchSensor, hitNormal_changed))
4008#define strcasecmp _stricmp
4011 WALK_SURFACE_HIGHEST = 0,
4012 WALK_SURFACE_LOWEST = 1,
4013 WALK_SURFACE_PRIORITY = 2,
4015void calculateViewingSpeedB();
4019 compile_geoSystem(X3D_NODE(node),node->_nodeType,&node->geoSystem,&node->__geoSystem);
4020 gs = GEOSYS(node->__geoSystem);
4022 update_origin(gs, X3D_NODE(node), &node->position, X3D_GEOORIGIN(node->geoOrigin));
4023 user2gc(gs,&node->position,1,&gcCoord);
4024 gc2gd(gs,&gcCoord,1,&node->__movedgd);
4030 node->_walkSurfacePriority = 0;
4031 for(
int i=0;i<node->walkSurface.n;i++){
4032 if(!strcasecmp(node->walkSurface.p[i]->strptr,
"HIGHEST")) node->_walkSurfacePriority |= WALK_SURFACE_HIGHEST;
4033 if(!strcasecmp(node->walkSurface.p[i]->strptr,
"LOWEST")) node->_walkSurfacePriority |= WALK_SURFACE_LOWEST;
4034 if(!strcasecmp(node->walkSurface.p[i]->strptr,
"PRIORITY")) node->_walkSurfacePriority |= WALK_SURFACE_PRIORITY;
4038 MARK_SFFLOAT_INOUT_EVENT(node->fieldOfView, node->__oldFieldOfView, offsetof (
struct X3D_GeoViewpoint, fieldOfView))
4039 MARK_SFBOOL_INOUT_EVENT(node->headlight, node->__oldHeadlight, offsetof (
struct X3D_GeoViewpoint, headlight))
4040 MARK_SFBOOL_INOUT_EVENT(node->jump, node->__oldJump, offsetof (
struct X3D_GeoViewpoint, jump))
4049 printf (
"compiled GeoViewpoint\n\n");
4052struct X3D_Node *getActiveLayerBoundViewpoint();
4053void CONVERT_BACK_TO_GD_OR_UTMC(Geosys *targetGeoSystem,
struct X3D_Node *geoorigin,
4062 struct SFVec3d tcsCoord, gdCoord;
4066 gs = GEOSYS(node->__geoSystem);
4073 pointxyz2double(tcsCoord.c,Pos);
4074 user2gc(gs,&node->position,1,&gcCoord);
4075 gc2gd(gs,&gcCoord,1,&gdCoord);
4076 tcs2gc(gs,&gdCoord,&tcsCoord,1,&gcCoord);
4078 gc2user(gs,&gcCoord,1,&node->position);
4079 gc2gd(gs,&gcCoord,1,&gdCoord);
4086 int FREEFLY = !Viewer()->collision;
4090 struct SFVec3d translate1, translate2;
4091 struct SFVec4d rotate1, rotate2, rotate3;
4092 Quaternion q1,q2,q3,q4;
4093 gc2tcs_transform(gs, &node->__movedgd, &translate1, &rotate1);
4094 gc2tcs_transform(gs, &gdCoord, &translate2, &rotate2);
4095 rotate2.c[3] = -rotate2.c[3];
4096 vrmlrot4d_to_quaternion(&q1,rotate1.c);
4097 vrmlrot4d_to_quaternion(&q2,rotate2.c);
4098 quaternion_multiply(&q3,&q1,&q2);
4099 quaternion_multiply(&q4,
Quat,&q3);
4100 quaternion_to_vrmlrot4d(&q4,rotate3.c);
4101 rotate3.c[3] = -rotate3.c[3];
4102 double2float(node->orientation.c,rotate3.c,4);
4107 double oo[4], pp[3];
4108 double deltagd[3], gd[3];
4109 vecdifd(deltagd,node->__movedgd.c,gdCoord.c);
4110 veccopyd(gd,node->__movedgd.c);
4114 if(!gs->gd_latitude_first){
4116 vecswizzle2d(deltagd);
4119 if(gs->gd_degrees) {
4121 vecscale2d(deltagd,deltagd,RADIANS_PER_DEGREE);
4122 vecscale2d(gd,gd,RADIANS_PER_DEGREE);
4124 double dazimuth, dlambda;
4129 dlambda = angleNormalized(deltagd[1]);
4131 dazimuth = sin(gd[0])*dlambda;
4132 vrmlrot_to_quaternion(&qaz,0.0,1.0,0.0,-dazimuth);
4133 quaternion_multiply(&qq,
Quat,&qaz);
4137 quaternion_to_vrmlrot(&qq,&oo[0],&oo[1],&oo[2],&oo[3]);
4139 double2float(node->orientation.c,oo,4);
4143 veccopyd(node->__movedgd.c,gdCoord.c);
4154 struct SFVec3d gcCoord, gdCoord, tcsCoord;
4156 gs = GEOSYS(node->__geoSystem);
4158 float2double(oo,node->orientation.c,4);
4159 vrmlrot_to_quaternion(
Quat,oo[0],oo[1],oo[2], -oo[3]);
4160 user2gc(gs,&node->position,1,&gcCoord);
4161 gc2gd(gs,&gcCoord,1,&gdCoord);
4162 gc2tcs(gs,&gdCoord,&gcCoord,1,&tcsCoord);
4163 double2pointxyz(Pos,tcsCoord.c);
4177 Quaternion qoo, qlo, qao, qgc;
4180 double oo[4], gd[3];
4182 planet = current_planet();
4186 moveCoords3d(GEOSYS(node->__geoSystem),&planet->autoOrigin,&planet->autoOrient,&node->position,1,&LCpos,&node->__movedgd);
4187 vecnegated(LCpos.c,LCpos.c);
4188 double2pointxyz(Pos,LCpos.c);
4194 float2double(oo,node->orientation.c,4);
4196 vrmlrot_to_quaternion(&qoo,oo[0],oo[1],oo[2], oo[3]);
4197 GeoOrient(X3D_NODE(node->geoOrigin), GEOSYS(node->__geoSystem), &node->__movedgd, &lo);
4198 vrmlrot_to_quaternion(&qlo,lo.c[0],lo.c[1],lo.c[2], -lo.c[3]);
4199 quaternion_multiply(&qgc,&qoo,&qlo);
4202 vrmlrot_to_quaternion(&qao,planet->autoOrient.c[0],planet->autoOrient.c[1],planet->autoOrient.c[2], -planet->autoOrient.c[3]);
4203 quaternion_multiply(
Quat,&qgc,&qao);
4204 quaternion_normalize(
Quat);
4210 double pos[3], oo[4];
4211 struct SFVec3d GCpos, gdCoord;
4213 Quaternion qao, qaoi, qgc, qlo, qoo;
4216 planet = current_planet();
4220 vrmlrot_to_quaternion(&qao,planet->autoOrient.c[0],planet->autoOrient.c[1],planet->autoOrient.c[2],planet->autoOrient.c[3]);
4221 pointxyz2double(pos,Pos);
4222 vecnegated(pos,pos);
4223 quaternion_rotationd(pos,&qao,pos);
4224 vecaddd(GCpos.c,planet->autoOrigin.c,pos);
4227 CONVERT_BACK_TO_GD_OR_UTMC(GEOSYS(node->__geoSystem), node->geoOrigin, &GCpos, &node->__movedgd, &node->position);
4232 quaternion_inverse(&qaoi,&qao);
4233 quaternion_multiply(&qgc,
Quat,&qao);
4237 GeoOrient(X3D_NODE(node->geoOrigin), GEOSYS(node->__geoSystem), &node->__movedgd, &lo);
4238 vrmlrot_to_quaternion(&qlo,lo.c[0],lo.c[1],lo.c[2], lo.c[3]);
4239 quaternion_multiply(&qoo,&qgc,&qlo);
4240 quaternion_normalize(&qoo);
4241 quaternion_to_vrmlrot(&qoo,&oo[0],&oo[1],&oo[2],&oo[3]);
4243 double2float(node->orientation.c,oo,4);
4247struct X3D_Node* getSelectedViewpoint();
4252 if (!renderstate()->render_vp)
return;
4254 node->_reachablethispass = TRUE;
4256 if((
struct X3D_Node*)node == getSelectedViewpoint() && !node->_donethispass){
4257 X3D_Viewer *
viewer = Viewer();
4258 node->_donethispass = 1;
4262 printf (
"prep_GeoViewpoint called\n");
4267 struct SFVec3d translation1, translation2, gcCoord, gdCoord;
4268 struct SFVec4d rotation1, rotation2;
4270 Geosys *gs = GEOSYS(node->__geoSystem);
4276 user2gc(gs,&node->position,1,&gcCoord);
4277 gc2gd(gs,&gcCoord,1, &gdCoord);
4279 float2double(oo,node->orientation.c,4);
4281 FW_GL_ROTATE_RADIANS(oo[3],oo[0],oo[1],oo[2]);
4284 gc2tcs_transform(gs,&gdCoord,&translation2,&rotation2);
4285 FW_GL_ROTATE_RADIANS(rotation2.c[3],rotation2.c[0],rotation2.c[1],rotation2.c[2]);
4286 FW_GL_TRANSLATE_D(translation2.c[0],translation2.c[1],translation2.c[2]);
4287 lcs2gc_transform(&rotation1,&translation1);
4288 FW_GL_TRANSLATE_D(translation1.c[0],translation1.c[1],translation1.c[2]);
4289 FW_GL_ROTATE_RADIANS(rotation1.c[3],rotation1.c[0],rotation1.c[1],rotation1.c[2]);
4293 planet = current_planet();
4294 node->_prepped_planet = planet->ID;
4309 FW_GL_GETINTEGERV(GL_VIEWPORT, viewPort);
4310 if(viewPort[2] > viewPort[3]) {
4312 viewer->fieldofview = node->fieldOfView/3.1415926536*180;
4314 a1 = node->fieldOfView;
4315 a1 = atan2(sin(a1),viewPort[2]/((
float)viewPort[3]) * cos(a1));
4316 viewer->fieldofview = a1/3.1415926536*180;
4318 if(
viewer->type != VIEWER_WALK){
4320 calculateViewingSpeedB();
4321 node->_resetRelativeHeight = !node->relativeHeight;
4324 printf (
"prep_GeoViewpoint, fieldOfView %f \n",node->fieldOfView);
4328void draw_viewpoint(
int type,
float *fov,
float aspect);
4330 float center[3],size[3];
4331 if(node->_show_pin_point || fwl_getShowViewpoints())
4332 draw_bbox(double2float(center,node->_pin_point.c,3),vecset3f(size,400000.f,400000.f,400000.f));
4333 if(fwl_getShowViewpoints()){
4334 FW_GL_PUSH_MATRIX();
4335 FW_GL_TRANSLATE_D(node->_position.c[0],node->_position.c[1],node->_position.c[2]);
4336 FW_GL_ROTATE_RADIANS( node->_orientation.c[3],node->_orientation.c[0],node->_orientation.c[1],
4337 node->_orientation.c[2]);
4339 draw_viewpoint(node->_nodeType,&node->fieldOfView,.6f);
4344void calculateViewingSpeedB() {
4346 ttglobal tg = gglobal();
4347 struct X3D_Node *boundvp = vector_back(
struct X3D_Node*,getActiveBindableStacks(tg)->viewpoint);
4349 if(boundvp->_nodeType == NODE_GeoViewpoint){
4350 double height, heightmin;
4354 X3D_Viewer *
viewer = Viewer();
4357 COMPILE_IF_REQUIRED(X3D_NODE(node));
4358 gdCoords = &node->__movedgd;
4359 height = gdCoords->c[2];
4360 heightmin = max(abs(height),50.0);
4361 viewer->speed = heightmin * node->speedFactor;
4363 static int count = 0;
4366 printf(
"height %lf speedFactor %lf speed %lf\n",height,node->speedFactor,
viewer->speed);
4372 set_naviWidthHeightStep(
4380static void calculateExamineModeDistance(
void) {
4384Viewer()->doExamineModeDistanceCalculations = TRUE;
4391 if (!(node->isBound))
return;
4393 viewer = ViewerByLayerId(node->_layerId);
4399 if(!node->_initializedOnce) {
4400 veccopyd(node->_position.c,node->position.c);
4401 veccopy4f(node->_orientation.c,node->orientation.c);
4402 node->_initializedOnce = TRUE;
4404 if(!node->retainUserOffsets){
4405 veccopyd(node->position.c,node->_position.c);
4406 veccopy4f(node->orientation.c,node->_orientation.c);
4408 fwl_set_viewer_type (VIEWER_WALK);
4411 svptr = node->navigationType.p;
4412 for (
int i = 0; i < node->navigationType.n; i++) {
4414 typeptr = svptr[i]->strptr;
4416 if (strcmp(typeptr,
"PAN") == 0) {
4417 viewer->oktypes[VIEWER_PAN] = TRUE;
4418 if (i == 0) fwl_set_viewer_type0(
viewer, VIEWER_PAN);
4420 if (strcmp(typeptr,
"ZOOM") == 0) {
4421 viewer->oktypes[VIEWER_ZOOM] = TRUE;
4422 if (i == 0) fwl_set_viewer_type0(
viewer, VIEWER_ZOOM);
4424 if (strcmp(typeptr,
"ANY") == 0) {
4425 viewer->oktypes[VIEWER_EXAMINE] = TRUE;
4426 viewer->oktypes[VIEWER_WALK] = TRUE;
4427 viewer->oktypes[VIEWER_EXFLY] = TRUE;
4428 viewer->oktypes[VIEWER_FLY] = TRUE;
4429 viewer->oktypes[VIEWER_EXPLORE] = TRUE;
4430 viewer->oktypes[VIEWER_LOOKAT] = TRUE;
4431 viewer->oktypes[VIEWER_SPHERICAL] = TRUE;
4432 viewer->oktypes[VIEWER_TURNTABLE] = TRUE;
4433 viewer->oktypes[VIEWER_DIST] = TRUE;
4434 viewer->oktypes[VIEWER_PAN] = TRUE;
4435 viewer->oktypes[VIEWER_ZOOM] = TRUE;
4436 if (i==0) fwl_set_viewer_type0(
viewer, VIEWER_WALK);
4440 if (
viewer->transitionType != VIEWER_TRANSITION_TELEPORT &&
viewer->wasBound) {
4442 viewer->vp2rnSaved = TRUE;
4447 matcopy(
viewer->slerp_viewmatrix,bstack->viewtransformmatrix);
4448 matcopy(
viewer->slerp_posorimatrix,bstack->posorimatrix);
4452 viewer->SLERPing = FALSE;
4453 viewer->startSLERPtime = TickTime();
4455 viewer->SLERPing2 = TRUE;
4456 viewer->SLERPing2justStarted = TRUE;
4460 viewer->SLERPing = FALSE;
4461 viewer->SLERPing2 = FALSE;
4467 printf (
"bind_GeoViewpoint, setting Viewer to %lf %lf %lf orient %f %f %f %f\n",node->__movedPosition.c[0],node->__movedPosition.c[1],
4468 node->__movedPosition.c[2],node->orientation.c[0],node->orientation.c[1],node->orientation.c[2],
4469 node->orientation.c[3]);
4470 printf (
" node %u fieldOfView %f\n",node,node->fieldOfView);
4473 vrmlrot_to_quaternion (&
viewer->Quat,node->__movedOrientation.c[0],
4474 node->__movedOrientation.c[1],node->__movedOrientation.c[2],node->__movedOrientation.c[3]);
4475 ttglobal tg = gglobal();
4476 int saveActive = tg->Bindable.activeLayer;
4477 tg->Bindable.activeLayer = node->_layerId;
4479 calculateViewingSpeedB();
4480 node->_resetRelativeHeight = !node->relativeHeight;
4482 calculateExamineModeDistance();
4483 fwl_setCollision(TRUE);
4484 tg->Bindable.activeLayer = saveActive;
4485 setMenuStatusVP (node->description->strptr);
4496 printf (
"compiling GeoLocation\n");
4500 compile_geoSystem(X3D_NODE(node),node->_nodeType,&node->geoSystem,&node->__geoSystem);
4501 gs = GEOSYS(node->__geoSystem);
4502 update_origin(gs, X3D_NODE(node), &node->geoCenter, X3D_GEOORIGIN(node->geoOrigin));
4505 MARK_SFVEC3D_INOUT_EVENT(node->geoCenter, node->__oldGeoCenter,offsetof (
struct X3D_GeoTransform, geoCenter))
4506 MARK_MFNODE_INOUT_EVENT(node->children, node->__oldChildren, offsetof (
struct X3D_GeoTransform, children))
4512 node->__do_center = verify_translate ((GLfloat *)node->center.c);
4513 node->__do_trans = verify_translate ((GLfloat *)node->translation.c);
4514 node->__do_scale = verify_scale ((GLfloat *)node->scale.c);
4515 node->__do_rotation = verify_rotate ((GLfloat *)node->rotation.c);
4516 node->__do_scaleO = verify_rotate ((GLfloat *)node->scaleOrientation.c);
4518 node->__do_anything = (node->__do_center ||
4521 node->__do_rotation ||
4524 REINITIALIZE_SORTED_NODES_FIELD(node->children,node->_sortedChildren);
4549 if(!renderstate()->render_vp) {
4551 push_transform_local_identity();
4552 if (node->__do_anything) {
4554 FW_GL_PUSH_MATRIX();
4555 FW_GL_PUSH_MATRIX();
4556 FW_GL_LOAD_IDENTITY();
4559 if (node->__do_trans)
4560 FW_GL_TRANSLATE_F(node->translation.c[0],node->translation.c[1],node->translation.c[2]);
4563 if (node->__do_center)
4564 FW_GL_TRANSLATE_F(node->center.c[0],node->center.c[1],node->center.c[2]);
4567 if (node->__do_rotation) {
4568 FW_GL_ROTATE_RADIANS(node->rotation.c[3], node->rotation.c[0],node->rotation.c[1],node->rotation.c[2]);
4572 if (node->__do_scaleO) {
4573 FW_GL_ROTATE_RADIANS(node->scaleOrientation.c[3], node->scaleOrientation.c[0], node->scaleOrientation.c[1],node->scaleOrientation.c[2]);
4578 if (node->__do_scale)
4579 FW_GL_SCALE_F(node->scale.c[0],node->scale.c[1],node->scale.c[2]);
4582 if (node->__do_scaleO)
4583 FW_GL_ROTATE_RADIANS(-node->scaleOrientation.c[3], node->scaleOrientation.c[0], node->scaleOrientation.c[1],node->scaleOrientation.c[2]);
4586 if (node->__do_center)
4587 FW_GL_TRANSLATE_F(-node->center.c[0],-node->center.c[1],-node->center.c[2]);
4592 FW_GL_GETDOUBLEV(GL_MODELVIEW_MATRIX,mat);
4594 FW_GL_TRANSFORM_D(mat);
4595 reset_transform_local(mat);
4606 if(!renderstate()->render_vp) {
4607 pop_transform_local();
4608 if (node->__do_anything) {
4613 if((node->_renderFlags & VF_Viewpoint) == VF_Viewpoint) {
4614 FW_GL_TRANSLATE_F(((node->center).c[0]),((node->center).c[1]),((node->center).c[2])
4616 FW_GL_ROTATE_RADIANS(((node->scaleOrientation).c[3]),((node->scaleOrientation).c[0]),((node->scaleOrientation).c[1]),((node->scaleOrientation).c[2])
4618 FW_GL_SCALE_F((
float)1.0/(((node->scale).c[0])),(
float)1.0/(((node->scale).c[1])),(
float)1.0/(((node->scale).c[2]))
4620 FW_GL_ROTATE_RADIANS(-(((node->scaleOrientation).c[3])),((node->scaleOrientation).c[0]),((node->scaleOrientation).c[1]),((node->scaleOrientation).c[2])
4622 FW_GL_ROTATE_RADIANS(-(((node->rotation).c[3])),((node->rotation).c[0]),((node->rotation).c[1]),((node->rotation).c[2])
4624 FW_GL_TRANSLATE_F(-(((node->center).c[0])),-(((node->center).c[1])),-(((node->center).c[2]))
4626 FW_GL_TRANSLATE_F(-(((node->translation).c[0])),-(((node->translation).c[1])),-(((node->translation).c[2]))
4631void geoprepT0(Geosys *geoSystem,
struct SFVec3d *userCoord){
4636 struct SFVec3d translation1, translation2, gcCoord, gdCoord;
4637 struct SFVec4d rotation1, rotation2;
4639 Geosys *gs = geoSystem;
4646 getEllipsoidParams(gs->ellipsoid, &A, &F);
4647 if(gs->spatial_system == GEOSP_GC && veclengthd(userCoord->c) < A/2.0){
4650 FW_GL_TRANSLATE_D(-userCoord->c[0],-userCoord->c[1],-userCoord->c[2]);
4652 user2gc(gs,userCoord,1,&gcCoord);
4653 gc2gd(gs,&gcCoord,1, &gdCoord);
4655 gc2tcs_transform(gs,&gdCoord,&translation2,&rotation2);
4656 FW_GL_ROTATE_RADIANS(rotation2.c[3],rotation2.c[0],rotation2.c[1],rotation2.c[2]);
4657 FW_GL_TRANSLATE_D(translation2.c[0],translation2.c[1],translation2.c[2]);
4658 lcs2gc_transform(&rotation1,&translation1);
4659 FW_GL_TRANSLATE_D(translation1.c[0],translation1.c[1],translation1.c[2]);
4660 FW_GL_ROTATE_RADIANS(rotation1.c[3],rotation1.c[0],rotation1.c[1],rotation1.c[2]);
4663 user2gc(gs,userCoord,1,&gcCoord);
4666 gc2tcsB_transform(gs,&gcCoord,&translation2,&rotation2);
4667 FW_GL_ROTATE_RADIANS(rotation2.c[3],rotation2.c[0],rotation2.c[1],rotation2.c[2]);
4668 FW_GL_TRANSLATE_D(translation2.c[0],translation2.c[1],translation2.c[2]);
4669 lcs2gc_transform(&rotation1,&translation1);
4670 FW_GL_TRANSLATE_D(translation1.c[0],translation1.c[1],translation1.c[2]);
4671 FW_GL_ROTATE_RADIANS(rotation1.c[3],rotation1.c[0],rotation1.c[1],rotation1.c[2]);
4675void geoprepT(Geosys *geoSystem,
struct SFVec3d *userCoord){
4679 if(!renderstate()->render_vp) {
4680 FW_GL_PUSH_MATRIX();
4681 push_transform_local_identity();
4682 FW_GL_PUSH_MATRIX();
4683 FW_GL_LOAD_IDENTITY();
4684 geoprepT0(geoSystem,userCoord);
4688 FW_GL_GETDOUBLEV(GL_MODELVIEW_MATRIX,mat);
4690 FW_GL_TRANSFORM_D(mat);
4691 reset_transform_local(mat);
4697void geofinT(Geosys *geoSystem,
struct SFVec3d *userCoord){
4698 if(!renderstate()->render_vp) {
4699 pop_transform_local();
4702 geoprep0(geoSystem,userCoord);
4712 RETURN_FROM_CHILD_IF_NOT_FOR_ME
4731 prep_sibAffectors((
struct X3D_Node*)node,&node->__sibAffectors);
4739 printf (
"transform - doing normalChildren\n");
4741 geoprepT(GEOSYS(node->__geoSystem),&node->geoCenter);
4743 prep_BBox((
struct BBoxFields*)&node->bboxCenter);
4745 normalChildren(node->children);
4747 pop_group_visible();
4750 extent6f2bbox(peek_group_extent(),node->bboxCenter.c,node->bboxSize.c);
4751 if(renderstate()->render_geom && (node->bboxDisplay || fwl_getDrawBoundingBoxes() )) {
4752 draw_bbox(node->bboxCenter.c,node->bboxSize.c);
4756 extent6f_mattransform4d(node->_extent,peek_group_extent(),peek_transform_local());
4760 geofinT(GEOSYS(node->__geoSystem),&node->geoCenter);
4763 extent6f_mattransform4d(node->_extent,node->_extent,peek_transform_local());
4764 union_group_extent(node->_extent);
4767 printf (
"transform - done normalChildren\n");
4771 fin_sibAffectors((
struct X3D_Node*)node,&node->__sibAffectors);
4776void CONVERT_BACK_TO_GD_OR_UTMC(Geosys *targetGeoSystem,
struct X3D_Node *geoorigin,
4793 Geosys *geoSystem = targetGeoSystem;
4795 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
4796 veccopyd(thisField->c,LCSpos->c);
4799 if (geoSystem != NULL) {
4801 if (geoSystem->spatial_system != GEOSP_GC) {
4803 gccToGdc (geoSystem, thisField, gdCoords);
4804 if(geoSystem->geoid_height || geoSystem->relativeHeight)
4805 gdCoords->c[2] -= userHeight2ellipsoidHeight(geoSystem,gdCoords);
4806 veccopyd(thisField->c,gdCoords->c);
4811 if (geoSystem->spatial_system == GEOSP_UTM || geoSystem->spatial_system == GEOSP_3TM || geoSystem->spatial_system == GEOSP_WM ) {
4815 if(geoSystem->spatial_system == GEOSP_UTM){
4816 gdToUtm3d(geoSystem,thisField->c, dtemp);
4817 veccopyd(thisField->c,dtemp);
4818 }
else if(geoSystem->spatial_system == GEOSP_3TM) {
4819 gdTo3tm3d(geoSystem,thisField->c, dtemp);
4820 veccopyd(thisField->c,dtemp);
4821 }
else if(geoSystem->spatial_system == GEOSP_WM) {
4822 gdToWm3d(geoSystem,thisField->c, dtemp);
4823 veccopyd(thisField->c,dtemp);
4860int geoelevationgrid_getGDHeight0(
struct X3D_GeoElevationGrid *node,
struct SFVec3d *gdCoord, Geosys *geoSystem,
double *gridHeight){
4864 float centerf[3],bottomf[3];
4865 double centerd[3], bottomd[3], spined[3], vertvecd[3];
4868 double tmin[3],tmax[3];
4871 GLDOUBLE awidth, atop, abottom, astep;
4872 ttglobal tg = gglobal();
4875 nodeSystem = GEOSYS(node->__geoSystem);
4876 if(!nodeSystem)
return 0;
4879 veccopyd(xxCoord.c,gdCoord->c);
4881 if(geoSystem->gd_latitude_first != GEOSYS(node->__geoSystem)->gd_latitude_first)
4882 vecswizzle2d(xxCoord.c);
4883 if(geoSystem->gd_degrees != GEOSYS(node->__geoSystem)->gd_degrees)
4884 if(geoSystem->gd_degrees == TRUE)
4885 vecscale2d(xxCoord.c,xxCoord.c,RADIANS_PER_DEGREE);
4887 vecscale2d(xxCoord.c,xxCoord.c,DEGREES_PER_RADIAN);
4892 if(nodeSystem->spatial_system == GEOSP_UTM || nodeSystem->spatial_system == GEOSP_3TM ) {
4896 if(nodeSystem->spatial_system == GEOSP_UTM){
4897 gdToUtm3d(GEOSYS(node->__geoSystem),xxCoord.c, dtemp);
4898 veccopyd(xxCoord.c,dtemp);
4899 }
else if(nodeSystem->spatial_system == GEOSP_3TM) {
4900 gdTo3tm3d(GEOSYS(node->__geoSystem),xxCoord.c, dtemp);
4901 veccopyd(xxCoord.c,dtemp);
4903 }
else if(nodeSystem->spatial_system == GEOSP_GC) {
4910 double emin[2],emax[2];
4912 double size[2], spacing[2];
4916 idimension[0] = node->xDimension;
4917 idimension[1] = node->zDimension;
4918 spacing[0] = node->xSpacing;
4919 spacing[1] = node->zSpacing;
4922 int swizzle_xzdimension = (XTM && nodeSystem->xtm_northing_first) || (!XTM && nodeSystem->gd_latitude_first);
4923 if(swizzle_xzdimension) {
4926 int itmp = idimension[0];
4927 idimension[0] = idimension[1];
4928 idimension[1] = itmp;
4929 vecswizzle2d(spacing);
4931 size[0] = spacing[0]*(idimension[0] -1);
4932 size[1] = spacing[1]*(idimension[1] -1);
4933 emin[0] = min(node->geoGridOrigin.c[0],node->geoGridOrigin.c[0]+size[0]);
4934 emax[0] = max(node->geoGridOrigin.c[0],node->geoGridOrigin.c[0]+size[0]);
4935 emin[1] = min(node->geoGridOrigin.c[1],node->geoGridOrigin.c[1]+size[1]);
4936 emax[1] = max(node->geoGridOrigin.c[1],node->geoGridOrigin.c[1]+size[1]);
4939 inside = xxCoord.c[0] <= emax[0] && xxCoord.c[0] >= emin[0];
4940 inside &= xxCoord.c[1] <= emax[1] && xxCoord.c[1] >= emin[1];
4943 double spinelength, vcenterd[3], pp[2];
4946 double deltah, gridpointf[3];
4950 pp[0] = xxCoord.c[0] - node->geoGridOrigin.c[0];
4951 pp[1] = xxCoord.c[1] - node->geoGridOrigin.c[1];
4954 if(swizzle_xzdimension) {
4964 double hh[4],gridheight;
4965 int i0,i1,i2,i3, ix, iz;
4966 ix = (int)(pp[0]/node->xSpacing);
4967 iz = (int)(pp[1]/node->zSpacing);
4973 i0 = iz * node->xDimension + ix;
4975 i2 = i0 + node->xDimension;
4978 hh[0] = node->height.p[i0];
4979 hh[1] = node->height.p[i1];
4980 hh[2] = node->height.p[i2];
4981 hh[3] = node->height.p[i3];
4983 cx = (pp[0] - ix*node->xSpacing)/node->xSpacing;
4984 cz = (pp[1] - iz*node->zSpacing)/node->zSpacing;
4988 gridheight = hh[0]*(1.0f - cz)*(1.0f - cx)
4989 + hh[1]*(1.0f - cz)*cx
4990 + hh[2]*cz*(1.0f - cx)
4992 gridheight *= node->yScale;
4994 *gridHeight = gridheight;
5021 float centerf[3],bottomf[3];
5022 double centerd[3], bottomd[3], spined[3], vertvecd[3], gridheight;
5023 struct SFVec3d *gdCoord, xxCoord;
5026 double tmin[3],tmax[3];
5028 GLDOUBLE awidth, atop, abottom, astep;
5029 ttglobal tg = gglobal();
5030 naviinfo = (
struct sNaviInfo *)tg->Bindable.naviinfo;
5033 gdCoord = &gvp->__movedgd;
5034 geoSystem = GEOSYS(node->__geoSystem);
5035 hit = geoelevationgrid_getGDHeight0(node, gdCoord, geoSystem, &gridheight);
5036 static int count =0;
5043 if(gvp->_resetRelativeHeight){
5044 naviinfo->height = gdCoord->c[2] - gridheight;
5045 gvp->_resetRelativeHeight = FALSE;
5049 double abottom = gdCoord->c[2] - naviinfo->height;
5050 double hhh = gridheight - abottom;
5052 double hhbelowfoot = hhh;
5058 if( hhh < abottom && hhh > -fi->fallHeight)
5063 fi->hfall = hhbelowfoot;
5065 if(hhbelowfoot > fi->hfall) fi->hfall = hhbelowfoot;
5072 if( hhh >= abottom )
5077 if( fi->isClimb == 0 )
5078 fi->hclimb = hhbelowfoot;
5080 fi->hclimb = DOUBLE_MAX(fi->hclimb,hhbelowfoot);
5086 double hhabovehead = hhh - head;
5088 if( hhabovehead > 0.0 )
5098 if( fi->isClimb == 0 ){
5099 fi->hclimb = hhabovehead + abottom;
5101 fi->hclimb = DOUBLE_MAX(fi->hclimb,hhabovehead + abottom);
5129 boundvp = getActiveLayerBoundViewpoint();
5130 if(boundvp && boundvp->_nodeType == NODE_GeoViewpoint){
5132 struct Planet *planet = current_planet();;
5134 if(gvp->_prepped_planet == planet->ID) compatible = TRUE;
5136 if(node->_nodeType == NODE_GeoElevationGrid && compatible ){
5142 if(0)
if(ihit == -1){
5149double getTerrainHeight(
int planetID, Geosys *geoSystem,
struct SFVec3d *gdCoord){
5156 ttglobal tg = gglobal();
5157 ppComponent_Geospatial p = (ppComponent_Geospatial)tg->Component_Geospatial.prv;
5159 int height_method = WALK_SURFACE_HIGHEST;
5161 walk_surface = NULL;
5163 if(!p->planet_stack)
return highest;
5167 struct X3D_Node *boundvp = vector_back(
struct X3D_Node*,getActiveBindableStacks(tg)->viewpoint);
5169 if(boundvp && boundvp->_nodeType == NODE_GeoViewpoint){
5172 if(node->_ichange != node->_change) compile_GeoViewpoint(node);
5173 height_method = node->_walkSurfacePriority;
5174 n_walk_surface = node->prioritySurfaces.n;
5175 walk_surface = node->prioritySurfaces.p;
5180 for(i=0;i<vectorSize(p->planet_stack);i++){
5181 planet = vector_get_ptr(
struct Planet,p->planet_stack,i);
5182 if(planet && planet->ID == planetID) {
5183 if(!planet->gegs)
break;
5184 for(j=0;j<vectorSize(planet->gegs);j++){
5188 if( geoelevationgrid_getGDHeight0(geg,gdCoord,geoSystem,&gridheight) == 1){
5191 if(nfound == 1) highest = gridheight;
5192 if(height_method & WALK_SURFACE_HIGHEST)
5193 highest = max(highest,gridheight);
5194 else if(height_method &WALK_SURFACE_LOWEST)
5195 highest = min(highest, gridheight);
5201 if(n_walk_surface && height_method & WALK_SURFACE_PRIORITY){
5203 for(i=0;i<n_walk_surface;i++){
5204 struct X3D_Node *node = walk_surface[i];
5205 if(node->_nodeType == NODE_GeoElevationGrid){
5209 if( geoelevationgrid_getGDHeight0(geg,gdCoord,geoSystem,&gridheight) == 1){
5212 if(mfound == 1) highest = gridheight;
5214 if(height_method & WALK_SURFACE_HIGHEST)
5215 highest = max(highest,gridheight);
5216 else if(height_method &WALK_SURFACE_LOWEST)
5217 highest = min(highest, gridheight);
5227void RegisterGeoElevationGrid(
struct X3D_Node *node,
int planetID){
5234 if(node && node->_nodeType == NODE_GeoElevationGrid){
5238 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
5239 if(!p->planet_stack) p->planet_stack = newStack(
struct Planet);
5242 for(i=0;i<vectorSize(p->planet_stack);i++){
5243 planet = vector_get_ptr(
struct Planet,p->planet_stack,i);
5244 if(planet->ID == planetID) {
5252 memset(&newplanet,0,
sizeof(
struct Planet));
5253 newplanet.ID = planetID;
5255 vector_pushBack(
struct Planet,p->planet_stack,newplanet);
5256 ifound = p->planet_stack->n -1;
5257 planet = vector_get_ptr(
struct Planet,p->planet_stack,ifound);
5259 if(planet->gegs == NULL) planet->gegs = newStack(
struct X3D_Node*);
5261 vector_pushBack(
struct X3D_Node*,planet->gegs,node);
5264void unRegisterGeoElevationGrid(
struct X3D_Node *node){
5267 if(node && node->_nodeType == NODE_GeoElevationGrid){
5270 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
5271 if(!p->planet_stack)
return;
5272 for(i=0;i<vectorSize(p->planet_stack);i++){
5273 struct Planet *planet = vector_get_ptr(
struct Planet,p->planet_stack,i);
5275 for(j=0;j<vectorSize(planet->gegs);j++){
5276 int k = vectorSize(planet->gegs) - j -1;
5279 vector_set(
struct X3D_Node*,planet->gegs,k,NULL);
5290 struct Planet *planet = current_planet();
5292 planet = add_planet(node->planetId);
5296 REINITIALIZE_SORTED_NODES_FIELD(node->children,node->_sortedChildren);
5307 push_planetId(node->planetId);
5309 if(!renderstate()->render_vp) {
5310 double aoo[4],ao[3];
5314 planet = current_planet();
5317 FW_GL_PUSH_MATRIX();
5318 push_transform_local_identity();
5319 FW_GL_PUSH_MATRIX();
5320 FW_GL_LOAD_IDENTITY();
5322 veccopyd(ao,planet->autoOrigin.c);
5323 veccopy4d(aoo,planet->autoOrient.c);
5324 FW_GL_TRANSLATE_D(ao[0], ao[1], ao[2]);
5325 FW_GL_ROTATE_RADIANS(aoo[3], aoo[0],aoo[1],aoo[2]);
5330 FW_GL_GETDOUBLEV(GL_MODELVIEW_MATRIX,mat);
5332 FW_GL_TRANSFORM_D(mat);
5333 reset_transform_local(mat);
5346 RETURN_FROM_CHILD_IF_NOT_FOR_ME
5348 prep_sibAffectors((
struct X3D_Node*)node,&node->__sibAffectors);
5349 prep_BBox((
struct BBoxFields*)&node->bboxCenter);
5351 normalChildren(node->children);
5354 fin_sibAffectors((
struct X3D_Node*)node,&node->__sibAffectors);
5361 if(!renderstate()->render_vp) {
5362 pop_transform_local();
5365 if ((node->_renderFlags & VF_Viewpoint) == VF_Viewpoint) {
5366 double aoo[4],ao[3];
5369 planet = current_planet();
5370 veccopyd(ao,planet->autoOrigin.c);
5371 veccopy4d(aoo,planet->autoOrient.c);
5373 FW_GL_ROTATE_RADIANS(-aoo[3], aoo[0],aoo[1],aoo[2]);
5374 FW_GL_TRANSLATE_D(-ao[0], -ao[1], -ao[2]);
5385void user2gc(Geosys * geoSystem,
struct SFVec3d *geo,
int n,
struct SFVec3d *gc){
5390 moveCoords3d(geoSystem,NULL,NULL,&geo[i],1,&gc[i],&gdCoord);
5393void gc2user(Geosys * geoSystem,
struct SFVec3d *gc,
int n,
struct SFVec3d *geo){
5398 CONVERT_BACK_TO_GD_OR_UTMC(geoSystem,NULL,&gc[i],&gdCoord,&geo[i]);
5401void gc2lcs_transform(
struct SFVec3d *translate,
struct SFVec4d *rotate){
5406 planet = current_planet();
5407 veccopyd(translate->c,planet->autoOrigin.c);
5409 vecnegated(translate->c,translate->c);
5410 veccopy4d(rotate->c,planet->autoOrient.c);
5411 rotate->c[3] = -rotate->c[3];
5413void gc2lcs(Geosys * geoSystem,
struct SFVec3d *gc,
int n,
struct SFVec3d *lcs){
5421 gc2lcs_transform(&translate, &rotate);
5422 vrmlrot_to_quaternion(&qup,rotate.c[0],rotate.c[1],rotate.c[2],rotate.c[3]);
5424 vecaddd(lcs[i].c,gc[i].c,translate.c);
5425 quaternion_rotationd(lcs[i].c,&qup,lcs[i].c);
5430void lcs2gc_transform(
struct SFVec4d *rotation,
struct SFVec3d *translation){
5435 planet = current_planet();
5436 veccopy4d(rotation->c,planet->autoOrient.c);
5437 veccopyd(translation->c,planet->autoOrigin.c);
5439void lcs2gc(Geosys * geoSystem,
struct SFVec3d *lcs,
int n,
struct SFVec3d *gc){
5446 lcs2gc_transform(&rotation, &translation);
5447 vrmlrot_to_quaternion(&qup,rotation.c[0],rotation.c[1],rotation.c[2],rotation.c[3]);
5449 quaternion_rotationd(gc[i].c,&qup,lcs[i].c);
5450 vecaddd(gc[i].c,gc[i].c,translation.c);
5457void gc2tcs_transform(Geosys * geoSystem,
struct SFVec3d *gdcenter,
struct SFVec3d *translate,
struct SFVec4d *rotate){
5467 GeoOrient(NULL,geoSystem,gdcenter,rotate);
5468 rotate->c[3] = -rotate->c[3];
5469 gd2gc(geoSystem,gdcenter,1,translate);
5471 vecnegated(translate->c,translate->c);
5474void tcs2gc_transform(Geosys * geoSystem,
struct SFVec3d *gdcenter,
struct SFVec4d *rotate,
struct SFVec3d *translate){
5483 GeoOrient(NULL,geoSystem,gdcenter,rotate);
5484 gd2gc(geoSystem,gdcenter,1,translate);
5487void gc2tcs(Geosys * geoSystem,
struct SFVec3d *gdcenter,
struct SFVec3d *gc,
int n,
struct SFVec3d *tcs){
5497 tcs2gc_transform(geoSystem, gdcenter, &rotate, &translate);
5498 vrmlrot_to_quaternion(&qlo,rotate.c[0],rotate.c[1],rotate.c[2], -rotate.c[3]);
5500 vecdifd(pp,gc[i].c,translate.c);
5501 quaternion_rotationd(tcs[i].c,&qlo,pp);
5505void tcs2gc(Geosys * geoSystem,
struct SFVec3d *gdcenter,
struct SFVec3d *tcs,
int n,
struct SFVec3d *gc){
5514 tcs2gc_transform(geoSystem, gdcenter, &rotate, &translate);
5515 vrmlrot_to_quaternion(&qlo,rotate.c[0],rotate.c[1],rotate.c[2], rotate.c[3]);
5517 quaternion_rotationd(pp,&qlo,tcs[i].c);
5518 vecaddd(gc[i].c,translate.c,pp);
5523void tcs2gcB(Geosys * geoSystem,
struct SFVec3d *gccenter,
struct SFVec3d *tcs,
int n,
struct SFVec3d *gc);
5524void gc2tcsB(Geosys * geoSystem,
struct SFVec3d *gccenter,
struct SFVec3d *gc,
int n,
struct SFVec3d *tcs);
5526void gc2tcsB(Geosys * geoSystem,
struct SFVec3d *gccenter,
struct SFVec3d *gc,
int n,
struct SFVec3d *tcs){
5536 tcs2gcB_transform(geoSystem, gccenter, &translate, &rotate);
5537 vrmlrot_to_quaternion(&qlo,rotate.c[0],rotate.c[1],rotate.c[2], -rotate.c[3]);
5539 vecdifd(pp,gc[i].c,translate.c);
5540 quaternion_rotationd(tcs[i].c,&qlo,pp);
5544void tcs2gcB(Geosys * geoSystem,
struct SFVec3d *gccenter,
struct SFVec3d *tcs,
int n,
struct SFVec3d *gc){
5553 tcs2gcB_transform(geoSystem, gccenter, &translate, &rotate);
5554 vrmlrot_to_quaternion(&qlo,rotate.c[0],rotate.c[1],rotate.c[2], rotate.c[3]);
5556 quaternion_rotationd(pp,&qlo,tcs[i].c);
5557 vecaddd(gc[i].c,translate.c,pp);
5585void gd2gc(Geosys * geoSystem,
struct SFVec3d *gd,
int n,
struct SFVec3d *gc){
5589 Gd_Gc3d(geoSystem,&gd[i],1,&gc[i]);
5592void gc2gd(Geosys * geoSystem,
struct SFVec3d *gc,
int n,
struct SFVec3d *gd){
5596 gccToGdc (geoSystem, &gc[i], &gd[i]);
5599void do_GeoConvert (
void *px){
5616 if(node->__geoSystem == NULL)
5617 compile_geoSystem (X3D_NODE(node),node->_nodeType, &node->geoSystem, &node->__geoSystem);
5619 if (!vecsamed(node->__oldgeoCoords.c,node->set_geoCoords.c)) {
5622 moveCoords3d(GEOSYS(node->__geoSystem),NULL,NULL,&node->set_geoCoords,1,&node->gcCoords_changed,&gdCoord);
5623 MARK_EVENT (px, offsetof (
struct X3D_GeoConvert, gcCoords_changed));
5624 veccopyd(node->__oldgeoCoords.c,node->set_geoCoords.c);
5626 if (!vecsamed(node->__oldgcCoords.c,node->set_gcCoords.c)){
5629 CONVERT_BACK_TO_GD_OR_UTMC(GEOSYS(node->__geoSystem),NULL,&node->set_gcCoords,&gdCoord,&node->geoCoords_changed);
5630 MARK_EVENT (px, offsetof (
struct X3D_GeoConvert, geoCoords_changed));
5631 veccopyd(node->__oldgcCoords.c,node->set_gcCoords.c);
5648 printf(
"in cmopile_GeoSRF\n");