Floor Plane Kinect v2

Ich versuche schon seid längerem für mein Programm mit der Kinect die Floor Plane ( welche die Kinect selber festlegt ) zu verändern. Ich verwende die BodyBasics. Mein Plan ist, dass die Kinect beim Starten des Programmes die FloorPlane so bearbeitet, dass sie am Ende den Boden als gerade Ebene betrachtet, damit sie den Menschen und das Skelett richtig erkennt und gerade das Skelett richtig darstellt. Außerdem wird die Kinect vielleicht bewegt, daher würde ich bei jedem 30 Frame oder so die Floor Plane erneut gerade stellen.

Ich weiß bereits wie ich die Floor Plane aufrufe und das klappt auch soweit, jedoch ist meine Berechnung falsch, sprich es wird nichts gerade gestellt.

yNew = vector(floorPlane.A, floorPlane.B, floorPlane.C);
zNew = vector(0, 1, -floorPlane.B/floorPlane.C);
zNew.Normalize();
xNew = CrossProduct(yNew, zNew);



rotMatrix = matrix(xNew.x, xNew.y, xNew.z, 0,
                   yNew.x, yNew.y, yNew.z, 0,
                   zNew.x, zNew.y, zNew.z, 0,
                   0,      0,      0,      1);

translationMatrix = matrix(1, 0, 0, 0,
                           0, 1, 0, -floorPlane.D,
                           0, 0, 1, 0,
                           0, 0, 0, 1);

transform = translationMatrix * rotMatrix;

So sieht meine Berechnung bisher aus .

Kann mir hier vielleicht einer helfen ?

Viele Grüße und Danke !