| 1 | """ |
|---|
| 2 | Quaternion math |
|---|
| 3 | """ |
|---|
| 4 | |
|---|
| 5 | __version__ = '0.1' |
|---|
| 6 | __authors__ = 'Mihai Maruseac (mihai.maruseac@gmail.com)' ,\ |
|---|
| 7 | 'Laura-Mihaela Vasilescu (vasilescu.laura@gmail.com)' |
|---|
| 8 | |
|---|
| 9 | import math |
|---|
| 10 | from pyglet.gl import * |
|---|
| 11 | |
|---|
| 12 | import Matrix |
|---|
| 13 | import Vector |
|---|
| 14 | |
|---|
| 15 | class Quaternion: |
|---|
| 16 | """ |
|---|
| 17 | Quaternion usefull stuff |
|---|
| 18 | """ |
|---|
| 19 | def __init__(self, *args): |
|---|
| 20 | if len(args) == 4: |
|---|
| 21 | # 4 real values |
|---|
| 22 | self.x = args[1] |
|---|
| 23 | self.y = args[2] |
|---|
| 24 | self.z = args[3] |
|---|
| 25 | self.w = args[0] |
|---|
| 26 | else: |
|---|
| 27 | arg = args[0] |
|---|
| 28 | if isinstance(arg, Quaternion): |
|---|
| 29 | self.x = arg.x |
|---|
| 30 | self.y = arg.y |
|---|
| 31 | self.z = arg.z |
|---|
| 32 | self.w = arg.w |
|---|
| 33 | elif isinstance(arg, Matrix.Matrix3): |
|---|
| 34 | self = self.fromMatrix(self, arg) |
|---|
| 35 | elif isinstance(arg, Matrix.Matrix4): |
|---|
| 36 | self = self.fromMatrix(self, arg) |
|---|
| 37 | else: |
|---|
| 38 | raise TypeError("Invalid argument") |
|---|
| 39 | |
|---|
| 40 | def __add__(self, other): |
|---|
| 41 | return Quaternion(self.w + other.w, |
|---|
| 42 | self.x + other.x, |
|---|
| 43 | self.y + other.y, |
|---|
| 44 | self.z + other.z) |
|---|
| 45 | |
|---|
| 46 | def __sub__(self, other): |
|---|
| 47 | return Quaternion(self.w - other.w, |
|---|
| 48 | self.x - other.x, |
|---|
| 49 | self.y - other.y, |
|---|
| 50 | self.z - other.z) |
|---|
| 51 | |
|---|
| 52 | def __neg__(self): |
|---|
| 53 | return Quaternion(self.w, -self.x, -self.y, -self.z) |
|---|
| 54 | |
|---|
| 55 | def __mul__(self, other): |
|---|
| 56 | if isinstance(other, Quaternion): |
|---|
| 57 | return Quaternion(\ |
|---|
| 58 | other.w * self.w - other.x * self.x -\ |
|---|
| 59 | other.y * self.y - other.z * self.z,\ |
|---|
| 60 | self.y * other.z - self.z * other.y +\ |
|---|
| 61 | self.w * other.x + self.x * other.w,\ |
|---|
| 62 | self.z * other.x - self.x * other.z +\ |
|---|
| 63 | self.w * other.y + self.y * other.w,\ |
|---|
| 64 | self.x * other.y - self.y * other.x +\ |
|---|
| 65 | self.w * other.z + self.z * other.w) |
|---|
| 66 | else: |
|---|
| 67 | return Quaternion(self.w * other, |
|---|
| 68 | self.x * other, |
|---|
| 69 | self.y * other, |
|---|
| 70 | self.z * other) |
|---|
| 71 | |
|---|
| 72 | |
|---|
| 73 | def __eq__(self, other): |
|---|
| 74 | return (self.x == other.x and \ |
|---|
| 75 | self.y == other.y and \ |
|---|
| 76 | self.z == other.z and \ |
|---|
| 77 | self.w == other.w) |
|---|
| 78 | |
|---|
| 79 | def __ne__(self, other): |
|---|
| 80 | return (not (self == other)) |
|---|
| 81 | |
|---|
| 82 | def __getitem__(self, key): |
|---|
| 83 | if key == 0: return self.w |
|---|
| 84 | elif key == 1: return self.x |
|---|
| 85 | elif key == 2: return self.y |
|---|
| 86 | elif key == 3: return self.w |
|---|
| 87 | else: raise IndexError("bad Quaternion index"); |
|---|
| 88 | |
|---|
| 89 | def __setitem__(self, key, val): |
|---|
| 90 | if key == 0: self.w = val |
|---|
| 91 | elif key == 1: self.x = val |
|---|
| 92 | elif key == 2: self.y = val |
|---|
| 93 | elif key == 3: self.z = val |
|---|
| 94 | else: raise IndexError("bad Quaternion index"); |
|---|
| 95 | |
|---|
| 96 | def length(self): |
|---|
| 97 | x = self.x ** 2 |
|---|
| 98 | y = self.y ** 2 |
|---|
| 99 | z = self.z ** 2 |
|---|
| 100 | w = self.w ** 2 |
|---|
| 101 | return math.sqrt(x + y + z +w) |
|---|
| 102 | |
|---|
| 103 | def normalize(self): |
|---|
| 104 | l = self.length() |
|---|
| 105 | self.x /= l |
|---|
| 106 | self.y /= l |
|---|
| 107 | self.w /= l |
|---|
| 108 | self.z /= l |
|---|
| 109 | return self |
|---|
| 110 | |
|---|
| 111 | def __str__(self): |
|---|
| 112 | return '[ w = ' + str(self.w) +\ |
|---|
| 113 | '; v = (' + str(self.x) +\ |
|---|
| 114 | ' ' + str(self.y) + \ |
|---|
| 115 | ' ' + str(self.z) + ')]' |
|---|
| 116 | |
|---|
| 117 | @staticmethod |
|---|
| 118 | def fromMatrix(self, matrix4or3): |
|---|
| 119 | trace = matrix4or3._m[0][0] + matrix4or3._m[1][1] + matrix4or3._m[2][2] + 1 |
|---|
| 120 | if trace > 0: |
|---|
| 121 | S = 0.5 / math.sqrt(trace) |
|---|
| 122 | self.w = 0.25 / S |
|---|
| 123 | self.x = (matrix4or3._m[2][1] - matrix4or3._m[1][2]) * S |
|---|
| 124 | self.y = (matrix4or3._m[0][2] - matrix4or3._m[2][0]) * S |
|---|
| 125 | self.z = (matrix4or3._m[1][0] - matrix4or3._m[0][1]) * S |
|---|
| 126 | else: |
|---|
| 127 | maxd = matrix4or3._m[0][0] |
|---|
| 128 | column = 0 |
|---|
| 129 | if matrix4or3._m[1][1] > maxd: |
|---|
| 130 | maxd = matrix4or3._m[1][1] |
|---|
| 131 | column = 1 |
|---|
| 132 | if matrix4or3._m[2][2] > maxd: |
|---|
| 133 | maxd = matrix4or3._m[2][2] |
|---|
| 134 | column = 2 |
|---|
| 135 | |
|---|
| 136 | if column == 0: |
|---|
| 137 | S = math.sqrt(1.0 + matrix4or3._m[0][0] - matrix4or3._m[1][1] \ |
|---|
| 138 | - matrix4or3._m[2][2]) * 2 |
|---|
| 139 | Qx = 0.5 / S |
|---|
| 140 | Qy = (matrix4or3._m[0][1] + matrix4or3._m[1][0]) / S |
|---|
| 141 | Qz = (matrix4or3._m[0][2] + matrix4or3._m[2][0]) / S |
|---|
| 142 | Qw = (matrix4or3._m[1][2] + matrix4or3._m[2][1]) / S |
|---|
| 143 | |
|---|
| 144 | if column == 1: |
|---|
| 145 | S = math.sqrt(1.0 - matrix4or3._m[0][0] + matrix4or3._m[1][1] \ |
|---|
| 146 | - matrix4or3._m[2][2]) * 2 |
|---|
| 147 | Qy = 0.5 / S |
|---|
| 148 | Qx = (matrix4or3._m[0][1] + matrix4or3._m[1][0]) / S |
|---|
| 149 | Qw = (matrix4or3._m[0][2] + matrix4or3._m[2][0]) / S |
|---|
| 150 | Qz = (matrix4or3._m[1][2] + matrix4or3._m[2][1]) / S |
|---|
| 151 | |
|---|
| 152 | if column == 2: |
|---|
| 153 | S = math.sqrt(1.0 - matrix4or3._m[0][0] - matrix4or3._m[1][1] \ |
|---|
| 154 | + matrix4or3._m[2][2]) * 2 |
|---|
| 155 | Qz = 0.5 / S |
|---|
| 156 | Qw = (matrix4or3._m[0][1] + matrix4or3._m[1][0]) / S |
|---|
| 157 | Qx = (matrix4or3._m[0][2] + matrix4or3._m[2][0]) / S |
|---|
| 158 | Qy = (matrix4or3._m[1][2] + matrix4or3._m[2][1]) / S |
|---|
| 159 | |
|---|
| 160 | self.x = Qx |
|---|
| 161 | self.y = Qy |
|---|
| 162 | self.z = Qz |
|---|
| 163 | self.w = Qw |
|---|
| 164 | return self |
|---|
| 165 | |
|---|
| 166 | def toMatrix3(self): |
|---|
| 167 | m3 = Matrix.Matrix3() |
|---|
| 168 | x = self.x |
|---|
| 169 | y = self.y |
|---|
| 170 | z = self.z |
|---|
| 171 | w = self.w |
|---|
| 172 | m3._m[0][0] = 1 - 2 * y ** 2 - 2 * z ** 2 |
|---|
| 173 | m3._m[0][1] = 2 * x * y - 2 * z * w |
|---|
| 174 | m3._m[0][2] = 2 * x * z + 2 * y * w |
|---|
| 175 | m3._m[1][0] = 2 * x * y + 2 * z * w |
|---|
| 176 | m3._m[1][1] = 1 - 2 * x ** 2 - 2 * z ** 2 |
|---|
| 177 | m3._m[1][2] = 2 * y * z - 2 * x * w |
|---|
| 178 | m3._m[2][0] = 2 * x * z - 2 * y * w |
|---|
| 179 | m3._m[2][1] = 2 * y * z + 2 * x * w |
|---|
| 180 | m3._m[2][2] = 1 - 2 * x ** 2 - 2 * y ** 2 |
|---|
| 181 | return m3 |
|---|
| 182 | |
|---|
| 183 | def toMatrix4(self): |
|---|
| 184 | m4 = Matrix.Matrix4() |
|---|
| 185 | x = self.x |
|---|
| 186 | y = self.y |
|---|
| 187 | z = self.z |
|---|
| 188 | w = self.w |
|---|
| 189 | m4._m[0][0] = 1 - 2 * y ** 2 - 2 * z ** 2 |
|---|
| 190 | m4._m[0][1] = 2 * x * y - 2 * z * w |
|---|
| 191 | m4._m[0][2] = 2 * x * z + 2 * y * w |
|---|
| 192 | m4._m[1][0] = 2 * x * y + 2 * z * w |
|---|
| 193 | m4._m[1][1] = 1 - 2 * x ** 2 - 2 * z ** 2 |
|---|
| 194 | m4._m[1][2] = 2 * y * z - 2 * x * w |
|---|
| 195 | m4._m[2][0] = 2 * x * z - 2 * y * w |
|---|
| 196 | m4._m[2][1] = 2 * y * z + 2 * x * w |
|---|
| 197 | m4._m[2][2] = 1 - 2 * x ** 2 - 2 * y ** 2 |
|---|
| 198 | m4._m[0][3] = m4._m[1][3] = m4._m[2][3] = 0 |
|---|
| 199 | m4._m[3][0] = m4._m[3][1] = m4._m[3][2] = 0 |
|---|
| 200 | m4._m[3][3] = 1 |
|---|
| 201 | return m4 |
|---|
| 202 | |
|---|
| 203 | def fromEuler(self, vectororvals): |
|---|
| 204 | # The order of rotations is important |
|---|
| 205 | # If we apply a sequence of rotations, but change the order, the result is different |
|---|
| 206 | # For exemple: |
|---|
| 207 | # 1. Rotate 90gr about x axis ; 2. Rotate 90gr about y axis ; 3. Rotate -90gr about x axis |
|---|
| 208 | # This gives 90gr rotation about z axis |
|---|
| 209 | # whereas |
|---|
| 210 | # 1. Rotate 90gr about x axis ; 2. Rotate -90gr about x axis ; 3. Rotate 90gr about y axis |
|---|
| 211 | # This gives 90gr rotation about y axis |
|---|
| 212 | # So, I'll consider: |
|---|
| 213 | # x axis - vectorvals[0] |
|---|
| 214 | # y axis - vectorvals[1] |
|---|
| 215 | # z axis - vectorvals[2] |
|---|
| 216 | c1 = cos(vectorvals.y / 2) |
|---|
| 217 | c2 = cos(vectorvals.z / 2) |
|---|
| 218 | c3 = cos(vectorvals.x / 2) |
|---|
| 219 | s1 = sin(vectorvals.y / 2) |
|---|
| 220 | s2 = sin(vectorvals.z / 2) |
|---|
| 221 | s3 = sin(vectorvals.x / 2) |
|---|
| 222 | self.x = s1 * s2 * c3 + c1 * c2 * s3 |
|---|
| 223 | self.y = s1 * c2 * c3 + c1 * s2 * s3 |
|---|
| 224 | self.z = c1 * s2 * c3 - s1 * c2 * s3 |
|---|
| 225 | self.w = c1 * c2 * c3 - s1 * s2 * s3 |
|---|
| 226 | return self |
|---|
| 227 | |
|---|
| 228 | def toEuler(self): |
|---|
| 229 | vectorvals = Vector.Vector3() |
|---|
| 230 | x = self.x |
|---|
| 231 | y = self.y |
|---|
| 232 | z = self.z |
|---|
| 233 | w = self.w |
|---|
| 234 | # north pole |
|---|
| 235 | if x * y + z * w > 0.499: |
|---|
| 236 | vectorvals.y = 2 * math.atan2(x,w) |
|---|
| 237 | vectorvals.z = math.pi / 2 |
|---|
| 238 | vectorvals.x = 0 |
|---|
| 239 | else: |
|---|
| 240 | # south pole |
|---|
| 241 | if x * y + z * w < -0.499: |
|---|
| 242 | vectorvals.y = -2 * math.atan2(x,w) |
|---|
| 243 | vectorvals.z = -1 * math.pi / 2 |
|---|
| 244 | vectorvals.x = 0 |
|---|
| 245 | else: |
|---|
| 246 | vectorvals.y = math.atan2(2 * y * w - 2 * x * z, |
|---|
| 247 | 1 - 2 * y ** 2 - 2 * z ** 2) |
|---|
| 248 | vectorvals.z = math.asin(2 * x * y + 2 * z * w) |
|---|
| 249 | vectorvals.x = math.atan2(2 * x * w - 2 * y * z, |
|---|
| 250 | 1 - 2 * x ** 2 - 2 * z ** 2) |
|---|
| 251 | return vectorvals |
|---|
| 252 | |
|---|
| 253 | @staticmethod |
|---|
| 254 | def dot(Q1, Q2): |
|---|
| 255 | return Q1.x * Q2.x + Q1.y * Q2.y + Q1.z * Q2.z + Q1.w * Q2.w |
|---|
| 256 | |
|---|
| 257 | @staticmethod |
|---|
| 258 | def LERP(Q1, Q2, t): |
|---|
| 259 | """ |
|---|
| 260 | Linearly interpolate each component, then normalize the Quaternion |
|---|
| 261 | """ |
|---|
| 262 | return (Q1 * (1-t) + Q2 * t).normalize() |
|---|
| 263 | |
|---|
| 264 | @staticmethod |
|---|
| 265 | def SLERP(Q1, Q2, t): |
|---|
| 266 | """ |
|---|
| 267 | Spherical linear interpolation |
|---|
| 268 | """ |
|---|
| 269 | dot = Quaternion.dot(Q1, Q2) |
|---|
| 270 | if dot < 0: |
|---|
| 271 | dot = -dot |
|---|
| 272 | Q3 = -Q2 |
|---|
| 273 | else: |
|---|
| 274 | Q3 = Q2 |
|---|
| 275 | |
|---|
| 276 | if dot < 0.95: |
|---|
| 277 | angle = math.acos(dot) |
|---|
| 278 | w1 = math.sin(angle * (1-t)) / math.sin(angle) |
|---|
| 279 | w2 = math.sin(angle * t) / math.sin(angle) |
|---|
| 280 | return Q1 * w1 + Q3 * w2 |
|---|
| 281 | else: |
|---|
| 282 | # The angle is small. We use linear interpolation |
|---|
| 283 | Quaternion.LERP(Q1, Q3, t) |
|---|
