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;
}
/// <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;
}
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 };
}
