-
Notifications
You must be signed in to change notification settings - Fork 70
/
SpatialGraphCoordinateSystem.cs
133 lines (117 loc) · 5.08 KB
/
SpatialGraphCoordinateSystem.cs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
#if WINDOWS_UWP
using Windows.Perception.Spatial;
#endif
using Microsoft.MixedReality.Toolkit.Utilities;
namespace QRTracking
{
public class SpatialGraphCoordinateSystem : MonoBehaviour
{
#if WINDOWS_UWP
private SpatialCoordinateSystem CoordinateSystem = null;
#endif
private System.Guid id;
public System.Guid Id
{
get
{
return id;
}
set
{
id = value;
#if WINDOWS_UWP
CoordinateSystem = Windows.Perception.Spatial.Preview.SpatialGraphInteropPreview.CreateCoordinateSystemForNode(id);
if (CoordinateSystem == null)
{
Debug.Log("Id= " + id + " Failed to acquire coordinate system");
}
#endif
}
}
void Awake()
{
}
// Use this for initialization
void Start()
{
#if WINDOWS_UWP
if (CoordinateSystem == null)
{
CoordinateSystem = Windows.Perception.Spatial.Preview.SpatialGraphInteropPreview.CreateCoordinateSystemForNode(id);
if (CoordinateSystem == null)
{
Debug.Log("Id= " + id + " Failed to acquire coordinate system");
}
}
#endif
}
private void UpdateLocation()
{
{
#if WINDOWS_UWP
if (CoordinateSystem == null)
{
CoordinateSystem = Windows.Perception.Spatial.Preview.SpatialGraphInteropPreview.CreateCoordinateSystemForNode(id);
if (CoordinateSystem == null)
{
Debug.Log("Id= " + id + " Failed to acquire coordinate system");
}
}
if (CoordinateSystem != null)
{
Quaternion rotation = Quaternion.identity;
Vector3 translation = new Vector3(0.0f, 0.0f, 0.0f);
System.IntPtr rootCoordnateSystemPtr = UnityEngine.XR.WindowsMR.WindowsMREnvironment.OriginSpatialCoordinateSystem;
SpatialCoordinateSystem rootSpatialCoordinateSystem = (SpatialCoordinateSystem)System.Runtime.InteropServices.Marshal.GetObjectForIUnknown(rootCoordnateSystemPtr);
// Get the relative transform from the unity origin
System.Numerics.Matrix4x4? relativePose = CoordinateSystem.TryGetTransformTo(rootSpatialCoordinateSystem);
if (relativePose != null)
{
System.Numerics.Vector3 scale;
System.Numerics.Quaternion rotation1;
System.Numerics.Vector3 translation1;
System.Numerics.Matrix4x4 newMatrix = relativePose.Value;
// Platform coordinates are all right handed and unity uses left handed matrices. so we convert the matrix
// from rhs-rhs to lhs-lhs
// Convert from right to left coordinate system
newMatrix.M13 = -newMatrix.M13;
newMatrix.M23 = -newMatrix.M23;
newMatrix.M43 = -newMatrix.M43;
newMatrix.M31 = -newMatrix.M31;
newMatrix.M32 = -newMatrix.M32;
newMatrix.M34 = -newMatrix.M34;
System.Numerics.Matrix4x4.Decompose(newMatrix, out scale, out rotation1, out translation1);
translation = new Vector3(translation1.X, translation1.Y, translation1.Z);
rotation = new Quaternion(rotation1.X, rotation1.Y, rotation1.Z, rotation1.W);
Pose pose = new Pose(translation, rotation);
// If there is a parent to the camera that means we are using teleport and we should not apply the teleport
// to these objects so apply the inverse
if (CameraCache.Main.transform.parent != null)
{
pose = pose.GetTransformedBy(CameraCache.Main.transform.parent);
}
gameObject.transform.SetPositionAndRotation(pose.position, pose.rotation);
//Debug.Log("Id= " + id + " QRPose = " + pose.position.ToString("F7") + " QRRot = " + pose.rotation.ToString("F7"));
}
else
{
// Debug.Log("Id= " + id + " Unable to locate qrcode" );
}
}
else
{
gameObject.SetActive(false);
}
#endif
}
}
// Update is called once per frame
void Update()
{
UpdateLocation();
}
}
}