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