U
    tc@                     @   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/video2bvh/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yxz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x0y0z0w1x1y1z1	q_productr   r   r   quat_dotT   s    DD r:   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divider:   )r$   r1   Zq_conjZq_invr   r   r   quat_inverseb   s    Br=   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,   matmulr;   )	r/   r0   r1   termswr   r   r   r9   r   r   r   quat_mulm   s    HHHHrA   c                 C   s   t t|| S )N)rA   r=   )r$   rr   r   r   quat_divide{   s    rC   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arctan2r;   
ValueError)r$   r   epsr1   r/   r0   q2q3r   r   r   eulerr   r   r   
quat2euler   s    

,00rN   )r   rD   )__doc__numpyr   r	   r   r'   r:   r=   rA   rC   rN   r   r   r   r   <module>   s   0