mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-13 05:10:08 +00:00
pybullet getClosestPoints
This commit is contained in:
parent
2bb53b311d
commit
0d47d61007
728
data/quadruped/quadruped.urdf
Normal file
728
data/quadruped/quadruped.urdf
Normal 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>
|
||||
|
@ -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");
|
||||
|
||||
}
|
||||
|
||||
|
@ -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
|
||||
|
@ -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);
|
||||
|
@ -2543,8 +2543,20 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
//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();
|
||||
m_data->m_cachedContactPoints.resize(0);
|
||||
|
||||
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++)
|
||||
{
|
||||
@ -2620,6 +2632,234 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
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();
|
||||
|
@ -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)
|
||||
{
|
||||
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
{
|
||||
|
@ -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,37 +1536,9 @@ 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;
|
||||
@ -1565,61 +1557,62 @@ 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);
|
||||
@ -1627,6 +1620,127 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) {
|
||||
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);
|
||||
return 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);
|
||||
|
@ -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);
|
||||
|
||||
|
@ -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)
|
||||
{
|
||||
}
|
||||
|
||||
|
@ -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))
|
||||
{
|
||||
|
@ -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;
|
||||
|
@ -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);
|
||||
|
@ -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();
|
||||
|
Loading…
Reference in New Issue
Block a user