Statistics
| Revision:

root / PyOpenGL-Demo / 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 IEEE-754(GLfloat), which i believe has max precision of 7 bits
43
Epsilon = 1.0e-5
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 non-zero
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 in-place 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 88-92
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/module-doctest.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 ()