198 | | |
199 | | |
200 | | // normalize X,Y,Z |
201 | | double inverse_length = 1.0/sqrt(X*X + Y*Y + Z*Z); |
202 | | |
203 | | X *= inverse_length; |
204 | | Y *= inverse_length; |
205 | | Z *= inverse_length; |
206 | | |
207 | | double length_XY = sqrt(X*X + Y*Y); |
208 | | double inverse_length_XY = 1.0/length_XY; |
209 | | |
210 | | // Vx = |(-Y,X,0)| |
211 | | localToWorld(0,0) = -Y*inverse_length_XY; |
212 | | localToWorld(0,1) = X*inverse_length_XY; |
213 | | localToWorld(0,2) = 0.0; |
214 | | |
215 | | // Vy = /(-Z*X/(sqrt(X*X+Y*Y), -Z*Y/(sqrt(X*X+Y*Y),sqrt(X*X+Y*Y))| |
216 | | double Vy_x = -Z*X*inverse_length_XY; |
217 | | double Vy_y = -Z*Y*inverse_length_XY; |
218 | | double Vy_z = length_XY; |
219 | | inverse_length = 1.0/sqrt(Vy_x*Vy_x + Vy_y*Vy_y + Vy_z*Vy_z); |
220 | | localToWorld(1,0) = Vy_x*inverse_length; |
221 | | localToWorld(1,1) = Vy_y*inverse_length; |
222 | | localToWorld(1,2) = Vy_z*inverse_length; |
223 | | |
224 | | // Vz = (X,Y,Z) |
225 | | localToWorld(2,0) = X; |
226 | | localToWorld(2,1) = Y; |
227 | | localToWorld(2,2) = Z; |
| 205 | computeCoordinateFrame(latitude, longitude, localToWorld); |
| 206 | } |
| 207 | |
| 208 | inline void EllipsoidModel::computeCoordinateFrame(double latitude, double longitude, osg::Matrixd& localToWorld) const |
| 209 | { |
| 210 | // Compute up vector |
| 211 | osg::Vec3d up ( cos(longitude)*cos(latitude), sin(longitude)*cos(latitude), sin(latitude)); |
| 212 | |
| 213 | // Compute east vector |
| 214 | osg::Vec3d east (-sin(longitude), cos(longitude), 0); |
| 215 | |
| 216 | // Compute north vector = outer product up x east |
| 217 | osg::Vec3d north = up ^ east; |
| 218 | |
| 219 | // set matrix |
| 220 | localToWorld(0,0) = east[0]; |
| 221 | localToWorld(0,1) = east[1]; |
| 222 | localToWorld(0,2) = east[2]; |
| 223 | |
| 224 | localToWorld(1,0) = north[0]; |
| 225 | localToWorld(1,1) = north[1]; |
| 226 | localToWorld(1,2) = north[2]; |
| 227 | |
| 228 | localToWorld(2,0) = up[0]; |
| 229 | localToWorld(2,1) = up[1]; |
| 230 | localToWorld(2,2) = up[2]; |
232 | | osg::Vec3d normal(X,Y,Z); |
233 | | normal.normalize(); |
234 | | return normal; |
| 235 | // Note latitude is angle between normal to ellipsoid surface and XY-plane |
| 236 | double latitude; |
| 237 | double longitude; |
| 238 | double altitude; |
| 239 | convertXYZToLatLongHeight(X,Y,Z,latitude,longitude,altitude); |
| 240 | |
| 241 | // Compute up vector |
| 242 | return osg::Vec3d( cos(longitude) * cos(latitude), |
| 243 | sin(longitude) * cos(latitude), |
| 244 | sin(latitude)); |