public void UnExecute()
{
if (m_points2Remove != null)
{
foreach (var point in m_points2Remove)
{
m_pointsContainer.Add(point);
}
}
if (m_lines2Remove != null)
{
foreach (var line in m_lines2Remove)
{
m_linesContainer.Add(line);
}
}
if (m_observations2Remove != null)
{
foreach (var observation in m_observations2Remove)
{
m_observationsContainer.Add(observation);
}
}
}
private void UpdateMap <T>(
ICollection <T> source,
Func <T, bool> isNewSelector,
Func <T, bool> isRemovedSelector,
Func <T, bool> justColoredSelector,
Func <T, bool> isValidSelector,
ICloudObjectsContainer <T> map)
{
if (source != null)
{
foreach (var element in source)
{
if (isValidSelector(element))
{
if (isNewSelector(element))
{
map.Add(element);
}
else if (isRemovedSelector(element))
{
map.Remove(element);
}
else if (justColoredSelector(element))
{
map.ChangeColor(element);
}
else
{
map.Update(element);
}
}
}
}
}
private void MakeConnectionsAndConnectionPlaceholdersFor(SlamObservation observation)
{
foreach (var covisible in observation.CovisibleInfos)
{
int connectionIdx = m_connections.FindIndex(c =>
c.obsId1 == observation.Point.id && c.obsId2 == covisible.id ||
c.obsId2 == observation.Point.id && c.obsId1 == covisible.id);
if (connectionIdx == -1)
{
connectionIdx = m_connections.Count;
m_connections.Add(new Connection(observation.Point.id, covisible.id, observation));
}
var connection = m_connections[connectionIdx];
SlamObservation covisibleObs = m_nodes.FirstOrDefault(node => node.Point.id == covisible.id);
if (
/*already in map*/ covisibleObs != null &&
/*is placeholder*/ covisibleObs.Point.id == connection.obsId2 && connection.lineId == -1)
{
SlamLine line = new SlamLine()
{
vert1 = observation.Point.position,
vert2 = covisibleObs.Point.position,
pointId1 = observation.Point.id,
pointId2 = covisibleObs.Point.id,
color1 = observation.Point.color,
color2 = covisibleObs.Point.color,
isRemoved = false
};
m_connections[connectionIdx] = new Connection(
connection.obsId1, connection.obsId2,
observation, covisibleObs,
m_lines.Add(line));
}
//Debug.Assert(m_connections[connectionIdx].first != null || m_connections[connectionIdx].second != null,
// $"[SlamObservationsContainer.UpdateConnectionsOf] connection.first == connection.second == null for " +
// $"id1 = {m_connections[connectionIdx].obsId1}, " +
// $"id2 = {m_connections[connectionIdx].obsId2}");
if (!(m_connections[connectionIdx].first != null || m_connections[connectionIdx].second != null))
{
throw new InvalidSlamContainerOperationException(
$"[SlamObservationsContainer.UpdateConnectionsOf] connection.first == connection.second == null for " +
$"id1 = {m_connections[connectionIdx].obsId1}, " +
$"id2 = {m_connections[connectionIdx].obsId2}");
}
}
}
private void ContinueTrack(Vector3 vert1, Vector3 vert2)
{
if (vert1 == vert2)
{
m_lineIdsHistory.Push(-1);
return;
}
SlamLine line = new SlamLine()
{
color1 = color,
isRemoved = false,
pointId1 = m_lineSegmentIdx,
pointId2 = ++m_lineSegmentIdx,
vert1 = vert1,
vert2 = vert2,
};
m_lastLineId = m_linesContainer.Add(line);
m_lineIdsHistory.Push(m_lastLineId);
m_linesContainer.Repaint();
}
public void Execute()
{
if (m_addedPoints != null)
{
foreach (var point in m_addedPoints)
{
m_pointsContainer.Add(point);
}
}
if (m_addedLines != null)
{
foreach (var line in m_addedLines)
{
m_linesContainer.Add(line);
}
}
if (m_addedObservations != null)
{
foreach (var observation in m_addedObservations)
{
m_observationsContainer.Add(observation);
}
}
}
