float ttcTorus(ACA i, ACA j, out Vector3 offset) { float xDiff, yDiff; float t1, t2, t3, t4; Vector3 o1, o2, o3, o4; if (transform.position.x > worldSize / 2) { xDiff = worldSize; } else { xDiff = -worldSize; } if (transform.position.z > worldSize / 2) { yDiff = worldSize; } else { yDiff = -worldSize; } o1 = new Vector3(xDiff, 0, 0); o2 = new Vector3(0, 0, yDiff); o3 = new Vector3(xDiff, 0, yDiff); o4 = new Vector3(0, 0, 0); ttc(i, j, out t1, o1); ttc(i, j, out t2, o2); ttc(i, j, out t3, o3); ttc(i, j, out t4, o4); if (t1 < t2 && t1 < t3 && t1 < t4) { offset = o1; return(t1); } else if (t2 < t1 && t2 < t3 && t2 < t4) { offset = o2; return(t2); } else if (t3 < t1 && t3 < t2 && t3 < t4) { offset = o3; return(t3); } else { offset = o4; return(t4); } }
// Use this for initialization void Start() { goalVelocity = Random.onUnitSphere; goalVelocity.y = 0; goalVelocity.Normalize(); size = 1.0f; speed = 1.0f; tH = 2.0f; agents.Add(this); if (agents.Count == 1) { for (int x = 0; x < 24; x++) { ACA next = Instantiate(this); next.transform.position = new Vector3(Random.Range(0f, 20f), 0, Random.Range(0f, 20f)); next.name = "Agent " + (x + 1); } } }
void ttc(ACA i, ACA j, out float t, Vector3 offset) { // Ignore this parameter initially //offset = Vector3.zero; // Now compute the TTC and put the result in t t = Mathf.Infinity; // temporary value float r = (i.size / 2f) + (j.size / 2f); Vector3 w; w = j.transform.position - (i.transform.position - offset); float c = Vector3.Dot(w, w) - r * r; if (c < 0) { t = 0; return; } Vector3 v = i.velocity - j.velocity; float a = Vector3.Dot(v, v); float b = Vector3.Dot(w, v); float discr = b * b - a * c; if (discr <= 0) { return; } t = (b - Mathf.Sqrt(discr)) / a; if (t < 0) { t = Mathf.Infinity; return; } //F goal = (Vg - V) * K //Favoid = (tH - t / t) * K * ((x1 + tv1) - (x2 +tv2)).normalized }