Skip to content

Instantly share code, notes, and snippets.

@unitycoder
Last active January 2, 2023 17:30
Show Gist options
  • Save unitycoder/cb4d21709b7e8b1f465396aeca95507b to your computer and use it in GitHub Desktop.
Save unitycoder/cb4d21709b7e8b1f465396aeca95507b to your computer and use it in GitHub Desktop.
Gyroscope Camera Script
// https://www.digikey.com/Site/Global/Layouts/DownloadPdf.ashx?pdfUrl=E7EEB830E57D43B09CE51BD7DF0E74D8
void Start()
{
gyro = Input.gyro; // Store the reference for Gyroscope sensor gyro.enabled = true; //Enable the Gyroscope sensor
}
void Update()
{
if (Time.timeScale != 0)
{
Quaternion transQuat = Quaternion.identity; //Adjust Unity output quaternion as per android SensorManager
transQuat.w = gyro.attitude.x; transQuat.x = gyro.attitude.y; transQuat.y = gyro.attitude.z; transQuat.z = gyro.attitude.w; transQuat = Quaternion.Euler(90, 0, 0) * transQuat;//change axis around
if (DefaultOrientationIsPortrait)
{
transform.Rotate(new Vector3(0, 0, 1), 90.0f); // apply z rotation to // correct camera orientation }
}
}
// not good
// https://gist.github.com/kormyen/a1e3c144a30fc26393f14f09989f03e1
using UnityEngine;
using System.Collections;
public class GyroCamera : MonoBehaviour
{
// STATE
private float _initialYAngle = 0f;
private float _appliedGyroYAngle = 0f;
private float _calibrationYAngle = 0f;
private Transform _rawGyroRotation;
private float _tempSmoothing;
// SETTINGS
[SerializeField] private float _smoothing = 0.1f;
private IEnumerator Start()
{
Input.gyro.enabled = true;
Application.targetFrameRate = 60;
_initialYAngle = transform.eulerAngles.y;
_rawGyroRotation = new GameObject("GyroRaw").transform;
_rawGyroRotation.position = transform.position;
_rawGyroRotation.rotation = transform.rotation;
// Wait until gyro is active, then calibrate to reset starting rotation.
yield return new WaitForSeconds(1);
StartCoroutine(CalibrateYAngle());
}
private void Update()
{
ApplyGyroRotation();
ApplyCalibration();
transform.rotation = Quaternion.Slerp(transform.rotation, _rawGyroRotation.rotation, _smoothing);
}
private IEnumerator CalibrateYAngle()
{
_tempSmoothing = _smoothing;
_smoothing = 1;
_calibrationYAngle = _appliedGyroYAngle - _initialYAngle; // Offsets the y angle in case it wasn't 0 at edit time.
yield return null;
_smoothing = _tempSmoothing;
}
private void ApplyGyroRotation()
{
_rawGyroRotation.rotation = Input.gyro.attitude;
_rawGyroRotation.Rotate(0f, 0f, 180f, Space.Self); // Swap "handedness" of quaternion from gyro.
_rawGyroRotation.Rotate(90f, 180f, 0f, Space.World); // Rotate to make sense as a camera pointing out the back of your device.
_appliedGyroYAngle = _rawGyroRotation.eulerAngles.y; // Save the angle around y axis for use in calibration.
}
private void ApplyCalibration()
{
_rawGyroRotation.Rotate(0f, -_calibrationYAngle, 0f, Space.World); // Rotates y angle back however much it deviated when calibrationYAngle was saved.
}
public void SetEnabled(bool value)
{
enabled = true;
StartCoroutine(CalibrateYAngle());
}
}
// from https://forum.unity3d.com/threads/sharing-gyroscope-camera-script-ios-tested.241825/#post-2954253
using UnityEngine;
using System.Collections;
public class GyroCamera1 : MonoBehaviour
{
private float initialYAngle = 0f;
private float appliedGyroYAngle = 0f;
private float calibrationYAngle = 0f;
void Start()
{
Application.targetFrameRate = 60;
initialYAngle = transform.eulerAngles.y;
}
void Update()
{
ApplyGyroRotation();
ApplyCalibration();
}
void OnGUI()
{
if( GUILayout.Button( "Calibrate", GUILayout.Width( 300 ), GUILayout.Height( 100 ) ) )
{
CalibrateYAngle();
}
}
public void CalibrateYAngle()
{
calibrationYAngle = appliedGyroYAngle - initialYAngle; // Offsets the y angle in case it wasn't 0 at edit time.
}
void ApplyGyroRotation()
{
transform.rotation = Input.gyro.attitude;
transform.Rotate( 0f, 0f, 180f, Space.Self ); // Swap "handedness" of quaternion from gyro.
transform.Rotate( 90f, 180f, 0f, Space.World ); // Rotate to make sense as a camera pointing out the back of your device.
appliedGyroYAngle = transform.eulerAngles.y; // Save the angle around y axis for use in calibration.
}
void ApplyCalibration()
{
transform.Rotate( 0f, -calibrationYAngle, 0f, Space.World ); // Rotates y angle back however much it deviated when calibrationYAngle was saved.
}
}
// not good
// Gyro camera rotation for Android, tested in Unity 2019.3.2 for API level 28, in landscape left orientation.
// Extended to include X and Z axis when claibrating, so you can re-center when looking in any direction, and at a variable speed.
// Attach this script to the parent GameObject of your camera. To re-center, from your main script, call CalibrateAngle, set a bool "letsGoToCenter" to true, a bool "goToCenterSlowly" to true or false, and a string "centerRotSpeed" to slow or fast.
// centerRotSpeed is ignored when goToCenterSlowly is false. You can use "Unity Remote 5" in the Play store to test Android gyro in Unity editor.
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
public class GyroCamera2 : MonoBehaviour
{
// STATE
private float _initialXAngle = 0f;
private float _appliedGyroXAngle = 0f;
private float _calibrationXAngle = 0f;
private float _initialYAngle = 0f;
private float _appliedGyroYAngle = 0f;
private float _calibrationYAngle = 0f;
private float _initialZAngle = 0f;
private float _appliedGyroZAngle = 0f;
private float _calibrationZAngle = 0f;
public Transform _rawGyroRotation;
Quaternion currentOffset = Quaternion.Euler(0f, 0f, 0f);
Quaternion currentAngle;
Quaternion newAngle;
float timeToLerp = 1f;
float timeLerped = 0.0f;
private float _tempSmoothing;
// SETTINGS
[SerializeField] private float _smoothing = 0.1f;
private IEnumerator Start()
{
Input.gyro.enabled = true;
_initialXAngle = transform.eulerAngles.x;
_initialYAngle = transform.eulerAngles.y;
_initialZAngle = transform.eulerAngles.z;
_rawGyroRotation = new GameObject("GyroRaw").transform;
_rawGyroRotation.position = transform.position;
_rawGyroRotation.rotation = transform.rotation;
// Wait until gyro is active, then calibrate to reset starting rotation.
yield return new WaitForSeconds(1);
StartCoroutine(CalibrateAngle());
}
private void Update()
{
ApplyGyroRotation();
ApplyCalibration();
newAngle = _rawGyroRotation.rotation * Quaternion.Inverse(currentOffset);
transform.rotation = Quaternion.Slerp(transform.rotation, newAngle, _smoothing);
}
public IEnumerator CalibrateAngle()
{
_tempSmoothing = _smoothing;
_smoothing = 1;
// Offsets the angle in case it wasn't 0 at edit time.
_calibrationXAngle = _appliedGyroXAngle - _initialXAngle;
_calibrationYAngle = _appliedGyroYAngle - _initialYAngle;
_calibrationZAngle = _appliedGyroZAngle - _initialZAngle;
yield return null;
_smoothing = _tempSmoothing;
currentOffset = _rawGyroRotation.rotation;
currentAngle = transform.rotation;
timeLerped = 0.0f;
}
private void ApplyGyroRotation()
{
_rawGyroRotation.rotation = Input.gyro.attitude;
_rawGyroRotation.Rotate(0f, 0f, 180f, Space.Self); // Swap "handedness" of quaternion from gyro.
_rawGyroRotation.Rotate(90f, 180f, 0f, Space.World); // Rotate to make sense as a camera pointing out the back of your device.
// Save the angle around axis for use in calibration.
_appliedGyroXAngle = _rawGyroRotation.eulerAngles.x;
_appliedGyroYAngle = _rawGyroRotation.eulerAngles.y;
_appliedGyroZAngle = _rawGyroRotation.eulerAngles.z;
}
private void ApplyCalibration()
{
// Rotates back however much it deviated when calibrationAngle was saved.
_rawGyroRotation.Rotate(_calibrationXAngle, -_calibrationYAngle, _calibrationZAngle, Space.World);
}
}
// best so far?
// https://developpaper.com/unity3d-uses-gyroscopes-to-control-node-rotation/
********************************************************************
Desc: logical class of gyroscope to camera.
*********************************************************************/
using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using UnityEngine;
namespace Game.Gyro
{
/// <summary>
///Responsibilities:
///1. Realize the influence and operation of gyroscope on camera;
///2. Try to reproduce the main interface cockpit effect of crash 3;
/// </summary>
class GyroCamera : MonoBehaviour
{
#Region statement
///< summary > input type of gyroscope
public enum EGyroInputType
{
/// <summary> RotateRate </summary>
RotateRate,
/// <summary> RotateRateUniased </summary>
RotateRateUniased,
/// <summary> UserAcceleration </summary>
UserAcceleration,
}
#endregion
#Region control variable
public float m_gyro_max_x = 15.0f;
public float m_gyro_max_y = 15.0f;
public float m_gyro_max_z = 15.0f;
#endregion
#Region variable
///Analog gyroscope input in < summary > editor development environment
public Vector3 m_editor_debug_input = Vector3.zero;
///The input parameters of the < summary > gyroscope are used to control the camera
public Vector3 m_gyro_input = Vector3.zero;
///< summary > current camera angle < / summary >
public Vector3 m_cur_euler = Vector3.zero;
///< summary > update frequency of gyroscope data
public int m_upate_rate = 30;
///< summary > input type of current gyroscope
public EGyroInputType m_gyro_input_type = EGyroInputType.RotateRate;
///< summary > coefficient of gyroscope
public float m_gyro_factor = 1.0f;
private Vector3 m_camera_init_euler = Vector3.zero;
private Transform mTransform;
#endregion
#Region access interface
///The input parameters of the < summary > gyroscope are used to control the camera
protected Vector3 GyroInput
{
get
{
return m_gyro_input;
}
set
{
m_gyro_input = value;
}
}
///< summary > input data type of gyroscope
protected EGyroInputType GyroInputType
{
get
{
return m_gyro_input_type;
}
set
{
m_gyro_input_type = value;
}
}
///< summary > coefficient of gyroscope
protected float GyroFactor
{
get
{
return m_gyro_factor;
}
set
{
m_gyro_factor = value;
}
}
///< summary > current rotation angle
protected Vector3 CurEuler
{
get
{
return m_cur_euler;
}
set
{
m_cur_euler = value;
}
}
#endregion
#region Unity
// Use this for initialization
void Start()
{
Input.gyro.enabled = true;
mTransform = gameObject.transform;
CurEuler = mTransform.localEulerAngles;
m_camera_init_euler = CurEuler;
}
///< summary > Draw UI for debugging
void OnGUI()
{
//GUI.Label(GetRect(0.1f, 0.05f), "Attitude: " + Input.gyro.attitude);
//GUI.Label(GetRect(0.1f, 0.15f), "Rotation: " + Input.gyro.rotationRate);
//GUI.Label(GetRect(0.1f, 0.25f), "RotationUnbiased: " + Input.gyro.rotationRateUnbiased);
//GUI.Label(GetRect(0.1f, 0.35f), "UserAcceleration: " + Input.gyro.userAcceleration);
////Coefficient of gyroscope
//{
// string t_factor_str = GUI.TextField(GetRect(0.7f, 0.05f), "" + GyroFactor);
// GyroFactor = float.Parse(t_factor_str);
//}
////Input parameters of gyroscope
//{
// if (GUI.Button(GetRect(0.8f, 0.8f, 0.2f), "" + GyroInputType))
// {
// switch (GyroInputType)
// {
// case EGyroInputType.RotateRate:
// GyroInputType = EGyroInputType.RotateRateUniased;
// break;
// case EGyroInputType.RotateRateUniased:
// GyroInputType = EGyroInputType.UserAcceleration;
// break;
// case EGyroInputType.UserAcceleration:
// GyroInputType = EGyroInputType.RotateRate;
// break;
// }
// }
//}
}
// Update is called once per frame
void Update()
{
//Set gyro update frequency
Input.gyro.updateInterval = 1.0f / m_upate_rate;
//Calculate the control data of the camera according to the gyroscope
UpdateGyro();
//Debugging under editor
#if UNITY_EDITOR
//The gyroscope can not be used in the development environment to simulate the data
GyroInput = m_editor_debug_input;
#endif
//Because of the uncertain range of the value, the control coefficient should be added
GyroInput = GyroInput * GyroFactor;
//Operate and change the camera according to the control data
UpdateCamera();
}
#endregion
#Region control logic
///Update the gyroscope data and calculate the corresponding control data
protected void UpdateGyro()
{
//Update the gyroscope data and calculate the control variables
switch (GyroInputType)
{// on the mobile phone, the left tilt x is negative, and the left tilt x is positive. Up tilt y is negative and down tilt y is positive
case EGyroInputType.RotateRate:
GyroInput = Input.gyro.rotationRate;
break;
case EGyroInputType.RotateRateUniased:
GyroInput = Input.gyro.rotationRateUnbiased;
break;
case EGyroInputType.UserAcceleration:
GyroInput = Input.gyro.userAcceleration;
break;
default:
Debug.LogError("GyroInputTypeNot defined: " + GyroInputType);
break;
}
}
///< summary > behavior of updating camera
protected void UpdateCamera()
{
//The Z parameter of gyro is not required
#if UNITY_EDITOR
Vector3 t_gyro_input = new Vector3(GyroInput.x, GyroInput.y, GyroInput.z);
#else
Vector3 t_gyro_input = new Vector3(0.0f, GyroInput.y, GyroInput.x);
#endif
CurEuler += t_gyro_input;
//Scope control
{
float t_x = ClampFloat(CurEuler.x, m_camera_init_euler.x, m_gyro_max_x);
float t_y = ClampFloat(CurEuler.y, m_camera_init_euler.y, m_gyro_max_y);
float t_z = ClampFloat(CurEuler.z, m_camera_init_euler.z, m_gyro_max_z);
CurEuler = new Vector3(t_x, t_y, t_z);
}
mTransform.localEulerAngles = CurEuler;
}
#endregion
#Region support function
protected float ClampFloat(float p_float, float p_init, float p_offset)
{
p_offset = Mathf.Abs(p_offset);
if (p_float > p_init + p_offset)
{
p_float = p_init + p_offset;
}
if (p_float < p_init - p_offset)
{
p_float = p_init - p_offset;
}
return p_float;
}
///< summary > get the approximate coordinates of GUI according to the percentage
protected Rect GetRect(float p_x_percent, float p_y_percent, float p_w = 0.5f, float p_h = 0.1f)
{
return new Rect(
Screen.width * p_x_percent, Screen.height * p_y_percent,
Screen.width * p_w, Screen.height * p_h);
}
#endregion
}
}
// not good
// https://developpaper.com/unity3d-realizes-vr-camera-function-based-on-gyroscope/
using UnityEngine;
using System.Collections;
public class GyroController : MonoBehaviour
{
// Fields
private readonly Quaternion baseIdentity = Quaternion.Euler(90f, 0f, 0f);
private Quaternion baseOrientation = Quaternion.Euler(90f, 0f, 0f);
private Quaternion baseOrientationRotationFix = Quaternion.identity;
private Quaternion calibration = Quaternion.identity;
private Quaternion cameraBase = Quaternion.identity;
private bool debug = true;
public static bool gyroAvaiable;
private bool gyroEnabled = true;
private Quaternion gyroInitialRotation;
public static bool gyroOff;
private Quaternion initialRotation;
private readonly Quaternion landscapeLeft = Quaternion.Euler(0f, 0f, -90f);
private readonly Quaternion landscapeRight = Quaternion.Euler(0f, 0f, 90f);
private const float lowPassFilterFactor = 0.1f;
private Quaternion offsetRotation;
private Quaternion referanceRotation = Quaternion.identity;
private readonly Quaternion upsideDown = Quaternion.Euler(0f, 0f, 180f);
// Methods
private void AttachGyro()
{
this.gyroEnabled = true;
this.ResetBaseOrientation();
this.UpdateCalibration(true);
this.UpdateCameraBaseRotation(true);
this.RecalculateReferenceRotation();
}
private void Awake()
{
gyroAvaiable = SystemInfo.supportsGyroscope;
}
private static Quaternion ConvertRotation(Quaternion q)
{
return new Quaternion(q.x, q.y, -q.z, -q.w);
}
private void DetachGyro()
{
this.gyroEnabled = false;
}
private Quaternion GetRotFix()
{
return Quaternion.identity;
}
private void RecalculateReferenceRotation()
{
this.referanceRotation = Quaternion.Inverse(this.baseOrientation) * Quaternion.Inverse(this.calibration);
}
private void ResetBaseOrientation()
{
this.baseOrientationRotationFix = this.GetRotFix();
this.baseOrientation = this.baseOrientationRotationFix * this.baseIdentity;
}
protected void Start()
{
Input.gyro.enabled = true;
base.enabled = true;
this.AttachGyro();
this.initialRotation = base.transform.localRotation;
this.gyroInitialRotation = Input.gyro.attitude;
}
private void Update()
{
gyroOff = PlayerPrefs.GetInt("gyro-off") == 1;
if (this.gyroEnabled )
{
base.transform.localRotation = Quaternion.Slerp(base.transform.localRotation, this.cameraBase * (ConvertRotation(this.referanceRotation * Input.gyro.attitude) * this.GetRotFix()), 0.5f);//0.1f
}
}
private void UpdateCalibration(bool onlyHorizontal)
{
if (onlyHorizontal)
{
Vector3 toDirection = (Vector3) (Input.gyro.attitude * -Vector3.forward);
toDirection.z = 0f;
if (toDirection == Vector3.zero)
{
this.calibration = Quaternion.identity;
}
else
{
this.calibration = Quaternion.FromToRotation((Vector3) (this.baseOrientationRotationFix * Vector3.up), toDirection);
}
}
else
{
this.calibration = Input.gyro.attitude;
}
}
private void UpdateCameraBaseRotation(bool onlyHorizontal)
{
if (onlyHorizontal)
{
Vector3 forward = base.transform.forward;
forward.y = 0f;
if (forward == Vector3.zero)
{
this.cameraBase = Quaternion.identity;
}
else
{
this.cameraBase = Quaternion.FromToRotation(Vector3.forward, forward);
}
}
else
{
this.cameraBase = base.transform.rotation;
}
}
}
// best but drifts, http://answers.unity.com/comments/1671752/view.html
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
public class GyroControllerBestish : $$anonymous$$onoBehaviour
{
GameObject camParent;
// Start is called before the first frame update
void Start()
{
camParent = new GameObject("CamParent");
camParent.transform.position = this.transform.position;
this.transform.parent = camParent.transform;
Input.gyro.enabled = true;
}
// Update is called once per frame
void Update()
{
camParent.transform.Rotate(0, -Input.gyro.rotationRateUnbiased.y, 0);
this.transform.Rotate(-Input.gyro.rotationRateUnbiased.x, 0, 0);
}
}
// by chatGPT
using UnityEngine;
public class GyroFreeLookCamera : MonoBehaviour
{
private Gyroscope gyro;
private Quaternion rot;
void Start()
{
gyro = Input.gyro;
gyro.enabled = true;
rot = new Quaternion(0, 0, 1, 0);
}
void Update()
{
transform.rotation = gyro.attitude * rot;
}
}
// not working nicely
// https://gist.github.com/wtekteam/85059c86f91c42752bb4
using UnityEngine;
using System.Collections;
// Activate head tracking using the gyroscope
// The script must be linked to the main camera
public class HeadTracking : MonoBehaviour
{
// The initials orientation
private int initialOrientationX;
private int initialOrientationY;
private int initialOrientationZ;
// Initialization
void Start ()
{
// Activate the gyroscope
Input.gyro.enabled = true;
Input.gyro.updateInterval = 0.01f;
// Save the firsts values
initialOrientationX = (int)Input.gyro.rotationRateUnbiased.x;
initialOrientationY = (int)Input.gyro.rotationRateUnbiased.y;
initialOrientationZ = (int)-Input.gyro.rotationRateUnbiased.z;
}
// Update is called once per frame
void Update ()
{
transform.Rotate (initialOrientationX -Input.gyro.rotationRateUnbiased.x, initialOrientationY -Input.gyro.rotationRateUnbiased.y, initialOrientationZ + Input.gyro.rotationRateUnbiased.z);
}
}
// https://answers.unity.com/questions/1629272/quaternions-applied-to-sensor-fusion-gyroattitude.html
using UnityEngine;
public class iOSOrientationTracker : MonoBehaviour
{
GameObject initialHeading;
private void Start()
{
Input.compensateSensors = true;
Input.gyro.enabled = false;
Input.gyro.enabled = true;
initialHeading = new GameObject("initialHeading");
initialHeading.transform.parent = transform;
initialHeading.transform.Rotate(Vector3.up, Input.compass.trueHeading, Space.Self);
}
public Quaternion GetQuaternion()
{
temp = Input.gyro.attitude;
//convert attitude to camera pose
temp.Set(temp.z, temp.w, temp.x, temp.y);
temp = Quaternion.Euler(-90, 180, 0) * temp;
return initialHeading.transform.localRotation * temp;
}
Quaternion temp = Quaternion.identity, lastPose = Quaternion.identity, lastAttitude = Quaternion.identity;
public Quaternion GetQuaternionAbsolute()
{
temp = Input.gyro.attitude;
//convert attitude to camera pose
temp.Set(temp.z, temp.w, temp.x, temp.y);
temp = Quaternion.Euler(-90, 180, 0) * temp;
//Get vertical rotation since last frame
float yRot = temp.eulerAngles.y - lastAttitude.eulerAngles.y;
if (yRot > 180)
yRot -= 360;
else if (yRot <= -180)
yRot += 360;
//update lastAttitude
lastAttitude = temp;
//apply rotation around Y and correct with compass
yRot = ((lastPose.eulerAngles.y + yRot) * .9f) + (Input.compass.trueHeading * .1f);
yRot -= temp.eulerAngles.y;
if (yRot > 360)
{
yRot -= 360;
if (yRot > 180)
yRot -= 360;
}
else if (yRot <= -180)
{
yRot += 360;
if (yRot <= -360)
yRot += 360;
}
//calculate pose with compass heading
lastPose = Quaternion.AngleAxis(yRot, Vector3.up) * temp;
return lastPose;
}
}
// https://github.com/Wave-Unity-Library/Motion-Mapping#gyro-cam
using UnityEngine;
public class MobileGyroCamera : MonoBehaviour
{
public float slerpValue = 0.2f;
public int verticalOffsetAngle = -90;
public int horizontalOffsetAngle = 0;
private Quaternion phoneOrientation;
private Quaternion correctedPhoneOrientation;
private Quaternion horizontalRotationCorrection;
private Quaternion verticalRotationCorrection;
private Quaternion inGameOrientation;
void Start()
{
Input.gyro.enabled = true;
}
void Update()
{
// camera orientation with gyro
phoneOrientation = Input.gyro.attitude;
correctedPhoneOrientation = new Quaternion(phoneOrientation.x, phoneOrientation.y, -phoneOrientation.z, -phoneOrientation.w);
verticalRotationCorrection = Quaternion.AngleAxis(verticalOffsetAngle, Vector3.left);
horizontalRotationCorrection = Quaternion.AngleAxis(horizontalOffsetAngle, Vector3.up);
inGameOrientation = horizontalRotationCorrection * verticalRotationCorrection * correctedPhoneOrientation;
transform.rotation = Quaternion.Slerp(transform.rotation, inGameOrientation, slerpValue);
}
}
// https://github.com/unitycoder/UnitySensorFusion
using UnityEngine;
using System.Collections;
public class SensorFusion : MonoBehaviour
{
static Vector3 accelerometer = new Vector3();
static Vector3 gyroscope = new Vector3();
static Quaternion filterToWorldQ = new Quaternion();
static Quaternion inverseWorldToScreenQ = new Quaternion();
static Quaternion worldToScreenQ = new Quaternion();
static Quaternion originalPoseAdjustQ = new Quaternion();
static Quaternion resetQ = new Quaternion();
static float previousTime;
const float PREDICTION_TIME = 0.040f;
static int windowOrientation = 0;
static bool isLandscape = false;
static ComplementaryFilter filter;
static PosePredictor posePredictor;
public static Quaternion GetOrientation()
{
Quaternion orientation = filter.GetOrientation();
// Predict orientation.
Quaternion predictedQ = posePredictor.GetPrediction(orientation, gyroscope, previousTime);
Quaternion o = filterToWorldQ;
o *= resetQ;
o *= predictedQ;
o *= worldToScreenQ;
return o;
}
public static void Recenter()
{
// Reduce to inverted yaw-only.
resetQ = filter.GetOrientation();
resetQ.x = 0;
resetQ.y = 0;
resetQ.z *= -1;
// Take into account extra transformations in landscape mode.
if(isLandscape)
resetQ *= inverseWorldToScreenQ;
// Take into account original pose.
resetQ *= originalPoseAdjustQ;
}
public static void SetScreenTransform()
{
worldToScreenQ = new Quaternion(0, 0, 0, 1);
switch(windowOrientation)
{
case 0:
break;
case 90:
worldToScreenQ = Quaternion.AngleAxis(-Mathf.PI / 2, new Vector3(0, 0, 1));
break;
case -90:
worldToScreenQ = Quaternion.AngleAxis(Mathf.PI / 2, new Vector3(0, 0, 1));
break;
case 180:
// TODO
break;
}
inverseWorldToScreenQ = Quaternion.Inverse(worldToScreenQ);
}
void Awake()
{
if(SystemInfo.supportsGyroscope)
{
filter = new ComplementaryFilter(.98f, PREDICTION_TIME);
Input.gyro.enabled = true;
}
else
{
filter = new ComplementaryFilter(.5f, PREDICTION_TIME);
}
posePredictor = new PosePredictor(PREDICTION_TIME);
filterToWorldQ = Quaternion.AngleAxis(-Mathf.PI / 2, new Vector3(1, 0, 0));
originalPoseAdjustQ = Quaternion.AngleAxis(-windowOrientation * Mathf.PI / 18, new Vector3(0, 0, 1));
SetScreenTransform();
if(isLandscape)
filterToWorldQ *= inverseWorldToScreenQ;
Recenter();
}
void LateUpdate()
{
Vector3 accGravity = Input.acceleration; // INCLUDING GRAVITY
Vector3 rotRate = Input.gyro.rotationRate;
float time = Time.time;
float deltaS = time - previousTime;
accelerometer = -accGravity;
gyroscope = rotRate;
filter.AddAccelerationSample(accelerometer, time);
filter.AddGyroSample(gyroscope, time);
previousTime = time;
}
public class ComplementaryFilter
{
/*
An implementation of a simple complementary filter, which fuses gyroscope and accelerometer data
Accelerometer data is very noisy, but stable over the long term.
Gyroscope data is smooth, but tends to drift over the long term.
This fusion is relatively simple:
1. Get orientation estimates from accelerometer by applying a low-pass filter on that data.
2. Get orientation estimates from gyroscope by integrating over time.
3. Combine the two estimates, weighing (1) in the long term, but (2) for the short term.
*/
SensorSample currentAccelSample = new SensorSample();
SensorSample currentGyroSample = new SensorSample();
SensorSample previousGyroSample = new SensorSample();
// Set the quaternion to be looking in the -z direction by default.
Quaternion filterQ = new Quaternion(1, 0, 0, 1);
Quaternion previousFilterQ = new Quaternion();
// Orientation based on the accelerometer.
Quaternion accelQ = new Quaternion();
// Whether or not the orientation has been initialized.
bool isOrientationInitialized = false;
// Running estimate of gravity based on the current orientation.
Vector3 estimatedGravity = new Vector3();
// Measured gravity based on accelerometer.
Vector3 measuredGravity = new Vector3();
float accelGyroFactor;
float predictionTime;
public ComplementaryFilter(float accelGyroFactor, float predictionTime)
{
this.accelGyroFactor = accelGyroFactor;
this.predictionTime = predictionTime;
}
public void AddAccelerationSample(Vector3 value, float time)
{
currentAccelSample.value = value;
currentAccelSample.time = time;
}
public void AddGyroSample(Vector3 value, float time)
{
currentGyroSample.value = value;
currentGyroSample.time = time;
float delta = time - previousGyroSample.time;
//
if(!isOrientationInitialized)
{
accelQ = AccelToQuaternion(currentAccelSample.value);
previousFilterQ = accelQ;
isOrientationInitialized = true;
return;
}
var deltaT = currentGyroSample.time -
previousGyroSample.time;
// Convert gyro rotation vector to a quaternion delta.
Quaternion gyroDeltaQ = GyroToQuaternionDelta(currentGyroSample.value, deltaT);
// filter_1 = K * (filter_0 + gyro * dT) + (1 - K) * accel.
filterQ = previousFilterQ;
filterQ *= gyroDeltaQ;
// Calculate the delta between the current estimated gravity and the real
// gravity vector from accelerometer.
Quaternion invFilterQ = new Quaternion();
invFilterQ = filterQ;
invFilterQ = Quaternion.Inverse(invFilterQ);
estimatedGravity = new Vector3(0, 0, -1);
estimatedGravity = ApplyQuaternion(invFilterQ, estimatedGravity);
estimatedGravity.Normalize();
measuredGravity = currentAccelSample.value;
measuredGravity.Normalize();
// Compare estimated gravity with measured gravity, get the delta quaternion
// between the two.
Quaternion deltaQ = new Quaternion();
deltaQ.SetFromToRotation(estimatedGravity, measuredGravity);
deltaQ = Quaternion.Inverse(deltaQ);
// Calculate the SLERP target: current orientation plus the measured-estimated
// quaternion delta.
Quaternion targetQ = new Quaternion();
targetQ = filterQ;
targetQ *= deltaQ;
// SLERP factor: 0 is pure gyro, 1 is pure accel.
filterQ = Quaternion.Slerp(filterQ, targetQ, 1 - accelGyroFactor);
previousFilterQ = filterQ;
//
previousGyroSample = currentGyroSample;
}
public Quaternion GetOrientation()
{
return this.filterQ;
}
public Quaternion AccelToQuaternion(Vector3 accel)
{
Vector3 normAccel = accel;
normAccel.Normalize();
Quaternion quat = new Quaternion();
quat.SetFromToRotation(new Vector3(0, 0, -1), normAccel);
quat = Quaternion.Inverse(quat);
return quat;
}
public Quaternion GyroToQuaternionDelta(Vector3 gyro, float dt)
{
// Extract axis and angle from the gyroscope data.
Quaternion quat = new Quaternion();
Vector3 axis = gyro;
axis.Normalize();
quat = Quaternion.AngleAxis(gyro.magnitude * dt, axis);
return quat;
}
public Vector3 ApplyQuaternion(Quaternion q, Vector3 v)
{
Vector3 o = new Vector3();
// calculate quat * vector
var ix = q.w * v.x + q.y * v.z - q.z * v.y;
var iy = q.w * v.y + q.z * v.x - q.x * v.z;
var iz = q.w * v.z + q.x * v.y - q.y * v.x;
var iw = -q.x * v.x - q.y * v.y - q.z * v.z;
// calculate result * inverse quat
o.x = ix * q.w + iw * -q.x + iy * -q.z - iz * -q.y;
o.y = iy * q.w + iw * -q.y + iz * -q.x - ix * -q.z;
o.z = iz * q.w + iw * -q.z + ix * -q.y - iy * -q.x;
return o;
}
}
public class PosePredictor
{
/*
Given an orientation and the gyroscope data, predicts the future orientation. This makes rendering appear faster.
Also see: http://msl.cs.uiuc.edu/~lavalle/papers/LavYerKatAnt14.pdf
*/
float predictionTime;
Quaternion previousQ = new Quaternion();
float previousTime;
Quaternion deltaQ = new Quaternion();
Quaternion outQ = new Quaternion();
public PosePredictor(float predictionTime)
{
this.predictionTime = predictionTime;
}
public Quaternion GetPrediction(Quaternion currentQ, Vector3 gyro, float time)
{
Debug.Log(currentQ);
if(previousTime == 0)
{
previousQ = currentQ;
previousTime = time;
return currentQ;
}
// Calculate axis and angle based on gyroscope rotation rate data.
Vector3 axis = new Vector3();
axis = gyro;
axis.Normalize();
float angularSpeed = gyro.magnitude;
// If we're rotating slowly, don't do prediction.
if(angularSpeed < Mathf.Deg2Rad * 20)
{
outQ = currentQ;
previousQ = currentQ;
return outQ;
}
// Get the predicted angle based on the time delta and latency.
float predictAngle = predictionTime;
deltaQ = Quaternion.AngleAxis(predictAngle, axis);
outQ = previousQ;
outQ *= deltaQ;
previousQ = currentQ;
return outQ;
}
}
public struct SensorSample
{
public Vector3 value;
public float time;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment