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