// Missing: SweptSpherePlaneOverlap
#region SegmentAABoxOverlap
/// <summary>
/// Indicates if a segment overlaps an AABox
/// </summary>
/// <param name="seg"></param>
/// <param name="AABox"></param>
/// <returns>bool</returns>
public static bool SegmentAABoxOverlap(Segment seg, AABox AABox)
{
Vector3 p0 = seg.Origin;
Vector3 p1 = seg.GetEnd();
float[] faceOffsets = new float[2];
// The AABox faces are aligned with the world directions. Loop
// over the 3 directions and do the two tests.
for (int iDir = 0; iDir < 3; iDir++)
{
int jDir = (iDir + 1) % 3;
int kDir = (iDir + 2) % 3;
// one plane goes through the origin, one is offset
faceOffsets[0] = JiggleUnsafe.Get(AABox.MinPos, iDir);
faceOffsets[1] = JiggleUnsafe.Get(AABox.MaxPos, iDir);
for (int iFace = 0; iFace < 2; iFace++)
{
// distance of each point from to the face plane
float dist0 = JiggleUnsafe.Get(ref p0, iDir) - faceOffsets[iFace];
float dist1 = JiggleUnsafe.Get(ref p1, iDir) - faceOffsets[iFace];
float frac = -1.0f;
if (dist0 * dist1 < -JiggleMath.Epsilon)
{
frac = -dist0 / (dist1 - dist0);
}
else if (System.Math.Abs(dist0) < JiggleMath.Epsilon)
{
frac = 0.0f;
}
else if (System.Math.Abs(dist1) < JiggleMath.Epsilon)
{
frac = 1.0f;
}
if (frac >= 0.0f)
{
//Assert(frac <= 1.0f);
Vector3 pt = seg.GetPoint(frac);
// check the point is within the face rectangle
if ((JiggleUnsafe.Get(ref pt, jDir) > JiggleUnsafe.Get(AABox.MinPos, jDir) - JiggleMath.Epsilon) &&
(JiggleUnsafe.Get(ref pt, jDir) < JiggleUnsafe.Get(AABox.MaxPos, jDir) + JiggleMath.Epsilon) &&
(JiggleUnsafe.Get(ref pt, kDir) > JiggleUnsafe.Get(AABox.MinPos, kDir) - JiggleMath.Epsilon) &&
(JiggleUnsafe.Get(ref pt, kDir) < JiggleUnsafe.Get(AABox.MaxPos, kDir) + JiggleMath.Epsilon))
{
return(true);
}
}
}
}
return(false);
}
public static bool SegmentAABoxOverlap(Segment seg, AABox AABox)
{
var p0 = seg.Origin;
var p1 = seg.GetEnd();
var faceOffsets = new float[2];
for (var iDir = 0; iDir < 3; iDir++)
{
var jDir = (iDir + 1) % 3;
var kDir = (iDir + 2) % 3;
faceOffsets[0] = JiggleUnsafe.Get(AABox.MinPos, iDir);
faceOffsets[1] = JiggleUnsafe.Get(AABox.MaxPos, iDir);
for (var iFace = 0; iFace < 2; iFace++)
{
var dist0 = JiggleUnsafe.Get(ref p0, iDir) - faceOffsets[iFace];
var dist1 = JiggleUnsafe.Get(ref p1, iDir) - faceOffsets[iFace];
var frac = -1.0f;
if (dist0 * dist1 < -JiggleMath.Epsilon)
{
frac = -dist0 / (dist1 - dist0);
}
else if (System.Math.Abs(dist0) < JiggleMath.Epsilon)
{
frac = 0.0f;
}
else if (System.Math.Abs(dist1) < JiggleMath.Epsilon)
{
frac = 1.0f;
}
if (frac >= 0.0f)
{
var pt = seg.GetPoint(frac);
if (JiggleUnsafe.Get(ref pt, jDir) > JiggleUnsafe.Get(AABox.MinPos, jDir) - JiggleMath.Epsilon && JiggleUnsafe.Get(ref pt, jDir) < JiggleUnsafe.Get(AABox.MaxPos, jDir) + JiggleMath.Epsilon && JiggleUnsafe.Get(ref pt, kDir) > JiggleUnsafe.Get(AABox.MinPos, kDir) - JiggleMath.Epsilon && JiggleUnsafe.Get(ref pt, kDir) < JiggleUnsafe.Get(AABox.MaxPos, kDir) + JiggleMath.Epsilon)
{
return(true);
}
}
}
}
return(false);
}
/// <summary>
/// Returns the vector representing the edge direction
/// </summary>
/// <param name="i"></param>
/// <returns>Vector3</returns>
public Vector3 GetSide(int i)
{
return JiggleUnsafe.Get(transform.Orientation, i) *
JiggleUnsafe.Get(ref sideLengths, i);
}
/// <summary>
/// Detect BoxBox Collisions.
/// </summary>
/// <param name="info"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param
public override void CollDetect(CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor)
{
Box box0 = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Box;
Box box1 = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Box;
Box oldBox0 = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Box;
Box oldBox1 = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Box;
Matrix dirs0 = box0.Orientation;
Matrix dirs1 = box1.Orientation;
seperatingAxes[0] = dirs0.Right;
seperatingAxes[1] = dirs0.Up;
seperatingAxes[2] = dirs0.Backward;
seperatingAxes[3] = dirs1.Right;
seperatingAxes[4] = dirs1.Up;
seperatingAxes[5] = dirs1.Backward;
Vector3.Cross(ref seperatingAxes[0], ref seperatingAxes[3], out seperatingAxes[6]);
Vector3.Cross(ref seperatingAxes[0], ref seperatingAxes[4], out seperatingAxes[7]);
Vector3.Cross(ref seperatingAxes[0], ref seperatingAxes[5], out seperatingAxes[8]);
Vector3.Cross(ref seperatingAxes[1], ref seperatingAxes[3], out seperatingAxes[9]);
Vector3.Cross(ref seperatingAxes[1], ref seperatingAxes[4], out seperatingAxes[10]);
Vector3.Cross(ref seperatingAxes[1], ref seperatingAxes[5], out seperatingAxes[11]);
Vector3.Cross(ref seperatingAxes[2], ref seperatingAxes[3], out seperatingAxes[12]);
Vector3.Cross(ref seperatingAxes[2], ref seperatingAxes[4], out seperatingAxes[13]);
Vector3.Cross(ref seperatingAxes[2], ref seperatingAxes[5], out seperatingAxes[14]);
// see if the boxes are separate along any axis, and if not keep a
// record of the depths along each axis
int i;
for (i = 0; i < 15; ++i)
{
// If we can't normalise the axis, skip it
float l2 = seperatingAxes[i].LengthSquared();
if (l2 < JiggleMath.Epsilon)
{
continue;
}
overlapDepth[i] = float.MaxValue;
if (Disjoint(out overlapDepth[i], ref seperatingAxes[i], box0, box1, collTolerance))
{
return;
}
}
// The box overlap, find the seperation depth closest to 0.
float minDepth = float.MaxValue;
int minAxis = -1;
for (i = 0; i < 15; ++i)
{
// If we can't normalise the axis, skip it
float l2 = seperatingAxes[i].LengthSquared();
if (l2 < JiggleMath.Epsilon)
{
continue;
}
// Normalise the separation axis and depth
float invl = 1.0f / (float)System.Math.Sqrt(l2);
seperatingAxes[i] *= invl;
overlapDepth[i] *= invl;
// If this axis is the minmum, select it
if (overlapDepth[i] < minDepth)
{
minDepth = overlapDepth[i];
minAxis = i;
}
}
if (minAxis == -1)
{
return;
}
// Make sure the axis is facing towards the 0th box.
// if not, invert it
Vector3 D = box1.GetCentre() - box0.GetCentre();
Vector3 N = seperatingAxes[minAxis];
float depth = overlapDepth[minAxis];
if (Vector3.Dot(D, N) > 0.0f)
{
N *= -1.0f;
}
float minA = MathHelper.Min(box0.SideLengths.X, MathHelper.Min(box0.SideLengths.Y, box0.SideLengths.Z));
float minB = MathHelper.Min(box1.SideLengths.X, MathHelper.Min(box1.SideLengths.Y, box1.SideLengths.Z));
float combinationDist = 0.05f * MathHelper.Min(minA, minB);
// the contact points
bool contactPointsFromOld = true;
contactPts.Clear();
if (depth > -JiggleMath.Epsilon)
{
GetBoxBoxIntersectionPoints(contactPts, oldBox0, oldBox1, combinationDist, collTolerance);
}
int numPts = contactPts.Count;
if (numPts == 0)
{
contactPointsFromOld = false;
GetBoxBoxIntersectionPoints(contactPts, box0, box1, combinationDist, collTolerance);
}
numPts = contactPts.Count;
Vector3 body0OldPos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1OldPos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
Vector3 body0NewPos = (info.Skin0.Owner != null) ? info.Skin0.Owner.Position : Vector3.Zero;
Vector3 body1NewPos = (info.Skin1.Owner != null) ? info.Skin1.Owner.Position : Vector3.Zero;
#region REFERENCE: Vector3 bodyDelta = body0NewPos - body0OldPos - body1NewPos + body1OldPos;
Vector3 bodyDelta;
Vector3.Subtract(ref body0NewPos, ref body0OldPos, out bodyDelta);
Vector3.Subtract(ref bodyDelta, ref body1NewPos, out bodyDelta);
Vector3.Add(ref bodyDelta, ref body1OldPos, out bodyDelta);
#endregion
#region REFERENCE: float bodyDeltaLen = Vector3.Dot(bodyDelta,N);
float bodyDeltaLen;
Vector3.Dot(ref bodyDelta, ref N, out bodyDeltaLen);
#endregion
float oldDepth = depth + bodyDeltaLen;
unsafe
{
#if USE_STACKALLOC
SmallCollPointInfo *collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI];
#else
SmallCollPointInfo[] collPtArray = SCPIStackAlloc();
fixed(SmallCollPointInfo *collPts = collPtArray)
#endif
{
int numCollPts = 0;
Vector3 SATPoint;
switch (minAxis)
{
// Box0 face, Box1 corner collision
case 0:
case 1:
case 2:
{
// Get the lowest point on the box1 along box1 normal
GetSupportPoint(out SATPoint, box1, -N);
break;
}
// We have a Box2 corner/Box1 face collision
case 3:
case 4:
case 5:
{
// Find with vertex on the triangle collided
GetSupportPoint(out SATPoint, box0, N);
break;
}
// We have an edge/edge collision
case 6:
case 7:
case 8:
case 9:
case 10:
case 11:
case 12:
case 13:
case 14:
{
{
// Retrieve which edges collided.
i = minAxis - 6;
int ia = i / 3;
int ib = i - ia * 3;
// find two P0, P1 point on both edges.
Vector3 P0, P1;
GetSupportPoint(out P0, box0, N);
GetSupportPoint(out P1, box1, -N);
// Find the edge intersection.
// plane along N and F, and passing through PB
Vector3 box0Orient, box1Orient;
JiggleUnsafe.Get(ref box0.transform.Orientation, ia, out box0Orient);
JiggleUnsafe.Get(ref box1.transform.Orientation, ib, out box1Orient);
#region REFERENCE: Vector3 planeNormal = Vector3.Cross(N, box1Orient[ib]);
Vector3 planeNormal;
Vector3.Cross(ref N, ref box1Orient, out planeNormal);
#endregion
#region REFERENCE: float planeD = Vector3.Dot(planeNormal, P1);
float planeD;
Vector3.Dot(ref planeNormal, ref P1, out planeD);
#endregion
// find the intersection t, where Pintersection = P0 + t*box edge dir
#region REFERENCE: float div = Vector3.Dot(box0Orient, planeNormal);
float div;
Vector3.Dot(ref box0Orient, ref planeNormal, out div);
#endregion
// plane and ray colinear, skip the intersection.
if (System.Math.Abs(div) < JiggleMath.Epsilon)
{
return;
}
float t = (planeD - Vector3.Dot(P0, planeNormal)) / div;
// point on edge of box0
#region REFERENCE: P0 += box0Orient * t;
P0 = Vector3.Add(Vector3.Multiply(box0Orient, t), P0);
#endregion
#region REFERENCE: SATPoint = (P0 + (0.5f * depth) * N);
Vector3.Multiply(ref N, 0.5f * depth, out SATPoint);
Vector3.Add(ref SATPoint, ref P0, out SATPoint);
#endregion
}
break;
}
default:
throw new Exception("Impossible switch");
}
// distribute the depth according to the distance to the SAT point
if (numPts > 0)
{
float minDist = float.MaxValue;
float maxDist = float.MinValue;
for (i = 0; i < numPts; ++i)
{
float dist = Distance.PointPointDistance(contactPts[i].Pos, SATPoint);
if (dist < minDist)
{
minDist = dist;
}
if (dist > maxDist)
{
maxDist = dist;
}
}
if (maxDist < minDist + JiggleMath.Epsilon)
{
maxDist = minDist + JiggleMath.Epsilon;
}
// got some intersection points
for (i = 0; i < numPts; ++i)
{
float minDepthScale = 0.0f;
float dist = Distance.PointPointDistance(contactPts[i].Pos, SATPoint);
float depthDiv = System.Math.Max(JiggleMath.Epsilon, maxDist - minDist);
float depthScale = (dist - minDist) / depthDiv;
depth = (1.0f - depthScale) * oldDepth + minDepthScale * depthScale * oldDepth;
if (contactPointsFromOld)
{
if (numCollPts < MaxLocalStackSCPI)
{
collPts[numCollPts++] = new SmallCollPointInfo(contactPts[i].Pos - body0OldPos, contactPts[i].Pos - body1OldPos, depth);
}
}
else
{
if (numCollPts < MaxLocalStackSCPI)
{
collPts[numCollPts++] = new SmallCollPointInfo(contactPts[i].Pos - body0NewPos, contactPts[i].Pos - body1NewPos, depth);
}
}
}
}
else
{
#region REFERENCE: collPts.Add(new CollPointInfo(SATPoint - body0NewPos, SATPoint - body1NewPos, oldDepth));
//collPts.Add(new CollPointInfo(SATPoint - body0NewPos, SATPoint - body1NewPos, oldDepth));
Vector3 cp0;
Vector3.Subtract(ref SATPoint, ref body0NewPos, out cp0);
Vector3 cp1;
Vector3.Subtract(ref SATPoint, ref body1NewPos, out cp1);
if (numCollPts < MaxLocalStackSCPI)
{
collPts[numCollPts++] = new SmallCollPointInfo(ref cp0, ref cp1, oldDepth);
}
#endregion
}
// report Collisions
collisionFunctor.CollisionNotify(ref info, ref N, collPts, numCollPts);
}
#if !USE_STACKALLOC
FreeStackAlloc(collPtArray);
#endif
}
}
/// <summary>
/// The AABox has a corner at the origin and size sides.
/// </summary>
private static int GetAABox2EdgeIntersectionPoints(List <ContactPoint> pts,
ref Vector3 sides, Box box, ref Vector3 edgePt0, ref Vector3 edgePt1,
ref Matrix origBoxOrient, ref Vector3 origBoxPos,
float combinationDistanceSq)
{
// The AABox faces are aligned with the world directions. Loop
// over the 3 directions and do the two tests. We know that the
// AABox has a corner at the origin
#region REFERENCE: Vector3 edgeDir = JiggleMath.NormalizeSafe(edgePt1 - edgePt0);
Vector3 edgeDir;
Vector3.Subtract(ref edgePt1, ref edgePt0, out edgeDir);
JiggleMath.NormalizeSafe(ref edgeDir);
#endregion
int num = 0;
for (int idir = 3; idir-- != 0;)
{
// skip edge/face tests if nearly parallel
if (System.Math.Abs(JiggleUnsafe.Get(ref edgeDir, idir)) < 0.1f)
{
continue;
}
int jdir = (idir + 1) % 3;
int kdir = (idir + 2) % 3;
for (int iface = 2; iface-- != 0;)
{
float offset = 0.0f;
if (iface == 1)
{
offset = JiggleUnsafe.Get(ref sides, idir);
}
float dist0 = JiggleUnsafe.Get(ref edgePt0, idir) - offset;
float dist1 = JiggleUnsafe.Get(ref edgePt1, idir) - offset;
float frac = -1.0f;
if (dist0 * dist1 < -JiggleMath.Epsilon)
{
frac = -dist0 / (dist1 - dist0);
}
else if (System.Math.Abs(dist0) < JiggleMath.Epsilon)
{
frac = 0.0f;
}
else if (System.Math.Abs(dist1) < JiggleMath.Epsilon)
{
frac = 1.0f;
}
if (frac >= 0.0f)
{
#region REFERENCE: Vector3 pt = (1.0f - frac) * edgePt0 + frac * edgePt1
Vector3 tmp; Vector3 pt;
Vector3.Multiply(ref edgePt1, frac, out tmp);
Vector3.Multiply(ref edgePt0, 1.0f - frac, out pt);
Vector3.Add(ref pt, ref tmp, out pt);
#endregion
// check the point is within the face rectangle
float ptJdir = JiggleUnsafe.Get(ref pt, jdir);
float ptKdir = JiggleUnsafe.Get(ref pt, kdir);
if ((ptJdir > -JiggleMath.Epsilon) &&
(ptJdir < JiggleUnsafe.Get(ref sides, jdir) + JiggleMath.Epsilon) &&
(ptKdir > -JiggleMath.Epsilon) &&
(ptKdir < JiggleUnsafe.Get(ref sides, kdir) + JiggleMath.Epsilon))
{
// woohoo got a point
#region REFERENCE: Vector3 pos = origBoxPos + Vector3.Transform(pt, origBoxOrient);
Vector3 pos;
Vector3.Transform(ref pt, ref origBoxOrient, out pos);
Vector3.Add(ref origBoxPos, ref pos, out pos);
#endregion
AddPoint(pts, ref pos, combinationDistanceSq);
if (++num == 2)
{
return(num);
}
}
}
}
}
return(num);
}
private static int GetAABox2EdgeIntersectionPoints(List <ContactPoint> pts, ref Vector3 sides, Box box, ref Vector3 edgePt0, ref Vector3 edgePt1, ref Matrix origBoxOrient, ref Vector3 origBoxPos, float combinationDistanceSq)
{
Vector3.Subtract(ref edgePt1, ref edgePt0, out var edgeDir);
JiggleMath.NormalizeSafe(ref edgeDir);
var num = 0;
var pt = new Vector3();
for (var idir = 3; idir-- != 0;)
{
if (System.Math.Abs(JiggleUnsafe.Get(ref edgeDir, idir)) < 0.1f)
{
continue;
}
var jdir = (idir + 1) % 3;
var kdir = (idir + 2) % 3;
for (var iface = 2; iface-- != 0;)
{
var offset = 0.0f;
if (iface == 1)
{
offset = JiggleUnsafe.Get(ref sides, idir);
}
var dist0 = JiggleUnsafe.Get(ref edgePt0, idir) - offset;
var dist1 = JiggleUnsafe.Get(ref edgePt1, idir) - offset;
var frac = -1.0f;
if (dist0 * dist1 < -JiggleMath.Epsilon)
{
frac = -dist0 / (dist1 - dist0);
}
else if (System.Math.Abs(dist0) < JiggleMath.Epsilon)
{
frac = 0.0f;
}
else if (System.Math.Abs(dist1) < JiggleMath.Epsilon)
{
frac = 1.0f;
}
if (frac >= 0.0f)
{
var tempFrac = 1.0f - frac;
pt.X = tempFrac * edgePt0.X + frac * edgePt1.X;
pt.Y = tempFrac * edgePt0.Y + frac * edgePt1.Y;
pt.Z = tempFrac * edgePt0.Z + frac * edgePt1.Z;
var ptJdir = JiggleUnsafe.Get(ref pt, jdir);
var ptKdir = JiggleUnsafe.Get(ref pt, kdir);
if (ptJdir > -JiggleMath.Epsilon && ptJdir < JiggleUnsafe.Get(ref sides, jdir) + JiggleMath.Epsilon && ptKdir > -JiggleMath.Epsilon && ptKdir < JiggleUnsafe.Get(ref sides, kdir) + JiggleMath.Epsilon)
{
Vector3.TransformNormal(ref pt, ref origBoxOrient, out var pos);
pos.X += origBoxPos.X;
pos.Y += origBoxPos.Y;
pos.Z += origBoxPos.Z;
AddPoint(pts, ref pos, combinationDistanceSq);
if (++num == 2)
{
return(num);
}
}
}
}
}
return(num);
}
/// <summary>
/// The AABox has a corner at the origin and size sides.
/// </summary>
/// <param name="pts"></param>
/// <param name="sides"></param>
/// <param name="box"></param>
/// <param name="edgePt0"></param>
/// <param name="edgePt1"></param>
/// <param name="origBoxOrient"></param>
/// <param name="origBoxPos"></param>
/// <param name="combinationDistanceSq"></param>
/// <returns>int</returns>
private static int GetAABox2EdgeIntersectionPoints(List <ContactPoint> pts,
ref Vector3 sides, Box box, ref Vector3 edgePt0, ref Vector3 edgePt1,
ref Matrix origBoxOrient, ref Vector3 origBoxPos,
float combinationDistanceSq)
{
// The AABox faces are aligned with the world directions. Loop
// over the 3 directions and do the two tests. We know that the
// AABox has a corner at the origin
#region REFERENCE: Vector3 edgeDir = JiggleMath.NormalizeSafe(edgePt1 - edgePt0);
Vector3 edgeDir;
Vector3.Subtract(ref edgePt1, ref edgePt0, out edgeDir);
JiggleMath.NormalizeSafe(ref edgeDir);
#endregion
int num = 0;
// BEN-OPTIMISATION: Reuse the one Vector3
Vector3 pt = new Vector3();
for (int idir = 3; idir-- != 0;)
{
// skip edge/face tests if nearly parallel
if (System.Math.Abs(JiggleUnsafe.Get(ref edgeDir, idir)) < 0.1f)
{
continue;
}
int jdir = (idir + 1) % 3;
int kdir = (idir + 2) % 3;
for (int iface = 2; iface-- != 0;)
{
float offset = 0.0f;
if (iface == 1)
{
offset = JiggleUnsafe.Get(ref sides, idir);
}
float dist0 = JiggleUnsafe.Get(ref edgePt0, idir) - offset;
float dist1 = JiggleUnsafe.Get(ref edgePt1, idir) - offset;
float frac = -1.0f;
if (dist0 * dist1 < -JiggleMath.Epsilon)
{
frac = -dist0 / (dist1 - dist0);
}
else if (System.Math.Abs(dist0) < JiggleMath.Epsilon)
{
frac = 0.0f;
}
else if (System.Math.Abs(dist1) < JiggleMath.Epsilon)
{
frac = 1.0f;
}
if (frac >= 0.0f)
{
#region REFERENCE: Vector3 pt = (1.0f - frac) * edgePt0 + frac * edgePt1
// BEN-OPTIMISATION: Reusing pt and inlined.
float tempFrac = 1.0f - frac;
pt.X = tempFrac * edgePt0.X + frac * edgePt1.X;
pt.Y = tempFrac * edgePt0.Y + frac * edgePt1.Y;
pt.Z = tempFrac * edgePt0.Z + frac * edgePt1.Z;
#endregion
// check the point is within the face rectangle
float ptJdir = JiggleUnsafe.Get(ref pt, jdir);
float ptKdir = JiggleUnsafe.Get(ref pt, kdir);
if ((ptJdir > -JiggleMath.Epsilon) &&
(ptJdir < JiggleUnsafe.Get(ref sides, jdir) + JiggleMath.Epsilon) &&
(ptKdir > -JiggleMath.Epsilon) &&
(ptKdir < JiggleUnsafe.Get(ref sides, kdir) + JiggleMath.Epsilon))
{
// woohoo got a point
#region REFERENCE: Vector3 pos = origBoxPos + Vector3.Transform(pt, origBoxOrient);
// BEN-OPTIMISATION: Inlined add because this function can be called very often!
Vector3 pos;
Vector3.TransformNormal(ref pt, ref origBoxOrient, out pos);
pos.X += origBoxPos.X;
pos.Y += origBoxPos.Y;
pos.Z += origBoxPos.Z;
#endregion
AddPoint(pts, ref pos, combinationDistanceSq);
if (++num == 2)
{
return(num);
}
}
}
}
}
return(num);
}
