void SetupBackgroundTexture()
{
int textureId = SDPlugin.SixDegreesSDK_GetBackgroundTexture();
if (textureId > 0)
{
int width = 1920;
int height = 1080;
unsafe
{
int *widthPtr = &width, heightPtr = &height;
SDPlugin.SixDegreesSDK_GetBackgroundTextureSize(widthPtr, heightPtr);
}
Debug.Log("Create External Texture:" + textureId + "(" + width + "x" + height + ")");
mBackgroundTexture = Texture2D.CreateExternalTexture(
width,
height,
TextureFormat.RGBA32,
false,
false,
new System.IntPtr(textureId));
mBackgroundTexture.filterMode = FilterMode.Point;
mBackgroundTexture.name = "camera_texture";
background.color = Color.black;
background.texture = mBackgroundTexture;
background.material.SetTexture("_MainTex", mBackgroundTexture);
}
}
void Update()
{
if (!SDPlugin.IsSDKReady)
{
return; // will try later
}
int blockBufferSize = 0;
int vertexBufferSize = 0;
int faceBufferSize = 0;
int newVersion = -1;
unsafe
{
newVersion = SDPlugin.SixDegreesSDK_GetMeshBlockInfo(&blockBufferSize, &vertexBufferSize, &faceBufferSize);
}
if (newVersion > meshVersion && blockBufferSize > 0 && vertexBufferSize > 0 && faceBufferSize > 0)
{
if (meshVersion < 0)
{
blockSize = SDPlugin.SixDegreesSDK_GetMeshBlockSize();
blocksPerChunk = Mathf.Max(1, Mathf.FloorToInt(chunkSize / blockSize));
chunkSize = blocksPerChunk * blockSize;
}
UpdateMesh(newVersion, blockBufferSize, vertexBufferSize, faceBufferSize);
}
else if (newVersion == 0 && meshVersion > 0 && blockBufferSize == 0)
{
// Mesh was reset after loading a new map
ClearMesh();
}
}
void FixedUpdate()
{
if (!SDPlugin.IsSDKReady)
{
return; // will try later
}
int blockBufferSize = 0;
int vertexBufferSize = 0;
int faceBufferSize = 0;
int newVersion = -1;
unsafe
{
newVersion = SDPlugin.SixDegreesSDK_GetBlockMeshInfo(&blockBufferSize, &vertexBufferSize, &faceBufferSize);
}
if (newVersion > meshVersion && blockBufferSize > 0 && vertexBufferSize > 0 && faceBufferSize > 0)
{
if (meshVersion < 0)
{
blockSize = SDPlugin.SixDegreesSDK_GetMeshBlockSize();
}
UpdateMesh(newVersion, blockBufferSize, vertexBufferSize, faceBufferSize);
}
else if (newVersion == 0 && meshVersion > 0 && blockBufferSize == 0)
{
// Mesh was reset after loading a new map
ClearMesh();
}
}
void UpdateCamera()
{
// enable the camera, update its rect and replace its default projection matrix
int width = 1920;
int height = 1080;
unsafe
{
int *widthPtr = &width, heightPtr = &height;
SDPlugin.SixDegreesSDK_GetBackgroundTextureSize(widthPtr, heightPtr);
}
mCamera.rect = SDCameraUtils.GetCameraRect(width, height);
mCamera.projectionMatrix = SDCameraUtils.GetProjectionMatrix();
mScreenOrientation = Screen.orientation;
}
public void SetupBackgroundTexture()
{
background.gameObject.SetActive(true);
IntPtr texturePtr = IntPtr.Zero;
#if UNITY_IOS && !UNITY_EDITOR
if (SystemInfo.graphicsDeviceType == UnityEngine.Rendering.GraphicsDeviceType.OpenGLES3)
{
int textureId = SDPlugin.SixDegreesSDK_GetEAGLBackgroundTexture();
texturePtr = new IntPtr(textureId);
}
else
#endif
{
texturePtr = SDPlugin.SixDegreesSDK_GetBackgroundTexture();
}
if (texturePtr != IntPtr.Zero)
{
int width = 1920;
int height = 1080;
unsafe
{
int *widthPtr = &width, heightPtr = &height;
SDPlugin.SixDegreesSDK_GetBackgroundTextureSize(widthPtr, heightPtr);
}
Debug.Log("Create External Texture:" + texturePtr + "(" + width + "x" + height + ")");
mBackgroundTexture = Texture2D.CreateExternalTexture(
width,
height,
TextureFormat.RGBA32,
false,
false,
texturePtr);
mBackgroundTexture.filterMode = FilterMode.Point;
mBackgroundTexture.name = "camera_texture";
background.color = Color.black;
background.texture = mBackgroundTexture;
background.material.SetTexture("_MainTex", mBackgroundTexture);
}
}
unsafe void UpdatePose()
{
fixed(float *ptr = &mPoseBuffer[0])
{
// R T
// 0 1
int bufferSize = 16;
mTrackingState = SDPlugin.SixDegreesSDK_GetPose(ptr, bufferSize);
}
switch (mTrackingState)
{
case (int)SDPlugin.SDTrackingQuality.Good:
case (int)SDPlugin.SDTrackingQuality.Limited:
{
Matrix4x4 mat = new Matrix4x4();
for (int col = 0; col < 4; col++)
{
for (int row = 0; row < 4; row++)
{
// Column major order
mat[row, col] = mPoseBuffer[col * 4 + row];
}
}
Vector3 position = mat.GetColumn(3);
Vector3 forward;
Vector3 upwards;
switch (Screen.orientation)
{
case ScreenOrientation.LandscapeRight:
forward = mat.GetColumn(2);
upwards = -mat.GetColumn(1);
break;
case ScreenOrientation.Portrait:
forward = mat.GetColumn(2);
upwards = -mat.GetColumn(0);
break;
case ScreenOrientation.PortraitUpsideDown:
forward = mat.GetColumn(2);
upwards = mat.GetColumn(0);
break;
default:
case ScreenOrientation.LandscapeLeft:
forward = mat.GetColumn(2);
upwards = mat.GetColumn(1);
break;
}
Quaternion rotation = Quaternion.LookRotation(forward, upwards);
transform.SetPositionAndRotation(position, rotation);
break;
}
case (int)SDPlugin.SDTrackingQuality.None:
default:
{
break;
}
}
}
void Start()
{
// disable if the device doesn't build meshes on the fly
enabled = SDPlugin.SixDegreesSDK_HasRealTimeMesh();
}
void UpdateMesh(int newVersion, int blockBufferSize, int vertexBufferSize, int faceBufferSize)
{
meshVersion = newVersion;
int[] blockArray = new int[blockBufferSize];
float[] vertexArray = new float[vertexBufferSize];
int[] faceArray = new int[faceBufferSize];
unsafe
{
fixed(int *blockBufferPtr = &blockArray[0], faceBufferPtr = &faceArray[0])
{
fixed(float *vertexBufferPtr = &vertexArray[0])
{
fullBlocks = SDPlugin.SixDegreesSDK_GetMeshBlocks(blockBufferPtr, vertexBufferPtr, faceBufferPtr,
blockBufferSize, vertexBufferSize, faceBufferSize);
}
}
}
bool gotAllBlocks = (fullBlocks == blockBufferSize / 6);
if (fullBlocks < 0)
{
Debug.Log("Error calling SixDegreesSDK_GetMeshBlocks(), will not update the mesh.");
return;
}
else if (fullBlocks == 0)
{
Debug.Log("SixDegreesSDK_GetMeshBlocks() gave us an empty mesh, will not update.");
return;
}
else if (!gotAllBlocks)
{
Debug.Log("SixDegreesSDK_GetMeshBlocks() returned " + fullBlocks + " full blocks, expected " + (blockBufferSize / 5));
}
int firstVertex = 0;
int firstTriangle = 0;
HashSet <Vector3Int> chunksToUpdate = new HashSet <Vector3Int>();
HashSet <Vector3Int> meshBlocks = new HashSet <Vector3Int>();
// Update all the full blocks returned by the API
int fullBlockSize = fullBlocks * 6;
for (int b = 0; b + 5 < fullBlockSize; b += 6)
{
Vector3Int blockCoords = new Vector3Int(blockArray[b], blockArray[b + 1], blockArray[b + 2]);
Vector3Int chunkCoords = GetChunkCoords(blockCoords);
int vertexCount = blockArray[b + 3];
int triangleCount = blockArray[b + 4];
int blockVersion = blockArray[b + 5];
meshBlocks.Add(blockCoords);
if (!IsChunkTooFar(chunkCoords))
{
SDMeshChunk chunk = GetOrCreateChunk(chunkCoords);
SDMeshBlock block = GetOrCreateBlock(chunk, blockCoords);
// Update block if it is outdated
if (block.version < blockVersion)
{
chunksToUpdate.Add(chunkCoords);
block.version = blockVersion;
block.meshVersion = meshVersion;
block.vertices.Clear();
block.normals.Clear();
block.triangles.Clear();
// copy vertices
int fullVertices = 0;
int lastBlockVertex = Mathf.Min(vertexBufferSize, (firstVertex + vertexCount) * 6);
for (int va = firstVertex * 6; va + 5 < lastBlockVertex; va += 6)
{
block.vertices.Add(new Vector3(vertexArray[va], vertexArray[va + 1], vertexArray[va + 2]));
block.normals.Add(new Vector3(vertexArray[va + 3], vertexArray[va + 4], vertexArray[va + 5]));
fullVertices++;
}
if (fullVertices != vertexCount)
{
Debug.Log("Got only " + fullVertices + " vertices out of " + vertexCount);
}
// copy faces
int fullTriangles = 0;
int lastBlockTriangle = Mathf.Min(faceBufferSize, (firstTriangle + triangleCount) * 3);
for (int fa = firstTriangle * 3; fa + 2 < lastBlockTriangle; fa += 3)
{
block.triangles.Add(new Vector3Int(faceArray[fa] - firstVertex, faceArray[fa + 1] - firstVertex, faceArray[fa + 2] - firstVertex));
fullTriangles++;
}
if (fullTriangles != triangleCount)
{
Debug.Log("Got only " + fullTriangles + " triangles out of " + triangleCount);
}
}
}
firstVertex += vertexCount;
firstTriangle += triangleCount;
}
// Clean up missing blocks only if we received all the expected full blocks
if (gotAllBlocks)
{
foreach (SDMeshChunk chunk in chunks.Values)
{
HashSet <Vector3Int> blocksToRemove = new HashSet <Vector3Int>(chunk.blocks.Keys);
blocksToRemove.ExceptWith(meshBlocks);
foreach (Vector3Int block in blocksToRemove)
{
chunk.blocks.Remove(block);
}
if (blocksToRemove.Count > 0)
{
chunksToUpdate.Add(chunk.coordinates);
}
}
}
// Update chunks with updated blocks
UpdateChunks(chunksToUpdate);
// Clean up far chunks
if (destroyFarChunks)
{
List <Vector3Int> allChunks = new List <Vector3Int>(chunks.Keys);
List <Vector3Int> chunksToDelete = allChunks.FindAll((Vector3Int chunkCoords) => IsChunkTooFar(chunkCoords));
DeleteChunks(chunksToDelete);
}
}
public static Matrix4x4 GetProjectionMatrix()
{
const int bufferSize = 16;
float[] projectionBuffer = new float[bufferSize];
unsafe
{
fixed(float *ptr = &projectionBuffer[0])
{
SDPlugin.SixDegreesSDK_GetProjection(ptr, bufferSize);
}
}
Matrix4x4 projectionMatrix = new Matrix4x4();
for (int row = 0; row < 4; row++)
{
for (int col = 0; col < 4; col++)
{
projectionMatrix[row, col] = projectionBuffer[row + col * 4];
}
}
bool portrait = Screen.orientation == ScreenOrientation.Portrait ||
Screen.orientation == ScreenOrientation.PortraitUpsideDown;
if (Mode == CameraMode.Fill)
{
// fix the projection when filling the screen
int width = 1920;
int height = 1080;
unsafe
{
int *widthPtr = &width, heightPtr = &height;
SDPlugin.SixDegreesSDK_GetBackgroundTextureSize(widthPtr, heightPtr);
}
float texAspect = (float)width / height;
float windowAspect = portrait ? (float)Screen.height / Screen.width
: (float)Screen.width / Screen.height;
if (texAspect > windowAspect)
{
float scaleFactor = texAspect / windowAspect;
projectionMatrix[0, 0] *= scaleFactor;
projectionMatrix[0, 2] *= scaleFactor;
}
if (windowAspect > texAspect)
{
float scaleFactor = windowAspect / texAspect;
projectionMatrix[1, 1] *= scaleFactor;
projectionMatrix[1, 2] *= scaleFactor;
}
}
if (portrait)
{
// fix the projection in portrait
float p00 = projectionMatrix[0, 0];
projectionMatrix[0, 0] = projectionMatrix[1, 1];
projectionMatrix[1, 1] = p00;
float p02 = projectionMatrix[0, 2];
projectionMatrix[0, 2] = projectionMatrix[1, 2];
projectionMatrix[1, 2] = p02;
}
return(projectionMatrix);
}
void UpdateMesh(int newVersion, int blockBufferSize, int vertexBufferSize, int faceBufferSize)
{
meshVersion = newVersion;
int[] blockArray = new int[blockBufferSize];
float[] vertexArray = new float[vertexBufferSize];
int[] faceArray = new int[faceBufferSize];
int fullBlocks = 0;
unsafe
{
fixed(int *blockBufferPtr = &blockArray[0], faceBufferPtr = &faceArray[0])
{
fixed(float *vertexBufferPtr = &vertexArray[0])
{
fullBlocks = SDPlugin.SixDegreesSDK_GetBlockMesh(blockBufferPtr, vertexBufferPtr, faceBufferPtr,
blockBufferSize, vertexBufferSize, faceBufferSize);
}
}
}
bool gotAllBlocks = (fullBlocks == blockBufferSize / 6);
if (fullBlocks < 0)
{
Debug.Log("Error calling SixDegreesSDK_GetMeshBlocks(), will not update the mesh.");
return;
}
else if (fullBlocks == 0)
{
Debug.Log("SixDegreesSDK_GetMeshBlocks() gave us an empty mesh, will not update.");
return;
}
else if (!gotAllBlocks)
{
Debug.Log("SixDegreesSDK_GetMeshBlocks() returned " + fullBlocks + " full blocks, expected " + (blockBufferSize / 6));
return;
}
int firstVertex = 0;
int firstTriangle = 0;
int normalsOffset = vertexBufferSize / 2;
// Update all the full blocks returned by the API
for (int b = 0; b < blockBufferSize; b += 6)
{
// Transform block coordinates from 6D right-handed coordinates to Unity left-handed coordinates
// By flipping the sign of Z
Vector3Int blockCoords = new Vector3Int(blockArray[b], blockArray[b + 1], -blockArray[b + 2]);
int vertexCount = blockArray[b + 3];
int triangleCount = blockArray[b + 4];
int blockVersion = blockArray[b + 5];
SDMeshBlock currentBlock;
if (!blocks.TryGetValue(blockCoords, out currentBlock))
{
currentBlock = new SDMeshBlock();
blocks[blockCoords] = currentBlock;
}
currentBlock.meshVersion = meshVersion;
// Update block if it is outdated
if (currentBlock.version < blockVersion)
{
currentBlock.version = blockVersion;
List <Vector3> vertices = new List <Vector3>();
List <Vector3> normals = new List <Vector3>();
List <int> triangles = new List <int>();
// copy vertices
for (int va = firstVertex; va < firstVertex + vertexCount; va++)
{
// Transform vertices from 6D right-handed coordinates to Unity left-handed coordinates
// By flipping the sign of Z
int pos = va * 3;
vertices.Add(new Vector3(vertexArray[pos], vertexArray[pos + 1], -vertexArray[pos + 2]));
int norm = normalsOffset + pos;
normals.Add(new Vector3(vertexArray[norm], vertexArray[norm + 1], -vertexArray[norm + 2]));
}
// copy faces
for (int fa = firstTriangle; fa < firstTriangle + triangleCount; fa++)
{
//block.triangles.Add(new Vector3Int(faceArray[fa] - firstVertex, faceArray[fa + 2] - firstVertex, faceArray[fa + 1] - firstVertex));
int f = fa * 3;
triangles.Add(faceArray[f] - firstVertex);
triangles.Add(faceArray[f + 2] - firstVertex);
triangles.Add(faceArray[f + 1] - firstVertex);
}
if (currentBlock.gameObject == null)
{
currentBlock.gameObject = Instantiate(meshPrefab);
currentBlock.gameObject.transform.parent = gameObject.transform;
if (!meshVisible && depthMaskMaterial != null)
{
MeshRenderer meshRenderer = currentBlock.gameObject.GetComponent <MeshRenderer>();
if (meshRenderer != null)
{
meshRenderer.material = depthMaskMaterial;
}
}
}
Mesh workMesh = currentBlock.gameObject.GetComponent <MeshFilter>().mesh;
workMesh.Clear();
workMesh.vertices = vertices.ToArray();
workMesh.normals = normals.ToArray();
workMesh.triangles = triangles.ToArray();
MeshCollider meshCollider = currentBlock.gameObject.GetComponent <MeshCollider>();
// update the collider less often for optimal performance
if (meshCollider && (currentBlock.colliderVersion < 0 || (currentBlock.colliderVersion + 10 < currentBlock.version)))
{
meshCollider.sharedMesh = workMesh;
currentBlock.colliderVersion = currentBlock.version;
}
}
firstVertex += vertexCount;
firstTriangle += triangleCount;
}
List <Vector3Int> keys = new List <Vector3Int>(blocks.Keys);
// Clean up obsolete blocks
foreach (Vector3Int blockCoords in keys)
{
if (blocks[blockCoords].meshVersion != meshVersion)
{
Destroy(blocks[blockCoords].gameObject.GetComponent <MeshFilter>().mesh);
Destroy(blocks[blockCoords].gameObject);
blocks.Remove(blockCoords);
}
}
}
