void UpdateRobotInfo()
{
// 移動先までの角度差を計算する
float angle = Vector3.Angle(currentGao.transform.position - Vector3.zero, nextGao.transform.position - Vector3.zero);
// 回転軸を割り出す
// currentGaoとnextGaoのどちらかは必ず辺のオブジェクトで、回転軸を持つ
edge = (currentGao.GetComponent <EdgeInformation>() != null) ? currentGao.GetComponent <EdgeInformation>() : nextGao.GetComponent <EdgeInformation>();
Vector3 axis = edge.vertex[0].transform.position - edge.vertex[1].transform.position; // 回転軸
// angle度でrobotのRotateAroundを行う
robot.transform.RotateAround(Vector3.zero, axis, angle);
// angle度で回転後の位置及び角度を記録する
Vector3 nextPos01 = robot.transform.position;
Vector3 nextAngle01 = robot.transform.eulerAngles;
// -2 * angle度でrobotのRotateAroundを行う（逆方向に回転させる）
robot.transform.RotateAround(Vector3.zero, axis, -2 * angle);
// -angle度で回転後の位置及び角度を記録する
Vector3 nextPos02 = robot.transform.position;
Vector3 nextAngle02 = robot.transform.eulerAngles;
// angle度でrobotのRotateAroundを行う（初期位置に戻す）
robot.transform.RotateAround(Vector3.zero, axis, angle);
// 上で調べたnextPosとnextGaoが原点と織りなす角度の差を求め、より小さい方を正しい回転方向として採用する
float angle01 = Vector3.Angle(nextGao.transform.position - Vector3.zero, nextPos01 - Vector3.zero);
float angle02 = Vector3.Angle(nextGao.transform.position - Vector3.zero, nextPos02 - Vector3.zero);
Vector3 nextAngle = (angle01 < angle02) ? nextAngle01 : nextAngle02;
// ロボットの位置
robot.transform.position = nextGao.transform.position;
// ロボットの方向
robot.transform.eulerAngles = nextAngle;
}
void MakeLists1_AroundVertex()
{
for (int i = 0; i < vertexList.Count; i++)
{
colList = Physics.OverlapSphere(vertexList[i].transform.position, searchDis_vertex); // 各頂点の原点位置を中心にオブジェクト探索
for (int j = 0; j < colList.Length; j++)
{
if (colList[j].gameObject.CompareTag("Edge"))
{
edge = colList[j].gameObject.GetComponent <EdgeInformation>();
// EdgeInformationに、辺に接する２頂点のtransform情報を与える
if (edge.vertex[0] == null)
{
edge.vertex[0] = vertexList[i];
}
else
{
edge.vertex[1] = vertexList[i];
}
int num = edge.EdgeNum; // 辺の識別番号 edgeNum を取得する
// 辺に接する頂点のリスト化
vertexList_FromEdge[num].Add(vertexList[i].gameObject); // ◆今のところデバッグ用としてのみの使用◆頂点のgameObject = vertexList[i].gameObject
}
}
}
}
void MakeLists2_AroundEdge()
{
for (int i = 0; i < edgeList.Count; i++)
{
colList = Physics.OverlapSphere(edgeList[i].transform.position, searchDis_edge); // 各辺の原点位置を中心にオブジェクト探索
edge = edgeList[i].GetComponent <EdgeInformation>();
for (int j = 0; j < colList.Length; j++)
{
if (colList[j].gameObject.CompareTag("Face"))
{
////////////////////////////////
//// 辺に接する面のリスト化 ////
////////////////////////////////
// EdgeInformationに、辺に接する２面のtransform情報を与える
if (edge.face[0] == null)
{
edge.face[0] = colList[j].gameObject;
}
else
{
edge.face[1] = colList[j].gameObject;
}
// 辺に接する面のリスト化
faceList_FromEdge[i].Add(colList[j].gameObject); // ◆今のところデバッグ用としてのみの使用◆辺の識別番号 = i、面のgameObject = colList[j].gameObject
////////////////////////////////
//// 面に接する辺のリスト化 ////
////////////////////////////////
face = colList[j].gameObject.GetComponent <FaceInformation>();
// FaceInformationに、面に接する４辺のtransform情報を与える
if (face.edge[0] == null)
{
face.edge[0] = edgeList[i];
}
else if (face.edge[1] == null)
{
face.edge[1] = edgeList[i];
}
else if (face.edge[2] == null)
{
face.edge[2] = edgeList[i];
}
else
{
face.edge[3] = edgeList[i];
}
// 面に接する辺のリスト化
edgeList_FromFace[face.FaceNum].Add(edgeList[i]); // ◆今のところデバッグ用としてのみの使用◆面の識別番号 = face.FaceNum、辺のgameObject = edgeList[i]
}
}
}
}
void NumberingAndMakeLists()
{
for (int i = 0; i < edgeList.Count; i++)
{
edge = edgeList[i].GetComponent <EdgeInformation>();
edge.EdgeNum = i; // 辺に識別番号を付与
vertexList_FromEdge.Add(new List <GameObject>()); // 辺の数だけ新規リストを作成
faceList_FromEdge.Add(new List <GameObject>()); // 辺の数だけ新規リストを作成
}
for (int j = 0; j < faceList.Count; j++)
{
face = faceList[j].GetComponent <FaceInformation>();
face.FaceNum = j; // 面に識別番号を付与
edgeList_FromFace.Add(new List <GameObject>()); // 面の数だけ新規リストを作成
}
}
void FindNearestFaceOrEdge()
{
// 直前までカメラが面していたのは面か辺か
if (!vertical && !horizontal) // 面の時
{
face = currentGao.GetComponent <FaceInformation>();
float?diff = null;
for (int i = 0; i < 4; i++)
{
// robotの探索移動前後の距離を比較し、最も距離が縮まった辺（のGameObject）を割り出す
// robotの探索移動前（＝現在いる面）と移動先候補の辺との距離
float beforeDis = (face.edge[i].transform.position - currentGao.transform.position).magnitude;
// robotの探索移動後と移動先候補の辺との距離
float afterDis = (face.edge[i].transform.position - robot.transform.position).magnitude;
if (diff == null || afterDis - beforeDis < diff)
{
diff = afterDis - beforeDis;
nextGao = face.edge[i];
}
}
}
else // 辺の時
{
edge = currentGao.GetComponent <EdgeInformation>();
float?diff = null;
for (int i = 0; i < 2; i++)
{
// robotの探索移動前後の距離を比較し、最も距離が縮まった面（のGameObject）を割り出す
// robotの探索移動前（＝現在いる面）と移動先候補の辺との距離
float beforeDis = (edge.face[i].transform.position - currentGao.transform.position).magnitude;
// robotの探索移動後と移動先候補の辺との距離
float afterDis = (edge.face[i].transform.position - robot.transform.position).magnitude;
if (diff == null || afterDis - beforeDis < diff)
{
diff = afterDis - beforeDis;
nextGao = edge.face[i];
}
}
}
}
public GameObject CreateEdge(Node source, Node target)
{
// create edge in graph
EdgeData data = new EdgeData();
data.label = "edge";
data.length = 60.0f;
Edge newEdge = new Edge(Guid.NewGuid().ToString(), source, target, data);
Edge createdEdge = graph.AddEdge(newEdge);
// create unity edge object
GameObject go = GameObject.Instantiate(EdgePrefab);
go.transform.parent = gameObject.transform;
gameNodes[createdEdge.ID] = go;
EdgeInformation edgeInfo = go.GetComponent <EdgeInformation>();
edgeInfo.edge = createdEdge;
return(go);
}
public void HandleQueue(DataQueue queue)
{
BaseEntity root;
BaseEntity refering;
GameObject go;
if (queue.GetFromQueue(out root, out refering))
{
Node rootNode = root != null?nodes.ContainsKey(root.Id) ? nodes[root.Id] : null : null;
Node referingNode = refering != null?nodes.ContainsKey(refering.Id) ? nodes[refering.Id] : null : null;
if (root != null && rootNode == null)
{
go = CreateNode(root.Id);
NodeInformation nodeInfo = go.GetComponent <NodeInformation>();
nodes[nodeInfo.node.ID] = nodeInfo.node;
rootNode = nodeInfo.node;
nodeInfo.entity = root;
}
if (refering != null && referingNode == null)
{
go = CreateNode(refering.Id);
NodeInformation nodeInfo = go.GetComponent <NodeInformation>();
nodes[nodeInfo.node.ID] = nodeInfo.node;
referingNode = nodeInfo.node;
nodeInfo.entity = refering;
}
if (rootNode != null && referingNode != null)
{
go = CreateEdge(rootNode, referingNode);
EdgeInformation edgeInfo = go.GetComponent <EdgeInformation>();
}
}
}
/// <summary>
/// Default constructor
/// </summary>
/// <param name="start">Start position of the edge</param>
/// <param name="end">End position of the edge</param>
/// <param name="information">Aditional information of the edge</param>
public NavigationEdge(Vector3 start, Vector3 end, EdgeInformation information)
{
this.start = start;
this.end = end;
this.information = information;
}
/// <summary>
/// Parametrized constructor
/// </summary>
/// <param name="start">Start node of the edge</param>
/// <param name="end">End node of the edge</param>
/// <param name="cost">Cost of the edge</param>
/// <param name="information">Extra information of the edge</param>
public AnnotatedEdge(int start, int end, float cost, EdgeInformation information)
: base(start, end, cost)
{
this.information = information;
}
/// <summary>
/// Default constructor
/// </summary>
/// <remarks>The edge information is set to normal</remarks>
public AnnotatedEdge()
{
this.information = EdgeInformation.Normal;
}
private void DrawFieldOfView()
{
int rayCount = Mathf.RoundToInt(viewAngle * meshResolution);
float rayAngleSize = viewAngle / rayCount;
List <Vector3> viewPoints = new List <Vector3>();
ViewRaycastInformation oldViewRaycast = new ViewRaycastInformation();
for (int i = 0; i <= rayCount; i++)
{
float currentAngle = transform.eulerAngles.y - viewAngle / 2 + rayAngleSize * i;
ViewRaycastInformation newViewRaycast = ViewRaycast(currentAngle);
if (i > 0)
{
bool edgeDistanceThresholdExceeded = Mathf.Abs(oldViewRaycast.distance - newViewRaycast.distance) > edgeDistanceThreshold;
if (oldViewRaycast.hit != newViewRaycast.hit || (oldViewRaycast.hit && newViewRaycast.hit && edgeDistanceThresholdExceeded))
{
// Find Edge
EdgeInformation edge = FindEdge(oldViewRaycast, newViewRaycast);
if (edge.pointA != Vector3.zero)
{
viewPoints.Add(edge.pointA);
}
if (edge.pointB != Vector3.zero)
{
viewPoints.Add(edge.pointB);
}
}
}
viewPoints.Add(newViewRaycast.point);
oldViewRaycast = newViewRaycast;
// Draw Mesh Lines
if (DrawGizmos)
{
Debug.DrawLine(transform.position, transform.position + DirectionFromAngle(currentAngle, true) * viewRadius, Color.blue);
}
}
int vertexCount = viewPoints.Count + 1;
Vector3[] vertices = new Vector3[vertexCount];
int[] triangles = new int[(vertexCount - 2) * 3];
vertices[0] = Vector3.zero;
for (int i = 0; i < vertexCount - 1; i++)
{
vertices[i + 1] = transform.InverseTransformPoint(viewPoints[i]) + Vector3.forward * maskCutawayDistance;
if (i < vertexCount - 2)
{
triangles[i * 3] = 0;
triangles[i * 3 + 1] = i + 1;
triangles[i * 3 + 2] = i + 2;
}
}
viewMesh.Clear();
viewMesh.vertices = vertices;
viewMesh.triangles = triangles;
viewMesh.RecalculateNormals();
}
/// <summary>
/// Default constructor
/// </summary>
/// <param name="start">Start position of the edge</param>
/// <param name="end">End position of the edge</param>
/// <param name="information">Aditional information of the edge</param>
public NavigationEdge(Vector3 start, Vector3 end, EdgeInformation information)
{
this.start = start;
this.end = end;
this.information = information;
}
/// <summary>
/// Parametrized constructor
/// </summary>
/// <param name="start">Start node of the edge</param>
/// <param name="end">End node of the edge</param>
/// <param name="cost">Cost of the edge</param>
/// <param name="information">Extra information of the edge</param>
public AnnotatedEdge(int start, int end, float cost, EdgeInformation information)
: base(start, end, cost)
{
this.information = information;
}
/// <summary>
/// Default constructor
/// </summary>
/// <remarks>The edge information is set to normal</remarks>
public AnnotatedEdge()
{
this.information = EdgeInformation.Normal;
}
