Converting a matrix back to euler angles?

BlitzMax Forums/BlitzMax Programming/Converting a matrix back to euler angles?

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.