static Pose CallPluginAtEndOfFrames(Transform3Df pose)
{
Matrix4x4 cameraPoseFromSolAR = Matrix4x4.identity;
var pos = pose.translation();
var rot = pose.rotation();
//cameraPoseFromSolAR.SetRow(0, new Vector4(rot.coeff(0, 0), rot.coeff(0, 1), rot.coeff(0, 2), pos.coeff(0, 0)));
//cameraPoseFromSolAR.SetRow(1, new Vector4(rot.coeff(1, 0), rot.coeff(1, 1), rot.coeff(1, 2), pos.coeff(0, 1)));
//cameraPoseFromSolAR.SetRow(2, new Vector4(rot.coeff(2, 0), rot.coeff(2, 1), rot.coeff(2, 2), pos.coeff(0, 2)));
//cameraPoseFromSolAR.SetRow(3, new Vector4(0, 0, 0, 1));
for (int r = 0; r < 3; ++r)
{
for (int c = 0; c < 3; ++c)
{
cameraPoseFromSolAR[r, c] = rot.coeff(r, c);
}
cameraPoseFromSolAR[r, 3] = pos.coeff(0, r);
}
Matrix4x4 unityCameraPose = invertMatrix * cameraPoseFromSolAR;
Vector3 forward = new Vector3(unityCameraPose.m02, unityCameraPose.m12, unityCameraPose.m22);
Vector3 up = new Vector3(unityCameraPose.m01, unityCameraPose.m11, unityCameraPose.m21);
var q = Quaternion.LookRotation(forward, -up);
var p = new Vector3(unityCameraPose.m03, unityCameraPose.m13, unityCameraPose.m23);
return(new Pose(p, q));
}
public static Pose ToUnity(this Transform3Df pose)
{
var inv = new Matrix4x4();
inv.SetRow(0, new Vector4(+1, +0, +0, +0));
inv.SetRow(1, new Vector4(+0, -1, +0, +0));
inv.SetRow(2, new Vector4(+0, +0, +1, +0));
inv.SetRow(3, new Vector4(+0, +0, +0, +1));
var rot = pose.rotation();
var trans = pose.translation();
var m = new Matrix4x4();
for (int r = 0; r < 3; ++r)
{
for (int c = 0; c < 3; ++c)
{
m[r, c] = rot.coeff(r, c);
}
m[r, 3] = trans.coeff(r, 0);
}
m.SetRow(3, new Vector4(0, 0, 0, 1));
m = m.inverse;
m = inv * m;
//m = m.inverse;
var v = m.GetColumn(3);
var q = Quaternion.LookRotation(m.GetColumn(2), m.GetColumn(1));
q = Quaternion.Inverse(q);
v = q * -v;
return(new Pose(v, q));
}
void Update()
{
if (isScanning && UpdateReady)
{
if (m_pipelineManager != null)
{
if (m_Unity_Webcam)
{
GetPhysicalCameraFrame();
}
Transform3Df pose = new Transform3Df();
var _returnCode = m_pipelineManager.udpate(pose);
if (_returnCode == PIPELINEMANAGER_RETURNCODE._NEW_POSE || _returnCode == PIPELINEMANAGER_RETURNCODE._NEW_POSE_AND_IMAGE)
{
if (!isMarkerFound)
{
isMarkerFound = true;
findDeviceCanvas.SetActive(false);
cardboardLoader.SetActive(true);
deviceCameraScript.gameObject.SetActive(false);
phoneManagerScript.ChangeVisibility(true);
}
Matrix4x4 cameraPoseFromSolAR = new Matrix4x4();
cameraPoseFromSolAR.SetRow(0, new Vector4(pose.rotation().coeff(0, 0), pose.rotation().coeff(0, 1), pose.rotation().coeff(0, 2), pose.translation().coeff(0, 0)));
cameraPoseFromSolAR.SetRow(1, new Vector4(pose.rotation().coeff(1, 0), pose.rotation().coeff(1, 1), pose.rotation().coeff(1, 2), pose.translation().coeff(1, 0)));
cameraPoseFromSolAR.SetRow(2, new Vector4(pose.rotation().coeff(2, 0), pose.rotation().coeff(2, 1), pose.rotation().coeff(2, 2), pose.translation().coeff(2, 0)));
cameraPoseFromSolAR.SetRow(3, new Vector4(0, 0, 0, 1));
Matrix4x4 invertMatrix = new Matrix4x4();
invertMatrix.SetRow(0, new Vector4(1, 0, 0, 0));
invertMatrix.SetRow(1, new Vector4(0, -1, 0, 0));
invertMatrix.SetRow(2, new Vector4(0, 0, 1, 0));
invertMatrix.SetRow(3, new Vector4(0, 0, 0, 1));
Matrix4x4 unityCameraPose = invertMatrix * cameraPoseFromSolAR;
Vector3 forward = new Vector3(unityCameraPose.m02, unityCameraPose.m12, unityCameraPose.m22);
Vector3 up = new Vector3(unityCameraPose.m01, unityCameraPose.m11, unityCameraPose.m21);
var rotation = Quaternion.LookRotation(forward, -up);
var position = -new Vector3(unityCameraPose.m03, unityCameraPose.m13, unityCameraPose.m23);
m_PrevSolARObj.localPosition = position;
m_RotationControl.localRotation = rotation;
var prevParent = m_PrevSolARObj.parent;
m_PrevSolARObj.parent = m_RotationControl;
m_RotationControl.localRotation = Quaternion.identity;
m_PrevSolARObj.parent = prevParent;
m_PrevSolARObj.localRotation = Quaternion.Inverse(rotation);
m_PrevSolARObj.Rotate(new Vector3(-90, 0, 0), Space.Self);
if (Screen.orientation == ScreenOrientation.LandscapeRight)
{
m_PrevSolARObj.localPosition -= new Vector3(cameraXShift, 0, 0);
}
else
{
m_PrevSolARObj.localPosition += new Vector3(cameraXShift, 0, 0);
}
var rotDist = Quaternion.Angle(prevRot, m_PrevSolARObj.rotation);
if (rotDist >= 10)
{
prevRot = m_PrevSolARObj.rotation;
socketSignalerScript.ResetGyroInverse();
}
var posDist = Vector3.Distance(m_SolARObj.position, m_PrevSolARObj.position);
float distLerpStatus = (Time.time - distLerpTime) * lerpSpeed;
////var angleDifferences = GetAngleDifferences(m_SolARObj.rotation, m_PrevSolARObj.rotation);
if (posDist >= 0.008 && distLerpStatus >= 1)
{
//Debug.Log("Pos dist: " + posDist);
//m_SolARObj.position = m_PrevSolARObj.position;
distLerpTime = Time.time;
curPos = m_SolARObj.position;
prevPos = m_PrevSolARObj.position;
}
distLerpStatus = (Time.time - distLerpTime) * lerpSpeed;
m_SolARObj.position = Vector3.Lerp(curPos, prevPos, distLerpStatus);
}
}
}
SetFusedRotation();
}
