annotate python/bouncing_cube_opencl.py @ 252:c4370696ccc7

added optimize function
author Windel Bouwman
date Tue, 30 Jul 2013 17:57:46 +0200
parents 5a965e9664f2
children
rev   line source
97
windel
parents:
diff changeset
1 from PyQt4.QtGui import *
windel
parents:
diff changeset
2 from PyQt4.QtCore import *
windel
parents:
diff changeset
3 from PyQt4.QtOpenGL import QGLWidget
windel
parents:
diff changeset
4 from OpenGL.GL import *
windel
parents:
diff changeset
5 from OpenGL.GLU import gluPerspective
windel
parents:
diff changeset
6 import sys
windel
parents:
diff changeset
7 from random import random
windel
parents:
diff changeset
8 from math import pi, cos, sin, fabs, sqrt
windel
parents:
diff changeset
9 from numpy import mat, array, ones, zeros, eye
windel
parents:
diff changeset
10 from numpy.linalg import norm
windel
parents:
diff changeset
11 import numpy as np
windel
parents:
diff changeset
12 import time
windel
parents:
diff changeset
13 import scipy.integrate
windel
parents:
diff changeset
14 import pyopencl
windel
parents:
diff changeset
15
windel
parents:
diff changeset
16 """
windel
parents:
diff changeset
17 Test script that lets a dice bounce.
windel
parents:
diff changeset
18 Converted from 20-sim equations into python code.
windel
parents:
diff changeset
19
windel
parents:
diff changeset
20 20-sim website:
windel
parents:
diff changeset
21 http://www.20sim.com
windel
parents:
diff changeset
22
windel
parents:
diff changeset
23 """
windel
parents:
diff changeset
24 def drawCube(w):
windel
parents:
diff changeset
25 glBegin(GL_QUADS) # Start Drawing The Cube
windel
parents:
diff changeset
26 glColor3f(0.0,1.0,0.0) # Set The Color To Blue
windel
parents:
diff changeset
27 glVertex3f( w, w,-w) # Top Right Of The Quad (Top)
windel
parents:
diff changeset
28 glVertex3f(-w, w,-w) # Top Left Of The Quad (Top)
windel
parents:
diff changeset
29 glVertex3f(-w, w, w) # Bottom Left Of The Quad (Top)
windel
parents:
diff changeset
30 glVertex3f( w, w, w) # Bottom Right Of The Quad (Top)
windel
parents:
diff changeset
31
windel
parents:
diff changeset
32 glColor3f(1.0,0.5,0.0) # Set The Color To Orange
windel
parents:
diff changeset
33 glVertex3f( w,-w, w) # Top Right Of The Quad (Bottom)
windel
parents:
diff changeset
34 glVertex3f(-w,-w, w) # Top Left Of The Quad (Bottom)
windel
parents:
diff changeset
35 glVertex3f(-w,-w,-w) # Bottom Left Of The Quad (Bottom)
windel
parents:
diff changeset
36 glVertex3f( w,-w,-w) # Bottom Right Of The Quad (Bottom)
windel
parents:
diff changeset
37
windel
parents:
diff changeset
38 glColor3f(1.0,0.0,0.0) # Set The Color To Red
windel
parents:
diff changeset
39 glVertex3f( w, w, w) # Top Right Of The Quad (Front)
windel
parents:
diff changeset
40 glVertex3f(-w, w, w) # Top Left Of The Quad (Front)
windel
parents:
diff changeset
41 glVertex3f(-w,-w, w) # Bottom Left Of The Quad (Front)
windel
parents:
diff changeset
42 glVertex3f( w,-w, w) # Bottom Right Of The Quad (Front)
windel
parents:
diff changeset
43
windel
parents:
diff changeset
44 glColor3f(1.0,1.0,0.0) # Set The Color To Yellow
windel
parents:
diff changeset
45 glVertex3f( w,-w,-w) # Bottom Left Of The Quad (Back)
windel
parents:
diff changeset
46 glVertex3f(-w,-w,-w) # Bottom Right Of The Quad (Back)
windel
parents:
diff changeset
47 glVertex3f(-w, w,-w) # Top Right Of The Quad (Back)
windel
parents:
diff changeset
48 glVertex3f( w, w,-w) # Top Left Of The Quad (Back)
windel
parents:
diff changeset
49
windel
parents:
diff changeset
50 glColor3f(0.0,0.0,1.0) # Set The Color To Blue
windel
parents:
diff changeset
51 glVertex3f(-w, w, w) # Top Right Of The Quad (Left)
windel
parents:
diff changeset
52 glVertex3f(-w, w,-w) # Top Left Of The Quad (Left)
windel
parents:
diff changeset
53 glVertex3f(-w,-w,-w) # Bottom Left Of The Quad (Left)
windel
parents:
diff changeset
54 glVertex3f(-w,-w, w) # Bottom Right Of The Quad (Left)
windel
parents:
diff changeset
55
windel
parents:
diff changeset
56 glColor3f(1.0,0.0,1.0) # Set The Color To Violet
windel
parents:
diff changeset
57 glVertex3f( w, w,-w) # Top Right Of The Quad (Right)
windel
parents:
diff changeset
58 glVertex3f( w, w, w) # Top Left Of The Quad (Right)
windel
parents:
diff changeset
59 glVertex3f( w,-w, w) # Bottom Left Of The Quad (Right)
windel
parents:
diff changeset
60 glVertex3f( w,-w,-w) # Bottom Right Of The Quad (Right)
windel
parents:
diff changeset
61 glEnd() # Done Drawing The Quad
windel
parents:
diff changeset
62
windel
parents:
diff changeset
63 def drawFloor(w, h):
windel
parents:
diff changeset
64 glBegin(GL_QUADS) # Start Drawing The Cube
windel
parents:
diff changeset
65
windel
parents:
diff changeset
66 glColor3f(1.0,0.5,0.0) # Set The Color To Orange
windel
parents:
diff changeset
67 glVertex3f( w,-w,h)# Top Right Of The Quad (Bottom)
windel
parents:
diff changeset
68 glVertex3f(-w,-w,h)# Top Left Of The Quad (Bottom)
windel
parents:
diff changeset
69 glVertex3f(-w,w,h)# Bottom Left Of The Quad (Bottom)
windel
parents:
diff changeset
70 glVertex3f( w,w,h)# Bottom Right Of The Quad (Bottom)
windel
parents:
diff changeset
71 glEnd() # Done Drawing The Quad
windel
parents:
diff changeset
72
windel
parents:
diff changeset
73 def drawAxis():
windel
parents:
diff changeset
74 glLineWidth(0.5)
windel
parents:
diff changeset
75 glBegin(GL_LINES)
windel
parents:
diff changeset
76 glColor3f(1.0, 0.0, 0.0)
windel
parents:
diff changeset
77 glVertex3f(0,0,0)
windel
parents:
diff changeset
78 glVertex3f(1,0,0)
windel
parents:
diff changeset
79 glColor3f(0.0, 1.0, 0.0)
windel
parents:
diff changeset
80 glVertex3f(0,0,0)
windel
parents:
diff changeset
81 glVertex3f(0,1,0)
windel
parents:
diff changeset
82 glColor3f(0.0, 0.0, 1.0)
windel
parents:
diff changeset
83 glVertex3f(0,0,0)
windel
parents:
diff changeset
84 glVertex3f(0,0,1)
windel
parents:
diff changeset
85 glEnd()
windel
parents:
diff changeset
86
windel
parents:
diff changeset
87
windel
parents:
diff changeset
88 def cross(A, B):
windel
parents:
diff changeset
89 a = A.A1
windel
parents:
diff changeset
90 b = B.A1
windel
parents:
diff changeset
91 return mat(np.cross(a, b)).T
windel
parents:
diff changeset
92
windel
parents:
diff changeset
93 def skew(X):
windel
parents:
diff changeset
94 Y = mat(zeros( (3, 3) ))
windel
parents:
diff changeset
95 a,b,c = X.A1
windel
parents:
diff changeset
96 Y[0,1] = -c
windel
parents:
diff changeset
97 Y[0,2] = b
windel
parents:
diff changeset
98 Y[1,0] = c
windel
parents:
diff changeset
99 Y[1,2] = -a
windel
parents:
diff changeset
100 Y[2,0] = -b
windel
parents:
diff changeset
101 Y[2,1] = a
windel
parents:
diff changeset
102 return Y
windel
parents:
diff changeset
103
windel
parents:
diff changeset
104 def adjoint(T):
windel
parents:
diff changeset
105 W = T[0:3, 0]
windel
parents:
diff changeset
106 V = T[3:6, 0]
windel
parents:
diff changeset
107 a = mat(zeros( (6,6) ) )
windel
parents:
diff changeset
108 a[0:3, 0:3] = skew(W)
windel
parents:
diff changeset
109 a[3:6, 0:3] = skew(V)
windel
parents:
diff changeset
110 a[3:6, 3:6] = skew(W)
windel
parents:
diff changeset
111 return a
windel
parents:
diff changeset
112
windel
parents:
diff changeset
113 def Adjoint(H):
windel
parents:
diff changeset
114 R = H[0:3, 0:3]
windel
parents:
diff changeset
115 P = H[0:3, 3]
windel
parents:
diff changeset
116 a = mat(zeros( (6,6) ) )
windel
parents:
diff changeset
117 a[0:3, 0:3] = R
windel
parents:
diff changeset
118 a[3:6, 3:6] = R
windel
parents:
diff changeset
119 a[3:6, 0:3] = skew(P) * R
windel
parents:
diff changeset
120 return a
windel
parents:
diff changeset
121
windel
parents:
diff changeset
122 def quatToR(q):
windel
parents:
diff changeset
123 x, y, z, w = q.A1
windel
parents:
diff changeset
124 r = mat(eye(3))
windel
parents:
diff changeset
125 r[0,0] = 1 - (2*y**2+2*z**2)
windel
parents:
diff changeset
126 r[0,1] = 2*x*y+2*z*w
windel
parents:
diff changeset
127 r[0,2] = 2*x*z - 2*y*w
windel
parents:
diff changeset
128 r[1,0] = 2*x*y-2*z*w
windel
parents:
diff changeset
129 r[1,1] = 1 - (2*x**2 + 2*z**2)
windel
parents:
diff changeset
130 r[1,2] = 2*y*z + 2*x*w
windel
parents:
diff changeset
131 r[2,0] = 2*x*z+2*y*w
windel
parents:
diff changeset
132 r[2,1] = 2*y*z - 2*x*w
windel
parents:
diff changeset
133 r[2,2] = 1 - (2*x**2+2*y**2)
windel
parents:
diff changeset
134 return r
windel
parents:
diff changeset
135
windel
parents:
diff changeset
136 def rotateAbout(axis, angle):
windel
parents:
diff changeset
137 ax, ay, az = (axis/norm(axis)).A1
windel
parents:
diff changeset
138 qx = ax*sin(angle/2.0)
windel
parents:
diff changeset
139 qy = ay*sin(angle/2.0)
windel
parents:
diff changeset
140 qz = az*sin(angle/2.0)
windel
parents:
diff changeset
141 qw = cos(angle/2.0)
windel
parents:
diff changeset
142 q = mat(array([qx,qy,qz,qw])).T
windel
parents:
diff changeset
143 return q
windel
parents:
diff changeset
144
windel
parents:
diff changeset
145 def normalizeQuaternion(quat):
windel
parents:
diff changeset
146 x,y,z,w = quat.A1
windel
parents:
diff changeset
147 magnitude = sqrt(x*x + y*y + z*z + w*w)
windel
parents:
diff changeset
148 x = x / magnitude
windel
parents:
diff changeset
149 y = y / magnitude
windel
parents:
diff changeset
150 z = z / magnitude
windel
parents:
diff changeset
151 w = w / magnitude
windel
parents:
diff changeset
152 quat[0, 0] = x
windel
parents:
diff changeset
153 quat[1, 0] = y
windel
parents:
diff changeset
154 quat[2, 0] = z
windel
parents:
diff changeset
155 quat[3, 0] = w
windel
parents:
diff changeset
156 return quat
windel
parents:
diff changeset
157
windel
parents:
diff changeset
158 def VTo4x4(V):
windel
parents:
diff changeset
159 v1, v2, v3 = V.A1
windel
parents:
diff changeset
160 return mat(array( \
windel
parents:
diff changeset
161 [[0.0, -v3, v2, -v1], \
windel
parents:
diff changeset
162 [ v3, 0.0, -v1, -v2], \
windel
parents:
diff changeset
163 [-v2, v1, 0.0, -v3], \
windel
parents:
diff changeset
164 [v1, v2, v3, 0.0] ]))
windel
parents:
diff changeset
165
windel
parents:
diff changeset
166 def homogeneous(R,p):
windel
parents:
diff changeset
167 H = mat(eye(4))
windel
parents:
diff changeset
168 H[0:3, 0:3] = R
windel
parents:
diff changeset
169 H[0:3, 3] = p
windel
parents:
diff changeset
170 return H
windel
parents:
diff changeset
171
windel
parents:
diff changeset
172 def rateOfChange(states, thetime, parameters):
windel
parents:
diff changeset
173 quat = states[0:4, 0] # Orientation (4)
windel
parents:
diff changeset
174 pos = states[4:7, 0] # Position (3)
windel
parents:
diff changeset
175 P = states[7:13, 0] # Momentum (6)
windel
parents:
diff changeset
176 massI, gravity = parameters
windel
parents:
diff changeset
177 # Rigid body parts:
windel
parents:
diff changeset
178 # Forward Kinematic chain:
windel
parents:
diff changeset
179 H = homogeneous(quatToR(quat), pos) # Forward kinematics
windel
parents:
diff changeset
180
windel
parents:
diff changeset
181 AdjX2 = mat(eye(6)) # The connectionpoint in the real world
windel
parents:
diff changeset
182 adjAdjX2_1 = adjoint(AdjX2[0:6,0])
windel
parents:
diff changeset
183 adjAdjX2_2 = adjoint(AdjX2[0:6,1])
windel
parents:
diff changeset
184 adjAdjX2_3 = adjoint(AdjX2[0:6,2])
windel
parents:
diff changeset
185 adjAdjX2_4 = adjoint(AdjX2[0:6,3])
windel
parents:
diff changeset
186 adjAdjX2_5 = adjoint(AdjX2[0:6,4])
windel
parents:
diff changeset
187 adjAdjX2_6 = adjoint(AdjX2[0:6,5])
windel
parents:
diff changeset
188 AdjInv2 = Adjoint(H.I)
windel
parents:
diff changeset
189 M2 = AdjInv2.T * (massI * AdjInv2) # Transfor mass to base frame
windel
parents:
diff changeset
190 MassMatrix = M2
windel
parents:
diff changeset
191
windel
parents:
diff changeset
192 wrenchGrav2 = mat( zeros((1,6)) )
windel
parents:
diff changeset
193 wrenchGrav2[0, 0:3] = -cross(gravity, pos).T
windel
parents:
diff changeset
194 wrenchGrav2[0, 3:6] = gravity.T
windel
parents:
diff changeset
195
windel
parents:
diff changeset
196 Bk = mat( zeros( (6,6) ))
windel
parents:
diff changeset
197 Bk[0:3, 0:3] = skew(P[0:3, 0])
windel
parents:
diff changeset
198 Bk[3:6, 0:3] = skew(P[3:6, 0])
windel
parents:
diff changeset
199 Bk[0:3, 3:6] = skew(P[3:6, 0])
windel
parents:
diff changeset
200
windel
parents:
diff changeset
201 # TODO: do this a cholesky:
windel
parents:
diff changeset
202 v = np.linalg.solve(MassMatrix, P) # Matrix inverse like thingy !
windel
parents:
diff changeset
203
windel
parents:
diff changeset
204 T2_00 = v # Calculate the relative twist!
windel
parents:
diff changeset
205 TM2 = T2_00.T * M2
windel
parents:
diff changeset
206 twistExternal = T2_00 # Twist van het blokje
windel
parents:
diff changeset
207 TMSum2 = TM2
windel
parents:
diff changeset
208
windel
parents:
diff changeset
209 PDotBodies = mat( zeros( (6,1)) )
windel
parents:
diff changeset
210 PDotBodies[0,0] = TMSum2 * (adjAdjX2_1 * T2_00)
windel
parents:
diff changeset
211 PDotBodies[1,0] = TMSum2 * (adjAdjX2_2 * T2_00)
windel
parents:
diff changeset
212 PDotBodies[2,0] = TMSum2 * (adjAdjX2_3 * T2_00)
windel
parents:
diff changeset
213 PDotBodies[3,0] = TMSum2 * (adjAdjX2_4 * T2_00)
windel
parents:
diff changeset
214 PDotBodies[4,0] = TMSum2 * (adjAdjX2_5 * T2_00)
windel
parents:
diff changeset
215 PDotBodies[5,0] = TMSum2 * (adjAdjX2_6 * T2_00)
windel
parents:
diff changeset
216
windel
parents:
diff changeset
217 PDot = -PDotBodies - Bk * v
windel
parents:
diff changeset
218 PDot += wrenchGrav2.T
windel
parents:
diff changeset
219
windel
parents:
diff changeset
220 ##### Contact wrench part:
windel
parents:
diff changeset
221 HB_W = H # Is H-matrix van het blokje
windel
parents:
diff changeset
222 WrenchB = mat(zeros( (1,6) ))
windel
parents:
diff changeset
223 for px in [-0.5, 0.5]:
windel
parents:
diff changeset
224 for py in [-0.5, 0.5]:
windel
parents:
diff changeset
225 for pz in [-0.5, 0.5]:
windel
parents:
diff changeset
226 HB1_B = homogeneous(mat(eye(3)), mat([px,py,pz]).T)
windel
parents:
diff changeset
227 HB1_W = HB_W * HB1_B
windel
parents:
diff changeset
228 HW1_W = homogeneous(mat(eye(3)), HB1_W[0:3,3])
windel
parents:
diff changeset
229 HW_W1 = HW1_W.I
windel
parents:
diff changeset
230 HB_W1 = HW_W1 * HB_W
windel
parents:
diff changeset
231
windel
parents:
diff changeset
232 AdjHB_W1 = Adjoint(HB_W1)
windel
parents:
diff changeset
233 TB_W1_W1 = AdjHB_W1 * twistExternal
windel
parents:
diff changeset
234 z = HB1_W[2, 3]
windel
parents:
diff changeset
235 vx, vy, vz = TB_W1_W1[3:6, 0].A1
windel
parents:
diff changeset
236 if z < 0:
windel
parents:
diff changeset
237 # Contact forces:
windel
parents:
diff changeset
238 Fx = -50.0*vx
windel
parents:
diff changeset
239 Fy = -50.0*vy
windel
parents:
diff changeset
240 Fz = -z*50000.0
windel
parents:
diff changeset
241 else:
windel
parents:
diff changeset
242 Fx = 0.0
windel
parents:
diff changeset
243 Fy = 0.0
windel
parents:
diff changeset
244 Fz = 0.0
windel
parents:
diff changeset
245 # TODO: reflect impulse
windel
parents:
diff changeset
246 WrenchW1 = mat([0,0,0,0,0,Fz])
windel
parents:
diff changeset
247 # Transform it back:
windel
parents:
diff changeset
248 WrenchB += (AdjHB_W1.T * WrenchW1.T).T
windel
parents:
diff changeset
249 ##### End of contact wrench
windel
parents:
diff changeset
250
windel
parents:
diff changeset
251 PDot += (WrenchB * AdjInv2).T
windel
parents:
diff changeset
252
windel
parents:
diff changeset
253 # Position and orientation rates:
windel
parents:
diff changeset
254 QOmega = VTo4x4(v[0:3, 0])
windel
parents:
diff changeset
255 quatDot = 0.5 * QOmega * quat
windel
parents:
diff changeset
256 vel = v[3:6, 0]
windel
parents:
diff changeset
257 posDot = skew(v[0:3]) * pos + vel
windel
parents:
diff changeset
258 # The rate vector:
windel
parents:
diff changeset
259 rates = mat(zeros( (13,1) ))
windel
parents:
diff changeset
260 rates[0:4, 0] = quatDot
windel
parents:
diff changeset
261 rates[4:7, 0] = posDot
windel
parents:
diff changeset
262 rates[7:13, 0] = PDot
windel
parents:
diff changeset
263 return rates
windel
parents:
diff changeset
264
windel
parents:
diff changeset
265 def fWrapper(y, t, parameters):
windel
parents:
diff changeset
266 y = mat(y).T
windel
parents:
diff changeset
267 dy = rateOfChange(y, t, parameters)
windel
parents:
diff changeset
268 return dy.T.A1
windel
parents:
diff changeset
269
windel
parents:
diff changeset
270 def SCIPY(endtime, dt, state, parameters):
windel
parents:
diff changeset
271 times = np.arange(0.0, endtime, dt)
windel
parents:
diff changeset
272 y0 = state.T.A1
windel
parents:
diff changeset
273 res = scipy.integrate.odeint(fWrapper, y0, times, args=(parameters,))
windel
parents:
diff changeset
274 states = []
windel
parents:
diff changeset
275 res = res.T
windel
parents:
diff changeset
276 r,c = res.shape
windel
parents:
diff changeset
277 for ci in range(0,c):
windel
parents:
diff changeset
278 states.append(mat(res[:,ci]).T)
windel
parents:
diff changeset
279 return states
windel
parents:
diff changeset
280
windel
parents:
diff changeset
281 def RK4(endtime, dt, state, parameters):
windel
parents:
diff changeset
282 t = 0.0
windel
parents:
diff changeset
283 states = []
windel
parents:
diff changeset
284 while t < endtime:
windel
parents:
diff changeset
285 newstate = mat (zeros( state.shape )) # Create a new object
windel
parents:
diff changeset
286
windel
parents:
diff changeset
287 #### Runge Kutta integration:
windel
parents:
diff changeset
288 k1 = rateOfChange(state, t, parameters)
windel
parents:
diff changeset
289 k2 = rateOfChange(state + 0.5*dt*k1, t, parameters)
windel
parents:
diff changeset
290 k3 = rateOfChange(state + 0.5*dt*k1, t, parameters)
windel
parents:
diff changeset
291 k4 = rateOfChange(state + dt*k3, t, parameters)
windel
parents:
diff changeset
292 newstate = state + (dt/6.0)*(k1+2*k2+2*k3+k4)
windel
parents:
diff changeset
293
windel
parents:
diff changeset
294 # Normalize quat:
windel
parents:
diff changeset
295 newstate[0:4, 0] = normalizeQuaternion(newstate[0:4, 0])
windel
parents:
diff changeset
296 states.append(newstate)
windel
parents:
diff changeset
297
windel
parents:
diff changeset
298 state = newstate
windel
parents:
diff changeset
299
windel
parents:
diff changeset
300 t += dt
windel
parents:
diff changeset
301 print state[6,0], t, ' ', (t/endtime)*100.0, '%'
windel
parents:
diff changeset
302 return states
windel
parents:
diff changeset
303
windel
parents:
diff changeset
304
windel
parents:
diff changeset
305 def simulate(endtime, dt):
windel
parents:
diff changeset
306 PInitial = mat( zeros((6,1)) )
windel
parents:
diff changeset
307 posInitial = mat(array([-1.2, -1.3, 2.8])).T
windel
parents:
diff changeset
308 quatInitial = rotateAbout(mat(array([0.2, 1.0, 0.4])).T, 0.5)
windel
parents:
diff changeset
309 # Parameters:
windel
parents:
diff changeset
310 gravity = mat( array([0,0,-9.81]) ).T
windel
parents:
diff changeset
311 massI = mat(eye(6)) * 1.01 # Mass matrix
windel
parents:
diff changeset
312 parameters = (massI, gravity)
windel
parents:
diff changeset
313
windel
parents:
diff changeset
314 # The state vector!
windel
parents:
diff changeset
315 state = mat(zeros( (13,1) ))
windel
parents:
diff changeset
316 state[0:4, 0] = quatInitial
windel
parents:
diff changeset
317 state[4:7, 0] = posInitial
windel
parents:
diff changeset
318 state[7:13, 0] = PInitial
windel
parents:
diff changeset
319
windel
parents:
diff changeset
320 return SCIPY(endtime, dt, state, parameters)
windel
parents:
diff changeset
321
windel
parents:
diff changeset
322 class W(QGLWidget):
windel
parents:
diff changeset
323 time = 0.0
windel
parents:
diff changeset
324 index = 0
windel
parents:
diff changeset
325 def __init__(self, states, dt, parent=None):
windel
parents:
diff changeset
326 super(W, self).__init__(parent)
windel
parents:
diff changeset
327 self.firstRun = True
windel
parents:
diff changeset
328 self.savePNGS = False
windel
parents:
diff changeset
329 self.dt = dt
windel
parents:
diff changeset
330 self.resize(500,500)
windel
parents:
diff changeset
331 self.states = states
windel
parents:
diff changeset
332 self.UP()
windel
parents:
diff changeset
333 t = QTimer(self)
windel
parents:
diff changeset
334 t.timeout.connect(self.UP)
windel
parents:
diff changeset
335 t.start(self.dt*1000.0)
windel
parents:
diff changeset
336
windel
parents:
diff changeset
337 def UP(self):
windel
parents:
diff changeset
338 self.time += self.dt
windel
parents:
diff changeset
339 if self.states:
windel
parents:
diff changeset
340 state = self.states[self.index]
windel
parents:
diff changeset
341 self.Dicequat = state[0:4, 0]
windel
parents:
diff changeset
342 self.Dicepos = state[4:7, 0]
windel
parents:
diff changeset
343 self.index += 1
windel
parents:
diff changeset
344 if self.index == len(self.states):
windel
parents:
diff changeset
345 self.index = 0
windel
parents:
diff changeset
346 self.firstRun = False
windel
parents:
diff changeset
347 # Paint:
windel
parents:
diff changeset
348 self.update()
windel
parents:
diff changeset
349 if self.firstRun:
windel
parents:
diff changeset
350 # Create png images for the movie:
windel
parents:
diff changeset
351 if self.savePNGS:
windel
parents:
diff changeset
352 pm = self.renderPixmap()
windel
parents:
diff changeset
353 pm.save('image'+str(self.index)+'.png')
windel
parents:
diff changeset
354
windel
parents:
diff changeset
355 def initializeGL(self):
windel
parents:
diff changeset
356 glClearColor(0.0, 0.5, 0.0, 1.0)
windel
parents:
diff changeset
357 glEnable(GL_DEPTH_TEST)
windel
parents:
diff changeset
358 glDepthFunc(GL_LESS)
windel
parents:
diff changeset
359 glShadeModel(GL_SMOOTH)
windel
parents:
diff changeset
360 def resizeGL(self,w,h):
windel
parents:
diff changeset
361 glViewport(0, 0, w, h)
windel
parents:
diff changeset
362 glMatrixMode(GL_PROJECTION)
windel
parents:
diff changeset
363 glLoadIdentity()
windel
parents:
diff changeset
364 gluPerspective(45.0, float(w)/float(h), 0.1, 100.0)
windel
parents:
diff changeset
365 glMatrixMode(GL_MODELVIEW)
windel
parents:
diff changeset
366 def paintGL(self):
windel
parents:
diff changeset
367 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) # Clear buffers
windel
parents:
diff changeset
368 glLoadIdentity() # Reset The View
windel
parents:
diff changeset
369
windel
parents:
diff changeset
370 glLoadIdentity()
windel
parents:
diff changeset
371 glTranslatef(0.0,-2.0,-10.0) # Move Left And Into The Screen
windel
parents:
diff changeset
372 glRotatef(-90.0, 1.0, 0.0, 0.0)
windel
parents:
diff changeset
373 drawFloor(2.0, 0.0)
windel
parents:
diff changeset
374 drawAxis()
windel
parents:
diff changeset
375 self.renderText(1.0, 0.0, 0.0, 'X')
windel
parents:
diff changeset
376 self.renderText(0.0, 1.0, 0.0, 'Y')
windel
parents:
diff changeset
377 self.renderText(0.0, 0.0, 1.0, 'Z')
windel
parents:
diff changeset
378
windel
parents:
diff changeset
379 self.renderText(0.0,0.0,1.2,str(self.time))
windel
parents:
diff changeset
380
windel
parents:
diff changeset
381 x,y,z = self.Dicepos.A1
windel
parents:
diff changeset
382 R = quatToR(self.Dicequat)
windel
parents:
diff changeset
383
windel
parents:
diff changeset
384 glTranslatef(x, y, z)
windel
parents:
diff changeset
385 # Trick to rotate the openGL matrix:
windel
parents:
diff changeset
386 r = R.A1
windel
parents:
diff changeset
387 rotR = (r[0], r[3], r[6], 0.0,
windel
parents:
diff changeset
388 r[1], r[4], r[7], 0.0,
windel
parents:
diff changeset
389 r[2], r[5], r[8], 0.0,
windel
parents:
diff changeset
390 0.0, 0.0, 0.0, 1.0)
windel
parents:
diff changeset
391 glMultMatrixd(rotR)
windel
parents:
diff changeset
392
windel
parents:
diff changeset
393 drawCube(0.6)
windel
parents:
diff changeset
394
windel
parents:
diff changeset
395 et = 20.0
windel
parents:
diff changeset
396 dt = 0.04
windel
parents:
diff changeset
397 print 'starting integration... endtime =', et, ' stepsize =', dt
windel
parents:
diff changeset
398 t0 = time.time()
windel
parents:
diff changeset
399 states = simulate(et, dt)
windel
parents:
diff changeset
400 t1 = time.time()
windel
parents:
diff changeset
401 print 'That was heavy, it took me ', t1-t0, ' seconds!'
windel
parents:
diff changeset
402 app = QApplication(sys.argv)
windel
parents:
diff changeset
403 w = W(states, dt)
windel
parents:
diff changeset
404 w.show()
windel
parents:
diff changeset
405 sys.exit(app.exec_())
windel
parents:
diff changeset
406