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>
/// Get info from preprocessed file using lasinfo and set it to main header
/// </summary>
private static void SetMainHeader(string pPreprocessedFilePath)
{
//FileInfo fi = new FileInfo(pPreprocessedFilePath);
string infoFilePath = CPreprocessController.currentTmpFolder + "\\" + CUtils.GetFileName(pPreprocessedFilePath) + "_i.txt";
string[] headerLines = CPreprocessController.GetHeaderLines(pPreprocessedFilePath, infoFilePath);
if (headerLines == null)
{
CDebug.Error("header lines are null");
//todo: is it ok to leave it as null??
//CProjectData.mainHeader = new CHeaderInfo();
}
else
{
CProjectData.mainHeader = new CHeaderInfo(headerLines);
}
//can be inited only after main header is set
CBitmapExporter.Init();
}
public static bool IsAtBufferZone(Vector3 pPoint)
{
Vector3 mainBotLeft = CProjectData.mainHeader.BotLeftCorner;
Vector3 mainTopRight = CProjectData.mainHeader.TopRightCorner;
float distanceFromWholeForestBorder = CUtils.GetDistanceFromBorder(pPoint, mainBotLeft, mainTopRight);
Vector3 tileBotLeft = CProjectData.currentTileHeader.BotLeftCorner;
Vector3 tileTopRight = CProjectData.currentTileHeader.TopRightCorner;
float distanceFromCurrentTileBorder = CUtils.GetDistanceFromBorder(pPoint, tileBotLeft, tileTopRight);
//is close at tile border
if (distanceFromCurrentTileBorder < CProjectData.bufferSize)
{
//is close at whole forest border - only corners are considered a buffer zone
if (distanceFromWholeForestBorder < CProjectData.bufferSize)
{
float distanceXFromCurrentTileBorder =
CUtils.GetDistanceFromBorderX(pPoint, tileBotLeft, tileTopRight);
float distanceYFromCurrentTileBorder =
CUtils.GetDistanceFromBorderY(pPoint, tileBotLeft, tileTopRight);
float distanceXFromForest =
CUtils.GetDistanceFromBorderX(pPoint, mainBotLeft, mainTopRight);
float distanceYFromForest =
CUtils.GetDistanceFromBorderY(pPoint, mainBotLeft, mainTopRight);
bool isAtTileCorner = distanceXFromCurrentTileBorder < CProjectData.bufferSize &&
distanceYFromCurrentTileBorder < CProjectData.bufferSize;
bool isNotAtForestCorner = distanceXFromForest > CProjectData.bufferSize ||
distanceYFromForest > CProjectData.bufferSize;
return(isAtTileCorner && isNotAtForestCorner);
}
return(true);
}
return(false);
}
/// <summary>
///
/// </summary>
private static Feature GetTreeBorder(CTree pTree)
{
List <Vector3> furthestPoints = pTree.GetFurthestPoints();
List <Coordinate> pointsCoords = new List <Coordinate>();
foreach (Vector3 p in furthestPoints)
{
CVector3D globalP = CUtils.GetGlobalPosition(p);
pointsCoords.Add(new Coordinate(globalP.X, globalP.Y));
}
pointsCoords.Add(pointsCoords[0]); //to close polygon
IPolygon polygon = factory.CreatePolygon(pointsCoords.ToArray());
//id
AttributesTable attributesTable = new AttributesTable();
attributesTable.Add(ATTR_ID, pTree.treeIndex);
//position
CVector3D globalTreepos = CUtils.GetGlobalPosition(pTree.peak.Center);
attributesTable.Add(ATTR_X, globalTreepos.X.ToString(NUM_FORMAT));
attributesTable.Add(ATTR_Y, globalTreepos.Y.ToString(NUM_FORMAT));
//area
attributesTable.Add(ATTR_AREA, pTree.GetArea());
//tree height
float treeHeight = pTree.GetTreeHeight();
attributesTable.Add(ATTR_HEIGHT, treeHeight.ToString(NUM_FORMAT));
//reftree type
//attributesTable.Add(ATTR_TYPE, pTree.assignedRefTree.RefTreeTypeName);
Feature feature = new Feature(polygon, attributesTable);
return(feature);
}
/// <summary>
/// Calculates similarity with other branch.
/// Range = [0,1]. 1 = Best match.
/// </summary>
public float GetSimilarityWith(CBranch pOtherBranch, Vector3 offsetToThisTree, Matrix4x4 scaleMatrix)
{
if (pOtherBranch.TreePoints.Count == 0)
{
if (TreePoints.Count == 0)
{
return(1);
}
//situation when other branch has no points.
//this can mean that data in this part of tree are just missing therefore it should
return(UNDEFINED_SIMILARITY);
}
float similarity = 0;
//CreateRotationY rotates point counter-clockwise => -pAngleOffset
//rotation has to be calculated in each branch
float angleOffsetRadians = CUtils.ToRadians(-(angleFrom - pOtherBranch.angleFrom));
Matrix4x4 rotationMatrix = Matrix4x4.CreateRotationY(angleOffsetRadians, tree.peak.Center);
foreach (CTreePoint p in pOtherBranch.TreePoints)
{
Vector3 movedPoint = p.Center + offsetToThisTree;
Vector3 scaledPoint = Vector3.Transform(movedPoint, scaleMatrix);
Vector3 rotatedPoint = Vector3.Transform(scaledPoint, rotationMatrix);
const int branchToleranceMultiply = 2;
if (Contains(rotatedPoint, branchToleranceMultiply))
{
similarity += 1f / pOtherBranch.TreePoints.Count;
}
}
if (similarity - 1 > 0.1f) //similarity can be > 1 due to float imprecision
{
CDebug.Error("Similarity rounding error too big. " + similarity);
}
similarity = Math.Min(1, similarity);
return(similarity);
}
/// <summary>
/// Add points from given class to the output and main output
/// </summary>
private static void AddPointsTo(ref StringBuilder pOutput, EClass pClass, ref DateTime start)
{
List <Vector3> points = CProjectData.Points.GetPoints(pClass);
string res;
DateTime lastDebug = DateTime.Now;
for (int i = 0; i < points.Count; i++)
{
if (CProjectData.backgroundWorker.CancellationPending)
{
return;
}
CDebug.Progress(i, points.Count, DEBUG_FREQUENCY, ref lastDebug, start, "Export las (ground points)");
Vector3 p = points[i];
CVector3D globalP = CUtils.GetGlobalPosition(p);
res = GetPointLine(globalP, 1, 0, GetClassColor(pClass)) + newLine;
pOutput.Append(res);
}
//mainOutput.Append(pOutput);
}
/// <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>
/// Sets position, scale and rotation of tree obj to match given pTargetTree
/// </summary>
private static void SetRefTreeObjTransform(ref CRefTree pRefTree, CTree pTargetTree, int pAngleOffset)
{
Vector3 arrayCenter = CProjectData.GetArrayCenter();
float minHeight = CProjectData.GetMinHeight();
//float treeHeight = pTargetTree.peak.maxHeight.Y - (float)groundHeight;
float treeHeight = pTargetTree.GetTreeHeight();
float heightRatio = treeHeight / pRefTree.GetTreeHeight();
pRefTree.Obj.Scale = heightRatio * Vector3.One;
//align position to tree
pRefTree.Obj.Position = pTargetTree.peak.Center;
pRefTree.Obj.Position.Z -= pRefTree.GetTreeHeight() * heightRatio;
//move obj so it is at 0,0,0
pRefTree.Obj.Position -= arrayCenter;
pRefTree.Obj.Position -= Vector3.UnitZ * minHeight;
pRefTree.Obj.Rotation = new Vector3(0, -pAngleOffset, 0);
//in OBJ format Y = height
CUtils.SwapYZ(ref pRefTree.Obj.Position);
}
public float GetDistanceTo(Vector3 pPoint)
{
return(CUtils.Get2DDistance(this.Center, pPoint));
}
/// <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++;
}
}
public List <Vector3> GetMainPoints(bool pAddDebugLine)
{
List <Vector3> points = new List <Vector3>();
points.Add(ballTop);
if (pAddDebugLine)
{
points.AddRange(CUtils.GetPointLine(ballTop, Vector3.UnitZ));
}
if (pAddDebugLine)
{
points.AddRange(CUtils.GetPointLine(furthestPoint2D, -Vector3.UnitZ));
}
if (ballBot != null)
{
points.Add((Vector3)ballBot);
if (pAddDebugLine)
{
points.AddRange(CUtils.GetPointLine((Vector3)ballBot, -Vector3.UnitZ));
}
}
if (IsValidMainPoint(furthestPointPlusX))
{
points.Add(furthestPointPlusX);
if (pAddDebugLine)
{
points.AddRange(CUtils.GetPointLine(furthestPointPlusX, Vector3.UnitX));
}
}
if (IsValidMainPoint(furthestPointMinusX))
{
points.Add(furthestPointMinusX);
if (pAddDebugLine)
{
points.AddRange(CUtils.GetPointLine(furthestPointMinusX, -Vector3.UnitX));
}
}
if (IsValidMainPoint(furthestPointPlusY))
{
points.Add(furthestPointPlusY);
if (pAddDebugLine)
{
points.AddRange(CUtils.GetPointLine(furthestPointPlusY, Vector3.UnitY));
}
}
if (IsValidMainPoint(furthestPointMinusY))
{
points.Add(furthestPointMinusY);
if (pAddDebugLine)
{
points.AddRange(CUtils.GetPointLine(furthestPointMinusY, -Vector3.UnitY));
}
}
return(points);
}
private float GetFurthestPointDist2D()
{
return(CUtils.Get2DDistance(ballTop, furthestPoint2D));
}
/// <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 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;
}
}
}
}
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 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 string Serialize()
{
return(CUtils.SerializeVector3(minBB) + " " + CUtils.SerializeVector3(maxBB));
}
private void ProcessUnassignedPoints()
{
DateTime debugStart = DateTime.Now;
DateTime previousDebugStart = DateTime.Now;
for (int i = 0; i < unassigned.Count; i++)
{
if (CProjectData.backgroundWorker.CancellationPending)
{
return;
}
CDebug.Progress(i, unassigned.Count, 100000, ref previousDebugStart, debugStart, "ProcessUnassignedPoints");
if (CProjectData.backgroundWorker.CancellationPending)
{
return;
}
Vector3 point = unassigned[i];
unassignedArray.AddPointInField(point);
if (CTreeManager.GetDetectMethod() == EDetectionMethod.Balls)
{
ballArray.AddPointInField(point);
}
}
if (CTreeManager.GetDetectMethod() == EDetectionMethod.Balls)
{
//unassignedArray.FillArray(); //doesnt make sense
const bool filterBasedOnheight = false;
//balls are expected to be in this height above ground
if (filterBasedOnheight)
{
ballArray.FilterPointsAtHeight(1.8f, 2.7f);
}
//add filtered points to detail array
List <Vector3> filteredPoints = ballArray.GetPoints();
foreach (Vector3 point in filteredPoints)
{
if (CProjectData.backgroundWorker.CancellationPending)
{
return;
}
ballDetailArray.AddPointInField(point);
}
List <CBallField> ballFields = new List <CBallField>();
//vege.Sort((b, a) => a.Z.CompareTo(b.Z)); //sort descending by height
List <CBallField> sortedFields = ballDetailArray.fields;
//sortedFields.Sort((a, b) => a.indexInField.Item1.CompareTo(b.indexInField.Item1));
//sortedFields.Sort((a, b) => a.indexInField.Item2.CompareTo(b.indexInField.Item2));
sortedFields.OrderBy(a => a.indexInField.Item1).ThenBy(a => a.indexInField.Item2);
//List<Vector3> mainPoints = new List<Vector3>();
debugStart = DateTime.Now;
previousDebugStart = DateTime.Now;
//process
for (int i = 0; i < sortedFields.Count; i++)
{
if (CProjectData.backgroundWorker.CancellationPending)
{
return;
}
CDebug.Progress(i, sortedFields.Count, 100000, ref previousDebugStart, debugStart, "Detecting balls");
CBallField field = (CBallField)sortedFields[i];
field.Detect();
if (field.ball != null && field.ball.isValid)
{
ballFields.Add(field);
ballsMainPoints.AddRange(field.ball.GetMainPoints(true));
ballsCenters.Add(field.ball.center);
ballsCenters.AddRange(CUtils.GetPointCross(field.ball.center));
//return;
ballsSurface.AddRange(field.ball.GetSurfacePoints());
}
}
foreach (CBallField field in ballFields)
{
ballPoints.AddRange(field.points);
}
}
}
/// <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;
}
/// <summary>
/// Calculates a rotation and translation that needs to be applied
/// on pSetA to match pSetB.
/// The process expects the matching points from sets having the same index.
/// </summary>
private static CRigidTransform CalculateRigidTransform(List <Vector3> pSetA, List <Vector3> pSetB)
{
//prevent modification of the input parameters
List <Vector3> setA = CUtils.GetCopy(pSetA);
List <Vector3> setB = CUtils.GetCopy(pSetB);
//setA and setB will be modified
List <Vector3> setAorig = CUtils.GetCopy(pSetA);
List <Vector3> setBorig = CUtils.GetCopy(pSetB);
Vector3 centroid_A = CUtils.GetAverage(setA);
Vector3 centroid_B = CUtils.GetAverage(setB);
CUtils.MovePointsBy(ref setA, -centroid_A);
CUtils.MovePointsBy(ref setB, -centroid_B);
Matrix mA = CreateMatrix(setA);
Matrix mB = CreateMatrix(setB);
Matrix A_multiply_B = mA.MultiplyTransposeBy(mB);
double[,] H = ConvertToDouble(A_multiply_B);
double[] w = new double[3];
double[,] u = new double[3, 3];
double[,] vt = new double[3, 3];
//U, S, Vt = linalg.svd(H)
//viz https://radfiz.org.ua/files/temp/Lab1_16/alglib-3.4.0.csharp/csharp/manual.csharp.html
const int u_needed = 2;
const int vt_needed = 2;
const int additional_memory = 2;
alglib.svd.rmatrixsvd(H, 3, 3, u_needed, vt_needed, additional_memory,
ref w, ref u, ref vt, null);
Matrix Vt = new Matrix(vt);
Matrix U = new Matrix(u);
Matrix rotation = Vt.MultiplyTransposeBy(U.Transpose(true));
//R = Vt.T * U.T
if (GetDeterminant(ConvertToDouble(rotation), 3) < 0)
{
//CDebug.Warning("Reflection detected");
//for(int i = 0; i < R.Rows; i++)
//{
// R[i, 2] *= -1; // NOT THE SAME!
//}
vt[2, 0] *= -1;
vt[2, 1] *= -1;
vt[2, 2] *= -1;
//Matrices need reinitialization - they are modified
Vt = new Matrix(vt);
U = new Matrix(u);
rotation = Vt.MultiplyTransposeBy(U.Transpose(true));
}
Matrix centerA =
new Matrix(new double[, ] {
{ centroid_A.X, centroid_A.Y, centroid_A.Z }
});
Matrix centerB =
new Matrix(new double[, ] {
{ centroid_B.X, centroid_B.Y, centroid_B.Z }
});
//create a copy - operations affects the original object
Matrix rotationCopy = new Matrix(rotation);
Matrix _R_mult_centerA = rotationCopy.Negate().MultiplyByTranspose(centerA);
Matrix translation = _R_mult_centerA + centerB.Transpose(true);
//CDebug.WriteLine("Rotation = ");
//CDebug.WriteLine("" + R);
//CDebug.WriteLine("Translation = ");
//CDebug.WriteLine("" + T);
float offset = GetOffset(setAorig, setBorig, rotation, translation);
return(new CRigidTransform(rotation, translation, offset));
}
