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