Beispiel #1
0
        /// <summary>
        /// Computes the forces exerted on the agent at each simulation step
        /// Our simulation model includes the following three forces:
        /// A driving force F_i indicating the preference of the agent i
        /// to walk in a certain direction at a certain speed, as defined in
        /// [D. Helbing, I. Farkas, and T. Vicsek, Nature 407, 487 (2000)],
        ///
        /// F_i = (V_iPref - V_i) / Epsilon
        ///
        /// This force can be replaced by a self-propelled force to simulate agents with
        /// no preferred direction of motion following the approach of[D.Grossman,
        /// I.S.Aranson, and E.B.Jacob, New J.Phys. 10, 023036(2008).]
        ///
        /// The agent-agent interaction force F_ij derived in Eq. (S2)
        /// of the Supplemental material. A similar force F_iO acting
        /// on agent i as a result of interaction with each static obstacle O
        /// present in the environment.
        ///
        /// In our simulations, we generally assume that
        /// obstacles are modeled as a collection of line segments. Then,
        ///
        /// F_iO = -Delta_r * (K*T^(-2)*e^(-T/T_0))
        ///
        /// but now T denotes the minimal intersection time between the ray
        /// x_i + t*v_i, t>0 and the 2D capsule resulting after
        /// sweeping O with the disc of the agent.
        /// </summary>
        protected void ComputeForces()
        {
            //driving force
            F = (vPref - velocity) / ksi;

            // Compute new neighbors of agent;
            proximityNeighbors.Clear();
            proximityToken.FindNeighbors(ExtensionMethods.Vector2ToVector3(position), neighborDist, ref proximityNeighbors);

            // compute the anticipatory force from each neighbor
            for (int i = 0; i < proximityNeighbors.Count; i++)
            {
                Agent other = (Agent)proximityNeighbors[i];
                if (id == other.id)
                {
                    continue;
                }

                float distanceSq = (other.position - position).sqrMagnitude;
                float radiusSq   = Mathf.Sqrt(other.radius + radius);
                if (System.Math.Abs(distanceSq - radiusSq) > EPSILON)
                {
                    // if agents are actually colliding use their separation distance
                    if (distanceSq < radiusSq)
                    {
                        radiusSq = Mathf.Sqrt(other.radius + radius - Mathf.Sqrt(distanceSq));
                    }

                    Vector2 w     = other.position - position;
                    Vector2 v     = velocity - other.velocity;
                    float   a     = Vector2.Dot(v, v);
                    float   b     = Vector2.Dot(w, v);
                    float   c     = Vector2.Dot(w, w) - radiusSq;
                    float   discr = b * b - a * c;
                    if (discr > 0.0f && (a < -EPSILON || a > EPSILON))
                    {
                        discr = Mathf.Sqrt(discr);
                        float t = (b - discr) / a;
                        if (t > 0)
                        {
                            F += -k *Mathf.Exp(-t / t0) * (v - (b * v - a * w) / discr) / (a * Mathf.Pow(t, m)) * (m / t + 1 / t0);
                        }
                    }
                }
            }

            //anticipatory forces from static obstacles
            for (int i = 0; i < Engine.Instance.GetObstacles.Count; ++i)
            {
                LineObstacle obstacle = Engine.Instance.GetObstacle(i);
                Vector2      n_w      = Global.ClosestPointLineSegment(obstacle.P1, obstacle.P2, position) - position;
                float        d_w      = n_w.sqrMagnitude;

                // Agent is moving away from obstacle, already colliding or obstacle too far away
                if (Vector2.Dot(velocity, n_w) < 0 || System.Math.Abs(d_w - (Mathf.Sqrt(radius))) < EPSILON || d_w > Mathf.Sqrt(neighborDist))
                {
                    continue;
                }

                // correct the radius, if the agent is already colliding
                float r = d_w < Mathf.Sqrt(radius) ? Mathf.Sqrt(d_w) : radius;
                float a = Vector2.Dot(velocity, velocity);
                bool  discCollision = false, segmentCollision = false;
                float t_min = Mathf.Infinity;

                float   b = 1, discr = 1;
                float   b_temp, discr_temp, c_temp, D_temp;
                Vector2 w_temp, w = Vector2.zero, o1_temp, o2_temp, o_temp, o = Vector2.zero;

                // time-to-collision with disc_1 of the capped rectangle (capsule)
                w_temp     = obstacle.P1 - position;
                b_temp     = Vector2.Dot(w_temp, velocity);
                c_temp     = Vector2.Dot(w_temp, w_temp) - (r * r);
                discr_temp = b_temp * b_temp - a * c_temp;
                if (discr_temp > .0f && (a < -EPSILON || a > EPSILON))
                {
                    discr_temp = Mathf.Sqrt(discr_temp);
                    float t = (b_temp - discr_temp) / a;
                    if (t > 0)
                    {
                        t_min         = t;
                        b             = b_temp;
                        discr         = discr_temp;
                        w             = w_temp;
                        discCollision = true;
                    }
                }

                // time-to-collision with disc_2 of the capsule
                w_temp     = obstacle.P2 - position;
                b_temp     = Vector2.Dot(w_temp, velocity);
                c_temp     = Vector2.Dot(w_temp, w_temp) - (r * r);
                discr_temp = b_temp * b_temp - a * c_temp;
                if (discr_temp > 0 && (a < -EPSILON || a > EPSILON))
                {
                    discr_temp = Mathf.Sqrt(discr_temp);
                    float t = (b_temp - discr_temp) / a;
                    if (t > 0 && t < t_min)
                    {
                        t_min         = t;
                        b             = b_temp;
                        discr         = discr_temp;
                        w             = w_temp;
                        discCollision = true;
                    }
                }

                // time-to-collision with segment_1 of the capsule
                o1_temp = obstacle.P1 + r * obstacle.Normal;
                o2_temp = obstacle.P2 + r * obstacle.Normal;
                o_temp  = o2_temp - o1_temp;

                D_temp = Global.Det(velocity, o_temp);
                if (System.Math.Abs(D_temp) > EPSILON)
                {
                    float inverseDet = 1.0f / D_temp;
                    float t          = Global.Det(o_temp, position - o1_temp) * inverseDet;
                    float s          = Global.Det(velocity, position - o1_temp) * inverseDet;
                    if (t > 0 && s >= 0 && s <= 1 && t < t_min)
                    {
                        t_min            = t;
                        o                = o_temp;
                        discCollision    = false;
                        segmentCollision = true;
                    }
                }

                // time-to-collision with segment_2 of the capsule
                o1_temp = obstacle.P1 - r * obstacle.Normal;
                o2_temp = obstacle.P2 - r * obstacle.Normal;
                o_temp  = o2_temp - o1_temp;

                D_temp = Global.Det(velocity, o_temp);
                if (System.Math.Abs(D_temp) > EPSILON)
                {
                    float inverseDet = 1.0f / D_temp;
                    float t          = Global.Det(o_temp, position - o1_temp) * inverseDet;
                    float s          = Global.Det(velocity, position - o1_temp) * inverseDet;
                    if (t > 0 && s >= 0 && s <= 1 && t < t_min)
                    {
                        t_min            = t;
                        o                = o_temp;
                        discCollision    = false;
                        segmentCollision = true;
                    }
                }

                if (discCollision)
                {
                    F += -k *Mathf.Exp(-t_min / t0) * (velocity - (b * velocity - a * w) / discr) / (a * Mathf.Pow(t_min, m)) * (m / t_min + 1 / t0);
                }
                else if (segmentCollision)
                {
                    F += k * Mathf.Exp(-t_min / t0) / (Mathf.Pow(t_min, m) * Global.Det(velocity, o)) * (m / t_min + 1 / t0) * new Vector2(-o.y, o.x);
                }
            }
        }
Beispiel #2
0
        /// <summary>
        /// Add a new line obstacle to the simulation.
        /// </summary>
        /// <param name="start">The start point of the line segment</param>
        /// <param name="end">The end point of the line segment</param>
        public void AddObstacle(Vector2 start, Vector2 end)
        {
            LineObstacle l = new LineObstacle(start, end);

            obstacles.Add(l);
        }