bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

copy of simmechanics_to_urdf

This commit is contained in:
Wim Meeussen 2011-02-28 15:15:31 -08:00
parent a3c35dc515 2d35a40ed0
commit 5a922ec667
166 changed files with 60874 additions and 2300 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

@ -1,6 +1,8 @@
cmake_minimum_required(VERSION 2.4.6) cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
set(ROSPACK_MAKEDIST true)
# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of # Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of
# directories (or patterns, but directories should suffice) that should # directories (or patterns, but directories should suffice) that should
# be excluded from the distro. This is not the place to put things that # be excluded from the distro. This is not the place to put things that
@ -14,4 +16,4 @@ include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# variables. # variables.
#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental) #list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental)
rosbuild_make_distribution(0.1.0) rosbuild_make_distribution(1.3.2)

362
arm_kinematics/arm_kinematics.cpp
View File

@ -1,362 +0,0 @@
// bsd license blah blah
#include <cstring>
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <tf_conversions/tf_kdl.h>
#include <tf/transform_datatypes.h>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/jntarray.hpp>
#include <kdl/chainiksolverpos_nr_jl.hpp>
#include <kdl/chainiksolvervel_pinv.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kinematics_msgs/GetPositionFK.h>
#include <kinematics_msgs/GetPositionIK.h>
#include <kinematics_msgs/GetKinematicSolverInfo.h>
#include <kinematics_msgs/KinematicSolverInfo.h>
using std::string;
static const std::string IK_SERVICE = "get_ik";
static const std::string FK_SERVICE = "get_fk";
static const std::string IK_INFO_SERVICE = "get_ik_solver_info";
static const std::string FK_INFO_SERVICE = "get_fk_solver_info";
class Kinematics {
public:
Kinematics();
bool init();
private:
ros::NodeHandle nh, nh_private;
std::string root_name, tip_name;
KDL::JntArray joint_min, joint_max;
KDL::Chain chain;
unsigned int num_joints;
KDL::ChainFkSolverPos_recursive* fk_solver;
KDL::ChainIkSolverPos_NR_JL *ik_solver_pos;
KDL::ChainIkSolverVel_pinv* ik_solver_vel;
ros::ServiceServer ik_service,ik_solver_info_service;
ros::ServiceServer fk_service,fk_solver_info_service;
tf::TransformListener tf_listener;
kinematics_msgs::KinematicSolverInfo info;
bool loadModel(const std::string xml);
bool readJoints(urdf::Model &robot_model);
int getJointIndex(const std::string &name);
int getKDLSegmentIndex(const std::string &name);
/**
* @brief This is the basic IK service method that will compute and return an IK solution.
* @param A request message. See service definition for GetPositionIK for more information on this message.
* @param The response message. See service definition for GetPositionIK for more information on this message.
*/
bool getPositionIK(kinematics_msgs::GetPositionIK::Request &request,
kinematics_msgs::GetPositionIK::Response &response);
/**
* @brief This is the basic kinematics info service that will return information about the kinematics node.
* @param A request message. See service definition for GetKinematicSolverInfo for more information on this message.
* @param The response message. See service definition for GetKinematicSolverInfo for more information on this message.
*/
bool getIKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
kinematics_msgs::GetKinematicSolverInfo::Response &response);
/**
* @brief This is the basic kinematics info service that will return information about the kinematics node.
* @param A request message. See service definition for GetKinematicSolverInfo for more information on this message.
* @param The response message. See service definition for GetKinematicSolverInfo for more information on this message.
*/
bool getFKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
kinematics_msgs::GetKinematicSolverInfo::Response &response);
/**
* @brief This is the basic forward kinematics service that will return information about the kinematics node.
* @param A request message. See service definition for GetPositionFK for more information on this message.
* @param The response message. See service definition for GetPositionFK for more information on this message.
*/
bool getPositionFK(kinematics_msgs::GetPositionFK::Request &request,
kinematics_msgs::GetPositionFK::Response &response);
};
Kinematics::Kinematics(): nh_private ("~") {
}
bool Kinematics::init() {
// Get URDF XML
std::string urdf_xml, full_urdf_xml;
nh.param("urdf_xml",urdf_xml,std::string("robot_description"));
nh.searchParam(urdf_xml,full_urdf_xml);
ROS_DEBUG("Reading xml file from parameter server");
std::string result;
if (!nh.getParam(full_urdf_xml, result)) {
ROS_FATAL("Could not load the xml from parameter server: %s", urdf_xml.c_str());
return false;
}
// Get Root and Tip From Parameter Service
if (!nh_private.getParam("root_name", root_name)) {
ROS_FATAL("GenericIK: No root name found on parameter server");
return false;
}
if (!nh_private.getParam("tip_name", tip_name)) {
ROS_FATAL("GenericIK: No tip name found on parameter server");
return false;
}
// Load and Read Models
if (!loadModel(result)) {
ROS_FATAL("Could not load models!");
return false;
}
// Get Solver Parameters
int maxIterations;
double epsilon;
nh_private.param("maxIterations", maxIterations, 1000);
nh_private.param("epsilon", epsilon, 1e-2);
// Build Solvers
fk_solver = new KDL::ChainFkSolverPos_recursive(chain);
ik_solver_vel = new KDL::ChainIkSolverVel_pinv(chain);
ik_solver_pos = new KDL::ChainIkSolverPos_NR_JL(chain, joint_min, joint_max,
*fk_solver, *ik_solver_vel, maxIterations, epsilon);
ROS_INFO("Advertising services");
fk_service = nh_private.advertiseService(FK_SERVICE,&Kinematics::getPositionFK,this);
ik_service = nh_private.advertiseService(IK_SERVICE,&Kinematics::getPositionIK,this);
ik_solver_info_service = nh_private.advertiseService(IK_INFO_SERVICE,&Kinematics::getIKSolverInfo,this);
fk_solver_info_service = nh_private.advertiseService(FK_INFO_SERVICE,&Kinematics::getFKSolverInfo,this);
return true;
}
bool Kinematics::loadModel(const std::string xml) {
urdf::Model robot_model;
KDL::Tree tree;
if (!robot_model.initString(xml)) {
ROS_FATAL("Could not initialize robot model");
return -1;
}
if (!kdl_parser::treeFromString(xml, tree)) {
ROS_ERROR("Could not initialize tree object");
return false;
}
if (!tree.getChain(root_name, tip_name, chain)) {
ROS_ERROR("Could not initialize chain object");
return false;
}
if (!readJoints(robot_model)) {
ROS_FATAL("Could not read information about the joints");
return false;
}
return true;
}
bool Kinematics::readJoints(urdf::Model &robot_model) {
num_joints = 0;
// get joint maxs and mins
boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name);
boost::shared_ptr<const urdf::Joint> joint;
while (link && link->name != root_name) {
joint = robot_model.getJoint(link->parent_joint->name);
if (!joint) {
ROS_ERROR("Could not find joint: %s",link->parent_joint->name.c_str());
return false;
}
if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
ROS_INFO( "adding joint: [%s]", joint->name.c_str() );
num_joints++;
}
link = robot_model.getLink(link->getParent()->name);
}
joint_min.resize(num_joints);
joint_max.resize(num_joints);
info.joint_names.resize(num_joints);
info.limits.resize(num_joints);
link = robot_model.getLink(tip_name);
unsigned int i = 0;
while (link && i < num_joints) {
joint = robot_model.getJoint(link->parent_joint->name);
if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
ROS_INFO( "getting bounds for joint: [%s]", joint->name.c_str() );
float lower, upper;
int hasLimits;
if ( joint->type != urdf::Joint::CONTINUOUS ) {
lower = joint->limits->lower;
upper = joint->limits->upper;
hasLimits = 1;
} else {
lower = -M_PI;
upper = M_PI;
hasLimits = 0;
}
int index = num_joints - i -1;
joint_min.data[index] = lower;
joint_max.data[index] = upper;
info.joint_names[index] = joint->name;
info.limits[index].joint_name = joint->name;
info.limits[index].has_position_limits = hasLimits;
info.limits[index].min_position = lower;
info.limits[index].max_position = upper;
i++;
}
link = robot_model.getLink(link->getParent()->name);
}
return true;
}
int Kinematics::getJointIndex(const std::string &name) {
for (unsigned int i=0; i < info.joint_names.size(); i++) {
if (info.joint_names[i] == name)
return i;
}
return -1;
}
int Kinematics::getKDLSegmentIndex(const std::string &name) {
int i=0;
while (i < (int)chain.getNrOfSegments()) {
if (chain.getSegment(i).getName() == name) {
return i+1;
}
i++;
}
return -1;
}
bool Kinematics::getPositionIK(kinematics_msgs::GetPositionIK::Request &request,
kinematics_msgs::GetPositionIK::Response &response) {
geometry_msgs::PoseStamped pose_msg_in = request.ik_request.pose_stamped;
tf::Stamped<tf::Pose> transform;
tf::Stamped<tf::Pose> transform_root;
tf::poseStampedMsgToTF( pose_msg_in, transform );
//Do the IK
KDL::JntArray jnt_pos_in;
KDL::JntArray jnt_pos_out;
jnt_pos_in.resize(num_joints);
for (unsigned int i=0; i < num_joints; i++) {
int tmp_index = getJointIndex(request.ik_request.ik_seed_state.joint_state.name[i]);
if (tmp_index >=0) {
jnt_pos_in(tmp_index) = request.ik_request.ik_seed_state.joint_state.position[i];
} else {
ROS_ERROR("i: %d, No joint index for %s",i,request.ik_request.ik_seed_state.joint_state.name[i].c_str());
}
}
//Convert F to our root_frame
try {
tf_listener.transformPose(root_name, transform, transform_root);
} catch (...) {
ROS_ERROR("Could not transform IK pose to frame: %s", root_name.c_str());
response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
return false;
}
KDL::Frame F_dest;
tf::TransformTFToKDL(transform_root, F_dest);
int ik_valid = ik_solver_pos->CartToJnt(jnt_pos_in, F_dest, jnt_pos_out);
if (ik_valid >= 0) {
response.solution.joint_state.name = info.joint_names;
response.solution.joint_state.position.resize(num_joints);
for (unsigned int i=0; i < num_joints; i++) {
response.solution.joint_state.position[i] = jnt_pos_out(i);
ROS_DEBUG("IK Solution: %s %d: %f",response.solution.joint_state.name[i].c_str(),i,jnt_pos_out(i));
}
response.error_code.val = response.error_code.SUCCESS;
return true;
} else {
ROS_DEBUG("An IK solution could not be found");
response.error_code.val = response.error_code.NO_IK_SOLUTION;
return true;
}
}
bool Kinematics::getIKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
kinematics_msgs::GetKinematicSolverInfo::Response &response) {
response.kinematic_solver_info = info;
return true;
}
bool Kinematics::getFKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
kinematics_msgs::GetKinematicSolverInfo::Response &response) {
response.kinematic_solver_info = info;
return true;
}
bool Kinematics::getPositionFK(kinematics_msgs::GetPositionFK::Request &request,
kinematics_msgs::GetPositionFK::Response &response) {
KDL::Frame p_out;
KDL::JntArray jnt_pos_in;
geometry_msgs::PoseStamped pose;
tf::Stamped<tf::Pose> tf_pose;
jnt_pos_in.resize(num_joints);
for (unsigned int i=0; i < num_joints; i++) {
int tmp_index = getJointIndex(request.robot_state.joint_state.name[i]);
if (tmp_index >=0)
jnt_pos_in(tmp_index) = request.robot_state.joint_state.position[i];
}
response.pose_stamped.resize(request.fk_link_names.size());
response.fk_link_names.resize(request.fk_link_names.size());
bool valid = true;
for (unsigned int i=0; i < request.fk_link_names.size(); i++) {
int segmentIndex = getKDLSegmentIndex(request.fk_link_names[i]);
ROS_DEBUG("End effector index: %d",segmentIndex);
ROS_DEBUG("Chain indices: %d",chain.getNrOfSegments());
if (fk_solver->JntToCart(jnt_pos_in,p_out,segmentIndex) >=0) {
tf_pose.frame_id_ = root_name;
tf_pose.stamp_ = ros::Time();
tf::PoseKDLToTF(p_out,tf_pose);
try {
tf_listener.transformPose(request.header.frame_id,tf_pose,tf_pose);
} catch (...) {
ROS_ERROR("Could not transform FK pose to frame: %s",request.header.frame_id.c_str());
response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
return false;
}
tf::poseStampedTFToMsg(tf_pose,pose);
response.pose_stamped[i] = pose;
response.fk_link_names[i] = request.fk_link_names[i];
response.error_code.val = response.error_code.SUCCESS;
} else {
ROS_ERROR("Could not compute FK for %s",request.fk_link_names[i].c_str());
response.error_code.val = response.error_code.NO_FK_SOLUTION;
valid = false;
}
}
return true;
}
int main(int argc, char **argv) {
ros::init(argc, argv, "arm_kinematics");
Kinematics k;
if (k.init()<0) {
ROS_ERROR("Could not initialize kinematics node");
return -1;
}
ros::spin();
return 0;
}

26
arm_kinematics/mainpage.dox
View File

@ -1,26 +0,0 @@
/**
\mainpage
\htmlinclude manifest.html
\b arm_kinematics is ...
<!--
Provide an overview of your package.
-->
\section codeapi Code API
<!--
Provide links to specific auto-generated API documentation within your
package that is of particular interest to a reader. Doxygen will
document pretty much every part of your code, so do your best here to
point the reader to the actual API.
If your codebase is fairly large or has different sets of APIs, you
should use the doxygen 'group' tag to keep these APIs together. For
example, the roscpp documentation has 'libros' group.
-->
*/

19
arm_kinematics/manifest.xml
View File

@ -1,19 +0,0 @@
<package>
<description brief="A generic package for computing Arm Kinematics">
A generic package for computing both forward and backward kinematics for arms.
Developed as an alternative to pr2_arm_kinematics for people not using the PR2.
</description>
<author>David Lu!!</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/arm_kinematics</url>
<depend package="roscpp"/>
<depend package="tf"/>
<depend package="sensor_msgs"/>
<depend package="tf_conversions"/>
<depend package="kdl_parser"/>
<depend package="kinematics_msgs"/>
</package>

36
assimp/Makefile Normal file
View File

@ -0,0 +1,36 @@
all: installed
PACKAGE_NAME=`rospack find assimp`
# Regular download
TARBALL_NAME = assimp-svn-811
TARBALL = build/$(TARBALL_NAME).tar.gz
TARBALL_URL = http://pr.willowgarage.com/downloads/$(TARBALL_NAME).tar.gz
TARBALL_PATCH = assimp.patch
UNPACK_CMD = tar xzf
SOURCE_DIR = build/$(TARBALL_NAME)
include $(shell rospack find mk)/download_unpack_build.mk
ASSIMP_DIR = $(shell rospack find assimp)
BOOST_INCLUDE_DIRS = $(shell rosboost-cfg --include_dirs)
BOOST_LIBRARY_DIRS = $(shell rosboost-cfg --lib_dirs signals,thread)
BOOST_LIBRARIES = $(shell rosboost-cfg --lflags signals,thread)
CMAKE_ARGS = -DCMAKE_INSTALL_PREFIX=$(PACKAGE_NAME) \
-D CMAKE_BUILD_TYPE=RelWithDebInfo \
-D boost_include_dirs=$(BOOST_INCLUDE_DIRS) \
-D boost_library_dirs=$(BOOST_LIBRARY_DIRS) \
-D boost_libraries="$(BOOST_LIBRARIES)"
installed: $(SOURCE_DIR)/unpacked
cd $(SOURCE_DIR) && mkdir -p build && cd build && cmake $(CMAKE_ARGS) .. && export PARALLEL_JOBS=ROS_PARALLEL_JOBS && make $(ROS_PARALLEL_JOBS) && make install
mkdir -p $(PACKAGE_NAME)/include/assimp
cp -r $(SOURCE_DIR)/include/* $(PACKAGE_NAME)/include/assimp/
touch installed
clean:
-cd $(SVN_DIR)/build && make clean
-rm -f installed
-rm -rf include lib
wipe: clean
rm -rf build

56
assimp/assimp.patch Normal file
View File

@ -0,0 +1,56 @@
Index: code/CMakeLists.txt
===================================================================
--- code/CMakeLists.txt (revision 811)
+++ code/CMakeLists.txt (working copy)
@@ -742,6 +742,8 @@
SOVERSION ${LIBRARY_SOVERSION}
)
+target_link_libraries(assimp ${boost_libraries})
+
INSTALL( TARGETS assimp DESTINATION ${LIB_INSTALL_DIR} )
INSTALL( FILES ${PUBLIC_HEADERS} DESTINATION ${INCLUDE_INSTALL_DIR}/assimp )
INSTALL( FILES ${COMPILER_HEADERS} DESTINATION ${INCLUDE_INSTALL_DIR}/assimp/Compiler )
Index: CMakeLists.txt
===================================================================
--- CMakeLists.txt (revision 811)
+++ CMakeLists.txt (working copy)
@@ -25,26 +25,21 @@
CONFIGURE_FILE( "${CMAKE_SOURCE_DIR}/assimp.pc.in" "${CMAKE_BINARY_DIR}/assimp.pc" @ONLY )
INSTALL( FILES "${CMAKE_BINARY_DIR}/assimp.pc" DESTINATION ${LIB_INSTALL_DIR}/pkgconfig/ )
-# Globally enbale Boost resp. the Boost workaround – it is also needed by the
-# tools which include the Assimp headers.
-IF ( ENABLE_BOOST_WORKAROUND )
- INCLUDE_DIRECTORIES( BoostWorkaround )
- ADD_DEFINITIONS( -DASSIMP_BUILD_BOOST_WORKAROUND )
- MESSAGE( STATUS "Building a non-boost version of Assimp." )
-ELSE ( ENABLE_BOOST_WORKAROUND )
- FIND_PACKAGE( Boost 1.35 )
+SET (boost_include_dirs "" CACHE STRING "Boost include paths. Use this to override automatic detection.")
+SET (boost_library_dirs "" CACHE STRING "Boost library paths. Use this to override automatic detection.")
+SET (boost_libraries "" CACHE STRING "Boost libraries. Use this to override automatic detection.")
- IF ( NOT Boost_FOUND )
- MESSAGE( FATAL_ERROR
- "Boost libraries (http://www.boost.org/) not found. "
- "You can build a non-boost version of Assimp with slightly reduced "
- "functionality by specifying -DENABLE_BOOST_WORKAROUND=ON."
- )
- ENDIF ( NOT Boost_FOUND )
- INCLUDE_DIRECTORIES( ${Boost_INCLUDE_DIRS} )
-ENDIF ( ENABLE_BOOST_WORKAROUND )
+message(status " debug: ${boost_include_dirs} ${boost_libraries}" )
+INCLUDE_DIRECTORIES(
+ include
+ ${boost_include_dirs}
+)
+LINK_DIRECTORIES(
+ ${boost_library_dirs}
+)
+
ADD_SUBDIRECTORY( code/ )
IF ( WIN32 )
ADD_SUBDIRECTORY( test/ )

25
assimp/manifest.xml Normal file
View File

@ -0,0 +1,25 @@
<package>
<description brief="assimp">
Open Asset Import Library (Short name: Assimp) is a free library to import various well-known 3D model formats
into applications. The library has been designed for maximum stability and efficiency. Written in C++, it is
licensed under the BSD license. The API is provided for both C and C++. Wrappers for Python and D are available,
more bindings are in development.
The library loads models in a straightforward in-memory format that can be easily read and processed by applications.
Various post processing steps can be executed on the imported data to optimize it for a particular purpose.
</description>
<author>ASSIMP Development Team</author>
<license>BSD</license>
<review status="3rdparty doc reviewed" notes=""/>
<url>http://assimp.sourceforge.net/</url>
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lassimp" />
<doxymaker external="http://assimp.sourceforge.net/main_doc.html"/>
</export>
<platform os="ubuntu" version="9.04"/>
<platform os="ubuntu" version="9.10"/>
<platform os="ubuntu" version="10.04"/>
</package>

14
collada_urdf/CMakeLists.txt Normal file
View File

@ -0,0 +1,14 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
set(ROS_BUILD_TYPE Debug)
rosbuild_init()
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
rosbuild_add_library(collada_urdf src/collada_urdf.cpp)
rosbuild_link_boost(collada_urdf system)
rosbuild_add_executable(urdf_to_collada src/urdf_to_collada.cpp)
target_link_libraries(urdf_to_collada collada_urdf)
rosbuild_add_gtest(test_collada_writer test/test_collada_urdf.cpp)
target_link_libraries(test_collada_writer collada_urdf)

0
arm_kinematics/Makefile → collada_urdf/Makefile
View File

90
collada_urdf/include/collada_urdf/collada_urdf.h Normal file
View File

@ -0,0 +1,90 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redstributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Tim Field */
#ifndef COLLADA_URDF_COLLADA_URDF_H
#define COLLADA_URDF_COLLADA_URDF_H
#include <string>
#include <boost/shared_ptr.hpp>
#include <dae.h>
#include "urdf/model.h"
namespace collada_urdf {
class ColladaUrdfException : public std::runtime_error
{
public:
ColladaUrdfException(std::string const& what);
};
/** Construct a COLLADA DOM from an URDF file
* \param file The filename from where to read the URDF
* \param dom The resulting COLLADA DOM
* \return true on success, false on failure
*/
bool colladaFromUrdfFile(std::string const& file, boost::shared_ptr<DAE>& dom);
/** Construct a COLLADA DOM from a string containing URDF
* \param xml A string containing the XML description of the robot
* \param dom The resulting COLLADA DOM
* \return true on success, false on failure
*/
bool colladaFromUrdfString(std::string const& xml, boost::shared_ptr<DAE>& dom);
/** Construct a COLLADA DOM from a TiXmlDocument containing URDF
* \param xml_doc The TiXmlDocument containing URDF
* \param dom The resulting COLLADA DOM
* \return true on success, false on failure
*/
bool colladaFromUrdfXml(TiXmlDocument* xml_doc, boost::shared_ptr<DAE>& dom);
/** Construct a COLLADA DOM from a URDF robot model
* \param robot_model The URDF robot model
* \param dom The resulting COLLADA DOM
* \return true on success, false on failure
*/
bool colladaFromUrdfModel(urdf::Model const& robot_model, boost::shared_ptr<DAE>& dom);
/** Write a COLLADA DOM to a file
* \param dom COLLADA DOM to write
* \param file The filename to write the document to
* \return true on success, false on failure
*/
bool colladaToFile(boost::shared_ptr<DAE> dom, std::string const& file);
}
#endif

22
collada_urdf/manifest.xml Normal file
View File

@ -0,0 +1,22 @@
<package>
<description brief="URDF to COLLADA converter">
This package contains a tool to convert Unified Robot Description Format (URDF) documents into COLLAborative Design Activity (COLLADA) documents.
Implements robot-specific COLLADA extensions as defined by
http://openrave.programmingvision.com/index.php/Started:COLLADA
</description>
<author>Tim Field and Rosen Diankov</author>
<license>BSD</license>
<review status="Doc reviewed" notes="2010-04-23"/>
<url>http://ros.org/wiki/collada_urdf</url>
<depend package="roscpp" />
<depend package="urdf" />
<depend package="colladadom" />
<depend package="resource_retriever" />
<depend package="angles" />
<depend package="assimp"/>
<platform os="ubuntu" version="9.04"/>
<platform os="ubuntu" version="9.10"/>
<platform os="ubuntu" version="10.04"/>
</package>

1332
collada_urdf/src/collada_urdf.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

66
collada_urdf/src/urdf_to_collada.cpp Normal file
View File

@ -0,0 +1,66 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redstributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Tim Field */
#include "collada_urdf/collada_urdf.h"
int main(int argc, char** argv)
{
if (argc != 3) {
std::cerr << "Usage: urdf_to_collada input.urdf output.dae" << std::endl;
return -1;
}
ros::init(argc, argv, "urdf_to_collada");
std::string input_filename(argv[1]);
std::string output_filename(argv[2]);
urdf::Model robot_model;
if( !robot_model.initFile(input_filename) ) {
ROS_ERROR("failed to open urdf file %s",input_filename.c_str());
}
boost::shared_ptr<DAE> dom;
if (!collada_urdf::colladaFromUrdfModel(robot_model, dom)) {
std::cerr << std::endl << "Error converting document" << std::endl;
return -1;
}
collada_urdf::colladaToFile(dom, output_filename);
std::cout << std::endl << "Document successfully written to " << output_filename << std::endl;
return 0;
}

3389
collada_urdf/test/pr2.urdf Normal file
View File

File diff suppressed because it is too large Load Diff

86
collada_urdf/test/test_collada_urdf.cpp Normal file
View File

@ -0,0 +1,86 @@
// Copyright (c) 2010, Willow Garage, Inc.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of Willow Garage, Inc. nor the names of its
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
/* Author: Tim Field */
#include "collada_urdf/collada_urdf.h"
#include <gtest/gtest.h>
#include <fstream>
#include <sstream>
#include <string>
std::string readTestUrdfString() {
std::ifstream file("test/pr2.urdf");
std::stringstream ss;
ss << file.rdbuf();
return ss.str();
}
TEST(collada_urdf, collada_from_urdf_file_works)
{
boost::shared_ptr<DAE> dom;
ASSERT_TRUE(collada_urdf::colladaFromUrdfFile("test/pr2.urdf", dom));
ASSERT_TRUE(collada_urdf::colladaToFile(dom, "test/pr2.dae"));
}
TEST(collada_urdf, collada_from_urdf_string_works)
{
std::string urdf_str = readTestUrdfString();
boost::shared_ptr<DAE> dom;
ASSERT_TRUE(collada_urdf::colladaFromUrdfString(urdf_str, dom));
ASSERT_TRUE(collada_urdf::colladaToFile(dom, "test/pr2.dae"));
}
TEST(collada_urdf, collada_from_urdf_xml_works)
{
TiXmlDocument urdf_xml;
ASSERT_TRUE(urdf_xml.Parse(readTestUrdfString().c_str()) > 0);
boost::shared_ptr<DAE> dom;
ASSERT_TRUE(collada_urdf::colladaFromUrdfXml(&urdf_xml, dom));
ASSERT_TRUE(collada_urdf::colladaToFile(dom, "test/pr2.dae"));
}
TEST(collada_urdf, collada_from_urdf_model_works)
{
urdf::Model robot_model;
TiXmlDocument urdf_xml;
ASSERT_TRUE(urdf_xml.Parse(readTestUrdfString().c_str()) > 0);
ASSERT_TRUE(robot_model.initXml(&urdf_xml));
boost::shared_ptr<DAE> dom;
ASSERT_TRUE(collada_urdf::colladaFromUrdfModel(robot_model, dom));
ASSERT_TRUE(collada_urdf::colladaToFile(dom, "test/pr2.dae"));
}
int main(int argc, char **argv) {
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

46
colladadom/Makefile Normal file
View File

@ -0,0 +1,46 @@
all: installed
TARBALL = build/collada-dom-2.2.zip
TARBALL_URL = http://pr.willowgarage.com/downloads/collada-dom-2.2.zip
SOURCE_DIR = build/colladadom
INITIAL_DIR = build/collada-dom
UNPACK_CMD = unzip
TARBALL_PATCH = collada-dom.patch
MD5SUM_FILE = collada-dom-2.2.zip.md5sum
include $(shell rospack find mk)/download_unpack_build.mk
ROOT = $(shell rospack find colladadom)
# Copied from build/colladadom/dom/Makefile:
# os: 'linux', 'mac', 'windows', or 'ps3'. Use the 'uname' command to decide the
# default value. To detect when we're on Windows we'll check to see if we're
# running on Cygwin or MinGW.
OS := linux
ifneq ($(shell uname | grep -i darwin),)
OS := mac
else ifneq ($(or $(shell uname | grep -i cygwin),$(shell uname | grep -i mingw)),)
OS := windows
endif
installed: wiped $(SOURCE_DIR)/unpacked
@echo "ROOT is: $(ROOT)"
-mkdir -p $(ROOT)/include
-mkdir -p $(ROOT)/lib
@echo "making it"
cd $(SOURCE_DIR)/dom && make $(ROS_PARALLEL_JOBS)
cp -rf $(SOURCE_DIR)/dom/include/* $(ROOT)/include/
cp -rf $(SOURCE_DIR)/dom/build/$(OS)-1.5/*.* $(ROOT)/lib/
touch installed
wiped: Makefile
make wipe
touch wiped
clean:
-cd $(SOURCE_DIR) && make clean
rm -rf $(ROOT)/include $(ROOT)/lib installed
wipe: clean
rm -rf build
.PHONY : clean download wipe

1
colladadom/collada-dom-2.2.zip.md5sum Normal file
View File

@ -0,0 +1 @@
bbb76ef2a8037c945c5cdf26829dcb7d build/collada-dom-2.2.zip

100
colladadom/collada-dom.patch Normal file
View File

@ -0,0 +1,100 @@
Index: dom/Makefile
===================================================================
--- dom/Makefile 2008-10-02 17:48:30.000000000 -0700
+++ dom/Makefile 2010-03-03 14:44:09.162398249 -0800
@@ -55,7 +55,7 @@
conf := release
# Collada version: '1.4', '1.5', or 'all'
-colladaVersion := 1.4
+colladaVersion := 1.5
# parser: 'libxml', 'tinyxml', or 'all'.
parser := libxml
@@ -188,7 +188,7 @@
ifneq ($(filter install uninstall installTest,$(MAKECMDGOALS)),)
# You can only install on Mac or Linux. Check for that.
ifeq ($(oss),linux)
-prefix := /usr/local
+prefix := ../../../
else ifeq ($(oss),mac)
prefix := /Library/Frameworks
else
Index: dom/make/common.mk
===================================================================
--- dom/make/common.mk 2008-05-14 17:30:42.000000000 -0700
+++ dom/make/common.mk 2010-04-07 17:16:11.000000000 -0700
@@ -17,6 +17,8 @@
debugSuffix :=
endif
+ccFlags += $(shell rosboost-cfg --cflags)
+
ifeq ($(os),mac)
# Add the -arch flags to specify what architectures we're building for.
ccFlags += $(addprefix -arch ,$(subst x86,i386,$(archs)))
Index: dom/make/domTest.mk
===================================================================
--- dom/make/domTest.mk 2008-09-03 08:51:18.000000000 -0700
+++ dom/make/domTest.mk 2010-04-07 17:17:18.000000000 -0700
@@ -55,13 +55,7 @@
endif
# Boost defs
-ifeq ($(os),linux)
-libOpts += -lboost_filesystem
-else
-includeOpts += -Iexternal-libs/boost
-libOpts += external-libs/boost/lib/$(buildID)/libboost_system.a
-libOpts += external-libs/boost/lib/$(buildID)/libboost_filesystem.a
-endif
+libOpts += $(shell rosboost-cfg --lflags system,filesystem)
ifeq ($(os),ps3)
# PS3 doesn't support C++ locales, so tell boost not to use them
ccFlags += -DBOOST_NO_STD_LOCALE
--- dom/src/dae/daeSIDResolver.cpp
+++ dom/src/dae/daeSIDResolver.cpp
@@ -152,6 +152,18 @@
list<string>& remainingPart) {
remainingPart.clear();
+ // custom change for following instance urls (Rosen Diankov)
+ if ( strncmp( container->getElementName(), "instance_", 9 ) == 0 ) {
+ daeURI *uri = (daeURI*)container->getAttributeValue("url");
+ if ( uri != NULL && uri->getElement() != NULL ) {
+ daeElement *e = findWithDots( uri->getElement(), s, profile, finder, remainingPart );
+ if ( e != NULL ) {
+ //found it
+ return e;
+ }
+ }
+ }
+
// First see if the whole thing resolves correctly
if (daeElement* result = finder(container, s, profile))
return result;
@@ -355,6 +367,23 @@
if ((!member.empty() || haveArrayIndex1) && result.scalar == NULL)
return daeSidRef::resolveData();
+ if( !!result.elt && !result.array && !result.scalar ) {
+ // if newparam, follow its SIDREF (Rosen Diankov)
+ if( strcmp(result.elt->getElementName(),"newparam") == 0) {
+ daeElement* psidref = result.elt->getChild("SIDREF");
+ if( !!psidref ) {
+ daeSidRef::resolveData newresult;
+ newresult = resolveImpl(daeSidRef(string("./") + psidref->getCharData(),result.elt->getParent(),sidRef.profile));
+ if( !newresult.elt ) {
+ newresult = resolveImpl(daeSidRef(psidref->getCharData(),result.elt->getParent(),sidRef.profile));
+ }
+ if( !!newresult.elt ) {
+ return newresult;
+ }
+ }
+ }
+ }
+
// SID resolution was successful.
return result;
}

2
urdf_tutorial/mainpage.dox → colladadom/mainpage.dox
View File

@ -2,7 +2,7 @@
\mainpage \mainpage
\htmlinclude manifest.html \htmlinclude manifest.html
\b urdf_tutorial is ... \b colladadom is ...
<!-- <!--
Provide an overview of your package. Provide an overview of your package.

28
colladadom/manifest.xml Normal file
View File

@ -0,0 +1,28 @@
<package>
<description brief="colladadom">
This package contains the COLLADA Document Object Model (DOM) which is an API that provides a C++ object representation of a COLLADA XML instance document.
Currently, this package pulls collada-dom-2.2 tar ball, and applies a local patch
containing
some custom changes,
various bug fixes as well as
forcing configure to use rosboost-cfg for boost compile/link flags.
See http://sourceforge.net/projects/collada-dom.
</description>
<author>Du Hung Hou, Khronos.org, Marcus Barnes, Greg Corson, Herbert Law, Sebastian Schwarz, Steven Thomas, John Hsu (ros package), Tim Field (ros package)</author>
<license>BSD</license>
<review status="Doc reviewed" notes=""/>
<url>http://ros.org/wiki/colladadom</url>
<rosdep name="libxml2"/>
<rosdep name="unzip"/>
<rosdep name="pcre"/>
<export>
<cpp lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lcollada15dom -lminizip -lz `rosboost-cfg --lflags system` `rosboost-cfg --lflags filesystem`" cflags="-I${prefix}/include -I${prefix}/include/1.5 `rosboost-cfg --cflags`"/>
<cpp os="osx" lflags="-F${prefix}/lib -framework Collada15Dom" cflags="-I${prefix}/include -I${prefix}/include/1.5"/>
</export>
<platform os="ubuntu" version="9.04"/>
<platform os="ubuntu" version="9.10"/>
<platform os="ubuntu" version="10.04"/>
</package>

1065
convex_decomposition/ConvexDecomposition/ConvexDecomposition/ConvexDecomposition.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

249
convex_decomposition/ConvexDecomposition/ConvexDecomposition/ConvexDecomposition.h Normal file
View File

@ -0,0 +1,249 @@
#ifndef CONVEX_DECOMPOSITION_H
#define CONVEX_DECOMPOSITION_H
namespace ConvexDecomposition
{
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
class ConvexResult
{
public:
ConvexResult(void)
{
mHullVcount = 0;
mHullVertices = 0;
mHullTcount = 0;
mHullIndices = 0;
}
ConvexResult(unsigned int hvcount,const double *hvertices,unsigned int htcount,const unsigned int *hindices)
{
mHullVcount = hvcount;
if ( mHullVcount )
{
mHullVertices = new double[mHullVcount*sizeof(double)*3];
memcpy(mHullVertices, hvertices, sizeof(double)*3*mHullVcount );
}
else
{
mHullVertices = 0;
}
mHullTcount = htcount;
if ( mHullTcount )
{
mHullIndices = new unsigned int[sizeof(unsigned int)*mHullTcount*3];
memcpy(mHullIndices,hindices, sizeof(unsigned int)*mHullTcount*3 );
}
else
{
mHullIndices = 0;
}
}
ConvexResult(const ConvexResult &r) // copy constructor, perform a deep copy of the data.
{
mHullVcount = r.mHullVcount;
if ( mHullVcount )
{
mHullVertices = new double[mHullVcount*sizeof(double)*3];
memcpy(mHullVertices, r.mHullVertices, sizeof(double)*3*mHullVcount );
}
else
{
mHullVertices = 0;
}
mHullTcount = r.mHullTcount;
if ( mHullTcount )
{
mHullIndices = new unsigned int[sizeof(unsigned int)*mHullTcount*3];
memcpy(mHullIndices, r.mHullIndices, sizeof(unsigned int)*mHullTcount*3 );
}
else
{
mHullIndices = 0;
}
}
~ConvexResult(void)
{
delete mHullVertices;
delete mHullIndices;
}
// the convex hull.
unsigned int mHullVcount;
double * mHullVertices;
unsigned int mHullTcount;
unsigned int *mHullIndices;
double mHullVolume; // the volume of the convex hull.
};
// convert from doubles back down to floats.
class FConvexResult
{
public:
FConvexResult(const ConvexResult &r)
{
mHullVcount = r.mHullVcount;
mHullVertices = 0;
if ( mHullVcount )
{
mHullVertices = new float[mHullVcount*3];
const double *src = r.mHullVertices;
float * dest = mHullVertices;
for (unsigned int i=0; i<mHullVcount; i++)
{
dest[0] = (float) src[0];
dest[1] = (float) src[1];
dest[2] = (float) src[2];
src+=3;
dest+=3;
}
}
mHullTcount = r.mHullTcount;
if ( mHullTcount )
{
mHullIndices = new unsigned int[mHullTcount*3];
memcpy(mHullIndices,r.mHullIndices,sizeof(unsigned int)*mHullTcount*3);
}
else
{
mHullIndices = 0;
}
mHullVolume = (float)r.mHullVolume;
}
~FConvexResult(void)
{
delete mHullVertices;
delete mHullIndices;
}
unsigned int mHullVcount;
float * mHullVertices;
unsigned int mHullTcount;
unsigned int *mHullIndices;
float mHullVolume; // the volume of the convex hull.
};
class ConvexDecompInterface
{
public:
virtual void ConvexDebugTri(const double *p1,const double *p2,const double *p3,unsigned int color) { };
virtual void ConvexDebugPoint(const double *p,double dist,unsigned int color) { };
virtual void ConvexDebugBound(const double *bmin,const double *bmax,unsigned int color) { };
virtual void ConvexDebugOBB(const double *sides, const double *matrix,unsigned int color) { };
virtual void ConvexDecompResult(ConvexResult &result) = 0;
};
// just to avoid passing a zillion parameters to the method the
// options are packed into this descriptor.
class DecompDesc
{
public:
DecompDesc(void)
{
mVcount = 0;
mVertices = 0;
mTcount = 0;
mIndices = 0;
mDepth = 5;
mCpercent = 5;
mPpercent = 5;
mMaxVertices = 32;
mSkinWidth = 0;
mCallback = 0;
}
// describes the input triangle.
unsigned int mVcount; // the number of vertices in the source mesh.
const double *mVertices; // start of the vertex position array. Assumes a stride of 3 doubles.
unsigned int mTcount; // the number of triangles in the source mesh.
unsigned int *mIndices; // the indexed triangle list array (zero index based)
// options
unsigned int mDepth; // depth to split, a maximum of 10, generally not over 7.
double mCpercent; // the concavity threshold percentage. 0=20 is reasonable.
double mPpercent; // the percentage volume conservation threshold to collapse hulls. 0-30 is reasonable.
// hull output limits.
unsigned int mMaxVertices; // maximum number of vertices in the output hull. Recommended 32 or less.
double mSkinWidth; // a skin width to apply to the output hulls.
ConvexDecompInterface *mCallback; // the interface to receive back the results.
};
// perform approximate convex decomposition on a mesh.
unsigned int performConvexDecomposition(const DecompDesc &desc); // returns the number of hulls produced.
};
#endif

483
convex_decomposition/ConvexDecomposition/ConvexDecomposition/bestfit.cpp Normal file
View File

@ -0,0 +1,483 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <math.h>
// Geometric Tools, Inc.
// http://www.geometrictools.com
// Copyright (c) 1998-2006. All Rights Reserved
//
// The Wild Magic Library (WM3) source code is supplied under the terms of
// the license agreement
// http://www.geometrictools.com/License/WildMagic3License.pdf
// and may not be copied or disclosed except in accordance with the terms
// of that agreement.
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include "bestfit.h"
namespace ConvexDecomposition
{
class Vec3
{
public:
Vec3(void) { };
Vec3(double _x,double _y,double _z) { x = _x; y = _y; z = _z; };
double dot(const Vec3 &v)
{
return x*v.x + y*v.y + z*v.z; // the dot product
}
double x;
double y;
double z;
};
class Eigen
{
public:
void DecrSortEigenStuff(void)
{
Tridiagonal(); //diagonalize the matrix.
QLAlgorithm(); //
DecreasingSort();
GuaranteeRotation();
}
void Tridiagonal(void)
{
double fM00 = mElement[0][0];
double fM01 = mElement[0][1];
double fM02 = mElement[0][2];
double fM11 = mElement[1][1];
double fM12 = mElement[1][2];
double fM22 = mElement[2][2];
m_afDiag[0] = fM00;
m_afSubd[2] = 0;
if (fM02 != (double)0.0)
{
double fLength = sqrt(fM01*fM01+fM02*fM02);
double fInvLength = ((double)1.0)/fLength;
fM01 *= fInvLength;
fM02 *= fInvLength;
double fQ = ((double)2.0)*fM01*fM12+fM02*(fM22-fM11);
m_afDiag[1] = fM11+fM02*fQ;
m_afDiag[2] = fM22-fM02*fQ;
m_afSubd[0] = fLength;
m_afSubd[1] = fM12-fM01*fQ;
mElement[0][0] = (double)1.0;
mElement[0][1] = (double)0.0;
mElement[0][2] = (double)0.0;
mElement[1][0] = (double)0.0;
mElement[1][1] = fM01;
mElement[1][2] = fM02;
mElement[2][0] = (double)0.0;
mElement[2][1] = fM02;
mElement[2][2] = -fM01;
m_bIsRotation = false;
}
else
{
m_afDiag[1] = fM11;
m_afDiag[2] = fM22;
m_afSubd[0] = fM01;
m_afSubd[1] = fM12;
mElement[0][0] = (double)1.0;
mElement[0][1] = (double)0.0;
mElement[0][2] = (double)0.0;
mElement[1][0] = (double)0.0;
mElement[1][1] = (double)1.0;
mElement[1][2] = (double)0.0;
mElement[2][0] = (double)0.0;
mElement[2][1] = (double)0.0;
mElement[2][2] = (double)1.0;
m_bIsRotation = true;
}
}
bool QLAlgorithm(void)
{
const int iMaxIter = 32;
for (int i0 = 0; i0 <3; i0++)
{
int i1;
for (i1 = 0; i1 < iMaxIter; i1++)
{
int i2;
for (i2 = i0; i2 <= (3-2); i2++)
{
double fTmp = fabs(m_afDiag[i2]) + fabs(m_afDiag[i2+1]);
if ( fabs(m_afSubd[i2]) + fTmp == fTmp )
break;
}
if (i2 == i0)
{
break;
}
double fG = (m_afDiag[i0+1] - m_afDiag[i0])/(((double)2.0) * m_afSubd[i0]);
double fR = sqrt(fG*fG+(double)1.0);
if (fG < (double)0.0)
{
fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG-fR);
}
else
{
fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG+fR);
}
double fSin = (double)1.0, fCos = (double)1.0, fP = (double)0.0;
for (int i3 = i2-1; i3 >= i0; i3--)
{
double fF = fSin*m_afSubd[i3];
double fB = fCos*m_afSubd[i3];
if (fabs(fF) >= fabs(fG))
{
fCos = fG/fF;
fR = sqrt(fCos*fCos+(double)1.0);
m_afSubd[i3+1] = fF*fR;
fSin = ((double)1.0)/fR;
fCos *= fSin;
}
else
{
fSin = fF/fG;
fR = sqrt(fSin*fSin+(double)1.0);
m_afSubd[i3+1] = fG*fR;
fCos = ((double)1.0)/fR;
fSin *= fCos;
}
fG = m_afDiag[i3+1]-fP;
fR = (m_afDiag[i3]-fG)*fSin+((double)2.0)*fB*fCos;
fP = fSin*fR;
m_afDiag[i3+1] = fG+fP;
fG = fCos*fR-fB;
for (int i4 = 0; i4 < 3; i4++)
{
fF = mElement[i4][i3+1];
mElement[i4][i3+1] = fSin*mElement[i4][i3]+fCos*fF;
mElement[i4][i3] = fCos*mElement[i4][i3]-fSin*fF;
}
}
m_afDiag[i0] -= fP;
m_afSubd[i0] = fG;
m_afSubd[i2] = (double)0.0;
}
if (i1 == iMaxIter)
{
return false;
}
}
return true;
}
void DecreasingSort(void)
{
//sort eigenvalues in decreasing order, e[0] >= ... >= e[iSize-1]
for (int i0 = 0, i1; i0 <= 3-2; i0++)
{
// locate maximum eigenvalue
i1 = i0;
double fMax = m_afDiag[i1];
int i2;
for (i2 = i0+1; i2 < 3; i2++)
{
if (m_afDiag[i2] > fMax)
{
i1 = i2;
fMax = m_afDiag[i1];
}
}
if (i1 != i0)
{
// swap eigenvalues
m_afDiag[i1] = m_afDiag[i0];
m_afDiag[i0] = fMax;
// swap eigenvectors
for (i2 = 0; i2 < 3; i2++)
{
double fTmp = mElement[i2][i0];
mElement[i2][i0] = mElement[i2][i1];
mElement[i2][i1] = fTmp;
m_bIsRotation = !m_bIsRotation;
}
}
}
}
void GuaranteeRotation(void)
{
if (!m_bIsRotation)
{
// change sign on the first column
for (int iRow = 0; iRow <3; iRow++)
{
mElement[iRow][0] = -mElement[iRow][0];
}
}
}
double mElement[3][3];
double m_afDiag[3];
double m_afSubd[3];
bool m_bIsRotation;
};
bool getBestFitPlane(unsigned int vcount,
const double *points,
unsigned int vstride,
const double *weights,
unsigned int wstride,
double *plane)
{
bool ret = false;
Vec3 kOrigin(0,0,0);
double wtotal = 0;
if ( 1 )
{
const char *source = (const char *) points;
const char *wsource = (const char *) weights;
for (unsigned int i=0; i<vcount; i++)
{
const double *p = (const double *) source;
double w = 1;
if ( wsource )
{
const double *ws = (const double *) wsource;
w = *ws; //
wsource+=wstride;
}
kOrigin.x+=p[0]*w;
kOrigin.y+=p[1]*w;
kOrigin.z+=p[2]*w;
wtotal+=w;
source+=vstride;
}
}
double recip = 1.0f / wtotal; // reciprocol of total weighting
kOrigin.x*=recip;
kOrigin.y*=recip;
kOrigin.z*=recip;
double fSumXX=0;
double fSumXY=0;
double fSumXZ=0;
double fSumYY=0;
double fSumYZ=0;
double fSumZZ=0;
if ( 1 )
{
const char *source = (const char *) points;
const char *wsource = (const char *) weights;
for (unsigned int i=0; i<vcount; i++)
{
const double *p = (const double *) source;
double w = 1;
if ( wsource )
{
const double *ws = (const double *) wsource;
w = *ws; //
wsource+=wstride;
}
Vec3 kDiff;
kDiff.x = w*(p[0] - kOrigin.x); // apply vertex weighting!
kDiff.y = w*(p[1] - kOrigin.y);
kDiff.z = w*(p[2] - kOrigin.z);
fSumXX+= kDiff.x * kDiff.x; // sume of the squares of the differences.
fSumXY+= kDiff.x * kDiff.y; // sume of the squares of the differences.
fSumXZ+= kDiff.x * kDiff.z; // sume of the squares of the differences.
fSumYY+= kDiff.y * kDiff.y;
fSumYZ+= kDiff.y * kDiff.z;
fSumZZ+= kDiff.z * kDiff.z;
source+=vstride;
}
}
fSumXX *= recip;
fSumXY *= recip;
fSumXZ *= recip;
fSumYY *= recip;
fSumYZ *= recip;
fSumZZ *= recip;
// setup the eigensolver
Eigen kES;
kES.mElement[0][0] = fSumXX;
kES.mElement[0][1] = fSumXY;
kES.mElement[0][2] = fSumXZ;
kES.mElement[1][0] = fSumXY;
kES.mElement[1][1] = fSumYY;
kES.mElement[1][2] = fSumYZ;
kES.mElement[2][0] = fSumXZ;
kES.mElement[2][1] = fSumYZ;
kES.mElement[2][2] = fSumZZ;
// compute eigenstuff, smallest eigenvalue is in last position
kES.DecrSortEigenStuff();
Vec3 kNormal;
kNormal.x = kES.mElement[0][2];
kNormal.y = kES.mElement[1][2];
kNormal.z = kES.mElement[2][2];
// the minimum energy
plane[0] = kNormal.x;
plane[1] = kNormal.y;
plane[2] = kNormal.z;
plane[3] = 0 - kNormal.dot(kOrigin);
return ret;
}
double getBoundingRegion(unsigned int vcount,const double *points,unsigned int pstride,double *bmin,double *bmax) // returns the diagonal distance
{
const unsigned char *source = (const unsigned char *) points;
bmin[0] = points[0];
bmin[1] = points[1];
bmin[2] = points[2];
bmax[0] = points[0];
bmax[1] = points[1];
bmax[2] = points[2];
for (unsigned int i=1; i<vcount; i++)
{
source+=pstride;
const double *p = (const double *) source;
if ( p[0] < bmin[0] ) bmin[0] = p[0];
if ( p[1] < bmin[1] ) bmin[1] = p[1];
if ( p[2] < bmin[2] ) bmin[2] = p[2];
if ( p[0] > bmax[0] ) bmax[0] = p[0];
if ( p[1] > bmax[1] ) bmax[1] = p[1];
if ( p[2] > bmax[2] ) bmax[2] = p[2];
}
double dx = bmax[0] - bmin[0];
double dy = bmax[1] - bmin[1];
double dz = bmax[2] - bmin[2];
return sqrt( dx*dx + dy*dy + dz*dz );
}
bool overlapAABB(const double *bmin1,const double *bmax1,const double *bmin2,const double *bmax2) // return true if the two AABB's overlap.
{
if ( bmax2[0] < bmin1[0] ) return false; // if the maximum is less than our minimum on any axis
if ( bmax2[1] < bmin1[1] ) return false;
if ( bmax2[2] < bmin1[2] ) return false;
if ( bmin2[0] > bmax1[0] ) return false; // if the minimum is greater than our maximum on any axis
if ( bmin2[1] > bmax1[1] ) return false; // if the minimum is greater than our maximum on any axis
if ( bmin2[2] > bmax1[2] ) return false; // if the minimum is greater than our maximum on any axis
return true; // the extents overlap
}
}; // end of namespace

90
convex_decomposition/ConvexDecomposition/ConvexDecomposition/bestfit.h Normal file
View File

@ -0,0 +1,90 @@
#ifndef BEST_FIT_H
#define BEST_FIT_H
// This routine was released in 'snippet' form
// by John W. Ratcliff mailto:jratcliff@infiniplex.net
// on March 22, 2006.
//
// This routine computes the 'best fit' plane equation to
// a set of input data points with an optional per vertex
// weighting component.
//
// The implementation for this was lifted directly from
// David Eberly's Magic Software implementation.
// computes the best fit plane to a collection of data points.
// returns the plane equation as A,B,C,D format. (Ax+By+Cz+D)
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
namespace ConvexDecomposition
{
bool getBestFitPlane(unsigned int vcount, // number of input data points
const double *points, // starting address of points array.
unsigned int vstride, // stride between input points.
const double *weights, // *optional point weighting values.
unsigned int wstride, // weight stride for each vertex.
double *plane);
double getBoundingRegion(unsigned int vcount,const double *points,unsigned int pstride,double *bmin,double *bmax); // returns the diagonal distance
bool overlapAABB(const double *bmin1,const double *bmax1,const double *bmin2,const double *bmax2); // return true if the two AABB's overlap.
};
#endif

368
convex_decomposition/ConvexDecomposition/ConvexDecomposition/bestfitobb.cpp Normal file
View File

@ -0,0 +1,368 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <assert.h>
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
// compute the 'best fit' oriented bounding box of an input point cloud by doing an exhaustive search.
// it spins the point cloud around searching for the minimal volume. It keeps narrowing down until
// it fails to find a better fit. The only dependency is on 'double_math'
//
// The inputs are:
//
// vcount : number of input vertices in the point cloud.
// points : a pointer to the first vertex.
// pstride : The stride between each point measured in bytes.
//
// The outputs are:
//
// sides : The length of the sides of the OBB as X, Y, Z distance.
// matrix : A pointer to a 4x4 matrix. This will contain the 3x3 rotation and the translation component.
// pos : The center of the OBB
// quat : The orientation of the OBB expressed as quaternion in the form of X,Y,Z,W
//
//
// Please email bug fixes or improvements to John W. Ratcliff at mailto:jratcliff@infiniplex.net
//
// If you find this source code useful donate a couple of bucks to my kid's fund raising website at
// www.amillionpixels.us
//
// More snippets at: www.codesuppository.com
//
#include "bestfitobb.h"
#include "float_math.h"
namespace ConvexDecomposition
{
// computes the OBB for this set of points relative to this transform matrix.
void computeOBB(unsigned int vcount,const double *points,unsigned int pstride,double *sides,double *matrix)
{
const char *src = (const char *) points;
double bmin[3] = { 1e9, 1e9, 1e9 };
double bmax[3] = { -1e9, -1e9, -1e9 };
for (unsigned int i=0; i<vcount; i++)
{
const double *p = (const double *) src;
double t[3];
fm_inverseRT(matrix, p, t ); // inverse rotate translate
if ( t[0] < bmin[0] ) bmin[0] = t[0];
if ( t[1] < bmin[1] ) bmin[1] = t[1];
if ( t[2] < bmin[2] ) bmin[2] = t[2];
if ( t[0] > bmax[0] ) bmax[0] = t[0];
if ( t[1] > bmax[1] ) bmax[1] = t[1];
if ( t[2] > bmax[2] ) bmax[2] = t[2];
src+=pstride;
}
double center[3];
sides[0] = bmax[0]-bmin[0];
sides[1] = bmax[1]-bmin[1];
sides[2] = bmax[2]-bmin[2];
center[0] = sides[0]*0.5f+bmin[0];
center[1] = sides[1]*0.5f+bmin[1];
center[2] = sides[2]*0.5f+bmin[2];
double ocenter[3];
fm_rotate(matrix,center,ocenter);
matrix[12]+=ocenter[0];
matrix[13]+=ocenter[1];
matrix[14]+=ocenter[2];
}
void computeBestFitOBB(unsigned int vcount,const double *points,unsigned int pstride,double *sides,double *matrix)
{
double bmin[3];
double bmax[3];
fm_getAABB(vcount,points,pstride,bmin,bmax);
double center[3];
center[0] = (bmax[0]-bmin[0])*0.5f + bmin[0];
center[1] = (bmax[1]-bmin[1])*0.5f + bmin[1];
center[2] = (bmax[2]-bmin[2])*0.5f + bmin[2];
double ax = 0;
double ay = 0;
double az = 0;
double sweep = 45.0f; // 180 degree sweep on all three axes.
double steps = 7.0f; // 7 steps on each axis)
double bestVolume = 1e9;
double angle[3];
while ( sweep >= 1 )
{
bool found = false;
double stepsize = sweep / steps;
for (double x=ax-sweep; x<=ax+sweep; x+=stepsize)
{
for (double y=ay-sweep; y<=ay+sweep; y+=stepsize)
{
for (double z=az-sweep; z<=az+sweep; z+=stepsize)
{
double pmatrix[16];
fm_eulerMatrix( x*FM_DEG_TO_RAD, y*FM_DEG_TO_RAD, z*FM_DEG_TO_RAD, pmatrix );
pmatrix[3*4+0] = center[0];
pmatrix[3*4+1] = center[1];
pmatrix[3*4+2] = center[2];
double psides[3];
computeOBB( vcount, points, pstride, psides, pmatrix );
double volume = psides[0]*psides[1]*psides[2]; // the volume of the cube
if ( volume < bestVolume )
{
bestVolume = volume;
sides[0] = psides[0];
sides[1] = psides[1];
sides[2] = psides[2];
angle[0] = ax;
angle[1] = ay;
angle[2] = az;
memcpy(matrix,pmatrix,sizeof(double)*16);
found = true; // yes, we found an improvement.
}
}
}
}
if ( found )
{
ax = angle[0];
ay = angle[1];
az = angle[2];
sweep*=0.5f; // sweep 1/2 the distance as the last time.
}
else
{
break; // no improvement, so just
}
}
}
void computeBestFitOBB(unsigned int vcount,const double *points,unsigned int pstride,double *sides,double *pos,double *quat)
{
double matrix[16];
computeBestFitOBB(vcount,points,pstride,sides,matrix);
fm_getTranslation(matrix,pos);
fm_matrixToQuat(matrix,quat);
}
void computeBestFitABB(unsigned int vcount,const double *points,unsigned int pstride,double *sides,double *pos)
{
double bmin[3];
double bmax[3];
bmin[0] = points[0];
bmin[1] = points[1];
bmin[2] = points[2];
bmax[0] = points[0];
bmax[1] = points[1];
bmax[2] = points[2];
const char *cp = (const char *) points;
for (unsigned int i=0; i<vcount; i++)
{
const double *p = (const double *) cp;
if ( p[0] < bmin[0] ) bmin[0] = p[0];
if ( p[1] < bmin[1] ) bmin[1] = p[1];
if ( p[2] < bmin[2] ) bmin[2] = p[2];
if ( p[0] > bmax[0] ) bmax[0] = p[0];
if ( p[1] > bmax[1] ) bmax[1] = p[1];
if ( p[2] > bmax[2] ) bmax[2] = p[2];
cp+=pstride;
}
sides[0] = bmax[0] - bmin[0];
sides[1] = bmax[1] - bmin[1];
sides[2] = bmax[2] - bmin[2];
pos[0] = bmin[0]+sides[0]*0.5f;
pos[1] = bmin[1]+sides[1]*0.5f;
pos[2] = bmin[2]+sides[2]*0.5f;
}
void computeBestFitOBB(unsigned int vcount,const float *points,unsigned int pstride,float *sides,float *pos,float *quat) // the float version of the routine.
{
double *temp = new double[vcount*3];
const char *src = (const char *)points;
double *dest = temp;
for (unsigned int i=0; i<vcount; i++)
{
const float *s = (const float *) src;
temp[0] = s[0];
temp[1] = s[1];
temp[2] = s[2];
temp+=3;
s+=pstride;
}
double dsides[3];
double dpos[3];
double dquat[3];
computeBestFitOBB(vcount,temp,sizeof(double)*3,dsides,dpos,dquat);
if ( sides )
{
sides[0] = (float) dsides[0];
sides[1] = (float) dsides[1];
sides[2] = (float) dsides[2];
}
if ( pos )
{
pos[0] = (float) dpos[0];
pos[1] = (float) dpos[1];
pos[2] = (float) dpos[2];
}
if ( quat )
{
quat[0] = (float) dquat[0];
quat[1] = (float) dquat[1];
quat[2] = (float) dquat[2];
quat[3] = (float) dquat[3];
}
delete temp;
}
void computeBestFitABB(unsigned int vcount,const float *points,unsigned int pstride,float *sides,float *pos)
{
float bmin[3];
float bmax[3];
bmin[0] = points[0];
bmin[1] = points[1];
bmin[2] = points[2];
bmax[0] = points[0];
bmax[1] = points[1];
bmax[2] = points[2];
const char *cp = (const char *) points;
for (unsigned int i=0; i<vcount; i++)
{
const float *p = (const float *) cp;
if ( p[0] < bmin[0] ) bmin[0] = p[0];
if ( p[1] < bmin[1] ) bmin[1] = p[1];
if ( p[2] < bmin[2] ) bmin[2] = p[2];
if ( p[0] > bmax[0] ) bmax[0] = p[0];
if ( p[1] > bmax[1] ) bmax[1] = p[1];
if ( p[2] > bmax[2] ) bmax[2] = p[2];
cp+=pstride;
}
sides[0] = bmax[0] - bmin[0];
sides[1] = bmax[1] - bmin[1];
sides[2] = bmax[2] - bmin[2];
pos[0] = bmin[0]+sides[0]*0.5f;
pos[1] = bmin[1]+sides[1]*0.5f;
pos[2] = bmin[2]+sides[2]*0.5f;
}
};

100
convex_decomposition/ConvexDecomposition/ConvexDecomposition/bestfitobb.h Normal file
View File

@ -0,0 +1,100 @@
#ifndef BEST_FIT_OBB_H
#define BEST_FIT_OBB_H
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
// compute the 'best fit' oriented bounding box of an input point cloud by doing an exhaustive search.
// it spins the point cloud around searching for the minimal volume. It keeps narrowing down until
// it fails to find a better fit. The only dependency is on 'double_math'
//
// The inputs are:
//
// vcount : number of input vertices in the point cloud.
// points : a pointer to the first vertex.
// pstride : The stride between each point measured in bytes.
//
// The outputs are:
//
// sides : The length of the sides of the OBB as X, Y, Z distance.
// matrix : A pointer to a 4x4 matrix. This will contain the 3x3 rotation and the translation component.
// pos : The center of the OBB
// quat : The orientation of the OBB expressed as quaternion in the form of X,Y,Z,W
//
//
// Please email bug fixes or improvements to John W. Ratcliff at mailto:jratcliff@infiniplex.net
//
// If you find this source code useful donate a couple of bucks to my kid's fund raising website at
// www.amillionpixels.us
//
// More snippets at: www.codesuppository.com
//
namespace ConvexDecomposition
{
void computeBestFitOBB(unsigned int vcount,const double *points,unsigned int pstride,double *sides,double *matrix);
void computeBestFitOBB(unsigned int vcount,const double *points,unsigned int pstride,double *sides,double *pos,double *quat);
void computeBestFitABB(unsigned int vcount,const double *points,unsigned int pstride,double *sides,double *pos);
void computeBestFitOBB(unsigned int vcount,const float *points,unsigned int pstride,float *sides,float *pos,float *quat); // the float version of the routine.
void computeBestFitABB(unsigned int vcount,const float *points,unsigned int pstride,float *sides,float *pos);
};
#endif

3650
convex_decomposition/ConvexDecomposition/ConvexDecomposition/cd_hull.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

250
convex_decomposition/ConvexDecomposition/ConvexDecomposition/cd_hull.h Normal file
View File

@ -0,0 +1,250 @@
#ifndef HULL_LIB_H
#define HULL_LIB_H
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
namespace ConvexDecomposition
{
class HullResult
{
public:
HullResult(void)
{
mPolygons = true;
mNumOutputVertices = 0;
mOutputVertices = 0;
mNumFaces = 0;
mNumIndices = 0;
mIndices = 0;
}
bool mPolygons; // true if indices represents polygons, false indices are triangles
unsigned int mNumOutputVertices; // number of vertices in the output hull
double *mOutputVertices; // array of vertices, 3 doubles each x,y,z
unsigned int mNumFaces; // the number of faces produced
unsigned int mNumIndices; // the total number of indices
unsigned int *mIndices; // pointer to indices.
// If triangles, then indices are array indexes into the vertex list.
// If polygons, indices are in the form (number of points in face) (p1, p2, p3, ..) etc..
};
class FHullResult
{
public:
FHullResult(const HullResult &r)
{
mPolygons = r.mPolygons;
mNumOutputVertices = r.mNumOutputVertices;
mNumFaces = r.mNumFaces;
mNumIndices = r.mNumIndices;
mIndices = 0;
mOutputVertices = 0;
if ( mNumIndices )
{
mIndices = new unsigned int[mNumIndices];
memcpy(mIndices,r.mIndices,sizeof(unsigned int)*mNumIndices);
}
if ( mNumOutputVertices )
{
mOutputVertices = new float[mNumOutputVertices*3];
const double *src = r.mOutputVertices;
float *dst = mOutputVertices;
for (unsigned int i=0; i<mNumOutputVertices; i++)
{
dst[0] = (float) src[0];
dst[1] = (float) src[1];
dst[2] = (float) src[2];
dst+=3;
src+=3;
}
}
}
~FHullResult(void)
{
delete mIndices;
delete mOutputVertices;
}
bool mPolygons; // true if indices represents polygons, false indices are triangles
unsigned int mNumOutputVertices; // number of vertices in the output hull
float *mOutputVertices; // array of vertices, 3 doubles each x,y,z
unsigned int mNumFaces; // the number of faces produced
unsigned int mNumIndices; // the total number of indices
unsigned int *mIndices; // pointer to indices.
// If triangles, then indices are array indexes into the vertex list.
// If polygons, indices are in the form (number of points in face) (p1, p2, p3, ..) etc..
};
enum HullFlag
{
QF_TRIANGLES = (1<<0), // report results as triangles, not polygons.
QF_REVERSE_ORDER = (1<<1), // reverse order of the triangle indices.
QF_SKIN_WIDTH = (1<<2), // extrude hull based on this skin width
QF_DEFAULT = 0
};
class HullDesc
{
public:
HullDesc(void)
{
mFlags = QF_DEFAULT;
mVcount = 0;
mVertices = 0;
mVertexStride = 0;
mNormalEpsilon = 0.001f;
mMaxVertices = 4096; // maximum number of points to be considered for a convex hull.
mSkinWidth = 0.01f; // default is one centimeter
};
HullDesc(HullFlag flag,
unsigned int vcount,
const double *vertices,
unsigned int stride)
{
mFlags = flag;
mVcount = vcount;
mVertices = vertices;
mVertexStride = stride;
mNormalEpsilon = 0.001f;
mMaxVertices = 4096;
mSkinWidth = 0.01f; // default is one centimeter
}
bool HasHullFlag(HullFlag flag) const
{
if ( mFlags & flag ) return true;
return false;
}
void SetHullFlag(HullFlag flag)
{
mFlags|=flag;
}
void ClearHullFlag(HullFlag flag)
{
mFlags&=~flag;
}
unsigned int mFlags; // flags to use when generating the convex hull.
unsigned int mVcount; // number of vertices in the input point cloud
const double *mVertices; // the array of vertices.
unsigned int mVertexStride; // the stride of each vertex, in bytes.
double mNormalEpsilon; // the epsilon for removing duplicates. This is a normalized value, if normalized bit is on.
double mSkinWidth;
unsigned int mMaxVertices; // maximum number of vertices to be considered for the hull!
};
enum HullError
{
QE_OK, // success!
QE_FAIL // failed.
};
// This class is used when converting a convex hull into a triangle mesh.
class ConvexHullVertex
{
public:
double mPos[3];
double mNormal[3];
double mTexel[2];
};
// A virtual interface to receive the triangles from the convex hull.
class ConvexHullTriangleInterface
{
public:
virtual void ConvexHullTriangle(const ConvexHullVertex &v1,const ConvexHullVertex &v2,const ConvexHullVertex &v3) = 0;
};
class HullLibrary
{
public:
HullError CreateConvexHull(const HullDesc &desc, // describes the input request
HullResult &result); // contains the resulst
HullError ReleaseResult(HullResult &result); // release memory allocated for this result, we are done with it.
// Utility function to convert the output convex hull as a renderable set of triangles. Unfolds the polygons into
// individual triangles, compute the vertex normals, and projects some texture co-ordinates.
HullError CreateTriangleMesh(HullResult &answer,ConvexHullTriangleInterface *iface);
private:
double ComputeNormal(double *n,const double *A,const double *B,const double *C);
void AddConvexTriangle(ConvexHullTriangleInterface *callback,const double *p1,const double *p2,const double *p3);
void BringOutYourDead(const double *verts,unsigned int vcount, double *overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount);
bool CleanupVertices(unsigned int svcount,
const double *svertices,
unsigned int stride,
unsigned int &vcount, // output number of vertices
double *vertices, // location to store the results.
double normalepsilon,
double *scale);
};
};
#endif

1209
convex_decomposition/ConvexDecomposition/ConvexDecomposition/cd_vector.h Normal file
View File

File diff suppressed because it is too large Load Diff

858
convex_decomposition/ConvexDecomposition/ConvexDecomposition/cd_wavefront.cpp Normal file
View File

@ -0,0 +1,858 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <ctype.h>
#pragma warning(disable:4996)
#include "cd_wavefront.h"
using namespace ConvexDecomposition;
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include <vector>
namespace ConvexDecomposition
{
typedef std::vector< int > IntVector;
typedef std::vector< double > FloatVector;
#if defined(__APPLE__) || defined(__CELLOS_LV2__)
#define stricmp(a, b) strcasecmp((a), (b))
#endif
/*******************************************************************/
/******************** InParser.h ********************************/
/*******************************************************************/
class InPlaceParserInterface
{
public:
virtual int ParseLine(int lineno,int argc,const char **argv) =0; // return TRUE to continue parsing, return FALSE to abort parsing process
};
enum SeparatorType
{
ST_DATA, // is data
ST_HARD, // is a hard separator
ST_SOFT, // is a soft separator
ST_EOS // is a comment symbol, and everything past this character should be ignored
};
class InPlaceParser
{
public:
InPlaceParser(void)
{
Init();
}
InPlaceParser(char *data,int len)
{
Init();
SetSourceData(data,len);
}
InPlaceParser(const char *fname)
{
Init();
SetFile(fname);
}
~InPlaceParser(void);
void Init(void)
{
mQuoteChar = 34;
mData = 0;
mLen = 0;
mMyAlloc = false;
for (int i=0; i<256; i++)
{
mHard[i] = ST_DATA;
mHardString[i*2] = i;
mHardString[i*2+1] = 0;
}
mHard[0] = ST_EOS;
mHard[32] = ST_SOFT;
mHard[9] = ST_SOFT;
mHard[13] = ST_SOFT;
mHard[10] = ST_SOFT;
}
void SetFile(const char *fname); // use this file as source data to parse.
void SetSourceData(char *data,int len)
{
mData = data;
mLen = len;
mMyAlloc = false;
};
int Parse(InPlaceParserInterface *callback); // returns true if entire file was parsed, false if it aborted for some reason
int ProcessLine(int lineno,char *line,InPlaceParserInterface *callback);
const char ** GetArglist(char *source,int &count); // convert source string into an arg list, this is a destructive parse.
void SetHardSeparator(char c) // add a hard separator
{
mHard[c] = ST_HARD;
}
void SetHard(char c) // add a hard separator
{
mHard[c] = ST_HARD;
}
void SetCommentSymbol(char c) // comment character, treated as 'end of string'
{
mHard[c] = ST_EOS;
}
void ClearHardSeparator(char c)
{
mHard[c] = ST_DATA;
}
void DefaultSymbols(void); // set up default symbols for hard seperator and comment symbol of the '#' character.
bool EOS(char c)
{
if ( mHard[c] == ST_EOS )
{
return true;
}
return false;
}
void SetQuoteChar(char c)
{
mQuoteChar = c;
}
private:
inline char * AddHard(int &argc,const char **argv,char *foo);
inline bool IsHard(char c);
inline char * SkipSpaces(char *foo);
inline bool IsWhiteSpace(char c);
inline bool IsNonSeparator(char c); // non seperator,neither hard nor soft
bool mMyAlloc; // whether or not *I* allocated the buffer and am responsible for deleting it.
char *mData; // ascii data to parse.
int mLen; // length of data
SeparatorType mHard[256];
char mHardString[256*2];
char mQuoteChar;
};
/*******************************************************************/
/******************** InParser.cpp ********************************/
/*******************************************************************/
void InPlaceParser::SetFile(const char *fname)
{
if ( mMyAlloc )
{
free(mData);
}
mData = 0;
mLen = 0;
mMyAlloc = false;
FILE *fph = fopen(fname,"rb");
if ( fph )
{
fseek(fph,0L,SEEK_END);
mLen = ftell(fph);
fseek(fph,0L,SEEK_SET);
if ( mLen )
{
mData = (char *) malloc(sizeof(char)*(mLen+1));
int ok = fread(mData, mLen, 1, fph);
if ( !ok )
{
free(mData);
mData = 0;
}
else
{
mData[mLen] = 0; // zero byte terminate end of file marker.
mMyAlloc = true;
}
}
fclose(fph);
}
}
InPlaceParser::~InPlaceParser(void)
{
if ( mMyAlloc )
{
free(mData);
}
}
#define MAXARGS 512
bool InPlaceParser::IsHard(char c)
{
return mHard[c] == ST_HARD;
}
char * InPlaceParser::AddHard(int &argc,const char **argv,char *foo)
{
while ( IsHard(*foo) )
{
const char *hard = &mHardString[*foo*2];
if ( argc < MAXARGS )
{
argv[argc++] = hard;
}
foo++;
}
return foo;
}
bool InPlaceParser::IsWhiteSpace(char c)
{
return mHard[c] == ST_SOFT;
}
char * InPlaceParser::SkipSpaces(char *foo)
{
while ( !EOS(*foo) && IsWhiteSpace(*foo) ) foo++;
return foo;
}
bool InPlaceParser::IsNonSeparator(char c)
{
if ( !IsHard(c) && !IsWhiteSpace(c) && c != 0 ) return true;
return false;
}
int InPlaceParser::ProcessLine(int lineno,char *line,InPlaceParserInterface *callback)
{
int ret = 0;
const char *argv[MAXARGS];
int argc = 0;
char *foo = line;
while ( !EOS(*foo) && argc < MAXARGS )
{
foo = SkipSpaces(foo); // skip any leading spaces
if ( EOS(*foo) ) break;
if ( *foo == mQuoteChar ) // if it is an open quote
{
foo++;
if ( argc < MAXARGS )
{
argv[argc++] = foo;
}
while ( !EOS(*foo) && *foo != mQuoteChar ) foo++;
if ( !EOS(*foo) )
{
*foo = 0; // replace close quote with zero byte EOS
foo++;
}
}
else
{
foo = AddHard(argc,argv,foo); // add any hard separators, skip any spaces
if ( IsNonSeparator(*foo) ) // add non-hard argument.
{
bool quote = false;
if ( *foo == mQuoteChar )
{
foo++;
quote = true;
}
if ( argc < MAXARGS )
{
argv[argc++] = foo;
}
if ( quote )
{
while (*foo && *foo != mQuoteChar ) foo++;
if ( *foo ) *foo = 32;
}
// continue..until we hit an eos ..
while ( !EOS(*foo) ) // until we hit EOS
{
if ( IsWhiteSpace(*foo) ) // if we hit a space, stomp a zero byte, and exit
{
*foo = 0;
foo++;
break;
}
else if ( IsHard(*foo) ) // if we hit a hard separator, stomp a zero byte and store the hard separator argument
{
const char *hard = &mHardString[*foo*2];
*foo = 0;
if ( argc < MAXARGS )
{
argv[argc++] = hard;
}
foo++;
break;
}
foo++;
} // end of while loop...
}
}
}
if ( argc )
{
ret = callback->ParseLine(lineno, argc, argv );
}
return ret;
}
int InPlaceParser::Parse(InPlaceParserInterface *callback) // returns true if entire file was parsed, false if it aborted for some reason
{
assert( callback );
if ( !mData ) return 0;
int ret = 0;
int lineno = 0;
char *foo = mData;
char *begin = foo;
while ( *foo )
{
if ( *foo == 10 || *foo == 13 )
{
lineno++;
*foo = 0;
if ( *begin ) // if there is any data to parse at all...
{
int v = ProcessLine(lineno,begin,callback);
if ( v ) ret = v;
}
foo++;
if ( *foo == 10 ) foo++; // skip line feed, if it is in the carraige-return line-feed format...
begin = foo;
}
else
{
foo++;
}
}
lineno++; // lasst line.
int v = ProcessLine(lineno,begin,callback);
if ( v ) ret = v;
return ret;
}
void InPlaceParser::DefaultSymbols(void)
{
SetHardSeparator(',');
SetHardSeparator('(');
SetHardSeparator(')');
SetHardSeparator('=');
SetHardSeparator('[');
SetHardSeparator(']');
SetHardSeparator('{');
SetHardSeparator('}');
SetCommentSymbol('#');
}
const char ** InPlaceParser::GetArglist(char *line,int &count) // convert source string into an arg list, this is a destructive parse.
{
const char **ret = 0;
static const char *argv[MAXARGS];
int argc = 0;
char *foo = line;
while ( !EOS(*foo) && argc < MAXARGS )
{
foo = SkipSpaces(foo); // skip any leading spaces
if ( EOS(*foo) ) break;
if ( *foo == mQuoteChar ) // if it is an open quote
{
foo++;
if ( argc < MAXARGS )
{
argv[argc++] = foo;
}
while ( !EOS(*foo) && *foo != mQuoteChar ) foo++;
if ( !EOS(*foo) )
{
*foo = 0; // replace close quote with zero byte EOS
foo++;
}
}
else
{
foo = AddHard(argc,argv,foo); // add any hard separators, skip any spaces
if ( IsNonSeparator(*foo) ) // add non-hard argument.
{
bool quote = false;
if ( *foo == mQuoteChar )
{
foo++;
quote = true;
}
if ( argc < MAXARGS )
{
argv[argc++] = foo;
}
if ( quote )
{
while (*foo && *foo != mQuoteChar ) foo++;
if ( *foo ) *foo = 32;
}
// continue..until we hit an eos ..
while ( !EOS(*foo) ) // until we hit EOS
{
if ( IsWhiteSpace(*foo) ) // if we hit a space, stomp a zero byte, and exit
{
*foo = 0;
foo++;
break;
}
else if ( IsHard(*foo) ) // if we hit a hard separator, stomp a zero byte and store the hard separator argument
{
const char *hard = &mHardString[*foo*2];
*foo = 0;
if ( argc < MAXARGS )
{
argv[argc++] = hard;
}
foo++;
break;
}
foo++;
} // end of while loop...
}
}
}
count = argc;
if ( argc )
{
ret = argv;
}
return ret;
}
/*******************************************************************/
/******************** Geometry.h ********************************/
/*******************************************************************/
class GeometryVertex
{
public:
double mPos[3];
double mNormal[3];
double mTexel[2];
};
class GeometryInterface
{
public:
virtual void NodeTriangle(const GeometryVertex *v1,const GeometryVertex *v2,const GeometryVertex *v3)
{
}
};
/*******************************************************************/
/******************** Obj.h ********************************/
/*******************************************************************/
class OBJ : public InPlaceParserInterface
{
public:
int LoadMesh(const char *fname,GeometryInterface *callback);
int ParseLine(int lineno,int argc,const char **argv); // return TRUE to continue parsing, return FALSE to abort parsing process
private:
void GetVertex(GeometryVertex &v,const char *face) const;
FloatVector mVerts;
FloatVector mTexels;
FloatVector mNormals;
GeometryInterface *mCallback;
friend class WavefrontObj;
};
/*******************************************************************/
/******************** Obj.cpp ********************************/
/*******************************************************************/
int OBJ::LoadMesh(const char *fname,GeometryInterface *iface)
{
int ret = 0;
mVerts.clear();
mTexels.clear();
mNormals.clear();
mCallback = iface;
InPlaceParser ipp(fname);
ipp.Parse(this);
return ret;
}
static const char * GetArg(const char **argv,int i,int argc)
{
const char * ret = 0;
if ( i < argc ) ret = argv[i];
return ret;
}
void OBJ::GetVertex(GeometryVertex &v,const char *face) const
{
v.mPos[0] = 0;
v.mPos[1] = 0;
v.mPos[2] = 0;
v.mTexel[0] = 0;
v.mTexel[1] = 0;
v.mNormal[0] = 0;
v.mNormal[1] = 1;
v.mNormal[2] = 0;
int index = atoi( face )-1;
const char *texel = strstr(face,"/");
if ( texel )
{
int tindex = atoi( texel+1) - 1;
if ( tindex >=0 && tindex < (int)(mTexels.size()/2) )
{
const double *t = &mTexels[tindex*2];
v.mTexel[0] = t[0];
v.mTexel[1] = t[1];
}
const char *normal = strstr(texel+1,"/");
if ( normal )
{
int nindex = atoi( normal+1 ) - 1;
if (nindex >= 0 && nindex < (int)(mNormals.size()/3) )
{
const double *n = &mNormals[nindex*3];
v.mNormal[0] = n[0];
v.mNormal[1] = n[1];
v.mNormal[2] = n[2];
}
}
}
if ( index >= 0 && index < (int)(mVerts.size()/3) )
{
const double *p = &mVerts[index*3];
v.mPos[0] = p[0];
v.mPos[1] = p[1];
v.mPos[2] = p[2];
}
}
int OBJ::ParseLine(int lineno,int argc,const char **argv) // return TRUE to continue parsing, return FALSE to abort parsing process
{
int ret = 0;
if ( argc >= 1 )
{
const char *foo = argv[0];
if ( *foo != '#' )
{
if ( strcmp(argv[0],"v") == 0 && argc == 4 )
{
double vx = (double) atof( argv[1] );
double vy = (double) atof( argv[2] );
double vz = (double) atof( argv[3] );
mVerts.push_back(vx);
mVerts.push_back(vy);
mVerts.push_back(vz);
}
else if ( strcmp(argv[0],"vt") == 0 && argc == 3 )
{
double tx = (double) atof( argv[1] );
double ty = (double) atof( argv[2] );
mTexels.push_back(tx);
mTexels.push_back(ty);
}
else if ( strcmp(argv[0],"vn") == 0 && argc == 4 )
{
double normalx = (double) atof(argv[1]);
double normaly = (double) atof(argv[2]);
double normalz = (double) atof(argv[3]);
mNormals.push_back(normalx);
mNormals.push_back(normaly);
mNormals.push_back(normalz);
}
else if ( strcmp(argv[0],"f") == 0 && argc >= 4 )
{
GeometryVertex v[32];
int vcount = argc-1;
for (int i=1; i<argc; i++)
{
GetVertex(v[i-1],argv[i] );
}
// need to generate a normal!
#if 0 // not currently implemented
if ( mNormals.empty() )
{
Vector3d<double> p1( v[0].mPos );
Vector3d<double> p2( v[1].mPos );
Vector3d<double> p3( v[2].mPos );
Vector3d<double> n;
n.ComputeNormal(p3,p2,p1);
for (int i=0; i<vcount; i++)
{
v[i].mNormal[0] = n.x;
v[i].mNormal[1] = n.y;
v[i].mNormal[2] = n.z;
}
}
#endif
mCallback->NodeTriangle(&v[0],&v[1],&v[2]);
if ( vcount >=3 ) // do the fan
{
for (int i=2; i<(vcount-1); i++)
{
mCallback->NodeTriangle(&v[0],&v[i],&v[i+1]);
}
}
}
}
}
return ret;
}
class BuildMesh : public GeometryInterface
{
public:
int GetIndex(const double *p)
{
int vcount = mVertices.size()/3;
if(vcount>0)
{
//New MS STL library checks indices in debug build, so zero causes an assert if it is empty.
const double *v = &mVertices[0];
for (int i=0; i<vcount; i++)
{
if ( v[0] == p[0] && v[1] == p[1] && v[2] == p[2] ) return i;
v+=3;
}
}
mVertices.push_back( p[0] );
mVertices.push_back( p[1] );
mVertices.push_back( p[2] );
return vcount;
}
virtual void NodeTriangle(const GeometryVertex *v1,const GeometryVertex *v2,const GeometryVertex *v3)
{
mIndices.push_back( GetIndex(v1->mPos) );
mIndices.push_back( GetIndex(v2->mPos) );
mIndices.push_back( GetIndex(v3->mPos) );
}
const FloatVector& GetVertices(void) const { return mVertices; };
const IntVector& GetIndices(void) const { return mIndices; };
private:
FloatVector mVertices;
IntVector mIndices;
};
WavefrontObj::WavefrontObj(void)
{
mVertexCount = 0;
mTriCount = 0;
mIndices = 0;
mVertices = 0;
}
WavefrontObj::~WavefrontObj(void)
{
delete mIndices;
delete mVertices;
}
unsigned int WavefrontObj::loadObj(const char *fname) // load a wavefront obj returns number of triangles that were loaded. Data is persists until the class is destructed.
{
unsigned int ret = 0;
delete mVertices;
mVertices = 0;
delete mIndices;
mIndices = 0;
mVertexCount = 0;
mTriCount = 0;
BuildMesh bm;
OBJ obj;
obj.LoadMesh(fname,&bm);
const FloatVector &vlist = bm.GetVertices();
const IntVector &indices = bm.GetIndices();
if ( vlist.size() )
{
mVertexCount = vlist.size()/3;
mVertices = new double[mVertexCount*3];
memcpy( mVertices, &vlist[0], sizeof(double)*mVertexCount*3 );
mTriCount = indices.size()/3;
mIndices = new int[mTriCount*3*sizeof(int)];
memcpy(mIndices, &indices[0], sizeof(int)*mTriCount*3);
ret = mTriCount;
}
else if( obj.mVerts.size() > 0 ) {
// take consecutive vertices
mVertexCount = obj.mVerts.size()/3;
mVertices = new double[mVertexCount*3];
memcpy( mVertices, &obj.mVerts[0], sizeof(double)*mVertexCount*3 );
mTriCount = mVertexCount/3;
mIndices = new int[mTriCount*3*sizeof(int)];
for(int i = 0; i < mVertexCount; ++i)
mIndices[i] = i;
ret = mTriCount;
}
return ret;
}
};

82
convex_decomposition/ConvexDecomposition/ConvexDecomposition/cd_wavefront.h Normal file
View File

@ -0,0 +1,82 @@
#ifndef CD_WAVEFRONT_OBJ_H
#define CD_WAVEFRONT_OBJ_H
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
namespace ConvexDecomposition
{
class WavefrontObj
{
public:
WavefrontObj(void);
~WavefrontObj(void);
unsigned int loadObj(const char *fname); // load a wavefront obj returns number of triangles that were loaded. Data is persists until the class is destructed.
int mVertexCount;
int mTriCount;
int *mIndices;
double *mVertices;
};
};
#endif

821
convex_decomposition/ConvexDecomposition/ConvexDecomposition/concavity.cpp Normal file
View File

@ -0,0 +1,821 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include <vector>
#include "concavity.h"
#include "raytri.h"
#include "bestfit.h"
#include "cd_hull.h"
#include "meshvolume.h"
#include "cd_vector.h"
#include "splitplane.h"
#include "ConvexDecomposition.h"
#define WSCALE 4
#define CONCAVE_THRESH 0.05f
namespace ConvexDecomposition
{
unsigned int getDebugColor(void)
{
static unsigned int colors[8] =
{
0xFF0000,
0x00FF00,
0x0000FF,
0xFFFF00,
0x00FFFF,
0xFF00FF,
0xFFFFFF,
0xFF8040
};
static int count = 0;
count++;
if ( count == 8 ) count = 0;
assert( count >= 0 && count < 8 );
unsigned int color = colors[count];
return color;
}
class Wpoint
{
public:
Wpoint(const Vector3d<double> &p,double w)
{
mPoint = p;
mWeight = w;
}
Vector3d<double> mPoint;
double mWeight;
};
typedef std::vector< Wpoint > WpointVector;
static inline double DistToPt(const double *p,const double *plane)
{
double x = p[0];
double y = p[1];
double z = p[2];
double d = x*plane[0] + y*plane[1] + z*plane[2] + plane[3];
return d;
}
static void intersect(const double *p1,const double *p2,double *split,const double *plane)
{
double dp1 = DistToPt(p1,plane);
double dp2 = DistToPt(p2,plane);
double dir[3];
dir[0] = p2[0] - p1[0];
dir[1] = p2[1] - p1[1];
dir[2] = p2[2] - p1[2];
double dot1 = dir[0]*plane[0] + dir[1]*plane[1] + dir[2]*plane[2];
double dot2 = dp1 - plane[3];
double t = -(plane[3] + dot2 ) / dot1;
split[0] = (dir[0]*t)+p1[0];
split[1] = (dir[1]*t)+p1[1];
split[2] = (dir[2]*t)+p1[2];
}
class CTri
{
public:
CTri(void) { };
CTri(const double *p1,const double *p2,const double *p3,unsigned int i1,unsigned int i2,unsigned int i3)
{
mProcessed = 0;
mI1 = i1;
mI2 = i2;
mI3 = i3;
mP1.Set(p1);
mP2.Set(p2);
mP3.Set(p3);
mPlaneD = mNormal.ComputePlane(mP1,mP2,mP3);
}
double Facing(const CTri &t)
{
double d = mNormal.Dot(t.mNormal);
return d;
}
// clip this line segment against this triangle.
bool clip(const Vector3d<double> &start,Vector3d<double> &end) const
{
Vector3d<double> sect;
bool hit = lineIntersectsTriangle(start.Ptr(), end.Ptr(), mP1.Ptr(), mP2.Ptr(), mP3.Ptr(), sect.Ptr() );
if ( hit )
{
end = sect;
}
return hit;
}
bool Concave(const Vector3d<double> &p,double &distance,Vector3d<double> &n) const
{
n.NearestPointInTriangle(p,mP1,mP2,mP3);
distance = p.Distance(n);
return true;
}
void addTri(unsigned int *indices,unsigned int i1,unsigned int i2,unsigned int i3,unsigned int &tcount) const
{
indices[tcount*3+0] = i1;
indices[tcount*3+1] = i2;
indices[tcount*3+2] = i3;
tcount++;
}
double getVolume(ConvexDecompInterface *callback) const
{
unsigned int indices[8*3];
unsigned int tcount = 0;
addTri(indices,0,1,2,tcount);
addTri(indices,3,4,5,tcount);
addTri(indices,0,3,4,tcount);
addTri(indices,0,4,1,tcount);
addTri(indices,1,4,5,tcount);
addTri(indices,1,5,2,tcount);
addTri(indices,0,3,5,tcount);
addTri(indices,0,5,2,tcount);
const double *vertices = mP1.Ptr();
if ( callback )
{
unsigned int color = getDebugColor();
#if 0
Vector3d<double> d1 = mNear1;
Vector3d<double> d2 = mNear2;
Vector3d<double> d3 = mNear3;
callback->ConvexDebugPoint(mP1.Ptr(),0.01f,0x00FF00);
callback->ConvexDebugPoint(mP2.Ptr(),0.01f,0x00FF00);
callback->ConvexDebugPoint(mP3.Ptr(),0.01f,0x00FF00);
callback->ConvexDebugPoint(d1.Ptr(),0.01f,0xFF0000);
callback->ConvexDebugPoint(d2.Ptr(),0.01f,0xFF0000);
callback->ConvexDebugPoint(d3.Ptr(),0.01f,0xFF0000);
callback->ConvexDebugTri(mP1.Ptr(), d1.Ptr(), d1.Ptr(),0x00FF00);
callback->ConvexDebugTri(mP2.Ptr(), d2.Ptr(), d2.Ptr(),0x00FF00);
callback->ConvexDebugTri(mP3.Ptr(), d3.Ptr(), d3.Ptr(),0x00FF00);
#else
for (unsigned int i=0; i<tcount; i++)
{
unsigned int i1 = indices[i*3+0];
unsigned int i2 = indices[i*3+1];
unsigned int i3 = indices[i*3+2];
const double *p1 = &vertices[ i1*3 ];
const double *p2 = &vertices[ i2*3 ];
const double *p3 = &vertices[ i3*3 ];
callback->ConvexDebugTri(p1,p2,p3,color);
}
#endif
}
double v = computeMeshVolume(mP1.Ptr(), tcount, indices );
return v;
}
double raySect(const Vector3d<double> &p,const Vector3d<double> &dir,Vector3d<double> &sect) const
{
double plane[4];
plane[0] = mNormal.x;
plane[1] = mNormal.y;
plane[2] = mNormal.z;
plane[3] = mPlaneD;
Vector3d<double> dest = p+dir*100000;
intersect( p.Ptr(), dest.Ptr(), sect.Ptr(), plane );
return sect.Distance(p); // return the intersection distance.
}
double planeDistance(const Vector3d<double> &p) const
{
double plane[4];
plane[0] = mNormal.x;
plane[1] = mNormal.y;
plane[2] = mNormal.z;
plane[3] = mPlaneD;
return DistToPt( p.Ptr(), plane );
}
bool samePlane(const CTri &t) const
{
const double THRESH = 0.001f;
double dd = fabs( t.mPlaneD - mPlaneD );
if ( dd > THRESH ) return false;
dd = fabs( t.mNormal.x - mNormal.x );
if ( dd > THRESH ) return false;
dd = fabs( t.mNormal.y - mNormal.y );
if ( dd > THRESH ) return false;
dd = fabs( t.mNormal.z - mNormal.z );
if ( dd > THRESH ) return false;
return true;
}
bool hasIndex(unsigned int i) const
{
if ( i == mI1 || i == mI2 || i == mI3 ) return true;
return false;
}
bool sharesEdge(const CTri &t) const
{
bool ret = false;
unsigned int count = 0;
if ( t.hasIndex(mI1) ) count++;
if ( t.hasIndex(mI2) ) count++;
if ( t.hasIndex(mI3) ) count++;
if ( count >= 2 ) ret = true;
return ret;
}
void debug(unsigned int color,ConvexDecompInterface *callback)
{
callback->ConvexDebugTri( mP1.Ptr(), mP2.Ptr(), mP3.Ptr(), color );
callback->ConvexDebugTri( mP1.Ptr(), mP1.Ptr(), mNear1.Ptr(), 0xFF0000 );
callback->ConvexDebugTri( mP2.Ptr(), mP2.Ptr(), mNear2.Ptr(), 0xFF0000 );
callback->ConvexDebugTri( mP2.Ptr(), mP3.Ptr(), mNear3.Ptr(), 0xFF0000 );
callback->ConvexDebugPoint( mNear1.Ptr(), 0.01f, 0xFF0000 );
callback->ConvexDebugPoint( mNear2.Ptr(), 0.01f, 0xFF0000 );
callback->ConvexDebugPoint( mNear3.Ptr(), 0.01f, 0xFF0000 );
}
double area(void)
{
double a = mConcavity*mP1.Area(mP2,mP3);
return a;
}
void addWeighted(WpointVector &list,ConvexDecompInterface *callback)
{
Wpoint p1(mP1,mC1);
Wpoint p2(mP2,mC2);
Wpoint p3(mP3,mC3);
Vector3d<double> d1 = mNear1 - mP1;
Vector3d<double> d2 = mNear2 - mP2;
Vector3d<double> d3 = mNear3 - mP3;
d1*=WSCALE;
d2*=WSCALE;
d3*=WSCALE;
d1 = d1 + mP1;
d2 = d2 + mP2;
d3 = d3 + mP3;
Wpoint p4(d1,mC1);
Wpoint p5(d2,mC2);
Wpoint p6(d3,mC3);
list.push_back(p1);
list.push_back(p2);
list.push_back(p3);
list.push_back(p4);
list.push_back(p5);
list.push_back(p6);
#if 0
callback->ConvexDebugPoint(mP1.Ptr(),0.01f,0x00FF00);
callback->ConvexDebugPoint(mP2.Ptr(),0.01f,0x00FF00);
callback->ConvexDebugPoint(mP3.Ptr(),0.01f,0x00FF00);
callback->ConvexDebugPoint(d1.Ptr(),0.01f,0xFF0000);
callback->ConvexDebugPoint(d2.Ptr(),0.01f,0xFF0000);
callback->ConvexDebugPoint(d3.Ptr(),0.01f,0xFF0000);
callback->ConvexDebugTri(mP1.Ptr(), d1.Ptr(), d1.Ptr(),0x00FF00);
callback->ConvexDebugTri(mP2.Ptr(), d2.Ptr(), d2.Ptr(),0x00FF00);
callback->ConvexDebugTri(mP3.Ptr(), d3.Ptr(), d3.Ptr(),0x00FF00);
Vector3d<double> np1 = mP1 + mNormal*0.05f;
Vector3d<double> np2 = mP2 + mNormal*0.05f;
Vector3d<double> np3 = mP3 + mNormal*0.05f;
callback->ConvexDebugTri(mP1.Ptr(), np1.Ptr(), np1.Ptr(), 0xFF00FF );
callback->ConvexDebugTri(mP2.Ptr(), np2.Ptr(), np2.Ptr(), 0xFF00FF );
callback->ConvexDebugTri(mP3.Ptr(), np3.Ptr(), np3.Ptr(), 0xFF00FF );
callback->ConvexDebugPoint( np1.Ptr(), 0.01F, 0XFF00FF );
callback->ConvexDebugPoint( np2.Ptr(), 0.01F, 0XFF00FF );
callback->ConvexDebugPoint( np3.Ptr(), 0.01F, 0XFF00FF );
#endif
}
Vector3d<double> mP1;
Vector3d<double> mP2;
Vector3d<double> mP3;
Vector3d<double> mNear1;
Vector3d<double> mNear2;
Vector3d<double> mNear3;
Vector3d<double> mNormal;
double mPlaneD;
double mConcavity;
double mC1;
double mC2;
double mC3;
unsigned int mI1;
unsigned int mI2;
unsigned int mI3;
int mProcessed; // already been added...
};
typedef std::vector< CTri > CTriVector;
bool featureMatch(CTri &m,const CTriVector &tris,ConvexDecompInterface *callback,const CTriVector &input_mesh)
{
bool ret = false;
double neardot = 0.707f;
m.mConcavity = 0;
//gLog->Display("*********** FEATURE MATCH *************\r\n");
//gLog->Display("Plane: %0.4f,%0.4f,%0.4f %0.4f\r\n", m.mNormal.x, m.mNormal.y, m.mNormal.z, m.mPlaneD );
//gLog->Display("*********************************************\r\n");
CTriVector::const_iterator i;
CTri nearest;
double near[3] = { 1e9, 1e9, 1e9 };
for (i=tris.begin(); i!=tris.end(); ++i)
{
const CTri &t = (*i);
//gLog->Display(" HullPlane: %0.4f,%0.4f,%0.4f %0.4f\r\n", t.mNormal.x, t.mNormal.y, t.mNormal.z, t.mPlaneD );
if ( t.samePlane(m) )
{
//gLog->Display("*** PLANE MATCH!!!\r\n");
ret = false;
break;
}
double dot = t.mNormal.Dot(m.mNormal);
if ( dot > neardot )
{
double d1 = t.planeDistance( m.mP1 );
double d2 = t.planeDistance( m.mP2 );
double d3 = t.planeDistance( m.mP3 );
if ( d1 > 0.001f || d2 > 0.001f || d3 > 0.001f ) // can't be near coplaner!
{
neardot = dot;
Vector3d<double> n1,n2,n3;
t.raySect( m.mP1, m.mNormal, m.mNear1 );
t.raySect( m.mP2, m.mNormal, m.mNear2 );
t.raySect( m.mP3, m.mNormal, m.mNear3 );
nearest = t;
ret = true;
}
}
}
if ( ret )
{
if ( 0 )
{
CTriVector::const_iterator i;
for (i=input_mesh.begin(); i!=input_mesh.end(); ++i)
{
const CTri &c = (*i);
if ( c.mI1 != m.mI1 && c.mI2 != m.mI2 && c.mI3 != m.mI3 )
{
c.clip( m.mP1, m.mNear1 );
c.clip( m.mP2, m.mNear2 );
c.clip( m.mP3, m.mNear3 );
}
}
}
//gLog->Display("*********************************************\r\n");
//gLog->Display(" HullPlaneNearest: %0.4f,%0.4f,%0.4f %0.4f\r\n", nearest.mNormal.x, nearest.mNormal.y, nearest.mNormal.z, nearest.mPlaneD );
m.mC1 = m.mP1.Distance( m.mNear1 );
m.mC2 = m.mP2.Distance( m.mNear2 );
m.mC3 = m.mP3.Distance( m.mNear3 );
m.mConcavity = m.mC1;
if ( m.mC2 > m.mConcavity ) m.mConcavity = m.mC2;
if ( m.mC3 > m.mConcavity ) m.mConcavity = m.mC3;
#if 0
callback->ConvexDebugTri( m.mP1.Ptr(), m.mP2.Ptr(), m.mP3.Ptr(), 0x00FF00 );
callback->ConvexDebugTri( m.mNear1.Ptr(), m.mNear2.Ptr(), m.mNear3.Ptr(), 0xFF0000 );
callback->ConvexDebugTri( m.mP1.Ptr(), m.mP1.Ptr(), m.mNear1.Ptr(), 0xFFFF00 );
callback->ConvexDebugTri( m.mP2.Ptr(), m.mP2.Ptr(), m.mNear2.Ptr(), 0xFFFF00 );
callback->ConvexDebugTri( m.mP3.Ptr(), m.mP3.Ptr(), m.mNear3.Ptr(), 0xFFFF00 );
#endif
}
else
{
//gLog->Display("No match\r\n");
}
//gLog->Display("*********************************************\r\n");
return ret;
}
bool isFeatureTri(CTri &t,CTriVector &flist,double fc,ConvexDecompInterface *callback,unsigned int color)
{
bool ret = false;
if ( t.mProcessed == 0 ) // if not already processed
{
double c = t.mConcavity / fc; // must be within 80% of the concavity of the parent.
if ( c > 0.85f )
{
// see if this triangle is a 'feature' triangle. Meaning it shares an
// edge with any existing feature triangle and is within roughly the same
// concavity of the parent.
if ( flist.size() )
{
CTriVector::iterator i;
for (i=flist.begin(); i!=flist.end(); ++i)
{
CTri &ftri = (*i);
if ( ftri.sharesEdge(t) )
{
t.mProcessed = 2; // it is now part of a feature.
flist.push_back(t); // add it to the feature list.
// callback->ConvexDebugTri( t.mP1.Ptr(), t.mP2.Ptr(),t.mP3.Ptr(), color );
ret = true;
break;
}
}
}
else
{
t.mProcessed = 2;
flist.push_back(t); // add it to the feature list.
// callback->ConvexDebugTri( t.mP1.Ptr(), t.mP2.Ptr(),t.mP3.Ptr(), color );
ret = true;
}
}
else
{
t.mProcessed = 1; // eliminated for this feature, but might be valid for the next one..
}
}
return ret;
}
double computeConcavity(unsigned int vcount,
const double *vertices,
unsigned int tcount,
const unsigned int *indices,
ConvexDecompInterface *callback,
double *plane, // plane equation to split on
double &volume)
{
double cret = 0;
volume = 1;
HullResult result;
HullLibrary hl;
HullDesc desc;
desc.mMaxVertices = 256;
desc.SetHullFlag(QF_TRIANGLES);
desc.mVcount = vcount;
desc.mVertices = vertices;
desc.mVertexStride = sizeof(double)*3;
HullError ret = hl.CreateConvexHull(desc,result);
if ( ret == QE_OK )
{
double bmin[3];
double bmax[3];
double diagonal = getBoundingRegion( result.mNumOutputVertices, result.mOutputVertices, sizeof(double)*3, bmin, bmax );
double dx = bmax[0] - bmin[0];
double dy = bmax[1] - bmin[1];
double dz = bmax[2] - bmin[2];
Vector3d<double> center;
center.x = bmin[0] + dx*0.5f;
center.y = bmin[1] + dy*0.5f;
center.z = bmin[2] + dz*0.5f;
double boundVolume = dx*dy*dz;
volume = computeMeshVolume2( result.mOutputVertices, result.mNumFaces, result.mIndices );
#if 1
// ok..now..for each triangle on the original mesh..
// we extrude the points to the nearest point on the hull.
const unsigned int *source = result.mIndices;
CTriVector tris;
for (unsigned int i=0; i<result.mNumFaces; i++)
{
unsigned int i1 = *source++;
unsigned int i2 = *source++;
unsigned int i3 = *source++;
const double *p1 = &result.mOutputVertices[i1*3];
const double *p2 = &result.mOutputVertices[i2*3];
const double *p3 = &result.mOutputVertices[i3*3];
// callback->ConvexDebugTri(p1,p2,p3,0xFFFFFF);
CTri t(p1,p2,p3,i1,i2,i3); //
tris.push_back(t);
}
// we have not pre-computed the plane equation for each triangle in the convex hull..
double totalVolume = 0;
CTriVector ftris; // 'feature' triangles.
const unsigned int *src = indices;
double maxc=0;
if ( 1 )
{
CTriVector input_mesh;
if ( 1 )
{
const unsigned int *src = indices;
for (unsigned int i=0; i<tcount; i++)
{
unsigned int i1 = *src++;
unsigned int i2 = *src++;
unsigned int i3 = *src++;
const double *p1 = &vertices[i1*3];
const double *p2 = &vertices[i2*3];
const double *p3 = &vertices[i3*3];
CTri t(p1,p2,p3,i1,i2,i3);
input_mesh.push_back(t);
}
}
CTri maxctri;
for (unsigned int i=0; i<tcount; i++)
{
unsigned int i1 = *src++;
unsigned int i2 = *src++;
unsigned int i3 = *src++;
const double *p1 = &vertices[i1*3];
const double *p2 = &vertices[i2*3];
const double *p3 = &vertices[i3*3];
CTri t(p1,p2,p3,i1,i2,i3);
featureMatch(t, tris, callback, input_mesh );
if ( t.mConcavity > CONCAVE_THRESH )
{
if ( t.mConcavity > maxc )
{
maxc = t.mConcavity;
maxctri = t;
}
double v = t.getVolume(0);
totalVolume+=v;
ftris.push_back(t);
}
}
}
if ( ftris.size() && 0 )
{
// ok..now we extract the triangles which form the maximum concavity.
CTriVector major_feature;
double maxarea = 0;
while ( maxc > CONCAVE_THRESH )
{
unsigned int color = getDebugColor(); //
CTriVector flist;
bool found;
double totalarea = 0;
do
{
found = false;
CTriVector::iterator i;
for (i=ftris.begin(); i!=ftris.end(); ++i)
{
CTri &t = (*i);
if ( isFeatureTri(t,flist,maxc,callback,color) )
{
found = true;
totalarea+=t.area();
}
}
} while ( found );
if ( totalarea > maxarea )
{
major_feature = flist;
maxarea = totalarea;
}
maxc = 0;
for (unsigned int i=0; i<ftris.size(); i++)
{
CTri &t = ftris[i];
if ( t.mProcessed != 2 )
{
t.mProcessed = 0;
if ( t.mConcavity > maxc )
{
maxc = t.mConcavity;
}
}
}
}
unsigned int color = getDebugColor();
WpointVector list;
for (unsigned int i=0; i<major_feature.size(); ++i)
{
major_feature[i].addWeighted(list,callback);
major_feature[i].debug(color,callback);
}
getBestFitPlane( list.size(), &list[0].mPoint.x, sizeof(Wpoint), &list[0].mWeight, sizeof(Wpoint), plane );
computeSplitPlane( vcount, vertices, tcount, indices, callback, plane );
}
else
{
computeSplitPlane( vcount, vertices, tcount, indices, callback, plane );
}
#endif
cret = totalVolume;
hl.ReleaseResult(result);
}
return cret;
}
};

81
convex_decomposition/ConvexDecomposition/ConvexDecomposition/concavity.h Normal file
View File

@ -0,0 +1,81 @@
#ifndef COMPUTE_CONCAVITY_H
#define COMPUTE_CONCAVITY_H
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
namespace ConvexDecomposition
{
class ConvexDecompInterface;
// compute's how 'concave' this object is and returns the total volume of the
// convex hull as well as the volume of the 'concavity' which was found.
double computeConcavity(unsigned int vcount,
const double *vertices,
unsigned int tcount,
const unsigned int *indices,
ConvexDecompInterface *callback,
double *plane,
double &volume);
};
#endif

378
convex_decomposition/ConvexDecomposition/ConvexDecomposition/fitsphere.cpp Normal file
View File

@ -0,0 +1,378 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <math.h>
#include "fitsphere.h"
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
/*
An Efficient Bounding Sphere
by Jack Ritter
from "Graphics Gems", Academic Press, 1990
*/
/* Routine to calculate tight bounding sphere over */
/* a set of points in 3D */
/* This contains the routine find_bounding_sphere(), */
/* the struct definition, and the globals used for parameters. */
/* The abs() of all coordinates must be < BIGNUMBER */
/* Code written by Jack Ritter and Lyle Rains. */
namespace ConvexDecomposition
{
#define BIGNUMBER 100000000.0 /* hundred million */
static inline void Set(double *n,double x,double y,double z)
{
n[0] = x;
n[1] = y;
n[2] = z;
};
static inline void Copy(double *dest,const double *source)
{
dest[0] = source[0];
dest[1] = source[1];
dest[2] = source[2];
}
double computeBoundingSphere(unsigned int vcount,const double *points,double *center)
{
double mRadius;
double mRadius2;
double xmin[3];
double xmax[3];
double ymin[3];
double ymax[3];
double zmin[3];
double zmax[3];
double dia1[3];
double dia2[3];
/* FIRST PASS: find 6 minima/maxima points */
Set(xmin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
Set(xmax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
Set(ymin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
Set(ymax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
Set(zmin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
Set(zmax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
for (unsigned i=0; i<vcount; i++)
{
const double *caller_p = &points[i*3];
if (caller_p[0]<xmin[0])
Copy(xmin,caller_p); /* New xminimum point */
if (caller_p[0]>xmax[0])
Copy(xmax,caller_p);
if (caller_p[1]<ymin[1])
Copy(ymin,caller_p);
if (caller_p[1]>ymax[1])
Copy(ymax,caller_p);
if (caller_p[2]<zmin[2])
Copy(zmin,caller_p);
if (caller_p[2]>zmax[2])
Copy(zmax,caller_p);
}
/* Set xspan = distance between the 2 points xmin & xmax (squared) */
double dx = xmax[0] - xmin[0];
double dy = xmax[1] - xmin[1];
double dz = xmax[2] - xmin[2];
double xspan = dx*dx + dy*dy + dz*dz;
/* Same for y & z spans */
dx = ymax[0] - ymin[0];
dy = ymax[1] - ymin[1];
dz = ymax[2] - ymin[2];
double yspan = dx*dx + dy*dy + dz*dz;
dx = zmax[0] - zmin[0];
dy = zmax[1] - zmin[1];
dz = zmax[2] - zmin[2];
double zspan = dx*dx + dy*dy + dz*dz;
/* Set points dia1 & dia2 to the maximally separated pair */
Copy(dia1,xmin);
Copy(dia2,xmax); /* assume xspan biggest */
double maxspan = xspan;
if (yspan>maxspan)
{
maxspan = yspan;
Copy(dia1,ymin);
Copy(dia2,ymax);
}
if (zspan>maxspan)
{
Copy(dia1,zmin);
Copy(dia2,zmax);
}
/* dia1,dia2 is a diameter of initial sphere */
/* calc initial center */
center[0] = (dia1[0]+dia2[0])*0.5f;
center[1] = (dia1[1]+dia2[1])*0.5f;
center[2] = (dia1[2]+dia2[2])*0.5f;
/* calculate initial radius**2 and radius */
dx = dia2[0]-center[0]; /* x component of radius vector */
dy = dia2[1]-center[1]; /* y component of radius vector */
dz = dia2[2]-center[2]; /* z component of radius vector */
mRadius2 = dx*dx + dy*dy + dz*dz;
mRadius = double(sqrt(mRadius2));
/* SECOND PASS: increment current sphere */
if ( 1 )
{
for (unsigned i=0; i<vcount; i++)
{
const double *caller_p = &points[i*3];
dx = caller_p[0]-center[0];
dy = caller_p[1]-center[1];
dz = caller_p[2]-center[2];
double old_to_p_sq = dx*dx + dy*dy + dz*dz;
if (old_to_p_sq > mRadius2) /* do r**2 test first */
{ /* this point is outside of current sphere */
double old_to_p = double(sqrt(old_to_p_sq));
/* calc radius of new sphere */
mRadius = (mRadius + old_to_p) * 0.5f;
mRadius2 = mRadius*mRadius; /* for next r**2 compare */
double old_to_new = old_to_p - mRadius;
/* calc center of new sphere */
double recip = 1.0f /old_to_p;
double cx = (mRadius*center[0] + old_to_new*caller_p[0]) * recip;
double cy = (mRadius*center[1] + old_to_new*caller_p[1]) * recip;
double cz = (mRadius*center[2] + old_to_new*caller_p[2]) * recip;
Set(center,cx,cy,cz);
}
}
}
return mRadius;
}
static inline void Set(float *n,float x,float y,float z)
{
n[0] = x;
n[1] = y;
n[2] = z;
};
static inline void Copy(float *dest,const float *source)
{
dest[0] = source[0];
dest[1] = source[1];
dest[2] = source[2];
}
float computeBoundingSphere(unsigned int vcount,const float *points,float *center)
{
float mRadius;
float mRadius2;
float xmin[3];
float xmax[3];
float ymin[3];
float ymax[3];
float zmin[3];
float zmax[3];
float dia1[3];
float dia2[3];
/* FIRST PASS: find 6 minima/maxima points */
Set(xmin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
Set(xmax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
Set(ymin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
Set(ymax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
Set(zmin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
Set(zmax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
for (unsigned i=0; i<vcount; i++)
{
const float *caller_p = &points[i*3];
if (caller_p[0]<xmin[0])
Copy(xmin,caller_p); /* New xminimum point */
if (caller_p[0]>xmax[0])
Copy(xmax,caller_p);
if (caller_p[1]<ymin[1])
Copy(ymin,caller_p);
if (caller_p[1]>ymax[1])
Copy(ymax,caller_p);
if (caller_p[2]<zmin[2])
Copy(zmin,caller_p);
if (caller_p[2]>zmax[2])
Copy(zmax,caller_p);
}
/* Set xspan = distance between the 2 points xmin & xmax (squared) */
float dx = xmax[0] - xmin[0];
float dy = xmax[1] - xmin[1];
float dz = xmax[2] - xmin[2];
float xspan = dx*dx + dy*dy + dz*dz;
/* Same for y & z spans */
dx = ymax[0] - ymin[0];
dy = ymax[1] - ymin[1];
dz = ymax[2] - ymin[2];
float yspan = dx*dx + dy*dy + dz*dz;
dx = zmax[0] - zmin[0];
dy = zmax[1] - zmin[1];
dz = zmax[2] - zmin[2];
float zspan = dx*dx + dy*dy + dz*dz;
/* Set points dia1 & dia2 to the maximally separated pair */
Copy(dia1,xmin);
Copy(dia2,xmax); /* assume xspan biggest */
float maxspan = xspan;
if (yspan>maxspan)
{
maxspan = yspan;
Copy(dia1,ymin);
Copy(dia2,ymax);
}
if (zspan>maxspan)
{
Copy(dia1,zmin);
Copy(dia2,zmax);
}
/* dia1,dia2 is a diameter of initial sphere */
/* calc initial center */
center[0] = (dia1[0]+dia2[0])*0.5f;
center[1] = (dia1[1]+dia2[1])*0.5f;
center[2] = (dia1[2]+dia2[2])*0.5f;
/* calculate initial radius**2 and radius */
dx = dia2[0]-center[0]; /* x component of radius vector */
dy = dia2[1]-center[1]; /* y component of radius vector */
dz = dia2[2]-center[2]; /* z component of radius vector */
mRadius2 = dx*dx + dy*dy + dz*dz;
mRadius = float(sqrt(mRadius2));
/* SECOND PASS: increment current sphere */
if ( 1 )
{
for (unsigned i=0; i<vcount; i++)
{
const float *caller_p = &points[i*3];
dx = caller_p[0]-center[0];
dy = caller_p[1]-center[1];
dz = caller_p[2]-center[2];
float old_to_p_sq = dx*dx + dy*dy + dz*dz;
if (old_to_p_sq > mRadius2) /* do r**2 test first */
{ /* this point is outside of current sphere */
float old_to_p = float(sqrt(old_to_p_sq));
/* calc radius of new sphere */
mRadius = (mRadius + old_to_p) * 0.5f;
mRadius2 = mRadius*mRadius; /* for next r**2 compare */
float old_to_new = old_to_p - mRadius;
/* calc center of new sphere */
float recip = 1.0f /old_to_p;
float cx = (mRadius*center[0] + old_to_new*caller_p[0]) * recip;
float cy = (mRadius*center[1] + old_to_new*caller_p[1]) * recip;
float cz = (mRadius*center[2] + old_to_new*caller_p[2]) * recip;
Set(center,cx,cy,cz);
}
}
}
return mRadius;
}
double computeSphereVolume(double r)
{
return (4.0f*3.141592654f*r*r*r)/3.0f; // 4/3 PI R cubed
}
};

70
convex_decomposition/ConvexDecomposition/ConvexDecomposition/fitsphere.h Normal file
View File

@ -0,0 +1,70 @@
#ifndef FIT_SPHERE_H
#define FIT_SPHERE_H
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
namespace ConvexDecomposition
{
double computeBoundingSphere(unsigned int vcount,const double *points,double *center);
float computeBoundingSphere(unsigned int vcount,const float *points,float *center);
double computeSphereVolume(double r);
};
#endif

463
convex_decomposition/ConvexDecomposition/ConvexDecomposition/float_math.cpp Normal file
View File

@ -0,0 +1,463 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <math.h>
#include "float_math.h"
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
// a set of routines that let you do common 3d math
// operations without any vector, matrix, or quaternion
// classes or templates.
//
// a vector (or point) is a 'double *' to 3 doubleing point numbers.
// a matrix is a 'double *' to an array of 16 doubleing point numbers representing a 4x4 transformation matrix compatible with D3D or OGL
// a quaternion is a 'double *' to 4 doubles representing a quaternion x,y,z,w
//
//
//
// Please email bug fixes or improvements to John W. Ratcliff at mailto:jratcliff@infiniplex.net
//
// If you find this source code useful donate a couple of bucks to my kid's fund raising website at
// www.amillionpixels.us
//
// More snippets at: www.codesuppository.com
//
namespace ConvexDecomposition
{
void fm_inverseRT(const double *matrix,const double *pos,double *t) // inverse rotate translate the point.
{
double _x = pos[0] - matrix[3*4+0];
double _y = pos[1] - matrix[3*4+1];
double _z = pos[2] - matrix[3*4+2];
// Multiply inverse-translated source vector by inverted rotation transform
t[0] = (matrix[0*4+0] * _x) + (matrix[0*4+1] * _y) + (matrix[0*4+2] * _z);
t[1] = (matrix[1*4+0] * _x) + (matrix[1*4+1] * _y) + (matrix[1*4+2] * _z);
t[2] = (matrix[2*4+0] * _x) + (matrix[2*4+1] * _y) + (matrix[2*4+2] * _z);
}
void fm_identity(double *matrix) // set 4x4 matrix to identity.
{
matrix[0*4+0] = 1;
matrix[1*4+1] = 1;
matrix[2*4+2] = 1;
matrix[3*4+3] = 1;
matrix[1*4+0] = 0;
matrix[2*4+0] = 0;
matrix[3*4+0] = 0;
matrix[0*4+1] = 0;
matrix[2*4+1] = 0;
matrix[3*4+1] = 0;
matrix[0*4+2] = 0;
matrix[1*4+2] = 0;
matrix[3*4+2] = 0;
matrix[0*4+3] = 0;
matrix[1*4+3] = 0;
matrix[2*4+3] = 0;
}
void fm_eulerMatrix(double ax,double ay,double az,double *matrix) // convert euler (in radians) to a dest 4x4 matrix (translation set to zero)
{
double quat[4];
fm_eulerToQuat(ax,ay,az,quat);
fm_quatToMatrix(quat,matrix);
}
void fm_getAABB(unsigned int vcount,const double *points,unsigned int pstride,double *bmin,double *bmax)
{
const unsigned char *source = (const unsigned char *) points;
bmin[0] = points[0];
bmin[1] = points[1];
bmin[2] = points[2];
bmax[0] = points[0];
bmax[1] = points[1];
bmax[2] = points[2];
for (unsigned int i=1; i<vcount; i++)
{
source+=pstride;
const double *p = (const double *) source;
if ( p[0] < bmin[0] ) bmin[0] = p[0];
if ( p[1] < bmin[1] ) bmin[1] = p[1];
if ( p[2] < bmin[2] ) bmin[2] = p[2];
if ( p[0] > bmax[0] ) bmax[0] = p[0];
if ( p[1] > bmax[1] ) bmax[1] = p[1];
if ( p[2] > bmax[2] ) bmax[2] = p[2];
}
}
void fm_eulerToQuat(double roll,double pitch,double yaw,double *quat) // convert euler angles to quaternion.
{
roll *= 0.5f;
pitch *= 0.5f;
yaw *= 0.5f;
double cr = cos(roll);
double cp = cos(pitch);
double cy = cos(yaw);
double sr = sin(roll);
double sp = sin(pitch);
double sy = sin(yaw);
double cpcy = cp * cy;
double spsy = sp * sy;
double spcy = sp * cy;
double cpsy = cp * sy;
quat[0] = ( sr * cpcy - cr * spsy);
quat[1] = ( cr * spcy + sr * cpsy);
quat[2] = ( cr * cpsy - sr * spcy);
quat[3] = cr * cpcy + sr * spsy;
}
void fm_quatToMatrix(const double *quat,double *matrix) // convert quaterinion rotation to matrix, zeros out the translation component.
{
double xx = quat[0]*quat[0];
double yy = quat[1]*quat[1];
double zz = quat[2]*quat[2];
double xy = quat[0]*quat[1];
double xz = quat[0]*quat[2];
double yz = quat[1]*quat[2];
double wx = quat[3]*quat[0];
double wy = quat[3]*quat[1];
double wz = quat[3]*quat[2];
matrix[0*4+0] = 1 - 2 * ( yy + zz );
matrix[1*4+0] = 2 * ( xy - wz );
matrix[2*4+0] = 2 * ( xz + wy );
matrix[0*4+1] = 2 * ( xy + wz );
matrix[1*4+1] = 1 - 2 * ( xx + zz );
matrix[2*4+1] = 2 * ( yz - wx );
matrix[0*4+2] = 2 * ( xz - wy );
matrix[1*4+2] = 2 * ( yz + wx );
matrix[2*4+2] = 1 - 2 * ( xx + yy );
matrix[3*4+0] = 0.0f;
matrix[3*4+1] = 0.0f;
matrix[3*4+2] = 0.0f;
matrix[0*4+3] = 0.0f;
matrix[1*4+3] = 0.0f;
matrix[2*4+3] = 0.0f;
matrix[3*4+3] =(double) 1.0f;
}
void fm_quatRotate(const double *quat,const double *v,double *r) // rotate a vector directly by a quaternion.
{
double left[4];
left[0] = quat[3]*v[0] + quat[1]*v[2] - v[1]*quat[2];
left[1] = quat[3]*v[1] + quat[2]*v[0] - v[2]*quat[0];
left[2] = quat[3]*v[2] + quat[0]*v[1] - v[0]*quat[1];
left[3] = - quat[0]*v[0] - quat[1]*v[1] - quat[2]*v[2];
r[0] = (left[3]*-quat[0]) + (quat[3]*left[0]) + (left[1]*-quat[2]) - (-quat[1]*left[2]);
r[1] = (left[3]*-quat[1]) + (quat[3]*left[1]) + (left[2]*-quat[0]) - (-quat[2]*left[0]);
r[2] = (left[3]*-quat[2]) + (quat[3]*left[2]) + (left[0]*-quat[1]) - (-quat[0]*left[1]);
}
void fm_getTranslation(const double *matrix,double *t)
{
t[0] = matrix[3*4+0];
t[1] = matrix[3*4+1];
t[2] = matrix[3*4+2];
}
void fm_matrixToQuat(const double *matrix,double *quat) // convert the 3x3 portion of a 4x4 matrix into a quaterion as x,y,z,w
{
double tr = matrix[0*4+0] + matrix[1*4+1] + matrix[2*4+2];
// check the diagonal
if (tr > 0.0f )
{
double s = (double) sqrt ( (double) (tr + 1.0f) );
quat[3] = s * 0.5f;
s = 0.5f / s;
quat[0] = (matrix[1*4+2] - matrix[2*4+1]) * s;
quat[1] = (matrix[2*4+0] - matrix[0*4+2]) * s;
quat[2] = (matrix[0*4+1] - matrix[1*4+0]) * s;
}
else
{
// diagonal is negative
int nxt[3] = {1, 2, 0};
double qa[4];
int i = 0;
if (matrix[1*4+1] > matrix[0*4+0]) i = 1;
if (matrix[2*4+2] > matrix[i*4+i]) i = 2;
int j = nxt[i];
int k = nxt[j];
double s = sqrt ( ((matrix[i*4+i] - (matrix[j*4+j] + matrix[k*4+k])) + 1.0f) );
qa[i] = s * 0.5f;
if (s != 0.0f ) s = 0.5f / s;
qa[3] = (matrix[j*4+k] - matrix[k*4+j]) * s;
qa[j] = (matrix[i*4+j] + matrix[j*4+i]) * s;
qa[k] = (matrix[i*4+k] + matrix[k*4+i]) * s;
quat[0] = qa[0];
quat[1] = qa[1];
quat[2] = qa[2];
quat[3] = qa[3];
}
}
double fm_sphereVolume(double radius) // return's the volume of a sphere of this radius (4/3 PI * R cubed )
{
return (4.0f / 3.0f ) * FM_PI * radius * radius * radius;
}
double fm_cylinderVolume(double radius,double h)
{
return FM_PI * radius * radius *h;
}
double fm_capsuleVolume(double radius,double h)
{
double volume = fm_sphereVolume(radius); // volume of the sphere portion.
double ch = h-radius*2; // this is the cylinder length
if ( ch > 0 )
{
volume+=fm_cylinderVolume(radius,ch);
}
return volume;
}
void fm_transform(const double *matrix,const double *v,double *t) // rotate and translate this point
{
t[0] = (matrix[0*4+0] * v[0]) + (matrix[1*4+0] * v[1]) + (matrix[2*4+0] * v[2]) + matrix[3*4+0];
t[1] = (matrix[0*4+1] * v[0]) + (matrix[1*4+1] * v[1]) + (matrix[2*4+1] * v[2]) + matrix[3*4+1];
t[2] = (matrix[0*4+2] * v[0]) + (matrix[1*4+2] * v[1]) + (matrix[2*4+2] * v[2]) + matrix[3*4+2];
}
void fm_rotate(const double *matrix,const double *v,double *t) // rotate and translate this point
{
t[0] = (matrix[0*4+0] * v[0]) + (matrix[1*4+0] * v[1]) + (matrix[2*4+0] * v[2]);
t[1] = (matrix[0*4+1] * v[0]) + (matrix[1*4+1] * v[1]) + (matrix[2*4+1] * v[2]);
t[2] = (matrix[0*4+2] * v[0]) + (matrix[1*4+2] * v[1]) + (matrix[2*4+2] * v[2]);
}
double fm_distance(const double *p1,const double *p2)
{
double dx = p1[0] - p2[0];
double dy = p1[1] - p2[1];
double dz = p1[2] - p2[2];
return sqrt( dx*dx + dy*dy + dz *dz );
}
double fm_distanceSquared(const double *p1,const double *p2)
{
double dx = p1[0] - p2[0];
double dy = p1[1] - p2[1];
double dz = p1[2] - p2[2];
return dx*dx + dy*dy + dz *dz;
}
double fm_computePlane(const double *A,const double *B,const double *C,double *n) // returns D
{
double vx = (B[0] - C[0]);
double vy = (B[1] - C[1]);
double vz = (B[2] - C[2]);
double wx = (A[0] - B[0]);
double wy = (A[1] - B[1]);
double wz = (A[2] - B[2]);
double vw_x = vy * wz - vz * wy;
double vw_y = vz * wx - vx * wz;
double vw_z = vx * wy - vy * wx;
double mag = sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));
if ( mag < 0.000001f )
{
mag = 0;
}
else
{
mag = 1.0f/mag;
}
double x = vw_x * mag;
double y = vw_y * mag;
double z = vw_z * mag;
double D = 0.0f - ((x*A[0])+(y*A[1])+(z*A[2]));
n[0] = x;
n[1] = y;
n[2] = z;
return D;
}
double fm_distToPlane(const double *plane,const double *p) // computes the distance of this point from the plane.
{
return p[0]*plane[0]+p[1]*plane[1]+p[2]*plane[2]+plane[3];
}
double fm_dot(const double *p1,const double *p2)
{
return p1[0]*p2[0]+p1[1]*p2[2]+p1[2]*p2[2];
}
void fm_cross(double *cross,const double *a,const double *b)
{
cross[0] = a[1]*b[2] - a[2]*b[1];
cross[1] = a[2]*b[0] - a[0]*b[2];
cross[2] = a[0]*b[1] - a[1]*b[0];
}
void fm_computeNormalVector(double *n,const double *p1,const double *p2)
{
n[0] = p2[0] - p1[0];
n[1] = p2[1] - p1[1];
n[2] = p2[2] - p1[2];
fm_normalize(n);
}
bool fm_computeWindingOrder(const double *p1,const double *p2,const double *p3) // returns true if the triangle is clockwise.
{
bool ret = false;
double v1[3];
double v2[3];
fm_computeNormalVector(v1,p1,p2); // p2-p1 (as vector) and then normalized
fm_computeNormalVector(v2,p1,p3); // p3-p1 (as vector) and then normalized
double cross[3];
fm_cross(cross, v1, v2 );
double ref[3] = { 1, 0, 0 };
double d = fm_dot( cross, ref );
if ( d <= 0 )
ret = false;
else
ret = true;
return ret;
}
void fm_normalize(double *n) // normalize this vector
{
double dist = n[0]*n[0] + n[1]*n[1] + n[2]*n[2];
double mag = 0;
if ( dist > 0.0000001f )
mag = 1.0f / sqrt(dist);
n[0]*=mag;
n[1]*=mag;
n[2]*=mag;
}
}; // end of namespace

112
convex_decomposition/ConvexDecomposition/ConvexDecomposition/float_math.h Normal file
View File

@ -0,0 +1,112 @@
#ifndef FLOAT_MATH_H
#define FLOAT_MATH_H
namespace ConvexDecomposition
{
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
// a set of routines that let you do common 3d math
// operations without any vector, matrix, or quaternion
// classes or templates.
//
// a vector (or point) is a 'double *' to 3 doubleing point numbers.
// a matrix is a 'double *' to an array of 16 doubleing point numbers representing a 4x4 transformation matrix compatible with D3D or OGL
// a quaternion is a 'double *' to 4 doubles representing a quaternion x,y,z,w
//
//
//
// Please email bug fixes or improvements to John W. Ratcliff at mailto:jratcliff@infiniplex.net
//
// If you find this source code useful donate a couple of bucks to my kid's fund raising website at
// www.amillionpixels.us
//
// More snippets at: www.codesuppository.com
//
const double FM_PI = 3.141592654f;
const double FM_DEG_TO_RAD = ((2.0f * FM_PI) / 360.0f);
const double FM_RAD_TO_DEG = (360.0f / (2.0f * FM_PI));
void fm_identity(double *matrix); // set 4x4 matrix to identity.
void fm_inverseRT(const double *matrix,const double *pos,double *t); // inverse rotate translate the point.
void fm_transform(const double *matrix,const double *pos,double *t); // rotate and translate this point.
void fm_rotate(const double *matrix,const double *pos,double *t); // only rotate the point by a 4x4 matrix, don't translate.
void fm_eulerMatrix(double ax,double ay,double az,double *matrix); // convert euler (in radians) to a dest 4x4 matrix (translation set to zero)
void fm_getAABB(unsigned int vcount,const double *points,unsigned int pstride,double *bmin,double *bmax);
void fm_eulerToQuat(double roll,double pitch,double yaw,double *quat); // convert euler angles to quaternion.
void fm_quatToMatrix(const double *quat,double *matrix); // convert quaterinion rotation to matrix, translation set to zero.
void fm_quatRotate(const double *quat,const double *v,double *r); // rotate a vector directly by a quaternion.
void fm_getTranslation(const double *matrix,double *t);
void fm_matrixToQuat(const double *matrix,double *quat); // convert the 3x3 portion of a 4x4 matrix into a quaterion as x,y,z,w
double fm_sphereVolume(double radius); // return's the volume of a sphere of this radius (4/3 PI * R cubed )
double fm_cylinderVolume(double radius,double h);
double fm_capsuleVolume(double radius,double h);
double fm_distance(const double *p1,const double *p2);
double fm_distanceSquared(const double *p1,const double *p2);
double fm_computePlane(const double *p1,const double *p2,const double *p3,double *n); // return D
double fm_distToPlane(const double *plane,const double *pos); // computes the distance of this point from the plane.
double fm_dot(const double *p1,const double *p2);
void fm_cross(double *cross,const double *a,const double *b);
void fm_computeNormalVector(double *n,const double *p1,const double *p2); // as P2-P1 normalized.
bool fm_computeWindingOrder(const double *p1,const double *p2,const double *p3); // returns true if the triangle is clockwise.
void fm_normalize(double *n); // normalize this vector
}; // end of nsamepace
#endif

251
convex_decomposition/ConvexDecomposition/ConvexDecomposition/meshvolume.cpp Normal file
View File

@ -0,0 +1,251 @@
#include "meshvolume.h"
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
namespace ConvexDecomposition
{
inline double det(const double *p1,const double *p2,const double *p3)
{
return p1[0]*p2[1]*p3[2] + p2[0]*p3[1]*p1[2] + p3[0]*p1[1]*p2[2] -p1[0]*p3[1]*p2[2] - p2[0]*p1[1]*p3[2] - p3[0]*p2[1]*p1[2];
}
double computeMeshVolume(const double *vertices,unsigned int tcount,const unsigned int *indices)
{
double volume = 0;
const double *p0 = vertices;
for (unsigned int i=0; i<tcount; i++,indices+=3)
{
const double *p1 = &vertices[ indices[0]*3 ];
const double *p2 = &vertices[ indices[1]*3 ];
const double *p3 = &vertices[ indices[2]*3 ];
volume+=det(p1,p2,p3); // compute the volume of the tetrahedran relative to the origin.
}
volume*=(1.0f/6.0f);
if ( volume < 0 )
volume*=-1;
return volume;
}
inline void CrossProduct(const double *a,const double *b,double *cross)
{
cross[0] = a[1]*b[2] - a[2]*b[1];
cross[1] = a[2]*b[0] - a[0]*b[2];
cross[2] = a[0]*b[1] - a[1]*b[0];
}
inline double DotProduct(const double *a,const double *b)
{
return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}
inline double tetVolume(const double *p0,const double *p1,const double *p2,const double *p3)
{
double a[3];
double b[3];
double c[3];
a[0] = p1[0] - p0[0];
a[1] = p1[1] - p0[1];
a[2] = p1[2] - p0[2];
b[0] = p2[0] - p0[0];
b[1] = p2[1] - p0[1];
b[2] = p2[2] - p0[2];
c[0] = p3[0] - p0[0];
c[1] = p3[1] - p0[1];
c[2] = p3[2] - p0[2];
double cross[3];
CrossProduct( b, c, cross );
double volume = DotProduct( a, cross );
if ( volume < 0 )
return -volume;
return volume;
}
inline double det(const double *p0,const double *p1,const double *p2,const double *p3)
{
return p1[0]*p2[1]*p3[2] + p2[0]*p3[1]*p1[2] + p3[0]*p1[1]*p2[2] -p1[0]*p3[1]*p2[2] - p2[0]*p1[1]*p3[2] - p3[0]*p2[1]*p1[2];
}
double computeMeshVolume2(const double *vertices,unsigned int tcount,const unsigned int *indices)
{
double volume = 0;
const double *p0 = vertices;
for (unsigned int i=0; i<tcount; i++,indices+=3)
{
const double *p1 = &vertices[ indices[0]*3 ];
const double *p2 = &vertices[ indices[1]*3 ];
const double *p3 = &vertices[ indices[2]*3 ];
volume+=tetVolume(p0,p1,p2,p3); // compute the volume of the tetrahdren relative to the root vertice
}
return volume * (1.0f / 6.0f );
}
//** Float versions
inline float det(const float *p1,const float *p2,const float *p3)
{
return p1[0]*p2[1]*p3[2] + p2[0]*p3[1]*p1[2] + p3[0]*p1[1]*p2[2] -p1[0]*p3[1]*p2[2] - p2[0]*p1[1]*p3[2] - p3[0]*p2[1]*p1[2];
}
float computeMeshVolume(const float *vertices,unsigned int tcount,const unsigned int *indices)
{
float volume = 0;
const float *p0 = vertices;
for (unsigned int i=0; i<tcount; i++,indices+=3)
{
const float *p1 = &vertices[ indices[0]*3 ];
const float *p2 = &vertices[ indices[1]*3 ];
const float *p3 = &vertices[ indices[2]*3 ];
volume+=det(p1,p2,p3); // compute the volume of the tetrahedran relative to the origin.
}
volume*=(1.0f/6.0f);
if ( volume < 0 )
volume*=-1;
return volume;
}
inline void CrossProduct(const float *a,const float *b,float *cross)
{
cross[0] = a[1]*b[2] - a[2]*b[1];
cross[1] = a[2]*b[0] - a[0]*b[2];
cross[2] = a[0]*b[1] - a[1]*b[0];
}
inline float DotProduct(const float *a,const float *b)
{
return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}
inline float tetVolume(const float *p0,const float *p1,const float *p2,const float *p3)
{
float a[3];
float b[3];
float c[3];
a[0] = p1[0] - p0[0];
a[1] = p1[1] - p0[1];
a[2] = p1[2] - p0[2];
b[0] = p2[0] - p0[0];
b[1] = p2[1] - p0[1];
b[2] = p2[2] - p0[2];
c[0] = p3[0] - p0[0];
c[1] = p3[1] - p0[1];
c[2] = p3[2] - p0[2];
float cross[3];
CrossProduct( b, c, cross );
float volume = DotProduct( a, cross );
if ( volume < 0 )
return -volume;
return volume;
}
inline float det(const float *p0,const float *p1,const float *p2,const float *p3)
{
return p1[0]*p2[1]*p3[2] + p2[0]*p3[1]*p1[2] + p3[0]*p1[1]*p2[2] -p1[0]*p3[1]*p2[2] - p2[0]*p1[1]*p3[2] - p3[0]*p2[1]*p1[2];
}
float computeMeshVolume2(const float *vertices,unsigned int tcount,const unsigned int *indices)
{
float volume = 0;
const float *p0 = vertices;
for (unsigned int i=0; i<tcount; i++,indices+=3)
{
const float *p1 = &vertices[ indices[0]*3 ];
const float *p2 = &vertices[ indices[1]*3 ];
const float *p3 = &vertices[ indices[2]*3 ];
volume+=tetVolume(p0,p1,p2,p3); // compute the volume of the tetrahdren relative to the root vertice
}
return volume * (1.0f / 6.0f );
}
};

70
convex_decomposition/ConvexDecomposition/ConvexDecomposition/meshvolume.h Normal file
View File

@ -0,0 +1,70 @@
#ifndef MESH_VOLUME_H
#define MESH_VOLUME_H
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
namespace ConvexDecomposition
{
double computeMeshVolume(const double *vertices,unsigned int tcount,const unsigned int *indices);
double computeMeshVolume2(const double *vertices,unsigned int tcount,const unsigned int *indices);
float computeMeshVolume(const float *vertices,unsigned int tcount,const unsigned int *indices);
};
#endif

314
convex_decomposition/ConvexDecomposition/ConvexDecomposition/planetri.cpp Normal file
View File

@ -0,0 +1,314 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include "planetri.h"
namespace ConvexDecomposition
{
static inline double DistToPt(const double *p,const double *plane)
{
double x = p[0];
double y = p[1];
double z = p[2];
double d = x*plane[0] + y*plane[1] + z*plane[2] + plane[3];
return d;
}
static PlaneTriResult getSidePlane(const double *p,const double *plane,double epsilon)
{
double d = DistToPt(p,plane);
if ( (d+epsilon) > 0 )
return PTR_FRONT; // it is 'in front' within the provided epsilon value.
return PTR_BACK;
}
static void add(const double *p,double *dest,unsigned int tstride,unsigned int &pcount)
{
char *d = (char *) dest;
d = d + pcount*tstride;
dest = (double *) d;
dest[0] = p[0];
dest[1] = p[1];
dest[2] = p[2];
pcount++;
assert( pcount <= 4 );
}
// assumes that the points are on opposite sides of the plane!
static void intersect(const double *p1,const double *p2,double *split,const double *plane)
{
double dp1 = DistToPt(p1,plane);
double dp2 = DistToPt(p2,plane);
double dir[3];
dir[0] = p2[0] - p1[0];
dir[1] = p2[1] - p1[1];
dir[2] = p2[2] - p1[2];
double dot1 = dir[0]*plane[0] + dir[1]*plane[1] + dir[2]*plane[2];
double dot2 = dp1 - plane[3];
double t = -(plane[3] + dot2 ) / dot1;
split[0] = (dir[0]*t)+p1[0];
split[1] = (dir[1]*t)+p1[1];
split[2] = (dir[2]*t)+p1[2];
}
#define MAXPTS 256
class point
{
public:
void set(const double *p)
{
x = p[0];
y = p[1];
z = p[2];
}
double x;
double y;
double z;
};
class polygon
{
public:
polygon(void)
{
mVcount = 0;
}
polygon(const double *p1,const double *p2,const double *p3)
{
mVcount = 3;
mVertices[0].set(p1);
mVertices[1].set(p2);
mVertices[2].set(p3);
}
int NumVertices(void) const { return mVcount; };
const point& Vertex(int index)
{
if ( index < 0 ) index+=mVcount;
return mVertices[index];
};
void set(const point *pts,int count)
{
for (int i=0; i<count; i++)
{
mVertices[i] = pts[i];
}
mVcount = count;
}
int mVcount;
point mVertices[MAXPTS];
};
class plane
{
public:
plane(const double *p)
{
normal.x = p[0];
normal.y = p[1];
normal.z = p[2];
D = p[3];
}
double Classify_Point(const point &p)
{
return p.x*normal.x + p.y*normal.y + p.z*normal.z + D;
}
point normal;
double D;
};
void Split_Polygon(polygon *poly, plane *part, polygon &front, polygon &back)
{
int count = poly->NumVertices ();
int out_c = 0, in_c = 0;
point ptA, ptB,outpts[MAXPTS],inpts[MAXPTS];
double sideA, sideB;
ptA = poly->Vertex (count - 1);
sideA = part->Classify_Point (ptA);
for (int i = -1; ++i < count;)
{
ptB = poly->Vertex(i);
sideB = part->Classify_Point(ptB);
if (sideB > 0)
{
if (sideA < 0)
{
point v;
intersect(&ptB.x, &ptA.x, &v.x, &part->normal.x );
outpts[out_c++] = inpts[in_c++] = v;
}
outpts[out_c++] = ptB;
}
else if (sideB < 0)
{
if (sideA > 0)
{
point v;
intersect(&ptB.x, &ptA.x, &v.x, &part->normal.x );
outpts[out_c++] = inpts[in_c++] = v;
}
inpts[in_c++] = ptB;
}
else
outpts[out_c++] = inpts[in_c++] = ptB;
ptA = ptB;
sideA = sideB;
}
front.set(&outpts[0], out_c);
back.set(&inpts[0], in_c);
}
PlaneTriResult planeTriIntersection(const double *_plane, // the plane equation in Ax+By+Cz+D format
const double *triangle, // the source triangle.
unsigned int tstride, // stride in bytes of the input and output *vertices*
double epsilon, // the co-planer epsilon value.
double *front, // the triangle in front of the
unsigned int &fcount, // number of vertices in the 'front' triangle
double *back, // the triangle in back of the plane
unsigned int &bcount) // the number of vertices in the 'back' triangle.
{
fcount = 0;
bcount = 0;
const char *tsource = (const char *) triangle;
// get the three vertices of the triangle.
const double *p1 = (const double *) (tsource);
const double *p2 = (const double *) (tsource+tstride);
const double *p3 = (const double *) (tsource+tstride*2);
PlaneTriResult r1 = getSidePlane(p1,_plane,epsilon); // compute the side of the plane each vertex is on
PlaneTriResult r2 = getSidePlane(p2,_plane,epsilon);
PlaneTriResult r3 = getSidePlane(p3,_plane,epsilon);
if ( r1 == r2 && r1 == r3 ) // if all three vertices are on the same side of the plane.
{
if ( r1 == PTR_FRONT ) // if all three are in front of the plane, then copy to the 'front' output triangle.
{
add(p1,front,tstride,fcount);
add(p2,front,tstride,fcount);
add(p3,front,tstride,fcount);
}
else
{
add(p1,back,tstride,bcount); // if all three are in 'back' then copy to the 'back' output triangle.
add(p2,back,tstride,bcount);
add(p3,back,tstride,bcount);
}
return r1; // if all three points are on the same side of the plane return result
}
polygon pi(p1,p2,p3);
polygon pfront,pback;
plane part(_plane);
Split_Polygon(&pi,&part,pfront,pback);
for (int i=0; i<pfront.mVcount; i++)
{
add( &pfront.mVertices[i].x, front, tstride, fcount );
}
for (int i=0; i<pback.mVcount; i++)
{
add( &pback.mVertices[i].x, back, tstride, bcount );
}
PlaneTriResult ret = PTR_SPLIT;
if ( fcount == 0 && bcount )
ret = PTR_BACK;
if ( bcount == 0 && fcount )
ret = PTR_FRONT;
return ret;
}
};

82
convex_decomposition/ConvexDecomposition/ConvexDecomposition/planetri.h Normal file
View File

@ -0,0 +1,82 @@
#ifndef PLANE_TRI_H
#define PLANE_TRI_H
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
namespace ConvexDecomposition
{
enum PlaneTriResult
{
PTR_FRONT,
PTR_BACK,
PTR_SPLIT,
};
PlaneTriResult planeTriIntersection(const double *plane, // the plane equation in Ax+By+Cz+D format
const double *triangle, // the source position triangle.
unsigned int tstride, // stride in bytes between vertices of the triangle.
double epsilon, // the co-planer epsilon value.
double *front, // the triangle in front of the
unsigned int &fcount, // number of vertices in the 'front' triangle.
double *back, // the triangle in back of the plane
unsigned int &bcount); // the number of vertices in the 'back' triangle.
};
#endif

160
convex_decomposition/ConvexDecomposition/ConvexDecomposition/raytri.cpp Normal file
View File

@ -0,0 +1,160 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <math.h>
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include "raytri.h"
namespace ConvexDecomposition
{
/* a = b - c */
#define vector(a,b,c) \
(a)[0] = (b)[0] - (c)[0]; \
(a)[1] = (b)[1] - (c)[1]; \
(a)[2] = (b)[2] - (c)[2];
#define innerProduct(v,q) \
((v)[0] * (q)[0] + \
(v)[1] * (q)[1] + \
(v)[2] * (q)[2])
#define crossProduct(a,b,c) \
(a)[0] = (b)[1] * (c)[2] - (c)[1] * (b)[2]; \
(a)[1] = (b)[2] * (c)[0] - (c)[2] * (b)[0]; \
(a)[2] = (b)[0] * (c)[1] - (c)[0] * (b)[1];
bool rayIntersectsTriangle(const double *p,const double *d,const double *v0,const double *v1,const double *v2,double &t)
{
double e1[3],e2[3],h[3],s[3],q[3];
double a,f,u,v;
vector(e1,v1,v0);
vector(e2,v2,v0);
crossProduct(h,d,e2);
a = innerProduct(e1,h);
if (a > -0.00001 && a < 0.00001)
return(false);
f = 1/a;
vector(s,p,v0);
u = f * (innerProduct(s,h));
if (u < 0.0 || u > 1.0)
return(false);
crossProduct(q,s,e1);
v = f * innerProduct(d,q);
if (v < 0.0 || u + v > 1.0)
return(false);
// at this stage we can compute t to find out where
// the intersection point is on the line
t = f * innerProduct(e2,q);
if (t > 0) // ray intersection
return(true);
else // this means that there is a line intersection
// but not a ray intersection
return (false);
}
bool lineIntersectsTriangle(const double *rayStart,const double *rayEnd,const double *p1,const double *p2,const double *p3,double *sect)
{
double dir[3];
dir[0] = rayEnd[0] - rayStart[0];
dir[1] = rayEnd[1] - rayStart[1];
dir[2] = rayEnd[2] - rayStart[2];
double d = sqrt(dir[0]*dir[0] + dir[1]*dir[1] + dir[2]*dir[2]);
double r = 1.0f / d;
dir[0]*=r;
dir[1]*=r;
dir[2]*=r;
double t;
bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t );
if ( ret )
{
if ( t > d )
{
sect[0] = rayStart[0] + dir[0]*t;
sect[1] = rayStart[1] + dir[1]*t;
sect[2] = rayStart[2] + dir[2]*t;
}
else
{
ret = false;
}
}
return ret;
}
}; // end of namespace

69
convex_decomposition/ConvexDecomposition/ConvexDecomposition/raytri.h Normal file
View File

@ -0,0 +1,69 @@
#ifndef RAY_TRI_H
#define RAY_TRI_H
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
namespace ConvexDecomposition
{
// returns true if the ray intersects the triangle.
bool lineIntersectsTriangle(const double *rayStart,const double *rayEnd,const double *p1,const double *p2,const double *p3,double *sect);
bool rayIntersectsTriangle(const double *p,const double *d,const double *v0,const double *v1,const double *v2,double &t);
};
#endif

339
convex_decomposition/ConvexDecomposition/ConvexDecomposition/splitplane.cpp Normal file
View File

@ -0,0 +1,339 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <float.h>
#include <math.h>
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include "splitplane.h"
#include "ConvexDecomposition.h"
#include "cd_vector.h"
#include "cd_hull.h"
#include "cd_wavefront.h"
#include "bestfit.h"
#include "planetri.h"
#include "vlookup.h"
#include "meshvolume.h"
#include "bestfitobb.h"
#include "float_math.h"
namespace ConvexDecomposition
{
static void computePlane(const double *A,const double *B,const double *C,double *plane)
{
double vx = (B[0] - C[0]);
double vy = (B[1] - C[1]);
double vz = (B[2] - C[2]);
double wx = (A[0] - B[0]);
double wy = (A[1] - B[1]);
double wz = (A[2] - B[2]);
double vw_x = vy * wz - vz * wy;
double vw_y = vz * wx - vx * wz;
double vw_z = vx * wy - vy * wx;
double mag = sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));
if ( mag < 0.000001f )
{
mag = 0;
}
else
{
mag = 1.0f/mag;
}
double x = vw_x * mag;
double y = vw_y * mag;
double z = vw_z * mag;
double D = 0.0f - ((x*A[0])+(y*A[1])+(z*A[2]));
plane[0] = x;
plane[1] = y;
plane[2] = z;
plane[3] = D;
}
class Rect3d
{
public:
Rect3d(void) { };
Rect3d(const double *bmin,const double *bmax)
{
mMin[0] = bmin[0];
mMin[1] = bmin[1];
mMin[2] = bmin[2];
mMax[0] = bmax[0];
mMax[1] = bmax[1];
mMax[2] = bmax[2];
}
void SetMin(const double *bmin)
{
mMin[0] = bmin[0];
mMin[1] = bmin[1];
mMin[2] = bmin[2];
}
void SetMax(const double *bmax)
{
mMax[0] = bmax[0];
mMax[1] = bmax[1];
mMax[2] = bmax[2];
}
void SetMin(double x,double y,double z)
{
mMin[0] = x;
mMin[1] = y;
mMin[2] = z;
}
void SetMax(double x,double y,double z)
{
mMax[0] = x;
mMax[1] = y;
mMax[2] = z;
}
double mMin[3];
double mMax[3];
};
void splitRect(unsigned int axis,
const Rect3d &source,
Rect3d &b1,
Rect3d &b2,
const double *midpoint)
{
switch ( axis )
{
case 0:
b1.SetMin(source.mMin);
b1.SetMax( midpoint[0], source.mMax[1], source.mMax[2] );
b2.SetMin( midpoint[0], source.mMin[1], source.mMin[2] );
b2.SetMax(source.mMax);
break;
case 1:
b1.SetMin(source.mMin);
b1.SetMax( source.mMax[0], midpoint[1], source.mMax[2] );
b2.SetMin( source.mMin[0], midpoint[1], source.mMin[2] );
b2.SetMax(source.mMax);
break;
case 2:
b1.SetMin(source.mMin);
b1.SetMax( source.mMax[0], source.mMax[1], midpoint[2] );
b2.SetMin( source.mMin[0], source.mMin[1], midpoint[2] );
b2.SetMax(source.mMax);
break;
}
}
bool computeSplitPlane(unsigned int vcount,
const double *vertices,
unsigned int tcount,
const unsigned int *indices,
ConvexDecompInterface *callback,
double *plane)
{
bool cret = false;
double sides[3];
double matrix[16];
computeBestFitOBB( vcount, vertices, sizeof(double)*3, sides, matrix );
double bmax[3];
double bmin[3];
bmax[0] = sides[0]*0.5f;
bmax[1] = sides[1]*0.5f;
bmax[2] = sides[2]*0.5f;
bmin[0] = -bmax[0];
bmin[1] = -bmax[1];
bmin[2] = -bmax[2];
double dx = sides[0];
double dy = sides[1];
double dz = sides[2];
double laxis = dx;
unsigned int axis = 0;
if ( dy > dx )
{
axis = 1;
laxis = dy;
}
if ( dz > dx && dz > dy )
{
axis = 2;
laxis = dz;
}
double p1[3];
double p2[3];
double p3[3];
p3[0] = p2[0] = p1[0] = bmin[0] + dx*0.5f;
p3[1] = p2[1] = p1[1] = bmin[1] + dy*0.5f;
p3[2] = p2[2] = p1[2] = bmin[2] + dz*0.5f;
Rect3d b(bmin,bmax);
Rect3d b1,b2;
splitRect(axis,b,b1,b2,p1);
// callback->ConvexDebugBound(b1.mMin,b1.mMax,0x00FF00);
// callback->ConvexDebugBound(b2.mMin,b2.mMax,0xFFFF00);
switch ( axis )
{
case 0:
p2[1] = bmin[1];
p2[2] = bmin[2];
if ( dz > dy )
{
p3[1] = bmax[1];
p3[2] = bmin[2];
}
else
{
p3[1] = bmin[1];
p3[2] = bmax[2];
}
break;
case 1:
p2[0] = bmin[0];
p2[2] = bmin[2];
if ( dx > dz )
{
p3[0] = bmax[0];
p3[2] = bmin[2];
}
else
{
p3[0] = bmin[0];
p3[2] = bmax[2];
}
break;
case 2:
p2[0] = bmin[0];
p2[1] = bmin[1];
if ( dx > dy )
{
p3[0] = bmax[0];
p3[1] = bmin[1];
}
else
{
p3[0] = bmin[0];
p3[1] = bmax[1];
}
break;
}
double tp1[3];
double tp2[3];
double tp3[3];
fm_transform(matrix,p1,tp1);
fm_transform(matrix,p2,tp2);
fm_transform(matrix,p3,tp3);
// callback->ConvexDebugTri(p1,p2,p3,0xFF0000);
computePlane(tp1,tp2,tp3,plane);
return true;
}
};

76
convex_decomposition/ConvexDecomposition/ConvexDecomposition/splitplane.h Normal file
View File

@ -0,0 +1,76 @@
#ifndef SPLIT_PLANE_H
#define SPLIT_PLANE_H
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
namespace ConvexDecomposition
{
class ConvexDecompInterface;
bool computeSplitPlane(unsigned int vcount,
const double *vertices,
unsigned int tcount,
const unsigned int *indices,
ConvexDecompInterface *callback,
double *plane);
};
#endif

410
convex_decomposition/ConvexDecomposition/ConvexDecomposition/triangulate.cpp Normal file
View File

@ -0,0 +1,410 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <math.h>
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include <vector> // Include STL vector class.
#include "triangulate.h"
namespace ConvexDecomposition
{
class Vec2d
{
public:
Vec2d(const double *v)
{
mX = v[0];
mY = v[1];
}
Vec2d(double x,double y)
{
Set(x,y);
};
double GetX(void) const { return mX; };
double GetY(void) const { return mY; };
void Set(double x,double y)
{
mX = x;
mY = y;
};
private:
double mX;
double mY;
};// Typedef an STL vector of vertices which are used to represent
// a polygon/contour and a series of triangles.
typedef std::vector< Vec2d > Vec2dVector;
static bool Process(const Vec2dVector &contour,Vec2dVector &result); // compute area of a contour/polygon
static double Area(const Vec2dVector &contour); // decide if point Px/Py is inside triangle defined by (Ax,Ay) (Bx,By) (Cx,Cy)
static bool InsideTriangle(double Ax, double Ay,double Bx, double By,double Cx, double Cy,double Px, double Py);
static bool Snip(const Vec2dVector &contour,int u,int v,int w,int n,int *V);
static const double EPSILON=0.0000000001f;
double Area(const Vec2dVector &contour)
{
int n = contour.size();
double A=0.0f;
for(int p=n-1,q=0; q<n; p=q++)
{
A+= contour[p].GetX()*contour[q].GetY() - contour[q].GetX()*contour[p].GetY();
}
return A*0.5f;
}
/*
InsideTriangle decides if a point P is Inside of the triangle
defined by A, B, C.
*/
bool InsideTriangle(double Ax, double Ay,double Bx, double By,double Cx, double Cy,double Px, double Py)
{
double ax, ay, bx, by, cx, cy, apx, apy, bpx, bpy, cpx, cpy;
double cCROSSap, bCROSScp, aCROSSbp; ax = Cx - Bx; ay = Cy - By;
bx = Ax - Cx; by = Ay - Cy;
cx = Bx - Ax; cy = By - Ay;
apx= Px - Ax; apy= Py - Ay;
bpx= Px - Bx; bpy= Py - By;
cpx= Px - Cx; cpy= Py - Cy; aCROSSbp = ax*bpy - ay*bpx;
cCROSSap = cx*apy - cy*apx;
bCROSScp = bx*cpy - by*cpx; return ((aCROSSbp >= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f));
};
bool Snip(const Vec2dVector &contour,int u,int v,int w,int n,int *V)
{
int p;
double Ax, Ay, Bx, By, Cx, Cy, Px, Py;
Ax = contour[V[u]].GetX();
Ay = contour[V[u]].GetY();
Bx = contour[V[v]].GetX();
By = contour[V[v]].GetY();
Cx = contour[V[w]].GetX();
Cy = contour[V[w]].GetY();
if ( EPSILON > (((Bx-Ax)*(Cy-Ay)) - ((By-Ay)*(Cx-Ax))) ) return false; for (p=0;p<n;p++)
{
if( (p == u) || (p == v) || (p == w) ) continue;
Px = contour[V[p]].GetX();
Py = contour[V[p]].GetY();
if (InsideTriangle(Ax,Ay,Bx,By,Cx,Cy,Px,Py)) return false;
} return true;
}
bool Process(const Vec2dVector &contour,Vec2dVector &result)
{
/* allocate and initialize list of Vertices in polygon */
int n = contour.size();
if ( n < 3 ) return false; int *V = new int[n]; /* we want a counter-clockwise polygon in V */ if ( 0.0f < Area(contour) )
for (int v=0; v<n; v++) V[v] = v;
else
for(int v=0; v<n; v++) V[v] = (n-1)-v; int nv = n; /* remove nv-2 Vertices, creating 1 triangle every time */
int count = 2*nv; /* error detection */
for(int m=0, v=nv-1; nv>2; )
{
/* if we loop, it is probably a non-simple polygon */
if (0 >= (count--))
{
//** Triangulate: ERROR - probable bad polygon!
return false;
} /* three consecutive vertices in current polygon, <u,v,w> */
int u = v ;
if (nv <= u) u = 0; /* previous */
v = u+1; if (nv <= v) v = 0; /* new v */
int w = v+1;
if (nv <= w) w = 0; /* next */
if ( Snip(contour,u,v,w,nv,V) )
{
int a,b,c,s,t; /* true names of the vertices */
a = V[u];
b = V[v];
c = V[w]; /* output Triangle */
result.push_back( contour[a] );
result.push_back( contour[b] );
result.push_back( contour[c] );
m++; /* remove v from remaining polygon */
for(s=v,t=v+1;t<nv;s++,t++) V[s] = V[t]; nv--; /* resest error detection counter */
count = 2*nv;
}
}
delete V;
return true;
}
unsigned int triangulate3d(unsigned int pcount, // number of points in the polygon
const double *vertices, // array of 3d vertices.
double *triangles, // memory to store output triangles
unsigned int maxTri, // maximum triangles we are allowed to output.
const double *plane)
{
unsigned int ret = 0;
FILE *fph = fopen("debug.obj", "wb");
if ( fph )
{
fprintf(fph,"v 10 10 0\r\n");
for (unsigned int i=0; i<pcount; i++)
{
fprintf(fph,"v %f %f %f\r\n", vertices[i*3+0], vertices[i*3+1], vertices[i*3+2]);
}
for (unsigned int i=0; i<pcount; i++)
{
unsigned int next = i+1;
if ( next == pcount ) next = 0;
fprintf(fph,"f %d %d %d\r\n", i+2, 1, next+2 );
}
fclose(fph);
}
if ( pcount >= 3 )
{
double normal[3];
normal[0] = plane[0];
normal[1] = plane[1];
normal[2] = plane[2];
double D = plane[3];
unsigned int i0 = 0;
unsigned int i1 = 1;
unsigned int i2 = 2;
unsigned int axis = 0;
// find the dominant axis.
double dx = fabs(normal[0]);
double dy = fabs(normal[1]);
double dz = fabs(normal[2]);
if ( dx > dy && dx > dz )
{
axis = 0;
i0 = 1;
i1 = 2;
i2 = 0;
}
else if ( dy > dx && dy > dz )
{
i0 = 0;
i1 = 2;
i2 = 1;
axis = 1;
}
else if ( dz > dx && dz > dy )
{
i0 = 0;
i1 = 1;
i2 = 2;
axis = 2;
}
double *ptemp = new double[pcount*2];
double *ptri = new double[maxTri*2*3];
const double *source = vertices;
double *dest = ptemp;
for (unsigned int i=0; i<pcount; i++)
{
dest[0] = source[i0];
dest[1] = source[i1];
dest+=2;
source+=3;
}
ret = triangulate2d(pcount, ptemp, ptri, maxTri );
// ok..now we have to copy it back and project the 3d component.
if ( ret )
{
const double *source = ptri;
double *dest = triangles;
double inverseZ = -1.0f / normal[i2];
for (unsigned int i=0; i<ret*3; i++)
{
dest[i0] = source[0];
dest[i1] = source[1];
dest[i2] = (normal[i0]*source[0] + normal[i1]*source[1] + D ) * inverseZ; // solve for projected component
dest+=3;
source+=2;
}
if ( 1 )
{
FILE *fph = fopen("test.obj","wb");
const double *source = triangles;
for (unsigned int i=0; i<ret*3; i++)
{
fprintf(fph,"v %f %f %f\r\n", source[0], source[1], source[2] );
source+=3;
}
int index = 1;
for (unsigned int i=0; i<ret; i++)
{
fprintf(fph,"f %d %d %d\r\n", index, index+1, index+2 );
index+=3;
}
fclose(fph);
}
}
delete ptri;
delete ptemp;
}
return ret;
}
unsigned int triangulate3d(unsigned int pcount, // number of points in the polygon
const unsigned int *indices, // polygon points using indices
const double *vertices, // base address for array indexing
double *triangles, // buffer to store output 3d triangles.
unsigned int maxTri, // maximum triangles we can output.
const double *plane)
{
unsigned int ret = 0;
if ( pcount )
{
// copy the indexed polygon out as a flat array of vertices.
double *ptemp = new double[pcount*3];
double *dest = ptemp;
for (unsigned int i=0; i<pcount; i++)
{
unsigned int index = indices[i];
const double *source = &vertices[index*3];
*dest++ = *source++;
*dest++ = *source++;
*dest++ = *source++;
}
ret = triangulate3d(pcount,ptemp,triangles,maxTri,plane);
delete ptemp;
}
return ret;
}
unsigned int triangulate2d(unsigned int pcount, // number of points in the polygon
const double *vertices, // address of input points (2d)
double *triangles, // destination buffer for output triangles.
unsigned int maxTri) // maximum number of triangles we can store.
{
unsigned int ret = 0;
const double *source = vertices;
Vec2dVector vlist;
for (unsigned int i=0; i<pcount; i++)
{
Vec2d v(source);
vlist.push_back(v);
source+=2;
}
Vec2dVector result;
bool ok = Process(vlist,result);
if ( ok )
{
ret = result.size()/3;
if ( ret < maxTri )
{
double *dest = triangles;
for (unsigned int i=0; i<ret*3; i++)
{
dest[0] = result[i].GetX();
dest[1] = result[i].GetY();
dest+=2;
}
}
else
{
ret = 0;
}
}
return ret;
}
}; // end of namespace

85
convex_decomposition/ConvexDecomposition/ConvexDecomposition/triangulate.h Normal file
View File

@ -0,0 +1,85 @@
#ifndef TRIANGULATE_H
#define TRIANGULATE_H
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
namespace ConvexDecomposition
{
// all 3d triangles should be co-planer. Doesn't bother to check, you should have done that yourself to begin with!
unsigned int triangulate3d(unsigned int pcount, // number of points in the polygon
const double *vertices, // array of 3d vertices.
double *triangles, // memory to store output triangles
unsigned int maxTri,
const double *plane); // maximum triangles we are allowed to output.
unsigned int triangulate3d(unsigned int pcount, // number of points in the polygon
const unsigned int *indices, // polygon points using indices
const double *vertices, // base address for array indexing
double *triangles, // buffer to store output 3d triangles.
unsigned int maxTri,
const double *plane); // maximum triangles we can output.
unsigned int triangulate2d(unsigned int pcount, // number of points in the polygon
const double *vertices, // address of input points (2d)
double *triangles, // destination buffer for output triangles.
unsigned int maxTri); // maximum number of triangles we can store.
};
#endif

340
convex_decomposition/ConvexDecomposition/ConvexDecomposition/vlookup.cpp Normal file
View File

@ -0,0 +1,340 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#pragma warning(disable:4786)
#include <vector>
#include <map>
#include <set>
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
// CodeSnippet provided by John W. Ratcliff
// on March 23, 2006.
//
// mailto: jratcliff@infiniplex.net
//
// Personal website: http://jratcliffscarab.blogspot.com
// Coding Website: http://codesuppository.blogspot.com
// FundRaising Blog: http://amillionpixels.blogspot.com
// Fundraising site: http://www.amillionpixels.us
// New Temple Site: http://newtemple.blogspot.com
//
// This snippet shows how to 'hide' the complexity of
// the STL by wrapping some useful piece of functionality
// around a handful of discrete API calls.
//
// This API allows you to create an indexed triangle list
// from a collection of raw input triangles. Internally
// it uses an STL set to build the lookup table very rapidly.
//
// Here is how you would use it to build an indexed triangle
// list from a raw list of triangles.
//
// (1) create a 'VertexLookup' interface by calling
//
// VertexLook vl = Vl_createVertexLookup();
//
// (2) For each vertice in each triangle call:
//
// unsigned int i1 = Vl_getIndex(vl,p1);
// unsigned int i2 = Vl_getIndex(vl,p2);
// unsigned int i3 = Vl_getIndex(vl,p3);
//
// save the 3 indices into your triangle list array.
//
// (3) Get the vertex array by calling:
//
// const double *vertices = Vl_getVertices(vl);
//
// (4) Get the number of vertices so you can copy them into
// your own buffer.
// unsigned int vcount = Vl_getVcount(vl);
//
// (5) Release the VertexLookup interface when you are done with it.
// Vl_releaseVertexLookup(vl);
//
// Teaches the following lessons:
//
// How to wrap the complexity of STL and C++ classes around a
// simple API interface.
//
// How to use an STL set and custom comparator operator for
// a complex data type.
//
// How to create a template class.
//
// How to achieve significant performance improvements by
// taking advantage of built in STL containers in just
// a few lines of code.
//
// You could easily modify this code to support other vertex
// formats with any number of interpolants.
#include "vlookup.h"
namespace ConvexDecomposition
{
class VertexPosition
{
public:
VertexPosition(void) { };
VertexPosition(const double *p)
{
mPos[0] = p[0];
mPos[1] = p[1];
mPos[2] = p[2];
};
void Set(int index,const double *pos)
{
const double * p = &pos[index*3];
mPos[0] = p[0];
mPos[1] = p[1];
mPos[2] = p[2];
};
double GetX(void) const { return mPos[0]; };
double GetY(void) const { return mPos[1]; };
double GetZ(void) const { return mPos[2]; };
double mPos[3];
};
template <typename Type> class VertexLess
{
public:
typedef std::vector< Type > VertexVector;
bool operator()(int v1,int v2) const;
static void SetSearch(const Type& match,VertexVector *list)
{
mFind = match;
mList = list;
};
private:
const Type& Get(int index) const
{
if ( index == -1 ) return mFind;
VertexVector &vlist = *mList;
return vlist[index];
}
static Type mFind; // vertice to locate.
static VertexVector *mList;
};
template <typename Type> class VertexPool
{
public:
typedef std::set<int, VertexLess<Type> > VertexSet;
typedef std::vector< Type > VertexVector;
int GetVertex(const Type& vtx)
{
VertexLess<Type>::SetSearch(vtx,&mVtxs);
typename VertexSet::iterator found;
found = mVertSet.find( -1 );
if ( found != mVertSet.end() )
{
return *found;
}
int idx = (int)mVtxs.size();
mVtxs.push_back( vtx );
mVertSet.insert( idx );
return idx;
};
const double * GetPos(int idx) const
{
return mVtxs[idx].mPos;
}
const Type& Get(int idx) const
{
return mVtxs[idx];
};
unsigned int GetSize(void) const
{
return mVtxs.size();
};
void Clear(int reservesize) // clear the vertice pool.
{
mVertSet.clear();
mVtxs.clear();
mVtxs.reserve(reservesize);
};
const VertexVector& GetVertexList(void) const { return mVtxs; };
void Set(const Type& vtx)
{
mVtxs.push_back(vtx);
}
unsigned int GetVertexCount(void) const
{
return mVtxs.size();
};
Type * GetBuffer(void)
{
return &mVtxs[0];
};
private:
VertexSet mVertSet; // ordered list.
VertexVector mVtxs; // set of vertices.
};
double tmpp[3] = {0,0,0};
template<> VertexPosition VertexLess<VertexPosition>::mFind = tmpp;
template<> std::vector<VertexPosition > *VertexLess<VertexPosition>::mList =0;
enum RDIFF
{
RD_EQUAL,
RD_LESS,
RD_GREATER
};
static RDIFF relativeDiff(const double *a,const double *b,double magnitude)
{
RDIFF ret = RD_EQUAL;
double m2 = magnitude*magnitude;
double dx = a[0]-b[0];
double dy = a[1]-b[1];
double dz = a[2]-b[2];
double d2 = (dx*dx)+(dy*dy)+(dz*dz);
if ( d2 > m2 )
{
if ( a[0] < b[0] ) ret = RD_LESS;
else if ( a[0] > b[0] ) ret = RD_GREATER;
else if ( a[1] < b[1] ) ret = RD_LESS;
else if ( a[1] > b[1] ) ret = RD_GREATER;
else if ( a[2] < b[2] ) ret = RD_LESS;
else if ( a[2] > b[2] ) ret = RD_GREATER;
}
return ret;
}
template<>
bool VertexLess<VertexPosition>::operator()(int v1,int v2) const
{
bool ret = false;
const VertexPosition& a = Get(v1);
const VertexPosition& b = Get(v2);
RDIFF d = relativeDiff(a.mPos,b.mPos,0.0001f);
if ( d == RD_LESS ) ret = true;
return ret;
};
VertexLookup Vl_createVertexLookup(void)
{
VertexLookup ret = new VertexPool< VertexPosition >;
return ret;
}
void Vl_releaseVertexLookup(VertexLookup vlook)
{
VertexPool< VertexPosition > *vp = (VertexPool< VertexPosition > *) vlook;
delete vp;
}
unsigned int Vl_getIndex(VertexLookup vlook,const double *pos) // get index.
{
VertexPool< VertexPosition > *vp = (VertexPool< VertexPosition > *) vlook;
VertexPosition p(pos);
return vp->GetVertex(p);
}
const double * Vl_getVertices(VertexLookup vlook)
{
VertexPool< VertexPosition > *vp = (VertexPool< VertexPosition > *) vlook;
return vp->GetPos(0);
}
unsigned int Vl_getVcount(VertexLookup vlook)
{
VertexPool< VertexPosition > *vp = (VertexPool< VertexPosition > *) vlook;
return vp->GetVertexCount();
}
}; // end of namespace

143
convex_decomposition/ConvexDecomposition/ConvexDecomposition/vlookup.h Normal file
View File

@ -0,0 +1,143 @@
#ifndef VLOOKUP_H
#define VLOOKUP_H
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
// CodeSnippet provided by John W. Ratcliff
// on March 23, 2006.
//
// mailto: jratcliff@infiniplex.net
//
// Personal website: http://jratcliffscarab.blogspot.com
// Coding Website: http://codesuppository.blogspot.com
// FundRaising Blog: http://amillionpixels.blogspot.com
// Fundraising site: http://www.amillionpixels.us
// New Temple Site: http://newtemple.blogspot.com
//
// This snippet shows how to 'hide' the complexity of
// the STL by wrapping some useful piece of functionality
// around a handful of discrete API calls.
//
// This API allows you to create an indexed triangle list
// from a collection of raw input triangles. Internally
// it uses an STL set to build the lookup table very rapidly.
//
// Here is how you would use it to build an indexed triangle
// list from a raw list of triangles.
//
// (1) create a 'VertexLookup' interface by calling
//
// VertexLook vl = Vl_createVertexLookup();
//
// (2) For each vertice in each triangle call:
//
// unsigned int i1 = Vl_getIndex(vl,p1);
// unsigned int i2 = Vl_getIndex(vl,p2);
// unsigned int i3 = Vl_getIndex(vl,p3);
//
// save the 3 indices into your triangle list array.
//
// (3) Get the vertex array by calling:
//
// const double *vertices = Vl_getVertices(vl);
//
// (4) Get the number of vertices so you can copy them into
// your own buffer.
// unsigned int vcount = Vl_getVcount(vl);
//
// (5) Release the VertexLookup interface when you are done with it.
// Vl_releaseVertexLookup(vl);
//
// Teaches the following lessons:
//
// How to wrap the complexity of STL and C++ classes around a
// simple API interface.
//
// How to use an STL set and custom comparator operator for
// a complex data type.
//
// How to create a template class.
//
// How to achieve significant performance improvements by
// taking advantage of built in STL containers in just
// a few lines of code.
//
// You could easily modify this code to support other vertex
// formats with any number of interpolants.
//
// Hide C++ classes from the rest of your application by
// keeping them in the CPP and wrapping them in a namespace
// Uses an STL set to create an index table for a bunch of vertex positions
// used typically to re-index a collection of raw triangle data.
namespace ConvexDecomposition
{
typedef void * VertexLookup;
VertexLookup Vl_createVertexLookup(void);
void Vl_releaseVertexLookup(VertexLookup vlook);
unsigned int Vl_getIndex(VertexLookup vlook,const double *pos); // get index.
const double * Vl_getVertices(VertexLookup vlook);
unsigned int Vl_getVcount(VertexLookup vlook);
};
#endif

668
convex_decomposition/ConvexDecomposition/DecomposeSample.cpp Normal file
View File

@ -0,0 +1,668 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <float.h>
#include <vector>
#include "./ConvexDecomposition/ConvexDecomposition.h"
#include "./ConvexDecomposition/cd_wavefront.h"
using namespace ConvexDecomposition;
// Test application to demonstrate how to use the ConvexDecomposition system.
typedef std::vector< ConvexResult * > ConvexResultVector;
static const char * fstring(float v)
{
static char data[64*16];
static int index=0;
char *ret = &data[index*64];
index++;
if (index == 16 ) index = 0;
if ( v == FLT_MIN ) return "-INF"; // collada notation for FLT_MIN and FLT_MAX
if ( v == FLT_MAX ) return "INF";
if ( v == 1 )
{
strcpy(ret,"1");
}
else if ( v == 0 )
{
strcpy(ret,"0");
}
else if ( v == - 1 )
{
strcpy(ret,"-1");
}
else
{
sprintf(ret,"%.9f", v );
const char *dot = strstr(ret,".");
if ( dot )
{
int len = (int)strlen(ret);
char *foo = &ret[len-1];
while ( *foo == '0' ) foo--;
if ( *foo == '.' )
*foo = 0;
else
foo[1] = 0;
}
}
return ret;
}
class CBuilder : public ConvexDecompInterface
{
public:
CBuilder(const char *fname,const DecompDesc &d)
{
strcpy(mBaseName,fname);
char *dot = strstr(mBaseName,".");
if ( dot ) *dot = 0;
sprintf(mObjName,"%s_convex.obj", mBaseName );
mBaseCount = 0;
mHullCount = 0;
mSkinWidth = (float)d.mSkinWidth;
mFph = fopen(mObjName,"wb");
printf("########################################################################\r\n");
printf("# Perfomring approximate convex decomposition for triangle mesh %s\r\n", fname );
printf("# Input Mesh has %d vertices and %d triangles.\r\n", d.mVcount, d.mTcount );
printf("# Recursion depth: %d\r\n", d.mDepth );
printf("# Concavity Threshold Percentage: %0.2f\r\n", d.mCpercent );
printf("# Hull Merge Volume Percentage: %0.2f\r\n", d.mPpercent );
printf("# Maximum output vertices per hull: %d\r\n", d.mMaxVertices );
printf("# Hull Skin Width: %0.2f\r\n", d.mSkinWidth );
printf("########################################################################\r\n");
if ( mFph )
{
fprintf(mFph,"########################################################################\r\n");
fprintf(mFph,"# Approximate convex decomposition for triangle mesh %s\r\n", fname );
fprintf(mFph,"# Input Mesh has %d vertices and %d triangles.\r\n", d.mVcount, d.mTcount );
fprintf(mFph,"# Recursion depth: %d\r\n", d.mDepth );
fprintf(mFph,"# Concavity Threshold Percentage: %0.2f\r\n", d.mCpercent );
fprintf(mFph,"# Hull Merge Volume Percentage: %0.2f\r\n", d.mPpercent );
fprintf(mFph,"# Maximum output vertices per hull: %d\r\n", d.mMaxVertices );
fprintf(mFph,"# Hull Skin Width: %0.2f\r\n", d.mSkinWidth );
fprintf(mFph,"########################################################################\r\n");
printf("Opened Wavefront file %s for output.\r\n", mObjName );
}
else
{
printf("Failed to open output file %s\r\n", mObjName );
}
}
~CBuilder(void)
{
if ( mFph )
{
fclose(mFph);
}
for (unsigned int i=0; i<mHulls.size(); i++)
{
ConvexResult *cr = mHulls[i];
delete cr;
}
}
virtual void ConvexDecompResult(ConvexResult &result)
{
// we have a hull...
mHullCount++;
printf("Received hull %d HullVolume(%0.3f)\r\n", mHullCount, result.mHullVolume );
if ( mFph )
{
ConvexResult *cr = new ConvexResult(result);
mHulls.push_back(cr);
fprintf(mFph,"########################################################################\r\n");
fprintf(mFph,"## Hull Piece %d with %d vertices and %d triangles.\r\n", mHullCount, result.mHullVcount, result.mHullTcount );
fprintf(mFph,"########################################################################\r\n");
for (unsigned int i=0; i<result.mHullVcount; i++)
{
const double *p = &result.mHullVertices[i*3];
fprintf(mFph,"v %0.9f %0.9f %0.9f\r\n", (float)p[0], (float)p[1], (float)p[2] );
}
if ( 1 )
{
const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullTcount; i++)
{
unsigned int i1 = *src++;
unsigned int i2 = *src++;
unsigned int i3 = *src++;
i1+=mBaseCount;
i2+=mBaseCount;
i3+=mBaseCount;
fprintf(mFph,"f %d %d %d\r\n", i1+1, i2+1, i3+1 );
}
}
mBaseCount+=result.mHullVcount; // advance the 'base index' counter.
}
}
void saveCOLLADA(FILE *fph,unsigned int index,ConvexResult *cr)
{
fprintf(fph," <geometry id=\"%s_%d-Mesh\" name=\"%s_%d-Mesh\">\r\n", mBaseName, index, mBaseName, index);
fprintf(fph," <convex_mesh>\r\n");
fprintf(fph," <source id=\"%s_%d-Position\">\r\n", mBaseName, index);
fprintf(fph," <float_array count=\"%d\" id=\"%s_%d-Position-array\">\r\n", cr->mHullVcount*3, mBaseName, index);
fprintf(fph," ");
for (unsigned int i=0; i<cr->mHullVcount; i++)
{
const double *p = &cr->mHullVertices[i*3];
fprintf(fph,"%s %s %s ", fstring((float)p[0]), fstring((float)p[1]), fstring((float)p[2]) );
if ( ((i+1)&3) == 0 && (i+1) < cr->mHullVcount )
{
fprintf(fph,"\r\n");
fprintf(fph," ");
}
}
fprintf(fph,"\r\n");
fprintf(fph," </float_array>\r\n");
fprintf(fph," <technique_common>\r\n");
fprintf(fph," <accessor count=\"%d\" source=\"#%s_%d-Position-array\" stride=\"3\">\r\n", cr->mHullVcount, mBaseName, index );
fprintf(fph," <param name=\"X\" type=\"float\"/>\r\n");
fprintf(fph," <param name=\"Y\" type=\"float\"/>\r\n");
fprintf(fph," <param name=\"Z\" type=\"float\"/>\r\n");
fprintf(fph," </accessor>\r\n");
fprintf(fph," </technique_common>\r\n");
fprintf(fph," </source>\r\n");
fprintf(fph," <vertices id=\"%s_%d-Vertex\">\r\n", mBaseName, index);
fprintf(fph," <input semantic=\"POSITION\" source=\"#%s_%d-Position\"/>\r\n", mBaseName, index);
fprintf(fph," </vertices>\r\n");
fprintf(fph," <triangles material=\"Material\" count=\"%d\">\r\n", cr->mHullTcount );
fprintf(fph," <input offset=\"0\" semantic=\"VERTEX\" source=\"#%s_%d-Vertex\"/>\r\n", mBaseName, index);
fprintf(fph," <p>\r\n");
fprintf(fph," ");
for (unsigned int i=0; i<cr->mHullTcount; i++)
{
unsigned int *tri = &cr->mHullIndices[i*3];
fprintf(fph,"%d %d %d ", tri[0], tri[1], tri[2] );
if ( ((i+1)&3) == 0 && (i+1) < cr->mHullTcount )
{
fprintf(fph,"\r\n");
fprintf(fph," ");
}
}
fprintf(fph,"\r\n");
fprintf(fph," </p>\r\n");
fprintf(fph," </triangles>\r\n");
fprintf(fph," </convex_mesh>\r\n");
fprintf(fph," </geometry>\r\n");
}
void saveCOLLADA(void)
{
char scratch[512];
sprintf(scratch,"%s.dae", mBaseName );
FILE *fph = fopen(scratch,"wb");
if ( fph )
{
printf("Saving convex decomposition of %d hulls to COLLADA file '%s'\r\n", (int)mHulls.size(), scratch );
fprintf(fph,"<?xml version=\"1.0\" encoding=\"utf-8\"?>\r\n");
fprintf(fph,"<COLLADA version=\"1.4.0\" xmlns=\"http://www.collada.org/2005/11/COLLADASchema\">\r\n");
fprintf(fph," <asset>\r\n");
fprintf(fph," <contributor>\r\n");
fprintf(fph," <author>NxuStream2 converter - http://www.ageia.com</author>\r\n");
fprintf(fph," <authoring_tool>PhysX Rocket, PhysX Viewer, or CreateDynamics</authoring_tool>\r\n");
fprintf(fph," <comments>questions to: jratcliff@ageia.com</comments>\r\n");
fprintf(fph," <copyright></copyright>\r\n");
fprintf(fph," <source_data>chair_gothic_tilted</source_data>\r\n");
fprintf(fph," </contributor>\r\n");
fprintf(fph," <unit meter=\"1\" name=\"meter\"/>\r\n");
fprintf(fph," <up_axis>Y_UP</up_axis>\r\n");
fprintf(fph," </asset>\r\n");
fprintf(fph," <library_materials>\r\n");
fprintf(fph," <material id=\"Material\" name=\"Material\">\r\n");
fprintf(fph," <instance_effect url=\"#Material-fx\"></instance_effect>\r\n");
fprintf(fph," </material>\r\n");
fprintf(fph," </library_materials>\r\n");
fprintf(fph," <library_effects>\r\n");
fprintf(fph," <effect id=\"Material-fx\" name=\"Material\">\r\n");
fprintf(fph," <profile_COMMON>\r\n");
fprintf(fph," <technique id=\"Material-fx-COMMON\" sid=\"COMMON\">\r\n");
fprintf(fph," <phong>\r\n");
fprintf(fph," <ambient>\r\n");
fprintf(fph," <color>0.803922 0.588235 0.92549 1</color>\r\n");
fprintf(fph," </ambient>\r\n");
fprintf(fph," <diffuse>\r\n");
fprintf(fph," <color>0.803922 0.588235 0.92549 1</color>\r\n");
fprintf(fph," </diffuse>\r\n");
fprintf(fph," <specular>\r\n");
fprintf(fph," <color>0.631373 0.631373 0.631373 1</color>\r\n");
fprintf(fph," </specular>\r\n");
fprintf(fph," <shininess>\r\n");
fprintf(fph," <float>1</float>\r\n");
fprintf(fph," </shininess>\r\n");
fprintf(fph," <reflective>\r\n");
fprintf(fph," <color>0 0 0 1</color>\r\n");
fprintf(fph," </reflective>\r\n");
fprintf(fph," <transparent>\r\n");
fprintf(fph," <color>1 1 1 1</color>\r\n");
fprintf(fph," </transparent>\r\n");
fprintf(fph," <transparency>\r\n");
fprintf(fph," <float>0</float>\r\n");
fprintf(fph," </transparency>\r\n");
fprintf(fph," </phong>\r\n");
fprintf(fph," </technique>\r\n");
fprintf(fph," </profile_COMMON>\r\n");
fprintf(fph," </effect>\r\n");
fprintf(fph," </library_effects>\r\n");
fprintf(fph," <library_geometries>\r\n");
for (unsigned int i=0; i<mHulls.size(); i++)
{
ConvexResult *cr = mHulls[i];
saveCOLLADA(fph,i,cr);
}
fprintf(fph," </library_geometries>\r\n");
fprintf(fph," <library_visual_scenes>\r\n");
fprintf(fph," <visual_scene id=\"Scene0-Visual\" name=\"Scene0-Visual\">\r\n");
fprintf(fph," <node id=\"%s-Node\" name=\"%s\" type=\"NODE\">\r\n", mBaseName, mBaseName );
fprintf(fph," <translate>0 0 0</translate>\r\n");
fprintf(fph," <rotate>1 0 0 0</rotate>\r\n");
fprintf(fph," </node>\r\n");
fprintf(fph," </visual_scene>\r\n");
fprintf(fph," </library_visual_scenes>\r\n");
fprintf(fph," <library_physics_materials>\r\n");
fprintf(fph," <physics_material id=\"pmat0_0-PhysicsMaterial\" name=\"pmat0_0-PhysicsMaterial\">\r\n");
fprintf(fph," <technique_common>\r\n");
fprintf(fph," <dynamic_friction>0.5</dynamic_friction>\r\n");
fprintf(fph," <restitution>0</restitution>\r\n");
fprintf(fph," <static_friction>0.5</static_friction>\r\n");
fprintf(fph," </technique_common>\r\n");
fprintf(fph," </physics_material>\r\n");
fprintf(fph," </library_physics_materials>\r\n");
fprintf(fph," <library_physics_models>\r\n");
fprintf(fph," <physics_model id=\"Scene0-PhysicsModel\">\r\n");
fprintf(fph," <rigid_body sid=\"%s-RigidBody\" name=\"%s\">\r\n", mBaseName, mBaseName);
fprintf(fph," <technique_common>\r\n");
fprintf(fph," <instance_physics_material url=\"#pmat0_0-PhysicsMaterial\"/>\r\n");
for (unsigned int i=0; i<mHulls.size(); i++)
{
ConvexResult *ch = mHulls[i];
fprintf(fph," <shape>\r\n");
fprintf(fph," <translate>0 0 0</translate>\r\n");
fprintf(fph," <rotate>1 0 0 0</rotate>\r\n");
fprintf(fph," <instance_physics_material url=\"#pmat0_0-PhysicsMaterial\"/>\r\n");
fprintf(fph," <density>1</density>\r\n");
fprintf(fph," <extra>\r\n");
fprintf(fph," <technique profile=\"PhysX\">\r\n");
fprintf(fph," <skinWidth>%s</skinWidth>\r\n", fstring( mSkinWidth ) );
fprintf(fph," </technique>\r\n");
fprintf(fph," </extra>\r\n");
fprintf(fph," <instance_geometry url=\"%s_%d-Mesh\"/>\r\n", mBaseName, i);
fprintf(fph," </shape>\r\n");
}
fprintf(fph," <dynamic>true</dynamic>\r\n");
fprintf(fph," <mass>1</mass>\r\n");
fprintf(fph," </technique_common>\r\n");
fprintf(fph," <extra>\r\n");
fprintf(fph," <technique profile=\"PhysX\">\r\n");
fprintf(fph," <extra>\r\n");
fprintf(fph," <technique profile=\"PhysX\">\r\n");
fprintf(fph," <linearDamping>0</linearDamping>\r\n");
fprintf(fph," <angularDamping>0</angularDamping>\r\n");
fprintf(fph," <solverIterationCount>32</solverIterationCount>\r\n");
fprintf(fph," </technique>\r\n");
fprintf(fph," </extra>\r\n");
fprintf(fph," <disable_collision>false</disable_collision>\r\n");
fprintf(fph," </technique>\r\n");
fprintf(fph," </extra>\r\n");
fprintf(fph," </rigid_body>\r\n");
fprintf(fph," </physics_model>\r\n");
fprintf(fph," </library_physics_models>\r\n");
fprintf(fph," <library_physics_scenes>\r\n");
fprintf(fph," <physics_scene id=\"Scene0-Physics\">\r\n");
fprintf(fph," <instance_physics_model url=\"#Scene0-PhysicsModel\">\r\n");
fprintf(fph," <instance_rigid_body target=\"#%s-Node\" body=\"%s-RigidBody\"/>\r\n", mBaseName, mBaseName );
fprintf(fph," <extra>\r\n");
fprintf(fph," <technique profile=\"PhysX\">\r\n");
fprintf(fph," </technique>\r\n");
fprintf(fph," </extra>\r\n");
fprintf(fph," </instance_physics_model>\r\n");
fprintf(fph," <technique_common>\r\n");
fprintf(fph," <gravity>0 -9.800000191 0</gravity>\r\n");
fprintf(fph," </technique_common>\r\n");
fprintf(fph," </physics_scene>\r\n");
fprintf(fph," </library_physics_scenes>\r\n");
fprintf(fph," <scene>\r\n");
fprintf(fph," <instance_visual_scene url=\"#Scene0-Visual\"/>\r\n");
fprintf(fph," <instance_physics_scene url=\"#Scene0-Physics\"/>\r\n");
fprintf(fph," </scene>\r\n");
fprintf(fph,"</COLLADA>\r\n");
fclose(fph);
}
else
{
printf("Failed to open file '%s' for write access.\r\n", scratch );
}
}
void saveXML(FILE *fph,unsigned int index,ConvexResult *cr)
{
fprintf(fph," <NxConvexMeshDesc id=\"%s_%d\">\r\n", mBaseName, index);
fprintf(fph," <points>\r\n");
fprintf(fph," ");
for (unsigned int i=0; i<cr->mHullVcount; i++)
{
const double *p = &cr->mHullVertices[i*3];
fprintf(fph,"%s %s %s ", fstring((float)p[0]), fstring((float)p[1]), fstring((float)p[2]) );
if ( ((i+1)&3) == 0 && (i+1) < cr->mHullVcount )
{
fprintf(fph,"\r\n");
fprintf(fph," ");
}
}
fprintf(fph,"\r\n");
fprintf(fph," </points>\r\n");
fprintf(fph," <triangles>\r\n");
fprintf(fph," ");
for (unsigned int i=0; i<cr->mHullTcount; i++)
{
unsigned int *tri = &cr->mHullIndices[i*3];
fprintf(fph,"%d %d %d ", tri[0], tri[1], tri[2] );
if ( ((i+1)&3) == 0 && (i+1) < cr->mHullTcount )
{
fprintf(fph,"\r\n");
fprintf(fph," ");
}
}
fprintf(fph,"\r\n");
fprintf(fph," </triangles>\r\n");
fprintf(fph," </NxConvexMeshDesc>\r\n");
}
void saveNxuStream(void)
{
char scratch[512];
sprintf(scratch,"%s.xml", mBaseName );
FILE *fph = fopen(scratch,"wb");
if ( fph )
{
printf("Saving convex decomposition of %d hulls to NxuStream file '%s'\r\n", mHulls.size(), scratch );
fprintf(fph,"<?xml version=\"1.0\" encoding=\"utf-8\"?>\r\n");
fprintf(fph," <NXUSTREAM2>\r\n");
fprintf(fph," <NxuPhysicsCollection id=\"beshon\" sdkVersion=\"244\" nxuVersion=\"100\">\r\n");
fprintf(fph," <NxPhysicsSDKDesc id=\"SDK\">\r\n");
fprintf(fph," <hwPageSize>65536</hwPageSize>\r\n");
fprintf(fph," <hwPageMax>256</hwPageMax>\r\n");
fprintf(fph," <hwConvexMax>2048</hwConvexMax>\r\n");
fprintf(fph," </NxPhysicsSDKDesc>\r\n");
for (unsigned int i=0; i<mHulls.size(); i++)
{
saveXML(fph, i, mHulls[i] );
}
fprintf(fph," <NxSceneDesc id=\"%s\">\r\n", mBaseName);
fprintf(fph," <filterBool>false</filterBool>\r\n");
fprintf(fph," <filterOp0>NX_FILTEROP_AND</filterOp0>\r\n");
fprintf(fph," <filterOp1>NX_FILTEROP_AND</filterOp1>\r\n");
fprintf(fph," <filterOp2>NX_FILTEROP_AND</filterOp2>\r\n");
fprintf(fph," <NxGroupsMask id=\"groupMask0\" bits0=\"0\" bits1=\"0\" bits2=\"0\" bits3=\"0\"/>\r\n");
fprintf(fph," <NxGroupsMask id=\"groupMask1\" bits0=\"0\" bits1=\"0\" bits2=\"0\" bits3=\"0\"/>\r\n");
fprintf(fph," <gravity>0 -9.800000191 0</gravity>\r\n");
fprintf(fph," <maxTimestep>0.016666668</maxTimestep>\r\n");
fprintf(fph," <maxIter>8</maxIter>\r\n");
fprintf(fph," <timeStepMethod>NX_TIMESTEP_FIXED</timeStepMethod>\r\n");
fprintf(fph," <maxBounds>FLT_MIN FLT_MIN FLT_MIN FLT_MAX FLT_MAX FLT_MAX</maxBounds>\r\n");
fprintf(fph," <NxSceneLimits id=\"limits\">\r\n");
fprintf(fph," <maxNbActors>0</maxNbActors>\r\n");
fprintf(fph," <maxNbBodies>0</maxNbBodies>\r\n");
fprintf(fph," <maxNbStaticShapes>0</maxNbStaticShapes>\r\n");
fprintf(fph," <maxNbDynamicShapes>0</maxNbDynamicShapes>\r\n");
fprintf(fph," <maxNbJoints>0</maxNbJoints>\r\n");
fprintf(fph," </NxSceneLimits>\r\n");
fprintf(fph," <simType>NX_SIMULATION_SW</simType>\r\n");
fprintf(fph," <groundPlane>true</groundPlane>\r\n");
fprintf(fph," <boundsPlanes>false</boundsPlanes>\r\n");
fprintf(fph," <NxSceneFlags id=\"flags\">\r\n");
fprintf(fph," <NX_SF_DISABLE_SSE>false</NX_SF_DISABLE_SSE>\r\n");
fprintf(fph," <NX_SF_DISABLE_COLLISIONS>false</NX_SF_DISABLE_COLLISIONS>\r\n");
fprintf(fph," <NX_SF_SIMULATE_SEPARATE_THREAD>true</NX_SF_SIMULATE_SEPARATE_THREAD>\r\n");
fprintf(fph," <NX_SF_ENABLE_MULTITHREAD>false</NX_SF_ENABLE_MULTITHREAD>\r\n");
fprintf(fph," </NxSceneFlags>\r\n");
fprintf(fph," <internalThreadCount>0</internalThreadCount>\r\n");
fprintf(fph," <backgroundThreadCount>0</backgroundThreadCount>\r\n");
fprintf(fph," <threadMask>1431655764</threadMask>\r\n");
fprintf(fph," <backgroundThreadMask>1431655764</backgroundThreadMask>\r\n");
fprintf(fph," <NxMaterialDesc id=\"Material_0\" userProperties=\"\" hasSpring=\"false\" materialIndex=\"0\">\r\n");
fprintf(fph," <dynamicFriction>0.5</dynamicFriction>\r\n");
fprintf(fph," <staticFriction>0.5</staticFriction>\r\n");
fprintf(fph," <restitution>0</restitution>\r\n");
fprintf(fph," <dynamicFrictionV>0</dynamicFrictionV>\r\n");
fprintf(fph," <staticFrictionV>0</staticFrictionV>\r\n");
fprintf(fph," <frictionCombineMode>NX_CM_AVERAGE</frictionCombineMode>\r\n");
fprintf(fph," <restitutionCombineMode>NX_CM_AVERAGE</restitutionCombineMode>\r\n");
fprintf(fph," <dirOfAnisotropy>1 0 0</dirOfAnisotropy>\r\n");
fprintf(fph," <NxMaterialFlag id=\"flags\">\r\n");
fprintf(fph," <NX_MF_ANISOTROPIC>false</NX_MF_ANISOTROPIC>\r\n");
fprintf(fph," <NX_MF_DISABLE_FRICTION>false</NX_MF_DISABLE_FRICTION>\r\n");
fprintf(fph," <NX_MF_DISABLE_STRONG_FRICTION>false</NX_MF_DISABLE_STRONG_FRICTION>\r\n");
fprintf(fph," </NxMaterialFlag>\r\n");
fprintf(fph," </NxMaterialDesc>\r\n");
fprintf(fph," <NxActorDesc id=\"%s\" userProperties=\"\" hasBody=\"true\" name=\"%s\">\r\n", mBaseName, mBaseName);
fprintf(fph," <globalPose>1 0 0 0 1 0 0 0 1 0 0 0 </globalPose>\r\n");
fprintf(fph," <NxBodyDesc>\r\n");
fprintf(fph," <mass>1</mass>\r\n");
fprintf(fph," <linearDamping>0</linearDamping>\r\n");
fprintf(fph," <angularDamping>0</angularDamping>\r\n");
fprintf(fph," <solverIterationCount>32</solverIterationCount>\r\n");
fprintf(fph," <NxBodyFlag id=\"flags\">\r\n");
fprintf(fph," <NX_BF_POSE_SLEEP_TEST>true</NX_BF_POSE_SLEEP_TEST>\r\n");
fprintf(fph," <NX_AF_DISABLE_COLLISION>false</NX_AF_DISABLE_COLLISION>\r\n");
fprintf(fph," </NxBodyFlag>\r\n");
fprintf(fph," </NxBodyDesc>\r\n");
fprintf(fph," <name>Bip01 Pelvis</name>\r\n");
for (unsigned int i=0; i<mHulls.size(); i++)
{
fprintf(fph," <NxConvexShapeDesc id=\"Shape_%d\" meshData=\"%s_%d\">\r\n", i, mBaseName, i);
fprintf(fph," <NxShapeDesc>\r\n");
fprintf(fph," <localPose>1 0 0 0 1 0 0 0 1 0 0 0 </localPose>\r\n");
fprintf(fph," <skinWidth>0</skinWidth>\r\n");
fprintf(fph," </NxShapeDesc>\r\n");
fprintf(fph," </NxConvexShapeDesc>\r\n");
}
fprintf(fph," </NxActorDesc>\r\n");
fprintf(fph," </NxSceneDesc>\r\n");
fprintf(fph," <NxSceneInstance id=\"%s\">\r\n", mBaseName);
fprintf(fph," <sceneName>beshon</sceneName>\r\n");
fprintf(fph," <NxuUserProperties></NxuUserProperties>\r\n");
fprintf(fph," <rootNode>1 0 0 0 1 0 0 0 1 0 0 0</rootNode>\r\n");
fprintf(fph," <newScene>false</newScene>\r\n");
fprintf(fph," <ignorePlane>true</ignorePlane>\r\n");
fprintf(fph," <numSceneInstances>0</numSceneInstances>\r\n");
fprintf(fph," </NxSceneInstance>\r\n");
fprintf(fph," </NxuPhysicsCollection>\r\n");
fprintf(fph,"</NXUSTREAM2>\r\n");
}
else
{
printf("Failed to open file '%s' for write access.\r\n", scratch );
}
}
float mSkinWidth;
unsigned int mHullCount;
FILE *mFph;
unsigned int mBaseCount;
char mObjName[512];
char mBaseName[512];
ConvexResultVector mHulls;
};
int main(int argc,const char **argv)
{
if ( argc < 2 )
{
printf("Usage: Test <meshanme.obj> (options)\r\n");
printf("\r\n");
printf("Options:\r\n");
printf("\r\n");
printf(" -d<depth> : How deep to recursively split. Values of 3-7 are reasonable.\r\n");
printf(" -c<percent> : Percentage of concavity to keep splitting. 0-20% is reasonable.\r\n");
printf(" -p<percent> : Percentage of volume delta to collapse hulls. 0-30% is reasonable.\r\n");
printf(" -v<maxverts> : Maximum number of vertices in the output hull. Default is 32.\r\n");
printf(" -s<skinwidth> : Skin Width inflation. Default is 0.\r\n");
}
else
{
unsigned int depth = 5;
float cpercent = 5;
float ppercent = 5;
unsigned int maxv = 32;
float skinWidth = 0;
// process command line switches.
for (int i=2; i<argc; i++)
{
const char *o = argv[i];
if ( strncmp(o,"-d",2) == 0 )
{
depth = (unsigned int) atoi( &o[2] );
if ( depth < 0 || depth > 10 )
{
depth = 5;
printf("Invalid depth value in switch, defaulting to 5.\r\n");
}
}
if ( strncmp(o,"-c",2) == 0 )
{
cpercent = (float) atof( &o[2] );
if ( cpercent < 0 || cpercent > 100 )
{
cpercent = 5;
printf("Invalid concavity percentage in switch, defaulting to 5.\r\n");
}
}
if ( strncmp(o,"-p",2) == 0 )
{
ppercent = (float) atof( &o[2] );
if ( ppercent < 0 || ppercent > 100 )
{
ppercent = 5;
printf("Invalid hull merge percentage in switch, defaulting to 5.\r\n");
}
}
if ( strncmp(o,"-v",2) == 0 )
{
maxv = (unsigned int) atoi( &o[2] );
if ( maxv < 8 || maxv > 256 )
{
maxv = 32;
printf("Invalid max vertices in switch, defaulting to 32.\r\n");
}
}
if ( strncmp(o,"-s",2) == 0 )
{
skinWidth = (float) atof( &o[2] );
if ( skinWidth < 0 || skinWidth > 0.1f )
{
skinWidth = 0;
printf("Invalid skinWidth in switch, defaulting to 0.\r\n");
}
}
}
WavefrontObj wo;
unsigned int tcount = wo.loadObj(argv[1]);
if ( tcount )
{
DecompDesc d;
d.mVcount = wo.mVertexCount;
d.mVertices = wo.mVertices;
d.mTcount = wo.mTriCount;
d.mIndices = (unsigned int *)wo.mIndices;
d.mDepth = depth;
d.mCpercent = cpercent;
d.mPpercent = ppercent;
d.mMaxVertices = maxv;
d.mSkinWidth = skinWidth;
CBuilder cb(argv[1],d); // receives the answers and writes out a wavefront OBJ file.
d.mCallback = &cb;
unsigned int count = performConvexDecomposition(d);
if ( count )
{
printf("Input triangle mesh with %d triangles produced %d output hulls.\r\n", d.mTcount, count );
cb.saveNxuStream(); // save results in NxuStream format.
cb.saveCOLLADA(); // save results in COLLADA physics 1.4.1 format.
}
else
{
printf("Failed to produce any convex hulls.\r\n");
}
}
else
{
// sorry..no
printf("Sorry unable to load file '%s'\r\n", argv[1] );
}
}
}

BIN
convex_decomposition/ConvexDecomposition/DecomposeSample.exe Normal file
View File

Binary file not shown.

21
convex_decomposition/ConvexDecomposition/DecomposeSample.sln Normal file
View File

@ -0,0 +1,21 @@
Microsoft Visual Studio Solution File, Format Version 8.00
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "DecomposeSample", "DecomposeSample.vcproj", "{0282D4E2-DA97-437D-9BDE-1C9F5E56F6F8}"
ProjectSection(ProjectDependencies) = postProject
EndProjectSection
EndProject
Global
GlobalSection(SolutionConfiguration) = preSolution
Debug = Debug
Release = Release
EndGlobalSection
GlobalSection(ProjectConfiguration) = postSolution
{0282D4E2-DA97-437D-9BDE-1C9F5E56F6F8}.Debug.ActiveCfg = Debug|Win32
{0282D4E2-DA97-437D-9BDE-1C9F5E56F6F8}.Debug.Build.0 = Debug|Win32
{0282D4E2-DA97-437D-9BDE-1C9F5E56F6F8}.Release.ActiveCfg = Release|Win32
{0282D4E2-DA97-437D-9BDE-1C9F5E56F6F8}.Release.Build.0 = Release|Win32
EndGlobalSection
GlobalSection(ExtensibilityGlobals) = postSolution
EndGlobalSection
GlobalSection(ExtensibilityAddIns) = postSolution
EndGlobalSection
EndGlobal

BIN
convex_decomposition/ConvexDecomposition/DecomposeSample.suo Normal file
View File

Binary file not shown.

240
convex_decomposition/ConvexDecomposition/DecomposeSample.vcproj Normal file
View File

@ -0,0 +1,240 @@
<?xml version="1.0" encoding="Windows-1252"?>
<VisualStudioProject
ProjectType="Visual C++"
Version="7.10"
Name="DecomposeSample"
SccProjectName=""
SccLocalPath="">
<Platforms>
<Platform
Name="Win32"/>
</Platforms>
<Configurations>
<Configuration
Name="Release|Win32"
OutputDirectory=".\Release"
IntermediateDirectory=".\Release"
ConfigurationType="1"
UseOfMFC="0"
ATLMinimizesCRunTimeLibraryUsage="FALSE"
CharacterSet="2">
<Tool
Name="VCCLCompilerTool"
Optimization="2"
InlineFunctionExpansion="1"
PreprocessorDefinitions="WIN32;NDEBUG;_CONSOLE"
StringPooling="TRUE"
RuntimeLibrary="0"
EnableFunctionLevelLinking="TRUE"
PrecompiledHeaderFile=".\Release/DecomposeSample.pch"
AssemblerListingLocation=".\Release/"
ObjectFile=".\Release/"
ProgramDataBaseFileName=".\Release/"
WarningLevel="3"
SuppressStartupBanner="TRUE"/>
<Tool
Name="VCCustomBuildTool"/>
<Tool
Name="VCLinkerTool"
OutputFile=".\Release/DecomposeSample.exe"
LinkIncremental="1"
SuppressStartupBanner="TRUE"
ProgramDatabaseFile=".\Release/DecomposeSample.pdb"
SubSystem="1"
TargetMachine="1"/>
<Tool
Name="VCMIDLTool"
TypeLibraryName=".\Release/DecomposeSample.tlb"
HeaderFileName=""/>
<Tool
Name="VCPostBuildEventTool"/>
<Tool
Name="VCPreBuildEventTool"/>
<Tool
Name="VCPreLinkEventTool"/>
<Tool
Name="VCResourceCompilerTool"
PreprocessorDefinitions="NDEBUG"
Culture="1033"/>
<Tool
Name="VCWebServiceProxyGeneratorTool"/>
<Tool
Name="VCXMLDataGeneratorTool"/>
<Tool
Name="VCWebDeploymentTool"/>
<Tool
Name="VCManagedWrapperGeneratorTool"/>
<Tool
Name="VCAuxiliaryManagedWrapperGeneratorTool"/>
</Configuration>
<Configuration
Name="Debug|Win32"
OutputDirectory=".\Debug"
IntermediateDirectory=".\Debug"
ConfigurationType="1"
UseOfMFC="0"
ATLMinimizesCRunTimeLibraryUsage="FALSE"
CharacterSet="2">
<Tool
Name="VCCLCompilerTool"
Optimization="0"
PreprocessorDefinitions="WIN32;_DEBUG;_CONSOLE"
BasicRuntimeChecks="3"
RuntimeLibrary="1"
PrecompiledHeaderFile=".\Debug/DecomposeSample.pch"
AssemblerListingLocation=".\Debug/"
ObjectFile=".\Debug/"
ProgramDataBaseFileName=".\Debug/"
WarningLevel="3"
SuppressStartupBanner="TRUE"
DebugInformationFormat="4"/>
<Tool
Name="VCCustomBuildTool"/>
<Tool
Name="VCLinkerTool"
OutputFile=".\Debug/DecomposeSample.exe"
LinkIncremental="1"
SuppressStartupBanner="TRUE"
GenerateDebugInformation="TRUE"
ProgramDatabaseFile=".\Debug/DecomposeSample.pdb"
SubSystem="1"
TargetMachine="1"/>
<Tool
Name="VCMIDLTool"
TypeLibraryName=".\Debug/DecomposeSample.tlb"
HeaderFileName=""/>
<Tool
Name="VCPostBuildEventTool"/>
<Tool
Name="VCPreBuildEventTool"/>
<Tool
Name="VCPreLinkEventTool"/>
<Tool
Name="VCResourceCompilerTool"
PreprocessorDefinitions="_DEBUG"
Culture="1033"/>
<Tool
Name="VCWebServiceProxyGeneratorTool"/>
<Tool
Name="VCXMLDataGeneratorTool"/>
<Tool
Name="VCWebDeploymentTool"/>
<Tool
Name="VCManagedWrapperGeneratorTool"/>
<Tool
Name="VCAuxiliaryManagedWrapperGeneratorTool"/>
</Configuration>
</Configurations>
<References>
</References>
<Files>
<Filter
Name="Source Files"
Filter="cpp;c;cxx;rc;def;r;odl;idl;hpj;bat">
<File
RelativePath=".\DecomposeSample.cpp">
</File>
</Filter>
<Filter
Name="Header Files"
Filter="h;hpp;hxx;hm;inl">
</Filter>
<Filter
Name="Resource Files"
Filter="ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe">
</Filter>
<Filter
Name="ConvexDecomposition"
Filter="">
<File
RelativePath=".\ConvexDecomposition\bestfit.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\bestfit.h">
</File>
<File
RelativePath=".\ConvexDecomposition\bestfitobb.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\bestfitobb.h">
</File>
<File
RelativePath=".\ConvexDecomposition\cd_hull.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\cd_hull.h">
</File>
<File
RelativePath=".\ConvexDecomposition\cd_vector.h">
</File>
<File
RelativePath=".\ConvexDecomposition\cd_wavefront.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\cd_wavefront.h">
</File>
<File
RelativePath=".\ConvexDecomposition\concavity.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\concavity.h">
</File>
<File
RelativePath=".\ConvexDecomposition\ConvexDecomposition.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\ConvexDecomposition.h">
</File>
<File
RelativePath=".\ConvexDecomposition\fitsphere.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\fitsphere.h">
</File>
<File
RelativePath=".\ConvexDecomposition\float_math.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\float_math.h">
</File>
<File
RelativePath=".\ConvexDecomposition\meshvolume.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\meshvolume.h">
</File>
<File
RelativePath=".\ConvexDecomposition\planetri.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\planetri.h">
</File>
<File
RelativePath=".\ConvexDecomposition\raytri.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\raytri.h">
</File>
<File
RelativePath=".\ConvexDecomposition\splitplane.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\splitplane.h">
</File>
<File
RelativePath=".\ConvexDecomposition\triangulate.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\triangulate.h">
</File>
<File
RelativePath=".\ConvexDecomposition\vlookup.cpp">
</File>
<File
RelativePath=".\ConvexDecomposition\vlookup.h">
</File>
</Filter>
</Files>
<Globals>
</Globals>
</VisualStudioProject>

93
convex_decomposition/ConvexDecomposition/Makefile Normal file
View File

@ -0,0 +1,93 @@
OBJS = DecomposeSample.o \
ConvexDecomposition/bestfit.o ConvexDecomposition/float_math.o \
ConvexDecomposition/bestfitobb.o ConvexDecomposition/meshvolume.o \
ConvexDecomposition/cd_hull.o ConvexDecomposition/planetri.o \
ConvexDecomposition/cd_wavefront.o ConvexDecomposition/raytri.o \
ConvexDecomposition/concavity.o ConvexDecomposition/splitplane.o \
ConvexDecomposition/ConvexDecomposition.o ConvexDecomposition/triangulate.o \
ConvexDecomposition/fitsphere.o ConvexDecomposition/vlookup.o
HEADERS = \
ConvexDecomposition/bestfit.h \
ConvexDecomposition/bestfitobb.h \
ConvexDecomposition/cd_hull.h \
ConvexDecomposition/cd_vector.h \
ConvexDecomposition/cd_wavefront.h \
ConvexDecomposition/concavity.h \
ConvexDecomposition/ConvexDecomposition.h \
ConvexDecomposition/fitsphere.h \
ConvexDecomposition/float_math.h \
ConvexDecomposition/meshvolume.h \
ConvexDecomposition/planetri.h \
ConvexDecomposition/raytri.h \
ConvexDecomposition/splitplane.h \
ConvexDecomposition/triangulate.h \
ConvexDecomposition/vlookup.h
CC = g++
DEBUG = -ggdb
CFLAGS = -IConvexDecomposition -Wall -c $(DEBUG)
LFLAGS = $(DEBUG)
convex_decomposition: $(OBJS)
$(CC) $(LFLAGS) $(OBJS) -o convex_decomposition
DecomposeSample.o: DecomposeSample.cpp
$(CC) $(CFLAGS) DecomposeSample.cpp -o $@
ConvexDecomposition/bestfit.o: ConvexDecomposition/bestfit.cpp
$(CC) $(CFLAGS) ConvexDecomposition/bestfit.cpp -o $@
ConvexDecomposition/bestfitobb.o: ConvexDecomposition/bestfitobb.cpp
$(CC) $(CFLAGS) ConvexDecomposition/bestfitobb.cpp -o $@
ConvexDecomposition/cd_hull.o: ConvexDecomposition/cd_hull.cpp
$(CC) $(CFLAGS) ConvexDecomposition/cd_hull.cpp -o $@
ConvexDecomposition/cd_wavefront.o: ConvexDecomposition/cd_wavefront.cpp
$(CC) $(CFLAGS) ConvexDecomposition/cd_wavefront.cpp -o $@
ConvexDecomposition/concavity.o: ConvexDecomposition/concavity.cpp
$(CC) $(CFLAGS) ConvexDecomposition/concavity.cpp -o $@
ConvexDecomposition/ConvexDecomposition.o: ConvexDecomposition/ConvexDecomposition.cpp
$(CC) $(CFLAGS) ConvexDecomposition/ConvexDecomposition.cpp -o $@
ConvexDecomposition/fitsphere.o: ConvexDecomposition/fitsphere.cpp
$(CC) $(CFLAGS) ConvexDecomposition/fitsphere.cpp -o $@
ConvexDecomposition/float_math.o: ConvexDecomposition/float_math.cpp
$(CC) $(CFLAGS) ConvexDecomposition/float_math.cpp -o $@
ConvexDecomposition/meshvolume.o: ConvexDecomposition/meshvolume.cpp
$(CC) $(CFLAGS) ConvexDecomposition/meshvolume.cpp -o $@
ConvexDecomposition/planetri.o: ConvexDecomposition/planetri.cpp
$(CC) $(CFLAGS) ConvexDecomposition/planetri.cpp -o $@
ConvexDecomposition/raytri.o: ConvexDecomposition/raytri.cpp
$(CC) $(CFLAGS) ConvexDecomposition/raytri.cpp -o $@
ConvexDecomposition/splitplane.o: ConvexDecomposition/splitplane.cpp
$(CC) $(CFLAGS) ConvexDecomposition/splitplane.cpp -o $@
ConvexDecomposition/triangulate.o: ConvexDecomposition/triangulate.cpp
$(CC) $(CFLAGS) ConvexDecomposition/triangulate.cpp -o $@
ConvexDecomposition/vlookup.o: ConvexDecomposition/vlookup.cpp ConvexDecomposition/vlookup.cpp
$(CC) $(CFLAGS) ConvexDecomposition/vlookup.cpp -o $@
install:
cp convex_decomposition ../../convex_decomposition/bin/
clean:
\rm *.o */*.o convex_decomposition
tar:
tar cfv ConvexDecomposition.tar DecomposeSample.cpp convex_decomposition Makefile \
ConvexDecomposition

3083
convex_decomposition/ConvexDecomposition/chair.obj Normal file
View File

File diff suppressed because it is too large Load Diff

75
convex_decomposition/ConvexDecomposition/readme.txt Normal file
View File

@ -0,0 +1,75 @@
ConvexDecomposition library written by John W. Ratcliff
Usage: Test <meshanme.obj> (options)
Options:
-d<depth> : How deep to recursively split. Values of 3-7 are reasonable.
-c<percent> : Percentage of concavity to keep splitting. 0-20 4324640s reasonable.
-p<percent> : Percentage of volume delta to collapse hulls. 0-30 4324544s reasonable.
-v<maxverts> : Maximum number of vertices in the output hull. Default is 32.
-s<skinwidth> : Skin Width inflation. Default is 0.0
To run the sample provided with defaults:
DecomposeSample chair.obj
Will produce 'chair_convex.obj' a Wavefront OBJ file useful for debug visualization.
'chair.dae' a COLLADA physics representation of the model.
'chair.xml' an Ageia PhysX NxuStream representation of the model.
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net
**
** Portions of this source has been released with the PhysXViewer application, as well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: jratcliff@infiniplex.net
** Personal website: http://jratcliffscarab.blogspot.com
** Coding Website: http://codesuppository.blogspot.com
** FundRaising Blog: http://amillionpixels.blogspot.com
** Fundraising site: http://www.amillionpixels.us
** New Temple Site: http://newtemple.blogspot.com
**
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

33
convex_decomposition/Makefile Normal file
View File

@ -0,0 +1,33 @@
all: installed
TARBALL = build/ConvexDecomposition.zip
TARBALL_URL = http://pr.willowgarage.com/downloads/ConvexDecomposition.zip
SOURCE_DIR = build/convex_decomposition
INITIAL_DIR = build/ConvexDecomposition
UNPACK_CMD = unzip
TARBALL_PATCH=convex_decomposition.patch
include $(shell rospack find mk)/download_unpack_build.mk
ROOT = $(shell rospack find convex_decomposition)/convex_decomposition
installed: wiped $(SOURCE_DIR)/unpacked
@echo "making it"
@echo "ROOT is: $(ROOT)"
-mkdir -p $(ROOT)
-mkdir -p $(ROOT)/bin
cd $(SOURCE_DIR) ; make $(ROS_PARALLEL_JOBS); make install
touch installed
wiped: Makefile
make wipe
touch wiped
clean:
-cd $(SOURCE_DIR) && make clean
rm -rf $(ROOT) installed
wipe: clean
rm -rf build
.PHONY : clean download wipe

429
convex_decomposition/convex_decomposition.patch Normal file
View File

@ -0,0 +1,429 @@
Index: ConvexDecomposition/vlookup.cpp
===================================================================
--- ConvexDecomposition/vlookup.cpp (revision 8299)
+++ ConvexDecomposition/vlookup.cpp (working copy)
@@ -164,7 +164,7 @@
};
-template <class Type> class VertexLess
+template <typename Type> class VertexLess
{
public:
typedef std::vector< Type > VertexVector;
@@ -188,7 +188,7 @@
static VertexVector *mList;
};
-template <class Type> class VertexPool
+template <typename Type> class VertexPool
{
public:
typedef std::set<int, VertexLess<Type> > VertexSet;
@@ -197,7 +197,7 @@
int GetVertex(const Type& vtx)
{
VertexLess<Type>::SetSearch(vtx,&mVtxs);
- VertexSet::iterator found;
+ typename VertexSet::iterator found;
found = mVertSet.find( -1 );
if ( found != mVertSet.end() )
{
@@ -254,10 +254,10 @@
VertexVector mVtxs; // set of vertices.
};
+double tmpp[3] = {0,0,0};
+template<> VertexPosition VertexLess<VertexPosition>::mFind = tmpp;
+template<> std::vector<VertexPosition > *VertexLess<VertexPosition>::mList =0;
-VertexPosition VertexLess<VertexPosition>::mFind;
-std::vector<VertexPosition > *VertexLess<VertexPosition>::mList=0;
-
enum RDIFF
{
RD_EQUAL,
@@ -288,6 +288,7 @@
}
+template<>
bool VertexLess<VertexPosition>::operator()(int v1,int v2) const
{
bool ret = false;
Index: ConvexDecomposition/ConvexDecomposition.cpp
===================================================================
--- ConvexDecomposition/ConvexDecomposition.cpp (revision 8299)
+++ ConvexDecomposition/ConvexDecomposition.cpp (working copy)
@@ -66,7 +66,7 @@
#include "cd_vector.h"
#include "cd_hull.h"
#include "bestfit.h"
-#include "PlaneTri.h"
+#include "planetri.h"
#include "vlookup.h"
#include "splitplane.h"
#include "meshvolume.h"
@@ -760,3 +760,3 @@
- assert( fcount >= 3 && fcount <= 4);
- assert( bcount >= 3 && bcount <= 4);
-
+ if ( fcount >= 3 && fcount <= 4)
+ if ( bcount >= 3 && bcount <= 4)
+ {
@@ -777,1 +777,1 @@
-
+ }
Index: ConvexDecomposition/splitplane.cpp
===================================================================
--- ConvexDecomposition/splitplane.cpp (revision 8299)
+++ ConvexDecomposition/splitplane.cpp (working copy)
@@ -67,7 +67,7 @@
#include "cd_hull.h"
#include "cd_wavefront.h"
#include "bestfit.h"
-#include "PlaneTri.h"
+#include "planetri.h"
#include "vlookup.h"
#include "meshvolume.h"
#include "bestfitobb.h"
Index: ConvexDecomposition/cd_vector.h
===================================================================
--- ConvexDecomposition/cd_vector.h (revision 8299)
+++ ConvexDecomposition/cd_vector.h (working copy)
@@ -309,12 +309,12 @@
Type FastMagnitude(void) const
{
- return Type(fast_sqrt(x * x + y * y + z * z));
+ return Type(sqrt(x * x + y * y + z * z));
};
Type FasterMagnitude(void) const
{
- return Type(faster_sqrt(x * x + y * y + z * z));
+ return Type(sqrt(x * x + y * y + z * z));
};
void Lerp(const Vector3d<Type>& from,const Vector3d<Type>& to,double slerp)
@@ -436,13 +436,13 @@
Type FastLength(void) const // length of vector.
{
- return Type(fast_sqrt( x*x + y*y + z*z ));
+ return Type(sqrt( x*x + y*y + z*z ));
};
Type FasterLength(void) const // length of vector.
{
- return Type(faster_sqrt( x*x + y*y + z*z ));
+ return Type(sqrt( x*x + y*y + z*z ));
};
Type Length2(void) const // squared distance, prior to square root.
@@ -518,7 +518,7 @@
inline double FastNormalize(void) // normalize to a unit vector, returns distance.
{
- double d = fast_sqrt( static_cast< double >( x*x + y*y + z*z ) );
+ double d = sqrt( static_cast< double >( x*x + y*y + z*z ) );
if ( d > 0 )
{
double r = 1.0f / d;
@@ -535,7 +535,7 @@
inline double FasterNormalize(void) // normalize to a unit vector, returns distance.
{
- double d = faster_sqrt( static_cast< double >( x*x + y*y + z*z ) );
+ double d = sqrt( static_cast< double >( x*x + y*y + z*z ) );
if ( d > 0 )
{
double r = 1.0f / d;
@@ -1029,7 +1029,8 @@
void Zero(void)
{
- x = y = z = 0;
+ x = 0;
+ y = 0;
};
Vector2d negative(void) const
@@ -1047,12 +1048,12 @@
Type fastmagnitude(void) const
{
- return (Type) fast_sqrt(x * x + y * y );
+ return (Type) sqrt(x * x + y * y );
}
Type fastermagnitude(void) const
{
- return (Type) faster_sqrt( x * x + y * y );
+ return (Type) sqrt( x * x + y * y );
}
void Reflection(Vector2d &a,Vector2d &b); // compute reflection vector.
@@ -1064,12 +1065,12 @@
Type FastLength(void) const // length of vector.
{
- return Type(fast_sqrt( x*x + y*y ));
+ return Type(sqrt( x*x + y*y ));
};
Type FasterLength(void) const // length of vector.
{
- return Type(faster_sqrt( x*x + y*y ));
+ return Type(sqrt( x*x + y*y ));
};
Type Length2(void) // squared distance, prior to square root.
@@ -1090,7 +1091,7 @@
Type dx = a.x - x;
Type dy = a.y - y;
Type d = dx*dx+dy*dy;
- return fast_sqrt(d);
+ return sqrt(d);
};
Type FasterDistance(const Vector2d &a) const // distance between two points.
@@ -1098,7 +1099,7 @@
Type dx = a.x - x;
Type dy = a.y - y;
Type d = dx*dx+dy*dy;
- return faster_sqrt(d);
+ return sqrt(d);
};
Type Distance2(Vector2d &a) // squared distance.
Index: ConvexDecomposition/float_math.cpp
===================================================================
--- ConvexDecomposition/float_math.cpp (revision 8299)
+++ ConvexDecomposition/float_math.cpp (working copy)
@@ -212,8 +212,14 @@
matrix[1*4+2] = 2 * ( yz + wx );
matrix[2*4+2] = 1 - 2 * ( xx + yy );
- matrix[3*4+0] =(double) matrix[3*4+1] = matrix[3*4+2] = 0.0f;
- matrix[0*4+3] =(double) matrix[1*4+3] = matrix[2*4+3] = 0.0f;
+ matrix[3*4+0] = 0.0f;
+ matrix[3*4+1] = 0.0f;
+ matrix[3*4+2] = 0.0f;
+
+ matrix[0*4+3] = 0.0f;
+ matrix[1*4+3] = 0.0f;
+ matrix[2*4+3] = 0.0f;
+
matrix[3*4+3] =(double) 1.0f;
}
Index: ConvexDecomposition/cd_wavefront.cpp
===================================================================
--- ConvexDecomposition/cd_wavefront.cpp (revision 8299)
+++ ConvexDecomposition/cd_wavefront.cpp (working copy)
@@ -672,7 +672,7 @@
const char *foo = argv[0];
if ( *foo != '#' )
{
- if ( stricmp(argv[0],"v") == 0 && argc == 4 )
+ if ( strcmp(argv[0],"v") == 0 && argc == 4 )
{
double vx = (double) atof( argv[1] );
double vy = (double) atof( argv[2] );
@@ -681,14 +681,14 @@
mVerts.push_back(vy);
mVerts.push_back(vz);
}
- else if ( stricmp(argv[0],"vt") == 0 && argc == 3 )
+ else if ( strcmp(argv[0],"vt") == 0 && argc == 3 )
{
double tx = (double) atof( argv[1] );
double ty = (double) atof( argv[2] );
mTexels.push_back(tx);
mTexels.push_back(ty);
}
- else if ( stricmp(argv[0],"vn") == 0 && argc == 4 )
+ else if ( strcmp(argv[0],"vn") == 0 && argc == 4 )
{
double normalx = (double) atof(argv[1]);
double normaly = (double) atof(argv[2]);
@@ -697,7 +697,7 @@
mNormals.push_back(normaly);
mNormals.push_back(normalz);
}
- else if ( stricmp(argv[0],"f") == 0 && argc >= 4 )
+ else if ( strcmp(argv[0],"f") == 0 && argc >= 4 )
{
GeometryVertex v[32];
Index: Makefile
===================================================================
--- Makefile (revision 0)
+++ Makefile (revision 8581)
@@ -0,0 +1,93 @@
+
+
+OBJS = DecomposeSample.o \
+ ConvexDecomposition/bestfit.o ConvexDecomposition/float_math.o \
+ ConvexDecomposition/bestfitobb.o ConvexDecomposition/meshvolume.o \
+ ConvexDecomposition/cd_hull.o ConvexDecomposition/planetri.o \
+ ConvexDecomposition/cd_wavefront.o ConvexDecomposition/raytri.o \
+ ConvexDecomposition/concavity.o ConvexDecomposition/splitplane.o \
+ ConvexDecomposition/ConvexDecomposition.o ConvexDecomposition/triangulate.o \
+ ConvexDecomposition/fitsphere.o ConvexDecomposition/vlookup.o
+
+HEADERS = \
+ ConvexDecomposition/bestfit.h \
+ ConvexDecomposition/bestfitobb.h \
+ ConvexDecomposition/cd_hull.h \
+ ConvexDecomposition/cd_vector.h \
+ ConvexDecomposition/cd_wavefront.h \
+ ConvexDecomposition/concavity.h \
+ ConvexDecomposition/ConvexDecomposition.h \
+ ConvexDecomposition/fitsphere.h \
+ ConvexDecomposition/float_math.h \
+ ConvexDecomposition/meshvolume.h \
+ ConvexDecomposition/planetri.h \
+ ConvexDecomposition/raytri.h \
+ ConvexDecomposition/splitplane.h \
+ ConvexDecomposition/triangulate.h \
+ ConvexDecomposition/vlookup.h
+
+CC = g++
+
+DEBUG = -ggdb
+
+CFLAGS = -IConvexDecomposition -Wall -c $(DEBUG)
+
+LFLAGS = $(DEBUG)
+
+convex_decomposition: $(OBJS)
+ $(CC) $(LFLAGS) $(OBJS) -o convex_decomposition
+
+DecomposeSample.o: DecomposeSample.cpp
+ $(CC) $(CFLAGS) DecomposeSample.cpp -o $@
+
+ConvexDecomposition/bestfit.o: ConvexDecomposition/bestfit.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/bestfit.cpp -o $@
+
+ConvexDecomposition/bestfitobb.o: ConvexDecomposition/bestfitobb.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/bestfitobb.cpp -o $@
+
+ConvexDecomposition/cd_hull.o: ConvexDecomposition/cd_hull.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/cd_hull.cpp -o $@
+
+ConvexDecomposition/cd_wavefront.o: ConvexDecomposition/cd_wavefront.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/cd_wavefront.cpp -o $@
+
+ConvexDecomposition/concavity.o: ConvexDecomposition/concavity.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/concavity.cpp -o $@
+
+ConvexDecomposition/ConvexDecomposition.o: ConvexDecomposition/ConvexDecomposition.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/ConvexDecomposition.cpp -o $@
+
+ConvexDecomposition/fitsphere.o: ConvexDecomposition/fitsphere.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/fitsphere.cpp -o $@
+
+ConvexDecomposition/float_math.o: ConvexDecomposition/float_math.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/float_math.cpp -o $@
+
+ConvexDecomposition/meshvolume.o: ConvexDecomposition/meshvolume.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/meshvolume.cpp -o $@
+
+ConvexDecomposition/planetri.o: ConvexDecomposition/planetri.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/planetri.cpp -o $@
+
+ConvexDecomposition/raytri.o: ConvexDecomposition/raytri.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/raytri.cpp -o $@
+
+ConvexDecomposition/splitplane.o: ConvexDecomposition/splitplane.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/splitplane.cpp -o $@
+
+ConvexDecomposition/triangulate.o: ConvexDecomposition/triangulate.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/triangulate.cpp -o $@
+
+ConvexDecomposition/vlookup.o: ConvexDecomposition/vlookup.cpp ConvexDecomposition/vlookup.cpp
+ $(CC) $(CFLAGS) ConvexDecomposition/vlookup.cpp -o $@
+
+install:
+ cp convex_decomposition ../../convex_decomposition/bin/
+
+clean:
+ \rm *.o */*.o convex_decomposition
+
+tar:
+ tar cfv ConvexDecomposition.tar DecomposeSample.cpp convex_decomposition Makefile \
+ ConvexDecomposition
Index: DecomposeSample.cpp
===================================================================
--- DecomposeSample.cpp (revision 8299)
+++ DecomposeSample.cpp (working copy)
@@ -6,7 +6,7 @@
#include <vector>
-#include "./ConvexDecomposition/convexdecomposition.h"
+#include "./ConvexDecomposition/ConvexDecomposition.h"
#include "./ConvexDecomposition/cd_wavefront.h"
using namespace ConvexDecomposition;
@@ -227,7 +227,7 @@
if ( fph )
{
- printf("Saving convex decomposition of %d hulls to COLLADA file '%s'\r\n", mHulls.size(), scratch );
+ printf("Saving convex decomposition of %d hulls to COLLADA file '%s'\r\n", (int)mHulls.size(), scratch );
fprintf(fph,"<?xml version=\"1.0\" encoding=\"utf-8\"?>\r\n");
fprintf(fph,"<COLLADA version=\"1.4.0\" xmlns=\"http://www.collada.org/2005/11/COLLADASchema\">\r\n");
@@ -537,7 +537,7 @@
};
-void main(int argc,const char **argv)
+int main(int argc,const char **argv)
{
if ( argc < 2 )
{
Index: DecomposeSample.cpp
===================================================================
--- DecomposeSample.cpp (revision 8299)
+++ DecomposeSample.cpp (working copy)
@@ -67,3 +67,3 @@
strcpy(mBaseName,fname);
- char *dot = strstr(mBaseName,".");
+ char *dot = strstr(mBaseName,".obj");
if ( dot ) *dot = 0;
Index: ConvexDecomposition/cd_wavefront.cpp
===================================================================
--- ConvexDecomposition/cd_wavefront.cpp (revision 8299)
+++ ConvexDecomposition/cd_wavefront.cpp (working copy)
@@ -573,6 +573,7 @@
FloatVector mNormals;
GeometryInterface *mCallback;
+ friend class WavefrontObj;
};
@@ -839,7 +840,17 @@
memcpy(mIndices, &indices[0], sizeof(int)*mTriCount*3);
ret = mTriCount;
}
-
+ else if( obj.mVerts.size() > 0 ) {
+ // take consecutive vertices
+ mVertexCount = obj.mVerts.size()/3;
+ mVertices = new double[mVertexCount*3];
+ memcpy( mVertices, &obj.mVerts[0], sizeof(double)*mVertexCount*3 );
+ mTriCount = mVertexCount/3;
+ mIndices = new int[mTriCount*3*sizeof(int)];
+ for(int i = 0; i < mVertexCount; ++i)
+ mIndices[i] = i;
+ ret = mTriCount;
+ }
return ret;
}

29
convex_decomposition/manifest.xml Normal file
View File

@ -0,0 +1,29 @@
<package>
<description brief="Convex Mesh Generation Library">
<p>
<tt>convex_decomposition</tt> contains the Convex Mesh Generation Library, by John Ratcliff. This library is used to auto-generate convex decomposed meshes for
collision and visualization of robot links.
</p>
<p>
We are using the latest and the only version available. We plan on deprecating this package
once we find a better alternative.
</p>
<p>
The Convex Mesh Generation Library is not provided by
any of the major OS package managers, and this version provided
here contains patches required for compilation.
These patches have not been submitted back as there
appears to be no available revision control.
</p>
</description>
<author>John Ratcliff</author>
<license>MIT</license>
<review status="3rdparty doc reviewed" notes=""/>
<url>http://www.amillionpixels.us/ConvexDecomposition.zip</url>
<rosdep name="unzip"/>
<platform os="ubuntu" version="9.04"/>
<platform os="ubuntu" version="9.10"/>
<platform os="ubuntu" version="10.04"/>
</package>

9
ivcon/CMakeLists.txt Normal file
View File

@ -0,0 +1,9 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rosbuild_init()
#uncomment for profiling
set(ROS_LINK_FLAGS "-lm" ${ROS_LINK_FLAGS})
rosbuild_add_executable(bin/ivcon src/ivcon.c)

1
ivcon/Makefile Normal file
View File

@ -0,0 +1 @@
include $(shell rospack find mk)/cmake.mk

23
ivcon/manifest.xml Normal file
View File

@ -0,0 +1,23 @@
<package>
<description brief="Mesh Conversion Utility">
Mesh Conversion Utility
Used to generate '.iv' files from '.stl' files. This package has not
been changed since 2001 and appears to be very stable. We plan on
keeping this package in this revision for mesh conversions. This
package is only available as a single source file for download. There
are no local modifications to this package.
</description>
<author>John Burkardt</author>
<license>GPL</license>
<review status="3rdparty doc reviewed" notes=""/>
<url>https://sourceforge.net/projects/ivcon/</url>
<export>
<cpp lflags="" cflags=""/>
</export>
<platform os="ubuntu" version="9.04"/>
<platform os="ubuntu" version="9.10"/>
<platform os="ubuntu" version="10.04"/>
</package>

16707
ivcon/src/ivcon.c Normal file
View File

File diff suppressed because it is too large Load Diff

5
joint_state_publisher/CMakeLists.txt
View File

@ -1,5 +0,0 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rosbuild_init()

206
joint_state_publisher/joint_state_publisher
View File

@ -1,206 +0,0 @@
#!/usr/bin/python
import roslib; roslib.load_manifest('joint_state_publisher')
import rospy
import wx
import xml.dom.minidom
from sensor_msgs.msg import JointState
from math import pi
from threading import Thread
RANGE = 10000
def get_param(name, value=None):
private = "~%s" % name
if rospy.has_param(private):
return rospy.get_param(private)
elif rospy.has_param(name):
return rospy.get_param(name)
else:
return value
class JointStatePublisher():
def __init__(self):
description = get_param('robot_description')
robot = xml.dom.minidom.parseString(description).getElementsByTagName('robot')[0]
self.free_joints = {}
self.joint_list = [] # for maintaining the original order of the joints
self.dependent_joints = get_param("dependent_joints", {})
# Find all non-fixed joints
for child in robot.childNodes:
if child.nodeType is child.TEXT_NODE:
continue
if child.localName == 'joint':
jtype = child.getAttribute('type')
if jtype == 'fixed':
continue
name = child.getAttribute('name')
if jtype == 'continuous':
minval = -pi
maxval = pi
else:
limit = child.getElementsByTagName('limit')[0]
minval = float(limit.getAttribute('lower'))
maxval = float(limit.getAttribute('upper'))
if name in self.dependent_joints:
continue
if minval > 0 or maxval < 0:
zeroval = (maxval + minval)/2
else:
zeroval = 0
joint = {'min':minval, 'max':maxval, 'zero':zeroval, 'value':zeroval }
self.free_joints[name] = joint
self.joint_list.append(name)
use_gui = get_param("use_gui", False)
if use_gui:
app = wx.App()
self.gui = JointStatePublisherGui("Joint State Publisher", self)
self.gui.Show()
Thread(target=app.MainLoop).start()
else:
self.gui = None
source_list = get_param("source_list", [])
self.sources = []
for source in source_list:
self.sources.append(rospy.Subscriber(source, JointState, self.source_cb))
self.pub = rospy.Publisher('joint_states', JointState)
def source_cb(self, msg):
for i in range(len(msg.name)):
name = msg.name[i]
position = msg.position[i]
if name in self.free_joints:
joint = self.free_joints[name]
joint['value'] = position
if self.gui is not None:
self.gui.update_sliders()
def loop(self):
hz = get_param("rate", 10) # 10hz
r = rospy.Rate(hz)
# Publish Joint States
while not rospy.is_shutdown():
msg = JointState()
msg.header.stamp = rospy.Time.now()
# Add Free Joints
for (name,joint) in self.free_joints.items():
msg.name.append(str(name))
msg.position.append(joint['value'])
# Add Dependent Joints
for (name,param) in self.dependent_joints.items():
parent = param['parent']
baseval = self.free_joints[parent]['value']
value = baseval * param.get('factor', 1)
msg.name.append(str(name))
msg.position.append(value)
self.pub.publish(msg)
r.sleep()
class JointStatePublisherGui(wx.Frame):
def __init__(self, title, jsp):
wx.Frame.__init__(self, None, -1, title, (-1, -1));
self.jsp = jsp
self.joint_map = {}
panel = wx.Panel(self, wx.ID_ANY);
box = wx.BoxSizer(wx.VERTICAL)
font = wx.Font(9, wx.SWISS, wx.NORMAL, wx.BOLD)
### Sliders ###
for name in self.jsp.joint_list:
joint = self.jsp.free_joints[name]
if joint['min'] == joint['max']:
continue
row = wx.GridSizer(1,2)
label = wx.StaticText(panel, -1, name)
label.SetFont(font)
row.Add(label, 1, wx.ALIGN_CENTER_VERTICAL)
display = wx.TextCtrl (panel, value=str(0),
style=wx.TE_READONLY | wx.ALIGN_RIGHT)
row.Add(display, flag= wx.ALIGN_RIGHT| wx.ALIGN_CENTER_VERTICAL)
box.Add(row, 1, wx.EXPAND)
slider = wx.Slider(panel, -1, RANGE/2, 0, RANGE,
style= wx.SL_AUTOTICKS | wx.SL_HORIZONTAL)
slider.SetFont(font)
box.Add(slider, 1, wx.EXPAND)
self.joint_map[name] = {'slidervalue':0, 'display':display,
'slider':slider, 'joint':joint}
### Buttons ###
self.ctrbutton = wx.Button(panel, 1, 'Center')
self.Bind(wx.EVT_SLIDER, self.sliderUpdate)
wx.EVT_BUTTON(self, 1, self.center_event)
box.Add(self.ctrbutton, 0, wx.EXPAND)
panel.SetSizer(box)
self.center()
box.Fit(self)
self.update_values()
def update_values(self):
for (name,joint_info) in self.joint_map.items():
purevalue = joint_info['slidervalue']
joint = joint_info['joint']
value = self.sliderToValue(purevalue, joint)
joint['value'] = value
self.update_sliders()
def update_sliders(self):
for (name,joint_info) in self.joint_map.items():
joint = joint_info['joint']
joint_info['slidervalue'] = self.valueToSlider(joint['value'], joint)
joint_info['slider'].SetValue(joint_info['slidervalue'])
joint_info['display'].SetValue("%.2f"%joint['value'])
def center_event(self, event):
self.center()
def center(self):
rospy.loginfo("Centering")
for (name,joint_info) in self.joint_map.items():
joint = joint_info['joint']
joint_info['slidervalue'] = self.valueToSlider(joint['zero'], joint)
self.update_values()
def sliderUpdate(self, event):
for (name,joint_info) in self.joint_map.items():
joint_info['slidervalue'] = joint_info['slider'].GetValue()
self.update_values()
def valueToSlider(self, value, joint):
return (value - joint['min']) * float(RANGE) / (joint['max'] - joint['min'])
def sliderToValue(self, slider, joint):
pctvalue = slider / float(RANGE)
return joint['min'] + (joint['max']-joint['min']) * pctvalue
if __name__ == '__main__':
try:
rospy.init_node('joint_state_publisher')
jsp = JointStatePublisher()
jsp.loop()
except rospy.ROSInterruptException: pass

15
joint_state_publisher/manifest.xml
View File

@ -1,15 +0,0 @@
<package>
<description brief="A node for publishing joint angles">
A node for publishing joint angles, either through a gui, or with
default values.
</description>
<author>David Lu!!</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/joint_state_publisher</url>
<depend package="rospy" />
<depend package="sensor_msgs" />
<rosdep name="wxpython" />
</package>

BIN
joint_state_publisher/screenshot.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 11 KiB

18
arm_kinematics/CMakeLists.txt → kdl_parser/CMakeLists.txt
View File

@ -11,9 +11,25 @@ include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rosbuild_init() rosbuild_init()
rosbuild_add_boost_directories()
#set the default path for built executables to the "bin" directory #set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory #set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
rosbuild_add_executable(arm_kinematics arm_kinematics.cpp) #uncomment if you have defined messages
#rosbuild_genmsg()
#uncomment if you have defined services
#rosbuild_gensrv()
rosbuild_add_library(${PROJECT_NAME} src/kdl_parser.cpp)
rosbuild_add_executable(check_kdl_parser src/check_kdl_parser.cpp )
target_link_libraries(check_kdl_parser ${PROJECT_NAME})
rosbuild_add_executable(test_parser test/test_kdl_parser.cpp )
target_link_libraries(test_parser ${PROJECT_NAME})
rosbuild_add_gtest_build_flags(test_parser)
rosbuild_add_rostest(${PROJECT_SOURCE_DIR}/test/test_kdl_parser.launch)

0
joint_state_publisher/Makefile → kdl_parser/Makefile
View File

83
kdl_parser/include/kdl_parser/kdl_parser.hpp Normal file
View File

@ -0,0 +1,83 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#ifndef KDL_PARSER_H
#define KDL_PARSER_H
#include <kdl/tree.hpp>
#include <string>
#include <urdf/model.h>
namespace kdl_parser{
/** Constructs a KDL tree from a file, given the file name
* \param file The filename from where to read the xml
* \param tree The resulting KDL Tree
* returns true on success, false on failure
*/
bool treeFromFile(const std::string& file, KDL::Tree& tree);
/** Constructs a KDL tree from the parameter server, given the parameter name
* \param param the name of the parameter on the parameter server
* \param tree The resulting KDL Tree
* returns true on success, false on failure
*/
bool treeFromParam(const std::string& param, KDL::Tree& tree);
/** Constructs a KDL tree from a string containing xml
* \param xml A string containting the xml description of the robot
* \param tree The resulting KDL Tree
* returns true on success, false on failure
*/
bool treeFromString(const std::string& xml, KDL::Tree& tree);
/** Constructs a KDL tree from a TiXmlDocument
* \param xml_doc The TiXmlDocument containting the xml description of the robot
* \param tree The resulting KDL Tree
* returns true on success, false on failure
*/
bool treeFromXml(TiXmlDocument *xml_doc, KDL::Tree& tree);
/** Constructs a KDL tree from a URDF robot model
* \param robot_model The URDF robot model
* \param tree The resulting KDL Tree
* returns true on success, false on failure
*/
bool treeFromUrdfModel(const urdf::Model& robot_model, KDL::Tree& tree);
}
#endif

26
kdl_parser/manifest.xml Normal file
View File

@ -0,0 +1,26 @@
<package>
<description brief="Package to parse urdf in to kdl tree">
The Kinematics and Dynamics Library (KDL) defines a tree structure to represent the kinematic and dynamic parameters of a robot mechanism. <tt>kdl_parser</tt> provides tools to construct a KDL tree from the robot representation in the <a href="http://www.ros.org/wiki/urdf">urdf</a> package.
</description>
<author>Wim Meeussen meeussen@willowgarage.com </author>
<license>BSD</license>
<review status="Doc reviewed" notes=""/>
<url>http://ros.org/wiki/kdl_parser</url>
<depend package="kdl" />
<depend package="tinyxml" />
<depend package="urdf" />
<depend package="roscpp" />
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lkdl_parser"/>
</export>
<platform os="ubuntu" version="9.04"/>
<platform os="ubuntu" version="9.10"/>
<platform os="ubuntu" version="10.04"/>
</package>

77
kdl_parser/src/check_kdl_parser.cpp Normal file
View File

@ -0,0 +1,77 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>
#include "kdl_parser/kdl_parser.hpp"
#include <iostream>
using namespace KDL;
using namespace std;
using namespace urdf;
void printLink(const SegmentMap::const_iterator& link, const std::string& prefix)
{
cout << prefix << "- Segment " << link->second.segment.getName() << " has " << link->second.children.size() << " children" << endl;
for (unsigned int i=0; i<link->second.children.size(); i++)
printLink(link->second.children[i], prefix + " ");
}
int main(int argc, char** argv)
{
if (argc < 2){
std::cerr << "Expect xml file to parse" << std::endl;
return -1;
}
Model robot_model;
if (!robot_model.initFile(argv[1]))
{cerr << "Could not generate robot model" << endl; return false;}
Tree my_tree;
if (!kdl_parser::treeFromUrdfModel(robot_model, my_tree))
{cerr << "Could not extract kdl tree" << endl; return false;}
// walk through tree
cout << " ======================================" << endl;
cout << " Tree has " << my_tree.getNrOfSegments() << " link(s) and a root link" << endl;
cout << " ======================================" << endl;
SegmentMap::const_iterator root = my_tree.getRootSegment();
printLink(root, "");
}

180
kdl_parser/src/kdl_parser.cpp Normal file
View File

@ -0,0 +1,180 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#include "kdl_parser/kdl_parser.hpp"
#include <kdl/frames_io.hpp>
using namespace std;
using namespace KDL;
namespace kdl_parser{
// construct vector
Vector toKdl(urdf::Vector3 v)
{
return Vector(v.x, v.y, v.z);
}
// construct rotation
Rotation toKdl(urdf::Rotation r)
{
return Rotation::Quaternion(r.x, r.y, r.z, r.w);
}
// construct pose
Frame toKdl(urdf::Pose p)
{
return Frame(toKdl(p.rotation), toKdl(p.position));
}
// construct joint
Joint toKdl(boost::shared_ptr<urdf::Joint> jnt)
{
Frame F_parent_jnt = toKdl(jnt->parent_to_joint_origin_transform);
switch (jnt->type){
case urdf::Joint::FIXED:{
return Joint(jnt->name, Joint::None);
}
case urdf::Joint::REVOLUTE:{
Vector axis = toKdl(jnt->axis);
return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
}
case urdf::Joint::CONTINUOUS:{
Vector axis = toKdl(jnt->axis);
return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
}
case urdf::Joint::PRISMATIC:{
Vector axis = toKdl(jnt->axis);
return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::TransAxis);
}
default:{
ROS_WARN("Converting unknown joint type of joint '%s' into a fixed joint", jnt->name.c_str());
return Joint(jnt->name, Joint::None);
}
}
return Joint();
}
// construct inertia
RigidBodyInertia toKdl(boost::shared_ptr<urdf::Inertial> i)
{
Frame origin = toKdl(i->origin);
// kdl specifies the inertia in the reference frame of the link, the urdf specifies the inertia in the inertia reference frame
return origin.M * RigidBodyInertia(i->mass, origin.p, RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz));
}
// recursive function to walk through tree
bool addChildrenToTree(boost::shared_ptr<const urdf::Link> root, Tree& tree)
{
std::vector<boost::shared_ptr<urdf::Link> > children = root->child_links;
ROS_DEBUG("Link %s had %i children", root->name.c_str(), (int)children.size());
// constructs the optional inertia
RigidBodyInertia inert(0);
if (root->inertial)
inert = toKdl(root->inertial);
// constructs the kdl joint
Joint jnt = toKdl(root->parent_joint);
// construct the kdl segment
Segment sgm(root->name, jnt, toKdl(root->parent_joint->parent_to_joint_origin_transform), inert);
// add segment to tree
tree.addSegment(sgm, root->parent_joint->parent_link_name);
// recurslively add all children
for (size_t i=0; i<children.size(); i++){
if (!addChildrenToTree(children[i], tree))
return false;
}
return true;
}
bool treeFromFile(const string& file, Tree& tree)
{
TiXmlDocument urdf_xml;
urdf_xml.LoadFile(file);
return treeFromXml(&urdf_xml, tree);
}
bool treeFromParam(const string& param, Tree& tree)
{
urdf::Model robot_model;
if (!robot_model.initParam(param)){
ROS_ERROR("Could not generate robot model");
return false;
}
return treeFromUrdfModel(robot_model, tree);
}
bool treeFromString(const string& xml, Tree& tree)
{
TiXmlDocument urdf_xml;
urdf_xml.Parse(xml.c_str());
return treeFromXml(&urdf_xml, tree);
}
bool treeFromXml(TiXmlDocument *xml_doc, Tree& tree)
{
urdf::Model robot_model;
if (!robot_model.initXml(xml_doc)){
ROS_ERROR("Could not generate robot model");
return false;
}
return treeFromUrdfModel(robot_model, tree);
}
bool treeFromUrdfModel(const urdf::Model& robot_model, Tree& tree)
{
tree = Tree(robot_model.getRoot()->name);
// add all children
for (size_t i=0; i<robot_model.getRoot()->child_links.size(); i++)
if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree))
return false;
return true;
}
}

3382
kdl_parser/test/pr2_desc.xml Normal file
View File

File diff suppressed because it is too large Load Diff

21
kdl_parser/test/pr2_desc_bracket.xml Normal file
View File

@ -0,0 +1,21 @@
<?xml version="1.0" ?>
<robot name="pr2">
<joint name="fl_caster_rotation_joint" type="revolute">
<axis xyz="0 0 1 /"/>
<anchor xyz="0 0 0"/>
<limit effort="100" k_velocity="10" velocity="100"/>
<calibration reference_position="0.0" values="1.5 -1"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="fl_caster_rotation_link">
<parent name="base_link"/>
<joint name="fl_caster_rotation_joint"/>
<inertial>
<mass value="3.473082"/>
<com xyz="0 0 0.07"/>
<inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
</inertial>
</robot>

21
kdl_parser/test/pr2_desc_bracket2.xml Normal file
View File

@ -0,0 +1,21 @@
<?xml version="1.0" ?>
<robot name="pr2">
<joint name="fl_caster_rotation_joint" type="revolute">
<axis xyz="0 0 1"/>
<anchor xyz="0 0 0"/>
<limit effort="100" k_velocity="10" velocity="100"/>
<calibration reference_position="0.0" values="1.5 -1"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="fl_caster_rotation_link">
<parent name="base_link"/>
<joint name="fl_caster_rotation_joint"/>
<inertial>
<mass value="3.473082}"/>
<com xyz="0 0 0.07"/>
<inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
</inertial>
</robot>

22
kdl_parser/test/pr2_desc_vector.xml Normal file
View File

@ -0,0 +1,22 @@
<?xml version="1.0" ?>
<robot name="pr2">
<joint name="fl_caster_rotation_joint" type="revolute">
<axis xyz="0 0"/>
<anchor xyz="0 0 0"/>
<limit effort="100" k_velocity="10" velocity="100"/>
<calibration reference_position="0.0" values="1.5 -1"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="fl_caster_rotation_link">
<parent name="base_link"/>
<origin rpy="0 0 1 9" xyz="0.2225 0.2225 0.0282"/>
<joint name="fl_caster_rotation_joint"/>
<inertial>
<mass value="3.473082"/>
<com xyz="0 0 0.07"/>
<inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
</inertial>
</robot>

96
kdl_parser/test/test_kdl_parser.cpp Normal file
View File

@ -0,0 +1,96 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#include <string>
#include <gtest/gtest.h>
#include <ros/ros.h>
#include "kdl_parser/kdl_parser.hpp"
using namespace kdl_parser;
int g_argc;
char** g_argv;
class TestParser : public testing::Test
{
public:
KDL::Tree my_tree;
protected:
/// constructor
TestParser()
{
}
/// Destructor
~TestParser()
{
}
};
TEST_F(TestParser, test)
{
for (int i=1; i<g_argc-2; i++){
ROS_ERROR("Testing file %s", g_argv[i]);
ASSERT_FALSE(treeFromFile(g_argv[i], my_tree));
}
ASSERT_TRUE(treeFromFile(g_argv[g_argc-1], my_tree));
ASSERT_EQ(my_tree.getNrOfJoints(), (unsigned int)44);
ASSERT_EQ(my_tree.getNrOfSegments(), (unsigned int)81);
ASSERT_TRUE(my_tree.getSegment("base_footprint") == my_tree.getRootSegment());
ASSERT_EQ(my_tree.getRootSegment()->second.children.size(), (unsigned int)1);
ASSERT_TRUE(my_tree.getSegment("base_link")->second.parent == my_tree.getRootSegment());
ASSERT_EQ(my_tree.getSegment("base_link")->second.segment.getInertia().getMass(), 116.0);
ASSERT_NEAR(my_tree.getSegment("base_link")->second.segment.getInertia().getRotationalInertia().data[0], 15.6107, 0.001);
SUCCEED();
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "test_kdl_parser");
g_argc = argc;
g_argv = argv;
return RUN_ALL_TESTS();
}

6
kdl_parser/test/test_kdl_parser.launch Normal file
View File

@ -0,0 +1,6 @@
<launch>
<test test-name="test_kdl_parser" pkg="kdl_parser" type="test_parser" args="$(find kdl_parser)/test/pr2_desc_vector.xml \
$(find kdl_parser)/test/pr2_desc_bracket.xml \
$(find kdl_parser)/test/pr2_desc_bracket2.xml \
$(find kdl_parser)/test/pr2_desc.xml" />
</launch>

19
urdf_tutorial/CMakeLists.txt → resource_retriever/CMakeLists.txt
View File

@ -21,10 +21,15 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#uncomment if you have defined services #uncomment if you have defined services
#rosbuild_gensrv() #rosbuild_gensrv()
#common commands for building c++ executables and libraries include(FindCURL)
#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
#target_link_libraries(${PROJECT_NAME} another_library) if(NOT CURL_FOUND)
#rosbuild_add_boost_directories() message("CURL not found! Aborting...")
#rosbuild_link_boost(${PROJECT_NAME} thread) fail()
#rosbuild_add_executable(example examples/example.cpp) endif(NOT CURL_FOUND)
#target_link_libraries(example ${PROJECT_NAME})
include_directories(${CURL_INCLUDE_DIRS})
rosbuild_add_library(${PROJECT_NAME} src/retriever.cpp)
target_link_libraries(${PROJECT_NAME} ${CURL_LIBRARIES})
add_subdirectory(test EXCLUDE_FROM_ALL)

0
urdf_tutorial/Makefile → resource_retriever/Makefile
View File

86
resource_retriever/include/resource_retriever/retriever.h Normal file
View File

@ -0,0 +1,86 @@
/*
* Copyright (C) 2009, Willow Garage, Inc.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef RESOURCE_RETRIEVER_RETRIEVER_H
#define RESOURCE_RETRIEVER_RETRIEVER_H
#include <stdint.h>
#include <string>
#include <boost/shared_array.hpp>
#include <stdexcept>
typedef void CURL;
namespace resource_retriever
{
class Exception : public std::runtime_error
{
public:
Exception(const std::string& file, const std::string& error_msg)
: std::runtime_error("Error retrieving file [" + file + "]: " + error_msg)
{}
};
/**
* \brief A combination of a pointer to data in memory along with the data's size.
*/
struct MemoryResource
{
MemoryResource()
: size(0)
{}
boost::shared_array<uint8_t> data;
uint32_t size;
};
/**
* \brief Retrieves files from from a url. Caches a CURL handle so multiple accesses to a single url
* will keep connections open.
*/
class Retriever
{
public:
Retriever();
~Retriever();
/**
* \brief Get a file and store it in memory
* \param url The url to retrieve. package://package/file will be turned into the correct file:// invocation
* \return The file, loaded into memory
* \throws resource_retriever::Exception if anything goes wrong.
*/
MemoryResource get(const std::string& url);
private:
CURL* curl_handle_;
};
} // namespace resource_retriever
#endif // RESOURCE_RETRIEVER_RETRIEVER_H

14
resource_retriever/mainpage.dox Normal file
View File

@ -0,0 +1,14 @@
/**
\mainpage
\htmlinclude manifest.html
\b resource_retriever is a package used for downloading files from a url. It also provides special handling of the package:// prefix, allowing things to be referenced
by path relative to a package.
\section codeapi Code API
- resource_retriever::Retriever -- Use this class to download files.
*/

29
resource_retriever/manifest.xml Normal file
View File

@ -0,0 +1,29 @@
<package>
<description brief="resource_retriever">
This package retrieves data from url-format files such as http://,
ftp://, package:// file://, etc., and loads the data into memory.
The package:// url for ros packages is translated into a local
file:// url. The resourse retriever was initially designed to load
mesh files into memory, but it can be used for any type of
data. The resource retriever is based on the the libcurl library.
</description>
<author>Josh Faust (jfaust@willowgarage.com)</author>
<license>BSD</license>
<review status="Doc reviewed" notes=""/>
<url>http://ros.org/wiki/resource_retriever</url>
<depend package="roslib"/>
<depend package="rosconsole"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -Wl,-rpath,${prefix}/lib -lresource_retriever"/>
</export>
<rosdep name="curl"/>
<platform os="ubuntu" version="9.04"/>
<platform os="ubuntu" version="9.10"/>
<platform os="ubuntu" version="10.04"/>
</package>

147
resource_retriever/src/retriever.cpp Normal file
View File

@ -0,0 +1,147 @@
/*
* Copyright (C) 2009, Willow Garage, Inc.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "resource_retriever/retriever.h"
#include <string.h>
#include <ros/package.h>
#include <ros/console.h>
#include <curl/curl.h>
namespace resource_retriever
{
class CURLStaticInit
{
public:
CURLStaticInit()
: initialized_(false)
{
CURLcode ret = curl_global_init(CURL_GLOBAL_ALL);
if (ret != 0)
{
ROS_ERROR("Error initializing libcurl! retcode = %d", ret);
}
else
{
initialized_ = true;
}
}
~CURLStaticInit()
{
if (initialized_)
{
curl_global_cleanup();
}
}
bool initialized_;
};
static CURLStaticInit g_curl_init;
Retriever::Retriever()
{
curl_handle_ = curl_easy_init();
}
Retriever::~Retriever()
{
if (curl_handle_)
{
curl_easy_cleanup(curl_handle_);
}
}
struct MemoryBuffer
{
std::vector<uint8_t> v;
};
size_t curlWriteFunc(void* buffer, size_t size, size_t nmemb, void* userp)
{
MemoryBuffer* membuf = (MemoryBuffer*)userp;
size_t prev_size = membuf->v.size();
membuf->v.resize(prev_size + size * nmemb);
memcpy(&membuf->v[prev_size], buffer, size * nmemb);
return size * nmemb;
}
MemoryResource Retriever::get(const std::string& url)
{
std::string mod_url = url;
if (url.find("package://") == 0)
{
mod_url.erase(0, strlen("package://"));
size_t pos = mod_url.find("/");
if (pos == std::string::npos)
{
throw Exception(url, "Could not parse package:// format into file:// format");
}
std::string package = mod_url.substr(0, pos);
mod_url.erase(0, pos);
std::string package_path = ros::package::getPath(package);
if (package_path.empty())
{
throw Exception(url, "Package [" + package + "] does not exist");
}
mod_url = "file://" + package_path + mod_url;
}
curl_easy_setopt(curl_handle_, CURLOPT_URL, mod_url.c_str());
curl_easy_setopt(curl_handle_, CURLOPT_WRITEFUNCTION, curlWriteFunc);
char error_buffer[CURL_ERROR_SIZE];
curl_easy_setopt(curl_handle_, CURLOPT_ERRORBUFFER , error_buffer);
MemoryResource res;
MemoryBuffer buf;
curl_easy_setopt(curl_handle_, CURLOPT_WRITEDATA, &buf);
CURLcode ret = curl_easy_perform(curl_handle_);
if (ret != 0)
{
throw Exception(mod_url, error_buffer);
}
else if (!buf.v.empty())
{
res.size = buf.v.size();
res.data.reset(new uint8_t[res.size]);
memcpy(res.data.get(), &buf.v[0], res.size);
}
return res;
}
}

4
resource_retriever/test/CMakeLists.txt Normal file
View File

@ -0,0 +1,4 @@
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR})
rosbuild_add_gtest(test/utest test.cpp)
target_link_libraries(test/utest ${PROJECT_NAME})

140
resource_retriever/test/test.cpp Normal file
View File

@ -0,0 +1,140 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include <gtest/gtest.h>
#include <resource_retriever/retriever.h>
#include <ros/package.h>
#include <ros/console.h>
using namespace resource_retriever;
TEST(Retriever, getByPackage)
{
try
{
Retriever r;
MemoryResource res = r.get("package://"ROS_PACKAGE_NAME"/test/test.txt");
ASSERT_EQ(res.size, 1);
ASSERT_EQ(res.data[0], 'A');
}
catch (Exception& e)
{
FAIL();
}
}
TEST(Retriever, largeFile)
{
try
{
std::string path = ros::package::getPath(ROS_PACKAGE_NAME) + "/test/large_file.dat";
FILE* f = fopen(path.c_str(), "w");
ASSERT_TRUE(f);
for (int i = 0; i < 1024*1024*50; ++i)
{
fprintf(f, "A");
}
fclose(f);
Retriever r;
MemoryResource res = r.get("package://"ROS_PACKAGE_NAME"/test/large_file.dat");
ASSERT_EQ(res.size, 1024*1024*50);
}
catch (Exception& e)
{
FAIL();
}
}
TEST(Retriever, http)
{
try
{
Retriever r;
MemoryResource res = r.get("http://pr.willowgarage.com/downloads/svnmerge.py");
ASSERT_GT(res.size, 0);
}
catch (Exception& e)
{
FAIL();
}
}
TEST(Retriever, invalidFiles)
{
Retriever r;
try
{
r.get("file://fail");
FAIL();
}
catch (Exception& e)
{
ROS_INFO("%s", e.what());
}
try
{
r.get("package://roscpp");
FAIL();
}
catch (Exception& e)
{
ROS_INFO("%s", e.what());
}
try
{
r.get("package://invalid_package_blah/test.xml");
FAIL();
}
catch (Exception& e)
{
ROS_INFO("%s", e.what());
}
}
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

1
resource_retriever/test/test.txt Normal file
View File

@ -0,0 +1 @@
A

55
robot_state_publisher/CMakeLists.txt Normal file
View File

@ -0,0 +1,55 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Set the build type. Options are:
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
# Debug : w/ debug symbols, w/o optimization
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
#set(ROS_BUILD_TYPE RelWithDebInfo)
rosbuild_init()
rosbuild_add_boost_directories()
#set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#uncomment if you have defined messages
#rosbuild_genmsg()
#uncomment if you have defined services
#rosbuild_gensrv()
rosbuild_add_library(${PROJECT_NAME} src/robot_state_publisher.cpp src/treefksolverposfull_recursive.cpp)
rosbuild_add_executable(state_publisher src/joint_state_listener.cpp )
target_link_libraries(state_publisher ${PROJECT_NAME})
rosbuild_add_executable(test_publisher test/test_publisher.cpp )
target_link_libraries(test_publisher ${PROJECT_NAME})
rosbuild_add_gtest_build_flags(test_publisher)
rosbuild_declare_test(test_publisher)
rosbuild_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_publisher.launch)
rosbuild_add_executable(test_one_link test/test_one_link.cpp )
target_link_libraries(test_one_link ${PROJECT_NAME})
rosbuild_add_gtest_build_flags(test_one_link)
rosbuild_declare_test(test_one_link)
rosbuild_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_one_link.launch)
rosbuild_add_executable(test_two_links_fixed_joint test/test_two_links_fixed_joint.cpp )
target_link_libraries(test_two_links_fixed_joint ${PROJECT_NAME})
rosbuild_add_gtest_build_flags(test_two_links_fixed_joint)
rosbuild_declare_test(test_two_links_fixed_joint)
rosbuild_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_two_links_fixed_joint.launch)
rosbuild_add_executable(test_two_links_moving_joint test/test_two_links_moving_joint.cpp )
target_link_libraries(test_two_links_moving_joint ${PROJECT_NAME})
rosbuild_add_gtest_build_flags(test_two_links_moving_joint)
rosbuild_declare_test(test_two_links_moving_joint)
rosbuild_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_two_links_moving_joint.launch)
# Download needed data file
rosbuild_download_test_data(http://pr.willowgarage.com/data/robot_state_publisher/joint_states_indexed.bag test/joint_states_indexed.bag 793e0b566ebe4698265a936b92fa2bba)

1
robot_state_publisher/Makefile Normal file
View File

@ -0,0 +1 @@
include $(shell rospack find mk)/cmake.mk

11
robot_state_publisher/doc.dox Normal file
View File

@ -0,0 +1,11 @@
/**
@mainpage
@htmlinclude manifest.html
This package contains the robot state publisher, which can be used as
a library or as a ROS node. The API documentation for the library can
be found here: robot_state_publisher::RobotStatePublisher
**/

74
robot_state_publisher/include/robot_state_publisher/joint_state_listener.h Normal file
View File

@ -0,0 +1,74 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#ifndef JOINT_STATE_LISTENER_H
#define JOINT_STATE_LISTENER_H
#include <kdl/tree.hpp>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include "robot_state_publisher/robot_state_publisher.h"
using namespace std;
using namespace ros;
using namespace KDL;
typedef boost::shared_ptr<sensor_msgs::JointState const> JointStateConstPtr;
namespace robot_state_publisher{
class JointStateListener{
public:
/** Constructor
* \param tree The kinematic model of a robot, represented by a KDL Tree
*/
JointStateListener(const KDL::Tree& tree);
/// Destructor
~JointStateListener();
private:
void callbackJointState(const JointStateConstPtr& state);
NodeHandle n_, n_tilde_;
Rate publish_rate_;
robot_state_publisher::RobotStatePublisher state_publisher_;
Subscriber joint_state_sub_;
};
}
#endif

86
robot_state_publisher/include/robot_state_publisher/robot_state_publisher.h Normal file
View File

@ -0,0 +1,86 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#ifndef ROBOT_STATE_PUBLISHER_H
#define ROBOT_STATE_PUBLISHER_H
#include <ros/ros.h>
#include <boost/scoped_ptr.hpp>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include "robot_state_publisher/treefksolverposfull_recursive.hpp"
namespace robot_state_publisher{
class RobotStatePublisher
{
public:
/** Constructor
* \param tree The kinematic model of a robot, represented by a KDL Tree
*/
RobotStatePublisher(const KDL::Tree& tree);
/// Destructor
~RobotStatePublisher(){};
/** Publish transforms to tf
* \param joint_positions A map of joint names and joint positions.
* \param time The time at which the joint positions were recorded
* returns true on success; return false when the robot model is empty or not all the joints in the robot model are specified in the joint map.
*/
bool publishTransforms(const std::map<std::string, double>& joint_positions, const ros::Time& time);
private:
ros::Publisher tf_publisher_;
bool flatten_tree_;
KDL::Tree tree_;
boost::scoped_ptr<KDL::TreeFkSolverPosFull_recursive> solver_;
std::string root_;
std::vector<tf::StampedTransform> transforms_;
tf::TransformBroadcaster tf_broadcaster_;
class empty_tree_exception: public std::exception{
virtual const char* what() const throw(){
return "Tree is empty";}
} empty_tree_ex;
};
}
#endif

53
robot_state_publisher/include/robot_state_publisher/treefksolverposfull_recursive.hpp Normal file
View File

@ -0,0 +1,53 @@
// Copyright (C) 2009 Willow Garage Inc
// Version: 1.0
// Author: Wim Meeussen <meeussen at willowgarage dot com>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDLTREEFKSOLVERPOSFULL_RECURSIVE_HPP
#define KDLTREEFKSOLVERPOSFULL_RECURSIVE_HPP
#include <kdl/tree.hpp>
#include <tf/transform_datatypes.h>
namespace KDL {
class TreeFkSolverPosFull_recursive
{
public:
TreeFkSolverPosFull_recursive(const Tree& _tree);
~TreeFkSolverPosFull_recursive();
int JntToCart(const std::map<std::string, double>& q_in, std::map<std::string, tf::Stamped<Frame> >& p_out, bool flatten_tree=true);
private:
void addFrameToMap(const std::map<std::string, double>& q_in,
std::map<std::string, tf::Stamped<KDL::Frame> >& p_out,
const tf::Stamped<KDL::Frame>& previous_frame,
const SegmentMap::const_iterator this_segment,
bool flatten_tree);
Tree tree;
};
}
#endif

34
robot_state_publisher/manifest.xml Normal file
View File

@ -0,0 +1,34 @@
<package>
<description brief="This package allows you to publish the state of a robot to the transform library topic">
This package allows you to publish the state of a robot to
<a href="http://ros.org/wiki/tf">tf</a>. Once the state gets published, it is
available to all components in the system that also use <tt>tf</tt>.
The package takes the joint angles of the robot as input
and publishes the 3D poses of the robot links, using a kinematic
tree model of the robot. The package can both be used as a library
and as a ROS node. This package has been well tested and the code
is stable. No major changes are planned in the near future
</description>
<author>Wim Meeussen meeussen@willowgarage.com </author>
<license>BSD</license>
<review status="Doc reviewed" notes=""/>
<url>http://ros.org/wiki/robot_state_publisher</url>
<depend package="kdl_parser" />
<depend package="sensor_msgs" />
<depend package="roscpp" />
<depend package="tf" />
<depend package="tf_conversions" />
<export>
<cpp cflags="-I${prefix}/include/" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lrobot_state_publisher" />
</export>
<platform os="ubuntu" version="9.04"/>
<platform os="ubuntu" version="9.10"/>
<platform os="ubuntu" version="10.04"/>
</package>

122
robot_state_publisher/src/joint_state_listener.cpp Normal file
View File

@ -0,0 +1,122 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#include <kdl/tree.hpp>
#include <ros/ros.h>
#include "robot_state_publisher/robot_state_publisher.h"
#include "robot_state_publisher/joint_state_listener.h"
#include <kdl_parser/kdl_parser.hpp>
using namespace std;
using namespace ros;
using namespace KDL;
using namespace robot_state_publisher;
JointStateListener::JointStateListener(const KDL::Tree& tree)
: n_tilde_("~"), publish_rate_(0.0), state_publisher_(tree)
{
// set publish frequency
double publish_freq;
n_tilde_.param("publish_frequency", publish_freq, 50.0);
publish_rate_ = Rate(publish_freq);
if (tree.getNrOfJoints() == 0){
boost::shared_ptr<sensor_msgs::JointState> empty_state(new sensor_msgs::JointState);
while (ros::ok()){
empty_state->header.stamp = ros::Time::now();
this->callbackJointState(empty_state);
publish_rate_.sleep();
}
}
else{
// subscribe to mechanism state
joint_state_sub_ = n_.subscribe("joint_states", 1, &JointStateListener::callbackJointState, this);
}
};
JointStateListener::~JointStateListener()
{};
void JointStateListener::callbackJointState(const JointStateConstPtr& state)
{
if (state->name.size() != state->position.size()){
ROS_ERROR("Robot state publisher received an invalid joint state vector");
return;
}
// get joint positions from state message
map<string, double> joint_positions;
for (unsigned int i=0; i<state->name.size(); i++)
joint_positions.insert(make_pair(state->name[i], state->position[i]));
state_publisher_.publishTransforms(joint_positions, state->header.stamp);
publish_rate_.sleep();
}
// ----------------------------------
// ----- MAIN -----------------------
// ----------------------------------
int main(int argc, char** argv)
{
// Initialize ros
ros::init(argc, argv, "robot_state_publisher");
NodeHandle node;
// gets the location of the robot description on the parameter server
KDL::Tree tree;
if (!kdl_parser::treeFromParam("robot_description", tree)){
ROS_ERROR("Failed to extract kdl tree from xml robot description");
return -1;
}
if (tree.getNrOfSegments() == 0){
ROS_WARN("Robot state publisher got an empty tree and cannot publish any state to tf");
ros::spin();
}
else{
JointStateListener state_publisher(tree);
ros::spin();
}
return 0;
}

99
robot_state_publisher/src/robot_state_publisher.cpp Normal file
View File

@ -0,0 +1,99 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#include "robot_state_publisher/robot_state_publisher.h"
#include <kdl/frames_io.hpp>
#include <tf_conversions/tf_kdl.h>
using namespace std;
using namespace ros;
using namespace KDL;
namespace robot_state_publisher{
RobotStatePublisher::RobotStatePublisher(const Tree& tree)
:tree_(tree)
{
// build tree solver
solver_.reset(new TreeFkSolverPosFull_recursive(tree_));
// get parameter to flatter tree or not
ros::NodeHandle n_private("~");
n_private.param("flatten_tree", flatten_tree_, false);
// advertise tf message
NodeHandle n;
tf_publisher_ = n.advertise<tf::tfMessage>("/tf", 5);
// get the root segment of the tree
SegmentMap::const_iterator root = tree.getRootSegment();
root_ = root->first;
}
bool RobotStatePublisher::publishTransforms(const map<string, double>& joint_positions, const Time& time)
{
// calculate transforms
map<string, tf::Stamped<Frame> > link_poses;
solver_->JntToCart(joint_positions, link_poses, flatten_tree_);
if (link_poses.empty()){
ROS_ERROR("Could not compute link poses. The tree or the state is invalid.");
return false;
}
transforms_.resize(link_poses.size());
unsigned int i = 0;
tf::Transform tf_frame;
for (map<string, tf::Stamped<Frame> >::const_iterator f=link_poses.begin(); f!=link_poses.end(); f++){
tf::TransformKDLToTF(f->second, transforms_[i]);
transforms_[i].stamp_ = time;
transforms_[i].frame_id_ = f->second.frame_id_;
transforms_[i].child_frame_id_ = f->first;
i++;
}
tf_broadcaster_.sendTransform(transforms_);
return true;
}
}

85
robot_state_publisher/src/treefksolverposfull_recursive.cpp Normal file
View File

@ -0,0 +1,85 @@
// Copyright (C) 2009 Willow Garage Inc
// Version: 1.0
// Author: Wim Meeussen <meeussen at willowgarage dot com>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#include "robot_state_publisher/treefksolverposfull_recursive.hpp"
#include <ros/ros.h>
#include <iostream>
#include <cstdio>
using namespace std;
namespace KDL {
TreeFkSolverPosFull_recursive::TreeFkSolverPosFull_recursive(const Tree& _tree):
tree(_tree)
{
}
TreeFkSolverPosFull_recursive::~TreeFkSolverPosFull_recursive()
{
}
int TreeFkSolverPosFull_recursive::JntToCart(const map<string, double>& q_in, map<string, tf::Stamped<Frame> >& p_out, bool flatten_tree)
{
// clear output
p_out.clear();
addFrameToMap(q_in, p_out, tf::Stamped<KDL::Frame>(KDL::Frame::Identity(), ros::Time(), tree.getRootSegment()->second.segment.getName()), tree.getRootSegment(), flatten_tree);
return 0;
}
void TreeFkSolverPosFull_recursive::addFrameToMap(const map<string, double>& q_in,
map<string, tf::Stamped<Frame> >& p_out,
const tf::Stamped<KDL::Frame>& previous_frame,
const SegmentMap::const_iterator this_segment,
bool flatten_tree)
{
// get pose of this segment
tf::Stamped<KDL::Frame> this_frame;
double jnt_p = 0;
if (this_segment->second.segment.getJoint().getType() != Joint::None){
map<string, double>::const_iterator jnt_pos = q_in.find(this_segment->second.segment.getJoint().getName());
if (jnt_pos == q_in.end()){
ROS_DEBUG("Warning: TreeFKSolverPosFull Could not find value for joint '%s'. Skipping this tree branch", this_segment->first.c_str());
return;
}
jnt_p = jnt_pos->second;
}
this_frame = tf::Stamped<KDL::Frame>(previous_frame * this_segment->second.segment.pose(jnt_p), ros::Time(), previous_frame.frame_id_);
if (this_segment->first != tree.getRootSegment()->first)
p_out.insert(make_pair(this_segment->first, tf::Stamped<KDL::Frame>(this_frame, ros::Time(), previous_frame.frame_id_)));
// get poses of child segments
for (vector<SegmentMap::const_iterator>::const_iterator child=this_segment->second.children.begin(); child !=this_segment->second.children.end(); child++){
if (flatten_tree)
addFrameToMap(q_in, p_out, this_frame, *child, flatten_tree);
else
addFrameToMap(q_in, p_out, tf::Stamped<KDL::Frame>(KDL::Frame::Identity(), ros::Time(), this_segment->first), *child, flatten_tree);
}
}
}

3
robot_state_publisher/test/one_link.urdf Normal file
View File

@ -0,0 +1,3 @@
<robot name="test_robot">
<link name="link1" />
</robot>

Some files were not shown because too many files have changed in this diff Show More