public static Vector3F FromQuaternion(QuaternionF q)
{
var pitch = QuaternionF.Pitch(q);
var roll = QuaternionF.Roll(q);
var yaw = QuaternionF.Yaw(q);
return new Vector3F(pitch, roll, yaw);
}
/// <summary>
/// Initializes a new instance of the <see cref="OrientationDataEventArgs" /> class.
/// </summary>
/// <param name="myo">The Myo that raised the event. Cannot be <c>null</c>.</param>
/// <param name="timestamp">The timestamp of the event.</param>
/// <param name="orientationData">The orientation.</param>
/// <param name="roll">The roll.</param>
/// <param name="pitch">The pitch.</param>
/// <param name="yaw">The yaw.</param>
/// <exception cref="System.ArgumentNullException">
/// The exception that is thrown when <paramref name="myo"/> is <c>null</c>.
/// </exception>
public OrientationDataEventArgs(IMyo myo, DateTime timestamp, QuaternionF orientationData, double roll, double pitch, double yaw)
: base(myo, timestamp)
{
Contract.Requires<ArgumentNullException>(myo != null, "myo");
Contract.Requires<ArgumentNullException>(orientationData != null, "orientationData");
this.Orientation = orientationData;
this.Roll = roll;
this.Pitch = pitch;
this.Yaw = yaw;
}
public void QuickTest()
{
QuaternionF thing = new QuaternionF(0, 1, 2, 3);
Expect(thing.X, Is.EqualTo(0));
}
public void MoveLeft()
{
QuaternionF thing = new QuaternionF(0, 1, 2, 3);
Expect(thing.X, Is.EqualTo(0));
}
public static Vector3F operator *(QuaternionF quat, Vector3F vec)
{
Contract.Requires<ArgumentNullException>(quat != null, "quat");
Contract.Requires<ArgumentNullException>(vec != null, "vec");
Contract.Ensures(Contract.Result<Vector3F>() != null);
var qvec = new QuaternionF(vec.X, vec.Y, vec.Z, 0);
var result = quat * qvec * quat.Conjugate();
return new Vector3F(result.X, result.Y, result.Z);
}
public static float Yaw(QuaternionF quat)
{
Contract.Requires<ArgumentNullException>(quat != null, "quat");
return (float)SysMath.Atan2(
2.0f * (quat._w * quat._z + quat._x * quat._y),
1.0f - 2.0f * (quat._y * quat._y + quat._z * quat._z));
}
public static float Pitch(QuaternionF quat)
{
Contract.Requires<ArgumentNullException>(quat != null, "quat");
return (float)SysMath.Asin(2.0f * (quat._w * quat._y - quat._z * quat._x));
}
public static QuaternionF Normalize(QuaternionF quat)
{
Contract.Requires<ArgumentNullException>(quat != null, "quat");
Contract.Ensures(Contract.Result<QuaternionF>() != null);
return (quat / quat.Magnitude());
}
public void GetEventOrientation_ValidParameters_ExpectedBridgeCalls()
{
// Setup
var myoHandle = new IntPtr(123);
var eventHandle = new IntPtr(789);
var expectedResult = new QuaternionF(10, 20, 30, 40);
var bridge = new Mock<IMyoDeviceBridge>(MockBehavior.Strict);
bridge
.Setup(x => x.EventGetOrientation32(eventHandle, OrientationIndex.W))
.Returns(expectedResult.W);
bridge
.Setup(x => x.EventGetOrientation32(eventHandle, OrientationIndex.X))
.Returns(expectedResult.X);
bridge
.Setup(x => x.EventGetOrientation32(eventHandle, OrientationIndex.Y))
.Returns(expectedResult.Y);
bridge
.Setup(x => x.EventGetOrientation32(eventHandle, OrientationIndex.Z))
.Returns(expectedResult.Z);
bridge
.Setup(x => x.EventGetOrientation64(eventHandle, OrientationIndex.W))
.Returns(expectedResult.W);
bridge
.Setup(x => x.EventGetOrientation64(eventHandle, OrientationIndex.X))
.Returns(expectedResult.X);
bridge
.Setup(x => x.EventGetOrientation64(eventHandle, OrientationIndex.Y))
.Returns(expectedResult.Y);
bridge
.Setup(x => x.EventGetOrientation64(eventHandle, OrientationIndex.Z))
.Returns(expectedResult.Z);
var myoErrorHandlerDriver = new Mock<IMyoErrorHandlerDriver>(MockBehavior.Strict);
var driver = MyoDeviceDriver.Create(
myoHandle,
bridge.Object,
myoErrorHandlerDriver.Object);
// Execute
var result = driver.GetEventOrientation(eventHandle);
// Assert
Assert.Equal(expectedResult.W, result.W);
Assert.Equal(expectedResult.X, result.X);
Assert.Equal(expectedResult.Y, result.Y);
Assert.Equal(expectedResult.Z, result.Z);
bridge.Verify(x => x.EventGetOrientation32(eventHandle, OrientationIndex.W), PlatformInvocation.Running32Bit ? Times.Once() : Times.Never());
bridge.Verify(x => x.EventGetOrientation32(eventHandle, OrientationIndex.X), PlatformInvocation.Running32Bit ? Times.Once() : Times.Never());
bridge.Verify(x => x.EventGetOrientation32(eventHandle, OrientationIndex.Y), PlatformInvocation.Running32Bit ? Times.Once() : Times.Never());
bridge.Verify(x => x.EventGetOrientation32(eventHandle, OrientationIndex.Z), PlatformInvocation.Running32Bit ? Times.Once() : Times.Never());
bridge.Verify(x => x.EventGetOrientation64(eventHandle, OrientationIndex.W), PlatformInvocation.Running32Bit ? Times.Never() : Times.Once());
bridge.Verify(x => x.EventGetOrientation64(eventHandle, OrientationIndex.X), PlatformInvocation.Running32Bit ? Times.Never() : Times.Once());
bridge.Verify(x => x.EventGetOrientation64(eventHandle, OrientationIndex.Y), PlatformInvocation.Running32Bit ? Times.Never() : Times.Once());
bridge.Verify(x => x.EventGetOrientation64(eventHandle, OrientationIndex.Z), PlatformInvocation.Running32Bit ? Times.Never() : Times.Once());
}
public static float Pitch(QuaternionF quat)
{
Contract.Requires <ArgumentNullException>(quat != null, "quat");
return((float)SysMath.Asin(2.0f * (quat._w * quat._y - quat._z * quat._x)));
}
public void GetOrientation_ValidState_EqualsConstructorParameter()
{
// Setup
var orientationData = new QuaternionF(1, 2, 3, 4);
var args = new OrientationDataEventArgs(
new Mock<IMyo>().Object,
DateTime.UtcNow,
orientationData,
0,
0,
0);
// Execute
var result = args.Orientation;
// Assert
Assert.Equal(orientationData, result);
}
public static float Yaw(QuaternionF quat)
{
return((float)SysMath.Atan2(
2.0f * (quat._w * quat._z + quat._x * quat._y),
1.0f - 2.0f * (quat._y * quat._y + quat._z * quat._z)));
}
public static float Pitch(QuaternionF quat)
{
return((float)SysMath.Asin(2.0f * (quat._w * quat._y - quat._z * quat._x)));
}
public static float Roll(QuaternionF quat)
{
return((float)SysMath.Atan2(
2.0f * (quat._w * quat._x + quat._y * quat._z),
1.0f - 2.0f * (quat._x * quat._x + quat._y * quat._y)));
}
public static QuaternionF Normalize(QuaternionF quat)
{
return(quat / quat.Magnitude());
}