/// <summary>
/// Method calles on every frame.
/// </summary>
public void Update()
{
if (this.ImuInitialized())
{
UnityEngine.Vector3 unity_acc = IMULocalizer.Instance.accelerometerValues;
UnityEngine.Vector3 unity_ori = EulerAnglesConversion.IMUToXYZ(IMULocalizer.Instance.localizerOrientation);
unity_ori = IMULocalizer.Instance.localizerOrientation;
Vector3 acc = new Vector3(unity_acc.x * (float)(9.81 / 8192), unity_acc.y * (float)(9.81 / 8192), unity_acc.z * (float)(9.81 / 8192));
Vector3 ori = new Vector3(unity_ori.x, unity_ori.y, unity_ori.z);
this.imuSource.AddMeasurements(IRescue.Core.Utils.StopwatchSingleton.Time, acc, ori);
}
}
public void IRescueToUnity()
{
Vector3 xyzd = new Vector3(90, -45, -45);
Vector3 zxyd = new Vector3(45, -90, -45);
Assert.AreEqual(Quaternion.Euler(zxyd).eulerAngles.ToString(), EulerAnglesConversion.XYZtoQuaternion(xyzd).eulerAngles.ToString());
xyzd = new Vector3(180, 180, 180);
zxyd = new Vector3(0, 0, 0);
Assert.AreEqual(Quaternion.Euler(zxyd).eulerAngles.ToString(), EulerAnglesConversion.XYZtoQuaternion(xyzd).eulerAngles.ToString());
xyzd = new Vector3(-90, 90, 90);
zxyd = new Vector3(0, -90, 180);
Assert.AreEqual(Quaternion.Euler(zxyd).eulerAngles.ToString(), EulerAnglesConversion.XYZtoQuaternion(xyzd).eulerAngles.ToString());
}
public void UnityToIRescue()
{
Vector3 xyzd = new Vector3(90, -45, -45);
Vector3 zxyd = new Vector3(45, -90, -45);
Vector3 actual = EulerAnglesConversion.ZXYtoXYZ(zxyd);
Assert.AreEqual(xyzd.ToString(), actual.ToString());
xyzd = new Vector3(90, 90, -90);
zxyd = new Vector3(0, -90, 180);
actual = EulerAnglesConversion.ZXYtoXYZ(zxyd);
Assert.AreEqual(xyzd.ToString(), actual.ToString());
xyzd = new Vector3(0, 0, 0);
zxyd = new Vector3(0, 0, 0);
actual = EulerAnglesConversion.ZXYtoXYZ(zxyd);
Assert.AreEqual(xyzd.ToString(), actual.ToString());
}
/// <summary>
/// Get all the transforms of the visible markers.
/// </summary>
/// <returns>Hash table with the marker id as the key and an IRVectorTransform as the value.</returns>
private Dictionary <int, TransformationMatrix> GetVisibleMarkers()
{
List <int> visibleMarkers = this.markerDetector.updatedMarkerTransforms;
if (visibleMarkers.Count > 0)
{
this.visibleMarkerTransforms = new Dictionary <int, TransformationMatrix>();
}
for (int i = 0; i < visibleMarkers.Count; i++)
{
int markerId = visibleMarkers[i];
UnityEngine.Vector3 metaOrientation = EulerAnglesConversion.ZXYtoXYZ(IMULocalizer.Instance.localizerOrientation);
this.markerDetector.SetMarkerTransform(markerId, ref this.markerTransform);
////Remove meta sdk added rotation for horizontal markers
this.markerTransform.Rotate(UnityEngine.Vector3.right, 90f);
UnityEngine.Vector3 xyzAngles = EulerAnglesConversion.ZXYtoXYZ(this.markerTransform.eulerAngles);
TransformationMatrix tcm = new TransformationMatrix(
this.markerTransform.position.x,
this.markerTransform.position.y,
this.markerTransform.position.z,
xyzAngles.x,
xyzAngles.y,
xyzAngles.z);
TransformationMatrix tum = new TransformationMatrix();
tum[3, 3] = 1;
// Remove the imu orientation that the meta sdk added.
TransformationMatrix Tcu = new TransformationMatrix(0, 0, 0, metaOrientation.x, metaOrientation.y, metaOrientation.z);
Tcu.Inverse().Multiply(tcm, tum);
// Rotate with 180 degrees in y to get transformation to the front of the marker instead of to the back.
tum.Multiply(new TransformationMatrix(0, 0, 0, 0, 180, 0), tum);
this.visibleMarkerTransforms.Add(markerId, tum);
}
return(this.visibleMarkerTransforms);
}
