Skip to content

Instantly share code, notes, and snippets.

@mhaberler
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.
Save mhaberler/4c4bc1b3ab5c570a64204eb7a163c5d7 to your computer and use it in GitHub Desktop.
# 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]
# 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]])
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment