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(); }