unity Vuforia物體移動的方向用AR箭頭表示出來
阿新 • • 發佈：2018-12-15
主要思想是採用投票箱機制，把物體移動的方向從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); } } }
