Here is my whole vector and matrix code. I just converted it to a more OO approach. The matrix code was partially based on MiniB3D mat code, with quaternion rotation added.
Rem
bbdoc:
EndRem
Type TVec2
'expose
Field x#,y#
Method Magnitude:Float()
Return Sqr(x*x+y*y)
EndMethod
Method Normalize()
Local m#=Magnitude()
x:/m
y:/m
EndMethod
Method Flip:TVec2()
Local v:TVec2=New TVec2
v.x=-x
v.y=-y
Return v
EndMethod
Method Copy:TVec2()
Local vec:TVec2=New TVec2
vec.x=x
vec.y=y
Return vec
EndMethod
Method ToAngle:Float()
Return ATan2(y,x)
EndMethod
Function FromAngle:TVec2(angle#)
Local vec:TVec2=New tvec2
vec.x=Cos(angle)
vec.y=Sin(angle)
Return vec
EndFunction
Method ToString:String()
Return x+", "+y
EndMethod
EndType
Rem
bbdoc:
EndRem
Type TVec3 Extends TVec2
'expose
Field z#
Method Magnitude:Float()
Return Sqr(x*x+y*y+z*z)
EndMethod
Method Normalize()
Local m#=Magnitude()
x:/m
y:/m
z:/m
EndMethod
Method Sub:TVec3(vec:TVec3)
Local newvec:TVec3=New tvec3
newvec.x=x-vec.x
newvec.y=y-vec.y
newvec.z=z-vec.z
Return newvec
EndMethod
Method Reverse:TVec3()
Local v:TVec3=New TVec3
v.x=-x
v.y=-y
v.z=-z
Return v
EndMethod
Method Invert:TVec3()
Local v:TVec3=New TVec3
v.x=1.0/x
v.y=1.0/y
v.z=1.0/z
Return v
EndMethod
Method Copy:TVec3()
Local vec:TVec3=New TVec3
vec.x=x
vec.y=y
vec.z=z
Return vec
EndMethod
Method ToQuat:TVec4()
Local cr#=Cos(-z/2.0)
Local cp#=Cos(x/2.0)
Local cy#=Cos(y/2.0)
Local sr#=Sin(-z/2.0)
Local sp#=Sin(x/2.0)
Local sy#=Sin(y/2.0)
Local cpcy#=cp#*cy#
Local spsy#=sp#*sy#
Local spcy#=sp#*cy#
Local cpsy#=cp#*sy#
Local quat:TVec4=New TVec4
quat.w=cr#*cpcy#+sr#*spsy#
quat.x=sr#*cpcy#-cr#*spsy#
quat.y=cr#*spcy#+sr#*cpsy#
quat.z=cr#*cpsy#-sr#*spcy#
Return quat
EndMethod
Method ToString$()
Return x+", "+y+", "+z
EndMethod
EndType
Rem
bbdoc:
EndRem
Type TVec4 Extends TVec3
'expose
Const QuatToEulerAccuracy#=0.001
Field w#
Method ToString$()
Return x+", "+y+", "+z+", "+w
EndMethod
Method Magnitude:Float()
Return Sqr(x*x+y*y+z*z+w*w)
EndMethod
Method Normalize()
Local m#=Magnitude()
x:/m
y:/m
z:/m
w:/m
EndMethod
Method Invert:TVec4()
Local v:TVec4=New TVec4
v.x=1.0/x
v.y=1.0/y
v.z=1.0/z
v.w=1.0/w
Return v
EndMethod
Method Reverse:TVec4()
Local v:TVec4=New TVec4
v.x=-x
v.y=-y
v.z=-z
v.w=-w
Return v
EndMethod
Method Copy:TVec4()
Local vec:TVec4=New TVec4
vec.x=x
vec.y=y
vec.z=z
vec.w=w
Return vec
EndMethod
Method ToEuler:TVec3()
Local sint#=(2.0*w*y)-(2.0*x*z)
Local cost_temp#=1.0-(sint#*sint#)
Local cost#,sinv#,cosv#,sinf#,cosf#
If Abs(cost_temp#)>QuatToEulerAccuracy
cost#=Sqr(cost_temp#)
Else
cost#=0.0
EndIf
If Abs(cost#)>QuatToEulerAccuracy
sinv#=((2.0*y*z)+(2.0*w*x))/cost#
cosv#=(1.0-(2.0*x*x)-(2.0*y*y))/cost#
sinf#=((2.0*x*y)+(2.0*w*z))/cost#
cosf#=(1.0-(2.0*y*y)-(2.0*z*z))/cost#
Else
sinv#=(2.0*w*x)-(2.0*y*z)
cosv#=1.0-(2.0*x*x)-(2.0*z*z)
sinf#=0.0
cosf#=1.0
EndIf
Local euler:TVec3=New TVec3
euler.z=-ATan2(sinv#,cosv#)
euler.x=ATan2(sint#,cost#)
euler.y=ATan2(sinf#,cosf#)
Return euler
EndMethod
EndType
Rem
bbdoc:
End Rem
Function Vec2:TVec2(x#,y#)
Local vec:TVec2=New TVec2
vec.x=x
vec.y=y
Return vec
EndFunction
Rem
bbdoc:
End Rem
Function Vec3:TVec3(x#,y#,z#)
Local vec:TVec3=New TVec3
vec.x=x
vec.y=y
vec.z=z
Return vec
EndFunction
Rem
bbdoc:
End Rem
Function Vec4:TVec4(x#,y#,z#,w#)
Local vec:TVec4=New TVec4
vec.x=x
vec.y=y
vec.z=z
vec.w=w
Return vec
EndFunction
Rem
bbdoc:
End Rem
Function FlipQuat:TVec4(quat:TVec4)
quat=quat.copy()
quat.w=-quat.w
Return quat
EndFunction
Rem
bbdoc:
End Rem
Function MulQuat:TVec4(quat0:TVec4,quat1:TVec4)
Local ax#,ay#,az#,aw#,bx#,by#,bz#,bw#,a#,b#,c#,d#,e#,f#,g#,h#
Local quat:TVec4
ax=quat0.x
ay=quat0.y
az=quat0.z
bw=quat0.w
bx=quat1.x
by=quat1.y
bz=quat1.z
bw=quat1.w
a#=(Aw#+Ax#)*(Bw#+Bx#)
b#=(Az#-Ay#)*(By#-Bz#)
c#=(Aw#-Ax#)*(By#+Bz#)
d#=(Ay#+Az#)*(Bw#-Bx#)
e#=(Ax#+Az#)*(Bx#+By#)
f#=(Ax#-Az#)*(Bx#-By#)
g#=(Aw#+Ay#)*(Bw#-Bz#)
h#=(Aw#-Ay#)*(Bw#+Bz#)
quat=New tvec4
quat.w=b#+(-e#-f#+g#+h#)/2.0
quat.x=a#-(e#+f#+g#+h#)/2.0
quat.y=c#+(e#-f#+g#-h#)/2.0
quat.z=d#+(e#-f#-g#+h#)/2.0
Return quat
EndFunction
Rem
bbdoc:
End Rem
Function PointPlaneDistance:Float(point:TVec3,plane:TVec4)
Return (plane.w-plane.x*point.x-plane.y*point.y-plane.z*point.z)
EndFunction
Rem
bbdoc:
End Rem
Function Dot:Float(vec0:TVec3,vec1:TVec3)
Return vec0.x*vec1.x+vec0.y*vec1.y+vec0.z*vec1.z
EndFunction
Rem
bbdoc:
End Rem
Function Cross:TVec3(vec0:TVec3,vec1:TVec3)
Local vec:TVec3=New tvec3
vec.x=vec0.y*vec1.z-vec0.z*vec1.y
vec.y=vec0.z*vec1.x-vec0.x*vec1.z
vec.z=vec0.x*vec1.y-vec0.y*vec1.x
Return vec
EndFunction
Rem
bbdoc:
End Rem
Type TMatrix
Field grid#[4,4]
Method LoadIdentity()
grid[0,0]=1.0
grid[1,0]=0.0
grid[2,0]=0.0
grid[3,0]=0.0
grid[0,1]=0.0
grid[1,1]=1.0
grid[2,1]=0.0
grid[3,1]=0.0
grid[0,2]=0.0
grid[1,2]=0.0
grid[2,2]=1.0
grid[3,2]=0.0
grid[0,3]=0.0
grid[1,3]=0.0
grid[2,3]=0.0
grid[3,3]=1.0
End Method
Method Copy:TMatrix()
Local mat:TMatrix=New TMatrix
mat.grid[0,0]=grid[0,0]
mat.grid[1,0]=grid[1,0]
mat.grid[2,0]=grid[2,0]
mat.grid[3,0]=grid[3,0]
mat.grid[0,1]=grid[0,1]
mat.grid[1,1]=grid[1,1]
mat.grid[2,1]=grid[2,1]
mat.grid[3,1]=grid[3,1]
mat.grid[0,2]=grid[0,2]
mat.grid[1,2]=grid[1,2]
mat.grid[2,2]=grid[2,2]
mat.grid[3,2]=grid[3,2]
mat.grid[0,3]=grid[0,3]
mat.grid[1,3]=grid[1,3]
mat.grid[2,3]=grid[2,3]
mat.grid[3,3]=grid[3,3]
Return mat
EndMethod
Method Overwrite(mat:TMatrix)
grid[0,0]=mat.grid[0,0]
grid[1,0]=mat.grid[1,0]
grid[2,0]=mat.grid[2,0]
grid[3,0]=mat.grid[3,0]
grid[0,1]=mat.grid[0,1]
grid[1,1]=mat.grid[1,1]
grid[2,1]=mat.grid[2,1]
grid[3,1]=mat.grid[3,1]
grid[0,2]=mat.grid[0,2]
grid[1,2]=mat.grid[1,2]
grid[2,2]=mat.grid[2,2]
grid[3,2]=mat.grid[3,2]
grid[0,3]=mat.grid[0,3]
grid[1,3]=mat.grid[1,3]
grid[2,3]=mat.grid[2,3]
grid[3,3]=mat.grid[3,3]
EndMethod
Method Inverse:TMatrix()
Local mat:TMatrix=New TMatrix
mat.loadidentity()
Local tx#=0
Local ty#=0
Local tz#=0
' The rotational part of the matrix is simply the transpose of the
' original matrix.
mat.grid[0,0] = grid[0,0]
mat.grid[1,0] = grid[0,1]
mat.grid[2,0] = grid[0,2]
mat.grid[0,1] = grid[1,0]
mat.grid[1,1] = grid[1,1]
mat.grid[2,1] = grid[1,2]
mat.grid[0,2] = grid[2,0]
mat.grid[1,2] = grid[2,1]
mat.grid[2,2] = grid[2,2]
' The right column vector of the matrix should always be [ 0 0 0 1 ]
' in most cases. . . you don't need this column at all because it'll
' never be used in the program, but since this code is used with GL
' and it does consider this column, it is here.
mat.grid[0,3] = 0
mat.grid[1,3] = 0
mat.grid[2,3] = 0
mat.grid[3,3] = 1
' The translation components of the original matrix.
tx = grid[3,0]
ty = grid[3,1]
tz = grid[3,2]
' Result = -(Tm * Rm) To get the translation part of the inverse
mat.grid[3,0] = -( (grid[0,0] * tx) + (grid[0,1] * ty) + (grid[0,2] * tz) )
mat.grid[3,1] = -( (grid[1,0] * tx) + (grid[1,1] * ty) + (grid[1,2] * tz) )
mat.grid[3,2] = -( (grid[2,0] * tx) + (grid[2,1] * ty) + (grid[2,2] * tz) )
Return mat
End Method
' optimised
Method Multiply(mat:TMatrix)
Local m00# = grid#[0,0]*mat.grid#[0,0] + grid#[1,0]*mat.grid#[0,1] + grid#[2,0]*mat.grid#[0,2] + grid#[3,0]*mat.grid#[0,3]
Local m01# = grid#[0,1]*mat.grid#[0,0] + grid#[1,1]*mat.grid#[0,1] + grid#[2,1]*mat.grid#[0,2] + grid#[3,1]*mat.grid#[0,3]
Local m02# = grid#[0,2]*mat.grid#[0,0] + grid#[1,2]*mat.grid#[0,1] + grid#[2,2]*mat.grid#[0,2] + grid#[3,2]*mat.grid#[0,3]
'Local m03# = grid#[0,3]*mat.grid#[0,0] + grid#[1,3]*mat.grid#[0,1] + grid#[2,3]*mat.grid#[0,2] + grid#[3,3]*mat.grid#[0,3]
Local m10# = grid#[0,0]*mat.grid#[1,0] + grid#[1,0]*mat.grid#[1,1] + grid#[2,0]*mat.grid#[1,2] + grid#[3,0]*mat.grid#[1,3]
Local m11# = grid#[0,1]*mat.grid#[1,0] + grid#[1,1]*mat.grid#[1,1] + grid#[2,1]*mat.grid#[1,2] + grid#[3,1]*mat.grid#[1,3]
Local m12# = grid#[0,2]*mat.grid#[1,0] + grid#[1,2]*mat.grid#[1,1] + grid#[2,2]*mat.grid#[1,2] + grid#[3,2]*mat.grid#[1,3]
'Local m13# = grid#[0,3]*mat.grid#[1,0] + grid#[1,3]*mat.grid#[1,1] + grid#[2,3]*mat.grid#[1,2] + grid#[3,3]*mat.grid#[1,3]
Local m20# = grid#[0,0]*mat.grid#[2,0] + grid#[1,0]*mat.grid#[2,1] + grid#[2,0]*mat.grid#[2,2] + grid#[3,0]*mat.grid#[2,3]
Local m21# = grid#[0,1]*mat.grid#[2,0] + grid#[1,1]*mat.grid#[2,1] + grid#[2,1]*mat.grid#[2,2] + grid#[3,1]*mat.grid#[2,3]
Local m22# = grid#[0,2]*mat.grid#[2,0] + grid#[1,2]*mat.grid#[2,1] + grid#[2,2]*mat.grid#[2,2] + grid#[3,2]*mat.grid#[2,3]
'Local m23# = grid#[0,3]*mat.grid#[2,0] + grid#[1,3]*mat.grid#[2,1] + grid#[2,3]*mat.grid#[2,2] + grid#[3,3]*mat.grid#[2,3]
Local m30# = grid#[0,0]*mat.grid#[3,0] + grid#[1,0]*mat.grid#[3,1] + grid#[2,0]*mat.grid#[3,2] + grid#[3,0]*mat.grid#[3,3]
Local m31# = grid#[0,1]*mat.grid#[3,0] + grid#[1,1]*mat.grid#[3,1] + grid#[2,1]*mat.grid#[3,2] + grid#[3,1]*mat.grid#[3,3]
Local m32# = grid#[0,2]*mat.grid#[3,0] + grid#[1,2]*mat.grid#[3,1] + grid#[2,2]*mat.grid#[3,2] + grid#[3,2]*mat.grid#[3,3]
'Local m33# = grid#[0,3]*mat.grid#[3,0] + grid#[1,3]*mat.grid#[3,1] + grid#[2,3]*mat.grid#[3,2] + grid#[3,3]*mat.grid#[3,3]
grid[0,0]=m00
grid[0,1]=m01
grid[0,2]=m02
'grid[0,3]=m03
grid[1,0]=m10
grid[1,1]=m11
grid[1,2]=m12
'grid[1,3]=m13
grid[2,0]=m20
grid[2,1]=m21
grid[2,2]=m22
'grid[2,3]=m23
grid[3,0]=m30
grid[3,1]=m31
grid[3,2]=m32
'grid[3,3]=m33
EndMethod
Method TForm4(position:TVec3,quaternion:TVec4,scale:TVec3)
Local px#,py#,pz#,qx#,qy#,qz#,qw#,sx#,sy#,sz#
px=position.x
py=position.y
pz=position.z
qx=quaternion.x
qy=quaternion.y
qz=quaternion.z
qw=quaternion.w
sx=scale.x
sy=scale.y
sz=scale.z
' identity
grid[0,3]=0.0
grid[1,3]=0.0
grid[2,3]=0.0
grid[3,3]=1.0
' translate
grid[3,0] = px#
grid[3,1] = py#
grid[3,2] = pz#
' rotate + scale
grid[0,0]=(1.0-2.0*qx*qx-2.0*qz*qz)*sx
grid[1,0]=-(2.0*qz*qy-2.0*qw*qx)*sx
grid[2,0]=-(2.0*qx*qy+2.0*qw*qz)*sx
grid[0,1]=-(2.0*qz*qy+2.0*qw*qx)*sy
grid[1,1]=(1.0-2.0*qy*qy-2.0*qx*qx)*sy
grid[2,1]=-(2.0*qw*qy-2.0*qx*qz)*sy
grid[0,2]=(2.0*qw*qz-2.0*qx*qy)*sz
grid[1,2]=(2.0*qx*qz+2.0*qw*qy)*sz
grid[2,2]=(1.0-2.0*qz*qz-2.0*qy*qy)*sz
End Method
Method Translate(vec:TVec3)
grid[3,0] = grid#[0,0]*vec.x# + grid#[1,0]*vec.y# + grid#[2,0]*vec.z# + grid#[3,0]
grid[3,1] = grid#[0,1]*vec.x# + grid#[1,1]*vec.y# + grid#[2,1]*vec.z# + grid#[3,1]
grid[3,2] = grid#[0,2]*vec.x# + grid#[1,2]*vec.y# + grid#[2,2]*vec.z# + grid#[3,2]
EndMethod
Method Scale(vec:TVec3)
grid[0,0]:*vec.x#
grid[0,1]:*vec.x#
grid[0,2]:*vec.x#
grid[1,0]:*vec.y#
grid[1,1]:*vec.y#
grid[1,2]:*vec.y#
grid[2,0]:*vec.z#
grid[2,1]:*vec.z#
grid[2,2]:*vec.z#
EndMethod
Method ExtractPosition:TVec3()
Local vec:TVec3
vec=New tvec3
vec.x=grid[3,0]
vec.y=grid[3,1]
vec.z=grid[3,2]
Return vec
EndMethod
'Don't know how to extract the scale relative to the rotation. Maybe rotate by the reversed rotation and get the scale?
'Method ExtractScale()
'x=Sqr(grid[0,0]*grid[0,0]+grid[0,1]*grid[0,1]+grid[0,2]*grid[0,2])
'y=Sqr(grid[1,0]*grid[1,0]+grid[1,1]*grid[1,1]+grid[1,2]*grid[1,2])
'z=Sqr(grid[2,0]*grid[2,0]+grid[2,1]*grid[2,1]+grid[2,2]*grid[2,2])
'EndMethod
Method ExtractQuat:TVec4()
Local x#,y#,z#,w#
Local fourWSquaredMinus1:Float = +grid[0,0]+grid[1,1]+grid[2,2]
Local fourXSquaredMinus1:Float = +grid[0,0]-grid[1,1]-grid[2,2]
Local fourYSquaredMinus1:Float = +grid[1,1]-grid[0,0]-grid[2,2]
Local fourZSquaredMinus1:Float = +grid[2,2]-grid[0,0]-grid[1,1]
Local biggestIndex=0
Local fourBiggestSquaredMinus1:Float=fourWSquaredMinus1
If fourXSquaredMinus1 > fourBiggestSquaredMinus1
fourBiggestSquaredMinus1 = fourXSquaredMinus1
biggestIndex=1
EndIf
If fourYSquaredMinus1 > fourBiggestSquaredMinus1
fourBiggestSquaredMinus1 = fourYSquaredMinus1
biggestIndex=2
EndIf
If fourZSquaredMinus1 > fourBiggestSquaredMinus1
fourBiggestSquaredMinus1 = fourZSquaredMinus1
biggestIndex=3
EndIf
Local biggestValue:Float=Sqr(fourBiggestSquaredMinus1+1.0)*0.5
Local mult:Float=0.25/biggestValue
Select BiggestIndex
Case 0
w#=BiggestValue
x#=(grid[1,2]-grid[2,1])*mult
y#=-(grid[2,0]-grid[0,2])*mult
z#=-(grid[0,1]-grid[1,0])*mult
Case 1
x#=-BiggestValue
w#=-( grid[1,2] - grid[2,1] )*mult
y#=( grid[0,1] + grid[1,0] )*mult
z#=( grid[2,0] + grid[0,2] )*mult
Case 2
y#=BiggestValue
w#=-(grid[2,0]-grid[0,2])*mult
x#=-(grid[0,1]+grid[1,0])*mult
z#=(grid[1,2]+grid[2,1])*mult
Case 3
z#=-BiggestValue
w#=(grid[0,1]-grid[1,0])*mult
x#=(grid[2,0]+grid[0,2])*mult
y#=-(grid[1,2]+grid[2,1])*mult
EndSelect
If w<-0.0
x=-x
y=-y
z=-z
w=-w
EndIf
Return vec4(x,y,z,w)
EndMethod
Method ExtractEuler:TVec3()
Local a#,b#,x#,y#,z#
x=-ATan2( grid[2,1],Sqr( grid[2,0]*grid[2,0]+grid[2,2]*grid[2,2] ) )
a#=grid[2,0]
b#=grid[2,2]
If a#<=0.0001 And a#>=-0.0001 Then a#=0
If b#<=0.0001 And b#>=-0.0001 Then b#=0
y=-ATan2(a#,b#)
a#=grid[0,1]
b#=grid[1,1]
If a#<=0.0001 And a#>=-0.0001 Then a#=0
If b#<=0.0001 And b#>=-0.0001 Then b#=0
z=ATan2(a#,b#)
Return vec3(x,y,z)
EndMethod
Method Rotate4(quat:TVec4)
Local qx#,qy#,qz#,qw#
'Scale removed for speed / doesn't work right anyways
'sx#=1'Sqr(grid[0,0]*grid[0,0]+grid[0,1]*grid[0,1]+grid[0,2]*grid[0,2])
'sy#=1'Sqr(grid[1,0]*grid[1,0]+grid[1,1]*grid[1,1]+grid[1,2]*grid[1,2])
'sz#=1'Sqr(grid[2,0]*grid[2,0]+grid[2,1]*grid[2,1]+grid[2,2]*grid[2,2])
quat=MulQuat(ExtractQuat(),quat)
qx#=quat.x
qy#=quat.y
qz#=quat.z
w#=quat.w
grid[0,3]=0.0
grid[1,3]=0.0
grid[2,3]=0.0
grid[3,3]=1.0
grid[0,0]=(1.0-2.0*qx*qx-2.0*qz*qz)'*sx
grid[1,0]=-(2.0*qz*qy-2.0*w*qx)'*sx
grid[2,0]=-(2.0*qx*qy+2.0*w*qz)'*sx
grid[0,1]=-(2.0*qz*qy+2.0*w*qx)'*sy
grid[1,1]=(1.0-2.0*qy*qy-2.0*qx*qx)'*sy
grid[2,1]=-(2.0*w*qy-2.0*qx*qz)'*sy
grid[0,2]=(2.0*w*qz-2.0*qx*qy)'*sz
grid[1,2]=(2.0*qx*qz+2.0*w*qy)'*sz
grid[2,2]=(1.0-2.0*qz*qz-2.0*qy*qy)'*sz
EndMethod
Method Rotate(euler:TVec3)
Local rx#,ry#,rz#
Local cos_ang#,sin_ang#
rx=euler.x
ry=euler.y
rz=euler.z
' yaw
cos_ang#=Cos(ry#)
sin_ang#=Sin(ry#)
Local m00# = grid#[0,0]*cos_ang + grid#[2,0]*-sin_ang#
Local m01# = grid#[0,1]*cos_ang + grid#[2,1]*-sin_ang#
Local m02# = grid#[0,2]*cos_ang + grid#[2,2]*-sin_ang#
grid[2,0] = grid#[0,0]*sin_ang# + grid#[2,0]*cos_ang
grid[2,1] = grid#[0,1]*sin_ang# + grid#[2,1]*cos_ang
grid[2,2] = grid#[0,2]*sin_ang# + grid#[2,2]*cos_ang
grid[0,0]=m00#
grid[0,1]=m01#
grid[0,2]=m02#
' pitch
cos_ang#=Cos(rx#)
sin_ang#=Sin(rx#)
Local m10# = grid#[1,0]*cos_ang + grid#[2,0]*sin_ang
Local m11# = grid#[1,1]*cos_ang + grid#[2,1]*sin_ang
Local m12# = grid#[1,2]*cos_ang + grid#[2,2]*sin_ang
grid[2,0] = grid#[1,0]*-sin_ang + grid#[2,0]*cos_ang
grid[2,1] = grid#[1,1]*-sin_ang + grid#[2,1]*cos_ang
grid[2,2] = grid#[1,2]*-sin_ang + grid#[2,2]*cos_ang
grid[1,0]=m10
grid[1,1]=m11
grid[1,2]=m12
' roll
cos_ang#=Cos(rz#)
sin_ang#=Sin(rz#)
m00# = grid#[0,0]*cos_ang# + grid#[1,0]*sin_ang#
m01# = grid#[0,1]*cos_ang# + grid#[1,1]*sin_ang#
m02# = grid#[0,2]*cos_ang# + grid#[1,2]*sin_ang#
grid[1,0] = grid#[0,0]*-sin_ang# + grid#[1,0]*cos_ang#
grid[1,1] = grid#[0,1]*-sin_ang# + grid#[1,1]*cos_ang#
grid[1,2] = grid#[0,2]*-sin_ang# + grid#[1,2]*cos_ang#
grid[0,0]=m00#
grid[0,1]=m01#
grid[0,2]=m02#
EndMethod
Method RotatePitch(ang#)
Local cos_ang#=Cos(ang#)
Local sin_ang#=Sin(ang#)
Local m10# = grid#[1,0]*cos_ang + grid#[2,0]*sin_ang
Local m11# = grid#[1,1]*cos_ang + grid#[2,1]*sin_ang
Local m12# = grid#[1,2]*cos_ang + grid#[2,2]*sin_ang
grid[2,0] = grid#[1,0]*-sin_ang + grid#[2,0]*cos_ang
grid[2,1] = grid#[1,1]*-sin_ang + grid#[2,1]*cos_ang
grid[2,2] = grid#[1,2]*-sin_ang + grid#[2,2]*cos_ang
grid[1,0]=m10
grid[1,1]=m11
grid[1,2]=m12
EndMethod
Method RotateYaw(ang#)
Local cos_ang#=Cos(ang#)
Local sin_ang#=Sin(ang#)
Local m00# = grid#[0,0]*cos_ang + grid#[2,0]*-sin_ang#
Local m01# = grid#[0,1]*cos_ang + grid#[2,1]*-sin_ang#
Local m02# = grid#[0,2]*cos_ang + grid#[2,2]*-sin_ang#
grid[2,0] = grid#[0,0]*sin_ang# + grid#[2,0]*cos_ang
grid[2,1] = grid#[0,1]*sin_ang# + grid#[2,1]*cos_ang
grid[2,2] = grid#[0,2]*sin_ang# + grid#[2,2]*cos_ang
grid[0,0]=m00#
grid[0,1]=m01#
grid[0,2]=m02#
End Method
Method RotateRoll(ang#)
Local cos_ang#=Cos(ang#)
Local sin_ang#=Sin(ang#)
Local m00# = grid#[0,0]*cos_ang# + grid#[1,0]*sin_ang#
Local m01# = grid#[0,1]*cos_ang# + grid#[1,1]*sin_ang#
Local m02# = grid#[0,2]*cos_ang# + grid#[1,2]*sin_ang#
grid[1,0] = grid#[0,0]*-sin_ang# + grid#[1,0]*cos_ang#
grid[1,1] = grid#[0,1]*-sin_ang# + grid#[1,1]*cos_ang#
grid[1,2] = grid#[0,2]*-sin_ang# + grid#[1,2]*cos_ang#
grid[0,0]=m00#
grid[0,1]=m01#
grid[0,2]=m02#
EndMethod
EndType
|