1 using System.Collections; 2 using System.Collections.Generic; 3 using UnityEngine; 4 using UnityEngine.UI; 5 using System.Runtime.InteropServices; 6 using System; 7 8 [System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] 9 10 11 public class NewBehaviourScript : MonoBehaviour { 12 13 14 Text text; 15 IntPtr sharedAPI; 16 IntPtr pybullet; 17 int m_cubeUid; 18 19 // Use this for initialization Start()20 void Start () { 21 text = GetComponent<Text>(); 22 23 24 pybullet = NativeMethods.b3ConnectSharedMemory(NativeConstants.SHARED_MEMORY_KEY); 25 if (NativeMethods.b3CanSubmitCommand(pybullet)==0) 26 { 27 pybullet = NativeMethods.b3ConnectPhysicsDirect(); 28 } 29 30 31 32 IntPtr cmd = NativeMethods.b3InitResetSimulationCommand(pybullet); 33 IntPtr status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd); 34 35 { 36 IntPtr command = NativeMethods.b3InitPhysicsParamCommand(pybullet); 37 NativeMethods.b3PhysicsParamSetGravity(command, 0, -10, 0); 38 IntPtr statusHandle = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, command); 39 } 40 41 int numBodies = NativeMethods.b3GetNumBodies(pybullet); 42 { 43 cmd = NativeMethods.b3LoadUrdfCommandInit(pybullet, "plane.urdf"); 44 Quaternion qq = Quaternion.Euler(-90, 0, 0); 45 NativeMethods.b3LoadUrdfCommandSetStartOrientation(cmd, qq.x, qq.y, qq.z, qq.w); 46 status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd); 47 } 48 49 50 cmd = NativeMethods.b3LoadUrdfCommandInit(pybullet, "cube.urdf"); 51 NativeMethods.b3LoadUrdfCommandSetStartPosition(cmd, 0, 20, 0); 52 Quaternion q = Quaternion.Euler(35, 0, 0); 53 NativeMethods.b3LoadUrdfCommandSetStartOrientation(cmd, q.x, q.y, q.z, q.w); 54 status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd); 55 m_cubeUid = NativeMethods.b3GetStatusBodyIndex(status); 56 57 EnumSharedMemoryServerStatus statusType = (EnumSharedMemoryServerStatus)NativeMethods.b3GetStatusType(status); 58 if (statusType == (EnumSharedMemoryServerStatus)EnumSharedMemoryServerStatus.CMD_URDF_LOADING_COMPLETED) 59 { 60 numBodies = NativeMethods.b3GetNumBodies(pybullet); 61 text.text = numBodies.ToString(); 62 cmd = NativeMethods.b3InitRequestVisualShapeInformation(pybullet, m_cubeUid); 63 status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd); 64 statusType = (EnumSharedMemoryServerStatus) NativeMethods.b3GetStatusType(status); 65 66 if (statusType == (EnumSharedMemoryServerStatus)EnumSharedMemoryServerStatus.CMD_VISUAL_SHAPE_INFO_COMPLETED) 67 { 68 b3VisualShapeInformation visuals = new b3VisualShapeInformation(); 69 NativeMethods.b3GetVisualShapeInformation(pybullet, ref visuals); 70 System.Console.WriteLine("visuals.m_numVisualShapes=" + visuals.m_numVisualShapes); 71 System.IntPtr visualPtr = visuals.m_visualShapeData; 72 73 for (int s = 0; s < visuals.m_numVisualShapes; s++) 74 { 75 b3VisualShapeData visual = (b3VisualShapeData)Marshal.PtrToStructure(visualPtr, typeof(b3VisualShapeData)); 76 visualPtr = new IntPtr(visualPtr.ToInt64()+(Marshal.SizeOf(typeof(b3VisualShapeData)))); 77 double x = visual.m_dimensions[0]; 78 double y = visual.m_dimensions[1]; 79 double z = visual.m_dimensions[2]; 80 System.Console.WriteLine("visual.m_visualGeometryType = " + visual.m_visualGeometryType); 81 System.Console.WriteLine("visual.m_dimensions" + x + "," + y + "," + z); 82 if (visual.m_visualGeometryType == (int)eUrdfGeomTypes.GEOM_MESH) 83 { 84 System.Console.WriteLine("visual.m_meshAssetFileName=" + visual.m_meshAssetFileName); 85 text.text = visual.m_meshAssetFileName; 86 } 87 } 88 } 89 if (numBodies > 0) 90 { 91 92 b3BodyInfo info=new b3BodyInfo(); 93 NativeMethods.b3GetBodyInfo(pybullet, 0, ref info ); 94 95 text.text = info.m_baseName; 96 } 97 98 99 } 100 101 } 102 public struct MyPos 103 { 104 public double x, y, z; 105 public double qx, qy, qz, qw; 106 } 107 // Update is called once per frame Update()108 void Update () { 109 if (pybullet != IntPtr.Zero) 110 { 111 IntPtr cmd = NativeMethods.b3InitStepSimulationCommand(pybullet); 112 IntPtr status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd); 113 } 114 if (m_cubeUid>=0) 115 { 116 IntPtr cmd_handle = 117 NativeMethods.b3RequestActualStateCommandInit(pybullet, m_cubeUid); 118 IntPtr status_handle = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd_handle); 119 120 EnumSharedMemoryServerStatus status_type = (EnumSharedMemoryServerStatus)NativeMethods.b3GetStatusType(status_handle); 121 122 if (status_type == EnumSharedMemoryServerStatus.CMD_ACTUAL_STATE_UPDATE_COMPLETED) 123 { 124 IntPtr p = IntPtr.Zero; 125 int objUid = 0; 126 int numDofQ = 0; 127 int numDofU = 0; 128 IntPtr inertialFrame = IntPtr.Zero; 129 IntPtr actualStateQ = IntPtr.Zero; 130 IntPtr actualStateQdot = IntPtr.Zero; 131 IntPtr joint_reaction_forces = IntPtr.Zero; 132 133 NativeMethods.b3GetStatusActualState( 134 status_handle, ref objUid, ref numDofQ , ref numDofU, 135 ref inertialFrame, ref actualStateQ, 136 ref actualStateQdot , ref joint_reaction_forces); 137 //Debug.Log("objUid=" + objUid.ToString()); 138 //Debug.Log("numDofQ=" + numDofQ.ToString()); 139 //Debug.Log("numDofU=" + numDofU.ToString()); 140 141 MyPos mpos = (MyPos)Marshal.PtrToStructure(actualStateQ, typeof(MyPos)); 142 //Debug.Log("pos=(" + mpos.x.ToString()+","+ mpos.y.ToString()+ "," + mpos.z.ToString()+")"); 143 //Debug.Log("orn=(" + mpos.qx.ToString() + "," + mpos.qy.ToString() + "," + mpos.qz.ToString() + mpos.qw.ToString() + ")"); 144 Vector3 pos = new Vector3((float)mpos.x, (float)mpos.y, (float)mpos.z); 145 Quaternion orn = new Quaternion((float)mpos.qx, (float)mpos.qy, (float)mpos.qz, (float)mpos.qw); 146 Vector3 dimensions = new Vector3(1, 1, 1); 147 CjLib.DebugUtil.DrawBox(pos, orn, dimensions, Color.black); 148 } 149 150 151 } 152 153 { 154 CjLib.DebugUtil.DrawLine(new Vector3(-1, 0, 0), new Vector3(1, 0, 0), Color.red); 155 CjLib.DebugUtil.DrawLine(new Vector3(0, -1, 0), new Vector3(0, 1, 0), Color.green); 156 CjLib.DebugUtil.DrawLine(new Vector3(0, 0, -1), new Vector3(0, 0, 1), Color.blue); 157 } 158 159 } 160 OnDestroy()161 void OnDestroy() 162 { 163 if (pybullet != IntPtr.Zero) 164 { 165 NativeMethods.b3DisconnectSharedMemory(pybullet); 166 } 167 168 } 169 } 170