Ejemplo n.º 1
0
		public override List<Gis.SelectedItem> Select(DVector3 nearPoint, DVector3 farPoint)
		{
			if (selectInfo == null) return null;

			var selectedItems = new List<Gis.SelectedItem>();

			for (int i = 0; i < model.Nodes.Count; i++)
			{
				var meshIndex = model.Nodes[i].MeshIndex;

				if (meshIndex < 0) {
					continue;
				}

				var mesh = model.Meshes[meshIndex];
				var info = selectInfo[i];

				var translation			= transforms[i] * Matrix.Scaling(0.001f);
				var modelWorld			= translation * modelRotationMatrix * Matrix.Translation(CartesianPos.ToVector3());
				var modelWorldinvert	= Matrix.Invert(modelWorld);

				var localNear	= Vector3.TransformCoordinate(nearPoint.ToVector3(),	modelWorldinvert);
				var localFar	= Vector3.TransformCoordinate(farPoint.ToVector3(),		modelWorldinvert);

				var localRay = new Ray(localNear, Vector3.Normalize(localFar - localNear));

				float distance;
				if (info.BoundingBox.Intersects(ref localRay, out distance)) {
					selectedItems.Add(new SelectedItem {
						BoundingBox				= info.BoundingBox,
						BoundingBoxTransform	= DMatrix.FromFloatMatrix(modelWorld),
						Distance				= distance,
						InstanceIndex			= 0,
						Name					= info.NodeName,
						NodeInd					= info.NodeIndex
					});
				}
				
			}

			return selectedItems;
		}
Ejemplo n.º 2
0
        /// <summary>
        /// Fits a plane to a point cloud near a user-specified location. This
        /// occurs in two passes. First, all points in cloud within
        /// <c>maxPixelDistance</c> to <c>uvCoordinates</c> after projection are kept. Then a
        /// plane is fit to the subset cloud using RANSAC. After the initial fit
        /// all inliers from the original cloud are used to refine the plane
        /// model.
        /// </summary>
        /// <returns>
        /// Common.ErrorType.TANGO_SUCCESS on success,
        /// Common.ErrorType.TANGO_INVALID on invalid input, and
        /// Common.ErrorType.TANGO_ERROR on failure.
        /// </returns>
        /// <param name="pointCloud">
        /// The point cloud. Cannot be null and must have at least three points.
        /// </param>
        /// <param name="pointCount">
        /// The number of points to read from the point cloud.
        /// </param>
        /// <param name="timestamp">The timestamp of the point cloud.</param>
        /// <param name="cameraIntrinsics">
        /// The camera intrinsics for the color camera. Cannot be null.
        /// </param>
        /// <param name="matrix">
        /// Transformation matrix of the color camera with respect to the Unity
        /// World frame.
        /// </param>
        /// <param name="uvCoordinates">
        /// The UV coordinates for the user selection. This is expected to be
        /// between (0.0, 0.0) and (1.0, 1.0).
        /// </param>
        /// <param name="intersectionPoint">
        /// The output point in depth camera coordinates that the user selected.
        /// </param>
        /// <param name="plane">The plane fit.</param>
        public static int FitPlaneModelNearClick(
            Vector3[] pointCloud, int pointCount, double timestamp,
            TangoCameraIntrinsics cameraIntrinsics, ref Matrix4x4 matrix,
            Vector2 uvCoordinates, out Vector3 intersectionPoint,
            out Plane plane)
        {
            GCHandle pointCloudHandle = GCHandle.Alloc(pointCloud,
                                                       GCHandleType.Pinned);

            TangoXYZij pointCloudXyzIj = new TangoXYZij();
            pointCloudXyzIj.timestamp = timestamp;
            pointCloudXyzIj.xyz_count = pointCount;
            pointCloudXyzIj.xyz = pointCloudHandle.AddrOfPinnedObject();

            DMatrix4x4 doubleMatrix = new DMatrix4x4(matrix);

            // Unity has Y pointing screen up; Tango camera has Y pointing
            // screen down.
            Vector2 uvCoordinatesTango = new Vector2(uvCoordinates.x,
                                                     1.0f - uvCoordinates.y);

            DVector3 doubleIntersectionPoint = new DVector3();
            double[] planeArray = new double[4];

            int returnValue = TangoSupportAPI.TangoSupport_fitPlaneModelNearPointMatrixTransform(
                pointCloudXyzIj, cameraIntrinsics, ref doubleMatrix,
                ref uvCoordinatesTango,
                out doubleIntersectionPoint, planeArray);

            if (returnValue != Common.ErrorType.TANGO_SUCCESS)
            {
                intersectionPoint = new Vector3(0.0f, 0.0f, 0.0f);
                plane = new Plane(new Vector3(0.0f, 0.0f, 0.0f), 0.0f);
            }
            else
            {
                intersectionPoint = doubleIntersectionPoint.ToVector3();
                Vector3 normal = new Vector3((float)planeArray[0],
                                             (float)planeArray[1],
                                             (float)planeArray[2]);
                float distance = (float)planeArray[3] / normal.magnitude;

                plane = new Plane(normal, distance);
            }

            pointCloudHandle.Free();

            return returnValue;
        }
Ejemplo n.º 3
0
		public static PolyGisLayer CreateFromUtmFbxModel(Game engine, string fileName)
		{
			var scene = engine.Content.Load<Scene>(fileName);

			var s = fileName.Split('_');
			double easting	= double.Parse(s[1]);
			double northing = double.Parse(s[2]);
			string region	= s[3];
			
			var transforms = new Matrix[scene.Nodes.Count];
			scene.ComputeAbsoluteTransforms(transforms);

			List<Gis.GeoPoint>	points = new List<Gis.GeoPoint>();
			List<int>			indeces = new List<int>();

			var oInfo = new SelectInfo[scene.Meshes.Count];

			for (int i = 0; i < scene.Nodes.Count; i++) {

				var meshIndex = scene.Nodes[i].MeshIndex;

				if (meshIndex < 0) {
					continue;
				}


				oInfo[meshIndex] = new SelectInfo {
					MeshIndex	= meshIndex,
					NodeIndex	= i,
					NodeName	= scene.Nodes[i].Name
				};


				int vertexOffset = points.Count;

				var world = transforms[i];

				double worldLon, worldLat;
				Gis.UtmToLatLon(easting + world.TranslationVector.X, northing - world.TranslationVector.Z, region, out worldLon, out worldLat);

				var worldBasis			= GeoHelper.CalculateBasisOnSurface(DMathUtil.DegreesToRadians(new DVector2(worldLon, worldLat)));
				var worldBasisInvert	= DMatrix.Invert(worldBasis);

				oInfo[meshIndex].WorldMatrix		= worldBasis;
				oInfo[meshIndex].WorldMatrixInvert	= worldBasisInvert;

				List<Vector3> cartPoints = new List<Vector3>();
				
				foreach (var vert in scene.Meshes[meshIndex].Vertices) {
					var pos = vert.Position;

					var worldPos	= Vector3.TransformCoordinate(pos, world);
					var worldNorm	= Vector3.TransformNormal(vert.Normal, world);


					double lon, lat;
					Gis.UtmToLatLon(easting + worldPos.X, northing - worldPos.Z, region, out lon, out lat);

					DVector3 norm = new DVector3(worldNorm.X, worldNorm.Z, worldNorm.Y);
					norm.Normalize();

					norm = DVector3.TransformNormal(norm, DMatrix.RotationYawPitchRoll(DMathUtil.DegreesToRadians(lon), DMathUtil.DegreesToRadians(lat), 0));
					norm.Normalize();

					norm.Y = -norm.Y;

					lon = DMathUtil.DegreesToRadians(lon) + 0.0000068;
					lat = DMathUtil.DegreesToRadians(lat) + 0.0000113;

					cartPoints.Add(DVector3.TransformCoordinate(GeoHelper.SphericalToCartesian(new DVector2(lon, lat), GeoHelper.EarthRadius + worldPos.Y / 1000.0f), worldBasisInvert).ToVector3());

					var point = new Gis.GeoPoint {
						Lon		= lon,
						Lat		= lat,
						Color	= vert.Color0,
						Tex0	= new Vector4(norm.ToVector3(), 0),
						Tex1	= new Vector4(0,0,0, worldPos.Y/1000.0f)
					};
					point.Color.Alpha = 0.5f;
					points.Add(point);
				}

				oInfo[meshIndex].BoundingBox = BoundingBox.FromPoints(cartPoints.ToArray());

				var inds = scene.Meshes[meshIndex].GetIndices();

				foreach (var ind in inds) {
					indeces.Add(vertexOffset + ind);
				}

			}

			return new PolyGisLayer(engine, points.ToArray(), indeces.ToArray(), false) { objectsInfo = oInfo };
		}