import math
[docs]
def normalize(v, tolerance=0.00001):
mag2 = sum(n * n for n in v)
assert not math.isclose(mag2, 0) # the magnitude of vector cannot be zero.
if abs(mag2 - 1.0) > tolerance:
mag = math.sqrt(mag2)
v = tuple(n / mag for n in v)
return v
[docs]
def quat_mult(q1, q2):
w1, x1, y1, z1 = q1
w2, x2, y2, z2 = q2
w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
return w, x, y, z
[docs]
def quat_conjugate(q):
q = normalize(q)
w, x, y, z = q
return (w, -x, -y, -z)
[docs]
def quat_vec_mult(q1, v1):
v1 = normalize(v1)
q2 = (0.0,) + v1
return quat_mult(quat_mult(q1, q2), quat_conjugate(q1))[1:]
[docs]
def axis_angle_to_quat(v, theta):
v = normalize(v)
x, y, z = v
theta /= 2
w = math.cos(theta)
x = x * math.sin(theta)
y = y * math.sin(theta)
z = z * math.sin(theta)
return w, x, y, z
[docs]
def quat_to_axis_angle(quat):
w, v = quat[0], quat[1:]
theta = math.acos(w) * 2.0
return normalize(v), theta
[docs]
def lat_lon_to_cart(lat, lon):
x = math.cos(lat) * math.cos(lon)
y = math.cos(lat) * math.sin(lon)
z = math.sin(lat)
return x, y, z
[docs]
def cart_to_lat_lon(x, y, z):
lat = math.asin(z)
lon = math.atan2(y, x)
return lat, lon
[docs]
def test_rotate(point, axis, angle):
"""test rotate point
:param point: point to be rotated (in lat,lon)
:param axis: rotation axis (in lat,lon)
:param angle: roation angle (in degrees)
:returns: new coordinates of the point (in lat,lon)
"""
v = lat_lon_to_cart(math.radians(point[0]), math.radians(point[1]))
axis = lat_lon_to_cart(math.radians(axis[0]), math.radians(axis[1]))
quat = axis_angle_to_quat(axis, math.radians(angle))
ret = quat_vec_mult(quat, v)
ret_lat_lon = cart_to_lat_lon(ret[0], ret[1], ret[2])
return math.degrees(ret_lat_lon[0]), math.degrees(ret_lat_lon[1])
if __name__ == "__main__":
print(test_rotate([12, 134], [90, 0], 34))