/// <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); }