''' library for 3D operations such as rotations. ''' import numpy, pickle from numpy import pi, sin, cos, arctan2, arcsin, arccos, dot, linspace, mean from numpy.linalg import norm dotProduct = numpy.dot crossProduct = numpy.cross def arcsin2( v, allowableNumericalError=10**-1 ): if -1 <= v and v <= 1: return arcsin(v) elif abs(v) -1 < allowableNumericalError: return pi/2 if v > 0 else -pi/2 else: raise ValueError("arcsin2 called with invalid input of %s" % v) def arccos2( v, allowableNumericalError=10**-1 ): if -1 <= v and v <= 1: return arccos(v) elif abs(v) -1 < allowableNumericalError: return 0 if v > 0 else pi else: raise ValueError("arccos2 called with invalid input of %s" % v) def normalize( v ): return v / norm(v) def quaternion(theta, u_x, u_y, u_z ): '''http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation returns q_1, q_2, q_3, q_0 as to match FreeCads, if wikipedias naming is used''' return ( u_x*sin(theta/2), u_y*sin(theta/2), u_z*sin(theta/2), cos(theta/2) ) def quaternion2(theta, u_x, u_y, u_z ): ''' returns in wikipedia order, i.e. cos(theta/2) element first ''' return ( cos(theta/2), u_x*sin(theta/2), u_y*sin(theta/2), u_z*sin(theta/2) ) def quaternion_to_euler( q_1, q_2, q_3, q_0): #order to match FreeCads, naming to match wikipedias ''' http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions for conversion to 3-1-3 Euler angles (dont know about this one, seems to me to be 3-2-1...) ''' psi = arctan2( 2*(q_0*q_1 + q_2*q_3), 1 - 2*(q_1**2 + q_2**2) ) phi = arcsin2( 2*(q_0*q_2 - q_3*q_1) ) theta = arctan2( 2*(q_0*q_3 + q_1*q_2), 1 - 2*(q_2**2 + q_3**2) ) return theta, phi, psi # gives same answer as FreeCADs toEuler function def quaternion_to_axis_and_angle( q_1, q_2, q_3, q_0): 'http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions' q = numpy.array( [q_1, q_2, q_3]) if norm(q) > 0: return q/norm(q), 2*arccos2(q_0) else: return numpy.array([1.0,0,0]), 2*arccos2(q_0) def azimuth_and_elevation_angles_to_axis( a, e): u_z = sin(e) u_x = cos(e)*cos(a) u_y = cos(e)*sin(a) return numpy.array([ u_x, u_y, u_z ]) def axis_to_azimuth_and_elevation_angles( u_x, u_y, u_z ): return arctan2( u_y, u_x), arcsin2(u_z) def quaternion_multiply( q1, q2 ): 'http://en.wikipedia.org/wiki/Quaternion#Hamilton_product' a_1, b_1, c_1, d_1 = q1 a_2, b_2, c_2, d_2 = q2 return numpy.array([ a_1*a_2 - b_1*b_2 - c_1*c_2 - d_1*d_2, a_1*b_2 + b_1*a_2 + c_1*d_2 - d_1*c_2, a_1*c_2 - b_1*d_2 + c_1*a_2 + d_1*b_2, a_1*d_2 + b_1*c_2 - c_1*b_2 + d_1*a_2 ]) def euler_to_quaternion(angle1, angle2, angle3, axis1=3, axis2=2, axis3=1): '''http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles ''' Q = [] for angle,axis in zip([angle1,angle2,angle3],[axis1,axis2,axis3]): q = numpy.array( [cos(angle/2),0,0,0 ] ) q[axis] = sin(angle/2) Q.append(q) q = quaternion_multiply( Q[0], quaternion_multiply( Q[1], Q[2] ) ) return q[1], q[2], q[3], q[0] def quaternion_rotation(p, q_1, q_2, q_3, q_0 ): ''' rotate the vector p using the quaternion u http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation ''' q = numpy.array( [q_0, q_1, q_2, q_3] ) q_inv = numpy.array( [q_0, -q_1, -q_2, -q_3] ) p_q = numpy.array( [ 0, p[0], p[1], p[2]] ) #p as a quaternion p_q_rotated = quaternion_multiply( q, quaternion_multiply( p_q, q_inv ) ) #print( p_q_rotated ) return p_q_rotated[1:] def euler_rotation(p, angle1, angle2, angle3, axis1=3, axis2=2, axis3=3 ): ''' http://en.wikipedia.org/wiki/Rotation_matrix , axis1=1, axis2=2, axis3=3 is the same as euler_ZYX_rotation''' R = numpy.eye(3) for angle,axis in zip([angle1,angle2,angle3],[axis1,axis2,axis3]): s = sin(angle) c = cos(angle) if axis == 1: #x rotation R_i = numpy.array([ [ 1, 0, 0], [ 0, c,-s], [ 0, s, c] ]) elif axis == 2: # y rotation R_i = numpy.array([ [ c, 0, s], [ 0, 1, 0], [-s, 0, c] ]) else: #z rotation R_i = numpy.array([ [ c,-s, 0], [ s, c, 0], [ 0, 0, 1] ]) #print(R_i) R = dotProduct(R_i, R) #print(R) #print('generic euler_rotation R') #print(R) return dotProduct(R, p) def euler_ZYX_rotation_matrix( angle1, angle2, angle3 ): ''' http://en.wikipedia.org/wiki/Rotation_matrix ''' c_1, s_1 = cos(angle1), sin(angle1) c_2, s_2 = cos(angle2), sin(angle2) c_3, s_3 = cos(angle3), sin(angle3) return numpy.array( [ [ c_1*c_2 , c_1*s_2*s_3 - c_3*s_1 , s_1*s_3 + c_1*c_3*s_2 ], [ c_2*s_1 , c_1*c_3 + s_1*s_2*s_3 , c_3*s_1*s_2 - c_1*s_3 ], [ - s_2 , c_2*s_3 , c_2*c_3 ] ]) def euler_ZYX_rotation(p, angle1, angle2, angle3 ): return dotProduct(euler_ZYX_rotation_matrix( angle1, angle2, angle3 ), p) def axis_rotation_matrix( theta, u_x, u_y, u_z ): ''' http://en.wikipedia.org/wiki/Rotation_matrix ''' return numpy.array( [ [ cos(theta) + u_x**2 * ( 1 - cos(theta)) , u_x*u_y*(1-cos(theta)) - u_z*sin(theta) , u_x*u_z*(1-cos(theta)) + u_y*sin(theta) ] , [ u_y*u_x*(1-cos(theta)) + u_z*sin(theta) , cos(theta) + u_y**2 * (1-cos(theta)) , u_y*u_z*(1-cos(theta)) - u_x*sin(theta )] , [ u_z*u_x*(1-cos(theta)) - u_y*sin(theta) , u_z*u_y*(1-cos(theta)) + u_x*sin(theta) , cos(theta) + u_z**2*(1-cos(theta)) ] ]) def axis_rotation( p, theta, u_x, u_y, u_z ): return dotProduct(axis_rotation_matrix( theta, u_x, u_y, u_z ), p) def azimuth_elevation_rotation_matrix(azi, ela, theta ): #print('azimuth_and_elevation_angles_to_axis(azi, ela) %s' % azimuth_and_elevation_angles_to_axis(azi, ela)) return axis_rotation_matrix( theta, *azimuth_and_elevation_angles_to_axis(azi, ela)) def azimuth_elevation_rotation( p, azi, ela, theta ): return dotProduct(azimuth_elevation_rotation_matrix( azi, ela, theta ), p) def rotation_matrix_to_euler_ZYX(R, debug=False, checkAnswer=False, tol=10**-6, tol_XZ_same_axis=10**-9 ): 'better way available at http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Rotation_matrix_.E2.86.94_Euler_angles' if 1.0 - abs(R[2,0]) > tol_XZ_same_axis : s_2 = -R[2,0] for angle2 in [ arcsin2(s_2), pi - arcsin2(s_2)]:#two options if debug: print(' angle2 %f' % angle2) c_2 = cos(angle2) s_3 = R[2,1] / c_2 c_3 = R[2,2] / c_2 for angle3 in [ arcsin2(s_3), pi - arcsin2(s_3)]: if debug: print(' angle2 %f, angle3 %f' % (angle2, angle3)) if abs(cos(angle3) - c_3) < tol: c_1 = max( min( R[0,0] / c_2, 1), -1) #c_1 = R[0,0] / c_2 s_1 = R[1,0] / c_2 for angle1 in [arccos2(c_1), -arccos2(c_1)]: if debug: print(' angle2 %f, angle3 %f, angle1 %f' % (angle2, angle3, angle1)) if abs(s_1 - sin(angle1)) < tol: if checkAnswer: rotation_matrix_to_euler_ZYX_check_answer( R, angle1, angle2, angle3) return angle1, angle2, angle3 #otherwise try axis orientated approach if debug: print('rotation_matrix_to_euler_ZYX - direct appoarch failed. Parsing to rotation_matrix_to_euler_ZYX_2') return rotation_matrix_to_euler_ZYX_2(R, debug) else: s_2 = -R[2,0] angle2 = arcsin2(s_2) c_2 = 0 debug = False #return rotation_matrix_to_euler_ZYX_2(R, debug) # euler_ZYX_rotation_matrix reduces to numpy.array( [ # [ c_1*c_2 , c_1*s_2*s_3 - c_3*s_1 , s_1*s_3 + c_1*c_3*s_2 ], # [ c_2*s_1 , c_1*c_3 + s_1*s_2*s_3 , c_3*s_1*s_2 - c_1*s_3 ], # [ - s_2 , c_2*s_3 , c_2*c_3 ] #which reduces to # [ 0 , s_2*c_1*s_3 - s_1*c_3 , s_1*s_3 + s_2*c_1*c_3 ], # [ 0 , c_1*c_3 + s_2*s_1*s_3 , s_1*c_3*s_2 - c_1*s_3 ], # [ - s_2, 0, 0] # triometric indeties # sin (angle1 + angle3) = s_1 c_3 + c_1 s_3 # cos (angle1 + angle3)= c_1 c_3 - s_1 s_3 # making angle3 negative: # sin (angle1 - angle3)= s_1 c_3 - c_1 s_3 # cos (angle1 - angle3)= c_1 c_3 + s_1 s_3 # let a = angle1 + angle3 # let b = angle1 - angle3 # elif s_2 == -1, R[1:,1:] reduces to # [ sin(a), -cos(a) ], # [ cos(a), sin(a) ], so # WTF are angle1 and angle3, about the same axis!? # Which makes sense since Y-axis rotation, mean x-angle and z-angle are applied about the same axis. so let angle3 = 0 #s_3 -> 0 c_3 -> 1 # euler_ZYX_rotation_matrix reduces # [ 0 , -s_1 , c_1*s_2 ], # [ 0 , c_1 , s_1*s_2 ], # [ - s_2 , 0 , 0 ] for angle1 in [ arcsin2(-R[0,1]), pi - arcsin2(-R[0,1]) ]: if debug: print(' angle2 %f, angle1 %f, angle3 %f' % (angle2, angle1, angle3)) #if debug: print(' cos(angle1) %f, R[0,2] %f' % (cos(angle1), R[0,2])) if abs(cos(angle1) - R[0,2]/s_2) < tol: return angle1, angle2, angle3 if debug: print('rotation_matrix_to_euler_ZYX - direct appoarch failed. Parsing to rotation_matrix_to_euler_ZYX_2') return rotation_matrix_to_euler_ZYX_2(R, debug) def rotation_matrix_to_euler_ZYX_check_answer( R, angle1, angle2, angle3, tol=10**-8, disp=False): R_out = euler_ZYX_rotation_matrix( angle1, angle2, angle3) error = numpy.linalg.norm(R - R_out) if disp: print('rotation_matrix_to_euler_ZYX_check_answer:') print(' norm(R - euler_ZYX_rotation_matrix( angle1, angle2, angle3)) %e' % error) if error > tol: raise RuntimeError('rotation_matrix_to_euler_ZYX check failed!. locals %s' % locals()) def rotation_matrix_to_euler_ZYX_2(R, debug=False): axis, angle = rotation_matrix_axis_and_angle_2(R) q_1, q_2, q_3, q_0 = quaternion(angle, *axis) return quaternion_to_euler( q_1, q_2, q_3, q_0) def rotation_matrix_axis_and_angle(R, debug=False, checkAnswer=True, errorThreshold=10**-7, angle_pi_tol = 10**-5): 'http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Rotation_matrix_.E2.86.94_Euler_axis.2Fangle' a = arccos2( 0.5 * ( R[0,0]+R[1,1]+R[2,2] - 1) ) if abs(a % pi) > angle_pi_tol and abs(pi - (a % pi)) > angle_pi_tol: msg='checking angles sign, angle %f' % a for angle in [a, -a]: u_x = 0.5* (R[2,1]-R[1,2]) / sin(angle) u_y = 0.5* (R[0,2]-R[2,0]) / sin(angle) u_z = 0.5* (R[1,0]-R[0,1]) / sin(angle) if abs( (1-cos(angle))*u_x*u_y - u_z*sin(angle) - R[0,1] ) < errorThreshold: msg = 'abs( (1-cos(angle))*u_x*u_y - u_z*sin(angle) - R[0,1] ) < 10**-6 check passed' break axis = numpy.array([u_x, u_y, u_z]) error = norm(axis_rotation_matrix(angle, *axis) - R) if debug: print(' norm(axis_rotation_matrix(angle, *axis) - R) %1.2e' % error) if error > errorThreshold: axis, angle = rotation_matrix_axis_and_angle_2(R, errorThreshold=errorThreshold, debug=debug, msg=msg) else: msg = 'abs(a % pi) > angle_pi_tol and abs(pi - (a % pi)) > angle_pi_tol' axis, angle = rotation_matrix_axis_and_angle_2( R, errorThreshold=errorThreshold, debug=debug, msg=msg) if numpy.isnan( angle ): raise RuntimeError('locals %s' % locals() ) return axis, angle def rotation_matrix_axis_and_angle_2(R, debug=False, errorThreshold=10**-7, msg=None): w, v = numpy.linalg.eig(R) #this method is not used at the primary method as numpy.linalg.eig does not return answers in high enough precision angle, axis = None, None eigErrors = abs(w -1) #errors from 1+0j i = (eigErrors == min(eigErrors)).tolist().index(True) axis = numpy.real(v[:,i]) if i != 1: angle = arccos2( numpy.real( w[1] ) ) else: angle = arccos2( numpy.real( w[0] ) ) error = norm(axis_rotation_matrix(angle, *axis) - R) if debug: print('rotation_matrix_axis_and_angle error %1.1e' % error) if error > errorThreshold: angle = -angle error = norm(axis_rotation_matrix(angle, *axis) - R) if error > errorThreshold: R_pickle_str = pickle.dumps(R) #R_abs_minus_identity = abs(R) - numpy.eye(3) print(R*R.transpose()) raise ValueError( 'rotation_matrix_axis_and_angle_2: no solution found! locals %s' % str(locals())) return axis, angle def plane_degrees_of_freedom( normalVector, debug=False, checkAnswer=False ): a,e = axis_to_azimuth_and_elevation_angles(*normalVector) dof1 = azimuth_and_elevation_angles_to_axis( a, e - pi/2) dof2 = azimuth_and_elevation_angles_to_axis( a+pi/2, 0) if checkAnswer: plane_degrees_of_freedom_check_answer( normalVector, dof1, dof2, debug ) return dof1, dof2 def plane_degrees_of_freedom_check_answer( normalVector, d1, d2, disp=False, tol=10**-12): if disp: print('checking plane_degrees_of_freedom result') print(' plane normal vector %s' % normalVector) print(' plane dof1 %s' % d1) print(' plane dof2 %s' % d2) Q = numpy.array([normalVector,d1,d2]) P = dotProduct(Q,Q.transpose()) error = norm(P - numpy.eye(3)) if disp: print(' dotProduct( array([normalVector,d1,d2]), array([normalVector,d1,d2]).transpose():') print(P) print(' error norm from eye(3) : %e' % error) if error > tol: raise RuntimeError('plane_degrees_of_freedom check failed!. locals %s' % locals()) def planeIntersection( normalVector1, normalVector2, debug=False, checkAnswer=False ): return normalize ( crossProduct(normalVector1, normalVector2) ) def planeIntersection_check_answer( normalVector1, normalVector2, d, disp=False, tol=10**-12): if disp: print('checking planeIntersection result') print(' plane normal vector 1 : %s' % normalVector1 ) print(' plane normal vector 2 : %s' % normalVector2 ) print(' d : %s' % d ) for t in [-3, 7, 12]: error1 = abs(dotProduct( normalVector1, d*t )) error2 = abs(dotProduct( normalVector2, d*t )) if disp:print(' d*(%1.1f) -> error1 %e, error2 %e' % (t, error1, error2) ) if error1 > tol or error2 > tol: raise RuntimeError(' planeIntersection check failed!. locals %s' % locals()) def distance_between_axes( p1, u1, p2, u2): ''' returns the shortest distance between to axes (or lines) in 3D space, where p1 is a point which line 1 goes through, and u1 is the direction vector for line 1. prob. minize d**2 where d**2 = (p1_x + u1_x*t1 - p2_x - u2_x*t2)**2 + (p1_y + u1_y*t1 - p2_y - u2_y*t2)**2 + (p1_z + u1_z*t1 - p2_z - u2_z*t2)**2 giving a quadratic 2 varaiable (X = [t1,t2]) problem in the form of 0.5 X^T A X + C^T B differenting it gives 0 = Q X + C using sympy to expand the abover expression > from sympy import * > x,y = symbols('x y') > expand( (x +y )**2) x**2 + 2*x*y + y**2 > t1, t2, p1_x, p1_y, p1_z, p2_x, p2_y, p2_z, u1_x, u1_y, u1_z, u2_x, u2_y, u2_z = symbols('t1, t2, p1_x, p1_y, p1_z, p2_x, p2_y, p2_z, u1_x, u1_y, u1_z, u2_x, u2_y, u2_z') > d_sqrd = (p1_x + u1_x*t1 - p2_x - u2_x*t2)**2 + (p1_y + u1_y*t1 - p2_y - u2_y*t2)**2 + (p1_z + u1_z*t1 - p2_z - u2_z*t2)**2 > expand(d_sqrd) > collect( expand(d_sqrd), [t1 , t2] ) ''' p1_x, p1_y, p1_z = p1 u1_x, u1_y, u1_z = u1 p2_x, p2_y, p2_z = p2 u2_x, u2_y, u2_z = u2 if numpy.array_equal( u1, u2 ) or numpy.array_equal( u1, -u2 ): #then assert numpy.linalg.norm( u1 ) != 0 # generated using sympy # > t, p1_x, p1_y, p1_z, p2_x, p2_y, p2_z, u1_x, u1_y, u1_z = symbols('t, p1_x, p1_y, p1_z, p2_x, p2_y, p2_z, u1_x, u1_y, u1_z') # > d_sqrd = (p1_x + u1_x*t - p2_x)**2 + (p1_y + u1_y*t - p2_y)**2 + (p1_z + u1_z*t - p2_z)**2 # > solve( diff( d_sqrd, t ), t ) # gives the expresssion for t_opt t = (-p1_x*u1_x - p1_y*u1_y - p1_z*u1_z + p2_x*u1_x + p2_y*u1_y + p2_z*u1_z)/(u1_x**2 + u1_y**2 + u1_z**2) d_sqrd = (p1_x - p2_x + t*u1_x)**2 + (p1_y - p2_y + t*u1_y)**2 + (p1_z - p2_z + t*u1_z)**2 else: t1_t1_coef = u1_x**2 + u1_y**2 + u1_z**2 #collect( expand(d_sqrd), [t1 , t2] ) t1_t2_coef = -2*u1_x*u2_x - 2*u1_y*u2_y - 2*u1_z*u2_z # collect( expand(d_sqrd), [t1*t2] ) t2_t2_coef = u2_x**2 + u2_y**2 + u2_z**2 t1_coef = 2*p1_x*u1_x + 2*p1_y*u1_y + 2*p1_z*u1_z - 2*p2_x*u1_x - 2*p2_y*u1_y - 2*p2_z*u1_z t2_coef =-2*p1_x*u2_x - 2*p1_y*u2_y - 2*p1_z*u2_z + 2*p2_x*u2_x + 2*p2_y*u2_y + 2*p2_z*u2_z A = numpy.array([ [ 2*t1_t1_coef , t1_t2_coef ] , [ t1_t2_coef, 2*t2_t2_coef ] ]) b = numpy.array([ t1_coef, t2_coef]) try: t1, t2 = numpy.linalg.solve(A,-b) except numpy.linalg.LinAlgError: print('distance_between_axes, failed to solve problem due to LinAlgError, using numerical solver instead') print(' variables : ') print(' p1 : %s' % p1 ) print(' u1 : %s' % u1 ) print(' p2 : %s' % p2 ) print(' u2 : %s' % u2 ) return distance_between_axes_fmin(p1, u1, p2, u2) d_sqrd = t1_t1_coef * t1**2 + t1_t2_coef * t1*t2 + t2_t2_coef * t2**2 + t1_coef*t1 + t2_coef*t2 + p1_x**2 - 2*p1_x*p2_x + p1_y**2 - 2*p1_y*p2_y + p1_z**2 - 2*p1_z*p2_z + p2_x**2 + p2_y**2 + p2_z**2 return d_sqrd ** 0.5 def distance_between_axes_fmin( p1, u1, p2, u2): from scipy.optimize import fmin_bfgs def distance(T): t1, t2 = T return numpy.linalg.norm( p1 + u1*t1 - (p2 + u2*t2) ) T_opt = fmin_bfgs( distance, [0 , 0], disp=False) return distance(T_opt) def distance_between_two_axes_3_points(p1,u1,p2,u2): ''' used for axial and circular edge constraints ''' # generated using sympy # > t, p1_x, p1_y, p1_z, p2_x, p2_y, p2_z, u1_x, u1_y, u1_z = symbols('t, p1_x, p1_y, p1_z, p2_x, p2_y, p2_z, u1_x, u1_y, u1_z') # > d_sqrd = (p1_x + u1_x*t - p2_x)**2 + (p1_y + u1_y*t - p2_y)**2 + (p1_z + u1_z*t - p2_z)**2 # > solve( diff( d_sqrd, t ), t ) # gives the expresssion for t_opt assert numpy.linalg.norm( u1 ) != 0 p1_x, p1_y, p1_z = p1 u1_x, u1_y, u1_z = u1 #if not (u1_x**2 + u1_y**2 + u1_z**2) == 1: # raise ValueError, "(u1_x**2 + u1_y**2 + u1_z**2) !=1 but rather %f " % ( u1_x**2 + u1_y**2 + u1_z**2 ) dist = 0 for axis2_t in [-10, 0, 10]: #find point on axis 1 which is closest p2_x, p2_y, p2_z = p2 + axis2_t*u2 t = (-p1_x*u1_x - p1_y*u1_y - p1_z*u1_z + p2_x*u1_x + p2_y*u1_y + p2_z*u1_z)/(u1_x**2 + u1_y**2 + u1_z**2) #should be able to drop this last term as it will equal 1... d_sqrd = (p1_x - p2_x + t*u1_x)**2 + (p1_y - p2_y + t*u1_y)**2 + (p1_z - p2_z + t*u1_z)**2 dist = dist + d_sqrd ** 0.5 return dist def distance_between_axis_and_point( p1,u1,p2 ): assert numpy.linalg.norm( u1 ) != 0 d = p2 - p1 offset = d - dotProduct(u1,d)*u1 #print(norm(offset)) return norm(offset) def distance_between_axis_and_point_old( p1, u1, p2 ): assert numpy.linalg.norm( u1 ) != 0 p1_x, p1_y, p1_z = p1 u1_x, u1_y, u1_z = u1 p2_x, p2_y, p2_z = p2 t = (-p1_x*u1_x - p1_y*u1_y - p1_z*u1_z + p2_x*u1_x + p2_y*u1_y + p2_z*u1_z) # dropped the (u1_x**2 + u1_y**2 + u1_z**2) term as it should equal 1 d_sqrd = (p1_x - p2_x + t*u1_x)**2 + (p1_y - p2_y + t*u1_y)**2 + (p1_z - p2_z + t*u1_z)**2 return d_sqrd ** 0.5 def rotation_required_to_rotate_a_vector_to_be_aligned_to_another_vector( v, v_ref ): c = crossProduct( v, v_ref) if norm(c) > 0: axis = normalize(c) else: #dont think this ever happens. axis, notUsed = plane_degrees_of_freedom( v ) #if dof_axis == None: angle = arccos2( dotProduct( v, v_ref )) #else: # axis3 = normalize ( crossProduct(v_ref, dof_axis) ) # a = dotProduct( v, v_ref ) #adjacent # o = dotProduct( v, axis3 ) #oppersite # angle = numpy.arctan2( o, a ) return axis, angle def rotation_required_to_rotate_a_vector_to_be_aligned_to_another_vector2( v, v_ref, dof_axis=None ): ''' calculate the axis in a method other then a crossduct. finding axis, so that dot(axis, v) == 0 and dot(axis, v_ref) == 0, using linear alegebra. ''' A_matrixs = [] for i in range(3): A_matrixs.append([ [v[j] for j in range(3) if j != i], [v_ref[j] for j in range(3) if j != i]] ) #prettyPrintArray( A_matrixs[-1] ) cond_number = map( numpy.linalg.cond, A_matrixs) minloc = cond_number.index(min(cond_number)) b = numpy.array([ -v[minloc], -v_ref[minloc] ]) c = numpy.linalg.solve( A_matrixs[minloc], b).tolist() c.insert(minloc,1) c = numpy.array(c) #* numpy.sign(crossProduct( v, v_ref)) if norm(c) > 0: axis = normalize(c) else: #dont think this ever happens. axis, notUsed = plane_degrees_of_freedom( v ) if dof_axis == None: angle = arccos2( dotProduct( v, v_ref )) #checking if negative angle should be used v_rotated = dotProduct( axis_rotation_matrix( angle, *axis), v) #print(' v_rotated %s' % (v_rotated) ) error = norm( v_ref - v_rotated ) if error > 1: angle = -angle else: axis3 = normalize ( crossProduct(v_ref, dof_axis) ) a = dotProduct( v, v_ref ) #adjacent o = dotProduct( v, axis3 ) #oppersite angle = numpy.arctan2( o, a ) return axis, angle def gram_schmidt_proj(u,v): return dot(v,u)/dot(u,u)*u def gram_schmidt_orthonormalization( v1, v2, v3 ): 'https://en.wikipedia.org/wiki/Gram%E2%80%93Schmidt_process' u1 = v1 u2 = v2 - gram_schmidt_proj(u1,v2) u3 = v3 - gram_schmidt_proj(u1,v3) - gram_schmidt_proj(u2,v3) return normalize(u1), normalize(u2), normalize(u3) def fit_plane_to_surface1( surface, n_u=3, n_v=3 ): uv = sum( [ [ (u,v) for u in linspace(0,1,n_u)] for v in linspace(0,1,n_v) ], [] ) P = [ surface.value(u,v) for u,v in uv ] #positions at u,v points N = [ crossProduct( *surface.tangent(u,v) ) for u,v in uv ] plane_norm = sum(N) / len(N) #plane's normal, averaging done to reduce error plane_pos = P[0] error = sum([ abs( dot(p - plane_pos, plane_norm) ) for p in P ]) return plane_norm, plane_pos, error def fit_rotation_axis_to_surface1( surface, n_u=3, n_v=3 ): 'should work for cylinders and pssibly cones (depending on the u,v mapping)' uv = sum( [ [ (u,v) for u in linspace(0,1,n_u)] for v in linspace(0,1,n_v) ], [] ) P = [ numpy.array(surface.value(u,v)) for u,v in uv ] #positions at u,v points N = [ crossProduct( *surface.tangent(u,v) ) for u,v in uv ] intersections = [] for i in range(len(N)-1): for j in range(i+1,len(N)): # based on the distance_between_axes( p1, u1, p2, u2) function, if 1 - abs(dot( N[i], N[j])) < 10**-6: continue #ignore parrallel case p1_x, p1_y, p1_z = P[i] u1_x, u1_y, u1_z = N[i] p2_x, p2_y, p2_z = P[j] u2_x, u2_y, u2_z = N[j] t1_t1_coef = u1_x**2 + u1_y**2 + u1_z**2 #should equal 1 t1_t2_coef = -2*u1_x*u2_x - 2*u1_y*u2_y - 2*u1_z*u2_z # collect( expand(d_sqrd), [t1*t2] ) t2_t2_coef = u2_x**2 + u2_y**2 + u2_z**2 #should equal 1 too t1_coef = 2*p1_x*u1_x + 2*p1_y*u1_y + 2*p1_z*u1_z - 2*p2_x*u1_x - 2*p2_y*u1_y - 2*p2_z*u1_z t2_coef =-2*p1_x*u2_x - 2*p1_y*u2_y - 2*p1_z*u2_z + 2*p2_x*u2_x + 2*p2_y*u2_y + 2*p2_z*u2_z A = numpy.array([ [ 2*t1_t1_coef , t1_t2_coef ] , [ t1_t2_coef, 2*t2_t2_coef ] ]) b = numpy.array([ t1_coef, t2_coef]) try: t1, t2 = numpy.linalg.solve(A,-b) except numpy.linalg.LinAlgError: continue #print('distance_between_axes, failed to solve problem due to LinAlgError, using numerical solver instead') pos_t1 = P[i] + numpy.array(N[i])*t1 pos_t2 = P[j] + N[j]*t2 intersections.append( pos_t1 ) intersections.append( pos_t2 ) if len(intersections) < 2: error = numpy.inf return 0, 0, error else: #fit vector to intersection points; http://mathforum.org/library/drmath/view/69103.html X = numpy.array(intersections) centroid = numpy.mean(X,axis=0) M = numpy.array([i - centroid for i in intersections ]) A = numpy.dot(M.transpose(), M) U,s,V = numpy.linalg.svd(A) #numpy docs: s : (..., K) The singular values for every matrix, sorted in descending order. axis_pos = centroid axis_dir = V[0] error = s[1] #dont know if this will work return axis_dir, axis_pos, error