unity Vuforia物体移动的方向用AR箭头表示出来

主要思想是采用投票箱机制，把物体移动的方向从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<GameObject> 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>();
		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<MeshRenderer> ().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<MeshRenderer> ().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<MeshRenderer> ().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<MeshRenderer> ().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);
		}

	}

}