KudanPositionTracker

KudanPositionTracker.cs

using UnityEngine;
using Kudan.AR;

public class KudanPositionTracker : MonoBehaviour {

  public KudanTracker _kudanTracker;
  private float scaleFactor = 0.0004f;

  private Vector3 origVector;
  private Vector3 origPos;

  private void StartTracking()
  {
    if (!_kudanTracker.ArbiTrackIsTracking())
    {
      Vector3 floorPosition;
      Quaternion floorOrientation;
      _kudanTracker.FloorPlaceGetPose(out floorPosition, out floorOrientation);
      _kudanTracker.ArbiTrackStart(floorPosition, floorOrientation);
    }
  }

  private void Awake()
  {
    origPos = gameObject.transform.localPosition;
  }

  void OnApplicationFocus(bool focus)
  {
    if (focus)
    {
      isFirst = true;
      startTime = Time.time;
    }
    else
    {
      _kudanTracker.StopTracking();
    }
  }

  private bool isFirst = true;
  private float startTime = 0f;

  void Update () {
    if (isFirst && Time.time-startTime > 5f)
    {
      isFirst = false;
      StartTracking();
      Vector3 origPosition;
      Quaternion origOrientation;
      _kudanTracker.ArbiTrackGetPose(out origPosition, out origOrientation);
      origVector = RevRotateVector(origOrientation, origPosition);
    }
    if (_kudanTracker.ArbiTrackIsTracking())
    {
      Vector3 pos;
      Quaternion o;
      _kudanTracker.ArbiTrackGetPose(out pos, out o);
      var curVector = RevRotateVector(o, pos);
      var diffPos = curVector - origVector;
      var newPos = new Vector3(
        diffPos.x * scaleFactor,
        -diffPos.y * scaleFactor,
        diffPos.z * scaleFactor);
      gameObject.transform.localPosition = (newPos + origPos);
    }
  }

  private Vector3 RevRotateVector(Quaternion o, Vector3 v)
  {
    var euler = o.eulerAngles;
    var vy = Quaternion.AngleAxis(-euler.y, Vector3.up) * v;
    var vyx = Quaternion.AngleAxis(-euler.x, Vector3.right) * vy;
    return vyx;
  }
}