using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System.IO;
using System.Linq;
using System.Net;
using System.Net.Sockets;
using System.Text;
public class Camera_udp : MonoBehaviour
{
public GameObject centerLines;
public GameObject light_source;
public GameObject virtul_endscope;
//中心点たどるよう変数
List<Vector3> worldVertices = new List<Vector3>();
List<Vector3> sortedWorldVertices = new List<Vector3>();
List<float> distanceOfPointsList = new List<float>();
int posIndex = 0;
//csv出力用
float nomalizedNum = (125.6f / 12.4f);
int whileCount = 0;
// string basePath = "C:\\UnityWorkSpace\\";
float beforeDistance = 0.0f;
//ボタン制御用
bool Is_starting = false;
//udp関連
static UdpClient udp;
// IPEndPoint remoteEP = null;
// csvログ用
StreamWriter log_sr;
// ボタンが押された場合、今回呼び出される関数
public void OnClick()
{
Debug.Log("同期開始"); // ログを出力
Is_starting = true;
GameObject obj = GameObject.Find("Canvas/Button");
Destroy(obj);
}
// Use this for initialization
void Start()
{
int LOCAL_PORT = 50007;
udp = new UdpClient(LOCAL_PORT);
udp.Client.ReceiveTimeout = 100;
var moveMatrix = centerLines.transform.localToWorldMatrix;
var centerVertices = centerLines.GetComponent<MeshFilter>().mesh.vertices;
foreach (var vertice in centerVertices)
{
worldVertices.Add(moveMatrix.MultiplyPoint3x4(vertice));
}
sortedWorldVertices = sortNearestPoint(light_source.transform.position, worldVertices);
distanceOfPointsList = getDistanceOfPointList(sortedWorldVertices);
if (worldVertices != null)
{
light_source.transform.position = sortedWorldVertices[0];
virtul_endscope.transform.position = sortedWorldVertices[0];
light_source.transform.LookAt(sortedWorldVertices[1]);
}
else Debug.Log("ヌル！！！");
log_sr = new StreamWriter(@"C:\Users\user\Documents\EndoNaviSystem\log.csv", false, Encoding.UTF8);
DateTime dt = DateTime.Now;
var start_dt_str = dt.ToString("yyyy/MM/dd HH:mm:ss.fff");
log_sr.WriteLine("start time: " + start_dt_str + " format: HHmmssfff,x,y,z");
}
/*
* 返り値: List<Vector3>
* 引数: カメラに近い順番に並んだ座標値のList
* 機能: 座標間の距離のListを作成する
*/
List<float> getDistanceOfPointList(List<Vector3> movePoints)
{
var pointsDistanceList = new List<float>();
for (int i = 0; i < movePoints.Count - 1; i++)
{
pointsDistanceList.Add((movePoints[i + 1] - movePoints[i]).magnitude);
}
return pointsDistanceList;
}
/*
* 返り値: float
* 引数: CSVファイルへのパス string
* 機能: 挿入距離を取得
*/
float getInsertionDistance(string pathToCSV)
{
float unityMoveDistance = 0.0f;
float moveDistance = 0.0f;
if (File.Exists(pathToCSV))
{
var moveList = new List<string>();
var sr = new StreamReader(pathToCSV);
while (!sr.EndOfStream)
{
string line = sr.ReadLine();
string[] values = line.Split(',');
moveList.AddRange(values);
}
moveDistance = float.Parse(moveList[0]);
if (beforeDistance != moveDistance)
{
unityMoveDistance = moveDistance * nomalizedNum;
//unityMoveDistance = moveDistance;
beforeDistance = moveDistance;
}
moveList.Clear();
sr.Close();
whileCount++;
}
//Debug.Log(whileCount);
return unityMoveDistance;
}
/*
* 返り値: List<Vector3>
* 引数: 最初のカメラ位置(Vector3), 世界系での中心座標(List<Vector3>)
* 機能: カメラから近い順にソートする(処理の都合上，第2引数のListは空になる．Copyとか足せばどうにかなる)
*/
List<Vector3> sortNearestPoint(Vector3 initCameraPoint, List<Vector3> centerVertices)
{
var sortedCenterVertices = new List<Vector3>();
float minDistance = float.MaxValue;
var nearestVertex = new Vector3();
centerVertices.Distinct();
foreach (var vertex in centerVertices)
{
float tempMagnitude = (vertex - initCameraPoint).magnitude;
if (tempMagnitude < minDistance)
{
nearestVertex = vertex;
minDistance = tempMagnitude;
}
}
sortedCenterVertices.Add(nearestVertex);
worldVertices.Remove(nearestVertex);
var currentVertex = nearestVertex;
int initialCount = centerVertices.Count;
for (int i = 0; i < initialCount; i++)
{
minDistance = float.MaxValue;
foreach (var vertex in worldVertices)
{
float tempMagnitude = (vertex - currentVertex).magnitude;
if (tempMagnitude < minDistance)
{
nearestVertex = vertex;
minDistance = tempMagnitude;
}
}
sortedCenterVertices.Add(nearestVertex);
worldVertices.Remove(nearestVertex);
currentVertex = nearestVertex;
}
return sortedCenterVertices;
}
/*
* 返り値: List<Vector3>
*/
/*
* 返り値: Vector3
* 引数: 世界系の中心座標(List<Vector3>), その座標系でのインデックス(int)
* 機能:LookAt先のvectorを演算，自分の先のベクターを利用．ハイパーパラメータとしてVector間隔，重心決定範囲を設定
*/
Vector3 calcBarycenter(List<Vector3> centers, int index)
{
// ハイパーパラメータ
int interval = 5;
int range = centers.Count / 50;
//以下処理
var tempVectors = new List<Vector3>();
var maxIndex = centers.Count;
for (int i = 0; i < range; i++)
{
if (maxIndex <= (index + (i * interval))) break;
tempVectors.Add(centers[index + (i * interval)]);
}
return new Vector3(tempVectors.Average(V => V.x), tempVectors.Average(V => V.y), tempVectors.Average(V => V.z));
}
/*
* 返り値: 無し
* 引数: 無し
* 機能: InvokeRepeating用，実質Update内の記述
*/
void Update()
{
float insertionDistance = 0.0f;
DateTime dt = DateTime.Now;
var cur_dt_str = dt.ToString("HHmmssfff");
var cur_camera_pos = virtul_endscope.transform.position;
log_sr.WriteLine(cur_dt_str + "," + cur_camera_pos[0].ToString() + "," + cur_camera_pos[1].ToString() + "," + cur_camera_pos[2].ToString());
IPEndPoint remoteEP = null;
try
{
byte[] data = udp.Receive(ref remoteEP);
string text = Encoding.UTF8.GetString(data);
insertionDistance = float.Parse(text);
}
catch
{
}
//Debug.Log(posIndex);
if (Is_starting)
{
if (0.0f < insertionDistance && posIndex != sortedWorldVertices.Count - 1)
{
float nearestDistance = (light_source.transform.position - sortedWorldVertices[posIndex + 1]).magnitude;
if (insertionDistance <= nearestDistance)
{
light_source.transform.LookAt(sortedWorldVertices[posIndex + 1]);
light_source.transform.Translate(0.0f, 0.0f, insertionDistance);
}
else
{
insertionDistance -= nearestDistance;
posIndex++;
while (true)
{
if (posIndex == sortedWorldVertices.Count - 1)
{
light_source.transform.position = sortedWorldVertices[posIndex];
virtul_endscope.transform.position = light_source.transform.position;
Debug.Log("最終到達地点に到着");
break;
}
else if (insertionDistance < distanceOfPointsList[posIndex])
{
light_source.transform.position = sortedWorldVertices[posIndex];
// ここは理想はトランスレイトではなく，ポジションに変える．そうすれば視界も安定する．
light_source.transform.LookAt(sortedWorldVertices[posIndex + 1]);
light_source.transform.Translate(0.0f, 0.0f, insertionDistance);
// virtualEndoscope.transform.LookAt(calcBarycenter(sortedWorldVertices, posIndex));
virtul_endscope.transform.position = light_source.transform.position;
break;
}
insertionDistance -= distanceOfPointsList[posIndex];
posIndex++;
}
}
}
else if (insertionDistance < 0.0f && posIndex != 0)
{
insertionDistance = -insertionDistance;
float nearestDistance = (light_source.transform.position - sortedWorldVertices[posIndex - 1]).magnitude;
if (insertionDistance <= nearestDistance)
{
if (posIndex != sortedWorldVertices.Count - 1){
light_source.transform.LookAt(sortedWorldVertices[posIndex + 1]);
}
light_source.transform.Translate(0.0f, 0.0f, -insertionDistance);
}
else
{
insertionDistance -= nearestDistance;
while (true)
{
if (posIndex == 0)
{
light_source.transform.position = sortedWorldVertices[posIndex];
virtul_endscope.transform.position = light_source.transform.position;
break;
}
else if (insertionDistance < distanceOfPointsList[posIndex - 1])
{
float tempRatio = insertionDistance / distanceOfPointsList[posIndex - 1];
Vector3 tempVector = sortedWorldVertices[posIndex] - sortedWorldVertices[posIndex - 1];
light_source.transform.position = new Vector3(sortedWorldVertices[posIndex].x - (tempVector.x * tempRatio), sortedWorldVertices[posIndex].y - (tempVector.y * tempRatio), sortedWorldVertices[posIndex].z - (tempVector.z * tempRatio));
//virtualEndoscope.transform.LookAt(calcBarycenter(sortedWorldVertices, posIndex));
light_source.transform.LookAt(sortedWorldVertices[posIndex]);
virtul_endscope.transform.position = light_source.transform.position;
posIndex--;
break;
}
insertionDistance -= distanceOfPointsList[posIndex - 1];
posIndex--;
}
}
}
}
}
}
