private List <TopologyIntersection> FindAllIntersections(Face2D target, List <ICuttableEdge> edges2cut)
{
Intersector intersector = new Intersector();
List <TopologyIntersection> res_list = new List <TopologyIntersection>();
for (int loop_idx = 0; loop_idx < target.Loops.Count; ++loop_idx)
{
var loop = target.Loops[loop_idx];
for (int target_edge_idx = 0; target_edge_idx < loop.Count; ++target_edge_idx)
{
List <TopologyIntersection> sorted_is = new List <TopologyIntersection>();
for (int tool_edge_idx = 0; tool_edge_idx < edges2cut.Count; ++tool_edge_idx)
{
var intersections = intersector.Intersect(loop[target_edge_idx], edges2cut[tool_edge_idx]);
foreach (var intersection in intersections)
{
TopologyIntersection res = new TopologyIntersection(
loop_idx, target_edge_idx, tool_edge_idx, intersection);
sorted_is.Add(res);
}
}
sorted_is.Sort(new TopologyIntersectionCompare_ByTarget());
res_list.AddRange(sorted_is);
}
}
return(res_list);
}
public ImageSensorData Measure()
{
var tmpLocation = GetCameraLocation(_robot);
var result = new KinectData(_settings.VerticalResolution, _settings.HorisontalResolution);
var horisontalAngle = -_settings.HorisontalViewAngle / 2.0;
var verticalAngle = -_settings.VerticalViewAngle / 2.0;
for (int i = 0; i < _settings.VerticalResolution; i++)
{
Frame3D mediateDirection = SensorRotation.VerticalFrameRotation(tmpLocation, -verticalAngle);
horisontalAngle = -_settings.HorisontalViewAngle / 2.0;
for (int j = 0; j < _settings.HorisontalResolution; j++)
{
Frame3D direction = SensorRotation.HorisontalFrameRotation(mediateDirection, horisontalAngle);
Ray ray = new Ray(tmpLocation.ToPoint3D(), SensorRotation.GetFrontDirection(direction));
var dist = double.PositiveInfinity;
foreach (var body in _worldRoot.GetSubtreeChildrenFirst())
{
if (_settings.Exclude.All(a => a != body))
{
var inter = Intersector.Intersect(body, ray);
dist = Math.Min(dist, inter);
}
}
result.Depth[i, j] = dist;
//verticalAngle += _settings.VStep;
horisontalAngle += _settings.HStep;
}
verticalAngle += _settings.VStep;
}
return(new ImageSensorData(result.GetBytes()));
}
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 bool GetScreenGroundPosition(int x, int y, uint size_x, uint size_y, out MapPos result, ClampFlags flags = ClampFlags.DEFAULT)
{
result = new MapPos();
Vec3D position;
Vec3 direction;
if (!GetScreenVectors(x, y, size_x, size_y, out position, out direction))
{
return(false);
}
Intersector isect = new Intersector();
if ((flags & ClampFlags.FRUSTRUM_CULL) != 0 && _camera != null && _camera.IsValid())
{
isect.SetCamera(_camera);
if (_camera.RoiPosition)
{
position = position - _camera.Position;
}
}
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, _camera.Position))
{
IntersectorResult res = isect.GetResult();
IntersectorData data = res.GetData(0);
result.position = data.position;
if (_camera.RoiPosition)
{
result.position = result.position + _camera.Position;
}
result.normal = data.normal;
result.clamped = true;
}
isect.Dispose(); // Drop handle and ignore GC
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(true);
}
public bool UpdatePosition(ref MapPos result, GroundClampType groundClamp = GroundClampType.NONE, ClampFlags flags = ClampFlags.DEFAULT)
{
if (groundClamp != GroundClampType.NONE)
{
// Add ROINode position as offset - Go to global 3D coordinate system as we need to clamp in global 3D
if (result.roiNode != null && result.roiNode.IsValid())
{
result.position += result.roiNode.Position;
}
Intersector isect = new Intersector();
Vec3D eyePos = new Vec3D(0, 0, 0);
if (_camera != null && _camera.IsValid())
{
eyePos = _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
{
eyePos = 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 - eyePos) + 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), 1, true, eyePos))
{
IntersectorResult res = isect.GetResult();
IntersectorData data = res.GetData(0);
result.position.x = data.position.x + eyePos.x;
result.position.y = data.position.y + eyePos.y;
result.position.z = data.position.z + eyePos.z;
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 (result.roiNode != null && result.roiNode.IsValid())
{
result.position -= result.roiNode.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);
}
/// <summary>
/// Get a Local mappos in Global ray direction
/// </summary>
/// <param name="global_position"></param>
/// <param name="direction"></param>
/// <param name="result"></param>
/// <param name="flags"></param>
/// <returns></returns>
public bool GetGroundPosition(Vec3D global_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 for clamp operation
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
global_position = global_position - origo;
isect.SetStartPosition((Vec3)global_position);
isect.SetDirection(direction);
if (isect.Intersect(_currentMap, IntersectQuery.ABC_TRI |
IntersectQuery.NEAREST_POINT |
(flags.HasFlag(ClampFlags.ALIGN_NORMAL_TO_SURFACE) ? IntersectQuery.NORMAL : 0) | //IntersectQuery.NORMAL |
(flags.HasFlag(ClampFlags.WAIT_FOR_DATA) ? IntersectQuery.WAIT_FOR_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;
}
isect.Dispose(); // Drop handle and ignore GC
result.local_orientation = GetLocalOrientation(result.position);
if (result.clampResult == IntersectQuery.NULL)
{
result.normal = result.local_orientation.GetCol(2);
}
return(ToLocal(result));
}