public double getHeadingDegrees()
{
double x = so3SensorFromWorld.get(2, 0);
double y = so3SensorFromWorld.get(2, 1);
double mag = Math.Sqrt(x * x + y * y);
if (mag < 0.1D)
{
return(0.0D);
}
double heading = -90.0D - Math.Atan2(y, x) / 3.141592653589793D * 180.0D;
if (heading < 0.0D)
{
heading += 360.0D;
}
if (heading >= 360.0D)
{
heading -= 360.0D;
}
return(heading);
}
private double[] glMatrixFromSo3(Matrix3x3d so3)
{
for (int r = 0; r < 3; r++)
{
for (int c = 0; c < 3; c++)
{
rotationMatrix[(4 * c + r)] = so3.get(r, c);
}
}
double tmp62_61 = (rotationMatrix[11] = 0.0D); rotationMatrix[7] = tmp62_61; rotationMatrix[3] = tmp62_61;
double tmp86_85 = (rotationMatrix[14] = 0.0D); rotationMatrix[13] = tmp86_85; rotationMatrix[12] = tmp86_85;
rotationMatrix[15] = 1.0D;
return(rotationMatrix);
}
public static void generatorField(int i, Matrix3x3d pos, Matrix3x3d result)
{
result.set(i, 0, 0.0D);
result.set((i + 1) % 3, 0, -pos.get((i + 2) % 3, 0));
result.set((i + 2) % 3, 0, pos.get((i + 1) % 3, 0));
}
public static void muFromSO3(Matrix3x3d so3, Vector3d result)
{
double cosAngle = (so3.get(0, 0) + so3.get(1, 1) + so3.get(2, 2) - 1.0D) * 0.5D;
result.set((so3.get(2, 1) - so3.get(1, 2)) / 2.0D, (so3.get(0, 2) - so3.get(2, 0)) / 2.0D, (so3.get(1, 0) - so3.get(0, 1)) / 2.0D);
double sinAngleAbs = result.length();
if (cosAngle > M_SQRT1_2)
{
if (sinAngleAbs > 0.0D)
{
result.scale(Math.Asin(sinAngleAbs) / sinAngleAbs);
}
}
else if (cosAngle > -M_SQRT1_2)
{
double angle = Math.Acos(cosAngle);
result.scale(angle / sinAngleAbs);
}
else
{
double angle = 3.141592653589793D - Math.Asin(sinAngleAbs);
double d0 = so3.get(0, 0) - cosAngle;
double d1 = so3.get(1, 1) - cosAngle;
double d2 = so3.get(2, 2) - cosAngle;
Vector3d r2 = muFromSO3R2;
if ((d0 * d0 > d1 * d1) && (d0 * d0 > d2 * d2))
{
r2.set(d0, (so3.get(1, 0) + so3.get(0, 1)) / 2.0D, (so3.get(0, 2) + so3.get(2, 0)) / 2.0D);
}
else if (d1 * d1 > d2 * d2)
{
r2.set((so3.get(1, 0) + so3.get(0, 1)) / 2.0D, d1, (so3.get(2, 1) + so3.get(1, 2)) / 2.0D);
}
else
{
r2.set((so3.get(0, 2) + so3.get(2, 0)) / 2.0D, (so3.get(2, 1) + so3.get(1, 2)) / 2.0D, d2);
}
if (Vector3d.dot(r2, result) < 0.0D)
{
r2.scale(-1.0D);
}
r2.normalize();
r2.scale(angle);
result.set(r2);
}
}