1
2
3
4
5
6 from Numeric import array, sum, sqrt, arccos, matrixmultiply, transpose, cos, \
7 sin, zeros, trace
8 from math import acos
9 from LinearAlgebra import determinant, eigenvectors
10 from MLab import eye
11 import sys
12 from math import pi
13
14
15 __doc__="Vector class, including rotation-related functions."
16
18 """
19 Return angles, axis pair that corresponds to rotation matrix m.
20 """
21
22
23 t=0.5*(trace(m)-1)
24 t=max(-1, t)
25 t=min(1, t)
26 angle=acos(t)
27 if angle<1e-15:
28
29 return 0.0, Vector(1,0,0)
30 elif angle<pi:
31
32 x=m[2,1]-m[1,2]
33 y=m[0,2]-m[2,0]
34 z=m[1,0]-m[0,1]
35 axis=Vector(x,y,z)
36 axis.normalize()
37 return angle, axis
38 else:
39
40 m00=m[0,0]
41 m11=m[1,1]
42 m22=m[2,2]
43 if m00>m11 and m00>m22:
44 x=sqrt(m00-m11-m22+0.5)
45 y=m[0,1]/(2*x)
46 z=m[0,2]/(2*x)
47 elif m11>m00 and m11>m22:
48 y=sqrt(m11-m00-m22+0.5)
49 x=m[0,1]/(2*y)
50 z=m[1,2]/(2*y)
51 else:
52 z=sqrt(m22-m00-m11+0.5)
53 x=m[0,2]/(2*z)
54 y=m[1,2]/(2*z)
55 axis=Vector(x,y,z)
56 axis.normalize()
57 return pi, axis
58
59
61 """
62 Returns the vector between a point and
63 the closest point on a line (ie. the perpendicular
64 projection of the point on the line).
65
66 @type line: L{Vector}
67 @param line: vector defining a line
68
69 @type point: L{Vector}
70 @param point: vector defining the point
71 """
72 line=line.normalized()
73 np=point.norm()
74 angle=line.angle(point)
75 return point-line**(np*cos(angle))
76
77
79 """
80 Calculate a left multiplying rotation matrix that rotates
81 theta rad around vector.
82
83 Example:
84
85 >>> m=rotaxis(pi, Vector(1,0,0))
86 >>> rotated_vector=any_vector.left_multiply(m)
87
88 @type theta: float
89 @param theta: the rotation angle
90
91
92 @type vector: L{Vector}
93 @param vector: the rotation axis
94
95 @return: The rotation matrix, a 3x3 Numeric array.
96 """
97 vector=vector.copy()
98 vector.normalize()
99 c=cos(theta)
100 s=sin(theta)
101 t=1-c
102 x,y,z=vector.get_array()
103 rot=zeros((3,3), "d")
104
105 rot[0,0]=t*x*x+c
106 rot[0,1]=t*x*y-s*z
107 rot[0,2]=t*x*z+s*y
108
109 rot[1,0]=t*x*y+s*z
110 rot[1,1]=t*y*y+c
111 rot[1,2]=t*y*z-s*x
112
113 rot[2,0]=t*x*z-s*y
114 rot[2,1]=t*y*z+s*x
115 rot[2,2]=t*z*z+c
116 return rot
117
118 rotaxis=rotaxis2m
119
121 """
122 Return a (left multiplying) matrix that mirrors p onto q.
123
124 Example:
125 >>> mirror=refmat(p,q)
126 >>> qq=p.left_multiply(mirror)
127 >>> print q, qq # q and qq should be the same
128
129 @type p,q: L{Vector}
130 @return: The mirror operation, a 3x3 Numeric array.
131 """
132 p.normalize()
133 q.normalize()
134 if (p-q).norm()<1e-5:
135 return eye(3)
136 pq=p-q
137 pq.normalize()
138 b=pq.get_array()
139 b.shape=(3, 1)
140 i=eye(3)
141 ref=i-2*matrixmultiply(b, transpose(b))
142 return ref
143
145 """
146 Return a (left multiplying) matrix that rotates p onto q.
147
148 Example:
149 >>> r=rotmat(p,q)
150 >>> print q, p.left_multiply(r)
151
152 @param p: moving vector
153 @type p: L{Vector}
154
155 @param q: fixed vector
156 @type q: L{Vector}
157
158 @return: rotation matrix that rotates p onto q
159 @rtype: 3x3 Numeric array
160 """
161 rot=matrixmultiply(refmat(q, -p), refmat(p, -p))
162 return rot
163
165 """
166 Calculate the angle between 3 vectors
167 representing 3 connected points.
168
169 @param v1, v2, v3: the tree points that define the angle
170 @type v1, v2, v3: L{Vector}
171
172 @return: angle
173 @rtype: float
174 """
175 v1=v1-v2
176 v3=v3-v2
177 return v1.angle(v3)
178
180 """
181 Calculate the dihedral angle between 4 vectors
182 representing 4 connected points. The angle is in
183 ]-pi, pi].
184
185 @param v1, v2, v3, v4: the four points that define the dihedral angle
186 @type v1, v2, v3, v4: L{Vector}
187 """
188 ab=v1-v2
189 cb=v3-v2
190 db=v4-v3
191 u=ab**cb
192 v=db**cb
193 w=u**v
194 angle=u.angle(v)
195
196 try:
197 if cb.angle(w)>0.001:
198 angle=-angle
199 except ZeroDivisionError:
200
201 pass
202 return angle
203
205 "3D vector"
206
208 if y is None and z is None:
209
210 if len(x)!=3:
211 raise "Vector: x is not a list/tuple/array of 3 numbers"
212 self._ar=array(x, 'd')
213 else:
214
215 self._ar=array((x, y, z), 'd')
216
218 x,y,z=self._ar
219 return "<Vector %.2f, %.2f, %.2f>" % (x,y,z)
220
222 "Return Vector(-x, -y, -z)"
223 a=-self._ar
224 return Vector(a)
225
227 "Return Vector+other Vector or scalar"
228 if isinstance(other, Vector):
229 a=self._ar+other._ar
230 else:
231 a=self._ar+array(other)
232 return Vector(a)
233
235 "Return Vector-other Vector or scalar"
236 if isinstance(other, Vector):
237 a=self._ar-other._ar
238 else:
239 a=self._ar-array(other)
240 return Vector(a)
241
243 "Return Vector.Vector (dot product)"
244 return sum(self._ar*other._ar)
245
247 "Return Vector(coords/a)"
248 a=self._ar/array(x)
249 return Vector(a)
250
252 "Return VectorxVector (cross product) or Vectorxscalar"
253 if isinstance(other, Vector):
254 a,b,c=self._ar
255 d,e,f=other._ar
256 c1=determinant(array(((b,c), (e,f))))
257 c2=-determinant(array(((a,c), (d,f))))
258 c3=determinant(array(((a,b), (d,e))))
259 return Vector(c1,c2,c3)
260 else:
261 a=self._ar*array(other)
262 return Vector(a)
263
266
269
271 "Return vector norm"
272 return sqrt(sum(self._ar*self._ar))
273
275 "Return square of vector norm"
276 return abs(sum(self._ar*self._ar))
277
279 "Normalize the Vector"
280 self._ar=self._ar/self.norm()
281
283 "Return a normalized copy of the Vector"
284 v=self.copy()
285 v.normalize()
286 return v
287
289 "Return angle between two vectors"
290 n1=self.norm()
291 n2=other.norm()
292 c=(self*other)/(n1*n2)
293
294 c=min(c,1)
295 c=max(-1,c)
296 return arccos(c)
297
299 "Return (a copy of) the array of coordinates"
300 return array(self._ar)
301
303 "Return Vector=Matrix x Vector"
304 a=matrixmultiply(matrix, self._ar)
305 return Vector(a)
306
308 "Return Vector=Vector x Matrix"
309 a=matrixmultiply(self._ar, matrix)
310 return Vector(a)
311
313 "Return a deep copy of the Vector"
314 return Vector(self._ar)
315
316 if __name__=="__main__":
317
318 from math import pi
319 from RandomArray import *
320 from Numeric import *
321
322 v1=Vector(0,0,1)
323 v2=Vector(0,0,0)
324 v3=Vector(0,1,0)
325 v4=Vector(1,1,0)
326
327 v4.normalize()
328
329 print v4
330
331 print calc_angle(v1, v2, v3)
332 dih=calc_dihedral(v1, v2, v3, v4)
333
334 assert(dih>0)
335 print "DIHEDRAL ", dih
336
337 ref=refmat(v1, v3)
338 rot=rotmat(v1, v3)
339
340 print v3
341 print v1.left_multiply(ref)
342 print v1.left_multiply(rot)
343 print v1.right_multiply(transpose(rot))
344
345
346 print v1-v2
347 print v1-1
348 print v1+(1,2,3)
349
350 print v1+v2
351 print v1+3
352 print v1-(1,2,3)
353
354 print v1*v2
355
356 print v1/2
357 print v1/(1,2,3)
358
359 print v1**v2
360 print v1**2
361 print v1**(1,2,3)
362
363 print v1.norm()
364
365 print v1.normsq()
366
367 v1[2]=10
368 print v1
369
370 print v1[2]
371
372 print array(v1)
373
374 print "ROT"
375
376 angle=random()*pi
377 axis=Vector(random(3)-random(3))
378 axis.normalize()
379
380 m=rotaxis(angle, axis)
381
382 cangle, caxis=m2rotaxis(m)
383
384 print angle-cangle
385 print axis-caxis
386 print
387