FreeWRL / FreeX3D 4.3.0
Component_Geospatial.c
1/*
2
3
4X3D Geospatial Component
5
6*/
7
8
9/****************************************************************************
10 This file is part of the FreeWRL/FreeX3D Distribution.
11
12 Copyright 2009 CRC Canada. (http://www.crc.gc.ca)
13
14 FreeWRL/FreeX3D is free software: you can redistribute it and/or modify
15 it under the terms of the GNU Lesser Public License as published by
16 the Free Software Foundation, either version 3 of the License, or
17 (at your option) any later version.
18
19 FreeWRL/FreeX3D is distributed in the hope that it will be useful,
20 but WITHOUT ANY WARRANTY; without even the implied warranty of
21 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
22 GNU General Public License for more details.
23
24 You should have received a copy of the GNU General Public License
25 along with FreeWRL/FreeX3D. If not, see <http://www.gnu.org/licenses/>.
26****************************************************************************/
27
28
29
30#include <config.h>
31#include <system.h>
32#include <display.h>
33#include <internal.h>
34
35#include <libFreeWRL.h>
36
37#include "../vrml_parser/Structs.h"
38#include "../vrml_parser/CRoutes.h"
39#include "../main/headers.h"
40
41#include "../world_script/fieldSet.h"
42#include "../x3d_parser/Bindable.h"
43#include "Collision.h"
44#include "quaternion.h"
45#include "Viewer.h"
46#include "../opengl/Frustum.h"
47#include "../opengl/Material.h"
48#include "../opengl/OpenGL_Utils.h"
49#include "../input/EAIHelpers.h" /* for newASCIIString() */
50
51#include "Polyrep.h"
52#include "LinearAlgebra.h"
53#include "Component_Shape.h" /* for appearance properties */
54#include "Component_Geospatial.h"
55#include "Children.h"
56#include "../scenegraph/RenderFuncs.h"
57#include "../ui/common.h"
58#ifdef HAVE_GEOLIB
59#include "fwgeolib.h"
60#define GEOLIB
61#endif
62
63int method_geolib(){
64#ifdef GEOLIB
65 return 0; //freewrl hand coded way, was working fine for more than decade
66 //return 1; //geographicLib / Karney, for testing - a way to independently verify transforms when hacking/refactoring code
67#else
68 return 0; //freewrl hand coded way
69#endif
70}
71
72void push_planetId(int planetId);
73int current_planetId();
74void pop_planetId();
75
76// according to some -web3d.org participants Andreas, Dick- the LCS
77// was an accommodation for old float-transform-stack browsers
78// and its not a spec requirement - GC is more normal if you have a double transform stack like freewrl
79// so to skip LCS set the following to TRUE, or to use an LCS, set to FALSE.
80static int USE_GC_FOR_LCS = TRUE;
81
82/*
83Jan 2018 dug9 understanding of ellipsoids, units, geoid, origins
84* acronyms: nodes
85 GVP - geoViewpoint
86 GEG - GeoElevationGrid
87 GL - geoLocation
88 GT - geoTransform
89 GPS - geoProximitySensor
90 GTS - geoTouchSensor
91 GPI - geoPositionInterpolator
92 GP - geoPlanet (non-spec - (meaning not in web3d.org specs for their v3.3 geoSpatial component, invented here))
93 GCV - geoConverter (non-spec)
94
95* acronyms: other
96 LCS - Local Coordinate System - shared cartesian coordinate system near nodes
97 http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/geodata.html#high-precisioncoords
98 TCS - topocentric coordinate system at specific geo location, with -Z north, Y up as per GL
99 http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/geodata.html#GeoLocation
100 - surveyors call it the local geodetic system LGS http://www2.unb.ca/gge/Pubs/LN16.pdf p.50
101 -- because its at ellipsoid height (not at terrrain height)
102 -- except LGS looks too much like LCS, and since LGS is a subset of topocentric coordinate systems, TCS is our chosen name
103 FCFS - First Come First Served - how AutoOrigin is generated automatically now 2018:
104 - first geoNode's TCS -> shared LCS
105 XTM - general acronym for transverse mercator map projections: either UTM or 3TM
106 GC - geoCentric coordinates
107 GD - geodetic coordinates (latitude, longitude aka lat,lon)
108 user - coordinate system entered by the scene author that are in the geoSystem entered by the scene author, for the same node
109 - one of GC, GD, XTM(UTM,3TM)
110 - if GD, then lat,lon in degrees or radians as per geoSystem, lat or lon first
111 - if XTM, then easting or northing first as per geoSystem
112
113
114
115* XTM: {UTM,3TM} - 3TM is UTM with no false easting or northing, scale factor .9999, and 3 degree zones
116 Feb 2018 we added 3TM capability
117* geosystem preservation, aka User Coordinates
118 we keep coordinates as they are given to us, and only convert degrees (if neceessary)
119 or swizzle (exchange x, y to y, x) NE/latlon if/when needed for internal calculation purposes,
120 and for shape mesh/polyrep.
121 That way fields are ready for SAI and routing in users units.
122 It doesn't make sense to route directly between different geosystems.
123 In theory there could/should be converter node types for that,
124 where you can set both input geosystem, and output geosystem,
125 or a flag on each node, saying to route in/out in (common) GC.
126* concentric ellipsoids:
127 we don't have a list of ellipsoid offsets (3DOF or 6DOF datum shift (and rotate)) to go between 'datums'.
128 so we treat different ellipsoids as being otherwise axes-aligned and co-centric
129 with each other when transforming
130 for insight into datum transforms see: http://www.dtic.mil/dtic/tr/fulltext/u2/a307127.pdf Appendix B
131* v3.3 compiled-in vs freewrl compiled + supplyable ellipsoids
132 The specs give a table of ellipsoids to compile in, and user can choose by name
133 Feb 2018 we added ('A#' 'F#', 'R#') to geoSystem so the user can supply other ellipsoids or spheres if needed
134* ellipsoid neutrality:
135 we don't try and force any one particular ellipsoid standard. The geoViewpoint's ellipsoid is
136 the one we use for SPEED, LEVEL calculations
137-- most coords are LCS at some point, in preparation for 3D viewing,
138 and whatever ellipsoid they came from, they can mix as LCS XYZ
139* single planet v3.3 vs multiple planets via <GeoPlanet/>
140 following specs 3.3, we can only do one world/planet in a scene. We can't do a planet and several moons
141 in geocoords in the same scene. Thats because we need to subtract the geoviewpoint
142 location -or geoOrigin / autoOrigin- from geoshapes, to get coordinates into single precision float range
143 for display. And to do that, we assume the geoviewpoint and geoShapes are on the
144 same planet.
145 + Feb 2018 we did 'depth slices' in rendering - up to 3 slices - to improve rendering
146 with large coordinates at the single planet and multi-planet scale
147 + Mar 2018 we added <GeoPlanet planetId='0'> <GeoNode1/><GeoNode2.../> </GeoPlanet>
148 that converts LCS to GC for orbital mechanics, and pushes and pops a planetId from a stack,
149 for use in node-node interactions. So we can now do multiple planets
150* UNITS as per specs
151 while we are interpreting radians as default for web3d specs v3.3, and degrees < 3.3
152 we haven't tested linear UNITS conversion on parsing for geocoordinates.
153 Internally we are assuming meters. (all ellipsoid constants are in meters,
154 as are falseEasting and falseNorthing constants for UTM, and geoid height correction)
155* geoid correction: needs a review to ensure 2-way, round-trip symmetry
156 if set, it means the scenefile height data is with repect to mean sea level MSL
157 and when converting to GC, where MSL is higher than the ellipsoid the correction is down
158 and vice versa when converting back from GC to GD or XTM.
159 A test: using lat,lon of mount everest -which would have a mass that pulls sea level up-
160 the correction should be GC = gdtogc(lat,lon, gdheight -abs(geoid_correction(lat,lon)) )
161* geoOrigin / autoOrigin - the specs changed in v3.3 to deprecate geoOrigin
162 we are supposed to automatically compute an origin to use
163 Feb 2018 we are using FCFS First Come First Served - the first geoNode to compile_ we use
164 its geoOrigin / geoPoint / geo something as an arbitrary origin for a LCS local coordinate
165 system.
166* LCS local coordinate system - a shared cartesion coordinate system for all geoNodes on one planet.
167 the specs use this name for geoOrigins:
168 LCSxyz = originRotation x (GCxyz - originXYZ)
169 http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/geodata.html#high-precisioncoords
170 All regular nodes not wrapped in GeoLocation use the LCS by default, or should.
171 Viewer: a few nav modes use LCS (examine, turntable)
172 GeoPlanet converts children's LCS back into GC coordinates for orbital mechanics, regular nodes working in GC,
173 and inter-planet transform stacks
174 NOTE: freewrl has double precision transform stacks, and if geoViewpoint and geo<Geometry> are in
175 the same local on a planet, then their local2gc x gc2local will largely cancel out in double
176 precision matrix multiplication - no geoOrigin / autoOrigin is needed, can put the stack in GC.
177 Its when mixing regular and geonodes without wrapping/unwrapping with GL and GT that local coords LCS
178 is helpful for precision. But this scenario is not reliably reproduced among browsers - there's no
179 clear specification for how to mix geo and non-geo without GT/GL wrappers: there's a geoOrigin or
180 autoOrigin, but it doesn't show the math how to use it (or does it?)
181 And for GEG freewrl uses floats internally for rendering mesh, so may need hlep from a transform wrapper
182 to maintain precision (but won't help with a one-mesh world without breaking up mesh)
183* TCS topocentric coordinate system aka NodeLocalSystem NLS
184 somewhat related, for any giving GeoLocationNode, the topocentric coordinate system TCS with X east, -Z north, Y up
185 http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/geodata.html#GeoLocation
186 in freewrl we use the topocentric alignment at Origin for our originRotation aka AutoOrient
187 Viewer: most nav modes use TopoCentric TCS (walk, fly, spherical, keyboard)
188* relative heights - not in the specs but we use it with GVP in WALK mode and GL (geoLocation)
189 2 methods of RELATIVE:
190 1. maintainRelative: (implicitly enforced in GVP WALK + COLLIDE)
191 you give an absolute height, and relativeHeight0 = absolute - terrainHeight
192 From then on absolute = terrainHeight + relativeHeight0
193 initial position is the relative height from then on -as you animate the xy / latlong position.
194 2. relativeHeight: (explicit field in GVP and GL)
195 - the .position or .geoCoords you give it initially, via route or direct access is
196 relative to terrain height, so absolute = gdCoords.c[2] + terrainHeight(gdCoords.c[0],.c[1])
197 if no terrain underneath GL, then terrainHeight = ellipsoid height == 0
198* routing geo coordinates, GeoConvert
199 the v3.3 specs don't mention explicitly what system the geoCoordinate
200 events are in, but they must be in the geoSystem of the source node.
201 It doesn't make sense to route GD output to GC or XTM input on another node.
202 And v3.3 gives no way to convert.
203 + Mar 2018 we added new nodetype GeoConvert. You use 2 of them, like so:
204 source.geoCoord -> user1 -> .set_geoCoord (geoConvert1) .gcCoord_Changed -> GC
205 GC -> .set_gcCoord (geoConvert2) .geoCoord_changed -> user2 -> destination.position
206 where geoConvert1.geoSystem == source.geoSystem and geoConvert2.geoSystem == destination.geoSystem
207
208
209* Transform stack versus GeoCoordinates
210 There are 2 ways to get the relative pose of two nodes:
211 a) via transform stack
212 b) via GeoCoordinates
213 Our current theory of operation:
214 1) when doing geoNode-geoNode interactions, you should use geoCoords for both via GC intermediary
215 - that means ignoring transforms in between, and ignoring the modelview matrix
216 - GC intermediary allows you to use different geoSystem for each geoNode
217 2) when doing geo-regular, regular-geo interactions, you should use the transform stack
218 - push/pop transforms like regular nodes do
219 - transform stack is in/wrt LCS (the shared autoOrigin-related cartesian system)
220 - RENDERING and geometry vertices are wrt stacks and LCS
221 - and so get the effect of transforms inbetween
222 - or better/more consistent across browsers: use helper nodes geoLocation and geoTransform
223 2a) geoLocation can wrap regular nodes to convert to geo
224 2b) geoTransform can wrap geoNodes to convert to regular stack
225 3) planets can be separated a few different ways:
226 a) Layers - using Layer_Component, with one planet per layer
227 x problem: each layer has an active viewpoint, leading to confused rendering
228 b) use transform stack somehow to tell if 2 nodes are close enough to be on the same planet
229 c) use a geoTransform to wrap each planet
230 (this implies you can only use one geoTransform per planet
231 which may not be what specs meant, or how other browsers are using it)
232 d) wrap geonodes in Planet nodes to keep them separate (but not web3d specs node)
233 We use d) Planet
234 - but geoTransform might be more specs-aligned by adding a planetId field to it
235 - so a planet can have multiple geoTransforms if they share the same planetId
236
237
238*/
239
240
241/*
242Coordinate Conversion algorithms were taken from 2 locations after
243reading and comprehending the references. The code selected was
244taken and modified, because the original coders "knew their stuff";
245any problems with the modified code should be sent to John Stewart.
246
247------
248References:
249
250Jean Meeus "Astronomical Algorithms", 2nd Edition, Chapter 11, page 82
251
252"World Geodetic System"
253http://en.wikipedia.org/wiki/WGS84
254http://en.wikipedia.org/wiki/Geodetic_system#Conversion
255
256"Mathworks Aerospace Blockset"
257http://www.mathworks.com/access/helpdesk/help/toolbox/aeroblks/index.html?/access/helpdesk/help/toolbox/aeroblks/geocentrictogeodeticlatitude.html&http://www.google.ca/search?hl=en&q=Geodetic+to+Geocentric+conversion+equation&btnG=Google+Search&meta=
258
259"TRANSFORMATION OF GEOCENTRIC TO GEODETIC COORDINATES WITHOUT APPROXIMATIONS"
260http://www.astro.uni.torun.pl/~kb/Papers/ASS/Geod-ASS.htm
261
262"Geodetic Coordinate Conversions"
263http://www.gmat.unsw.edu.au/snap/gps/clynch_pdfs/coordcvt.pdf
264
265"TerrestrialCoordinates.c"
266http://www.lsc-group.phys.uwm.edu/lal/slug/nightly/doxygen/html/TerrestrialCoordinates_8c.html
267
268------
269Code Conversions:
270
271Geodetic to UTM:
272UTM to Geodetic:
273 Geo::Coordinates::UTM - Perl extension for Latitiude Longitude conversions.
274 Copyright (c) 2000,2002,2004,2007 by Graham Crookham. All rights reserved.
275
276Geocentric to Geodetic:
277Geodetic to Geocentric:
278 Filename: Gdc_To_Gcc_Converter.java
279 Author: Dan Toms, SRI International
280 Package: GeoTransform <http://www.ai.sri.com/geotransform/>
281 Acknowledgements:
282 The algorithms used in the package were created by Ralph Toms and
283 first appeared as part of the SEDRIS Coordinate Transformation API.
284 These were subsequently modified for this package. This package is
285 not part of the SEDRIS project, and the Java code written for this
286 package has not been certified or tested for correctness by NIMA.
287
288
289*********************************************************************/
290/* compileGeosystem - encode the return value such that srf->p[x] is...
291 0: spatial reference frame (GEOSP_UTM, GEOSP_GC, GEOSP_GD);
292 1: ellipsoid index (defaults to GEOSP_WE)
293 2: UTM zone number, 1..60. INT_ID_UNDEFINED = not specified
294 3: UTM: if "northing_first" TRUE, if "easting_first", FALSE
295 4: UTM: if "S" - value is FALSE, not S, value is TRUE
296 5: GD: if "latitude_first" TRUE, if "longitude_first", FALSE
297 6: GD: true if geoid height
298 7: GD: TRUE: decimal degrees, FALSE radians
299*/
300
301struct _geosys {
302 int spatial_system; //0
303 int ellipsoid; //1
304 int xtm_zone; //2
305 int xtm_northing_first; //3
306 int utm_northern_hemisphere; //4
307 int gd_latitude_first; //5
308 int geoid_height; //6
309 int gd_degrees; //7
310 int relativeHeight; //8
311};
312//#define GEOSYS( geosystem ) ((Geosys *)geosystem)
313
314int isNodetypeGeospatial(int nodetype, int specversion){
315 //its geospatial if it has a geoSystem field (GeoMetadata doesn't, a few DIS v3.3 do)
316 int iret =
317 nodetype == NODE_GeoCoordinate || nodetype == NODE_GeoElevationGrid || nodetype == NODE_GeoLOD
318 || nodetype == NODE_GeoLocation || nodetype == NODE_GeoPositionInterpolator
319 || nodetype == NODE_GeoProximitySensor || nodetype == NODE_GeoTouchSensor
320 || nodetype == NODE_GeoTransform || nodetype == NODE_GeoViewpoint || nodetype == NODE_GeoOrigin ? TRUE : FALSE;
321 if(!iret && specversion > 320) {
322 iret =
323 nodetype == NODE_EspduTransform || nodetype == NODE_ReceiverPdu || nodetype == NODE_SignalPdu
324 || nodetype == NODE_TransmitterPdu ? TRUE : FALSE;
325 }
326 return iret;
327}
328int isNodeGeospatial(struct X3D_Node* node){
329 return isNodetypeGeospatial(node->_nodeType, X3D_PROTO(node->_executionContext)->__specversion);
330}
331
332#define STRICT33 TRUE //TRUE: if v3.3 assume incoming GD in base angle units (radians) FALSE: assume like v3.2-- degrees
333
334/* defines used to get a SFVec3d into/outof a function that expects a MFVec3d */
335#define MF_SF_TEMPS struct Multi_Vec3d mIN; struct Multi_Vec3d mOUT; struct Multi_Vec3d gdCoords;
336#define FREE_MF_SF_TEMPS FREE_IF_NZ(gdCoords.p); FREE_IF_NZ(mOUT.p); FREE_IF_NZ(mIN.p);
337
338#define COMPILE_GEOSYSTEM(me) compile_geoSystem (X3D_NODE(me), me->_nodeType, &me->geoSystem, &me->__geoSystem);
339
340#define RADIANS_PER_DEGREE (double)0.0174532925199432957692
341#define DEGREES_PER_RADIAN (double)57.2957795130823208768
342
343
344/* for UTM, GC, GD conversions */
345#define UTM_SCALE (double)0.9996
346#define UTM_FALSE_EASTING 500000.0 //500k m
347#define UTM_FALSE_NORTHING 10000000.0 //10M m
348#define UTM_ZONE_SIZE 6.0
349
350#define U3TM_SCALE (double)0.9999
351#define U3TM_FALSE_EASTING 0.0
352#define U3TM_FALSE_NORTHING 0.0
353#define U3TM_ZONE_SIZE 3.0
354
355
356/* for Gd_Gc conversions */
357#define GEOEL_AA_A (double)6377563.396
358#define GEOEL_AA_F (double)299.3249646
359#define GEOEL_AM_A (double)6377340.189
360#define GEOEL_AM_F (double)299.3249646
361#define GEOEL_AN_A (double)6378160
362#define GEOEL_AN_F (double)298.25
363#define GEOEL_BN_A (double)6377483.865
364#define GEOEL_BN_F (double)299.1528128
365#define GEOEL_BR_A (double)6377397.155
366#define GEOEL_BR_F (double)299.1528128
367#define GEOEL_CC_A (double)6378206.4
368#define GEOEL_CC_F (double)294.9786982
369#define GEOEL_CD_A (double)6378249.145
370#define GEOEL_CD_F (double)293.465
371#define GEOEL_EA_A (double)6377276.345
372#define GEOEL_EA_F (double)300.8017
373#define GEOEL_EB_A (double)6377298.556
374#define GEOEL_EB_F (double)300.8017
375#define GEOEL_EC_A (double)6377301.243
376#define GEOEL_EC_F (double)300.8017
377#define GEOEL_ED_A (double)6377295.664
378#define GEOEL_ED_F (double)300.8017
379#define GEOEL_EE_A (double)6377304.063
380#define GEOEL_EE_F (double)300.8017
381#define GEOEL_EF_A (double)6377309.613
382#define GEOEL_EF_F (double)300.8017
383#define GEOEL_FA_A (double)6378155
384#define GEOEL_FA_F (double)298.3
385#define GEOEL_HE_A (double)6378200
386#define GEOEL_HE_F (double)298.3
387#define GEOEL_HO_A (double)6378270
388#define GEOEL_HO_F (double)297
389#define GEOEL_ID_A (double)6378160
390#define GEOEL_ID_F (double)298.247
391#define GEOEL_IN_A (double)6378388
392#define GEOEL_IN_F (double)297
393#define GEOEL_KA_A (double)6378245
394#define GEOEL_KA_F (double)298.3
395#define GEOEL_RF_A (double)6378137
396#define GEOEL_RF_F (double)298.257222101
397#define GEOEL_SA_A (double)6378160
398#define GEOEL_SA_F (double)298.25
399#define GEOEL_WD_A (double)6378135
400#define GEOEL_WD_F (double)298.26
401#define GEOEL_WE_A (double)6378137
402#define GEOEL_WE_F (double)298.257223563
403
404
405
406#define ELLIPSOIDB(typ) \
407 case typ: *semimajor = typ##_A; *flattening = 1.0/typ##_F; break;
408
409struct ellipsoid { double a, b, f;} extra_ellipsoid[10];
410int nextra_ellipsoid = 1; //we start at 1 so we can use negative numbers as sentinal values to get here
411
412int getEllipsoidParams(int etype, double *semimajor, double *flattening){
413 //returns a, f where f is flattening (not inverse flattening) ie flattening = 1/298
414 int iret = 0;
415 *semimajor = *flattening = 0.0;
416 if(etype < 0){
417 //sentinal value etype is negative
418 *semimajor = extra_ellipsoid[-etype].a;
419 *flattening = extra_ellipsoid[-etype].f;
420 }else{
421 switch (etype) {
422 ELLIPSOIDB(GEOEL_AA)
423 ELLIPSOIDB(GEOEL_AM)
424 ELLIPSOIDB(GEOEL_AN)
425 ELLIPSOIDB(GEOEL_BN)
426 ELLIPSOIDB(GEOEL_BR)
427 ELLIPSOIDB(GEOEL_CC)
428 ELLIPSOIDB(GEOEL_CD)
429 ELLIPSOIDB(GEOEL_EA)
430 ELLIPSOIDB(GEOEL_EB)
431 ELLIPSOIDB(GEOEL_EC)
432 ELLIPSOIDB(GEOEL_ED)
433 ELLIPSOIDB(GEOEL_EE)
434 ELLIPSOIDB(GEOEL_EF)
435 ELLIPSOIDB(GEOEL_FA)
436 ELLIPSOIDB(GEOEL_HE)
437 ELLIPSOIDB(GEOEL_HO)
438 ELLIPSOIDB(GEOEL_ID)
439 ELLIPSOIDB(GEOEL_IN)
440 ELLIPSOIDB(GEOEL_KA)
441 ELLIPSOIDB(GEOEL_RF)
442 ELLIPSOIDB(GEOEL_SA)
443 ELLIPSOIDB(GEOEL_WD)
444 ELLIPSOIDB(GEOEL_WE)
445 default: printf ("unknown ellipsoid type: %s\n", stringGEOSPATIALType(etype));
446 }
447 }
448 if(*semimajor > 0.0) iret = 1;
449 //printf("ellipsoid etype %d semi-major %lf flattening %lf\n",etype,*semimajor,*flattening);
450 return iret;
451}
452
453
454#define INITIALIZE_GEOSPATIAL(me) \
455 initializeGeospatial((struct X3D_GeoOrigin **) &me->geoOrigin);
456
457
458void CONVERT_BACK_TO_GD_OR_UTMB(Geosys *targetGeoSystem, struct X3D_Node *GeoOrigin,
459 struct SFVec3d *thisField);
460
461void compile_geoSystem (struct X3D_Node *, int nodeType, struct Multi_String *args, struct X3D_Node **srf);
462static void gccToGdc (Geosys *geoSystem, struct SFVec3d *gcc, struct SFVec3d *gdc);
463void calculateViewingSpeed(void);
464
465struct Planet {
466 int ID;
467 Stack *gegs;
468 struct SFVec4d autoOrient; //tilts to get from GC xyz to TCS at autoOrigin GC
469 struct SFVec3d autoOrigin; //GC coords
470 int autoOriginSet;
471};
472
473void clear_planets(Stack *planet_stack){
474 // call from Component_Geospatial_clear() at end of run
475 int i;
476 struct Planet *planet;
477 if(planet_stack)
478 for(i=0;i<vectorSize(planet_stack);i++){
479 planet = vector_get_ptr(struct Planet,planet_stack,i);
480 if(planet && planet->gegs) deleteVector(struct X3D_Node *, planet->gegs);
481 }
482 deleteVector(struct Planet,planet_stack);
483}
484
486 //struct SFVec4d autoOrient;
487 //struct SFVec3d autoOrigin;
488 //int autoOriginSet;
489 //struct X3D_GeoOrigin *go;
490 int geoLodLevel;// = 0;
491 void * gcgdpars[50];
492 Stack *planet_stack;
493 Stack *current_planet_stack;
494#ifdef GEOLIB
495 void * fgeopars[50];
496#endif //GEOLIB
497
499void *Component_Geospatial_constructor(){
500 void *v = MALLOCV(sizeof(struct pComponent_Geospatial));
501 memset(v,0,sizeof(struct pComponent_Geospatial));
502 return v;
503}
504void Component_Geospatial_init(struct tComponent_Geospatial *t){
505 //public
506 //private
507 t->prv = Component_Geospatial_constructor();
508 {
509
511 //p->go = createNewX3DNode0(NODE_GeoOrigin);
512 p->geoLodLevel = 0;
513 {
514 struct Planet planet;
515 memset(&planet,0,sizeof(struct Planet));
516 vecset4d(planet.autoOrient.c,0.0,0.0,1.0,0.0);
517 vecsetd(planet.autoOrigin.c,0.0,0.0,0.0);
518 planet.autoOriginSet = USE_GC_FOR_LCS; // FALSE; //FALSE;
519
520 p->planet_stack = newStack(struct Planet);
521 stack_push(struct Planet,p->planet_stack,planet); //default planet
522
523 }
524 p->current_planet_stack = newStack(int);
525 stack_push(int,p->current_planet_stack,0); //default planet
526 memset(p->gcgdpars,0,50*sizeof(void*));
527 #ifdef GEOLIB
528 memset(p->fgeopars,0,50*sizeof(void*));
529 #endif //GEOLIB
530 }
531}
532
533void Component_Geospatial_clear(struct tComponent_Geospatial *t){
534 if(t->prv )
535 {
537 if(p->planet_stack){
538 clear_planets(p->planet_stack);
539 p->planet_stack = NULL;
540 }
541 if(p->current_planet_stack){
542 FREE_IF_NZ(p->current_planet_stack->data);
543 FREE_IF_NZ(p->current_planet_stack);
544 p->current_planet_stack = NULL;
545 }
546
547 }
548}
549//ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
550void push_planetId(int planetId){
551 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
552 Stack *current_planet_stack = (Stack*)p->current_planet_stack;
553 stack_push(int,current_planet_stack,planetId);
554}
555int current_planetId(){
556 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
557 Stack *current_planet_stack = (Stack*)p->current_planet_stack;
558 return stack_top(int,current_planet_stack);
559}
560void pop_planetId(){
561 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
562 Stack *current_planet_stack = (Stack*)p->current_planet_stack;
563 stack_pop(int,current_planet_stack);
564}
565struct Planet *add_planet(int planetId){
566 struct Planet planet, *ppointer;
567 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
568 memset(&planet,0,sizeof(struct Planet));
569 vecset4d(planet.autoOrient.c,0.0,0.0,1.0,0.0);
570 vecsetd(planet.autoOrigin.c,0.0,0.0,0.0);
571 planet.ID = planetId;
572 planet.autoOriginSet = USE_GC_FOR_LCS; //FALSE; //FALSE;
573 stack_push(struct Planet,p->planet_stack,planet); //default planet
574 ppointer = vector_get_ptr(struct Planet,p->planet_stack,p->planet_stack->n-1);
575 return ppointer;
576}
577struct Planet *current_planet(){
578 int planetId, i;
579 struct Planet *planet;
580 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
581 planetId = stack_top(int,p->current_planet_stack);
582 //we should sort planets by planetId or use a hash, or compile planetId to index
583 planet = NULL;
584 for(i=0;i<vectorSize(p->planet_stack);i++){
585 planet = vector_get_ptr(struct Planet,p->planet_stack,i);
586 if(planetId == planet->ID){
587 return planet;
588 }
589 }
590 return NULL;
591}
592
593
594// http://www.colorado.edu/geography/gcraft/notes/datum/geoid84.html
595char geoid[][36] = {
596//-180 longitude ---------------------------------------------------------- + 170 longitude
597{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}, //+90 north pole
598{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}, //+80
599{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}, //+70
600{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}, //+60
601{-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}, //+50
602{-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}, //+40
603{-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}, //+30
604{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}, //+20
605{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}, //+10
606{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}, //equator
607{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}, //-10
608{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}, //-20
609{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}, //-30
610{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}, //-40
611{-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}, //-50
612{-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}, //-60
613{-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}, //-70
614{-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}, //-80
615{-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} //-90 south pole
616};
617
618static double geoidCorrection(double latitudeDeg, double longitudeDeg)
619{
620 //assumes: GD coords in degrees, -180 to +180 range for longitude, -90 to +90 latitude range
621 //returns: what to add to sea level height to get an ellipsoid height
622 // - does not add it - you do that in the caller.
623 // - http://en.wikipedia.org/wiki/Geoid has a global diagram to check the sign/sense of the correction
624 //benefits: allows you to mix GPS and topographic data in the same scene and have good alignment
625 //usage: put 2 GeoOrigins one for GPS and one for local Topographic in your scene, but set them (initially)
626 // both the same, and set them both without the 'WGS84' (geoid) geosystem option.
627 // Apply option 'WGS84' (geoid) option to your topographic data GeoCoordinate GeoSystem (except GeoOrigin).
628 // Then touch up when viewing them in the same scene by adjusting your local topographic geoOrigin height.
629 int il, ip, il1, ip1;
630 double dl, dp, d00, d01, d10, d11, d;
631 //step 1: find the cell indexes
632 il = (int)(longitudeDeg/10.0) + 18 -1; //longitude cell
633 ip = 18 - ((int)(latitudeDeg/10.0) + 9); //latitude cell
634 il1 = il + 1;
635 ip1 = ip - 1;
636 if(il1 > 35) il1 = 0;
637 if(ip1 < 0) ip1 = 0;
638 //step 2: compute the corners of the cell
639 d00 = (float)geoid[ip][il]; //lower left
640 d01 = (float)geoid[ip1][il]; //upper left
641 d10 = (float)geoid[ip][il1]; //lower right
642 d11 = (float)geoid[ip1][il1]; //upper right
643 //step 3: find the position in the cell
644 dl = longitudeDeg - (-180.0 + (float)(il)*10.0);
645 dp = latitudeDeg - (90.0 - (float)(ip)*10.0);
646 //step 4: find the normalized position within the cell range 0-1
647 dl /= 10.0;
648 dp /= 10.0;
649 //step 5: bilinear interpolate from 4 corners to position in cell
650 d = (1.0 - dl)*(1.0 - dp)*d00 + (dl)*(1.0 - dp)*d10 + (1.0-dl)*(dp)*d01 + (dl)*(dp)*d11;
651 // d is how much higher the geoid is from the ellipsoid.
652 d = -d; // to correct a geoid map to ellpsoid, subtract this amount
653 return (double)d;
654}
655
656
657/* convert GD ellipsiod to GC coordinates. swizzles and converts degrad as needed. */
658
659// swizzles and converts degrad as needed
660static void Gd_Gc3d_fw(Geosys *geoSystem, struct SFVec3d *inc, int n, struct SFVec3d *outc) {
661 int geotype, lat_first, geoid;
662 double radius, flattening;
663 geotype = geoSystem->ellipsoid;
664 lat_first = geoSystem->gd_latitude_first;
665 geoid = geoSystem->geoid_height;
666 if(getEllipsoidParams(geotype,&radius,&flattening))
667 {
668 int i;
669 double A = radius;
670 double A2 = radius*radius;
671 double F = flattening;
672 double C = A*((double)1.0 - F);
673 double C2 = C*C;
674 double Eps2 = F*((double)2.0 - F);
675 double Eps25 = (double) 0.25 * Eps2;
676
677 int latitude = 0;
678 int longitude = 1;
679 int elevation = 2;
680
681 double source_lat;
682 double source_lon;
683 double slat;
684 double slat2;
685 double clat;
686 double Rn;
687 double RnPh;
688
689 if (!lat_first) {
690 printf ("Gd_Gc, NOT lat first\n");
691 latitude = 1; longitude = 0;
692 }
693
695 //if (outc->n < inc->n) {
696 // FREE_IF_NZ(outc->p);
697 // outc->p = MALLOC(struct SFVec3d *, sizeof (struct SFVec3d) * inc->n);
698 // outc->n = inc->n;
699 //}
700 #ifdef VERBOSE
701 printf ("Gd_Gc, have n of %d\n",inc->n);
702 #endif
703
704 for (i=0; i<n; i++) {
705 #ifdef VERBOSE
706 printf ("Gd_Gc, ining lat %lf long %lf ele %lf ",LATITUDE_IN, LONGITUDE_IN, ELEVATION_IN);
707 #endif
708
709 if(geoSystem->gd_degrees == FALSE){
710 //version 3.3+ by default in 'angle base units' which are radians
711 source_lat = inc[i].c[latitude]; //LATITUDE_IN;
712 source_lon = inc[i].c[longitude]; //LONGITUDE_IN;
713 }else{
714 //version 3.2- by default in degrees
715 source_lat = RADIANS_PER_DEGREE * inc[i].c[latitude];// LATITUDE_IN;
716 source_lon = RADIANS_PER_DEGREE * inc[i].c[longitude]; //LONGITUDE_IN;
717 }
718
719 #ifdef VERBOSE
720 printf ("Source Latitude %lf Source Longitude %lf\n",source_lat, source_lon);
721 #endif
722
723 slat = sin(source_lat);
724 slat2 = slat*slat;
725 clat = cos(source_lat);
726
727 #ifdef VERBOSE
728 printf ("slat %lf slat2 %lf clat %lf\n",slat, slat2, clat);
729 #endif
730
731
732 /* square root approximation for Rn */
733 Rn = A / ( (.25 - Eps25 * slat2 + .9999944354799/4) + (.25-Eps25 * slat2)/(.25 - Eps25 * slat2 + .9999944354799/4));
734
735 RnPh = Rn + inc[i].c[elevation]; //ELEVATION_IN;
736
737 //MAR 2018 we are doing geoid at the user2something, something2user level now, higher in the call stack
738 if(geoid && FALSE){
739 double dlatin, dlongin;
740 dlatin = inc[i].c[latitude]; //LATITUDE_IN;
741 dlongin = inc[i].c[longitude]; //LONGITUDE_IN;
742 if(geoSystem->gd_degrees == FALSE){
743 dlatin *= DEGREES_PER_RADIAN;
744 dlongin *= DEGREES_PER_RADIAN;
745 }
746 RnPh += geoidCorrection(dlatin,dlongin); //LATITUDE_IN,LONGITUDE_IN);
747 }
748 #ifdef VERBOSE
749 printf ("Rn %lf RnPh %lf\n",Rn, RnPh);
750 #endif
751
752 outc[i].c[0] = RnPh * clat * cos(source_lon); //GC_X_OUT
753 outc[i].c[1] = RnPh * clat * sin(source_lon);
754 outc[i].c[2] = ((C2 / A2) * Rn + inc[i].c[elevation]) * slat; //ELEVATION_IN
755
756 #ifdef VERBOSE
757 printf ("Gd_Gc, outing x %lf y %lf z %lf\n", GC_X_OUT, GC_Y_OUT, GC_Z_OUT);
758 #endif
759 }
760 }
761}
762#ifdef GEOLIB
763double dclamp(double fval, double fstart, double fend);
764static void* fwgeo_gc[50];
765static void Gd_Gc3d_geolib(Geosys *geoSystem, struct SFVec3d *inc, int n, struct SFVec3d *outc){
766 int i,geotype;
767 double semimajor, flattening, gd[3], gc[3];
768 //printf("hi from Gd_Gc3d_geolib\n");
769 getEllipsoidParams(geoSystem->ellipsoid,&semimajor,&flattening);
770 if(FALSE && flattening == 0.0){
771 //easy spherical coords, but geolib OK without this, just for testing here
772 double radius;
773 for(i=0;i<n;i++){
774 veccopyd(gd,inc[i].c);
775 if(!geoSystem->gd_latitude_first) vecswizzle2d(gd);
776 if(geoSystem->gd_degrees) vecscale2d(gd,gd,RADIANS_PER_DEGREE);
777 radius = semimajor + gd[2];
778 gc[0] = cos(gd[0])*cos(gd[1])*radius;
779 gc[1] = cos(gd[0])*sin(gd[1])*radius;
780 gc[2] = sin(gd[0])*radius;
781 veccopyd(outc[i].c,gc);
782 //if(i<10) printf("gd2gc sphere %lf %lf %lf\n",gc[0],gc[1],gc[2]);
783 }
784 }
785 else
786 {
787 geotype = geoSystem->ellipsoid;
788 if(geotype < 0) geotype = -geotype + GEOELLIPSOID_COUNT;
789 if(!fwgeo_gc[geotype]){
790 fwgeo_gc[geotype] = fgeo_initializeGC(semimajor,flattening);
791 }
792 for(i=0;i<n;i++){
793 veccopyd(gd,inc[i].c);
794 if(!geoSystem->gd_latitude_first) vecswizzle2d(gd);
795 if(!geoSystem->gd_degrees) vecscale2d(gd,gd,DEGREES_PER_RADIAN); //geolib uses degrees
796 // if you get 1.#QNAN00000000000 coming out here, its because geolib insists on -90,90 for lat
797 // and GEG has a bit of rounding error noise that exceeds slightly .0000001
798 //if(n>1 && i < 200) printf("before clamp %ld %lf %lf %lf\n",i,gd[0],gd[1],gd[2]);
799 gd[0] = dclamp(gd[0],-90.0,90.0);
800 fgeo_gd2gc(fwgeo_gc[geotype], gd[0], gd[1], gd[2], &gc[0], &gc[1], &gc[2]);
801 veccopyd(outc[i].c,gc);
802 //if(i<10) printf("gd2gc geolb %lf %lf %lf\n",gc[0],gc[1],gc[2]);
803 }
804 }
805 if(n>1)
806 getchar();
807}
808#endif //GEOLIB
809static void Gd_Gc3d(Geosys *geoSystem, struct SFVec3d *inc, int n, struct SFVec3d *outc){
810 int i;
811#ifdef GEOLIB
812 if(method_geolib()){
813 Gd_Gc3d_geolib(geoSystem,inc,n,outc);
814 //printf("geolib gd:\n");
815 //for(i=0;i<min(200,n);i++){
816 // printf("%d %lf %lf %lf\n",i,outc[i].c[0],outc[i].c[1],outc[i].c[2]);
817 //}
818 }else
819#endif //GEOLIB
820 {
821 double semimajor, flattening;
822 getEllipsoidParams(geoSystem->ellipsoid,&semimajor,&flattening);
823 if(flattening == 0.0){
824 //easy spherical coords
825 //Gd_Gc3d_fw and/or its gc2gd complement has a problem with moon geoSystem 'R173...' 'F0.0'
826 double radius, gd[3], gc[3];
827 for(i=0;i<n;i++){
828 veccopyd(gd,inc[i].c);
829 if(!geoSystem->gd_latitude_first) vecswizzle2d(gd);
830 if(geoSystem->gd_degrees) vecscale2d(gd,gd,RADIANS_PER_DEGREE);
831 radius = semimajor + gd[2];
832 gc[0] = cos(gd[0])*cos(gd[1])*radius;
833 gc[1] = cos(gd[0])*sin(gd[1])*radius;
834 gc[2] = sin(gd[0])*radius;
835 veccopyd(outc[i].c,gc);
836 //if(i<10) printf("gd2gc sphere %lf %lf %lf\n",gc[0],gc[1],gc[2]);
837 }
838 }
839 else
840 {
841 Gd_Gc3d_fw(geoSystem,inc,n,outc);
842 //printf("fw gd:\n");
843 //for(i=0;i<min(5,n);i++){
844 // printf("%d %lf %lf %lf\n",i,outc[i].c[0],outc[i].c[1],outc[i].c[2]);
845 //}
846 //printf("\n");
847 }
848 }
849}
850/* convert UTM to GC coordinates by converting to GD as an intermediary step
851 we swizzle both lat,long and east,north
852 we convert to radians if necessary
853*/
854static void Xtm_Gd3d(Geosys *geoSystem, struct SFVec3d *inc, int n, struct SFVec3d *outc, double radius, double flatten,
855 double scaleFactor, double falseEasting, double falseNorthing, double zoneSize) {
856
857 int hemisphere_north, zone, northing_first;
858 int i;
859 int northing = 0; /* for determining which input value is northing */
860 int easting = 1; /* for determining which input value is easting */
861 int elevation = 2; /* elevation is always third value, input AND output */
862 int latitude = 0; /* always return latitude as first value */
863 int longitude = 1; /* always return longtitude as second value */
864
865 /* create the ERM constants. */
866 double F = flatten;
867 double Eccentricity = (F) * (2.0-F);
868
869 double myEasting;
870 double myphi1rad;
871 double myN1;
872 double myT1;
873 double myC1;
874 double myR1;
875 double myD;
876 double Latitude;
877 double Longitude;
878 double longitudeOriginDeg;
879 double myeccPrimeSquared;
880 double myNorthing;
881 double northingDRCT1;
882 double eccRoot;
883 double calcConstantTerm1;
884 double calcConstantTerm2;
885 double calcConstantTerm3;
886 double calcConstantTerm4;
887
888 if(geoSystem->gd_latitude_first == FALSE){
889 latitude = 1;
890 longitude = 0;
891 }
892
893 hemisphere_north = geoSystem->utm_northern_hemisphere;
894 zone = geoSystem->xtm_zone;
895 northing_first = geoSystem->xtm_northing_first;
896
897
898 /* is the values specified with an "easting_first?" */
899 if (!northing_first) { northing = 1; easting = 0; }
900 //printf("Xtm_Gd hemisphere-north=%d zone=%d northing_first=%d\n",hemisphere_north,zone,northing_first);
901 //printf("Xtm_Gd scalefactor %lf falseEasting %lf falseNorthing %lf zoneSize %lf\n",scaleFactor, falseEasting, falseNorthing, zoneSize);
902
903 #ifdef VERBOSE
904 if (!northing_first) printf ("UTM to GD, not northing first, flipping norhting and easting\n");
905 #endif
906
907 #ifdef VERBOSE
908 if (northing_first) printf ("Utm_Gd: northing first\n"); else printf ("Utm_Gd: NOT northing_first\n");
909 if (!hemisphere_north) printf ("Utm_Gd: NOT hemisphere_north\n"); else printf ("Utm_Gd: hemisphere_north\n");
910 #endif
911
912
913 /* enough room for output? */
914 //if (outc->n < inc->n) {
915 // FREE_IF_NZ(outc->p);
916 // outc->p = MALLOC(struct SFVec3d *, sizeof (struct SFVec3d) * inc->n);
917 // outc->n = inc->n;
918 //}
919
920 /* constants for all UTM vertices */
921 longitudeOriginDeg = (zone -1) * zoneSize - 180. + zoneSize*.5;
922 myeccPrimeSquared = Eccentricity/(((double) 1.0) - Eccentricity);
923 eccRoot = (((double)1.0) - sqrt (((double)1.0) - Eccentricity))/
924 (((double)1.0) + sqrt (((double)1.0) - Eccentricity));
925
926 calcConstantTerm1 = ((double)1.0) -Eccentricity/
927 ((double)4.0) - ((double)3.0) *Eccentricity*Eccentricity/
928 ((double)64.0) -((double)5.0) *Eccentricity*Eccentricity*Eccentricity/((double)256.0);
929
930 calcConstantTerm2 = ((double)3.0) * eccRoot/((double)2.0) - ((double)27.0) *eccRoot*eccRoot*eccRoot/((double)32.0);
931 calcConstantTerm3 = ((double)21.0) * eccRoot*eccRoot/ ((double)16.0) - ((double)55.0) *eccRoot*eccRoot*eccRoot*eccRoot/ ((double)32.0);
932 calcConstantTerm4 = ((double)151.0) *eccRoot*eccRoot*eccRoot/ ((double)96.0);
933
934 #ifdef VERBOSE
935 printf ("zone %d\n",zone);
936 printf ("longitudeOriginDeg %lf\n",longitudeOriginDeg);
937 printf ("myeccPrimeSquared %lf\n",myeccPrimeSquared);
938 printf ("eccRoot %lf\n",eccRoot);
939 #endif
940
941 /* go through each vertex specified */
942 for(i=0;i<n;i++) {
943 /* get the values for THIS UTM vertex */
944 outc[i].c[elevation] = inc[i].c[elevation]; //ELEVATION_OUT = ELEVATION_IN;
945
946 myEasting = inc[i].c[easting] - falseEasting; //UTM_FALSE_EASTING; //500000; //EASTING_IN
947 if (hemisphere_north) myNorthing = inc[i].c[northing]; //NORTHING_IN;
948 else myNorthing = inc[i].c[northing] - falseNorthing; //(double)UTM_FALSE_NORTHING; //10000000.0; //NORTHING_IN
949
950 #ifdef VERBOSE
951 printf ("myEasting %lf\n",myEasting);
952 printf ("myNorthing %lf\n",myNorthing);
953 #endif
954
955
956 /* scale the northing */
957 myNorthing= myNorthing / scaleFactor;
958 northingDRCT1 = myNorthing /(radius * calcConstantTerm1);
959
960 myphi1rad = northingDRCT1 +
961 calcConstantTerm2 * sin(((double)2.0) *northingDRCT1)+
962 calcConstantTerm3 * sin(((double)4.0) *northingDRCT1)+
963 calcConstantTerm4 * sin(((double)6.0) *northingDRCT1);
964
965 myN1 = radius/sqrt(((double)1.0) - Eccentricity * sin(myphi1rad) * sin (myphi1rad));
966 myT1 = tan(myphi1rad) * tan(myphi1rad);
967 myC1 = Eccentricity * cos(myphi1rad) * cos (myphi1rad);
968 myR1 = radius * (((double)1.0) - Eccentricity) / pow(((double)1.0) - Eccentricity * sin(myphi1rad) * sin (myphi1rad), 1.5);
969 myD = myEasting/(myN1*scaleFactor);
970
971 Latitude = myphi1rad-(myN1*tan(myphi1rad)/myR1)*
972 (myD*myD/((double)2.0) -
973 (((double)5.0) + ((double)3.0) *myT1+ ((double)10.0) *myC1-
974 ((double)4.0) *myC1*myC1- ((double)9.0) *myeccPrimeSquared)*
975 myD*myD*myD*myD/((double)24.0) +(((double)61.0) +((double)90.0) *
976 myT1+((double)298.0) *myC1+ ((double)45.0) *myT1*myT1-
977 ((double)252.0) * myeccPrimeSquared- ((double)3.0) *myC1*myC1)*myD*myD*myD*myD*myD*myD/((double)720.0));
978
979 Longitude = (myD-(((double)1.0)+((double)2.0)*myT1+myC1)*myD*myD*myD/((double)6.0)+(((double)5.0) - ((double)2.0) *myC1+
980 ((double)28.0) *myT1-((double)3.0) *myC1*myC1+
981 ((double)8.0) *myeccPrimeSquared+((double)24.0) *myT1*myT1)*myD*myD*myD*myD*myD/120)/cos(myphi1rad);
982
983 if(geoSystem->gd_degrees == FALSE){
984 //version 3.3+ works in angle base units (radians) by default
985 outc[i].c[latitude] = Latitude ; //LATITUDE_OUT
986 outc[i].c[longitude] = longitudeOriginDeg*RADIANS_PER_DEGREE + Longitude; //LONGITUDE_OUT
987 }else{
988 //version 3.2- works in degrees by default
989 outc[i].c[latitude] = Latitude * DEGREES_PER_RADIAN;
990 outc[i].c[longitude] = longitudeOriginDeg + (Longitude * DEGREES_PER_RADIAN);
991 }
992
993
994 #ifdef VERBOSE
995 /* printf ("myNorthing scaled %lf\n",myNorthing);
996 printf ("northingDRCT1 %lf\n",northingDRCT1);
997 printf ("myphi1rad %lf\n",myphi1rad);
998 printf ("myN1 %lf\n",myN1);
999 printf ("myT1 %lf\n",myT1);
1000 printf ("myC1 %lf\n",myC1);
1001 printf ("myR1 %lf\n",myR1);
1002 printf ("myD %lf\n",myD);
1003 printf ("latitude %lf\n",Latitude);
1004 printf ("longitude %lf\n",Longitude);
1005 */
1006 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);
1007 #endif
1008 }
1009}
1010
1011#ifdef GEOLIB
1012static void Xtm_Gd3d_geolib(Geosys *geoSystem, struct SFVec3d *inc, int n, struct SFVec3d *outc,
1013 double radius, double flatten, double scaleFactor, double falseEasting, double falseNorthing,
1014 double zoneSize) {
1015
1016 int hemisphere_north, zone, northing_first, geotype;
1017 int i;
1018 int northing = 0; /* for determining which input value is northing */
1019 int easting = 1; /* for determining which input value is easting */
1020 int elevation = 2; /* elevation is always third value, input AND output */
1021 int latitude = 0; /* always return latitude as first value */
1022 int longitude = 1; /* always return longtitude as second value */
1023
1024 /* create the ERM constants. */
1025 double F = flatten;
1026 double dlon0;
1027 double dLatitude;
1028 double dLongitude;
1029 double dlongitudeOrigin;
1030 double myEasting;
1031 double myNorthing;
1032 void *fgeo;
1033 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
1034
1035 if(geoSystem->gd_latitude_first == FALSE){
1036 latitude = 1;
1037 longitude = 0;
1038 }
1039
1040 hemisphere_north = geoSystem->utm_northern_hemisphere;
1041 zone = geoSystem->xtm_zone;
1042 northing_first = geoSystem->xtm_northing_first;
1043 geotype = geoSystem->ellipsoid;
1044 if(geotype < 0) geotype = -geotype + GEOELLIPSOID_COUNT;
1045 if(!p->fgeopars[geotype])
1046 p->fgeopars[geotype] = fgeo_initializeTM(radius, F, 1.0);
1047 fgeo = p->fgeopars[geotype];
1048 /* is the values specified with an "easting_first?" */
1049 if (!northing_first) { northing = 1; easting = 0; }
1050 //printf("Xtm_Gd hemisphere-north=%d zone=%d northing_first=%d\n",hemisphere_north,zone,northing_first);
1051 //printf("Xtm_Gd scalefactor %lf falseEasting %lf falseNorthing %lf zoneSize %lf\n",scaleFactor, falseEasting, falseNorthing, zoneSize);
1052
1053 #ifdef VERBOSE
1054 if (!northing_first) printf ("UTM to GD, not northing first, flipping norhting and easting\n");
1055 #endif
1056
1057 #ifdef VERBOSE
1058 if (northing_first) printf ("Utm_Gd: northing first\n"); else printf ("Utm_Gd: NOT northing_first\n");
1059 if (!hemisphere_north) printf ("Utm_Gd: NOT hemisphere_north\n"); else printf ("Utm_Gd: hemisphere_north\n");
1060 #endif
1061
1062 /* constants for all UTM vertices */
1063 dlongitudeOrigin = (zone -1) * zoneSize - 180. + zoneSize*.5;
1064
1065 /* go through each vertex specified */
1066 for(i=0;i<n;i++) {
1067 /* get the values for THIS UTM vertex */
1068 outc[i].c[elevation] = inc[i].c[elevation]; //ELEVATION_OUT = ELEVATION_IN;
1069
1070 myEasting = inc[i].c[easting] - falseEasting; //UTM_FALSE_EASTING; //500000; //EASTING_IN
1071 if (hemisphere_north) myNorthing = inc[i].c[northing]; //NORTHING_IN;
1072 else myNorthing = inc[i].c[northing] - falseNorthing; //(double)UTM_FALSE_NORTHING; //10000000.0; //NORTHING_IN
1073
1074 myEasting /= scaleFactor;
1075 myNorthing /= scaleFactor;
1076
1077 #ifdef VERBOSE
1078 printf ("myEasting %lf\n",myEasting);
1079 printf ("myNorthing %lf\n",myNorthing);
1080 #endif
1081
1082 //this adds CM longitude on, no need to add it later.
1083 // works in decimal degrees
1084 fgeo_tm2gd(fgeo,myEasting, myNorthing, dlongitudeOrigin, &dLatitude, &dLongitude);
1085
1086 if(geoSystem->gd_degrees == FALSE){
1087 //version 3.3+ works in angle base units (radians) by default
1088 outc[i].c[latitude] = dLatitude * RADIANS_PER_DEGREE ; //LATITUDE_OUT
1089 outc[i].c[longitude] = dLongitude*RADIANS_PER_DEGREE ; //LONGITUDE_OUT
1090 }else{
1091 //version 3.2- works in degrees by default
1092 outc[i].c[latitude] = dLatitude;
1093 outc[i].c[longitude] = dLongitude;
1094 }
1095 }
1096}
1097#endif //GEOLIB
1098/* compileGeosystem - encode the return value such that srf->p[x] is...
1099 0: spatial reference frame (GEOSP_UTM, GEOSP_GC, GEOSP_GD);
1100 1: ellipsoid index (defaults to GEOSP_WE)
1101 2: UTM zone number, 1..60. INT_ID_UNDEFINED = not specified
1102 3: UTM: if "northing_first" TRUE, if "easting_first", FALSE
1103 4: UTM: if "S" - value is FALSE, not S, value is TRUE
1104 5: GD: if "latitude_first" TRUE, if "longitude_first", FALSE
1105 6: GD: true if geoid height
1106 7: GD: TRUE: decimal degrees, FALSE radians
1107*/
1108static void gdToUtm3d(Geosys *geoSystem, double *gdcoords, double *xtmcoords);
1109static void gdTo3tm3d(Geosys *geoSystem, double *gdcoords, double *xtmcoords);
1110static void Utm_Gd3d(Geosys *geoSystem, struct SFVec3d *inc, int n, struct SFVec3d *outc) {
1111 double semimajor, flattening;
1112 getEllipsoidParams(geoSystem->ellipsoid,&semimajor,&flattening);
1113 #ifdef GEOLIB
1114 if(method_geolib())
1115 Xtm_Gd3d_geolib(geoSystem, inc, n, outc, semimajor, flattening, UTM_SCALE, UTM_FALSE_EASTING, UTM_FALSE_NORTHING, UTM_ZONE_SIZE);
1116 else
1117 #endif //GEOLIB
1118 Xtm_Gd3d(geoSystem, inc, n, outc, semimajor, flattening, UTM_SCALE, UTM_FALSE_EASTING, UTM_FALSE_NORTHING, UTM_ZONE_SIZE);
1119 if(0){
1120 //round trip verification, want to convert a UTM -> GD -> (UTM, 3TM)
1121 double xtm[3], neh[3];
1122 int izone = -1;
1123 printf("in UTM_gd3d\n");
1124 printf("UTM y %lf x %lf h %lf zone %d\n",inc->c[0],inc->c[1],inc->c[2],geoSystem->xtm_zone);
1125 gdToUtm3d(geoSystem,outc->c, xtm);
1126 veccopyd(neh,xtm);
1127 if(!geoSystem->xtm_northing_first) vecswizzle2d(neh);
1128 printf("UTM y %lf x %lf h %lf zone %d\n",neh[0],neh[1],neh[2],geoSystem->xtm_zone);
1129 izone = -1;
1130 gdTo3tm3d(geoSystem,outc->c, xtm);
1131 veccopyd(neh,xtm);
1132 if(!geoSystem->xtm_northing_first) vecswizzle2d(neh);
1133 printf("3TM y %lf x %lf h %lf zone %d\n",neh[0],neh[1],neh[2],geoSystem->xtm_zone);
1134 }
1135}
1136static void U3tm_Gd3d(Geosys *geoSystem, struct SFVec3d *inc, int n, struct SFVec3d *outc) {
1137 double semimajor, flattening;
1138 getEllipsoidParams(geoSystem->ellipsoid,&semimajor,&flattening);
1139 #ifdef GEOLIB
1140 if(method_geolib())
1141 Xtm_Gd3d_geolib(geoSystem, inc, n, outc, semimajor, flattening, U3TM_SCALE, U3TM_FALSE_EASTING, U3TM_FALSE_NORTHING, U3TM_ZONE_SIZE);
1142 else
1143 #endif //GEOLIB
1144 Xtm_Gd3d(geoSystem, inc, n, outc, semimajor, flattening, U3TM_SCALE, U3TM_FALSE_EASTING, U3TM_FALSE_NORTHING, U3TM_ZONE_SIZE);
1145
1146}
1147double getTerrainHeight(int planetID, Geosys *geoSystem, struct SFVec3d *gdCoord);
1148double userHeight2ellipsoidHeight(Geosys *geoSystem, struct SFVec3d *gdCoord){
1149 double additionalHeight = 0.0;
1150 if(geoSystem->geoid_height == TRUE){
1151 //MSL (mean sea level) to ellipsoid height
1152 // ellipsoidHeight = MSL + geoid(location)
1153 // so gd are in ellipsoid heights like GPS? (vs sea level heights)
1154 double gddegrees[3];
1155 if(geoSystem->gd_degrees) veccopyd(gddegrees,gdCoord->c);
1156 else vecscaled(gddegrees,gdCoord->c,DEGREES_PER_RADIAN);
1157 if(!geoSystem->gd_latitude_first) vecswizzle2d(gddegrees);
1158 additionalHeight += geoidCorrection(gddegrees[0],gddegrees[1]);
1159 }
1160 if(geoSystem->relativeHeight == TRUE){
1161 // ellipsoidHeight = height + TerrainHeight(planet,location)
1162 struct Planet *planet = current_planet();
1163 additionalHeight += getTerrainHeight( planet->ID, geoSystem, gdCoord);
1164 }
1165 return additionalHeight;
1166}
1167
1168/* take a set of coords, and a geoSystem, and create a set of moved coords */
1169/* we keep around the GD coords because we need them for rotation calculations */
1170/* parameters:
1171 geoSystem: compiled geoSystem integer array pointer
1172 inCoords: coordinate structure for input coordinates, ANY coordinate type
1173 outCoords: area for GC coordinates. Will MALLOC size if required
1174 gdCoords: GD coordinates, used for rotation calculations in later stages. WILL MALLOC THIS */
1175
1176
1177static void moveCoords3d (Geosys * geoSystem, struct SFVec3d *offset, struct SFVec4d *yup,
1178 struct SFVec3d *inCoords, int n, struct SFVec3d *outCoords, struct SFVec3d *gdCoords) {
1179 // offset is in GC
1180 int i;
1181
1182 /* GD Geosystem - copy coordinates, and convert them to GC */
1183 switch (geoSystem->spatial_system) {
1184 case GEOSP_GD:
1185 {
1186 /* just copy the coordinates for the GD temporary return */
1187 memcpy (gdCoords, inCoords, sizeof (struct SFVec3d) * n);
1188 if(geoSystem->geoid_height || geoSystem->relativeHeight)
1189 for(i=0; i < n; i++)
1190 gdCoords[i].c[2] += userHeight2ellipsoidHeight(geoSystem,&gdCoords[i]);
1191
1192 /* GD_Gd_Gc_convert (inCoords, outCoords); */
1193 Gd_Gc3d(geoSystem,gdCoords,n,outCoords);
1194
1195 }
1196 break;
1197 case GEOSP_GC:
1198 /* an earth-fixed geocentric coord; no conversion required for gc value returns */
1199 for (i=0; i< n; i++) {
1200 veccopyd(outCoords[i].c,inCoords[i].c);
1201 /* convert this coord from GC to GD, using ellipsoid in geoSystem[1] */
1202 gccToGdc (geoSystem, &inCoords[i], &gdCoords[i]);
1203 }
1204
1205 break;
1206 case GEOSP_UTM:
1207 {
1208 /* GD coords will be returned from the conversion process....*/
1209 /* first, convert UTM to GC, then GD, then GD to GC */
1210 /* see the compileGeosystem function for geoSystem fields */
1211 Utm_Gd3d(geoSystem,inCoords,n, gdCoords);
1212 if(geoSystem->geoid_height || geoSystem->relativeHeight)
1213 for(i=0; i < n; i++)
1214 gdCoords[i].c[2] += userHeight2ellipsoidHeight(geoSystem,&gdCoords[i]);
1215 Gd_Gc3d(geoSystem,gdCoords,n,outCoords);
1216 }
1217 break;
1218 case GEOSP_3TM:
1219 {
1220 /* GD coords will be returned from the conversion process....*/
1221 /* first, convert UTM to GC, then GD, then GD to GC */
1222 /* see the compileGeosystem function for geoSystem fields */
1223 U3tm_Gd3d(geoSystem,inCoords,n, gdCoords);
1224 if(geoSystem->geoid_height || geoSystem->relativeHeight)
1225 for(i=0; i < n; i++)
1226 gdCoords[i].c[2] += userHeight2ellipsoidHeight(geoSystem,&gdCoords[i]);
1227 Gd_Gc3d(geoSystem,gdCoords,n,outCoords);
1228 }
1229 break;
1230
1231 default :
1232 printf ("incorrect geoSystem field, %s\n",stringGEOSPATIALType(geoSystem->spatial_system));
1233 return;
1234
1235 }
1236 if(offset || yup){
1237 if(offset)
1238 for(i=0;i<n;i++){
1239 //take offset off GC coords
1240 vecdifd(outCoords[i].c,outCoords[i].c,offset->c);
1241 }
1242 if(yup){
1243 Quaternion qup;
1244 vrmlrot_to_quaternion(&qup,yup->c[0],yup->c[1],yup->c[2],-yup->c[3]);
1245 for(i=0;i<n;i++){
1246 //take offset off GC coords
1247 quaternion_rotationd(outCoords[i].c,&qup,outCoords[i].c);
1248 }
1249 }
1250 }
1251}
1252
1253
1254static void initializeGeospatial (struct X3D_GeoOrigin **nodeptr) {
1255 //MF_SF_TEMPS
1256 int specversion;
1257 struct X3D_GeoOrigin *node = NULL;
1258
1259 #ifdef VERBOSE
1260 printf ("\ninitializing GeoSpatial code nodeptr %u\n",*nodeptr);
1261 #endif
1262
1263 if (*nodeptr != NULL) {
1264 if (X3D_GEOORIGIN(*nodeptr)->_nodeType != NODE_GeoOrigin) {
1265 printf ("expected a GeoOrigin node, but got a node of type %s\n",
1266 stringNodeType(X3D_GEOORIGIN(*nodeptr)->_nodeType));
1267 *nodeptr = NULL;
1268 return;
1269 } else {
1270 /* printf ("um, just setting geoorign to %u\n",(*nodeptr)); */
1271 node = X3D_GEOORIGIN(*nodeptr);
1272 }
1273 specversion = X3D_PROTO(node->_executionContext)->__specversion;
1274 /* printf ("initGeoSpatial ich %d ch %d\n",node->_ichange, node->_change); */
1275
1276 if NODE_NEEDS_COMPILING {
1277 //struct SFVec3d gdCoords;
1278 //struct SFVec3d offset;
1279 compile_geoSystem (X3D_NODE(node),node->_nodeType, &node->geoSystem, &node->__geoSystem);
1280 //INIT_MF_FROM_SF(node,geoCoords)
1281 moveCoords3d(GEOSYS(node->__geoSystem), NULL, NULL,
1282 &node->geoCoords,1, &node->__movedCoords, &node->__movedgd);
1283 //COPY_MF_TO_SF(node, __movedCoords)
1284 node->rotateYUp = FALSE; //Mar2018 rotateYup isn't working properly for us, get a wild angle
1285 //... H: we already do it with autoOrient H: we do it better with autoOrient H: we did it wrong all along
1286 if(node->rotateYUp == TRUE)
1287 {
1288 struct SFVec4d orient;
1289 int i;
1290 Quaternion qz,qx,qr;
1291 double dangle;
1292 Geosys *gs;
1293
1294 dangle = node->__movedgd.c[1];
1295 gs = GEOSYS(node->__geoSystem);
1296 //if(node->__geoSystem->.p[7] == TRUE)
1297 if(gs->gd_degrees)
1298 dangle *= RADIANS_PER_DEGREE;
1299 dangle += RADIANS_PER_DEGREE*90.0;
1300 vrmlrot_to_quaternion (&qz,0.0, 0.0, 1.0, dangle);
1301
1302 #ifdef VERBOSE
1303 printf ("GeoOrient qz angle (deg) %lf angle (rad) %lf quat: %lf %lf %lf %lf\n",dangle*DEGREES_PER_RADIAN,
1304 dangle,qz.x, qz.y, qz.z,qz.w);
1305 #endif
1306
1307 dangle = node->__movedgd.c[0];
1308 if(gs->gd_degrees == TRUE)
1309 dangle *= RADIANS_PER_DEGREE;
1310 dangle = RADIANS_PER_DEGREE*180.0 - dangle;
1311 vrmlrot_to_quaternion (&qx,1.0, 0.0, 0.0,dangle);
1312
1313 #ifdef VERBOSE
1314 printf ("GeoOrient qx angle (deg) %lf angle (rad) %lf quat: %lf %lf %lf %lf\n",
1315 dangle*DEGREES_PER_RADIAN, dangle, qx.x, qx.y, qx.z,qx.w);
1316 #endif
1317
1318 //quaternion_add (&qr, &qx, &qz);
1319 quaternion_multiply(&qr,&qz,&qx);
1320
1321 #ifdef VERBOSE
1322 printf ("GeoOrient qr %lf %lf %lf %lf\n",qr.x, qr.y, qr.z,qr.w);
1323 #endif
1324
1325 quaternion_to_vrmlrot(&qr, &orient.c[0], &orient.c[1], &orient.c[2], &orient.c[3]);
1326 for(i=0;i<4;i++)
1327 node->__rotyup.c[i] = orient.c[i];
1328 }else{
1329 int i;
1330 for(i=0;i<4;i++)
1331 node->__rotyup.c[i] = 0.0;
1332 node->__rotyup.c[1] = 1.0;
1333 }
1334
1335 #ifdef VERBOSE
1336 printf ("initializeGeospatial, __movedCoords %lf %lf %lf, ryup %d, geoSystem %d %d %d %d\n",
1337 node->__movedCoords.c[0],
1338 node->__movedCoords.c[1],
1339 node->__movedCoords.c[2],
1340 node->rotateYUp,
1341 node->__geoSystem.p[0],
1342 node->__geoSystem.p[1],
1343 node->__geoSystem.p[2],
1344 node->__geoSystem.p[3]);
1345 node->__geoSystem.p[4]);
1346 node->__geoSystem.p[5]);
1347 node->__geoSystem.p[6]);
1348 node->__geoSystem.p[7]);
1349 printf ("initializeGeospatial, done\n\n");
1350 #endif
1351
1352 //FREE_MF_SF_TEMPS
1353 MARK_NODE_COMPILED
1354 }
1355 }
1356}
1357
1358/* for converting from GC to GD */
1359struct gcgd {
1360double A, F, C, A2, C2, Eps2, Eps21, Eps25, C254, C2DA, CEE,
1361 CE2, CEEps2, TwoCEE, tem, ARat1, ARat2, BRat1, BRat2, B1,B2,B3,B4,B5;
1362};
1363
1364/* for converting BACK to GD from GC */
1365struct gcgd* initializeGcToGdParams(int type, double A, double F) {
1366 struct gcgd *g;
1367 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
1368 if(type < 0) type = -type + GEOELLIPSOID_COUNT;
1369 if(p->gcgdpars[type]) return p->gcgdpars[type];
1370 g = malloc(sizeof(struct gcgd));
1371 p->gcgdpars[type] = g;
1372 /* Create the ERM constants. */
1373 g->A = A;
1374 g->A2 = A * A;
1375 g->F =F;
1376 g->C =(A) * (1-g->F);
1377 g->C2 = g->C * g->C;
1378 g->Eps2 =(g->F) * (2.0-g->F);
1379 g->Eps21 =g->Eps2 - 1.0;
1380 g->Eps25 =.25 * (g->Eps2);
1381 g->C254 =54.0 * g->C2;
1382
1383 g->C2DA = g->C2 / A;
1384 g->CE2 = g->A2 - g->C2;
1385 g->tem = g->CE2 / g->C2;
1386 g->CEE = g->Eps2 * g->Eps2;
1387 g->TwoCEE =2.0 * g->CEE;
1388 g->CEEps2 =g->Eps2 * g->CE2;
1389
1390 /* UPPER BOUNDS ON POINT */
1391
1392
1393 g->ARat1 =pow((A + 50005.0),2);
1394 g->ARat2 =(g->ARat1) / pow((g->C+50005.0),2);
1395
1396 /* LOWER BOUNDS ON POINT */
1397
1398 g->BRat1 =pow((A-10005.0),2);
1399 g->BRat2 =(g->BRat1) / pow((g->C-10005.0),2);
1400
1401 /* use WE ellipsoid */
1402 g->B1=0.100225438677758E+01;
1403 g->B2=-0.393246903633930E-04;
1404 g->B3=0.241216653453483E+12;
1405 g->B4=0.133733602228679E+14;
1406 g->B5=0.984537701867943E+00;
1407 return g;
1408}
1409
1410/* convert BACK to a GD coordinate, from GC coordinates using ellipsoid */
1411static void gccToGdc_fw (Geosys *geoSystem, struct SFVec3d *gcc, struct SFVec3d *gdc) {
1412 int latitude = 0;
1413 int longitude = 1;
1414 int elevation = 2;
1415 double GCC_X, GCC_Y, GCC_Z;
1416 double A,F;
1417 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,
1418 s,roe,arg,v,zo;
1419 struct gcgd *g;
1420 #ifdef VERBOSE
1421 printf ("gccToGdc input %lf %lf %lf\n",GCC_X, GCC_Y, GCC_Z);
1422 #endif
1423 GCC_X = gcc->c[0];
1424 GCC_Y = gcc->c[1];
1425 GCC_Z = gcc->c[2];
1426 if(geoSystem->gd_latitude_first == FALSE){
1427 latitude = 1; longitude = 0;
1428 }
1429 getEllipsoidParams(geoSystem->ellipsoid,&A,&F);
1430 g = initializeGcToGdParams(geoSystem->ellipsoid,A,F);
1431
1432 w2=GCC_X * GCC_X + GCC_Y * GCC_Y;
1433 w=sqrt(w2);
1434 z2=GCC_Z * GCC_Z;
1435
1436 testu=w2 + g->ARat2 * z2;
1437 testb=w2 + g->BRat2 * z2;
1438
1439 if ((testb > g->BRat1) && (testu < g->ARat1))
1440 {
1441
1442 /*POINT IS BETWEEN-10 KIL AND 50 KIL, SO COMPUTE TANGENT LATITUDE */
1443
1444 top= GCC_Z * (g->B1 + (g->B2 * w2 + g->B3) /
1445 (g->B4 + w2 * g->B5 + z2));
1446
1447 top2=top*top;
1448
1449 rr=top2+w2;
1450
1451 q=sqrt(rr);
1452
1453 /* ****************************************************************
1454
1455 COMPUTE H IN LINE SQUARE ROOT OF 1-EPS2*SIN*SIN. USE SHORT BINOMIAL
1456 EXPANSION PLUS ONE ITERATION OF NEWTON'S METHOD FOR SQUARE ROOTS.
1457 */
1458
1459 s12=top2/rr;
1460
1461 rnn = g->A / ( (.25 - g->Eps25*s12 + .9999944354799/4) + (.25-g->Eps25*s12)/(.25 - g->Eps25*s12 + .9999944354799/4));
1462 s1=top/q;
1463
1464 /******************************************************************/
1465
1466 /* TEST FOR H NEAR POLE. if SIN(¯)**2 <= SIN(45.)**2 THEN NOT NEAR A POLE.*/
1467
1468 if (s12 < .50)
1469 gdc->c[elevation] = q-rnn;
1470 else
1471 gdc->c[elevation] = GCC_Z / s1 + (g->Eps21 * rnn);
1472 gdc->c[latitude] = atan(top / w);
1473 gdc->c[longitude] = atan2(GCC_Y,GCC_X);
1474 }
1475 /* POINT ABOVE 50 KILOMETERS OR BELOW -10 KILOMETERS */
1476 else /* Do Exact Solution ************ */
1477 {
1478 wp2=GCC_X * GCC_X + GCC_Y * GCC_Y;
1479 zp2=GCC_Z * GCC_Z;
1480 wp=sqrt(wp2);
1481 cf=g->C254 * zp2;
1482 gee=wp2 - (g->Eps21 * zp2) - g->CEEps2;
1483 alpha=cf / (gee*gee);
1484 cl=g->CEE * wp2 * alpha / gee;
1485 arg2=cl * (cl + 2.0);
1486 s1=1.0 + cl + sqrt(arg2);
1487 s=pow(s1,(1.0/3.0));
1488 p=alpha / (3.0 * pow(( s + (1.0/s) + 1.0),2));
1489 xarg= 1.0 + (g->TwoCEE * p);
1490 q=sqrt(xarg);
1491 r2= -p * (2.0 * (1.0 - g->Eps2) * zp2 / ( q * ( 1.0 + q) ) + wp2);
1492 r1=(1.0 + (1.0 / q));
1493 r2 /=g->A2;
1494
1495 /* DUE TO PRECISION ERRORS THE ARGUMENT MAY BECOME NEGATIVE IF SO SET THE ARGUMENT TO ZERO.*/
1496
1497 if (r1+r2 > 0.0)
1498 ro = g->A * sqrt( .50 * (r1+r2));
1499 else
1500 ro=0.0;
1501
1502 ro=ro - p * g->Eps2 * wp / ( 1.0 + q);
1503 //arg0 = pow(( wp - Eps2 * ro),2) + zp2;
1504 roe = g->Eps2 * ro;
1505 arg = pow(( wp - roe),2) + zp2;
1506 v=sqrt(arg - g->Eps2 * zp2);
1507 zo=g->C2DA * GCC_Z / v;
1508 gdc->c[elevation] = sqrt(arg) * (1.0 - g->C2DA / v);
1509 top=GCC_Z+ g->tem*zo;
1510 gdc->c[latitude] = atan( top / wp );
1511 gdc->c[longitude] =atan2(GCC_Y,GCC_X);
1512 } /* end of Exact solution */
1513
1514 if(geoSystem->gd_degrees == TRUE){
1515 //v3.2- works in degrees by default, v3.3+ works in 'angle base units' (radians) by default
1516 gdc->c[latitude] *= DEGREES_PER_RADIAN;
1517 gdc->c[longitude] *= DEGREES_PER_RADIAN;
1518 }
1519#undef VERBOSE
1520
1521}
1522#ifdef GEOLIB
1523static void gccToGdc_geolib (Geosys *geoSystem, struct SFVec3d *gcc, struct SFVec3d *gdc){
1524 int geotype;
1525 double gd[3],gc[3], semimajor,flattening;
1526 //printf("hi from gccToGdc_geolib\n");
1527 getEllipsoidParams(geoSystem->ellipsoid,&semimajor,&flattening);
1528 if(FALSE && flattening == 0.0){
1529 //easy spherical coords, although geolib doesn't need help, just for testing here
1530 double radius, horizontal_radius;
1531 veccopyd(gc,gcc->c);
1532 //printf("gc2gd gc %lf %lf %lf\n",gc[0],gc[1],gc[2]);
1533 radius = veclengthd(gc);
1534 horizontal_radius = veclength2d(gc);
1535 gd[0] = atan2(gc[2],horizontal_radius);
1536 gd[1] = atan2(gc[1],gc[0]);
1537 gd[2] = radius - semimajor;
1538 //printf("radius %lf semimajor %lf\n",radius,semimajor);
1539 if(!geoSystem->gd_latitude_first) vecswizzle2d(gd);
1540 if(geoSystem->gd_degrees) vecscale2d(gd,gd,DEGREES_PER_RADIAN);
1541 //printf("gc2gd sphere gd %lf %lf %lf\n",gd[0],gd[1],gd[2]);
1542 veccopyd(gdc->c,gd);
1543 }
1544 else
1545 {
1546 geotype = geoSystem->ellipsoid;
1547 if(geotype < 0) geotype = -geotype + GEOELLIPSOID_COUNT;
1548 if(!fwgeo_gc[geotype]){
1549 fwgeo_gc[geotype] = fgeo_initializeGC(semimajor,flattening);
1550 }
1551 veccopyd(gc,gcc->c);
1552 // function(semimajor,flattening,gc[0],gc[1],gc[2],&gd[0],&gd[1],&gd[2]);
1553 fgeo_gc2gd(fwgeo_gc[geotype],gc[0],gc[1],gc[2], &gd[0],&gd[1],&gd[2]);
1554 if(!geoSystem->gd_latitude_first) vecswizzle2d(gd);
1555 if(!geoSystem->gd_degrees) vecscale2d(gd,gd,RADIANS_PER_DEGREE);
1556 veccopyd(gdc->c,gd);
1557 //printf("gc2gd geolb gd %lf %lf %lf\n",gd[0],gd[1],gd[2]);
1558 }
1559
1560}
1561#endif //GEOLIB
1562static void gccToGdc (Geosys *geoSystem, struct SFVec3d *gcc, struct SFVec3d *gdc){
1563#ifdef GEOLIB
1564 if(method_geolib()){
1565 gccToGdc_geolib(geoSystem,gcc,gdc);
1566 //vecprint3db("gl gdc ",gdc->c,"\n");
1567 }else
1568#endif //GEOLIB
1569 {
1570 double semimajor, flattening;
1571 getEllipsoidParams(geoSystem->ellipsoid,&semimajor,&flattening);
1572 if(flattening == 0.0){
1573 //easy spherical coords
1574 //gccToGdc_fw and/or its gd2gc complement has a problem with moon geoSystem 'R173...' 'F0.0'
1575 double radius, horizontal_radius, gd[3], gc[3];
1576 veccopyd(gc,gcc->c);
1577 //printf("gc2gd gc %lf %lf %lf\n",gc[0],gc[1],gc[2]);
1578 radius = veclengthd(gc);
1579 horizontal_radius = veclength2d(gc);
1580 gd[0] = atan2(gc[2],horizontal_radius);
1581 gd[1] = atan2(gc[1],gc[0]);
1582 gd[2] = radius - semimajor;
1583 //printf("radius %lf semimajor %lf\n",radius,semimajor);
1584 if(!geoSystem->gd_latitude_first) vecswizzle2d(gd);
1585 if(geoSystem->gd_degrees) vecscale2d(gd,gd,DEGREES_PER_RADIAN);
1586 //printf("gc2gd sphere gd %lf %lf %lf\n",gd[0],gd[1],gd[2]);
1587 veccopyd(gdc->c,gd);
1588 }
1589 else
1590 {
1591 gccToGdc_fw(geoSystem,gcc,gdc);
1592 //vecprint3db("fw gdc ",gdc->c,"\n");
1593 }
1594 }
1595}
1596/* convert a GDC BACK to a UTM coordinate ASSUMES LAT LON RADIANS*/
1597static void gdToXtm(double radius, double flattening, double latitude, double longitude, double scaleFactor,
1598 double falseEasting, double falseNorthing, double zoneSize, int *zone, double *easting, double *northing)
1599{
1600#define DEG2RAD (PI/180.00)
1601//#define GEOSP_WE_INV 0.00669438
1602 double lat_radian;
1603 double long_radian;
1604 double myScale;
1605 double longOrigin;
1606 double longOriginradian, dlon;
1607 double eccentprime, e2, A, F;
1608 double NNN;
1609 double TTT;
1610 double CCC;
1611 double AAA;
1612 double MMM;
1613
1614 A = radius;
1615 F = flattening;
1616 //e2 = 2.0*F - F*F;
1617 e2 = F*(2. - F);
1618
1619 /* calculate the zone number if it is less than zero. If greater than zero, leave alone! */
1620 dlon = longitude * DEGREES_PER_RADIAN;
1621 if (*zone < 0)
1622 *zone = (int) (((dlon + 180.0)/zoneSize) + 1);
1623
1624 lat_radian = latitude;
1625 long_radian = longitude;
1626 myScale = scaleFactor; //0.9996;
1627 longOrigin = (*zone - 1)*zoneSize - 180.0 + zoneSize/2.0; //3;
1628 longOriginradian = longOrigin * DEG2RAD;
1629 eccentprime = e2/(1.0-e2);
1630
1631 /*
1632 printf ("lat_radian %lf long_radian %lf myScale %lf longOrigin %d longOriginradian %lf eccentprime %lf\n",
1633 lat_radian, long_radian, myScale, longOrigin, longOriginradian, eccentprime);
1634 */
1635 //http://www.engr.usask.ca/classes/CE/316/notes/CE%20316%20CH%204C%2031-1-12%20-INSTRUCTOR.pdf
1636
1637 NNN = A / sqrt(1.0-e2 * sin(lat_radian)*sin(lat_radian));
1638 TTT = tan(lat_radian) * tan(lat_radian);
1639 CCC = eccentprime * cos(lat_radian)*cos(lat_radian);
1640 AAA = cos(lat_radian) * (long_radian - longOriginradian);
1641 MMM = A
1642 * ( ( 1. - e2/4 - 3. * e2 * e2/64
1643 - 5.0 * e2 * e2 * e2/256.0
1644 ) * lat_radian
1645 - ( 3. * e2/8 + 3. * e2 * e2/32.
1646 + 45. * e2 * e2 * e2/1024.
1647 ) * sin(2. * lat_radian)
1648 + ( 15. * e2 * e2/256. +
1649 45. * e2 * e2 * e2/1024.
1650 ) * sin(4. * lat_radian)
1651 - ( 35. * e2 * e2 * e2/3072.
1652 ) * sin(6. * lat_radian)
1653 );
1654
1655 /* printf ("N %lf T %lf C %lf A %lf M %lf\n",NNN,TTT,CCC,AAA,MMM); */
1656
1657 *easting = myScale*NNN*(AAA+(1-TTT+CCC)*AAA*AAA*AAA/6
1658 + (5.-18.*TTT+TTT*TTT+72.*CCC-58.*eccentprime)*AAA*AAA*AAA*AAA*AAA/120.)
1659 + falseEasting; //500000.0;
1660
1661 *northing= myScale * ( MMM + NNN*tan(lat_radian) *
1662 ( 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.));
1663
1664 if (latitude < 0) *northing += falseNorthing; //10000000.0;*/
1665
1666 #ifdef VERBOSE
1667 printf ("gdToUtm: lat %lf long %lf zone %d -> easting %lf northing %lf\n",latitude, longitude, *zone,*easting, *northing);
1668 #endif
1669}
1670
1671#ifdef GEOLIB
1672//assumes LAT, LON in radians
1673static void gdToXtm_geolib(int geotype, double radius, double flattening, double latitude, double longitude, double scaleFactor,
1674 double falseEasting, double falseNorthing, double zoneSize, int *zone, double *easting, double *northing)
1675{
1676 double F, dlon0, dlat, dlon;
1677 void *fgeo;
1678 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
1679
1680 F = flattening;
1681 if(!p->fgeopars[geotype])
1682 p->fgeopars[geotype] = fgeo_initializeTM(radius, F, 1.0);
1683 fgeo = p->fgeopars[geotype];
1684
1685 /* calculate the zone number if it is less than zero. If greater than zero, leave alone! */
1686 //Q. is longitude in degrees, or does that depend on UNITS, specversion and strict33?
1687 dlon = longitude * DEGREES_PER_RADIAN;
1688 if (*zone < 0)
1689 *zone = (int) (((dlon + 180.0)/zoneSize) + 1);
1690
1691 dlon0 = (*zone -1) * zoneSize - 180. + zoneSize*.5;
1692 dlat = latitude * DEGREES_PER_RADIAN;
1693
1694 fgeo_gd2tm(fgeo,dlat,dlon,dlon0,easting, northing);
1695 *easting *= scaleFactor;
1696 *northing *= scaleFactor;
1697
1698 if (latitude < 0.0) *northing += falseNorthing; //10000000.0;
1699 *easting += falseEasting;
1700
1701 #ifdef VERBOSE
1702 printf ("gdToUtm: lat %lf long %lf zone %d -> easting %lf northing %lf\n",latitude, longitude, *zone,*easting, *northing);
1703 #endif
1704}
1705#endif //GEOLIB
1706
1707/* compileGeosystem - encode the return value such that srf->p[x] is...
1708 0: spatial reference frame (GEOSP_UTM, GEOSP_GC, GEOSP_GD);
1709 1: ellipsoid index (defaults to GEOSP_WE)
1710 2: UTM zone number, 1..60. INT_ID_UNDEFINED = not specified
1711 3: UTM: if "northing_first" TRUE, if "easting_first", FALSE
1712 4: UTM: if "S" - value is FALSE, not S, value is TRUE
1713 5: GD: if "latitude_first" TRUE, if "longitude_first", FALSE
1714 6: GD: true if geoid height
1715 7: GD: TRUE: decimal degrees, FALSE radians
1716*/
1717
1718static void gdToUtm3d(Geosys *geoSystem, double *gdcoords, double *xtmcoords) {
1719 double semimajor, flattening;
1720 double gdradians[3];
1721 int geotype, northing_first, latitude_first, is_degrees, *zone;
1722
1723 geotype = geoSystem->ellipsoid; //ellipsoid index
1724 northing_first = geoSystem->xtm_northing_first;
1725 latitude_first = geoSystem->gd_latitude_first;
1726 is_degrees = geoSystem->gd_degrees;
1727
1728 if(is_degrees) vecscaled(gdradians,gdcoords,RADIANS_PER_DEGREE);
1729 else veccopyd(gdradians,gdcoords);
1730 if(!latitude_first) vecswizzle2d(gdradians); //unswizzle if swizzled
1731
1732 getEllipsoidParams(geotype,&semimajor,&flattening);
1733 zone = &geoSystem->xtm_zone;
1734#ifdef GEOLIB
1735 if(method_geolib())
1736 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]);
1737 else
1738#endif //GEOLIB
1739 gdToXtm(semimajor,flattening, gdradians[0],gdradians[1], UTM_SCALE, UTM_FALSE_EASTING, UTM_FALSE_NORTHING, UTM_ZONE_SIZE, zone, &xtmcoords[1], &xtmcoords[0]);
1740
1741 if(!northing_first) vecswizzle2d(xtmcoords);
1742 xtmcoords[2] = gdcoords[2];
1743}
1744static void gdTo3tm3d(Geosys *geoSystem, double *gdcoords, double *xtmcoords) {
1745 double semimajor, flattening;
1746 double gdradians[3];
1747 int geotype, northing_first, latitude_first, is_degrees, *zone;
1748
1749 geotype = geoSystem->ellipsoid; //ellipsoid index
1750 northing_first = geoSystem->xtm_northing_first;
1751 latitude_first = geoSystem->gd_latitude_first;
1752 is_degrees = geoSystem->gd_degrees;
1753
1754 if(is_degrees) vecscaled(gdradians,gdcoords,RADIANS_PER_DEGREE);
1755 else veccopyd(gdradians,gdcoords);
1756 if(!latitude_first) vecswizzle2d(gdradians);
1757
1758 getEllipsoidParams(geotype,&semimajor,&flattening);
1759 zone = &geoSystem->xtm_zone;
1760#ifdef GEOLIB
1761 if(method_geolib())
1762 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]);
1763 else
1764#endif //GEOLIB
1765 gdToXtm(semimajor, flattening, gdradians[0],gdradians[1], U3TM_SCALE, U3TM_FALSE_EASTING, U3TM_FALSE_NORTHING, U3TM_ZONE_SIZE, zone, &xtmcoords[1], &xtmcoords[0]);
1766
1767 if(!northing_first) vecswizzle2d(xtmcoords);
1768 xtmcoords[2] = gdcoords[2];
1769}
1770
1771/* calculate the rotation needed to apply to this position on the GC coordinate location
1772 a) rotate from equatorial plane GC X,Y to TCS (topocentric coordinate system) plane
1773 b) rotate from Z up to Y up
1774*/
1775static void GeoOrient (struct X3D_Node *geoOrigin, Geosys *geoSystem, struct SFVec3d *gdCoords, struct SFVec4d *orient) {
1776 Quaternion qx;
1777 Quaternion qz;
1778 Quaternion qr;
1779 double dangle, gdcoords[3];
1780
1781 orient->c[0] = 0.0;
1782 orient->c[1] = 1.0;
1783 orient->c[2] = 0.0;
1784 orient->c[3] = 0.0;
1785 /* is this a straight GC geoSystem? If so, we do not do any orientation */
1786 if (geoSystem != NULL) {
1787 if (geoSystem->spatial_system == GEOSP_GC) {
1788 #ifdef VERBOSE
1789 printf ("GeoOrient - simple GC, so no orient\n");
1790 #endif
1791 return;
1792 }
1793 }
1794 if(geoOrigin)
1795 {
1796 if(((struct X3D_GeoOrigin*)geoOrigin)->rotateYUp == TRUE) return;
1797 }
1798
1799 #ifdef VERBOSE
1800 printf ("GeoOrient - gdCoords->c[0,1] is %f %f\n",gdCoords->c[0],gdCoords->c[1]);
1801 #endif
1802
1803 /* initialize qx and qz */
1804 veccopyd(gdcoords,gdCoords->c);
1805 if(!geoSystem->gd_latitude_first) vecswizzle2d(gdcoords);
1806 dangle = gdcoords[1]; //longitude
1807 if(geoSystem->gd_degrees == TRUE)
1808 dangle *= RADIANS_PER_DEGREE;
1809 dangle += RADIANS_PER_DEGREE*90.0;
1810 vrmlrot_to_quaternion (&qz,0.0, 0.0, 1.0, dangle);
1811
1812 #ifdef VERBOSE
1813 printf ("GeoOrient qz angle (deg) %lf angle (rad) %lf quat: %lf %lf %lf %lf\n",((double)90.0 + gdCoords->c[1]),
1814 RADIANS_PER_DEGREE*((double)90.0 + gdCoords->c[1]),qz.x, qz.y, qz.z,qz.w);
1815 #endif
1816
1817 dangle = gdcoords[0]; //latitude
1818 if(geoSystem->gd_degrees == TRUE)
1819 dangle *= RADIANS_PER_DEGREE;
1820 dangle = RADIANS_PER_DEGREE*180.0 - dangle;
1821 vrmlrot_to_quaternion (&qx,1.0, 0.0, 0.0, dangle);
1822
1823 #ifdef VERBOSE
1824 printf ("GeoOrient qx angle (deg) %lf angle (rad) %lf quat: %lf %lf %lf %lf\n",
1825 ((double)180.0 - gdCoords->c[0]), RADIANS_PER_DEGREE*((double)180.0 - gdCoords->c[0]), qx.x, qx.y, qx.z,qx.w);
1826 #endif
1827
1828 //quaternion_add (&qr, &qx, &qz);
1829 //quaternion_print(&qr,"added\n");
1830 quaternion_multiply(&qr, &qz, &qx);
1831 //quaternion_print(&qr,"multiplied\n");
1832
1833 #ifdef VERBOSE
1834 printf ("GeoOrient qr %lf %lf %lf %lf\n",qr.x, qr.y, qr.z,qr.w);
1835 #endif
1836
1837 quaternion_to_vrmlrot(&qr, &orient->c[0], &orient->c[1], &orient->c[2], &orient->c[3]);
1838 vecnormald(orient->c,orient->c);
1839 #ifdef VERBOSE
1840 printf ("GeoOrient rotation %lf %lf %lf %lf\n",orient->c[0], orient->c[1], orient->c[2], orient->c[3]);
1841 #endif
1842}
1843
1844/* compileGeosystem - encode the return value such that srf->p[x] is...
1845 0: spatial reference frame (GEOSP_UTM, GEOSP_GC, GEOSP_GD);
1846 1: ellipsoid index (defaults to GEOSP_WE)
1847 2: UTM zone number, 1..60. INT_ID_UNDEFINED = not specified
1848 3: UTM: if "northing_first" TRUE, if "easting_first", FALSE
1849 4: UTM: if "S" - value is FALSE, not S, value is TRUE
1850 5: GD: if "latitude_first" TRUE, if "longitude_first", FALSE
1851 6: GD: true if geoid height
1852 7: GD: TRUE: decimal degrees, FALSE radians
1853*/
1855 char *c;
1856 int i;
1857};
1858char * stringint_int2string(struct stringint *table, int itype){
1859 int i = 0;
1860 while(table[i].c){
1861 if(table[i].i == itype) return table[i].c;
1862 i++;
1863 }
1864 return NULL;
1865}
1866int stringint_string2int(struct stringint *table, const char *ctype){
1867 int i = 0;
1868 while(table[i].c){
1869 if(!strcmp(table[i].c,ctype)) return table[i].i;
1870 i++;
1871 }
1872 return -1;
1873}
1874struct stringint lookup_ellipsoids [] = {
1875 {"AA",GEOEL_AA},
1876 {"AM",GEOEL_AM},
1877 {"AN",GEOEL_AN},
1878 {"BN",GEOEL_BN},
1879 {"BR",GEOEL_BR},
1880 {"CC",GEOEL_CC},
1881 {"CD",GEOEL_CD},
1882 {"EA",GEOEL_EA},
1883 {"EB",GEOEL_EB},
1884 {"EC",GEOEL_EC},
1885 {"ED",GEOEL_ED},
1886 {"EE",GEOEL_EE},
1887 {"EF",GEOEL_EF},
1888 {"FA",GEOEL_FA},
1889 {"HE",GEOEL_HE},
1890 {"HO",GEOEL_HO},
1891 {"ID",GEOEL_ID},
1892 {"IN",GEOEL_IN},
1893 {"KA",GEOEL_KA},
1894 {"RF",GEOEL_RF},
1895 {"SA",GEOEL_SA},
1896 {"WD",GEOEL_WD},
1897 {"WE",GEOEL_WE},
1898 {NULL,-1},
1899};
1900struct stringint lookup_spatialreferencesys [] = {
1901 {"GC",GEOSP_GC},
1902 {"GD",GEOSP_GD},
1903 {"UTM",GEOSP_UTM},
1904 {"3TM",GEOSP_3TM},
1905 {NULL,-1},
1906};
1907
1908
1909void compile_geoSystem (struct X3D_Node *node, int nodeType, struct Multi_String *args, struct X3D_Node **nodegeosys) {
1910 int i, specversion, nextra;
1911 indexT this_srf = INT_ID_UNDEFINED;
1912 indexT this_srf_ind = INT_ID_UNDEFINED;
1913 struct ellipsoid ee;
1914 Geosys *srf = GEOSYS(*nodegeosys);
1915
1916 #ifdef VERBOSE
1917 printf ("start of compile_geoSystem\n");
1918 #endif
1919
1920 /* malloc the area required for internal settings, if required */
1921 if (srf==NULL) {
1922 srf = malloc(sizeof(Geosys));
1923 register_node_gc(node,(void*)srf);
1924 *nodegeosys = X3D_NODE(srf);
1925 }
1926
1927 /* set these as defaults */
1928 srf->spatial_system = GEOSP_GD;
1929 srf->ellipsoid = GEOEL_WE;
1930 srf->xtm_zone = INT_ID_UNDEFINED;
1931 srf->xtm_northing_first = TRUE; //XTM: northing first
1932 srf->utm_northern_hemisphere = TRUE; //northern hemisphere for UTM
1933 srf->gd_latitude_first = TRUE; //GD: lat first
1934 srf->geoid_height = FALSE; //geoid - not GC, just GD/UTM
1935 specversion = X3D_PROTO(node->_executionContext)->__specversion;
1936 if(specversion > 320 && STRICT33){
1937 //version 3.3+ by default in 'angle base units' which are radians
1938 srf->gd_degrees = FALSE; //GD: TRUE decimal degrees, FALSE: radians
1939 }else{
1940 //version 3.2- by default in degrees
1941 srf->gd_degrees = TRUE; //GD: TRUE decimal degrees, FALSE: radians
1942 }
1943 srf->relativeHeight = FALSE; //relative height flag, not set below, its set during specific node compile
1944 /* if nothing specified, we just use these defaults */
1945 if (args->n==0) return;
1946
1947 //2018 we allow the user to specify ellipsoid (A and (B or IF (inverse flattening) or F (flattening)) or R radius
1948 nextra = FALSE;
1949 ee.a = ee.b = ee.f = 0.0;
1950
1951 /* first go through, and find the Spatial Reference Frame, GD, UTM, or GC */
1952 for (i=0; i<args->n; i++) {
1953 int itype = stringint_string2int(lookup_spatialreferencesys,args->p[i]->strptr);
1954 if(itype > -1){
1955 this_srf = itype;
1956 this_srf_ind = i;
1957 }
1958 }
1959
1960 /* did we find a GC, GD, or UTM? */
1961 if (this_srf == INT_ID_UNDEFINED) {
1962 ConsoleMessage ("geoSystem in node %s, must have GC, GD or UTM",stringNodeType(nodeType));
1963 return;
1964 }
1965
1966 srf->spatial_system = (int) this_srf;
1967 /* go through and ensure that we have the correct parameters for this spatial reference frame */
1968 if (this_srf == GEOSP_GC) {
1969 //srf->p[1] = INT_ID_UNDEFINED;
1970 //nothing to do
1971 } else if (this_srf == GEOSP_GD || this_srf == GEOSP_3TM || this_srf == GEOSP_UTM) {
1972 for (i=0; i<args->n; i++) {
1973 if (i != this_srf_ind) {
1974 int iellipse;
1975 char *str = args->p[i]->strptr;
1976 /* printf ("geosp_gd, ind %d i am %d string %s\n",i, this_srf_ind,args->p[i]->strptr); */
1977 iellipse = stringint_string2int(lookup_ellipsoids,str);
1978 if(iellipse > -1){
1979 srf->ellipsoid = iellipse;
1980 }else{
1981 //GD specifics
1982 if (strcmp("latitude_first", str) == 0) {
1983 srf->gd_latitude_first = TRUE;
1984 } else if (strcmp("longitude_first", str) == 0) {
1985 srf->gd_latitude_first = FALSE;
1986 } else if(strcmp ("WGS84",str) == 0){
1987 srf->geoid_height = TRUE; //geoid
1988 } else
1989 //ellipsoid parameters specified
1990 if(str[0] == 'R') {
1991 //radius
1992 double radius;
1993 sscanf(args->p[i]->strptr,"R%lf",&radius);
1994 nextra = TRUE;
1995 ee.a = radius;
1996 } else if (str[0] == 'A') {
1997 //radius
1998 double a;
1999 sscanf(str,"A%lf",&a);
2000 nextra = TRUE;
2001 ee.a = a;
2002 } else if (str[0] == 'B') {
2003 //radius
2004 double b;
2005 sscanf(str,"B%lf",&b);
2006 nextra = TRUE;
2007 ee.b = b;
2008 } else if (!strncmp(str,"IF",2)) {
2009 //radius
2010 double invf;
2011 sscanf(str,"IF%lf",&invf);
2012 nextra = TRUE;
2013 ee.f = 1.0/invf;
2014 } else if (str[0] == 'F') {
2015 //radius
2016 double f;
2017 sscanf(str,"F%lf",&f);
2018 nextra = TRUE;
2019 ee.f = f;
2020 } else
2021 //XTM
2022 if (strcmp ("S",str) == 0) {
2023 srf->utm_northern_hemisphere = FALSE;
2024 } else if (strcmp ("N",str) == 0) {
2025 srf->utm_northern_hemisphere = TRUE; // default
2026 } else if (str[0] == 'Z') {
2027 int zone = -1;
2028 sscanf(str,"Z%d",&zone);
2029 /* printf ("zone found as %d\n",zone); */
2030 srf->xtm_zone = zone;
2031 } else if (strcmp("northing_first",str) == 0) {
2032 srf->xtm_northing_first = TRUE;
2033 } else if (strcmp("easting_first",str) == 0) {
2034 srf->xtm_northing_first = FALSE;
2035 } else
2036 //UNHANDLED
2037 {
2038 ConsoleMessage("geoSystem parameter %s not handled, in node %s",str,stringNodeType(nodeType));
2039 }
2040 }
2041 }
2042 }
2043 }
2044
2045 if(nextra){
2046 int ifound;
2047 if(ee.f == 0.0){
2048 //compute ellipsoid inverse flattening if not given
2049 double a,b,invf;
2050 a = extra_ellipsoid[nextra_ellipsoid].a;
2051 b = extra_ellipsoid[nextra_ellipsoid].b;
2052 ee.f = 1.0;
2053 if(ee.a != 0.0 && ee.b != 0.0){
2054 ee.f = (ee.a-ee.b)/ee.a;
2055 }else if(ee.a != 0.0){
2056 //likely radius, in which case (a-b) == 0
2057 ee.f = 0.0;
2058 }
2059
2060 }
2061 //see if we already have this ellipsoid, if not add it, else use it
2062 // (in case we have hundreds of nodes with the same user-defined ellipsoid)
2063 ifound = nextra_ellipsoid;
2064 for(i=1;i<nextra_ellipsoid;i++){
2065 if(extra_ellipsoid[i].a == ee.a && extra_ellipsoid[i].f == ee.f){
2066 ifound = i;
2067 break;
2068 }
2069 }
2070 extra_ellipsoid[ifound] = ee;
2071 srf->ellipsoid = -ifound; //negative sentinal value for extra ellipsoids
2072 if(ifound == nextra_ellipsoid)
2073 nextra_ellipsoid++;
2074 }
2075
2076 #ifdef VERBOSE
2077 printf ("printf done compileGeoSystem\n");
2078 #endif
2079
2080}
2081//ever get tired of those long parameter lists?
2082//how about wrapping up the call parameters in a struct
2083// and passing (a pointer to) the struct?
2084//especially to 'demacroize' while keeping generallized across related nodes
2085//we don't have the concept of an 'interface' -cluster of related fields-
2086//and in general we can't rely on fields being in a consistent order or offset from node start.
2087
2088//TRANSFORMING FROM GEOSPATIAL TO SHARED LOCAL aka LCS LOCAL COORDINATE SYSTEM
2089// terminology:
2090// geocentric GC - center of molten core of eath is 0,0,0
2091// geospatial aligned GCA - X through Grenwich, Z through north pole
2092// node local (NL): relative to node's own 'origin' ie position, geoGridOrigin, etc
2093// node local aligned (NLA): Y 'up', -Z toward north pole
2094// shared local (SL): relative to a single shared origin for all geo nodes for a planet
2095// shared local aligned (SLA): relative to 'up' and 'north' at the shared origin
2096// root node, root node aligned RNRNA - the regular scene 0,0,0 at the root level, and alignemnt
2097// SLSLA aka LCS could be designed to be co-incident with and aligned with RNRNA
2098// procedure:
2099// A. convert to GC
2100// 1. convert node 'origin' from XTM -> GD -> GC
2101// 2. convert any geometry in the node from XTM -> GD -> GC
2102// B. convert to NLNLA
2103// 3. subtract node origin GC from geometry GC to get node-local geocentric-aligned NLGCA
2104// 4. compute LocalOrientation - the rotation to apply to NLGCA to get NLNLA node local aligned = LocalOrient
2105// C. convert to SLSLA
2106// 5. compute tilt to get node geometry from NLNLA to NLSLA shared local aligned = offsetOrient
2107// 6. compute offset to get NLSLA to shared local SLSLA
2108// summary order of transforms:
2109// GC2NL
2110// GCA2NLA - H: this depends how the node is defined.
2111// NLA2SLA
2112// NL2SL
2113/* moved to Component_Geospatial.h
2114//void user2gd(Geosys * geoSystem, struct SFVec3d *geo, int n, struct SFVec3d *gd);
2115//void gd2user(Geosys * geoSystem, struct SFVec3d *gd, int n, struct SFVec3d *geo);
2116void user2gc(Geosys * geoSystem, struct SFVec3d *geo, int n, struct SFVec3d *gc);
2117void gc2user(Geosys * geoSystem, struct SFVec3d *gc, int n, struct SFVec3d *geo);
2118void gc2lcs(Geosys * geoSystem, struct SFVec3d *gc, int n, struct SFVec3d *lcs);
2119void lcs2gc(Geosys * geoSystem, struct SFVec3d *lcs, int n, struct SFVec3d *gc);
2120void gd2gc(Geosys * geoSystem, struct SFVec3d *gd, int n, struct SFVec3d *gc);
2121void gc2gd(Geosys * geoSystem, struct SFVec3d *gc, int n, struct SFVec3d *gd);
2122void gc2tcs(Geosys * geoSystem, struct SFVec3d *gdcenter, struct SFVec3d *gc, int n, struct SFVec3d *tcs);
2123void tcs2gc(Geosys * geoSystem, struct SFVec3d *gdcenter, struct SFVec3d *tcs, int n, struct SFVec3d *gc);
2124void lcs2gc_transform(struct SFVec4d *rotation, struct SFVec3d *translation);
2125void gc2lcs_transform(struct SFVec3d *translate, struct SFVec4d *rotate);
2126void gc2tcs_transform(Geosys * geoSystem, struct SFVec3d *gdcenter, struct SFVec3d *translate, struct SFVec4d *rotate);
2127void tcs2gc_transform(Geosys * geoSystem, struct SFVec3d *gdcenter, struct SFVec4d *rotate, struct SFVec3d *translate);
2128void geoprep(Geosys *geoSystem, struct SFVec3d *userCoord);
2129void geofin(Geosys *geoSystem, struct SFVec3d *userCoord);
2130void geoprepT(Geosys *geoSystem, struct SFVec3d *userCoord);
2131void geofinT(Geosys *geoSystem, struct SFVec3d *userCoord);
2132*/
2133
2134void update_origin(Geosys *geoSystem, struct X3D_Node *node, struct SFVec3d *userCoord, struct X3D_GeoOrigin *geoOrigin)
2135{
2136 // assumes __geoSystem is already compiled.
2137 // version < 3.3 - will try and use geoOrigin
2138 // version 3.3+ - ignors geoOrigin and uses FCFS (first (node) come first served) shared origin for a planet
2139 struct Planet *planet;
2140 int specversion;
2141 specversion = X3D_PROTO(node->_executionContext)->__specversion;
2142
2143 planet = current_planet();
2144 if(!planet->autoOriginSet){
2145 if(geoOrigin && specversion < 330 ){
2146 //geoOrgin is deprecated and tolerated in 3.0 - 3.2, but not tolerated in 3.3+
2147 //to simplify, we are using FCFS on a single geoOrigin.
2148 struct SFVec3d offset, *poffset;
2149 struct SFVec4d yup, *pyup;
2150 pyup = NULL;
2151 poffset = NULL;
2152 double *cc;
2153 initializeGeospatial(&geoOrigin);
2154 veccopyd(planet->autoOrigin.c,geoOrigin->__movedCoords.c);
2155 GeoOrient(X3D_NODE(geoOrigin), GEOSYS(geoOrigin->__geoSystem), &geoOrigin->__movedgd, &planet->autoOrient);
2156 planet->autoOriginSet = TRUE;
2157 }else{
2158 struct SFVec3d gdCoord;
2159 user2gc(geoSystem,userCoord,1,&planet->autoOrigin);
2160 gc2gd(geoSystem,&planet->autoOrigin,1,&gdCoord);
2161 GeoOrient(X3D_NODE(geoOrigin), geoSystem, &gdCoord, &planet->autoOrient);
2162 planet->autoOriginSet = TRUE;
2163 }
2164 }
2165}
2166
2167/************************************************************************/
2168void compile_GeoCoordinate (struct X3D_GeoCoordinate * node) {
2169 int i;
2170 struct SFVec3d gcCoord, lcsCoord;
2171 Geosys *gs;
2172 COMPILE_GEOSYSTEM(node)
2173 gs = GEOSYS(node->__geoSystem);
2174 FREE_IF_NZ(node->__movedCoords.p);
2175 node->__movedCoords.p = MALLOC (struct SFVec3f *, sizeof (struct SFVec3f) *node->point.n);
2176
2177 for(i=0;i<node->point.n;i++){
2178 user2gc(gs,&node->point.p[i],1,&gcCoord);
2179 gc2lcs(gs,&gcCoord,1,&lcsCoord);
2180 double2float(node->__movedCoords.p[i].c,lcsCoord.c,3);
2181 }
2182 node->__movedCoords.n = node->point.n;
2183 MARK_NODE_COMPILED
2184
2185 /* events */
2186 /* MARK_SFNODE_INOUT_EVENT(node->metadata, node->__oldmetadata, offsetof (struct X3D_GeoCoordinate, metadata)) */
2187}
2188
2189
2190/************************************************************************/
2191/* GeoElevationGrid */
2192/************************************************************************/
2193
2194/* check validity of ElevationGrid fields */
2195int checkX3DGeoElevationGridFields (struct X3D_GeoElevationGrid *node, float **points, int *npoints) {
2196 MF_SF_TEMPS
2197 int i,j;
2198 int nx;
2199 double xSp;
2200 int nz;
2201 double zSp;
2202 double *height;
2203 int ntri;
2204 int nh;
2205 struct X3D_PolyRep *rep;
2206 float *newpoints;
2207 int nquads;
2208 int *cindexptr;
2209 float *texcoord = NULL;
2210 //double myHeightAboveEllip = 0.0;
2211 int mySRF = 0;
2212 Geosys *gs;
2213
2214 nx = node->xDimension;
2215 xSp = node->xSpacing;
2216 nz = node->zDimension;
2217 zSp = node->zSpacing;
2218 height = node->height.p;
2219 nh = node->height.n;
2220
2221 COMPILE_GEOSYSTEM(node)
2222 /* various values for converting to GD/UTM, etc */
2223 if (node->__geoSystem != NULL) {
2224 mySRF = GEOSYS(node->__geoSystem)->spatial_system;
2225 /* NOTE - DO NOT DO THIS CALCULATION - it is added in later
2226 myHeightAboveEllip = getEllipsoidRadius(node->__geoSystem.p[1]);
2227 */
2228 }
2229
2230 rep = node->_intern;
2231
2232 /* work out how many triangles/quads we will have */
2233 ntri = (nx && nz ? 2 * (nx-1) * (nz-1) : 0);
2234 nquads = ntri/2;
2235 //printf("nx %d nz %d nquads %d ntri %d\n",nx,nz,nquads,ntri);
2236 /* check validity of input fields */
2237 if(nh != nx * nz) {
2238 if (nh > nx * nz) {
2239 printf ("GeoElevationgrid: warning: x,y vs. height: %d * %d ne %d:\n", nx,nz,nh);
2240 } else {
2241 printf ("GeoElevationgrid: error: x,y vs. height: %d * %d ne %d:\n", nx,nz,nh);
2242 return FALSE;
2243 }
2244 }
2245
2246 /* do we have any triangles? */
2247 if ((nx < 2) || (nz < 2)) {
2248 printf ("GeoElevationGrid: xDimension and zDimension less than 2 %d %d\n", nx,nz);
2249 return FALSE;
2250 }
2251
2252 //printf ("checkX3DGeoElevationGrid - node->texCoord %p\n",node->texCoord);
2253
2254
2255 /* any texture coordinates passed in? if so, DO NOT generate any texture coords here. */
2256 if (!(node->texCoord)) {
2257 /* allocate memory for texture coords */
2258 FREE_IF_NZ(rep->GeneratedTexCoords[0]);
2259
2260 /* 6 vertices per quad each vertex has a 2-float tex coord mapping */
2261 texcoord = rep->GeneratedTexCoords[0] = MALLOC (float *, sizeof (float) * nquads * 12);
2262
2263 rep->tcindex=0; /* we will generate our own mapping */
2264 }
2265
2266 /* make up points array */
2267 /* a point is a vertex and consists of 3 floats (x,y,z) */
2268 newpoints = MALLOC (float *, sizeof (float) * nz * nx * 3);
2269
2270 FREE_IF_NZ(rep->actualCoord);
2271 rep->actualCoord = (float *)newpoints;
2272
2273 /* make up coord index */
2274 if (node->_coordIndex.n > 0) {FREE_IF_NZ(node->_coordIndex.p);}
2275 node->_coordIndex.p = MALLOC (int *, sizeof(int) * nquads * 5);
2276 cindexptr = node->_coordIndex.p;
2277
2278 node->_coordIndex.n = nquads * 5; //H: 4 points and -1 to end the face
2279 /* return the newpoints array to the caller */
2280 *points = newpoints;
2281 *npoints = node->_coordIndex.n;
2282
2283 #ifdef VERBOSE
2284 printf ("coordindex:\n");
2285 #endif
2286
2287 /* ElevationGrids go 1 - 2 - 3 - 4 we go 1 - 4 - 3 - 2 */
2288 //printf ("GeoElevationGrids, nz %d, nx %d\n",nz,nx);
2289
2290 for (j = 0; j < (nz -1); j++) {
2291 for (i=0; i < (nx-1) ; i++) {
2292 #ifdef VERBOSE
2293 printf (" %d %d %d %d %d\n", j*nx+i, j*nx+i+nx, j*nx+i+nx+1, j*nx+i+1, -1);
2294 #endif
2295
2296#ifdef WINDING_ELEVATIONGRID
2297 *cindexptr = j*nx+i; cindexptr++; /* 1 */
2298 *cindexptr = j*nx+i+nx; cindexptr++; /* 2 */
2299 *cindexptr = j*nx+i+nx+1; cindexptr++; /* 3 */
2300 *cindexptr = j*nx+i+1; cindexptr++; /* 4 */
2301 *cindexptr = -1; cindexptr++;
2302#else
2303 *cindexptr = j*nx+i; cindexptr++; /* 1 */
2304 *cindexptr = j*nx+i+1; cindexptr++; /* 4 */
2305 *cindexptr = j*nx+i+nx+1; cindexptr++; /* 3 */
2306 *cindexptr = j*nx+i+nx; cindexptr++; /* 2 */
2307 *cindexptr = -1; cindexptr++;
2308#endif
2309
2310 }
2311 }
2312
2313 /* tex coords These need to be streamed now; that means for each quad, each vertex needs its tex coords. */
2314 /* if the texCoord node exists, let render_TextureCoordinate (or whatever the node is) do our work for us */
2315 if (!(node->texCoord)) {
2316 //printf ("geoelevationgrid, doing %d x %d texture coords; tcoord %p\n",nz-1,nx-1,texcoord);
2317 for (j = 0; j < (nz -1); j++) {
2318 for (i=0; i < (nx-1) ; i++) {
2319 /* first triangle, 3 vertexes */
2320#ifdef WINDING_ELEVATIONGRID
2321 /* first tri */
2322/* 1 */ *texcoord = ((float) (i+0)/(nx-1)); texcoord++;
2323 *texcoord = ((float)(j+0)/(nz-1)); texcoord ++;
2324
2325/* 2 */ *texcoord = ((float) (i+0)/(nx-1)); texcoord++;
2326 *texcoord = ((float)(j+1)/(nz-1)); texcoord ++;
2327
2328/* 3 */ *texcoord = ((float) (i+1)/(nx-1)); texcoord++;
2329 *texcoord = ((float)(j+1)/(nz-1)); texcoord ++;
2330
2331 /* second tri */
2332/* 1 */ *texcoord = ((float) (i+0)/(nx-1)); texcoord++;
2333 *texcoord = ((float)(j+0)/(nz-1)); texcoord ++;
2334
2335/* 3 */ *texcoord = ((float) (i+1)/(nx-1)); texcoord++;
2336 *texcoord = ((float)(j+1)/(nz-1)); texcoord ++;
2337
2338/* 4 */ *texcoord = ((float) (i+1)/(nx-1)); texcoord++;
2339 *texcoord = ((float)(j+0)/(nz-1)); texcoord ++;
2340#else
2341 /* first tri */
2342 *texcoord = ((float) (i+0)/(nx-1)); texcoord++; /* 1 */
2343 *texcoord = ((float) (j+0)/(nz-1)); texcoord++;
2344
2345 *texcoord = ((float) (i+1)/(nx-1)); texcoord++; /* 4 */
2346 *texcoord = ((float) (j+0)/(nz-1)); texcoord++;
2347
2348 *texcoord = ((float) (i+1)/(nx-1)); texcoord++; /* 3 */
2349 *texcoord = ((float) (j+1)/(nz-1)); texcoord++;
2350
2351 /* second tri */
2352 *texcoord = ((float) (i+0)/(nx-1)); texcoord++; /* 1 */
2353 *texcoord = ((float) (j+0)/(nz-1)); texcoord++;
2354
2355 *texcoord = ((float) (i+1)/(nx-1)); texcoord++; /* 3 */
2356 *texcoord = ((float) (j+1)/(nz-1)); texcoord++;
2357
2358 *texcoord = ((float) (i+0)/(nx-1)); texcoord++; /* 2 */
2359 *texcoord = ((float) (j+1)/(nz-1)); texcoord++;
2360
2361#endif
2362 }
2363 }
2364 //for (i=0; i<10; i++) printf ("geoele tc %d is %f\n",i,rep->GeneratedTexCoords[i]);
2365 }
2366
2367 /* Render_Polyrep will use this number of triangles */
2368 rep->ntri = ntri;
2369
2370 /* initialize arrays used for passing values into/out of the MOVE_TO_ORIGIN(node) values */
2371 mIN.n = nx * nz;
2372 mIN.p = MALLOC (struct SFVec3d *, sizeof (struct SFVec3d) * mIN.n);
2373
2374 mOUT.n=0; mOUT.p = NULL;
2375 gdCoords.n=0; gdCoords.p = NULL;
2376 struct SFVec3d lastpoint, firstpoint;
2377 /* make up a series of points, then go and convert them to local coords */
2378 for (j=0; j<nz; j++) {
2379 for (i=0; i < nx; i++) {
2380 int k = i+(j*nx);
2381 #ifdef VERBOSE
2382 printf (" %lf %lf %lf # (hei ind %d) point [%d, %d]\n",
2383 xSp * i,
2384 height[i+(j*nx)] * ((double)node->yScale),
2385 zSp * j,
2386 i+(j*nx), i,j);
2387 #endif
2388
2389
2390 /* Make up a new vertex. Add the geoGridOrigin to every point */
2391
2392 if ((mySRF == GEOSP_GD) || (mySRF == GEOSP_UTM) || (mySRF == GEOSP_3TM)) {
2393 /* GD - give it to em in Latitude/Longitude/Elevation order */
2394 /* UTM- or give it to em in Northing/Easting/Elevation order */
2395 /* latitude - range of -90 to +90 */
2396 mIN.p[k].c[0] = zSp * j + node->geoGridOrigin.c[0];
2397
2398 /* longitude - range -180 to +180, or 0 to 360 */
2399 mIN.p[k].c[1] =xSp * i + node->geoGridOrigin.c[1];
2400
2401 /* elevation, above geoid */
2402 mIN.p[k].c[2] = (height[k] *(node->yScale)) + node->geoGridOrigin.c[2];
2403 veccopyd(lastpoint.c,mIN.p[k].c);
2404 if(i==0 && j==0) veccopyd(firstpoint.c,mIN.p[k].c);
2405 //+ myHeightAboveEllip;
2406 } else {
2407 /* nothing quite specified here - what do we really do??? */
2408 mIN.p[k].c[0] = zSp * j + node->geoGridOrigin.c[0];
2409
2410 mIN.p[k].c[1] =xSp * i + node->geoGridOrigin.c[1];
2411
2412 mIN.p[k].c[2] = (height[k] *(node->yScale)) + node->geoGridOrigin.c[2];
2413 //+ myHeightAboveEllip;
2414
2415 }
2416 /* printf ("height made up of %lf, geoGridOrigin %lf, myHeightAboveEllip %lf\n",(height[i+(j*nx)] *(node->yScale)),node->geoGridOrigin.c[2], myHeightAboveEllip); */
2417 }
2418 }
2419 #ifdef VERBOSE
2420 vecprint3db("firstpoint",firstpoint.c,"\n");
2421 vecprint3db(" lastpoint",lastpoint.c,"\n");
2422 vecprint3db("nx*nz",mIN.p[nx*nz -1].c,"\n");
2423 printf ("points before moving origin, lat, lon, height, index:\n");
2424 for (j=0; j<nz; j++) {
2425 for (i=0; i < nx; i++) {
2426 int k = i+(j*nx);
2427 printf (" %lf %lf %lf %d\n",mIN.p[k].c[0],
2428 mIN.p[k].c[1],mIN.p[k].c[2],k);
2429
2430 }
2431 printf("\n");
2432 }
2433 #endif
2434
2435 /* convert this point to a local coordinate */
2436 {
2437 Geosys *gs;
2438 compile_geoSystem(X3D_NODE(node),node->_nodeType,&node->geoSystem,&node->__geoSystem);
2439 gs = GEOSYS(node->__geoSystem);
2440
2441 update_origin(gs, X3D_NODE(node), &node->geoGridOrigin, X3D_GEOORIGIN(node->geoOrigin));
2442 mOUT.p = MALLOC(struct SFVec3d*,sizeof(struct SFVec3d)*mIN.n);
2443 mOUT.n = mIN.n;
2444 user2gc(gs,mIN.p,mIN.n,mOUT.p);
2445 #ifdef VERBOSE
2446 printf ("points in gc XYZ, index:\n");
2447 for (j=0; j<nz; j++) {
2448 for (i=0; i < nx; i++) {
2449 double ci[3], co[3];
2450 int k = i+(j*nx);
2451 veccopyd(co,mOUT.p[k].c);
2452 printf (" %lf %lf %lf %d\n",co[0],co[1],co[2],k);
2453 veccopyd(ci,mIN.p[k].c);
2454 printf (" %lf %lf %lf %d\n",ci[0],ci[1],ci[2],k);
2455
2456 }
2457 printf("\n");
2458 }
2459 #endif
2460
2461 gc2lcs(gs,mOUT.p,mOUT.n,mOUT.p);
2462
2463 }
2464
2465
2466 /* copy the resulting array back to the ElevationGrid */
2467
2468 for (j=0; j<nz; j++) {
2469 for (i=0; i < nx; i++) {
2470 /* copy this coordinate into our ElevationGrid array */
2471 int k = i+(j*nx);
2472 double2float(newpoints,mOUT.p[k].c,3);
2473 newpoints += 3;
2474 }
2475 }
2476 #ifdef VERBOSE
2477 printf ("points converted to mesh coords, xyz index:\n");
2478 newpoints = rep->actualCoord;
2479 for (j=0; j<nz; j++) {
2480 for (i=0; i < nx; i++) {
2481 /* copy this coordinate into our ElevationGrid array */
2482 int k = i+(j*nx);
2483 printf (" %f %f %f %d\n",newpoints[0],newpoints[1],newpoints[2],k);
2484 newpoints += 3;
2485 }
2486 printf("\n");
2487 }
2488 #endif //VERBOSE
2489 FREE_MF_SF_TEMPS;
2490 return TRUE;
2491}
2492
2493
2494/* a GeoElevationGrid creates a "real" elevationGrid node as a child for rendering. */
2495void compile_GeoElevationGrid (struct X3D_GeoElevationGrid * node) {
2496// 2018 not called, see compile stack in render_
2497}
2498int planetInPlanets(int planet, struct Multi_Int32 *planets){
2499 int i,ifound = -1;
2500 for(i=0;i<planets->n;i++)
2501 if(planets->p[i] == planet) ifound = i;
2502 return ifound > -1;
2503}
2504void RegisterGeoElevationGrid(struct X3D_Node *node, int planetID);
2505void render_GeoElevationGrid (struct X3D_GeoElevationGrid *node) {
2506 /*compile stack for geoElevationGrid:
2507 checkX3DGeoElelvationGridFields *see function above
2508 make_genericfaceset
2509 compile_polyrep
2510 compileNode
2511 render_GeoElevationGrid *you are here
2512 */
2513 //INITIALIZE_GEOSPATIAL(node)
2514 int planetID = 0;
2515 initializeGeospatial((struct X3D_GeoOrigin **) &node->geoOrigin);
2516 planetID = current_planetId();
2517 COMPILE_POLY_IF_REQUIRED (NULL, NULL, node->color, node->normal, node->texCoord)
2518 CULL_FACE(node->solid)
2519 render_polyrep(node);
2520 if(!planetInPlanets(planetID,&node->__planets)){
2521 //planetID default 0 for now
2522 // will be "P#" in geosystem, or <GeoPlanet ID="#"><GeoElevationGrid/></GeoPlanet>
2523 RegisterGeoElevationGrid(X3D_NODE(node), planetID);
2524 node->__planets.p = realloc(node->__planets.p,(node->__planets.n+1)*sizeof(int));
2525 node->__planets.p[node->__planets.n] = planetID;
2526 node->__planets.n++;
2527 }
2528}
2529
2530/************************************************************************/
2531/* GeoLocation */
2532/************************************************************************/
2533//double adjust_geoLocationRelativeHeight(struct X3D_GeoLocation *node,int planetID);
2534void compile_GeoLocation (struct X3D_GeoLocation * node) {
2535 // JAS int i;
2536 int specversion;
2537 struct SFVec3d gdCoord, gcCoord;
2538 struct SFVec4d locOrient;
2539 struct Planet *planet;
2540 Geosys *gs;
2541
2542 planet = current_planet();
2543 #ifdef VERBOSE
2544 printf ("compiling GeoLocation\n");
2545 #endif
2546 //step 1 compute origin
2547 compile_geoSystem(X3D_NODE(node),node->_nodeType,&node->geoSystem,&node->__geoSystem);
2548 gs = GEOSYS(node->__geoSystem);
2549 if(node->relativeHeight) gs->relativeHeight = TRUE; //handy for user2anything conversion function: don't need to pass node
2550
2551 update_origin(gs, X3D_NODE(node), &node->geoCoords, X3D_GEOORIGIN(node->geoOrigin));
2552
2553 /* did the geoCoords change?? */
2554 MARK_SFVEC3D_INOUT_EVENT(node->geoCoords, node->__oldgeoCoords, offsetof (struct X3D_GeoLocation, geoCoords))
2555
2556 /* how about the children field ?? */
2557 MARK_MFNODE_INOUT_EVENT(node->children, node->__oldChildren, offsetof (struct X3D_GeoLocation, children))
2558
2559 REINITIALIZE_SORTED_NODES_FIELD(node->children,node->_sortedChildren);
2560 MARK_NODE_COMPILED
2561
2562 /* events */
2563 /* MARK_SFNODE_INOUT_EVENT(node->metadata, node->__oldmetadata, offsetof (struct X3D_GeoLocation, metadata)) */
2564
2565 INITIALIZE_EXTENT;
2566
2567 #ifdef VERBOSE
2568 printf ("compiled GeoLocation\n\n");
2569 #endif
2570}
2571
2572void child_GeoLocation (struct X3D_GeoLocation *node) {
2573 CHILDREN_COUNT
2574 //LOCAL_LIGHT_SAVE
2575 //INITIALIZE_GEOSPATIAL(node)
2576 COMPILE_IF_REQUIRED
2577
2578 OCCLUSIONTEST
2579
2580
2581 /* {
2582 int x;
2583 struct X3D_Node *xx;
2584
2585 printf ("child_GeoLocation, this %d \n",node);
2586 for (x=0; x<nc; x++) {
2587 xx = X3D_NODE(node->children.p[x]);
2588 printf (" ch %d type %s dist %f\n",node->children.p[x],stringNodeType(xx->_nodeType),xx->_dist);
2589 }
2590 } */
2591
2592 /* Check to see if we have to check for collisions for this transform. */
2593
2594 RETURN_FROM_CHILD_IF_NOT_FOR_ME
2595
2596 /* do we have a local for a child? */
2597 //LOCAL_LIGHT_CHILDREN(node->children);
2598 prep_sibAffectors((struct X3D_Node*)node,&node->__sibAffectors);
2599
2600 /* now, just render the non-directionalLight children */
2601
2602 /* printf ("GeoLocation %d, flags %d, render_sensitive %d\n",
2603 node,node->_renderFlags,render_sensitive); */
2604
2605 #ifdef CHILDVERBOSE
2606 printf ("GeoLocation - doing normalChildren\n");
2607 #endif
2608
2609 normalChildren(node->children);
2610
2611 #ifdef CHILDVERBOSE
2612 printf ("GeoLocation - done normalChildren\n");
2613 #endif
2614
2615 //LOCAL_LIGHT_OFF
2616 fin_sibAffectors((struct X3D_Node*)node,&node->__sibAffectors);
2617
2618}
2619
2620/* do transforms, calculate the distance */
2621void prep_GeoLocation (struct X3D_GeoLocation *node) {
2622 //INITIALIZE_GEOSPATIAL(node)
2623 COMPILE_IF_REQUIRED
2624
2625 /* rendering the viewpoint means doing the inverse transformations in reverse order (while poping stack),
2626 * so we do nothing here in that case -ncoder */
2627
2628 /* printf ("prep_GeoLocation, render_hier vp %d geom %d light %d sens %d blend %d prox %d col %d\n",
2629 render_vp,render_geom,render_light,render_sensitive,render_blend,render_proximity,render_collision); */
2630
2631 /* do we have any geometry visible, and are we doing anything with geometry? */
2632 OCCLUSIONTEST
2633
2634 if(!renderstate()->render_vp) {
2635 geoprep(GEOSYS(node->__geoSystem),&node->geoCoords);
2636 /* did either we or the Viewpoint move since last time? */
2637 RECORD_DISTANCE
2638 if(renderstate()->render_boxes) extent6f_draw(node->_extent);
2639 }
2640}
2641void fin_GeoLocation (struct X3D_GeoLocation *node) {
2642 //INITIALIZE_GEOSPATIAL(node)
2643 COMPILE_IF_REQUIRED
2644 OCCLUSIONTEST
2645
2646 geofin(GEOSYS(node->__geoSystem),&node->geoCoords);
2647}
2648
2649/************************************************************************/
2650/* GeoLOD */
2651/************************************************************************/
2652void add_node_to_broto_context(struct X3D_Proto *currentContext,struct X3D_Node *node);
2653
2654void deleteMallocedFieldValue(int type,union anyVrml *fieldPtr);
2655void LOAD_CHILD(struct X3D_GeoLOD *node, struct X3D_Node **childNode, struct Multi_String *childUrl) {
2656 /* printf ("start of LOAD_CHILD, url has %d strings\n",node->childUrl.n); */
2657 int i;
2658 if (childUrl->n > 0) {
2659 /* create new inline node, link it in */
2660 if (*childNode == NULL) {
2661 *childNode = createNewX3DNode(NODE_Inline);
2662 if(node->_executionContext)
2663 add_node_to_broto_context(X3D_PROTO(node->_executionContext),X3D_NODE(*childNode));
2664 ADD_PARENT(X3D_NODE(*childNode), X3D_NODE(node));
2665 }
2666 /* copy over the URL from parent */
2667 deleteMallocedFieldValue(FIELDTYPE_MFString,(union anyVrml*)&X3D_INLINE(*childNode)->url);
2668 X3D_INLINE(*childNode)->url.p = MALLOC(struct Uni_String **, sizeof(struct Uni_String)*childUrl->n);
2669 for (i=0; i<childUrl->n; i++) {
2670 /* printf ("copying over url %s\n",node->childUrl.p[i]->strptr); */
2671 X3D_INLINE(*childNode)->url.p[i] = newASCIIString(childUrl->p[i]->strptr);
2672 }
2673 /* printf ("loading, and urlCount is %d\n",node->childUrl.n); */
2674 X3D_INLINE(*childNode)->url.n = childUrl->n;
2675 X3D_INLINE(*childNode)->load = TRUE;
2676 }
2677}
2678
2679#define UNLOAD_CHILD(childNode) \
2680 if (node->childNode != NULL) \
2681 X3D_INLINE(node->childNode)->load = FALSE;
2682
2683
2684static void GeoLODchildren (struct X3D_GeoLOD *node) {
2685 int load = node->__inRange;
2686
2687 /* lets see if we still have to load this one... */
2688 if (((node->__childloadstatus)==0) && (load)) {
2689 #ifdef VERBOSE
2690 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
2691
2692 printf ("GeoLODchildren - have to LOAD_CHILD for node %u (level %d)\n",node,p->geoLodLevel);
2693 #endif
2694
2695 LOAD_CHILD(node,&node->__child1Node,&node->child1Url);
2696 LOAD_CHILD(node,&node->__child2Node,&node->child2Url);
2697 LOAD_CHILD(node,&node->__child3Node,&node->child3Url);
2698 LOAD_CHILD(node,&node->__child4Node,&node->child4Url);
2699
2700 //LOAD_CHILD(__child1Node,child1Url)
2701 //LOAD_CHILD(__child2Node,child2Url)
2702 //LOAD_CHILD(__child3Node,child3Url)
2703 //LOAD_CHILD(__child4Node,child4Url)
2704 node->__childloadstatus = 1;
2705 }
2706}
2707//void GeoLODchildren1 (struct X3D_GeoLOD *node) {
2708// GeoLODchildren(node);
2709//}
2710static void GeoUnLODchildren (struct X3D_GeoLOD *node) {
2711 int load = node->__inRange;
2712
2713 if (!(load) && ((node->__childloadstatus) != 0)) {
2714 #ifdef VERBOSE
2715 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
2716 printf ("GeoLODloadChildren, removing children from node %u level %d\n",node,p->geoLodLevel);
2717 #endif
2718 UNLOAD_CHILD(__child1Node)
2719 UNLOAD_CHILD(__child2Node)
2720 UNLOAD_CHILD(__child3Node)
2721 UNLOAD_CHILD(__child4Node)
2722
2723 node->__childloadstatus = 0;
2724 }
2725}
2726
2727
2728static void GeoLODrootUrl (struct X3D_GeoLOD *node) {
2729 int load = node->__inRange == 0; //dug9 it's when you are out of range that you should get the rootnode
2730
2731 /* lets see if we still have to load this one... */
2732 if (((node->__rooturlloadstatus)==0) && (load)) {
2733 #ifdef VERBOSE
2734 printf ("GeoLODrootUrl - have to LOAD_CHILD for node %u\n",node);
2735 #endif
2736
2737 LOAD_CHILD(node,&node->__rootUrl, &node->rootUrl);
2738 //LOAD_CHILD(__rootUrl, rootUrl)
2739
2740 node->__rooturlloadstatus = 1;
2741 }
2742}
2743
2744
2745static void GeoUnLODrootUrl (struct X3D_GeoLOD *node) {
2746 int load = node->__inRange;
2747
2748 if (!(load) && ((node->__rooturlloadstatus) != 0)) {
2749 #ifdef VERBOSE
2750 printf ("GeoLODloadChildren, removing rootUrl\n");
2751 #endif
2752 node->__childloadstatus = 0;
2753 }
2754}
2755
2756
2757
2758void compile_GeoLOD (struct X3D_GeoLOD * node) {
2759 Geosys *gs;
2760 struct SFVec3d gcCoord;
2761 compile_geoSystem(X3D_NODE(node),node->_nodeType,&node->geoSystem,&node->__geoSystem);
2762 gs = GEOSYS(node->__geoSystem);
2763
2764 update_origin(gs, X3D_NODE(node), &node->center, X3D_GEOORIGIN(node->geoOrigin));
2765 user2gc(gs,&node->center,1,&gcCoord);
2766 gc2lcs(gs,&gcCoord,1,&node->__movedCoords);
2767 MARK_NODE_COMPILED
2768
2769}
2770#undef VERBOSE
2771
2772
2773void child_GeoLOD (struct X3D_GeoLOD *node) {
2774 int i;
2775 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
2776
2777 COMPILE_IF_REQUIRED
2778
2779 #ifdef VERBOSE
2780 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",
2781 node,
2782 p->geoLodLevel,
2783 node->_renderFlags,
2784 render_vp,render_geom,render_light,render_sensitive,render_blend,render_proximity,render_collision);
2785 #endif
2786 //ConsoleMessage("glod kids=%d\r",node->children.n);
2787 /* for debugging purposes... */
2788 if (node->__level == -1) node->__level = p->geoLodLevel;
2789 else if (node->__level != p->geoLodLevel) {
2790 printf ("hmmm - GeoLOD %p was level %d, now %d\n",node,node->__level, p->geoLodLevel);
2791 }
2792
2793 #ifdef VERBOSE
2794 if ( node->__inRange) {
2795 printf ("GeoLOD %u (level %d) closer\n",node,p->geoLodLevel);
2796 } else {
2797 printf ("GeoLOD %u (level %d) farther away\n",node,p->geoLodLevel);
2798 }
2799 #endif
2800
2801 /* if we are out of range, use the rootNode or rootUrl field */
2802 /* else, use the child1Url through the child4Url fields */
2803 if (!(node->__inRange)) {
2804 /* printf ("GeoLOD, node %u, doing rootNode, rootNode.n = %d\n",node,node->rootNode.n); */
2805 /* do we need to unload children that are no longer needed? */
2806 GeoUnLODchildren (node);
2807
2808 if (node->rootNode.n != 0) {
2809 for (i=0; i<node->rootNode.n; i++) {
2810 #ifdef VERBOSE
2811 printf ("GeoLOD %u is rendering rootNode %u",node,node->rootNode.p[i]);
2812 if (node->rootNode.p[i]!=NULL) printf (" (%s) ",stringNodeType(X3D_NODE(node->rootNode.p[i])->_nodeType));
2813 printf("\n");
2814 #endif
2815
2816 render_node (node->rootNode.p[i]);
2817 }
2818 } else if (node->rootUrl.n != 0) {
2819
2820 /* try and load the root from the rootUrl */
2821 GeoLODrootUrl (node);
2822
2823 /* render this rootUrl */
2824 if (node->__rootUrl != NULL) {
2825 #ifdef VERBOSE
2826 printf ("GeoLOD %u is rendering rootUrl %u",node,node->__rootUrl);
2827 if (node->__rootUrl != NULL) printf (" (%s) ", stringNodeType(X3D_NODE(node->__rootUrl)->_nodeType));
2828 printf ("\n");
2829 #endif
2830
2831 render_node (node->__rootUrl);
2832 }
2833
2834
2835 }
2836 } else {
2837 p->geoLodLevel++;
2838
2839 /* go through 4 kids */
2840 GeoLODchildren (node);
2841
2842 /* get rid of the rootUrl node, if it is loaded */
2843 GeoUnLODrootUrl (node);
2844
2845 #ifdef VERBOSE
2846 printf ("rendering children at %d, they are: ",p->geoLodLevel);
2847 if (node->child1Url.n>0) printf (" :%s: ",node->child1Url.p[0]->strptr);
2848 if (node->child2Url.n>0) printf (" :%s: ",node->child2Url.p[0]->strptr);
2849 if (node->child3Url.n>0) printf (" :%s: ",node->child3Url.p[0]->strptr);
2850 if (node->child4Url.n>0) printf (" :%s: ",node->child4Url.p[0]->strptr);
2851 printf ("\n");
2852 #endif
2853
2854 /* render these children */
2855 #ifdef VERBOSE
2856 printf ("GeoLOD %u is rendering children %u ", node, node->__child1Node);
2857 if (node->__child1Node != NULL) printf (" (%s) ",stringNodeType(X3D_NODE(node->__child1Node)->_nodeType));
2858 printf (" %u ", node->__child2Node);
2859 if (node->__child2Node != NULL) printf (" (%s) ",stringNodeType(X3D_NODE(node->__child2Node)->_nodeType));
2860 printf (" %u ", node->__child3Node);
2861 if (node->__child3Node != NULL) printf (" (%s) ",stringNodeType(X3D_NODE(node->__child3Node)->_nodeType));
2862 printf (" %u ", node->__child4Node);
2863 if (node->__child4Node != NULL) printf (" (%s) ",stringNodeType(X3D_NODE(node->__child4Node)->_nodeType));
2864 printf ("\n");
2865 #endif
2866
2867 if (node->__child1Node != NULL) render_node (node->__child1Node);
2868 if (node->__child2Node != NULL) render_node (node->__child2Node);
2869 if (node->__child3Node != NULL) render_node (node->__child3Node);
2870 if (node->__child4Node != NULL) render_node (node->__child4Node);
2871 p->geoLodLevel--;
2872
2873 }
2874}
2875
2876/************************************************************************/
2877/* GeoMetaData */
2878/************************************************************************/
2879
2880void compile_GeoMetadata (struct X3D_GeoMetadata * node) {
2881 #ifdef VERBOSE
2882 printf ("compiling GeoMetadata\n");
2883
2884 #endif
2885
2886 MARK_NODE_COMPILED
2887}
2888
2889/************************************************************************/
2890/* GeoOrigin */
2891/************************************************************************/
2892
2893void compile_GeoOrigin (struct X3D_GeoOrigin * node) {
2894 #ifdef VERBOSE
2895 printf ("compiling GeoOrigin\n");
2896 #endif
2897
2898 ConsoleMessage ("compiling GeoOrigin\n"); //this doesn't get called - see line 654 in initializeGeospatial()
2899 /* INITIALIZE_GEOSPATIAL */
2900 COMPILE_GEOSYSTEM(node)
2901 {
2902 int i;
2903 for(i=0;i<4;i++)
2904 node->__rotyup.c[i] = 0.0;
2905 node->__rotyup.c[1] = 1.0;
2906 }
2907 MARK_NODE_COMPILED
2908
2909 /* events */
2910 /* MARK_SFNODE_INOUT_EVENT(node->metadata, node->__oldmetadata, offsetof (struct X3D_GeoOrigin, metadata)) */
2911 MARK_SFVEC3D_INOUT_EVENT(node->geoCoords,node->__oldgeoCoords,offsetof (struct X3D_GeoOrigin, geoCoords))
2912 //dug9 may 2015 commented out __old.. see also geoViewpoint
2913 //MARK_MFSTRING_INOUT_EVENT(node->geoSystem,node->__oldMFString,offsetof (struct X3D_GeoOrigin, geoSystem))
2914}
2915
2916/************************************************************************/
2917/* GeoPositionInterpolator */
2918/************************************************************************/
2919
2920void compile_GeoPositionInterpolator (struct X3D_GeoPositionInterpolator * node) {
2921
2922 #ifdef VERBOSE
2923 printf ("compiling GeoPositionInterpolator\n");
2924 #endif
2925
2926 int i;
2927 Geosys *gs;
2928 struct SFVec3d gcCoord, lcsCoord;
2929 compile_geoSystem(X3D_NODE(node),node->_nodeType,&node->geoSystem,&node->__geoSystem);
2930 gs = GEOSYS(node->__geoSystem);
2931 FREE_IF_NZ(node->__movedValue.p);
2932 node->__movedValue.p = MALLOC(struct SFVec3f*,node->keyValue.n * sizeof(struct SFVec3f));
2933 node->__movedValue.n = node->keyValue.n;
2934 for(i=0;i<node->keyValue.n;i++){
2935 user2gc(gs,&node->keyValue.p[i],1,&gcCoord);
2936 gc2lcs(gs,&gcCoord,1,&lcsCoord);
2937 double2float(node->__movedValue.p[i].c,lcsCoord.c,3);
2938 }
2939 MARK_NODE_COMPILED
2940
2941 /* events */
2942 /* MARK_SFNODE_INOUT_EVENT(node->metadata, node->__oldmetadata, offsetof (struct X3D_GeoPositionInterpolator, metadata)) */
2943}
2944
2945/* PositionInterpolator, ColorInterpolator, GeoPositionInterpolator */
2946/* Called during the "events_processed" section of the event loop, */
2947/* so this is called ONLY when there is something required to do, thus */
2948/* there is no need to look at whether it is active or not */
2949
2950/* GeoPositionInterpolator == PositionIterpolator but with geovalue_changed and coordinate conversions */
2951// http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/geodata.html#GeoPositionInterpolator
2952// Mar 2018 interpretation: value_changed in LCS, geovalue_changed in geo coords
2953// I think we should either do 2 separate interpolations: 1) LCS 2) user geocoords
2954// or interpolate in geocoords and convert the resulting geovalue_changed to LCS for value_changed
2955void do_GeoPositionInterpolator (void *innode) {
2956 int specversion;
2957 struct X3D_GeoPositionInterpolator *node;
2958 int kin, kvin, counter, tmp;
2959 struct SFVec3d *kVs_user; //geo
2960 struct SFVec3f *kVs_lcs; //LCS
2961 /* struct SFColor *kVs */
2962
2963 if (!innode) return;
2964 node = (struct X3D_GeoPositionInterpolator *) innode;
2965
2966 if (NODE_NEEDS_COMPILING) compile_GeoPositionInterpolator(node);
2967 kvin = node->__movedValue.n;
2968 kVs_lcs = node->__movedValue.p;
2969 kVs_user = node->keyValue.p;
2970 kin = node->key.n;
2971 MARK_EVENT (innode, offsetof (struct X3D_GeoPositionInterpolator, value_changed));
2972 MARK_EVENT (innode, offsetof (struct X3D_GeoPositionInterpolator, geovalue_changed));
2973
2974 /* did the key or keyValue change? */
2975 if (node->__oldKeyValuePtr.p != node->keyValue.p) {
2976 MARK_EVENT (innode, offsetof (struct X3D_GeoPositionInterpolator, keyValue));
2977 node->__oldKeyValuePtr.p= node->keyValue.p;
2978 }
2979 if (node->__oldKeyPtr.p != node->key.p) {
2980 MARK_EVENT (innode, offsetof (struct X3D_GeoPositionInterpolator, key));
2981 node->__oldKeyPtr.p = node->key.p;
2982 }
2983
2984
2985 #ifdef SEVERBOSE
2986 printf("do_GeoPos: Position/Color interp, node %u kin %d kvin %d set_fraction %f\n",
2987 node, kin, kvin, node->set_fraction);
2988 #endif
2989
2990 /* make sure we have the keys and keyValues */
2991 if ((kvin == 0) || (kin == 0)) {
2992 node->value_changed.c[0] = (float) 0.0;
2993 node->value_changed.c[1] = (float) 0.0;
2994 node->value_changed.c[2] = (float) 0.0;
2995 node->geovalue_changed.c[0] = 0.0;
2996 node->geovalue_changed.c[1] = 0.0;
2997 node->geovalue_changed.c[2] = 0.0;
2998 return;
2999 }
3000
3001 if (kin>kvin) kin=kvin; /* means we don't use whole of keyValue, but... */
3002
3003 /* set_fraction less than or greater than keys */
3004 if (node->set_fraction <= ((node->key).p[0])) {
3005 veccopyd(node->geovalue_changed.c,kVs_user[0].c);
3006 veccopy3f(node->value_changed.c,kVs_lcs[0].c);
3007 } else if (node->set_fraction >= node->key.p[kin-1]) {
3008 memcpy ((void *)&node->geovalue_changed, (void *)&kVs_user[kvin-1], sizeof (struct SFVec3d));
3009 veccopyd(node->geovalue_changed.c,kVs_user[kvin-1].c);
3010 veccopy3f(node->value_changed.c,kVs_lcs[kvin-1].c);
3011 } else {
3012 /* have to go through and find the key before */
3013 float fpart, fdif[3];
3014 double dpart, ddif[3];
3015 counter = find_key(kin,((float)(node->set_fraction)),node->key.p);
3016
3017 //LCS to value_changed
3018 fpart = (node->set_fraction - node->key.p[counter-1]) /
3019 (node->key.p[counter] - node->key.p[counter-1]);
3020 veclerp3f(node->value_changed.c,kVs_lcs[counter-1].c,kVs_lcs[counter].c,fpart);
3021
3022 //geo to geovalue_changed
3023 dpart = fpart;
3024 veclerpd(node->geovalue_changed.c,kVs_user[counter-1].c,kVs_user[counter].c,dpart);
3025
3026 //for (tmp=0; tmp<3; tmp++) {
3027 // node->geovalue_changed.c[tmp] =
3028 // (node->set_fraction - node->key.p[counter-1]) /
3029 // (node->key.p[counter] - node->key.p[counter-1]) *
3030 // (kVs[counter].c[tmp] - kVs[counter-1].c[tmp]) + kVs[counter-1].c[tmp];
3031 //}
3032
3033 }
3034
3035 /* convert this back into the requested spatial format */
3036 //CONVERT_BACK_TO_GD_OR_UTM(node->geovalue_changed)
3037 //CONVERT_BACK_TO_GD_OR_UTMB(GEOSYS(node->__geoSystem), node->geoOrigin, &node->geovalue_changed);
3039 //for (tmp=0;tmp<3;tmp++) node->value_changed.c[tmp] = (float)node->geovalue_changed.c[tmp];
3040
3041 #ifdef SEVERBOSE
3042 printf ("Pos/Col, new value (%f %f %f)\n",
3043 node->value_changed.c[0],node->value_changed.c[1],node->value_changed.c[2]);
3044 printf ("geovalue_changed %lf %lf %lf\n",node->geovalue_changed.c[0], node->geovalue_changed.c[1], node->geovalue_changed.c[2]);
3045 #endif
3046}
3047
3048/************************************************************************/
3049/* GeoProximitySensor */
3050/************************************************************************/
3051
3052void compile_GeoProximitySensor (struct X3D_GeoProximitySensor * node) {
3053 int specversion;
3054
3055 #ifdef VERBOSE
3056 printf ("compiling GeoProximitySensor\n");
3057 #endif
3058 int i;
3059 Geosys *gs;
3060 struct SFVec3d gcCoord, lcsCoord, gdCoord;
3061 specversion = X3D_PROTO(node->_executionContext)->__specversion;
3062 if(specversion < 330){
3063 //in web3d version 3.3 they changed the name from .geoCenter to .center.
3064 //we'll copy here and use .center in other GPS functions
3065 if(!(veclengthd(node->geoCenter.c) == 0.0))
3066 veccopyd(node->center.c,node->geoCenter.c);
3067 }
3068
3069 compile_geoSystem(X3D_NODE(node),node->_nodeType,&node->geoSystem,&node->__geoSystem);
3070 gs = GEOSYS(node->__geoSystem);
3071 user2gc(gs,&node->center,1,&gcCoord);
3072 gc2lcs(gs,&gcCoord,1,&node->__movedCoords);
3073 gc2gd(gs,&gcCoord,1,&gdCoord);
3074 GeoOrient(node->geoOrigin, GEOSYS(node->__geoSystem), &gdCoord, &node->__localOrient);
3075 MARK_NODE_COMPILED
3076 MARK_SFVEC3D_INOUT_EVENT(node->geoCenter, node->__oldGeoCenter,offsetof (struct X3D_GeoProximitySensor, geoCenter))
3077 MARK_SFVEC3F_INOUT_EVENT(node->size, node->__oldSize,offsetof (struct X3D_GeoProximitySensor, size))
3078
3079 /* events */
3080 /* MARK_SFNODE_INOUT_EVENT(node->metadata, node->__oldmetadata, offsetof (struct X3D_GeoProximitySensor, metadata)) */
3081
3082
3083 #ifdef VERBOSE
3084 printf ("compiled GeoProximitySensor\n\n");
3085 #endif
3086}
3087
3088
3089void tcs2gcB_transform(Geosys *geoSystem, struct SFVec3d *gcCoord, struct SFVec3d* translate, struct SFVec4d *rotate)
3090{
3091 struct SFVec3d gdCoord;
3092 double A,F;
3093 Geosys *gs = geoSystem;
3094
3095 getEllipsoidParams(gs->ellipsoid, &A, &F);
3096 if(gs->spatial_system == GEOSP_GC && veclengthd(gcCoord->c) < A/2.0){
3097 vecset4d(rotate->c,0.0,1.0,0.0,0.0);
3098 veccopyd(translate->c,gcCoord->c);
3099 }else{
3100 gc2gd(gs,gcCoord,1, &gdCoord);
3101 tcs2gc_transform(gs,&gdCoord,rotate,translate);
3102 }
3103}
3104void gc2tcsB_transform(Geosys *geoSystem, struct SFVec3d *gcCoord, struct SFVec3d* translate, struct SFVec4d *rotate)
3105{
3106 struct SFVec3d gdCoord;
3107 double A,F;
3108 Geosys *gs = geoSystem;
3109
3110 getEllipsoidParams(gs->ellipsoid, &A, &F);
3111 if(gs->spatial_system == GEOSP_GC && veclengthd(gcCoord->c) < A/2.0){
3112 //..can I do a gc2tcs_transform that's equivalent?
3113 vecset4d(rotate->c,0.0,1.0,0.0,0.0);
3114 vecsetd(translate->c,-gcCoord->c[0],-gcCoord->c[1],-gcCoord->c[2]);
3115 }else{
3116 gc2gd(gs,gcCoord,1, &gdCoord);
3117 gc2tcs_transform(gs,&gdCoord,translate,rotate);
3118 }
3119}
3120
3121 //PROXIMITYSENSOR(GeoProximitySensor,__movedCoords,INITIALIZE_GEOSPATIAL(node),COMPILE_IF_REQUIRED)
3122//#define PROXIMITYSENSOR(type,center,initializer1,initializer2)
3123void geoprep0(Geosys *geoSystem, struct SFVec3d *userCoord){
3124
3125 //geonode TCS to transform stack LCS
3126 struct SFVec3d translation1, translation2, gcCoord, gdCoord;
3127 struct SFVec4d rotation1, rotation2;
3128 double A,F;
3129 Geosys *gs = geoSystem;
3130
3131 //How this works
3132 //the modelview matrix transforms the object -in this case proximitySensor bounding box-
3133 // into viewpoint space.
3134 //the geo object is aligned and sized in object TCS (topocentric coordinate system)
3135 //so we need to get from TCS for this node into LCS for the transform stack
3136 // the viewpoint code gets us from LCS into viewpoint space (aka TCS for viewpoint)
3137 // object-TCS > LCS -transform stack- LCS > TCS-vp
3138 // the stack goes in this order:
3139 // vp-TCS < LCS
3140 // LCS < TCS-object
3141 // here we do a 2-step TCS > LCS: TCS > GC, GC > LCS, which on the stack looks like
3142 // LCS < GC object push first
3143 // GC < TCS object push second
3144 // draw TCS object
3145 if(0){
3146 getEllipsoidParams(gs->ellipsoid, &A, &F);
3147 if(gs->spatial_system == GEOSP_GC && veclengthd(userCoord->c) < A/2.0){
3148 //likely DIS with local coordinates ie near molten core of earth
3149 // and around here the ellipsoid radius M is fiddly, so we thunk to simple GC aligned local
3150 FW_GL_TRANSLATE_D(userCoord->c[0],userCoord->c[1],userCoord->c[2]);
3151 }else{
3152 user2gc(gs,userCoord,1,&gcCoord);
3153
3154 gc2lcs_transform(&translation1,&rotation1);
3155 FW_GL_ROTATE_RADIANS(rotation1.c[3],rotation1.c[0],rotation1.c[1],rotation1.c[2]);
3156 FW_GL_TRANSLATE_D(translation1.c[0],translation1.c[1],translation1.c[2]);
3157 gc2gd(gs,&gcCoord,1, &gdCoord);
3158 tcs2gc_transform(gs,&gdCoord,&rotation2,&translation2);
3159 FW_GL_TRANSLATE_D(translation2.c[0],translation2.c[1],translation2.c[2]);
3160 FW_GL_ROTATE_RADIANS(rotation2.c[3],rotation2.c[0],rotation2.c[1],rotation2.c[2]);
3161 }
3162 }else{
3163 user2gc(gs,userCoord,1,&gcCoord);
3164 gc2lcs_transform(&translation1,&rotation1);
3165 FW_GL_ROTATE_RADIANS(rotation1.c[3],rotation1.c[0],rotation1.c[1],rotation1.c[2]);
3166 FW_GL_TRANSLATE_D(translation1.c[0],translation1.c[1],translation1.c[2]);
3167 tcs2gcB_transform(gs,&gcCoord,&translation2,&rotation2);
3168 FW_GL_TRANSLATE_D(translation2.c[0],translation2.c[1],translation2.c[2]);
3169 FW_GL_ROTATE_RADIANS(rotation2.c[3],rotation2.c[0],rotation2.c[1],rotation2.c[2]);
3170 }
3171}
3172void geoprep(Geosys *geoSystem, struct SFVec3d *userCoord){
3173 if(geoSystem){
3174 if(!renderstate()->render_vp) {
3175 FW_GL_PUSH_MATRIX();
3176 geoprep0(geoSystem,userCoord);
3177 }
3178 }
3179}
3180void geoprepT0(Geosys *geoSystem, struct SFVec3d *userCoord);
3181void geofin(Geosys *geoSystem, struct SFVec3d *userCoord){
3182 if(geoSystem){
3183 if(!renderstate()->render_vp) {
3184 FW_GL_POP_MATRIX();
3185 }else{
3186 geoprepT0(geoSystem,userCoord);
3187 }
3188 }
3189}
3190void render_GeoProximitySensor(struct X3D_GeoProximitySensor *node){
3191 //just for rendering the extent/bounding box
3192 if(renderstate()->render_boxes) {
3193 COMPILE_IF_REQUIRED
3194 geoprep(GEOSYS(node->__geoSystem),&node->center);
3195 extent6f_draw(node->_extent);
3196 geofin(GEOSYS(node->__geoSystem),&node->center);
3197 }
3198}
3199
3200void proximity_GeoProximitySensor (struct X3D_GeoProximitySensor *node) {
3201 /* Viewer pos = t_r2 */
3202 double cx,cy,cz;
3203 double len;
3204 struct point_XYZ dr1r2;
3205 struct point_XYZ dr2r3;
3206 struct point_XYZ nor1,nor2;
3207 struct point_XYZ ins;
3208 static struct point_XYZ yvec = {0,0.05,0};
3209 static struct point_XYZ zvec = {0,0,-0.05};
3210 static struct point_XYZ zpvec = {0,0,0.05};
3211 static struct point_XYZ orig = {0,0,0};
3212 struct point_XYZ t_zvec, t_yvec, t_orig, t_center;
3213 GLDOUBLE modelMatrix[16];
3214 GLDOUBLE projMatrix[16];
3215 GLDOUBLE view2prox[16];
3216
3217 if(!((node->enabled))) return;
3218 COMPILE_IF_REQUIRED
3219
3220 geoprep(GEOSYS(node->__geoSystem),&node->center);
3221 FW_GL_GETDOUBLEV(GL_MODELVIEW_MATRIX, modelMatrix);
3222 geofin(GEOSYS(node->__geoSystem),&node->center);
3223 matinverseAFFINE(view2prox,modelMatrix);
3224 if(1){
3225 //feature-AFFINE_GLU_UNPROJECT
3226 transform(&t_orig,&orig,view2prox);
3227 }
3228 transform(&t_center,&orig, view2prox);
3229
3230
3231 /*printf ("\n");
3232 printf ("unprojected, t_orig (0,0,0) %lf %lf %lf\n",t_orig.x, t_orig.y, t_orig.z);
3233 printf ("unprojected, t_yvec (0,0.05,0) %lf %lf %lf\n",t_yvec.x, t_yvec.y, t_yvec.z);
3234 printf ("unprojected, t_zvec (0,0,-0.05) %lf %lf %lf\n",t_zvec.x, t_zvec.y, t_zvec.z);
3235 */
3236 //inside test in TCS
3237 cx = t_center.x; //minus 0,0,0
3238 cy = t_center.y;
3239 cz = t_center.z;
3240 {
3241 float cc[3];
3242 //how draw bounding box? doesn't seem to draw on proximity pass
3243 // H: you need a render_proximity
3244 vecscale3f(cc,node->size.c,.5);
3245 extent6f_constructor(node->_extent,-cc[0],cc[0],-cc[1],cc[1],-cc[2],cc[2]);
3246 //if(renderstate()->render_boxes) extent6f_draw(node->_extent);
3247 }
3248 if(((node->size).c[0]) == 0 || ((node->size).c[1]) == 0 || ((node->size).c[2]) == 0) return;
3249
3250 if(fabs(cx) > ((node->size).c[0])/2 ||
3251 fabs(cy) > ((node->size).c[1])/2 ||
3252 fabs(cz) > ((node->size).c[2])/2) return;
3253 /* printf ("within (Geo)ProximitySensor\n"); */
3254
3255 /* Ok, we now have to compute... */
3256 (node->__hit) /*cget*/ = 1;
3257
3258 /* Position */
3259 //in TCS
3260 ((node->__t1).c[0]) = (float)t_center.x;
3261 ((node->__t1).c[1]) = (float)t_center.y;
3262 ((node->__t1).c[2]) = (float)t_center.z;
3263
3264
3265 {
3266 Quaternion quat;
3267 double oo[4];
3268 struct SFVec3d tcsCoord, gcCoord, userCoord;
3269 matrix_to_quaternion(&quat,modelMatrix);
3270 quaternion_normalize(&quat);
3271 quaternion_to_vrmlrot(&quat,&oo[0],&oo[1],&oo[2],&oo[3]);
3272 vecnormald(oo,oo);
3273 oo[3] = -oo[3];
3274 double2float(node->__t2.c,oo,4);
3275 float2double(tcsCoord.c,node->__t1.c,3);
3276 // generate geoCoord_changed here instead of in do_GPS
3277 {
3278 struct X3D_Node *boundvp = getActiveLayerBoundViewpoint();
3279
3280 if(boundvp && boundvp->_nodeType == NODE_GeoViewpoint){
3281 struct SFVec3d gcCoord, geoCoord;
3282 struct X3D_GeoViewpoint *gvp = (struct X3D_GeoViewpoint *)boundvp;
3283 user2gc(GEOSYS(gvp->__geoSystem),&gvp->position,1,&gcCoord);
3284 } else {
3285 tcs2gc(GEOSYS(node->__geoSystem),&node->center,&tcsCoord,1,&gcCoord);
3286 }
3287 gc2user(GEOSYS(node->__geoSystem),&gcCoord,1,&userCoord);
3288 veccopyd(node->__t3.c,userCoord.c);
3289 }
3290 }
3291}
3292
3293
3294/* GeoProximitySensor code for ClockTick */
3295void do_GeoProximitySensorTick( void *ptr) {
3296 int specversion;
3297 struct X3D_GeoProximitySensor *node = (struct X3D_GeoProximitySensor *)ptr;
3298
3299 /* if not enabled, do nothing */
3300 if (!node) return;
3301 if (node->__oldEnabled != node->enabled) {
3302 node->__oldEnabled = node->enabled;
3303 MARK_EVENT(X3D_NODE(node),offsetof (struct X3D_GeoProximitySensor, enabled));
3304 }
3305 if (!node->enabled) return;
3306
3307 /* isOver state */
3308 /* did we get a signal? */
3309 if (node->__hit) {
3310 if (!node->isActive) {
3311 #ifdef SEVERBOSE
3312 printf ("PROX - initial defaults\n");
3313 #endif
3314
3315 node->isActive = 1;
3316 node->enterTime = TickTime();
3317 MARK_EVENT (ptr, offsetof(struct X3D_GeoProximitySensor, isActive));
3318 MARK_EVENT (ptr, offsetof(struct X3D_GeoProximitySensor, enterTime));
3319
3320 }
3321
3322 /* now, has anything changed? */
3323 if (memcmp ((void *) &node->position_changed,(void *) &node->__t1,sizeof(struct SFColor))) {
3324 #ifdef SEVERBOSE
3325 printf ("PROX - position changed!!! \n");
3326 #endif
3327
3328 memcpy ((void *) &node->position_changed,
3329 (void *) &node->__t1,sizeof(struct SFColor));
3330 MARK_EVENT (ptr, offsetof(struct X3D_GeoProximitySensor, position_changed));
3331
3332 #ifdef VERBOSE
3333 printf ("do_GeoProximitySensorTick, position changed; it now is %lf %lf %lf\n",node->position_changed.c[0],
3334 node->position_changed.c[1], node->position_changed.c[2]);
3335 printf ("nearPlane is %lf\n",Viewer.nearPlane);
3336
3337 #endif
3338
3339 if(!vecsamed(node->geoCoord_changed.c,node->__t3.c)){
3340 veccopyd(node->geoCoord_changed.c,node->__t3.c);
3341 MARK_EVENT (ptr, offsetof(struct X3D_GeoProximitySensor, geoCoord_changed));
3342 }
3343 }
3344 if (memcmp ((void *) &node->orientation_changed, (void *) &node->__t2,sizeof(struct SFRotation))) {
3345 #ifdef SEVERBOSE
3346 printf ("PROX - orientation changed!!!\n ");
3347 #endif
3348
3349 memcpy ((void *) &node->orientation_changed,
3350 (void *) &node->__t2,sizeof(struct SFRotation));
3351 MARK_EVENT (ptr, offsetof(struct X3D_GeoProximitySensor, orientation_changed));
3352 }
3353 } else {
3354 if (node->isActive) {
3355 #ifdef SEVERBOSE
3356 printf ("PROX - stopping\n");
3357 #endif
3358
3359 node->isActive = 0;
3360 node->exitTime = TickTime();
3361 MARK_EVENT (ptr, offsetof(struct X3D_GeoProximitySensor, isActive));
3362
3363 MARK_EVENT (ptr, offsetof(struct X3D_GeoProximitySensor, exitTime));
3364 }
3365 }
3366 node->__hit=FALSE;
3367}
3368
3369
3370/************************************************************************/
3371/* GeoTouchSensor */
3372/************************************************************************/
3373
3374void compile_GeoTouchSensor (struct X3D_GeoTouchSensor * node) {
3375 #ifdef VERBOSE
3376 printf ("compiling GeoTouchSensor\n");
3377 #endif
3378 compile_geoSystem(X3D_NODE(node),node->_nodeType,&node->geoSystem,&node->__geoSystem);
3379 MARK_NODE_COMPILED
3380
3381 /* events */
3382 /* MARK_SFNODE_INOUT_EVENT(node->metadata, node->__oldmetadata, offsetof (struct X3D_GeoTouchSensor, metadata)) */
3383
3384}
3385
3386void do_GeoTouchSensor ( void *ptr, int ev, int but1, int over) {
3387
3388 int specversion;
3389 struct X3D_GeoTouchSensor *node = (struct X3D_GeoTouchSensor *)ptr;
3390 struct point_XYZ normalval; /* different structures for normalization calls */
3391 ttglobal tg;
3392 COMPILE_IF_REQUIRED
3393
3394 #ifdef SENSVERBOSE
3395 printf ("%lf: TS ",TickTime());
3396 if (ev==ButtonPress) printf ("ButtonPress ");
3397 else if (ev==ButtonRelease) printf ("ButtonRelease ");
3398 else if (ev==KeyPress) printf ("KeyPress ");
3399 else if (ev==KeyRelease) printf ("KeyRelease ");
3400 else if (ev==MotionNotify) printf ("%lf MotionNotify ");
3401 else printf ("ev %d ",ev);
3402
3403 if (but1) printf ("but1 TRUE "); else printf ("but1 FALSE ");
3404 if (over) printf ("over TRUE "); else printf ("over FALSE ");
3405 printf ("\n");
3406 #endif
3407
3408 /* if not enabled, do nothing */
3409 if (!node) return;
3410 if (node->__oldEnabled != node->enabled) {
3411 node->__oldEnabled = node->enabled;
3412 MARK_EVENT(X3D_NODE(node),offsetof (struct X3D_GeoTouchSensor, enabled));
3413 }
3414 if (!node->enabled) return;
3415 tg = gglobal();
3416 /* isOver state */
3417 if ((ev == overMark) && (over != node->isOver)) {
3418 #ifdef SENSVERBOSE
3419 printf ("TS %u, isOver changed %d\n",node, over);
3420 #endif
3421 node->isOver = over;
3422 MARK_EVENT (ptr, offsetof (struct X3D_GeoTouchSensor, isOver));
3423 }
3424
3425 /* active */
3426 /* button presses */
3427 if (ev == ButtonPress) {
3428 node->isActive=1;
3429 MARK_EVENT (ptr, offsetof (struct X3D_GeoTouchSensor, isActive));
3430 #ifdef SENSVERBOSE
3431 printf ("touchSens %u, butPress\n",node);
3432 #endif
3433
3434 node->touchTime = TickTime();
3435 MARK_EVENT(ptr, offsetof (struct X3D_GeoTouchSensor, touchTime));
3436
3437 } else if (ev == ButtonRelease) {
3438 #ifdef SENSVERBOSE
3439 printf ("touchSens %u, butRelease\n",node);
3440 #endif
3441 node->isActive=0;
3442 MARK_EVENT (ptr, offsetof (struct X3D_GeoTouchSensor, isActive));
3443 }
3444
3445 /* hitPoint and hitNormal */
3446 /* save the current hitPoint for determining if this changes between runs */
3447 memcpy ((void *) &node->_oldhitPoint, (void *) &tg->RenderFuncs.ray_save_posn,sizeof(struct SFColor));
3448
3449 /* did the hitPoint change between runs? */
3450 if ((APPROX(node->_oldhitPoint.c[0],node->hitPoint_changed.c[0])!= TRUE) ||
3451 (APPROX(node->_oldhitPoint.c[1],node->hitPoint_changed.c[1])!= TRUE) ||
3452 (APPROX(node->_oldhitPoint.c[2],node->hitPoint_changed.c[2])!= TRUE)) {
3453
3454 #ifdef SENSVERBOSE
3455 printf ("GeoTouchSens, hitPoint changed: %f %f %f\n",node->hitPoint_changed.c[0],
3456 node->hitPoint_changed.c[1], node->hitPoint_changed.c[2]);
3457 #endif
3458
3459 memcpy ((void *) &node->hitPoint_changed, (void *) &node->_oldhitPoint, sizeof(struct SFColor));
3460 //vecprint3fb("hitpoint",node->hitPoint_changed.c,"\n");
3461 MARK_EVENT(ptr, offsetof (struct X3D_GeoTouchSensor, hitPoint_changed));
3462
3463 /* convert this back into the requested GeoSpatial format... */
3464 node->hitGeoCoord_changed.c[0] = (double) node->hitPoint_changed.c[0];
3465 node->hitGeoCoord_changed.c[1] = (double) node->hitPoint_changed.c[1];
3466 node->hitGeoCoord_changed.c[2] = (double) node->hitPoint_changed.c[2];
3467
3468 MARK_EVENT (ptr, offsetof(struct X3D_GeoTouchSensor, hitGeoCoord_changed));
3469
3470 #ifdef SENSVERBOSE
3471 printf ("\nhitGeoCoord_changed as a GCC, %lf %lf %lf\n",
3472 node->hitGeoCoord_changed.c[0],
3473 node->hitGeoCoord_changed.c[1],
3474 node->hitGeoCoord_changed.c[2]);
3475 #endif
3476 struct SFVec3d gcCoord;
3477 Geosys *gs = GEOSYS(node->__geoSystem);
3478 lcs2gc(gs,&node->hitGeoCoord_changed,1,&gcCoord);
3479 gc2user(gs,&gcCoord,1,&node->hitGeoCoord_changed);
3480 //vecprint3db("user",node->hitGeoCoord_changed.c,"\n");
3481 }
3482
3483 /* have to normalize normal; change it from SFColor to struct point_XYZ. */
3484 normalval.x = tg->RenderFuncs.hyp_save_norm[0];
3485 normalval.y = tg->RenderFuncs.hyp_save_norm[1];
3486 normalval.z = tg->RenderFuncs.hyp_save_norm[2];
3487 normalize_vector(&normalval);
3488 node->_oldhitNormal.c[0] = (float) normalval.x;
3489 node->_oldhitNormal.c[1] = (float) normalval.y;
3490 node->_oldhitNormal.c[2] = (float) normalval.z;
3491
3492 /* did the hitNormal change between runs? */
3493 MARK_SFVEC3F_INOUT_EVENT(node->hitNormal_changed,node->_oldhitNormal,offsetof (struct X3D_GeoTouchSensor, hitNormal_changed))
3494}
3495
3496
3497
3498/************************************************************************/
3499/* GeoViewpoint */
3500/************************************************************************/
3501void calculateViewingSpeedB();
3502void compile_GeoViewpoint (struct X3D_GeoViewpoint * node) {
3503 Geosys *gs;
3504 struct SFVec3d gcCoord;
3505 compile_geoSystem(X3D_NODE(node),node->_nodeType,&node->geoSystem,&node->__geoSystem);
3506 gs = GEOSYS(node->__geoSystem);
3507
3508 update_origin(gs, X3D_NODE(node), &node->position, X3D_GEOORIGIN(node->geoOrigin));
3509 user2gc(gs,&node->position,1,&gcCoord);
3510 gc2gd(gs,&gcCoord,1,&node->__movedgd);
3511 MARK_NODE_COMPILED
3512
3513 /* events */
3514 /* MARK_SFNODE_INOUT_EVENT(node->metadata, node->__oldmetadata, offsetof (struct X3D_GeoViewpoint, metadata)) */
3515 MARK_SFFLOAT_INOUT_EVENT(node->fieldOfView, node->__oldFieldOfView, offsetof (struct X3D_GeoViewpoint, fieldOfView))
3516 MARK_SFBOOL_INOUT_EVENT(node->headlight, node->__oldHeadlight, offsetof (struct X3D_GeoViewpoint, headlight))
3517 MARK_SFBOOL_INOUT_EVENT(node->jump, node->__oldJump, offsetof (struct X3D_GeoViewpoint, jump))
3518 /*
3519 //dug9 may 2015 I'm not sure what the __old stuff was for (H: debugging) but
3520 //shallow copying a string pointer -or MFString or SFString- makes it hard to generically free during exit
3521 //see opengl_utils.c in cbFreeMallocedBuiltinField
3522 MARK_SFSTRING_INOUT_EVENT(node->description,node->__oldSFString, offsetof(struct X3D_GeoViewpoint, description))
3523 MARK_MFSTRING_INOUT_EVENT(node->navType,node->__oldMFString, offsetof(struct X3D_GeoViewpoint, navType))
3524 */
3525 #ifdef VERBOSE
3526 printf ("compiled GeoViewpoint\n\n");
3527 #endif
3528}
3529struct X3D_Node *getActiveLayerBoundViewpoint();
3530void CONVERT_BACK_TO_GD_OR_UTMC(Geosys *targetGeoSystem, struct X3D_Node *geoorigin,
3531 struct SFVec3d *LCSpos, struct SFVec3d *gdCoords, struct SFVec3d *thisField);
3532
3533void geoviewpoint_update_TCS(struct X3D_GeoViewpoint *node, Quaternion *Quat, struct point_XYZ *Pos){
3534 //Theory of operation:
3535 // NLA - node local alignment
3536 // we use viewer as a 3D pointing device relative to our GVP node's local coordinate system
3537 // (-Z to north pole, X east, Y up) at GVP
3538
3539 struct SFVec3d tcsCoord, gdCoord; //GCpos,
3540 //Quaternion qlc2gc;
3541 Geosys *gs;
3542 struct SFVec3d gcCoord;
3543 gs = GEOSYS(node->__geoSystem);
3544
3545 Quaternion qlo, q2;
3546 struct SFVec4d lo;
3547
3548 //1. update .position
3549 //convert current TCS back to GC
3550 pointxyz2double(tcsCoord.c,Pos);
3551 user2gc(gs,&node->position,1,&gcCoord);
3552 gc2gd(gs,&gcCoord,1,&gdCoord);
3553 tcs2gc(gs,&gdCoord,&tcsCoord,1,&gcCoord);
3554 //convert GC back to .position and new gd
3555 gc2user(gs,&gcCoord,1,&node->position);
3556 gc2gd(gs,&gcCoord,1,&gdCoord);
3557
3558 MARK_EVENT(X3D_NODE(node),offsetof(struct X3D_GeoViewpoint,position));
3559
3560 //2. update .orientation that's in TCS and stays in TCS, although TCS at a different location
3561 //Why? lets say we move straight ahead. Viewer->Quat doesn't change. But the same .orientation
3562 //at 2 different locations means we turned. So we need to un-do the implied turn.
3563 int FREEFLY = !Viewer()->collision;
3564 if(FREEFLY){
3565 //if we want to fly in any direction without being clamped to the planet surface
3566 // then we need to undo the implied 3 axis rotation between the previous and current location
3567 struct SFVec3d translate1, translate2;
3568 struct SFVec4d rotate1, rotate2, rotate3;
3569 Quaternion q1,q2,q3,q4;
3570 gc2tcs_transform(gs, &node->__movedgd, &translate1, &rotate1);
3571 gc2tcs_transform(gs, &gdCoord, &translate2, &rotate2);
3572 rotate2.c[3] = -rotate2.c[3];
3573 vrmlrot4d_to_quaternion(&q1,rotate1.c);
3574 vrmlrot4d_to_quaternion(&q2,rotate2.c);
3575 quaternion_multiply(&q3,&q1,&q2);
3576 quaternion_multiply(&q4,Quat,&q3);
3577 quaternion_to_vrmlrot4d(&q4,rotate3.c);
3578 rotate3.c[3] = -rotate3.c[3];
3579 double2float(node->orientation.c,rotate3.c,4);
3580 }else{
3581 //else if we're following the curvature of the earth, then we just undo the implied azimuth part of the rotation
3582 //2.a comput aziumth correction dAzimuth = sin(latitude) x (Longitude2 - Longitude1)
3583 // or dA = sin(phi)*dlambda
3584 double oo[4], pp[3];
3585 double deltagd[3], gd[3];
3586 vecdifd(deltagd,node->__movedgd.c,gdCoord.c);
3587 veccopyd(gd,node->__movedgd.c);
3588 //veccopyd(gd,gdCoord.c);
3589 //5: GD: if "latitude_first" TRUE, if "longitude_first", FALSE
3590 //7: GD: TRUE: decimal degrees, FALSE radians
3591 if(!gs->gd_latitude_first){
3592 //get latitude first
3593 vecswizzle2d(deltagd);
3594 vecswizzle2d(gd);
3595 }
3596 if(gs->gd_degrees) {
3597 //get radians
3598 vecscale2d(deltagd,deltagd,RADIANS_PER_DEGREE);
3599 vecscale2d(gd,gd,RADIANS_PER_DEGREE);
3600 }
3601 double dazimuth, dlambda;
3602 Quaternion qaz, qq;
3603 //as we cross the mid-pacific time zone (PI from grenwich)
3604 // our longitude goes from -PI to +PI.
3605 // For azimuth correction we want the incremental/acute longitude difference
3606 dlambda = angleNormalized(deltagd[1]);
3607 //if(fabs(gd[0]) > 30.0*RADIANS_PER_DEGREE){
3608 dazimuth = sin(gd[0])*dlambda;
3609 vrmlrot_to_quaternion(&qaz,0.0,1.0,0.0,-dazimuth); //?? different sign than old offset0 way??
3610 quaternion_multiply(&qq,Quat,&qaz);
3611 //}else{
3612 // qq = *Quat;
3613 //}
3614 quaternion_to_vrmlrot(&qq,&oo[0],&oo[1],&oo[2],&oo[3]);
3615 oo[3] = -oo[3];
3616 double2float(node->orientation.c,oo,4);
3617 }
3618 MARK_EVENT(X3D_NODE(node),offsetof(struct X3D_GeoViewpoint,orientation));
3619 //update gdcoord for next delta
3620 veccopyd(node->__movedgd.c,gdCoord.c);
3621
3622
3623}
3624
3625void geoviewpoint_fetch_TCS(struct X3D_GeoViewpoint *node, Quaternion *Quat, struct point_XYZ *Pos){
3626 //Theory of operation:
3627 // NLA - node local alignment
3628 // we use viewer as a 3D pointing device relative to our GVP node's local coordinate system
3629 // (-Z to north pole, X east, Y up) at GVP
3630 double oo[4];
3631 struct SFVec3d gcCoord, gdCoord, tcsCoord;
3632 Geosys *gs;
3633 gs = GEOSYS(node->__geoSystem);
3634
3635 float2double(oo,node->orientation.c,4);
3636 vrmlrot_to_quaternion(Quat,oo[0],oo[1],oo[2], -oo[3]);
3637 user2gc(gs,&node->position,1,&gcCoord);
3638 gc2gd(gs,&gcCoord,1,&gdCoord);
3639 gc2tcs(gs,&gdCoord,&gcCoord,1,&tcsCoord);
3640 double2pointxyz(Pos,tcsCoord.c);
3641 //Pos->x = Pos->y = Pos->z = 0.0;
3642}
3643
3644void geoviewpoint_fetch_LCS(struct X3D_GeoViewpoint *node, Quaternion *Quat, struct point_XYZ *Pos){
3645 //returns LCS/LCA - should be similar to prep_geoViewpoint
3646 //
3647 //LCS - local coordinate system - a shared euclidean system for a planet's data
3648 //UCS - user coordinate system, what the scene author specifies in the scene file
3649 // might be GC, GD (degrees or radians, lat or long first), XTM (UTM/3TM, easting or northing first) and w/wo geoid
3650 //LCA/GCA/UCA - alignment - the orientation part
3651 struct Planet *planet;
3652 struct SFVec3d LCpos;;
3653 struct SFVec4d lo;
3654 Quaternion qoo, qlo, qao, qgc;
3655 //ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
3656
3657 double oo[4], gd[3];
3658
3659 planet = current_planet();
3660 //step 1 convert user coordinates UCS to LCS coordinates
3661 // GC = f(UCS) //function depends on user coordinate system
3662 // LCS = (GC - autoOffset) x autoOrient^
3663 moveCoords3d(GEOSYS(node->__geoSystem),&planet->autoOrigin,&planet->autoOrient,&node->position,1,&LCpos,&node->__movedgd);
3664 vecnegated(LCpos.c,LCpos.c); //like prep_viewpoint?
3665 double2pointxyz(Pos,LCpos.c);
3666
3667 //step 2 convert user alignement UCA to local coordinate alignement LCA
3668 //step 2a convert UCA to GCA
3669 //GCA = f(UCA)
3670 // = LO^ x UCA (for GD and XTM)
3671 float2double(oo,node->orientation.c,4);
3672 oo[3] = -oo[3]; //like prep_viewpoint?
3673 vrmlrot_to_quaternion(&qoo,oo[0],oo[1],oo[2], oo[3]);
3674 GeoOrient(X3D_NODE(node->geoOrigin), GEOSYS(node->__geoSystem), &node->__movedgd, &lo);
3675 vrmlrot_to_quaternion(&qlo,lo.c[0],lo.c[1],lo.c[2], -lo.c[3]);
3676 quaternion_multiply(&qgc,&qoo,&qlo);
3677 //step 2b convert from GCA to LCA
3678 //LCA = AO^ x GCA
3679 vrmlrot_to_quaternion(&qao,planet->autoOrient.c[0],planet->autoOrient.c[1],planet->autoOrient.c[2], -planet->autoOrient.c[3]);
3680 quaternion_multiply(Quat,&qgc,&qao);
3681 quaternion_normalize(Quat);
3682
3683}
3684
3685void geoviewpoint_update_LCS(struct X3D_GeoViewpoint *node, Quaternion *Quat, struct point_XYZ *Pos){
3686 struct Planet *planet;
3687 double pos[3], oo[4];
3688 struct SFVec3d GCpos, gdCoord;
3689 struct SFVec4d lo;
3690 Quaternion qao, qaoi, qgc, qlo, qoo;
3691 //ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
3692
3693 planet = current_planet();
3694 //step 1 convert LCS to UCS
3695 //step 1.a converte LCS to GC
3696 // GC = (autoOrient x LCPos) + autoOffset
3697 vrmlrot_to_quaternion(&qao,planet->autoOrient.c[0],planet->autoOrient.c[1],planet->autoOrient.c[2],planet->autoOrient.c[3]);
3698 pointxyz2double(pos,Pos);
3699 vecnegated(pos,pos); //like prep_viewpoint?
3700 quaternion_rotationd(pos,&qao,pos);
3701 vecaddd(GCpos.c,planet->autoOrigin.c,pos);
3702
3703 //step 1.b UCS = f(GC)
3704 CONVERT_BACK_TO_GD_OR_UTMC(GEOSYS(node->__geoSystem), node->geoOrigin, &GCpos, &node->__movedgd, &node->position);
3705 MARK_EVENT(X3D_NODE(node),offsetof(struct X3D_GeoViewpoint,position));
3706 //step 2 convert LCA to UCA
3707 //step 2a. convert LCA to GCA
3708 //GCA = AO x LCA
3709 quaternion_inverse(&qaoi,&qao);
3710 quaternion_multiply(&qgc,Quat,&qao);
3711 //step 2.b convert GCA to UCA
3712 // UCA = f(GCA)
3713 // = LO x GCA for GD and XTM
3714 GeoOrient(X3D_NODE(node->geoOrigin), GEOSYS(node->__geoSystem), &node->__movedgd, &lo);
3715 vrmlrot_to_quaternion(&qlo,lo.c[0],lo.c[1],lo.c[2], lo.c[3]);
3716 quaternion_multiply(&qoo,&qgc,&qlo);
3717 quaternion_normalize(&qoo);
3718 quaternion_to_vrmlrot(&qoo,&oo[0],&oo[1],&oo[2],&oo[3]);
3719 oo[3] = -oo[3];
3720 double2float(node->orientation.c,oo,4);
3721 MARK_EVENT(X3D_NODE(node),offsetof(struct X3D_GeoViewpoint,orientation));
3722
3723}
3724
3725void prep_GeoViewpoint (struct X3D_GeoViewpoint *node) {
3726 struct Planet *planet;
3727 double a1;
3728 GLint viewPort[10];
3729 if (!renderstate()->render_vp) return;
3730
3731 if((struct X3D_Node*)node == getActiveLayerBoundViewpoint() && !node->_donethispass){
3732 X3D_Viewer *viewer = Viewer();
3733 node->_donethispass = 1; //if the vp id DEF/USED multiple places in the scengraph,
3734 COMPILE_IF_REQUIRED
3735
3736 #ifdef VERBOSE
3737 printf ("prep_GeoViewpoint called\n");
3738 #endif
3739
3740 /* perform GeoViewpoint translations */
3741 {
3742 struct SFVec3d translation1, translation2, gcCoord, gdCoord;
3743 struct SFVec4d rotation1, rotation2;
3744 double oo[4];
3745 Geosys *gs = GEOSYS(node->__geoSystem);
3746
3747 //How this works
3748 //we need to get from an orientation in TCS to LCS via the transform stack
3749 // the stack goes in this order:
3750 // GVP.orientation < GVP-TCS < LCS-transform_stack
3751 user2gc(gs,&node->position,1,&gcCoord);
3752 gc2gd(gs,&gcCoord,1, &gdCoord);
3753
3754 float2double(oo,node->orientation.c,4);
3755 oo[3] = -oo[3];
3756 FW_GL_ROTATE_RADIANS(oo[3],oo[0],oo[1],oo[2]);
3757
3758 //geoprepT
3759 gc2tcs_transform(gs,&gdCoord,&translation2,&rotation2);
3760 FW_GL_ROTATE_RADIANS(rotation2.c[3],rotation2.c[0],rotation2.c[1],rotation2.c[2]);
3761 FW_GL_TRANSLATE_D(translation2.c[0],translation2.c[1],translation2.c[2]);
3762 lcs2gc_transform(&rotation1,&translation1);
3763 FW_GL_TRANSLATE_D(translation1.c[0],translation1.c[1],translation1.c[2]);
3764 FW_GL_ROTATE_RADIANS(rotation1.c[3],rotation1.c[0],rotation1.c[1],rotation1.c[2]);
3765
3766 }
3767
3768 planet = current_planet();
3769 node->_prepped_planet = planet->ID;
3770
3771 /* we have a new currentPosInModel now... */
3772 /* printf ("currentPosInModel was %lf %lf %lf\n", Viewer.currentPosInModel.x, Viewer.currentPosInModel.y, Viewer.currentPosInModel.z); */
3773
3774 /* the AntiPos has been applied in the trans and rots above, so we do not need to do it here */
3775 //getCurrentPosInModel(FALSE);
3776
3777
3778 /* now, lets work on the GeoViewpoint fieldOfView.
3779 Q why now, here?
3780 A.the window can be resized on any frame. so can't do it once in compile_geoviewpoint
3781 -and analogously we do it in prep_viewpoint and prep_orthoviewpoint
3782 */
3783 FW_GL_GETINTEGERV(GL_VIEWPORT, viewPort);
3784 if(viewPort[2] > viewPort[3]) {
3785 a1=0;
3786 viewer->fieldofview = node->fieldOfView/3.1415926536*180;
3787 } else {
3788 a1 = node->fieldOfView;
3789 a1 = atan2(sin(a1),viewPort[2]/((float)viewPort[3]) * cos(a1));
3790 viewer->fieldofview = a1/3.1415926536*180;
3791 }
3792 if(viewer->type != VIEWER_WALK){
3793 //adjust target walk height in FLY mode
3794 calculateViewingSpeedB();
3795 node->_resetRelativeHeight = !node->relativeHeight;
3796 }
3797 #ifdef VERBOSE
3798 printf ("prep_GeoViewpoint, fieldOfView %f \n",node->fieldOfView);
3799 #endif
3800 }
3801}
3802
3803/* GeoViewpoint speeds and avatar sizes are depenent on elevation above WGS_84. These are calculated here */
3804void calculateViewingSpeedB() {
3805 /* the current position is the GC coordinate */
3806 ttglobal tg = gglobal();
3807 struct X3D_Node *boundvp = vector_back(struct X3D_Node*,getActiveBindableStacks(tg)->viewpoint);
3808
3809 if(boundvp->_nodeType == NODE_GeoViewpoint){
3810 double height, heightmin;
3811 int resetHeight;
3812 struct SFVec3d *gdCoords;
3813 struct X3D_GeoViewpoint *node = (struct X3D_GeoViewpoint*)boundvp;
3814 X3D_Viewer *viewer = Viewer();
3815
3816 //INITIALIZE_GEOSPATIAL(node)
3817 COMPILE_IF_REQUIRED(X3D_NODE(node));
3818 gdCoords = &node->__movedgd;
3819 height = gdCoords->c[2];
3820 heightmin = max(abs(height),50.0);
3821 viewer->speed = heightmin * node->speedFactor;
3822 if(0){
3823 static int count = 0;
3824 count++;
3825 if(count % 20 == 0)
3826 printf("height %lf speedFactor %lf speed %lf\n",height,node->speedFactor,viewer->speed);
3827 }
3828 if (viewer->speed < 1.0) viewer->speed=1.0;
3829
3830
3831 /* set the navigation info - use the GeoVRML algorithms */
3832 set_naviWidthHeightStep(
3833 height/1.6 *0.25,
3834 height,
3835 height/1.6 *0.25);
3836
3837 }
3838}
3839
3840static void calculateExamineModeDistance(void) {
3841/*
3842 printf ("bind_GeoViewpoint - calculateExamineModeDistance\n");
3843*/
3844Viewer()->doExamineModeDistanceCalculations = TRUE;
3845
3846}
3847void bind_GeoViewpoint (struct X3D_GeoViewpoint *node) {
3849
3850 /* did bind_node tell us we could bind this guy? */
3851 if (!(node->isBound)) return;
3852
3853 viewer = ViewerByLayerId(node->_layerId);
3854
3855 COMPILE_IF_REQUIRED
3856
3857 /* set Viewer position and orientation */
3858
3859 if(!node->_initializedOnce) {
3860 veccopyd(node->_position.c,node->position.c);
3861 veccopy4f(node->_orientation.c,node->orientation.c);
3862 node->_initializedOnce = TRUE;
3863 }
3864 if(!node->retainUserOffsets){
3865 veccopyd(node->position.c,node->_position.c);
3866 veccopy4f(node->orientation.c,node->_orientation.c);
3867 }
3868
3869
3870 if (viewer->transitionType != VIEWER_TRANSITION_TELEPORT && viewer->wasBound) {
3871 //save the previous vp pose, in root space, for future slerps
3872 viewer->vp2rnSaved = TRUE; //we bind after prep_viewpoint > setup_viewpoint in rendersceneupdatescene0
3873 //we bind from the root, so this would be setup_viewpoint_1() and _2()
3874 //- the viewmatrix including .position,.orientation,.Pos,.Quat, stereo
3875 {
3876 bindablestack* bstack = getActiveBindableStacks(gglobal());
3877 matcopy(viewer->slerp_viewmatrix,bstack->viewtransformmatrix);
3878 matcopy(viewer->slerp_posorimatrix,bstack->posorimatrix);
3879
3880 }
3881
3882 viewer->SLERPing = FALSE;
3883 viewer->startSLERPtime = TickTime();
3884 /* slerp Mark II */
3885 viewer->SLERPing2 = TRUE;
3886 viewer->SLERPing2justStarted = TRUE;
3887 //printf("binding\n");
3888
3889 } else {
3890 viewer->SLERPing = FALSE;
3891 viewer->SLERPing2 = FALSE;
3892 }
3893
3894 viewer->wasBound = TRUE;
3895
3896 #ifdef VERBOSE
3897 printf ("bind_GeoViewpoint, setting Viewer to %lf %lf %lf orient %f %f %f %f\n",node->__movedPosition.c[0],node->__movedPosition.c[1],
3898 node->__movedPosition.c[2],node->orientation.c[0],node->orientation.c[1],node->orientation.c[2],
3899 node->orientation.c[3]);
3900 printf (" node %u fieldOfView %f\n",node,node->fieldOfView);
3901 #endif
3902
3903 vrmlrot_to_quaternion (&viewer->Quat,node->__movedOrientation.c[0],
3904 node->__movedOrientation.c[1],node->__movedOrientation.c[2],node->__movedOrientation.c[3]);
3905
3906 calculateViewingSpeedB();
3907 node->_resetRelativeHeight = !node->relativeHeight;
3908
3909 calculateExamineModeDistance();
3910 setMenuStatusVP (node->description->strptr);
3911 fwl_set_viewer_type (VIEWER_WALK);
3912 fwl_setCollision(TRUE);
3913}
3914
3915
3916/************************************************************************/
3917/* GeoTransform */
3918/************************************************************************/
3919
3920void compile_GeoTransform (struct X3D_GeoTransform * node) {
3921 #ifdef VERBOSE
3922 printf ("compiling GeoLocation\n");
3923 #endif
3924
3925 Geosys *gs;
3926 compile_geoSystem(X3D_NODE(node),node->_nodeType,&node->geoSystem,&node->__geoSystem);
3927 gs = GEOSYS(node->__geoSystem);
3928 update_origin(gs, X3D_NODE(node), &node->geoCenter, X3D_GEOORIGIN(node->geoOrigin));
3929
3930
3931 MARK_SFVEC3D_INOUT_EVENT(node->geoCenter, node->__oldGeoCenter,offsetof (struct X3D_GeoTransform, geoCenter))
3932 MARK_MFNODE_INOUT_EVENT(node->children, node->__oldChildren, offsetof (struct X3D_GeoTransform, children))
3933
3934
3935 INITIALIZE_EXTENT;
3936
3937 /* printf ("changed Transform for node %u\n",node); */
3938 node->__do_center = verify_translate ((GLfloat *)node->center.c);
3939 node->__do_trans = verify_translate ((GLfloat *)node->translation.c);
3940 node->__do_scale = verify_scale ((GLfloat *)node->scale.c);
3941 node->__do_rotation = verify_rotate ((GLfloat *)node->rotation.c);
3942 node->__do_scaleO = verify_rotate ((GLfloat *)node->scaleOrientation.c);
3943
3944 node->__do_anything = (node->__do_center ||
3945 node->__do_trans ||
3946 node->__do_scale ||
3947 node->__do_rotation ||
3948 node->__do_scaleO);
3949
3950 REINITIALIZE_SORTED_NODES_FIELD(node->children,node->_sortedChildren);
3951 MARK_NODE_COMPILED
3952}
3953
3954
3955//March 2018 GT GeoTransform strategy:
3956//- do compile_ prep_ fin_ child_ like Transform, except:
3957//-- center = 0,0,0 always
3958//-- in compile, also compile geosystem
3959//-- in child, wrap normalChildren call with a geoprepT and geofinT
3960
3961/* do transforms, calculate the distance */
3962void prep_GeoTransform (struct X3D_GeoTransform *node) {
3963
3964 COMPILE_IF_REQUIRED
3965
3966 /* rendering the viewpoint means doing the inverse transformations in reverse order (while poping stack),
3967 * so we do nothing here in that case -ncoder */
3968
3969 /* printf ("prep_Transform, render_hier vp %d geom %d light %d sens %d blend %d prox %d col %d\n",
3970 render_vp,render_geom,render_light,render_sensitive,render_blend,render_proximity,render_collision); */
3971
3972 /* do we have any geometry visible, and are we doing anything with geometry? */
3973 OCCLUSIONTEST
3974
3975 if(!renderstate()->render_vp) {
3976 /* do we actually have any thing to rotate/translate/scale?? */
3977 if (node->__do_anything) {
3978
3979 FW_GL_PUSH_MATRIX();
3980
3981 /* TRANSLATION */
3982 if (node->__do_trans)
3983 FW_GL_TRANSLATE_F(node->translation.c[0],node->translation.c[1],node->translation.c[2]);
3984
3985 /* CENTER */
3986 if (node->__do_center)
3987 FW_GL_TRANSLATE_F(node->center.c[0],node->center.c[1],node->center.c[2]);
3988
3989 /* ROTATION */
3990 if (node->__do_rotation) {
3991 FW_GL_ROTATE_RADIANS(node->rotation.c[3], node->rotation.c[0],node->rotation.c[1],node->rotation.c[2]);
3992 }
3993
3994 /* SCALEORIENTATION */
3995 if (node->__do_scaleO) {
3996 FW_GL_ROTATE_RADIANS(node->scaleOrientation.c[3], node->scaleOrientation.c[0], node->scaleOrientation.c[1],node->scaleOrientation.c[2]);
3997 }
3998
3999
4000 /* SCALE */
4001 if (node->__do_scale)
4002 FW_GL_SCALE_F(node->scale.c[0],node->scale.c[1],node->scale.c[2]);
4003
4004 /* REVERSE SCALE ORIENTATION */
4005 if (node->__do_scaleO)
4006 FW_GL_ROTATE_RADIANS(-node->scaleOrientation.c[3], node->scaleOrientation.c[0], node->scaleOrientation.c[1],node->scaleOrientation.c[2]);
4007
4008 /* REVERSE CENTER */
4009 if (node->__do_center)
4010 FW_GL_TRANSLATE_F(-node->center.c[0],-node->center.c[1],-node->center.c[2]);
4011 }
4012
4013 RECORD_DISTANCE
4014
4015 }
4016}
4017
4018
4019void fin_GeoTransform (struct X3D_GeoTransform *node) {
4020 OCCLUSIONTEST
4021
4022 if(!renderstate()->render_vp) {
4023 if (node->__do_anything) {
4024 FW_GL_POP_MATRIX();
4025 }
4026 } else {
4027 /*Rendering the viewpoint only means finding it, and calculating the reverse WorldView matrix.*/
4028 if((node->_renderFlags & VF_Viewpoint) == VF_Viewpoint) {
4029 FW_GL_TRANSLATE_F(((node->center).c[0]),((node->center).c[1]),((node->center).c[2])
4030 );
4031 FW_GL_ROTATE_RADIANS(((node->scaleOrientation).c[3]),((node->scaleOrientation).c[0]),((node->scaleOrientation).c[1]),((node->scaleOrientation).c[2])
4032 );
4033 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]))
4034 );
4035 FW_GL_ROTATE_RADIANS(-(((node->scaleOrientation).c[3])),((node->scaleOrientation).c[0]),((node->scaleOrientation).c[1]),((node->scaleOrientation).c[2])
4036 );
4037 FW_GL_ROTATE_RADIANS(-(((node->rotation).c[3])),((node->rotation).c[0]),((node->rotation).c[1]),((node->rotation).c[2])
4038 );
4039 FW_GL_TRANSLATE_F(-(((node->center).c[0])),-(((node->center).c[1])),-(((node->center).c[2]))
4040 );
4041 FW_GL_TRANSLATE_F(-(((node->translation).c[0])),-(((node->translation).c[1])),-(((node->translation).c[2]))
4042 );
4043 }
4044 }
4045}
4046void geoprepT0(Geosys *geoSystem, struct SFVec3d *userCoord){
4047 //LCS -> TCS
4048 //transform stack LCS (shared Local Coordinate System)
4049 // to this node TCS (topocentric coordinate system
4050
4051 struct SFVec3d translation1, translation2, gcCoord, gdCoord;
4052 struct SFVec4d rotation1, rotation2;
4053 double A,F;
4054 Geosys *gs = geoSystem;
4055
4056 //How this works
4057 //we need to get from child LCS to TCS for this node via the transform stack
4058 // the stack goes in this order:
4059 // GT-TCS < LCS-children
4060 if(0){
4061 getEllipsoidParams(gs->ellipsoid, &A, &F);
4062 if(gs->spatial_system == GEOSP_GC && veclengthd(userCoord->c) < A/2.0){
4063 //likely DIS with local coordinates ie near molten core of earth
4064 // and around here the ellipsoid radius M is fiddly, so we thunk to simple GC aligned local
4065 FW_GL_TRANSLATE_D(-userCoord->c[0],-userCoord->c[1],-userCoord->c[2]);
4066 }else{
4067 user2gc(gs,userCoord,1,&gcCoord);
4068 gc2gd(gs,&gcCoord,1, &gdCoord);
4069
4070 gc2tcs_transform(gs,&gdCoord,&translation2,&rotation2);
4071 FW_GL_ROTATE_RADIANS(rotation2.c[3],rotation2.c[0],rotation2.c[1],rotation2.c[2]);
4072 FW_GL_TRANSLATE_D(translation2.c[0],translation2.c[1],translation2.c[2]);
4073 lcs2gc_transform(&rotation1,&translation1);
4074 FW_GL_TRANSLATE_D(translation1.c[0],translation1.c[1],translation1.c[2]);
4075 FW_GL_ROTATE_RADIANS(rotation1.c[3],rotation1.c[0],rotation1.c[1],rotation1.c[2]);
4076 }
4077 }else{
4078 user2gc(gs,userCoord,1,&gcCoord);
4079 //gc2gd(gs,&gcCoord,1, &gdCoord);
4080 //gc2tcs_transform(gs,&gdCoord,&translation2,&rotation2);
4081 gc2tcsB_transform(gs,&gcCoord,&translation2,&rotation2);
4082 FW_GL_ROTATE_RADIANS(rotation2.c[3],rotation2.c[0],rotation2.c[1],rotation2.c[2]);
4083 FW_GL_TRANSLATE_D(translation2.c[0],translation2.c[1],translation2.c[2]);
4084 lcs2gc_transform(&rotation1,&translation1);
4085 FW_GL_TRANSLATE_D(translation1.c[0],translation1.c[1],translation1.c[2]);
4086 FW_GL_ROTATE_RADIANS(rotation1.c[3],rotation1.c[0],rotation1.c[1],rotation1.c[2]);
4087
4088 }
4089}
4090void geoprepT(Geosys *geoSystem, struct SFVec3d *userCoord){
4091 //LCS -> TCS
4092 //transform stack LCS (shared Local Coordinate System)
4093 // to this node TCS (topocentric coordinate system
4094 if(!renderstate()->render_vp) {
4095 FW_GL_PUSH_MATRIX();
4096 geoprepT0(geoSystem,userCoord);
4097 }
4098
4099}
4100void geofinT(Geosys *geoSystem, struct SFVec3d *userCoord){
4101 if(!renderstate()->render_vp) {
4102 FW_GL_POP_MATRIX();
4103 }else{
4104 geoprep0(geoSystem,userCoord);
4105 }
4106
4107}
4108void child_GeoTransform (struct X3D_GeoTransform *node) {
4109 CHILDREN_COUNT
4110 //LOCAL_LIGHT_SAVE
4111 //INITIALIZE_GEOSPATIAL(node)
4112 COMPILE_IF_REQUIRED
4113 OCCLUSIONTEST
4114 RETURN_FROM_CHILD_IF_NOT_FOR_ME
4115
4116 /* any children at all? */
4117 if (nc==0) return;
4118
4119 /* {
4120 int x;
4121 struct X3D_Node *xx;
4122
4123 printf ("child_Transform, this %d \n",node);
4124 for (x=0; x<nc; x++) {
4125 xx = X3D_NODE(node->children.p[x]);
4126 printf (" ch %d type %s dist %f\n",node->children.p[x],stringNodeType(xx->_nodeType),xx->_dist);
4127 }
4128 } */
4129
4130
4131 /* do we have a local light for a child? */
4132 //LOCAL_LIGHT_CHILDREN(node->children);
4133 prep_sibAffectors((struct X3D_Node*)node,&node->__sibAffectors);
4134
4135 /* now, just render the non-directionalLight children */
4136
4137 /* printf ("Transform %d, flags %d, render_sensitive %d\n",
4138 node,node->_renderFlags,render_sensitive); */
4139
4140 #ifdef CHILDVERBOSE
4141 printf ("transform - doing normalChildren\n");
4142 #endif
4143 geoprepT(GEOSYS(node->__geoSystem),&node->geoCenter);
4144 normalChildren(node->children);
4145 geofinT(GEOSYS(node->__geoSystem),&node->geoCenter);
4146 #ifdef CHILDVERBOSE
4147 printf ("transform - done normalChildren\n");
4148 #endif
4149
4150 //LOCAL_LIGHT_OFF
4151 fin_sibAffectors((struct X3D_Node*)node,&node->__sibAffectors);
4152
4153}
4154
4155//CONVERT_BACK_TO_GD_OR_UTMB(geoSystem, geoOrigin, thisField);
4156void CONVERT_BACK_TO_GD_OR_UTMC(Geosys *targetGeoSystem, struct X3D_Node *geoorigin,
4157 struct SFVec3d *LCSpos, struct SFVec3d *gdCoords, struct SFVec3d *thisField) {
4158 //assumes incoming thisField is in GC system
4159 //outputs thisField in targetGeoSystem
4160
4161/* compileGeosystem - encode the return value such that srf->p[x] is...
4162 0: spatial reference frame (GEOSP_UTM, GEOSP_GC, GEOSP_GD);
4163 1: ellipsoid index (defaults to GEOSP_WE)
4164 2: UTM zone number, 1..60. INT_ID_UNDEFINED = not specified
4165 3: UTM: if "northing_first" TRUE, if "easting_first", FALSE
4166 4: UTM: if "S" - value is FALSE, not S, value is TRUE
4167 5: GD: if "latitude_first" TRUE, if "longitude_first", FALSE
4168 6: GD: true if geoid height
4169 7: GD: TRUE: decimal degrees, FALSE radians
4170*/
4171
4172 /* do we need to change this from a GCC? */
4173 Geosys *geoSystem = targetGeoSystem;
4174 struct X3D_GeoOrigin *geoOrigin = (struct X3D_GeoOrigin*)geoorigin;
4175 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
4176 veccopyd(thisField->c,LCSpos->c);
4177 // already in GC system //if(0) vecaddd(thisField->c,thisField->c,p->autoOrigin.c);
4178
4179 if (geoSystem != NULL) { /* do we have a GeoSystem specified?? if not, dont do this! */
4180
4181 if (geoSystem->spatial_system != GEOSP_GC) {
4182 /* have to convert to GD or UTM. Go to GD first */
4183 gccToGdc (geoSystem, thisField, gdCoords);
4184 if(geoSystem->geoid_height || geoSystem->relativeHeight)
4185 gdCoords->c[2] -= userHeight2ellipsoidHeight(geoSystem,gdCoords);
4186 veccopyd(thisField->c,gdCoords->c);
4187
4188 /* printf ("changed as a GDC, %lf %lf %lf\n", thisField.c[0], thisField.c[1], thisField.c[2]); */
4189
4190 /* is this a GD? if so, go no further */
4191 if (geoSystem->spatial_system == GEOSP_UTM || geoSystem->spatial_system == GEOSP_3TM ) {
4192 /* convert this to UTM or 3TM */
4193 double dtemp[3];
4194
4195 if(geoSystem->spatial_system == GEOSP_UTM){
4196 gdToUtm3d(geoSystem,thisField->c, dtemp);
4197 veccopyd(thisField->c,dtemp);
4198 }else if(geoSystem->spatial_system == GEOSP_3TM) {
4199 gdTo3tm3d(geoSystem,thisField->c, dtemp);
4200 veccopyd(thisField->c,dtemp);
4201 }
4202
4203 /* printf ("changed as a UTM, %lf %lf %lf\n", thisField[0], thisField[1], thisField[2]); */
4204 }
4205 }
4206 }
4207}
4208
4209/*
4210WALK navigation:
4211(VPbindPose) + userOffsets[ (cumulative navigation) + (camera tilts/orientation) ]
4212The gravity height adustment (and general collision) goes into cumulative navigation
4213The mouse navigation goes into cumulative navigation
4214The LEVEL and FLY tilts go into orientation
4215
4216GEO WALK navigation:
4217almost everything is related to ellipsoid / GD coordinates
4218- LEVEL - relative to current gdCoords/GD position/GD up vector
4219- orientation (camera tilts) relative to GD vertical
4220- gravity correction - along GD vertical
4221- SPEED - relative to GD height above GEG terrain, ellipsoid or GD radius from GC center, along GD vertical
4222retainedUserOffsets are stored as absolute GD postion + orientation:
4223 userOffsets = retainedGDposition - vpBind + orientation
4224except: GD pose transformed into SLSLA for rendering, picking and extents
4225
4226*/
4227
4228/* compileGeosystem - encode the return value such that srf->p[x] is...
4229 0: spatial reference frame (GEOSP_UTM, GEOSP_GC, GEOSP_GD);
4230 1: ellipsoid index (defaults to GEOSP_WE)
4231 2: UTM zone number, 1..60. INT_ID_UNDEFINED = not specified
4232 3: UTM: if "northing_first" TRUE, if "easting_first", FALSE
4233 4: UTM: if "S" - value is FALSE, not S, value is TRUE
4234 5: GD: if "latitude_first" TRUE, if "longitude_first", FALSE
4235 6: GD: true if geoid height
4236 7: GD: TRUE: decimal degrees, FALSE radians
4237*/
4238int geoelevationgrid_getGDHeight0(struct X3D_GeoElevationGrid *node, struct SFVec3d *gdCoord, Geosys *geoSystem, double *gridHeight){
4239 int hit;
4240 //we'll work in gd coord.
4241 struct point_XYZ result;
4242 float centerf[3],bottomf[3];
4243 double centerd[3], bottomd[3], spined[3], vertvecd[3];
4244 struct SFVec3d xxCoord;
4245 double cosine;
4246 double tmin[3],tmax[3]; /* MBB for facet */
4247 struct sNaviInfo *naviinfo;
4248 Geosys *nodeSystem;
4249 GLDOUBLE awidth, atop, abottom, astep;
4250 ttglobal tg = gglobal();
4251 //naviinfo = (struct sNaviInfo *)tg->Bindable.naviinfo;
4252
4253 nodeSystem = GEOSYS(node->__geoSystem);
4254 hit = -1; //caller: watch out, this can be -1 on return. only 1 means true hit
4255 //get target node's gdCoord into GEG's gdcoord
4256 veccopyd(xxCoord.c,gdCoord->c);
4257 //printf("gdCoord %lf %lf %lf\n",gdCoord->c[0],gdCoord->c[1],gdCoord->c[2]);
4258 if(geoSystem->gd_latitude_first != GEOSYS(node->__geoSystem)->gd_latitude_first)
4259 vecswizzle2d(xxCoord.c);
4260 if(geoSystem->gd_degrees != GEOSYS(node->__geoSystem)->gd_degrees)
4261 if(geoSystem->gd_degrees == TRUE)
4262 vecscale2d(xxCoord.c,xxCoord.c,RADIANS_PER_DEGREE);
4263 else
4264 vecscale2d(xxCoord.c,xxCoord.c,DEGREES_PER_RADIAN);
4265
4266
4267 //get target nodes' gd coord into GEG's geosystem if not gd
4268 int XTM = FALSE;
4269 if(nodeSystem->spatial_system == GEOSP_UTM || nodeSystem->spatial_system == GEOSP_3TM ) {
4270 // convert GVP's gdCoord to UTM or 3TM, in GEGs user order
4271 XTM = TRUE;
4272 double dtemp[3];
4273 if(nodeSystem->spatial_system == GEOSP_UTM){
4274 gdToUtm3d(GEOSYS(node->__geoSystem),xxCoord.c, dtemp);
4275 veccopyd(xxCoord.c,dtemp);
4276 }else if(nodeSystem->spatial_system == GEOSP_3TM) {
4277 gdTo3tm3d(GEOSYS(node->__geoSystem),xxCoord.c, dtemp);
4278 veccopyd(xxCoord.c,dtemp);
4279 }
4280 } else if(nodeSystem->spatial_system == GEOSP_GC) {
4281 //no such thing as GC GEG
4282 return hit;
4283 }
4284
4285
4286 int inside;
4287 double emin[2],emax[2];
4288 //not sure what space the GEG's node->_extent is in, so will recalculate here in its user coordinates
4289 double size[2], spacing[2];
4290 int idimension[2];
4291 //we'll put dimension and spacing into x/easting/longitude-first order,
4292 // to capture xDimension,zDimension naming, then swizzle as needed
4293 idimension[0] = node->xDimension; //assume x is longitude/easting
4294 idimension[1] = node->zDimension; //assume z is latitude/northing
4295 spacing[0] = node->xSpacing; //x long/east
4296 spacing[1] = node->zSpacing; //z lat/north
4297 //GEGs Geosystem
4298 //to do some extent math, we'll swizzle into user order
4299 int swizzle_xzdimension = (XTM && nodeSystem->xtm_northing_first) || (!XTM && nodeSystem->gd_latitude_first);
4300 if(swizzle_xzdimension) {
4301 //swizzle spacing,dimension into GEG's user order
4302 //printf("early dimension swizzle\n");
4303 int itmp = idimension[0];
4304 idimension[0] = idimension[1];
4305 idimension[1] = itmp;
4306 vecswizzle2d(spacing);
4307 }
4308 size[0] = spacing[0]*(idimension[0] -1); //take off one column and one row to get cells (vs points)
4309 size[1] = spacing[1]*(idimension[1] -1);
4310 emin[0] = min(node->geoGridOrigin.c[0],node->geoGridOrigin.c[0]+size[0]);
4311 emax[0] = max(node->geoGridOrigin.c[0],node->geoGridOrigin.c[0]+size[0]);
4312 emin[1] = min(node->geoGridOrigin.c[1],node->geoGridOrigin.c[1]+size[1]);
4313 emax[1] = max(node->geoGridOrigin.c[1],node->geoGridOrigin.c[1]+size[1]);
4314 //printf("xxCoord= %lf %lf %lf\n",xxCoord.c[0],xxCoord.c[1],xxCoord.c[2]);
4315 //printf("emin= %lf %lf emax= %lf %lf\n",emin[0],emin[1],emax[0],emax[1]);
4316 inside = xxCoord.c[0] <= emax[0] && xxCoord.c[0] >= emin[0];
4317 inside &= xxCoord.c[1] <= emax[1] && xxCoord.c[1] >= emin[1];
4318 //printf("b");
4319 if(inside){
4320 double spinelength, vcenterd[3], pp[2];
4321 //double x,z,
4322 double cx,cz;
4323 double deltah, gridpointf[3];
4324 //printf("c\n");
4325 hit = 0;
4326 //see if grid height is below, between or above avatar
4327 pp[0] = xxCoord.c[0] - node->geoGridOrigin.c[0]; //latitude first/northing first default? or x == 0, z == 1?
4328 pp[1] = xxCoord.c[1] - node->geoGridOrigin.c[1];
4329
4330 //get pp from user order into x-first, z-second order - our old math below assumes x-first order
4331 if(swizzle_xzdimension) {
4332 //printf("swizzling user into xz\n");
4333 vecswizzle2d(pp);
4334 }
4335 //printf("x,z= %lf %lf\n",pp[0],pp[1]);
4336 //node->xDimension
4337 // z h2 h3
4338 // ^ h0 h1
4339 // |-->x
4340 //(ix,iz)
4341 double hh[4],gridheight;
4342 int i0,i1,i2,i3, ix, iz;
4343 ix = (int)(pp[0]/node->xSpacing);
4344 iz = (int)(pp[1]/node->zSpacing);
4345 //int nh = node->height.n;
4346 //printf("total h = %d\n",nh);
4347
4348 //printf("xspacing,zspacing,xdimension zdimension= %lf %lf %d %d\n",node->xSpacing,node->zSpacing,node->xDimension, node->zDimension);
4349 //printf("ix,iz= %d %d\n",ix,iz);
4350 i0 = iz * node->xDimension + ix;
4351 i1 = i0 + 1;
4352 i2 = i0 + node->xDimension;
4353 i3 = i2 + 1;
4354 //printf("i0-i3 = %d %d %d %d\n",i0,i1,i2,i3); //should all be < height.n
4355 hh[0] = node->height.p[i0];
4356 hh[1] = node->height.p[i1];
4357 hh[2] = node->height.p[i2];
4358 hh[3] = node->height.p[i3];
4359 //normalize cell x and z
4360 cx = (pp[0] - ix*node->xSpacing)/node->xSpacing;
4361 cz = (pp[1] - iz*node->zSpacing)/node->zSpacing;
4362 //height interpolation by finite elements > bilinear interpolotion of height
4363 // (could do cubic using 3x3 chunks)
4364 //printf("cx %lf cz %lf\n",cx,cz);
4365 gridheight = hh[0]*(1.0f - cz)*(1.0f - cx)
4366 + hh[1]*(1.0f - cz)*cx
4367 + hh[2]*cz*(1.0f - cx)
4368 + hh[3]*cz*cx;
4369 gridheight *= node->yScale;
4370 //printf("_");
4371 *gridHeight = gridheight;
4372 //printf("\tgridheight=%lf\n",gridheight);
4373 hit = 1;
4374 }
4375 return hit;
4376}
4377
4378int geoelevationgrid_disp2(struct X3D_GeoElevationGrid *node, struct X3D_GeoViewpoint *gvp){
4379 // general polyrep collision does a few ugly things:
4380 // 1. transforms all the points into collision/avatar space
4381 // 2. iterates over all the triangles (a few times)
4382 // this geoElevationGrid optimization will take a few shortcuts:
4383 // a) only do gravity, if enabled
4384 // b) transform avatar gravity vector into grid space
4385 // c) use geoElevationGrid rows, columns, xspace,zspace, to look up heights by avatar position N,E or lat,lon in grid space
4386 // d) see if its a collision
4387 // e) update the climing / falling parameters (and skip wall penetration detection and bump collision testing)
4388 // if for some reason it can't handle it, it returns -1, and then the generic collision can be called
4389 struct sFallInfo *fi;
4390 int hit,i;
4391
4392 hit = -1; //0 handled, and no colliision, 1=handled and collision, -1=not handled (not woaking, or gravity vector not perpendicular to grid)
4393 fi = FallInfo();
4394
4395 if(fi->walking)
4396 {
4397 struct point_XYZ result;
4398 float centerf[3],bottomf[3];
4399 double centerd[3], bottomd[3], spined[3], vertvecd[3], gridheight;
4400 struct SFVec3d *gdCoord, xxCoord;
4401 Geosys *geoSystem;
4402 double cosine;
4403 double tmin[3],tmax[3]; /* MBB for facet */
4404 struct sNaviInfo *naviinfo;
4405 GLDOUBLE awidth, atop, abottom, astep;
4406 ttglobal tg = gglobal();
4407 naviinfo = (struct sNaviInfo *)tg->Bindable.naviinfo;
4408
4409 //GVP gdcoord and geosys
4410 gdCoord = &gvp->__movedgd;
4411 geoSystem = GEOSYS(node->__geoSystem);
4412 hit = geoelevationgrid_getGDHeight0(node, gdCoord, geoSystem, &gridheight);
4413 static int count =0;
4414 count++;
4415 //if(hit != 1) printf("no carpet %d\n",count);
4416 if(hit == 1){
4417 //hit = 1;
4418 // scraped from:
4419 // accumulateFallingClimbing(abottom,atop,astep,p,num,n,tmin,tmax); //y1, y2, p, num, n);
4420 if(gvp->_resetRelativeHeight){
4421 naviinfo->height = gdCoord->c[2] - gridheight;
4422 gvp->_resetRelativeHeight = FALSE; //we do just once per WALK 'session' (WALK turned on, or bind with WALK on)
4423 //printf("walking resetRelative gridHeight=%lf gdCoordc2=%lf\n",gridheight,gdCoord->c[2]);
4424 }
4425 //printf("=\n");
4426 double abottom = gdCoord->c[2] - naviinfo->height; //100; // - avatar height?
4427 double hhh = gridheight - abottom;
4428 //printf("gridHeight %lf avatarHeight %lf gdc2 %lf naviih %lf\n",hhh,abottom,gdCoord->c[2],naviinfo->height);
4429 double hhbelowfoot = hhh; //hhh - abottom;
4430 //fi->fallHeight = 1000000.0;
4431 if( hhh < 0.0 )
4432 {
4433 //printf("V");
4434 /* falling */
4435 if( hhh < abottom && hhh > -fi->fallHeight)
4436 {
4437 //printf("v");
4438 /* FALLING */
4439 if(fi->hits ==0)
4440 fi->hfall = hhbelowfoot; //hh - y1;
4441 else
4442 if(hhbelowfoot > fi->hfall) fi->hfall = hhbelowfoot; //hh - y1;
4443 fi->hits++;
4444 fi->isFall = 1;
4445 //printf("hfall %lf\n",fi->hfall);
4446 }else{
4447 //printf("~");
4448 /* regular below / nadir collision - below avatar center but above avatar's feet which are at 0.0 - avatar.height*/
4449 if( hhh >= abottom ) /* && hh <= (y1-ystep) ) //no step height implementation */
4450 {
4451 /* CLIMBING. handled elsewhere for displacements, except annihilates any fall*/
4452 fi->canFall = 0;
4453
4454 if( fi->isClimb == 0 )
4455 fi->hclimb = hhbelowfoot; //hh - y1;
4456 else
4457 fi->hclimb = DOUBLE_MAX(fi->hclimb,hhbelowfoot);
4458 fi->isClimb = 1;
4459 }
4460 }
4461 }
4462 double head = 0.0;
4463 double hhabovehead = hhh - head;
4464 abottom = 0.0;
4465 if( hhabovehead > 0.0 )
4466 {
4467 //printf("H");
4468 /* climbing from undergound */
4469 //if( hhabovehead < fi->climbHeight)
4470 {
4471 //printf("^");
4472 /* CLIMBING */
4473 fi->canFall = 0;
4474
4475 if( fi->isClimb == 0 ){
4476 fi->hclimb = hhabovehead + abottom; //hh - y1;
4477 }else{
4478 fi->hclimb = DOUBLE_MAX(fi->hclimb,hhabovehead + abottom);
4479 }
4480 fi->isClimb = 1;
4481 //printf("hclimb %lf abottom %lf hhabovehead %lf\n",fi->hclimb,abottom,hhabovehead);
4482 }
4483 }
4484 }
4485 }
4486
4487 return hit;
4488}
4489
4490void collide_GeoElevationGrid(struct X3D_GeoElevationGrid *node){
4491 /*
4492 For examine and fly navigation modes, there's no gravity direction.
4493 Specifications say for geo walk navigation mode gravity vector is down along GD ellipsoid vertical
4494 with respect to (wrt) the current GD (geodetic/ellipsoidal latitude, longitude, height)
4495 posistion of the viewpoint, not including the viewpoint's orientation
4496 field.
4497 When you collide in geo walk mode, the avatar collision
4498 volume is aligned to current viewpoint GD vertical.
4499 */
4500
4501 int ihit = -1;
4502 int compatible;
4503 struct X3D_Node *boundvp;
4504
4505 compatible = FALSE;
4506 boundvp = getActiveLayerBoundViewpoint();
4507 if(boundvp && boundvp->_nodeType == NODE_GeoViewpoint){
4508 struct X3D_GeoViewpoint *gvp;
4509 struct Planet *planet = current_planet();;
4510 gvp = (struct X3D_GeoViewpoint *)boundvp;
4511 if(gvp->_prepped_planet == planet->ID) compatible = TRUE;
4512 }
4513 if(node->_nodeType == NODE_GeoElevationGrid && compatible ){
4514 ihit = geoelevationgrid_disp2((struct X3D_GeoElevationGrid*)node, (struct X3D_GeoViewpoint *)boundvp);
4515 //if(ihit==0) printf("0");
4516 //if(ihit==1) printf("1");
4517 //if(ihit==-1) printf(".");
4518 }
4519 if(0) if(ihit == -1){
4520 //above couldn't handle it, thunking to generic
4521 //problem: if its a really dense grid, the framerate plummets
4522 collide_genericfaceset ((struct X3D_IndexedFaceSet *)node );
4523 }
4524
4525}
4526double getTerrainHeight(int planetID, Geosys *geoSystem, struct SFVec3d *gdCoord){
4527 int i,j,nfound;
4528 struct Planet *planet;
4529 double highest;
4530 //find planet
4531 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
4532 highest = 0.0; //this means we can't go below ground. It also means if no GEG found, then we are relative to ellipsoid
4533 if(!p->planet_stack) return highest; //no GEGs registered, stick to absolute height
4534 nfound = 0;
4535 planet = NULL;
4536 for(i=0;i<vectorSize(p->planet_stack);i++){
4537 planet = vector_get_ptr(struct Planet,p->planet_stack,i);
4538 if(planet && planet->ID == planetID) {
4539 if(!planet->gegs) return highest; //no gegs registered
4540 for(j=0;j<vectorSize(planet->gegs);j++){
4541 double gridheight;
4542 struct X3D_GeoElevationGrid *geg = vector_get(struct X3D_GeoElevationGrid*,planet->gegs,j);
4543 if(geg)
4544 if( geoelevationgrid_getGDHeight0(geg,gdCoord,geoSystem,&gridheight) == 1){
4545 //make a list of hits, and pick the highest one, in case there are grid overlays etc.
4546 nfound++;
4547 if(nfound == 1) highest = gridheight;
4548 highest = max(highest,gridheight);
4549 //printf("p %d g %x h %lf\n",planetID,geg,highest);
4550 }
4551 }
4552 }
4553 }
4554 return highest;
4555}
4556
4557void RegisterGeoElevationGrid(struct X3D_Node *node, int planetID){
4558 //call this from render_geoelevationgrid, or collide_?, so we get the planet from
4559 // a) geoSystem "P#"
4560 // b) X3DGeoPlanet.planetID="#" which is pushed and popped, so DEF/USE can put get GEG in different planets
4561 // this implies you can have DEF/USE multiple USEs of GEGs for different planets (but just once per planet?),
4562 // so {planet,geg}
4563 // should be the unique index
4564 if(node && node->_nodeType == NODE_GeoElevationGrid){
4565 int i,j,ifound;
4566 struct Planet *planet;
4567 //add to planet
4568 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
4569 if(!p->planet_stack) p->planet_stack = newStack(struct Planet);
4570 ifound = -1;
4571 planet = NULL;
4572 for(i=0;i<vectorSize(p->planet_stack);i++){
4573 planet = vector_get_ptr(struct Planet,p->planet_stack,i);
4574 if(planet->ID == planetID) {
4575 //right planet
4576 ifound = i;
4577 break;
4578 }
4579 }
4580 if(ifound == -1){
4581 struct Planet newplanet;
4582 memset(&newplanet,0,sizeof(struct Planet));
4583 newplanet.ID = planetID;
4584 //printf("adding planet # %d\n",planetID);
4585 vector_pushBack(struct Planet,p->planet_stack,newplanet);
4586 ifound = p->planet_stack->n -1;
4587 planet = vector_get_ptr(struct Planet,p->planet_stack,ifound);
4588 }
4589 if(planet->gegs == NULL) planet->gegs = newStack(struct X3D_Node*);
4590 //printf("adding GEG %x to planet # %d\n",node,planetID);
4591 vector_pushBack(struct X3D_Node*,planet->gegs,node);
4592 }
4593}
4594void unRegisterGeoElevationGrid(struct X3D_Node *node){
4595 //call this from unRegisterX3DAnyNode, which is called from unload_broto, which is called
4596 // when unloading an Inline (including GeoLOD inlines)
4597 if(node && node->_nodeType == NODE_GeoElevationGrid){
4598 int i,j;
4599 //remove all instances from all planets
4600 ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
4601 if(!p->planet_stack) return;
4602 for(i=0;i<vectorSize(p->planet_stack);i++){
4603 struct Planet *planet = vector_get_ptr(struct Planet,p->planet_stack,i);
4604 if(planet->gegs)
4605 for(j=0;j<vectorSize(planet->gegs);j++){
4606 int k = vectorSize(planet->gegs) - j -1;
4607 struct X3D_Node *geg = vector_get(struct X3D_Node*,planet->gegs,k);
4608 if(geg == node){
4609 vector_set(struct X3D_Node*,planet->gegs,k,NULL);
4610 }
4611 }
4612 }
4613 }
4614
4615}
4616
4617
4618void compile_GeoPlanet(struct X3D_GeoPlanet *node){
4619 {
4620 struct Planet *planet = current_planet();
4621 if(planet == NULL){
4622 planet = add_planet(node->planetId);
4623 //printf("planet=%x\n",planet);
4624 }
4625 }
4626 REINITIALIZE_SORTED_NODES_FIELD(node->children,node->_sortedChildren);
4627 MARK_NODE_COMPILED
4628
4629 /* events */
4630 /* MARK_SFNODE_INOUT_EVENT(node->metadata, node->__oldmetadata, offsetof (struct X3D_GeoLocation, metadata)) */
4631
4632 INITIALIZE_EXTENT;
4633
4634}
4635
4636void prep_GeoPlanet(struct X3D_GeoPlanet *node){
4637 push_planetId(node->planetId);
4638 COMPILE_IF_REQUIRED
4639 if(!renderstate()->render_vp) {
4640 double aoo[4],ao[3];
4641 struct Planet *planet;
4642 //ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
4643
4644 planet = current_planet();
4645 //we need to get the LCS to GC transform on the stack
4646 FW_GL_PUSH_MATRIX();
4647 veccopyd(ao,planet->autoOrigin.c);
4648 veccopy4d(aoo,planet->autoOrient.c);
4649 FW_GL_TRANSLATE_D(ao[0], ao[1], ao[2]);
4650 FW_GL_ROTATE_RADIANS(aoo[3], aoo[0],aoo[1],aoo[2]);
4651
4652
4653 /* did either we or the Viewpoint move since last time? */
4654 RECORD_DISTANCE
4655 if(renderstate()->render_boxes) extent6f_draw(node->_extent);
4656 }
4657
4658}
4659
4660void child_GeoPlanet(struct X3D_GeoPlanet *node){
4661 CHILDREN_COUNT
4662 //LOCAL_LIGHT_SAVE
4663 //INITIALIZE_GEOSPATIAL(node)
4664 COMPILE_IF_REQUIRED
4665// OCCLUSIONTEST
4666 RETURN_FROM_CHILD_IF_NOT_FOR_ME
4667
4668 //LOCAL_LIGHT_CHILDREN(node->children);
4669 prep_sibAffectors((struct X3D_Node*)node,&node->__sibAffectors);
4670
4671 normalChildren(node->children);
4672
4673 fin_sibAffectors((struct X3D_Node*)node,&node->__sibAffectors);
4674}
4675void fin_GeoPlanet(struct X3D_GeoPlanet *node){
4676 //pop LCS to GC transform
4677 COMPILE_IF_REQUIRED
4678 OCCLUSIONTEST
4679
4680 if(!renderstate()->render_vp) {
4681 FW_GL_POP_MATRIX();
4682 } else {
4683 if ((node->_renderFlags & VF_Viewpoint) == VF_Viewpoint) {
4684 double aoo[4],ao[3];
4685 struct Planet *planet;
4686 //ppComponent_Geospatial p = (ppComponent_Geospatial)gglobal()->Component_Geospatial.prv;
4687 planet = current_planet();
4688 veccopyd(ao,planet->autoOrigin.c);
4689 veccopy4d(aoo,planet->autoOrient.c);
4690
4691 FW_GL_ROTATE_RADIANS(-aoo[3], aoo[0],aoo[1],aoo[2]);
4692 FW_GL_TRANSLATE_D(-ao[0], -ao[1], -ao[2]);
4693
4694 }
4695 }
4696 pop_planetId();
4697
4698}
4699
4700//by 'user' coordinates we mean as authored in the scene file and specfied by geosystem by the scene author
4701// when converting from GC to user, we might find the user _is_ GC.
4702// with these functions you don't need to know or care about shortcuts.
4703void user2gc(Geosys * geoSystem, struct SFVec3d *geo, int n, struct SFVec3d *gc){
4704 //UNTESTED
4705 int i;
4706 struct SFVec3d gdCoord;
4707 for(i=0;i<n;i++){
4708 moveCoords3d(geoSystem,NULL,NULL,&geo[i],1,&gc[i],&gdCoord);
4709 }
4710}
4711void gc2user(Geosys * geoSystem, struct SFVec3d *gc, int n, struct SFVec3d *geo){
4712 //UNTESTED
4713 int i;
4714 struct SFVec3d gdCoord;
4715 for(i=0;i<n;i++){
4716 CONVERT_BACK_TO_GD_OR_UTMC(geoSystem,NULL,&gc[i],&gdCoord,&geo[i]);
4717 }
4718}
4719void gc2lcs_transform(struct SFVec3d *translate, struct SFVec4d *rotate){
4720 //converts from GC geocentric, to LCS local coordinate system
4721 //LCS = GC - origin
4722 int i;
4723 struct Planet *planet;
4724 planet = current_planet();
4725 veccopyd(translate->c,planet->autoOrigin.c);
4726 //vecscaled(translate->c,translate->c,-1.0);
4727 vecnegated(translate->c,translate->c);
4728 veccopy4d(rotate->c,planet->autoOrient.c);
4729 rotate->c[3] = -rotate->c[3];
4730}
4731void gc2lcs(Geosys * geoSystem, struct SFVec3d *gc, int n, struct SFVec3d *lcs){
4732 //converts from GC geocentric, to LCS local coordinate system
4733 //LCS = GC - origin
4734
4735 int i;
4736 struct SFVec3d translate;
4737 struct SFVec4d rotate;
4738 Quaternion qup;
4739 gc2lcs_transform(&translate, &rotate);
4740 vrmlrot_to_quaternion(&qup,rotate.c[0],rotate.c[1],rotate.c[2],rotate.c[3]);
4741 for(i=0;i<n;i++){
4742 vecaddd(lcs[i].c,gc[i].c,translate.c);
4743 quaternion_rotationd(lcs[i].c,&qup,lcs[i].c);
4744 //vecprint3db("lcs1",lcs[i].c,"\n");
4745 }
4746
4747}
4748void lcs2gc_transform(struct SFVec4d *rotation, struct SFVec3d *translation){
4749 //converts from local coorinate system to GC geocentric
4750 //GC = LCS + origin
4751 int i;
4752 struct Planet *planet;
4753 planet = current_planet();
4754 veccopy4d(rotation->c,planet->autoOrient.c);
4755 veccopyd(translation->c,planet->autoOrigin.c);
4756}
4757void lcs2gc(Geosys * geoSystem, struct SFVec3d *lcs, int n, struct SFVec3d *gc){
4758 //converts from local coorinate system to GC geocentric
4759 //GC = LCS + origin
4760 int i;
4761 struct SFVec4d rotation;
4762 struct SFVec3d translation;
4763 Quaternion qup;
4764 lcs2gc_transform(&rotation, &translation);
4765 vrmlrot_to_quaternion(&qup,rotation.c[0],rotation.c[1],rotation.c[2],rotation.c[3]);
4766 for(i=0;i<n;i++){
4767 quaternion_rotationd(gc[i].c,&qup,lcs[i].c);
4768 vecaddd(gc[i].c,gc[i].c,translation.c);
4769 }
4770 //vecprint3db("gc1",gc[0].c,"\n");
4771}
4772
4773
4774
4775void gc2tcs_transform(Geosys * geoSystem, struct SFVec3d *gdcenter, struct SFVec3d *translate, struct SFVec4d *rotate){
4776 // GC -> TCS
4777 // convert from GC to node local aligned aka topocentric coordinate system TCS
4778 //TCS = localOrient x (GC - gdcenter)
4779
4780 int i;
4781 double pp[3];
4782 Quaternion qlo;
4783 struct SFVec4d lo;
4784 struct SFVec3d gcCenter;
4785 GeoOrient(NULL,geoSystem,gdcenter,rotate);
4786 rotate->c[3] = -rotate->c[3];
4787 gd2gc(geoSystem,gdcenter,1,translate);
4788 //vecscaled(translate->c,translate->c,-1.0);
4789 vecnegated(translate->c,translate->c);
4790
4791}
4792void tcs2gc_transform(Geosys * geoSystem, struct SFVec3d *gdcenter, struct SFVec4d *rotate, struct SFVec3d *translate){
4793 // TCS -> GC
4794 // convert from node-local-algined aka topocentric coordinate system TCS to GC
4795 // GC = (inverse(localOrient) x TCS) + gdcenter
4796 int i;
4797 double pp[3];
4798 Quaternion qlo;
4799 struct SFVec4d lo;
4800 struct SFVec3d gcCenter;
4801 GeoOrient(NULL,geoSystem,gdcenter,rotate);
4802 gd2gc(geoSystem,gdcenter,1,translate);
4803
4804}
4805void gc2tcs(Geosys * geoSystem, struct SFVec3d *gdcenter, struct SFVec3d *gc, int n, struct SFVec3d *tcs){
4806 // GC -> TCS
4807 // convert from GC to node local aligned aka topocentric coordinate system TCS
4808 //TCS = localOrient x (GC - gdcenter)
4809
4810 int i;
4811 double pp[3];
4812 Quaternion qlo;
4813 struct SFVec4d rotate;
4814 struct SFVec3d translate;
4815 tcs2gc_transform(geoSystem, gdcenter, &rotate, &translate);
4816 vrmlrot_to_quaternion(&qlo,rotate.c[0],rotate.c[1],rotate.c[2], -rotate.c[3]);
4817 for(i=0;i<n;i++){
4818 vecdifd(pp,gc[i].c,translate.c);
4819 quaternion_rotationd(tcs[i].c,&qlo,pp);
4820 }
4821
4822}
4823void tcs2gc(Geosys * geoSystem, struct SFVec3d *gdcenter, struct SFVec3d *tcs, int n, struct SFVec3d *gc){
4824 // TCS -> GC
4825 // convert from node-local-algined aka topocentric coordinate system TCS to GC
4826 // GC = (inverse(localOrient) x TCS) + gdcenter
4827 int i;
4828 double pp[3];
4829 Quaternion qlo;
4830 struct SFVec4d rotate;
4831 struct SFVec3d translate;
4832 tcs2gc_transform(geoSystem, gdcenter, &rotate, &translate);
4833 vrmlrot_to_quaternion(&qlo,rotate.c[0],rotate.c[1],rotate.c[2], rotate.c[3]);
4834 for(i=0;i<n;i++){
4835 quaternion_rotationd(pp,&qlo,tcs[i].c);
4836 vecaddd(gc[i].c,translate.c,pp);
4837 }
4838
4839}
4840
4841void tcs2gcB(Geosys * geoSystem, struct SFVec3d *gccenter, struct SFVec3d *tcs, int n, struct SFVec3d *gc);
4842void gc2tcsB(Geosys * geoSystem, struct SFVec3d *gccenter, struct SFVec3d *gc, int n, struct SFVec3d *tcs);
4843
4844void gc2tcsB(Geosys * geoSystem, struct SFVec3d *gccenter, struct SFVec3d *gc, int n, struct SFVec3d *tcs){
4845 // GC -> TCS
4846 // convert from GC to node local aligned aka topocentric coordinate system TCS
4847 //TCS = localOrient x (GC - gdcenter)
4848
4849 int i;
4850 double pp[3];
4851 Quaternion qlo;
4852 struct SFVec4d rotate;
4853 struct SFVec3d translate;
4854 tcs2gcB_transform(geoSystem, gccenter, &translate, &rotate);
4855 vrmlrot_to_quaternion(&qlo,rotate.c[0],rotate.c[1],rotate.c[2], -rotate.c[3]);
4856 for(i=0;i<n;i++){
4857 vecdifd(pp,gc[i].c,translate.c);
4858 quaternion_rotationd(tcs[i].c,&qlo,pp);
4859 }
4860
4861}
4862void tcs2gcB(Geosys * geoSystem, struct SFVec3d *gccenter, struct SFVec3d *tcs, int n, struct SFVec3d *gc){
4863 // TCS -> GC
4864 // convert from node-local-algined aka topocentric coordinate system TCS to GC
4865 // GC = (inverse(localOrient) x TCS) + gdcenter
4866 int i;
4867 double pp[3];
4868 Quaternion qlo;
4869 struct SFVec4d rotate;
4870 struct SFVec3d translate;
4871 tcs2gcB_transform(geoSystem, gccenter, &translate, &rotate);
4872 vrmlrot_to_quaternion(&qlo,rotate.c[0],rotate.c[1],rotate.c[2], rotate.c[3]);
4873 for(i=0;i<n;i++){
4874 quaternion_rotationd(pp,&qlo,tcs[i].c);
4875 vecaddd(gc[i].c,translate.c,pp);
4876 }
4877
4878}
4879
4880
4881/*
4882//as with geoConvert, its more reliable to go user2gc gc2anything and vice versa, rather
4883// than user2gd. That's because ideally we go geo2geo(source_geosystem,dest_geosystem).
4884// and when we do user2gd its confusing which geosystem we are using for the gd
4885// and are the gd lat first, radians, or are they what the user are?
4886void user2gd(struct Multi_Int32* geoSystem, struct SFVec3d *geo, int n, struct SFVec3d *gd){
4887 //UNTESTED
4888 int i;
4889 struct SFVec3d gcCoord;
4890 for(i=0;i<n;i++)
4891 moveCoords3d(geoSystem,NULL,NULL,&geo[i],1,&gcCoord,&gd[i]);
4892}
4893void gd2user(struct Multi_Int32* geoSystem, struct SFVec3d *gd, int n, struct SFVec3d *geo){
4894 //UNTESTED
4895 int i;
4896 struct SFVec3d gdCoord, gcCoord;
4897 for(i=0;i<n;i++){
4898 Gd_Gc3d(geoSystem,&gd[i],1,&gcCoord);
4899 gc2user(geoSystem,&gcCoord,1,&geo[i]);
4900 }
4901}
4902*/
4903void gd2gc(Geosys * geoSystem, struct SFVec3d *gd, int n, struct SFVec3d *gc){
4904 //UNTESTED
4905 int i;
4906 for(i=0;i<n;i++){
4907 Gd_Gc3d(geoSystem,&gd[i],1,&gc[i]);
4908 }
4909}
4910void gc2gd(Geosys * geoSystem, struct SFVec3d *gc, int n, struct SFVec3d *gd){
4911 //UNTESTED
4912 int i;
4913 for(i=0;i<n;i++){
4914 gccToGdc (geoSystem, &gc[i], &gd[i]);
4915 }
4916}
4917void do_GeoConvert (void *px){
4918 // web3d v3.3 specs missing a converter node - you can route between
4919 // geoNodes, but what if 2 nodes have different geoSystem?
4920 // this geoConvert node solves that, you create 2 of these and chain them:
4921 // myGeoNode1 -> set_geoCoord (geoConvert1) gcCoord_changed -> set_gcCoord (geoConvert2) geoCoord_changed -> myGeoNode2
4922 // where geoConvert1.geoSystem == myGeoNode1.geoSystem
4923 // and geoConvert2.geoSystem == myGeoNode2.geoSystem
4924 //
4925 // If we've done above nodes well, then any scene routing of geoCoords should be
4926 // in so-called 'user coords' -as specified by the scene designer in geoSystem
4927 // for example longitude first, degrees etc.
4928 // That means we shouldn't see any LCS - the Local Coordinate System you get after taking off geoOrigin or AutoOrigin
4929 // we'll just see full geo coords and full gc coords in routing
4930
4931 struct X3D_GeoConvert *node;
4932 node = (struct X3D_GeoConvert *) px;
4933 if (!node) return;
4934 if(node->__geoSystem == NULL)
4935 compile_geoSystem (X3D_NODE(node),node->_nodeType, &node->geoSystem, &node->__geoSystem);
4936
4937 if (!vecsamed(node->__oldgeoCoords.c,node->set_geoCoords.c)) {
4938 struct SFVec3d gdCoord;
4939 //user2gc
4940 moveCoords3d(GEOSYS(node->__geoSystem),NULL,NULL,&node->set_geoCoords,1,&node->gcCoords_changed,&gdCoord);
4941 MARK_EVENT (px, offsetof (struct X3D_GeoConvert, gcCoords_changed));
4942 veccopyd(node->__oldgeoCoords.c,node->set_geoCoords.c);
4943 }
4944 if (!vecsamed(node->__oldgcCoords.c,node->set_gcCoords.c)){
4945 struct SFVec3d gdCoord;
4946 //gc2user
4947 CONVERT_BACK_TO_GD_OR_UTMC(GEOSYS(node->__geoSystem),NULL,&node->set_gcCoords,&gdCoord,&node->geoCoords_changed);
4948 MARK_EVENT (px, offsetof (struct X3D_GeoConvert, geoCoords_changed));
4949 veccopyd(node->__oldgcCoords.c,node->set_gcCoords.c);
4950 }
4951}
Definition Viewer.h:139