/// <summary>
/// Converts a global mappos to a local under a roi
/// </summary>
/// <param name="result"></param>
/// <returns></returns>
public bool ToLocal(MapPos result)
{
if (result.node != null) // Already local
{
return(true);
}
if (_topRoi == null)
{
return(false);
}
RoiNode roi = _topRoi.GetClosestRoiNode(result.position);
result.node = roi;
// Remove roiNode position as offset - Go to local RoiNode based coordinate system
if (roi != null && roi.IsValid())
{
result.position -= roi.Position;
return(true);
}
return(false); // We failed to convert
}
public bool GetGroundPosition(Vec3D position, Vec3 direction, out MapPos result, ClampFlags flags = ClampFlags.DEFAULT)
{
result = new MapPos();
// Coordinate is now in world Cartesian coordinates (Roi Position)
Vec3D origo = new Vec3D(0, 0, 0); // Set used origo
Intersector isect = new Intersector();
// Check camera frustrum -----------------------------------------------------------
if (_camera != null && _camera.IsValid())
{
origo = _camera.Position;
if ((flags & ClampFlags.FRUSTRUM_CULL) != 0)
{
isect.SetCamera(_camera);
}
}
// Adjust intersector to use origo as center
position = position - origo;
isect.SetStartPosition((Vec3)position);
isect.SetDirection(direction);
if (isect.Intersect(_currentMap, IntersectQuery.NEAREST_POINT | IntersectQuery.NORMAL | ((flags & ClampFlags.WAIT_FOR_DATA) != 0 ? IntersectQuery.WAIT_FOR_DYNAMIC_DATA : 0), 1, true, origo))
{
IntersectorResult res = isect.GetResult();
IntersectorData data = res.GetData(0);
result.position = data.position + origo;
result.normal = data.normal;
result.clamped = true;
}
isect.Dispose(); // Drop handle and ignore GC
if (_topRoi != null)
{
RoiNode roi = _topRoi.GetClosestRoiNode(result.position);
result.node = roi;
// Remove roiNode position as offset - Go to local RoiNode based coordinate system
if (roi != null && roi.IsValid())
{
result.position -= roi.Position;
}
}
return(true);
}
public Vec3D LocalToWorld(MapPos mappos)
{
Vec3D result = mappos.position;
RoiNode roi = mappos.node as RoiNode;
if (roi != null && roi.IsValid())
{
result += roi.Position;
}
return(new Vec3D(result.x, result.y, -result.z));
}
public bool LocalToWorld(MapPos pos, out CartPos 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 Vec3D GlobalPosition(Vec3 enu_offset = default)
{
Vec3D result = position;
RoiNode roi = node as RoiNode;
// If we are a roi node based position we add roi position as local origin
if (roi != null && roi.IsValid())
{
result += roi.Position;
}
result += local_orientation * enu_offset;
return(result);
}
private void MapToRoi(MapPos result)
{
if (_topRoi == null)
{
return;
}
RoiNode roi = _topRoi.GetClosestRoiNode(result.position);
result.node = roi;
// Remove roiNode position as offset - Go to local RoiNode based coordinate system
if (roi != null && roi.IsValid())
{
result.position -= roi.Position;
}
}
public bool GetCartPos(MapPos pos, out CartPos result)
{
Coordinate converter = new Coordinate();
Vec3D position = pos.position;
result = new CartPos();
// 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.GetCartPos(out result));
}
/// <summary>
/// Converts a local mappos under a roi to a global position
/// </summary>
/// <param name="result"></param>
/// <returns></returns>
public bool ToGlobal(MapPos result)
{
if (result.node == null) // Already global
{
return(true);
}
// Remove roiNode position as offset - Go to local RoiNode based coordinate system
RoiNode roi = result.node as RoiNode;
if (roi != null && roi.IsValid())
{
result.position += roi.Position;
result.node = null;
return(true);
}
return(false); // We failed to convert
}
public bool UpdatePosition(MapPos result, GroundClampType groundClamp = GroundClampType.NONE, ClampFlags flags = ClampFlags.DEFAULT)
{
if (_currentMap == null) // No map
{
return(false);
}
if (groundClamp != GroundClampType.NONE)
{
// Add ROINode position as offset - Go to global 3D coordinate system as we need to clamp in global 3D
RoiNode roi = result.node as RoiNode;
if (roi != null && roi.IsValid())
{
result.position += roi.Position;
}
Intersector isect = new Intersector();
Vec3D origo = new Vec3D(0, 0, 0);
// Check camera frustrum -----------------------------------------------------------
if (_camera != null && _camera.IsValid())
{
origo = _camera.Position;
if ((flags & ClampFlags.FRUSTRUM_CULL) != 0)
{
isect.SetCamera(_camera);
}
}
if ((flags & ClampFlags.ISECT_LOD_QUALITY) != 0) // Lets stand in the ray to get highest quality
{
origo = result.position;
}
Vec3 up = new Vec3(result.local_orientation.v13, result.local_orientation.v23, result.local_orientation.v33);
// TODO: Fix this....
if (up.x == 0 && up.y == 0 && up.z == 0)
{
up.y = 1;
}
isect.SetStartPosition((Vec3)(result.position - origo) + 10000.0f * up);
isect.SetDirection(-up);
if (isect.Intersect(_currentMap, IntersectQuery.NEAREST_POINT |
IntersectQuery.NORMAL |
((flags & ClampFlags.WAIT_FOR_DATA) != 0 ? IntersectQuery.WAIT_FOR_DYNAMIC_DATA : 0) |
((flags & ClampFlags.UPDATE_DATA) != 0 ? IntersectQuery.UPDATE_DYNAMIC_DATA : 0)
, 1, true, origo))
{
IntersectorResult res = isect.GetResult();
IntersectorData data = res.GetData(0);
result.position = data.position + origo;
result.normal = data.normal;
result.clamped = true;
}
else
{
result.normal = up;
result.clamped = false;
}
if (groundClamp == GroundClampType.GROUND)
{
result.normal = result.local_orientation.GetCol(2);
}
isect.Dispose(); // Drop handle and ignore GC
// Remove ROINode position as offset - Go to local coordinate system under ROI Node
if (roi != null && roi.IsValid())
{
result.position -= roi.Position;
}
}
return(true);
}
public bool UpdatePosition(MapPos result, GroundClampType groundClamp = GroundClampType.NONE, ClampFlags flags = ClampFlags.DEFAULT)
{
if (_currentMap == null) // No map
{
return(false);
}
result.normal = result.local_orientation.GetCol(2);
if (groundClamp != GroundClampType.NONE)
{
// Add ROINode position as offset - Go to global 3D coordinate system as we need to clamp in global 3D
RoiNode roi = result.node as RoiNode;
if (roi != null && roi.IsValid())
{
result.position += roi.Position;
}
// The defined down vector
Vec3 down = new Vec3(-result.local_orientation.v13, -result.local_orientation.v23, -result.local_orientation.v33);
// Check triangel ground
if (result.clampResult.HasFlag(IntersectQuery.ABC_TRI))
{
if (Intersect(result.position, down, result.a, result.b, result.c, out Vec3D p))
{
result.position = p;
ToLocal(result);
return(true);
}
}
// Check new intersector
Intersector isect = new Intersector();
Vec3D origo = new Vec3D(0, 0, 0);
// Check camera frustrum -----------------------------------------------------------
if (_camera != null && _camera.IsValid())
{
origo = _camera.Position;
if ((flags & ClampFlags.FRUSTRUM_CULL) != 0)
{
isect.SetCamera(_camera);
}
}
if ((flags & ClampFlags.ISECT_LOD_QUALITY) != 0) // Lets stand in the ray to get highest quality
{
origo = result.position;
}
isect.SetStartPosition((Vec3)(result.position - origo) - 10000.0f * down); // Move backwards
isect.SetDirection(down);
if (isect.Intersect(_currentMap, IntersectQuery.NEAREST_POINT | IntersectQuery.ABC_TRI |
(flags.HasFlag(ClampFlags.ALIGN_NORMAL_TO_SURFACE) ? IntersectQuery.NORMAL : 0) | //IntersectQuery.NORMAL |
(flags.HasFlag(ClampFlags.WAIT_FOR_DATA) ? IntersectQuery.WAIT_FOR_DYNAMIC_DATA : 0) |
(flags.HasFlag(ClampFlags.UPDATE_DATA) ? IntersectQuery.UPDATE_DYNAMIC_DATA : 0)
, 1, true, origo))
{
IntersectorResult res = isect.GetResult();
IntersectorData data = res.GetData(0);
result.position = data.coordinate + origo;
if ((data.resultMask & IntersectQuery.NORMAL) != 0)
{
result.normal = data.normal;
}
if (data.resultMask.HasFlag(IntersectQuery.ABC_TRI))
{
result.a = data.a + origo;
result.b = data.b + origo;
result.c = data.c + origo;
}
result.clampResult = data.resultMask;
}
else
{
result.clampResult = IntersectQuery.NULL;
}
//if (groundClamp == GroundClampType.GROUND)
//{
// result.normal = result.local_orientation.GetCol(2);
//}
isect.Dispose(); // Drop handle and ignore GC
// Remove ROINode position as offset - Go to local coordinate system under ROI Node
ToLocal(result);
}
result.clampFlags = flags;
result.clampType = groundClamp;
return(true);
}