'''
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
