Based on my understanding and the rotation math, I get the following results.
Note 1: This was rough and ready, more testing should happen an tune as needed. It was more a guide to get you started.
Note 2: X and Y Angle are in radians and would be the angle used to rotate the sampled vector to map back to the “correct” X,Y,Z values.
Note 3: the the final vector would be the 2nd one in each, I was just showing the output after just the X correction, then after the Y Correction.
(X,Y,Z) = (0,0,1000)
X Angle : -0.0000
Post X Correction : (0.0000, 0.0000, 1000.0000)
Y Angle : 0.0000
Post Y Correction : (0.0000, 0.0000, 1000.0000)
(X,Y,Z) = (991,0,2)
X Angle : -1.5688
Post X Correction : (0.0000, 0.0000, 991.0020)
Y Angle : 0.0000
Post Y Correction : (0.0000, 0.0000, 991.0020)
(X,Y,Z) = (-500,600,500)
X Angle : 0.7854
Post X Correction : (-0.0000, 600.0000, 707.1068)
Y Angle : 0.7036
Post Y Correction : (-0.0000, 0.0000, 927.3619)
(X,Y,Z) = (-32,100,1018)
X Angle : 0.0314
Post X Correction : (0.0000, 100.0000, 1018.5028)
Y Angle : 0.0979
Post Y Correction : (0.0000, -0.0000, 1023.4002)
X Angle : 0.0000
Post X Correction : (0.0000, -997.0000, -7.0000)
Y Angle : 1.5638
Post Y Correction : (0.0000, 0.0000, -997.0246)
So in theory now you have the stationary X,Y Rotation Angles. using those same angles on a moving vector should adjust that moving vector back to an aligned X,Y; but we don’t have an Z adjustment.
i.e. which way is forward/front. In theory we should be able to get that by simply getting a sample when moving forwards and some then apply the X,Y correction Angle from above, that should leave the us with the data to work out the rotation around Z.
rough test code (from c++builder as a windows apps, so I/O from windows objects; Im sure you can clean that )
AnsiString S1,S2;
double XAngle, YAngle;
float X, Y, Z;
float nX, nY, nZ;
X = frmX->Text.ToDouble();
Y = frmY->Text.ToDouble();
Z = frmZ->Text.ToDouble();
// Find angle to rotate around Y
// tan -1 (X/Z)
XAngle = -1 * atan(X/Z);
//XAngle = -1 * atan(Z/X);
Memo1->Lines->Add("X Angle : "+S2.sprintf ("%0.4f",XAngle));
// Rotate around Y by XAngle
nX = (X * cos (XAngle)) + (Z * sin (XAngle));
nY = Y;
nZ = (-1 * X * sin (XAngle)) + (Z * cos (XAngle));
Memo1->Lines->Add("Post X Correction : "+S2.sprintf ("(%0.4f, %0.4f, %0.4f)",nX,nY,nZ));
X = nX;
Y = nY;
Z = nZ;
// Find angle to rotate around X
// tan (X/Z)
YAngle = atan(Y/Z);
Memo1->Lines->Add("Y Angle : "+S2.sprintf ("%0.4f",YAngle));
// Rotate around X by YAngle
nX = X;
nY = (Y * cos (YAngle)) - (Z * sin (YAngle));
nZ = (Y * sin (YAngle)) + (Z * cos (YAngle));
Memo1->Lines->Add("Post Y Correction : "+S2.sprintf ("(%0.4f, %0.4f, %0.4f)",nX,nY,nZ));
edit: Ref to rotation math
. No promises here, this will require research.
