AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-12 21:00:11 +00:00
Code Issues Releases Wiki Activity

update unity3d/examples/NewBehaviourScript.cs to load a plane and cube, synchronize the world transform

Browse Source 
added debug rendering from CjLib (with DrawLine from my fork here: https://github.com/erwincoumans/unity-cj-lib)
This commit is contained in:
erwincoumans 2017-10-04 12:21:52 -07:00
parent 1b03871b4d
commit 05b71a521d
3 changed files with 350 additions and 284 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

538
examples/pybullet/unity3d/autogen/NativeMethods.cs
View File

File diff suppressed because it is too large Load Diff

84
examples/pybullet/unity3d/examples/NewBehaviourScript.cs
View File

@ -14,25 +14,52 @@ public class NewBehaviourScript : MonoBehaviour {
Text text;
IntPtr sharedAPI;
IntPtr pybullet;
int m_cubeUid;
// Use this for initialization
void Start () {
text = GetComponent<Text>();
pybullet = NativeMethods.b3ConnectSharedMemory(NativeConstants.SHARED_MEMORY_KEY);
if (NativeMethods.b3CanSubmitCommand(pybullet)==0)
{
pybullet = NativeMethods.b3ConnectPhysicsDirect();
}
pybullet = NativeMethods.b3ConnectPhysicsDirect();// NativeMethods.b3ConnectSharedMemory(NativeConstants.SHARED_MEMORY_KEY);
IntPtr cmd = NativeMethods.b3InitResetSimulationCommand(pybullet);
IntPtr status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
{
IntPtr command = NativeMethods.b3InitPhysicsParamCommand(pybullet);
NativeMethods.b3PhysicsParamSetGravity(command, 0, -10, 0);
IntPtr statusHandle = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, command);
}
int numBodies = NativeMethods.b3GetNumBodies(pybullet);
cmd = NativeMethods.b3LoadUrdfCommandInit(pybullet, "plane.urdf");
{
cmd = NativeMethods.b3LoadUrdfCommandInit(pybullet, "plane.urdf");
Quaternion qq = Quaternion.Euler(-90, 0, 0);
NativeMethods.b3LoadUrdfCommandSetStartOrientation(cmd, qq.x, qq.y, qq.z, qq.w);
status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
}
cmd = NativeMethods.b3LoadUrdfCommandInit(pybullet, "cube.urdf");
NativeMethods.b3LoadUrdfCommandSetStartPosition(cmd, 0, 20, 0);
Quaternion q = Quaternion.Euler(35, 0, 0);
NativeMethods.b3LoadUrdfCommandSetStartOrientation(cmd, q.x, q.y, q.z, q.w);
status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
m_cubeUid = NativeMethods.b3GetStatusBodyIndex(status);
EnumSharedMemoryServerStatus statusType = (EnumSharedMemoryServerStatus)NativeMethods.b3GetStatusType(status);
if (statusType == (EnumSharedMemoryServerStatus)EnumSharedMemoryServerStatus.CMD_URDF_LOADING_COMPLETED)
{
numBodies = NativeMethods.b3GetNumBodies(pybullet);
text.text = numBodies.ToString();
cmd = NativeMethods.b3InitRequestVisualShapeInformation(pybullet, 0);
cmd = NativeMethods.b3InitRequestVisualShapeInformation(pybullet, m_cubeUid);
status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
statusType = (EnumSharedMemoryServerStatus) NativeMethods.b3GetStatusType(status);
@ -72,7 +99,11 @@ public class NewBehaviourScript : MonoBehaviour {
}
}
public struct MyPos
{
public double x, y, z;
public double qx, qy, qz, qw;
}
// Update is called once per frame
void Update () {
if (pybullet != IntPtr.Zero)
@ -80,6 +111,51 @@ public class NewBehaviourScript : MonoBehaviour {
IntPtr cmd = NativeMethods.b3InitStepSimulationCommand(pybullet);
IntPtr status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
}
if (m_cubeUid>=0)
{
IntPtr cmd_handle =
NativeMethods.b3RequestActualStateCommandInit(pybullet, m_cubeUid);
IntPtr status_handle = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd_handle);
EnumSharedMemoryServerStatus status_type = (EnumSharedMemoryServerStatus)NativeMethods.b3GetStatusType(status_handle);
if (status_type == EnumSharedMemoryServerStatus.CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
IntPtr p = IntPtr.Zero;
int objUid = 0;
int numDofQ = 0;
int numDofU = 0;
IntPtr inertialFrame = IntPtr.Zero;
IntPtr actualStateQ = IntPtr.Zero;
IntPtr actualStateQdot = IntPtr.Zero;
IntPtr joint_reaction_forces = IntPtr.Zero;
NativeMethods.b3GetStatusActualState(
status_handle, ref objUid, ref numDofQ , ref numDofU,
ref inertialFrame, ref actualStateQ,
ref actualStateQdot , ref joint_reaction_forces);
//Debug.Log("objUid=" + objUid.ToString());
//Debug.Log("numDofQ=" + numDofQ.ToString());
//Debug.Log("numDofU=" + numDofU.ToString());
MyPos mpos = (MyPos)Marshal.PtrToStructure(actualStateQ, typeof(MyPos));
//Debug.Log("pos=(" + mpos.x.ToString()+","+ mpos.y.ToString()+ "," + mpos.z.ToString()+")");
//Debug.Log("orn=(" + mpos.qx.ToString() + "," + mpos.qy.ToString() + "," + mpos.qz.ToString() + mpos.qw.ToString() + ")");
Vector3 pos = new Vector3((float)mpos.x, (float)mpos.y, (float)mpos.z);
Quaternion orn = new Quaternion((float)mpos.qx, (float)mpos.qy, (float)mpos.qz, (float)mpos.qw);
Vector3 dimensions = new Vector3(1, 1, 1);
CjLib.DebugUtil.DrawBox(pos, orn, dimensions, Color.black);
}
}
{
CjLib.DebugUtil.DrawLine(new Vector3(-1, 0, 0), new Vector3(1, 0, 0), Color.red);
CjLib.DebugUtil.DrawLine(new Vector3(0, -1, 0), new Vector3(0, 1, 0), Color.green);
CjLib.DebugUtil.DrawLine(new Vector3(0, 0, -1), new Vector3(0, 0, 1), Color.blue);
}
}
void OnDestroy()

12
examples/pybullet/unity3d/examples/NewBehaviourScript.cs.meta
View File

@ -1,12 +0,0 @@
fileFormatVersion: 2
guid: 6197b3a0389e92c47b7d8698e5f61f06
timeCreated: 1505961790
licenseType: Free
MonoImporter:
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant: