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

pybullet getClosestPoints

Browse Source
This commit is contained in:
erwincoumans 2016-11-09 21:01:04 -08:00
parent 2bb53b311d
commit 0d47d61007
15 changed files with 1373 additions and 139 deletions
Show Stats 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");
}

66
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -1274,7 +1274,66 @@ 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])
{
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_AABB_OVERLAP;
command->m_requestContactPointArguments.m_aabbQueryMin[0] = aabbMin[0];
command->m_requestContactPointArguments.m_aabbQueryMin[1] = aabbMin[1];
command->m_requestContactPointArguments.m_aabbQueryMin[2] = aabbMin[2];
command->m_requestContactPointArguments.m_aabbQueryMax[0] = aabbMax[0];
command->m_requestContactPointArguments.m_aabbQueryMax[1] = aabbMax[1];
command->m_requestContactPointArguments.m_aabbQueryMax[2] = aabbMax[2];
return commandHandle;
}
void b3GetAABBOverlapResults(b3PhysicsClientHandle physClient, struct b3AABBOverlapData* data)
{
data->m_numOverlappingObjects = 0;
// data->m_objectUniqueIds
}
void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData)
@ -1286,6 +1345,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);

384
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -2541,86 +2541,326 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
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;
const btRigidBody* bodyB = btRigidBody::upcast(manifold->getBody1());
if (bodyB)
int mode = CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS;
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_AABB_OVERLAP:
{
//clientCmd.m_requestContactPointArguments.m_aabbQueryMin
btVector3 aabbMin,aabbMax;
aabbMin.setValue(clientCmd.m_requestContactPointArguments.m_aabbQueryMin[0],
clientCmd.m_requestContactPointArguments.m_aabbQueryMin[1],
clientCmd.m_requestContactPointArguments.m_aabbQueryMin[2]);
aabbMax.setValue(clientCmd.m_requestContactPointArguments.m_aabbQueryMax[0],
clientCmd.m_requestContactPointArguments.m_aabbQueryMax[1],
clientCmd.m_requestContactPointArguments.m_aabbQueryMax[2]);
struct MyBroadphaseCallback : public btBroadphaseAabbCallback
{
b3AlignedObjectArray<int> m_bodyUniqueIds;
b3AlignedObjectArray<int> m_links;
MyBroadphaseCallback()
{
}
virtual ~MyBroadphaseCallback()
{
}
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;
}
};
MyBroadphaseCallback callback;
m_data->m_dynamicsWorld->getBroadphase()->aabbTest(aabbMin,aabbMax,callback);
int totalBytesPerObject = 2 * sizeof(int);
int pairCapacity = bufferSizeInBytes / totalBytesPerObject - 1;
if (callback.m_bodyUniqueIds.size() < pairCapacity)
{
serverCmd.m_type = CMD_AABB_OVERLAP_COMPLETED;
int* pairStorage = (int*)bufferServerToClient;
for (int i = 0; i < callback.m_bodyUniqueIds.size(); i++)
{
pairStorage[i * 2] = callback.m_bodyUniqueIds[i];
pairStorage[i * 2+1] = callback.m_links[i];
}
}
else
{
serverCmd.m_type = CMD_AABB_OVERLAP_FAILED;
}
hasStatus = true;
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();

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)
{

10
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -143,11 +143,21 @@ 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;
double m_aabbQueryMin[3];
double m_aabbQueryMax[3];
int m_mode;
};
struct RequestVisualShapeDataArgs

17
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -80,6 +80,8 @@ enum EnumSharedMemoryServerStatus
CMD_CALCULATED_JACOBIAN_FAILED,
CMD_CONTACT_POINT_INFORMATION_COMPLETED,
CMD_CONTACT_POINT_INFORMATION_FAILED,
CMD_AABB_OVERLAP_COMPLETED,
CMD_AABB_OVERLAP_FAILED,
CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED,
CMD_CALCULATE_INVERSE_KINEMATICS_FAILED,
CMD_SAVE_WORLD_COMPLETED,
@ -158,6 +160,13 @@ struct b3DebugLines
const float* m_linesColor;//float red,green,blue times 'm_numDebugLines'.
};
struct b3AABBOverlapData
{
int m_numOverlappingObjects;
int* m_objectUniqueIds;
int* m_links;
};
struct b3CameraImageData
{
int m_pixelWidth;
@ -192,6 +201,14 @@ struct b3ContactPointData
// double m_angularFrictionForce;
};
enum
{
CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS = 0,
CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS = 1,
CONTACT_QUERY_MODE_AABB_OVERLAP = 2,
};
struct b3ContactInformation
{

234
examples/pybullet/pybullet.c
View File

@ -680,6 +680,12 @@ static int pybullet_internalGetBasePositionAndOrientation(
baseOrientation[2] = 0.;
baseOrientation[3] = 1.;
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
Py_INCREF(Py_None);
return Py_None;
}
{
{
b3SharedMemoryCommandHandle cmd_handle =
@ -1352,6 +1358,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 +1460,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 +1502,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 +1536,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 +1557,188 @@ 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;
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);
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;
int i;
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 +2561,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 +2610,7 @@ static PyMethodDef SpamMethods[] = {
// loadSnapshot
// raycast info
// object names
// collision query
{NULL, NULL, 0, NULL} /* Sentinel */
};
@ -2541,6 +2666,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);

6
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;

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

@ -49,17 +49,19 @@ protected:
int m_index0;
int m_index1;
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();