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 < Simulator.Instance.getNumAgents(); ++i)
{
Vector2 goalVector = goals[i] - Simulator.Instance.getAgentPosition(i);
if (RVOMath.absSq(goalVector) > 1.0f)
{
goalVector = RVOMath.normalize(goalVector);
}
Simulator.Instance.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;
Simulator.Instance.setAgentPrefVelocity(i, Simulator.Instance.getAgentPrefVelocity(i) +
dist * new Vector2((float)Math.Cos(angle), (float)Math.Sin(angle)));
}
}
// Update is called once per frame
void Update()
{
int agentNum = Simulator.Instance.getNumAgents();
try
{
for (int i = 0; i < agentNum; i++)
{
RVO.Vector2 agentPos = Simulator.Instance.getAgentPosition(i);
RVO.Vector2 goalVector = rvoGameObjs[i].GetComponent <RVO_Agent>().nextPathNode() - agentPos;
if (RVOMath.absSq(goalVector) > thresholdToMove)
{
goalVector = RVOMath.normalize(goalVector) * speed_target;
}
Simulator.Instance.setAgentPrefVelocity(i, goalVector);
rvoGameObjs[i].transform.position = toUnityPosition(agentPos);
}
Simulator.Instance.doStep();
}
catch (System.Exception ex)
{
Debug.Log(ex.StackTrace);
}
}
// Update is called once per frame
void Update()
{
int agentNumber = Simulator.Instance.getNumAgents();
try
{
for (int i = 0; i < agentNumber; i++)
{
RVO.Vector2 agentLoc = Simulator.Instance.getAgentPosition(i);
RVO.Vector2 station = rvoGameObj[i].GetComponent <RVOAgent>().calculateNextStation() - agentLoc;
if (RVOMath.absSq(station) > 1.0f)
{
station = RVOMath.normalize(station);
}
Simulator.Instance.setAgentPrefVelocity(i, station);
agentPositions[i] = Simulator.Instance.getAgentPosition(i);
//Debug.Log(agentLoc + " /// " + agentPositions[i]);
}
Simulator.Instance.doStep();
}
catch (System.Exception ex) {
Debug.Log("Exeption: " + ex.Message);
}
}
// Update is called once per frame
void Update()
{
int agentNum = Simulator.Instance.getNumAgents();
Debug.Log("simulatorAir agent number = " + agentNum.ToString());
try
{
for (int i = 0; i < agentNum; i++)
{
RVO.Vector2 agentPos = Simulator.Instance.getAgentPosition(i);
RVO.Vector2 goalVector = rvoGameObjs[i].GetComponent <RVO_AirAgentMove>().goalVector();
if (RVOMath.absSq(goalVector) > thresholdToMove)
{
goalVector = goalVector * speed_target;
}
Simulator.Instance.setAgentPrefVelocity(i, goalVector);
rvoGameObjs[i].transform.position = toUnityPosition(agentPos);
}
Simulator.Instance.doStep();
}
catch (System.Exception ex)
{
Debug.Log(ex.StackTrace);
}
}
// Update is called once per frame
void FixedUpdate()
{
rallyIsReady = GameObject.FindGameObjectWithTag("Manager").GetComponent <UI_ButtonControl>().SpawnIsDone;
if (rallyIsReady)
{
int agentNum = Simulator.Instance.getNumAgents();
try
{
for (int i = 0; i < agentNum; i++)
{
RVO.Vector2 agentPos = Simulator.Instance.getAgentPosition(i);
RVO.Vector2 goalVector = rvoGameObjs[i].GetComponent <RVO_Agent>().nextPathNode() - agentPos;
if (RVOMath.absSq(goalVector) > thresholdToMove)
{
goalVector = RVOMath.normalize(goalVector) * speed_target;
}
Simulator.Instance.setAgentPrefVelocity(i, goalVector);
rvoGameObjs[i].transform.position = toUnityPosition(agentPos);
}
Simulator.Instance.doStep();
}
catch (System.Exception ex)
{
Debug.Log(ex.StackTrace);
}
}
}
// Update is called once per frame
void Update()
{
if (sid >= 0)
{
//更改前进的正方向
//position
Vector2 pos = Simulator.Instance.getAgentPosition(sid);
//direction
Vector2 vel = Simulator.Instance.getAgentPrefVelocity(sid);
//position
transform.position = new Vector3(pos.x, transform.position.y, pos.y);
//direction
if (Math.Abs(vel.x) > 0.01f && Math.Abs(vel.y) > 0.01f)
{
transform.forward = new Vector3(vel.x, 0, vel.y).normalized;
}
}
//if (Input.GetMouseButton(0))
//{
// Debug.LogError("GetMouseButton(0)+左击");
//}
//else if (Input.GetMouseButton(1))
//{
// Debug.LogError("GetMouseButton(1)+右击");
//}
if (!Input.GetMouseButton(1))
{
Simulator.Instance.setAgentPrefVelocity(sid, KInt2.zero);
return;
}
//点击鼠标的右键进入到以下的逻辑
//鼠标点位 和 角色的位置点
// 改进的vector2
KInt2 goalVector = GameMainManager.Instance.mousePosition - Simulator.Instance.getAgentPosition(sid);//GameMainManager.Instance.mousePosition
//归一化
if (RVOMath.absSq(goalVector) > 1)
{
goalVector = RVOMath.normalize(goalVector);
}
Simulator.Instance.setAgentPrefVelocity(sid, goalVector);
/* Perturb a little to avoid deadlocks due to perfect symmetry. */
float angle = (float)m_random.NextDouble() * 2.0f * (float)Math.PI;
float dist = (float)m_random.NextDouble() * 0.0001f;
Simulator.Instance.setAgentPrefVelocity(sid, Simulator.Instance.getAgentPrefVelocity(sid) + dist * new KInt2((float)Math.Cos(angle), (float)Math.Sin(angle)));
}
public Vector2 GetGoalVector(int agentId, Vector2 agentPos)
{
Vector2 goalVector = goals[agentId] - agentPos;
if (RVOMath.absSq(goalVector) > 1.0f)
{
goalVector = RVOMath.normalize(goalVector);
}
return(goalVector);
}
bool reachedGoal()
{
/* Check if all agents have reached their goals. */
for (int i = 0; i < Simulator.Instance.getNumAgents(); ++i)
{
if (RVOMath.absSq(Simulator.Instance.getAgentPosition(i) - goals[i]) > 400.0f)
{
return(false);
}
}
return(true);
}
void updateRVO()
{
if (corner >= agent.path.corners.Length)
{
return;
}
RVO.Vector2 agentLoc = Simulator.Instance.getAgentPosition(agentId);
RVO.Vector2 goalVector = toRVOVector(agent.path.corners[corner]) - agentLoc;
if (RVOMath.absSq(goalVector) > 1.0f)
{
goalVector = RVOMath.normalize(goalVector);
}
Simulator.Instance.setAgentPrefVelocity(agentId, goalVector);
}
private Vector2 getPreferredVelocities(int id, Vector2 location)
{
Vector2 goalVector = goals[id] - location;
if (RVOMath.absSq(goalVector) > 1.0f)
{
goalVector = RVOMath.normalize(goalVector);
}
float angle = (float)random.NextDouble() * 2.0f * (float)Math.PI;
float dist = (float)random.NextDouble() * 0.0001f;
/* Perturb a little to avoid deadlocks due to perfect symmetry. */
goalVector = goalVector + dist * new Vector2((float)Math.Cos(angle), (float)Math.Sin(angle));
return(goalVector);
}
// Update is called once per frame
void Update()
{
if (sid >= 0)
{
Vector2 pos = (Vector2)(VInt2)Simulator.Instance.getAgentPosition(sid);
Vector2 vel = (Vector2)(VInt2)Simulator.Instance.getAgentPrefVelocity(sid);
transform.position = new Vector3(pos.x, transform.position.y, pos.y);
if (Math.Abs(vel.x) > 0.01f && Math.Abs(vel.y) > 0.01f)
{
transform.forward = new Vector3(vel.x, 0, vel.y).normalized;
}
}
Simulator.Instance.setAgentPrefVelocity(sid, VInt2.zero);
//KInt2 goalVector = GameMainManager.Instance.mousePosition - Simulator.Instance.getAgentPosition(sid);//GameMainManager.Instance.mousePosition
VInt2 goalVector = (VInt2)targetpos - Simulator.Instance.getAgentPosition(sid);//GameMainManager.Instance.mousePosition
/*if (((VInt2) goalVector).sqrMagnitudeLong < 1000)
* {
* return;
* }*/
if (RVOMath.absSq((KInt2)goalVector) > 1)
{
goalVector = (VInt2)RVOMath.normalize((KInt2)goalVector);
}
else
{
return;
}
Simulator.Instance.setAgentPrefVelocity(sid, goalVector);
/* Perturb a little to avoid deadlocks due to perfect symmetry. */
/*float angle = (float) m_random.NextDouble()*2.0f*(float) Math.PI;
* float dist = (float) m_random.NextDouble()*0.0001f;
*
* Simulator.Instance.setAgentPrefVelocity(sid, Simulator.Instance.getAgentPrefVelocity(sid) +
* dist*
* new KInt2((float) Math.Cos(angle), (float) Math.Sin(angle)));*/
/*Simulator.Instance.setAgentPrefVelocity(sid, Simulator.Instance.getAgentPrefVelocity(sid) +
* (VInt2)(dist*
* new KInt2((float) Math.Cos(angle), (float) Math.Sin(angle))));*/
}
// Update is called once per frame
void Update()
{
for (int i = 0; i < Simulator.Instance.getNumAgents(); i++)
{
RVO.Vector2 agentLoc = Simulator.Instance.getAgentPosition(i);
RVO.Vector2 goalVector = toRVOVector(goals[i]) - agentLoc;
if (RVOMath.absSq(goalVector) > 1.0f)
{
goalVector = RVOMath.normalize(goalVector);
}
Simulator.Instance.setAgentPrefVelocity(i, goalVector);
RVOAgents[i].transform.localPosition = toUnityVector(agentLoc);
}
Simulator.Instance.doStep();
}
void SetPreferredVelocities()
{
/*
* Set the preferred velocity to be a vector of unit magnitude
* (speed) in the direction of the goal.
*/
Vector3 g = Vector3.zero;
for (int i = 0; i < Simulator.Instance.getNumAgents(); ++i)
{
RVO.Vector2 goalVector = goals[i] - Simulator.Instance.getAgentPosition(i);
if (RVOMath.absSq(goalVector) > 1.0f)
{
goalVector = RVOMath.normalize(goalVector);
}
Simulator.Instance.setAgentPrefVelocity(i, goalVector);
}
}
void setPreferredVelocities()
{
for (int i = 0; i < Simulator.Instance.getNumAgents(); ++i)
{
RVO.Vector2 goalVector = goals[i] - Simulator.Instance.getAgentPosition(i);
if (RVOMath.absSq(goalVector) > 1.0f)
{
goalVector = RVOMath.normalize(goalVector) * speed;
}
Simulator.Instance.setAgentPrefVelocity(i, goalVector);
float angle = (float)random.NextDouble() * 2.0f * (float)Math.PI;
float dist = (float)random.NextDouble() * 0.0001f;
Simulator.Instance.setAgentPrefVelocity(i, Simulator.Instance.getAgentPrefVelocity(i) +
dist * new RVO.Vector2((float)Math.Cos(angle), (float)Math.Sin(angle)));
}
}
// Update is called once per frame
void Update()
{
if (sid >= 0)
{
Vector3 pos = Simulator.Instance.getAgentPosition(sid);
Vector3 vel = Simulator.Instance.getAgentPrefVelocity(sid);
transform.position = new Vector3(pos.x, transform.position.y, pos.y);
if (Math.Abs(vel.x) > 0.01f && Math.Abs(vel.y) > 0.01f)
{
transform.forward = new Vector3(vel.x, 0, vel.y).normalized;
}
Debug.Log("Priority " + Simulator.Instance.getAgentPriority(sid));
if (Simulator.Instance.getAgentPriority(sid) > 1)
{
Debug.Log("Count " + Simulator.Instance.getAgentNumAgentNeighbors(sid));
}
}
if (!Input.GetMouseButton(1))
{
Simulator.Instance.setAgentPrefVelocity(sid, new Vector3(0, 0));
return;
}
Vector3 goalVector = GameMainManager.Instance.mousePosition - Simulator.Instance.getAgentPosition(sid);
if (RVOMath.absSq(goalVector) > 1.0f)
{
goalVector = RVOMath.normalize(goalVector) * Simulator.Instance.getAgentMaxSpeed(sid);
}
Simulator.Instance.setAgentPrefVelocity(sid, goalVector);
///* Perturb a little to avoid deadlocks due to perfect symmetry. */
//float angle = (float) mrandom.NextDouble()*2.0f*(float) Math.PI;
//float dist = (float) mrandom.NextDouble()*0.0001f;
//Simulator.Instance.setAgentPrefVelocity(sid, Simulator.Instance.getAgentPrefVelocity(sid) +
// dist*
// new Vector3((float) Math.Cos(angle), (float) Math.Sin(angle)));
}
public void Execute(int i)
{
var index = Indexes[i];
float2 agentLoc = Simulator.Instance.getAgentPosition(index);
SpawnAgentSystem.agents.TryGetValue(index, out var agent);
int l = waypoints[agent].Length;
if (l == 0)
{
commands.DestroyEntity(i, agent);
SpawnAgentSystem.agents.Remove(index);
Simulator.Instance.removeAgent(index);
return;
}
var next = GridGeneratorSystem.GridToWorldPos(waypoints[agent][l - 1].Value);
float2 goalVector = next - agentLoc;
if (RVOMath.absSq(goalVector) > 1.0f)
{
goalVector = math.normalize(goalVector);
}
Simulator.Instance.setAgentPrefVelocity(index, goalVector);
var newPos = new float3(agentLoc.x, 0.15f, agentLoc.y);
Positions[agent] = new Position {
Value = newPos
};
var dir = next - agentLoc;
// remove waypoint
if (dir.x < 0.1f && dir.y < 0.1f)
{
waypoints[agent].RemoveAt(l - 1);
}
}
/// <summary>
/// 根据agent当前位置与目的地计算设置最佳速度矢量，每次更新位置需要调用
/// </summary>
static 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 < Simulator.Instance.getNumAgents(); ++i)
{
RVO.Vector2 goalVector;
//给agent的属性赋值，方便计算path
RVO.Vector2 pos = Simulator.Instance.getAgentPosition(i);
_agents[i].positionNow = new Simulate.Vector2(pos.x_, pos.y_);
if (_agents[i].navPoints.Count > 0)
{
goalVector = new RVO.Vector2(_agents[i].navPoints[0].x_, _agents[i].navPoints[0].y_) - Simulator.Instance.getAgentPosition(i);
}
else
{
goalVector = new RVO.Vector2(0, 0);
}
if (RVOMath.absSq(goalVector) > 1.0f)
{
goalVector = RVOMath.normalize(goalVector);
}
//乘以一个系数让其接近人行走速度
goalVector *= 1.6f;//人快走的速度
Simulator.Instance.setAgentPrefVelocity(i, goalVector);
/* Perturb a little to avoid deadlocks due to perfect symmetry. */
float angle = (float)Simulate.MathHelper.random.NextDouble() * 2.0f * (float)Math.PI;
float dist = (float)Simulate.MathHelper.random.NextDouble() * 0.0001f;
Simulator.Instance.setAgentPrefVelocity(i, Simulator.Instance.getAgentPrefVelocity(i) +
dist * new RVO.Vector2((float)Math.Cos(angle), (float)Math.Sin(angle)));
}
}
// Update is called once per frame
void Update()
{
if (sid >= 0)
{
Vector2 pos = Simulator.Instance.getAgentPosition(sid);
Vector2 vel = Simulator.Instance.getAgentPrefVelocity(sid);
transform.position = new Vector3(pos.x(), transform.position.y, pos.y());
if (Math.Abs(vel.x()) > 0.01f && Math.Abs(vel.y()) > 0.01f)
{
transform.forward = new Vector3(vel.x(), 0, vel.y()).normalized;
}
}
if (!Input.GetMouseButton(1))
{
Simulator.Instance.setAgentPrefVelocity(sid, new Vector2(0, 0));
return;
}
Vector2 goalVector = GameMainManager.Instance.mousePosition - Simulator.Instance.getAgentPosition(sid);
//Debug.Log($"--vec1--{RVOMath.absSq(goalVector)}");
if (RVOMath.absSq(goalVector) > 1.0f)
{
goalVector = RVOMath.normalize(goalVector);
}
//Debug.Log($"--vec2--{RVOMath.absSq(goalVector)}");
Simulator.Instance.setAgentPrefVelocity(sid, goalVector);
/* Perturb a little to avoid deadlocks due to perfect symmetry. */
float angle = (float)m_random.NextDouble() * 2.0f * (float)Math.PI;
float dist = (float)m_random.NextDouble() * 0.0001f;
Simulator.Instance.setAgentPrefVelocity(sid, Simulator.Instance.getAgentPrefVelocity(sid) +
dist *
new Vector2((float)Math.Cos(angle), (float)Math.Sin(angle)));
}
// Update is called once per frame
void Update()
{
if (sid >= 0)
{
Vector2 pos = Simulator.Instance.getAgentPosition(sid);
Vector2 vel = Simulator.Instance.getAgentPrefVelocity(sid);
transform.position = new Vector3(pos.x, transform.position.y, pos.y);
/*if (Math.Abs(vel.x) > 0.01f && Math.Abs(vel.y) > 0.01f)
* transform.forward = new Vector3(vel.x, 0, vel.y).normalized;*/
}
if (!Input.GetMouseButton(1))
{
Simulator.Instance.setAgentPrefVelocity(sid, VInt2.zero);
return;
}
VInt2 goalVector = (VInt2)WWZTest.Instance.mousePosition - Simulator.Instance.getAgentPosition(sid);//GameMainManager.Instance.mousePosition
if (RVOMath.absSq((KInt2)goalVector) > 1)
{
goalVector = (VInt2)RVOMath.normalize((KInt2)goalVector);
}
Simulator.Instance.setAgentPrefVelocity(sid, goalVector);
/* Perturb a little to avoid deadlocks due to perfect symmetry. */
/*float angle = (float) m_random.NextDouble()*2.0f*(float) Math.PI;
* float dist = (float) m_random.NextDouble()*0.0001f;
*
* Simulator.Instance.setAgentPrefVelocity(sid, Simulator.Instance.getAgentPrefVelocity(sid) +
* dist*
* new KInt2((float) Math.Cos(angle), (float) Math.Sin(angle)));*/
}
void doStep()
{
/* Create agent ORCA lines. */
for (int i = 0; i < 1; ++i)
{
Vector2 relativePosition = otherPosition_ - position_;
Vector2 relativeVelocity = velocity_ - otherVelocity_;
float distSq = RVOMath.absSq(relativePosition);
float combinedRadius = radius_ + otherRadius_;
float combinedRadiusSq = RVOMath.sqr(combinedRadius);
Line line;
Vector2 u;
if (distSq > combinedRadiusSq)
{
/* No collision. */
Vector2 w = relativeVelocity - invTimeHorizon * relativePosition;
/* Vector from cutoff center to relative velocity. */
float wLengthSq = RVOMath.absSq(w);
float dotProduct1 = w * relativePosition;
//Debug.Log("w:" + w.ToString() + " dotProduct1:" + dotProduct1.ToString());
if (dotProduct1 < 0.0f && RVOMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq)
{
/* Project on cut-off circle. */
float wLength = RVOMath.sqrt(wLengthSq);
Vector2 unitW = w / wLength;
line.direction = new Vector2(unitW.y(), -unitW.x());
u = (combinedRadius * invTimeHorizon - wLength) * unitW;
}
else
{
/* Project on legs. */
float leg = RVOMath.sqrt(distSq - combinedRadiusSq);
if (RVOMath.det(relativePosition, w) > 0.0f)
{
/* Project on left leg. */
line.direction = new Vector2(relativePosition.x() * leg - relativePosition.y() * combinedRadius, relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
}
else
{
/* Project on right leg. */
line.direction = -new Vector2(relativePosition.x() * leg + relativePosition.y() * combinedRadius, -relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
}
float dotProduct2 = relativeVelocity * line.direction;
u = dotProduct2 * line.direction - relativeVelocity;
}
}
else
{
/* Collision. Project on cut-off circle of time timeStep. */
float invTimeStep = 1.0f / Simulator.Instance.timeStep_;
/* Vector from cutoff center to relative velocity. */
Vector2 w = relativeVelocity - invTimeStep * relativePosition;
float wLength = RVOMath.abs(w);
Vector2 unitW = w / wLength;
line.direction = new Vector2(unitW.y(), -unitW.x());
u = (combinedRadius * invTimeStep - wLength) * unitW;
}
line.point = velocity_ + 0.5f * u;
Debug.Log("u:" + u.ToString() + " Point:" + line.point.ToString() + " Direction:" + line.direction.ToString());
orcaLines_.Add(line);
}
float maxSpeed = 2.0f;
Vector2 prefVelocity = new Vector2(1, 1);
Vector2 newVelocity = new Vector2(0, 0);
linearProgram1(orcaLines_, 0, maxSpeed, prefVelocity, false, ref newVelocity);
//Debug.Log("prefVelocity:" + prefVelocity.ToString() + " newVelocity:" + newVelocity.ToString());
/*
* int lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, ref newVelocity_);
*
* if (lineFail < orcaLines_.Count)
* {
* linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, ref newVelocity_);
* }
*/
}
private bool linearProgram1(IList <Line> lines, int lineNo, float radius, Vector2 optVelocity, bool directionOpt, ref Vector2 result)
{
float dotProduct = lines[lineNo].point * lines[lineNo].direction;
float discriminant = RVOMath.sqr(dotProduct) + RVOMath.sqr(radius) - RVOMath.absSq(lines[lineNo].point);
if (discriminant < 0.0f)
{
/* Max speed circle fully invalidates line lineNo. */
return(false);
}
float sqrtDiscriminant = RVOMath.sqrt(discriminant);
float tLeft = -dotProduct - sqrtDiscriminant;
float tRight = -dotProduct + sqrtDiscriminant;
for (int i = 0; i < lineNo; ++i)
{
float denominator = RVOMath.det(lines[lineNo].direction, lines[i].direction);
float numerator = RVOMath.det(lines[i].direction, lines[lineNo].point - lines[i].point);
if (RVOMath.fabs(denominator) <= RVOMath.RVO_EPSILON)
{
/* Lines lineNo and i are (almost) parallel. */
if (numerator < 0.0f)
{
return(false);
}
continue;
}
float t = numerator / denominator;
if (denominator >= 0.0f)
{
/* Line i bounds line lineNo on the right. */
tRight = Math.Min(tRight, t);
}
else
{
/* Line i bounds line lineNo on the left. */
tLeft = Math.Max(tLeft, t);
}
if (tLeft > tRight)
{
return(false);
}
}
if (directionOpt)
{
/* Optimize direction. */
if (optVelocity * lines[lineNo].direction > 0.0f)
{
/* Take right extreme. */
result = lines[lineNo].point + tRight * lines[lineNo].direction;
}
else
{
/* Take left extreme. */
result = lines[lineNo].point + tLeft * lines[lineNo].direction;
}
}
else
{
/* Optimize closest point. */
float t = lines[lineNo].direction * (optVelocity - lines[lineNo].point);
//Debug.Log(tLeft.ToString() + " " + t.ToString() + " " + tRight.ToString());
if (t < tLeft)
{
result = lines[lineNo].point + tLeft * lines[lineNo].direction;
}
else if (t > tRight)
{
result = lines[lineNo].point + tRight * lines[lineNo].direction;
}
else
{
result = lines[lineNo].point + t * lines[lineNo].direction;
}
}
return(true);
}
// Update is called once per frame
void Update()
{
//寻路未完成则不执行移动
if (!isPathFound)
{
return;
}
//到最后一个寻路点之前，都会执行
if (curPathId < paths.Count)
{
if (sid >= 0)
{
VInt3 pos = (VInt3)Simulator.Instance.getAgentPosition(sid);
Vector2 vel = (Vector2)(VInt2)Simulator.Instance.getAgentPrefVelocity(sid);
/*if (PathfindingUtility.isCollide(transform.position, pos, out hit))
* {
* pos = hit.point;
* }*/
transform.position = new Vector3(pos.x / 1000f, transform.position.y, pos.z / 1000f);
if (Math.Abs(vel.x) > 0.01f && Math.Abs(vel.y) > 0.01f)
{
transform.forward = new Vector3(vel.x, 0, vel.y).normalized;
}
}
Simulator.Instance.setAgentPrefVelocity(sid, VInt2.zero);
//KInt2 goalVector = GameMainManager.Instance.mousePosition - Simulator.Instance.getAgentPosition(sid);//GameMainManager.Instance.mousePosition
VInt2 goalVector = (VInt2)targetpos - Simulator.Instance.getAgentPosition(sid);//GameMainManager.Instance.mousePosition
/*if (((VInt2) goalVector).sqrMagnitudeLong < 1000)
* {
* return;
* }*/
if (RVOMath.absSq((KInt2)goalVector) > 1)
{
goalVector = (VInt2)RVOMath.normalize((KInt2)goalVector);
}
else
{
//已经到达当前的寻路终点，将目标替换成下一个点，当前路径点id也+1
if (curPathId < paths.Count - 1)
{
curPathId++;
targetpos = paths[curPathId];
}
return;
}
Simulator.Instance.setAgentPrefVelocity(sid, goalVector);
/* Perturb a little to avoid deadlocks due to perfect symmetry. */
//float angle = (float) m_random.NextDouble()*2.0f*(float) Math.PI;
//float dist = (float) m_random.NextDouble()*0.0001f;
//Simulator.Instance.setAgentPrefVelocity(sid, Simulator.Instance.getAgentPrefVelocity(sid) + new VInt2((int) (Math.Cos(angle) * 1000), (int) (Math.Sin(angle)) * 1000) * (int)(dist * 1000));
/*Simulator.Instance.setAgentPrefVelocity(sid, Simulator.Instance.getAgentPrefVelocity(sid) +
* (VInt2)(dist*
* new KInt2((float) Math.Cos(angle), (float) Math.Sin(angle))));*/
}
}
void FixedUpdate()
{
if (stopSimulation)
{
foreach (GameObject agent in rvoGameObj)
{
Destroy(agent);
}
rvoGameObj.Clear();
agentPositions.Clear();
return;
}
int agentNUmber = Simulator.Instance.getNumAgents();
try
{
for (int i = 0; i < agentNUmber; i++)
{
RVO.Vector2 agentLoc = Simulator.Instance.getAgentPosition(i);
RVO.Vector2 station = rvoGameObj[i].GetComponent <RVOAgent>().calculateNextStation() - agentLoc;
Vector3 agent_lossyScale = rvoGameObj[i].transform.lossyScale;
if (RVOMath.absSq(station) > 1.0f)
{
station = RVOMath.normalize(station);
}
Simulator.Instance.setAgentPrefVelocity(i, station);
agentPositions[i] = Simulator.Instance.getAgentPosition(i);
if (isCovidMode)
{
if (!rvoGameObj[i].GetComponent <RVOAgent>().destinazioneRaggiunta())
{
Simulator.Instance.setAgentRadius(i, 0f);
Simulator.Instance.setAgentMaxSpeed(i, 0f);
Simulator.Instance.setAgentVelocity(i, new RVO.Vector2(0, 0));
Simulator.Instance.setAgentPrefVelocity(i, new RVO.Vector2(0, 0));
}
else
{
//..
//Simulator.Instance.setAgentDefaults( 2f, 15, 1.0f, 10.0f, getRadiusByDistancing( 1f ), 2.3f, new RVO.Vector2( 0.0f, 0.0f ) );
Simulator.Instance.setAgentRadius(i, getRadiusByDistancing(0.5f));
Simulator.Instance.setAgentMaxSpeed(i, 2.3f);
rvoGameObj[i].GetComponent <CapsuleCollider>().radius = 0.1f;
}
}
else
{
if (Simulator.Instance.getAgentNumAgentNeighbors(i) > 3)
{
Simulator.Instance.setAgentRadius(i, 0.3f);
rvoGameObj[i].GetComponent <CapsuleCollider>().radius = 0.1f;
}
else
{
Simulator.Instance.setAgentRadius(i, 0.56f);
rvoGameObj[i].GetComponent <CapsuleCollider>().radius = 0.3f;
}
}
}
Simulator.Instance.doStep();
}
catch (System.Exception ex)
{
//Debug.Log( ex.StackTrace );
}
}
// Update is called once per frame
void Update()
{
if (sid >= 0)
{
Vector3 pos = Simulator.Instance.getAgentPosition(sid);
Vector3 vel = Simulator.Instance.getAgentPrefVelocity(sid);
transform.position = new Vector3(pos.x, transform.position.y, pos.y);
if (Math.Abs(vel.x) > 0.01f && Math.Abs(vel.y) > 0.01f)
{
transform.forward = new Vector3(vel.x, 0, vel.y).normalized;
}
}
GetComponentInChildren <TextMesh>().text = "n: " + Simulator.Instance.getAgentNumAgentNeighbors(sid);
GetComponentInChildren <LineRenderer>().SetPositions(new Vector3[] { transform.position, goal });
Vector3 goalTf = new Vector3(goal.x, goal.z, goal.y);
Vector3 goalVec = goalTf - Simulator.Instance.getAgentPosition(sid);
if (RVOMath.absSq(goalVec) > 1.0f)
{
goalVec = RVOMath.normalize(goalVec);
}
Simulator.Instance.setAgentPrefVelocity(sid, goalVec * GameMainManager.Instance.AgentMaxSpeed);
float angle_ = (float)mrandom.NextDouble() * 2.0f * (float)Math.PI;
float dist_ = (float)mrandom.NextDouble() * 0.0001f;
Simulator.Instance.setAgentPrefVelocity(sid, Simulator.Instance.getAgentPrefVelocity(sid) +
dist_ *
new Vector3((float)Math.Cos(angle_), (float)Math.Sin(angle_)));
//foreach (var c in orcaObj.GetComponentsInChildren<LineRenderer>())
//{
// Destroy(c.gameObject);
//}
//var oLines = Simulator.Instance.getAgentOrcaLines(sid);
//foreach (Line line in oLines)
//{
// GameObject go = new GameObject();
// LineRenderer r = go.AddComponent<LineRenderer>();
// go.transform.parent = orcaObj.transform;
// var p1 = orcaObj.transform.position + line.point - line.direction * 20.0f;
// var p2 = orcaObj.transform.position + line.point + line.direction * 20.0f;
// p1.z = p1.y;
// p1.y = 0.0f;
// p2.z = p2.y;
// p2.y = 0.0f;
// r.SetPositions(new Vector3[] { p1, p2 });
//}
return;
if (!Input.GetMouseButton(1))
{
Simulator.Instance.setAgentPrefVelocity(sid, new Vector3(0, 0));
return;
}
Vector3 goalVector = GameMainManager.Instance.mousePosition - Simulator.Instance.getAgentPosition(sid);
if (RVOMath.absSq(goalVector) > 1.0f)
{
goalVector = RVOMath.normalize(goalVector);
}
Simulator.Instance.setAgentPrefVelocity(sid, goalVector);
/* Perturb a little to avoid deadlocks due to perfect symmetry. */
float angle = (float)mrandom.NextDouble() * 2.0f * (float)Math.PI;
float dist = (float)mrandom.NextDouble() * 0.0001f;
Simulator.Instance.setAgentPrefVelocity(sid, Simulator.Instance.getAgentPrefVelocity(sid) +
dist *
new Vector3((float)Math.Cos(angle), (float)Math.Sin(angle)));
}