Exemplo n.º 1
0
def rotation_matrix_from_vector_alignment(before, after):
    """
    >>> # General input/output test
    >>> V1 = norm(np.random.random(3))
    >>> V2 = norm([1,1,1])
    >>> R = rotation_matrix_from_vector_alignment(V1, V2)
    >>> V3 = transform_direction(V1, R)
    >>> cmp_points(V2, V3)
    True
    >>> # Catch the special case in which we cannot take the cross product
    >>> V1 = [0,0,1]
    >>> V2 = [0,0,-1]
    >>> R = rotation_matrix_from_vector_alignment(V1, V2)
    >>> V3 = transform_direction(V1, R)
    >>> cmp_points(V2, V3)
    True
    """
    # The angle between the vectors must not be 0 or 180 (i.e. so we can take a cross product)
    thedot = np.dot(before, after)
    if cmp_floats(thedot, 1.) == True:
        # Vectors are parallel
        return tf.identity_matrix()
        
    if cmp_floats(thedot, -1.) == True:
        # Vectors are anti-parallel
        print "Vectors are anti-parallel this might crash."
        
    axis = np.cross(before, after)            # get the axis of rotation
    angle = np.arccos(np.dot(before, after))  # get the rotation angle
    return rotation_matrix(angle, axis)
Exemplo n.º 2
0
 def rotate(self, angle, axis):
     self.shape.append_transform(tf.rotation_matrix(angle, axis))
Exemplo n.º 3
0
 def rotate(self, angle, axis):
     self.shape.append_transform(tf.rotation_matrix(angle, axis))
def transform_direction(direction, transform):
    angle, axis, point = tf.rotation_from_matrix(transform)
    rotation_transform = tf.rotation_matrix(angle, axis)
    return np.array(np.dot(rotation_transform, np.matrix(np.concatenate((direction, [1.]))).transpose()).transpose()[0,0:3]).squeeze()
Exemplo n.º 5
0
        #areat = triangle_area_3d ( x[i], y[i], z[i], x[i+1], 
        #  y[i+1], z[i+1], x[n-1], y[n-1], z[n-1] );
        #    
        #area = area + areat;
        #*cx = *cx + areat * ( x[i] + x[i+1] + x[n-1] ) / 3.0;
        #*cy = *cy + areat * ( y[i] + y[i+1] + y[n-1] ) / 3.0;
        #*cz = *cz + areat * ( z[i] + z[i+1] + z[n-1] ) / 3.0;
        #
        #}
        #
        #*cx = *cx / area;
        #*cy = *cy / area;
        #*cz = *cz / area;
        #
        
if __name__ == "__main__":
    import doctest
    #doctest.testmod()
    
    if False:
        # Catch the special case in which we cannot take the cross product
        V1 = [0,0,1]                                                          
        V2 = [0,0,-1]
        #import pdb; pdb.set_trace()                                                      
        R = rotation_matrix_from_vector_alignment(V1, V2)
        R2 = rotation_matrix(np.pi, [1,0,0])
        V3 = transform_direction(V1, R)
        print R2
        print cmp_points(V2, V3)