9 Mar
2006
9 Mar
'06
2:57 a.m.
Just curious ... what improvements have you been able to make to Jasmine?
3D models use alot of matrix multiplies... Usually this is done with three physical matrices. Each multiply creates a new matrix for the result... If the code does many many multiplies such as a multi-jointed 3D model and camera motions, then quite a bit of memory gets burned, (8x16 bytes + headers for each discarded matrix). I wrapped one of the primatives and made a "multiply and discard" method which overwrote one of the operands with the result. This new code can replace many instances of the old 3-way call hence reducing the time spent in the GC...
I also identified two methods in the matrix class which did the exact same thing, I removed the one with the larger call graph and re-named the other. (it had fewer senders...)
Most of my changes dealt with these two items...
/ me tried to exparament with Big Mesh teapot but ran into the flaming signed pointer bug... =P
the video drivers I curently have installed has some texture bugs too. =\
reviewing my changes, I see I did a lot of scope demotions too. Unfortunately the compiler doesn't seem to honor scope in such a way that it could be a performance boost. =\ (If a variable is only used in a conditional branch, it doesn't need to exist unless the branch is executed... )
I attached the version I currently have on my system. Note the removed methods.
--
Don't let your schoolwork get in the way of your learning.
http://users.rcn.com/alangrimes/
'From Jasmine-rc1 of 7 October 2004 [latest update: #280] on 8 March 2006 at 8:56:29 pm'!
B3DFloatArray variableWordSubclass: #B3DMatrix4x4
instanceVariableNames: ''
classVariableNames: 'B3DIdentityMatrix B3DZeroMatrix '
poolDictionaries: ''
category: 'Balloon3D-Kernel-Vectors'!
!B3DMatrix4x4 commentStamp: '<historical>' prior: 0!
I represent a general 4x4 transformation matrix commonly used in computer graphics.!
!B3DMatrix4x4 methodsFor: '*Croquet' stamp: 'das 9/17/2003 13:09'!
localBoxToGlobal: aTBox
^TBox min: (self localPointToGlobal: aTBox min) max: (self localPointToGlobal: aTBox max).! !
!B3DMatrix4x4 methodsFor: '*Croquet' stamp: 'ar 10/1/2004 09:53'!
orientation
"This is used to return just the orientation part of the matrix.The translation part is 0.0."
| mat |
mat _ self clone.
mat translationX: 0.0 y: 0.0 z: 0.0.
^ mat.! !
!B3DMatrix4x4 methodsFor: 'accessing' stamp: 'jsp 2/11/1999 14:09'!
at: i at: j
^ self at: ((i - 1) * 4 + j).
! !
!B3DMatrix4x4 methodsFor: 'accessing' stamp: 'jsp 2/11/1999 14:09'!
at: i at: j put: aValue
^ self at: ((i - 1) * 4 + j) put: aValue.
! !
!B3DMatrix4x4 methodsFor: 'accessing' stamp: 'atg 10/17/2005 10:11'!
pitchYawRoll
"Assume the receiver describes an orthonormal 3x3 matrix"
^ self a23 negated arcSin radiansToDegrees @ (self a13 arcTan: self a33) radiansToDegrees @ ( self a21 arcTan: self a22) radiansToDegrees! !
!B3DMatrix4x4 methodsFor: 'accessing' stamp: 'atg 10/13/2005 19:55'!
pitchYawRoll: ypr
"Assume the receiver describes an orthonormal 3x3 matrix"
self
loadFrom: (((self class identity rotationAroundZ: ypr z)
multiplyAndDiscard: ((self class identity rotationAroundY: ypr y)
multiplyAndDiscard: (self class identity rotationAroundX: ypr x)))
translation: self translation)! !
!B3DMatrix4x4 methodsFor: 'accessing' stamp: 'atg 10/18/2005 18:37'!
rotation
"Return the angular rotation around each axis of the matrix"
| vRow1 vRow2 vRow3 vAngles |
vRow1 _ self row1.
vRow1 normalize.
vRow2 _ self row2.
vRow2 _ vRow2 + (vRow1 * (vRow1 dot: vRow2) negated).
vRow2 normalize.
vRow3 _ self row3.
vRow3 _ vRow3 + (vRow1 * (vRow1 dot: vRow3) negated).
vRow3 _ vRow3 + (vRow2 * (vRow2 dot: vRow3) negated).
vRow3 normalize.
(vRow1
dot: (vRow2 cross: vRow3))
< 0.0
ifTrue: [vRow1 _ vRow1 negated.
vRow2 _ vRow2 negated.
vRow3 _ vRow3 negated].
vAngles _ B3DVector3 new.
vAngles at: 2 put: (vRow1 at: 3) negated arcSin.
(vAngles at: 2) cos ~= 0.0
ifTrue: [vAngles
at: 1
put: ((vRow2 at: 3)
arcTan: (vRow3 at: 3)).
vAngles
at: 3
put: ((vRow1 at: 2)
arcTan: (vRow1 at: 1))]
ifFalse: [vAngles
at: 1
put: ((vRow2 at: 1)
arcTan: (vRow2 at: 2)).
vAngles at: 3 put: 0.0].
vAngles at: 1 put: (vAngles at: 1) radiansToDegrees.
vAngles at: 2 put: (vAngles at: 2) radiansToDegrees.
vAngles at: 3 put: (vAngles at: 3) radiansToDegrees.
^ vAngles! !
!B3DMatrix4x4 methodsFor: 'accessing' stamp: 'jsp 2/24/1999 09:46'!
rotation: aVector
| xRot yRot zRot cosPitch sinPitch cosYaw sinYaw cosRoll sinRoll |
xRot _ (aVector x) degreesToRadians.
yRot _ (aVector y) degreesToRadians.
zRot _ (aVector z) degreesToRadians.
cosPitch _ xRot cos.
sinPitch _ xRot sin.
cosYaw _ yRot cos.
sinYaw _ yRot sin.
cosRoll _ zRot cos.
sinRoll _ zRot sin.
self a11: (cosRoll*cosYaw).
self a12: (sinRoll*cosYaw).
self a13: (sinYaw negated).
self a21: ((cosRoll*sinYaw*sinPitch) - (sinRoll*cosPitch)).
self a22: ((cosRoll*cosPitch) + (sinRoll*sinYaw*sinPitch)).
self a23: (cosYaw*sinPitch).
self a31: ((cosRoll*sinYaw*cosPitch) + (sinRoll*sinPitch)).
self a32: ((sinRoll*sinYaw*cosPitch) - (cosRoll*sinPitch)).
self a33: (cosYaw*cosPitch).
^ self.
! !
!B3DMatrix4x4 methodsFor: 'accessing'!
rotation: anAngle around: aVector3
"set up a rotation matrix around the direction aVector3"
self loadFrom: (B3DRotation angle: anAngle axis: aVector3) asMatrix4x4! !
!B3DMatrix4x4 methodsFor: 'accessing'!
rotation: anAngle aroundX: xValue y: yValue z: zValue
"set up a rotation matrix around the direction x/y/z"
^self rotation: anAngle around:(B3DVector3 with: xValue with: yValue with: zValue)! !
!B3DMatrix4x4 methodsFor: 'accessing' stamp: 'ar 2/1/1999 21:34'!
rotationAroundX: anAngle
| rad s c |
rad := anAngle degreesToRadians.
s := rad sin.
c := rad cos.
self a22: c.
self a23: s negated.
self a33: c.
self a32: s.
^self! !
!B3DMatrix4x4 methodsFor: 'accessing' stamp: 'ar 2/1/1999 21:34'!
rotationAroundY: anAngle
| rad s c |
rad := anAngle degreesToRadians.
s := rad sin.
c := rad cos.
self a11: c.
self a13: s.
self a33: c.
self a31: s negated.
^self! !
!B3DMatrix4x4 methodsFor: 'accessing' stamp: 'ar 2/1/1999 21:35'!
rotationAroundZ: anAngle
| rad s c |
rad := anAngle degreesToRadians.
s := rad sin.
c := rad cos.
self a11: c.
self a12: s negated.
self a22: c.
self a21: s.
^self! !
!B3DMatrix4x4 methodsFor: 'accessing' stamp: 'ar 4/16/1999 21:51'!
squaredDistanceFrom: aMatrix
| sum |
sum _ 0.0.
1 to: 4 do:[:i|
1 to: 4 do:[:j|
sum _ sum + ((self at: i at: j) - (aMatrix at: i at: j)) squared]].
^sum! !
!B3DMatrix4x4 methodsFor: 'accessing'!
translation
^(B3DVector3 x: self a14 y: self a24 z: self a34)! !
!B3DMatrix4x4 methodsFor: 'accessing' stamp: 'atg 10/12/2005 21:51'!
translationX: xValue y: yValue z: zValue
^ self a14: xValue;
a24: yValue;
a34: zValue! !
!B3DMatrix4x4 methodsFor: 'arithmetic' stamp: 'ar 2/2/2001 15:47'!
+ aB3DMatrix
"Optimized for Matrix/Matrix operations"
<primitive: 'primitiveAddFloatArray' module: 'FloatArrayPlugin'>
^super + aB3DMatrix! !
!B3DMatrix4x4 methodsFor: 'arithmetic' stamp: 'ar 2/2/2001 15:47'!
- aB3DMatrix
"Optimized for Matrix/Matrix operations"
<primitive: 'primitiveSubFloatArray' module: 'FloatArrayPlugin'>
^super - aB3DMatrix! !
!B3DMatrix4x4 methodsFor: 'comparing' stamp: 'ar 2/1/1999 21:53'!
squaredErrorDistanceTo: anotherMatrix
| result temp |
result := self - anotherMatrix.
temp := 0.
1 to: 4 do: [:i | 1 to: 4 do: [:j| temp := temp + ((result at: i-1*4+j) squared)]].
^temp sqrt.! !
!B3DMatrix4x4 methodsFor: 'converting'!
asMatrix4x4
^self! !
!B3DMatrix4x4 methodsFor: 'converting' stamp: 'jsp 3/5/1999 15:31'!
asQuaternion
"Convert the matrix to a quaternion"
| x y z a a2 x2 y2 a4 |
a2 _ 0.25 * (1.0 + (self a11) + (self a22) + (self a33)).
(a2 > 0) ifTrue: [
a _ a2 sqrt.
a4 _ 4.0 * a.
x _ ((self a32) - (self a23)) / a4.
y _ ((self a13) - (self a31)) / a4.
z _ ((self a21) - (self a12)) / a4.
]
ifFalse: [
a _ 0.
x2 _ -0.5 * ((self a22) + (self a33)).
(x2 > 0) ifTrue: [
x _ x2 sqrt.
x2 _ 2 * x.
y _ (self a21) / x2.
z _ (self a31) / x2.
]
ifFalse: [
x _ 0.
y2 _ 0.5 * (1.0 - (self a33)).
(y2 > 0) ifTrue: [
y _ y2 sqrt.
y2 _ 2 * y.
z _ (self a32) / y2.
]
ifFalse: [
y _ 0.0.
z _ 1.0.
]
]
].
^ (B3DRotation a: a b: x c: y d: z).
! !
!B3DMatrix4x4 methodsFor: 'double dispatching' stamp: 'ar 2/1/1999 21:49'!
printOn: aStream
"Print the receiver on aStream"
1 to: 4 do:[:r|
1 to: 4 do:[:c|
(self at: r-1*4+c) printOn: aStream.
aStream nextPut: Character space].
(r < 4) ifTrue:[aStream nextPut: Character cr]].! !
!B3DMatrix4x4 methodsFor: 'double dispatching' stamp: 'ar 2/8/1999 20:11'!
productFromMatrix4x4: matrix
"Multiply a 4x4 matrix with the receiver."
| result |
result := self class new.
result a11: ((matrix a11 * self a11) + (matrix a12 * self a21) +
(matrix a13 * self a31) + (matrix a14 * self a41)).
result a12: ((matrix a11 * self a12) + (matrix a12 * self a22) +
(matrix a13 * self a32) + (matrix a14 * self a42)).
result a13: ((matrix a11 * self a13) + (matrix a12 * self a23) +
(matrix a13 * self a33) + (matrix a14 * self a43)).
result a14: ((matrix a11 * self a14) + (matrix a12 * self a24) +
(matrix a13 * self a34) + (matrix a14 * self a44)).
result a21: ((matrix a21 * self a11) + (matrix a22 * self a21) +
(matrix a23 * self a31) + (matrix a24 * self a41)).
result a22: ((matrix a21 * self a12) + (matrix a22 * self a22) +
(matrix a23 * self a32) + (matrix a24 * self a42)).
result a23: ((matrix a21 * self a13) + (matrix a22 * self a23) +
(matrix a23 * self a33) + (matrix a24 * self a43)).
result a24: ((matrix a21 * self a14) + (matrix a22 * self a24) +
(matrix a23 * self a34) + (matrix a24 * self a44)).
result a31: ((matrix a31 * self a11) + (matrix a32 * self a21) +
(matrix a33 * self a31) + (matrix a34 * self a41)).
result a32: ((matrix a31 * self a12) + (matrix a32 * self a22) +
(matrix a33 * self a32) + (matrix a34 * self a42)).
result a33: ((matrix a31 * self a13) + (matrix a32 * self a23) +
(matrix a33 * self a33) + (matrix a34 * self a43)).
result a34: ((matrix a31 * self a14) + (matrix a32 * self a24) +
(matrix a33 * self a34) + (matrix a34 * self a44)).
result a41: ((matrix a41 * self a11) + (matrix a42 * self a21) +
(matrix a43 * self a31) + (matrix a44 * self a41)).
result a42: ((matrix a41 * self a12) + (matrix a42 * self a22) +
(matrix a43 * self a32) + (matrix a44 * self a42)).
result a43: ((matrix a41 * self a13) + (matrix a42 * self a23) +
(matrix a43 * self a33) + (matrix a44 * self a43)).
result a44: ((matrix a41 * self a14) + (matrix a42 * self a24) +
(matrix a43 * self a34) + (matrix a44 * self a44)).
^result! !
!B3DMatrix4x4 methodsFor: 'double dispatching'!
productFromVector3: aVector3
"Multiply aVector (temporarily converted to 4D) with the receiver"
| x y z rx ry rz rw |
x := aVector3 x.
y := aVector3 y.
z := aVector3 z.
rx := (x * self a11) + (y * self a21) + (z * self a31) + self a41.
ry := (x * self a12) + (y * self a22) + (z * self a32) + self a42.
rz := (x * self a13) + (y * self a23) + (z * self a33) + self a43.
rw := (x * self a14) + (y * self a24) + (z * self a34) + self a44.
^B3DVector3 x:(rx/rw) y: (ry/rw) z: (rz/rw)! !
!B3DMatrix4x4 methodsFor: 'double dispatching'!
productFromVector4: aVector4
"Multiply aVector with the receiver"
| x y z w rx ry rz rw |
x := aVector4 x.
y := aVector4 y.
z := aVector4 z.
w := aVector4 w.
rx := (x * self a11) + (y * self a21) + (z * self a31) + (w * self a41).
ry := (x * self a12) + (y * self a22) + (z * self a32) + (w * self a42).
rz := (x * self a13) + (y * self a23) + (z * self a33) + (w * self a43).
rw := (x * self a14) + (y * self a24) + (z * self a34) + (w * self a44).
^B3DVector4 x:rx y: ry z: rz w: rw! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:29'!
a11
"Return the element a11"
^self at: 1! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:28'!
a11: aNumber
"Store the element a11"
self at: 1 put: aNumber! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:29'!
a12
"Return the element a12"
^self at: 2! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:28'!
a12: aNumber
"Store the element a12"
self at: 2 put: aNumber! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:29'!
a13
"Return the element a13"
^self at: 3! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:28'!
a13: aNumber
"Store the element a13"
self at: 3 put: aNumber! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:29'!
a14
"Return the element a14"
^self at: 4! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:28'!
a14: aNumber
"Store the element a14"
self at: 4 put: aNumber! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:29'!
a21
"Return the element a21"
^self at: 5! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:28'!
a21: aNumber
"Store the element a21"
self at: 5 put: aNumber! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:29'!
a22
"Return the element a22"
^self at: 6! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:28'!
a22: aNumber
"Store the element a22"
self at: 6 put: aNumber! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:29'!
a23
"Return the element a23"
^self at: 7! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:28'!
a23: aNumber
"Store the element a23"
self at: 7 put: aNumber! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:29'!
a24
"Return the element a24"
^self at: 8! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:28'!
a24: aNumber
"Store the element a24"
self at: 8 put: aNumber! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:29'!
a31
"Return the element a31"
^self at: 9! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:28'!
a31: aNumber
"Store the element a31"
self at: 9 put: aNumber! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:29'!
a32
"Return the element a32"
^self at: 10! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:28'!
a32: aNumber
"Store the element a32"
self at: 10 put: aNumber! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:29'!
a33
"Return the element a33"
^self at: 11! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:28'!
a33: aNumber
"Store the element a33"
self at: 11 put: aNumber! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:29'!
a34
"Return the element a34"
^self at: 12! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:28'!
a34: aNumber
"Store the element a34"
self at: 12 put: aNumber! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:29'!
a41
"Return the element a41"
^self at: 13! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:28'!
a41: aNumber
"Store the element a41"
self at: 13 put: aNumber! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:29'!
a42
"Return the element a42"
^self at: 14! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:28'!
a42: aNumber
"Store the element a42"
self at: 14 put: aNumber! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:29'!
a43
"Return the element a43"
^self at: 15! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:28'!
a43: aNumber
"Store the element a43"
self at: 15 put: aNumber! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:29'!
a44
"Return the element a44"
^self at: 16! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'ar 2/1/1999 21:28'!
a44: aNumber
"Store the element a44"
self at: 16 put: aNumber! !
!B3DMatrix4x4 methodsFor: 'element-access' stamp: 'atg 10/13/2005 08:21'!
scalingX: xValue y: yValue z: zValue
^self a11: self a11 * xValue;
a22: self a22 * yValue;
a33: self a33 * zValue.! !
!B3DMatrix4x4 methodsFor: 'initialize' stamp: 'ar 2/1/1999 21:26'!
setBSplineBase
"Set the receiver to the BSpline base matrix"
"for further information see:
Foley, van Dam, Feiner, Hughes
'Computer Graphics: Principles and Practice'
Addison-Wesley Publishing Company
Second Edition, pp. 505"
self
a11: -1.0 / 6.0; a12: 3.0 / 6.0; a13: -3.0 / 6.0; a14: 1.0 / 6.0;
a21: 3.0 / 6.0; a22: -6.0 / 6.0; a23: 3.0 / 6.0; a24: 0.0 / 6.0;
a31: -3.0 / 6.0; a32: 0.0 / 6.0; a33: 3.0 / 6.0; a34: 0.0 / 6.0;
a41: 1.0 / 6.0; a42: 4.0 / 6.0; a43: 1.0 / 6.0; a44: 0.0 / 6.0
! !
!B3DMatrix4x4 methodsFor: 'initialize' stamp: 'ar 2/1/1999 21:26'!
setBetaSplineBaseBias: beta1 tension: beta2
"Set the receiver to the betaSpline base matrix
if beta1=1 and beta2=0 then the bSpline base matrix will be returned"
"for further information see:
Foley, van Dam, Feiner, Hughes
'Computer Graphics: Principles and Practice'
Addison-Wesley Publishing Company
Second Edition, pp. 505"
| b12 b13 delta |
b12 := beta1 * beta1.
b13 := beta1 * b12.
delta := 1.0 / (beta2 + (2.0 * b13) + 4.0 * (b12 + beta1) +2.0).
self
a11: delta * -2.0 * b13;
a12: delta * 2.0 * (beta2 + b13 + b12 + beta1);
a13: delta * -2.0 * (beta2 + b12 + beta1 + 1.0);
a14: delta * 2.0;
a21: delta * 6.0 * b13;
a22: delta * -3.0 * (beta2 + (2.0 * (b13 + b12)));
a23: delta * 3.0 * (beta2 + (2.0 * b12));
a24: 0.0;
a31: delta * -6.0 * b13;
a32: delta * 6.0 * (b13 - beta1);
a33: delta * 6.0 * beta1;
a34: 0.0;
a41: delta * 2.0 * b13;
a42: delta * (beta2 + 4.0 * (b12 + beta1));
a43: delta * 2.0;
a44: 0.0
! !
!B3DMatrix4x4 methodsFor: 'initialize' stamp: 'ar 2/1/1999 21:27'!
setBezierBase
"Set the receiver to the bezier base matrix"
"for further information see:
Foley, van Dam, Feiner, Hughes
'Computer Graphics: Principles and Practice'
Addison-Wesley Publishing Company
Second Edition, pp. 505"
self
a11: -1.0; a12: 3.0; a13: -3.0; a14: 1.0;
a21: 3.0; a22: -6.0; a23: 3.0; a24: 0.0;
a31: -3.0; a32: 3.0; a33: 0.0; a34: 0.0;
a41: 1.0; a42: 0.0; a43: 0.0; a44: 0.0! !
!B3DMatrix4x4 methodsFor: 'initialize' stamp: 'ar 2/1/1999 21:27'!
setCardinalBase
"Set the receiver to the cardinal spline base matrix - just catmull * 2"
"for further information see:
Foley, van Dam, Feiner, Hughes
'Computer Graphics: Principles and Practice'
Addison-Wesley Publishing Company
Second Edition, pp. 505"
self
a11: -1.0; a12: 3.0; a13: -3.0; a14: 1.0;
a21: 2.0; a22: -5.0; a23: 4.0; a24: -1.0;
a31: -1.0; a32: 0.0; a33: 1.0; a34: 0.0;
a41: 0.0; a42: 2.0; a43: 0.0; a44: 0.0
! !
!B3DMatrix4x4 methodsFor: 'initialize' stamp: 'ar 2/1/1999 21:27'!
setCatmullBase
"Set the receiver to the Catmull-Rom base matrix"
"for further information see:
Foley, van Dam, Feiner, Hughes
'Computer Graphics: Principles and Practice'
Addison-Wesley Publishing Company
Second Edition, pp. 505"
self
a11: -0.5; a12: 1.5; a13: -1.5; a14: 0.5;
a21: 1.0; a22: -2.5; a23: 2.0; a24: -0.5;
a31: -0.5; a32: 0.0; a33: 0.5; a34: 0.0;
a41: 0.0; a42: 1.0; a43: 0.0; a44: 0.0
! !
!B3DMatrix4x4 methodsFor: 'initialize'!
setIdentity
"Set the receiver to the identity matrix"
self loadFrom: B3DIdentityMatrix! !
!B3DMatrix4x4 methodsFor: 'initialize' stamp: 'ar 2/1/1999 21:27'!
setPolylineBase
"Set the receiver to the polyline base matrix :)"
self
a11: 0.0; a12: 0.0; a13: 0.0; a14: 0.0;
a21: 0.0; a22: 0.0; a23: 0.0; a24: 0.0;
a31: 0.0; a32: -1.0; a33: 1.0; a34: 0.0;
a41: 0.0; a42: 1.0; a43: 0.0; a44: 0.0
! !
!B3DMatrix4x4 methodsFor: 'initialize' stamp: 'ar 2/15/1999 02:55'!
setScale: aVector
self
a11: aVector x;
a22: aVector y;
a33: aVector z! !
!B3DMatrix4x4 methodsFor: 'initialize'!
setZero
"Set the receiver to the zero matrix"
self loadFrom: B3DZeroMatrix! !
!B3DMatrix4x4 methodsFor: 'initialize' stamp: 'das 5/16/2002 11:11'!
skew: vector
"Set the skew-symetric matrix up"
self a21: vector z.
self a12: vector z negated.
self a31: vector y negated.
self a13: vector y.
self a32: vector x.
self a23: vector x negated.
! !
!B3DMatrix4x4 methodsFor: 'initialize' stamp: 'das 2/6/2005 03:53'!
up: u at: a
| side up at |
side _ (a cross: u) normalized negated.
up _ (side cross: a) normalized negated.
at _ a normalized.
self a11: side x.
self a21: side y.
self a31: side z.
self a12: up x.
self a22: up y.
self a32: up z.
self a13: at x.
self a23: at y.
self a33: at z.
self a44: 1.0.
! !
!B3DMatrix4x4 methodsFor: 'row-access' stamp: 'das 12/11/2002 14:35'!
column1
"Return column 1"
^ (B3DVector3 x: (self a11) y: (self a21) z: (self a31)).
! !
!B3DMatrix4x4 methodsFor: 'row-access' stamp: 'das 12/11/2002 14:37'!
column1: col
"Set column 1"
self a11: col x.
self a21: col y.
self a31: col z.
! !
!B3DMatrix4x4 methodsFor: 'row-access' stamp: 'das 12/11/2002 14:36'!
column2
"Return column 2"
^ (B3DVector3 x: (self a12) y: (self a22) z: (self a32)).
! !
!B3DMatrix4x4 methodsFor: 'row-access' stamp: 'das 12/11/2002 14:37'!
column2: col
"Set column 2"
self a12: col x.
self a22: col y.
self a32: col z.
! !
!B3DMatrix4x4 methodsFor: 'row-access' stamp: 'das 12/11/2002 14:37'!
column3
"Return column 3"
^ (B3DVector3 x: (self a13) y: (self a23) z: (self a33)).
! !
!B3DMatrix4x4 methodsFor: 'row-access' stamp: 'das 12/11/2002 14:37'!
column3: col
"Set column 3"
self a13: col x.
self a23: col y.
self a33: col z.
! !
!B3DMatrix4x4 methodsFor: 'row-access' stamp: 'das 10/19/2004 22:00'!
lookAt
"Return column 3"
^ (B3DVector3 x: (self a13) y: (self a23) z: (self a33)).
! !
!B3DMatrix4x4 methodsFor: 'row-access' stamp: 'das 10/19/2004 22:01'!
lookSide
"Return column 1"
^ (B3DVector3 x: (self a11) y: (self a21) z: (self a31)).
! !
!B3DMatrix4x4 methodsFor: 'row-access' stamp: 'das 10/19/2004 22:01'!
lookUp
"Return column 2"
^ (B3DVector3 x: (self a12) y: (self a22) z: (self a32)).
! !
!B3DMatrix4x4 methodsFor: 'row-access' stamp: 'jsp 2/24/1999 17:10'!
row1
"Return row 1"
^ (B3DVector3 x: (self a11) y: (self a12) z: (self a13)).
! !
!B3DMatrix4x4 methodsFor: 'row-access' stamp: 'das 9/24/2002 10:27'!
row1: row
"Set row 1"
self a11: row x.
self a12: row y.
self a13: row z.
! !
!B3DMatrix4x4 methodsFor: 'row-access' stamp: 'jsp 2/24/1999 17:11'!
row2
"Return row 2"
^ (B3DVector3 x: (self a21) y: (self a22) z: (self a23)).
! !
!B3DMatrix4x4 methodsFor: 'row-access' stamp: 'das 9/24/2002 10:28'!
row2: row
"Set row 2"
self a21: row x.
self a22: row y.
self a23: row z.
! !
!B3DMatrix4x4 methodsFor: 'row-access' stamp: 'jsp 2/24/1999 17:11'!
row3
"Return row 3"
^ (B3DVector3 x: (self a31) y: (self a32) z: (self a33)).
! !
!B3DMatrix4x4 methodsFor: 'row-access' stamp: 'das 9/24/2002 10:29'!
row3: row
"Set row 3"
self a31: row x.
self a32: row y.
self a33: row z.
! !
!B3DMatrix4x4 methodsFor: 'solving' stamp: 'atg 10/18/2005 19:16'!
inplaceDecomposeLU
"Decompose the receiver in place by using gaussian elimination w/o
pivot search"
1
to: 4
do: [:j | "i-th equation (row)"
j + 1
to: 4
do: [:i | | x |
x _ (self at: i at: j)
/ (self at: j at: j).
j
to: 4
do: [:k | self
at: i
at: k
put: (self at: i at: k)
- ((self at: j at: k)
* x)].
self
at: i
at: j
put: x]]! !
!B3DMatrix4x4 methodsFor: 'solving' stamp: 'ar 5/22/2000 17:13'!
inplaceHouseHolderInvert
"Solve the linear equation self * aVector = x by using HouseHolder's transformation.
Note: This scheme is numerically better than using gaussian elimination even though it takes
somewhat longer"
| d x sigma beta sum s|
<primitive:'b3dInplaceHouseHolderInvert' module:'Squeak3D'>
x _ B3DMatrix4x4 identity.
d _ B3DMatrix4x4 new.
1 to: 4 do:[:j|
sigma := 0.0.
j to: 4 do:[:i| sigma := sigma + ((self at: i at: j) squared)].
sigma isZero ifTrue:[^nil]. "matrix is singular"
((self at: j at: j) < 0.0)
ifTrue:[ s:= sigma sqrt]
ifFalse:[ s:= sigma sqrt negated].
1 to: 4 do:[:r| d at: j at: r put: s].
beta := 1.0 / ( s * (self at: j at: j) - sigma).
self at: j at: j put: ((self at: j at: j) - s).
"update remaining columns"
j+1 to: 4 do:[:k|
sum := 0.0.
j to: 4 do:[:i| sum := sum + ((self at: i at: j) * (self at: i at: k))].
sum := sum * beta.
j to: 4 do:[:i|
self at: i at: k put: ((self at: i at: k) + ((self at: i at: j) * sum))]].
"update vector"
1 to: 4 do:[:r|
sum := nil.
j to: 4 do:[:i|
sum := sum isNil
ifTrue:[(x at: i at: r) * (self at: i at: j)]
ifFalse:[sum + ((x at: i at: r) * (self at: i at: j))]].
sum := sum * beta.
j to: 4 do:[:i|
x at: i at: r put:((x at: i at: r) + (sum * (self at: i at: j)))].
].
].
"Now calculate result"
1 to: 4 do:[:r|
4 to: 1 by: -1 do:[:i|
i+1 to: 4 do:[:j|
x at: i at: r put: ((x at: i at: r) - ((x at: j at: r) * (self at: i at: j))) ].
x at: i at: r put: ((x at: i at: r) / (d at: i at: r))].
].
self loadFrom: x.
"Return receiver"! !
!B3DMatrix4x4 methodsFor: 'solving'!
inplaceHouseHolderTransform: aVector
"Solve the linear equation self * aVector = x by using HouseHolder's transformation.
Note: This scheme is numerically better than using gaussian elimination even though it takes
somewhat longer"
| d x sigma beta sum s|
x := Array with: aVector x with: aVector y with: aVector z with: aVector w.
d := Array new: 4.
1 to: 4 do:[:j|
sigma := 0.0.
j to: 4 do:[:i| sigma := sigma + ((self at: i at: j) squared)].
sigma isZero ifTrue:[^nil]. "matrix is singular"
((self at: j at: j) < 0.0)
ifTrue:[ s:= d at: j put: (sigma sqrt)]
ifFalse:[ s:= d at: j put: (sigma sqrt negated)].
beta := 1.0 / ( s * (self at: j at: j) - sigma).
self at: j at: j put: ((self at: j at: j) - s).
"update remaining columns"
j+1 to: 4 do:[:k|
sum := 0.0.
j to: 4 do:[:i| sum := sum + ((self at: i at: j) * (self at: i at: k))].
sum := sum * beta.
j to: 4 do:[:i|
self at: i at: k put: ((self at: i at: k) + ((self at: i at: j) * sum))]].
"update vector"
sum := nil.
j to: 4 do:[:i|
sum := sum isNil
ifTrue:[(x at: i) * (self at: i at: j)]
ifFalse:[sum + ((x at: i) * (self at: i at: j))]].
sum := sum * beta.
j to: 4 do:[:i|
x at: i put:((x at: i) + (sum * (self at: i at: j)))].
].
"Now calculate result"
4 to: 1 by: -1 do:[:i|
i+1 to: 4 do:[:j|
x at: i put: ((x at: i) - ((x at: j) * (self at: i at: j))) ].
x at: i put: ((x at: i) / (d at: i))].
^B3DVector4 x: (x at: 1) y: (x at: 2) z: (x at: 3) w: (x at: 4)
! !
!B3DMatrix4x4 methodsFor: 'solving' stamp: 'ar 6/23/2002 14:28'!
solve3x3: aVector
"Solve a 3x3 system of linear equations. Assume that all the a[4,x] and a[x,4] are zero.
NOTE: This is a hack, but it's the fastest way for now."
| m |
m := self clone.
m a44: 1. "need this for inversion"
m := m inplaceHouseHolderInvert.
m ifNil:[^nil].
^m localDirToGlobal: aVector.! !
!B3DMatrix4x4 methodsFor: 'solving' stamp: 'ar 2/1/1999 21:52'!
solve: aVector
^self clone inplaceHouseHolderTransform: aVector
"or:
^self clone inplaceDecomposeLU solveLU: aVector
"! !
!B3DMatrix4x4 methodsFor: 'solving'!
solveLU: aVector
"Given a decomposed matrix using gaussian elimination solve the linear equations."
| x v |
v := Array with: aVector x with: aVector y with: aVector z with: aVector w.
"L first"
1 to: 4 do:[:i| "Top to bottom"
x := 0.0.
1 to: i-1 do:[:j|
"From left to right w/o diagonal element"
x := x + ((v at: j) * (self at: i at: j))].
"No need to divide by the diagonal element - this is always 1.0 in L"
v at: i put: (v at: i) - x].
"Now U"
4 to: 1 by: -1 do:[:i| "Bottom to top"
x := 0.0.
4 to: i+1 by: -1 do:[:j|
"From right to left w/o diagonal element"
x := x + ((v at: j) * (self at: i at: j))].
"Divide by diagonal element"
v at: i put: (v at: i) - x / (self at: i at: i)].
^B3DVector4 x: (v at: 1) y: (v at: 2) z: (v at: 3) w: (v at: 4)
! !
!B3DMatrix4x4 methodsFor: 'testing' stamp: 'ar 2/1/1999 21:54'!
isIdentity
^self = B3DIdentityMatrix! !
!B3DMatrix4x4 methodsFor: 'testing' stamp: 'ar 2/1/1999 21:54'!
isZero
^self = B3DZeroMatrix! !
!B3DMatrix4x4 methodsFor: 'transforming' stamp: 'atg 10/13/2005 10:04'!
composeWith: aB3DMatrix4x4
" returns X = parameter * self "
| result |
result _ self class new.
self
privateTransformMatrix: self
with: aB3DMatrix4x4
into: result.
^ result! !
!B3DMatrix4x4 methodsFor: 'transforming' stamp: 'atg 10/13/2005 18:35'!
composeWith: m2 times: nTimes
"Perform a 4x4 matrix exponentiation and multiplication."
| result |
result _ self.
nTimes negative
ifTrue: [self halt].
nTimes >= 2
ifTrue: [result _ result
composeWith: (m2 composeWith: m2)
times: nTimes // 2].
nTimes \ 2 = 1
ifTrue: [result multiplyAndDiscard: m2].
^ result! !
!B3DMatrix4x4 methodsFor: 'transforming' stamp: 'ar 2/15/1999 23:56'!
composedWithGlobal: aB3DMatrix4x4
| result |
result _ self class new.
self privateTransformMatrix: aB3DMatrix4x4 with: self into: result.
^result! !
!B3DMatrix4x4 methodsFor: 'transforming' stamp: 'ar 5/21/2000 16:34'!
inverseTransformation
"Return the inverse matrix of the receiver."
^self clone inplaceHouseHolderInvert.! !
!B3DMatrix4x4 methodsFor: 'transforming' stamp: 'atg 10/18/2005 19:21'!
localDirToGlobal: aVector
"Multiply direction vector with the receiver"
| x y z |
<primitive: 'b3dTransformDirection' module:'Squeak3D'>
x _ aVector x.
y _ aVector y.
z _ aVector z.
^ B3DVector3
x: x * self a11 + (y * self a12) + (z * self a13)
y: x * self a21 + (y * self a22) + (z * self a23)
z: x * self a31 + (y * self a32) + (z * self a33)! !
!B3DMatrix4x4 methodsFor: 'transforming' stamp: 'ar 9/18/2002 17:30'!
localPointToGlobal: aVector
"Multiply aVector (temporarily converted to 4D) with the receiver"
| x y z rx ry rz rw |
<primitive: 'b3dTransformPoint' module: 'Squeak3D'>
x := aVector x.
y := aVector y.
z := aVector z.
rx := (x * self a11) + (y * self a12) + (z * self a13) + self a14.
ry := (x * self a21) + (y * self a22) + (z * self a23) + self a24.
rz := (x * self a31) + (y * self a32) + (z * self a33) + self a34.
rw := (x * self a41) + (y * self a42) + (z * self a43) + self a44.
^B3DVector3 x:(rx/rw) y: (ry/rw) z: (rz/rw)! !
!B3DMatrix4x4 methodsFor: 'transforming' stamp: 'atg 10/11/2005 01:09'!
multiplyAndDiscard: aB3DMatrix4x4
"derived from 'composedWithLocal'"
self
privateTransformMatrix: self
with: aB3DMatrix4x4
into: self! !
!B3DMatrix4x4 methodsFor: 'transforming' stamp: 'das 11/9/2004 17:14'!
normalize
self column1: (self column2 cross: self column3) normalized.
self column2: (self column3 cross: self column1) normalized.
self column3: (self column1 cross: self column2) normalized.! !
!B3DMatrix4x4 methodsFor: 'transforming' stamp: 'ar 9/18/2002 17:30'!
orthoNormInverse
| m x y z rx ry rz |
<primitive: 'b3dOrthoNormInverseMatrix' module: 'Squeak3D'>
m := self clone.
"transpose upper 3x3 matrix"
m a11: self a11; a12: self a21; a13: self a31.
m a21: self a12; a22: self a22; a23: self a32.
m a31: self a13; a32: self a23; a33: self a33.
"Compute inverse translation vector"
x := self a14.
y := self a24.
z := self a34.
rx := (x * m a11) + (y * m a12) + (z * m a13).
ry := (x * m a21) + (y * m a22) + (z * m a23).
rz := (x * m a31) + (y * m a32) + (z * m a33).
m a14: 0.0-rx; a24: 0.0-ry; a34: 0.0-rz.
^m
" Used to be:
m _ self clone.
v _ m translation.
m translation: B3DVector3 zero.
m _ m transposed.
v _ (m localPointToGlobal: v) negated.
m translation: v.
^ m.
"! !
!B3DMatrix4x4 methodsFor: 'transforming' stamp: 'atg 10/18/2005 19:12'!
quickTransformV3ArrayFrom: srcArray to: dstArray
"Transform the 3 element vertices from srcArray to dstArray.
ASSUMPTION: a41 = a42 = a43 = 0.0 and a44 = 1.0"
| a11 a12 a13 a14 a21 a22 a23 a24 a31 a32 a33 a34 |
self flag: #b3dPrimitive.
a11 _ self a11.
a12 _ self a12.
a13 _ self a13.
a14 _ self a14.
a21 _ self a21.
a22 _ self a22.
a23 _ self a23.
a24 _ self a24.
a31 _ self a31.
a32 _ self a32.
a33 _ self a33.
a34 _ self a34.
1
to: srcArray size
do: [:i |
| x y z index |
index _ i - 1 * 3.
x _ srcArray floatAt: index + 1.
y _ srcArray floatAt: index + 2.
z _ srcArray floatAt: index + 3.
dstArray floatAt: index + 1 put: a11 * x + (a12 * y) + (a13 * z) + a14.
dstArray floatAt: index + 2 put: a21 * x + (a22 * y) + (a23 * z) + a24.
dstArray floatAt: index + 3 put: a31 * x + (a32 * y) + (a33 * z) + a34].
^ dstArray! !
!B3DMatrix4x4 methodsFor: 'transforming' stamp: 'atg 10/18/2005 19:23'!
scaleBy: aVector
"equivalent to making a new scaling matrix then doing composeWith: "
1
to: 3
do: [:j | 1
to: 4
do: [:i |
| index |
index _ i - 1 * 3 + j.
self at: index put: (self at: index)
* (aVector at: j)]]! !
!B3DMatrix4x4 methodsFor: 'transforming' stamp: 'atg 10/12/2005 21:50'!
translation: aVector
self
a14: aVector x;
a24: aVector y;
a34: aVector z! !
!B3DMatrix4x4 methodsFor: 'transforming' stamp: 'ar 9/18/2002 17:30'!
transposed
"Return a transposed copy of the receiver"
| matrix |
<primitive: 'b3dTransposeMatrix' module: 'Squeak3D'>
matrix := self class new.
matrix
a11: self a11; a12: self a21; a13: self a31; a14: self a41;
a21: self a12; a22: self a22; a23: self a32; a24: self a42;
a31: self a13; a32: self a23; a33: self a33; a34: self a43;
a41: self a14; a42: self a24; a43: self a34; a44: self a44.
^matrix! !
!B3DMatrix4x4 methodsFor: 'private' stamp: 'ar 11/7/2000 14:48'!
privateTransformMatrix: m1 with: m2 into: m3
"Perform a 4x4 matrix multiplication
m2 * m1 = m3
being equal to first transforming points by m2 and then by m1.
Note that m1 may be identical to m3.
NOTE: The primitive implementation does NOT return m3 - and so don't we!!"
| c1 c2 c3 c4 |
<primitive: 'b3dTransformMatrixWithInto' module:'Squeak3D'>
m2 == m3 ifTrue:[^self error:'Argument and result matrix identical'].
c1 _ ((m1 a11 * m2 a11) + (m1 a12 * m2 a21) +
(m1 a13 * m2 a31) + (m1 a14 * m2 a41)).
c2 _ ((m1 a11 * m2 a12) + (m1 a12 * m2 a22) +
(m1 a13 * m2 a32) + (m1 a14 * m2 a42)).
c3 _ ((m1 a11 * m2 a13) + (m1 a12 * m2 a23) +
(m1 a13 * m2 a33) + (m1 a14 * m2 a43)).
c4 _ ((m1 a11 * m2 a14) + (m1 a12 * m2 a24) +
(m1 a13 * m2 a34) + (m1 a14 * m2 a44)).
m3 a11: c1; a12: c2; a13: c3; a14: c4.
c1 _ ((m1 a21 * m2 a11) + (m1 a22 * m2 a21) +
(m1 a23 * m2 a31) + (m1 a24 * m2 a41)).
c2 _ ((m1 a21 * m2 a12) + (m1 a22 * m2 a22) +
(m1 a23 * m2 a32) + (m1 a24 * m2 a42)).
c3 _ ((m1 a21 * m2 a13) + (m1 a22 * m2 a23) +
(m1 a23 * m2 a33) + (m1 a24 * m2 a43)).
c4 _ ((m1 a21 * m2 a14) + (m1 a22 * m2 a24) +
(m1 a23 * m2 a34) + (m1 a24 * m2 a44)).
m3 a21: c1; a22: c2; a23: c3; a24: c4.
c1 _ ((m1 a31 * m2 a11) + (m1 a32 * m2 a21) +
(m1 a33 * m2 a31) + (m1 a34 * m2 a41)).
c2 _ ((m1 a31 * m2 a12) + (m1 a32 * m2 a22) +
(m1 a33 * m2 a32) + (m1 a34 * m2 a42)).
c3 _ ((m1 a31 * m2 a13) + (m1 a32 * m2 a23) +
(m1 a33 * m2 a33) + (m1 a34 * m2 a43)).
c4 _ ((m1 a31 * m2 a14) + (m1 a32 * m2 a24) +
(m1 a33 * m2 a34) + (m1 a34 * m2 a44)).
m3 a31: c1; a32: c2; a33: c3; a34: c4.
c1 _ ((m1 a41 * m2 a11) + (m1 a42 * m2 a21) +
(m1 a43 * m2 a31) + (m1 a44 * m2 a41)).
c2 _ ((m1 a41 * m2 a12) + (m1 a42 * m2 a22) +
(m1 a43 * m2 a32) + (m1 a44 * m2 a42)).
c3 _ ((m1 a41 * m2 a13) + (m1 a42 * m2 a23) +
(m1 a43 * m2 a33) + (m1 a44 * m2 a43)).
c4 _ ((m1 a41 * m2 a14) + (m1 a42 * m2 a24) +
(m1 a43 * m2 a34) + (m1 a44 * m2 a44)).
m3 a41: c1; a42: c2; a43: c3; a44: c4.! !
"-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- "!
B3DMatrix4x4 class
instanceVariableNames: ''!
!B3DMatrix4x4 class methodsFor: 'instance creation'!
identity
^self new setIdentity! !
!B3DMatrix4x4 class methodsFor: 'instance creation' stamp: 'ar 2/1/1999 21:25'!
numElements
^16! !
!B3DMatrix4x4 class methodsFor: 'instance creation' stamp: 'ar 2/15/1999 23:58'!
rotatedBy: angle around: axis centeredAt: origin
"Create a matrix rotating points around the given origin using the angle/axis pair"
| xform |
xform _ self withOffset: origin negated.
xform _ xform composedWithGlobal:(B3DRotation angle: angle axis: axis) asMatrix4x4.
xform _ xform composedWithGlobal: (self withOffset: origin).
^xform! !
!B3DMatrix4x4 class methodsFor: 'instance creation' stamp: 'das 5/16/2002 11:12'!
skew: vector
^ self new skew: vector.! !
!B3DMatrix4x4 class methodsFor: 'instance creation' stamp: 'das 1/25/2005 21:53'!
up: up at: at
" construct and orthonormal matrix from the up and at vectors."
^ self new up: up at: at.! !
!B3DMatrix4x4 class methodsFor: 'instance creation' stamp: 'atg 10/12/2005 21:50'!
withOffset: amount
^self identity translation: amount! !
!B3DMatrix4x4 class methodsFor: 'instance creation' stamp: 'ar 5/10/2001 15:27'!
withRotation: angle around: axis
^self new rotation: angle around: axis! !
!B3DMatrix4x4 class methodsFor: 'instance creation' stamp: 'ar 5/20/2001 00:12'!
withScale: amount
^self identity setScale: amount! !
!B3DMatrix4x4 class methodsFor: 'instance creation'!
zero
^self new! !
!B3DMatrix4x4 class methodsFor: 'class initialization' stamp: 'ar 2/1/1999 21:58'!
initialize
"B3DMatrix4x4 initialize"
B3DZeroMatrix _ self new.
B3DIdentityMatrix _ self new.
B3DIdentityMatrix a11: 1.0; a22: 1.0; a33: 1.0; a44: 1.0.! !
B3DMatrix4x4 initialize!