public Matrix3 GetLocalOrientation(LatPos pos)
{
Coordinate converter = new Coordinate();
converter.SetLatPos(pos);
switch (_mapType)
{
default:
{
return(new Matrix3());
}
case MapType.UTM:
return(new Matrix3(new Vec3(1, 0, 0), new Vec3(0, 0, -1), new Vec3(0, 1, 0))); // East North Up vectors
case MapType.GEOCENTRIC:
CartPos cartpos;
if (!converter.GetCartPos(out cartpos))
{
Message.Send("Controller", MessageLevel.WARNING, "Failed to convert to Geocentric");
return(new Matrix3());
}
return(converter.GetOrientationMatrix(cartpos));
}
}
/// <summary>
/// Returns a local orientation matrix for a specific LatPos position with east,north and up base vectors
/// </summary>
/// <param name="latpos">Geodetic position</param>
/// <param name="ellipsoid"></param>
/// <returns>Matrix of [east][north][up] vectors</returns>
public Matrix3 GetOrientationMatrix(LatPos latpos, Ellipsoid ellipsoid = Ellipsoid.WGS84)
{
Matrix3 mat = new Matrix3();
Coordinate_getOrientationMatrix_LatPos(GetNativeReference(), ref latpos, ellipsoid, ref mat);
return(mat);
}
public bool GetPosition(LatPos pos, out MapPos result, GroundClampType groundClamp = GroundClampType.NONE, ClampFlags flags = ClampFlags.DEFAULT)
{
result = new MapPos
{
local_orientation = new Matrix3(new Vec3(1, 0, 0), new Vec3(0, 0, -1), new Vec3(0, 1, 0)) // East North Up vectors
};
return(SetPosition(result, pos, groundClamp, flags));
}
public bool LocalToWorld(MapPos pos, out LatPos result)
{
Vec3D position = pos.position;
RoiNode roi = pos.node as RoiNode;
if (roi != null && roi.IsValid()) // Convert to a global position
{
position += roi.Position;
}
return(GlobalToWorld(position, out result));
}
public bool GlobalToWorld(Vec3D global_position, out LatPos result)
{
// TODO:Coordinate.GetGlobalCoordinate(global_position+_origin, _coordSystem, _metaData, out LatPos lp);
Coordinate converter = new Coordinate();
if (!GlobalToWorld(converter, global_position))
{
result = null;
return(false);
}
return(converter.GetLatPos(out result));
}
public bool SetPosition(MapPos result, LatPos pos, GroundClampType groundClamp = GroundClampType.NONE, ClampFlags flags = ClampFlags.DEFAULT)
{
Coordinate converter = new Coordinate();
converter.SetLatPos(pos);
if (!WorldToMap(converter, ref result.position, ref result.local_orientation))
{
return(false);
}
MapToRoi(result);
return(UpdatePosition(result, groundClamp, flags));
}
public double GetAltitude(LatPos pos, ClampFlags flags = ClampFlags.DEFAULT)
{
MapPos mapPos;
if (!GetPosition(pos, out mapPos, GroundClampType.GROUND, flags))
{
return(0);
}
LatPos updatedPos;
if (!GetPosition(mapPos, out updatedPos))
{
return(0);
}
return(updatedPos.altitude);
}
public bool GetLatPos(MapPos pos, out LatPos result)
{
Coordinate converter = new Coordinate();
Vec3D position = pos.position;
result = new LatPos();
// Check possibly local 3D under a roiNode
RoiNode roi = pos.node as RoiNode;
if (roi != null && roi.IsValid())
{
position += roi.Position;
}
switch (_mapType)
{
case MapType.UNKNOWN:
{
return(false);
}
case MapType.UTM:
UTMPos utmpos = new UTMPos(_utmZone, _north, -(position.z + _origin.z), position.x + _origin.x, position.y + _origin.y);
converter.SetUTMPos(utmpos);
break;
case MapType.GEOCENTRIC:
CartPos cartpos = new CartPos(position.x + _origin.x, position.y + _origin.y, position.z + _origin.z);
converter.SetCartPos(cartpos);
break;
}
return(converter.GetLatPos(out result));
}
public bool GetLatPos(MapPos pos, out LatPos latpos)
{
return(_controller.GetPosition(pos, out latpos));
}
public double GetAltitude(LatPos pos, ClampFlags flags = ClampFlags.DEFAULT)
{
return(_controller.GetAltitude(pos, flags));
}
private static extern IntPtr LatPos_create_dynamic(ref LatPos pos);
private static extern void Coordinate_getOrientationMatrix_LatPos(IntPtr nativeReference, ref LatPos pos, Ellipsoid ellipsoid, ref Matrix3 matrix);
private static extern bool Coordinate_getLatPos(IntPtr nativeReference, ref LatPos pos, Datum datum);
private static extern void Coordinate_setLatPos(IntPtr nativeReference, ref LatPos pos, Datum datum);
private static extern bool LatPos_create_pos(IntPtr native_reference, [Out] out LatPos pos);
public bool GetMapPosition(LatPos latpos, out MapPos pos, GroundClampType groundClamp, ClampFlags flags = ClampFlags.DEFAULT)
{
return(_controller.GetPosition(latpos, out pos, groundClamp, flags));
}
public void SetLatPos(LatPos pos, Datum datum = Datum.WGS84)
{
Coordinate_setLatPos(GetNativeReference(), ref pos, datum);
}
public bool GetLatPos(out LatPos pos, Datum datum = Datum.WGS84)
{
pos = new LatPos();
return(Coordinate_getLatPos(GetNativeReference(), ref pos, datum));
}
public bool GetPosition(LatPos pos, out MapPos result, GroundClampType groundClamp = GroundClampType.NONE, ClampFlags flags = ClampFlags.DEFAULT)
{
Coordinate converter = new Coordinate();
converter.SetLatPos(pos);
result = new MapPos
{
local_orientation = new Matrix3(new Vec3(1, 0, 0), new Vec3(0, 0, -1), new Vec3(0, 1, 0)) // East North Up vectors
};
// Convert to global 3D coordinate in appropriate target system
switch (_mapType)
{
case MapType.UNKNOWN:
{
return(false);
}
case MapType.UTM:
UTMPos utmpos;
if (!converter.GetUTMPos(out utmpos))
{
Message.Send("Controller", MessageLevel.WARNING, "Failed to convert to UTM");
return(false);
}
result.position = new Vec3D(utmpos.easting, utmpos.h, -utmpos.northing) - _origin;
// Possibly compensate for zone and north as well XXX
break;
case MapType.GEOCENTRIC:
CartPos cartpos;
if (!converter.GetCartPos(out cartpos))
{
Message.Send("Controller", MessageLevel.WARNING, "Failed to convert to Geocentric");
return(false);
}
result.local_orientation = converter.GetOrientationMatrix(cartpos);
result.position = new Vec3D(cartpos.x, cartpos.y, cartpos.z) - _origin;
break;
}
// We have now global 3D coordinates
// Check possibly local 3D under a roiNode
if (_topRoi != null)
{
result.roiNode = _topRoi.GetClosestRoiNode(result.position);
// Remove roiNode position as offset - Go to local RoiNode based coordinate system
if (result.roiNode != null && result.roiNode.IsValid())
{
result.position -= result.roiNode.Position;
}
}
return(UpdatePosition(ref result, groundClamp, flags));
}
private static extern IntPtr LatPos_asString(ref LatPos pos);
