root / PyOpenGLDemo / NeHe / lesson48 / ArcBall.py @ 1
History  View  Annotate  Download (9.6 kB)
1 
"""


2 
ArcBall.py  Math utilities, vector, matrix types and ArcBall quaternion rotation class

3 

4 
>>> unit_test_ArcBall_module ()

5 
unit testing ArcBall

6 
Quat for first drag

7 
[ 0.08438914 0.08534209 0.06240178 0.99080837]

8 
First transform

9 
[[ 0.97764552 0.1380603 0.15858325 0. ]

10 
[ 0.10925253 0.97796899 0.17787792 0. ]

11 
[0.17964739 0.15657592 0.97119039 0. ]

12 
[ 0. 0. 0. 1. ]]

13 
LastRot at end of first drag

14 
[[ 0.97764552 0.1380603 0.15858325]

15 
[ 0.10925253 0.97796899 0.17787792]

16 
[0.17964739 0.15657592 0.97119039]]

17 
Quat for second drag

18 
[ 0.00710336 0.31832787 0.02679029 0.94757545]

19 
Second transform

20 
[[ 0.88022292 0.08322023 0.46720669 0. ]

21 
[ 0.14910145 0.98314685 0.10578787 0. ]

22 
[ 0.45052907 0.16277808 0.8777966 0. ]

23 
[ 0. 0. 0. 1.00000001]]

24 
"""

25  
26 
try:

27 
import numpy as Numeric 
28 
def sumDot( a,b ): 
29 
return Numeric.dot (a, b)

30 
except ImportError, err: 
31 
try:

32 
import Numeric 
33 
def sumDot( a,b ): 
34 
return sum (Numeric.dot (a, b) ) 
35 
except ImportError, err: 
36 
print "This demo requires the numpy or Numeric extension, sorry" 
37 
import sys 
38 
sys.exit() 
39 
import copy 
40 
from math import sqrt 
41  
42 
# //assuming IEEE754(GLfloat), which i believe has max precision of 7 bits

43 
Epsilon = 1.0e5

44  
45  
46 
class ArcBallT: 
47 
def __init__ (self, NewWidth, NewHeight): 
48 
self.m_StVec = Vector3fT ()

49 
self.m_EnVec = Vector3fT ()

50 
self.m_AdjustWidth = 1.0 
51 
self.m_AdjustHeight = 1.0 
52 
self.setBounds (NewWidth, NewHeight)

53  
54 
def __str__ (self): 
55 
str_rep = ""

56 
str_rep += "StVec = " + str (self.m_StVec) 
57 
str_rep += "\nEnVec = " + str (self.m_EnVec) 
58 
str_rep += "\n scale coords %f %f" % (self.m_AdjustWidth, self.m_AdjustHeight) 
59 
return str_rep

60  
61 
def setBounds (self, NewWidth, NewHeight): 
62 
# //Set new bounds

63 
assert (NewWidth > 1.0 and NewHeight > 1.0), "Invalid width or height for bounds." 
64 
# //Set adjustment factor for width/height

65 
self.m_AdjustWidth = 1.0 / ((NewWidth  1.0) * 0.5) 
66 
self.m_AdjustHeight = 1.0 / ((NewHeight  1.0) * 0.5) 
67  
68 
def _mapToSphere (self, NewPt): 
69 
# Given a new window coordinate, will modify NewVec in place

70 
X = 0

71 
Y = 1

72 
Z = 2

73  
74 
NewVec = Vector3fT () 
75 
# //Copy paramter into temp point

76 
TempPt = copy.copy (NewPt) 
77 
print 'NewPt', NewPt, TempPt 
78 
# //Adjust point coords and scale down to range of [1 ... 1]

79 
TempPt [X] = (NewPt [X] * self.m_AdjustWidth)  1.0 
80 
TempPt [Y] = 1.0  (NewPt [Y] * self.m_AdjustHeight) 
81 
# //Compute the square of the length of the vector to the point from the center

82 
length = sumDot( TempPt, TempPt) 
83 
# //If the point is mapped outside of the sphere... (length > radius squared)

84 
if (length > 1.0): 
85 
# //Compute a normalizing factor (radius / sqrt(length))

86 
norm = 1.0 / sqrt (length);

87  
88 
# //Return the "normalized" vector, a point on the sphere

89 
NewVec [X] = TempPt [X] * norm; 
90 
NewVec [Y] = TempPt [Y] * norm; 
91 
NewVec [Z] = 0.0;

92 
else: # //Else it's on the inside 
93 
# //Return a vector to a point mapped inside the sphere sqrt(radius squared  length)

94 
NewVec [X] = TempPt [X] 
95 
NewVec [Y] = TempPt [Y] 
96 
NewVec [Z] = sqrt (1.0  length)

97  
98 
return NewVec

99  
100 
def click (self, NewPt): 
101 
# //Mouse down (Point2fT

102 
self.m_StVec = self._mapToSphere (NewPt) 
103 
return

104  
105 
def drag (self, NewPt): 
106 
# //Mouse drag, calculate rotation (Point2fT Quat4fT)

107 
""" drag (Point2fT mouse_coord) > new_quaternion_rotation_vec

108 
"""

109 
X = 0

110 
Y = 1

111 
Z = 2

112 
W = 3

113  
114 
self.m_EnVec = self._mapToSphere (NewPt) 
115  
116 
# //Compute the vector perpendicular to the begin and end vectors

117 
# Perp = Vector3fT ()

118 
Perp = Vector3fCross(self.m_StVec, self.m_EnVec); 
119  
120 
NewRot = Quat4fT () 
121 
# //Compute the length of the perpendicular vector

122 
if (Vector3fLength(Perp) > Epsilon): # //if its nonzero 
123 
# //We're ok, so return the perpendicular vector as the transform after all

124 
NewRot[X] = Perp[X]; 
125 
NewRot[Y] = Perp[Y]; 
126 
NewRot[Z] = Perp[Z]; 
127 
# //In the quaternion values, w is cosine (theta / 2), where theta is rotation angle

128 
NewRot[W] = Vector3fDot(self.m_StVec, self.m_EnVec); 
129 
else: # //if its zero 
130 
# //The begin and end vectors coincide, so return a quaternion of zero matrix (no rotation)

131 
NewRot.X = NewRot.Y = NewRot.Z = NewRot.W = 0.0;

132 

133 
return NewRot

134  
135  
136 
# ##################### Math utility ##########################################

137  
138  
139 
def Matrix4fT (): 
140 
return Numeric.identity (4, 'f') 
141  
142 
def Matrix3fT (): 
143 
return Numeric.identity (3, 'f') 
144  
145 
def Quat4fT (): 
146 
return Numeric.zeros (4, 'f') 
147  
148 
def Vector3fT (): 
149 
return Numeric.zeros (3, 'f') 
150  
151 
def Point2fT (x = 0.0, y = 0.0): 
152 
pt = Numeric.zeros (2, 'f') 
153 
pt [0] = x

154 
pt [1] = y

155 
return pt

156  
157 
def Vector3fDot(u, v): 
158 
# Dot product of two 3f vectors

159 
dotprod = Numeric.dot (u,v) 
160 
return dotprod

161  
162 
def Vector3fCross(u, v): 
163 
# Cross product of two 3f vectors

164 
X = 0

165 
Y = 1

166 
Z = 2

167 
cross = Numeric.zeros (3, 'f') 
168 
cross [X] = (u[Y] * v[Z])  (u[Z] * v[Y]) 
169 
cross [Y] = (u[Z] * v[X])  (u[X] * v[Z]) 
170 
cross [Z] = (u[X] * v[Y])  (u[Y] * v[X]) 
171 
return cross

172  
173 
def Vector3fLength (u): 
174 
mag_squared = sumDot(u,u) 
175 
mag = sqrt (mag_squared) 
176 
return mag

177 

178 
def Matrix3fSetIdentity (): 
179 
return Numeric.identity (3, 'f') 
180  
181 
def Matrix3fMulMatrix3f (matrix_a, matrix_b): 
182 
return sumDot( matrix_a, matrix_b )

183  
184  
185  
186  
187 
def Matrix4fSVD (NewObj): 
188 
X = 0

189 
Y = 1

190 
Z = 2

191 
s = sqrt ( 
192 
( (NewObj [X][X] * NewObj [X][X]) + (NewObj [X][Y] * NewObj [X][Y]) + (NewObj [X][Z] * NewObj [X][Z]) + 
193 
(NewObj [Y][X] * NewObj [Y][X]) + (NewObj [Y][Y] * NewObj [Y][Y]) + (NewObj [Y][Z] * NewObj [Y][Z]) + 
194 
(NewObj [Z][X] * NewObj [Z][X]) + (NewObj [Z][Y] * NewObj [Z][Y]) + (NewObj [Z][Z] * NewObj [Z][Z]) ) / 3.0 )

195 
return s

196  
197 
def Matrix4fSetRotationScaleFromMatrix3f(NewObj, three_by_three_matrix): 
198 
# Modifies NewObj inplace by replacing its upper 3x3 portion from the

199 
# passed in 3x3 matrix.

200 
# NewObj = Matrix4fT ()

201 
NewObj [0:3,0:3] = three_by_three_matrix 
202 
return NewObj

203  
204 
# /**

205 
# * Sets the rotational component (upper 3x3) of this matrix to the matrix

206 
# * values in the T precision Matrix3d argument; the other elements of

207 
# * this matrix are unchanged; a singular value decomposition is performed

208 
# * on this object's upper 3x3 matrix to factor out the scale, then this

209 
# * object's upper 3x3 matrix components are replaced by the passed rotation

210 
# * components, and then the scale is reapplied to the rotational

211 
# * components.

212 
# * @param three_by_three_matrix T precision 3x3 matrix

213 
# */

214 
def Matrix4fSetRotationFromMatrix3f (NewObj, three_by_three_matrix): 
215 
scale = Matrix4fSVD (NewObj) 
216  
217 
NewObj = Matrix4fSetRotationScaleFromMatrix3f(NewObj, three_by_three_matrix); 
218 
scaled_NewObj = NewObj * scale # Matrix4fMulRotationScale(NewObj, scale);

219 
return scaled_NewObj

220  
221 
def Matrix3fSetRotationFromQuat4f (q1): 
222 
# Converts the H quaternion q1 into a new equivalent 3x3 rotation matrix.

223 
X = 0

224 
Y = 1

225 
Z = 2

226 
W = 3

227  
228 
NewObj = Matrix3fT () 
229 
n = sumDot(q1, q1) 
230 
s = 0.0

231 
if (n > 0.0): 
232 
s = 2.0 / n

233 
xs = q1 [X] * s; ys = q1 [Y] * s; zs = q1 [Z] * s 
234 
wx = q1 [W] * xs; wy = q1 [W] * ys; wz = q1 [W] * zs 
235 
xx = q1 [X] * xs; xy = q1 [X] * ys; xz = q1 [X] * zs 
236 
yy = q1 [Y] * ys; yz = q1 [Y] * zs; zz = q1 [Z] * zs 
237 
# This math all comes about by way of algebra, complex math, and trig identities.

238 
# See Lengyel pages 8892

239 
NewObj [X][X] = 1.0  (yy + zz); NewObj [Y][X] = xy  wz; NewObj [Z][X] = xz + wy;

240 
NewObj [X][Y] = xy + wz; NewObj [Y][Y] = 1.0  (xx + zz); NewObj [Z][Y] = yz  wx;

241 
NewObj [X][Z] = xz  wy; NewObj [Y][Z] = yz + wx; NewObj [Z][Z] = 1.0  (xx + yy)

242  
243 
return NewObj

244  
245  
246  
247  
248  
249  
250 
def unit_test_ArcBall_module (): 
251 
# Unit testing of the ArcBall calss and the real math behind it.

252 
# Simulates a click and drag followed by another click and drag.

253 
print "unit testing ArcBall" 
254 
Transform = Matrix4fT () 
255 
LastRot = Matrix3fT () 
256 
ThisRot = Matrix3fT () 
257  
258 
ArcBall = ArcBallT (640, 480) 
259 
# print "The ArcBall with NO click"

260 
# print ArcBall

261 
# First click

262 
LastRot = copy.copy (ThisRot) 
263 
mouse_pt = Point2fT (500,250) 
264 
ArcBall.click (mouse_pt) 
265 
# print "The ArcBall with first click"

266 
# print ArcBall

267 
# First drag

268 
mouse_pt = Point2fT (475, 275) 
269 
ThisQuat = ArcBall.drag (mouse_pt) 
270 
# print "The ArcBall after first drag"

271 
# print ArcBall

272 
# print

273 
# print

274 
print "Quat for first drag" 
275 
print ThisQuat

276 
ThisRot = Matrix3fSetRotationFromQuat4f (ThisQuat) 
277 
# Linear Algebra matrix multiplication A = old, B = New : C = A * B

278 
ThisRot = Matrix3fMulMatrix3f (LastRot, ThisRot) 
279 
Transform = Matrix4fSetRotationFromMatrix3f (Transform, ThisRot) 
280 
print "First transform" 
281 
print Transform

282 
# Done with first drag

283  
284  
285 
# second click

286 
LastRot = copy.copy (ThisRot) 
287 
print "LastRot at end of first drag" 
288 
print LastRot

289 
mouse_pt = Point2fT (350,260) 
290 
ArcBall.click (mouse_pt) 
291 
# second drag

292 
mouse_pt = Point2fT (450, 260) 
293 
ThisQuat = ArcBall.drag (mouse_pt) 
294 
# print "The ArcBall"

295 
# print ArcBall

296 
print "Quat for second drag" 
297 
print ThisQuat

298 
ThisRot = Matrix3fSetRotationFromQuat4f (ThisQuat) 
299 
ThisRot = Matrix3fMulMatrix3f (LastRot, ThisRot) 
300 
# print ThisRot

301 
Transform = Matrix4fSetRotationFromMatrix3f (Transform, ThisRot) 
302 
print "Second transform" 
303 
print Transform

304 
# Done with second drag

305 
LastRot = copy.copy (ThisRot) 
306  
307 
def _test (): 
308 
# This will run doctest's unit testing capability.

309 
# see http://www.python.org/doc/current/lib/moduledoctest.html

310 
#

311 
# doctest introspects the ArcBall module for all docstrings

312 
# that look like interactive python sessions and invokes

313 
# the same commands then and there as unit tests to compare

314 
# the output generated. Very nice for unit testing and

315 
# documentation.

316 
import doctest, ArcBall 
317 
return doctest.testmod (ArcBall)

318  
319 
if __name__ == "__main__": 
320 
# Invoke our function that runs python's doctest unit testing tool.

321 
_test () 
322  
323 
# unit_test ()
