public override void setupScenario()
{
// dépassement
RVO.Vector2 position = new RVO.Vector2(5, 5f);
// face à face
//RVO.Vector2 position = new RVO.Vector2(25, 5f);
sim_.addAgent(position, 0, false, false, 1f, 10, 2f, 2f, 0.35f, 1f, new RVO.Vector2(0, 0));
// dépassement
sim_.setAgentGoal(0, new RVO.Vector2(corridor_lenght * 1.5f, 5));
// face à face
//sim_.setAgentGoal(0, new RVO.Vector2(0,5));
position = new RVO.Vector2(0, 5.1f);
sim_.addAgent(position, 0, true, false, 1f, 10, 2f, 2f, 0.35f, 4f, new RVO.Vector2(0, 0));
sim_.setAgentGoal(1, new RVO.Vector2(corridor_lenght * 1.5f, 5.1f));
colors.Add(new Color(0.000f, 0.000f, 0.804f));
colors.Add(new Color(0.118f, 0.565f, 1.000f));
colors.Add(new Color(0.251f, 0.878f, 0.816f));
colors.Add(new Color(0.400f, 0.804f, 0.667f));
colors.Add(new Color(0.604f, 0.804f, 0.196f));
colors.Add(new Color(0.804f, 0.804f, 0.196f));
colors.Add(new Color(0.804f, 0.804f, 0.096f));
colors.Add(new Color(0.94f, 0.804f, 0.10f));
}
/** Used to set the velocity of all Agents considering their velocity and goal **/
public void setPreferredVelocities()
{
/*
* Set the preferred velocity to be a vector of unit magnitude
* (speed) in the direction of the goal.
*/
for (int i = 0; i < sim_.getNumAgents(); ++i)
{
RVO.Vector2 goalVector = sim_.getAgentGoal(i) - sim_.getAgentPosition(i);
if (RVO.Vector2.absSq(goalVector) > 1.0f)
{
goalVector = RVO.Vector2.normalize(goalVector);
}
sim_.setAgentPrefVelocity(i, goalVector);
/* Perturb a little to avoid deadlocks due to perfect symmetry. */
float angle = (float)random.NextDouble() * 2.0f * (float)Math.PI;
float dist = (float)random.NextDouble() * 0.0001f;
sim_.setAgentPrefVelocity(i, sim_.getAgentPrefVelocity(i) +
dist * new RVO.Vector2((float)Math.Cos(angle), (float)Math.Sin(angle)));
}
}
public void LoadMap()
{
m_mapMgr.Init(100, 100);
m_pathFinder.Init(1000, m_mapMgr);
// todo 初始化grid地图碰撞
m_agendIdGenerator += 1;
UnityEngine.GameObject obstacleGo = UnityEngine.GameObject.Find("Obstacle");
UnityEngine.GameObject obstaclePrefab = UnityEngine.Resources.Load("Barrier") as UnityEngine.GameObject;
Vector2i centerI = new Vector2i((int)obstacleGo.transform.position.x, (int)obstacleGo.transform.position.z);
int lengh = (int)obstacleGo.transform.localScale.x;
int width = (int)obstacleGo.transform.localScale.z;
List <Vector2i> obstacleGridList = new List <Vector2i>();
for (int i = -lengh / 2; i < lengh / 2; ++i)
{
for (int j = -width / 2; j < width / 2; ++j)
{
Vector2i iter = centerI + new Vector2i(i, j);
UnityEngine.GameObject.Instantiate(obstaclePrefab, new UnityEngine.Vector3(iter.m_x, -1, iter.m_y), UnityEngine.Quaternion.identity);
obstacleGridList.Add(iter);
}
}
m_mapMgr.RegisetrPosList(obstacleGridList, m_agendIdGenerator);
// todo 初始化rvo地图碰撞
// 格子寻路都为整数格，所以沟边尽量以整数中心计算占用格子数
RVO.Vector2 center = new RVO.Vector2(obstacleGo.transform.position.x, obstacleGo.transform.position.z);
RVO.Vector2 scale = new RVO.Vector2(obstacleGo.transform.localScale.x, obstacleGo.transform.localScale.z);
RVO.Vector2 point1 = center + new RVO.Vector2(-scale.x_ / 2, +scale.y_ / 2);
RVO.Vector2 point2 = center + new RVO.Vector2(+scale.x_ / 2, +scale.y_ / 2);
RVO.Vector2 point3 = center + new RVO.Vector2(+scale.x_ / 2, -scale.y_ / 2);
RVO.Vector2 point4 = center + new RVO.Vector2(-scale.x_ / 2, -scale.y_ / 2);
List <RVO.Vector2> obstacleList = new List <RVO.Vector2>();
obstacleList.Add(point1);
obstacleList.Add(point2);
obstacleList.Add(point3);
obstacleList.Add(point4);
RVO.Simulator.Instance.addObstacle(obstacleList);
RVO.Simulator.Instance.processObstacles();
}
private void setVelocity(int i, RVO.Vector2 vector2)
{
sim_.agents_[i].velocity_ = vector2;
}
// Update is called once per frame
void Update()
{
for (int block = 0; block < workers.Count; ++block)
{
doneEvents_[block].Reset();
ThreadPool.QueueUserWorkItem(workers[block].clear);
}
WaitHandle.WaitAll(doneEvents_);
updateWorker(workers.Count);
if (!reachedGoal())
{
if (hum_but_.isOn && !human_prefab)
{
int range = agents.Count;
for (int i = 0; i < range; i++)
{
Destroy(agents[i].gameObject);
addAgent(human, agents[i].position, sim_.getDefaultRadius(), i);
}
human_prefab = true;
}
else if (!hum_but_.isOn && human_prefab)
{
int range = agents.Count;
for (int i = 0; i < range; i++)
{
Destroy(agents[i].gameObject);
addAgent(prefab, agents[i].position, sim_.getDefaultRadius(), i);
}
human_prefab = false;
}
setAgentsProperties();
setPreferredVelocities();
sim_.initialize_virtual_and_agents();
for (int i = 0; i < getNumAgents(); i++)
{
Vector2 agent_position = sim_.getAgentPosition(i);
Vector2 p1 = agent_position + new Vector2(corridor_length_, 0);
sim_.addVirtualAgent(0, p1);
}
doStep(true);
/* Output the current global time. */
//print(Simulator.Instance.getGlobalTime());
if (follow_but_.isOn != follow_)
{
follow_ = follow_but_.isOn;
for (int i = 0; i < getNumAgents(); ++i)
{
sim_.agents_[i].follows_ = follow_;
}
}
if (save_but_.isOn != sim_.save)
{
sim_.save = save_but_.isOn;
}
Vector3 pos3 = Camera.main.transform.position;
Camera.main.transform.position = new Vector3(pos3.x, camera_height_.value, pos3.z);
int totot = getNumAgents();
for (int i = 0; i < getNumAgents(); ++i)
{
Vector2 position = sim_.getAgentPosition(i);
agents[i].transform.position = new Vector3(position.x(), 0f, position.y());
RVO.Vector2 vector2 = sim_.getAgentVelocity(i);
agents[i].rotation = Quaternion.LookRotation(new Vector3(vector2.x_, 0, vector2.y_));
if (human_prefab)
{
if (Vector2.absSq(sim_.getAgentVelocity(i) * 4) > 1.5f)
{
agents[i].GetComponent <Animator>().CrossFade("mixamo.com", 10, 1);
}
agents[i].GetComponent <Animator>().speed = Vector2.absSq(sim_.getAgentVelocity(i) * 4);
}
if (!human_prefab)
{
setColor(i);
}
}
}
else
{
for (int i = 0; i < getNumAgents(); ++i)
{
agents[i].transform.GetComponent <Rigidbody>().isKinematic = true;
}
}
for (int block = 0; block < workers.Count; ++block)
{
doneEvents_[block].Reset();
ThreadPool.QueueUserWorkItem(workers[block].computeMedVelocity);
}
WaitHandle.WaitAll(doneEvents_);
String tmp = "";
for (int i = 0; i < workers.Count; i++)
{
tmp += Vector2.abs(workers[i].vit_moy) + "\t";
}
using (TextWriter tw = new StreamWriter(name, true))
{
tw.WriteLine(tmp);
}
}
/** Change the position of the agent[i] in the simulation - Used to create corridor like scenario **/
internal void setPosition(int i, RVO.Vector2 vector2)
{
sim_.agents_[i].position_ = vector2;
}
