/// <summary>
/// Set controller's transformation based on received pose.
/// </summary>
/// <param name="pose">Received Tango pose data.</param>
private void _UpdateTransformationFromPose(TangoPoseData pose)
{
// Remember the previous position, so you can do delta motion
m_prevTangoPosition = m_tangoPosition;
m_prevTangoRotation = m_tangoRotation;
// The callback pose is for device with respect to start of service pose.
if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
Vector3 position;
Quaternion rotation;
TangoSupport.TangoPoseToWorldTransform(pose, out position, out rotation);
m_uwTuc = Matrix4x4.TRS(position, rotation, Vector3.one);
Matrix4x4 uwOffsetTuc = m_uwOffsetTuw * m_uwTuc;
m_tangoPosition = uwOffsetTuc.GetColumn(3);
m_tangoRotation = Quaternion.LookRotation(uwOffsetTuc.GetColumn(2), uwOffsetTuc.GetColumn(1));
// Other pose data -- Pose count gets reset if pose status just became valid.
if (pose.status_code != m_poseStatus)
{
m_poseCount = 0;
}
m_poseCount++;
// Other pose data -- Pose delta time.
m_poseDeltaTime = (float)pose.timestamp - m_poseTimestamp;
m_poseTimestamp = (float)pose.timestamp;
}
m_poseStatus = pose.status_code;
if (m_clutchActive)
{
// When clutching, preserve position.
m_tangoPosition = m_prevTangoPosition;
// When clutching, preserve yaw, keep changes in pitch, roll.
Vector3 rotationAngles = m_tangoRotation.eulerAngles;
rotationAngles.y = m_prevTangoRotation.eulerAngles.y;
m_tangoRotation.eulerAngles = rotationAngles;
}
// Calculate final position and rotation deltas and apply them.
Vector3 deltaPosition = m_tangoPosition - m_prevTangoPosition;
Quaternion deltaRotation = m_tangoRotation * Quaternion.Inverse(m_prevTangoRotation);
if (m_characterMotion && m_characterController != null)
{
m_characterController.Move(deltaPosition);
transform.rotation = deltaRotation * transform.rotation;
}
else
{
transform.position = transform.position + deltaPosition;
transform.rotation = deltaRotation * transform.rotation;
}
}
/// <summary>
/// Estimates the depth of a point on a screen, based on nearest neighbors.
/// </summary>
/// <returns>
/// <c>true</c> if a successful depth estimate was obtained.
/// </returns>
/// <param name="cam">The Unity camera.</param>
/// <param name="pos">The point in pixel coordinates to perform depth estimation.</param>
/// <param name="colorCameraPoint">
/// The point (x, y, z), where (x, y) is the back-projection of the UV
/// coordinates to the color camera space and z is the z coordinate of
/// the point in the point cloud nearest to the user selection after
/// projection onto the image plane. If there is not a point cloud point
/// close to the user selection after projection onto the image plane,
/// then the point will be set to (0.0, 0.0, 0.0) and isValidPoint will
/// be set to false.
/// </param>
public bool EstimateDepthOnScreen(Camera cam, Vector2 pos, out Vector3 colorCameraPoint)
{
// Set up parameters
Matrix4x4 colorCameraTUnityWorld = m_colorCameraTUnityCamera * cam.transform.worldToLocalMatrix;
Vector2 normalizedPos = cam.ScreenToViewportPoint(pos);
// If the camera has a TangoARScreen attached, it is not displaying the entire color camera image. Correct
// the normalized coordinates by taking the clipping into account.
TangoARScreen arScreen = cam.gameObject.GetComponent <TangoARScreen>();
if (arScreen != null)
{
normalizedPos = arScreen.ViewportPointToCameraImagePoint(normalizedPos);
}
bool isValidPoint;
int returnType = TangoSupport.ScreenCoordinateToWorldNearestNeighbor(
m_points,
m_pointsCount,
m_depthTimestamp,
m_colorCameraIntrinsics,
ref colorCameraTUnityWorld,
normalizedPos,
out colorCameraPoint,
out isValidPoint);
if (returnType != Common.ErrorType.TANGO_SUCCESS)
{
Debug.LogErrorFormat("TangoSupport.ScreenCoordinateToWorldNearestNeighbor failed with error code {0}.",
returnType);
}
return((returnType == Common.ErrorType.TANGO_SUCCESS) && isValidPoint);
}
/// <summary>
/// Given a screen coordinate, find a plane that most closely fits depth values in that area.
///
/// This assumes you are using this in an AR context.
/// </summary>
/// <returns><c>true</c>, if plane was found, <c>false</c> otherwise.</returns>
/// <param name="cam">The Unity camera.</param>
/// <param name="pos">The point in screen space to perform detection on.</param>
/// <param name="planeCenter">Filled in with the center of the plane in Unity world space.</param>
/// <param name="plane">Filled in with a model of the plane in Unity world space.</param>
public bool FindPlane(Camera cam, Vector2 pos, out Vector3 planeCenter, out Plane plane)
{
Matrix4x4 colorCameraTUnityWorld = m_colorCameraTUnityCamera * cam.transform.worldToLocalMatrix;
Vector2 normalizedPos = cam.ScreenToViewportPoint(pos);
// If the camera has a TangoARScreen attached, it is not displaying the entire color camera image. Correct
// the normalized coordinates by taking the clipping into account.
TangoARScreen arScreen = cam.gameObject.GetComponent <TangoARScreen>();
if (arScreen != null)
{
normalizedPos = arScreen.ViewportPointToCameraImagePoint(normalizedPos);
}
int returnValue = TangoSupport.FitPlaneModelNearClick(
m_points, m_pointsCount, m_depthTimestamp, m_colorCameraIntrinsics, ref colorCameraTUnityWorld, normalizedPos,
out planeCenter, out plane);
if (returnValue == Common.ErrorType.TANGO_SUCCESS)
{
return(true);
}
else
{
return(false);
}
}
private void StartTango()
{
tango = new Tango(this, new Runnable(() =>
{
Log.Debug(Tag, "TangoRunnable");
try
{
TangoSupport.Initialize();
var tangoConfig = GetTangoConfig(tango);
tango.Connect(tangoConfig);
TangoAddListeners();
tango.ConnectTextureId(TangoCameraIntrinsics.TangoCameraColor, -1);
}
catch (TangoOutOfDateException e)
{
Log.Error(Tag, "TangoOutOfDateException", e);
}
catch (TangoErrorException e)
{
Log.Error(Tag, "TangoErrorException", e);
}
catch (TangoInvalidException e)
{
Log.Error(Tag, "TangoInvalidException", e);
}
}));
}
/// <summary>
/// Fix this! Doesn't output correct values. Input matrix probably not correct.
/// Generates the depth map using nearest neighbor upsampling.
/// </summary>
private void GenerateDepthMap_NearestNeighbor(ref TangoUnityDepth tangoUnityDepth)
{
TangoPoseData poseData = new TangoPoseData();
TangoCoordinateFramePair pair;
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
PoseProvider.GetPoseAtTime(poseData, tangoUnityDepth.m_timestamp, pair);
if (poseData.status_code != TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
return;
}
Vector3 position;
Quaternion rotation;
TangoSupport.TangoPoseToWorldTransform(poseData, out position, out rotation);
Matrix4x4 ccWorld = Matrix4x4.TRS(position, rotation, Vector3.one);
bool isValid = false;
Vector3 colorCameraPoint = new Vector3();
for (int i = 0; i < _depthMapWidth; i++)
{
for (int j = 0; j < _depthMapHeight; j++)
{
if (TangoSupport.ScreenCoordinateToWorldNearestNeighbor(
_PointCloud.m_points, _PointCloud.m_pointsCount,
tangoUnityDepth.m_timestamp,
_ccIntrinsics,
ref ccWorld,
new Vector2(i / (float)_depthMapWidth, j / (float)_depthMapHeight),
out colorCameraPoint, out isValid) == Common.ErrorType.TANGO_INVALID)
{
_depthTexture.SetPixel(i, j, Color.red);
continue;
}
if (isValid)
{
float c = 1 - colorCameraPoint.z / 4.5f;
_depthTexture.SetPixel(i, j, new Color(c, c, c));
}
else
{
_depthTexture.SetPixel(i, j, Color.black);
}
}
}
_depthTexture.Apply();
_DepthMapQuad.sharedMaterial.mainTexture = _depthTexture;
//_debugMessage = "DepthAvailable: " + _waitingForDepth.ToString() + "\n" +
// " points: " + _PointCloud.m_pointsCount + "\n" +
// " timestamp: " + tangoUnityDepth.m_timestamp.ToString("0.00") + "\n" +
// " XYZ:" + colorCameraPoint.ToString();
}
public void OnTangoImageAvailableEventHandler(Tango.TangoEnums.TangoCameraId cameraId,
Tango.TangoUnityImageData imageBuffer)
{
TangoSupport.DetectMarkers(imageBuffer, cameraId,
TangoSupport.MarkerType.QRCODE, MARKER_SIZE, markerList);
if (markerList.Count > 0)
{
TangoSupport.Marker marker = markerList[0];
qrcodePlane.transform.position = marker.m_translation;
qrcodePlane.transform.rotation = marker.m_orientation;
var bottomToTop = marker.m_corner3DP3 - marker.m_corner3DP0;
var leftToRight = marker.m_corner3DP1 - marker.m_corner3DP0;
plane.transform.localScale = new Vector3(leftToRight.magnitude, 1, bottomToTop.magnitude) * 0.1f;
}
}
/// <summary>
/// Given a screen coordinate, finds a plane that most closely fits the
/// depth values in that area.
///
/// This function is slow, as it looks at every single point in the point
/// cloud. Avoid calling this more than once a frame. This also assumes the
/// Unity camera intrinsics match the device's color camera.
/// </summary>
/// <returns><c>true</c>, if a plane was found; <c>false</c> otherwise.</returns>
/// <param name="cam">The Unity camera.</param>
/// <param name="pos">The point in screen space to perform detection on.</param>
/// <param name="planeCenter">Filled in with the center of the plane in Unity world space.</param>
/// <param name="plane">Filled in with a model of the plane in Unity world space.</param>
public bool FindPlane(Camera cam, Vector2 pos, out Vector3 planeCenter, out Plane plane)
{
if (m_pointsCount == 0)
{
// No points to check, maybe not connected to the service yet
planeCenter = Vector3.zero;
plane = new Plane();
return(false);
}
Vector2 normalizedPos = cam.ScreenToViewportPoint(pos);
// If the camera has a TangoARScreen attached, it is not displaying the entire color camera image. Correct
// the normalized coordinates by taking the clipping into account.
TangoARScreen arScreen = cam.gameObject.GetComponent <TangoARScreen>();
if (arScreen != null)
{
normalizedPos = arScreen.ViewportPointToCameraImagePoint(normalizedPos);
}
DVector4 planeModel = new DVector4();
bool returnValue = TangoSupport.FitPlaneModelNearClick(
m_mostRecentPointCloud,
arScreen.m_screenUpdateTime,
normalizedPos,
out planeCenter,
out planeModel);
planeCenter = m_mostRecentUnityWorldTDepthCamera.MultiplyPoint3x4(planeCenter);
Vector3 normal = new Vector3((float)planeModel.x,
(float)planeModel.y,
(float)planeModel.z);
normal = m_mostRecentUnityWorldTDepthCamera.MultiplyVector(normal);
Vector3.Normalize(normal);
float distance = (float)planeModel.w / normal.magnitude;
plane = new Plane(normal, distance);
return(returnValue);
}
/// <summary>
/// Given a screen coordinate, find a plane that most closely fits depth values in that area.
///
/// This assumes you are using this in an AR context.
/// </summary>
/// <returns><c>true</c>, if plane was found, <c>false</c> otherwise.</returns>
/// <param name="cam">The Unity camera.</param>
/// <param name="pos">The point in screen space to perform detection on.</param>
/// <param name="planeCenter">Filled in with the center of the plane in Unity world space.</param>
/// <param name="plane">Filled in with a model of the plane in Unity world space.</param>
public bool FindPlane(Camera cam, Vector2 pos, out Vector3 planeCenter, out Plane plane)
{
Matrix4x4 unityWorldTColorCamera = m_unityWorldTStartService * m_startServiceTDevice * Matrix4x4.Inverse(m_imuTDevice) * m_imuTColorCamera;
Matrix4x4 colorCameraTUnityWorld = unityWorldTColorCamera.inverse;
Vector2 normalizedPos = cam.ScreenToViewportPoint(pos);
int returnValue = TangoSupport.FitPlaneModelNearClick(
m_points, m_pointsCount, m_pointsTimestamp, m_colorCameraIntrinsics, ref colorCameraTUnityWorld, normalizedPos,
out planeCenter, out plane);
if (returnValue == Common.ErrorType.TANGO_SUCCESS)
{
return(true);
}
else
{
return(false);
}
}
/// <summary>
/// Detect one or more markers in the input image.
/// </summary>
/// <param name="cameraId">
/// Returned camera ID.
/// </param>
/// <param name="imageBuffer">
/// Color camera image buffer.
/// </param>
public void OnTangoImageAvailableEventHandler(TangoEnums.TangoCameraId cameraId,
TangoUnityImageData imageBuffer)
{
TangoSupport.DetectMarkers(imageBuffer, cameraId,
TangoSupport.MarkerType.ARTAG, MARKER_SIZE, m_markerList);
for (int i = 0; i < m_markerList.Count; ++i)
{
TangoSupport.Marker marker = m_markerList[i];
int markerId = Convert.ToInt32(marker.m_content);
int planetIndex = (markerId - 1) % m_planets.Length;
if (m_planets[planetIndex] == null)
{
m_planets[planetIndex] = Instantiate <GameObject>(m_planetsPrefabs[planetIndex]);
}
m_planets[planetIndex].transform.position = marker.m_translation;
m_planets[planetIndex].transform.rotation = marker.m_orientation;
}
}
/// <summary>
/// Given a screen coordinate, finds a plane that most closely fits the
/// depth values in that area.
///
/// This function is slow, as it looks at every single point in the point
/// cloud. Avoid calling this more than once a frame. This also assumes the
/// Unity camera intrinsics match the device's color camera.
/// </summary>
/// <returns><c>true</c>, if a plane was found; <c>false</c> otherwise.</returns>
/// <param name="cam">The Unity camera.</param>
/// <param name="pos">The point in screen space to perform detection on.</param>
/// <param name="planeCenter">Filled in with the center of the plane in Unity world space.</param>
/// <param name="plane">Filled in with a model of the plane in Unity world space.</param>
public bool FindPlane(Camera cam, Vector2 pos, out Vector3 planeCenter, out Plane plane)
{
if (m_pointsCount == 0)
{
// No points to check, maybe not connected to the service yet
planeCenter = Vector3.zero;
plane = new Plane();
return(false);
}
Matrix4x4 colorCameraTUnityWorld = m_colorCameraTUnityCamera * cam.transform.worldToLocalMatrix;
Vector2 normalizedPos = cam.ScreenToViewportPoint(pos);
// If the camera has a TangoARScreen attached, it is not displaying the entire color camera image. Correct
// the normalized coordinates by taking the clipping into account.
TangoARScreen arScreen = cam.gameObject.GetComponent <TangoARScreen>();
if (arScreen != null)
{
normalizedPos = arScreen.ViewportPointToCameraImagePoint(normalizedPos);
}
TangoCameraIntrinsics alignedIntrinsics = new TangoCameraIntrinsics();
VideoOverlayProvider.GetDeviceOrientationAlignedIntrinsics(TangoEnums.TangoCameraId.TANGO_CAMERA_COLOR,
alignedIntrinsics);
int returnValue = TangoSupport.FitPlaneModelNearClick(
m_points, m_pointsCount, m_depthTimestamp, alignedIntrinsics, ref colorCameraTUnityWorld,
normalizedPos, out planeCenter, out plane);
if (returnValue == Common.ErrorType.TANGO_SUCCESS)
{
return(true);
}
else
{
return(false);
}
}
protected override void OnResume()
{
Log.Debug(Tag, "OnResume");
base.OnResume();
_tango = new Tango(this, new Runnable(() =>
{
Log.Debug(Tag, "TangoRunnable");
//try
//{
TangoSupport.Initialize();
_tangoConfig = SetupTangoConfig(_tango);
_tango.Connect(_tangoConfig);
startupTango();
//}
//catch (TangoOutOfDateException e)
//{
// Log.Error(Tag, GetString(R.
// string.exception_out_of_date),
// e)
// ;
//}
//catch (TangoErrorException e)
//{
// Log.Error(Tag, GetString(R.
// string.exception_tango_error),
// e)
// ;
//}
//catch (TangoInvalidException e)
//{
// Log.Error(Tag, GetString(R.
// string.exception_tango_invalid),
// e)
// ;
//}
}));
}
/// <summary>
/// Estimates the depth of a point on a screen, based on nearest neighbors.
/// </summary>
/// <returns>
/// <c>true</c> if a successful depth estimate was obtained.
/// </returns>
/// <param name="cam">The Unity camera.</param>
/// <param name="pos">The point in pixel coordinates to perform depth estimation.</param>
/// <param name="colorCameraPoint">
/// The point (x, y, z), where (x, y) is the back-projection of the UV
/// coordinates to the color camera space and z is the z coordinate of
/// the point in the point cloud nearest to the user selection after
/// projection onto the image plane. If there is not a point cloud point
/// close to the user selection after projection onto the image plane,
/// then the point will be set to (0.0, 0.0, 0.0) and isValidPoint will
/// be set to false.
/// </param>
public bool EstimateDepthOnScreen(Camera cam, Vector2 pos, out Vector3 colorCameraPoint)
{
// Set up parameters
Matrix4x4 colorCameraTUnityWorld = TangoSupport.COLOR_CAMERA_T_UNITY_CAMERA * cam.transform.worldToLocalMatrix;
Vector2 normalizedPos = cam.ScreenToViewportPoint(pos);
// If the camera has a TangoARScreen attached, it is not displaying the entire color camera image. Correct
// the normalized coordinates by taking the clipping into account.
TangoARScreen arScreen = cam.gameObject.GetComponent <TangoARScreen>();
if (arScreen != null)
{
normalizedPos = arScreen.ViewportPointToCameraImagePoint(normalizedPos);
}
bool returnValue = TangoSupport.ScreenCoordinateToWorldNearestNeighbor(
m_mostRecentPointCloud,
arScreen.m_screenUpdateTime,
normalizedPos,
out colorCameraPoint);
return(returnValue);
}
/// <summary>
/// Detect one or more markers in the input image.
/// </summary>
/// <param name="cameraId">
/// Returned camera ID.
/// </param>
/// <param name="imageBuffer">
/// Color camera image buffer.
/// </param>
public void OnTangoImageAvailableEventHandler(TangoEnums.TangoCameraId cameraId,
TangoUnityImageData imageBuffer)
{
TangoSupport.DetectMarkers(imageBuffer, cameraId,
TangoSupport.MarkerType.ARTAG, MARKER_SIZE, m_markerList);
for (int i = 0; i < m_markerList.Count; ++i)
{
TangoSupport.Marker marker = m_markerList[i];
if (m_markerObjects.ContainsKey(marker.m_content))
{
GameObject markerObject = m_markerObjects[marker.m_content];
markerObject.GetComponent <MarkerVisualizationObject>().SetMarker(marker);
}
else
{
GameObject markerObject = Instantiate <GameObject>(m_markerPrefab);
m_markerObjects.Add(marker.m_content, markerObject);
markerObject.GetComponent <MarkerVisualizationObject_Custom>().SetMarker(marker);
}
}
}
protected override void OnResume()
{
base.OnResume();
_tango = new Tango(this, new Runnable(() =>
{
Log.Debug(Tag, "TangoRunnable");
try
{
TangoSupport.Initialize();
_tangoConfig = SetupTangoConfig(_tango);
_tango.Connect(_tangoConfig);
_isConnected = true;
var adfs = _tango.ListAreaDescriptions();
// get the number of adfs
var nr = adfs.Count;
var textView = FindViewById <TextView>(Resource.Id.textView2);
RunOnUiThread(() => textView.Text = nr.ToString());
}
catch (TangoOutOfDateException e)
{
Log.Error(Tag, "TangoOutOfDateException", e);
}
catch (TangoErrorException e)
{
// this exception gets thrown
Log.Error(Tag, "TangoErrorException", e);
}
catch (TangoInvalidException e)
{
Log.Error(Tag, "TangoInvalidException", e);
}
}));
}
/// <summary>
/// Update AR screen rendering and attached Camera's projection matrix.
/// </summary>
/// <param name="displayRotation">Activity (screen) rotation.</param>
/// <param name="colorCameraRotation">Color camera sensor rotation.</param>
private void _SetRenderAndCamera(OrientationManager.Rotation displayRotation,
OrientationManager.Rotation colorCameraRotation)
{
float cameraWidth = (float)Screen.width;
float cameraHeight = (float)Screen.height;
#pragma warning disable 0219
// Here we are computing if current display orientation is landscape or portrait.
// AndroidHelper.GetAndroidDefaultOrientation() returns 1 if device default orientation is in portrait,
// returns 2 if device default orientation is landscape. Adding device default orientation with
// how much the display is rotated from default orientation will get us the result of current display
// orientation. (landscape vs. portrait)
bool isLandscape = (AndroidHelper.GetDefaultOrientation() + (int)displayRotation) % 2 == 0;
bool needToFlipCameraRatio = false;
float cameraRatio = (float)Screen.width / (float)Screen.height;
#pragma warning restore 0219
#if !UNITY_EDITOR
// In most cases, we don't need to flip the camera width and height. However, in some cases Unity camera
// only updates a couple of frames after the display changed callback from Android; thus, we need to flip the width
// and height in this case.
//
// This does not happen in the editor, because the emulated device does not ever rotate.
needToFlipCameraRatio = (!isLandscape & (cameraRatio > 1.0f)) || (isLandscape & (cameraRatio < 1.0f));
if (needToFlipCameraRatio)
{
cameraRatio = 1.0f / cameraRatio;
float tmp = cameraWidth;
cameraWidth = cameraHeight;
cameraHeight = tmp;
}
#endif
TangoCameraIntrinsics alignedIntrinsics = new TangoCameraIntrinsics();
TangoCameraIntrinsics intrinsics = new TangoCameraIntrinsics();
VideoOverlayProvider.GetDeviceOrientationAlignedIntrinsics(TangoEnums.TangoCameraId.TANGO_CAMERA_COLOR,
alignedIntrinsics);
VideoOverlayProvider.GetIntrinsics(TangoEnums.TangoCameraId.TANGO_CAMERA_COLOR,
intrinsics);
if (alignedIntrinsics.width != 0 && alignedIntrinsics.height != 0)
{
// The camera to which this script is attached is an Augmented Reality camera. The color camera
// image must fill that camera's viewport. That means we must clip the color camera image to make
// its ratio the same as the Unity camera. If we don't do this the color camera image will be
// stretched non-uniformly, making a circle into an ellipse.
float widthRatio = (float)cameraWidth / (float)alignedIntrinsics.width;
float heightRatio = (float)cameraHeight / (float)alignedIntrinsics.height;
if (widthRatio >= heightRatio)
{
m_uOffset = 0;
m_vOffset = (1 - (heightRatio / widthRatio)) / 2;
}
else
{
m_uOffset = (1 - (widthRatio / heightRatio)) / 2;
m_vOffset = 0;
}
// Note that here we are passing in non-inverted intrinsics, because the YUV conversion is still operating
// on native buffer layout.
OrientationManager.Rotation rotation = TangoSupport.RotateFromAToB(displayRotation, colorCameraRotation);
_MaterialUpdateForIntrinsics(m_uOffset, m_vOffset, rotation);
_CameraUpdateForIntrinsics(m_camera, alignedIntrinsics, m_uOffset, m_vOffset);
if (m_arCameraPostProcess != null)
{
m_arCameraPostProcess.SetupIntrinsic(intrinsics);
}
}
else
{
Debug.LogError("AR Camera intrinsic is not valid.");
}
}
/// <summary>
/// Minji Kim 2017.09.25
/// Find Edge Wrapper
/// call FindEdgesNearPoint
/// TangoSupport_findEdgesNearPoint
/// </summary>
/// <param name="cam"></param>
/// <param name="pos"></param>
/// <returns></returns>
public bool FindEdges(TangoUnityImageData imageBuffer,
Camera cam, Vector2 pos, out TangoSupport.TangoSupportEdge[] edges, out int num_edges)
//out Vector3[] end_points, out Vector3 nearest_on_edge)
{
if (m_pointsCount == 0)
{
// No points to check, maybe not connected to the service yet
edges = new TangoSupport.TangoSupportEdge[1];
num_edges = 0;
return(false);
}
Vector2 normalizedPos = cam.ScreenToViewportPoint(pos);
// If the camera has a TangoARScreen attached, it is not displaying the entire color camera image. Correct
// the normalized coordinates by taking the clipping into account.
TangoARScreen arScreen = cam.gameObject.GetComponent <TangoARScreen>();
if (arScreen != null)
{
normalizedPos = arScreen.ViewportPointToCameraImagePoint(normalizedPos);
}
// if the image data has not been updated, update it manually
if (imageBuffer.data == null)
{
GetTangoUnityImageData(ref imageBuffer);
}
bool returnValue = TangoSupport.FindEdgesNearPoint(
imageBuffer,
m_mostRecentPointCloud,
arScreen.m_screenUpdateTime,
normalizedPos,
out edges,
out num_edges
);
Vector3[] start = new Vector3[num_edges];
Vector3[] end = new Vector3[num_edges];
Vector3[] near = new Vector3[num_edges];
TangoSupport.TangoSupportEdge[] n_edges = new TangoSupport.TangoSupportEdge[num_edges];
for (int j = 0; j < num_edges; j++)
{
start[j] = new Vector3(edges[j].end_points_x1, edges[j].end_points_y1, edges[j].end_points_z1);
end[j] = new Vector3(edges[j].end_points_x2, edges[j].end_points_y2, edges[j].end_points_z2);
near[j] = new Vector3(edges[j].closest_point_on_edge_x, edges[j].closest_point_on_edge_y,
edges[j].closest_point_on_edge_z);
start[j] = m_mostRecentUnityWorldTDepthCamera.MultiplyVector(start[j]);
end[j] = m_mostRecentUnityWorldTDepthCamera.MultiplyVector(end[j]);
near[j] = m_mostRecentUnityWorldTDepthCamera.MultiplyVector(near[j]);
Vector3.Normalize(start[j]);
Vector3.Normalize(end[j]);
Vector3.Normalize(near[j]);
n_edges[j].end_points_x1 = start[j][0];
n_edges[j].end_points_y1 = start[j][1];
n_edges[j].end_points_z1 = start[j][2];
n_edges[j].end_points_x2 = end[j][0];
n_edges[j].end_points_y2 = end[j][1];
n_edges[j].end_points_z2 = end[j][2];
n_edges[j].closest_point_on_edge_x = near[j][0];
n_edges[j].closest_point_on_edge_y = near[j][1];
n_edges[j].closest_point_on_edge_z = near[j][2];
}
edges = n_edges;
return(returnValue);
}
