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
example
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.