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)); }