Merge 90eb29a into 2b2cc5f

transforms3d/quaternions.py

@@ -304,10 +304,105 @@ def qinverse(q):
     return qconjugate(q) / qnorm(q)
 
 
-def qeye():
+def qeye(dtype = np.float):
     ''' Return identity quaternion '''
-    return np.array([1.0,0,0,0])
+    return np.array([1.0,0,0,0], dtype = dtype)
 
+def qexp(q):
+    ''' Return exponential of quaternion
+    Parameters
+    ----------
+    q : 4 element sequence
+       w, i, j, k of quaternion
+
+    Returns
+    -------
+    exp(q) : the quaternion exponential
+    
+    Notes
+    -----
+    See: https://en.wikipedia.org/wiki/Quaternion#Exponential,_logarithm,_and_power
+    '''
+    q = np.array(q) #to ensure there is a dtype
+    w, v = q[0], q[1:]
+    norm = np.sqrt(np.dot(v, v))
+    result = np.zeros((4,), q.dtype)
+
+    if norm == 0.:
+        return qeye(q.dtype)
+
+    result[0] =  np.cos(norm)
+    result[1:] = np.sin(norm)/norm * v
+    return result * np.exp(w)
+
+def qlog(q):
+    ''' Return natual logarithm of quaternion
+    Parameters
+    ----------
+    q : 4 element sequence
+       w, i, j, k of quaternion
+
+    Returns
+    -------
+    log(q) : natual logarithm of quaternion
+    
+    Notes
+    -----
+    See: https://en.wikipedia.org/wiki/Quaternion#Exponential,_logarithm,_and_power
+    '''
+    q = np.array(q) #to ensure there is a dtype
+    qnorm_ = qnorm(q)
+    if qnorm == 0.:
+        return qeye(q.dtype)
+
+    w, v = q[0], q[1:]
+    vnorm = np.sqrt(np.dot(v, v))
+    result = np.zeros((4,), q.dtype)
+
+    if vnorm == 0.:
+        return qeye(q.dtype)
+
+    result[0] =  np.log(qnorm_)
+    result[1:] = v/vnorm * np.arccos(w/qnorm_)
+    return result
+
+def qpow(q, n):
+    ''' Return the power of quaternion
+    Parameters
+    ----------
+    q : 4 element sequence
+       w, i, j, k of quaternion
+    n : a real number
+    
+    Returns
+    -------
+    pow(q,n) : the quaternion nth power, 
+    
+    Notes
+    -----
+    See: https://en.wikipedia.org/wiki/Quaternion#Exponential,_logarithm,_and_power
+    '''
+    q = np.array(q) #to ensure there is a dtype
+    qnorm_ = qnorm(q)
+
+    if qnorm == 0.:
+        return qeye(q.dtype)
+
+    w, v = q[0], q[1:]
+
+    nnorm = np.sqrt(np.dot(v, v))
+    result = np.zeros((4,), q.dtype)
+
+    if nnorm == 0.:
+        return qeye(q.dtype)
+
+    theta = np.arccos(w/qnorm_)
+    n_hat = v/nnorm
+
+
+    result[0] = np.cos(n*theta)
+    result[1:] = n_hat * np.sin(n*theta)
+    return result *  np.power(qnorm_, n)
 
 def rotate_vector(v, q):
     ''' Apply transformation in quaternion `q` to vector `v`

transforms3d/tests/test_quaternions.py

@@ -104,6 +104,27 @@ def test_qeye():
     assert np.all([1,0,0,0]==qi)
     assert np.allclose(tq.quat2mat(qi), np.eye(3))
 
+def test_qexp():
+    angular_velocity_pure_quaterion = np.array([0., math.pi, 0, 0])
+    dt = 1.0
+    q_integrate_angular_vel = tq.qexp(angular_velocity_pure_quaterion * dt/2)
+    #see https://www.ashwinnarayan.com/post/how-to-integrate-quaternions/ near the end. 
+    #The formula q(t) = qexp(q_w * t / 2), where q_w is [0 w_x, w_y, w_z] represents angular velocity in x,y,z, 
+    #produces a quaternion that represents the integration of angular velocity w during time t  
+    #so this test rotate the y vector [0 1 0], at math.pi ras/s around the x axis for 1 sec. This is the main use case for using qexp
+    assert np.allclose(tq.rotate_vector(np.array([0,1,0]), q_integrate_angular_vel), np.array([0,-1,0]))
+
+    # from https://www.mathworks.com/help/aerotbx/ug/quatexp.html
+    assert np.allclose(tq.qexp(np.array([0, 0, 0.7854, 0])), np.array([0.7071, 0., 0.7071, 0.]), atol=1e-05)
+
+def test_qlog():
+    #from https://www.mathworks.com/help/aerotbx/ug/quatlog.html?s_tid=doc_ta
+    assert np.allclose(tq.qlog(np.array([0.7071, 0, 0.7071, 0])), np.array([0., 0., 0.7854, 0.]), atol=1e-05)
+
+
+def test_qpow():
+    #https://www.mathworks.com/help/aerotbx/ug/quatpower.html?searchHighlight=quaternion%20power&s_tid=doc_srchtitle
+    assert np.allclose(tq.qpow(np.array([0.7071, 0, 0.7071, 0]), 2), np.array([0, 0, 1, 0]), atol=1e-05)   
 
 def test_qnorm():
     qi = tq.qeye()