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

Merge pull request #436 from erwincoumans/master

Browse Source 
minor refactoring,update stb_truetype, disable gimpact by default in btWorldImporter
This commit is contained in:
erwincoumans
2015-07-27 13:41:26 -07:00
parent f866142ebe 26531f3fbc
commit 63150432e0
67 changed files with 5375 additions and 587 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
Extras/Serialize/BulletFileLoader/bFile.cpp
View File

@@ -1601,11 +1601,11 @@ void bFile::writeChunks(FILE* fp, bool fixupPointers)
// Ouch! need to rebuild the struct
short *oldStruct,*curStruct;
char *oldType, *newType;
int oldLen, curLen, reverseOld;
int curLen, reverseOld;
oldStruct = fileDna->getStruct(dataChunk.dna_nr);
oldType = fileDna->getType(oldStruct[0]);
oldLen = fileDna->getLength(oldStruct[0]);
//int oldLen = fileDna->getLength(oldStruct[0]);
///don't try to convert Link block data, just memcpy it. Other data can be converted.
reverseOld = mMemoryDNA->getReverseType(oldType);

3
Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.cpp
View File

@@ -18,8 +18,9 @@ subject to the following restrictions:
#include "../BulletFileLoader/btBulletFile.h"
#include "btBulletDynamicsCommon.h"
#ifndef USE_GIMPACT
#include "BulletCollision/Gimpact/btGImpactShape.h"
#endif
//#define USE_INTERNAL_EDGE_UTILITY

9
Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp
View File

@@ -15,8 +15,9 @@ subject to the following restrictions:
#include "btWorldImporter.h"
#include "btBulletDynamicsCommon.h"
#ifdef USE_GIMPACT
#include "BulletCollision/Gimpact/btGImpactShape.h"
#endif
btWorldImporter::btWorldImporter(btDynamicsWorld* world)
:m_dynamicsWorld(world),
m_verboseMode(0)
@@ -177,6 +178,7 @@ btCollisionShape* btWorldImporter::convertCollisionShape( btCollisionShapeData*
}
case GIMPACT_SHAPE_PROXYTYPE:
{
#ifdef USE_GIMPACT
btGImpactMeshShapeData* gimpactData = (btGImpactMeshShapeData*) shapeData;
if (gimpactData->m_gimpactSubType == CONST_GIMPACT_TRIMESH_SHAPE)
{
@@ -195,6 +197,7 @@ btCollisionShape* btWorldImporter::convertCollisionShape( btCollisionShapeData*
{
printf("unsupported gimpact sub type\n");
}
#endif//USE_GIMPACT
break;
}
//The btCapsuleShape* API has issue passing the margin/scaling/halfextents unmodified through the API
@@ -1792,9 +1795,13 @@ btCollisionShape* btWorldImporter::createConvexTriangleMeshShape(btStridingMeshI
}
btGImpactMeshShape* btWorldImporter::createGimpactShape(btStridingMeshInterface* trimesh)
{
#ifdef USE_GIMPACT
btGImpactMeshShape* shape = new btGImpactMeshShape(trimesh);
m_allocatedCollisionShapes.push_back(shape);
return shape;
#else
return 0;
#endif
}
btConvexHullShape* btWorldImporter::createConvexHullShape()

80
data/kuka_lwr/arm_base.urdf.xacro Normal file
View File

@@ -0,0 +1,80 @@
<?xml version="1.0"?>
<robot name="LWR">
<include filename="$(find lmtlwr)/model/kuka_lwr_arm.urdf.xacro"/>
<include filename="$(find lmtlwr)/model/gazebo.urdf.xacro"/>
<include filename="$(find lmtlwr)/model/materials.xml"/>
-
<kuka_lwr_arm parent="base_link" name="lwr" right="1" tool_name="">
<origin xyz="0 0 0" rpy="0 0 0"/>
</kuka_lwr_arm>
<link name="base_link" />
<link name="table">
<visual>
<origin xyz="0 0 -0.02"/>
<geometry>
<box size="2 2 0.04"/>
</geometry>
<material name="wood">
<texture filename="package://lmtlwr/model/wood.jpg"/>
<color rgba="0.65 0.5 0.4 0.8"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="2 2 0.03"/>
</geometry>
</collision>
</link>
- <link name="aluplate">
<visual>
<origin xyz="0 0 -0.0075" />
<geometry>
<box size="0.275 0.275 0.015" xyz="0.2 0.2 0.0"/>
</geometry>
<material name="Silver" />
</visual>
</link>
<link name="lwr_tool">
<visual>
<origin xyz="0 0 0" />
<geometry>
<mesh filename="package://lmtlwr/model/tools/jr3-stabilo.dae" scale="0.0254 0.0254 0.0254"/>
</geometry>
</visual>
</link>
<link name="lwr_tool_tcp" />
<joint name="arm7_to_tcp" type="fixed">
<parent link="lwr_arm_7_link" />
<child link="lwr_tool_tcp"/>
<origin xyz="-0.028 0.010 0.203" />
</joint>
<joint name="arm7_to_tool" type="fixed">
<parent link="lwr_arm_7_link" />
<child link="lwr_tool"/>
<origin xyz="0 0 0" />
</joint>
<joint name="table_to_base" type="fixed">
<parent link="table" />
<child link="aluplate"/>
<origin xyz="0 0 0.015" />
</joint>
<joint name="aluplate_to_base" type="fixed">
<parent link="aluplate" />
<child link="base_link"/>
<origin xyz="0 0 0" rpy="0 0 45" />
</joint>
</robot>

16
data/kuka_lwr/gazebo.urdf.xacro Normal file
View File

@@ -0,0 +1,16 @@
<?xml version="1.0"?>
<robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface">
<gazebo>
<controller:gazebo_ros_controller_manager name="gazebo_controller_manager" plugin="libgazebo_ros_controller_manager.so">
<alwaysOn>true</alwaysOn>
<robotParam>robot_description</robotParam>
<updateRate>1000.0</updateRate>
<interface:audio name="gazebo_mechanism_control_dummy_iface" />
</controller:gazebo_ros_controller_manager>
</gazebo>
</robot>

296
data/kuka_lwr/kuka.urdf Normal file
View File

@@ -0,0 +1,296 @@
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from kuka_lwr_arm.urdf.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="kuka" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor">
<!--
Little helper macro to define the inertia matrix needed
for links.
-->
<!-- length is along the y-axis! -->
<!-- right is either 1 (for right arm) or -1 (for left arm) -->
<link name="calib_kuka_arm_base_link">
<inertial>
<mass value="0"/>
<origin rpy="0 0 0" xyz="0 0 0.055"/>
<inertia ixx="0.00381666666667" ixy="0" ixz="0" iyy="0.0036" iyz="0" izz="0.00381666666667"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes_arm/arm_base.dae"/>
</geometry>
<material name="Orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes_arm/convex/arm_base_convex.stl"/>
</geometry>
</collision>
</link>
<gazebo reference="calib_kuka_arm_base_link">
<material value="kuka-lwr.material"/>
</gazebo>
<joint name="kuka_arm_0_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0.11"/>
<axis xyz="0 0 1"/>
<limit effort="204" lower="-2.96705972839" upper="2.96705972839" velocity="1.91986217719"/>
<safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.93215314335" soft_upper_limit="2.93215314335"/>
<dynamics damping="0.1"/>
<parent link="calib_kuka_arm_base_link"/>
<child link="kuka_arm_1_link"/>
</joint>
<link name="kuka_arm_1_link">
<inertial>
<mass value="2.0"/>
<origin rpy="0 0 0" xyz="0 0 0.130"/>
<inertia ixx="0.0136666666667" ixy="0" ixz="0" iyy="0.003" iyz="0" izz="0.0118666666667"/>
</inertial>
<visual>
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes_arm/arm_segment_a.dae"/>
</geometry>
<material name="Orange"/>
</visual>
<collision>
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes_arm/convex/arm_segment_a_convex.stl"/>
</geometry>
</collision>
</link>
<gazebo reference="kuka_arm_1_link">
<material value="kuka-lwr.material"/>
</gazebo>
<transmission name="kuka_arm_0_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="kuka_arm_0_motor"/>
<joint name="kuka_arm_0_joint"/>
<mechanicalReduction>1.0</mechanicalReduction>
</transmission>
<joint name="kuka_arm_1_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0.20"/>
<axis xyz="0 -1 0"/>
<!--limit lower="${-120 * M_PI / 180}" upper="${120 * M_PI / 180}"
effort="306" velocity="${arm_velocity_scale_factor * 110 * M_PI / 180}" /-->
<!-- nalt: Reduced limits to avoid contact with table - kinda hacky, should not be done here -->
<limit effort="306" lower="-1.57079632679" upper="1.57079632679" velocity="1.91986217719"/>
<safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.05948851735" soft_upper_limit="2.05948851735"/>
<dynamics damping="0.1"/>
<parent link="kuka_arm_1_link"/>
<child link="kuka_arm_2_link"/>
</joint>
<link name="kuka_arm_2_link">
<inertial>
<mass value="2.0"/>
<origin rpy="0 0 0" xyz="0 0.06 0.07"/>
<inertia ixx="0.0136666666667" ixy="0" ixz="0" iyy="0.003" iyz="0" izz="0.0118666666667"/>
</inertial>
<visual>
<origin rpy="3.14159265359 0 3.14159265359" xyz="0 0 0.2"/>
<geometry>
<mesh filename="meshes_arm/arm_segment_b.dae"/>
</geometry>
<material name="Orange"/>
</visual>
<collision>
<origin rpy="3.14159265359 0 3.14159265359" xyz="0 0 0.2"/>
<geometry>
<mesh filename="meshes_arm/convex/arm_segment_b_convex.stl"/>
</geometry>
</collision>
</link>
<gazebo reference="kuka_arm_2_link">
<material value="kuka-lwr.material"/>
</gazebo>
<transmission name="kuka_arm_1_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="kuka_arm_1_motor"/>
<joint name="kuka_arm_1_joint"/>
<mechanicalReduction>1.0</mechanicalReduction>
</transmission>
<joint name="kuka_arm_2_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0.20"/>
<axis xyz="0 0 1"/>
<limit effort="204" lower="-2.96705972839" upper="2.96705972839" velocity="2.26892802759"/>
<safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.93215314335" soft_upper_limit="2.93215314335"/>
<dynamics damping="0.1"/>
<parent link="kuka_arm_2_link"/>
<child link="kuka_arm_3_link"/>
</joint>
<link name="kuka_arm_3_link">
<inertial>
<mass value="2.0"/>
<origin rpy="0 0 0" xyz="0 0.06 0.130"/>
<inertia ixx="0.0136666666667" ixy="0" ixz="0" iyy="0.003" iyz="0" izz="0.0118666666667"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes_arm/arm_segment_a.dae"/>
</geometry>
<material name="Orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes_arm/convex/arm_segment_a_convex.stl"/>
</geometry>
</collision>
</link>
<gazebo reference="kuka_arm_3_link">
<material value="kuka-lwr.material"/>
</gazebo>
<transmission name="kuka_arm_2_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="kuka_arm_2_motor"/>
<joint name="kuka_arm_2_joint"/>
<mechanicalReduction>1.0</mechanicalReduction>
</transmission>
<joint name="kuka_arm_3_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0.20"/>
<axis xyz="0 1 0"/>
<limit effort="306" lower="-2.09439510239" upper="2.09439510239" velocity="2.26892802759"/>
<safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.05948851735" soft_upper_limit="2.05948851735"/>
<dynamics damping="0.1"/>
<parent link="kuka_arm_3_link"/>
<child link="kuka_arm_4_link"/>
</joint>
<link name="kuka_arm_4_link">
<inertial>
<mass value="2.0"/>
<origin rpy="0 0 0" xyz="0 -0.06 0.07"/>
<inertia ixx="0.0136666666667" ixy="0" ixz="0" iyy="0.003" iyz="0" izz="0.0118666666667"/>
</inertial>
<visual>
<origin rpy="0 3.14159265359 3.14159265359" xyz="0 0 0.2"/>
<geometry>
<mesh filename="meshes_arm/arm_segment_b.dae"/>
</geometry>
<material name="Orange"/>
</visual>
<collision>
<origin rpy="0 3.14159265359 3.14159265359" xyz="0 0 0.2"/>
<geometry>
<mesh filename="meshes_arm/convex/arm_segment_b_convex.stl"/>
</geometry>
</collision>
</link>
<gazebo reference="kuka_arm_4_link">
<material value="kuka-lwr.material"/>
</gazebo>
<transmission name="kuka_arm_3_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="kuka_arm_3_motor"/>
<joint name="kuka_arm_3_joint"/>
<mechanicalReduction>1.0</mechanicalReduction>
</transmission>
<joint name="kuka_arm_4_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0.20"/>
<axis xyz="0 0 1"/>
<limit effort="204" lower="-2.96705972839" upper="2.96705972839" velocity="2.26892802759"/>
<safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.93215314335" soft_upper_limit="2.93215314335"/>
<dynamics damping="0.1"/>
<parent link="kuka_arm_4_link"/>
<child link="kuka_arm_5_link"/>
</joint>
<link name="kuka_arm_5_link">
<inertial>
<mass value="2.0"/>
<origin rpy="0 0 0" xyz="0 0 0.124"/>
<inertia ixx="0.0126506666667" ixy="0" ixz="0" iyy="0.003" iyz="0" izz="0.0108506666667"/>
</inertial>
<visual>
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
<geometry name="kuka_arm_5_geom">
<mesh filename="meshes_arm/arm_segment_last.dae"/>
</geometry>
<material name="Orange"/>
</visual>
<collision>
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes_arm/convex/arm_segment_last_convex.stl"/>
</geometry>
</collision>
</link>
<gazebo reference="kuka_arm_5_link">
<material value="kuka-lwr.material"/>
</gazebo>
<transmission name="kuka_arm_4_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="kuka_arm_4_motor"/>
<joint name="kuka_arm_4_joint"/>
<mechanicalReduction>1.0</mechanicalReduction>
</transmission>
<joint name="kuka_arm_5_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0.19"/>
<axis xyz="0 -1 0"/>
<limit effort="306" lower="-2.09439510239" upper="2.09439510239" velocity="3.14159265359"/>
<safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.05948851735" soft_upper_limit="2.05948851735"/>
<dynamics damping="0.1"/>
<parent link="kuka_arm_5_link"/>
<child link="kuka_arm_6_link"/>
</joint>
<link name="kuka_arm_6_link">
<inertial>
<mass value="2.0"/>
<origin rpy="0 0 0" xyz="0 0 0.0625"/>
<inertia ixx="0.00520833333333" ixy="0" ixz="0" iyy="0.00520833333333" iyz="0" izz="0.00520833333333"/>
</inertial>
<visual>
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes_arm/arm_wrist.dae"/>
</geometry>
<material name="Grey"/>
</visual>
<collision>
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes_arm/convex/arm_wrist_convex.stl"/>
</geometry>
</collision>
</link>
<gazebo reference="kuka_arm_6_link">
<material value="kuka-lwr.material"/>
</gazebo>
<transmission name="kuka_arm_5_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="kuka_arm_5_motor"/>
<joint name="kuka_arm_5_joint"/>
<mechanicalReduction>1.0</mechanicalReduction>
</transmission>
<joint name="kuka_arm_6_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0.078"/>
<axis xyz="0 0 1"/>
<limit effort="204" lower="-2.96705972839" upper="2.96705972839" velocity="3.14159265359"/>
<safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.93215314335" soft_upper_limit="2.93215314335"/>
<dynamics damping="0.1"/>
<parent link="kuka_arm_6_link"/>
<child link="kuka_arm_7_link"/>
</joint>
<link name="kuka_arm_7_link">
<inertial>
<mass value="2.0"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.333333333333" ixy="0" ixz="0" iyy="0.333333333333" iyz="0" izz="0.333333333333"/>
</inertial>
<visual>
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes_arm/arm_flanche_angle.dae"/>
</geometry>
<material name="Grey"/>
</visual>
<collision>
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes_arm/convex/arm_flanche_angle__convex.stl"/>
</geometry>
</collision>
</link>
<transmission name="kuka_arm_6_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="kuka_arm_6_motor"/>
<joint name="kuka_arm_6_joint"/>
<mechanicalReduction>1.0</mechanicalReduction>
</transmission>
</robot>

418
data/kuka_lwr/kuka_lwr_arm.urdf.xacro Normal file
View File

@@ -0,0 +1,418 @@
<?xml version="1.0"?>
<robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface">
<property name="M_PI" value="3.1415926535897931" />
<property name="name" value="kuka" />
<!--
Little helper macro to define the inertia matrix needed
for links.
-->
<macro name="cuboid_inertia_def" params="width height length mass">
<inertia ixx="${mass * (height * height + length * length) / 12)}"
iyy="${mass * (width * width + length * length) / 12)}"
izz="${mass * (width * width + height * height) / 12)}"
ixy="0" iyz="0" ixz="0"/>
</macro>
<!-- length is along the y-axis! -->
<macro name="cylinder_inertia_def" params="radius length mass">
<inertia ixx="${mass * (3 * radius * radius + length * length) / 12)}"
iyy="${mass * radius* radius / 2)}"
izz="${mass * (3 * radius * radius + length * length) / 12)}"
ixy="0" iyz="0" ixz="0"/>
</macro>
<property name="arm_elem_link_mass" value="2.0"/>
<property name="arm_elem_ball_link_mass" value="2.0"/>
<property name="arm_elem_end_link_mass" value="2.0"/>
<property name="safety_controller_k_pos" value="100" />
<property name="safety_controller_k_vel" value="2" />
<property name="joint_damping" value="0.1" />
<property name="arm_velocity_scale_factor" value="1"/>
<property name="right" value="0" />
<!-- right is either 1 (for right arm) or -1 (for left arm) -->
<joint name="${name}_arm_base_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0" />
<child link="calib_${name}_arm_base_link"/>
</joint>
<link name="calib_${name}_arm_base_link">
<inertial>
<mass value="2"/>
<origin xyz="0 0 0.055" rpy="0 0 0"/>
<cylinder_inertia_def radius="0.06" length="0.11"
mass="2"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes_arm/arm_base.dae"/>
</geometry>
<material name="Orange"/>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes_arm/convex/arm_base_convex.stl"/>
</geometry>
</collision>
</link>
<gazebo reference="calib_${name}_arm_base_link">
<material value="kuka-lwr.material"/>
</gazebo>
<joint name="${name}_arm_0_joint" type="revolute">
<origin xyz="0 0 0.11" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
<limit lower="${-170 * M_PI / 180}" upper="${170 * M_PI / 180}"
effort="204" velocity="${arm_velocity_scale_factor * 110 * M_PI / 180}" />
<safety_controller soft_lower_limit="${-168 * M_PI / 180}"
soft_upper_limit="${168 * M_PI / 180}"
k_position="${safety_controller_k_pos}"
k_velocity="${safety_controller_k_vel}"/>
<dynamics damping="${joint_damping}"/>
<parent link="calib_${name}_arm_base_link"/>
<child link="${name}_arm_1_link"/>
</joint>
<link name="${name}_arm_1_link">
<inertial>
<mass value="${arm_elem_link_mass}"/>
<origin rpy="0 0 0" xyz="0 0 0.130"/>
<cuboid_inertia_def length="0.12" width="0.06" height="0.260"
mass="${arm_elem_link_mass}"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 ${M_PI}"/>
<geometry>
<mesh filename="meshes_arm/arm_segment_a.dae"/>
</geometry>
<material name="Orange"/>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 ${M_PI}"/>
<geometry>
<mesh filename="meshes_arm/convex/arm_segment_a_convex.stl"/>
</geometry>
</collision>
</link>
<gazebo reference="${name}_arm_1_link">
<material value="kuka-lwr.material"/>
</gazebo>
<transmission name="${name}_arm_0_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="${name}_arm_0_motor"/>
<joint name="${name}_arm_0_joint"/>
<mechanicalReduction>1.0</mechanicalReduction>
</transmission>
<joint name="${name}_arm_1_joint" type="revolute">
<origin xyz="0 0 0.20" rpy="0 0 0"/>
<axis xyz="0 -1 0"/>
<!--limit lower="${-120 * M_PI / 180}" upper="${120 * M_PI / 180}"
effort="306" velocity="${arm_velocity_scale_factor * 110 * M_PI / 180}" /-->
<!-- nalt: Reduced limits to avoid contact with table - kinda hacky, should not be done here -->
<limit lower="${-90 * M_PI / 180}" upper="${90 * M_PI / 180}"
effort="306" velocity="${arm_velocity_scale_factor * 110 * M_PI / 180}" />
<safety_controller soft_lower_limit="${-118 * M_PI / 180}"
soft_upper_limit="${118 * M_PI / 180}"
k_position="${safety_controller_k_pos}"
k_velocity="${safety_controller_k_vel}"/>
<dynamics damping="${joint_damping}"/>
<parent link="${name}_arm_1_link"/>
<child link="${name}_arm_2_link"/>
</joint>
<link name="${name}_arm_2_link">
<inertial>
<mass value="${arm_elem_link_mass}"/>
<origin rpy="0 0 0" xyz="0 0.06 ${0.130 - 0.06}"/>
<cuboid_inertia_def length="0.12" width="0.06" height="0.260"
mass="${arm_elem_link_mass}"/>
</inertial>
<visual>
<origin xyz="0 0 0.2" rpy="${M_PI} 0 ${M_PI}"/>
<geometry>
<mesh filename="meshes_arm/arm_segment_b.dae"/>
</geometry>
<material name="Orange"/>
</visual>
<collision>
<origin xyz="0 0 0.2" rpy="${M_PI} 0 ${M_PI}"/>
<geometry>
<mesh filename="meshes_arm/convex/arm_segment_b_convex.stl"/>
</geometry>
</collision>
</link>
<gazebo reference="${name}_arm_2_link">
<material value="kuka-lwr.material"/>
</gazebo>
<transmission name="${name}_arm_1_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="${name}_arm_1_motor"/>
<joint name="${name}_arm_1_joint"/>
<mechanicalReduction>1.0</mechanicalReduction>
</transmission>
<joint name="${name}_arm_2_joint" type="revolute">
<origin xyz="0 0 0.20" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
<limit lower="${-170 * M_PI / 180}" upper="${170 * M_PI / 180}"
effort="204" velocity="${arm_velocity_scale_factor * 130 * M_PI / 180}" />
<safety_controller soft_lower_limit="${-168 * M_PI / 180}"
soft_upper_limit="${168 * M_PI / 180}"
k_position="${safety_controller_k_pos}"
k_velocity="${safety_controller_k_vel}"/>
<dynamics damping="${joint_damping}"/>
<parent link="${name}_arm_2_link"/>
<child link="${name}_arm_3_link"/>
</joint>
<link name="${name}_arm_3_link">
<inertial>
<mass value="${arm_elem_link_mass}"/>
<origin rpy="0 0 0" xyz="0 0.06 0.130"/>
<cuboid_inertia_def length="0.12" width="0.06" height="0.260"
mass="${arm_elem_link_mass}"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes_arm/arm_segment_a.dae"/>
</geometry>
<material name="Orange"/>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes_arm/convex/arm_segment_a_convex.stl"/>
</geometry>
</collision>
</link>
<gazebo reference="${name}_arm_3_link">
<material value="kuka-lwr.material"/>
</gazebo>
<transmission name="${name}_arm_2_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="${name}_arm_2_motor"/>
<joint name="${name}_arm_2_joint"/>
<mechanicalReduction>1.0</mechanicalReduction>
</transmission>
<joint name="${name}_arm_3_joint" type="revolute">
<origin xyz="0 0 0.20" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<limit lower="${-120 * M_PI / 180}" upper="${120 * M_PI / 180}"
effort="306" velocity="${arm_velocity_scale_factor * 130 * M_PI / 180}" />
<safety_controller soft_lower_limit="${-118 * M_PI / 180}"
soft_upper_limit="${118 * M_PI / 180}"
k_position="${safety_controller_k_pos}"
k_velocity="${safety_controller_k_vel}"/>
<dynamics damping="${joint_damping}"/>
<parent link="${name}_arm_3_link"/>
<child link="${name}_arm_4_link"/>
</joint>
<link name="${name}_arm_4_link">
<inertial>
<mass value="${arm_elem_link_mass}"/>
<origin rpy="0 0 0" xyz="0 -0.06 ${0.130 - 0.06}"/>
<cuboid_inertia_def length="0.12" width="0.06" height="0.2600"
mass="${arm_elem_link_mass}"/>
</inertial>
<visual>
<origin xyz="0 0 0.2" rpy="0 ${M_PI} ${M_PI}"/>
<geometry>
<mesh filename="meshes_arm/arm_segment_b.dae"/>
</geometry>
<material name="Orange"/>
</visual>
<collision>
<origin xyz="0 0 0.2" rpy="0 ${M_PI} ${M_PI}"/>
<geometry>
<mesh filename="meshes_arm/convex/arm_segment_b_convex.stl"/>
</geometry>
</collision>
</link>
<gazebo reference="${name}_arm_4_link">
<material value="kuka-lwr.material"/>
</gazebo>
<transmission name="${name}_arm_3_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="${name}_arm_3_motor"/>
<joint name="${name}_arm_3_joint"/>
<mechanicalReduction>1.0</mechanicalReduction>
</transmission>
<joint name="${name}_arm_4_joint" type="revolute">
<origin xyz="0 0 0.20" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
<limit lower="${-170 * M_PI / 180}" upper="${170 * M_PI / 180}"
effort="204" velocity="${arm_velocity_scale_factor * 130 * M_PI / 180}" />
<safety_controller soft_lower_limit="${-168 * M_PI / 180}"
soft_upper_limit="${168 * M_PI / 180}"
k_position="${safety_controller_k_pos}"
k_velocity="${safety_controller_k_vel}"/>
<dynamics damping="${joint_damping}"/>
<parent link="${name}_arm_4_link"/>
<child link="${name}_arm_5_link"/>
</joint>
<link name="${name}_arm_5_link">
<inertial>
<mass value="${arm_elem_link_mass}"/>
<origin rpy="0 0 0" xyz="0 0 0.124"/>
<cuboid_inertia_def length="0.12" width="0.06" height="0.248"
mass="${arm_elem_link_mass}"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 ${M_PI}"/>
<geometry name="${name}_arm_5_geom">
<mesh filename="meshes_arm/arm_segment_last.dae"/>
</geometry>
<material name="Orange"/>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 ${M_PI}"/>
<geometry>
<mesh filename="meshes_arm/convex/arm_segment_last_convex.stl"/>
</geometry>
</collision>
</link>
<gazebo reference="${name}_arm_5_link">
<material value="kuka-lwr.material"/>
</gazebo>
<transmission name="${name}_arm_4_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="${name}_arm_4_motor"/>
<joint name="${name}_arm_4_joint"/>
<mechanicalReduction>1.0</mechanicalReduction>
</transmission>
<joint name="${name}_arm_5_joint" type="revolute">
<origin xyz="0 0 0.19" rpy="0 0 0"/>
<axis xyz="0 -1 0"/>
<limit lower="${-120 * M_PI / 180}" upper="${120 * M_PI / 180}"
effort="306" velocity="${arm_velocity_scale_factor * 180 * M_PI / 180}" />
<safety_controller soft_lower_limit="${-118 * M_PI / 180}"
soft_upper_limit="${118 * M_PI / 180}"
k_position="${safety_controller_k_pos}"
k_velocity="${safety_controller_k_vel}"/>
<dynamics damping="${joint_damping}"/>
<parent link="${name}_arm_5_link"/>
<child link="${name}_arm_6_link"/>
</joint>
<link name="${name}_arm_6_link">
<inertial>
<mass value="${arm_elem_ball_link_mass}"/>
<origin rpy="0 0 0" xyz="0 0 0.0625"/>
<cuboid_inertia_def length="0.125" width="0.125" height="0.125"
mass="${arm_elem_ball_link_mass}"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 ${M_PI}"/>
<geometry>
<mesh filename="meshes_arm/arm_wrist.dae"/>
</geometry>
<material name="Grey"/>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 ${M_PI}"/>
<geometry>
<mesh filename="meshes_arm/convex/arm_wrist_convex.stl"/>
</geometry>
</collision>
</link>
<gazebo reference="${name}_arm_6_link">
<material value="kuka-lwr.material"/>
</gazebo>
<transmission name="${name}_arm_5_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="${name}_arm_5_motor"/>
<joint name="${name}_arm_5_joint"/>
<mechanicalReduction>1.0</mechanicalReduction>
</transmission>
<joint name="${name}_arm_6_joint" type="revolute">
<origin xyz="0 0 0.078" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
<limit lower="${-170 * M_PI / 180}" upper="${170 * M_PI / 180}"
effort="204" velocity="${arm_velocity_scale_factor * 180 * M_PI / 180}" />
<safety_controller soft_lower_limit="${-168 * M_PI / 180}"
soft_upper_limit="${168 * M_PI / 180}"
k_position="${safety_controller_k_pos}"
k_velocity="${safety_controller_k_vel}"/>
<dynamics damping="${joint_damping}"/>
<parent link="${name}_arm_6_link"/>
<child link="${name}_arm_7_link"/>
</joint>
<link name="${name}_arm_7_link">
<inertial>
<mass value="${arm_elem_end_link_mass}"/>
<origin xyz="0 0 0"/>
<cuboid_inertia_def length="1" width="1" height="1"
mass="${arm_elem_end_link_mass}"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 ${right * -1/4 * M_PI + M_PI}"/>
<geometry>
<mesh filename="meshes_arm/arm_flanche_angle.dae"/>
</geometry>
<material name="Grey"/>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 ${right * -1/4 * M_PI + M_PI}"/>
<geometry>
<mesh filename="meshes_arm/convex/arm_flanche_angle__convex.stl"/>
</geometry>
</collision>
</link>
<link name="lwr_arm_hand_link" />
<gazebo reference="${name}_arm_7_link">
<material value="kuka-lwr.material"/>
</gazebo>
<transmission name="${name}_arm_6_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="${name}_arm_6_motor"/>
<joint name="${name}_arm_6_joint"/>
<mechanicalReduction>1.0</mechanicalReduction>
</transmission>
<joint name="${name}_arm_hand_fixed_joint" type="fixed">
<origin xyz="${-right * 0.075} -0.075 -0.094"
rpy="${0.5*right*M_PI} 0 ${(1.5 + 0.25*right)*M_PI}"/>
<parent link="${name}_arm_7_link"/>
<child link="${name}_arm_hand_link"/>
</joint>
</robot>

34
data/kuka_lwr/materials.xml Normal file
View File

@@ -0,0 +1,34 @@
<?xml version="1.0"?>
<robot>
<material name="DarkGrey">
<color rgba="0.3 0.3 0.3 1.0"/>
</material>
<material name="Black">
<color rgba="0 0 0 1.0"/>
</material>
<material name="Orange">
<color rgba="1.0 0.487 0 1.0"/>
</material>
<material name="Silver">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
<material name="Grey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
<material name="Blue">
<color rgba="0.0 0.0 0.8 1.0"/>
</material>
<material name="HandGray">
<color rgba="0.953 0.996 0.694 1.0"/>
</material>
<material name="SickBlue">
<color rgba="0.3058 0.5921 0.7294 1.0"/>
</material>
</robot>

12
data/kuka_lwr/meshes_arm/COPYRIGHT Normal file
View File

@@ -0,0 +1,12 @@
The meshes of the lightweight robot are based on a blender model
which was re-designed from scratch. (It can be downloaded at
http://toychest.in.tum.de/wiki/_media/projects:lwr-arm.blend)
This blender model, as well as the meshes found in this folder
are licensed under CC-BY-3.0:
Copyright (c) 2010 by Ingo Kresse <kresse@in.tum.de>.
This work is made available under the terms of the
Creative Commons Attribution 3.0 license,
http://creativecommons.org/licenses/by/3.0/.

162
data/kuka_lwr/meshes_arm/arm_base.dae Normal file
View File

File diff suppressed because one or more lines are too long

BIN
data/kuka_lwr/meshes_arm/arm_base.stl Normal file
View File

Binary file not shown.

158
data/kuka_lwr/meshes_arm/arm_flanche.dae Normal file
View File

File diff suppressed because one or more lines are too long

BIN
data/kuka_lwr/meshes_arm/arm_flanche.stl Normal file
View File

Binary file not shown.

162
data/kuka_lwr/meshes_arm/arm_flanche_angle.dae Normal file
View File

@@ -0,0 +1,162 @@
<?xml version="1.0" encoding="utf-8"?>
<COLLADA version="1.4.0" xmlns="http://www.collada.org/2005/11/COLLADASchema">
<asset>
<contributor>
<author>Illusoft Collada 1.4.0 plugin for Blender - http://colladablender.illusoft.com</author>
<authoring_tool>Blender v:249 - Illusoft Collada Exporter v:0.3.162</authoring_tool>
<comments></comments>
<copyright></copyright>
<source_data>file:///home/ingo/models/lwr/meshes/arm_flanche.blend</source_data>
</contributor>
<created>2010-07-28T12:31:52.074222</created>
<modified>2010-07-28T12:31:52.074265</modified>
<unit meter="1" name="meter"/>
<up_axis>Z_UP</up_axis>
</asset>
<library_effects>
<effect id="Black-fx" name="Black-fx">
<profile_COMMON>
<technique sid="blender">
<phong>
<emission>
<color>0.00000 0.00000 0.00000 1</color>
</emission>
<ambient>
<color>0.10000 0.10000 0.10000 1</color>
</ambient>
<diffuse>
<color>0.10000 0.10000 0.10000 1</color>
</diffuse>
<specular>
<color>0.50000 0.50000 0.50000 1</color>
</specular>
<shininess>
<float>12.5</float>
</shininess>
<reflective>
<color>1.00000 1.00000 1.00000 1</color>
</reflective>
<reflectivity>
<float>1.0</float>
</reflectivity>
<transparent>
<color>1 1 1 1</color>
</transparent>
<transparency>
<float>0.0</float>
</transparency>
</phong>
</technique>
</profile_COMMON>
</effect>
<effect id="Grey-fx" name="Grey-fx">
<profile_COMMON>
<technique sid="blender">
<phong>
<emission>
<color>0.00000 0.00000 0.00000 1</color>
</emission>
<ambient>
<color>0.21666 0.22745 0.25000 1</color>
</ambient>
<diffuse>
<color>0.60667 0.63686 0.70000 1</color>
</diffuse>
<specular>
<color>0.70000 0.70000 0.70000 1</color>
</specular>
<shininess>
<float>12.5</float>
</shininess>
<reflective>
<color>1.00000 1.00000 1.00000 1</color>
</reflective>
<reflectivity>
<float>0.0</float>
</reflectivity>
<transparent>
<color>1 1 1 1</color>
</transparent>
<transparency>
<float>0.0</float>
</transparency>
</phong>
</technique>
</profile_COMMON>
</effect>
</library_effects>
<library_materials>
<material id="Black" name="Black">
<instance_effect url="#Black-fx"/>
</material>
<material id="Grey" name="Grey">
<instance_effect url="#Grey-fx"/>
</material>
</library_materials>
<library_geometries>
<geometry id="flanche_001" name="flanche_001">
<mesh>
<source id="flanche_001-Position">
<float_array count="312" id="flanche_001-Position-array">0.00701 -0.03071 0.00000 0.00701 -0.03071 0.00800 -0.00701 -0.03071 0.00000 -0.00701 -0.03071 0.00800 0.01964 -0.02463 0.00000 0.01964 -0.02463 0.00800 -0.01964 -0.02463 0.00000 -0.01964 -0.02463 0.00800 0.02838 -0.01367 0.00000 0.02838 -0.01367 0.00800 -0.02838 -0.01367 0.00000 -0.02838 -0.01367 0.00800 0.03150 0.00000 0.00000 0.03150 0.00000 0.00800 -0.03150 -0.00000 0.00000 -0.03150 -0.00000 0.00800 0.03150 0.05550 0.00000 -0.03150 0.05550 0.00000 0.03150 0.05881 0.00800 -0.03150 0.05881 0.00800 0.03150 0.09300 -0.09300 0.03150 0.09300 -0.03750 0.02838 0.09300 -0.10667 0.01964 0.09300 -0.11763 0.00701 0.09300 -0.12371 -0.00701 0.09300 -0.12371 -0.01964 0.09300 -0.11763 -0.02838 0.09300 -0.10667 -0.03150 0.09300 -0.09300 -0.03150 0.09300 -0.03750 0.03150 0.10100 -0.09300 0.03150 0.10100 -0.03419 0.02838 0.10100 -0.10667 0.01964 0.10100 -0.11763 0.00701 0.10100 -0.12371 -0.00701 0.10100 -0.12371 -0.01964 0.10100 -0.11763 -0.02838 0.10100 -0.10667 -0.03150 0.10100 -0.09300 -0.03150 0.10100 -0.03419 -0.02828 0.02828 -0.02000 -0.02222 0.03326 -0.02000 -0.01531 0.03696 -0.02000 -0.00780 0.03923 -0.02000 -0.00000 0.04000 -0.02000 0.00780 0.03923 -0.02000 0.01531 0.03696 -0.02000 0.02222 0.03326 -0.02000 0.02828 0.02828 -0.02000 0.03326 0.02222 -0.02000 0.03696 0.01531 -0.02000 0.03923 0.00780 -0.02000 0.04000 -0.00000 -0.02000 0.03923 -0.00780 -0.02000 0.03696 -0.01531 -0.02000 0.03326 -0.02222 -0.02000 0.02828 -0.02828 -0.02000 0.02222 -0.03326 -0.02000 0.01531 -0.03696 -0.02000 0.00780 -0.03923 -0.02000 -0.00000 -0.04000 -0.02000 -0.00780 -0.03923 -0.02000 -0.01531 -0.03696 -0.02000 -0.02222 -0.03326 -0.02000 -0.02828 -0.02828 -0.02000 -0.03326 -0.02222 -0.02000 -0.03696 -0.01531 -0.02000 -0.03923 -0.00780 -0.02000 -0.04000 0.00000 -0.02000 -0.03923 0.00780 -0.02000 -0.03696 0.01531 -0.02000 -0.03326 0.02222 -0.02000 -0.02090 0.02090 -0.00000 -0.01642 0.02458 -0.00000 -0.01131 0.02731 -0.00000 -0.00577 0.02899 -0.00000 0.00000 0.02956 -0.00000 0.00577 0.02899 -0.00000 0.01131 0.02731 -0.00000 0.01642 0.02458 -0.00000 0.02090 0.02090 -0.00000 0.02458 0.01642 -0.00000 0.02731 0.01131 -0.00000 0.02899 0.00577 -0.00000 0.02956 -0.00000 -0.00000 0.02899 -0.00577 -0.00000 0.02731 -0.01131 -0.00000 0.02458 -0.01642 -0.00000 0.02090 -0.02090 -0.00000 0.01642 -0.02458 -0.00000 0.01131 -0.02731 -0.00000 0.00577 -0.02899 -0.00000 0.00000 -0.02956 -0.00000 -0.00577 -0.02899 -0.00000 -0.01131 -0.02731 -0.00000 -0.01642 -0.02458 -0.00000 -0.02090 -0.02090 -0.00000 -0.02458 -0.01642 -0.00000 -0.02731 -0.01131 -0.00000 -0.02899 -0.00577 -0.00000 -0.02956 -0.00000 -0.00000 -0.02899 0.00577 -0.00000 -0.02731 0.01131 -0.00000 -0.02458 0.01642 -0.00000</float_array>
<technique_common>
<accessor count="104" source="#flanche_001-Position-array" stride="3">
<param type="float" name="X"></param>
<param type="float" name="Y"></param>
<param type="float" name="Z"></param>
</accessor>
</technique_common>
</source>
<source id="flanche_001-Normals">
<float_array count="324" id="flanche_001-Normals-array">-0.00000 0.00000 1.00000 0.00000 0.00000 1.00000 0.00000 -0.00000 1.00000 0.00000 -0.00000 1.00000 0.00000 -0.00000 1.00000 0.00000 -0.00000 1.00000 0.00000 -0.00000 1.00000 0.00000 -0.00000 1.00000 0.00000 0.00000 -1.00000 -0.00000 0.00000 -1.00000 -0.00000 0.00000 -1.00000 -0.00000 0.00000 -1.00000 -0.00000 0.00000 -1.00000 -0.00000 0.00000 -1.00000 0.00000 0.00000 -1.00000 0.00000 0.00000 -1.00000 0.00000 1.00000 -0.00000 0.00000 1.00000 -0.00000 0.00000 1.00000 -0.00000 0.00000 1.00000 0.00000 0.00000 1.00000 0.00000 0.00000 1.00000 -0.00000 0.00000 1.00000 -0.00000 0.00000 1.00000 -0.00000 0.00000 -1.00000 0.00000 0.00000 -1.00000 -0.00000 0.00000 -1.00000 -0.00000 0.00000 -1.00000 0.00000 -0.00000 -1.00000 -0.00000 -0.00000 -1.00000 0.00000 0.00000 -1.00000 -0.00000 -0.00000 -1.00000 -0.00000 1.00000 0.00000 0.00000 1.00000 0.00000 0.00000 1.00000 0.00000 0.00000 1.00000 0.00000 0.00000 1.00000 0.00000 -0.00000 1.00000 0.00000 0.00000 -1.00000 -0.00000 -0.00000 -1.00000 -0.00000 0.00000 -1.00000 -0.00000 0.00000 -1.00000 -0.00000 -0.00000 -1.00000 -0.00000 0.00000 -1.00000 -0.00000 0.00000 0.00000 0.70711 0.70711 0.00000 0.70711 0.70711 0.00000 -0.70711 -0.70711 -0.00000 -0.70711 -0.70711 -0.97493 -0.00000 -0.22249 -0.97493 0.00000 -0.22249 -0.78184 -0.00000 -0.62348 -0.78184 0.00000 -0.62348 -0.43384 -0.00000 -0.90099 -0.43384 0.00000 -0.90099 -0.00000 -0.00000 -1.00000 -0.00000 0.00000 -1.00000 0.43384 0.00000 -0.90099 0.43384 0.00000 -0.90099 0.78184 0.00000 -0.62348 0.78184 0.00000 -0.62348 0.97493 0.00000 -0.22249 0.97493 0.00000 -0.22249 0.97493 -0.22249 0.00000 0.97493 -0.22249 0.00000 0.78184 -0.62349 0.00000 0.78184 -0.62349 0.00000 0.43384 -0.90099 0.00000 0.43384 -0.90099 0.00000 0.00000 -1.00000 0.00000 0.00000 -1.00000 0.00000 -0.43384 -0.90099 -0.00000 -0.43384 -0.90099 0.00000 -0.78184 -0.62349 -0.00000 -0.78184 -0.62349 0.00000 -0.97493 -0.22249 -0.00000 -0.97493 -0.22249 0.00000 -0.56293 0.68593 0.46109 -0.41830 0.78258 0.46109 -0.25758 0.84915 0.46109 -0.08698 0.88308 0.46109 0.08698 0.88308 0.46109 0.25759 0.84915 0.46109 0.41830 0.78258 0.46109 0.56293 0.68593 0.46109 0.68594 0.56293 0.46109 0.78258 0.41830 0.46109 0.84915 0.25759 0.46109 0.88308 0.08698 0.46109 0.88308 -0.08698 0.46109 0.84915 -0.25759 0.46109 0.78258 -0.41830 0.46109 0.68594 -0.56293 0.46109 0.56293 -0.68594 0.46109 0.41830 -0.78258 0.46109 0.25759 -0.84915 0.46109 0.08698 -0.88308 0.46109 -0.08698 -0.88308 0.46109 -0.25758 -0.84915 0.46109 -0.41830 -0.78258 0.46109 -0.56293 -0.68594 0.46109 -0.68593 -0.56293 0.46109 -0.78258 -0.41830 0.46109 -0.84915 -0.25759 0.46109 -0.88308 -0.08698 0.46109 -0.88308 0.08697 0.46109 -0.84915 0.25758 0.46109 -0.78258 0.41829 0.46109 -0.68594 0.56293 0.46109</float_array>
<technique_common>
<accessor count="108" source="#flanche_001-Normals-array" stride="3">
<param type="float" name="X"></param>
<param type="float" name="Y"></param>
<param type="float" name="Z"></param>
</accessor>
</technique_common>
</source>
<vertices id="flanche_001-Vertex">
<input semantic="POSITION" source="#flanche_001-Position"/>
</vertices>
<triangles count="64" material="Black">
<input offset="0" semantic="VERTEX" source="#flanche_001-Vertex"/>
<input offset="1" semantic="NORMAL" source="#flanche_001-Normals"/>
<p>40 76 72 76 73 76 73 76 41 76 40 76 41 77 73 77 74 77 74 77 42 77 41 77 42 78 74 78 75 78 75 78 43 78 42 78 43 79 75 79 76 79 76 79 44 79 43 79 44 80 76 80 77 80 77 80 45 80 44 80 45 81 77 81 78 81 78 81 46 81 45 81 46 82 78 82 79 82 79 82 47 82 46 82 47 83 79 83 80 83 80 83 48 83 47 83 48 84 80 84 81 84 81 84 49 84 48 84 49 85 81 85 82 85 82 85 50 85 49 85 50 86 82 86 83 86 83 86 51 86 50 86 51 87 83 87 84 87 84 87 52 87 51 87 52 88 84 88 85 88 85 88 53 88 52 88 53 89 85 89 86 89 86 89 54 89 53 89 54 90 86 90 87 90 87 90 55 90 54 90 55 91 87 91 88 91 88 91 56 91 55 91 56 92 88 92 89 92 89 92 57 92 56 92 57 93 89 93 90 93 90 93 58 93 57 93 58 94 90 94 91 94 91 94 59 94 58 94 59 95 91 95 92 95 92 95 60 95 59 95 60 96 92 96 93 96 93 96 61 96 60 96 61 97 93 97 94 97 94 97 62 97 61 97 62 98 94 98 95 98 95 98 63 98 62 98 63 99 95 99 96 99 96 99 64 99 63 99 64 100 96 100 97 100 97 100 65 100 64 100 65 101 97 101 98 101 98 101 66 101 65 101 66 102 98 102 99 102 99 102 67 102 66 102 67 103 99 103 100 103 100 103 68 103 67 103 68 104 100 104 101 104 101 104 69 104 68 104 69 105 101 105 102 105 102 105 70 105 69 105 70 106 102 106 103 106 103 106 71 106 70 106 72 107 40 107 71 107 71 107 103 107 72 107</p>
</triangles>
<triangles count="76" material="Grey">
<input offset="0" semantic="VERTEX" source="#flanche_001-Vertex"/>
<input offset="1" semantic="NORMAL" source="#flanche_001-Normals"/>
<p>13 0 18 0 19 0 19 1 15 1 13 1 13 2 15 2 11 2 13 3 11 3 9 3 9 4 11 4 7 4 9 5 7 5 5 5 5 6 7 6 3 6 5 7 3 7 1 7 17 8 16 8 14 8 14 9 16 9 12 9 14 10 12 10 10 10 10 11 12 11 8 11 10 12 8 12 6 12 6 13 8 13 4 13 6 14 4 14 2 14 4 15 0 15 2 15 39 16 31 16 38 16 38 17 31 17 30 17 38 18 30 18 37 18 30 19 32 19 33 19 35 20 36 20 37 20 37 21 30 21 35 21 35 22 30 22 33 22 35 23 33 23 34 23 20 24 21 24 29 24 22 25 20 25 23 25 23 26 20 26 25 26 23 27 25 27 24 27 26 28 25 28 27 28 27 29 25 29 20 29 27 30 20 30 28 30 28 31 20 31 29 31 20 32 30 32 21 32 21 33 30 33 31 33 21 34 31 34 16 34 16 35 31 35 18 35 16 36 18 36 12 36 12 37 18 37 13 37 14 38 15 38 17 38 17 39 15 39 19 39 17 40 19 40 29 40 29 41 19 41 39 41 29 42 39 42 28 42 28 43 39 43 38 43 39 44 19 44 31 44 31 45 19 45 18 45 17 46 29 46 16 46 16 47 29 47 21 47 28 48 38 48 37 48 28 49 37 49 27 49 27 50 37 50 36 50 27 51 36 51 26 51 26 52 36 52 35 52 26 53 35 53 25 53 25 54 35 54 34 54 25 55 34 55 24 55 24 56 34 56 33 56 24 57 33 57 23 57 23 58 33 58 32 58 23 59 32 59 22 59 22 60 32 60 30 60 22 61 30 61 20 61 12 62 13 62 9 62 12 63 9 63 8 63 8 64 9 64 5 64 8 65 5 65 4 65 4 66 5 66 1 66 1 67 0 67 4 67 0 68 1 68 3 68 0 69 3 69 2 69 2 70 3 70 7 70 2 71 7 71 6 71 6 72 7 72 11 72 6 73 11 73 10 73 10 74 11 74 15 74 10 75 15 75 14 75</p>
</triangles>
</mesh>
</geometry>
</library_geometries>
<library_visual_scenes>
<visual_scene id="Scene" name="Scene">
<node layer="L1" id="flanche" name="flanche">
<matrix>
1.0 0.0 0.0 0.0
0.0 1.0 0.0 0.0
0.0 0.0 1.0 0.0
0.0 0.0 0.0 1.0
</matrix>
<instance_geometry url="#flanche_001">
<bind_material>
<technique_common>
<instance_material symbol="Black" target="#Black">
<bind_vertex_input input_semantic="TEXCOORD" input_set="1" semantic="CHANNEL1"/>
</instance_material>
<instance_material symbol="Grey" target="#Grey">
<bind_vertex_input input_semantic="TEXCOORD" input_set="1" semantic="CHANNEL1"/>
</instance_material>
</technique_common>
</bind_material>
</instance_geometry>
</node>
</visual_scene>
</library_visual_scenes>
<scene>
<instance_visual_scene url="#Scene"/>
</scene>
</COLLADA>

BIN
data/kuka_lwr/meshes_arm/arm_flanche_angle.stl Normal file
View File

Binary file not shown.

162
data/kuka_lwr/meshes_arm/arm_segment_a.dae Normal file
View File

File diff suppressed because one or more lines are too long

BIN
data/kuka_lwr/meshes_arm/arm_segment_a.stl Normal file
View File

Binary file not shown.

162
data/kuka_lwr/meshes_arm/arm_segment_b.dae Normal file
View File

File diff suppressed because one or more lines are too long

BIN
data/kuka_lwr/meshes_arm/arm_segment_b.stl Normal file
View File

Binary file not shown.

116
data/kuka_lwr/meshes_arm/arm_segment_last.dae Normal file
View File

File diff suppressed because one or more lines are too long

BIN
data/kuka_lwr/meshes_arm/arm_segment_last.stl Normal file
View File

Binary file not shown.

116
data/kuka_lwr/meshes_arm/arm_wrist.dae Normal file
View File

File diff suppressed because one or more lines are too long

BIN
data/kuka_lwr/meshes_arm/arm_wrist.stl Normal file
View File

Binary file not shown.

4
data/kuka_lwr/meshes_arm/convex/README Normal file
View File

@@ -0,0 +1,4 @@
convex collision hulls, created using
rosrun convex_decomposition convex_decomposition <mesh.obj> -v24 -p10 -s0.01
rosrun ivcon ivcon <mesh_convex.obj> <mesh_convex.stlb>

BIN
data/kuka_lwr/meshes_arm/convex/arm_base_convex.stl Normal file
View File

Binary file not shown.

BIN
data/kuka_lwr/meshes_arm/convex/arm_flanche_angle_convex.stl Normal file
View File

Binary file not shown.

BIN
data/kuka_lwr/meshes_arm/convex/arm_flanche_convex.stl Normal file
View File

Binary file not shown.

BIN
data/kuka_lwr/meshes_arm/convex/arm_segment_a_convex.stl Normal file
View File

Binary file not shown.

BIN
data/kuka_lwr/meshes_arm/convex/arm_segment_b_convex.stl Normal file
View File

Binary file not shown.

BIN
data/kuka_lwr/meshes_arm/convex/arm_segment_last_convex.stl Normal file
View File

Binary file not shown.

BIN
data/kuka_lwr/meshes_arm/convex/arm_wrist_convex.stl Normal file
View File

Binary file not shown.

1823
data/kuka_lwr/tools/jr3-stabilo.dae Normal file
View File

File diff suppressed because one or more lines are too long

BIN
data/kuka_lwr/tools/jr3-stabilo.skp Normal file
View File

Binary file not shown.

4
examples/BasicDemo/BasicExample.cpp
View File

@@ -73,7 +73,7 @@ void BasicExample::initPhysics()
{
btScalar mass(0.);
btRigidBody* body = createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1));
createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1));
}
@@ -113,7 +113,7 @@ void BasicExample::initPhysics()
btScalar(2.0*j)));
btRigidBody* body = createRigidBody(mass,startTransform,colShape);
createRigidBody(mass,startTransform,colShape);
}

1
examples/BasicDemo/main.cpp
View File

@@ -24,7 +24,6 @@ subject to the following restrictions:
int main(int argc, char* argv[])
{
struct PhysicsInterface* pint = 0;
DummyGUIHelper noGfx;

1
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -17,6 +17,7 @@ SET(App_ExampleBrowser_SRCS
ExampleEntries.h
../SharedMemory/PhysicsServer.cpp
../SharedMemory/PhysicsClient.cpp
../SharedMemory/PhysicsClientC_API.cpp
../SharedMemory/PhysicsServerExample.cpp
../SharedMemory/PhysicsClientExample.cpp
../SharedMemory/RobotControlExample.cpp

4
examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.cpp
View File

@@ -167,8 +167,8 @@ void GwenUserInterface::setExampleDescription(const char* message)
m_data->m_exampleInfoTextOutput->Clear();
int fixedWidth = m_data->m_exampleInfoTextOutput->GetBounds().w-25;
for (int endPos=0;endPos<=wrapmessage.length();endPos++)
int wrapLen = int(wrapmessage.length());
for (int endPos=0;endPos<=wrapLen;endPos++)
{
std::string sub = wrapmessage.substr(startPos,(endPos-startPos));
Gwen::Point pt = m_data->pRenderer->MeasureText(m_data->pCanvas->GetSkin()->GetDefaultFont(),sub);

3
examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View File

@@ -240,7 +240,6 @@ void openFileDemo(const char* filename)
options.m_fileName = filename;
char fullPath[1024];
int fileType = 0;
sprintf(fullPath, "%s", filename);
b3FileUtils::toLower(fullPath);
if (strstr(fullPath, ".urdf"))
@@ -296,7 +295,6 @@ void selectDemo(int demoIndex)
{
if (gui)
{
bool isLeft = true;
gui->setStatusBarMessage("Status: OK", false);
}
b3Printf("Selected demo: %s",gAllExamples->getExampleName(demoIndex));
@@ -429,7 +427,6 @@ struct MyMenuItemHander :public Gwen::Event::Handler
Gwen::String laa = Gwen::Utility::UnicodeToString(la);
//const char* ha = laa.c_str();
bool handled = false;
selectDemo(sCurrentHightlighted);
saveCurrentDemoEntry(sCurrentDemoIndex, startFileName);

2
examples/ExampleBrowser/main.cpp
View File

@@ -16,7 +16,7 @@
int main(int argc, char* argv[])
{
b3CommandLineArgs args(argc,argv);
b3Clock clock;
b3Clock clock;
ExampleEntries examples;

8
examples/ExampleBrowser/premake4.lua
View File

@@ -48,8 +48,12 @@
files {
"**.cpp",
"**.h",
"*.cpp",
"*.h",
"GwenGUISupport/*.cpp",
"GwenGUISupport/*.h",
"../SharedMemory/PhysicsClientC_API.cpp",
"../SharedMemory/PhysicsClientC_API.h",
"../SharedMemory/PhysicsServerExample.cpp",
"../SharedMemory/PhysicsClientExample.cpp",
"../SharedMemory/RobotControlExample.cpp",

3
examples/Importers/ImportBullet/SerializeSetup.cpp
View File

@@ -59,8 +59,6 @@ void SerializeSetup::initPhysics()
int numPrefixes = sizeof(prefix)/sizeof(const char*);
char relativeFileName[1024];
FILE* f=0;
bool fileFound = false;
int result = 0;
for (int i=0;!f && i<numPrefixes;i++)
{
@@ -68,7 +66,6 @@ void SerializeSetup::initPhysics()
f = fopen(relativeFileName,"rb");
if (f)
{
fileFound = true;
break;
}
}

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

@@ -231,7 +231,7 @@ void ImportUrdfSetup::initPhysics()
if (loadOk)
{
printTree(u2b,u2b.getRootLinkIndex());
//printTree(u2b,u2b.getRootLinkIndex());
//u2b.printTree();

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

@@ -156,8 +156,8 @@ class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createRevoluteJoint(in
userInfo->m_urdfJointType = URDFRevoluteJoint;
userInfo->m_lowerJointLimit = jointLowerLimit;
userInfo->m_upperJointLimit = jointUpperLimit;
userInfo->m_urdfIndex = urdfLinkIndex;
}
userInfo->m_urdfIndex = urdfLinkIndex;
dof6->setUserConstraintPtr(userInfo);
m_6DofConstraints.push_back(dof6);
return dof6;

61
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -10,7 +10,6 @@
#include "URDFImporterInterface.h"
#include "MultiBodyCreationInterface.h"
#include <string>
#include "Bullet3Common/b3Logging.h"
static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter;
static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter);
@@ -35,42 +34,6 @@ static btVector3 selectColor2()
return color;
}
void printTree(const URDFImporterInterface& u2b, int linkIndex, int indentationLevel)
{
btScalar mass;
btVector3 localInertia;
btTransform inertialFrame;
u2b.getMassAndInertia(linkIndex,mass,localInertia,inertialFrame);
std::string name = u2b.getLinkName(linkIndex);
for(int j=0;j<indentationLevel;j++)
b3Printf(" "); //indent
b3Printf("link %s mass=%f\n",name.c_str(),mass);
for(int j=0;j<indentationLevel;j++)
b3Printf(" "); //indent
b3Printf("local inertia:%f,%f,%f\n",localInertia[0],
localInertia[1],
localInertia[2]);
btAlignedObjectArray<int> childIndices;
u2b.getLinkChildIndices(linkIndex,childIndices);
int numChildren = childIndices.size();
indentationLevel+=2;
int count = 0;
for (int i=0;i<numChildren;i++)
{
int childLinkIndex = childIndices[i];
std::string name = u2b.getLinkName(childLinkIndex);
for(int j=0;j<indentationLevel;j++)
b3Printf(" "); //indent
b3Printf("child(%d).name=%s with childIndex=%d\n",(count++)+1, name.c_str(),childLinkIndex);
// first grandchild
printTree(u2b,childLinkIndex,indentationLevel);
}
}
struct URDF2BulletCachedData
@@ -182,7 +145,7 @@ void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedDat
void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
{
b3Printf("start converting/extracting data from URDF interface\n");
//b3Printf("start converting/extracting data from URDF interface\n");
btTransform linkTransformInWorldSpace;
linkTransformInWorldSpace.setIdentity();
@@ -194,9 +157,9 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex);
btRigidBody* parentRigidBody = 0;
std::string name = u2b.getLinkName(urdfLinkIndex);
b3Printf("link name=%s urdf link index=%d\n",name.c_str(),urdfLinkIndex);
b3Printf("mb link index = %d\n",mbLinkIndex);
//std::string name = u2b.getLinkName(urdfLinkIndex);
//b3Printf("link name=%s urdf link index=%d\n",name.c_str(),urdfLinkIndex);
//b3Printf("mb link index = %d\n",mbLinkIndex);
btTransform parentLocalInertialFrame;
parentLocalInertialFrame.setIdentity();
@@ -205,11 +168,11 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
if (urdfParentIndex==-2)
{
b3Printf("root link has no parent\n");
//b3Printf("root link has no parent\n");
} else
{
b3Printf("urdf parent index = %d\n",urdfParentIndex);
b3Printf("mb parent index = %d\n",mbParentIndex);
//b3Printf("urdf parent index = %d\n",urdfParentIndex);
//b3Printf("mb parent index = %d\n",mbParentIndex);
parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex);
u2b.getMassAndInertia(urdfParentIndex, parentMass,parentLocalInertiaDiagonal,parentLocalInertialFrame);
@@ -305,7 +268,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
{
//todo: adjust the center of mass transform and pivot axis properly
b3Printf("Fixed joint (btMultiBody)\n");
//b3Printf("Fixed joint (btMultiBody)\n");
btQuaternion rot = offsetInA.inverse().getRotation();//parent2joint.inverse().getRotation();
cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin(),disableParentCollision);
@@ -314,7 +277,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
} else
{
b3Printf("Fixed joint\n");
//b3Printf("Fixed joint\n");
btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
@@ -343,7 +306,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
if (enableConstraints)
world1->addConstraint(dof6,true);
b3Printf("Revolute/Continuous joint\n");
//b3Printf("Revolute/Continuous joint\n");
}
break;
}
@@ -371,13 +334,13 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
if (enableConstraints)
world1->addConstraint(dof6,true);
b3Printf("Prismatic\n");
//b3Printf("Prismatic\n");
}
break;
}
default:
{
b3Printf("Error: unsupported joint type in URDF (%d)\n", jointType);
//b3Printf("Error: unsupported joint type in URDF (%d)\n", jointType);
btAssert(0);
}
}

1
examples/Importers/ImportURDFDemo/URDF2Bullet.h
View File

@@ -13,7 +13,6 @@ class URDFImporterInterface;
class MultiBodyCreationInterface;
void printTree(const URDFImporterInterface& u2b, int linkIndex, int identationLevel=0);
void ConvertURDF2Bullet(const URDFImporterInterface& u2b,

4
examples/MultiBody/MultiDofDemo.cpp
View File

@@ -320,7 +320,7 @@ void MultiDofDemo::addColliders_testMultiDof(btMultiBody *pMultiBody, btMultiBod
{
// float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
if (1)
@@ -360,7 +360,7 @@ void MultiDofDemo::addColliders_testMultiDof(btMultiBody *pMultiBody, btMultiBod
btVector3 posr = local_origin[i+1];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
btScalar quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
btCollisionShape* box = new btBoxShape(linkHalfExtents);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);

9
examples/MultiBody/TestJointTorqueSetup.cpp
View File

@@ -96,7 +96,6 @@ void TestJointTorqueSetup::initPhysics()
m_guiHelper->createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity();
btVector3 groundOrigin(-0.4f, 3.f, 0.f);
btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
groundOrigin[upAxis] -=.5;
groundOrigin[2]-=0.6;
start.setOrigin(groundOrigin);
@@ -269,11 +268,11 @@ void TestJointTorqueSetup::initPhysics()
local_origin.resize(pMultiBody->getNumLinks() + 1);
world_to_local[0] = pMultiBody->getWorldToBaseRot();
local_origin[0] = pMultiBody->getBasePos();
double friction = 1;
// double friction = 1;
{
// float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
// btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
if (1)
@@ -326,7 +325,7 @@ void TestJointTorqueSetup::initPhysics()
btVector3 posr = local_origin[i+1];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
btScalar quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
btCollisionShape* shape =0;
if (i==0)
@@ -422,7 +421,7 @@ void TestJointTorqueSetup::stepSimulation(float deltaTime)
m_multiBody->getBaseOmega()[2]
);
*/
btScalar jointVel =m_multiBody->getJointVel(0);
// btScalar jointVel =m_multiBody->getJointVel(0);
// b3Printf("child angvel = %f",jointVel);

4
examples/OpenGLWindow/GLInstancingRenderer.cpp
View File

@@ -1170,7 +1170,7 @@ struct PointerCaster
#if 0
static void b3CreateFrustum(
float left,
float right,
@@ -1202,7 +1202,7 @@ static void b3CreateFrustum(
frustum[3*4+3] = float(0);
}
#endif
static void b3Matrix4x4Mul(GLfloat aIn[4][4], GLfloat bIn[4][4], GLfloat result[4][4])
{

13
examples/OpenGLWindow/SimpleCamera.cpp
View File

@@ -10,13 +10,13 @@ struct SimpleCameraInternalData
:m_cameraTargetPosition(b3MakeVector3(0,0,0)),
m_cameraDistance(20),
m_cameraUp(b3MakeVector3(0,1,0)),
m_cameraUpAxis(1),
m_cameraForward(b3MakeVector3(1,0,0)),
m_frustumZNear(0.01),
m_frustumZFar(1000),
m_cameraUpAxis(1),
m_yaw(20),
m_pitch(0),
m_aspect(1)
m_aspect(1),
m_frustumZNear(0.01),
m_frustumZFar(1000)
{
}
b3Vector3 m_cameraTargetPosition;
@@ -83,7 +83,7 @@ static void b3CreateFrustum(
#if 0
static void b3CreateDiagonalMatrix(float value, float result[4][4])
{
for (int i=0;i<4;i++)
@@ -100,7 +100,6 @@ static void b3CreateDiagonalMatrix(float value, float result[4][4])
}
}
}
static void b3CreateOrtho(float left, float right, float bottom, float top, float zNear, float zFar, float result[4][4])
{
b3CreateDiagonalMatrix(1.f,result);
@@ -112,7 +111,7 @@ static void b3CreateOrtho(float left, float right, float bottom, float top, floa
result[3][1] = - (top + bottom) / (top - bottom);
result[3][2] = - (zFar + zNear) / (zFar - zNear);
}
#endif
static void b3CreateLookAt(const b3Vector3& eye, const b3Vector3& center,const b3Vector3& up, float result[16])
{
b3Vector3 f = (center - eye).normalized();

9
examples/OpenGLWindow/SimpleOpenGL2App.cpp
View File

@@ -325,9 +325,8 @@ void SimpleOpenGL2App::drawText3D( const char* txt, float worldPosX, float world
float camPos[4];
cam->getCameraPosition(camPos);
b3Vector3 cp= b3MakeVector3(camPos[0],camPos[2],camPos[1]);
b3Vector3 p = b3MakeVector3(worldPosX,worldPosY,worldPosZ);
float dx=0;
//b3Vector3 cp= b3MakeVector3(camPos[0],camPos[2],camPos[1]);
// b3Vector3 p = b3MakeVector3(worldPosX,worldPosY,worldPosZ);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
@@ -399,11 +398,11 @@ void SimpleOpenGL2App::drawText3D( const char* txt, float worldPosX, float world
float z = 2.f*winz-1.f;//*(far
float identity[16]={1,0,0,0,
/*float identity[16]={1,0,0,0,
0,1,0,0,
0,0,1,0,
0,0,0,1};
*/
PrimVertex vertexData[4] = {
{ PrimVec4(-1.f+2.f*x0/float(screenWidth), 1.f-2.f*y0/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)},
{ PrimVec4(-1.f+2.f*x0/float(screenWidth), 1.f-2.f*y1/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)},

18
examples/OpenGLWindow/SimpleOpenGL3App.cpp
View File

@@ -224,8 +224,8 @@ void SimpleOpenGL3App::drawText3D( const char* txt, float worldPosX, float world
float camPos[4];
cam->getCameraPosition(camPos);
b3Vector3 cp= b3MakeVector3(camPos[0],camPos[2],camPos[1]);
b3Vector3 p = b3MakeVector3(worldPosX,worldPosY,worldPosZ);
//b3Vector3 cp= b3MakeVector3(camPos[0],camPos[2],camPos[1]);
//b3Vector3 p = b3MakeVector3(worldPosX,worldPosY,worldPosZ);
//float dist = (cp-p).length();
//float dv = 0;//dist/1000.f;
//
@@ -545,8 +545,8 @@ void SimpleOpenGL3App::drawGrid(DrawGridData data)
};
//b3Vector3 gridColor = b3MakeVector3(0.5,0.5,0.5);
b3AlignedObjectArray<unsigned int> indices;
b3AlignedObjectArray<b3Vector3> vertices;
b3AlignedObjectArray<unsigned int> indices;
b3AlignedObjectArray<b3Vector3> vertices;
int lineIndex=0;
for(int i=-gridSize;i<=gridSize;i++)
{
@@ -564,7 +564,7 @@ void SimpleOpenGL3App::drawGrid(DrawGridData data)
indices.push_back(lineIndex++);
vertices.push_back(to);
indices.push_back(lineIndex++);
m_instancingRenderer->drawLine(from,to,gridColor);
// m_instancingRenderer->drawLine(from,to,gridColor);
}
b3Assert(glGetError() ==GL_NO_ERROR);
@@ -583,16 +583,16 @@ void SimpleOpenGL3App::drawGrid(DrawGridData data)
indices.push_back(lineIndex++);
vertices.push_back(to);
indices.push_back(lineIndex++);
m_instancingRenderer->drawLine(from,to,gridColor);
// m_instancingRenderer->drawLine(from,to,gridColor);
}
}
/*m_instancingRenderer->drawLines(&vertices[0].x,
m_instancingRenderer->drawLines(&vertices[0].x,
gridColor,
vertices.size(),sizeof(b3Vector3),&indices[0],indices.size(),1);
*/
m_instancingRenderer->drawLine(b3MakeVector3(0,0,0),b3MakeVector3(1,0,0),b3MakeVector3(1,0,0),3);
m_instancingRenderer->drawLine(b3MakeVector3(0,0,0),b3MakeVector3(0,1,0),b3MakeVector3(0,1,0),3);
@@ -766,7 +766,7 @@ void SimpleOpenGL3App::dumpNextFrameToPng(const char* filename)
m_data->m_renderTexture->init(m_instancingRenderer->getScreenWidth(),this->m_instancingRenderer->getScreenHeight(),renderTextureId, RENDERTEXTURE_COLOR);
}
bool result = m_data->m_renderTexture->enable();
m_data->m_renderTexture->enable();
}

1545
examples/OpenGLWindow/stb_truetype.h
View File

File diff suppressed because it is too large Load Diff

9
examples/SharedMemory/PhysicsClient.cpp
View File

@@ -6,6 +6,7 @@
#include "../Utils/b3ResourcePath.h"
#include "../../Extras/Serialize/BulletFileLoader/btBulletFile.h"
#include "../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h"
#include "SharedMemoryBlock.h"
//copied from btMultiBodyLink.h
@@ -18,7 +19,7 @@ enum JointType
struct PhysicsClientSharedMemoryInternalData
{
SharedMemoryInterface* m_sharedMemory;
SharedMemoryExampleData* m_testBlock1;
SharedMemoryBlock* m_testBlock1;
btAlignedObjectArray<bParse::btBulletFile*> m_robotMultiBodyData;
btAlignedObjectArray<PoweredJointInfo> m_poweredJointInfo;
@@ -89,7 +90,7 @@ bool PhysicsClientSharedMemory::isConnected() const
bool PhysicsClientSharedMemory::connect(bool allowSharedMemoryInitialization)
{
bool allowCreation = true;
m_data->m_testBlock1 = (SharedMemoryExampleData*)m_data->m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE, allowCreation);
m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE, allowCreation);
if (m_data->m_testBlock1)
{
@@ -97,7 +98,7 @@ bool PhysicsClientSharedMemory::connect(bool allowSharedMemoryInitialization)
{
if (allowSharedMemoryInitialization)
{
InitSharedMemoryExampleData(m_data->m_testBlock1);
InitSharedMemoryBlock(m_data->m_testBlock1);
b3Printf("Created and initialized shared memory block");
m_data->m_isConnected = true;
} else
@@ -123,7 +124,7 @@ bool PhysicsClientSharedMemory::connect(bool allowSharedMemoryInitialization)
bool PhysicsClientSharedMemory::processServerStatus(ServerStatus& serverStatus)
bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverStatus)
{
btAssert(m_data->m_testBlock1);
bool hasStatus = false;

13
examples/SharedMemory/PhysicsClient.h
View File

@@ -3,17 +3,6 @@
#include "SharedMemoryCommands.h"
#include <string>
struct PoweredJointInfo
{
std::string m_linkName;
std::string m_jointName;
int m_jointType;
int m_qIndex;
int m_uIndex;
};
class PhysicsClientSharedMemory //: public CommonPhysicsClientInterface
{
@@ -31,7 +20,7 @@ public:
virtual bool isConnected() const;
// return true if there is a status, and fill in 'serverStatus'
virtual bool processServerStatus(ServerStatus& serverStatus);
virtual bool processServerStatus(SharedMemoryStatus& serverStatus);
virtual bool canSubmitCommand() const;

4
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -211,7 +211,7 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
if (m_physicsClient.isConnected())
{
ServerStatus status;
SharedMemoryStatus status;
bool hasStatus = m_physicsClient.processServerStatus(status);
if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
{
@@ -219,7 +219,7 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
{
PoweredJointInfo info;
m_physicsClient.getPoweredJointInfo(i,info);
b3Printf("1-DOF PoweredJoint %s at q-index %d and u-index %d\n",info.m_jointName.c_str(),info.m_qIndex,info.m_uIndex);
b3Printf("1-DOF PoweredJoint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
}
}

9
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -17,6 +17,7 @@
#include "LinearMath/btSerializer.h"
#include "Bullet3Common/b3Logging.h"
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "SharedMemoryBlock.h"
struct UrdfLinkNameMapUtil
{
@@ -31,7 +32,7 @@ struct UrdfLinkNameMapUtil
struct PhysicsServerInternalData
{
SharedMemoryInterface* m_sharedMemory;
SharedMemoryExampleData* m_testBlock1;
SharedMemoryBlock* m_testBlock1;
bool m_isConnected;
btScalar m_physicsDeltaTime;
//btAlignedObjectArray<btJointFeedback*> m_jointFeedbacks;
@@ -84,14 +85,14 @@ bool PhysicsServerSharedMemory::connectSharedMemory(bool allowSharedMemoryInitia
bool allowCreation = true;
m_data->m_testBlock1 = (SharedMemoryExampleData*)m_data->m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE,allowCreation);
m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE,allowCreation);
if (m_data->m_testBlock1)
{
if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
{
if (allowSharedMemoryInitialization)
{
InitSharedMemoryExampleData(m_data->m_testBlock1);
InitSharedMemoryBlock(m_data->m_testBlock1);
b3Printf("Created and initialized shared memory block");
m_data->m_isConnected = true;
} else
@@ -212,7 +213,7 @@ bool PhysicsServerSharedMemory::loadUrdf(const char* fileName, const btVector3&
tr.setIdentity();
tr.setOrigin(pos);
tr.setRotation(orn);
int rootLinkIndex = u2b.getRootLinkIndex();
//int rootLinkIndex = u2b.getRootLinkIndex();
// printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation(m_data->m_guiHelper);

1
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -57,7 +57,6 @@ PhysicsServerExample::PhysicsServerExample(GUIHelperInterface* helper)
m_wantsShutdown(false)
{
b3Printf("Started PhysicsServer\n");
}

26
examples/SharedMemory/RobotControlExample.cpp
View File

@@ -10,6 +10,7 @@
#include "PhysicsClient.h"
#include "SharedMemoryCommon.h"
#include "../Utils/b3Clock.h"
#include "PhysicsClientC_API.h"
//const char* blaatnaam = "basename";
@@ -100,11 +101,20 @@ void MyCallback2(int buttonId, bool buttonState, void* userPtr)
case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
{
command.m_type = CMD_SEND_PHYSICS_SIMULATION_PARAMETERS;
command.m_physSimParamArgs.m_gravityAcceleration[0] = 0;
command.m_physSimParamArgs.m_gravityAcceleration[1] = 0;
command.m_physSimParamArgs.m_gravityAcceleration[2] = -10;
command.m_updateFlags = SIM_PARAM_UPDATE_GRAVITY;
//#ifdef USE_C_API
b3InitPhysicsParamCommand(&command);
b3PhysicsParamSetGravity(&command, 0,0,-10);
// #else
//
// command.m_type = CMD_SEND_PHYSICS_SIMULATION_PARAMETERS;
// command.m_physSimParamArgs.m_gravityAcceleration[0] = 0;
// command.m_physSimParamArgs.m_gravityAcceleration[1] = 0;
// command.m_physSimParamArgs.m_gravityAcceleration[2] = -10;
// command.m_physSimParamArgs.m_updateFlags = SIM_PARAM_UPDATE_GRAVITY;
// #endif // USE_C_API
cl->enqueueCommand(command);
break;
@@ -286,7 +296,7 @@ void RobotControlExample::stepSimulation(float deltaTime)
if (m_physicsClient.isConnected())
{
ServerStatus status;
SharedMemoryStatus status;
bool hasStatus = m_physicsClient.processServerStatus(status);
if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
{
@@ -294,12 +304,12 @@ void RobotControlExample::stepSimulation(float deltaTime)
{
PoweredJointInfo info;
m_physicsClient.getPoweredJointInfo(i,info);
b3Printf("1-DOF PoweredJoint %s at q-index %d and u-index %d\n",info.m_jointName.c_str(),info.m_qIndex,info.m_uIndex);
b3Printf("1-DOF PoweredJoint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
if (m_numMotors<MAX_NUM_MOTORS)
{
char motorName[1024];
sprintf(motorName,"%s q'", info.m_jointName.c_str());
sprintf(motorName,"%s q'", info.m_jointName);
MyMotorInfo* motorInfo = &m_motorTargetVelocities[m_numMotors];
motorInfo->m_velTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex;

51
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -63,11 +63,11 @@ enum EnumSharedMemoryServerStatus
struct UrdfArgs
{
char m_urdfFileName[MAX_URDF_FILENAME_LENGTH];
double m_initialPosition[3];
double m_initialOrientation[4];
bool m_useMultiBody;
bool m_useFixedBase;
char m_urdfFileName[MAX_URDF_FILENAME_LENGTH];
double m_initialPosition[3];
double m_initialOrientation[4];
int m_useMultiBody;
int m_useFixedBase;
};
@@ -81,7 +81,7 @@ struct SetJointFeedbackArgs
{
int m_bodyUniqueId;
int m_linkId;
bool m_isEnabled;
int m_isEnabled;
};
//todo: discuss and decide about control mode and combinations
@@ -132,7 +132,6 @@ enum EnumUpdateFlags
///The control mode determines the state variables used for motor control.
struct SendPhysicsSimulationParameters
{
double m_deltaTime;
double m_gravityAcceleration[3];
int m_numSimulationSubSteps;
@@ -161,25 +160,26 @@ struct SendActualStateArgs
};
typedef struct SharedMemoryCommand SharedMemoryCommand_t;
struct SharedMemoryCommand
{
int m_type;
int m_type;
smUint64_t m_timeStamp;
int m_sequenceNumber;
smUint64_t m_timeStamp;
//a bit fields to tell which parameters need updating
//for example m_updateFlags = SIM_PARAM_UPDATE_DELTA_TIME | SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS;
smUint64_t m_updateFlags;
//a bit fields to tell which parameters need updating
//for example m_updateFlags = SIM_PARAM_UPDATE_DELTA_TIME | SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS;
int m_updateFlags;
union
{
UrdfArgs m_urdfArguments;
InitPoseArgs m_initPoseArgs;
SendPhysicsSimulationParameters m_physSimParamArgs;
BulletDataStreamArgs m_dataStreamArguments;
SendDesiredStateArgs m_sendDesiredStateCommandArgument;
RequestActualStateArgs m_requestActualStateInformationCommandArgument;
struct UrdfArgs m_urdfArguments;
struct InitPoseArgs m_initPoseArgs;
struct SendPhysicsSimulationParameters m_physSimParamArgs;
struct BulletDataStreamArgs m_dataStreamArguments;
struct SendDesiredStateArgs m_sendDesiredStateCommandArgument;
struct RequestActualStateArgs m_requestActualStateInformationCommandArgument;
};
};
@@ -193,11 +193,20 @@ struct SharedMemoryStatus
union
{
BulletDataStreamArgs m_dataStreamArguments;
SendActualStateArgs m_sendActualStateArgs;
struct BulletDataStreamArgs m_dataStreamArguments;
struct SendActualStateArgs m_sendActualStateArgs;
};
};
typedef SharedMemoryStatus ServerStatus;
struct PoweredJointInfo
{
char* m_linkName;
char* m_jointName;
int m_jointType;
int m_qIndex;
int m_uIndex;
};
#endif //SHARED_MEMORY_COMMANDS_H

44
examples/SharedMemory/SharedMemoryInterface.h
View File

@@ -1,49 +1,6 @@
#ifndef SHARED_MEMORY_INTERFACE_H
#define SHARED_MEMORY_INTERFACE_H
#define SHARED_MEMORY_KEY 12347
#define SHARED_MEMORY_MAGIC_NUMBER 64738
#define SHARED_MEMORY_MAX_COMMANDS 32
#define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (256*1024)
#include "SharedMemoryCommands.h"
struct SharedMemoryExampleData
{
int m_magicId;
SharedMemoryCommand m_clientCommands[SHARED_MEMORY_MAX_COMMANDS];
SharedMemoryStatus m_serverCommands[SHARED_MEMORY_MAX_COMMANDS];
int m_numClientCommands;
int m_numProcessedClientCommands;
int m_numServerCommands;
int m_numProcessedServerCommands;
//m_bulletStreamDataClientToServer is a way for the client to create collision shapes, rigid bodies and constraints
//the Bullet data structures are more general purpose than the capabilities of a URDF file.
char m_bulletStreamDataClientToServer[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
//m_bulletStreamDataServerToClient is used to send (debug) data from server to client, for
//example to provide all details of a multibody including joint/link names, after loading a URDF file.
char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
};
inline void InitSharedMemoryExampleData(SharedMemoryExampleData* sharedMemoryBlock)
{
sharedMemoryBlock->m_numClientCommands = 0;
sharedMemoryBlock->m_numServerCommands = 0;
sharedMemoryBlock->m_numProcessedClientCommands=0;
sharedMemoryBlock->m_numProcessedServerCommands=0;
sharedMemoryBlock->m_magicId = SHARED_MEMORY_MAGIC_NUMBER;
}
#define SHARED_MEMORY_SIZE sizeof(SharedMemoryExampleData)
class SharedMemoryInterface
{
public:
@@ -54,5 +11,6 @@ class SharedMemoryInterface
virtual void* allocateSharedMemory(int key, int size, bool allowCreation) =0;
virtual void releaseSharedMemory(int key, int size) =0;
};
#endif

1
examples/SharedMemory/main.cpp
View File

@@ -27,7 +27,6 @@ int main(int argc, char* argv[])
{
b3CommandLineArgs args(argc, argv);
struct PhysicsInterface* pint = 0;
DummyGUIHelper noGfx;

6
examples/SimpleOpenGL3/main.cpp
View File

@@ -14,9 +14,9 @@ int main(int argc, char* argv[])
SimpleOpenGL3App* app = new SimpleOpenGL3App("SimpleOpenGL3App",1024,768);
app->m_instancingRenderer->setCameraDistance(13);
app->m_instancingRenderer->setCameraPitch(0);
app->m_instancingRenderer->setCameraTargetPosition(b3MakeVector3(0,0,0));
app->m_instancingRenderer->getActiveCamera()->setCameraDistance(13);
app->m_instancingRenderer->getActiveCamera()->setCameraPitch(0);
app->m_instancingRenderer->getActiveCamera()->setCameraTargetPosition(0,0,0);
assert(glGetError()==GL_NO_ERROR);

1
examples/ThirdPartyLibs/Gwen/Gwen.h
View File

@@ -24,7 +24,6 @@
THE SOFTWARE.
*/
#pragma once
#ifndef GWEN_GWEN_H
#define GWEN_GWEN_H

2
examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
View File

@@ -171,7 +171,7 @@ updateVertex(
return it->second;
}
assert(in_positions.size() > (3*i.v_idx+2));
assert(static_cast<int>(in_positions.size()) > (3*i.v_idx+2));
positions.push_back(in_positions[3*i.v_idx+0]);
positions.push_back(in_positions[3*i.v_idx+1]);

346
src/Bullet3Common/b3Vector3.h
View File

@@ -3,8 +3,8 @@ Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans http://bulletphysic
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -22,8 +22,6 @@ subject to the following restrictions:
#include "b3MinMax.h"
#include "b3AlignedAllocator.h"
#ifdef B3_USE_DOUBLE_PRECISION
#define b3Vector3Data b3Vector3DoubleData
#define b3Vector3DataName "b3Vector3DoubleData"
@@ -99,7 +97,7 @@ public:
b3SimdFloat4 mVec128;
float m_floats[4];
struct {float x,y,z,w;};
};
#else
union
@@ -133,9 +131,9 @@ public:
public:
/**@brief Add a vector to this one
/**@brief Add a vector to this one
* @param The vector to add to this one */
B3_FORCE_INLINE b3Vector3& operator+=(const b3Vector3& v)
{
@@ -144,7 +142,7 @@ public:
#elif defined(B3_USE_NEON)
mVec128 = vaddq_f32(mVec128, v.mVec128);
#else
m_floats[0] += v.m_floats[0];
m_floats[0] += v.m_floats[0];
m_floats[1] += v.m_floats[1];
m_floats[2] += v.m_floats[2];
#endif
@@ -154,20 +152,20 @@ public:
/**@brief Subtract a vector from this one
* @param The vector to subtract */
B3_FORCE_INLINE b3Vector3& operator-=(const b3Vector3& v)
B3_FORCE_INLINE b3Vector3& operator-=(const b3Vector3& v)
{
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
mVec128 = _mm_sub_ps(mVec128, v.mVec128);
#elif defined(B3_USE_NEON)
mVec128 = vsubq_f32(mVec128, v.mVec128);
#else
m_floats[0] -= v.m_floats[0];
m_floats[0] -= v.m_floats[0];
m_floats[1] -= v.m_floats[1];
m_floats[2] -= v.m_floats[2];
#endif
return *this;
}
/**@brief Scale the vector
* @param s Scale factor */
B3_FORCE_INLINE b3Vector3& operator*=(const b3Scalar& s)
@@ -179,16 +177,16 @@ public:
#elif defined(B3_USE_NEON)
mVec128 = vmulq_n_f32(mVec128, s);
#else
m_floats[0] *= s;
m_floats[0] *= s;
m_floats[1] *= s;
m_floats[2] *= s;
#endif
return *this;
}
/**@brief Inversely scale the vector
/**@brief Inversely scale the vector
* @param s Scale factor to divide by */
B3_FORCE_INLINE b3Vector3& operator/=(const b3Scalar& s)
B3_FORCE_INLINE b3Vector3& operator/=(const b3Scalar& s)
{
b3FullAssert(s != b3Scalar(0.0));
@@ -199,7 +197,7 @@ public:
vs = b3_pshufd_ps(vs, 0x00); // (S S S S)
mVec128 = _mm_mul_ps(mVec128, vs);
return *this;
#else
return *this *= b3Scalar(1.0) / s;
@@ -210,7 +208,7 @@ public:
* @param v The other vector in the dot product */
B3_FORCE_INLINE b3Scalar dot(const b3Vector3& v) const
{
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 vd = _mm_mul_ps(mVec128, v.mVec128);
__m128 z = _mm_movehl_ps(vd, vd);
__m128 y = _mm_shuffle_ps(vd, vd, 0x55);
@@ -219,12 +217,12 @@ public:
return _mm_cvtss_f32(vd);
#elif defined(B3_USE_NEON)
float32x4_t vd = vmulq_f32(mVec128, v.mVec128);
float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_low_f32(vd));
float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_low_f32(vd));
x = vadd_f32(x, vget_high_f32(vd));
return vget_lane_f32(x, 0);
#else
return m_floats[0] * v.m_floats[0] +
m_floats[1] * v.m_floats[1] +
#else
return m_floats[0] * v.m_floats[0] +
m_floats[1] * v.m_floats[1] +
m_floats[2] * v.m_floats[2];
#endif
}
@@ -249,7 +247,7 @@ public:
* This is symantically treating the vector like a point */
B3_FORCE_INLINE b3Scalar distance(const b3Vector3& v) const;
B3_FORCE_INLINE b3Vector3& safeNormalize()
B3_FORCE_INLINE b3Vector3& safeNormalize()
{
b3Vector3 absVec = this->absolute();
int maxIndex = absVec.maxAxis();
@@ -262,35 +260,35 @@ public:
return *this;
}
/**@brief Normalize this vector
/**@brief Normalize this vector
* x^2 + y^2 + z^2 = 1 */
B3_FORCE_INLINE b3Vector3& normalize()
B3_FORCE_INLINE b3Vector3& normalize()
{
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
// dot product first
__m128 vd = _mm_mul_ps(mVec128, mVec128);
__m128 z = _mm_movehl_ps(vd, vd);
__m128 y = _mm_shuffle_ps(vd, vd, 0x55);
vd = _mm_add_ss(vd, y);
vd = _mm_add_ss(vd, z);
#if 0
vd = _mm_sqrt_ss(vd);
vd = _mm_div_ss(b3v1110, vd);
vd = b3_splat_ps(vd, 0x80);
mVec128 = _mm_mul_ps(mVec128, vd);
#else
// NR step 1/sqrt(x) - vd is x, y is output
y = _mm_rsqrt_ss(vd); // estimate
// one step NR
// NR step 1/sqrt(x) - vd is x, y is output
y = _mm_rsqrt_ss(vd); // estimate
// one step NR
z = b3v1_5;
vd = _mm_mul_ss(vd, b3vHalf); // vd * 0.5
vd = _mm_mul_ss(vd, b3vHalf); // vd * 0.5
//x2 = vd;
vd = _mm_mul_ss(vd, y); // vd * 0.5 * y0
vd = _mm_mul_ss(vd, y); // vd * 0.5 * y0 * y0
z = _mm_sub_ss(z, vd); // 1.5 - vd * 0.5 * y0 * y0
z = _mm_sub_ss(z, vd); // 1.5 - vd * 0.5 * y0 * y0
y = _mm_mul_ss(y, z); // y0 * (1.5 - vd * 0.5 * y0 * y0)
@@ -299,9 +297,9 @@ public:
#endif
return *this;
#else
#else
return *this /= length();
#endif
}
@@ -310,48 +308,48 @@ public:
B3_FORCE_INLINE b3Vector3 normalized() const;
/**@brief Return a rotated version of this vector
* @param wAxis The axis to rotate about
* @param wAxis The axis to rotate about
* @param angle The angle to rotate by */
B3_FORCE_INLINE b3Vector3 rotate( const b3Vector3& wAxis, const b3Scalar angle ) const;
/**@brief Return the angle between this and another vector
* @param v The other vector */
B3_FORCE_INLINE b3Scalar angle(const b3Vector3& v) const
B3_FORCE_INLINE b3Scalar angle(const b3Vector3& v) const
{
b3Scalar s = b3Sqrt(length2() * v.length2());
b3FullAssert(s != b3Scalar(0.0));
return b3Acos(dot(v) / s);
}
/**@brief Return a vector will the absolute values of each element */
B3_FORCE_INLINE b3Vector3 absolute() const
B3_FORCE_INLINE b3Vector3 absolute() const
{
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
return b3MakeVector3(_mm_and_ps(mVec128, b3v3AbsfMask));
#elif defined(B3_USE_NEON)
return b3Vector3(vabsq_f32(mVec128));
#else
#else
return b3MakeVector3(
b3Fabs(m_floats[0]),
b3Fabs(m_floats[1]),
b3Fabs(m_floats[0]),
b3Fabs(m_floats[1]),
b3Fabs(m_floats[2]));
#endif
}
/**@brief Return the cross product between this and another vector
/**@brief Return the cross product between this and another vector
* @param v The other vector */
B3_FORCE_INLINE b3Vector3 cross(const b3Vector3& v) const
{
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 T, V;
T = b3_pshufd_ps(mVec128, B3_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0)
V = b3_pshufd_ps(v.mVec128, B3_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0)
V = _mm_mul_ps(V, mVec128);
T = _mm_mul_ps(T, v.mVec128);
V = _mm_sub_ps(V, T);
V = b3_pshufd_ps(V, B3_SHUFFLE(1, 2, 0, 3));
return b3MakeVector3(V);
#elif defined(B3_USE_NEON)
@@ -361,7 +359,7 @@ public:
float32x2_t Vlow = vget_low_f32(v.mVec128);
T = vcombine_f32(vext_f32(Tlow, vget_high_f32(mVec128), 1), Tlow);
V = vcombine_f32(vext_f32(Vlow, vget_high_f32(v.mVec128), 1), Vlow);
V = vmulq_f32(V, mVec128);
T = vmulq_f32(T, v.mVec128);
V = vsubq_f32(V, T);
@@ -369,7 +367,7 @@ public:
// form (Y, Z, X, _);
V = vcombine_f32(vext_f32(Vlow, vget_high_f32(V), 1), Vlow);
V = (float32x4_t)vandq_s32((int32x4_t)V, b3vFFF0Mask);
return b3Vector3(V);
#else
return b3MakeVector3(
@@ -385,14 +383,14 @@ public:
// cross:
__m128 T = _mm_shuffle_ps(v1.mVec128, v1.mVec128, B3_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0)
__m128 V = _mm_shuffle_ps(v2.mVec128, v2.mVec128, B3_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0)
V = _mm_mul_ps(V, v1.mVec128);
T = _mm_mul_ps(T, v2.mVec128);
V = _mm_sub_ps(V, T);
V = _mm_shuffle_ps(V, V, B3_SHUFFLE(1, 2, 0, 3));
// dot:
// dot:
V = _mm_mul_ps(V, mVec128);
__m128 z = _mm_movehl_ps(V, V);
__m128 y = _mm_shuffle_ps(V, V, 0x55);
@@ -408,7 +406,7 @@ public:
float32x2_t Vlow = vget_low_f32(v2.mVec128);
T = vcombine_f32(vext_f32(Tlow, vget_high_f32(v1.mVec128), 1), Tlow);
V = vcombine_f32(vext_f32(Vlow, vget_high_f32(v2.mVec128), 1), Vlow);
V = vmulq_f32(V, v1.mVec128);
T = vmulq_f32(T, v2.mVec128);
V = vsubq_f32(V, T);
@@ -416,29 +414,29 @@ public:
// form (Y, Z, X, _);
V = vcombine_f32(vext_f32(Vlow, vget_high_f32(V), 1), Vlow);
// dot:
// dot:
V = vmulq_f32(mVec128, V);
float32x2_t x = vpadd_f32(vget_low_f32(V), vget_low_f32(V));
float32x2_t x = vpadd_f32(vget_low_f32(V), vget_low_f32(V));
x = vadd_f32(x, vget_high_f32(V));
return vget_lane_f32(x, 0);
#else
return
m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) +
m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) +
return
m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) +
m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) +
m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]);
#endif
}
/**@brief Return the axis with the smallest value
/**@brief Return the axis with the smallest value
* Note return values are 0,1,2 for x, y, or z */
B3_FORCE_INLINE int minAxis() const
{
return m_floats[0] < m_floats[1] ? (m_floats[0] <m_floats[2] ? 0 : 2) : (m_floats[1] <m_floats[2] ? 1 : 2);
}
/**@brief Return the axis with the largest value
/**@brief Return the axis with the largest value
* Note return values are 0,1,2 for x, y, or z */
B3_FORCE_INLINE int maxAxis() const
B3_FORCE_INLINE int maxAxis() const
{
return m_floats[0] < m_floats[1] ? (m_floats[1] <m_floats[2] ? 2 : 1) : (m_floats[0] <m_floats[2] ? 2 : 0);
}
@@ -448,12 +446,12 @@ public:
return absolute().minAxis();
}
B3_FORCE_INLINE int closestAxis() const
B3_FORCE_INLINE int closestAxis() const
{
return absolute().maxAxis();
}
B3_FORCE_INLINE void setInterpolate3(const b3Vector3& v0, const b3Vector3& v1, b3Scalar rt)
{
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
@@ -480,10 +478,10 @@ public:
#endif
}
/**@brief Return the linear interpolation between this and another vector
* @param v The other vector
/**@brief Return the linear interpolation between this and another vector
* @param v The other vector
* @param t The ration of this to v (t = 0 => return this, t=1 => return other) */
B3_FORCE_INLINE b3Vector3 lerp(const b3Vector3& v, const b3Scalar& t) const
B3_FORCE_INLINE b3Vector3 lerp(const b3Vector3& v, const b3Scalar& t) const
{
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 vt = _mm_load_ss(&t); // (t 0 0 0)
@@ -491,23 +489,23 @@ public:
__m128 vl = _mm_sub_ps(v.mVec128, mVec128);
vl = _mm_mul_ps(vl, vt);
vl = _mm_add_ps(vl, mVec128);
return b3MakeVector3(vl);
#elif defined(B3_USE_NEON)
float32x4_t vl = vsubq_f32(v.mVec128, mVec128);
vl = vmulq_n_f32(vl, t);
vl = vaddq_f32(vl, mVec128);
return b3Vector3(vl);
#else
return
#else
return
b3MakeVector3( m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
m_floats[2] + (v.m_floats[2] - m_floats[2]) * t);
#endif
}
/**@brief Elementwise multiply this vector by the other
/**@brief Elementwise multiply this vector by the other
* @param v The other vector */
B3_FORCE_INLINE b3Vector3& operator*=(const b3Vector3& v)
{
@@ -515,8 +513,8 @@ public:
mVec128 = _mm_mul_ps(mVec128, v.mVec128);
#elif defined(B3_USE_NEON)
mVec128 = vmulq_f32(mVec128, v.mVec128);
#else
m_floats[0] *= v.m_floats[0];
#else
m_floats[0] *= v.m_floats[0];
m_floats[1] *= v.m_floats[1];
m_floats[2] *= v.m_floats[2];
#endif
@@ -541,7 +539,7 @@ public:
/**@brief Set the w value */
B3_FORCE_INLINE void setW(b3Scalar _w) { m_floats[3] = _w;};
//B3_FORCE_INLINE b3Scalar& operator[](int i) { return (&m_floats[0])[i]; }
//B3_FORCE_INLINE b3Scalar& operator[](int i) { return (&m_floats[0])[i]; }
//B3_FORCE_INLINE const b3Scalar& operator[](int i) const { return (&m_floats[0])[i]; }
///operator b3Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
B3_FORCE_INLINE operator b3Scalar *() { return &m_floats[0]; }
@@ -551,10 +549,10 @@ public:
{
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
#else
return ((m_floats[3]==other.m_floats[3]) &&
(m_floats[2]==other.m_floats[2]) &&
(m_floats[1]==other.m_floats[1]) &&
#else
return ((m_floats[3]==other.m_floats[3]) &&
(m_floats[2]==other.m_floats[2]) &&
(m_floats[1]==other.m_floats[1]) &&
(m_floats[0]==other.m_floats[0]));
#endif
}
@@ -565,7 +563,7 @@ public:
}
/**@brief Set each element to the max of the current values and the values of another b3Vector3
* @param other The other b3Vector3 to compare with
* @param other The other b3Vector3 to compare with
*/
B3_FORCE_INLINE void setMax(const b3Vector3& other)
{
@@ -582,7 +580,7 @@ public:
}
/**@brief Set each element to the min of the current values and the values of another b3Vector3
* @param other The other b3Vector3 to compare with
* @param other The other b3Vector3 to compare with
*/
B3_FORCE_INLINE void setMin(const b3Vector3& other)
{
@@ -609,16 +607,16 @@ public:
void getSkewSymmetricMatrix(b3Vector3* v0,b3Vector3* v1,b3Vector3* v2) const
{
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 V = _mm_and_ps(mVec128, b3vFFF0fMask);
__m128 V0 = _mm_xor_ps(b3vMzeroMask, V);
__m128 V2 = _mm_movelh_ps(V0, V);
__m128 V1 = _mm_shuffle_ps(V, V0, 0xCE);
V0 = _mm_shuffle_ps(V0, V, 0xDB);
V2 = _mm_shuffle_ps(V2, V, 0xF9);
v0->mVec128 = V0;
v1->mVec128 = V1;
v2->mVec128 = V2;
@@ -634,19 +632,19 @@ public:
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
mVec128 = (__m128)_mm_xor_ps(mVec128, mVec128);
#elif defined(B3_USE_NEON)
int32x4_t vi = vdupq_n_s32(0);
int32x4_t vi = vdupq_n_s32(0);
mVec128 = vreinterpretq_f32_s32(vi);
#else
#else
setValue(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
#endif
}
B3_FORCE_INLINE bool isZero() const
B3_FORCE_INLINE bool isZero() const
{
return m_floats[0] == b3Scalar(0) && m_floats[1] == b3Scalar(0) && m_floats[2] == b3Scalar(0);
}
B3_FORCE_INLINE bool fuzzyZero() const
B3_FORCE_INLINE bool fuzzyZero() const
{
return length2() < B3_EPSILON;
}
@@ -662,18 +660,18 @@ public:
B3_FORCE_INLINE void serializeDouble(struct b3Vector3DoubleData& dataOut) const;
B3_FORCE_INLINE void deSerializeDouble(const struct b3Vector3DoubleData& dataIn);
/**@brief returns index of maximum dot product between this and vectors in array[]
* @param array The other vectors
* @param array_count The number of other vectors
* @param array The other vectors
* @param array_count The number of other vectors
* @param dotOut The maximum dot product */
B3_FORCE_INLINE long maxDot( const b3Vector3 *array, long array_count, b3Scalar &dotOut ) const;
B3_FORCE_INLINE long maxDot( const b3Vector3 *array, long array_count, b3Scalar &dotOut ) const;
/**@brief returns index of minimum dot product between this and vectors in array[]
* @param array The other vectors
* @param array_count The number of other vectors
* @param dotOut The minimum dot product */
B3_FORCE_INLINE long minDot( const b3Vector3 *array, long array_count, b3Scalar &dotOut ) const;
* @param array The other vectors
* @param array_count The number of other vectors
* @param dotOut The minimum dot product */
B3_FORCE_INLINE long minDot( const b3Vector3 *array, long array_count, b3Scalar &dotOut ) const;
/* create a vector as b3Vector3( this->dot( b3Vector3 v0 ), this->dot( b3Vector3 v1), this->dot( b3Vector3 v2 )) */
B3_FORCE_INLINE b3Vector3 dot3( const b3Vector3 &v0, const b3Vector3 &v1, const b3Vector3 &v2 ) const
@@ -691,7 +689,7 @@ public:
a2 = _mm_and_ps( a2, b3vxyzMaskf);
r = _mm_add_ps( r, b3CastdTo128f (_mm_move_sd( b3CastfTo128d(a2), b3CastfTo128d(b1) )));
return b3MakeVector3(r);
#elif defined(B3_USE_NEON)
static const uint32x4_t xyzMask = (const uint32x4_t){ -1, -1, -1, 0 };
float32x4_t a0 = vmulq_f32( v0.mVec128, this->mVec128);
@@ -702,15 +700,15 @@ public:
float32x2_t b0 = vadd_f32( vpadd_f32( vget_low_f32(a0), vget_low_f32(a1)), zLo.val[0] );
float32x2_t b1 = vpadd_f32( vpadd_f32( vget_low_f32(a2), vget_high_f32(a2)), vdup_n_f32(0.0f));
return b3Vector3( vcombine_f32(b0, b1) );
#else
#else
return b3MakeVector3( dot(v0), dot(v1), dot(v2));
#endif
}
};
/**@brief Return the sum of two vectors (Point symantics)*/
B3_FORCE_INLINE b3Vector3
operator+(const b3Vector3& v1, const b3Vector3& v2)
B3_FORCE_INLINE b3Vector3
operator+(const b3Vector3& v1, const b3Vector3& v2)
{
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
return b3MakeVector3(_mm_add_ps(v1.mVec128, v2.mVec128));
@@ -718,15 +716,15 @@ operator+(const b3Vector3& v1, const b3Vector3& v2)
return b3MakeVector3(vaddq_f32(v1.mVec128, v2.mVec128));
#else
return b3MakeVector3(
v1.m_floats[0] + v2.m_floats[0],
v1.m_floats[1] + v2.m_floats[1],
v1.m_floats[0] + v2.m_floats[0],
v1.m_floats[1] + v2.m_floats[1],
v1.m_floats[2] + v2.m_floats[2]);
#endif
}
/**@brief Return the elementwise product of two vectors */
B3_FORCE_INLINE b3Vector3
operator*(const b3Vector3& v1, const b3Vector3& v2)
B3_FORCE_INLINE b3Vector3
operator*(const b3Vector3& v1, const b3Vector3& v2)
{
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
return b3MakeVector3(_mm_mul_ps(v1.mVec128, v2.mVec128));
@@ -734,14 +732,14 @@ operator*(const b3Vector3& v1, const b3Vector3& v2)
return b3MakeVector3(vmulq_f32(v1.mVec128, v2.mVec128));
#else
return b3MakeVector3(
v1.m_floats[0] * v2.m_floats[0],
v1.m_floats[1] * v2.m_floats[1],
v1.m_floats[0] * v2.m_floats[0],
v1.m_floats[1] * v2.m_floats[1],
v1.m_floats[2] * v2.m_floats[2]);
#endif
}
/**@brief Return the difference between two vectors */
B3_FORCE_INLINE b3Vector3
B3_FORCE_INLINE b3Vector3
operator-(const b3Vector3& v1, const b3Vector3& v2)
{
#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
@@ -754,28 +752,28 @@ operator-(const b3Vector3& v1, const b3Vector3& v2)
return b3MakeVector3((float32x4_t)vandq_s32((int32x4_t)r, b3vFFF0Mask));
#else
return b3MakeVector3(
v1.m_floats[0] - v2.m_floats[0],
v1.m_floats[1] - v2.m_floats[1],
v1.m_floats[0] - v2.m_floats[0],
v1.m_floats[1] - v2.m_floats[1],
v1.m_floats[2] - v2.m_floats[2]);
#endif
}
/**@brief Return the negative of the vector */
B3_FORCE_INLINE b3Vector3
B3_FORCE_INLINE b3Vector3
operator-(const b3Vector3& v)
{
#if (defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE))
__m128 r = _mm_xor_ps(v.mVec128, b3vMzeroMask);
return b3MakeVector3(_mm_and_ps(r, b3vFFF0fMask));
return b3MakeVector3(_mm_and_ps(r, b3vFFF0fMask));
#elif defined(B3_USE_NEON)
return b3MakeVector3((b3SimdFloat4)veorq_s32((int32x4_t)v.mVec128, (int32x4_t)b3vMzeroMask));
#else
#else
return b3MakeVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
#endif
}
/**@brief Return the vector scaled by s */
B3_FORCE_INLINE b3Vector3
B3_FORCE_INLINE b3Vector3
operator*(const b3Vector3& v, const b3Scalar& s)
{
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
@@ -791,10 +789,10 @@ operator*(const b3Vector3& v, const b3Scalar& s)
}
/**@brief Return the vector scaled by s */
B3_FORCE_INLINE b3Vector3
B3_FORCE_INLINE b3Vector3
operator*(const b3Scalar& s, const b3Vector3& v)
{
return v * s;
{
return v * s;
}
/**@brief Return the vector inversely scaled by s */
@@ -821,13 +819,13 @@ operator/(const b3Vector3& v1, const b3Vector3& v2)
#if (defined(B3_USE_SSE_IN_API)&& defined (B3_USE_SSE))
__m128 vec = _mm_div_ps(v1.mVec128, v2.mVec128);
vec = _mm_and_ps(vec, b3vFFF0fMask);
return b3MakeVector3(vec);
return b3MakeVector3(vec);
#elif defined(B3_USE_NEON)
float32x4_t x, y, v, m;
x = v1.mVec128;
y = v2.mVec128;
v = vrecpeq_f32(y); // v ~ 1/y
m = vrecpsq_f32(y, v); // m = (2-v*y)
v = vmulq_f32(v, m); // vv = v*m ~~ 1/y
@@ -838,47 +836,47 @@ operator/(const b3Vector3& v1, const b3Vector3& v2)
return b3Vector3(v);
#else
return b3MakeVector3(
v1.m_floats[0] / v2.m_floats[0],
v1.m_floats[0] / v2.m_floats[0],
v1.m_floats[1] / v2.m_floats[1],
v1.m_floats[2] / v2.m_floats[2]);
#endif
}
/**@brief Return the dot product between two vectors */
B3_FORCE_INLINE b3Scalar
b3Dot(const b3Vector3& v1, const b3Vector3& v2)
{
return v1.dot(v2);
B3_FORCE_INLINE b3Scalar
b3Dot(const b3Vector3& v1, const b3Vector3& v2)
{
return v1.dot(v2);
}
/**@brief Return the distance squared between two vectors */
B3_FORCE_INLINE b3Scalar
b3Distance2(const b3Vector3& v1, const b3Vector3& v2)
{
return v1.distance2(v2);
b3Distance2(const b3Vector3& v1, const b3Vector3& v2)
{
return v1.distance2(v2);
}
/**@brief Return the distance between two vectors */
B3_FORCE_INLINE b3Scalar
b3Distance(const b3Vector3& v1, const b3Vector3& v2)
{
return v1.distance(v2);
b3Distance(const b3Vector3& v1, const b3Vector3& v2)
{
return v1.distance(v2);
}
/**@brief Return the angle between two vectors */
B3_FORCE_INLINE b3Scalar
b3Angle(const b3Vector3& v1, const b3Vector3& v2)
{
return v1.angle(v2);
b3Angle(const b3Vector3& v1, const b3Vector3& v2)
{
return v1.angle(v2);
}
/**@brief Return the cross product of two vectors */
B3_FORCE_INLINE b3Vector3
b3Cross(const b3Vector3& v1, const b3Vector3& v2)
{
return v1.cross(v2);
B3_FORCE_INLINE b3Vector3
b3Cross(const b3Vector3& v1, const b3Vector3& v2)
{
return v1.cross(v2);
}
B3_FORCE_INLINE b3Scalar
@@ -888,10 +886,10 @@ b3Triple(const b3Vector3& v1, const b3Vector3& v2, const b3Vector3& v3)
}
/**@brief Return the linear interpolation between two vectors
* @param v1 One vector
* @param v2 The other vector
* @param v1 One vector
* @param v2 The other vector
* @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */
B3_FORCE_INLINE b3Vector3
B3_FORCE_INLINE b3Vector3
b3Lerp(const b3Vector3& v1, const b3Vector3& v2, const b3Scalar& t)
{
return v1.lerp(v2, t);
@@ -918,7 +916,7 @@ B3_FORCE_INLINE b3Vector3 b3Vector3::normalized() const
#else
return *this / length();
#endif
}
}
B3_FORCE_INLINE b3Vector3 b3Vector3::rotate( const b3Vector3& wAxis, const b3Scalar _angle ) const
{
@@ -931,25 +929,25 @@ B3_FORCE_INLINE b3Vector3 b3Vector3::rotate( const b3Vector3& wAxis, const b3Sca
__m128 C = wAxis.cross( b3MakeVector3(mVec128) ).mVec128;
O = _mm_and_ps(O, b3vFFF0fMask);
b3Scalar scos = b3Cos( _angle );
__m128 vsin = _mm_load_ss(&ssin); // (S 0 0 0)
__m128 vcos = _mm_load_ss(&scos); // (S 0 0 0)
__m128 Y = b3_pshufd_ps(O, 0xC9); // (Y Z X 0)
__m128 Z = b3_pshufd_ps(O, 0xD2); // (Z X Y 0)
O = _mm_add_ps(O, Y);
vsin = b3_pshufd_ps(vsin, 0x80); // (S S S 0)
O = _mm_add_ps(O, Z);
vcos = b3_pshufd_ps(vcos, 0x80); // (S S S 0)
vsin = vsin * C;
O = O * wAxis.mVec128;
__m128 X = mVec128 - O;
vsin = vsin * C;
O = O * wAxis.mVec128;
__m128 X = mVec128 - O;
O = O + vsin;
vcos = vcos * X;
O = O + vcos;
O = O + vcos;
return b3MakeVector3(O);
#else
b3Vector3 o = wAxis * wAxis.dot( *this );
@@ -974,7 +972,7 @@ B3_FORCE_INLINE long b3Vector3::maxDot( const b3Vector3 *array, long array_
#endif
if( array_count < scalar_cutoff )
#else
#endif//B3_USE_SSE || B3_USE_NEON
{
b3Scalar maxDot = -B3_INFINITY;
@@ -983,7 +981,7 @@ B3_FORCE_INLINE long b3Vector3::maxDot( const b3Vector3 *array, long array_
for( i = 0; i < array_count; i++ )
{
b3Scalar dot = array[i].dot(*this);
if( dot > maxDot )
{
maxDot = dot;
@@ -1016,27 +1014,27 @@ B3_FORCE_INLINE long b3Vector3::minDot( const b3Vector3 *array, long array_
#else
#error unhandled arch!
#endif
if( array_count < scalar_cutoff )
#endif//B3_USE_SSE || B3_USE_NEON
{
b3Scalar minDot = B3_INFINITY;
int i = 0;
int ptIndex = -1;
for( i = 0; i < array_count; i++ )
{
b3Scalar dot = array[i].dot(*this);
if( dot < minDot )
{
minDot = dot;
ptIndex = i;
}
}
dotOut = minDot;
return ptIndex;
}
#if defined (B3_USE_SSE) || defined (B3_USE_NEON)
@@ -1049,21 +1047,21 @@ class b3Vector4 : public b3Vector3
{
public:
B3_FORCE_INLINE b3Vector4 absolute4() const
B3_FORCE_INLINE b3Vector4 absolute4() const
{
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
return b3MakeVector4(_mm_and_ps(mVec128, b3vAbsfMask));
#elif defined(B3_USE_NEON)
return b3Vector4(vabsq_f32(mVec128));
#else
#else
return b3MakeVector4(
b3Fabs(m_floats[0]),
b3Fabs(m_floats[1]),
b3Fabs(m_floats[0]),
b3Fabs(m_floats[1]),
b3Fabs(m_floats[2]),
b3Fabs(m_floats[3]));
#endif
@@ -1126,34 +1124,34 @@ public:
minIndex = 3;
minVal = m_floats[3];
}
return minIndex;
}
B3_FORCE_INLINE int closestAxis4() const
B3_FORCE_INLINE int closestAxis4() const
{
return absolute4().maxAxis4();
}
/**@brief Set x,y,z and zero w
/**@brief Set x,y,z and zero w
* @param x Value of x
* @param y Value of y
* @param z Value of z
*/
/* void getValue(b3Scalar *m) const
/* void getValue(b3Scalar *m) const
{
m[0] = m_floats[0];
m[1] = m_floats[1];
m[2] =m_floats[2];
}
*/
/**@brief Set the values
/**@brief Set the values
* @param x Value of x
* @param y Value of y
* @param z Value of z
@@ -1299,7 +1297,7 @@ B3_FORCE_INLINE void b3Vector3::deSerialize(const struct b3Vector3Data& dataIn)
}
inline b3Vector3 b3MakeVector3(b3Scalar x,b3Scalar y,b3Scalar z)
{
@@ -1309,7 +1307,7 @@ inline b3Vector3 b3MakeVector3(b3Scalar x,b3Scalar y,b3Scalar z)
}
inline b3Vector3 b3MakeVector3(b3Scalar x,b3Scalar y,b3Scalar z, b3Scalar w)
{
{
b3Vector3 tmp;
tmp.setValue(x,y,z);
tmp.w = w;
@@ -1323,7 +1321,7 @@ inline b3Vector4 b3MakeVector4(b3Scalar x,b3Scalar y,b3Scalar z,b3Scalar w)
return tmp;
}
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
inline b3Vector3 b3MakeVector3( b3SimdFloat4 v)
{

16
src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
View File

@@ -59,15 +59,13 @@ PHY_ScalarType hdt, bool flipQuadEdges
)
{
// validation
btAssert(heightStickWidth > 1 && "bad width");
btAssert(heightStickLength > 1 && "bad length");
btAssert(heightfieldData && "null heightfield data");
btAssert(heightStickWidth > 1);// && "bad width");
btAssert(heightStickLength > 1);// && "bad length");
btAssert(heightfieldData);// && "null heightfield data");
// btAssert(heightScale) -- do we care? Trust caller here
btAssert(minHeight <= maxHeight && "bad min/max height");
btAssert(upAxis >= 0 && upAxis < 3 &&
"bad upAxis--should be in range [0,2]");
btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT &&
"Bad height data type enum");
btAssert(minHeight <= maxHeight);// && "bad min/max height");
btAssert(upAxis >= 0 && upAxis < 3);// && "bad upAxis--should be in range [0,2]");
btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT);// && "Bad height data type enum");
// initialize member variables
m_shapeType = TERRAIN_SHAPE_PROXYTYPE;
@@ -110,7 +108,7 @@ PHY_ScalarType hdt, bool flipQuadEdges
default:
{
//need to get valid m_upAxis
btAssert(0 && "Bad m_upAxis");
btAssert(0);// && "Bad m_upAxis");
}
}

2
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
View File

@@ -19,7 +19,7 @@ subject to the following restrictions:
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
#include "btMultiBodySolverConstraint.h"
//#define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
#define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
class btMultiBody;