ó
œkõWc           @   s.   d  d l  Z d  d l Z d „  Z d „  Z d S(   iÿÿÿÿNc         C   sT   |  t  j j |  ƒ } | t  j j | ƒ } t  j t  j t  j | | ƒ d d ƒ ƒ S(   s×  
    Note: this function borrowed from David Wolever (http://stackoverflow.com/questions/2827393/angles-between-two-n-dimensional-vectors-in-python/13849249#13849249)
    Returns the angle in radians between vectors 'v1' and 'v2'::

            >>> angle_between((1, 0, 0), (0, 1, 0))
            1.5707963267948966
            >>> angle_between((1, 0, 0), (1, 0, 0))
            0.0
            >>> angle_between((1, 0, 0), (-1, 0, 0))
            3.141592653589793
    g      ð¿g      ð?(   t   npt   linalgt   normt   arccost   clipt   dot(   t   v1t   v2t   v1_ut   v2_u(    (    sF   /home/andrew/catkin_ws/src/landmark_localizer/scripts/geometryUtils.pyt   angle_between   s    c         C   sD   t  j t j |  ƒ t j |  ƒ g t j |  ƒ t j |  ƒ g g ƒ S(   s>   Uses the math cos and sin functions for improved efficiency(?)(   R    t   arrayt   matht   cost   sin(   t   yaw(    (    sF   /home/andrew/catkin_ws/src/landmark_localizer/scripts/geometryUtils.pyt   get2dRotmatFromYaw   s    "(   t   numpyR    R   R
   R   (    (    (    sF   /home/andrew/catkin_ws/src/landmark_localizer/scripts/geometryUtils.pyt   <module>   s   	