Instantly share code, notes, and snippets.

# mhaberler/CesiumCZMLOrientation.py

Forked from rothnic/CesiumCZMLOrientation.py
Created June 24, 2021 09:58
Show Gist options
• Save mhaberler/4c4bc1b3ab5c570a64204eb7a163c5d7 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters. Learn more about bidirectional Unicode characters
 # Intended to convert a typical radar lla and az el to a cesium quaternion orientation for CZML # Uses numpy, transformations.py (http://www.lfd.uci.edu/~gohlke/code/transformations.py.html), # and ecef.py (https://code.google.com/p/pysatel/source/browse/trunk/coord.py?r=22) def azEl2Quaternion(lat, lon, alt, az, el): rotZ = rotation_matrix(math.radians(180+az), [0,0,1]) rotY = rotation_matrix(math.radians(-(90+el)), [0,1,0]) rotM = np.dot(rotZ, rotY) origin = geodetic2ecef(lat, lon, alt) origin = np.multiply(origin, 1000) locTransform = northEastDownToFixedFrame(origin) transMatrix = np.dot(locTransform, rotM) tempQ = quaternion_from_matrix(transMatrix) return [tempQ[1], tempQ[2], tempQ[3], tempQ[0]] radar1 = azEl2Quaternion(42.3, -88.5, 0, -110, 20) # Output: [-0.56863161832163933, -0.42476180301542149, 0.69877439159571153, 0.089161892050613742] radar2 = azEl2Quaternion(36, -85, 0, -65, 20) # Output(wrong): [-0.40704469036283503, -0.27047076453169133, 0.78617746720121096, 0.3782660117511954] # Correct: [0.40704469036283503, 0.27047076453169133, -0.78617746720121096, 0.3782660117511954]
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters. Learn more about bidirectional Unicode characters
 # Partial implementation of Cesium's northEastDownToFixedFrame method from Javascript to Python def northEastDownToFixedFrame(origin): # # if (CesiumMath.equalsEpsilon(origin.x, 0.0, CesiumMath.EPSILON14) && # CesiumMath.equalsEpsilon(origin.y, 0.0, CesiumMath.EPSILON14)) { # // The poles are special cases. If x and y are zero, assume origin is at a pole. # var sign = CesiumMath.sign(origin.z); # if (!defined(result)) { # return new Matrix4( # -sign, 0.0, 0.0, origin.x, # 0.0, 1.0, 0.0, origin.y, # 0.0, 0.0, -sign, origin.z, # 0.0, 0.0, 0.0, 1.0); # } # result[0] = -sign; # result[1] = 0.0; # result[2] = 0.0; # result[3] = 0.0; # result[4] = 0.0; # result[5] = 1.0; # result[6] = 0.0; # result[7] = 0.0; # result[8] = 0.0; # result[9] = 0.0; # result[10] = -sign; # result[11] = 0.0; # result[12] = origin.x; # result[13] = origin.y; # result[14] = origin.z; # result[15] = 1.0; # return result; # } # ellipsoid = np.array([6378137.0, 6378137.0, 6356752.3142451793]) tangent = np.array([0,0,0]) tangent[0] = -origin[1] tangent[1] = origin[0] tangent[2] = 0.0 nnorm = np.array([(1/ellipsoid[0]**2), (1/ellipsoid[1]**2), (1/ellipsoid[2]**2)]) nnorm = np.mat(nnorm) normal = np.multiply(origin, nnorm) normal = normal/LA.norm(normal) tangent = tangent/LA.norm(tangent, 3) bitangent = np.cross(normal, tangent) normal = np.asarray(normal[0]) normal = np.asarray(normal[0]) bitangent = np.asarray(bitangent[0]) # if (!defined(result)) { # return new Matrix4( # bitangent.x, tangent.x, -normal.x, origin.x, # bitangent.y, tangent.y, -normal.y, origin.y, # bitangent.z, tangent.z, -normal.z, origin.z, # 0.0, 0.0, 0.0, 1.0); # } return np.array([[bitangent[0], tangent[0], -normal[0], origin[0]], [bitangent[1], tangent[1], -normal[1], origin[1]], [bitangent[2], tangent[2], -normal[2], origin[2]], [0, 0, 0, 1]])