• API
• FAQ
• Tools
• Archive
SHARE
TWEET

# Unity2Ros

a guest Jul 23rd, 2019 56 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
1. //Usage will be CalculateCoordinateTransformation(virtualFrameObject.transform, VICONFrameObject.transform);
2.     private void CalculateCoordinateTransformation(Transform virtualFrameTransform, Transform viconFrameTransform)
3.     {
4.         //Matrix4x4 viconFrameObj = Matrix4x4.identity;
5.         //viconFrameObj.SetTRS(viconFrameTransform.position, viconFrameTransform.rotation, new Vector3(1, 1, 1));
6.
7.         //Matrix4x4 virtualFrame = Matrix4x4.identity;
8.         //virtualFrame.SetTRS(virtualFrameTransform.position, virtualFrameTransform.rotation, new Vector3(1, 1, 1));
9.
10.         //unityToVicon = virtualFrame * viconFrameObj.inverse;
11.         //Vector4 hh = unityToVicon.GetColumn(3);
12.
13.         //unityToVicon_translation.Set(hh.x, hh.y, hh.z);
14.         //unityToVicon_rotation = unityToVicon.rotation;
15.
16.
18.         ///
19.
20.         Vector3 Unity_Pos = virtualFrameTransform.position;
21.         Quaternion Unity_Rot = Quaternion.Normalize(virtualFrameTransform.rotation);
22.
23.         // Vector3 offset_v = new Vector3(2, 2, 5 / 3) * 0.0254f;
24.         //Quaternion offset_q = new Quaternion(offset_v.x, offset_v.y, offset_v.z, 0);
25.         //offset_q = Unity_Rot * offset_q * new Quaternion(-Unity_Rot.x, -Unity_Rot.y, -Unity_Rot.z, Unity_Rot.w);
26.
27.         //Unity_Pos -= new Vector3(offset_q.x, offset_q.y, offset_q.z);
28.         //Unity_Rot = Quaternion.Normalize(Unity_Rot * Quaternion.Normalize(Quaternion.Euler(0, 90, 0)));
29.
30.         Vector3 LHS_Vicon_Pos = new Vector3(viconFrameTransform.position.x, viconFrameTransform.position.y, viconFrameTransform.position.z).Ros2Unity();
31.         Quaternion LHS_Vicon_Rot = new Quaternion(viconFrameTransform.rotation.x, viconFrameTransform.rotation.y, viconFrameTransform.rotation.z, viconFrameTransform.rotation.w).Ros2Unity();
32.
33.
34.         Quaternion LHS_Vicon_Rot_Inv = Quaternion.Normalize(Quaternion.Inverse(LHS_Vicon_Rot));
35.         Quaternion LHS_Vicon_Pos_Q = new Quaternion(LHS_Vicon_Pos.x, LHS_Vicon_Pos.y, LHS_Vicon_Pos.z, 0);
36.         Quaternion v1 = (LHS_Vicon_Rot_Inv * LHS_Vicon_Pos_Q) * new Quaternion(-LHS_Vicon_Rot_Inv.x, -LHS_Vicon_Rot_Inv.y, -LHS_Vicon_Rot_Inv.z, LHS_Vicon_Rot_Inv.w);
37.         Quaternion v2 = (Unity_Rot * v1) * new Quaternion(-Unity_Rot.x, -Unity_Rot.y, -Unity_Rot.z, Unity_Rot.w);
38.
39.         unityToVicon_translation = Unity_Pos - new Vector3(v2.x, v2.y, v2.z);
40.         unityToVicon_rotation = Quaternion.Normalize(new Quaternion(-LHS_Vicon_Rot.x, -LHS_Vicon_Rot.y, -LHS_Vicon_Rot.z, LHS_Vicon_Rot.w) * Unity_Rot);
41.
42.         unityToVicon = Matrix4x4.TRS(unityToVicon_translation, unityToVicon_rotation, new Vector3(1, 1, 1));
43.
44.         Debug.Log("Virtual Frame Object Position: " + virtualFrameTransform.position);
45.         Debug.Log("Virtual Frame Object Rotation: " + virtualFrameTransform.rotation);
46.         Debug.Log("Vicon Frame Object Position: " + viconFrameTransform.position);
47.         Debug.Log("Vicon Frame Object Rotation: " + viconFrameTransform.rotation);
48.
49.         Debug.Log("Output Position: " + unityToVicon_translation);
50.         Debug.Log("Output Rotation: " + unityToVicon_rotation);
51.         ////////
52.
53.         UpdateMapObject();
54.     }
55.
56.     private void UpdateMapObject()
57.     {
58.         if (VICONMapFrame != null)
59.         {
60.             VICONMapFrame.transform.position = unityToVicon_translation;
61.             VICONMapFrame.transform.rotation = unityToVicon_rotation;
62.         }
63.     }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.
Not a member of Pastebin yet?