BspSegment GetOriginalSourceBspSegment(BspSegment segment)
{
var bspSegment = segment;
while (bspSegment.OriginalSegment != null)
{
bspSegment = bspSegment.OriginalSegment;
}
var collisionBspSegment = bspSegment as BspSegmentForCollsion;
if (collisionBspSegment == null)
{
throw new InvalidProgramException("不是BspSegmentForCollsion！");
}
return(collisionBspSegment.SourceSegment);
}
public static IEnumerable <BspSegmentForCollsion> CreateSegmentsForCollision(BspSegment segment, double range)
{
var startPoint = segment.LineSegment.StartPoint;
var endPoint = segment.LineSegment.EndPoint;
var direction = segment.LineSegment.Direction;
var perp = direction.GetPerpendicularVector().GetNormal() * range;
var topleft = startPoint + perp;
var topright = endPoint + perp;
var bottomleft = startPoint - perp;
var bottomright = endPoint - perp;
var topSegment = new BspSegmentForCollsion()
{
EntityId = segment.EntityId,
LineSegment = new LineSegment2d(topleft, topright),
SourceSegment = segment
};
var rightSegment = new BspSegmentForCollsion()
{
EntityId = segment.EntityId,
LineSegment = new LineSegment2d(topright, bottomright),
SourceSegment = segment
};
var bottomSegment = new BspSegmentForCollsion()
{
EntityId = segment.EntityId,
LineSegment = new LineSegment2d(bottomright, bottomleft),
SourceSegment = segment
};
var leftSegment = new BspSegmentForCollsion()
{
EntityId = segment.EntityId,
LineSegment = new LineSegment2d(bottomleft, topleft),
SourceSegment = segment
};
return(new BspSegmentForCollsion[]
{
topSegment,
rightSegment,
bottomSegment,
leftSegment
});
}
public CurveBspNode(BspSegment segment)
{
Segments = new BspSegment[] { segment };
}
private void SplitSegment(BspSegment sourceSegment, BspSegment targetSegment,
out BspSegment left, out BspSegment right, out BspSegment inline,
out double startVal, out double endVal)
{
left = null;
right = null;
inline = null;
startVal = 0.0;
endVal = 0.0;
var sourceStart = sourceSegment.LineSegment.StartPoint;
var sourceEnd = sourceSegment.LineSegment.EndPoint;
var targetStart = targetSegment.LineSegment.StartPoint;
var targetEnd = targetSegment.LineSegment.EndPoint;
// We need to make sure targetSegment is not a point
if (targetStart == targetEnd)
{
return;
}
// Allan: I found IsLeft is not good to determine whether a point is left or right
// if the segment is too long, so divide it by length.
var sourceLength = sourceSegment.LineSegment.Length;
var targetLength = targetSegment.LineSegment.Length;
var length = sourceLength + targetLength;
startVal = IsLeft(sourceStart, sourceEnd, targetStart) / length;
endVal = IsLeft(sourceStart, sourceEnd, targetEnd) / length;
//// For fixing some error by small values.
//using (var switcher = new SafeToleranceOverride(0.0005, 0.0005))
//{
// if (targetStart.IsEqualTo(sourceStart) || targetStart.IsEqualTo(sourceEnd))
// startVal = 0.0;
// if (targetEnd.IsEqualTo(sourceStart) || targetEnd.IsEqualTo(sourceEnd))
// endVal = 0.0;
//}
// Left
if (startVal.EqualsWithTolerance(0.0) && endVal.EqualsWithTolerance(0.0))
{
inline = targetSegment;
}
else if (startVal.Larger(0.0) && endVal.Larger(0.0))
{
left = targetSegment;
}
else if (startVal.Smaller(0.0) && endVal.Smaller(0.0))
{
right = targetSegment;
}
else if (startVal.EqualsWithTolerance(0.0))
{
var sourceParam = sourceSegment.LineSegment.GetParameterOf(targetStart);
var splitInfos = new List <BspSplitInfo>();
if (targetSegment.StartSplitInfos != null && targetSegment.StartSplitInfos.Any())
{
splitInfos.AddRange(targetSegment.StartSplitInfos);
}
splitInfos.Add(new BspSplitInfo()
{
IsSplitted = false,
SourceSegment = sourceSegment,
SourceParam = sourceParam
});
targetSegment.StartSplitInfos = splitInfos.ToArray();
if (endVal.Larger(0.0))
{
left = targetSegment;
}
else
{
right = targetSegment;
}
}
else if (endVal.EqualsWithTolerance(0.0))
{
var sourceParam = sourceSegment.LineSegment.GetParameterOf(targetEnd);
var splitInfos = new List <BspSplitInfo>();
if (targetSegment.EndSplitInfos != null && targetSegment.EndSplitInfos.Any())
{
splitInfos.AddRange(targetSegment.EndSplitInfos);
}
splitInfos.Add(new BspSplitInfo()
{
IsSplitted = false,
SourceSegment = sourceSegment,
SourceParam = sourceParam
});
targetSegment.EndSplitInfos = splitInfos.ToArray();
if (startVal.Larger(0.0))
{
left = targetSegment;
}
else
{
right = targetSegment;
}
}
else
{
// Split
var line2d = new Line2d(sourceStart, sourceEnd);
var points = line2d.IntersectWith(targetSegment.LineSegment);
if (points != null && points.Length > 0)
{
// Only one intersect point here.
var intersect = points[0];
var segmentStart = new LineSegment2d(targetStart, intersect);
var segmentEnd = new LineSegment2d(intersect, targetEnd);
// 如果segmentStart过短，就认为交点是targetStart。
// 否则在计算过程中就会矫枉过正，导致本不是相交点的地方成为相交点
if (segmentStart.Length.SmallerOrEqual(DoubleExtensions.STolerance))
{
if (endVal.EqualsWithTolerance(0.0))
{
inline = targetSegment;
}
else if (endVal.Larger(0.0))
{
left = targetSegment;
}
else if (endVal.Smaller(0.0))
{
right = targetSegment;
}
}
else if (segmentEnd.Length.SmallerOrEqual(DoubleExtensions.STolerance))
{
if (startVal.EqualsWithTolerance(0.0))
{
inline = targetSegment;
}
else if (startVal.Larger(0.0))
{
left = targetSegment;
}
else if (startVal.Smaller(0.0))
{
right = targetSegment;
}
}
else
{
var sourceParam = sourceSegment.LineSegment.GetParameterOf(intersect);
var splitInfo = new BspSplitInfo()
{
SourceSegment = sourceSegment,
SourceParam = sourceParam
};
BspSegment templeft = null;
if (segmentStart.Length.Larger(0.0))
{
templeft = new BspSegment()
{
LineSegment = segmentStart,
EntityId = targetSegment.EntityId,
StartSplitInfos = targetSegment.StartSplitInfos,
EndSplitInfos = new BspSplitInfo[] { splitInfo },
OriginalSegment = targetSegment
};
}
BspSegment tempRight = null;
if (segmentEnd.Length.Larger(0.0))
{
tempRight = new BspSegment()
{
LineSegment = segmentEnd,
EntityId = targetSegment.EntityId,
StartSplitInfos = new BspSplitInfo[] { splitInfo },
EndSplitInfos = targetSegment.EndSplitInfos,
OriginalSegment = targetSegment
};
}
if (startVal.LargerOrEqual(0.0))
{
left = templeft;
right = tempRight;
}
else
{
left = tempRight;
right = templeft;
}
}
}
else
{
// 如果没有任何交点，我们就认为它是inline的，因为往往因为计算精度，
// 导致startVal和endVal的值在0.001左右，但是用IntersectWith计算不出任何交点
inline = targetSegment;
}
}
}
private void UpdateSplitInfoForInlineSegments(BspSegment segment, List <BspSegment> inlineSegments)
{
// If the segment is not splited, just return.
if (segment.StartSplitInfos == null && segment.EndSplitInfos == null)
{
return;
}
bool start = false;
bool end = false;
var startSplitInfos = segment.StartSplitInfos;
if (startSplitInfos != null && startSplitInfos.Length > 0)
{
var param = inlineSegments[0].LineSegment.GetParameterOf(segment.LineSegment.StartPoint);
var info = startSplitInfos.FirstOrDefault(it => it.SourceSegment == inlineSegments[0] && it.SourceParam.EqualsWithTolerance(param));
if (info != null)
{
start = true;
}
}
if (!start)
{
var endSplitInfos = segment.EndSplitInfos;
if (endSplitInfos != null && endSplitInfos.Length > 0)
{
var param = inlineSegments[0].LineSegment.GetParameterOf(segment.LineSegment.EndPoint);
var info = endSplitInfos.FirstOrDefault(it => it.SourceSegment == inlineSegments[0] && it.SourceParam.EqualsWithTolerance(param));
if (info != null)
{
end = true;
}
}
}
if (!start && !end)
{
return;
}
// Set segment's splitinfo
var point = start ? segment.LineSegment.StartPoint : segment.LineSegment.EndPoint;
var splitInfos = new List <BspSplitInfo>();
for (int i = 1; i < inlineSegments.Count; i++)
{
// 需要判断是否在线上，因为GetParameterOf函数是不准确的。
// 另外，虽然共线，也是近似共线，所以要用IsOn
var isOn = inlineSegments[i].LineSegment.IsOn(point);
if (!isOn)
{
continue;
}
var param = inlineSegments[i].LineSegment.GetParameterOf(point);
splitInfos.Add(new BspSplitInfo()
{
SourceSegment = inlineSegments[i],
SourceParam = param
});
}
if (start)
{
splitInfos.InsertRange(0, segment.StartSplitInfos);
segment.StartSplitInfos = splitInfos.ToArray();
}
else
{
splitInfos.InsertRange(0, segment.EndSplitInfos);
segment.EndSplitInfos = splitInfos.ToArray();
}
}
private CurveBspNode Create(IEnumerable <BspSegment> segments, int depth, CurveBspNode parent, bool isLeft)
{
if (segments == null || !segments.Any())
{
return(null);
}
BspSegment segment = null;
// We need to make sure segment is not a point
foreach (var bspSegment in segments)
{
var startPoint = bspSegment.LineSegment.StartPoint;
var endPoint = bspSegment.LineSegment.EndPoint;
if (startPoint != endPoint)
{
segment = bspSegment;
break;
}
}
if (segment == null)
{
return(null);
}
var node = new CurveBspNode(segment);
node.Depth = depth;
node.Parent = parent;
node.IsLeft = isLeft;
// Left segments
var leftSegments = new List <BspSegment>();
// Right segments
var rightSegments = new List <BspSegment>();
// Inline segments
var inlineSegments = new List <BspSegment>();
inlineSegments.Add(segment);
var leftSegsStartOnSplitLine = new List <BspSegment>();
var leftSegsEndOnSplitLine = new List <BspSegment>();
var rightSegsStartOnSplitLine = new List <BspSegment>();
var rightSegsEndOnSplitLine = new List <BspSegment>();
foreach (var nextSegment in segments)
{
if (nextSegment == segment)
{
continue;
}
BspSegment left, right, inline;
double startVal, endVal;
SplitSegment(segment, nextSegment, out left, out right, out inline, out startVal, out endVal);
if (left != null)
{
leftSegments.Add(left);
}
if (right != null)
{
rightSegments.Add(right);
}
if (inline != null)
{
inlineSegments.Add(inline);
}
if (left != null && right == null)
{
if (startVal.EqualsWithTolerance(0.0))
{
leftSegsStartOnSplitLine.Add(left);
}
else if (endVal.EqualsWithTolerance(0.0))
{
leftSegsEndOnSplitLine.Add(left);
}
}
else if (left == null && right != null)
{
if (startVal.EqualsWithTolerance(0.0))
{
rightSegsStartOnSplitLine.Add(right);
}
else if (endVal.EqualsWithTolerance(0.0))
{
rightSegsEndOnSplitLine.Add(right);
}
}
}
// Update node's segment's information.
if (inlineSegments.Count > 1)
{
node.Segments = inlineSegments.ToArray();
// Update all left segments and right segment's split info
foreach (var leftSegment in leftSegments)
{
UpdateSplitInfoForInlineSegments(leftSegment, inlineSegments);
}
foreach (var rightSegment in rightSegments)
{
UpdateSplitInfoForInlineSegments(rightSegment, inlineSegments);
}
}
//
if (leftSegsStartOnSplitLine.Count > 0)
{
foreach (var bspSegment in leftSegsStartOnSplitLine)
{
var startPoint = bspSegment.LineSegment.StartPoint;
var rightStartSegs = rightSegsStartOnSplitLine.Where(
it => it.LineSegment.StartPoint.IsEqualTo(startPoint));
var rightEndSegs = rightSegsEndOnSplitLine.Where(
it => it.LineSegment.EndPoint.IsEqualTo(startPoint));
var splitInfos = new List <BspSplitInfo>();
foreach (var rightStartSeg in rightStartSegs)
{
splitInfos.Add(new BspSplitInfo()
{
IsSplitted = false,
SourceSegment = rightStartSeg,
SourceParam = 0.0
});
}
foreach (var rightEndSeg in rightEndSegs)
{
splitInfos.Add(new BspSplitInfo()
{
IsSplitted = false,
SourceSegment = rightEndSeg,
SourceParam = 1.0
});
}
if (splitInfos.Count > 0)
{
if (bspSegment.StartSplitInfos != null && bspSegment.StartSplitInfos.Length > 0)
{
splitInfos.InsertRange(0, bspSegment.StartSplitInfos);
}
bspSegment.StartSplitInfos = splitInfos.ToArray();
}
}
}
if (leftSegsEndOnSplitLine.Count > 0)
{
foreach (var bspSegment in leftSegsEndOnSplitLine)
{
var endPoint = bspSegment.LineSegment.EndPoint;
var rightStartSegs = rightSegsStartOnSplitLine.Where(
it => it.LineSegment.StartPoint.IsEqualTo(endPoint));
var rightEndSegs = rightSegsEndOnSplitLine.Where(
it => it.LineSegment.EndPoint.IsEqualTo(endPoint));
var splitInfos = new List <BspSplitInfo>();
foreach (var rightStartSeg in rightStartSegs)
{
splitInfos.Add(new BspSplitInfo()
{
IsSplitted = false,
SourceSegment = rightStartSeg,
SourceParam = 0.0
});
}
foreach (var rightEndSeg in rightEndSegs)
{
splitInfos.Add(new BspSplitInfo()
{
IsSplitted = false,
SourceSegment = rightEndSeg,
SourceParam = 1.0
});
}
if (splitInfos.Count > 0)
{
if (bspSegment.EndSplitInfos != null && bspSegment.EndSplitInfos.Length > 0)
{
splitInfos.InsertRange(0, bspSegment.EndSplitInfos);
}
bspSegment.EndSplitInfos = splitInfos.ToArray();
}
}
}
if (depth < this._parallelDepth)
{
// Parellel to improve performance.
System.Threading.Tasks.Parallel.Invoke(
() => node.LeftChild = Create(leftSegments, depth + 1, node, true),
() => node.RightChild = Create(rightSegments, depth + 1, node, false));
}
else
{
node.LeftChild = Create(leftSegments, depth + 1, node, true);
node.RightChild = Create(rightSegments, depth + 1, node, false);
}
return(node);
}
