void ComputNewVelocity() { m_debugDraw.ClearLine(m_hDraw); Vector3 dirWithDistance = destination - transform.position; Vector3 desirevelocity = dirWithDistance.normalized; // Vector3 relativPosition = m_neighbor.transform.position - transform.position; Vector3 relativeVelocity = velocity - m_neighbor.velocity; float relativeDistanceSq = relativPosition.sqrMagnitude; float combinedRadius = radius + m_neighbor.radius; float cosAngleSq = relativeDistanceSq / (relativeDistanceSq * relativeDistanceSq + combinedRadius * combinedRadius); // float cosVelocity = Vector3.Dot(desirevelocity, relativPosition); //for debugdraw m_debugDraw.DrawLine(m_hDraw, transform.position, m_neighbor.transform.position, Color.blue); velocity = desirevelocity * maxspeed; m_debugDraw.DrawLine(m_hDraw, transform.position, transform.position + velocity, Color.green); }
// Update is called once per frame void Update() { //RVO.Simulator.Instance.setTimeStep(Time.deltaTime); //if(RVO.Simulator.Instance.getNumAgents()== MAX_AGENT) RVO.Simulator.Instance.doStep(); foreach (var AgentItem in m_agentIdlist) { RVO.Vector2 pos = RVO.Simulator.Instance.getAgentPosition(AgentItem.Key); RVO.Vector2 vel = RVO.Simulator.Instance.getAgentVelocity(AgentItem.Key); Vector3 gmPos = AgentItem.Value.position; gmPos.x = pos.x_; gmPos.z = pos.y_; AgentItem.Value.position = gmPos; } for (int i = 0; i < m_obstacleList.Count; ++i) { //Simulator.Instance.getNextObstacleVertexNo(i); int hObs = i; RVO.Vector2 v0; RVO.Vector2 v1; Vector3 from; Vector3 to; from.y = to.y = 0.1f; do { int next = Simulator.Instance.getNextObstacleVertexNo(hObs); v0 = Simulator.Instance.getObstacleVertex(hObs); v1 = Simulator.Instance.getObstacleVertex(next); from.x = v0.x_; from.z = v0.y_; to.x = v1.x_; to.z = v1.y_; m_debugDraw.DrawLine(m_hDrawObstacle, from, to, Color.red); hObs = Simulator.Instance.getNextObstacleVertexNo(hObs); }while (hObs != i); } m_debugDraw.ClearLine(m_hDrawPointList); Vector3 lineFrom; Vector3 lineTo; lineFrom.y = lineTo.y = 0.1f; int pointCount = m_pointList.Count; for (int i = 0; i < pointCount; ++i) { RVO.Vector2 v0 = m_pointList[i]; RVO.Vector2 v1 = (i + 1) < pointCount ? m_pointList[i + 1] : m_pointList[0]; lineFrom.x = v0.x_; lineFrom.z = v0.y_; lineTo.x = v1.x_; lineTo.z = v1.y_; m_debugDraw.DrawLine(m_hDrawPointList, lineFrom, lineTo, Color.blue); } if (Input.GetMouseButtonDown(0)) { do { Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition); RaycastHit hit; if (!Physics.Raycast(ray, out hit)) { break; } if (m_mouseFunc == MouseFunc.AddAgent) { if (hit.collider.gameObject.layer != LayerMask.NameToLayer("Ground")) { break; } GameObject gmAgent = Instantiate(m_playerPrefabs, hit.point, Quaternion.identity) as GameObject; m_agentIdlist.Add(gmAgent.GetComponent <RVO2Agent>().RVOAgentHandle, gmAgent.transform); } if (m_mouseFunc == MouseFunc.AddObstacle) { RVO.Vector2 hitPos = new RVO.Vector2(hit.point.x, hit.point.z); if (m_pointList.Count > 2) { RVO.Vector2 last = m_pointList[m_pointList.Count - 1]; if (RVO.RVOMath.absSq(hitPos - last) <= 0.05f) { int hObstacle = RVO.Simulator.Instance.addObstacle(m_pointList); m_obstacleList.Add(hObstacle); m_pointList.Clear(); RVO.Simulator.Instance.processObstacles(); break; } } m_pointList.Add(hitPos); } if (m_mouseFunc == MouseFunc.MovAgent) { if (hit.collider.gameObject.layer == LayerMask.NameToLayer("Ground")) { if (m_selectedAgent) { m_selectedAgent.SetDestPos(hit.point); } break; } if (hit.collider.gameObject.layer == LayerMask.NameToLayer("Player")) { m_selectedAgent = hit.collider.gameObject.GetComponent <RVO2Agent>(); } } } while (false); } if (Input.GetMouseButton(0)) { Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition); RaycastHit hit; Physics.Raycast(ray, out hit); Vector3 MAX_VECTOR = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue); if (m_min == MAX_VECTOR) { m_min = hit.point; } m_max = hit.point; if (m_selectCollider.enabled == false && (m_max - m_min).sqrMagnitude > 1.0f) { m_selectCollider.enabled = true; } if (m_selectCollider != null) { m_selectCollider.center = Vector3.Lerp(m_min, m_max, 0.5f); Vector3 delta = m_max - m_min; m_selectCollider.size = new Vector3(Mathf.Abs(delta.x), 2.0f, Mathf.Abs(delta.z)); } } else { m_min = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue); m_max = new Vector3(float.MinValue, float.MinValue, float.MinValue); m_selectCollider.enabled = false; } }