/// <summary>
/// Adds the circle.
/// </summary>
/// <param name="position">The position.</param>
/// <param name="normal">The normal.</param>
/// <param name="radius">The radius.</param>
/// <param name="segments">The segments.</param>
public void AddCircle(Vector3 position, Vector3 normal, float radius, int segments)
{
if (segments < 3)
{
throw new ArgumentNullException("too few segments, at least 3");
}
normal.Normalize();
float sectionAngle = (float)(2.0 * Math.PI / segments);
var start = new Vector3(radius, 0.0f, 0.0f);
var current = new Vector3(radius, 0.0f, 0.0f);
var next = new Vector3(0.0f, 0.0f, 0.0f);
int posStart = positions.Count;
positions.Add(current);
int currIndex = posStart;
for (int i = 1; i < segments; i++)
{
next.X = radius * (float)Math.Cos(i * sectionAngle);
next.Z = radius * (float)Math.Sin(i * sectionAngle);
current = next;
positions.Add(current);
lineListIndices.Add(currIndex);
lineListIndices.Add(++currIndex);
}
lineListIndices.Add(currIndex);
lineListIndices.Add(posStart);
var axis = Vector3.Cross(Vector3.UnitY, normal);
var transform = Matrix.Translation(position);
if (axis.LengthSquared() > 1e-6)
{
axis.Normalize();
transform = Matrix.RotationAxis(axis, (float)Math.Acos(Vector3.Dot(Vector3.UnitY, normal))) * transform;
}
for (int i = posStart; i < positions.Count; ++i)
{
positions[i] = Vector3.TransformCoordinate(positions[i], transform);
}
}
/// <summary>
/// Source: http://geomalgorithms.com/a07-_distance.html
/// ~2341920 tests per second</summary>
/// <param name="s0"></param>
/// <param name="s1"></param>
/// <param name="t0"></param>
/// <param name="t1"></param>
/// <param name="sp"></param>
/// <param name="tp"></param>
/// <param name="sc"></param>
/// <param name="tc"></param>
/// <param name="sIsRay"></param>
/// <returns></returns>
public static float GetLineToLineDistance(
Vector3 s0, Vector3 s1, Vector3 t0, Vector3 t1, out Vector3 sp, out Vector3 tp, out float sc, out float tc, bool sIsRay = false)
{
Vector3 u = s1 - s0;
Vector3 v = t1 - t0;
Vector3 w = s0 - t0;
float a = Vector3.Dot(u, u); // always >= 0
float b = Vector3.Dot(u, v);
float c = Vector3.Dot(v, v); // always >= 0
float d = Vector3.Dot(u, w);
float e = Vector3.Dot(v, w);
float D = a * c - b * b; // always >= 0
float sN, sD = D; // sc = sN / sD, default sD = D >= 0
float tN, tD = D; // tc = tN / tD, default tD = D >= 0
// compute the line parameters of the two closest points
if (D < float.Epsilon)
{
// the lines are almost parallel
sN = 0.0f; // force using point P0 on segment S1
sD = 1.0f; // to prevent possible division by 0.0 later
tN = e;
tD = c;
}
else
{
// get the closest points on the infinite lines
sN = (b * e - c * d);
tN = (a * e - b * d);
if (!sIsRay)
{
if (sN < 0.0f)
{
// sc < 0 => the s=0 edge is visible
sN = 0.0f;
tN = e;
tD = c;
}
else if (sN > sD)
{
// sc > 1 => the s=1 edge is visible
sN = sD;
tN = e + b;
tD = c;
}
}
}
if (tN < 0.0f)
{
// tc < 0 => the t=0 edge is visible
tN = 0.0f;
// recompute sc for this edge
if (-d < 0.0f)
{
sN = 0.0f;
}
else if (-d > a)
{
sN = sD;
}
else
{
sN = -d;
sD = a;
}
}
else if (tN > tD)
{
// tc > 1 => the t=1 edge is visible
tN = tD;
// recompute sc for this edge
if ((-d + b) < 0.0f)
{
sN = 0;
}
else if ((-d + b) > a)
{
sN = sD;
}
else
{
sN = (-d + b);
sD = a;
}
}
// finally do the division to get sc and tc
sc = (Math.Abs(sN) < float.Epsilon ? 0.0f : sN / sD);
tc = (Math.Abs(tN) < float.Epsilon ? 0.0f : tN / tD);
// get the difference of the two closest points
sp = s0 + (sc * u);
tp = t0 + (tc * v);
var tv = sp - tp;
return(tv.Length()); // return the closest distance
}