主要思想是采用投票箱机制,把物体移动的方向从360归一到8个方向:
用到的主要代码如下:
以相机为坐标原点,把移动的小码变换到大码坐标下:
坐标变换
Matrix4x4 extinsic_obj1 = image1.transform.localToWorldMatrix;//image1 相对于camera的 transform metric
Matrix4x4 extinsic_obj2 = imageTarget.transform.localToWorldMatrix;//imageTarget 相对于camera的 transform metric
Matrix4x4 relat = extinsic_obj2.inverse * extinsic_obj1 ;
//Vector4 controlPoint = new Vector4 (image1.transform.position.x, image1.transform.position.y, image1.transform.position.z, 1.0f);
Vector4 controlPoint = new Vector4 (0,0, 0, 1.0f);
Vector4 new_controlPoint = relat * controlPoint;
如果坐标原点在大码上,则不需要变换了。(当然也没错,但没有必要了)
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System.IO;
using System;
using System.Threading;
using System.IO.Ports;
using System.Diagnostics;
using System.Runtime.InteropServices;
using System.Text;
public class Guidance: MonoBehaviour {
public GameObject imageTarget;
//public GameObject arrow;
public GameObject image1;
public GameObject cyl;
public GameObject startpoint;
public GameObject thispoint;
GameObject arrow1;
GameObject arrow2;
GameObject arrow3;
GameObject arrow4;
GameObject arrow5;
GameObject arrow6;
GameObject arrow7;
GameObject arrow8;
GameObject point1;
GameObject point2;
GameObject point3;
GameObject point4;
GameObject point5;
public List pointList;
public int signal=0;
public int i;
public int feedback;
int n=0;
public bool start=false;
public float x0=0.0f;
float y0=0.0f;
public float z0=0.0f;
public float x1=0.0f;
public float z1=-0.1f;
public float d=0.0f;
public float alpha;
public float beta;
public float theta;
// Use this for initialization
void Start () {
pointList=new List();
GameObject point1=GameObject.Find("point0");
point1.SetActive (false);
GameObject point2=GameObject.Find("point1");
point2.SetActive (false);
GameObject point3=GameObject.Find("point2");
point3.SetActive (false);
GameObject point4=GameObject.Find("point3");
point4.SetActive (false);
GameObject point5=GameObject.Find("point4");
point5.SetActive (false);
pointList.Add(point1);
pointList.Add(point2);
pointList.Add(point3);
pointList.Add(point4);
pointList.Add(point5);
arrow1=GameObject.Find("arrow1");
arrow1.SetActive (false);
arrow2=GameObject.Find("arrow2");
arrow2.SetActive (false);
arrow3=GameObject.Find("arrow3");
arrow3.SetActive (false);
arrow4=GameObject.Find("arrow4");
arrow4.SetActive (false);
arrow5=GameObject.Find("arrow5");
arrow5.SetActive (false);
arrow6=GameObject.Find("arrow6");
arrow6.SetActive (false);
arrow7=GameObject.Find("arrow7");
arrow7.SetActive (false);
arrow8=GameObject.Find("arrow8");
arrow8.SetActive (false);
i = 0;
feedback = 1;
thispoint = pointList[0];
//arrow.SetActive (false);
}
// Update is called once per frame
void Update () {
if (start) {
Matrix4x4 extinsic_obj1 = image1.transform.localToWorldMatrix;//image1 相对于camera的 transform metric
Matrix4x4 extinsic_obj2 = imageTarget.transform.localToWorldMatrix;//imageTarget 相对于camera的 transform metric
Matrix4x4 relat = extinsic_obj2.inverse * extinsic_obj1 ;
//Vector4 controlPoint = new Vector4 (image1.transform.position.x, image1.transform.position.y, image1.transform.position.z, 1.0f);
Vector4 controlPoint = new Vector4 (0,0, 0, 1.0f);
Vector4 new_controlPoint = relat * controlPoint;
// //相对于小码的局部坐标
// thispoint.transform.localPosition = new Vector3(new_controlPoint.x , new_controlPoint.y , new_controlPoint.z );
//image 相对于 imageTarget 的 local position
x0 = new_controlPoint.x;
y0 = new_controlPoint.y;
z0 = new_controlPoint.z;
//imageTarget 上的点 相对于 imageTarget 的 local position
x1 = thispoint.transform.localPosition.x;
z1 = thispoint.transform.localPosition.z;
// x1 = thispoint.transform.position.x;
// z1 = thispoint.transform.position.z;
d = Mathf.Sqrt ((x1 - x0) * (x1 - x0) + (z1 - z0) * (z1 - z0));
//alpha=image1.transform.rotation.eulerAngles.y;
alpha = image1.transform.localEulerAngles.y;
// if (alpha > 360)
// alpha = alpha - 360;
if (alpha > 180)
alpha = alpha - 360;
beta = Mathf.Atan2 ((z1 - z0), (x1 - x0)) * 180 / Mathf.PI;
theta = alpha + beta;
//haptic feedback
if (feedback == 0) {
//arrow.SetActive (false);
arrow1.SetActive (false);
arrow2.SetActive (false);
arrow3.SetActive (false);
arrow4.SetActive (false);
arrow5.SetActive (false);
arrow6.SetActive (false);
arrow7.SetActive (false);
arrow8.SetActive (false);
if (d > 0.02) {
if (theta >= -22.5 && theta < 22.5) {
signal = 1;
}
if (theta >= 22.5 && theta < 67.5) {
signal = 8;
}
if (theta >= 67.5 && theta < 112.5) {
signal = 7;
}
if (theta >= 112.5 && theta < 157.5) {
signal = 6;
}
if (theta >= 157.5 && theta <= 180 || theta >= -180 && theta < -157.5) {
signal = 5;
}
if (theta >= -157.5 && theta < -112.5) {
signal = 4;
}
if (theta >= -112.5 && theta < -67.5) {
signal = 3;
}
if (theta >= -67.5 && theta < -22.5) {
signal = 2;
}
} else {
signal = 0;
// WriteData ("0");
if (i == 4) {
start = false;
}
n = 1;
}
if (n == 1 && d > 0.02) {
i++;
if (i >= 5)
i = 0;
thispoint = pointList [i];
n = 0;
}
}
//visual feedback
if (feedback == 1) {
//arrow.SetActive (true);
//arrow.transform.localPosition = new Vector3(x0,y0,z0);
if (d > 0.02) {
if (theta >= -22.5 && theta < 22.5) {
arrow1.SetActive (true);
arrow2.SetActive (false);
arrow3.SetActive (false);
arrow4.SetActive (false);
arrow5.SetActive (false);
arrow6.SetActive (false);
arrow7.SetActive (false);
arrow8.SetActive (false);
// arrow.transform.localRotation = Quaternion.Euler (0, -90, 0);
}
if (theta >= 22.5 && theta < 67.5) {
arrow1.SetActive (false);
arrow2.SetActive (false);
arrow3.SetActive (false);
arrow4.SetActive (false);
arrow5.SetActive (false);
arrow6.SetActive (false);
arrow7.SetActive (false);
arrow8.SetActive (true);
// arrow.transform.localRotation = Quaternion.Euler (0, -135, 0);
}
if (theta >= 67.5 && theta < 112.5) {
arrow1.SetActive (false);
arrow2.SetActive (false);
arrow3.SetActive (false);
arrow4.SetActive (false);
arrow5.SetActive (false);
arrow6.SetActive (false);
arrow7.SetActive (true);
arrow8.SetActive (false);
// arrow.transform.localRotation = Quaternion.Euler (0, -180, 0);
}
if (theta >= 112.5 && theta < 157.5) {
arrow1.SetActive (false);
arrow2.SetActive (false);
arrow3.SetActive (false);
arrow4.SetActive (false);
arrow5.SetActive (false);
arrow6.SetActive (true);
arrow7.SetActive (false);
arrow8.SetActive (false);
// arrow.transform.localRotation = Quaternion.Euler (0, 135, 0);
}
if (theta >= 157.5 && theta <= 180 || theta >= -180 && theta < -157.5) {
arrow1.SetActive (false);
arrow2.SetActive (false);
arrow3.SetActive (false);
arrow4.SetActive (false);
arrow5.SetActive (true);
arrow6.SetActive (false);
arrow7.SetActive (false);
arrow8.SetActive (false);
// arrow.transform.localRotation = Quaternion.Euler (0, 90, 0);
}
if (theta >= -157.5 && theta < -112.5) {
arrow1.SetActive (false);
arrow2.SetActive (false);
arrow3.SetActive (false);
arrow4.SetActive (true);
arrow5.SetActive (false);
arrow6.SetActive (false);
arrow7.SetActive (false);
arrow8.SetActive (false);
// arrow.transform.localRotation = Quaternion.Euler (0, 45, 0);
}
if (theta >= -112.5 && theta < -67.5) {
arrow1.SetActive (false);
arrow2.SetActive (false);
arrow3.SetActive (true);
arrow4.SetActive (false);
arrow5.SetActive (false);
arrow6.SetActive (false);
arrow7.SetActive (false);
arrow8.SetActive (false);
// arrow.transform.localRotation = Quaternion.Euler (0, 0, 0);
}
if (theta >= -67.5 && theta < -22.5) {
arrow1.SetActive (false);
arrow2.SetActive (true);
arrow3.SetActive (false);
arrow4.SetActive (false);
arrow5.SetActive (false);
arrow6.SetActive (false);
arrow7.SetActive (false);
arrow8.SetActive (false);
// arrow.transform.localRotation = Quaternion.Euler (0, -45, 0);
}
// if (theta >= -22.5 && theta < 22.5) {
// arrow.transform.localRotation = Quaternion.Euler (0, -90, 0);
// }
// if (theta >= 22.5 && theta < 67.5) {
// arrow.transform.localRotation = Quaternion.Euler (0, -135, 0);
// }
// if (theta >= 67.5 && theta < 112.5) {
// arrow.transform.localRotation = Quaternion.Euler (0, -180, 0);
// }
// if (theta >= 112.5 && theta < 157.5) {
// arrow.transform.localRotation = Quaternion.Euler (0, 135, 0);
// }
// if (theta >= 157.5 && theta <= 180 || theta >= -180 && theta < -157.5) {
// arrow.transform.localRotation = Quaternion.Euler (0, 90, 0);
// }
// if (theta >= -157.5 && theta < -112.5) {
// arrow.transform.localRotation = Quaternion.Euler (0, 45, 0);
// }
// if (theta >= -112.5 && theta < -67.5) {
// arrow.transform.localRotation = Quaternion.Euler (0, 0, 0);
// }
// if (theta >= -67.5 && theta < -22.5) {
// arrow.transform.localRotation = Quaternion.Euler (0, -45, 0);
// }
} else {
cyl.GetComponent ().material.color = Color.green;
//pointList [i].SetActive (true);
//arrow.SetActive (false);
arrow1.SetActive (false);
arrow2.SetActive (false);
arrow3.SetActive (false);
arrow4.SetActive (false);
arrow5.SetActive (false);
arrow6.SetActive (false);
arrow7.SetActive (false);
arrow8.SetActive (false);
if (i == 4) {
start = false;
}
n = 1;
}
if (n == 1 && d > 0.02) {
//arrow.SetActive (true);
cyl.GetComponent ().material.color = Color.red;
//pointList [i].SetActive (false);
i++;
if (i >= 5)
i = 0;
thispoint = pointList [i];
n = 0;
}
}
//visual+haptic feedback
if (feedback == 2) {
//arrow.SetActive (true);
//arrow.transform.localPosition = new Vector3(x0,y0,z0);
if (d > 0.02) {
//arrow.transform.localRotation = Quaternion.Euler (0,-beta,90);//
if (theta >= -22.5 && theta < 22.5) {
signal = 1;
//WriteData ("1");
arrow1.SetActive (true);
arrow2.SetActive (false);
arrow3.SetActive (false);
arrow4.SetActive (false);
arrow5.SetActive (false);
arrow6.SetActive (false);
arrow7.SetActive (false);
arrow8.SetActive (false);
//arrow.transform.localRotation = Quaternion.Euler (0, 0, 90);
//arrow.transform.localRotation = Quaternion.Euler (0, -90, 0);
//Debug.Log ("1");
}
if (theta >= 22.5 && theta < 67.5) {
signal = 8;
//WriteData ("8");
arrow1.SetActive (false);
arrow2.SetActive (false);
arrow3.SetActive (false);
arrow4.SetActive (false);
arrow5.SetActive (false);
arrow6.SetActive (false);
arrow7.SetActive (false);
arrow8.SetActive (true);
//arrow.transform.localRotation = Quaternion.Euler (0, -45, 90);
//arrow.transform.localRotation = Quaternion.Euler (0, -135, 0);
//Debug.Log ("8");
}
if (theta >= 67.5 && theta < 112.5) {
signal = 7;
//WriteData ("7");
arrow1.SetActive (false);
arrow2.SetActive (false);
arrow3.SetActive (false);
arrow4.SetActive (false);
arrow5.SetActive (false);
arrow6.SetActive (false);
arrow7.SetActive (true);
arrow8.SetActive (false);
// arrow.transform.localRotation = Quaternion.Euler (0, -90, 90);
//arrow.transform.localRotation = Quaternion.Euler (0, -180, 0);
//Debug.Log ("7");
}
if (theta >= 112.5 && theta < 157.5) {
signal = 6;
//WriteData ("6");
arrow1.SetActive (false);
arrow2.SetActive (false);
arrow3.SetActive (false);
arrow4.SetActive (false);
arrow5.SetActive (false);
arrow6.SetActive (true);
arrow7.SetActive (false);
arrow8.SetActive (false);
// arrow.transform.localRotation = Quaternion.Euler (0, -135, 90);
//arrow.transform.localRotation = Quaternion.Euler (0, 135, 0);
//Debug.Log ("6");
}
if (theta >= 157.5 && theta <= 180 || theta >= -180 && theta < -157.5) {
signal = 5;
//WriteData ("5");
arrow1.SetActive (false);
arrow2.SetActive (false);
arrow3.SetActive (false);
arrow4.SetActive (false);
arrow5.SetActive (true);
arrow6.SetActive (false);
arrow7.SetActive (false);
arrow8.SetActive (false);
// arrow.transform.localRotation = Quaternion.Euler (0, -180, 90);
//arrow.transform.localRotation = Quaternion.Euler (0, 90, 0);
//Debug.Log ("5");
}
if (theta >= -157.5 && theta < -112.5) {
signal = 4;
//WriteData ("4");
arrow1.SetActive (false);
arrow2.SetActive (false);
arrow3.SetActive (false);
arrow4.SetActive (true);
arrow5.SetActive (false);
arrow6.SetActive (false);
arrow7.SetActive (false);
arrow8.SetActive (false);
// arrow.transform.localRotation = Quaternion.Euler (0, 135, 90);
//arrow.transform.localRotation = Quaternion.Euler (0, 45, 0);
//Debug.Log ("4");
}
if (theta >= -112.5 && theta < -67.5) {
signal = 3;
//WriteData ("3");
arrow1.SetActive (false);
arrow2.SetActive (false);
arrow3.SetActive (true);
arrow4.SetActive (false);
arrow5.SetActive (false);
arrow6.SetActive (false);
arrow7.SetActive (false);
arrow8.SetActive (false);
// arrow.transform.localRotation = Quaternion.Euler (0, 90, 90);
//arrow.transform.localRotation = Quaternion.Euler (0, 0, 0);
//Debug.Log ("3");
}
if (theta >= -67.5 && theta < -22.5) {
signal = 2;
//WriteData ("2");arrow1.SetActive (false);
arrow1.SetActive (false);
arrow2.SetActive (true);
arrow3.SetActive (false);
arrow4.SetActive (false);
arrow5.SetActive (false);
arrow6.SetActive (false);
arrow7.SetActive (false);
arrow8.SetActive (false);
// arrow.transform.localRotation = Quaternion.Euler (0, 45, 90);
//arrow.transform.localRotation = Quaternion.Euler (0, -45, 0);
//Debug.Log ("2");
}
} else {
signal = 0;
// WriteData ("0");
cyl.GetComponent ().material.color = Color.green;
// pointList [i].SetActive (true);
//arrow.SetActive (false);
arrow1.SetActive (false);
arrow2.SetActive (false);
arrow3.SetActive (false);
arrow4.SetActive (false);
arrow5.SetActive (false);
arrow6.SetActive (false);
arrow7.SetActive (false);
arrow8.SetActive (false);
if (i == 4) {
start = false;
}
n = 1;
}
if (n == 1 && d > 0.02) {
//arrow.SetActive (true);
cyl.GetComponent ().material.color = Color.red;
// pointList [i].SetActive (false);
i++;
if (i >= 5)
i = 0;
thispoint = pointList [i];
n = 0;
}
}
}
}
public void OnButtonClick_haptic ()
{
feedback = 0;
}
public void OnButtonClick_visual ()
{
feedback = 1;
}
public void OnButtonClick_vishaptic ()
{
feedback = 2;
}
public void OnButtonClick_route1()
{
setPath (1);
}
public void OnButtonClick_route2()
{
setPath (2);
}
public void OnButtonClick_route3()
{
setPath (3);
}
public void OnButtonClick_start()
{
start = true;
}
public void OnButtonClick_end()
{
start = false;
arrow1.SetActive (false);
arrow2.SetActive (false);
arrow3.SetActive (false);
arrow4.SetActive (false);
arrow5.SetActive (false);
arrow6.SetActive (false);
arrow7.SetActive (false);
arrow8.SetActive (false);
i = 0;
thispoint = pointList [0];
n = 0;
}
public void OnButtonClick_display()
{
pointList[0].SetActive (true);
pointList[1].SetActive (true);
pointList[2].SetActive (true);
pointList[3].SetActive (true);
pointList[4].SetActive (true);
}
public void OnButtonClick_nodisplay()
{
pointList[0].SetActive (false);
pointList[1].SetActive (false);
pointList[2].SetActive (false);
pointList[3].SetActive (false);
pointList[4].SetActive (false);
}
public void setPath(int path)
{
if (path == 1) {
startpoint.transform.localPosition=new Vector3(-0.4f,0,-0.4f);
pointList[0].transform.localPosition = new Vector3 (-0.2f,0,0);
pointList[1].transform.localPosition = new Vector3 (-0.4f,0,0.4f);
pointList[2].transform.localPosition = new Vector3 (0,0,0.2f);
pointList[3].transform.localPosition = new Vector3 (0.2f,0,-0.2f);
pointList[4].transform.localPosition = new Vector3 (0.4f,0,0.2f);
}
if (path == 2) {
startpoint.transform.localPosition=new Vector3(0.4f,0,-0.4f);
pointList[0].transform.localPosition = new Vector3 (0,0,-0.2f);
pointList[1].transform.localPosition = new Vector3 (-0.4f,0,-0.4f);
pointList[2].transform.localPosition = new Vector3 (-0.2f,0,0);
pointList[3].transform.localPosition = new Vector3 (0.2f,0,0.2f);
pointList[4].transform.localPosition = new Vector3 (0,-0.2f,0.4f);
}
if (path == 3) {
startpoint.transform.localPosition=new Vector3(0.4f,0,0.4f);
pointList[0].transform.localPosition = new Vector3 (0.2f,0,0);
pointList[1].transform.localPosition = new Vector3 (0.4f,0,-0.4f);
pointList[2].transform.localPosition = new Vector3 (0,0,-0.2f);
pointList[3].transform.localPosition = new Vector3 (-0.2f,0,0.2f);
pointList[4].transform.localPosition = new Vector3 (-0.4f,0,-0.2f);
}
}
}