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)
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()
#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)