/// <summary>
/// Prepare the obstacles for further calculations
/// </summary>
/// <param name="position">Current position</param>
/// <param name="desiredVel">Desired velocity</param>
public void Prepare(Vector3 position, Vector3 desiredVel)
{
//prepare obstacles
for (int i = 0; i < numCircles; i++)
{
//side
Vector3 pa = position;
Vector3 pb = circles[i].Position;
Vector3 orig = new Vector3(0, 0, 0);
circles[i].Dp = pb - pa;
circles[i].Dp.Normalize();
Vector3 dv = circles[i].DesiredVel - desiredVel;
float a = Triangle3.Area2D(orig, circles[i].Dp, dv);
if (a < 0.01f)
{
circles[i].Np.X = -circles[i].Dp.Z;
circles[i].Np.Z = circles[i].Dp.X;
}
else
{
circles[i].Np.X = circles[i].Dp.Z;
circles[i].Np.Z = -circles[i].Dp.X;
}
}
for (int i = 0; i < numSegments; i++)
{
//precalculate if the agent is close to the segment
float r = 0.01f;
float t;
segments[i].Touch = Distance.PointToSegment2DSquared(ref position, ref segments[i].P, ref segments[i].Q, out t) < (r * r);
}
}
public const int VERTS_PER_POLYGON = 6; //max number of vertices
/// <summary>
/// Generate an accurate sample of random points in the convex polygon and pick a point.
/// </summary>
/// <param name="pts">The polygon's points data</param>
/// <param name="s">A random float</param>
/// <param name="t">Another random float</param>
/// <param name="pt">The resulting point</param>
public static void RandomPointInConvexPoly(Vector3[] pts, float s, float t, out Vector3 pt)
{
//Calculate triangle areas
float[] areas = new float[pts.Length];
float areaSum = 0.0f;
float area;
for (int i = 2; i < pts.Length; i++)
{
Triangle3.Area2D(ref pts[0], ref pts[i - 1], ref pts[i], out area);
areaSum += Math.Max(0.001f, area);
areas[i] = area;
}
//Find sub triangle weighted by area
float threshold = s * areaSum;
float accumulatedArea = 0.0f;
float u = 0.0f;
int triangleVertex = 0;
for (int i = 2; i < pts.Length; i++)
{
float currentArea = areas[i];
if (threshold >= accumulatedArea && threshold < (accumulatedArea + currentArea))
{
u = (threshold - accumulatedArea) / currentArea;
triangleVertex = i;
break;
}
accumulatedArea += currentArea;
}
float v = (float)Math.Sqrt(t);
float a = 1 - v;
float b = (1 - u) * v;
float c = u * v;
if (triangleVertex > 0)
{
Vector3 pointA = pts[0];
Vector3 pointB = pts[triangleVertex - 1];
Vector3 pointC = pts[triangleVertex];
pt = a * pointA + b * pointB + c * pointC;
}
else
{
Console.WriteLine("triangleVertex<1");
Vector3 pointA = pts[0];
Vector3 pointB = pts[0];
Vector3 pointC = pts[0];
pt = a * pointA + b * pointB + c * pointC;
}
}
