Forward Kinematics with Denavit Hartenberg

Hi,

I am trying to simulate and verify Denavit Hartenberg(DH) Parameters with Unity , since Unity is a left handed coordinate system and Denavit Hartenberg is a right handed coordinate system approach I am facing some issues, which i can't explain to myself. First of all I am using Matrix4x4 since homogenous Matrices are default for DH, I am converting these to Quaternion and Vector3 for setting Position and Orientation of Objects, with functions I have found on the Forum and were marked as correct.

So I have the robot as 3D-Model and can move the joints per button press and save the current state of degree for the DH calculations. Then there is a function which calculates the current Matrix of the state of degree:

 Matrix4x4 mat = Matrix4x4.identity;
         float alpha = 90;
         float d = 281;
         mat[0, 0] = Mathf.Cos(TORAD * theta2);
         mat[0, 1] = -Mathf.Cos(TORAD * alpha) * Mathf.Sin(TORAD * theta2);
         mat[0, 2] = Mathf.Sin(TORAD * alpha) * Mathf.Sin(TORAD * theta2);
 
 
         mat[1, 0] = Mathf.Sin(TORAD * theta2);
         mat[1, 1] = Mathf.Cos(TORAD * alpha) * Mathf.Cos(TORAD * theta2);
         mat[1, 2] = -Mathf.Sin(TORAD * alpha) * Mathf.Cos(TORAD * theta2);
 
 
         mat[2, 1] = Mathf.Sin(TORAD * alpha);
         mat[2, 2] = Mathf.Cos(TORAD * alpha);
         mat[2, 3] = d;
         return mat;

Then I have a reference object on which I set the position and orientation as result, after converting each DH Matrix to left handed system

 DH1 = getDH1(a1Behav.theta1);
             DH2 = getDH2(a2Behav.theta2);
             DH3 = getDH3(a3Behav.theta3);
             DH4 = getDH4(a4Behav.theta4);
             DHtmp = ToggleYZ * DH1 * ToggleYZ * DH2 *  DH3*DH4 ;
             ConcatMat = StandardTranslate*DHtmp ;
             Coord.transform.position=(ExtractTranslationFromMatrix(ref ConcatMat));
             Coord.transform.rotation = (ExtractRotationFromMatrix(ref ConcatMat));

With: Toggle_YZ = ( {1, 0, 0, 0} {0, 0, 1, 0} {0, 1, 0, 0} {0, 0, 0, 1})

Now to my Problem: Everything works fine until the third joint, if I toggle the Matrix the reference Object is still rotating around the joint but around the wrong Axis, if I don't toggle it it rotates around the right axis but there is something wrong with the rotation I think. I also have to negate the joint distance (d-parameter of DH) like this, which should not be correct:

 Matrix4x4 mat = Matrix4x4.identity;
         float alpha = 45;
         float d = -50;
         float a = 282.2f;
              
        
         mat[0, 0] = Mathf.Cos(TORAD * theta3);
         mat[0, 1] = -Mathf.Cos(TORAD * alpha) * Mathf.Sin(TORAD * theta3);
         mat[0, 2] = Mathf.Sin(TORAD * alpha) * Mathf.Sin(TORAD * theta3);
         mat[0, 3] = a* -Mathf.Cos(TORAD * theta3);
 
         mat[1, 0] = Mathf.Sin(TORAD * theta3);
         mat[1, 1] = Mathf.Cos(TORAD * alpha) * Mathf.Cos(TORAD * theta3);
         mat[1, 2] = -Mathf.Sin(TORAD * alpha) * Mathf.Cos(TORAD * theta3);
         mat[1, 3] = a* Mathf.Sin(TORAD * theta3);
 
 
         mat[2, 1] = Mathf.Sin(TORAD * alpha);
         mat[2, 2] = Mathf.Cos(TORAD * alpha);
         mat[2, 3] = d;
         return mat;

I have tested 4 of 6 joints and the third and fourth are only looking somehow correct if i don't toggle them. Maybe i have misunderstood something with the rotations in unity.

I would appreciate any hints why it seems like the first two matrices have to be converted and the thrird and fourth have not. Also any experience with Denavit Hartenberg in unity which is shared would be nice.