U
    Uf@                     @   sR   d Z ddlZdd Zdd Zdd Zd	d
 Zdd Zdd Zdd Z	dddZ
dS )z~
! left handed coordinate, z-up, y-forward
! left to right rotation matrix multiply: v'=vR
! non-standard quaternion multiply
    Nc                 C   s   | t tj| d S )Ng-q=)maxnplinalgnorm)x r   :/home/daniela/catkin_ws/src/hpe/src/bvh_skeleton/math3d.py	normalize
   s    r	   c           
      C   s   |dkst | ||d}dddg}||d }||d }||d }t||d  ||d < tt|||d d	   |||d d	   ||d < tt|||d d	   |||d d	   ||d < t|d |d |d g}	|	S )
N)ZyzxZyxzZxyzxzyzxyzyx)r   yzr   r   r   r            )AssertionErrorindexr	   r   crossasarray)
x_diry_dirz_dirorderaxisnameZidx0Zidx1Zidx2dcmr   r   r   dcm_from_axis   s"    
  r   c                 C   sp  t dg}t | }|dkrt |d }d| |d< | d | d  d|  |d< | d	 | d
  d|  |d< | d | d  d|  |d< nt | }|d |d krH|d |d krHt |d |d  |d  d }d| |d< |dkrd| }| d	 | d
  | |d< | d | d  | |d< | d | d  | |d< n$|d |d krt |d |d  |d  d }d| |d< |dkrd| }| d | d  | |d< | d	 | d
  | |d< | d | d  | |d< nt |d |d  |d  d }d| |d< |dkr$d| }| d | d  | |d< | d | d  | |d< | d	 | d
  | |d< |S )N   r   g      ?g      ?)r   r   )r   r   g       @r   )r   r   )r   r   r   )r   r   )r   r   r   )r   zerostracesqrtdiag)r   qtrZsqtrp1dZsqdip1r   r   r   dcm2quat$   sB    
 
$""
"
r&   c                 C   s   | j }t| ddg} t|ddg}| d d df | d d df | d d df | d d df f\}}}}|d d df |d d df |d d df |d d df f\}}}	}
|| ||  ||	  ||
  }tj|dd}t|ddg}t||S Nr   r   r   r   r   r   )shaper   reshapeexpand_dimstile)q0q1original_shapeZw0x0Zy0Zz0Zw1x1y1z1	q_productr   r   r   quat_dotT   s    DD r6   c                 C   s   | j }t| ddg} | d d df | d d df  | d d df  | d d df  g}tj|dd}t|t||}t||S r'   )r*   r   r+   stackdivider6   )r#   r0   Zq_conjZq_invr   r   r   quat_inverseb   s    Br9   c           	      C   sx  | j }t|dddg}t| dddg} t|| }|d d ddf |d d ddf  |d d ddf  |d d ddf  }|d d ddf |d d ddf  |d d ddf  |d d ddf  }|d d ddf |d d ddf  |d d ddf  |d d ddf  }|d d ddf |d d ddf  |d d ddf  |d d ddf  }tj||||gdd}t||S )Nr(   r   r   r   r   r   r)   )r*   r   r+   matmulr7   )	r.   r/   r0   Ztermswr   r   r   r5   r   r   r   quat_mulm   s    HHHHr<   c                 C   s   t t|| S )N)r<   r9   )r#   rr   r   r   quat_divide{   s    r>   r   :0yE>c                 C   s"  t | j}d|d< t| ddg} | d d df }| d d df }| d d df }| d d df }|dkrttd|| ||   d| d| }td|| ||   dd|| ||    }	td|| ||   dd|| ||    }
tj|
||	gdd}ntd	t||S )
Nr   r(   r   r   r   r   r   r)   zNot implemented)	listr*   r   r+   arcsincliparctan2r7   
ValueError)r#   r   epsr0   r.   r/   Zq2Zq3r   r   r   eulerr   r   r   
quat2euler   s    

,00rG   )r   r?   )__doc__numpyr   r	   r   r&   r6   r9   r<   r>   rG   r   r   r   r   <module>   s   0