1 /**************************************************************************\
2 * Copyright (c) Kongsberg Oil & Gas Technologies AS
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are
7 * met:
8 *
9 * Redistributions of source code must retain the above copyright notice,
10 * this list of conditions and the following disclaimer.
11 *
12 * Redistributions in binary form must reproduce the above copyright
13 * notice, this list of conditions and the following disclaimer in the
14 * documentation and/or other materials provided with the distribution.
15 *
16 * Neither the name of the copyright holder nor the names of its
17 * contributors may be used to endorse or promote products derived from
18 * this software without specific prior written permission.
19 *
20 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
23 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
24 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
25 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
26 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
27 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
28 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
29 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
30 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 \**************************************************************************/
32
33 /*!
34 \class SoGeo Inventor/misc/SoGeo.h
35 \brief The SoGeo class is used to initialize the geo nodes in Coin, and has some utility geo coordinate functions.
36
37
38 */
39
40 #include <Inventor/misc/SoGeo.h>
41 #include "coindefs.h"
42 #include <Inventor/nodes/SoGeoOrigin.h>
43 #include <Inventor/nodes/SoGeoLocation.h>
44 #include <Inventor/nodes/SoGeoSeparator.h>
45 #include <Inventor/nodes/SoGeoCoordinate.h>
46 #include <Inventor/elements/SoGeoElement.h>
47 #include <Inventor/SbString.h>
48 #include <Inventor/SbVec3f.h>
49 #include <Inventor/SbVec3d.h>
50 #include <Inventor/SbMatrix.h>
51 #include <Inventor/SbDPMatrix.h>
52 #include <Inventor/errors/SoDebugError.h>
53
54 #include "SbGeoProjection.h"
55 #include "SbUTMProjection.h"
56 #include "SbGeoAngle.h"
57 #include "SbGeoEllipsoid.h"
58
59 #include <cassert>
60 #include <cstdio>
61 #include <cstdlib>
62 #include <cmath>
63
64 void
init(void)65 SoGeo::init(void)
66 {
67 SoGeoElement::initClass();
68 SoGeoOrigin::initClass();
69 SoGeoLocation::initClass();
70 SoGeoSeparator::initClass();
71 SoGeoCoordinate::initClass();
72 }
73
74 // return zone number stored in string, or 0 if parsing failed
find_utm_zone(const SbString & s)75 static int find_utm_zone(const SbString & s)
76 {
77 if (s.getLength() < 2) return 0;
78 if (s[0] != 'Z' && s[1] != 'z') return 0;
79
80 return (int) atoi(s.getString()+1);
81 }
82
83 SbVec3d
toGD(const SbString * originsystem,const int numoriginsys,const SbVec3d & origincoords)84 SoGeo::toGD(const SbString * originsystem,
85 const int numoriginsys,
86 const SbVec3d & origincoords)
87 {
88 assert(0 && "not implemented yet");
89 return SbVec3d(0.0, 0.0, 0.0);
90 // to convert from GC to GD see this article
91 // http://en.wikipedia.org/wiki/Geodetic_system
92
93 // SbUTMProjection already supports from UTM to GD
94 }
95
96 SbVec3d
fromGD(const SbVec3d & gd,const SbString * tosystem,const int numtosys)97 SoGeo::fromGD(const SbVec3d & gd,
98 const SbString * tosystem,
99 const int numtosys)
100 {
101 // to convert from GD to GC see this article
102 // http://en.wikipedia.org/wiki/Geodetic_system
103 assert(0 && "not implemented yet");
104 return SbVec3d(0.0, 0.0, 0.0);
105 }
106
find_coordinate_system(const SbString * system,const int COIN_UNUSED_ARG (numsys),const SbVec3d & coords)107 static SbDPMatrix find_coordinate_system(const SbString * system,
108 const int COIN_UNUSED_ARG(numsys),
109 const SbVec3d & coords)
110 {
111 SbVec3d p;
112 if (system[0] == "GC") {
113 p = coords;
114 }
115 else {
116 double latitude, longitude, elev;
117
118 if (system[0] == "UTM") {
119 SbUTMProjection proj(find_utm_zone(system[1]), SbGeoEllipsoid("WGS84"));
120 SbGeoAngle lat, lng;
121
122 proj.unproject(coords[0], coords[1], &lat, &lng);
123
124 latitude = lat.rad();
125 longitude = lng.rad();
126 elev = coords[2];
127 }
128 else if (system[0] == "GD") {
129 latitude = coords[0] * M_PI / 180.0;
130 longitude = coords[1] * M_PI / 180.0;
131 elev = coords[2];
132 }
133 else {
134 assert(0 && "not supported");
135 latitude = longitude = elev = 0.0;
136 }
137
138 // formula based on http://en.wikipedia.org/wiki/Geodetic_system
139
140 double a = 6378137.0; // earth semimajor axis in meters
141 double f = 1.0/298.257223563; // reciprocal flattening
142 double e2 = 2*f - f*f; // eccentricity squared
143
144 double sinlat = sin(latitude);
145 double coslat = cos(latitude);
146 double chi = sqrt(1.0 - e2 * (sinlat*sinlat));
147
148
149 p[0] = (a / chi + elev) * coslat * cos(longitude);
150 p[1] = (a / chi + elev) * coslat * sin(longitude);
151 p[2] = (a * (1.0-e2)/ chi + elev) * sinlat;
152
153
154 #if 0 // for debugging
155 SbUTMProjection utm(17, SbGeoEllipsoid("WGS84"));
156 double east, north;
157 SbGeoAngle lat(latitude);
158 SbGeoAngle lng(longitude);
159
160 utm.project(lat,lng, &east, &north);
161
162 fprintf(stderr,"zone 17 coords: %g %g\n",
163 east, north);
164 #endif // debugging
165
166 }
167
168 SbVec3d Z = p;
169 (void) Z.normalize();
170
171 // FIXME: handle the case when origin is at the north or south pole
172 SbVec3d Y(0.0, 0.0, 1.0);
173 SbVec3d X = Y.cross(Z);
174 (void) X.normalize();
175 Y = Z.cross(X);
176 (void) Y.normalize();
177
178 SbDPMatrix m = SbDPMatrix::identity();
179
180 for (int i = 0; i < 3; i++) {
181 m[0][i] = X[i];
182 m[1][i] = Y[i];
183 m[2][i] = Z[i];
184 m[3][i] = p[i];
185 }
186
187 #if 0 // for debugging
188 SbGeoAngle lat(latitude);
189 SbGeoAngle lng(longitude);
190
191 fprintf(stderr,"coordinate system matrix: (%f %f) (%d %d %.2f, %d %d %.2f\n",
192 latitude*180/M_PI, longitude*180.0/M_PI,
193 lat.deg(), lat.minutes(), lat.seconds(),
194 lng.deg(), lng.minutes(), lng.seconds());
195 m.print(stderr);
196 fprintf(stderr,"\n");
197 #endif // debugging
198
199 return m;
200 }
201
202 //
203 // Currently not used. Kept here since it might be useful to find and
204 // UTM zone from lat/long
205 //
find_utm_projection(const SbString * system,const int COIN_UNUSED_ARG (numsystem),const SbVec3d & coords,SbVec3d & projcoords)206 static SbUTMProjection find_utm_projection(const SbString * system,
207 const int COIN_UNUSED_ARG(numsystem),
208 const SbVec3d & coords,
209 SbVec3d & projcoords)
210 {
211 if (system[0] != "UTM") { // find an UTM zone to project into
212 assert(system[0] == "GD");
213 // project to an UTM zone
214
215 double degree = coords[1];
216 int zone = int ((degree + 180.0) / 6.0);
217
218 SbGeoAngle lat(coords[0], 0.0, 0.0, 'N');
219 SbGeoAngle lng(coords[1], 0.0, 0.0, 'N');
220
221 SbUTMProjection proj(zone, SbGeoEllipsoid("WGS84"));
222
223 double east, north;
224 proj.project(lat, lng, &east, &north);
225
226 projcoords[0] = east;
227 projcoords[1] = north;
228 projcoords[2] = coords[2];
229
230 return proj;
231 }
232
233 // just return the original UTM Zone and coords
234 projcoords = coords;
235 return SbUTMProjection(find_utm_zone(system[1]), SbGeoEllipsoid("WGS84"));
236 }
237
238 SbDPMatrix
calculateDPTransform(const SbString * originsystem,const int numoriginsys,const SbVec3d & geocoords,const SbString * localsystem,const int numlocalsys,const SbVec3d & localcoords)239 SoGeo::calculateDPTransform(const SbString * originsystem,
240 const int numoriginsys,
241 const SbVec3d & geocoords,
242 const SbString * localsystem,
243 const int numlocalsys,
244 const SbVec3d & localcoords)
245 {
246 // start on 2; the first index is always the projection type, and if UTM the second should always be a zone
247 for (int i = 2; i < numoriginsys; i++) {
248 if (originsystem[i] == "FLAT") {
249 SbMatrix m;
250 m.makeIdentity();
251 bool valid = (originsystem[0] == "UTM" &&
252 localsystem[0] == originsystem[0] &&
253 localsystem[1] == originsystem[1]);
254 if (!valid){
255 SoDebugError::post("SoGeo::calculateTransform", "FLAT projections only supported within the same UTM zone");
256 return m;
257 }
258 m.setTranslate(SbVec3f(localcoords - geocoords));
259 return m;
260 }
261 }
262
263 SbDPMatrix om = find_coordinate_system(originsystem, numoriginsys, geocoords);
264 SbDPMatrix lm = find_coordinate_system(localsystem, numlocalsys, localcoords);
265 return lm * om.inverse();
266 }
267
268 SbMatrix
calculateTransform(const SbString * originsystem,const int numoriginsys,const SbVec3d & geocoords,const SbString * localsystem,const int numlocalsys,const SbVec3d & localcoords)269 SoGeo::calculateTransform(const SbString * originsystem,
270 const int numoriginsys,
271 const SbVec3d & geocoords,
272 const SbString * localsystem,
273 const int numlocalsys,
274 const SbVec3d & localcoords)
275 {
276 SbDPMatrix r = SoGeo::calculateDPTransform(originsystem, numoriginsys, geocoords,
277 localsystem, numlocalsys, localcoords);
278 // transform to a single precision matrix.
279 return SbMatrix(static_cast<float>(r[0][0]), static_cast<float>(r[0][1]),
280 static_cast<float>(r[0][2]), static_cast<float>(r[0][3]),
281 static_cast<float>(r[1][0]), static_cast<float>(r[1][1]),
282 static_cast<float>(r[1][2]), static_cast<float>(r[1][3]),
283 static_cast<float>(r[2][0]), static_cast<float>(r[2][1]),
284 static_cast<float>(r[2][2]), static_cast<float>(r[2][3]),
285 static_cast<float>(r[3][0]), static_cast<float>(r[3][1]),
286 static_cast<float>(r[3][2]), static_cast<float>(r[3][3]));
287 }
288