private void UpdateRelativeTransform() { convertMatrixRelative = ConvertMatrix.LeftMultiply(convertMatrixWhenSendImagSC, convertMatrixWhenSendImagSLAM); convertMatrixRelative.Inverse(); /* * ConvertMatrix convertMatrixRelativeNew = ConvertMatrix.LeftMultiply(convertMatrixWhenSendImagSC, convertMatrixWhenSendImagSLAM); * convertMatrixRelativeNew.Inverse(); * * // direct assign if not initialized * if (!systemInited) * { * convertMatrixRelative = convertMatrixRelativeNew; * return; * } * * Vector3 changed_position = (convertMatrixRelative.GetPosition() - convertMatrixRelativeNew.GetPosition()); * if(changed_position.magnitude > 1.0) * { * return; * } * * // we choose to use smooth adjustment to offer better user experience * // init variables for coroutine * quaternionRelativeFrom = convertMatrixRelative.GetQuaternion(); * quaternionRelativeTo = convertMatrixRelativeNew.GetQuaternion(); * positionRelativeFrom = convertMatrixRelative.GetPosition(); * positionRelativeTo = convertMatrixRelativeNew.GetPosition(); * * // stop the old coroutine if exist * if (coroutine != null) * { * StopCoroutine(coroutine); * } * * // start the new coroutine * coroutine = WaitAndRotate(0.2f); * StartCoroutine(coroutine); */ }
private void UpdateRelativeTransform() { convertMatrixRelative = ConvertMatrix.LeftMultiply(convertMatrixWhenSendImagSC, convertMatrixWhenSendImagSLAM); convertMatrixRelative.Inverse(); }