| 1 |
|
|---|
| 2 |
|
|---|
| 3 | import numpy as np
|
|---|
| 4 | import simulation_helpers as sh
|
|---|
| 5 |
|
|---|
| 6 | #
|
|---|
| 7 | # This is a simple implementation I have looked up in the wiki
|
|---|
| 8 | # THIS ONE is used by Turnable class currently
|
|---|
| 9 | #
|
|---|
| 10 | def make_rotation_matrix( nn, a ):
|
|---|
| 11 | """ return rotation matrix for angle a around nn """
|
|---|
| 12 | R = np.zeros( (3,3) )
|
|---|
| 13 | R[0,0] = nn[0]*nn[0] * (1-np.cos(a)) + np.cos(a)
|
|---|
| 14 | R[1,0] = nn[0]*nn[1] * (1-np.cos(a)) + nn[2]*np.sin(a)
|
|---|
| 15 | R[2,0] = nn[0]*nn[2] * (1-np.cos(a)) - nn[1]*np.sin(a)
|
|---|
| 16 |
|
|---|
| 17 | R[0,1] = nn[0]*nn[1] * (1-np.cos(a)) - nn[2]*np.sin(a)
|
|---|
| 18 | R[1,1] = nn[1]*nn[1] * (1-np.cos(a)) + np.cos(a)
|
|---|
| 19 | R[2,1] = nn[1]*nn[2] * (1-np.cos(a)) + nn[0]*np.sin(a)
|
|---|
| 20 |
|
|---|
| 21 | R[0,2] = nn[0]*nn[2] * (1-np.cos(a)) + nn[1]*np.sin(a)
|
|---|
| 22 | R[1,2] = nn[1]*nn[2] * (1-np.cos(a)) - nn[0]*np.sin(a)
|
|---|
| 23 | R[2,2] = nn[2]*nn[2] * (1-np.cos(a)) + np.cos(a)
|
|---|
| 24 |
|
|---|
| 25 | return R
|
|---|
| 26 |
|
|---|
| 27 | #
|
|---|
| 28 | # This is a smarter implementation I have looked up on the web somewhere
|
|---|
| 29 | # I have timed it and it seems not to be faster then the upper one
|
|---|
| 30 | # However the returned rotation matrices of both differ by a sign
|
|---|
| 31 | # in two off axis elements... not sure what this means :-|
|
|---|
| 32 | #
|
|---|
| 33 | def make_axis_rotation_matrix(direction, angle):
|
|---|
| 34 | """
|
|---|
| 35 | Create a rotation matrix corresponding to the rotation around a general
|
|---|
| 36 | axis by a specified angle.
|
|---|
| 37 | R = dd^T + cos(a) (I - dd^T) + sin(a) skew(d)
|
|---|
| 38 | Parameters:
|
|---|
| 39 | angle : float a
|
|---|
| 40 | direction : array d
|
|---|
| 41 | """
|
|---|
| 42 | d = np.array(direction, dtype=np.float64)
|
|---|
| 43 | d /= np.linalg.norm(d)
|
|---|
| 44 | eye = np.eye(3, dtype=np.float64)
|
|---|
| 45 | ddt = np.outer(d, d)
|
|---|
| 46 | skew = np.array([[ 0, d[2], -d[1]],
|
|---|
| 47 | [-d[2], 0, d[0]],
|
|---|
| 48 | [d[1], -d[0], 0]], dtype=np.float64)
|
|---|
| 49 | mtx = ddt + np.cos(angle) * (eye - ddt) + np.sin(angle) * skew
|
|---|
| 50 | return mtx
|
|---|
| 51 |
|
|---|
| 52 |
|
|---|
| 53 | class Turnable( object ):
|
|---|
| 54 | """ *Virtual* class for inheritance of turn() method only
|
|---|
| 55 |
|
|---|
| 56 | turn( axis, angle) - turn any member of class, which is
|
|---|
| 57 | contained in list-like self._turnables.
|
|---|
| 58 |
|
|---|
| 59 | sub-class constructor should fill self._turnables
|
|---|
| 60 | """
|
|---|
| 61 | def turn(self, axis, angle):
|
|---|
| 62 | """ turn all members in self.turnables around *axis* by angle
|
|---|
| 63 |
|
|---|
| 64 | *axis* : can be any 3-element np.array or np.array - constructable
|
|---|
| 65 | like e.g. a list
|
|---|
| 66 | *angle* : in radians
|
|---|
| 67 | """
|
|---|
| 68 |
|
|---|
| 69 | axis = np.array(axis)
|
|---|
| 70 |
|
|---|
| 71 | if sh.length(axis) != 1.:
|
|---|
| 72 | axis /= sh.length(axis)
|
|---|
| 73 |
|
|---|
| 74 | R = make_rotation_matrix( axis, angle )
|
|---|
| 75 |
|
|---|
| 76 | for turnable in self._turnables:
|
|---|
| 77 | if hasattr(self, turnable):
|
|---|
| 78 | setattr( self, turnable, np.dot(R, getattr( self, turnable)) )
|
|---|
| 79 |
|
|---|
| 80 | # standard __repr__
|
|---|
| 81 | def __repr__( self ):
|
|---|
| 82 | return "%s(%r)" % (self.__class__, self.__dict__)
|
|---|
| 83 |
|
|---|
