コード例 #1
0
        public Vector3 ClampMovement(Vector3 direction)
        {
            Vector3 vector  = direction * this.delta;
            Vector3 vector2 = base.transform.position + direction;
            Vector3 vector3 = vector2;
            float   num     = 0f;
            int     num2    = 0;

            this.vos.Clear();
            float magnitude = this.velocity.magnitude;

            LocalAvoidance[] array = this.agents;
            for (int i = 0; i < array.Length; i++)
            {
                LocalAvoidance localAvoidance = array[i];
                if (!(localAvoidance == this) && !(localAvoidance == null))
                {
                    Vector3 vector4    = localAvoidance.transform.position - base.transform.position;
                    float   magnitude2 = vector4.magnitude;
                    float   num3       = this.radius + localAvoidance.radius;
                    if (magnitude2 <= vector.magnitude * this.delta + num3 + magnitude + localAvoidance.GetVelocity().magnitude)
                    {
                        if (num2 <= 50)
                        {
                            num2++;
                            LocalAvoidance.VO vO = new LocalAvoidance.VO();
                            vO.origin    = base.transform.position + Vector3.Lerp(this.velocity * this.delta, localAvoidance.GetVelocity() * this.delta, this.responability);
                            vO.direction = vector4.normalized;
                            if (num3 > vector4.magnitude)
                            {
                                vO.angle = 1.57079637f;
                            }
                            else
                            {
                                vO.angle = (float)Math.Asin((double)(num3 / magnitude2));
                            }
                            vO.limit = magnitude2 - num3;
                            if (vO.limit < 0f)
                            {
                                vO.origin += vO.direction * vO.limit;
                                vO.limit   = 0f;
                            }
                            float num4 = Mathf.Atan2(vO.direction.z, vO.direction.x);
                            vO.pRight = new Vector3(Mathf.Cos(num4 + vO.angle), 0f, Mathf.Sin(num4 + vO.angle));
                            vO.pLeft  = new Vector3(Mathf.Cos(num4 - vO.angle), 0f, Mathf.Sin(num4 - vO.angle));
                            vO.nLeft  = new Vector3(Mathf.Cos(num4 + vO.angle - 1.57079637f), 0f, Mathf.Sin(num4 + vO.angle - 1.57079637f));
                            vO.nRight = new Vector3(Mathf.Cos(num4 - vO.angle + 1.57079637f), 0f, Mathf.Sin(num4 - vO.angle + 1.57079637f));
                            this.vos.Add(vO);
                        }
                    }
                }
            }
            if (this.resType == LocalAvoidance.ResolutionType.Geometric)
            {
                for (int j = 0; j < this.vos.Count; j++)
                {
                    if (this.vos[j].Contains(vector3))
                    {
                        num = float.PositiveInfinity;
                        if (this.drawGizmos)
                        {
                            Debug.DrawRay(vector3, Vector3.down, Color.red);
                        }
                        vector3 = base.transform.position;
                        break;
                    }
                }
                if (this.drawGizmos)
                {
                    for (int k = 0; k < this.vos.Count; k++)
                    {
                        this.vos[k].Draw(Color.black);
                    }
                }
                if (num == 0f)
                {
                    return(vector);
                }
                List <LocalAvoidance.VOLine> list = new List <LocalAvoidance.VOLine>();
                for (int l = 0; l < this.vos.Count; l++)
                {
                    LocalAvoidance.VO vO2     = this.vos[l];
                    float             num5    = (float)Math.Atan2((double)vO2.direction.z, (double)vO2.direction.x);
                    Vector3           vector5 = vO2.origin + new Vector3((float)Math.Cos((double)(num5 + vO2.angle)), 0f, (float)Math.Sin((double)(num5 + vO2.angle))) * vO2.limit;
                    Vector3           vector6 = vO2.origin + new Vector3((float)Math.Cos((double)(num5 - vO2.angle)), 0f, (float)Math.Sin((double)(num5 - vO2.angle))) * vO2.limit;
                    Vector3           end     = vector5 + new Vector3((float)Math.Cos((double)(num5 + vO2.angle)), 0f, (float)Math.Sin((double)(num5 + vO2.angle))) * 100f;
                    Vector3           end2    = vector6 + new Vector3((float)Math.Cos((double)(num5 - vO2.angle)), 0f, (float)Math.Sin((double)(num5 - vO2.angle))) * 100f;
                    int num6 = (!Polygon.Left(vO2.origin, vO2.origin + vO2.direction, base.transform.position + this.velocity)) ? 2 : 1;
                    list.Add(new LocalAvoidance.VOLine(vO2, vector5, end, true, 1, num6 == 1));
                    list.Add(new LocalAvoidance.VOLine(vO2, vector6, end2, true, 2, num6 == 2));
                    list.Add(new LocalAvoidance.VOLine(vO2, vector5, vector6, false, 3, false));
                    bool flag  = false;
                    bool flag2 = false;
                    if (!flag)
                    {
                        for (int m = 0; m < this.vos.Count; m++)
                        {
                            if (m != l && this.vos[m].Contains(vector5))
                            {
                                flag = true;
                                break;
                            }
                        }
                    }
                    if (!flag2)
                    {
                        for (int n = 0; n < this.vos.Count; n++)
                        {
                            if (n != l && this.vos[n].Contains(vector6))
                            {
                                flag2 = true;
                                break;
                            }
                        }
                    }
                    vO2.AddInt(0f, flag, 1);
                    vO2.AddInt(0f, flag2, 2);
                    vO2.AddInt(0f, flag, 3);
                    vO2.AddInt(1f, flag2, 3);
                }
                for (int num7 = 0; num7 < list.Count; num7++)
                {
                    for (int num8 = num7 + 1; num8 < list.Count; num8++)
                    {
                        LocalAvoidance.VOLine vOLine  = list[num7];
                        LocalAvoidance.VOLine vOLine2 = list[num8];
                        if (vOLine.vo != vOLine2.vo)
                        {
                            float num9;
                            float num10;
                            if (Polygon.IntersectionFactor(vOLine.start, vOLine.end, vOLine2.start, vOLine2.end, out num9, out num10))
                            {
                                if (num9 >= 0f && num10 >= 0f && (vOLine.inf || num9 <= 1f) && (vOLine2.inf || num10 <= 1f))
                                {
                                    Vector3 p     = vOLine.start + (vOLine.end - vOLine.start) * num9;
                                    bool    flag3 = vOLine.wrongSide || vOLine2.wrongSide;
                                    if (!flag3)
                                    {
                                        for (int num11 = 0; num11 < this.vos.Count; num11++)
                                        {
                                            if (this.vos[num11] != vOLine.vo && this.vos[num11] != vOLine2.vo && this.vos[num11].Contains(p))
                                            {
                                                flag3 = true;
                                                break;
                                            }
                                        }
                                    }
                                    vOLine.vo.AddInt(num9, flag3, vOLine.id);
                                    vOLine2.vo.AddInt(num10, flag3, vOLine2.id);
                                    if (this.drawGizmos)
                                    {
                                        Debug.DrawRay(vOLine.start + (vOLine.end - vOLine.start) * num9, Vector3.up, (!flag3) ? Color.green : Color.magenta);
                                    }
                                }
                            }
                        }
                    }
                }
                for (int num12 = 0; num12 < this.vos.Count; num12++)
                {
                    Vector3 vector7;
                    if (this.vos[num12].FinalInts(vector2, base.transform.position + this.velocity, this.drawGizmos, out vector7))
                    {
                        float sqrMagnitude = (vector7 - vector2).sqrMagnitude;
                        if (sqrMagnitude < num)
                        {
                            vector3 = vector7;
                            num     = sqrMagnitude;
                            if (this.drawGizmos)
                            {
                                Debug.DrawLine(vector2 + Vector3.up, vector3 + Vector3.up, Color.red);
                            }
                        }
                    }
                }
                if (this.drawGizmos)
                {
                    Debug.DrawLine(vector2 + Vector3.up, vector3 + Vector3.up, Color.red);
                }
                return(Vector3.ClampMagnitude(vector3 - base.transform.position, vector.magnitude * this.maxSpeedScale));
            }
            else
            {
                if (this.resType == LocalAvoidance.ResolutionType.Sampled)
                {
                    Vector3 a          = vector;
                    Vector3 normalized = a.normalized;
                    Vector3 a2         = Vector3.Cross(normalized, Vector3.up);
                    int     num13      = 10;
                    int     num14      = 0;
                    while (num14 < 10)
                    {
                        float num15 = (float)(3.1415926535897931 * (double)this.circlePoint / (double)num13);
                        float num16 = (float)(3.1415926535897931 - (double)this.circlePoint * 3.1415926535897931) * 0.5f;
                        for (int num17 = 0; num17 < num13; num17++)
                        {
                            float   num18   = num15 * (float)num17;
                            Vector3 vector8 = base.transform.position + vector - (a * (float)Math.Sin((double)(num18 + num16)) * (float)num14 * this.circleScale + a2 * (float)Math.Cos((double)(num18 + num16)) * (float)num14 * this.circleScale);
                            if (this.CheckSample(vector8, this.vos))
                            {
                                return(vector8 - base.transform.position);
                            }
                        }
                        num14++;
                        num13 += 2;
                    }
                    for (int num19 = 0; num19 < this.samples.Length; num19++)
                    {
                        Vector3 vector9 = base.transform.position + this.samples[num19].x * a2 + this.samples[num19].z * normalized + this.samples[num19].y * a;
                        if (this.CheckSample(vector9, this.vos))
                        {
                            return(vector9 - base.transform.position);
                        }
                    }
                    return(Vector3.zero);
                }
                return(Vector3.zero);
            }
        }
コード例 #2
0
 // Use this for initialization
 void Start()
 {
     targetPoint = transform.forward * targetPointDist + transform.position;
     controller  = GetComponent <LocalAvoidance>();
 }