Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #858 from erwincoumans/master

Browse Source 
pybullet getClosestPoints collision query. center gripper (1 version)
This commit is contained in:
erwincoumans
2016-11-10 12:08:17 -08:00
committed by GitHub
parent e65f9af230 9b5aa9a723
commit 4c2c2bf74b
23 changed files with 1599 additions and 146 deletions
Download Patch File Download Diff File Expand all files Collapse all files

728
data/quadruped/quadruped.urdf Normal file
View File

@@ -0,0 +1,728 @@
<?xml version="0.0" ?>
<robot name="quadruped">
<link name="base_chassis_link">
<visual>
<geometry>
<box size=".226 0.16 .07"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".226 0.16 .07"/>
</geometry>
</collision>
<inertial>
<mass value="1."/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<link name="motor_front_rightR_link">
<visual>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_rightR_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="motor_front_rightR_link"/>
<origin rpy="1.57075 0 0" xyz="0.21 -0.125 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_front_rightL_link">
<visual>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_rightL_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="motor_front_rightL_link"/>
<origin rpy="1.57075 0 0" xyz="0.21 -0.06 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_front_leftR_link">
<visual>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_leftR_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="motor_front_leftR_link"/>
<origin rpy="1.57075 0 0" xyz="0.21 0.125 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_front_leftL_link">
<visual>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_leftL_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="motor_front_leftL_link"/>
<origin rpy="1.57075 0 0" xyz="0.21 0.06 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_back_rightR_link">
<visual>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_rightR_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="motor_back_rightR_link"/>
<origin rpy="1.57075 0 0" xyz="-0.21 -0.125 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_back_rightL_link">
<visual>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_rightL_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="motor_back_rightL_link"/>
<origin rpy="1.57075 0 0" xyz="-0.21 -0.06 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_back_leftR_link">
<visual>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_leftR_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="motor_back_leftR_link"/>
<origin rpy="1.57075 0 0" xyz="-0.21 0.125 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_back_leftL_link">
<visual>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_leftL_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="motor_back_leftL_link"/>
<origin rpy="1.57075 0 0" xyz="-0.21 0.06 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_back_leftL_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_leftL_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_leftL_link"/>
<child link="upper_leg_back_leftL_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.02"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_back_leftR_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="hip_leftR_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_leftR_link"/>
<child link="upper_leg_back_leftR_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 0.02"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_back_leftR_link">
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_back_leftR_link" type="revolute">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_leftR_link"/>
<child link="lower_leg_back_leftR_link"/>
<origin rpy="0 0 0" xyz="0.0 0.0 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_back_leftL_link">
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_back_leftL_link" type="revolute">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_leftL_link"/>
<child link="lower_leg_back_leftL_link"/>
<origin rpy="0 0 0" xyz="0.0 0.0 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_front_leftL_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_leftL_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_leftL_link"/>
<child link="upper_leg_front_leftL_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.02"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_front_leftR_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="hip_front_leftR_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_leftR_link"/>
<child link="upper_leg_front_leftR_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 0.02"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_front_leftR_link">
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_front_leftR_link" type="revolute">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_leftR_link"/>
<child link="lower_leg_front_leftR_link"/>
<origin rpy="0 0 0" xyz="0.0 0.0 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_front_leftL_link">
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_front_leftL_link" type="revolute">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_leftL_link"/>
<child link="lower_leg_front_leftL_link"/>
<origin rpy="0 0 0" xyz="0.0 0.0 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_back_rightL_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_rightL_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_rightL_link"/>
<child link="upper_leg_back_rightL_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 0.02"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_back_rightR_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="hip_rightR_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_rightR_link"/>
<child link="upper_leg_back_rightR_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.02"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_back_rightR_link">
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_back_rightR_link" type="revolute">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_rightR_link"/>
<child link="lower_leg_back_rightR_link"/>
<origin rpy="0 0 0" xyz="0.0 0.0 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_back_rightL_link">
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_back_rightL_link" type="revolute">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_rightL_link"/>
<child link="lower_leg_back_rightL_link"/>
<origin rpy="0 0 0" xyz="0.0 0.0 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_front_rightL_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_rightL_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_rightL_link"/>
<child link="upper_leg_front_rightL_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 0.02"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_front_rightR_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="hip_front_rightR_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_rightR_link"/>
<child link="upper_leg_front_rightR_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.02"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_front_rightR_link">
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_front_rightR_link" type="revolute">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_rightR_link"/>
<child link="lower_leg_front_rightR_link"/>
<origin rpy="0 0 0" xyz="0.0 0.0 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_front_rightL_link">
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_front_rightL_link" type="revolute">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_rightL_link"/>
<child link="lower_leg_front_rightL_link"/>
<origin rpy="0 0 0" xyz="0.0 0.0 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
</robot>

2
examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
View File

@@ -135,7 +135,7 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
if (gFileNameArray.size()==0)
{
gFileNameArray.push_back("r2d2.urdf");
gFileNameArray.push_back("quadruped/quadruped.urdf");
}

2
examples/SharedMemory/PhysicsClient.h
View File

@@ -48,6 +48,8 @@ public:
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData)=0;
virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects) = 0;
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) = 0;
};

69
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1274,7 +1274,69 @@ void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int body
command->m_requestContactPointArguments.m_objectBIndexFilter = bodyUniqueIdB;
}
void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
///compute the closest points between two bodies
b3SharedMemoryCommandHandle b3InitClosestDistanceQuery(b3PhysicsClientHandle physClient)
{
b3SharedMemoryCommandHandle commandHandle =b3InitRequestContactPointInformation(physClient);
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
command->m_updateFlags = CMD_REQUEST_CONTACT_POINT_HAS_QUERY_MODE;
command->m_requestContactPointArguments.m_mode = CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS;
return commandHandle;
}
void b3SetClosestDistanceFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA)
{
b3SetContactFilterBodyA(commandHandle,bodyUniqueIdA);
}
void b3SetClosestDistanceFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB)
{
b3SetContactFilterBodyB(commandHandle,bodyUniqueIdB);
}
void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle commandHandle, double distance)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
command->m_updateFlags += CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD;
command->m_requestContactPointArguments.m_closestDistanceThreshold = distance;
}
///get all the bodies that touch a given axis aligned bounding box specified in world space (min and max coordinates)
b3SharedMemoryCommandHandle b3InitAABBOverlapQuery(b3PhysicsClientHandle physClient, const double aabbMin[3], const double aabbMax[3])
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REQUEST_AABB_OVERLAP;
command->m_updateFlags = 0;
command->m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex = 0;
command->m_requestOverlappingObjectsArgs.m_aabbQueryMin[0] = aabbMin[0];
command->m_requestOverlappingObjectsArgs.m_aabbQueryMin[1] = aabbMin[1];
command->m_requestOverlappingObjectsArgs.m_aabbQueryMin[2] = aabbMin[2];
command->m_requestOverlappingObjectsArgs.m_aabbQueryMax[0] = aabbMax[0];
command->m_requestOverlappingObjectsArgs.m_aabbQueryMax[1] = aabbMax[1];
command->m_requestOverlappingObjectsArgs.m_aabbQueryMax[2] = aabbMax[2];
return (b3SharedMemoryCommandHandle)command;
}
void b3GetAABBOverlapResults(b3PhysicsClientHandle physClient, struct b3AABBOverlapData* data)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl)
{
cl->getCachedOverlappingObjects(data);
}
}
void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData)
@@ -1286,6 +1348,11 @@ void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3Con
}
}
void b3GetClosestPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo)
{
b3GetContactPointInformation(physClient,contactPointInfo);
}
//request visual shape information

11
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -99,6 +99,17 @@ void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int body
void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo);
///compute the closest points between two bodies
b3SharedMemoryCommandHandle b3InitClosestDistanceQuery(b3PhysicsClientHandle physClient);
void b3SetClosestDistanceFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA);
void b3SetClosestDistanceFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle commandHandle, double distance);
void b3GetClosestPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo);
///get all the bodies that touch a given axis aligned bounding box specified in world space (min and max coordinates)
b3SharedMemoryCommandHandle b3InitAABBOverlapQuery(b3PhysicsClientHandle physClient, const double aabbMin[3],const double aabbMax[3]);
void b3GetAABBOverlapResults(b3PhysicsClientHandle physClient, struct b3AABBOverlapData* data);
//request visual shape information
b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA);
void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo);

46
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -40,7 +40,7 @@ struct PhysicsClientSharedMemoryInternalData {
btAlignedObjectArray<int> m_cachedSegmentationMaskBuffer;
btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
btAlignedObjectArray<b3OverlappingObject> m_cachedOverlappingObjects;
btAlignedObjectArray<b3VisualShapeData> m_cachedVisualShapes;
btAlignedObjectArray<int> m_bodyIdsRequestInfo;
@@ -596,6 +596,30 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
b3Warning("Inverse Dynamics computations failed");
break;
}
case CMD_REQUEST_AABB_OVERLAP_FAILED:
{
b3Warning("Overlapping object query failed");
break;
}
case CMD_REQUEST_AABB_OVERLAP_COMPLETED:
{
if (m_data->m_verboseOutput)
{
b3Printf("Overlapping object request completed");
}
int startOverlapIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex;
int numOverlapCopied = serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied;
m_data->m_cachedOverlappingObjects.resize(startOverlapIndex + numOverlapCopied);
b3OverlappingObject* objects = (b3OverlappingObject*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
for (int i = 0; i < numOverlapCopied; i++)
{
m_data->m_cachedOverlappingObjects[startOverlapIndex + i] = objects[i];
}
break;
}
case CMD_CONTACT_POINT_INFORMATION_COMPLETED:
{
if (m_data->m_verboseOutput)
@@ -741,6 +765,18 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
}
}
if (serverCmd.m_type == CMD_REQUEST_AABB_OVERLAP_COMPLETED)
{
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
if (serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects > 0 && serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied)
{
command.m_type = CMD_REQUEST_AABB_OVERLAP;
command.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex + serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied;
submitClientCommand(command);
return 0;
}
}
if (serverCmd.m_type == CMD_CONTACT_POINT_INFORMATION_COMPLETED)
{
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
@@ -873,6 +909,14 @@ void PhysicsClientSharedMemory::getCachedContactPointInformation(struct b3Contac
}
void PhysicsClientSharedMemory::getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects)
{
overlappingObjects->m_numOverlappingObjects = m_data->m_cachedOverlappingObjects.size();
overlappingObjects->m_overlappingObjects = m_data->m_cachedOverlappingObjects.size() ?
&m_data->m_cachedOverlappingObjects[0] : 0;
}
void PhysicsClientSharedMemory::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo)
{
visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size();

2
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@@ -57,6 +57,8 @@ public:
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects);
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
};

68
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -44,6 +44,7 @@ struct PhysicsDirectInternalData
btAlignedObjectArray<int> m_cachedSegmentationMask;
btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
btAlignedObjectArray<b3OverlappingObject> m_cachedOverlappingObjects;
btAlignedObjectArray<b3VisualShapeData> m_cachedVisualShapes;
@@ -309,6 +310,60 @@ bool PhysicsDirect::processVisualShapeData(const struct SharedMemoryCommand& org
return m_data->m_hasStatus;
}
bool PhysicsDirect::processOverlappingObjects(const struct SharedMemoryCommand& orgCommand)
{
SharedMemoryCommand command = orgCommand;
const SharedMemoryStatus& serverCmd = m_data->m_serverStatus;
do
{
bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
int timeout = 1024 * 1024 * 1024;
while ((!hasStatus) && (timeout-- > 0))
{
const SharedMemoryStatus* stat = processServerStatus();
if (stat)
{
hasStatus = true;
}
}
m_data->m_hasStatus = hasStatus;
if (hasStatus)
{
if (m_data->m_verboseOutput)
{
b3Printf("Overlapping Objects Request OK\n");
}
int startOverlapIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex;
int numOverlapCopied = serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied;
m_data->m_cachedOverlappingObjects.resize(startOverlapIndex + numOverlapCopied);
b3OverlappingObject* objects = (b3OverlappingObject*)&m_data->m_bulletStreamDataServerToClient[0];
for (int i = 0; i < numOverlapCopied; i++)
{
m_data->m_cachedOverlappingObjects[startOverlapIndex + i] = objects[i];
}
if (serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects > 0 && serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied)
{
m_data->m_hasStatus = false;
command.m_type = CMD_REQUEST_AABB_OVERLAP;
command.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex + serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied;
}
}
} while (serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects > 0 && serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied);
return m_data->m_hasStatus;
}
bool PhysicsDirect::processContactPointData(const struct SharedMemoryCommand& orgCommand)
{
@@ -525,6 +580,7 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
{
switch (serverCmd.m_type)
{
case CMD_REQUEST_INTERNAL_DATA_COMPLETED:
{
if (serverCmd.m_numDataStreamBytes)
@@ -635,6 +691,10 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
{
return processVisualShapeData(command);
}
if (command.m_type == CMD_REQUEST_AABB_OVERLAP)
{
return processOverlappingObjects(command);
}
bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
m_data->m_hasStatus = hasStatus;
@@ -761,6 +821,14 @@ void PhysicsDirect::getCachedContactPointInformation(struct b3ContactInformation
}
void PhysicsDirect::getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects)
{
overlappingObjects->m_numOverlappingObjects = m_data->m_cachedOverlappingObjects.size();
overlappingObjects->m_overlappingObjects = m_data->m_cachedOverlappingObjects.size() ?
&m_data->m_cachedOverlappingObjects[0] : 0;
}
void PhysicsDirect::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo)
{
visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size();

5
examples/SharedMemory/PhysicsDirect.h
View File

@@ -20,6 +20,8 @@ protected:
bool processContactPointData(const struct SharedMemoryCommand& orgCommand);
bool processOverlappingObjects(const struct SharedMemoryCommand& orgCommand);
bool processVisualShapeData(const struct SharedMemoryCommand& orgCommand);
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
@@ -75,8 +77,11 @@ public:
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects);
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
//those 2 APIs are for internal use for visualization
virtual bool connect(struct GUIHelperInterface* guiHelper);
virtual void renderScene();

6
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@@ -145,3 +145,9 @@ void PhysicsLoopBack::getCachedVisualShapeInformation(struct b3VisualShapeInform
{
return m_data->m_physicsClient->getCachedVisualShapeInformation(visualShapesInfo);
}
void PhysicsLoopBack::getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects)
{
return m_data->m_physicsClient->getCachedOverlappingObjects(overlappingObjects);
}

2
examples/SharedMemory/PhysicsLoopBack.h
View File

@@ -62,6 +62,8 @@ public:
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects);
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
};

436
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -318,6 +318,45 @@ struct SaveWorldObjectData
std::string m_fileName;
};
struct MyBroadphaseCallback : public btBroadphaseAabbCallback
{
b3AlignedObjectArray<int> m_bodyUniqueIds;
b3AlignedObjectArray<int> m_links;
MyBroadphaseCallback()
{
}
virtual ~MyBroadphaseCallback()
{
}
void clear()
{
m_bodyUniqueIds.clear();
m_links.clear();
}
virtual bool process(const btBroadphaseProxy* proxy)
{
btCollisionObject* colObj = (btCollisionObject*)proxy->m_clientObject;
btMultiBodyLinkCollider* mbl = btMultiBodyLinkCollider::upcast(colObj);
if (mbl)
{
int bodyUniqueId = mbl->m_multiBody->getUserIndex2();
m_bodyUniqueIds.push_back(bodyUniqueId);
m_links.push_back(mbl->m_link);
return true;
}
int bodyUniqueId = colObj->getUserIndex2();
if (bodyUniqueId >= 0)
{
m_bodyUniqueIds.push_back(bodyUniqueId);
m_links.push_back(mbl->m_link);
}
return true;
}
};
struct PhysicsServerCommandProcessorInternalData
{
///handle management
@@ -453,6 +492,8 @@ struct PhysicsServerCommandProcessorInternalData
SharedMemoryDebugDrawer* m_remoteDebugDrawer;
btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
MyBroadphaseCallback m_cachedOverlappingObjects;
btAlignedObjectArray<int> m_sdfRecentLoadedBodies;
@@ -2535,92 +2576,321 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
hasStatus = true;
break;
}
case CMD_REQUEST_AABB_OVERLAP:
{
SharedMemoryStatus& serverCmd = serverStatusOut;
int curObjectIndex = clientCmd.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex;
if (0== curObjectIndex)
{
//clientCmd.m_requestContactPointArguments.m_aabbQueryMin
btVector3 aabbMin, aabbMax;
aabbMin.setValue(clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMin[0],
clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMin[1],
clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMin[2]);
aabbMax.setValue(clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMax[0],
clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMax[1],
clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMax[2]);
m_data->m_cachedOverlappingObjects.clear();
m_data->m_dynamicsWorld->getBroadphase()->aabbTest(aabbMin, aabbMax, m_data->m_cachedOverlappingObjects);
}
int totalBytesPerObject = sizeof(b3OverlappingObject);
int overlapCapacity = bufferSizeInBytes / totalBytesPerObject - 1;
int numOverlap = m_data->m_cachedOverlappingObjects.m_bodyUniqueIds.size();
int remainingObjects = numOverlap - curObjectIndex;
int curNumObjects = btMin(overlapCapacity, remainingObjects);
if (numOverlap < overlapCapacity)
{
b3OverlappingObject* overlapStorage = (b3OverlappingObject*)bufferServerToClient;
for (int i = 0; i < m_data->m_cachedOverlappingObjects.m_bodyUniqueIds.size(); i++)
{
overlapStorage[i].m_objectUniqueId = m_data->m_cachedOverlappingObjects.m_bodyUniqueIds[i];
overlapStorage[i].m_linkIndex = m_data->m_cachedOverlappingObjects.m_links[i];
}
serverCmd.m_type = CMD_REQUEST_AABB_OVERLAP_COMPLETED;
int m_startingOverlappingObjectIndex;
int m_numOverlappingObjectsCopied;
int m_numRemainingOverlappingObjects;
serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex = clientCmd.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex;
serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied = m_data->m_cachedOverlappingObjects.m_bodyUniqueIds.size();
serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects = remainingObjects - curNumObjects;
}
else
{
serverCmd.m_type = CMD_REQUEST_AABB_OVERLAP_FAILED;
}
hasStatus = true;
break;
}
case CMD_REQUEST_CONTACT_POINT_INFORMATION:
{
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_sendContactPointArgs.m_numContactPointsCopied = 0;
//make a snapshot of the contact manifolds into individual contact points
if (clientCmd.m_requestContactPointArguments.m_startingContactPointIndex==0)
{
int numContactManifolds = m_data->m_dynamicsWorld->getDispatcher()->getNumManifolds();
if (clientCmd.m_requestContactPointArguments.m_startingContactPointIndex == 0)
{
m_data->m_cachedContactPoints.resize(0);
m_data->m_cachedContactPoints.reserve(numContactManifolds*4);
for (int i=0;i<numContactManifolds;i++)
{
const btPersistentManifold* manifold = m_data->m_dynamicsWorld->getDispatcher()->getInternalManifoldPointer()[i];
int linkIndexA = -1;
int linkIndexB = -1;
int objectIndexB = -1;
int mode = CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS;
const btRigidBody* bodyB = btRigidBody::upcast(manifold->getBody1());
if (bodyB)
if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_QUERY_MODE)
{
mode = clientCmd.m_requestContactPointArguments.m_mode;
}
switch (mode)
{
case CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS:
{
int numContactManifolds = m_data->m_dynamicsWorld->getDispatcher()->getNumManifolds();
m_data->m_cachedContactPoints.reserve(numContactManifolds * 4);
for (int i = 0; i < numContactManifolds; i++)
{
objectIndexB = bodyB->getUserIndex2();
}
const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
if (mblB && mblB->m_multiBody)
{
linkIndexB = mblB->m_link;
objectIndexB = mblB->m_multiBody->getUserIndex2();
}
const btPersistentManifold* manifold = m_data->m_dynamicsWorld->getDispatcher()->getInternalManifoldPointer()[i];
int linkIndexA = -1;
int linkIndexB = -1;
int objectIndexA = -1;
const btRigidBody* bodyA = btRigidBody::upcast(manifold->getBody0());
if (bodyA)
{
objectIndexA = bodyA->getUserIndex2();
}
const btMultiBodyLinkCollider* mblA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
if (mblA && mblA->m_multiBody)
{
linkIndexA = mblA->m_link;
int objectIndexB = -1;
objectIndexA = mblA->m_multiBody->getUserIndex2();
}
btAssert(bodyA || mblA);
//apply the filter, if the user provides it
if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter>=0)
{
if ((clientCmd.m_requestContactPointArguments.m_objectAIndexFilter != objectIndexA) &&
(clientCmd.m_requestContactPointArguments.m_objectAIndexFilter != objectIndexB))
continue;
}
//apply the second object filter, if the user provides it
if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter>=0)
{
if ((clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexA) &&
(clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexB))
continue;
}
for (int p=0;p<manifold->getNumContacts();p++)
{
b3ContactPointData pt;
pt.m_bodyUniqueIdA = objectIndexA;
pt.m_bodyUniqueIdB = objectIndexB;
const btManifoldPoint& srcPt = manifold->getContactPoint(p);
pt.m_contactDistance = srcPt.getDistance();
pt.m_contactFlags = 0;
pt.m_linkIndexA = linkIndexA;
pt.m_linkIndexB = linkIndexB;
for (int j=0;j<3;j++)
const btRigidBody* bodyB = btRigidBody::upcast(manifold->getBody1());
if (bodyB)
{
pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j];
pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j];
pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j];
objectIndexB = bodyB->getUserIndex2();
}
const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
if (mblB && mblB->m_multiBody)
{
linkIndexB = mblB->m_link;
objectIndexB = mblB->m_multiBody->getUserIndex2();
}
int objectIndexA = -1;
const btRigidBody* bodyA = btRigidBody::upcast(manifold->getBody0());
if (bodyA)
{
objectIndexA = bodyA->getUserIndex2();
}
const btMultiBodyLinkCollider* mblA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
if (mblA && mblA->m_multiBody)
{
linkIndexA = mblA->m_link;
objectIndexA = mblA->m_multiBody->getUserIndex2();
}
btAssert(bodyA || mblA);
//apply the filter, if the user provides it
if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0)
{
if ((clientCmd.m_requestContactPointArguments.m_objectAIndexFilter != objectIndexA) &&
(clientCmd.m_requestContactPointArguments.m_objectAIndexFilter != objectIndexB))
continue;
}
//apply the second object filter, if the user provides it
if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter >= 0)
{
if ((clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexA) &&
(clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexB))
continue;
}
for (int p = 0; p < manifold->getNumContacts(); p++)
{
b3ContactPointData pt;
pt.m_bodyUniqueIdA = objectIndexA;
pt.m_bodyUniqueIdB = objectIndexB;
const btManifoldPoint& srcPt = manifold->getContactPoint(p);
pt.m_contactDistance = srcPt.getDistance();
pt.m_contactFlags = 0;
pt.m_linkIndexA = linkIndexA;
pt.m_linkIndexB = linkIndexB;
for (int j = 0; j < 3; j++)
{
pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j];
pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j];
pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j];
}
pt.m_normalForce = srcPt.getAppliedImpulse() / m_data->m_physicsDeltaTime;
// pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1;
m_data->m_cachedContactPoints.push_back(pt);
}
pt.m_normalForce = srcPt.getAppliedImpulse()/m_data->m_physicsDeltaTime;
// pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1;
m_data->m_cachedContactPoints.push_back (pt);
}
}
}
break;
}
case CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS:
{
//todo(erwincoumans) compute closest points between all, and vs all, pair
btScalar closestDistanceThreshold = 0.f;
if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD)
{
closestDistanceThreshold = clientCmd.m_requestContactPointArguments.m_closestDistanceThreshold;
}
int bodyUniqueIdA = clientCmd.m_requestContactPointArguments.m_objectAIndexFilter;
int bodyUniqueIdB = clientCmd.m_requestContactPointArguments.m_objectBIndexFilter;
btAlignedObjectArray<btCollisionObject*> setA;
btAlignedObjectArray<btCollisionObject*> setB;
btAlignedObjectArray<int> setALinkIndex;
btAlignedObjectArray<int> setBLinkIndex;
if (bodyUniqueIdA >= 0)
{
InteralBodyData* bodyA = m_data->getHandle(bodyUniqueIdA);
if (bodyA)
{
if (bodyA->m_multiBody)
{
if (bodyA->m_multiBody->getBaseCollider())
{
setA.push_back(bodyA->m_multiBody->getBaseCollider());
setALinkIndex.push_back(-1);
}
for (int i = 0; i < bodyA->m_multiBody->getNumLinks(); i++)
{
if (bodyA->m_multiBody->getLink(i).m_collider)
{
setA.push_back(bodyA->m_multiBody->getLink(i).m_collider);
setALinkIndex.push_back(i);
}
}
}
if (bodyA->m_rigidBody)
{
setA.push_back(bodyA->m_rigidBody);
setALinkIndex.push_back(-1);
}
}
}
if (bodyUniqueIdB>=0)
{
InteralBodyData* bodyB = m_data->getHandle(bodyUniqueIdB);
if (bodyB)
{
if (bodyB->m_multiBody)
{
if (bodyB->m_multiBody->getBaseCollider())
{
setB.push_back(bodyB->m_multiBody->getBaseCollider());
setBLinkIndex.push_back(-1);
}
for (int i = 0; i < bodyB->m_multiBody->getNumLinks(); i++)
{
if (bodyB->m_multiBody->getLink(i).m_collider)
{
setB.push_back(bodyB->m_multiBody->getLink(i).m_collider);
setBLinkIndex.push_back(i);
}
}
}
if (bodyB->m_rigidBody)
{
setB.push_back(bodyB->m_rigidBody);
setBLinkIndex.push_back(-1);
}
}
}
{
///ContactResultCallback is used to report contact points
struct MyContactResultCallback : public btCollisionWorld::ContactResultCallback
{
//short int m_collisionFilterGroup;
//short int m_collisionFilterMask;
int m_bodyUniqueIdA;
int m_bodyUniqueIdB;
int m_linkIndexA;
int m_linkIndexB;
btScalar m_deltaTime;
btAlignedObjectArray<b3ContactPointData>& m_cachedContactPoints;
MyContactResultCallback(btAlignedObjectArray<b3ContactPointData>& pointCache)
:m_cachedContactPoints(pointCache)
{
}
virtual ~MyContactResultCallback()
{
}
virtual bool needsCollision(btBroadphaseProxy* proxy0) const
{
//bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0;
//collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask);
//return collides;
return true;
}
virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1)
{
b3ContactPointData pt;
pt.m_bodyUniqueIdA = m_bodyUniqueIdA;
pt.m_bodyUniqueIdB = m_bodyUniqueIdB;
const btManifoldPoint& srcPt = cp;
pt.m_contactDistance = srcPt.getDistance();
pt.m_contactFlags = 0;
pt.m_linkIndexA = m_linkIndexA;
pt.m_linkIndexB = m_linkIndexB;
for (int j = 0; j < 3; j++)
{
pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j];
pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j];
pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j];
}
pt.m_normalForce = srcPt.getAppliedImpulse() / m_deltaTime;
// pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1;
m_cachedContactPoints.push_back(pt);
return 1;
}
};
MyContactResultCallback cb(m_data->m_cachedContactPoints);
cb.m_bodyUniqueIdA = bodyUniqueIdA;
cb.m_bodyUniqueIdB = bodyUniqueIdB;
cb.m_deltaTime = m_data->m_physicsDeltaTime;
for (int i = 0; i < setA.size(); i++)
{
cb.m_linkIndexA = setALinkIndex[i];
for (int j = 0; j < setB.size(); j++)
{
cb.m_linkIndexB = setBLinkIndex[j];
cb.m_closestDistanceThreshold = closestDistanceThreshold;
this->m_data->m_dynamicsWorld->contactPairTest(setA[i], setB[j], cb);
}
}
}
break;
}
default:
{
b3Warning("Unknown contact query mode: %d", mode);
}
}
}
int numContactPoints = m_data->m_cachedContactPoints.size();
@@ -3588,6 +3858,8 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
{
m_data->m_gripperRigidbodyFixed->setFrameInB(btMatrix3x3(gVRGripperOrn));
m_data->m_gripperRigidbodyFixed->setPivotInB(gVRGripperPos);
btScalar avg = 0.f;
for (int i = 0; i < m_data->m_gripperMultiBody->getNumLinks(); i++)
{
if (supportsJointMotor(m_data->m_gripperMultiBody, i))
@@ -3599,6 +3871,13 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
btScalar posTarget = 0.1 + (1 - btMin(btScalar(0.75),gVRGripperAnalog)*btScalar(1.5))*SIMD_HALF_PI*0.29;
btScalar maxPosTarget = 0.55;
btScalar correction = 0.f;
if (avg)
{
correction = m_data->m_gripperMultiBody->getJointPos(i) - avg;
}
if (m_data->m_gripperMultiBody->getJointPos(i) < 0)
{
m_data->m_gripperMultiBody->setJointPos(i,0);
@@ -3608,10 +3887,19 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
m_data->m_gripperMultiBody->setJointPos(i, maxPosTarget);
}
motor->setPositionTarget(posTarget, 1);
if (avg)
{
motor->setPositionTarget(avg, 1);
}
else
{
motor->setPositionTarget(posTarget, 1);
}
motor->setVelocityTarget(0, 0.5);
btScalar maxImp = 1*m_data->m_physicsDeltaTime;
btScalar maxImp = (1+0.1*i)*m_data->m_physicsDeltaTime;
motor->setMaxAppliedImpulse(maxImp);
avg = m_data->m_gripperMultiBody->getJointPos(i);
//motor->setRhsClamp(gRhsClamp);
}
}
@@ -3619,7 +3907,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
}
// Inverse kinematics for KUKA
//if (0)
if (m_data->m_KukaId>=0)
{
InternalBodyHandle* bodyHandle = m_data->getHandle(m_data->m_KukaId);
if (bodyHandle && bodyHandle->m_multiBody && bodyHandle->m_multiBody->getNumDofs()==7)

17
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -969,6 +969,23 @@ void PhysicsServerExample::renderScene()
static char line0[1024];
static char line1[1024];
//draw all user-debug-lines
//add array of lines
//draw all user- 'text3d' messages
if (m_guiHelper)
{
btVector3 from(0, 0, 0);
btVector3 toX(1, 1, 1);
btVector3 color(0, 1, 0);
double width = 2;
m_guiHelper->getAppInterface()->m_renderer->drawLine(from, toX, color, width);
m_guiHelper->getAppInterface()->drawText3D("hi", 1, 1, 1, 1);
}
if (gEnableRealTimeSimVR)
{

24
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -143,11 +143,26 @@ enum EnumRequestPixelDataUpdateFlags
};
enum EnumRequestContactDataUpdateFlags
{
CMD_REQUEST_CONTACT_POINT_HAS_QUERY_MODE=1,
CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD=2,
};
struct RequestContactDataArgs
{
int m_startingContactPointIndex;
int m_objectAIndexFilter;
int m_objectBIndexFilter;
double m_closestDistanceThreshold;
int m_mode;
};
struct RequestOverlappingObjectsArgs
{
int m_startingOverlappingObjectIndex;
double m_aabbQueryMin[3];
double m_aabbQueryMax[3];
};
struct RequestVisualShapeDataArgs
@@ -510,6 +525,7 @@ struct SharedMemoryCommand
struct CalculateJacobianArgs m_calculateJacobianArguments;
struct CreateJointArgs m_createJointArguments;
struct RequestContactDataArgs m_requestContactPointArguments;
struct RequestOverlappingObjectsArgs m_requestOverlappingObjectsArgs;
struct RequestVisualShapeDataArgs m_requestVisualShapeDataArguments;
struct UpdateVisualShapeDataArgs m_updateVisualShapeDataArguments;
struct LoadTextureArgs m_loadTextureArguments;
@@ -530,6 +546,13 @@ struct SendContactDataArgs
int m_numRemainingContactPoints;
};
struct SendOverlappingObjectsArgs
{
int m_startingOverlappingObjectIndex;
int m_numOverlappingObjectsCopied;
int m_numRemainingOverlappingObjects;
};
struct SharedMemoryStatus
{
int m_type;
@@ -552,6 +575,7 @@ struct SharedMemoryStatus
struct CalculateInverseDynamicsResultArgs m_inverseDynamicsResultArgs;
struct CalculateJacobianResultArgs m_jacobianResultArgs;
struct SendContactDataArgs m_sendContactPointArgs;
struct SendOverlappingObjectsArgs m_sendOverlappingObjectsArgs;
struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs;
struct SendVisualShapeDataArgs m_sendVisualShapeArgs;
};

22
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -34,6 +34,7 @@ enum EnumSharedMemoryClientCommand
CMD_CALCULATE_JACOBIAN,
CMD_CREATE_JOINT,
CMD_REQUEST_CONTACT_POINT_INFORMATION,
CMD_REQUEST_AABB_OVERLAP,
CMD_SAVE_WORLD,
CMD_REQUEST_VISUAL_SHAPE_INFO,
CMD_UPDATE_VISUAL_SHAPE,
@@ -80,6 +81,8 @@ enum EnumSharedMemoryServerStatus
CMD_CALCULATED_JACOBIAN_FAILED,
CMD_CONTACT_POINT_INFORMATION_COMPLETED,
CMD_CONTACT_POINT_INFORMATION_FAILED,
CMD_REQUEST_AABB_OVERLAP_COMPLETED,
CMD_REQUEST_AABB_OVERLAP_FAILED,
CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED,
CMD_CALCULATE_INVERSE_KINEMATICS_FAILED,
CMD_SAVE_WORLD_COMPLETED,
@@ -158,6 +161,18 @@ struct b3DebugLines
const float* m_linesColor;//float red,green,blue times 'm_numDebugLines'.
};
struct b3OverlappingObject
{
int m_objectUniqueId;
int m_linkIndex;
};
struct b3AABBOverlapData
{
int m_numOverlappingObjects;
struct b3OverlappingObject* m_overlappingObjects;
};
struct b3CameraImageData
{
int m_pixelWidth;
@@ -192,6 +207,13 @@ struct b3ContactPointData
// double m_angularFrictionForce;
};
enum
{
CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS = 0,
CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS = 1,
};
struct b3ContactInformation
{

253
examples/pybullet/pybullet.c
View File

@@ -680,6 +680,11 @@ static int pybullet_internalGetBasePositionAndOrientation(
baseOrientation[2] = 0.;
baseOrientation[3] = 1.;
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return 0;
}
{
{
b3SharedMemoryCommandHandle cmd_handle =
@@ -1352,6 +1357,10 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args)
int i;
PyObject* pyResultList = 0;
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (size == 1)
{
if (!PyArg_ParseTuple(args, "i", &objectUniqueId)) {
@@ -1450,6 +1459,11 @@ static PyObject* pybullet_resetVisualShapeData(PyObject* self, PyObject* args)
b3SharedMemoryStatusHandle statusHandle;
int statusType;
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (size == 4)
{
if (!PyArg_ParseTuple(args, "iiii", &objectUniqueId, &jointIndex, &shapeIndex, &textureUniqueId)) {
@@ -1487,6 +1501,11 @@ static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args)
b3SharedMemoryStatusHandle statusHandle;
int statusType;
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (size == 1)
{
if (!PyArg_ParseTuple(args, "s", &filename)) {
@@ -1516,38 +1535,10 @@ static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args)
return Py_None;
}
static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) {
int size = PySequence_Size(args);
int objectUniqueIdA = -1;
int objectUniqueIdB = -1;
b3SharedMemoryCommandHandle commandHandle;
struct b3ContactInformation contactPointData;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
int i;
PyObject* pyResultList = 0;
if (size == 1) {
if (!PyArg_ParseTuple(args, "i", &objectUniqueIdA)) {
PyErr_SetString(SpamError, "Error parsing object unique id");
return NULL;
}
}
if (size == 2) {
if (!PyArg_ParseTuple(args, "ii", &objectUniqueIdA, &objectUniqueIdB)) {
PyErr_SetString(SpamError, "Error parsing object unique id");
return NULL;
}
}
commandHandle = b3InitRequestContactPointInformation(sm);
b3SetContactFilterBodyA(commandHandle, objectUniqueIdA);
b3SetContactFilterBodyB(commandHandle, objectUniqueIdB);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) {
/*
static PyObject* MyConvertContactPoint( struct b3ContactInformation* contactPointPtr)
{
/*
0 int m_contactFlags;
1 int m_bodyUniqueIdA;
2 int m_bodyUniqueIdB;
@@ -1565,66 +1556,208 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) {
15 double m_normalForce;
*/
b3GetContactPointInformation(sm, &contactPointData);
pyResultList = PyTuple_New(contactPointData.m_numContactPoints);
for (i = 0; i < contactPointData.m_numContactPoints; i++) {
int i;
PyObject* pyResultList = PyTuple_New(contactPointPtr->m_numContactPoints);
for (i = 0; i < contactPointPtr->m_numContactPoints; i++) {
PyObject* contactObList = PyTuple_New(16); // see above 16 fields
PyObject* item;
item =
PyInt_FromLong(contactPointData.m_contactPointData[i].m_contactFlags);
PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_contactFlags);
PyTuple_SetItem(contactObList, 0, item);
item = PyInt_FromLong(
contactPointData.m_contactPointData[i].m_bodyUniqueIdA);
contactPointPtr->m_contactPointData[i].m_bodyUniqueIdA);
PyTuple_SetItem(contactObList, 1, item);
item = PyInt_FromLong(
contactPointData.m_contactPointData[i].m_bodyUniqueIdB);
contactPointPtr->m_contactPointData[i].m_bodyUniqueIdB);
PyTuple_SetItem(contactObList, 2, item);
item =
PyInt_FromLong(contactPointData.m_contactPointData[i].m_linkIndexA);
PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_linkIndexA);
PyTuple_SetItem(contactObList, 3, item);
item =
PyInt_FromLong(contactPointData.m_contactPointData[i].m_linkIndexB);
PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_linkIndexB);
PyTuple_SetItem(contactObList, 4, item);
item = PyFloat_FromDouble(
contactPointData.m_contactPointData[i].m_positionOnAInWS[0]);
contactPointPtr->m_contactPointData[i].m_positionOnAInWS[0]);
PyTuple_SetItem(contactObList, 5, item);
item = PyFloat_FromDouble(
contactPointData.m_contactPointData[i].m_positionOnAInWS[1]);
contactPointPtr->m_contactPointData[i].m_positionOnAInWS[1]);
PyTuple_SetItem(contactObList, 6, item);
item = PyFloat_FromDouble(
contactPointData.m_contactPointData[i].m_positionOnAInWS[2]);
contactPointPtr->m_contactPointData[i].m_positionOnAInWS[2]);
PyTuple_SetItem(contactObList, 7, item);
item = PyFloat_FromDouble(
contactPointData.m_contactPointData[i].m_positionOnBInWS[0]);
contactPointPtr->m_contactPointData[i].m_positionOnBInWS[0]);
PyTuple_SetItem(contactObList, 8, item);
item = PyFloat_FromDouble(
contactPointData.m_contactPointData[i].m_positionOnBInWS[1]);
contactPointPtr->m_contactPointData[i].m_positionOnBInWS[1]);
PyTuple_SetItem(contactObList, 9, item);
item = PyFloat_FromDouble(
contactPointData.m_contactPointData[i].m_positionOnBInWS[2]);
contactPointPtr->m_contactPointData[i].m_positionOnBInWS[2]);
PyTuple_SetItem(contactObList, 10, item);
item = PyFloat_FromDouble(
contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[0]);
contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[0]);
PyTuple_SetItem(contactObList, 11, item);
item = PyFloat_FromDouble(
contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[1]);
contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[1]);
PyTuple_SetItem(contactObList, 12, item);
item = PyFloat_FromDouble(
contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[2]);
contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[2]);
PyTuple_SetItem(contactObList, 13, item);
item = PyFloat_FromDouble(
contactPointData.m_contactPointData[i].m_contactDistance);
contactPointPtr->m_contactPointData[i].m_contactDistance);
PyTuple_SetItem(contactObList, 14, item);
item = PyFloat_FromDouble(
contactPointData.m_contactPointData[i].m_normalForce);
contactPointPtr->m_contactPointData[i].m_normalForce);
PyTuple_SetItem(contactObList, 15, item);
PyTuple_SetItem(pyResultList, i, contactObList);
}
return pyResultList;
}
static PyObject* pybullet_getOverlappingObjects(PyObject* self, PyObject* args, PyObject *keywds)
{
PyObject* aabbMinOb=0, *aabbMaxOb=0;
double aabbMin[3];
double aabbMax[3];
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
struct b3AABBOverlapData overlapData;
int i;
static char *kwlist[] = { "aabbMin", "aabbMax", NULL };
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO", kwlist,
&aabbMinOb, &aabbMaxOb))
return NULL;
pybullet_internalSetVectord(aabbMinOb, aabbMin);
pybullet_internalSetVectord(aabbMaxOb, aabbMax);
commandHandle = b3InitAABBOverlapQuery(sm, aabbMin, aabbMax);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
b3GetAABBOverlapResults(sm, &overlapData);
if (overlapData.m_numOverlappingObjects)
{
PyObject* pyResultList = PyTuple_New(overlapData.m_numOverlappingObjects);
//For huge amount of overlap, we could use numpy instead (see camera pixel data)
//What would Python do with huge amount of data? Pass it onto TensorFlow!
for (i = 0; i < overlapData.m_numOverlappingObjects; i++) {
PyObject* overlap = PyTuple_New(2);//body unique id and link index
PyObject* item;
item =
PyInt_FromLong(overlapData.m_overlappingObjects[i].m_objectUniqueId);
PyTuple_SetItem(overlap, 0, item);
item =
PyInt_FromLong(overlapData.m_overlappingObjects[i].m_linkIndex);
PyTuple_SetItem(overlap, 1, item);
PyTuple_SetItem(pyResultList, i, overlap);
}
return pyResultList;
}
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, PyObject *keywds)
{
int size = PySequence_Size(args);
int bodyUniqueIdA = -1;
int bodyUniqueIdB = -1;
double distanceThreshold = 0.f;
b3SharedMemoryCommandHandle commandHandle;
struct b3ContactInformation contactPointData;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
PyObject* pyResultList = 0;
static char *kwlist[] = { "bodyA", "bodyB", "distance", NULL };
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid", kwlist,
&bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold))
return NULL;
commandHandle = b3InitClosestDistanceQuery(sm);
b3SetClosestDistanceFilterBodyA(commandHandle, bodyUniqueIdA);
b3SetClosestDistanceFilterBodyB(commandHandle, bodyUniqueIdB);
b3SetClosestDistanceThreshold(commandHandle, distanceThreshold);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) {
b3GetContactPointInformation(sm, &contactPointData);
return MyConvertContactPoint(&contactPointData);
}
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, PyObject *keywds) {
int size = PySequence_Size(args);
int bodyUniqueIdA = -1;
int bodyUniqueIdB = -1;
b3SharedMemoryCommandHandle commandHandle;
struct b3ContactInformation contactPointData;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
PyObject* pyResultList = 0;
static char *kwlist[] = { "bodyA", "bodyB", NULL };
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|ii", kwlist,
&bodyUniqueIdA, &bodyUniqueIdB))
return NULL;
commandHandle = b3InitRequestContactPointInformation(sm);
b3SetContactFilterBodyA(commandHandle, bodyUniqueIdA);
b3SetContactFilterBodyB(commandHandle, bodyUniqueIdB);
//b3SetContactQueryMode(commandHandle, mode);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) {
b3GetContactPointInformation(sm, &contactPointData);
return MyConvertContactPoint(&contactPointData);
}
Py_INCREF(Py_None);
@@ -2447,11 +2580,21 @@ static PyMethodDef SpamMethods[] = {
#endif
},
{"getContactPointData", pybullet_getContactPointData, METH_VARARGS,
"Return the contact point information for all or some of pairwise "
"object-object collisions. Optional arguments one or two object unique "
{"getContactPoints", pybullet_getContactPointData, METH_VARARGS | METH_KEYWORDS,
"Return existing contact points after the stepSimulation command. "
"Optional arguments one or two object unique "
"ids, that need to be involved in the contact."},
{"getClosestPoints", pybullet_getClosestPointData, METH_VARARGS | METH_KEYWORDS,
"Compute the closest points between two objects, if the distance is below a given threshold."
"Input is two objects unique ids and distance threshold."
},
{ "getOverlappingObjects", pybullet_getOverlappingObjects, METH_VARARGS | METH_KEYWORDS,
"Return all the objects that have overlap with a given "
"axis-aligned bounding box volume (AABB)."
"Input are two vectors defining the AABB in world space [min_x,min_y,min_z],[max_x,max_y,max_z]."
},
{"getVisualShapeData", pybullet_getVisualShapeData, METH_VARARGS,
"Return the visual shape information for one object." },
@@ -2486,6 +2629,7 @@ static PyMethodDef SpamMethods[] = {
// loadSnapshot
// raycast info
// object names
// collision query
{NULL, NULL, 0, NULL} /* Sentinel */
};
@@ -2541,6 +2685,9 @@ initpybullet(void)
PyModule_AddIntConstant(m, "LINK_FRAME", EF_LINK_FRAME);
PyModule_AddIntConstant(m, "WORLD_FRAME", EF_WORLD_FRAME);
PyModule_AddIntConstant(m, "CONTACT_REPORT_EXISTING", CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS);
PyModule_AddIntConstant(m, "CONTACT_RECOMPUTE_CLOSEST", CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS);
SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
Py_INCREF(SpamError);
PyModule_AddObject(m, "error", SpamError);

1
src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
View File

@@ -1252,6 +1252,7 @@ void btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionOb
if (algorithm)
{
btBridgedManifoldResult contactPointResult(&obA,&obB, resultCallback);
contactPointResult.m_closestPointDistanceThreshold = resultCallback.m_closestDistanceThreshold;
//discrete collision detection query
algorithm->processCollision(&obA,&obB, getDispatchInfo(),&contactPointResult);

4
src/BulletCollision/CollisionDispatch/btCollisionWorld.h
View File

@@ -412,10 +412,12 @@ public:
{
short int m_collisionFilterGroup;
short int m_collisionFilterMask;
btScalar m_closestDistanceThreshold;
ContactResultCallback()
:m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
m_collisionFilterMask(btBroadphaseProxy::AllFilter)
m_collisionFilterMask(btBroadphaseProxy::AllFilter),
m_closestDistanceThreshold(0)
{
}

24
src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
View File

@@ -161,6 +161,13 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide
childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
btVector3 thresholdVec(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold);
aabbMin0 -= thresholdVec;
aabbMin1 -= thresholdVec;
aabbMax0 += thresholdVec;
aabbMax1 += thresholdVec;
if (gCompoundCompoundChildShapePairCallback)
{
if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1))
@@ -217,10 +224,12 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide
static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a,
const btDbvtAabbMm& b, const btTransform& xform)
const btDbvtAabbMm& b, const btTransform& xform, btScalar distanceThreshold)
{
btVector3 newmin,newmax;
btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax);
newmin -= btVector3(distanceThreshold, distanceThreshold, distanceThreshold);
newmax += btVector3(distanceThreshold, distanceThreshold, distanceThreshold);
btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin,newmax);
return Intersect(a,newb);
}
@@ -229,7 +238,7 @@ static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a,
static inline void MycollideTT( const btDbvtNode* root0,
const btDbvtNode* root1,
const btTransform& xform,
btCompoundCompoundLeafCallback* callback)
btCompoundCompoundLeafCallback* callback, btScalar distanceThreshold)
{
if(root0&&root1)
@@ -241,7 +250,7 @@ static inline void MycollideTT( const btDbvtNode* root0,
stkStack[0]=btDbvt::sStkNN(root0,root1);
do {
btDbvt::sStkNN p=stkStack[--depth];
if(MyIntersect(p.a->volume,p.b->volume,xform))
if(MyIntersect(p.a->volume,p.b->volume,xform, distanceThreshold))
{
if(depth>treshold)
{
@@ -343,7 +352,7 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
const btTransform xform=col0ObjWrap->getWorldTransform().inverse()*col1ObjWrap->getWorldTransform();
MycollideTT(tree0->m_root,tree1->m_root,xform,&callback);
MycollideTT(tree0->m_root,tree1->m_root,xform,&callback, resultOut->m_closestPointDistanceThreshold);
//printf("#compound-compound child/leaf overlap =%d \r",callback.m_numOverlapPairs);
@@ -383,7 +392,9 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
newChildWorldTrans0 = orgTrans0*childTrans0 ;
childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
}
btVector3 thresholdVec(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold);
aabbMin0 -= thresholdVec;
aabbMax0 += thresholdVec;
{
btTransform orgInterpolationTrans1;
const btCollisionShape* childShape1 = 0;
@@ -398,7 +409,8 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
}
aabbMin1 -= thresholdVec;
aabbMax1 += thresholdVec;
if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
{

2
src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
View File

@@ -365,7 +365,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
// input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold();
//} else
//{
input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold()+resultOut->m_closestPointDistanceThreshold;
// }
input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;

1
src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
View File

@@ -96,6 +96,7 @@ btManifoldResult::btManifoldResult(const btCollisionObjectWrapper* body0Wrap,con
m_index0(-1),
m_index1(-1)
#endif //DEBUG_PART_INDEX
, m_closestPointDistanceThreshold(0)
{
}

6
src/BulletCollision/CollisionDispatch/btManifoldResult.h
View File

@@ -53,13 +53,15 @@ protected:
public:
btManifoldResult()
#ifdef DEBUG_PART_INDEX
:
#ifdef DEBUG_PART_INDEX
m_partId0(-1),
m_partId1(-1),
m_index0(-1),
m_index1(-1)
#endif //DEBUG_PART_INDEX
m_closestPointDistanceThreshold(0)
{
}
@@ -142,6 +144,8 @@ public:
return m_body1Wrap->getCollisionObject();
}
btScalar m_closestPointDistanceThreshold;
/// in the future we can let the user override the methods to combine restitution and friction
static btScalar calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1);
static btScalar calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1);

2
src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
View File

@@ -62,7 +62,7 @@ void btSphereSphereCollisionAlgorithm::processCollision (const btCollisionObject
#endif
///iff distance positive, don't generate a new contact
if ( len > (radius0+radius1))
if ( len > (radius0+radius1+resultOut->m_closestPointDistanceThreshold))
{
#ifndef CLEAR_MANIFOLD
resultOut->refreshContactPoints();