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 );
}
}
bool Mesh2Obstacle()
{
if (mesh.vertexCount == 0 || mesh.triangles.Length == 0)
{
LogMgr.LogError("Convert Error");
return(false);
}
BeginSample("Mesh2Obstacle");
//old
Vector3[] vectorVertices = mesh.vertices;
int[] triangles = mesh.triangles;
//create new
KInt3[] vertices = new KInt3[mesh.vertexCount];
Dictionary <KInt3, int> hashedVerts = new Dictionary <KInt3, int> (mesh.vertexCount);
int[] newVertices = new int[vertices.Length];
//从模型坐标系转换到世界坐标系
for (int i = 0; i < vertices.Length; i++)
{
vertices [i] = (KInt3)MeshMatrix.MultiplyPoint3x4(vectorVertices [i]);
ShowProgress("从模型坐标系转换到世界坐标系", (float)i / vertices.Length);
}
//旧顶点在新顶点中的索引
int newVerIdx = 0;
for (int i = 0; i < vertices.Length; i++)
{
//当hash 顶点集合不包含这个顶点的时候，把这个点加入进去,去除共点
if (!hashedVerts.ContainsKey(vertices [i]))
{
newVertices [newVerIdx] = i;
hashedVerts.Add(vertices [i], newVerIdx);
newVerIdx++;
}
ShowProgress("构建映射", (float)i / vertices.Length);
}
for (int x = 0; x < triangles.Length; x++)
{
//把老的顶点索引映射到新的顶点索引上
KInt3 vertex = vertices [triangles [x]];
triangles [x] = hashedVerts [vertex];
ShowProgress("三角映射", (float)x / triangles.Length);
}
KInt3[] totalIntVertices = vertices;
vertices = new KInt3[newVerIdx];
for (int i = 0; i < newVerIdx; i++)
{
vertices [i] = totalIntVertices [newVertices [i]];
ShowProgress("顶点索引转换", (float)i / newVerIdx);
}
//顶点创建结束
#if UNITY_EDITOR
this.ObsVertices = vertices;
#endif
if (triangles.Length % 3 != 0)
{
LogMgr.LogErrorFormat("triangle lenth error :{0}", triangles.Length);
}
//创建三角
ObsTriangles = new NavmeshTriangle[triangles.Length / 3];
for (int i = 0; i < ObsTriangles.Length; i++)
{
NavmeshTriangle tri = new NavmeshTriangle();
ObsTriangles [i] = tri;
#if UNITY_EDITOR
//init
tri.uid = i;
#endif
//
tri.v0 = triangles [i * 3];
tri.v1 = triangles [i * 3 + 1];
tri.v2 = triangles [i * 3 + 2];
if (RVOMath.IsClockwiseXZ(vertices [tri.v0], vertices [tri.v1], vertices [tri.v2]) == false)
{
int tmp = tri.v0;
tri.v0 = tri.v2;
tri.v2 = tmp;
}
//finish position
tri.v03 = vertices [tri.v0];
tri.v13 = vertices [tri.v1];
tri.v23 = vertices [tri.v2];
tri.v03xz = new KInt2(tri.v03.x, tri.v03.z);
tri.v13xz = new KInt2(tri.v13.x, tri.v13.z);
tri.v23xz = new KInt2(tri.v23.x, tri.v23.z);
tri.averagePos = (tri.v03 + tri.v13 + tri.v23) / 3;
if (this.ExactMode >= ExactType.TWO)
{
tri.xzPos = new KInt2(tri.averagePos.x, tri.averagePos.z);
if (point2triangle.ContainsKey(tri.xzPos))
{
point2triangle [tri.xzPos].Add(tri);
}
else
{
point2triangle [tri.xzPos] = new List <NavmeshTriangle> ();
point2triangle [tri.xzPos].Add(tri);
}
}
ShowProgress("构建三角形", (float)i / ObsTriangles.Length);
}
Dictionary <KInt2, NavmeshTriangle> sides = new Dictionary <KInt2, NavmeshTriangle> (triangles.Length * 3);
//创建三角形的边
for (int i = 0, j = 0; i < triangles.Length; j += 1, i += 3)
{
sides [KInt2.ToInt2(triangles [i + 0], triangles [i + 1])] = ObsTriangles [j];
sides [KInt2.ToInt2(triangles [i + 1], triangles [i + 2])] = ObsTriangles [j];
sides [KInt2.ToInt2(triangles [i + 2], triangles [i + 0])] = ObsTriangles [j];
}
HashSet <NavmeshTriangle> connections = new HashSet <NavmeshTriangle> ();
for (int i = 0, j = 0; i < triangles.Length; j += 1, i += 3)
{
connections.Clear();
NavmeshTriangle node = ObsTriangles [j];
for (int q = 0; q < 3; q++)
{
NavmeshTriangle other;
//如果是mesh中的边，则加入
if (sides.TryGetValue(KInt2.ToInt2(triangles [i + ((q + 1) % 3)], triangles [i + q]), out other))
{
connections.Add(other);
}
}
//拷贝当前的一份连接点，而不是赋值引用过去
node.connections = new List <NavmeshTriangle> ();
node.connections.AddRange(connections);
node.CreateSharedInfo();
ShowProgress("构建连接点", (float)i / (triangles.Length / 3));
}
ClearProgressBar();
EndSample();
return(true);
}
// 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)));
}
void Update()
{
if (isOk && agentIndex != -1)
{
/* dist */
transform.position = RVOWithUnity.Vec2ToVec3(Simulator.Instance.getAgentPosition(agentIndex));
/* turn */
if (RVOMath.abs(Simulator.Instance.getAgentVelocity(agentIndex)) > 0.0f)
{
Vector3 direction = RVOWithUnity.Vec2ToVec3(Simulator.Instance.getAgentVelocity(agentIndex));
float angle = Vector3.Angle(Vector3.right, direction);
if (Vector3.Cross(Vector3.right, direction).y < 0)
{
angle = 360 - angle;
}
transform.rotation = Quaternion.Euler(new Vector3(transform.rotation.x, angle, transform.rotation.z));
}
if (!isStop)
{
/* Velocity */
velocity = Simulator.Instance.getAgentVelocity(agentIndex);
velocityVec3 = RVOWithUnity.Vec2ToVec3(velocity);
speed = RVOMath.abs(velocity);
/* Update Station */
station = pathNodes[nowNode];
Vector2 stationVector2 = RVOWithUnity.Vec3ToVec2(station);
Vector2 transformVector2 = RVOWithUnity.Vec3ToVec2(transform.position);
Vector2 distVector2 = stationVector2 - transformVector2;
Vector2 lastStationVector2 = RVOWithUnity.Vec3ToVec2(pathNodes[nowNode - 1]);
Vector2 lastToNowNormalize = RVOMath.normalize(stationVector2 - lastStationVector2);
Vector2 verticalDistVector2 = new Vector2(-lastToNowNormalize.y_, lastToNowNormalize.x_);
//turnDist = (speed / angularSpeed) / changeStation;
if (RVOWithUnity.isTwoSide(verticalDistVector2, distVector2, lastToNowNormalize))
{
nowNode++;
slowStationNum = Mathf.Max(0, slowStationNum - 1);
//Debug.Log(RVOWithUnity.Vec3ToVec2(pathNodes[nowNode]));
if (nowNode > pathNodes.Count - 1)
{
isStop = true;
prefSpeed = 0.0f;
}
else
{
station = pathNodes[nowNode];
}
if (slowStationNum == 0)
{
prefSpeed = maxSpeed;
}
}
else
{
float slowDist = Mathf.Abs(speed * speed - minSpeedToTurn * minSpeedToTurn) / (acceleration * slowStation * 2);
if (RVOMath.abs(distVector2) >= slowDist && slowStationNum == 0)
{
prefSpeed = maxSpeed;
}
if (RVOMath.abs(distVector2) < slowDist)
{
/* Calc the slowScale */
if (nowNode + 1 > pathNodes.Count - 1)
{
prefSpeed = minSpeedToTurn;
turnDist = 0.1f;
}
else
{
int nextTurnNode = nowNode + 1;
Vector2 nextStation = RVOWithUnity.Vec3ToVec2(pathNodes[nextTurnNode]);
Vector2 lastStation = stationVector2;
Vector2 nextDirection = nextStation - lastStation;
float turnAngleDist = Vector3.Angle(velocityVec3, RVOWithUnity.Vec2ToVec3(nextDirection));
if (nowNode + 1 <= pathNodes.Count - 1 && slowStationNum == 0)
{
slowStationNum++;
float turnAngle = turnAngleDist;
//if (gameObject.name == "boat")
// Debug.Log(lastStation + "/|/" + nextStation + "/|/" + speed * ignoreStepFactor);
while (RVOMath.abs(nextDirection) < speed * ignoreStepFactor && nextTurnNode + 1 <= pathNodes.Count - 1)
{
nextTurnNode++;
slowStationNum++;
lastStation = nextStation;
nextStation = RVOWithUnity.Vec3ToVec2(pathNodes[nextTurnNode]);
Vector2 lastDirection = nextDirection;
nextDirection = nextStation - lastStation;
//Debug.Log(nextStation + " " + lastStation);
turnAngle += Vector3.Angle(RVOWithUnity.Vec2ToVec3(lastDirection), RVOWithUnity.Vec2ToVec3(nextDirection));
}
prefSpeed = minSpeedToTurn + (maxSpeed - minSpeedToTurn) * Mathf.Max(turningFactor - turnAngle, turningFactor / 10.0f) / turningFactor;
}
turnDist = Mathf.Tan(turnAngleDist * Mathf.Deg2Rad / 2) * (prefSpeed * prefSpeed / angularSpeed) / 200.0f;
}
}
if (RVOMath.abs(distVector2) < turnDist)
{
nowNode++;
slowStationNum = Mathf.Max(0, slowStationNum - 1);
if (nowNode > pathNodes.Count - 1)
{
isStop = true;
prefSpeed = 0.0f;
}
else
{
station = pathNodes[nowNode];
}
}
}
}
}
}
private void Update()
{
if (inited == false)
{
return;
}
//if ((Time.time - lastTime) > 0.25f)
//{
// lastTime = Time.time;
// for (int i = 0; i < goals.Count; i++)
// {
// goals[i] += dirs[i] * 0.8f;
// Debug.DrawLine(goals[i].ToVec3XZ(),goals[i].ToVec3XZ()+Vector3.up*5f,Color.cyan,0.25f);
// }
//}
if (ReachedGoal() == false)
{
SetPreferredVelocities();
Simulator.Instance.doStep();
}
for (int i = 0; i < agents.Count; i++)
{
vec2 = Simulator.Instance.getAgentPosition(i);
realPos.x = vec2.x();
realPos.z = vec2.y();
vec2 = Simulator.Instance.getAgentVelocity(i);
velocity.x = vec2.x_;
velocity.z = vec2.y_;
agentPos = agents[i].transform.position;
Debug.DrawLine(agentPos + Vector3.up * 1f,
agentPos + velocity.normalized * 2.5f + Vector3.up * 1f,
Color.green, Time.deltaTime);
Debug.DrawLine(realPos, realPos + Vector3.up * 10, Color.red, Time.deltaTime);
agents[i].transform.position = Vector3.Lerp(agentPos, realPos, Time.deltaTime * RVOMath.abs(Simulator.Instance.getAgentVelocity(i)));
}
}
// 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))));*/
}
}
public static Vector2 CalcPrefVelocity(Vector2 agentPosition, Vector2 goalPosisiton, RVOAgent agent)
{
Vector2 prefDistance = goalPosisiton - agentPosition;
if (agent.GetIsStop() == true)
{
return(new Vector2(0f, 0f));
}
if (agent.isPlayer == true)
{
/* Is inertial model used */
Vector2 nowVelocity = Simulator.Instance.getAgentVelocity(agent.GetIndex());
float nowSpeed = RVOMath.abs(nowVelocity);
Vector2 prefVelocity = RVOMath.normalize(prefDistance) * agent.prefSpeed;//nowSpeed -> prefSpeed
Vector2 finalVelocity = new Vector2(0, 0);
if (nowSpeed < agent.minSpeedToTurn)
{
/* Not fast enough to turn */
Vector2 advanceDirection = GetDirection(agent);
if (agent.prefSpeed > nowSpeed) //Speed up fitting
{
finalVelocity = nowVelocity + advanceDirection * agent.acceleration;
}
else //Speed down fitting
{
if (nowSpeed - agent.prefSpeed < agent.acceleration)
{
finalVelocity = prefVelocity;
}
else
{
finalVelocity = nowVelocity - advanceDirection * agent.acceleration;
}
}
}
else
{
/* turn */
float direction = RVOMath.det(nowVelocity, prefVelocity);
if (direction > eps) //left
{
Vector2 rightVerticalVelocity = new Vector2(-nowVelocity.y_, nowVelocity.x_);
rightVerticalVelocity = RVOMath.normalize(rightVerticalVelocity) * agent.speed * agent.angularSpeed;
if (RVOMath.det(nowVelocity + rightVerticalVelocity, prefVelocity) < eps) //success
{
finalVelocity = RVOMath.normalize(prefVelocity) * nowSpeed;
}
else
{
finalVelocity = RVOMath.normalize(nowVelocity + rightVerticalVelocity) * nowSpeed;
}
}
if (direction >= -eps && direction <= eps) //same direction
{
finalVelocity = RVOMath.normalize(prefVelocity) * nowSpeed;
}
if (direction < -eps) //right
{
Vector2 leftVerticalVelocity = new Vector2(nowVelocity.y_, -nowVelocity.x_);
leftVerticalVelocity = RVOMath.normalize(leftVerticalVelocity) * agent.speed * agent.angularSpeed;
if (RVOMath.det(nowVelocity + leftVerticalVelocity, prefVelocity) > -eps) //success
{
finalVelocity = RVOMath.normalize(prefVelocity) * nowSpeed;
}
else
{
finalVelocity = RVOMath.normalize(nowVelocity + leftVerticalVelocity) * nowSpeed;
}
}
//Debug.Log("!!!" + RVOMath.abs(finalVelocity).ToString());
/* acc and slow */
if (agent.prefSpeed - nowSpeed > eps) //acc
{
if (IsVectorNear(nowVelocity, prefVelocity, 0.05f))
{
finalVelocity = finalVelocity + RVOMath.normalize(finalVelocity) * Mathf.Min(agent.acceleration, agent.prefSpeed - nowSpeed);
}
}
else //slow
{
finalVelocity = finalVelocity - RVOMath.normalize(finalVelocity) * Mathf.Min(agent.acceleration, nowSpeed - agent.prefSpeed);
}
// Debug.Log("!!!" + RVOMath.abs(finalVelocity).ToString());
}
Vector2 ditherVelocity = GetDitherVelocity(ditherSize);
if (RVOMath.abs(finalVelocity) < eps)
{
return(finalVelocity);
}
else
{
return(finalVelocity + ditherVelocity);
}
}
else
{
Vector2 ditherVelocity = GetDitherVelocity(ditherSize);
Vector2 prefVelocity = RVOMath.normalize(prefDistance);
return(prefVelocity * agent.prefSpeed + ditherVelocity);
}
}
