private bool IsPartOfPeak(Vector3 pPointCenter)
{
float distance2D = CUtils.Get2DDistance(Center, pPointCenter);
float zDiff = Math.Abs(Center.Z - pPointCenter.Z);
return(distance2D < GetMaxPeakExtent() && zDiff < MAX_PEAK_Y_DIFF);
}
private void RefreshFurthestPoint(Vector3 pPoint)
{
float pointDistToPeak = CUtils.Get2DDistance(pPoint, tree.peak);
if (pointDistToPeak > furthestPointDistance)
{
furthestPoint = pPoint;
}
}
/// <summary>
/// Returns closest trees to the point being in maximal distance of X steps.
/// Trees are sorted based on distance to the point
/// </summary>
public List <CTree> GetTreesInMaxStepsFrom(Vector3 pPoint, int pSteps)
{
Tuple <int, int> index = GetIndexInArray(pPoint);
//TODO: test newer approach using treeFields and sorting based on distance from point
//to the field rather than to tree peak.
//If ok => delete
//List<CTree> trees = new List<CTree>();
Dictionary <CTree, CTreeField> treeFields = new Dictionary <CTree, CTreeField>();
for (int x = index.Item1 - pSteps; x <= index.Item1 + pSteps; x++)
{
for (int y = index.Item2 - pSteps; y <= index.Item2 + pSteps; y++)
{
CTreeField field = GetField(x, y);
if (field != null)
{
List <CTree> detectedTrees = field.DetectedTrees;
foreach (CTree tree in detectedTrees)
{
//if(!trees.Contains(tree))
// trees.Add(tree);
CTreeField detectedField;
if (treeFields.TryGetValue(tree, out detectedField))
{
if (detectedField.GetDistanceTo(pPoint) < field.GetDistanceTo(pPoint))
{
continue;
}
treeFields[tree] = field;
continue;
}
treeFields.Add(tree, field);
}
}
}
}
////todo: sort based on a distance from point to the closest field where tree has been detected.
////in this implementation some tree in the result can be actually further from point than other
//trees.Sort((a, b) => CUtils.Get2DDistance(pPoint, a.Center).CompareTo(CUtils.Get2DDistance(pPoint, b.Center)));
List <KeyValuePair <CTree, CTreeField> > treeFieldList = treeFields.ToList();
//sort based on a distance from point to the closest field where tree has been detected
treeFieldList.Sort((a, b) => CUtils.Get2DDistance(pPoint, a.Value.Center).CompareTo(CUtils.Get2DDistance(pPoint, b.Value.Center)));
List <CTree> result = treeFieldList.ToDictionary(x => x.Key, x => x.Value).Keys.ToList();
return(result);
//return trees;
}
protected List <TypeField> GetPathFrom(TypeField pFieldFrom, TypeField pFieldTo, float pMaxPathLength = int.MaxValue)
{
//todo: check if generated path is valid
TypeField current = pFieldFrom;
List <TypeField> path = new List <TypeField>()
{
current
};
float pathLength = 0;
while (!current.Equals(pFieldTo) && pathLength < pMaxPathLength)
{
EDirection dir = CField.GetDirection(current, pFieldTo);
TypeField newCurrent = (TypeField)current.GetNeighbour(dir);
pathLength += CUtils.Get2DDistance(current, newCurrent);
current = newCurrent;
path.Add(current);
}
return(path);
}
public List <CField> GetFieldsInDistance(int pSteps)
{
List <CField> fields = new List <CField>();
for (int x = -pSteps; x <= pSteps; x++)
{
for (int y = -pSteps; y <= pSteps; y++)
{
CField field = GetFieldWithOffset(x, y);
if (field != null)
{
fields.Add(field);
}
}
}
fields.Sort((a, b) =>
CUtils.Get2DDistance(Center, a.Center).CompareTo(
CUtils.Get2DDistance(Center, b.Center)));
return(fields);
}
/// <summary>
/// Merge trees with similar height which have peaks too close
/// </summary>
private static float GetMergeValidFacor(CTree pTreeToMerge, CTree pPossibleTree)
{
float factor = 0;
float peakHeightDiff = pPossibleTree.peak.Z - pTreeToMerge.peak.Z;
float treeExtent = CParameterSetter.treeExtent * CParameterSetter.treeExtentMultiply;
float similarHeightFactor = (treeExtent - peakHeightDiff) / treeExtent;
bool isSimilarHeight = similarHeightFactor > 0.5f;
if (!isSimilarHeight)
{
return(0);
}
float peakDist2D = CUtils.Get2DDistance(pTreeToMerge.peak, pPossibleTree.peak);
if (peakDist2D < treeExtent)
{
return(1);
}
return(factor);
}
/// <summary>
/// Is point closer than the "furthestPoint" of this or one
/// of neighbouring branches
/// </summary>
public bool IsPointInExtent(Vector3 pPoint)
{
float pointDistToPeak = CUtils.Get2DDistance(pPoint, tree.peak);
bool thisBranchInExtent = furthestPointDistance > pointDistToPeak;
Vector3 closestPoint = GetClosestPointTo(pPoint);
float distToClosestPoint = Vector3.Distance(pPoint, closestPoint);
if (thisBranchInExtent && distToClosestPoint < 1)
{
return(true);
}
CBranch branch1 = GetNeigbourBranch(-1);
float dist1 = branch1.furthestPointDistance;
bool leftBranchInExtent = dist1 > pointDistToPeak;
float distToLeftPoint = CUtils.Get2DDistance(pPoint, branch1.furthestPoint);
//restrict only to points close to the furthest point.
//for too large branches it was including points way too far
bool leftBranchPointClose = distToLeftPoint < 1;
if (leftBranchInExtent && leftBranchPointClose)
{
return(true);
}
CBranch branch2 = GetNeigbourBranch(1);
float dist2 = branch2.furthestPointDistance;
bool rightBranchInExtent = dist2 > pointDistToPeak;
float distToRightPoint = CUtils.Get2DDistance(pPoint, branch2.furthestPoint);
bool rightBranchPointClose = distToRightPoint < 1;
if (rightBranchInExtent && rightBranchPointClose)
{
return(true);
}
return(false);
}
/// <summary>
/// Merges all trees, whose peak has good AddFactor to another tree
/// </summary>
private static void MergeGoodAddFactorTrees(bool pOnlyInvalid)
{
DateTime mergeStart = DateTime.Now;
DateTime previousMergeStart = DateTime.Now;
int iteration = 0;
int maxIterations = Trees.Count;
for (int i = Trees.Count - 1; i >= 0; i--)
{
if (CProjectData.backgroundWorker.CancellationPending)
{
return;
}
if (i >= Trees.Count)
{
//CDebug.WriteLine("Tree was deleted");
continue;
}
CTree treeToMerge = Trees[i];
if (treeToMerge.treeIndex == 48 || treeToMerge.treeIndex == 1001)
{
CDebug.WriteLine("");
}
if (pOnlyInvalid && !treeToMerge.isValid && treeToMerge.IsAtBorder())
{
//CDebug.Warning(treeToMerge + " is at border");
continue;
}
//dont merge tree if it is local maximum
//if some tree is very close (in neighbouring detail field)
//merge it with the highest one
if (detectMethod == EDetectionMethod.AddFactor2D)
{
if (treeToMerge.Equals(22))
{
CDebug.WriteLine();
}
if (treeToMerge.IsLocalMaximum())
{
continue;
}
CTreeField f = CProjectData.Points.treeDetailArray.GetFieldContainingPoint(treeToMerge.peak.Center);
List <CTree> treesInHood = f.GetDetectedTreesFromNeighbourhood();
foreach (CTree tree in treesInHood)
{
if (tree.Equals(treeToMerge))
{
continue;
}
MergeTrees(tree, treeToMerge);
break;
}
continue;
}
List <CTree> possibleTrees = GetPossibleTreesToMergeWith(treeToMerge, EPossibleTreesMethod.ClosestHigher);
Vector3 pPoint = treeToMerge.peak.Center;
float bestAddPointFactor = 0;
CTree selectedTree = null;
foreach (CTree possibleTree in possibleTrees)
{
bool isFar = false;
bool isSimilarHeight = false;
if (possibleTree.isValid && treeToMerge.isValid)
{
float mergeFactor = GetMergeValidFacor(treeToMerge, possibleTree);
if (mergeFactor > 0.9f)
{
selectedTree = possibleTree;
break;
}
}
if (pOnlyInvalid && possibleTree.isValid && treeToMerge.isValid)
{
continue;
}
if (treeToMerge.isValid)
{
//treeToMerge is always lower
float possibleTreeHeight = possibleTree.GetTreeHeight();
float treeToMergeHeight = treeToMerge.GetTreeHeight();
const float maxPeaksDistance = 1;
float peaksDist = CUtils.Get2DDistance(treeToMerge.peak, possibleTree.peak);
if (peaksDist > maxPeaksDistance)
{
isFar = true;
}
const float maxPeakHeightDiff = 1;
if (possibleTreeHeight - treeToMergeHeight < maxPeakHeightDiff)
{
isSimilarHeight = true;
}
}
float addPointFactor = possibleTree.GetAddPointFactor(pPoint, treeToMerge);
float requiredFactor = 0.5f;
if (isFar)
{
requiredFactor += 0.1f;
}
if (isSimilarHeight)
{
requiredFactor += 0.1f;
}
if (pOnlyInvalid)
{
requiredFactor -= 0.2f;
}
if (addPointFactor > requiredFactor && addPointFactor > bestAddPointFactor)
{
selectedTree = possibleTree;
bestAddPointFactor = addPointFactor;
if (bestAddPointFactor > 0.9f)
{
break;
}
}
}
if (selectedTree != null)
{
float dist = CUtils.Get2DDistance(treeToMerge.peak.Center, selectedTree.peak.Center);
treeToMerge = MergeTrees(ref treeToMerge, ref selectedTree, pOnlyInvalid);
}
CDebug.Progress(iteration, maxIterations, 50, ref previousMergeStart, mergeStart, "merge");
iteration++;
}
}
/// <summary>
/// Mergind method for Detection2D
/// </summary>
private static void Merge2DTrees()
{
return;
for (int i = Trees.Count - 1; i >= 0; i--)
{
CTree treeToMerge = Trees[i];
if (treeToMerge.Equals(173))
{
CDebug.WriteLine();
}
//if(treeToMerge.IsLocalMaximum())
//{
// continue;
//}
List <CTree> possibleTrees =
GetPossibleTreesToMergeWith(treeToMerge, EPossibleTreesMethod.ClosestHigher);
for (int t = 0; t < possibleTrees.Count; t++)
{
//merge trees if the target tree can be reached from the treeToMerge
//and they are of different heights
CTree tree = possibleTrees[t];
float maxRadius = CTreeRadiusCalculator.GetTreeRadius(tree);
float treeAvgExtent = tree.GetAverageExtent();
float maxDist = Math.Max(maxRadius, treeAvgExtent);
float treeDist = CUtils.Get2DDistance(treeToMerge, tree);
if (treeDist > maxDist)
{
continue;
}
/*float heightDiff = tree.GetTreeHeight() - treeToMerge.GetTreeHeight();
* if(heightDiff < 2)
* {
* continue;
* }*/
bool pathContainsTree = false;
/*
* CTreeField closeField = tree.GetClosestField(treeToMerge.peak.Center);
* bool pathContainsTree =
* CProjectData.Points.treeDetailArray.PathContainsTree(
* treeToMerge.peak.Center, closeField.Center, tree, 1, 1);
*
* if(!pathContainsTree)
* {
* pathContainsTree =
* CProjectData.Points.treeDetailArray.PathContainsTree(
* treeToMerge.peak.Center, tree.peak.Center, tree, 1, 1);
* }*/
bool canAdd = tree.CanAdd(treeToMerge.peak.Center, false);
if (pathContainsTree || canAdd)
{
treeToMerge = MergeTrees(ref treeToMerge, ref tree, true);
break;
}
}
/*List<CField> fields = treeToMerge.peakDetailField.GetFieldsInDistance(5);
* foreach(CField field in fields)
* {
* CTreeField treeField = (CTreeField)field;
* CTree detectedTree = treeField.GetSingleDetectedTree();
* if(detectedTree != null && !detectedTree.Equals(treeToMerge))
* {
* float heightDiff = detectedTree.GetTreeHeight() - treeToMerge.GetTreeHeight();
* if(heightDiff > 2)
* {
* treeToMerge = MergeTrees(ref treeToMerge, ref detectedTree, true);
* break;
* }
* }
* }*/
}
}
private float GetAddPointFactorInRefTo(Vector3 pPoint, Vector3 pReferencePoint,
bool pSameBranch, bool pMerging, CTree pTreeToMerge = null)
{
//during merging it is expected, that added peak will be higher
if (!pMerging && pPoint.Z > pReferencePoint.Z)
{
//points are added in descending order. if true => pPoint belongs to another tree
return(0);
}
float pointDistToRef = Vector3.Distance(pPoint, pReferencePoint);
if (pointDistToRef < 0.2)
{
return(1);
}
float refDistToPeak = CUtils.Get2DDistance(pReferencePoint, tree.peak);
float pointDistToPeak = CUtils.Get2DDistance(pPoint, tree.peak);
if (!pMerging && pSameBranch)
{
if (pointDistToPeak < refDistToPeak)
{
return(1);
}
}
float distToPeakDiff = pointDistToPeak - refDistToPeak;
if (!pMerging && distToPeakDiff < 0.3)
{
return(1);
}
if (!pMerging && distToPeakDiff > 0.5f && refDistToPeak > 0.5f)
{
float peakRefPointAngle = CUtils.AngleBetweenThreePoints(tree.peak.Center, pReferencePoint, pPoint);
//new point is too far from furthest point and goes too much out of direction of peak->furthestPoint
if (peakRefPointAngle < 180 - 45)
{
return(0);
}
}
float refAngleToPoint =
CUtils.AngleBetweenThreePoints(pReferencePoint - Vector3.UnitZ, pReferencePoint, pPoint);
Vector3 suitablePeakPoint = tree.peak.Center;
float peakAngleToPoint =
CUtils.AngleBetweenThreePoints(suitablePeakPoint - Vector3.UnitZ, suitablePeakPoint, pPoint);
float angle = Math.Min(refAngleToPoint, peakAngleToPoint);
float maxBranchAngle = CTree.GetMaxBranchAngle(suitablePeakPoint, pPoint);
float unacceptableAngle = maxBranchAngle;
if (!pMerging && angle > unacceptableAngle)
{
return(0);
}
unacceptableAngle += 30;
unacceptableAngle = Math.Min(unacceptableAngle, 100);
float angleFactor = (unacceptableAngle - angle) / unacceptableAngle;
float unacceptableDistance = tree.GetTreeExtentFor(pPoint,
pMerging ? CParameterSetter.treeExtentMultiply : 1);
unacceptableDistance += 0.5f;
if (pointDistToPeak > unacceptableDistance)
{
return(0);
}
unacceptableDistance += 0.5f;
float distFactor = (unacceptableDistance - pointDistToPeak) / unacceptableDistance;
Vector3 closestPoint = GetClosestPointTo(pPoint);
float distFromClosestPoint = Vector3.Distance(pPoint, closestPoint);
float maxDistFromClosest = 0.5f;
float distToClosestFactor = 2 * (maxDistFromClosest + 0.2f - distFromClosestPoint);
distToClosestFactor = Math.Max(0, distToClosestFactor);
float totalFactor;
if (pMerging)
{
if (distFromClosestPoint < maxDistFromClosest && distToPeakDiff < maxDistFromClosest)
{
return(1);
}
}
if (pTreeToMerge != null && pMerging && pTreeToMerge.isValid)
{
int factorCount = 3;
if (distToClosestFactor < .1f)
{
factorCount -= 1;
}
totalFactor = (distToClosestFactor + angleFactor + distFactor) / factorCount;
}
else
{
//let dist factor have higher influence
totalFactor = (angleFactor + 1.5f * distFactor) / 2.5f;
}
return(totalFactor);
}
public CBall(List <Vector3> pPoints)
{
pPoints.Sort((a, b) => b.Z.CompareTo(a.Z));
//todo: make balltop a list of points and get avg
ballTop = pPoints[0];
furthestPoint2D = ballTop;
furthestPointPlusX = ballTop;
furthestPointMinusX = ballTop;
furthestPointPlusY = ballTop;
furthestPointMinusY = ballTop;
foreach (Vector3 point in pPoints)
{
float zDiff = ballTop.Z - point.Z;
if (zDiff > GetMaxPointsDist(1))
{
break;
}
bool isInBallExtent = Vector3.Distance(point, ballTop) > GetMaxPointsDist(1);
float dist2D = CUtils.Get2DDistance(point, ballTop);
UpdateFurthestPoints(point);
if (ballBot == null && dist2D < 0.01 && zDiff > GetMaxPointsDist(0) / 2)
{
ballBot = point;
}
if (dist2D > GetMaxPointsDist(3) / 2)
{
isValid = false;
return;
}
if (!isInBallExtent)
{
bool isUnderBallTop = dist2D < 0.1f;
if (!isUnderBallTop)
{
isValid = false;
return;
}
}
}
if (ballBot != null)
{
float topBotDiffZ = ballTop.Z - ((Vector3)ballBot).Z;
//if bot is too close then it is probably not a ball
if (topBotDiffZ < GetMaxPointsDist(2) / 2)
{
isValid = false;
return;
}
}
float furthestDist2D = GetFurthestPointDist2D();
float maxDist = GetMaxPointsDist(-3) / 2;
if (furthestDist2D < maxDist)
{
isValid = false;
return;
}
isValid = HasValidMainPoints();
if (isValid)
{
center = CalculateCenter();
}
}
private void UpdateFurthestPoints(Vector3 pPoint)
{
if (!IsAtMainPointZDistance(pPoint))
{
return;
}
float minFurthestPointDist2D = GetMaxPointsDist(-6) / 2;
Vector3 diff3D = ballTop - pPoint;
float diffX = Math.Abs(diff3D.X);
float diffY = Math.Abs(diff3D.Y);
float diffZ = Math.Abs(diff3D.Z);
if (diffZ < minFurthestPointDist2D)
{
return;
}
float dist2D = CUtils.Get2DDistance(pPoint, ballTop);
if (dist2D > GetFurthestPointDist2D())
{
furthestPoint2D = pPoint;
}
float diff;
if (diffY < DIST_TOLLERANCE)
{
if (pPoint.X > ballTop.X)
{
diff = pPoint.X - ballTop.X;
if (/*diff > minFurthestPointDist2D &&*/ diff >= furthestPointPlusX.X - ballTop.X)
{
furthestPointPlusX = pPoint;
}
}
else
{
diff = ballTop.X - pPoint.X;
if (/*diff > minFurthestPointDist2D && */ diff >= ballTop.X - furthestPointMinusX.X)
{
furthestPointMinusX = pPoint;
}
}
}
if (diffX < DIST_TOLLERANCE)
{
if (pPoint.Y > ballTop.Y)
{
diff = pPoint.Y - ballTop.Y;
if (/*diff > minFurthestPointDist2D &&*/ diff >= furthestPointPlusY.Y - ballTop.Y)
{
furthestPointPlusY = pPoint;
}
}
else
{
diff = ballTop.Y - pPoint.Y;
if (/*diff > minFurthestPointDist2D && */ diff >= ballTop.Y - furthestPointMinusY.Y)
{
furthestPointMinusY = pPoint;
}
}
}
}
private float GetFurthestPointDist2D()
{
return(CUtils.Get2DDistance(ballTop, furthestPoint2D));
}
public float GetDistanceTo(Vector3 pPoint)
{
return(CUtils.Get2DDistance(this.Center, pPoint));
}