I've wrote a function that's supposed to do that, but it only works if the rotation matrix was generated while rotated on one axis only.
i.e 45,0,0 converted into a rotation matrix is corrected converted back to 45,0,0, but I use something like 45,24,45 it'll be returned as some funky angle and as a result doesn't match the initial values at all.
any ideas?
here's the code
Type Matrix
Method New()
Mat=New Double[16]
End Method
Method Delete()
' Release Mat
Mat=Null
End Method
Method Identity()
Local Index:Int
Mat[ 0 ] = 1
For Index=1 To 15
Mat[ Index ]=0
Next
End Method
Method Pitch( nPitch:Double )
Self.Identity()
mat[5]=Cos( nPitch )
mat[6]=-Sin( nPitch )
mat[9]=Sin( nPitch )
mat[10]=Cos( nPitch )
mat[15]=1
End Method
Method Yaw( nYaw:Double )
Self.Identity()
mat[0]=Cos( nYaw )
mat[2]=Sin( nYaw )
mat[5]=1
mat[8]=-Sin( nYaw )
Mat[10]=Cos( nYaw )
mat[15]=1
End Method
Method Roll( nRoll:Double )
Self.Identity()
mat[0]=Cos( nRoll )
mat[1]=-Sin( nRoll )
mat[4]=Sin( nRoll )
mat[5]=Cos( nRoll )
mat[15]=1
mat[10]=1
End Method
Method Position( nX:Double,nY:Double,nZ:Double )
Self.Identity()
mat[0] = 1
mat[3] = nX
mat[5] = 1
mat[7] = nY
mat[11] = nZ
mat[15] = 1
End Method
Method Copy( IN:Matrix)
Local Index:Int,Val:Double
For Index=0 To 15
Mat[Index]=IN.Mat[Index]
Next
End Method
Method getrotate()
If mat[4] > 0.998
Local heading! = ATan2(mat[2],mat[10])
Local attitude! = Pi/2
Local bank! = 0
realpitch=attitude
realyaw=heading
realroll=bank
Return
EndIf
If (mat[4] < -0.998) '// singularity at south pole
Local heading! = ATan2(mat[2],mat[10])
Local attitude! = Pi/2;
Local bank! = 0;
realpitch=attitude
realyaw=heading
realroll=bank
Return
EndIf
Local heading! = ATan2(-mat[8],mat[0]);
Local bank! = ATan2(-mat[6],mat[5])
Local attitude! = ASin(mat[4])
realpitch=attitude
realyaw=heading
realroll=bank
End Method
Method Rotate( nPitch:Double,nYaw:Double,nRoll:Double )
Local A!,B!,C!,D!,E!,F!,AD!,BD!
A = Cos( npitch)
B = Sin( npitch)
C = Cos(nyaw)
D = Sin(nyaw)
E = Cos(nroll)
F = Sin(nroll)
AD = A * D
BD = B * D
mat[0] = C * E
mat[1] = -C * F
mat[2] = D
mat[4] = BD * E + A * F
mat[5] = -BD * F + A * E
mat[6] = -B * C
mat[8] = -AD * E + B * F
mat[9] = AD * F + B * E
mat[10] = A * C
' mat[3] = mat[7] = mat[11] = mat[12] = mat[13] = mat[14] = 0
mat[3]=0
mat[7]=0
mat[11]=0
mat[12]=0
mat[13]=0
mat[14]=0
mat[15] = 1
' mat[12]=100
' mat[13]=0
'mat[14]=0
End Method
Method Rotateo( nPitch:Double,nYaw:Double,nRoll:Double,X:Double=0,Y:Double=0,Z:Double=0 )
Self.identity()
Local Temp:Matrix = New Matrix
Self.Pitch( nPitch )
Temp.Copy( Self )
Self.Yaw( nYaw )
Temp.Multiply( Self )
Self.Roll( nRoll )
Temp.Multiply( Self )
Self.Position( X,Y,Z )
Temp.Multiply( Self )
Self.Copy( Temp )
End Method
Method TForm_Point( X:Double,Y:Double,Z:Double )
Rem
Method TransformVec:Vector( i:Vector )
Local r:Vector = New Vector
Local w# = 1.0/( m30 + m31 + m32 + m33 )
r.x = ( ( m00*i.x ) + ( m01*i.y ) + ( m02*i.z ) + m03 ) * w
r.y = ( ( m10*i.x ) + ( m11*i.y ) + ( m12*i.z ) + m13 ) * w
r.z = ( ( m20*i.x ) + ( m21*i.y ) + ( m22*i.z ) + m23 ) * w
Return r
End Rem
tFormX = (mat[0] * X) + (mat[1] * Y) + (mat[2] * Z) + mat[3]
tFormY = (mat[4] * X) + (mat[5] * Y) + (mat[6] * Z) + mat[7]
tFormZ = (mat[8] * X) + (mat[9] * Y) + (Mat[10] * Z) + mat[11]
End Method
Method TForm_Point2D( X:Double,Y:Double )
Local Z=0
tFormX = (mat[0] * X) + (mat[1] * Y) + (mat[2] * Z) + mat[3]
tFormY = (mat[4] * X) + (mat[5] * Y) + (mat[6] * Z) + mat[7]
End Method
Method Multiply( With:Matrix )
Local Temp:Matrix = New Matrix
Local M:Int,m1:Int,m2:Int
For m=0 To 3
For m1=0 To 3
Temp.Mat[ m1*4+m]=0
For m2=0 To 3
Temp.Mat[ m1*4+m]:+With.Mat[m2*4+m]*Mat[m1*4+m2]
Next
Next
Next
Local Index:Int,Val:Double
For Val:Double =EachIn Temp.mat
Mat[Index]=Val
Index:+1
Next
'Release Temp
End Method
Method Debug( PrintFunc:Int(Out:String) )
Local mi
Local lin:String
For Local m=0 To 3
lin=""
For Local J=0 To 3
If mi<16
lin=lin+(m)+"-"+(j)+" : "+Mat[mi]+" "
mi:+1
EndIf
Next
DebugLog lin
Next
End Method
Field Realpitch!,realyaw!,realroll!
Field Mat:Double[]
Field tFormX:Double,tFormY:Double,tFormZ:Double,tW:Double
End Type
example
local mat:matrix = new matrix
mat.rotate( 45,0,0 )
mat.getrotate()
mat.debug( debuglog )
mat.rotate( mat.realroll,mat.realyaw,mat.realpitch ) 'pitch/roll needs to be reversed, function outputs it in the wrong order.
mat.debug( debuglog )
'As you can this generates the same matrix, so conversation was correct.
'but..
Mat.Rotate(45,0,45)
mat.getrotate()
mat.debug( debuglog )
mat.rotate( mat.realroll,mat.realyaw,mat.realpitch )
mat.debug( debuglog )
'generates incorrect angles(In getrotate) and as a result the matrices don't match.
|
