IEnumerator Rotate(CoordinateFrame unityFrame, CoordinateFrame frame2, Vector3 unityAngles)
{
yield return(new WaitForSeconds(1));
CoordinateFrameConverter conv = new CoordinateFrameConverter(unityFrame, frame2);
EulerAngles eu1 = new EulerAngles(unityAngles.z, unityAngles.x, unityAngles.y);
EulerAngles eu2 = conv.ConvertEulerAngles(eu1);
int steps = 40;
EulerAngles teu1 = new EulerAngles(0, 0, 0);
EulerAngles teu2 = new EulerAngles(0, 0, 0);
for (int e = 0; e < 3; e++)
{
for (int i = 0; i < steps; i++)
{
teu1[e] = teu1[e] + eu1[e] * 1.0f / steps;
teu2[e] = teu2[e] + eu2[e] * 1.0f / steps;
target1.localRotation = Conversions.ToQuaternion(unityFrame.RotationOrder, teu1);
target2.localRotation = Conversions.ToQuaternion(unityFrame.RotationOrder, conv.inverse.ConvertEulerAngles(teu2));
yield return(null);
}
}
}
// Transforms euler orders back and forth between all rotation orders
// to make sure the conversions result in the same rotation
IEnumerator RunRotationTests(List <AxisSet> orders)
{
int issues = 0;
foreach (var o1 in orders)
{
foreach (var o2 in orders)
{
EulerAngles e = GetRandomAngles();
EulerAngles to = Conversions.ConvertEulerOrder(o1, o2, e);
EulerAngles back = Conversions.ConvertEulerOrder(o2, o1, to);
// Check if the rotations are the same or if the rotations
// have the equivalent effect on transforming vectors
bool equal = AreRotationsEquivalent(o1, e, back);
if (!equal)
{
Debug.LogError("Error converting " + o1.ToString(true) + " > " + o2.ToString(true));
Debug.Log(e.ToString("0.00000") + " > " + to.ToString("0.00000") + " > " + back.ToString("0.00000"));
AreRotationsEquivalent(o1, e, back, true);
issues++;
}
}
yield return(null);
}
Debug.Log("Ran into " + issues + " issues when doing rotation conversions");
}
public void ToEulerAnglesTest(double real, double x, double y, double z, double[] expectedAsArray)
{
var quat = new Quaternion(real, x, y, z);
var eulerAngles = quat.ToEulerAngles();
var expected = new EulerAngles(Angle.FromRadians(expectedAsArray[0]), Angle.FromRadians(expectedAsArray[1]), Angle.FromRadians(expectedAsArray[2]));
Assert.AreEqual(expected,eulerAngles);
}
/** Initializer by one euler angle.
Initialize the quaternion with the single euler angle where its index
is given in the first argument.
@param idx Index of the euler angle to initialize
@param angle The euler angle in radians */
public Quaternion(EulerAngles index, double angle)
{
var sinAngle2 = Math.Sin(0.5 * angle);
var cosAngle2 = Math.Cos(0.5 * angle);
if (index == EulerAngles.ePhi)
{
Q1 = cosAngle2;
Q2 = sinAngle2;
Q3 = 0.0;
Q4 = 0.0;
}
else
{
if (index == EulerAngles.eTht)
{
Q1 = cosAngle2;
Q2 = 0.0;
Q3 = sinAngle2;
Q4 = 0.0;
}
else
{
Q1 = cosAngle2;
Q2 = 0.0;
Q3 = 0.0;
Q4 = sinAngle2;
}
}
}
public void EulerToQuaternion_QuaternionToEuler_ResultingOrientationIsCloseToOriginal(
[Values] math.RotationOrder rotationOrder,
[Values(-90f, -45, 0f, 45, 90f)] float x,
[Values(-90f, -45, 0f, 45, 90f)] float y,
[Values(-90f, -45, 0f, 45, 90f)] float z
)
{
var inputEulerAngles = new EulerAngles {
RotationOrder = rotationOrder, Value = new float3(x, y, z)
};
var inputQuaternion = (quaternion)inputEulerAngles;
Assume.That(math.abs(math.length(inputQuaternion.value)), Is.EqualTo(1.0f).Within(1e-05));
EulerAngles outputEulerAngles = new EulerAngles {
RotationOrder = inputEulerAngles.RotationOrder
};
outputEulerAngles.SetValue(inputQuaternion);
quaternion outputQuaternion = (quaternion)outputEulerAngles;
Assume.That(math.abs(math.length(outputQuaternion.value)), Is.EqualTo(1.0f).Within(1e-05));
Assert.That(outputQuaternion, Is.OrientedEquivalentTo(inputQuaternion));
}
private static void VerifyEuler(EulerAngles eulerAngles)
{
var rotationMatrix = Matrix.CreateRotationZYX(eulerAngles);
var rotationQuaternion = Quaternion.FromRotationMatrix(rotationMatrix);
Assert.AreEqual(eulerAngles, rotationQuaternion.ToEuler());
}
/// <summary>
/// Initializer by one euler angle.
/// Initialize the quaternion with the single euler angle where its index
/// is given in the first argument.
/// </summary>
/// <param name="idx">Index of the euler angle to initialize</param>
/// <param name="angle">The euler angle in radians</param>
public Quaternion(EulerAngles idx, double angle)
{
this.cache = false;
double angle2 = 0.5 * angle;
double Sangle2 = Math.Sin(angle2);
double Cangle2 = Math.Cos(angle2);
if (idx == EulerAngles.ePhi)
{
this.W = Cangle2;
this.X = Sangle2;
this.Y = 0.0;
this.Z = 0.0;
}
else if (idx == EulerAngles.eTht)
{
this.W = Cangle2;
this.X = 0.0;
this.Y = Sangle2;
this.Z = 0.0;
}
else
{
this.W = Cangle2;
this.X = 0.0;
this.Y = 0.0;
this.Z = Sangle2;
}
}
//ncrunch: no coverage start
private void UpdateText()
{
EulerAngles euler = camera.Rotation.ToEuler();
text.Text = "Yaw: " + euler.Yaw.ToInvariantString("0") + " Pitch: " +
euler.Pitch.ToInvariantString("0") + " Roll: " + euler.Roll.ToInvariantString("0");
}
private void Sensors_ReadingAvailable(Sensors.Reading reading)
{
OrientationTracker.Primary.UpdateWithReading(reading);
//OrientationTracker.Secondary.UpdateWithReading(reading, -1, true); // uncomment if using two IMUs
if ((DateTime.Now - last).TotalMilliseconds > 30)
{
EulerAngles orientation = OrientationTracker.Primary.EstimateOrientation().GetEulerAngles();
int resolution = 1;
int yaw = resolution * (int)Math.Round(orientation.Yaw * 180 / Math.PI / resolution);
int pitch = resolution * (int)Math.Round(orientation.Pitch * 180 / Math.PI / resolution);
int roll = resolution * (int)Math.Round(orientation.Roll * 180 / Math.PI / resolution);
if (RemoveGravityCheckbox.Checked)
{
reading = OrientationTracker.SubtractGravity(reading);
}
Invoke(new MethodInvoker(delegate
{
StatusBox.Text = "";
StatusBox.Text += "ax = " + reading.Accelerometer1.X + " m/s^2" + Environment.NewLine;
StatusBox.Text += "ay = " + reading.Accelerometer1.Y + " m/s^2" + Environment.NewLine;
StatusBox.Text += "az = " + reading.Accelerometer1.Z + " m/s^2" + Environment.NewLine;
StatusBox.Text += "gx = " + reading.Gyroscope1.X + " deg / s" + Environment.NewLine;
StatusBox.Text += "gy = " + reading.Gyroscope1.Y + " deg / s" + Environment.NewLine;
StatusBox.Text += "gz = " + reading.Gyroscope1.Z + " deg / s" + Environment.NewLine;
StatusBox.Text += "mx = " + reading.Magnetometer1.X + " gauss" + Environment.NewLine;
StatusBox.Text += "my = " + reading.Magnetometer1.Y + " gauss" + Environment.NewLine;
StatusBox.Text += "mz = " + reading.Magnetometer1.Z + " gauss" + Environment.NewLine;
StatusBox.Text += yaw + Environment.NewLine + pitch + Environment.NewLine + roll;
}));
last = DateTime.Now;
}
}
public FormILFMatrixCalculator(RotationMatrix matrix)
{
this.m_Device = null;
this.m_Mesh = null;
this.m_Paused = false;
InitializeComponent();
this.m_FormRenderSurface = new FormILFRenderSurface();
this.panelRenderSurface.Controls.Add(this.m_FormRenderSurface);
this.m_FormRenderSurface.Visible = true;
if (!this.InitializeGraphics())
{
MessageBox.Show("Failed to initialize Direct3D.", "TRE Explorer", MessageBoxButtons.OK, MessageBoxIcon.Error);
}
EulerAngles eulerAngles = matrix.ToEulerAngles();
this.trackBarXPitch.Value = eulerAngles.xPitch;
this.trackBarYYaw.Value = eulerAngles.yYaw;
this.trackBarZRoll.Value = eulerAngles.zRoll;
this.m_OriginalXPitch = eulerAngles.xPitch;
this.m_OriginalYYaw = eulerAngles.yYaw;
this.m_OriginalZRoll = eulerAngles.zRoll;
this.UpdateMatrix();
}
/// <summary>
/// Method called when an OSC message associated with this data object is received.
/// </summary>
/// <param name="message">Received OSC message.</param>
/// <returns>True if the OSC message was successfully parsed.</returns>
protected override bool OnMessageReceived(OscMessage message)
{
if (message.Count != 3)
{
return(false);
}
if (!(message[0] is float))
{
return(false);
}
if (!(message[1] is float))
{
return(false);
}
if (!(message[2] is float))
{
return(false);
}
EulerAngles = new EulerAngles((float)message[0], (float)message[1], (float)message[2]);
return(true);
}
public EulerAngles Manipulate(EulerAngles processVariable, EulerAngles setpoint, double elapsedTime)
{
double manipulatedVariablePitch = _controllerX.Manipulate(processVariable.Pitch, setpoint.Pitch, elapsedTime);
double manipulatedVariableRoll = _controllerX.Manipulate(processVariable.Roll, setpoint.Roll, elapsedTime);
double manipulatedVariableYaw = _controllerX.Manipulate(processVariable.Yaw, setpoint.Yaw, elapsedTime);
manipulatedVariablePitch += 360;
manipulatedVariableRoll += 360;
manipulatedVariableYaw += 360;
if (manipulatedVariablePitch > 180d)
{
manipulatedVariablePitch -= 360d;
}
if (manipulatedVariableRoll > 180d)
{
manipulatedVariableRoll -= 360d;
}
if (manipulatedVariableYaw > 180d)
{
manipulatedVariableYaw -= 360d;
}
return(new EulerAngles(manipulatedVariablePitch, manipulatedVariableRoll, manipulatedVariableYaw));
}
public void setInitialAttitude(EulerAngles attitude)
{
msclPINVOKE.InertialNode_setInitialAttitude(swigCPtr, EulerAngles.getCPtr(attitude));
if (msclPINVOKE.SWIGPendingException.Pending)
{
throw msclPINVOKE.SWIGPendingException.Retrieve();
}
}
public void setSensorToVehicleTransformation(EulerAngles angles)
{
msclPINVOKE.InertialNode_setSensorToVehicleTransformation(swigCPtr, EulerAngles.getCPtr(angles));
if (msclPINVOKE.SWIGPendingException.Pending)
{
throw msclPINVOKE.SWIGPendingException.Retrieve();
}
}
public void Create()
{
var euler = new EulerAngles(1, 2, 3);
Assert.AreEqual(euler.Pitch, 1);
Assert.AreEqual(euler.Yaw, 2);
Assert.AreEqual(euler.Roll, 3);
}
public void ToEulerAnglesTest(double real, double x, double y, double z, double[] expectedAsArray)
{
var quat = new Quaternion(real, x, y, z);
var eulerAngles = quat.ToEulerAngles();
var expected = new EulerAngles(Angle.FromRadians(expectedAsArray[0]), Angle.FromRadians(expectedAsArray[1]), Angle.FromRadians(expectedAsArray[2]));
Assert.IsTrue(eulerAngles.Equals(expected, TestTolerance));
}
public static List <double> QuaternionToEuler(double w, double x, double y, double z)
{
double sqw = w * w;
double sqx = x * x;
double sqy = y * y;
double sqz = z * z;
// invs (inverse square length) is only required if quaternion is not already normalised
double invs = 1 / (sqx + sqy + sqz + sqw);
double m00 = (sqx - sqy - sqz + sqw) * invs; // since sqw + sqx + sqy + sqz =1/invs*invs
double m11 = (-sqx + sqy - sqz + sqw) * invs;
double m22 = (-sqx - sqy + sqz + sqw) * invs;
double tmp1 = x * y;
double tmp2 = z * w;
double m10 = 2.0 * (tmp1 + tmp2) * invs;
double m01 = 2.0 * (tmp1 - tmp2) * invs;
tmp1 = x * z;
tmp2 = y * w;
double m20 = 2.0 * (tmp1 - tmp2) * invs;
double m02 = 2.0 * (tmp1 + tmp2) * invs;
tmp1 = y * z;
tmp2 = x * w;
double m21 = 2.0 * (tmp1 + tmp2) * invs;
double m12 = 2.0 * (tmp1 - tmp2) * invs;
w = Math.Sqrt(1.0 + m00 + m11 + m22) / 2.0;
double w4 = (4.0 * w);
x = (m21 - m12) / w4;
y = (m02 - m20) / w4;
z = (m10 - m01) / w4;
Q1 q;
if (w < 0)
{
q = new Q1(-w, -x, -y, -z);
}
else
{
q = new Q1(w, x, y, z);
}
EulerAngles e = q.ToEulerAngles();
// Console.WriteLine($"q = [{w}, {x}, {y}, {z}]: rzyx = [{e.Gamma.Radians}, {e.Beta.Radians}, {e.Alpha.Radians}]");
// Console.WriteLine($"q = [{w}, {x}, {y}, {z}]: rzyx = [{e.Gamma.Degrees}, {e.Beta.Degrees}, {e.Alpha.Degrees}]");
return(new List <double>
{
e.Gamma.Degrees, // z
e.Beta.Degrees, // y
e.Alpha.Degrees, // x
});
}
public static bool AreRotationsEquivalent(AxisSet axes, AxisSet rotOrder, EulerAngles a, EulerAngles b, bool debug = false)
{
AxisSet equivOrder = Conversions.ToEquivelentRotationOrder(axes, new AxisSet("XY-Z", true), rotOrder);
Quaternion qe = Conversions.ToQuaternion(equivOrder, a);
Quaternion qback = Conversions.ToQuaternion(equivOrder, b);
return(AreQuaternionsEquivalent(qe, qback, debug));
}
public EulerAngles getSensorToVehicleTransformation()
{
EulerAngles ret = new EulerAngles(msclPINVOKE.InertialNode_getSensorToVehicleTransformation(swigCPtr), true);
if (msclPINVOKE.SWIGPendingException.Pending)
{
throw msclPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
// 親の基準座標系内のローカルの基準座標系の位置と方向を指定し、
// 親->ローカルへの座標変換を実行する行列をセットアップする
public void SetupParentToLocal(Vec3 pos, EulerAngles orient)
{
// 回転行列を作成する
var orientMatrix = new RotationMatrix();
orientMatrix.Setup(orient);
// 4x3行列をセットアップする
// SetupParentToLocal(Vec3, RotationMatrix)を呼ぶことで簡略化されるが処理速度は最速ではない
SetupParentToLocal(pos, orientMatrix);
}
public void Equals()
{
var euler1 = new EulerAngles(1, 2, 3);
var euler2 = new EulerAngles(3, 4, 5);
Assert.AreNotEqual(euler1, euler2);
Assert.AreEqual(euler1, new EulerAngles(1, 2, 3));
Assert.IsTrue(euler1 == new EulerAngles(1, 2, 3));
Assert.IsTrue(euler1 != euler2);
Assert.IsTrue(euler1.Equals((object)new EulerAngles(1, 2, 3)));
}
public void GetHashCodeViaDictionary()
{
var first = new EulerAngles(1, 2, 3);
var second = new EulerAngles(3, 4, 5);
var eulerAngles = new Dictionary <EulerAngles, int> {
{ first, 1 }, { second, 2 }
};
Assert.IsTrue(eulerAngles.ContainsKey(first));
Assert.IsTrue(eulerAngles.ContainsKey(second));
Assert.IsFalse(eulerAngles.ContainsKey(new EulerAngles(5, 6, 7)));
}
/// <summary>
/// Extension method for EulerAngles class.
/// Converts the rotation yaw-pitch roll (intrinsic Tait-Bryan angles) to quaternion.
/// </summary>
/// <param name="angles"></param>
/// <returns>
/// The quaternion corresponding to rotation by the angles.
/// </returns>
public static Quaternion ToQuaternion(this EulerAngles angles)
{
double alpha = angles.Alpha.Radians / 2;
double beta = angles.Beta.Radians / 2;
double gamma = angles.Gamma.Radians / 2;
return(new Quaternion(
Math.Cos(alpha) * Math.Cos(beta) * Math.Cos(gamma) + Math.Sin(alpha) * Math.Sin(beta) * Math.Sin(gamma),
Math.Sin(alpha) * Math.Cos(beta) * Math.Cos(gamma) - Math.Cos(alpha) * Math.Sin(beta) * Math.Sin(gamma),
Math.Cos(alpha) * Math.Sin(beta) * Math.Cos(gamma) + Math.Sin(alpha) * Math.Cos(beta) * Math.Sin(gamma),
Math.Cos(alpha) * Math.Cos(beta) * Math.Sin(gamma) - Math.Sin(alpha) * Math.Sin(beta) * Math.Cos(gamma)));
}
public void TestRotation()
{
Rotation r = new Rotation(EulerAngles.FromDegrees(-90, 0, 0));
Assert.AreEqual(90.0, r.Angle.Degrees, MathConstant.EPSYLON);
Assert.AreEqual(-1.0, r.Axis.z, MathConstant.EPSYLON);
r.SetTurn(EulerAngles.FromDegrees(270, 0, 0));
Assert.AreEqual(270.0, r.Angle.Degrees, MathConstant.EPSYLON);
Assert.AreEqual(1.0, r.Axis.z, MathConstant.EPSYLON);
}
public void SetValue_WhenRotationOrder_ReturnsExpectedValue(
math.RotationOrder rotationOrder, quaternion value, float3 expectedEulerAngles
)
{
var eulerAngles = new EulerAngles {
RotationOrder = rotationOrder
};
eulerAngles.SetValue(value);
Assert.That(eulerAngles.Value, Is.PrettyCloseTo(expectedEulerAngles));
}
// 回転を受信
public void UpdateEulerAngles(string msg)
{
eulerAngles = JsonUtility.FromJson<EulerAngles>(msg);
var rotation = Quaternion.Euler(90, 0, 0);
rotation = rotation * Quaternion.AngleAxis(eulerAngles.x, Vector3.forward);
rotation = rotation * Quaternion.AngleAxis(-eulerAngles.y, Vector3.right);
rotation = rotation * Quaternion.AngleAxis(-eulerAngles.z, Vector3.up);
// カメラの姿勢を変更
Camera.main.transform.localRotation = rotation;
UpdateText();
}
public void TestAnglesInitialization()
{
EulerAngles angles = EulerAngles.FromDegrees(90.0, -110.0, 50.0);
Assert.AreEqual(90.0, angles.Heading.Degrees, MathConstant.EPSYLON);
Assert.AreEqual(-110.0, angles.Elevation.Degrees, MathConstant.EPSYLON);
Assert.AreEqual(50.0, angles.Bank.Degrees, MathConstant.EPSYLON);
angles.Normalize();
Assert.AreEqual(-90.0, angles.Heading.Degrees, MathConstant.EPSYLON);
Assert.AreEqual(-70.0, angles.Elevation.Degrees, MathConstant.EPSYLON);
Assert.AreEqual(-130.0, angles.Bank.Degrees, MathConstant.EPSYLON);
}
public void SetToRotateInertialToObject(EulerAngles orientation)
{
float sh, sp, sb;
float ch, cp, cb;
MathUtil.sinCos(orientation.pitch * 0.5f, out sp, out cp);
MathUtil.sinCos(orientation.heading * 0.5f, out sh, out ch);
MathUtil.sinCos(orientation.bank * 0.5f, out sb, out cb);
w = ch * cp * cb + sh * sp * sb;
x = -ch * sp * cb - sh * cp * sb;
y = ch * sp * sb - sh * cp * cb;
z = sh * sp * cb - ch * cp * sb;
}
public static List <double> YZXToZYX(double y, double z, double x)
{
z = z * Math.PI / 180.0;
y = y * Math.PI / 180.0;
x = x * Math.PI / 180.0;
double c1 = Math.Cos(y), s1 = Math.Sin(y);
double c2 = Math.Cos(z), s2 = Math.Sin(z);
double c3 = Math.Cos(x), s3 = Math.Sin(x);
double m00 = c1 * c2;
double m01 = s1 * s3 - c1 * c3 * s2;
double m02 = c3 * s1 + c1 * s2 * s3;
double m10 = s2;
double m11 = c2 * c3;
double m12 = -c2 * s3;
double m20 = -c2 * s1;
double m21 = c1 * s3 + c3 * s1 * s2;
double m22 = c1 * c3 - s1 * s2 * s3;
double w = Math.Sqrt(1.0 + m00 + m11 + m22) / 2.0;
double w4 = (4.0 * w);
x = (m21 - m12) / w4;
y = (m02 - m20) / w4;
z = (m10 - m01) / w4;
Q1 q;
if (w < 0)
{
q = new Q1(-w, -x, -y, -z);
}
else
{
q = new Q1(w, x, y, z);
}
EulerAngles e = q.ToEulerAngles();
return(new List <double>
{
e.Gamma.Degrees, // z
e.Beta.Degrees, // y
e.Alpha.Degrees, // x
});
}
// Run the tests
void Start()
{
Debug.Assert(AreVectorsEquivalent(
Conversions.ConvertPosition("+X+Y-Z", "+Y-Z+X", new Vector3(1, 2, 3)),
new Vector3(-3, 1, -2)
));
Debug.Assert(AreVectorsEquivalent(
Conversions.ConvertPosition("-Z-X+Y", "+X-Z+Y", new Vector3(1, 2, 3)),
new Vector3(-3, 2, 1)
));
Debug.Assert(
Conversions.ToQuaternion(new AxisSet("-Z-X-Y", true), new EulerAngles(30, 10, 20)) == Quaternion.Euler(10, 20, 30)
);
Debug.Assert(ExpectException(() => new AxisSet("-Z-XZ", false)));
Debug.Assert(ExpectException(() => new AxisSet("-Z-ZY", false)));
Debug.Assert(ExpectException(() => new AxisSet("X--YZ", false)));
Debug.Assert(ExpectException(() => new AxisSet("-+XYZ", false)));
Debug.Assert(ExpectException(() => new AxisSet("-+XUZ", false)));
Debug.Assert(ExpectException(() => new AxisSet("-Z-ZY", true)));
Debug.Assert(ExpectException(() => new CoordinateFrame("XYZ", "XXZ")));
Debug.Assert(ExpectException(() => new CoordinateFrame("XYX", "XYZ")));
// Cases that failed before fixing the floating point math issue
{
EulerAngles e = new EulerAngles(170, 90, -27);
EulerAngles to = Conversions.ConvertEulerOrder(Frames.Unity.RotationOrder, Frames.Unity.RotationOrder, e);
Debug.Assert(AreRotationsEquivalent(Frames.Unity.RotationOrder, e, to));
}
{
AxisSet ro = new AxisSet("-X-Y-X", true);
EulerAngles e = new EulerAngles(0, 90, 210);
EulerAngles to = Conversions.ConvertEulerOrder(Frames.Unity.RotationOrder, ro, e);
EulerAngles back = Conversions.ConvertEulerOrder(ro, Frames.Unity.RotationOrder, to);
Debug.Assert(AreRotationsEquivalent(Frames.Unity.RotationOrder, e, back));
}
StartCoroutine(RunConversionTests());
}
public override int GetHashCode()
{
int hash = 1;
if (header_ != null)
{
hash ^= Header.GetHashCode();
}
if (MeasurementTime != 0D)
{
hash ^= MeasurementTime.GetHashCode();
}
if (Type != 0)
{
hash ^= Type.GetHashCode();
}
if (position_ != null)
{
hash ^= Position.GetHashCode();
}
if (eulerAngles_ != null)
{
hash ^= EulerAngles.GetHashCode();
}
if (linearVelocity_ != null)
{
hash ^= LinearVelocity.GetHashCode();
}
if (angularVelocity_ != null)
{
hash ^= AngularVelocity.GetHashCode();
}
if (linearAcceleration_ != null)
{
hash ^= LinearAcceleration.GetHashCode();
}
hash ^= positionCovariance_.GetHashCode();
hash ^= eulerAnglesCovariance_.GetHashCode();
hash ^= linearVelocityCovariance_.GetHashCode();
hash ^= angularVelocityCovariance_.GetHashCode();
hash ^= linearAccelerationCovariance_.GetHashCode();
return(hash);
}
private Quaternion ConvertEulerAnglesToQuaternion(EulerAngles euler)
{
var c1 = Math.Cos(euler.Precession / 2);
var s1 = Math.Sin(euler.Precession / 2);
var c2 = Math.Cos(euler.Nutation / 2);
var s2 = Math.Sin(euler.Nutation / 2);
var c3 = Math.Cos(euler.Rotation / 2);
var s3 = Math.Sin(euler.Rotation / 2);
var c1c2 = c1 * c2;
var s1s2 = s1 * s2;
var w = (c1c2 * c3) - (s1s2 * s3);
var x = (s1 * c2 * c3) + (c1 * s2 * s3);
var y = (c1 * s2 * c3) - (s1 * c2 * s3);
var z = (c1c2 * s3) + (s1s2 * c3);
return(new Quaternion(w, x, y, z));
}
public SimpleIntegrationMotionPlusFuser()
{
integrator = new Integrator(3);
FusedValues = new EulerAngles(0, 0, 0);
}
public void HandleIMUData(double yawDown, double pitchLeft, double rollLeft, double accX, double accY, double accZ)
{
integrator.Update(new [] { yawDown, pitchLeft, rollLeft });
FusedValues = new EulerAngles(integrator.Values[0], integrator.Values[1], integrator.Values[2]);
}
