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