import math
import numpy as np
[docs]
def EncodeIDAsColor(instance_id: int):
"""
Encode the object id to a color.
Args:
instance_id: Int, the id of the object.
Return:
List: The encoded color in [r, g, b, 255] format.
"""
r = (instance_id * 16807 + 187) % 256
g = (instance_id * 48271 + 79) % 256
b = (instance_id * 95849 + 233) % 256
return [r, g, b, 255]
[docs]
def UnityEulerToQuaternion(eular: list) -> list:
"""
Transform euler angle to quaternion in Unity.
Args:
eular: List of length 3, representing euler angle in [x, y, z]
order and measured in degree.
Return:
List: The transformed quaternion in [x, y, z, w] format.
"""
xx = math.radians(eular[0])
yy = math.radians(eular[1])
zz = math.radians(eular[2])
x = math.cos(yy / 2) * math.sin(xx / 2) * math.cos(zz / 2) + math.sin(
yy / 2
) * math.cos(xx / 2) * math.sin(zz / 2)
y = math.sin(yy / 2) * math.cos(xx / 2) * math.cos(zz / 2) - math.cos(
yy / 2
) * math.sin(xx / 2) * math.sin(zz / 2)
z = math.cos(yy / 2) * math.cos(xx / 2) * math.sin(zz / 2) - math.sin(
yy / 2
) * math.sin(xx / 2) * math.cos(zz / 2)
w = math.cos(yy / 2) * math.cos(xx / 2) * math.cos(zz / 2) + math.sin(
yy / 2
) * math.sin(xx / 2) * math.sin(zz / 2)
return [x, y, z, w]
[docs]
def UnityQuaternionToEular(quaternion: list) -> list:
"""
Transform quaternion to euler angle in Unity.
Args:
quaternion: List of length 4, representing quaternion in [x, y, z, w]
order.
Return:
List: The transformed euler angle in [x, y, z] order and measured in degree.
"""
xx = quaternion[0]
yy = quaternion[1]
zz = quaternion[2]
ww = quaternion[3]
x = math.asin(2 * ww * xx - 2 * yy * zz)
y = math.atan2(2 * ww * yy + 2 * xx * zz, 1 - 2 * xx * xx - 2 * yy * yy)
z = math.atan2(2 * ww * zz + 2 * xx * yy, 1 - 2 * xx * xx - 2 * zz * zz)
x = math.degrees(x)
y = math.degrees(y)
z = math.degrees(z)
return [x, y, z]
[docs]
def CheckKwargs(kwargs: dict, compulsory_params: list):
legal = True
for param in compulsory_params:
if param not in kwargs.keys():
legal = False
assert legal, "Parameters illegal, parameter <%s> missing." % param
[docs]
def GetMatrix(quat=[0,0,0,1]) -> np.ndarray:
"""
Transform the position and quaternion into a transformation matrix.
Args:
quat: List of length 4, representing the [x, y, z, w] quaternion.
Return:
numpy.ndarray: the transformation matrix.
"""
q = quat.copy()
n = np.dot(q, q)
if n < np.finfo(q.dtype).eps:
return np.identity(4)
q = q * np.sqrt(2.0 / n)
q = np.outer(q, q)
matrix = np.array([
[1.0 - q[1, 1] - q[2, 2], -(q[2, 3] - q[1, 0]), q[1, 3] + q[2, 0]],
[q[2, 3] + q[1, 0], -(1.0 - q[1, 1] - q[3, 3]), q[1, 2] - q[3, 0]],
[-(q[1, 3] - q[2, 0]), q[1, 2] + q[3, 0], -(1.0 - q[2, 2] - q[3, 3])],
[0, 0, 0]],
dtype=float)
return matrix