private void OnDrawGizmos()
{
if (!UnityEngine.Application.isPlaying)
{
return;
}
var pcas = _service.GetAllPCA();
if (pcas.Count == 0)
{
return;
}
Vector3 pos = Vector3.zero;
Quaternion rot = Quaternion.identity;
GetAverage(pcas, out pos, out rot);
Gizmos.color = Color.red;
Gizmos.DrawWireSphere(pos, 0.05f);
Gizmos.color = Color.green;
Vector3 n = rot * Vector3.up;
Gizmos.DrawLine(pos, pos + n * 0.1f);
}
Pose ISpatialAnchorSystem.GetAnchorPose(Dictionary <PCAID, Pose> comparePCAData)
{
var localPCA = _pcaService.GetAllPCA();
var filterdLocalPCA = localPCA
.Where(p => comparePCAData.ContainsKey(p.ID))
.ToArray();
if (filterdLocalPCA.Length == 0)
{
return(default);
private void AdaptToRemotePose()
{
var localPCA = _pcaService.GetAllPCA();
var filterdLocalPCA = localPCA
.Where(p => _remotePCADatabase.ContainsKey(p.ID))
.ToArray();
if (filterdLocalPCA.Length == 0)
{
return;
}
Vector3 averagePos = Vector3.zero;
float x = 0, y = 0, z = 0, w = 0;
Quaternion firstRot = filterdLocalPCA[0].Rotation;
bool foundFirstRot = false;
foreach (var p in filterdLocalPCA)
{
Pose offsetPose = _spatialAnchorUtility.ConvertToWorldPose(p, _remotePCADatabase[p.ID]);
averagePos += offsetPose.position;
if (!foundFirstRot)
{
firstRot = offsetPose.rotation;
foundFirstRot = true;
}
float dot = Quaternion.Dot(firstRot, offsetPose.rotation);
float multi = dot > 0f ? 1f : -1f;
x += offsetPose.rotation.x * multi;
y += offsetPose.rotation.y * multi;
z += offsetPose.rotation.z * multi;
w += offsetPose.rotation.w * multi;
}
averagePos /= filterdLocalPCA.Length;
float k = 1f / Mathf.Sqrt(x * x + y * y + z * z + w * w);
Quaternion averageRot = new Quaternion(x * k, y * k, z * k, w * k);
transform.SetPositionAndRotation(averagePos, averageRot);
}
