mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-13 13:20:07 +00:00
pybullet getClosestPoints
This commit is contained in:
erwincoumans
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)
|
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;
|
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)
|
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
|
//request visual shape information
|
||||||
|
|||||||
@ -99,6 +99,17 @@ void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int body
|
|||||||
void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
|
void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
|
||||||
void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo);
|
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
|
//request visual shape information
|
||||||
b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA);
|
b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA);
|
||||||
void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo);
|
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
|
//make a snapshot of the contact manifolds into individual contact points
|
||||||
if (clientCmd.m_requestContactPointArguments.m_startingContactPointIndex == 0)
|
if (clientCmd.m_requestContactPointArguments.m_startingContactPointIndex == 0)
|
||||||
{
|
{
|
||||||
int numContactManifolds = m_data->m_dynamicsWorld->getDispatcher()->getNumManifolds();
|
|
||||||
m_data->m_cachedContactPoints.resize(0);
|
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);
|
m_data->m_cachedContactPoints.reserve(numContactManifolds * 4);
|
||||||
for (int i = 0; i < numContactManifolds; i++)
|
for (int i = 0; i < numContactManifolds; i++)
|
||||||
{
|
{
|
||||||
@ -2620,6 +2632,234 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
|||||||
m_data->m_cachedContactPoints.push_back(pt);
|
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();
|
int numContactPoints = m_data->m_cachedContactPoints.size();
|
||||||
|
|||||||
@ -969,6 +969,23 @@ void PhysicsServerExample::renderScene()
|
|||||||
static char line0[1024];
|
static char line0[1024];
|
||||||
static char line1[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)
|
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
|
struct RequestContactDataArgs
|
||||||
{
|
{
|
||||||
int m_startingContactPointIndex;
|
int m_startingContactPointIndex;
|
||||||
int m_objectAIndexFilter;
|
int m_objectAIndexFilter;
|
||||||
int m_objectBIndexFilter;
|
int m_objectBIndexFilter;
|
||||||
|
double m_closestDistanceThreshold;
|
||||||
|
double m_aabbQueryMin[3];
|
||||||
|
double m_aabbQueryMax[3];
|
||||||
|
int m_mode;
|
||||||
};
|
};
|
||||||
|
|
||||||
struct RequestVisualShapeDataArgs
|
struct RequestVisualShapeDataArgs
|
||||||
|
|||||||
@ -80,6 +80,8 @@ enum EnumSharedMemoryServerStatus
|
|||||||
CMD_CALCULATED_JACOBIAN_FAILED,
|
CMD_CALCULATED_JACOBIAN_FAILED,
|
||||||
CMD_CONTACT_POINT_INFORMATION_COMPLETED,
|
CMD_CONTACT_POINT_INFORMATION_COMPLETED,
|
||||||
CMD_CONTACT_POINT_INFORMATION_FAILED,
|
CMD_CONTACT_POINT_INFORMATION_FAILED,
|
||||||
|
CMD_AABB_OVERLAP_COMPLETED,
|
||||||
|
CMD_AABB_OVERLAP_FAILED,
|
||||||
CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED,
|
CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED,
|
||||||
CMD_CALCULATE_INVERSE_KINEMATICS_FAILED,
|
CMD_CALCULATE_INVERSE_KINEMATICS_FAILED,
|
||||||
CMD_SAVE_WORLD_COMPLETED,
|
CMD_SAVE_WORLD_COMPLETED,
|
||||||
@ -158,6 +160,13 @@ struct b3DebugLines
|
|||||||
const float* m_linesColor;//float red,green,blue times 'm_numDebugLines'.
|
const float* m_linesColor;//float red,green,blue times 'm_numDebugLines'.
|
||||||
};
|
};
|
||||||
|
|
||||||
|
struct b3AABBOverlapData
|
||||||
|
{
|
||||||
|
int m_numOverlappingObjects;
|
||||||
|
int* m_objectUniqueIds;
|
||||||
|
int* m_links;
|
||||||
|
};
|
||||||
|
|
||||||
struct b3CameraImageData
|
struct b3CameraImageData
|
||||||
{
|
{
|
||||||
int m_pixelWidth;
|
int m_pixelWidth;
|
||||||
@ -192,6 +201,14 @@ struct b3ContactPointData
|
|||||||
// double m_angularFrictionForce;
|
// 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
|
struct b3ContactInformation
|
||||||
{
|
{
|
||||||
|
|||||||
@ -680,6 +680,12 @@ static int pybullet_internalGetBasePositionAndOrientation(
|
|||||||
baseOrientation[2] = 0.;
|
baseOrientation[2] = 0.;
|
||||||
baseOrientation[3] = 1.;
|
baseOrientation[3] = 1.;
|
||||||
|
|
||||||
|
if (0 == sm) {
|
||||||
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||||
|
Py_INCREF(Py_None);
|
||||||
|
return Py_None;
|
||||||
|
}
|
||||||
|
|
||||||
{
|
{
|
||||||
{
|
{
|
||||||
b3SharedMemoryCommandHandle cmd_handle =
|
b3SharedMemoryCommandHandle cmd_handle =
|
||||||
@ -1352,6 +1358,10 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args)
|
|||||||
int i;
|
int i;
|
||||||
PyObject* pyResultList = 0;
|
PyObject* pyResultList = 0;
|
||||||
|
|
||||||
|
if (0 == sm) {
|
||||||
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
if (size == 1)
|
if (size == 1)
|
||||||
{
|
{
|
||||||
if (!PyArg_ParseTuple(args, "i", &objectUniqueId)) {
|
if (!PyArg_ParseTuple(args, "i", &objectUniqueId)) {
|
||||||
@ -1450,6 +1460,11 @@ static PyObject* pybullet_resetVisualShapeData(PyObject* self, PyObject* args)
|
|||||||
b3SharedMemoryStatusHandle statusHandle;
|
b3SharedMemoryStatusHandle statusHandle;
|
||||||
int statusType;
|
int statusType;
|
||||||
|
|
||||||
|
if (0 == sm) {
|
||||||
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
if (size == 4)
|
if (size == 4)
|
||||||
{
|
{
|
||||||
if (!PyArg_ParseTuple(args, "iiii", &objectUniqueId, &jointIndex, &shapeIndex, &textureUniqueId)) {
|
if (!PyArg_ParseTuple(args, "iiii", &objectUniqueId, &jointIndex, &shapeIndex, &textureUniqueId)) {
|
||||||
@ -1487,6 +1502,11 @@ static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args)
|
|||||||
b3SharedMemoryStatusHandle statusHandle;
|
b3SharedMemoryStatusHandle statusHandle;
|
||||||
int statusType;
|
int statusType;
|
||||||
|
|
||||||
|
if (0 == sm) {
|
||||||
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
if (size == 1)
|
if (size == 1)
|
||||||
{
|
{
|
||||||
if (!PyArg_ParseTuple(args, "s", &filename)) {
|
if (!PyArg_ParseTuple(args, "s", &filename)) {
|
||||||
@ -1516,37 +1536,9 @@ static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args)
|
|||||||
return Py_None;
|
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) {
|
static PyObject* MyConvertContactPoint( struct b3ContactInformation* contactPointPtr)
|
||||||
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) {
|
|
||||||
/*
|
/*
|
||||||
0 int m_contactFlags;
|
0 int m_contactFlags;
|
||||||
1 int m_bodyUniqueIdA;
|
1 int m_bodyUniqueIdA;
|
||||||
@ -1565,61 +1557,62 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) {
|
|||||||
15 double m_normalForce;
|
15 double m_normalForce;
|
||||||
*/
|
*/
|
||||||
|
|
||||||
b3GetContactPointInformation(sm, &contactPointData);
|
int i;
|
||||||
pyResultList = PyTuple_New(contactPointData.m_numContactPoints);
|
|
||||||
for (i = 0; i < contactPointData.m_numContactPoints; 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* contactObList = PyTuple_New(16); // see above 16 fields
|
||||||
PyObject* item;
|
PyObject* item;
|
||||||
item =
|
item =
|
||||||
PyInt_FromLong(contactPointData.m_contactPointData[i].m_contactFlags);
|
PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_contactFlags);
|
||||||
PyTuple_SetItem(contactObList, 0, item);
|
PyTuple_SetItem(contactObList, 0, item);
|
||||||
item = PyInt_FromLong(
|
item = PyInt_FromLong(
|
||||||
contactPointData.m_contactPointData[i].m_bodyUniqueIdA);
|
contactPointPtr->m_contactPointData[i].m_bodyUniqueIdA);
|
||||||
PyTuple_SetItem(contactObList, 1, item);
|
PyTuple_SetItem(contactObList, 1, item);
|
||||||
item = PyInt_FromLong(
|
item = PyInt_FromLong(
|
||||||
contactPointData.m_contactPointData[i].m_bodyUniqueIdB);
|
contactPointPtr->m_contactPointData[i].m_bodyUniqueIdB);
|
||||||
PyTuple_SetItem(contactObList, 2, item);
|
PyTuple_SetItem(contactObList, 2, item);
|
||||||
item =
|
item =
|
||||||
PyInt_FromLong(contactPointData.m_contactPointData[i].m_linkIndexA);
|
PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_linkIndexA);
|
||||||
PyTuple_SetItem(contactObList, 3, item);
|
PyTuple_SetItem(contactObList, 3, item);
|
||||||
item =
|
item =
|
||||||
PyInt_FromLong(contactPointData.m_contactPointData[i].m_linkIndexB);
|
PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_linkIndexB);
|
||||||
PyTuple_SetItem(contactObList, 4, item);
|
PyTuple_SetItem(contactObList, 4, item);
|
||||||
item = PyFloat_FromDouble(
|
item = PyFloat_FromDouble(
|
||||||
contactPointData.m_contactPointData[i].m_positionOnAInWS[0]);
|
contactPointPtr->m_contactPointData[i].m_positionOnAInWS[0]);
|
||||||
PyTuple_SetItem(contactObList, 5, item);
|
PyTuple_SetItem(contactObList, 5, item);
|
||||||
item = PyFloat_FromDouble(
|
item = PyFloat_FromDouble(
|
||||||
contactPointData.m_contactPointData[i].m_positionOnAInWS[1]);
|
contactPointPtr->m_contactPointData[i].m_positionOnAInWS[1]);
|
||||||
PyTuple_SetItem(contactObList, 6, item);
|
PyTuple_SetItem(contactObList, 6, item);
|
||||||
item = PyFloat_FromDouble(
|
item = PyFloat_FromDouble(
|
||||||
contactPointData.m_contactPointData[i].m_positionOnAInWS[2]);
|
contactPointPtr->m_contactPointData[i].m_positionOnAInWS[2]);
|
||||||
PyTuple_SetItem(contactObList, 7, item);
|
PyTuple_SetItem(contactObList, 7, item);
|
||||||
|
|
||||||
item = PyFloat_FromDouble(
|
item = PyFloat_FromDouble(
|
||||||
contactPointData.m_contactPointData[i].m_positionOnBInWS[0]);
|
contactPointPtr->m_contactPointData[i].m_positionOnBInWS[0]);
|
||||||
PyTuple_SetItem(contactObList, 8, item);
|
PyTuple_SetItem(contactObList, 8, item);
|
||||||
item = PyFloat_FromDouble(
|
item = PyFloat_FromDouble(
|
||||||
contactPointData.m_contactPointData[i].m_positionOnBInWS[1]);
|
contactPointPtr->m_contactPointData[i].m_positionOnBInWS[1]);
|
||||||
PyTuple_SetItem(contactObList, 9, item);
|
PyTuple_SetItem(contactObList, 9, item);
|
||||||
item = PyFloat_FromDouble(
|
item = PyFloat_FromDouble(
|
||||||
contactPointData.m_contactPointData[i].m_positionOnBInWS[2]);
|
contactPointPtr->m_contactPointData[i].m_positionOnBInWS[2]);
|
||||||
PyTuple_SetItem(contactObList, 10, item);
|
PyTuple_SetItem(contactObList, 10, item);
|
||||||
|
|
||||||
item = PyFloat_FromDouble(
|
item = PyFloat_FromDouble(
|
||||||
contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[0]);
|
contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[0]);
|
||||||
PyTuple_SetItem(contactObList, 11, item);
|
PyTuple_SetItem(contactObList, 11, item);
|
||||||
item = PyFloat_FromDouble(
|
item = PyFloat_FromDouble(
|
||||||
contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[1]);
|
contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[1]);
|
||||||
PyTuple_SetItem(contactObList, 12, item);
|
PyTuple_SetItem(contactObList, 12, item);
|
||||||
item = PyFloat_FromDouble(
|
item = PyFloat_FromDouble(
|
||||||
contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[2]);
|
contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[2]);
|
||||||
PyTuple_SetItem(contactObList, 13, item);
|
PyTuple_SetItem(contactObList, 13, item);
|
||||||
|
|
||||||
item = PyFloat_FromDouble(
|
item = PyFloat_FromDouble(
|
||||||
contactPointData.m_contactPointData[i].m_contactDistance);
|
contactPointPtr->m_contactPointData[i].m_contactDistance);
|
||||||
PyTuple_SetItem(contactObList, 14, item);
|
PyTuple_SetItem(contactObList, 14, item);
|
||||||
item = PyFloat_FromDouble(
|
item = PyFloat_FromDouble(
|
||||||
contactPointData.m_contactPointData[i].m_normalForce);
|
contactPointPtr->m_contactPointData[i].m_normalForce);
|
||||||
PyTuple_SetItem(contactObList, 15, item);
|
PyTuple_SetItem(contactObList, 15, item);
|
||||||
|
|
||||||
PyTuple_SetItem(pyResultList, i, contactObList);
|
PyTuple_SetItem(pyResultList, i, contactObList);
|
||||||
@ -1627,6 +1620,127 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) {
|
|||||||
return pyResultList;
|
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);
|
Py_INCREF(Py_None);
|
||||||
return Py_None;
|
return Py_None;
|
||||||
}
|
}
|
||||||
@ -2447,11 +2561,21 @@ static PyMethodDef SpamMethods[] = {
|
|||||||
#endif
|
#endif
|
||||||
},
|
},
|
||||||
|
|
||||||
{"getContactPointData", pybullet_getContactPointData, METH_VARARGS,
|
{"getContactPoints", pybullet_getContactPointData, METH_VARARGS | METH_KEYWORDS,
|
||||||
"Return the contact point information for all or some of pairwise "
|
"Return existing contact points after the stepSimulation command. "
|
||||||
"object-object collisions. Optional arguments one or two object unique "
|
"Optional arguments one or two object unique "
|
||||||
"ids, that need to be involved in the contact."},
|
"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,
|
{"getVisualShapeData", pybullet_getVisualShapeData, METH_VARARGS,
|
||||||
"Return the visual shape information for one object." },
|
"Return the visual shape information for one object." },
|
||||||
|
|
||||||
@ -2486,6 +2610,7 @@ static PyMethodDef SpamMethods[] = {
|
|||||||
// loadSnapshot
|
// loadSnapshot
|
||||||
// raycast info
|
// raycast info
|
||||||
// object names
|
// object names
|
||||||
|
// collision query
|
||||||
|
|
||||||
{NULL, NULL, 0, NULL} /* Sentinel */
|
{NULL, NULL, 0, NULL} /* Sentinel */
|
||||||
};
|
};
|
||||||
@ -2541,6 +2666,9 @@ initpybullet(void)
|
|||||||
PyModule_AddIntConstant(m, "LINK_FRAME", EF_LINK_FRAME);
|
PyModule_AddIntConstant(m, "LINK_FRAME", EF_LINK_FRAME);
|
||||||
PyModule_AddIntConstant(m, "WORLD_FRAME", EF_WORLD_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);
|
SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
|
||||||
Py_INCREF(SpamError);
|
Py_INCREF(SpamError);
|
||||||
PyModule_AddObject(m, "error", SpamError);
|
PyModule_AddObject(m, "error", SpamError);
|
||||||
|
|||||||
@ -1252,6 +1252,7 @@ void btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionOb
|
|||||||
if (algorithm)
|
if (algorithm)
|
||||||
{
|
{
|
||||||
btBridgedManifoldResult contactPointResult(&obA,&obB, resultCallback);
|
btBridgedManifoldResult contactPointResult(&obA,&obB, resultCallback);
|
||||||
|
contactPointResult.m_closestPointDistanceThreshold = resultCallback.m_closestDistanceThreshold;
|
||||||
//discrete collision detection query
|
//discrete collision detection query
|
||||||
algorithm->processCollision(&obA,&obB, getDispatchInfo(),&contactPointResult);
|
algorithm->processCollision(&obA,&obB, getDispatchInfo(),&contactPointResult);
|
||||||
|
|
||||||
|
|||||||
@ -412,10 +412,12 @@ public:
|
|||||||
{
|
{
|
||||||
short int m_collisionFilterGroup;
|
short int m_collisionFilterGroup;
|
||||||
short int m_collisionFilterMask;
|
short int m_collisionFilterMask;
|
||||||
|
btScalar m_closestDistanceThreshold;
|
||||||
|
|
||||||
ContactResultCallback()
|
ContactResultCallback()
|
||||||
:m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
|
: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);
|
childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
|
||||||
childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
|
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)
|
||||||
{
|
{
|
||||||
if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1))
|
if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1))
|
||||||
@ -217,10 +224,12 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide
|
|||||||
|
|
||||||
|
|
||||||
static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a,
|
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;
|
btVector3 newmin,newmax;
|
||||||
btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,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);
|
btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin,newmax);
|
||||||
return Intersect(a,newb);
|
return Intersect(a,newb);
|
||||||
}
|
}
|
||||||
@ -229,7 +238,7 @@ static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a,
|
|||||||
static inline void MycollideTT( const btDbvtNode* root0,
|
static inline void MycollideTT( const btDbvtNode* root0,
|
||||||
const btDbvtNode* root1,
|
const btDbvtNode* root1,
|
||||||
const btTransform& xform,
|
const btTransform& xform,
|
||||||
btCompoundCompoundLeafCallback* callback)
|
btCompoundCompoundLeafCallback* callback, btScalar distanceThreshold)
|
||||||
{
|
{
|
||||||
|
|
||||||
if(root0&&root1)
|
if(root0&&root1)
|
||||||
@ -241,7 +250,7 @@ static inline void MycollideTT( const btDbvtNode* root0,
|
|||||||
stkStack[0]=btDbvt::sStkNN(root0,root1);
|
stkStack[0]=btDbvt::sStkNN(root0,root1);
|
||||||
do {
|
do {
|
||||||
btDbvt::sStkNN p=stkStack[--depth];
|
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)
|
if(depth>treshold)
|
||||||
{
|
{
|
||||||
@ -343,7 +352,7 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
|
|||||||
|
|
||||||
|
|
||||||
const btTransform xform=col0ObjWrap->getWorldTransform().inverse()*col1ObjWrap->getWorldTransform();
|
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);
|
//printf("#compound-compound child/leaf overlap =%d \r",callback.m_numOverlapPairs);
|
||||||
|
|
||||||
@ -383,7 +392,9 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
|
|||||||
newChildWorldTrans0 = orgTrans0*childTrans0 ;
|
newChildWorldTrans0 = orgTrans0*childTrans0 ;
|
||||||
childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
|
childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
|
||||||
}
|
}
|
||||||
|
btVector3 thresholdVec(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold);
|
||||||
|
aabbMin0 -= thresholdVec;
|
||||||
|
aabbMax0 += thresholdVec;
|
||||||
{
|
{
|
||||||
btTransform orgInterpolationTrans1;
|
btTransform orgInterpolationTrans1;
|
||||||
const btCollisionShape* childShape1 = 0;
|
const btCollisionShape* childShape1 = 0;
|
||||||
@ -398,7 +409,8 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
|
|||||||
childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
|
childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
aabbMin1 -= thresholdVec;
|
||||||
|
aabbMax1 += thresholdVec;
|
||||||
|
|
||||||
if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
|
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();
|
// input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold();
|
||||||
//} else
|
//} 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;
|
input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
|
||||||
|
|||||||
@ -53,13 +53,15 @@ protected:
|
|||||||
public:
|
public:
|
||||||
|
|
||||||
btManifoldResult()
|
btManifoldResult()
|
||||||
#ifdef DEBUG_PART_INDEX
|
|
||||||
:
|
:
|
||||||
|
#ifdef DEBUG_PART_INDEX
|
||||||
|
|
||||||
m_partId0(-1),
|
m_partId0(-1),
|
||||||
m_partId1(-1),
|
m_partId1(-1),
|
||||||
m_index0(-1),
|
m_index0(-1),
|
||||||
m_index1(-1)
|
m_index1(-1)
|
||||||
#endif //DEBUG_PART_INDEX
|
#endif //DEBUG_PART_INDEX
|
||||||
|
m_closestPointDistanceThreshold(0)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -142,6 +144,8 @@ public:
|
|||||||
return m_body1Wrap->getCollisionObject();
|
return m_body1Wrap->getCollisionObject();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
btScalar m_closestPointDistanceThreshold;
|
||||||
|
|
||||||
/// in the future we can let the user override the methods to combine restitution and friction
|
/// 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 calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1);
|
||||||
static btScalar calculateCombinedFriction(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
|
#endif
|
||||||
|
|
||||||
///iff distance positive, don't generate a new contact
|
///iff distance positive, don't generate a new contact
|
||||||
if ( len > (radius0+radius1))
|
if ( len > (radius0+radius1+resultOut->m_closestPointDistanceThreshold))
|
||||||
{
|
{
|
||||||
#ifndef CLEAR_MANIFOLD
|
#ifndef CLEAR_MANIFOLD
|
||||||
resultOut->refreshContactPoints();
|
resultOut->refreshContactPoints();
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user