diff --git a/trunk/fcl/CMakeLists.txt b/CMakeLists.txt
similarity index 100%
rename from trunk/fcl/CMakeLists.txt
rename to CMakeLists.txt
diff --git a/trunk/fcl/CMakeModules/FCLVersion.cmake b/CMakeModules/FCLVersion.cmake
similarity index 100%
rename from trunk/fcl/CMakeModules/FCLVersion.cmake
rename to CMakeModules/FCLVersion.cmake
diff --git a/trunk/fcl/Makefile b/Makefile
similarity index 100%
rename from trunk/fcl/Makefile
rename to Makefile
diff --git a/branches/point_cloud/CMakeLists.txt b/branches/point_cloud/CMakeLists.txt
deleted file mode 100644
index 28105dd5aa07a561c1429ae4530c22e0e36b34ec..0000000000000000000000000000000000000000
--- a/branches/point_cloud/CMakeLists.txt
+++ /dev/null
@@ -1,17 +0,0 @@
-cmake_minimum_required(VERSION 2.4.6)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
-
-# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of
-# directories (or patterns, but directories should suffice) that should
-# be excluded from the distro. This is not the place to put things that
-# should be ignored everywhere, like "build" directories; that happens in
-# rosbuild/rosbuild.cmake. Here should be listed packages that aren't
-# ready for inclusion in a distro.
-#
-# This list is combined with the list in rosbuild/rosbuild.cmake. Note
-# that CMake 2.6 may be required to ensure that the two lists are combined
-# properly. CMake 2.4 seems to have unpredictable scoping rules for such
-# variables.
-#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental)
-
-rosbuild_make_distribution(0.1.0)
diff --git a/branches/point_cloud/Makefile b/branches/point_cloud/Makefile
deleted file mode 100644
index a818ccadb911aa86a9120d25981d64abf102dd30..0000000000000000000000000000000000000000
--- a/branches/point_cloud/Makefile
+++ /dev/null
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake_stack.mk
\ No newline at end of file
diff --git a/branches/point_cloud/ann/Makefile b/branches/point_cloud/ann/Makefile
deleted file mode 100644
index 17a131efc7443e113a51d78b6ad56863179901b6..0000000000000000000000000000000000000000
--- a/branches/point_cloud/ann/Makefile
+++ /dev/null
@@ -1,37 +0,0 @@
-all: installed
-
-#
-# Download, extract and compile from a released tarball:
-#
-TARBALL = build/ann_1.1.2.tar.gz
-TARBALL_URL = http://www.cs.umd.edu/~mount/ANN/Files/1.1.2/ann_1.1.2.tar.gz
-TARBALL_PATCH = fANN.diff
-INITIAL_DIR = build/ann_1.1.2
-SOURCE_DIR = build/ann-tar
-include $(shell rospack find mk)/download_unpack_build.mk
-
-INSTALL_DIR = ann
-CMAKE = cmake
-CMAKE_ARGS = -D CMAKE_BUILD_TYPE="Release" -D CMAKE_INSTALL_PREFIX=`rospack find ann`/$(INSTALL_DIR)
-MAKE_ARGS = linux-g++
-MAKE = make
-
-installed: wiped $(SOURCE_DIR)/unpacked
- cd $(SOURCE_DIR) && make $(ROS_PARALLEL_JOBS) $(MAKE_ARGS)
- mkdir -p $(INSTALL_DIR)/include
- mkdir -p $(INSTALL_DIR)/include/ann
- cp -r $(SOURCE_DIR)/include/ANN/*.h $(INSTALL_DIR)/include/ann
- cp -r $(SOURCE_DIR)/bin $(INSTALL_DIR)
- cp -r $(SOURCE_DIR)/lib $(INSTALL_DIR)
- touch installed
-
-clean:
- rm -rf build
- rm -rf $(INSTALL_DIR) installed
-
-wiped: Makefile
- make wipe
- touch wiped
-
-wipe: clean
- rm -rf build patched
diff --git a/branches/point_cloud/ann/fANN.diff b/branches/point_cloud/ann/fANN.diff
deleted file mode 100644
index 6c7650f3e8501faf8ac77151ce3218f5fb232a23..0000000000000000000000000000000000000000
--- a/branches/point_cloud/ann/fANN.diff
+++ /dev/null
@@ -1,24 +0,0 @@
-diff -purN ann_1.1.2/include/ANN/ANN.h ann_1.1.2_modified/include/ANN/ANN.h
---- include/ANN/ANN.h 2010-01-27 20:40:01.000000000 -0800
-+++ include/ANN/ANN.h 2011-08-30 12:13:26.629632052 -0700
-@@ -232,7 +232,7 @@ const ANNdist ANN_DIST_INF = ANN_DBL_MAX
- // strictly positive.
- //----------------------------------------------------------------------
-
--const ANNbool ANN_ALLOW_SELF_MATCH = ANNtrue;
-+const ANNbool ANN_ALLOW_SELF_MATCH = ANNfalse;
-
- //----------------------------------------------------------------------
- // Norms and metrics:
-diff -purN ann_1.1.2/Make-config ann_1.1.2_modified/Make-config
---- Make-config 2010-01-27 20:40:01.000000000 -0800
-+++ Make-config 2011-08-30 12:12:42.916876291 -0700
-@@ -72,7 +72,7 @@ linux-g++:
- $(MAKE) targets \
- "ANNLIB = libANN.a" \
- "C++ = g++" \
-- "CFLAGS = -O3" \
-+ "CFLAGS = -O3 -fPIC" \
- "MAKELIB = ar ruv" \
- "RANLIB = true"
-
diff --git a/branches/point_cloud/ann/fPIC.diff b/branches/point_cloud/ann/fPIC.diff
deleted file mode 100644
index 8fb372dace2ec45c93b4a7d49400979e3da23e47..0000000000000000000000000000000000000000
--- a/branches/point_cloud/ann/fPIC.diff
+++ /dev/null
@@ -1,11 +0,0 @@
---- Make-config 2010-01-27 20:40:01.000000000 -0800
-+++ Make-config 2011-08-25 14:29:00.000000000 -0700
-@@ -72,7 +72,7 @@ linux-g++:
- $(MAKE) targets \
- "ANNLIB = libANN.a" \
- "C++ = g++" \
-- "CFLAGS = -O3" \
-+ "CFLAGS = -O3 -fPIC" \
- "MAKELIB = ar ruv" \
- "RANLIB = true"
-
diff --git a/branches/point_cloud/ann/mainpage.dox b/branches/point_cloud/ann/mainpage.dox
deleted file mode 100644
index dcdfd4167fa49fa22710274e39aad23db4051a9a..0000000000000000000000000000000000000000
--- a/branches/point_cloud/ann/mainpage.dox
+++ /dev/null
@@ -1,26 +0,0 @@
-/**
-\mainpage
-\htmlinclude manifest.html
-
-\b ann 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.
--->
-
-
-*/
diff --git a/branches/point_cloud/ann/manifest.xml b/branches/point_cloud/ann/manifest.xml
deleted file mode 100644
index a6db3d649d290f1f3706389003ebeaeb735d6aae..0000000000000000000000000000000000000000
--- a/branches/point_cloud/ann/manifest.xml
+++ /dev/null
@@ -1,15 +0,0 @@
-<package>
- <description brief="ann">
- This package is a wrapper on the ann library available from <a href="http://www.cs.umd.edu/~mount/ANN/">ANN library</a>. This package does not modify the contents of the original library in any manner and only wraps it for easy distribution with the ROS packaging system.
- </description>
- <author>Maintained by Jia Pan, Sachin Chitta</author>
- <license>BSD</license>
- <review status="unreviewed" notes=""/>
- <url>http://ros.org/wiki/ann</url>
- <export>
- <cpp cflags="-I${prefix}/ann/include" lflags="-L${prefix}/ann/lib -Wl,-rpath,${prefix}/ann/lib -lANN"/>
- </export>
-
-</package>
-
-
diff --git a/branches/point_cloud/collision_space_fcl/CMakeLists.txt b/branches/point_cloud/collision_space_fcl/CMakeLists.txt
deleted file mode 100644
index eaa6f169c31ed1df540cb73d56b946b8e2a61f80..0000000000000000000000000000000000000000
--- a/branches/point_cloud/collision_space_fcl/CMakeLists.txt
+++ /dev/null
@@ -1,39 +0,0 @@
-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()
-
-#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()
-
-#common commands for building c++ executables and libraries
-#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
-#target_link_libraries(${PROJECT_NAME} another_library)
-#rosbuild_add_boost_directories()
-#rosbuild_link_boost(${PROJECT_NAME} thread)
-#rosbuild_add_executable(example examples/example.cpp)
-#target_link_libraries(example ${PROJECT_NAME})
-
-rosbuild_add_library(collision_space_fcl
-src/environment.cpp
-src/environment_objects.cpp
-src/environmentFCL.cpp)
-
-rosbuild_add_gtest(test_collision_space_fcl test/test_collision_space_fcl.cpp)
-target_link_libraries(test_collision_space_fcl collision_space_fcl)
-target_link_libraries(test_collision_space_fcl assimp)
\ No newline at end of file
diff --git a/branches/point_cloud/collision_space_fcl/Makefile b/branches/point_cloud/collision_space_fcl/Makefile
deleted file mode 100644
index b75b928f20ef9ea4f509a17db62e4e31b422c27f..0000000000000000000000000000000000000000
--- a/branches/point_cloud/collision_space_fcl/Makefile
+++ /dev/null
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
diff --git a/branches/point_cloud/collision_space_fcl/include/collision_space_fcl/environment.h b/branches/point_cloud/collision_space_fcl/include/collision_space_fcl/environment.h
deleted file mode 100644
index 5bf15885b40d9d2d50e5bce489b19c1366fe94c1..0000000000000000000000000000000000000000
--- a/branches/point_cloud/collision_space_fcl/include/collision_space_fcl/environment.h
+++ /dev/null
@@ -1,366 +0,0 @@
-/*********************************************************************
- * 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 Ioan Sucan */
-
-#ifndef COLLISION_SPACE_FCL_ENVIRONMENT_MODEL_H
-#define COLLISION_SPACE_FCL_ENVIRONMENT_MODEL_H
-
-#include "collision_space_fcl/environment_objects.h"
-#include <planning_models/kinematic_model.h>
-#include <planning_models/kinematic_state.h>
-#include <geometric_shapes/bodies.h>
-#include <LinearMath/btVector3.h>
-#include <boost/thread/recursive_mutex.hpp>
-#include <boost/shared_ptr.hpp>
-#include <vector>
-#include <string>
-
-
-/** \brief Main namespace */
-namespace collision_space_fcl
-{
-
-/** \brief A class describing an environment for a kinematic
- robot. This is the base (abstract) definition. Different
- implementations are possible. The class is aware of a set of
- obstacles and a robot model. The obstacles are placed in different
- namespaces so they can be added and removed selectively.
-*/
-class EnvironmentModel
-{
-public:
-
- enum BodyType {
- LINK,
- ATTACHED,
- OBJECT
- };
-
- /** \brief Definition of a contact point */
- struct Contact
- {
- /** \brief contact position */
- btVector3 pos;
- /** \brief normal unit vector at contact */
- btVector3 normal;
- /** \brief depth (penetration between bodies) */
- double depth;
-
- /** \brief The first body involved in the contact */
- std::string body_name_1;
- BodyType body_type_1;
-
- /** \brief The first body involved in the contact */
- std::string body_name_2;
- BodyType body_type_2;
- };
-
- /** \brief Definition of a contact that is allowed */
- struct AllowedContact
- {
- /// the bound where the contact is allowed
- boost::shared_ptr<bodies::Body> bound;
-
- std::string body_name_1;
- std::string body_name_2;
-
- /// tha maximum depth for the contact
- double depth;
- };
-
- typedef std::map<std::string, std::map<std::string, std::vector<AllowedContact> > > AllowedContactMap;
-
- /** \brief Definition of a structure for the allowed collision matrix */
- /* False means that no collisions are allowed, true means ok */
- class AllowedCollisionMatrix
- {
- public:
-
- AllowedCollisionMatrix(){
- valid_ = true;
- }
-
- AllowedCollisionMatrix(const std::vector<std::string>& names,
- bool allowed = false);
-
- AllowedCollisionMatrix(const std::vector<std::vector<bool> >& all_coll_vectors,
- const std::map<std::string, unsigned int>& all_coll_indices);
-
- AllowedCollisionMatrix(const AllowedCollisionMatrix& acm);
-
- bool getAllowedCollision(const std::string& name1, const std::string& name2,
- bool& allowed_collision) const;
-
- bool getAllowedCollision(unsigned int i, unsigned int j,
- bool& allowed_collision) const;
-
- bool hasEntry(const std::string& name) const;
-
- bool getEntryIndex(const std::string& name,
- unsigned int& index) const;
-
- bool getEntryName(const unsigned int ind,
- std::string& name) const;
-
- bool removeEntry(const std::string& name);
-
- bool addEntry(const std::string& name, bool allowed);
-
- bool changeEntry(bool allowed);
-
- bool changeEntry(const std::string& name, bool allowed);
-
- bool changeEntry(const std::string& name1,
- const std::string& name2,
- bool allowed);
-
- bool changeEntry(const unsigned int ind_1,
- const unsigned int ind_2,
- bool allowed);
-
- bool changeEntry(const std::string& name,
- const std::vector<std::string>& change_names,
- bool allowed);
-
- bool changeEntry(const std::vector<std::string>& change_names_1,
- const std::vector<std::string>& change_names_2,
- bool allowed);
-
- bool getValid() const {
- return valid_;
- };
-
- unsigned int getSize() const {
- return allowed_entries_.size();
- }
-
- typedef boost::bimap<std::string, unsigned int> entry_type;
-
- const entry_type& getEntriesBimap() const {
- return allowed_entries_bimap_;
- }
-
- void print(std::ostream& out) const;
-
- private:
-
- bool valid_;
- std::vector<std::vector<bool> > allowed_entries_;
-
- entry_type allowed_entries_bimap_;
- };
-
- EnvironmentModel(void)
- {
- verbose_ = false;
- objects_ = new EnvironmentObjects();
- use_altered_collision_matrix_ = false;
- }
-
- virtual ~EnvironmentModel(void)
- {
- if (objects_)
- delete objects_;
- }
-
- /**********************************************************************/
- /* Collision Environment Configuration */
- /**********************************************************************/
-
- /** \brief Add a robot model. Ignore robot links if their name is not
- specified in the string vector. The scale argument can be
- used to increase or decrease the size of the robot's
- bodies (multiplicative factor). The padding can be used to
- increase or decrease the robot's bodies with by an
- additive term */
- virtual void setRobotModel(const planning_models::KinematicModel* model,
- const AllowedCollisionMatrix& acm,
- const std::map<std::string, double>& link_padding_map,
- double default_padding = 0.0,
- double scale = 1.0);
-
- /** \brief Get robot scale */
- double getRobotScale(void) const;
-
- /** \brief Get robot padding */
- double getRobotPadding(void) const;
-
- /** \brief Update the positions of the geometry used in collision detection */
- virtual void updateRobotModel(const planning_models::KinematicState* state) = 0;
-
- /** \brief Update the set of bodies that are attached to the robot (re-creates them) */
- virtual void updateAttachedBodies() = 0;
-
- /** \brief Get the robot model */
- const planning_models::KinematicModel* getRobotModel(void) const;
-
- /**********************************************************************/
- /* Collision Checking Routines */
- /**********************************************************************/
-
-
- /** \brief Check if a model is in collision with environment and self. Contacts are not computed */
- virtual bool isCollision(void) const = 0;
-
- /** \brief Check for self collision. Contacts are not computed */
- virtual bool isSelfCollision(void) const = 0;
-
- /** \brief Check whether the model is in collision with the environment. Self collisions are not checked */
- virtual bool isEnvironmentCollision(void) const = 0;
-
- /** \brief Get the list of contacts (collisions). The maximum total number of contacts to be returned can be specified, and the max per pair of objects that are in collision*/
- virtual bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_total = 1, unsigned int max_per_pair = 1) const = 0;
-
- /** \brief This function will get the complete list of contacts between any two potentially colliding bodies. The num per contacts specifies the number of contacts per pair that will be returned */
- virtual bool getAllCollisionContacts(std::vector<Contact> &contacts, unsigned int num_per_contact = 1) const = 0;
-
- /**********************************************************************/
- /* Collision Bodies */
- /**********************************************************************/
-
- /** \brief Remove all objects from collision model */
- virtual void clearObjects(void) = 0;
-
- /** \brief Remove objects from a specific namespace in the collision model */
- virtual void clearObjects(const std::string &ns) = 0;
-
- /** \brief Tells whether or not there is an object with the given name in the collision model */
- virtual bool hasObject(const std::string& ns) const = 0;
-
- /** \brief Add a static collision object to the map. The user releases ownership of the passed object. Memory allocated for the shape is freed by the collision environment. */
- virtual void addObject(const std::string &ns, shapes::StaticShape *shape) = 0;
-
- /** \brief Add a collision object to the map. The user releases ownership of the passed object. Memory allocated for the shape is freed by the collision environment.*/
- virtual void addObject(const std::string &ns, shapes::Shape* shape, const btTransform &pose) = 0;
-
- /** \brief Add a set of collision objects to the map. The user releases ownership of the passed objects. Memory allocated for the shapes is freed by the collision environment.*/
- virtual void addObjects(const std::string &ns, const std::vector<shapes::Shape*> &shapes, const std::vector<btTransform> &poses) = 0;
-
- virtual void getAttachedBodyPoses(std::map<std::string, std::vector<btTransform> >& pose_map) const = 0;
-
- /** \briefs Sets a temporary robot padding on the indicated links */
- virtual void setAlteredLinkPadding(const std::map<std::string, double>& link_padding_map);
-
- /** \briefs Reverts link padding to that set at robot initialization */
- virtual void revertAlteredLinkPadding();
-
- const std::map<std::string,double>& getDefaultLinkPaddingMap() const;
-
- std::map<std::string,double> getCurrentLinkPaddingMap() const;
-
- double getCurrentLinkPadding(std::string name) const;
-
- /** \brief Get the objects currently contained in the model */
- const EnvironmentObjects* getObjects(void) const;
-
- const AllowedCollisionMatrix& getDefaultAllowedCollisionMatrix() const;
-
- const AllowedCollisionMatrix& getCurrentAllowedCollisionMatrix() const;
-
- /** \brief set the matrix for collision touch to use in lieu of the default settings */
- virtual void setAlteredCollisionMatrix(const AllowedCollisionMatrix& acm);
-
- /** \brief reverts to using default settings for allowed collisions */
- virtual void revertAlteredCollisionMatrix();
-
- /** \brief sets the allowed contacts that will be used in collision checking */
- virtual void setAllowedContacts(const std::vector<AllowedContact>& allowed_contacts);
-
- /** \brief gets the allowed contacts that will be used in collision checking */
- const std::vector<AllowedContact>& getAllowedContacts() const;
-
- /** \brief clear out all allowed contacts */
- void clearAllowedContacts() {
- allowed_contact_map_.clear();
- allowed_contacts_.clear();
- }
-
- /**********************************************************************/
- /* Miscellaneous Routines */
- /**********************************************************************/
-
- /** \brief Provide interface to a lock. Use carefully! */
- void lock(void) const;
-
- /** \brief Provide interface to a lock. Use carefully! */
- void unlock(void) const;
-
- /** \brief Enable/disable verbosity */
- void setVerbose(bool verbose);
-
- /** \brief Check the state of verbosity */
- bool getVerbose(void) const;
-
- /** \brief Clone the environment. */
- virtual EnvironmentModel* clone(void) const = 0;
-
-protected:
-
- /** \brief Mutex used to lock the datastructure */
- mutable boost::recursive_mutex lock_;
-
- /** \brief Flag to indicate whether verbose mode is on */
- bool verbose_;
-
- /** \brief Loaded robot model */
- const planning_models::KinematicModel* robot_model_;
-
- /** \brief List of objects contained in the environment */
- EnvironmentObjects *objects_;
-
- /** \brief Scaling used for robot links */
- double robot_scale_;
-
- /** \brief padding used for robot links */
- double default_robot_padding_;
-
- AllowedCollisionMatrix default_collision_matrix_;
- AllowedCollisionMatrix altered_collision_matrix_;
-
- bool use_altered_collision_matrix_;
-
- std::map<std::string, double> default_link_padding_map_;
- std::map<std::string, double> altered_link_padding_map_;
-
- bool use_altered_link_padding_map_;
-
- AllowedContactMap allowed_contact_map_;
- std::vector<AllowedContact> allowed_contacts_;
-
-};
-}
-
-#endif
-
diff --git a/branches/point_cloud/collision_space_fcl/include/collision_space_fcl/environmentFCL.h b/branches/point_cloud/collision_space_fcl/include/collision_space_fcl/environmentFCL.h
deleted file mode 100644
index c772c034d1afb6ec8463422aa46f891076a5b9c0..0000000000000000000000000000000000000000
--- a/branches/point_cloud/collision_space_fcl/include/collision_space_fcl/environmentFCL.h
+++ /dev/null
@@ -1,293 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-#ifndef COLLISION_SPACE_FCL_ENVIRONMENT_MODEL_FCL_H
-#define COLLISION_SPACE_FCL_ENVIRONMENT_MODEL_FCL_H
-
-#include "collision_space_fcl/environment.h"
-#include "fcl/broad_phase_collision.h"
-#include <map>
-#include <vector>
-
-namespace collision_space_fcl
-{
-
-/** \brief A class describing an environment for a kinematic robot using ODE */
-class EnvironmentModelFCL : public EnvironmentModel
-{
-
- friend bool collisionCallBack(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* cdata);
-
-public:
-
- EnvironmentModelFCL(void);
-
- virtual ~EnvironmentModelFCL(void);
-
-
- /** \brief Get the list of contacts (collisions). The maximum total number of contacts to be returned can be specified, and the max per pair of objects that's in collision*/
- virtual bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_total = 1, unsigned int max_per_pair = 1) const;
-
- /** \brief This function will get the complete list of contacts between any two potentially colliding bodies. The num per contacts specifies the number of contacts per pair that will be returned */
- virtual bool getAllCollisionContacts(std::vector<Contact> &contacts, unsigned int num_per_contact = 1) const;
-
- /** \brief Check if a model is in collision */
- virtual bool isCollision(void) const;
-
- /** \brief Check if a model is in self collision */
- virtual bool isSelfCollision(void) const;
-
- /** \brief Check if a model is in environment collision */
- virtual bool isEnvironmentCollision(void) const;
-
- /** \brief Remove all objects from collision model */
- virtual void clearObjects(void);
-
- /** \brief Remove objects from a specific namespace in the collision model */
- virtual void clearObjects(const std::string& ns);
-
- /** \brief Tells whether or not there is an object with the given name in the collision model */
- virtual bool hasObject(const std::string& ns) const;
-
- /** \brief Add a static collision object to the map. The user releases ownership of the passed object. Memory allocated for the shape is freed by the collision environment. */
- virtual void addObject(const std::string& ns, shapes::StaticShape* shape);
-
- /** \brief Add a collision object to the map. The user releases ownership of the passed object. Memory allocated for the shape is freed by the collision environment. */
- virtual void addObject(const std::string& ns, shapes::Shape* shape, const btTransform& pose);
-
- /** \brief Add a set of collision objects to the map. The user releases ownership of the passed objects. Memory allocated for the shapes is freed by the collision environment. */
- virtual void addObjects(const std::string& ns, const std::vector<shapes::Shape*>& shapes, const std::vector<btTransform>& poses);
-
- virtual void getAttachedBodyPoses(std::map<std::string, std::vector<btTransform> >& pose_map) const;
-
- /** \brief Add a robot model. Ignore robot links if their name is not
- specified in the string vector. The scale argument can be
- used to increase or decrease the size of the robot's
- bodies (multiplicative factor). The padding can be used to
- increase or decrease the robot's bodies with by an
- additive term */
- virtual void setRobotModel(const planning_models::KinematicModel* model,
- const AllowedCollisionMatrix& allowed_collision_matrix,
- const std::map<std::string, double>& link_padding_map,
- double default_padding = 0.0,
- double scale = 1.0);
-
- /** \brief Update the positions of the geometry used in collision detection */
- virtual void updateRobotModel(const planning_models::KinematicState* state);
-
- /** \brief Update the set of bodies that are attached to the robot (re-creates them) */
- virtual void updateAttachedBodies(void);
-
- /** \brief Update the set of bodies that are attached to the robot (re-creates them) using the indicated padding or default if non-specified */
- virtual void updateAttachedBodies(const std::map<std::string, double>& link_padding_map);
-
- /** \briefs Sets a temporary robot padding on the indicated links */
- virtual void setAlteredLinkPadding(const std::map<std::string, double>& link_padding_map);
-
- /** \briefs Reverts link padding to that set at robot initialization */
- virtual void revertAlteredLinkPadding();
-
- /** \brief Clone the environment */
- virtual EnvironmentModel* clone(void) const;
-
-protected:
-
- /** \brief Geometry for attachment */
- struct AttGeom
- {
- ~AttGeom()
- {
- for(unsigned int i = 0; i < geom.size(); i++)
- {
- delete geom[i];
- }
- for(unsigned int i = 0; i < padded_geom.size(); i++)
- {
- delete padded_geom[i];
- }
- }
-
- std::vector<fcl::CollisionObject*> geom; // managed by self_geom_manager
- std::vector<fcl::CollisionObject*> padded_geom; // for collision with environment geometry
- const planning_models::KinematicModel::AttachedBodyModel* att;
- unsigned int index;
- };
-
- /** \brief Geometry for Link */
- struct LinkGeom
- {
- ~LinkGeom()
- {
- for(unsigned int i = 0; i < geom.size(); i++)
- {
- delete geom[i];
- }
- for(unsigned int i = 0; i < padded_geom.size(); i++)
- {
- delete padded_geom[i];
- }
- deleteAttachedBodies();
- }
-
- void deleteAttachedBodies()
- {
- for(unsigned int i = 0; i < att_bodies.size(); i++)
- {
- delete att_bodies[i];
- }
- att_bodies.clear();
- }
-
- std::vector<fcl::CollisionObject*> geom; // managed by self_geom_manager
- std::vector<fcl::CollisionObject*> padded_geom; // for collision with environment geometry
- std::vector<AttGeom*> att_bodies;
- const planning_models::KinematicModel::LinkModel* link;
- unsigned int index;
- };
-
- struct CollisionData
- {
- CollisionData(void)
- {
- done = false;
- collides = false;
- max_contacts_total = 0;
- max_contacts_pair = 0;
- contacts = NULL;
- allowed_collision_matrix = NULL;
- allowed = NULL;
- exhaustive = false;
- }
-
- //these are parameters
- unsigned int max_contacts_total;
- unsigned int max_contacts_pair;
- const AllowedCollisionMatrix* allowed_collision_matrix;
- const std::map<fcl::CollisionObject*, std::pair<std::string, BodyType> >* geom_lookup_map;
- const AllowedContactMap *allowed;
- bool exhaustive;
-
- //these are for return info
- bool done;
- bool collides;
- std::vector<EnvironmentModelFCL::Contact>* contacts;
-
- //for the last collision found
- std::string body_name_1;
- BodyType body_type_1;
-
- std::string body_name_2;
- BodyType body_type_2;
- };
-
-
- struct ModelInfo
- {
- std::vector<LinkGeom*> link_geom;
- };
-
- struct CollisionNamespace
- {
- CollisionNamespace(const std::string& nm) : name(nm)
- {
- env_geom_manager.reset(new fcl::SSaPCollisionManager());
- }
-
- virtual ~CollisionNamespace(void)
- {
- clear();
- }
-
- void clear(void)
- {
- for(unsigned int i = 0; i < geoms.size(); ++i)
- delete geoms[i];
- geoms.clear();
- env_geom_manager->clear();
- }
-
- std::string name;
- std::vector<fcl::CollisionObject*> geoms;
- boost::shared_ptr<fcl::BroadPhaseCollisionManager> env_geom_manager;
- };
-
-
- /** \brief Internal function for collision detection */
- void testCollision(CollisionData* data) const;
-
- /** \brief Internal function for collision detection */
- void testSelfCollision(CollisionData* data) const;
-
- /** \brief Internal function for collision detection */
- void testEnvironmentCollision(CollisionData* data) const;
-
- /** \brief Internal function for collision detection */
- void testObjectCollision(CollisionNamespace* cn, CollisionData* data) const;
-
- fcl::CollisionObject* copyGeom(fcl::CollisionObject* geom) const;
-
- void createRobotModel();
-
- fcl::CollisionObject* createGeom(const shapes::Shape* shape, double scale, double padding);
-
- fcl::CollisionObject* createGeom(const shapes::StaticShape* shape);
-
- void updateGeom(fcl::CollisionObject* geom, const btTransform& pose) const;
-
- void addAttachedBody(LinkGeom* lg, const planning_models::KinematicModel::AttachedBodyModel* attm,
- double padd);
-
- std::map<std::string, bool> attached_bodies_in_collision_matrix_;
-
- void setAttachedBodiesLinkPadding();
-
- void revertAttachedBodiesLinkPadding();
-
- void freeMemory(void);
-
- ModelInfo model_geom_;
- std::map<std::string, CollisionNamespace*> coll_namespaces_;
-
- std::map<fcl::CollisionObject*, std::pair<std::string, BodyType> > geom_lookup_map_;
-
- bool previous_set_robot_model_;
-
- boost::shared_ptr<fcl::BroadPhaseCollisionManager> self_geom_manager;
-
-};
-}
-
-#endif
diff --git a/branches/point_cloud/collision_space_fcl/include/collision_space_fcl/environment_objects.h b/branches/point_cloud/collision_space_fcl/include/collision_space_fcl/environment_objects.h
deleted file mode 100644
index dde9d7fea01b92a9df463f9c15d0ae7d28761e2f..0000000000000000000000000000000000000000
--- a/branches/point_cloud/collision_space_fcl/include/collision_space_fcl/environment_objects.h
+++ /dev/null
@@ -1,117 +0,0 @@
-/*********************************************************************
-* 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 Ioan Sucan, E. Gil Jones */
-
-#ifndef COLLISION_SPACE_FCL_ENVIRONMENT_OBJECTS_H
-#define COLLISION_SPACE_FCL_ENVIRONMENT_OBJECTS_H
-
-#include <vector>
-#include <string>
-#include <map>
-
-#include <geometric_shapes/shapes.h>
-#include <LinearMath/btTransform.h>
-
-namespace collision_space_fcl
-{
-
-/** \brief List of objects contained in the environment (not including robot links) */
-class EnvironmentObjects
-{
-public:
- EnvironmentObjects(void)
- {
- }
-
- ~EnvironmentObjects(void)
- {
- clearObjects();
- }
-
- struct NamespaceObjects
- {
- /** \brief An array of static shapes */
- std::vector< shapes::StaticShape* > static_shape;
-
- /** \brief An array of shapes */
- std::vector< shapes::Shape* > shape;
-
- /** \brief An array of shape poses */
- std::vector< btTransform > shape_pose;
- };
-
- /** \brief Get the list of namespaces */
- std::vector<std::string> getNamespaces(void) const;
-
- /** \brief Get the list of objects */
- const NamespaceObjects& getObjects(const std::string &ns) const;
-
- /** \brief Get the list of objects */
- NamespaceObjects& getObjects(const std::string &ns);
-
- /** \brief Add a static object to the namespace. The user releases ownership of the object. */
- void addObject(const std::string &ns, shapes::StaticShape *shape);
-
- /** \brief Add an object to the namespace. The user releases ownership of the object. */
- void addObject(const std::string &ns, shapes::Shape *shape, const btTransform &pose);
-
- /** \brief Remove object. Object equality is verified by comparing pointers. Ownership of the object is renounced upon. Returns true on success. */
- bool removeObject(const std::string &ns, const shapes::Shape *shape);
-
- /** \brief Remove object. Object equality is verified by comparing pointers. Ownership of the object is renounced upon. Returns true on success. */
- bool removeObject(const std::string &ns, const shapes::StaticShape *shape);
-
- /** \brief Clear the objects in a specific namespace. Memory is freed. */
- void clearObjects(const std::string &ns);
-
- /** \brief Clear all objects. Memory is freed. */
- void clearObjects(void);
-
- /** \brief Adds namespace without necessary adding a shape. */
- void addObjectNamespace(const std::string ns);
-
- /** \brief Clone this instance of the class */
- EnvironmentObjects* clone(void) const;
-
-private:
-
- std::map<std::string, NamespaceObjects> objects_;
- NamespaceObjects empty_;
-};
-
-}
-
-#endif
-
diff --git a/branches/point_cloud/collision_space_fcl/mainpage.dox b/branches/point_cloud/collision_space_fcl/mainpage.dox
deleted file mode 100644
index a9b82ede2fe2f3ab3571ee27debc54b00f1177ce..0000000000000000000000000000000000000000
--- a/branches/point_cloud/collision_space_fcl/mainpage.dox
+++ /dev/null
@@ -1,26 +0,0 @@
-/**
-\mainpage
-\htmlinclude manifest.html
-
-\b collision_space_fcl 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.
--->
-
-
-*/
diff --git a/branches/point_cloud/collision_space_fcl/manifest.xml b/branches/point_cloud/collision_space_fcl/manifest.xml
deleted file mode 100644
index d43a59c18e958b90a73fdbd1ee295132cf6b960d..0000000000000000000000000000000000000000
--- a/branches/point_cloud/collision_space_fcl/manifest.xml
+++ /dev/null
@@ -1,23 +0,0 @@
-<package>
- <description brief="collision_space_fcl">
-
- collision_space_fcl
-
- </description>
- <author>Jia Pan</author>
- <license>BSD</license>
- <review status="unreviewed" notes=""/>
-
- <depend package="rosconsole"/>
- <depend package="planning_models"/>
- <depend package="geometric_shapes"/>
- <depend package="bullet"/>
- <depend package="fcl"/>
-
- <url>http://ros.org/wiki/collision_space_fcl</url>
- <export>
- <cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lcollision_space_fcl `rosboost-cfg --lflags thread`"/>
- </export>
-</package>
-
-
diff --git a/branches/point_cloud/collision_space_fcl/src/environment.cpp b/branches/point_cloud/collision_space_fcl/src/environment.cpp
deleted file mode 100644
index b7cd1f47aa35c12045d4346ee179c8fb878334e9..0000000000000000000000000000000000000000
--- a/branches/point_cloud/collision_space_fcl/src/environment.cpp
+++ /dev/null
@@ -1,454 +0,0 @@
-/*********************************************************************
-* 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 Ioan Sucan */
-
-#include "collision_space_fcl/environment.h"
-#include <ros/console.h>
-#include <iomanip>
-
-collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::AllowedCollisionMatrix(const std::vector<std::string>& names,
- bool allowed)
-{
- unsigned int ns = names.size();
- allowed_entries_.resize(ns);
- for(unsigned int i = 0; i < ns; i++) {
- allowed_entries_[i].resize(ns,allowed);
- allowed_entries_bimap_.insert(entry_type::value_type(names[i], i));
- }
- valid_ = true;
-}
-
-collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::AllowedCollisionMatrix(const std::vector<std::vector<bool> >& all_coll_vectors,
- const std::map<std::string, unsigned int>& all_coll_indices)
-{
- unsigned int num_outer = all_coll_vectors.size();
- valid_ = true;
- if(all_coll_indices.size() != all_coll_vectors.size()) {
- valid_ = false;
- ROS_WARN_STREAM("Indices size " << all_coll_indices.size() << " not equal to num vecs " << all_coll_vectors.size());
- return;
- }
- for(std::map<std::string, unsigned int>::const_iterator it = all_coll_indices.begin();
- it != all_coll_indices.end();
- it++) {
- allowed_entries_bimap_.insert(entry_type::value_type(it->first, it->second));
- }
-
- if(allowed_entries_bimap_.left.size() != all_coll_indices.size()) {
- valid_ = false;
- ROS_WARN_STREAM("Some strings or values in allowed collision matrix are repeated");
- }
- if(allowed_entries_bimap_.right.begin()->first != 0) {
- valid_ = false;
- ROS_WARN_STREAM("No entry with index 0 in map");
- }
- if(allowed_entries_bimap_.right.rbegin()->first != num_outer-1) {
- valid_ = false;
- ROS_WARN_STREAM("Last index should be " << num_outer << " but instead is " << allowed_entries_bimap_.right.rbegin()->first);
- }
-
- for(unsigned int i = 0; i < num_outer; i++) {
- if(num_outer != all_coll_vectors[i].size()) {
- valid_ = false;
- ROS_WARN_STREAM("Entries size for " << allowed_entries_bimap_.right.at(i) << " is " << all_coll_vectors[i].size() << " instead of " << num_outer);
- }
- }
- allowed_entries_ = all_coll_vectors;
-}
-
-collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::AllowedCollisionMatrix(const AllowedCollisionMatrix& acm)
-{
- valid_ = acm.valid_;
- allowed_entries_ = acm.allowed_entries_;
- allowed_entries_bimap_ = acm.allowed_entries_bimap_;
-}
-
-bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::getAllowedCollision(const std::string& name1,
- const std::string& name2,
- bool& allowed_collision) const
-{
- entry_type::left_const_iterator it1 = allowed_entries_bimap_.left.find(name1);
- if(it1 == allowed_entries_bimap_.left.end()) {
- return false;
- }
- entry_type::left_const_iterator it2 = allowed_entries_bimap_.left.find(name2);
- if(it2 == allowed_entries_bimap_.left.end()) {
- return false;
- }
- if(it1->second > allowed_entries_.size()) {
- ROS_INFO_STREAM("Something wrong with acm entry for " << name1);
- return false;
- }
- if(it2->second > allowed_entries_[it1->second].size()) {
- ROS_INFO_STREAM("Something wrong with acm entry for " << name2);
- return false;
- }
- allowed_collision = allowed_entries_[it1->second][it2->second];
- return true;
-}
-
-bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::getAllowedCollision(unsigned int i, unsigned int j,
- bool& allowed_collision) const
-{
- if(i > allowed_entries_.size() || j > allowed_entries_[i].size()) {
- return false;
- }
- allowed_collision = allowed_entries_[i][j];
- return true;
-}
-
-bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::hasEntry(const std::string& name) const
-{
- return(allowed_entries_bimap_.left.find(name) != allowed_entries_bimap_.left.end());
-}
-
-bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::getEntryIndex(const std::string& name,
- unsigned int& index) const
-{
- entry_type::left_const_iterator it1 = allowed_entries_bimap_.left.find(name);
- if(it1 == allowed_entries_bimap_.left.end()) {
- return false;
- }
- index = it1->second;
- return true;
-}
-
-bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::getEntryName(const unsigned int ind,
- std::string& name) const
-{
- entry_type::right_const_iterator it1 = allowed_entries_bimap_.right.find(ind);
- if(it1 == allowed_entries_bimap_.right.end()) {
- return false;
- }
- name = it1->second;
- return true;
-}
-
-bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::removeEntry(const std::string& name) {
- if(allowed_entries_bimap_.left.find(name) == allowed_entries_bimap_.left.end()) {
- return false;
- }
- unsigned int last_index = allowed_entries_bimap_.size()-1;
- unsigned int ind = allowed_entries_bimap_.left.find(name)->second;
- allowed_entries_.erase(allowed_entries_.begin()+ind);
- for(unsigned int i = 0; i < allowed_entries_.size(); i++) {
- allowed_entries_[i].erase(allowed_entries_[i].begin()+ind);
- }
- allowed_entries_bimap_.left.erase(name);
- //if this is last ind, no need to decrement
- if(ind != last_index) {
- //sanity checks
- entry_type::right_iterator it = allowed_entries_bimap_.right.find(last_index);
- if(it == allowed_entries_bimap_.right.end()) {
- ROS_INFO_STREAM("Something wrong with last index " << last_index << " ind " << ind);
- }
- //now we need to decrement the index for everything after this
- for(unsigned int i = ind+1; i <= last_index; i++) {
- entry_type::right_iterator it = allowed_entries_bimap_.right.find(i);
- if(it == allowed_entries_bimap_.right.end()) {
- ROS_WARN_STREAM("Problem in replace " << i);
- return false;
- }
- bool successful_replace = allowed_entries_bimap_.right.replace_key(it, i-1);
- if(!successful_replace) {
- ROS_WARN_STREAM("Can't replace");
- return false;
- }
- }
- }
- return true;
-}
-
-bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::addEntry(const std::string& name,
- bool allowed)
-{
- if(allowed_entries_bimap_.left.find(name) != allowed_entries_bimap_.left.end()) {
- return false;
- }
- unsigned int ind = allowed_entries_.size();
- allowed_entries_bimap_.insert(entry_type::value_type(name,ind));
- std::vector<bool> new_entry(ind+1, allowed);
- allowed_entries_.resize(ind+1);
- allowed_entries_[ind] = new_entry;
- for(unsigned int i = 0; i < ind; i++) {
- allowed_entries_[i].resize(ind+1);
- allowed_entries_[i][ind] = allowed;
- }
- return true;
-}
-
-bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::changeEntry(bool allowed)
-{
- for(unsigned int i = 0; i < allowed_entries_.size(); i++) {
- for(unsigned int j = 0; j < allowed_entries_[i].size(); j++) {
- allowed_entries_[i][j] = allowed;
- allowed_entries_[j][i] = allowed;
- }
- }
- return true;
-}
-
-bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::changeEntry(const std::string& name1,
- const std::string& name2,
- bool allowed) {
- entry_type::left_const_iterator it1 = allowed_entries_bimap_.left.find(name1);
- if(it1 == allowed_entries_bimap_.left.end()) {
- return false;
- }
- entry_type::left_const_iterator it2 = allowed_entries_bimap_.left.find(name2);
- if(it2 == allowed_entries_bimap_.left.end()) {
- return false;
- }
- allowed_entries_[it1->second][it2->second] = allowed;
- allowed_entries_[it2->second][it1->second] = allowed;
- return true;
-}
-
-bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::changeEntry(unsigned int i, unsigned int j,
- bool allowed)
-{
- if(i > allowed_entries_.size() || j > allowed_entries_[i].size()) {
- return false;
- }
- allowed_entries_[i][j] = allowed;
- allowed_entries_[j][i] = allowed;
- return true;
-}
-bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::changeEntry(const std::string& name,
- bool allowed)
-{
- if(allowed_entries_bimap_.left.find(name) == allowed_entries_bimap_.left.end()) {
- return false;
- }
- unsigned int ind = allowed_entries_bimap_.left.find(name)->second;
- for(unsigned int i = 0; i < allowed_entries_.size(); i++) {
- allowed_entries_[i][ind] = allowed;
- allowed_entries_[ind][i] = allowed;
- }
- return true;
-}
-
-bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::changeEntry(const std::string& name,
- const std::vector<std::string>& change_names,
- bool allowed)
-{
- bool ok = true;
- if(allowed_entries_bimap_.left.find(name) == allowed_entries_bimap_.left.end()) {
- ROS_DEBUG_STREAM("No entry for " << name);
- ok = false;
- return ok;
- }
- unsigned int ind_1 = allowed_entries_bimap_.left.find(name)->second;
- for(unsigned int i = 0; i < change_names.size(); i++) {
- if(allowed_entries_bimap_.left.find(change_names[i]) == allowed_entries_bimap_.left.end()) {
- ROS_DEBUG_STREAM("No entry for " << change_names[i]);
- ok = false;
- continue;
- }
- unsigned int ind_2 = allowed_entries_bimap_.left.find(change_names[i])->second;
- if(ind_1 >= allowed_entries_.size()) {
- ROS_ERROR_STREAM("Got an index entry for name " << name << " ind " << ind_1 << " but only have "
- << allowed_entries_.size() << " in allowed collision matrix.");
- return false;
- }
- if(ind_2 >= allowed_entries_[ind_1].size()) {
- ROS_ERROR_STREAM("Got an index entry for name " << change_names[i] << " index " << ind_2 << " but only have " <<
- allowed_entries_[ind_1].size() << " in allowed collision matrix.");
- return false;
- }
- allowed_entries_[ind_1][ind_2] = allowed;
- allowed_entries_[ind_2][ind_1] = allowed;
- }
- return ok;
-}
-
-bool collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::changeEntry(const std::vector<std::string>& change_names_1,
- const std::vector<std::string>& change_names_2,
- bool allowed)
-{
- bool ok = true;
- for(unsigned int i = 0; i < change_names_1.size(); i++) {
- if(!changeEntry(change_names_1[i], change_names_2, allowed)) {
- ok = false;
- }
- }
- return ok;
-}
-
-void collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix::print(std::ostream& out) const {
- for(entry_type::right_const_iterator it = allowed_entries_bimap_.right.begin(); it != allowed_entries_bimap_.right.end(); it++) {
- out << std::setw(40) << it->second;
- out << " | ";
- for(entry_type::right_const_iterator it2 = allowed_entries_bimap_.right.begin(); it2 != allowed_entries_bimap_.right.end(); it2++) {
- out << std::setw(3) << allowed_entries_[it->first][it2->first];
- }
- out << std::endl;
- }
-}
-
-bool collision_space_fcl::EnvironmentModel::getVerbose(void) const
-{
- return verbose_;
-}
-
-void collision_space_fcl::EnvironmentModel::setVerbose(bool verbose)
-{
- verbose_ = verbose;
-}
-
-const collision_space_fcl::EnvironmentObjects* collision_space_fcl::EnvironmentModel::getObjects(void) const
-{
- return objects_;
-}
-
-const planning_models::KinematicModel* collision_space_fcl::EnvironmentModel::getRobotModel(void) const
-{
- return robot_model_;
-}
-
-double collision_space_fcl::EnvironmentModel::getRobotScale(void) const
-{
- return robot_scale_;
-}
-
-double collision_space_fcl::EnvironmentModel::getRobotPadding(void) const
-{
- return default_robot_padding_;
-}
-
-void collision_space_fcl::EnvironmentModel::setRobotModel(const planning_models::KinematicModel* model,
- const AllowedCollisionMatrix& acm,
- const std::map<std::string, double>& link_padding_map,
- double default_padding,
- double scale)
-{
- robot_model_ = model;
- default_collision_matrix_ = acm;
- robot_scale_ = scale;
- default_robot_padding_ = default_padding;
- default_link_padding_map_ = link_padding_map;
-}
-
-void collision_space_fcl::EnvironmentModel::lock(void) const
-{
- lock_.lock();
-}
-
-void collision_space_fcl::EnvironmentModel::unlock(void) const
-{
- lock_.unlock();
-}
-
-const collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix&
-collision_space_fcl::EnvironmentModel::getDefaultAllowedCollisionMatrix() const
-{
- return default_collision_matrix_;
-}
-
-const collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix&
-collision_space_fcl::EnvironmentModel::getCurrentAllowedCollisionMatrix() const {
- if(use_altered_collision_matrix_) {
- return altered_collision_matrix_;
- }
- return default_collision_matrix_;
-}
-
-void collision_space_fcl::EnvironmentModel::setAlteredCollisionMatrix(const AllowedCollisionMatrix& acm) {
- use_altered_collision_matrix_ = true;
- altered_collision_matrix_ = acm;
-}
-
-void collision_space_fcl::EnvironmentModel::revertAlteredCollisionMatrix() {
- use_altered_collision_matrix_ = false;
-}
-
-void collision_space_fcl::EnvironmentModel::setAlteredLinkPadding(const std::map<std::string, double>& new_link_padding) {
- altered_link_padding_map_.clear();
- for(std::map<std::string, double>::const_iterator it = new_link_padding.begin();
- it != new_link_padding.end();
- it++) {
- if(default_link_padding_map_.find(it->first) == default_link_padding_map_.end()) {
- //don't have this currently
- continue;
- }
- //only putting in altered padding if it's different
- if(default_link_padding_map_.find(it->first)->second != it->second) {
- altered_link_padding_map_[it->first] = it->second;
- }
- }
- use_altered_link_padding_map_ = true;
-}
-
-void collision_space_fcl::EnvironmentModel::revertAlteredLinkPadding() {
- altered_link_padding_map_.clear();
- use_altered_link_padding_map_ = false;
-}
-
-const std::map<std::string, double>& collision_space_fcl::EnvironmentModel::getDefaultLinkPaddingMap() const {
- return default_link_padding_map_;
-}
-
-std::map<std::string, double> collision_space_fcl::EnvironmentModel::getCurrentLinkPaddingMap() const {
- std::map<std::string, double> ret_map = default_link_padding_map_;
- for(std::map<std::string, double>::const_iterator it = altered_link_padding_map_.begin();
- it != altered_link_padding_map_.end();
- it++) {
- ret_map[it->first] = it->second;
- }
- return ret_map;
-}
-
-double collision_space_fcl::EnvironmentModel::getCurrentLinkPadding(std::string name) const {
- if(altered_link_padding_map_.find(name) != altered_link_padding_map_.end()) {
- return altered_link_padding_map_.find(name)->second;
- } else if(default_link_padding_map_.find(name) != default_link_padding_map_.end()) {
- return default_link_padding_map_.find(name)->second;
- }
- return 0.0;
-}
-
-void collision_space_fcl::EnvironmentModel::setAllowedContacts(const std::vector<AllowedContact>& allowed_contacts)
-{
- allowed_contact_map_.clear();
- allowed_contacts_ = allowed_contacts;
- for(unsigned int i = 0; i < allowed_contacts.size(); i++) {
- allowed_contact_map_[allowed_contacts_[i].body_name_1][allowed_contacts_[i].body_name_2].push_back(allowed_contacts_[i]);
- allowed_contact_map_[allowed_contacts_[i].body_name_2][allowed_contacts_[i].body_name_1].push_back(allowed_contacts_[i]);
- }
-}
-
-const std::vector<collision_space_fcl::EnvironmentModel::AllowedContact>& collision_space_fcl::EnvironmentModel::getAllowedContacts() const {
- return allowed_contacts_;
-}
diff --git a/branches/point_cloud/collision_space_fcl/src/environmentFCL.cpp b/branches/point_cloud/collision_space_fcl/src/environmentFCL.cpp
deleted file mode 100644
index 1f4e8d007322d9206dc46df0318740610819c77f..0000000000000000000000000000000000000000
--- a/branches/point_cloud/collision_space_fcl/src/environmentFCL.cpp
+++ /dev/null
@@ -1,1127 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-#include "collision_space_fcl/environmentFCL.h"
-#include "fcl/geometric_shape_to_BVH_model.h"
-#include "fcl/collision.h"
-#include "fcl/primitive.h"
-#include "fcl/BVH_utility.h"
-#include <geometric_shapes/shape_operations.h>
-#include <ros/console.h>
-#include <cassert>
-#include <cstdio>
-#include <cmath>
-#include <algorithm>
-#include <map>
-
-namespace collision_space_fcl
-{
-
-EnvironmentModelFCL::EnvironmentModelFCL(void) : EnvironmentModel()
-{
- previous_set_robot_model_ = false;
- self_geom_manager.reset(new fcl::SSaPCollisionManager());
-}
-
-
-EnvironmentModelFCL::~EnvironmentModelFCL(void)
-{
- freeMemory();
-}
-
-
-void EnvironmentModelFCL::freeMemory(void)
-{
- for(unsigned int j = 0; j < model_geom_.link_geom.size(); ++j)
- delete model_geom_.link_geom[j];
- model_geom_.link_geom.clear();
- for(std::map<std::string, CollisionNamespace*>::iterator it = coll_namespaces_.begin(); it != coll_namespaces_.end(); ++it)
- delete it->second;
- coll_namespaces_.clear();
- self_geom_manager->clear();
-}
-
-
-void EnvironmentModelFCL::setRobotModel(const planning_models::KinematicModel* model,
- const AllowedCollisionMatrix& allowed_collision_matrix,
- const std::map<std::string, double>& link_padding_map,
- double default_padding,
- double scale)
-{
- EnvironmentModel::setRobotModel(model, allowed_collision_matrix, link_padding_map, default_padding, scale);
- if(previous_set_robot_model_)
- {
- for(unsigned int j = 0; j < model_geom_.link_geom.size(); ++j)
- delete model_geom_.link_geom[j];
- model_geom_.link_geom.clear();
- self_geom_manager->clear();
- attached_bodies_in_collision_matrix_.clear();
- geom_lookup_map_.clear();
- }
- createRobotModel();
- previous_set_robot_model_ = true;
-}
-
-
-void EnvironmentModelFCL::getAttachedBodyPoses(std::map<std::string, std::vector<btTransform> >& pose_map) const
-{
- pose_map.clear();
-
- const unsigned int n = model_geom_.link_geom.size();
- for(unsigned int i = 0; i < n; ++i)
- {
- LinkGeom* lg = model_geom_.link_geom[i];
-
- /* create new set of attached bodies */
- const unsigned int nab = lg->att_bodies.size();
- std::vector<btTransform> nbt;
- for(unsigned int j = 0; j < nab; ++j)
- {
- for(unsigned int k = 0; k < lg->att_bodies[j]->geom.size(); k++)
- {
- const fcl::SimpleQuaternion& q = lg->att_bodies[j]->geom[k]->getQuatRotation();
- const fcl::Vec3f& t = lg->att_bodies[j]->geom[k]->getTranslation();
- nbt.push_back(btTransform(btQuaternion(q.getX(), q.getY(), q.getZ(), q.getW()), btVector3(t[0], t[1], t[2])));
- }
-
- pose_map[lg->att_bodies[j]->att->getName()] = nbt;
- }
- }
-}
-
-
-void EnvironmentModelFCL::createRobotModel()
-{
- for(unsigned int i = 0; i < robot_model_->getLinkModels().size(); ++i)
- {
- /* skip this link if we have no geometry or if the link
- name is not specified as enabled for collision
- checking */
- const planning_models::KinematicModel::LinkModel* link = robot_model_->getLinkModels()[i];
- if (!link || !link->getLinkShape())
- continue;
-
- LinkGeom* lg = new LinkGeom();
- lg->link = link;
- if(!default_collision_matrix_.getEntryIndex(link->getName(), lg->index))
- {
- ROS_WARN_STREAM("Link " << link->getName() << " not in provided collision matrix");
- }
-
- double padd = default_robot_padding_;
- if(default_link_padding_map_.find(link->getName()) != default_link_padding_map_.end())
- {
- padd = default_link_padding_map_.find(link->getName())->second;
- }
- ROS_DEBUG_STREAM("Link " << link->getName() << " padding " << padd);
-
- fcl::CollisionObject* unpadd_g = createGeom(link->getLinkShape(), 1.0, 0.0);
- assert(unpadd_g);
- lg->geom.push_back(unpadd_g);
- self_geom_manager->registerObject(unpadd_g);
- geom_lookup_map_[unpadd_g] = std::pair<std::string, BodyType>(link->getName(), LINK);
-
- fcl::CollisionObject* padd_g = createGeom(link->getLinkShape(), robot_scale_, padd);
- assert(padd_g);
- lg->padded_geom.push_back(padd_g);
- geom_lookup_map_[padd_g] = std::pair<std::string, BodyType>(link->getName(), LINK);
- const std::vector<planning_models::KinematicModel::AttachedBodyModel*>& attached_bodies = link->getAttachedBodyModels();
- for(unsigned int j = 0 ; j < attached_bodies.size() ; ++j)
- {
- padd = default_robot_padding_;
- if(default_link_padding_map_.find(attached_bodies[j]->getName()) != default_link_padding_map_.end())
- padd = default_link_padding_map_.find(attached_bodies[j]->getName())->second;
- else if(default_link_padding_map_.find("attached") != default_link_padding_map_.end())
- padd = default_link_padding_map_.find("attached")->second;
- addAttachedBody(lg, attached_bodies[j], padd);
- }
- model_geom_.link_geom.push_back(lg);
- }
-}
-
-
-fcl::CollisionObject* EnvironmentModelFCL::createGeom(const shapes::StaticShape *shape)
-{
- fcl::CollisionObject* g = NULL;
- switch(shape->type)
- {
- case shapes::PLANE:
- {
- // TODO: plane implementation
- ROS_WARN_STREAM("Plane is not fully implemented using FCL yet");
- }
- break;
- default:
- break;
- }
- return g;
-}
-
-
-fcl::CollisionObject* EnvironmentModelFCL::createGeom(const shapes::Shape *shape, double scale, double padding)
-{
- fcl::BVHModel<fcl::OBB>* g = new fcl::BVHModel<fcl::OBB>();
-
- switch(shape->type)
- {
- case shapes::SPHERE:
- {
- fcl::generateBVHModelPointCloud(*g, fcl::Sphere(static_cast<const shapes::Sphere*>(shape)->radius * scale + padding));
- fcl::Uncertainty* uc = new fcl::Uncertainty[g->num_vertices];
- fcl::estimateSamplingUncertainty(g->vertices, g->num_vertices, uc);
- fcl::BVHExpand(*g, uc, 1);
- g->uc = uc;
- }
- break;
- case shapes::BOX:
- {
- const double *size = static_cast<const shapes::Box*>(shape)->size;
- fcl::generateBVHModelPointCloud(*g, fcl::Box(size[0] * scale + padding * 2.0, size[1] * scale + padding * 2.0, size[2] * scale + padding * 2.0));
- fcl::Uncertainty* uc = new fcl::Uncertainty[g->num_vertices];
- fcl::estimateSamplingUncertainty(g->vertices, g->num_vertices, uc);
- fcl::BVHExpand(*g, uc, 1);
- g->uc = uc;
- }
- break;
- case shapes::CYLINDER:
- {
- fcl::generateBVHModelPointCloud(*g, fcl::Cylinder(static_cast<const shapes::Cylinder*>(shape)->radius * scale + padding,
- static_cast<const shapes::Cylinder*>(shape)->length * scale + padding * 2.0));
- fcl::Uncertainty* uc = new fcl::Uncertainty[g->num_vertices];
- fcl::estimateSamplingUncertainty(g->vertices, g->num_vertices, uc);
- fcl::BVHExpand(*g, uc, 1);
- g->uc = uc;
- }
- break;
- case shapes::MESH:
- {
- const shapes::Mesh *mesh = static_cast<const shapes::Mesh*>(shape);
- if(mesh->vertexCount > 0 && mesh->triangleCount > 0)
- {
- std::vector<fcl::Triangle> tri_indices(mesh->triangleCount);
- for(unsigned int i = 0; i < mesh->triangleCount; ++i)
- tri_indices[i] = fcl::Triangle(mesh->triangles[3 * i], mesh->triangles[3 * i + 1], mesh->triangles[3 * i + 2]);
-
- std::vector<fcl::Vec3f> points(mesh->vertexCount);
- double sx = 0.0, sy = 0.0, sz = 0.0;
- for(unsigned int i = 0; i < mesh->vertexCount; ++i)
- {
- points[i] = fcl::Vec3f(mesh->vertices[3 * i], mesh->vertices[3 * i + 1], mesh->vertices[3 * i + 2]);
- sx += points[i][0];
- sy += points[i][1];
- sz += points[i][2];
- }
- // the center of the mesh
- sx /= (double)mesh->vertexCount;
- sy /= (double)mesh->vertexCount;
- sz /= (double)mesh->vertexCount;
-
- // scale the mesh
- for(unsigned int i = 0; i < mesh->vertexCount; ++i)
- {
- // vector from center to the vertex
- double dx = points[i][0] - sx;
- double dy = points[i][1] - sy;
- double dz = points[i][2] - sz;
-
- // length of vector
- //double norm = sqrt(dx * dx + dy * dy + dz * dz);
-
- double ndx = ((dx > 0) ? dx+padding : dx-padding);
- double ndy = ((dy > 0) ? dy+padding : dy-padding);
- double ndz = ((dz > 0) ? dz+padding : dz-padding);
-
- // the new distance of the vertex from the center
- //double fact = scale + padding/norm;
- points[i] = fcl::Vec3f(sx + ndx, sy + ndy, sz + ndz);
- }
-
- g->beginModel();
- g->addSubModel(points);
- g->endModel();
- g->computeAABB();
-
- fcl::Uncertainty* uc = new fcl::Uncertainty[g->num_vertices];
- fcl::estimateSamplingUncertainty(g->vertices, g->num_vertices, uc);
- fcl::BVHExpand(*g, uc, 1);
- g->uc = uc;
- }
- }
- break;
- default:
- ;
- }
-
- return g;
-}
-
-
-void EnvironmentModelFCL::updateGeom(fcl::CollisionObject* geom, const btTransform &pose) const
-{
- btQuaternion q = pose.getRotation();
- const btVector3& o = pose.getOrigin();
- geom->setQuatRotation(fcl::SimpleQuaternion(q.getW(), q.getX(), q.getY(), q.getZ()));
- geom->setTranslation(fcl::Vec3f(o.getX(), o.getY(), o.getZ()));
- geom->computeCachedAABB(); // update AABB
-}
-
-
-void EnvironmentModelFCL::updateAttachedBodies()
-{
- updateAttachedBodies(default_link_padding_map_);
-}
-
-
-
-void EnvironmentModelFCL::updateAttachedBodies(const std::map<std::string, double>& link_padding_map)
-{
- //getting rid of all entries associated with the current attached bodies
- for(std::map<std::string, bool>::iterator it = attached_bodies_in_collision_matrix_.begin();
- it != attached_bodies_in_collision_matrix_.end();
- it++)
- {
- if(!default_collision_matrix_.removeEntry(it->first))
- {
- ROS_WARN_STREAM("No entry in default collision matrix for attached body " << it->first <<
- " when there really should be.");
- }
- }
-
- attached_bodies_in_collision_matrix_.clear();
- for(unsigned int i = 0; i < model_geom_.link_geom.size(); ++i)
- {
- LinkGeom* lg = model_geom_.link_geom[i];
-
- for(unsigned int j = 0; j < lg->att_bodies.size(); j++)
- {
- for(unsigned int k = 0; k < lg->att_bodies[j]->geom.size(); k++)
- {
- geom_lookup_map_.erase(lg->att_bodies[j]->geom[k]);
- self_geom_manager->unregisterObject(lg->att_bodies[j]->geom[k]);
- }
- for(unsigned int k = 0; k < lg->att_bodies[j]->padded_geom.size(); k++)
- {
- geom_lookup_map_.erase(lg->att_bodies[j]->padded_geom[k]);
- }
- }
- lg->deleteAttachedBodies();
-
- /* create new set of attached bodies */
- const std::vector<planning_models::KinematicModel::AttachedBodyModel*>& attached_bodies = lg->link->getAttachedBodyModels();
- for(unsigned int j = 0; j < attached_bodies.size(); ++j)
- {
- double padd = default_robot_padding_;
- if(link_padding_map.find(attached_bodies[j]->getName()) != link_padding_map.end())
- {
- padd = link_padding_map.find(attached_bodies[j]->getName())->second;
- }
- else if (link_padding_map.find("attached") != link_padding_map.end())
- {
- padd = link_padding_map.find("attached")->second;
- }
- ROS_DEBUG_STREAM("Updating attached body " << attached_bodies[j]->getName());
- addAttachedBody(lg, attached_bodies[j], padd);
- }
- }
-}
-
-
-
-void EnvironmentModelFCL::addAttachedBody(LinkGeom* lg, const planning_models::KinematicModel::AttachedBodyModel* attm, double padd)
-{
-
- AttGeom* attg = new AttGeom();
- attg->att = attm;
-
- if(!default_collision_matrix_.addEntry(attm->getName(), false))
- {
- ROS_WARN_STREAM("Must already have an entry in allowed collision matrix for " << attm->getName());
- }
- else
- {
- ROS_DEBUG_STREAM("Adding entry for " << attm->getName());
- }
-
- attached_bodies_in_collision_matrix_[attm->getName()] = true;
- default_collision_matrix_.getEntryIndex(attm->getName(), attg->index);
- //setting touch links
- for(unsigned int i = 0; i < attm->getTouchLinks().size(); i++)
- {
- if(default_collision_matrix_.hasEntry(attm->getTouchLinks()[i]))
- {
- if(!default_collision_matrix_.changeEntry(attm->getName(), attm->getTouchLinks()[i], true))
- {
- ROS_WARN_STREAM("No entry in allowed collision matrix for " << attm->getName() << " and " << attm->getTouchLinks()[i]);
- }
- else
- {
- ROS_DEBUG_STREAM("Adding touch link for " << attm->getName() << " and " << attm->getTouchLinks()[i]);
- }
- }
- }
- for(unsigned int i = 0; i < attm->getShapes().size(); i++)
- {
- fcl::CollisionObject* ga = createGeom(attm->getShapes()[i], 1.0, 0.0);
- assert(ga);
- attg->geom.push_back(ga);
- self_geom_manager->registerObject(ga);
- geom_lookup_map_[ga] = std::pair<std::string, BodyType>(attm->getName(), ATTACHED);
-
- fcl::CollisionObject* padd_ga = createGeom(attm->getShapes()[i], robot_scale_, padd);
- assert(padd_ga);
- attg->padded_geom.push_back(padd_ga);
- geom_lookup_map_[padd_ga] = std::pair<std::string, BodyType>(attm->getName(), ATTACHED);
- }
- lg->att_bodies.push_back(attg);
-}
-
-
-
-void EnvironmentModelFCL::setAttachedBodiesLinkPadding()
-{
- for(unsigned int i = 0; i < model_geom_.link_geom.size(); ++i)
- {
- LinkGeom* lg = model_geom_.link_geom[i];
-
- const std::vector<planning_models::KinematicModel::AttachedBodyModel*>& attached_bodies = lg->link->getAttachedBodyModels();
- for(unsigned int j = 0; j < attached_bodies.size(); ++j)
- {
- double new_padd = -1.0;
- if(altered_link_padding_map_.find(attached_bodies[j]->getName()) != altered_link_padding_map_.end())
- {
- new_padd = altered_link_padding_map_.find(attached_bodies[j]->getName())->second;
- }
- else if(altered_link_padding_map_.find("attached") != altered_link_padding_map_.end())
- {
- new_padd = altered_link_padding_map_.find("attached")->second;
- }
- if(new_padd != -1.0)
- {
- for(unsigned int k = 0; k < attached_bodies[j]->getShapes().size(); k++)
- {
- geom_lookup_map_.erase(lg->att_bodies[j]->padded_geom[k]);
- delete lg->att_bodies[j]->padded_geom[k];
- fcl::CollisionObject* padd_ga = createGeom(attached_bodies[j]->getShapes()[k], robot_scale_, new_padd);
- assert(padd_ga);
- lg->att_bodies[j]->padded_geom[k] = padd_ga;
- geom_lookup_map_[padd_ga] = std::pair<std::string, BodyType>(attached_bodies[j]->getName(), ATTACHED);
- }
- }
- }
- }
-}
-
-
-void EnvironmentModelFCL::revertAttachedBodiesLinkPadding()
-{
- for(unsigned int i = 0; i < model_geom_.link_geom.size(); ++i)
- {
- LinkGeom* lg = model_geom_.link_geom[i];
-
- const std::vector<planning_models::KinematicModel::AttachedBodyModel*>& attached_bodies = lg->link->getAttachedBodyModels();
- for(unsigned int j = 0; j < attached_bodies.size(); ++j)
- {
- double new_padd = -1.0;
- if(altered_link_padding_map_.find(attached_bodies[j]->getName()) != altered_link_padding_map_.end())
- {
- new_padd = default_link_padding_map_.find(attached_bodies[j]->getName())->second;
- }
- else if(altered_link_padding_map_.find("attached") != altered_link_padding_map_.end())
- {
- new_padd = default_link_padding_map_.find("attached")->second;
- }
- if(new_padd != -1.0)
- {
- for(unsigned int k = 0; k < attached_bodies[j]->getShapes().size(); k++)
- {
- geom_lookup_map_.erase(lg->att_bodies[j]->padded_geom[k]);
- delete lg->att_bodies[j]->padded_geom[k];
- fcl::CollisionObject* padd_ga = createGeom(attached_bodies[j]->getShapes()[k], robot_scale_, new_padd);
- assert(padd_ga);
- lg->att_bodies[j]->padded_geom[k] = padd_ga;
- geom_lookup_map_[padd_ga] = std::pair<std::string, BodyType>(attached_bodies[j]->getName(), ATTACHED);
- }
- }
- }
- }
-}
-
-
-
-void EnvironmentModelFCL::updateRobotModel(const planning_models::KinematicState* state)
-{
- const unsigned int n = model_geom_.link_geom.size();
-
- for(unsigned int i = 0; i < n; ++i)
- {
- const planning_models::KinematicState::LinkState* link_state = state->getLinkState(model_geom_.link_geom[i]->link->getName());
- if(link_state == NULL)
- {
- ROS_WARN_STREAM("No link state for link " << model_geom_.link_geom[i]->link->getName());
- continue;
- }
- updateGeom(model_geom_.link_geom[i]->geom[0], link_state->getGlobalCollisionBodyTransform());
- updateGeom(model_geom_.link_geom[i]->padded_geom[0], link_state->getGlobalCollisionBodyTransform());
- const std::vector<planning_models::KinematicState::AttachedBodyState*>& attached_bodies = link_state->getAttachedBodyStateVector();
- for(unsigned int j = 0; j < attached_bodies.size(); ++j)
- {
- for(unsigned int k = 0; k < attached_bodies[j]->getGlobalCollisionBodyTransforms().size(); k++)
- {
- updateGeom(model_geom_.link_geom[i]->att_bodies[j]->geom[k], attached_bodies[j]->getGlobalCollisionBodyTransforms()[k]);
- updateGeom(model_geom_.link_geom[i]->att_bodies[j]->padded_geom[k], attached_bodies[j]->getGlobalCollisionBodyTransforms()[k]);
- }
- }
- }
-
- self_geom_manager->update();
-}
-
-
-void EnvironmentModelFCL::setAlteredLinkPadding(const std::map<std::string, double>& new_link_padding)
-{
-
- //updating altered map
- EnvironmentModel::setAlteredLinkPadding(new_link_padding);
-
- for(unsigned int i = 0; i < model_geom_.link_geom.size(); i++)
- {
- LinkGeom* lg = model_geom_.link_geom[i];
-
- if(altered_link_padding_map_.find(lg->link->getName()) != altered_link_padding_map_.end())
- {
- double new_padding = altered_link_padding_map_.find(lg->link->getName())->second;
- const planning_models::KinematicModel::LinkModel* link = lg->link;
- if (!link || !link->getLinkShape())
- {
- ROS_WARN_STREAM("Can't get kinematic model for link " << link->getName() << " to make new padding");
- continue;
- }
- ROS_DEBUG_STREAM("Setting padding for link " << lg->link->getName() << " from "
- << default_link_padding_map_[lg->link->getName()]
- << " to " << new_padding);
- //otherwise we clear out the data associated with the old one
- for(unsigned int j = 0; j < lg->padded_geom.size() ; ++j)
- {
- geom_lookup_map_.erase(lg->padded_geom[j]);
- delete lg->padded_geom[j];
- }
- lg->padded_geom.clear();
- fcl::CollisionObject* g = createGeom(link->getLinkShape(), robot_scale_, new_padding);
- assert(g);
- lg->padded_geom.push_back(g);
- geom_lookup_map_[g] = std::pair<std::string, BodyType>(link->getName(), LINK);
- }
- }
- //this does all the work
- setAttachedBodiesLinkPadding();
-}
-
-
-void EnvironmentModelFCL::revertAlteredLinkPadding()
-{
- for(unsigned int i = 0; i < model_geom_.link_geom.size(); i++)
- {
- LinkGeom* lg = model_geom_.link_geom[i];
-
- if(altered_link_padding_map_.find(lg->link->getName()) != altered_link_padding_map_.end())
- {
- double old_padding = default_link_padding_map_.find(lg->link->getName())->second;
- const planning_models::KinematicModel::LinkModel* link = lg->link;
- if (!link || !link->getLinkShape())
- {
- ROS_WARN_STREAM("Can't get kinematic model for link " << link->getName() << " to revert to old padding");
- continue;
- }
- //otherwise we clear out the data associated with the old one
- for(unsigned int j = 0; j < lg->padded_geom.size(); ++j)
- {
- geom_lookup_map_.erase(lg->padded_geom[j]);
- delete lg->padded_geom[j];
- }
- ROS_DEBUG_STREAM("Reverting padding for link " << lg->link->getName() << " from " << altered_link_padding_map_[lg->link->getName()]
- << " to " << old_padding);
- lg->padded_geom.clear();
- fcl::CollisionObject* g = createGeom(link->getLinkShape(), robot_scale_, old_padding);
- assert(g);
- lg->padded_geom.push_back(g);
- geom_lookup_map_[g] = std::pair<std::string, BodyType>(link->getName(), LINK);
- }
- }
- revertAttachedBodiesLinkPadding();
-
- //clears altered map
- EnvironmentModel::revertAlteredLinkPadding();
-}
-
-
-bool EnvironmentModelFCL::getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_total, unsigned int max_per_pair) const
-{
- contacts.clear();
- CollisionData cdata;
- cdata.allowed_collision_matrix = &getCurrentAllowedCollisionMatrix();
- cdata.geom_lookup_map = &geom_lookup_map_;
- cdata.contacts = &contacts;
- cdata.max_contacts_total = max_total;
- cdata.max_contacts_pair = max_per_pair;
- if (!allowed_contacts_.empty())
- cdata.allowed = &allowed_contact_map_;
- contacts.clear();
- testCollision(&cdata);
- return cdata.collides;
-}
-
-
-bool EnvironmentModelFCL::getAllCollisionContacts(std::vector<Contact> &contacts, unsigned int num_contacts_per_pair) const
-{
- contacts.clear();
- CollisionData cdata;
- cdata.geom_lookup_map = &geom_lookup_map_;
- cdata.allowed_collision_matrix = &getCurrentAllowedCollisionMatrix();
- cdata.contacts = &contacts;
- cdata.max_contacts_total = UINT_MAX;
- cdata.max_contacts_pair = num_contacts_per_pair;
- cdata.exhaustive = true;
- if (!allowed_contacts_.empty())
- cdata.allowed = &allowed_contact_map_;
- contacts.clear();
- testCollision(&cdata);
- return cdata.collides;
-}
-
-bool EnvironmentModelFCL::isCollision(void) const
-{
- CollisionData cdata;
- cdata.allowed_collision_matrix = &getCurrentAllowedCollisionMatrix();
- cdata.geom_lookup_map = &geom_lookup_map_;
- if(!allowed_contacts_.empty())
- {
- cdata.allowed = &allowed_contact_map_;
- ROS_DEBUG_STREAM("Got contacts size " << cdata.allowed->size());
- }
- else
- {
- ROS_DEBUG_STREAM("No allowed contacts");
- }
-
- testCollision(&cdata);
- return cdata.collides;
-}
-
-
-bool EnvironmentModelFCL::isSelfCollision(void) const
-{
- CollisionData cdata;
- cdata.geom_lookup_map = &geom_lookup_map_;
- cdata.allowed_collision_matrix = &getCurrentAllowedCollisionMatrix();
- if (!allowed_contacts_.empty())
- cdata.allowed = &allowed_contact_map_;
- testSelfCollision(&cdata);
- return cdata.collides;
-}
-
-
-bool EnvironmentModelFCL::isEnvironmentCollision(void) const
-{
- CollisionData cdata;
- cdata.geom_lookup_map = &geom_lookup_map_;
- cdata.allowed_collision_matrix = &getCurrentAllowedCollisionMatrix();
- if (!allowed_contacts_.empty())
- cdata.allowed = &allowed_contact_map_;
- testEnvironmentCollision(&cdata);
- return cdata.collides;
-}
-
-
-bool collisionCallBack(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* cdata_)
-{
- EnvironmentModelFCL::CollisionData* cdata = (EnvironmentModelFCL::CollisionData*)cdata_;
-
- if(cdata->done) return true;
-
- // first figure out what check is happening
- bool check_in_allowed_collision_matrix = true;
-
- std::string object_name;
-
- std::map<fcl::CollisionObject*, std::pair<std::string, EnvironmentModelFCL::BodyType> >::const_iterator it1 = cdata->geom_lookup_map->find(o1);
- std::map<fcl::CollisionObject*, std::pair<std::string, EnvironmentModelFCL::BodyType> >::const_iterator it2 = cdata->geom_lookup_map->find(o2);
-
- if(it1 != cdata->geom_lookup_map->end())
- {
- cdata->body_name_1 = it1->second.first;
- cdata->body_type_1 = it1->second.second;
- }
- else
- {
- //ROS_WARN_STREAM("Object to be added does not have entry in geometry lookup map");
- cdata->body_name_1 = "";
- cdata->body_type_1 = EnvironmentModelFCL::OBJECT;
- check_in_allowed_collision_matrix = false;
- }
-
- if(it2 != cdata->geom_lookup_map->end())
- {
- cdata->body_name_2 = it2->second.first;
- cdata->body_type_2 = it2->second.second;
- }
- else
- {
- //ROS_WARN_STREAM("Object to be added does not have entry in geometry lookup map");
- cdata->body_name_2 = "";
- cdata->body_type_2 = EnvironmentModelFCL::OBJECT;
- check_in_allowed_collision_matrix = false;
- }
-
-
- // determine whether or not this collision is allowed in the self_collision matrix
- if (cdata->allowed_collision_matrix && check_in_allowed_collision_matrix)
- {
- bool allowed;
- if(!cdata->allowed_collision_matrix->getAllowedCollision(cdata->body_name_1, cdata->body_name_2, allowed))
- {
- ROS_WARN_STREAM("No entry in allowed collision matrix for " << cdata->body_name_1 << " and " << cdata->body_name_2);
- return cdata->done;
- }
- if(allowed)
- {
- ROS_DEBUG_STREAM("Will not test for collision between " << cdata->body_name_1 << " and " << cdata->body_name_2);
- return cdata->done;
- }
- else
- {
- ROS_DEBUG_STREAM("Will test for collision between " << cdata->body_name_1 << " and " << cdata->body_name_2);
- }
-
- }
-
- // do the actual collision check to get the desired number of contacts
-
- if(!cdata->contacts && !cdata->allowed)
- {
- bool enable_contact = false;
- bool exhaustive = false;
- std::vector<fcl::Contact> contacts;
- int num_contacts = fcl::collide(o1, o2, 1, exhaustive, enable_contact, contacts);
-
- if(num_contacts > 0)
- {
- cdata->collides = true;
- cdata->done = true;
- }
- }
- else
- {
- unsigned int num_not_allowed = 0;
- // here is quite subtle: the exhaustive is high level one in collision space
- // if exhaustive, then each pair will report at most cdata->max_contacts contacts
- // else, then we report at most cdata->max_contacts in whole.
- // so both cases correspond to the exhaustive = false in low level FCL collision.
- int num_max_contacts;
- if(cdata->exhaustive)
- num_max_contacts = cdata->max_contacts_pair;
- else
- num_max_contacts = cdata->max_contacts_total - cdata->contacts->size();
-
-
- bool enable_contact = true;
- std::vector<fcl::Contact> contacts;
- int num_contacts = fcl::collide(o1, o2, num_max_contacts, false, enable_contact, contacts);
-
- for(int i = 0; i < num_contacts; ++i)
- {
- //already enough contacts, so just quit
- if(!cdata->exhaustive && cdata->max_contacts_total > 0 && cdata->contacts->size() >= cdata->max_contacts_total)
- {
- break;
- }
-
- fcl::Vec3f normal = contacts[i].normal;
- fcl::Vec3f contact_p = contacts[i].pos;
- fcl::BVH_REAL depth = contacts[i].penetration_depth;
-
- btVector3 pos(contact_p[0], contact_p[1], contact_p[2]);
-
- //figure out whether the contact is allowed
- //allowed contacts only allowed with objects for now
- bool allowed = false;
- if(cdata->allowed)
- {
- EnvironmentModel::AllowedContactMap::const_iterator it1 = cdata->allowed->find(cdata->body_name_1);
- if(it1 != cdata->allowed->end())
- {
- std::map<std::string, std::vector<EnvironmentModel::AllowedContact> >::const_iterator it2 = it1->second.find(cdata->body_name_2);
- if(it2 != it1->second.end())
- {
- ROS_DEBUG_STREAM("Testing allowed contact for " << cdata->body_name_1 << " and " << cdata->body_name_2 << " num " << i);
- ROS_DEBUG_STREAM("Contact at " << pos[0] << " " << pos[1] << " " << pos[2]);
-
- const std::vector<EnvironmentModel::AllowedContact>& av = it2->second;
- for(unsigned int j = 0; j < av.size(); j++)
- {
- if(av[j].bound->containsPoint(pos))
- {
- if(av[j].depth >= fabs(depth))
- {
- allowed = true;
- ROS_DEBUG_STREAM("Contact allowed by allowed collision region");
- break;
- }
- else
- {
- ROS_DEBUG_STREAM("Depth check failing " << av[j].depth << " detected " << depth);
- }
- }
- }
- }
- }
- }
-
- if(!allowed)
- {
- cdata->collides = true;
- num_not_allowed++;
-
- if(cdata->contacts != NULL)
- {
- if(num_not_allowed <= cdata->max_contacts_pair)
- {
- EnvironmentModelFCL::Contact add;
- add.pos = pos;
- add.normal = btVector3(normal[0], normal[1], normal[2]);
- add.depth = depth;
-
- add.body_name_1 = cdata->body_name_1;
- add.body_name_2 = cdata->body_name_2;
- add.body_type_1 = cdata->body_type_1;
- add.body_type_2 = cdata->body_type_2;
-
- cdata->contacts->push_back(add);
- if(!cdata->exhaustive && cdata->contacts->size() >= cdata->max_contacts_total)
- cdata->done = true;
- }
- }
- }
- else
- {
- cdata->done = true;
- }
- }
- }
-
- return cdata->done;
-}
-
-
-void EnvironmentModelFCL::testObjectCollision(CollisionNamespace *cn, CollisionData *cdata) const
-{
- if(cn->env_geom_manager->empty())
- {
- ROS_WARN_STREAM("Problem - collide2 required for body collision for " << cn->name);
- return;
- }
-
- cn->env_geom_manager->setup();
- for(int i = model_geom_.link_geom.size() - 1; i >= 0; --i)
- {
- LinkGeom* lg = model_geom_.link_geom[i];
-
- bool allowed = false;
- if(cdata->allowed_collision_matrix)
- {
- if(!cdata->allowed_collision_matrix->getAllowedCollision(cn->name, lg->link->getName(), allowed))
- {
- ROS_WARN_STREAM("No entry in cdata allowed collision matrix for " << cn->name << " and " << lg->link->getName());
- return;
- }
- }
-
- //have to test collisions with link
- if(!allowed)
- {
- ROS_DEBUG_STREAM("Will test for collision between object " << cn->name << " and link " << lg->link->getName());
- for(unsigned int j = 0; j < lg->padded_geom.size(); j++)
- {
- //have to figure
- unsigned int current_contacts_size = 0;
- if(cdata->contacts)
- {
- current_contacts_size = cdata->contacts->size();
- }
-
- //collision core
- cn->env_geom_manager->collide(lg->padded_geom[j], cdata, collisionCallBack);
-
- if(cdata->contacts && cdata->contacts->size() > current_contacts_size)
- {
- //new contacts must mean collision
- for(unsigned int k = current_contacts_size; k < cdata->contacts->size(); k++)
- {
- if(cdata->contacts->at(k).body_type_1 == OBJECT)
- {
- cdata->contacts->at(k).body_name_1 = cn->name;
- }
- else if(cdata->contacts->at(k).body_type_2 == OBJECT)
- {
- cdata->contacts->at(k).body_name_2 = cn->name;
- }
- else
- {
- ROS_WARN_STREAM("New contacts really should have an object as one of the bodies");
- }
- }
- }
- if(cdata->done)
- {
- return;
- }
- }
- }
- else
- {
- ROS_DEBUG_STREAM("Will not test for allowed collision between object " << cn->name << " and link " << lg->link->getName());
- }
-
- //now we need to do the attached bodies
- for(unsigned int j = 0; j < lg->att_bodies.size(); j++)
- {
- std::string att_name = lg->att_bodies[j]->att->getName();
- allowed = false;
- if(cdata->allowed_collision_matrix)
- {
- if(!cdata->allowed_collision_matrix->getAllowedCollision(cn->name, att_name, allowed))
- {
- ROS_WARN_STREAM("No entry in current allowed collision matrix for " << cn->name << " and " << att_name);
- return;
- }
- }
- if(!allowed)
- {
- ROS_DEBUG_STREAM("Will test for collision between object " << cn->name << " and attached object " << att_name);
- for(unsigned int k = 0; k < lg->att_bodies[j]->padded_geom.size(); k++)
- {
- //have to figure
- unsigned int current_contacts_size = 0;
- if(cdata->contacts)
- {
- current_contacts_size = cdata->contacts->size();
- }
-
- //collision core
- cn->env_geom_manager->collide(lg->att_bodies[j]->padded_geom[k], cdata, collisionCallBack);
-
- if(cdata->contacts && cdata->contacts->size() > current_contacts_size)
- {
- //new contacts must mean collision
- for(unsigned int l = current_contacts_size; l < cdata->contacts->size(); l++)
- {
- if(cdata->contacts->at(l).body_type_1 == OBJECT)
- {
- cdata->contacts->at(l).body_name_1 = cn->name;
- }
- else if(cdata->contacts->at(l).body_type_2 == OBJECT)
- {
- cdata->contacts->at(l).body_name_2 = cn->name;
- }
- else
- {
- ROS_WARN_STREAM("New contacts really should have an object as one of the bodys");
- }
- }
- }
- if(cdata->done)
- {
- return;
- }
- }
- }
- else
- {
- ROS_DEBUG_STREAM("Will not test for allowed collision between object " << cn->name << " and attached object " << att_name);
- }
- }
- }
-}
-
-
-void EnvironmentModelFCL::testCollision(CollisionData *cdata) const
-{
- testSelfCollision(cdata);
- testEnvironmentCollision(cdata);
-}
-
-
-void EnvironmentModelFCL::testSelfCollision(CollisionData *cdata) const
-{
- self_geom_manager->setup();
- self_geom_manager->collide(cdata, collisionCallBack);
-}
-
-
-void EnvironmentModelFCL::testEnvironmentCollision(CollisionData *cdata) const
-{
- /* check collision with other bodies until done*/
- for(std::map<std::string, CollisionNamespace*>::const_iterator it = coll_namespaces_.begin() ; it != coll_namespaces_.end() && !cdata->done ; ++it)
- {
- testObjectCollision(it->second, cdata);
- }
-}
-
-
-bool EnvironmentModelFCL::hasObject(const std::string& ns) const
-{
- if(coll_namespaces_.find(ns) != coll_namespaces_.end())
- {
- return true;
- }
- return false;
-}
-
-
-void EnvironmentModelFCL::addObjects(const std::string& ns, const std::vector<shapes::Shape*>& shapes, const std::vector<btTransform>& poses)
-{
- assert(shapes.size() == poses.size());
- std::map<std::string, CollisionNamespace*>::iterator it = coll_namespaces_.find(ns);
- CollisionNamespace* cn = NULL;
- if(it == coll_namespaces_.end())
- {
- cn = new CollisionNamespace(ns);
- coll_namespaces_[ns] = cn;
- default_collision_matrix_.addEntry(ns, false);
- }
- else
- {
- cn = it->second;
- }
-
- //we're going to create the namespace in objects_ even if it doesn't have anything in it
- objects_->addObjectNamespace(ns);
-
- unsigned int n = shapes.size();
- for(unsigned int i = 0; i < n; ++i)
- {
- fcl::CollisionObject* g = createGeom(shapes[i], 1.0, 0.0);
- assert(g);
- updateGeom(g, poses[i]);
- cn->geoms.push_back(g);
- cn->env_geom_manager->registerObject(g);
- objects_->addObject(ns, shapes[i], poses[i]);
- }
-}
-
-
-void EnvironmentModelFCL::addObject(const std::string& ns, shapes::Shape* shape, const btTransform& pose)
-{
- std::map<std::string, CollisionNamespace*>::iterator it = coll_namespaces_.find(ns);
- CollisionNamespace* cn = NULL;
- if(it == coll_namespaces_.end())
- {
- cn = new CollisionNamespace(ns);
- coll_namespaces_[ns] = cn;
- default_collision_matrix_.addEntry(ns, false);
- }
- else
- cn = it->second;
-
- fcl::CollisionObject* g = createGeom(shape, 1.0, 0.0);
- assert(g);
-
- default_collision_matrix_.addEntry(ns, false);
-
- updateGeom(g, pose);
- cn->geoms.push_back(g);
- objects_->addObject(ns, shape, pose);
-}
-
-
-void EnvironmentModelFCL::addObject(const std::string& ns, shapes::StaticShape* shape)
-{
- std::map<std::string, CollisionNamespace*>::iterator it = coll_namespaces_.find(ns);
- CollisionNamespace* cn = NULL;
- if(it == coll_namespaces_.end())
- {
- cn = new CollisionNamespace(ns);
- coll_namespaces_[ns] = cn;
- default_collision_matrix_.addEntry(ns, false);
- }
- else
- cn = it->second;
-
- fcl::CollisionObject* g = createGeom(shape);
- assert(g);
- cn->geoms.push_back(g);
- objects_->addObject(ns, shape);
-}
-
-
-void EnvironmentModelFCL::clearObjects(void)
-{
- for(std::map<std::string, CollisionNamespace*>::iterator it = coll_namespaces_.begin(); it != coll_namespaces_.end(); ++it)
- {
- default_collision_matrix_.removeEntry(it->first);
- delete it->second;
- }
- coll_namespaces_.clear();
- objects_->clearObjects();
-}
-
-
-void EnvironmentModelFCL::clearObjects(const std::string &ns)
-{
- std::map<std::string, CollisionNamespace*>::iterator it = coll_namespaces_.find(ns);
- if(it != coll_namespaces_.end())
- {
- default_collision_matrix_.removeEntry(ns);
- delete it->second;
- coll_namespaces_.erase(ns);
- }
- objects_->clearObjects(ns);
-}
-
-
-fcl::CollisionObject* EnvironmentModelFCL::copyGeom(fcl::CollisionObject* geom) const
-{
- // TODO
- return NULL;
-}
-
-
-EnvironmentModel* EnvironmentModelFCL::clone(void) const
-{
- // TODO
- return NULL;
-}
-
-}
diff --git a/branches/point_cloud/collision_space_fcl/src/environment_objects.cpp b/branches/point_cloud/collision_space_fcl/src/environment_objects.cpp
deleted file mode 100644
index 0fa3f123b7282a580e25d675cf4fe783f97c2477..0000000000000000000000000000000000000000
--- a/branches/point_cloud/collision_space_fcl/src/environment_objects.cpp
+++ /dev/null
@@ -1,151 +0,0 @@
-/*********************************************************************
-* 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 Ioan Sucan */
-
-#include "collision_space_fcl/environment_objects.h"
-#include <geometric_shapes/shape_operations.h>
-
-std::vector<std::string> collision_space_fcl::EnvironmentObjects::getNamespaces(void) const
-{
- std::vector<std::string> ns;
- for (std::map<std::string, NamespaceObjects>::const_iterator it = objects_.begin() ; it != objects_.end() ; ++it)
- ns.push_back(it->first);
- return ns;
-}
-
-const collision_space_fcl::EnvironmentObjects::NamespaceObjects& collision_space_fcl::EnvironmentObjects::getObjects(const std::string &ns) const
-{
- std::map<std::string, NamespaceObjects>::const_iterator it = objects_.find(ns);
- if (it == objects_.end())
- return empty_;
- else
- return it->second;
-}
-
-collision_space_fcl::EnvironmentObjects::NamespaceObjects& collision_space_fcl::EnvironmentObjects::getObjects(const std::string &ns)
-{
- return objects_[ns];
-}
-
-void collision_space_fcl::EnvironmentObjects::addObject(const std::string &ns, shapes::StaticShape *shape)
-{
- objects_[ns].static_shape.push_back(shape);
-}
-
-void collision_space_fcl::EnvironmentObjects::addObject(const std::string &ns, shapes::Shape *shape, const btTransform &pose)
-{
- objects_[ns].shape.push_back(shape);
- objects_[ns].shape_pose.push_back(pose);
-}
-
-bool collision_space_fcl::EnvironmentObjects::removeObject(const std::string &ns, const shapes::Shape *shape)
-{
- std::map<std::string, NamespaceObjects>::iterator it = objects_.find(ns);
- if (it != objects_.end())
- {
- unsigned int n = it->second.shape.size();
- for (unsigned int i = 0 ; i < n ; ++i)
- if (it->second.shape[i] == shape)
- {
- it->second.shape.erase(it->second.shape.begin() + i);
- it->second.shape_pose.erase(it->second.shape_pose.begin() + i);
- return true;
- }
- }
- return false;
-}
-
-bool collision_space_fcl::EnvironmentObjects::removeObject(const std::string &ns, const shapes::StaticShape *shape)
-{
- std::map<std::string, NamespaceObjects>::iterator it = objects_.find(ns);
- if (it != objects_.end())
- {
- unsigned int n = it->second.static_shape.size();
- for (unsigned int i = 0 ; i < n ; ++i)
- if (it->second.static_shape[i] == shape)
- {
- it->second.static_shape.erase(it->second.static_shape.begin() + i);
- return true;
- }
- }
- return false;
-}
-
-void collision_space_fcl::EnvironmentObjects::clearObjects(const std::string &ns)
-{
- std::map<std::string, NamespaceObjects>::iterator it = objects_.find(ns);
- if (it != objects_.end())
- {
- unsigned int n = it->second.static_shape.size();
- for (unsigned int i = 0 ; i < n ; ++i)
- delete it->second.static_shape[i];
- n = it->second.shape.size();
- for (unsigned int i = 0 ; i < n ; ++i)
- delete it->second.shape[i];
- objects_.erase(it);
- }
-}
-
-void collision_space_fcl::EnvironmentObjects::clearObjects(void)
-{
- std::vector<std::string> ns = getNamespaces();
- for (unsigned int i = 0 ; i < ns.size() ; ++i)
- clearObjects(ns[i]);
-}
-
-void collision_space_fcl::EnvironmentObjects::addObjectNamespace(const std::string ns)
-{
- if(objects_.find(ns) == objects_.end()) {
- objects_[ns] = NamespaceObjects();
- }
- //doesn't do anything if the object is already in objects_
-}
-
-collision_space_fcl::EnvironmentObjects* collision_space_fcl::EnvironmentObjects::clone(void) const
-{
- EnvironmentObjects *c = new EnvironmentObjects();
- for (std::map<std::string, NamespaceObjects>::const_iterator it = objects_.begin() ; it != objects_.end() ; ++it)
- {
- NamespaceObjects &ns = c->objects_[it->first];
- unsigned int n = it->second.static_shape.size();
- for (unsigned int i = 0 ; i < n ; ++i)
- ns.static_shape.push_back(shapes::cloneShape(it->second.static_shape[i]));
- n = it->second.shape.size();
- for (unsigned int i = 0 ; i < n ; ++i)
- ns.shape.push_back(shapes::cloneShape(it->second.shape[i]));
- ns.shape_pose = it->second.shape_pose;
- }
- return c;
-}
diff --git a/branches/point_cloud/collision_space_fcl/test/test_collision_space_fcl.cpp b/branches/point_cloud/collision_space_fcl/test/test_collision_space_fcl.cpp
deleted file mode 100644
index da318e304f35d396e3dba061175a14d062077746..0000000000000000000000000000000000000000
--- a/branches/point_cloud/collision_space_fcl/test/test_collision_space_fcl.cpp
+++ /dev/null
@@ -1,456 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-#include <planning_models/kinematic_model.h>
-#include <planning_models/kinematic_state.h>
-#include <gtest/gtest.h>
-#include <sstream>
-#include <ctype.h>
-#include <ros/package.h>
-#include <collision_space_fcl/environmentFCL.h>
-
-//urdf location relative to the planning_models path
-static const std::string rel_path = "/test_urdf/robot.xml";
-
-class TestCollisionSpaceFCL : public testing::Test {
-protected:
-
- virtual void SetUp() {
-
- full_path_ = ros::package::getPath("planning_models")+rel_path;
-
- urdf_ok_ = urdf_model_.initFile(full_path_);
-
- std::vector<planning_models::KinematicModel::MultiDofConfig> multi_dof_configs;
- //now this should work with an identity transform
- planning_models::KinematicModel::MultiDofConfig config("base_joint");
- config.type = "Planar";
- config.parent_frame_id = "base_footprint";
- config.child_frame_id = "base_footprint";
- multi_dof_configs.push_back(config);
-
- std::vector<planning_models::KinematicModel::GroupConfig> gcs;
- planning_models::KinematicModel::GroupConfig left_arm("left_arm",
- "torso_lift_link",
- "l_wrist_roll_link");
-
- planning_models::KinematicModel::GroupConfig right_arm("right_arm",
- "torso_lift_link",
- "r_wrist_roll_link");
-
- kinematic_model_ = new planning_models::KinematicModel(urdf_model_,
- gcs,
- multi_dof_configs);
-
- coll_space_ = new collision_space_fcl::EnvironmentModelFCL();
- };
-
- virtual void TearDown() {
- delete kinematic_model_;
- delete coll_space_;
- }
-
-protected:
-
- urdf::Model urdf_model_;
- bool urdf_ok_;
- std::string full_path_;
- collision_space_fcl::EnvironmentModelFCL* coll_space_;
- planning_models::KinematicModel* kinematic_model_;
-};
-
-
-TEST_F(TestCollisionSpaceFCL, TestInit) {
- std::vector<std::string> links;
- kinematic_model_->getLinkModelNames(links);
- std::map<std::string, double> link_padding_map;
-
- {
- collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix acm(links,true);
- coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
- //all AllowedCollisions set to true, so no collision
- ASSERT_FALSE(coll_space_->isCollision());
- }
-
- {
- collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix acm(links,false);
-
- coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
-
- //now we are in collision with nothing disabled
- ASSERT_TRUE(coll_space_->isCollision());
- }
-
- //one more time for good measure
- {
- collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix acm(links,false);
-
- coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
-
- //now we are in collision with nothing disabled
- ASSERT_TRUE(coll_space_->isCollision());
- }
-}
-
-
-TEST_F(TestCollisionSpaceFCL, TestACM) {
- std::vector<std::string> links;
- kinematic_model_->getLinkModelNames(links);
- std::map<std::string, double> link_padding_map;
-
- //first we get
- {
- collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix acm(links, false);
- coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
-
- planning_models::KinematicState state(kinematic_model_);
- state.setKinematicStateToDefault();
-
- coll_space_->updateRobotModel(&state);
-
- //at default state in collision
- ASSERT_TRUE(coll_space_->isCollision());
-
- //now we get the full set of collisions in the default state
- std::vector<collision_space_fcl::EnvironmentModel::AllowedContact> ac;
- std::vector<collision_space_fcl::EnvironmentModel::Contact> contacts;
-
- coll_space_->getAllCollisionContacts(ac, contacts, 1);
-
- ASSERT_TRUE(contacts.size() > 1);
- //now we change all these pairwise to true
- for(unsigned int i = 0; i < contacts.size(); i++) {
- ASSERT_TRUE(contacts[i].body_type_1 == collision_space_fcl::EnvironmentModel::LINK);
- ASSERT_TRUE(contacts[i].body_type_2 == collision_space_fcl::EnvironmentModel::LINK);
- ASSERT_TRUE(acm.changeEntry(contacts[i].body_name_1,contacts[i].body_name_2, true));
- }
-
- coll_space_->setAlteredCollisionMatrix(acm);
-
- //with all of these disabled, no more collisions
- ASSERT_FALSE(coll_space_->isCollision());
-
- coll_space_->revertAlteredCollisionMatrix();
- ASSERT_TRUE(coll_space_->isCollision());
- }
-}
-
-TEST_F(TestCollisionSpaceFCL, TestAttachedObjects)
-{
- std::vector<std::string> links;
- kinematic_model_->getLinkModelNames(links);
- std::map<std::string, double> link_padding_map;
-
- collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix acm(links, false);
- coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
-
- {
- //indented cause the state needs to cease to exist before we add the attached body
- planning_models::KinematicState state(kinematic_model_);
- state.setKinematicStateToDefault();
-
- coll_space_->updateRobotModel(&state);
-
- //now we get the full set of collisions in the default state
- std::vector<collision_space_fcl::EnvironmentModel::AllowedContact> ac;
- std::vector<collision_space_fcl::EnvironmentModel::Contact> contacts;
-
- coll_space_->getAllCollisionContacts(ac, contacts, 1);
-
- //now we change all these pairwise to true
- for(unsigned int i = 0; i < contacts.size(); i++) {
- ASSERT_TRUE(contacts[i].body_type_1 == collision_space_fcl::EnvironmentModel::LINK);
- ASSERT_TRUE(contacts[i].body_type_2 == collision_space_fcl::EnvironmentModel::LINK);
- ASSERT_TRUE(acm.changeEntry(contacts[i].body_name_1,contacts[i].body_name_2, true));
- }
-
- coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
- coll_space_->updateRobotModel(&state);
- }
-
- //now we shouldn't be in collision
- ASSERT_FALSE(coll_space_->isCollision());
-
- const planning_models::KinematicModel::LinkModel *link = kinematic_model_->getLinkModel("base_link");
-
- //first a single box
- shapes::Sphere* sphere1 = new shapes::Sphere();
- sphere1->radius = .1;
-
- shapes::Box* box2 = new shapes::Box();
- box2->size[0] = .05;
- box2->size[1] = .05;
- box2->size[2] = .05;
-
- std::vector<shapes::Shape*> shape_vector;
- shape_vector.push_back(sphere1);
-
- btTransform pose;
- pose.setIdentity();
-
- std::vector<btTransform> poses;
- poses.push_back(pose);
-
- std::vector<std::string> touch_links;
-
- planning_models::KinematicModel::AttachedBodyModel* ab1 =
- new planning_models::KinematicModel::AttachedBodyModel(link, "box_1",
- poses,
- touch_links,
- shape_vector);
-
- kinematic_model_->addAttachedBodyModel(link->getName(), ab1);
- coll_space_->updateAttachedBodies();
-
- const collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix& aft_attached
- = coll_space_->getDefaultAllowedCollisionMatrix();
-
- ASSERT_TRUE(aft_attached.hasEntry("box_1"));
- bool allowed;
- EXPECT_TRUE(aft_attached.getAllowedCollision("box_1", link->getName(), allowed));
- EXPECT_FALSE(allowed);
-
- {
- //indented cause the state needs to cease to exist before we add the attached body
- planning_models::KinematicState state(kinematic_model_);
- state.setKinematicStateToDefault();
-
- coll_space_->updateRobotModel(&state);
- }
-
- //now it collides
- ASSERT_TRUE(coll_space_->isCollision());
-
- kinematic_model_->clearLinkAttachedBodyModel(link->getName(), "box_1");
- coll_space_->updateAttachedBodies();
-
- ASSERT_FALSE(aft_attached.hasEntry("box_1"));
-
- //now adding an attached object with two boxes, this time with two objects
- shape_vector.clear();
- shape_vector.push_back(box2);
- pose.getOrigin().setX(.1);
- poses.clear();
- poses.push_back(pose);
- touch_links.push_back("r_gripper_palm_link");
- touch_links.push_back("r_gripper_r_finger_link");
- touch_links.push_back("r_gripper_l_finger_link");
- touch_links.push_back("r_gripper_r_finger_tip_link");
- touch_links.push_back("r_gripper_l_finger_tip_link");
- touch_links.push_back("base_link");
-
- planning_models::KinematicModel::AttachedBodyModel* ab2 =
- new planning_models::KinematicModel::AttachedBodyModel(link, "box_2",
- poses,
- touch_links,
- shape_vector);
- kinematic_model_->addAttachedBodyModel(link->getName(), ab2);
- coll_space_->updateAttachedBodies();
-
- ASSERT_TRUE(aft_attached.hasEntry("box_2"));
- EXPECT_TRUE(aft_attached.getAllowedCollision("box_2", link->getName(), allowed));
- EXPECT_TRUE(allowed);
-
- {
- //indented cause the state needs to cease to exist before we add the attached body
- planning_models::KinematicState state(kinematic_model_);
- state.setKinematicStateToDefault();
- coll_space_->updateRobotModel(&state);
- }
-
- //now it doesn't collide
- ASSERT_FALSE(coll_space_->isCollision());
-}
-
-TEST_F(TestCollisionSpaceFCL, TestStaticObjects)
-{
- std::vector<std::string> links;
- kinematic_model_->getLinkModelNames(links);
- std::map<std::string, double> link_padding_map;
-
- collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix acm(links, false);
- coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
-
- {
- //indented cause the state needs to cease to exist before we add the attached body
- planning_models::KinematicState state(kinematic_model_);
- state.setKinematicStateToDefault();
-
- coll_space_->updateRobotModel(&state);
- }
-
- ASSERT_FALSE(coll_space_->isEnvironmentCollision());
-
- shapes::Sphere* sphere1 = new shapes::Sphere();
- sphere1->radius = .2;
-
- btTransform pose;
- pose.setIdentity();
-
- std::vector<btTransform> poses;
- poses.push_back(pose);
-
- std::vector<shapes::Shape*> shape_vector;
- shape_vector.push_back(sphere1);
-
- coll_space_->addObjects("obj1", shape_vector, poses);
-
-
- ASSERT_TRUE(coll_space_->isEnvironmentCollision());
-
-
-
- //Now test interactions between static and attached objects
-
- const planning_models::KinematicModel::LinkModel *link = kinematic_model_->getLinkModel("base_link");
-
- shapes::Box* att_box = new shapes::Box();
- att_box->size[0] = .05;
- att_box->size[1] = .05;
- att_box->size[2] = .05;
-
- std::vector<shapes::Shape*> att_shapes;
- att_shapes.push_back(att_box);
-
- btTransform att_pose;
- att_pose.setIdentity();
-
- std::vector<btTransform> att_poses;
- att_poses.push_back(att_pose);
-
- std::vector<std::string> touch_links;
- touch_links.push_back("base_link");
- touch_links.push_back("base_footprint");
-
- planning_models::KinematicModel::AttachedBodyModel* ab1 =
- new planning_models::KinematicModel::AttachedBodyModel(link, "att1",
- att_poses,
- touch_links,
- att_shapes);
-
- kinematic_model_->addAttachedBodyModel(link->getName(), ab1);
- coll_space_->updateAttachedBodies();
-
- {
- //indented cause the state needs to cease to exist before we add the attached body
- planning_models::KinematicState state(kinematic_model_);
- state.setKinematicStateToDefault();
-
- coll_space_->updateRobotModel(&state);
- }
-
- ASSERT_TRUE(coll_space_->isEnvironmentCollision());
-
- //now we get the full set of collisions in the default state
- std::vector<collision_space_fcl::EnvironmentModel::AllowedContact> ac;
- std::vector<collision_space_fcl::EnvironmentModel::Contact> contacts;
-
- coll_space_->getAllCollisionContacts(ac, contacts, 1);
-
- //now we change all these pairwise to true
- for(unsigned int i = 0; i < contacts.size(); i++) {
- if(contacts[i].body_type_1 == collision_space_fcl::EnvironmentModel::OBJECT) {
- ASSERT_TRUE(contacts[i].body_name_1 == "obj1");
- }
- if(contacts[i].body_type_2 == collision_space_fcl::EnvironmentModel::OBJECT) {
- ASSERT_TRUE(contacts[i].body_name_2 == "obj1");
- }
- }
-
- acm = coll_space_->getDefaultAllowedCollisionMatrix();
- bool allowed;
- ASSERT_TRUE(acm.getAllowedCollision("obj1","att1",allowed));
- EXPECT_FALSE(allowed);
-
- ASSERT_TRUE(acm.changeEntry(link->getName(), "obj1", true));
- ASSERT_TRUE(acm.changeEntry("base_footprint", "obj1", true));
- coll_space_->setAlteredCollisionMatrix(acm);
-
- {
- //indented cause the state needs to cease to exist before we add the attached body
- planning_models::KinematicState state(kinematic_model_);
- state.setKinematicStateToDefault();
-
- coll_space_->updateRobotModel(&state);
- }
-
- /* Remove because sphere-box collision does not work for mesh */
- // EXPECT_TRUE(coll_space_->isEnvironmentCollision());
-
- ASSERT_TRUE(acm.changeEntry("att1", "obj1", true));
- coll_space_->setAlteredCollisionMatrix(acm);
-
- allowed = false;
- ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("att1","obj1", allowed));
- EXPECT_TRUE(allowed);
-
- ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("obj1","att1", allowed));
- EXPECT_TRUE(allowed);
-
- ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("base_link","obj1", allowed));
- EXPECT_TRUE(allowed);
-
- ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("obj1","base_link", allowed));
- EXPECT_TRUE(allowed);
-
- ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("obj1","base_footprint", allowed));
- EXPECT_TRUE(allowed);
-
- EXPECT_FALSE(coll_space_->isEnvironmentCollision());
- contacts.clear();
-
- coll_space_->getAllCollisionContacts(ac, contacts, 1);
-
- //now we change all these pairwise to true
- for(unsigned int i = 0; i < contacts.size(); i++) {
- if(contacts[i].body_type_1 == collision_space_fcl::EnvironmentModel::OBJECT) {
- ASSERT_TRUE(contacts[i].body_name_1 == "obj1");
- ROS_INFO_STREAM(contacts[i].body_name_2);
- }
- if(contacts[i].body_type_2 == collision_space_fcl::EnvironmentModel::OBJECT) {
- ASSERT_TRUE(contacts[i].body_name_2 == "obj1");
- ROS_INFO_STREAM(contacts[i].body_name_1);
- }
- }
-
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/branches/point_cloud/collision_space_fcl_test/CMakeLists.txt b/branches/point_cloud/collision_space_fcl_test/CMakeLists.txt
deleted file mode 100644
index a661bbcc1996c71f42871b665bbe3724f6207f51..0000000000000000000000000000000000000000
--- a/branches/point_cloud/collision_space_fcl_test/CMakeLists.txt
+++ /dev/null
@@ -1,33 +0,0 @@
-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()
-
-#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()
-
-#common commands for building c++ executables and libraries
-#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
-#target_link_libraries(${PROJECT_NAME} another_library)
-#rosbuild_add_boost_directories()
-#rosbuild_link_boost(${PROJECT_NAME} thread)
-#rosbuild_add_executable(example examples/example.cpp)
-#target_link_libraries(example ${PROJECT_NAME})
-
-rosbuild_add_gtest(test_collision_space_fcl test/test_collision_space_fcl.cpp)
-target_link_libraries(test_collision_space_fcl assimp)
diff --git a/branches/point_cloud/collision_space_fcl_test/Makefile b/branches/point_cloud/collision_space_fcl_test/Makefile
deleted file mode 100644
index b75b928f20ef9ea4f509a17db62e4e31b422c27f..0000000000000000000000000000000000000000
--- a/branches/point_cloud/collision_space_fcl_test/Makefile
+++ /dev/null
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
diff --git a/branches/point_cloud/collision_space_fcl_test/mainpage.dox b/branches/point_cloud/collision_space_fcl_test/mainpage.dox
deleted file mode 100644
index b21790339305c7794f39d7cc90a5cb3964bbf2d6..0000000000000000000000000000000000000000
--- a/branches/point_cloud/collision_space_fcl_test/mainpage.dox
+++ /dev/null
@@ -1,26 +0,0 @@
-/**
-\mainpage
-\htmlinclude manifest.html
-
-\b collision_space_ccd_test 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.
--->
-
-
-*/
diff --git a/branches/point_cloud/collision_space_fcl_test/manifest.xml b/branches/point_cloud/collision_space_fcl_test/manifest.xml
deleted file mode 100644
index 46a558142eb5cf920a07abd318b877203818c0ec..0000000000000000000000000000000000000000
--- a/branches/point_cloud/collision_space_fcl_test/manifest.xml
+++ /dev/null
@@ -1,24 +0,0 @@
-<package>
- <description brief="collision_space_ccd_test">
-
- collision_space_ccd_test
-
- </description>
- <author>Sachin Chitta</author>
- <license>BSD</license>
- <review status="unreviewed" notes=""/>
- <url>http://ros.org/wiki/collision_space_ccd_test</url>
-
- <depend package="planning_models"/>
- <depend package="collision_space_fcl"/>
- <depend package="collision_space"/>
- <depend package="tf"/>
- <depend package="arm_navigation_msgs"/>
-
- <platform os="ubuntu" version="9.04"/>
- <platform os="ubuntu" version="9.10"/>
- <platform os="ubuntu" version="10.04"/>
-
-</package>
-
-
diff --git a/branches/point_cloud/collision_space_fcl_test/objects/meshes/9300.stl b/branches/point_cloud/collision_space_fcl_test/objects/meshes/9300.stl
deleted file mode 100644
index 7159c022ac7d8b620ffddf91aee16a8cc9f19b8e..0000000000000000000000000000000000000000
Binary files a/branches/point_cloud/collision_space_fcl_test/objects/meshes/9300.stl and /dev/null differ
diff --git a/branches/point_cloud/collision_space_fcl_test/test/test_collision_space_fcl.cpp b/branches/point_cloud/collision_space_fcl_test/test/test_collision_space_fcl.cpp
deleted file mode 100644
index 83ce49d24724745177b86766bd7b2eeb577d93c0..0000000000000000000000000000000000000000
--- a/branches/point_cloud/collision_space_fcl_test/test/test_collision_space_fcl.cpp
+++ /dev/null
@@ -1,755 +0,0 @@
-/*********************************************************************
-* 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 E. Gil Jones */
-
-#include <planning_models/kinematic_model.h>
-#include <planning_models/kinematic_state.h>
-#include <gtest/gtest.h>
-#include <sstream>
-#include <ctype.h>
-#include <ros/package.h>
-
-#include <collision_space/environmentODE.h>
-#include <collision_space_fcl/environmentFCL.h>
-
-#include <tf/transform_datatypes.h>
-#include <arm_navigation_msgs/RobotState.h>
-
-#include <geometric_shapes/shape_operations.h>
-
-#define NUM_MESH_OBJECTS 10
-#define NUM_RANDOM_OBJECTS 100
-#define NUM_TEST_CONFIGURATIONS 1000
-
-//urdf location relative to the planning_models path
-static const std::string rel_path = "/test_urdf/robot.xml";
-
-inline double gen_rand(double min, double max)
-{
- int rand_num = rand()%100+1;
- double result = min + (double)((max-min)*rand_num)/101.0;
- return result;
-}
-
-inline geometry_msgs::Quaternion generateRandomUnitQuaternion() {
-
- geometry_msgs::Quaternion quat;
- quat.x = gen_rand(-1.0, 1.0);
- quat.w = gen_rand(-1.0, 1.0);
- quat.z = gen_rand(-1.0, 1.0);
- quat.w = gen_rand(-1.0, 1.0);
-
- double mag = sqrt(pow(quat.x, 2.0)+pow(quat.y, 2.0)+pow(quat.z, 2.0)+pow(quat.w, 2.0));
-
- quat.x /= mag;
- quat.y /= mag;
- quat.z /= mag;
- quat.w /= mag;
-
- return quat;
-}
-
-inline std::string getNumberedString(const std::string &object, unsigned int num)
-{
- std::stringstream my_string;
- my_string << object << num;
- return my_string.str();
-}
-
-class TestCollisionSpaceFCL : public testing::Test {
-public:
-
- void spinThread() {
- lock_.lock();
- coll_space_->isCollision();
- lock_.unlock();
- }
-
-protected:
-
- virtual void SetUp() {
-
- full_path_ = ros::package::getPath("planning_models")+rel_path;
-
- urdf_ok_ = urdf_model_.initFile(full_path_);
-
- std::vector<planning_models::KinematicModel::MultiDofConfig> multi_dof_configs;
- //now this should work with an identity transform
- planning_models::KinematicModel::MultiDofConfig config("base_joint");
- config.type = "Planar";
- config.parent_frame_id = "base_footprint";
- config.child_frame_id = "base_footprint";
- multi_dof_configs.push_back(config);
-
- std::vector<planning_models::KinematicModel::GroupConfig> gcs;
- planning_models::KinematicModel::GroupConfig left_arm("left_arm",
- "torso_lift_link",
- "l_wrist_roll_link");
-
- planning_models::KinematicModel::GroupConfig right_arm("right_arm",
- "torso_lift_link",
- "r_wrist_roll_link");
- gcs.push_back(left_arm);
- gcs.push_back(right_arm);
- kinematic_model_ = new planning_models::KinematicModel(urdf_model_,
- gcs,
- multi_dof_configs);
- coll_space_ode_ = new collision_space::EnvironmentModelODE();
- coll_space_ = new collision_space_fcl::EnvironmentModelFCL();
-
- workspace_x_extents_ = 1.0;
- workspace_y_extents_ = 1.0;
- workspace_z_extents_ = 1.0;
- ros::Time::init();
- };
-
- virtual void TearDown() {
- delete kinematic_model_;
- // delete coll_space_;
- // delete coll_space_ode_;
- }
-
- void getJointBoundsMap(const std::string &group, std::map<std::string, std::pair<double, double> > &joint_bounds_map)
- {
- const planning_models::KinematicModel::JointModelGroup* joint_model_group = kinematic_model_->getModelGroup(group);
- if(joint_model_group == NULL) {
- ROS_WARN_STREAM("No joint group " << group);
- return;
- }
-
- for(unsigned int i = 0; i < joint_model_group->getJointModels().size(); i++) {
- const planning_models::KinematicModel::JointModel* jm = joint_model_group->getJointModels()[i];
- std::pair<double, double> bounds;
- jm->getVariableBounds(jm->getName(), bounds);
- joint_bounds_map[jm->getName()] = bounds;
- }
- }
-
- void setupForRandomConfigurations()
- {
- srand(time(NULL));
- std::vector<std::string> links;
- kinematic_model_->getLinkModelNames(links);
- std::map<std::string, double> link_padding_map;
-
- collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix acm(links, false);
- collision_space::EnvironmentModel::AllowedCollisionMatrix acm_ode(links, false);
-
- coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
- coll_space_ode_->setRobotModel(kinematic_model_, acm_ode, link_padding_map);
-
- {
- //indented cause the state needs to cease to exist before we add the attached body
- planning_models::KinematicState state(kinematic_model_);
- state.setKinematicStateToDefault();
-
- coll_space_->updateRobotModel(&state);
- coll_space_ode_->updateRobotModel(&state);
- }
-
- ASSERT_FALSE(coll_space_->isEnvironmentCollision());
- ASSERT_FALSE(coll_space_ode_->isEnvironmentCollision());
- }
-
- void testForRandomConfigurations()
- {
- getJointBoundsMap("right_arm",joint_bounds_map_right_);
- getJointBoundsMap("left_arm",joint_bounds_map_left_);
-
- std::vector<arm_navigation_msgs::RobotState> robot_states_right;
- std::vector<arm_navigation_msgs::RobotState> robot_states_left;
- // Now generate random robot states and test them for collisions
- for(unsigned int i=0; i < NUM_TEST_CONFIGURATIONS; i++)
- {
- arm_navigation_msgs::RobotState robot_state_right = generateRandomRobotStateWithinLimits(joint_bounds_map_right_);
- arm_navigation_msgs::RobotState robot_state_left = generateRandomRobotStateWithinLimits(joint_bounds_map_left_);
- robot_states_right.push_back(robot_state_right);
- robot_states_left.push_back(robot_state_left);
- }
-
- std::vector<bool> is_collision, is_collision_ode;
- is_collision.resize(robot_states_right.size());
- is_collision_ode.resize(robot_states_right.size());
-
- {
- //indented cause the state needs to cease to exist before we add the attached body
- planning_models::KinematicState state(kinematic_model_);
- state.setKinematicStateToDefault();
- planning_models::KinematicState::JointStateGroup *right_state = state.getJointStateGroup("right_arm");
- planning_models::KinematicState::JointStateGroup *left_state = state.getJointStateGroup("left_arm");
-
- ros::Time start_time = ros::Time::now();
- for(unsigned int i=0; i < robot_states_right.size(); i++)
- {
- right_state->setKinematicState(robot_states_right[i].joint_state.position);
- left_state->setKinematicState(robot_states_left[i].joint_state.position);
- coll_space_->updateRobotModel(&state);
- is_collision[i] = coll_space_->isEnvironmentCollision();
- }
- ros::Duration duration = ros::Time::now()-start_time;
- ROS_INFO("FCL: Collision time: %f",duration.toSec());
-
- start_time = ros::Time::now();
- for(unsigned int i=0; i < robot_states_right.size(); i++)
- {
- right_state->setKinematicState(robot_states_right[i].joint_state.position);
- left_state->setKinematicState(robot_states_left[i].joint_state.position);
- coll_space_ode_->updateRobotModel(&state);
- is_collision_ode[i] = coll_space_ode_->isEnvironmentCollision();
- }
- duration = ros::Time::now()-start_time;
- ROS_INFO("ODE: Collision time: %f",duration.toSec());
-
- unsigned int counter = 0;
- for(unsigned int i=0; i < robot_states_right.size(); i++)
- if(is_collision_ode[i] != is_collision[i])
- counter++;
- ROS_DEBUG("%d mismatches between ODE and FCL result (out of %d)",(int) counter,(int) robot_states_right.size());
-
- ASSERT_TRUE(counter == 0);
- }
- }
-
- arm_navigation_msgs::RobotState generateRandomRobotStateWithinLimits(std::map<std::string, std::pair<double, double> > &joint_bounds_map)
- {
- arm_navigation_msgs::RobotState rs;
- rs.joint_state.header.frame_id = "torso_lift_link";
- rs.joint_state.header.stamp = ros::Time::now();
-
- for(std::map<std::string,std::pair<double, double> >::iterator it = joint_bounds_map.begin();
- it != joint_bounds_map.end();
- it++) {
- rs.joint_state.name.push_back(it->first);
- rs.joint_state.position.push_back(gen_rand(it->second.first, it->second.second));
- }
- return rs;
- }
-
- geometry_msgs::Pose generateRandomPoseInWorkspace() {
-
- geometry_msgs::Pose rp;
- rp.position.x = gen_rand(-workspace_x_extents_, workspace_x_extents_);
- rp.position.y = gen_rand(-workspace_y_extents_, workspace_y_extents_);
- rp.position.z = gen_rand(-workspace_z_extents_, workspace_z_extents_);
-
- rp.orientation = generateRandomUnitQuaternion();
-
- return rp;
-
- }
-
-
-protected:
-
- boost::mutex lock_;
- urdf::Model urdf_model_;
- bool urdf_ok_;
- std::string full_path_;
- collision_space::EnvironmentModelODE* coll_space_ode_;
- collision_space_fcl::EnvironmentModelFCL* coll_space_;
- planning_models::KinematicModel* kinematic_model_;
- std::map<std::string, std::pair<double, double> > joint_bounds_map_right_, joint_bounds_map_left_;
- double workspace_x_extents_, workspace_y_extents_, workspace_z_extents_;
-};
-
-
-
-TEST_F(TestCollisionSpaceFCL, TestInit) {
- std::vector<std::string> links;
- kinematic_model_->getLinkModelNames(links);
- std::map<std::string, double> link_padding_map;
-
- {
- collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix acm(links,true);
- coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
- //all AllowedCollisions set to true, so no collision
- ASSERT_FALSE(coll_space_->isCollision());
- }
-
- {
- collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix acm(links,false);
-
- coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
-
- //now we are in collision with nothing disabled
- ASSERT_TRUE(coll_space_->isCollision());
- }
-
- //one more time for good measure
- {
- collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix acm(links,false);
-
- coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
-
- //now we are in collision with nothing disabled
- ASSERT_TRUE(coll_space_->isCollision());
- }
-}
-
-
-TEST_F(TestCollisionSpaceFCL, TestACM) {
- std::vector<std::string> links;
- kinematic_model_->getLinkModelNames(links);
- std::map<std::string, double> link_padding_map;
-
- //first we get
- {
- collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix acm(links, false);
- coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
-
- planning_models::KinematicState state(kinematic_model_);
- state.setKinematicStateToDefault();
- coll_space_->updateRobotModel(&state);
-
- //at default state in collision
- ASSERT_TRUE(coll_space_->isCollision());
-
- //now we get the full set of collisions in the default state
- std::vector<collision_space_fcl::EnvironmentModel::Contact> contacts;
-
- coll_space_->getAllCollisionContacts(contacts, 1);
-
- ASSERT_TRUE(contacts.size() > 1);
- //now we change all these pairwise to true
- for(unsigned int i = 0; i < contacts.size(); i++) {
- ASSERT_TRUE(contacts[i].body_type_1 == collision_space_fcl::EnvironmentModel::LINK);
- ASSERT_TRUE(contacts[i].body_type_2 == collision_space_fcl::EnvironmentModel::LINK);
- ASSERT_TRUE(acm.changeEntry(contacts[i].body_name_1,contacts[i].body_name_2, true));
- }
-
- coll_space_->setAlteredCollisionMatrix(acm);
-
- //with all of these disabled, no more collisions
- ASSERT_FALSE(coll_space_->isCollision());
-
- coll_space_->revertAlteredCollisionMatrix();
- ASSERT_TRUE(coll_space_->isCollision());
- }
-}
-
-TEST_F(TestCollisionSpaceFCL, TestAttachedObjects)
-{
- std::vector<std::string> links;
- kinematic_model_->getLinkModelNames(links);
- std::map<std::string, double> link_padding_map;
-
- collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix acm(links, false);
- coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
-
- {
- //indented cause the state needs to cease to exist before we add the attached body
- planning_models::KinematicState state(kinematic_model_);
- state.setKinematicStateToDefault();
-
- coll_space_->updateRobotModel(&state);
-
- //now we get the full set of collisions in the default state
- std::vector<collision_space_fcl::EnvironmentModel::Contact> contacts;
-
- coll_space_->getAllCollisionContacts(contacts, 1);
-
- //now we change all these pairwise to true
- for(unsigned int i = 0; i < contacts.size(); i++) {
- ASSERT_TRUE(contacts[i].body_type_1 == collision_space_fcl::EnvironmentModel::LINK);
- ASSERT_TRUE(contacts[i].body_type_2 == collision_space_fcl::EnvironmentModel::LINK);
- ASSERT_TRUE(acm.changeEntry(contacts[i].body_name_1,contacts[i].body_name_2, true));
- }
-
- coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
- coll_space_->updateRobotModel(&state);
- }
-
- //now we shouldn't be in collision
- ASSERT_FALSE(coll_space_->isCollision());
-
- const planning_models::KinematicModel::LinkModel *link = kinematic_model_->getLinkModel("base_link");
-
- //first a single box
- shapes::Sphere* sphere1 = new shapes::Sphere();
- sphere1->radius = .1;
-
- shapes::Box* box2 = new shapes::Box();
- box2->size[0] = .05;
- box2->size[1] = .05;
- box2->size[2] = .05;
-
- std::vector<shapes::Shape*> shape_vector;
- shape_vector.push_back(sphere1);
-
- btTransform pose;
- pose.setIdentity();
-
- std::vector<btTransform> poses;
- poses.push_back(pose);
-
- std::vector<std::string> touch_links;
-
- planning_models::KinematicModel::AttachedBodyModel* ab1 =
- new planning_models::KinematicModel::AttachedBodyModel(link, "box_1",
- poses,
- touch_links,
- shape_vector);
-
- kinematic_model_->addAttachedBodyModel(link->getName(), ab1);
- coll_space_->updateAttachedBodies();
-
- const collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix& aft_attached
- = coll_space_->getDefaultAllowedCollisionMatrix();
-
- ASSERT_TRUE(aft_attached.hasEntry("box_1"));
- bool allowed;
- EXPECT_TRUE(aft_attached.getAllowedCollision("box_1", link->getName(), allowed));
- EXPECT_FALSE(allowed);
-
- {
- //indented cause the state needs to cease to exist before we add the attached body
- planning_models::KinematicState state(kinematic_model_);
- state.setKinematicStateToDefault();
-
- coll_space_->updateRobotModel(&state);
- }
-
- //now it collides
- ASSERT_TRUE(coll_space_->isCollision());
-
- kinematic_model_->clearLinkAttachedBodyModel(link->getName(), "box_1");
- coll_space_->updateAttachedBodies();
-
- ASSERT_FALSE(aft_attached.hasEntry("box_1"));
-
- //now adding an attached object with two boxes, this time with two objects
- shape_vector.clear();
- shape_vector.push_back(box2);
- pose.getOrigin().setX(.1);
- poses.clear();
- poses.push_back(pose);
- touch_links.push_back("r_gripper_palm_link");
- touch_links.push_back("r_gripper_r_finger_link");
- touch_links.push_back("r_gripper_l_finger_link");
- touch_links.push_back("r_gripper_r_finger_tip_link");
- touch_links.push_back("r_gripper_l_finger_tip_link");
- touch_links.push_back("base_link");
-
- planning_models::KinematicModel::AttachedBodyModel* ab2 =
- new planning_models::KinematicModel::AttachedBodyModel(link, "box_2",
- poses,
- touch_links,
- shape_vector);
- kinematic_model_->addAttachedBodyModel(link->getName(), ab2);
- coll_space_->updateAttachedBodies();
-
- ASSERT_TRUE(aft_attached.hasEntry("box_2"));
- EXPECT_TRUE(aft_attached.getAllowedCollision("box_2", link->getName(), allowed));
- EXPECT_TRUE(allowed);
-
- {
- //indented cause the state needs to cease to exist before we add the attached body
- planning_models::KinematicState state(kinematic_model_);
- state.setKinematicStateToDefault();
-
- coll_space_->updateRobotModel(&state);
- }
-
- //now it doesn't collide
- ASSERT_FALSE(coll_space_->isCollision());
-}
-
-TEST_F(TestCollisionSpaceFCL, TestStaticObjects)
-{
- std::vector<std::string> links;
- kinematic_model_->getLinkModelNames(links);
- std::map<std::string, double> link_padding_map;
-
- collision_space_fcl::EnvironmentModel::AllowedCollisionMatrix acm(links, false);
- coll_space_->setRobotModel(kinematic_model_, acm, link_padding_map);
-
- {
- //indented cause the state needs to cease to exist before we add the attached body
- planning_models::KinematicState state(kinematic_model_);
- state.setKinematicStateToDefault();
-
- coll_space_->updateRobotModel(&state);
- }
-
- ASSERT_FALSE(coll_space_->isEnvironmentCollision());
-
- shapes::Sphere* sphere1 = new shapes::Sphere();
- sphere1->radius = .2;
-
- btTransform pose;
- pose.setIdentity();
-
- std::vector<btTransform> poses;
- poses.push_back(pose);
-
- std::vector<shapes::Shape*> shape_vector;
- shape_vector.push_back(sphere1);
-
- coll_space_->addObjects("obj1", shape_vector, poses);
-
-
- ASSERT_TRUE(coll_space_->isEnvironmentCollision());
-
-
-
- //Now test interactions between static and attached objects
-
- const planning_models::KinematicModel::LinkModel *link = kinematic_model_->getLinkModel("base_link");
-
- shapes::Box* att_box = new shapes::Box();
- att_box->size[0] = .05;
- att_box->size[1] = .05;
- att_box->size[2] = .05;
-
- std::vector<shapes::Shape*> att_shapes;
- att_shapes.push_back(att_box);
-
- btTransform att_pose;
- att_pose.setIdentity();
-
- std::vector<btTransform> att_poses;
- att_poses.push_back(att_pose);
-
- std::vector<std::string> touch_links;
- touch_links.push_back("base_link");
- touch_links.push_back("base_footprint");
-
- planning_models::KinematicModel::AttachedBodyModel* ab1 =
- new planning_models::KinematicModel::AttachedBodyModel(link, "att1",
- att_poses,
- touch_links,
- att_shapes);
-
- kinematic_model_->addAttachedBodyModel(link->getName(), ab1);
- coll_space_->updateAttachedBodies();
-
- {
- //indented cause the state needs to cease to exist before we add the attached body
- planning_models::KinematicState state(kinematic_model_);
- state.setKinematicStateToDefault();
-
- coll_space_->updateRobotModel(&state);
- }
-
- ASSERT_TRUE(coll_space_->isEnvironmentCollision());
-
- //now we get the full set of collisions in the default state
- std::vector<collision_space_fcl::EnvironmentModel::Contact> contacts;
-
- coll_space_->getAllCollisionContacts(contacts, 1);
-
- //now we change all these pairwise to true
- for(unsigned int i = 0; i < contacts.size(); i++) {
- if(contacts[i].body_type_1 == collision_space_fcl::EnvironmentModel::OBJECT) {
- ASSERT_TRUE(contacts[i].body_name_1 == "obj1");
- }
- if(contacts[i].body_type_2 == collision_space_fcl::EnvironmentModel::OBJECT) {
- ASSERT_TRUE(contacts[i].body_name_2 == "obj1");
- }
- }
-
- acm = coll_space_->getDefaultAllowedCollisionMatrix();
- bool allowed;
- ASSERT_TRUE(acm.getAllowedCollision("obj1","att1",allowed));
- EXPECT_FALSE(allowed);
-
- ASSERT_TRUE(acm.changeEntry(link->getName(), "obj1", true));
- ASSERT_TRUE(acm.changeEntry("base_footprint", "obj1", true));
- coll_space_->setAlteredCollisionMatrix(acm);
-
- {
- //indented cause the state needs to cease to exist before we add the attached body
- planning_models::KinematicState state(kinematic_model_);
- state.setKinematicStateToDefault();
-
- coll_space_->updateRobotModel(&state);
- }
-
- /* Remove because sphere-box collision does not work for mesh */
- // EXPECT_TRUE(coll_space_->isEnvironmentCollision());
-
- ASSERT_TRUE(acm.changeEntry("att1", "obj1", true));
- coll_space_->setAlteredCollisionMatrix(acm);
-
- allowed = false;
- ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("att1","obj1", allowed));
- EXPECT_TRUE(allowed);
-
- ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("obj1","att1", allowed));
- EXPECT_TRUE(allowed);
-
- ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("base_link","obj1", allowed));
- EXPECT_TRUE(allowed);
-
- ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("obj1","base_link", allowed));
- EXPECT_TRUE(allowed);
-
- ASSERT_TRUE(coll_space_->getCurrentAllowedCollisionMatrix().getAllowedCollision("obj1","base_footprint", allowed));
- EXPECT_TRUE(allowed);
-
- EXPECT_FALSE(coll_space_->isEnvironmentCollision());
- contacts.clear();
-
- coll_space_->getAllCollisionContacts(contacts, 1);
-
- //now we change all these pairwise to true
- for(unsigned int i = 0; i < contacts.size(); i++) {
- if(contacts[i].body_type_1 == collision_space_fcl::EnvironmentModel::OBJECT) {
- ASSERT_TRUE(contacts[i].body_name_1 == "obj1");
- ROS_INFO_STREAM(contacts[i].body_name_2);
- }
- if(contacts[i].body_type_2 == collision_space_fcl::EnvironmentModel::OBJECT) {
- ASSERT_TRUE(contacts[i].body_name_2 == "obj1");
- ROS_INFO_STREAM(contacts[i].body_name_1);
- }
- }
-}
-
-TEST_F(TestCollisionSpaceFCL, TestODEvsFCLSphere)
-{
- setupForRandomConfigurations();
- for(unsigned int i=0; i < NUM_RANDOM_OBJECTS; i++)
- {
- btTransform pose;
- shapes::Sphere* sphere1 = new shapes::Sphere();
- sphere1->radius = .1;
-
- geometry_msgs::Pose pose_msg = generateRandomPoseInWorkspace();
- tf::poseMsgToTF(pose_msg,pose);
-
- std::vector<btTransform> poses;
- poses.push_back(pose);
-
- std::vector<shapes::Shape*> shape_vector;
- shape_vector.push_back(sphere1);
-
- std::string object_id = getNumberedString("obj",i);
-
- coll_space_->addObjects(object_id, shape_vector, poses);
- coll_space_ode_->addObjects(object_id, shape_vector, poses);
- }
- testForRandomConfigurations();
-}
-
-TEST_F(TestCollisionSpaceFCL, TestODEvsFCLCylinder)
-{
- setupForRandomConfigurations();
- for(unsigned int i=0; i < NUM_RANDOM_OBJECTS; i++)
- {
- btTransform pose;
- shapes::Cylinder* c1 = new shapes::Cylinder();
- c1->radius = .1;
- c1->length = 0.3;
-
- geometry_msgs::Pose pose_msg = generateRandomPoseInWorkspace();
- tf::poseMsgToTF(pose_msg,pose);
-
- std::vector<btTransform> poses;
- poses.push_back(pose);
-
- std::vector<shapes::Shape*> shape_vector;
- shape_vector.push_back(c1);
-
- std::string object_id = getNumberedString("obj",i);
-
- coll_space_->addObjects(object_id, shape_vector, poses);
- coll_space_ode_->addObjects(object_id, shape_vector, poses);
- }
- testForRandomConfigurations();
-}
-
-TEST_F(TestCollisionSpaceFCL, TestODEvsFCLBox)
-{
- setupForRandomConfigurations();
- for(unsigned int i=0; i < NUM_RANDOM_OBJECTS; i++)
- {
- btTransform pose;
- shapes::Box* box = new shapes::Box();
- box->size[0] = 0.1;
- box->size[1] = 0.2;
- box->size[2] = 0.3;
-
- geometry_msgs::Pose pose_msg = generateRandomPoseInWorkspace();
- tf::poseMsgToTF(pose_msg,pose);
-
- std::vector<btTransform> poses;
- poses.push_back(pose);
-
- std::vector<shapes::Shape*> shape_vector;
- shape_vector.push_back(box);
-
- std::string object_id = getNumberedString("obj",i);
-
- coll_space_->addObjects(object_id, shape_vector, poses);
- coll_space_ode_->addObjects(object_id, shape_vector, poses);
- }
- testForRandomConfigurations();
-}
-
-TEST_F(TestCollisionSpaceFCL, TestODEvsFCLMesh)
-{
- setupForRandomConfigurations();
- std::string full_path = ros::package::getPath("collision_space_fcl_test")+"/objects/meshes/9300.stl";
- for(unsigned int i=0; i < NUM_MESH_OBJECTS; i++)
- {
- shapes::Mesh* mesh = shapes::createMeshFromBinaryStl(full_path.c_str());
- ASSERT_TRUE(mesh);
- btTransform pose;
- geometry_msgs::Pose pose_msg = generateRandomPoseInWorkspace();
- tf::poseMsgToTF(pose_msg,pose);
-
- std::vector<btTransform> poses;
- poses.push_back(pose);
-
- std::vector<shapes::Shape*> shape_vector;
- shape_vector.push_back(mesh);
-
- std::string object_id = getNumberedString("obj",i);
-
- coll_space_->addObjects(object_id, shape_vector, poses);
- coll_space_ode_->addObjects(object_id, shape_vector, poses);
- delete mesh;
- }
- testForRandomConfigurations();
- // coll_space_->clearObjects();
- // coll_space_ode_->clearObjects();
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/branches/point_cloud/fcl/CMakeLists.txt b/branches/point_cloud/fcl/CMakeLists.txt
deleted file mode 100644
index d30f8cfe9077c94ffad0f200acc291362d8feaf9..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/CMakeLists.txt
+++ /dev/null
@@ -1,50 +0,0 @@
-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)
-set(ROS_BUILD_TYPE Release)
-rosbuild_init()
-
-#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()
-
-#common commands for building c++ executables and libraries
-#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
-#target_link_libraries(${PROJECT_NAME} another_library)
-#rosbuild_add_boost_directories()
-#rosbuild_link_boost(${PROJECT_NAME} thread)
-#rosbuild_add_executable(example examples/example.cpp)
-#target_link_libraries(example ${PROJECT_NAME})
-
-add_definitions(-DUSE_PQP=1)
-add_definitions(-DUSE_SVMLIGHT=1)
-
-rosbuild_add_library(${PROJECT_NAME} src/AABB.cpp src/OBB.cpp src/RSS.cpp src/vec_3f.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/geometric_shapes_intersect.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp)
-
-rosbuild_add_gtest(test_core_collision test/test_core_collision.cpp)
-target_link_libraries(test_core_collision fcl)
-
-rosbuild_add_gtest(test_core_distance test/test_core_distance.cpp)
-target_link_libraries(test_core_distance fcl)
-
-rosbuild_add_executable(test_core_collision_point test/test_core_collision_point.cpp)
-target_link_libraries(test_core_collision_point fcl)
-
-rosbuild_add_gtest(test_core_geometric_shapes test/test_core_geometric_shapes.cpp)
-target_link_libraries(test_core_geometric_shapes fcl)
-
-rosbuild_add_gtest(test_core_broad_phase test/test_core_broad_phase.cpp)
-target_link_libraries(test_core_broad_phase fcl)
\ No newline at end of file
diff --git a/branches/point_cloud/fcl/Makefile b/branches/point_cloud/fcl/Makefile
deleted file mode 100644
index b75b928f20ef9ea4f509a17db62e4e31b422c27f..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/Makefile
+++ /dev/null
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
diff --git a/branches/point_cloud/fcl/include/fcl/AABB.h b/branches/point_cloud/fcl/include/fcl/AABB.h
deleted file mode 100644
index 16574e25e6d157f5a4bff18721790da4e1f37359..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/AABB.h
+++ /dev/null
@@ -1,185 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-#ifndef FCL_AABB_H
-#define FCL_AABB_H
-
-
-#include "fcl/BVH_internal.h"
-#include "fcl/vec_3f.h"
-
-/** \brief Main namespace */
-namespace fcl
-{
-
-/** \brief A class describing the AABB collision structure, which is a box in 3D space determined by two diagonal points */
-class AABB
-{
-public:
- /** \brief The min point in the AABB */
- Vec3f min_;
- /** \brief The max point in the AABB */
- Vec3f max_;
-
- /** \brief Constructor creating an AABB with infinite size */
- AABB();
-
- /** \brief Constructor creating an AABB at position v with zero size */
- AABB(const Vec3f& v) : min_(v), max_(v)
- {
- }
-
- /** \brief Constructor creating an AABB with two endpoints a and b */
- AABB(const Vec3f& a, const Vec3f&b)
- {
- min_ = min(a, b);
- max_ = max(a, b);
- }
-
- /** \brief Check whether two AABB are overlap */
- inline bool overlap(const AABB& other) const
- {
- if(min_[0] > other.max_[0]) return false;
- if(min_[1] > other.max_[1]) return false;
- if(min_[2] > other.max_[2]) return false;
-
- if(max_[0] < other.min_[0]) return false;
- if(max_[1] < other.min_[1]) return false;
- if(max_[2] < other.min_[2]) return false;
-
- return true;
- }
-
- /** \brief Check whether two AABB are overlapped along specific axis */
- inline bool axisOverlap(const AABB& other, int axis_id) const
- {
- if(min_[axis_id] > other.max_[axis_id]) return false;
-
- if(max_[axis_id] < other.min_[axis_id]) return false;
-
- return true;
- }
-
- /** \brief Check whether two AABB are overlap and return the overlap part */
- inline bool overlap(const AABB& other, AABB& overlap_part) const
- {
- if(!overlap(other))
- return false;
-
- overlap_part.min_ = max(min_, other.min_);
- overlap_part.max_ = min(max_, other.max_);
- return true;
- }
-
-
- /** \brief Check whether the AABB contains a point */
- inline bool contain(const Vec3f& p) const
- {
- if(p[0] < min_[0] || p[0] > max_[0]) return false;
- if(p[1] < min_[1] || p[1] > max_[1]) return false;
- if(p[2] < min_[2] || p[2] > max_[2]) return false;
-
- return true;
- }
-
- /** \brief Merge the AABB and a point */
- inline AABB& operator += (const Vec3f& p)
- {
- min_ = min(min_, p);
- max_ = max(max_, p);
- return *this;
- }
-
- /** \brief Merge the AABB and another AABB */
- inline AABB& operator += (const AABB& other)
- {
- min_ = min(min_, other.min_);
- max_ = max(max_, other.max_);
- return *this;
- }
-
- /** \brief Return the merged AABB of current AABB and the other one */
- inline AABB operator + (const AABB& other) const
- {
- AABB res(*this);
- return res += other;
- }
-
- /** \brief Width of the AABB */
- inline BVH_REAL width() const
- {
- return max_[0] - min_[0];
- }
-
- /** \brief Height of the AABB */
- inline BVH_REAL height() const
- {
- return max_[1] - min_[1];
- }
-
- /** \brief Depth of the AABB */
- inline BVH_REAL depth() const
- {
- return max_[2] - min_[2];
- }
-
- /** \brief Volume of the AABB */
- inline BVH_REAL volume() const
- {
- return width() * height() * depth();
- }
-
- /** \brief Size of the AABB, for split order */
- inline BVH_REAL size() const
- {
- return (max_ - min_).sqrLength();
- }
-
- /** \brief Center of the AABB */
- inline Vec3f center() const
- {
- return (min_ + max_) * 0.5;
- }
-
- /** \brief The distance between two AABB
- * Not implemented.
- */
- BVH_REAL distance(const AABB& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
-};
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/BV.h b/branches/point_cloud/fcl/include/fcl/BV.h
deleted file mode 100644
index 895c811eb8d48c52614b86b7b6f0a36ceb5f8c86..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/BV.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-#ifndef FCL_BV_H
-#define FCL_BV_H
-
-
-#include "fcl/kDOP.h"
-#include "fcl/AABB.h"
-#include "fcl/OBB.h"
-#include "fcl/RSS.h"
-
-/** \brief Main namespace */
-namespace fcl
-{
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/BVH_front.h b/branches/point_cloud/fcl/include/fcl/BVH_front.h
deleted file mode 100644
index ac22081562b1d56fb7d5d4fe88017170d28c53aa..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/BVH_front.h
+++ /dev/null
@@ -1,67 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-#ifndef FCL_BVH_FRONT_H
-#define FCL_BVH_FRONT_H
-
-
-#include <list>
-
-/** \brief Main namespace */
-namespace fcl
-{
-
-/** \brief A class describing the node for BVH front */
-struct BVHFrontNode
-{
- bool valid; // not valid when collision detected on the front node
- int left, right;
-
- BVHFrontNode(int left_, int right_)
- {
- left = left_;
- right = right_;
-
- valid = true;
- }
-};
-
-/** \brief A class describing the BVH front list */
-typedef std::list<BVHFrontNode> BVHFrontList;
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/BVH_internal.h b/branches/point_cloud/fcl/include/fcl/BVH_internal.h
deleted file mode 100644
index 3aa681947180118b817b78792e8838cc41860147..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/BVH_internal.h
+++ /dev/null
@@ -1,87 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-#ifndef FCL_BVH_INTERNAL_H
-#define FCL_BVH_INTERNAL_H
-
-namespace fcl
-{
-
-typedef double BVH_REAL;
-
-/** \brief States for BVH construction
- * empty->begun->processed ->replace_begun->processed -> ......
- * |
- * |-> update_begun -> updated -> .....
- * */
-enum BVHBuildState
-{
- BVH_BUILD_STATE_EMPTY, // empty state, immediately after constructor
- BVH_BUILD_STATE_BEGUN, // after beginModel(), state for adding geometry primitives
- BVH_BUILD_STATE_PROCESSED, // after tree has been build, ready for cd use
- BVH_BUILD_STATE_UPDATE_BEGUN, // after beginUpdateModel(), state for updating geometry primitives
- BVH_BUILD_STATE_UPDATED, // after tree has been build for updated geometry, ready for ccd use
- BVH_BUILD_STATE_REPLACE_BEGUN, // after beginReplaceModel(), state for replacing geometry primitives
-};
-
-/** \brief Error code for BVH */
-enum BVHReturnCode
-{
- BVH_OK = 0,
- BVH_ERR_MODEL_OUT_OF_MEMORY = -1,
- BVH_ERR_OUT_OF_MEMORY = -2,
- BVH_ERR_UNPROCESSED_MODEL = -3,
- BVH_ERR_BUILD_OUT_OF_SEQUENCE = -4,
- BVH_ERR_BUILD_EMPTY_MODEL = -5,
- BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME = -6,
- BVH_ERR_UNSUPPORTED_FUNCTION = -7,
- BVH_ERR_UNUPDATED_MODEL = -8,
- BVH_ERR_INCORRECT_DATA = -9,
- BVH_ERR_UNKNOWN = -10
-};
-
-/** \brief BVH model type */
-enum BVHModelType
-{
- BVH_MODEL_UNKNOWN,
- BVH_MODEL_TRIANGLES,
- BVH_MODEL_POINTCLOUD
-};
-
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/BVH_model.h b/branches/point_cloud/fcl/include/fcl/BVH_model.h
deleted file mode 100644
index 3084b1cc67da37ff09085a8f85e23e9335610ded..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/BVH_model.h
+++ /dev/null
@@ -1,282 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-#ifndef FCL_BVH_MODEL_H
-#define FCL_BVH_MODEL_H
-
-#include "fcl/collision_object.h"
-#include "fcl/BVH_internal.h"
-#include "fcl/BV.h"
-#include "fcl/BV_node.h"
-#include "fcl/primitive.h"
-#include "fcl/vec_3f.h"
-#include "fcl/BV_splitter.h"
-#include "fcl/BV_fitter.h"
-#include <vector>
-#include <boost/shared_ptr.hpp>
-
-/** \brief Main namespace */
-namespace fcl
-{
-
-/** \brief A class describing the bounding hierarchy of a mesh model */
-template<typename BV>
-class BVHModel : public CollisionObject
-{
-private:
- int num_tris_allocated;
- int num_vertices_allocated;
- int num_bvs_allocated;
- int num_vertex_updated; // for ccd vertex update
- unsigned int* primitive_indices;
-
-public:
-
- /** \brief The state of BVH building process */
- BVHBuildState build_state;
-
- /** \brief Split rule to split one BV node into two children */
- boost::shared_ptr<BVSplitterBase<BV> > bv_splitter;
-
- /** \brief Fitting rule to fit a BV node to a set of geometry primitives */
- boost::shared_ptr<BVFitterBase<BV> > bv_fitter;
-
- /** \brief Model type */
- BVHModelType getModelType() const
- {
- if(num_tris && num_vertices)
- return BVH_MODEL_TRIANGLES;
- else if(num_vertices)
- return BVH_MODEL_POINTCLOUD;
- else
- return BVH_MODEL_UNKNOWN;
- }
-
- /** \brief Constructing an empty BVH */
- BVHModel()
- {
- vertices = NULL;
- tri_indices = NULL;
- bvs = NULL;
-
- num_tris = 0;
- num_tris_allocated = 0;
- num_vertices = 0;
- num_vertices_allocated = 0;
- num_bvs = 0;
- num_bvs_allocated = 0;
-
- prev_vertices = NULL;
-
- primitive_indices = NULL;
-
- build_state = BVH_BUILD_STATE_EMPTY;
-
- bv_splitter.reset(new BVSplitter<BV>(SPLIT_METHOD_MEAN));
- bv_fitter.reset(new BVFitter<BV>());
-
- uc = NULL;
- }
-
- BVHModel(const BVHModel& other);
-
-
- ~BVHModel()
- {
- delete [] vertices;
- delete [] tri_indices;
- delete [] bvs;
-
- delete [] prev_vertices;
-
- delete [] primitive_indices;
-
- delete [] uc;
- }
-
- /** \brief We provide getBV() and getNumBVs() because BVH may be compressed (in future), so
- we must provide some flexibility here */
-
- /** \brief Get the BV of index id */
- const BVNode<BV>& getBV(int id) const
- {
- return bvs[id];
- }
-
- /** \brief Get the BV of index id */
- BVNode<BV>& getBV(int id)
- {
- return bvs[id];
- }
-
- /** \brief Get the number of BVs */
- int getNumBVs() const
- {
- return num_bvs;
- }
-
- /** \brief Get the object type: it is a BVH */
- OBJECT_TYPE getObjectType() const { return OT_BVH; }
-
- /** \brief Get the BV type */
- NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
-
- /** \brief Compute the AABB for the BVH, used for broad-phase collision */
- void computeAABB();
-
- /** \brief Geometry point data */
- Vec3f* vertices;
-
- /** \brief Geometry triangle index data, will be NULL for point clouds */
- Triangle* tri_indices;
-
- /** \brief Geometry point data in previous frame */
- Vec3f* prev_vertices;
-
- /** \brief Number of triangles */
- int num_tris;
-
- /** \brief Number of points */
- int num_vertices;
-
- Uncertainty* uc;
-
- /** \brief Begin a new BVH model */
- int beginModel(int num_tris = 0, int num_vertices = 0);
-
- /** \brief Add one point in the new BVH model */
- int addVertex(const Vec3f& p);
-
- /** \brief Add one triangle in the new BVH model */
- int addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
-
- /** \brief Add a set of triangles in the new BVH model */
- int addSubModel(const std::vector<Vec3f>& ps, const std::vector<Triangle>& ts);
-
- /** \brief Add a set of points in the new BVH model */
- int addSubModel(const std::vector<Vec3f>& ps);
-
- /** \brief End BVH model construction, will build the bounding volume hierarchy */
- int endModel();
-
-
- /** \brief Replace the geometry information of current frame (i.e. should have the same mesh topology with the previous frame) */
- int beginReplaceModel();
-
- /** \brief Replace one point in the old BVH model */
- int replaceVertex(const Vec3f& p);
-
- /** \brief Replace one triangle in the old BVH model */
- int replaceTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
-
- /** \brief Replace a set of points in the old BVH model */
- int replaceSubModel(const std::vector<Vec3f>& ps);
-
- /** \brief End BVH model replacement, will also refit or rebuild the bounding volume hierarchy */
- int endReplaceModel(bool refit = true, bool bottomup = true);
-
-
- /** \brief Replace the geometry information of current frame (i.e. should have the same mesh topology with the previous frame).
- * The current frame will be saved as the previous frame in prev_vertices.
- * */
- int beginUpdateModel();
-
- /** \brief Update one point in the old BVH model */
- int updateVertex(const Vec3f& p);
-
- /** \brief Update one triangle in the old BVH model */
- int updateTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
-
- /** \brief Update a set of points in the old BVH model */
- int updateSubModel(const std::vector<Vec3f>& ps);
-
- /** \brief End BVH model update, will also refit or rebuild the bounding volume hierarchy */
- int endUpdateModel(bool refit = true, bool bottomup = true);
-
- /** \brief Check the number of memory used */
- int memUsage(int msg) const;
-
-private:
-
- /** \brief Bounding volume hierarchy */
- BVNode<BV>* bvs;
-
- /** \brief Number of BV nodes in bounding volume hierarchy */
- int num_bvs;
-
- /** \brief Build the bounding volume hierarchy */
- int buildTree();
-
- /** \brief Refit the bounding volume hierarchy */
- int refitTree(bool bottomup);
-
- /** \brief Refit the bounding volume hierarchy in a top-down way (slow but more compact)*/
- int refitTree_topdown();
-
- /** \brief Refit the bounding volume hierarchy in a bottom-up way (fast but less compact) */
- int refitTree_bottomup();
-
- /** \brief Recursive kernel for hierarchy construction */
- int recursiveBuildTree(int bv_id, int first_primitive, int num_primitives);
-
- /** \brief Recursive kernel for bottomup refitting */
- int recursiveRefitTree_bottomup(int bv_id);
-
-};
-
-
-/** Specialization of getNodeType() for BVHModel with different BV types */
-template<>
-NODE_TYPE BVHModel<AABB>::getNodeType() const;
-
-template<>
-NODE_TYPE BVHModel<OBB>::getNodeType() const;
-
-template<>
-NODE_TYPE BVHModel<RSS>::getNodeType() const;
-
-template<>
-NODE_TYPE BVHModel<KDOP<16> >::getNodeType() const;
-
-template<>
-NODE_TYPE BVHModel<KDOP<18> >::getNodeType() const;
-
-template<>
-NODE_TYPE BVHModel<KDOP<24> >::getNodeType() const;
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/BVH_utility.h b/branches/point_cloud/fcl/include/fcl/BVH_utility.h
deleted file mode 100644
index 3cd45820b78cdff538b190bb500a64bb536051af..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/BVH_utility.h
+++ /dev/null
@@ -1,117 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#ifndef FCL_BVH_UTILITY_H
-#define FCL_BVH_UTILITY_H
-
-#include "fcl/primitive.h"
-#include "fcl/vec_3f.h"
-#include "fcl/BVH_model.h"
-
-
-namespace fcl
-{
-
-/** \brief Expand the BVH bounding boxes according to uncertainty */
-template<typename BV>
-void BVHExpand(BVHModel<BV>& model, const Uncertainty* ucs, BVH_REAL r)
-{
- for(int i = 0; i < model.num_bvs; ++i)
- {
- BVNode<BV>& bvnode = model.getBV(i);
-
- BV bv;
- for(int j = 0; j < bvnode.num_primitives; ++j)
- {
- int v_id = bvnode.first_primitive + j;
- const Uncertainty& uc = ucs[v_id];
-
- Vec3f& v = model.vertices[bvnode.first_primitive + j];
-
- for(int k = 0; k < 3; ++k)
- {
- bv += (v + uc.axis[k] * (r * uc.sigma[k]));
- bv += (v - uc.axis[k] * (r * uc.sigma[k]));
- }
- }
-
- bvnode.bv = bv;
- }
-}
-
-/** \brief Expand the BVH bounding boxes according to uncertainty, for OBB */
-void BVHExpand(BVHModel<OBB>& model, const Uncertainty* ucs, BVH_REAL r);
-
-
-/** \brief Expand the BVH bounding boxes according to uncertainty, for RSS */
-void BVHExpand(BVHModel<RSS>& model, const Uncertainty* ucs, BVH_REAL r);
-
-/** \brief Estimate the uncertainty of point clouds due to sampling procedure */
-void estimateSamplingUncertainty(Vec3f* vertices, int num_vertices, Uncertainty* ucs);
-
-/** \brief Compute the covariance matrix for a set or subset of points. */
-void getCovariance(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, Vec3f M[3]);
-
-/** \brief Compute the covariance matrix for a set or subset of triangles. */
-void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f M[3]);
-
-
-/** \brief Compute the RSS bounding volume parameters: radius, rectangle size and the origin.
- * The bounding volume axes are known.
- */
-void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, Vec3f axis[3], Vec3f& origin, BVH_REAL l[2], BVH_REAL& r);
-
-
-/** \brief Compute the RSS bounding volume parameters: radius, rectangle size and the origin.
- * The bounding volume axes are known.
- */
-void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& origin, BVH_REAL l[2], BVH_REAL& r);
-
-/** \brief Compute the bounding volume extent and center for a set or subset of points.
- * The bounding volume axes are known.
- */
-void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, Vec3f axis[3], Vec3f& center, Vec3f& extent);
-
-
-/** \brief Compute the bounding volume extent and center for a set or subset of points.
- * The bounding volume axes are known.
- */
-void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& center, Vec3f& extent);
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/BV_fitter.h b/branches/point_cloud/fcl/include/fcl/BV_fitter.h
deleted file mode 100644
index 9b583af152e3130d8d44e7f3d2b33fa45b0e626a..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/BV_fitter.h
+++ /dev/null
@@ -1,256 +0,0 @@
-
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-#ifndef FCL_BV_FITTER_H
-#define FCL_BV_FITTER_H
-
-#include "fcl/BVH_internal.h"
-#include "fcl/primitive.h"
-#include "fcl/vec_3f.h"
-#include "fcl/OBB.h"
-#include "fcl/RSS.h"
-#include <iostream>
-
-/** \brief Main namespace */
-namespace fcl
-{
-
-/** \brief Compute a bounding volume that fits a set of n points. */
-template<typename BV>
-void fit(Vec3f* ps, int n, BV& bv)
-{
- for(int i = 0; i < n; ++i)
- {
- bv += ps[i];
- }
-}
-
-template<>
-void fit<OBB>(Vec3f* ps, int n, OBB& bv);
-
-template<>
-void fit<RSS>(Vec3f* ps, int n, RSS& bv);
-
-
-template<typename BV>
-class BVFitterBase
-{
-public:
- virtual void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) = 0;
-
- virtual void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_) = 0;
-
- virtual BV fit(unsigned int* primitive_indices, int num_primitives) = 0;
-
- virtual void clear() = 0;
-};
-
-/** \brief A class for fitting a bounding volume to a set of points */
-template<typename BV>
-class BVFitter : public BVFitterBase<BV>
-{
-public:
- /** \brief Prepare the geometry primitive data for fitting */
- void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
- {
- vertices = vertices_;
- prev_vertices = NULL;
- tri_indices = tri_indices_;
- type = type_;
- }
-
- /** \brief Prepare the geometry primitive data for fitting */
- void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_)
- {
- vertices = vertices_;
- prev_vertices = prev_vertices_;
- tri_indices = tri_indices_;
- type = type_;
- }
-
- /** \brief Compute a bounding volume that fits a set of primitives (points or triangles).
- * The primitive data was set by set function and primitive_indices is the primitive index relative to the data
- */
- BV fit(unsigned int* primitive_indices, int num_primitives)
- {
- BV bv;
-
- if(type == BVH_MODEL_TRIANGLES) /* The primitive is triangle */
- {
- for(int i = 0; i < num_primitives; ++i)
- {
- Triangle t = tri_indices[primitive_indices[i]];
- bv += vertices[t[0]];
- bv += vertices[t[1]];
- bv += vertices[t[2]];
-
- if(prev_vertices) /* When fitting both current and previous frame */
- {
- bv += prev_vertices[t[0]];
- bv += prev_vertices[t[1]];
- bv += prev_vertices[t[2]];
- }
- }
- }
- else if(type == BVH_MODEL_POINTCLOUD) /* The primitive is point */
- {
- for(int i = 0; i < num_primitives; ++i)
- {
- bv += vertices[primitive_indices[i]];
-
- if(prev_vertices) /* When fitting both current and previous frame */
- {
- bv += prev_vertices[primitive_indices[i]];
- }
- }
- }
-
- return bv;
- }
-
- /** \brief Clear the geometry primitive data */
- void clear()
- {
- vertices = NULL;
- prev_vertices = NULL;
- tri_indices = NULL;
- type = BVH_MODEL_UNKNOWN;
- }
-
-private:
-
- Vec3f* vertices;
- Vec3f* prev_vertices;
- Triangle* tri_indices;
- BVHModelType type;
-};
-
-
-/** \brief Specification of BVFitter for OBB bounding volume */
-template<>
-class BVFitter<OBB> : public BVFitterBase<OBB>
-{
-public:
- /** \brief Prepare the geometry primitive data for fitting */
- void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
- {
- vertices = vertices_;
- prev_vertices = NULL;
- tri_indices = tri_indices_;
- type = type_;
- }
-
- /** \brief Prepare the geometry primitive data for fitting */
- void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_)
- {
- vertices = vertices_;
- prev_vertices = prev_vertices_;
- tri_indices = tri_indices_;
- type = type_;
- }
-
- /** \brief Compute a bounding volume that fits a set of primitives (points or triangles).
- * The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
- */
- OBB fit(unsigned int* primitive_indices, int num_primitives);
-
- /** \brief Clear the geometry primitive data */
- void clear()
- {
- vertices = NULL;
- prev_vertices = NULL;
- tri_indices = NULL;
- type = BVH_MODEL_UNKNOWN;
- }
-
-private:
-
- Vec3f* vertices;
- Vec3f* prev_vertices;
- Triangle* tri_indices;
- BVHModelType type;
-};
-
-
-/** \brief Specification of BVFitter for RSS bounding volume */
-template<>
-class BVFitter<RSS> : public BVFitterBase<RSS>
-{
-public:
- /** \brief Prepare the geometry primitive data for fitting */
- void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
- {
- vertices = vertices_;
- prev_vertices = NULL;
- tri_indices = tri_indices_;
- type = type_;
- }
-
- /** \brief Prepare the geometry primitive data for fitting */
- void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_)
- {
- vertices = vertices_;
- prev_vertices = prev_vertices_;
- tri_indices = tri_indices_;
- type = type_;
- }
-
- /** \brief Compute a bounding volume that fits a set of primitives (points or triangles).
- * The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
- */
- RSS fit(unsigned int* primitive_indices, int num_primitives);
-
- /** \brief Clear the geometry primitive data */
- void clear()
- {
- vertices = NULL;
- prev_vertices = NULL;
- tri_indices = NULL;
- type = BVH_MODEL_UNKNOWN;
- }
-
-private:
-
- Vec3f* vertices;
- Vec3f* prev_vertices;
- Triangle* tri_indices;
- BVHModelType type;
-};
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/BV_node.h b/branches/point_cloud/fcl/include/fcl/BV_node.h
deleted file mode 100644
index 26268e04734f2c0e63a36cb6c37766cf4ca2875e..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/BV_node.h
+++ /dev/null
@@ -1,94 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#ifndef FCL_BV_NODE_H
-#define FCL_BV_NODE_H
-
-/** \brief Main namespace */
-namespace fcl
-{
-
-/** \brief A class describing a bounding volume node */
-template<typename BV>
-struct BVNode
-{
- /** \brief A bounding volume */
- BV bv;
-
- /** \brief An index for first child node or primitive
- * If the value is positive, it is the index of the first child bv node
- * If the value is negative, it is -(primitive index + 1)
- * Zero is not used.
- */
- int first_child;
-
- /** \brief The start id the primitive belonging to the current node. The index is referred to the primitive_indices in BVHModel and from that
- * we can obtain the primitive's index in original data indirectly.
- */
- int first_primitive;
-
- /** \brief The number of primitives belonging to the current node */
- int num_primitives;
-
- /** \brief Whether current node is a leaf node (i.e. contains a primitive index */
- bool isLeaf() const { return first_child < 0; }
-
- /** \brief Return the primitive index. The index is referred to the original data (i.e. vertices or tri_indices) in BVHModel */
- int primitiveId() const { return -(first_child + 1); }
-
- /** \brief Return the index of the first child. The index is referred to the bounding volume array (i.e. bvs) in BVHModel */
- int leftChild() const { return first_child; }
-
- /** \brief Return the index of the second child. The index is referred to the bounding volume array (i.e. bvs) in BVHModel */
- int rightChild() const { return first_child + 1; }
-
- /** \brief Check whether two BVNode collide */
- bool overlap(const BVNode& other) const
- {
- return bv.overlap(other.bv);
- }
-
- BVH_REAL distance(const BVNode& other, Vec3f* P1 = NULL, Vec3f* P2 = NULL) const
- {
- return bv.distance(other.bv, P1, P2);
- }
-
-};
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/BV_splitter.h b/branches/point_cloud/fcl/include/fcl/BV_splitter.h
deleted file mode 100644
index 61a7d37b1bc85486d0dfb148db526f4c39f10539..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/BV_splitter.h
+++ /dev/null
@@ -1,291 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-#ifndef FCL_BV_SPLITTER_H
-#define FCL_BV_SPLITTER_H
-
-
-#include "fcl/BVH_internal.h"
-#include "fcl/primitive.h"
-#include "fcl/vec_3f.h"
-#include "fcl/OBB.h"
-#include <vector>
-#include <iostream>
-
-/** \brief Main namespace */
-namespace fcl
-{
-
-/** \brief Base class for BV splitting operation */
-template<typename BV>
-class BVSplitterBase
-{
-public:
- /** \brief Set the geometry data needed by the split rule */
- virtual void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) = 0;
-
- /** \brief Compute the split rule according to a subset of geometry and the corresponding BV node */
- virtual void computeRule(const BV& bv, unsigned int* primitive_indices, int num_primitives) = 0;
-
- /** \brief Apply the split rule on a given point */
- virtual bool apply(const Vec3f& q) const = 0;
-
- /** \brief Clear the geometry data set before */
- virtual void clear() = 0;
-};
-
-
-enum SplitMethodType {SPLIT_METHOD_MEAN, SPLIT_METHOD_MEDIAN, SPLIT_METHOD_BV_CENTER};
-
-
-/** \brief A class describing the split rule that splits each BV node */
-template<typename BV>
-class BVSplitter : public BVSplitterBase<BV>
-{
-public:
-
- BVSplitter(SplitMethodType method)
- {
- split_method = method;
- }
-
- /** \brief Set the geometry data needed by the split rule */
- void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
- {
- vertices = vertices_;
- tri_indices = tri_indices_;
- type = type_;
- }
-
- /** \brief Compute the split rule according to a subset of geometry and the corresponding BV node */
- void computeRule(const BV& bv, unsigned int* primitive_indices, int num_primitives)
- {
- switch(split_method)
- {
- case SPLIT_METHOD_MEAN:
- computeRule_mean(bv, primitive_indices, num_primitives);
- break;
- case SPLIT_METHOD_MEDIAN:
- computeRule_median(bv, primitive_indices, num_primitives);
- break;
- case SPLIT_METHOD_BV_CENTER:
- computeRule_bvcenter(bv, primitive_indices, num_primitives);
- break;
- default:
- std::cerr << "Split method not supported" << std::endl;
- }
- }
-
- /** \brief Apply the split rule on a given point */
- bool apply(const Vec3f& q) const
- {
- return q[split_axis] > split_value;
- }
-
- /** \brief Clear the geometry data set before */
- void clear()
- {
- vertices = NULL;
- tri_indices = NULL;
- type = BVH_MODEL_UNKNOWN;
- }
-
-private:
-
- /** \brief The split axis */
- int split_axis;
-
- /** \brief The split threshold */
- BVH_REAL split_value;
-
- Vec3f* vertices;
- Triangle* tri_indices;
- BVHModelType type;
- SplitMethodType split_method;
-
- /** \brief Split the node from center */
- void computeRule_bvcenter(const BV& bv, unsigned int* primitive_indices, int num_primitives)
- {
- Vec3f center = bv.center();
- int axis = 2;
-
- if(bv.width() >= bv.height() && bv.width() >= bv.depth())
- axis = 0;
- else if(bv.height() >= bv.width() && bv.height() >= bv.depth())
- axis = 1;
-
- split_axis = axis;
- split_value = center[axis];
- }
-
- /** \brief Split the node according to the mean of the data contained */
- void computeRule_mean(const BV& bv, unsigned int* primitive_indices, int num_primitives)
- {
- int axis = 2;
-
- if(bv.width() >= bv.height() && bv.width() >= bv.depth())
- axis = 0;
- else if(bv.height() >= bv.width() && bv.height() >= bv.depth())
- axis = 1;
-
- split_axis = axis;
- BVH_REAL sum = 0;
-
- if(type == BVH_MODEL_TRIANGLES)
- {
- for(int i = 0; i < num_primitives; ++i)
- {
- const Triangle& t = tri_indices[primitive_indices[i]];
- sum += ((vertices[t[0]][split_axis] + vertices[t[1]][split_axis] + vertices[t[2]][split_axis]) / 3);
- }
- }
- else if(type == BVH_MODEL_POINTCLOUD)
- {
- for(int i = 0; i < num_primitives; ++i)
- {
- sum += vertices[primitive_indices[i]][split_axis];
- }
- }
-
- split_value = sum / num_primitives;
- }
-
- /** \brief Split the node according to the median of the data contained */
- void computeRule_median(const BV& bv, unsigned int* primitive_indices, int num_primitives)
- {
- int axis = 2;
-
- if(bv.width() >= bv.height() && bv.width() >= bv.depth())
- axis = 0;
- else if(bv.height() >= bv.width() && bv.height() >= bv.depth())
- axis = 1;
-
- split_axis = axis;
- std::vector<BVH_REAL> proj(num_primitives);
-
- if(type == BVH_MODEL_TRIANGLES)
- {
- for(int i = 0; i < num_primitives; ++i)
- {
- const Triangle& t = tri_indices[primitive_indices[i]];
- proj[i] = (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] + vertices[t[2]][split_axis]) / 3;
- }
- }
- else if(type == BVH_MODEL_POINTCLOUD)
- {
- for(int i = 0; i < num_primitives; ++i)
- proj[i] = vertices[primitive_indices[i]][split_axis];
- }
-
- std::sort(proj.begin(), proj.end());
-
- if(num_primitives % 2 == 1)
- {
- split_value = proj[(num_primitives - 1) / 2];
- }
- else
- {
- split_value = (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2;
- }
- }
-};
-
-
-
-/** \brief BVHSplitRule specialization for OBB */
-template<>
-class BVSplitter<OBB> : public BVSplitterBase<OBB>
-{
-public:
-
- BVSplitter(SplitMethodType method)
- {
- split_method = method;
- }
-
- /** \brief Set the geometry data needed by the split rule */
- void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
- {
- vertices = vertices_;
- tri_indices = tri_indices_;
- type = type_;
- }
-
- /** \brief Compute the split rule according to a subset of geometry and the corresponding BV node */
- void computeRule(const OBB& bv, unsigned int* primitive_indices, int num_primitives)
- {
- Vec3f center = bv.center();
- split_vector = bv.axis[0];
- split_value = center[0];
- }
-
- /** \brief Apply the split rule on a given point */
- bool apply(const Vec3f& q) const
- {
- return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
- }
-
- /** \brief Clear the geometry data set before */
- void clear()
- {
- vertices = NULL;
- tri_indices = NULL;
- type = BVH_MODEL_UNKNOWN;
- }
-
-private:
-
- /** \brief Split the node from center */
- void computeRule_bvcenter(const OBB& bv, unsigned int* primitive_indices, int num_primitives);
-
- /** \brief Split the node according to the mean of the data contained */
- void computeRule_mean(const OBB& bv, unsigned int* primitive_indices, int num_primitives);
-
- /** \brief Split the node according to the median of the data contained */
- void computeRule_median(const OBB& bv, unsigned int* primitive_indices, int num_primitives);
-
- BVH_REAL split_value;
- Vec3f split_vector;
-
- Vec3f* vertices;
- Triangle* tri_indices;
- BVHModelType type;
- SplitMethodType split_method;
-};
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/OBB.h b/branches/point_cloud/fcl/include/fcl/OBB.h
deleted file mode 100644
index 34565d22cd600de5397b3e239b3be00d99cd4e0e..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/OBB.h
+++ /dev/null
@@ -1,153 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-#ifndef FCL_PQP_H
-#define FCL_PQP_H
-
-#include "fcl/BVH_internal.h"
-#include "fcl/vec_3f.h"
-
-/** \brief Main namespace */
-namespace fcl
-{
-
-/** \brief OBB class */
-class OBB
-{
-public:
- /** \brief Orientation of OBB */
- Vec3f axis[3]; // R[i] is the ith column of the orientation matrix, or the axis of the OBB
-
- /** \brief center of OBB */
- Vec3f To;
-
- /** \brief Half dimensions of OBB */
- Vec3f extent;
-
- OBB() {}
-
- /** \brief Check collision between two OBB */
- bool overlap(const OBB& other) const;
-
- /** \brief Check collision between two OBB and return the overlap part.
- * For OBB, we return nothing, as the overlap part of two obbs usually is not an obb
- */
- bool overlap(const OBB& other, OBB& overlap_part) const
- {
- return overlap(other);
- }
-
- /** \brief Check whether the OBB contains a point */
- inline bool contain(const Vec3f& p) const;
-
- /** \brief A simple way to merge the OBB and a point, not compact. */
- OBB& operator += (const Vec3f& p);
-
- /** \brief Merge the OBB and another OBB */
- OBB& operator += (const OBB& other)
- {
- *this = *this + other;
- return *this;
- }
-
- /** \brief Return the merged OBB of current OBB and the other one */
- OBB operator + (const OBB& other) const;
-
- /** \brief Width of the OBB */
- inline BVH_REAL width() const
- {
- return 2 * extent[0];
- }
-
- /** \brief Height of the OBB */
- inline BVH_REAL height() const
- {
- return 2 * extent[1];
- }
-
- /** \brief Depth of the OBB */
- inline BVH_REAL depth() const
- {
- return 2 * extent[2];
- }
-
- /** \brief Volume of the OBB */
- inline BVH_REAL volume() const
- {
- return width() * height() * depth();
- }
-
- /** \brief Size of the OBB, for split order */
- inline BVH_REAL size() const
- {
- return extent.sqrLength();
- }
-
- /** \brief Center of the OBB */
- inline Vec3f center() const
- {
- return To;
- }
-
- /** \brief The distance between two OBB
- * Not implemented
- */
- BVH_REAL distance(const OBB& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
-
-
-private:
-
- /** Compute the 8 vertices of a OBB */
- void computeVertices(Vec3f vertex[8]) const;
-
- /** \brief OBB merge method when the centers of two smaller OBB are far away */
- static OBB merge_largedist(const OBB& b1, const OBB& b2);
-
- /** \brief OBB merge method when the centers of two smaller OBB are close */
- static OBB merge_smalldist(const OBB& b1, const OBB& b2);
-
-public:
- /** Kernel check whether two OBB are disjoint */
- static bool obbDisjoint(const Vec3f B[3], const Vec3f& T, const Vec3f& a, const Vec3f& b);
-
-};
-
-
-bool overlap(const Vec3f R0[3], const Vec3f& T0, const OBB& b1, const OBB& b2);
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/RSS.h b/branches/point_cloud/fcl/include/fcl/RSS.h
deleted file mode 100644
index c839ac72442390c4d9c8298eb4b302ec1bc42f46..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/RSS.h
+++ /dev/null
@@ -1,178 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-#ifndef FCL_RSS_H
-#define FCL_RSS_H
-
-#include "fcl/BVH_internal.h"
-#include "fcl/vec_3f.h"
-
-namespace fcl
-{
-
-class RSS
-{
-public:
- /** \brief Orientation of RSS */
- Vec3f axis[3];
-
- /** \brief position of rectangle (origin of the rectangle) */
- Vec3f Tr;
-
- /** \brief side lengths of rectangle */
- BVH_REAL l[2];
-
- /** \brief radius of sphere summed with rectangle to form RSS */
- BVH_REAL r;
-
- RSS() {}
-
- /** \brief Check collision between two RSS */
- bool overlap(const RSS& other) const;
-
- /** \brief Check collision between two RSS and return the overlap part.
- * For RSS, we return nothing, as the overlap part of two RSSs usually is not a RSS
- */
- bool overlap(const RSS& other, RSS& overlap_part) const
- {
- return overlap(other);
- }
-
- /** \brief Check whether the RSS contains a point */
- inline bool contain(const Vec3f& p) const;
-
- /** \brief A simple way to merge the RSS and a point, not compact.
- * THIS FUNCTION STILL HAS PROBLEM!!
- */
- RSS& operator += (const Vec3f& p);
-
- /** \brief Merge the RSS and another RSS */
- inline RSS& operator += (const RSS& other)
- {
- *this = *this + other;
- return *this;
- }
-
- /** \brief Return the merged RSS of current RSS and the other one */
- RSS operator + (const RSS& other) const;
-
- /** \brief Width of the RSS */
- inline BVH_REAL width() const
- {
- return l[0] + 2 * r;
- }
-
- /** \brief Height of the RSS */
- inline BVH_REAL height() const
- {
- return l[1] + 2 * r;
- }
-
- /** \brief Depth of the RSS */
- inline BVH_REAL depth() const
- {
- return 2 * r;
- }
-
- /** \brief Volume of the RSS */
- inline BVH_REAL volume() const
- {
- return (l[0] * l[1] * 2 * r + 4 * 3.1415926 * r * r * r);
- }
-
- /** \brief Size of the RSS, for split order */
- inline BVH_REAL size() const
- {
- return (sqrt(l[0] * l[0] + l[1] * l[1]) + 2 * r);
- }
-
- /** \brief The RSS center */
- inline Vec3f center() const
- {
- return Tr;
- }
-
- /** \brief the distance between two RSS */
- BVH_REAL distance(const RSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
-
-protected:
-
- /** \brief Clip val between a and b */
- static void clipToRange(BVH_REAL& val, BVH_REAL a, BVH_REAL b);
-
- /** \brief Finds the parameters t & u corresponding to the two closest points on a pair of line segments.
- * The first segment is defined as Pa + A*t, 0 <= t <= a, where "Pa" is one endpoint of the segment, "A" is a unit vector
- * pointing to the other endpoint, and t is a scalar that produces all the points between the two endpoints. Since "A" is a unit
- * vector, "a" is the segment's length.
- * The second segment is defined as Pb + B*u, 0 <= u <= b.
- * Many of the terms needed by the algorithm are already computed for other purposes,so we just pass these terms into the function
- * instead of complete specifications of each segment. "T" in the dot products is the vector betweeen Pa and Pb.
- * Reference: "On fast computation of distance between line segments." Vladimir J. Lumelsky, in Information Processing Letters, no. 21, pages 55-61, 1985.
- */
- static void segCoords(BVH_REAL& t, BVH_REAL& u, BVH_REAL a, BVH_REAL b, BVH_REAL A_dot_B, BVH_REAL A_dot_T, BVH_REAL B_dot_T);
-
- /** \brief Returns whether the nearest point on rectangle edge
- * Pb + B*u, 0 <= u <= b, to the rectangle edge,
- * Pa + A*t, 0 <= t <= a, is within the half space
- * determined by the point Pa and the direction Anorm.
- *
- * A,B, and Anorm are unit vectors.
- * T is the vector between Pa and Pb.
- */
- static bool inVoronoi(BVH_REAL a, BVH_REAL b, BVH_REAL Anorm_dot_B, BVH_REAL Anorm_dot_T, BVH_REAL A_dot_B, BVH_REAL A_dot_T, BVH_REAL B_dot_T);
-
-public:
-
- /** \brief distance between two oriented rectangles
- * P and Q (optional return values) are the closest points in the rectangles, both are in the local frame of the first rectangle
- */
- static BVH_REAL rectDistance(const Vec3f Rab[3], Vec3f const& Tab, const BVH_REAL a[2], const BVH_REAL b[2], Vec3f* P = NULL, Vec3f* Q = NULL);
-
-};
-
-/** \brief distance between two RSS bounding volumes
- * P and Q (optional return values) are the closest points in the rectangles, not the RSS. But the direction P - Q is the correct direction for cloest points
- * Notice that P and Q are both in the local frame of the first RSS (not global frame and not even the local frame of object 1)
- */
-BVH_REAL distance(const Vec3f R0[3], const Vec3f& T0, const RSS& b1, const RSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
-
-bool overlap(const Vec3f R0[3], const Vec3f& T0, const RSS& b1, const RSS& b2);
-
-
-}
-
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/broad_phase_collision.h b/branches/point_cloud/fcl/include/fcl/broad_phase_collision.h
deleted file mode 100644
index 8d670ea9d9f1709aca720cac568a58f6aaa9e326..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/broad_phase_collision.h
+++ /dev/null
@@ -1,392 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-
-#ifndef FCL_BROAD_PHASE_COLLISION_H
-#define FCL_BROAD_PHASE_COLLISION_H
-
-#include "fcl/collision_object.h"
-#include "fcl/collision_data.h"
-#include "fcl/AABB.h"
-#include "fcl/interval_tree.h"
-#include <vector>
-#include <list>
-
-namespace fcl
-{
-
-
-
-bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cdata);
-
-/** \brief return value is whether can stop now */
-typedef bool (*CollisionCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata);
-
-class BroadPhaseCollisionManager
-{
-public:
- virtual void registerObject(CollisionObject* obj) = 0;
-
- virtual void unregisterObject(CollisionObject* obj) = 0;
-
- virtual void setup() = 0;
-
- virtual void update() = 0;
-
- virtual void clear() = 0;
-
- virtual void getObjects(std::vector<CollisionObject*>& objs) const = 0;
-
- virtual void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const = 0;
-
- virtual void collide(void* cdata, CollisionCallBack callback) const = 0;
-
- virtual bool empty() const = 0;
-
- virtual size_t size() const = 0;
-};
-
-class NaiveCollisionManager : public BroadPhaseCollisionManager
-{
-public:
- NaiveCollisionManager() {}
-
- void unregisterObject(CollisionObject* obj);
-
- void registerObject(CollisionObject* obj);
-
- void setup();
-
- void update();
-
- void clear();
-
- void getObjects(std::vector<CollisionObject*>& objs) const;
-
- void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
-
- void collide(void* cdata, CollisionCallBack callback) const;
-
- bool empty() const;
-
- inline size_t size() const { return objs.size(); }
-
-protected:
-
- std::list<CollisionObject*> objs;
-};
-
-class SaPCollisionManager : public BroadPhaseCollisionManager
-{
-public:
-
- SaPCollisionManager()
- {
- elist[0] = NULL;
- elist[1] = NULL;
- elist[2] = NULL;
- }
-
- ~SaPCollisionManager()
- {
- clear();
- }
-
- void unregisterObject(CollisionObject* obj);
-
- void registerObject(CollisionObject* obj);
-
- void setup();
-
- void update();
-
- void clear();
-
- void getObjects(std::vector<CollisionObject*>& objs) const;
-
- void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
-
- void collide(void* cdata, CollisionCallBack callback) const;
-
- bool empty() const;
-
- inline size_t size() const { return AABB_arr.size(); }
-
-protected:
-
- struct EndPoint;
-
- struct SaPAABB
- {
- CollisionObject* obj;
- EndPoint* lo;
- EndPoint* hi;
- AABB cached;
- };
-
- struct EndPoint
- {
- char minmax;
- SaPAABB* aabb;
- EndPoint* prev[3];
- EndPoint* next[3];
- const Vec3f& getVal() const
- {
- if(minmax == 0) return aabb->cached.min_;
- else return aabb->cached.max_;
- }
-
- Vec3f& getVal()
- {
- if(minmax == 0) return aabb->cached.min_;
- else return aabb->cached.max_;
- }
- };
-
- struct SaPPair
- {
- SaPPair(CollisionObject* a, CollisionObject* b)
- {
- obj1 = a;
- obj2 = b;
- }
-
- CollisionObject* obj1;
- CollisionObject* obj2;
- };
-
- class isUnregistered
- {
- CollisionObject* obj;
-
- public:
- isUnregistered(CollisionObject* obj_)
- {
- obj = obj_;
- }
-
- bool operator() (const SaPPair& pair)
- {
- return (pair.obj1 == obj) || (pair.obj2 == obj);
- }
- };
-
- class isNotValidPair
- {
- CollisionObject* obj1;
- CollisionObject* obj2;
-
- public:
- isNotValidPair(CollisionObject* obj1_, CollisionObject* obj2_)
- {
- obj1 = obj1_;
- obj2 = obj2_;
- }
-
- bool operator() (const SaPPair& pair)
- {
- return (pair.obj1 == obj1 && pair.obj2 == obj2) || (pair.obj1 == obj2 && pair.obj2 == obj1);
- }
- };
-
-
- EndPoint* elist[3];
- std::list<SaPAABB*> AABB_arr;
-
- std::list<SaPPair> overlap_pairs;
-};
-
-class SSaPCollisionManager : public BroadPhaseCollisionManager
-{
-public:
- SSaPCollisionManager()
- {
- setup_ = false;
- }
-
- void unregisterObject(CollisionObject* obj);
-
- void registerObject(CollisionObject* obj);
-
- void setup();
-
- void update();
-
- void clear();
-
- void getObjects(std::vector<CollisionObject*>& objs) const;
-
- void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
-
- void collide(void* cdata, CollisionCallBack callback) const;
-
- bool empty() const;
-
- inline size_t size() const { return objs_x.size(); }
-
-protected:
-
- struct SortByXLow
- {
- bool operator()(const CollisionObject* a, const CollisionObject* b) const
- {
- if(a->getCachedAABB().min_[0] < b->getCachedAABB().min_[0])
- return true;
- return false;
- }
- };
-
- struct SortByYLow
- {
- bool operator()(const CollisionObject* a, const CollisionObject* b) const
- {
- if(a->getCachedAABB().min_[1] < b->getCachedAABB().min_[1])
- return true;
- return false;
- }
- };
-
- struct SortByZLow
- {
- bool operator()(const CollisionObject* a, const CollisionObject* b) const
- {
- if(a->getCachedAABB().min_[2] < b->getCachedAABB().min_[2])
- return true;
- return false;
- }
- };
-
- class DummyCollisionObject : public CollisionObject
- {
- public:
- DummyCollisionObject(const AABB& aabb)
- {
- aabb_cache = aabb;
- }
-
- void computeAABB() {}
- };
-
- void checkColl(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end,
- CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
-
-
- std::vector<CollisionObject*> objs_x;
- std::vector<CollisionObject*> objs_y;
- std::vector<CollisionObject*> objs_z;
-
- bool setup_;
-};
-
-
-class IntervalTreeCollisionManager : public BroadPhaseCollisionManager
-{
-public:
- IntervalTreeCollisionManager()
- {
- setup_ = false;
- for(int i = 0; i < 3; ++i)
- interval_trees[i] = NULL;
- }
-
- void unregisterObject(CollisionObject* obj);
-
- void registerObject(CollisionObject* obj);
-
- void setup();
-
- void update();
-
- void clear();
-
- void getObjects(std::vector<CollisionObject*>& objs) const;
-
- void checkColl(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end,
- CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
-
- void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
-
- void collide(void* cdata, CollisionCallBack callback) const;
-
- bool empty() const;
-
- inline size_t size() const { return endpoints[0].size() / 2; }
-
-protected:
-
-
- struct EndPoint
- {
- CollisionObject* obj; // pointer to endpoint geometry;
- BVH_REAL value; // endpoint value
- char minmax; // '0' if interval min, '1' if interval max
- };
-
- struct SortByValue
- {
- bool operator()(const EndPoint& a, const EndPoint& b) const
- {
- if(a.value < b.value)
- return true;
- return false;
- }
- };
-
- struct SAPInterval : public Interval
- {
- CollisionObject* obj;
- SAPInterval(double low_, double high_, CollisionObject* obj_)
- {
- low = low_;
- high = high_;
- obj = obj_;
- }
- };
-
-
- std::vector<EndPoint> endpoints[3];
-
- IntervalTree* interval_trees[3];
-
- bool setup_;
-
-};
-
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/collision.h b/branches/point_cloud/fcl/include/fcl/collision.h
deleted file mode 100644
index 44f4375e6bd2e191819436f8b99c21e85fe3a9f5..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/collision.h
+++ /dev/null
@@ -1,59 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#ifndef FCL_COLLISION_H
-#define FCL_COLLISION_H
-
-#include "fcl/vec_3f.h"
-#include "fcl/collision_object.h"
-#include "fcl/collision_data.h"
-
-/** \brief Main namespace */
-namespace fcl
-{
-
-/** \brief Main collision interface: given two collision objects, and the requirements for contacts, including num of max contacts, whether perform exhaustive collision (i.e., returning
- * returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function
- * performs the collision between them.
- * Return value is the number of contacts returned
- */
-int collide(const CollisionObject* o1, const CollisionObject* o2,
- int num_max_contacts, bool exhaustive, bool enable_contact,
- std::vector<Contact>& contacts);
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/collision_data.h b/branches/point_cloud/fcl/include/fcl/collision_data.h
deleted file mode 100644
index 164ac241e2497e074be2929ea6d10ac91c01c50e..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/collision_data.h
+++ /dev/null
@@ -1,75 +0,0 @@
-#ifndef FCL_COLLISION_DATA_H
-#define FCL_COLLISION_DATA_H
-
-#include "fcl/collision_object.h"
-#include "fcl/vec_3f.h"
-#include <vector>
-
-
-namespace fcl
-{
-
-struct Contact
-{
- BVH_REAL penetration_depth;
- Vec3f normal;
- Vec3f pos;
- const CollisionObject* o1;
- const CollisionObject* o2;
- int b1;
- int b2;
-
- Contact()
- {
- o1 = NULL;
- o2 = NULL;
- b1 = -1;
- b2 = -1;
- }
-
- Contact(const CollisionObject* o1_, const CollisionObject* o2_, int b1_, int b2_)
- {
- o1 = o1_;
- o2 = o2_;
- b1 = b1_;
- b2 = b2_;
- }
-
- Contact(const CollisionObject* o1_, const CollisionObject* o2_, int b1_, int b2_,
- const Vec3f& pos_, const Vec3f& normal_, BVH_REAL depth_)
- {
- o1 = o1_;
- o2 = o2_;
- b1 = b1_;
- b2 = b2_;
- normal = normal_;
- pos = pos_;
- penetration_depth = depth_;
- }
-};
-
-struct CollisionData
-{
- CollisionData()
- {
- done = false;
- is_collision = false;
- num_max_contacts = 1;
- enable_contact = false;
- exhaustive = false;
- }
-
- bool done;
- bool is_collision;
- bool exhaustive;
- unsigned int num_max_contacts;
- bool enable_contact;
-
- std::vector<Contact> contacts;
-};
-
-
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/collision_func_matrix.h b/branches/point_cloud/fcl/include/fcl/collision_func_matrix.h
deleted file mode 100644
index 9fdb2649da9fb197eaa070abcda6fd33a48fae31..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/collision_func_matrix.h
+++ /dev/null
@@ -1,63 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#ifndef FCL_COLLISION_FUNC_MATRIX_H
-#define FCL_COLLISION_FUNC_MATRIX_H
-
-
-#include "fcl/collision_object.h"
-#include "fcl/collision_data.h"
-
-namespace fcl
-{
-
-
-typedef int (*CollisionFunc)(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts);
-
-
-struct CollisionFunctionMatrix
-{
- CollisionFunc collision_matrix[14][14];
-
- CollisionFunctionMatrix();
-};
-
-
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/collision_node.h b/branches/point_cloud/fcl/include/fcl/collision_node.h
deleted file mode 100644
index 00d3137707bef565fbb6035cb46a5b002fef1e77..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/collision_node.h
+++ /dev/null
@@ -1,58 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#ifndef FCL_COLLISION_NODE_H
-#define FCL_COLLISION_NODE_H
-
-#include "fcl/traversal_node_base.h"
-#include "fcl/traversal_node_bvhs.h"
-#include "fcl/BVH_front.h"
-
-namespace fcl
-{
-
-void collide(CollisionTraversalNodeBase* node, BVHFrontList* front_list = NULL);
-
-void selfCollide(CollisionTraversalNodeBase* node, BVHFrontList* front_list = NULL);
-
-void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list = NULL, int qsize = 2);
-
-void distance(MeshDistanceTraversalNodeRSS* node, BVHFrontList* front_list = NULL, int qsize = 2);
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/collision_object.h b/branches/point_cloud/fcl/include/fcl/collision_object.h
deleted file mode 100644
index 1ab149305db307784dee634086dbcebe27215d34..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/collision_object.h
+++ /dev/null
@@ -1,127 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#ifndef FCL_COLLISION_OBJECT_BASE_H
-#define FCL_COLLISION_OBJECT_BASE_H
-
-#include "fcl/AABB.h"
-#include "fcl/transform.h"
-
-namespace fcl
-{
-
-enum OBJECT_TYPE {OT_UNKNOWN, OT_BVH, OT_GEOM};
-
-enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_KDOP16, BV_KDOP18, BV_KDOP24,
- GEOM_BOX, GEOM_SPHERE, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE};
-
-/** \brief Base class for all objects participating in collision */
-class CollisionObject
-{
-public:
- virtual ~CollisionObject() {}
-
- virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; }
-
- virtual NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
-
- virtual void computeAABB() = 0;
-
- inline const AABB& getCachedAABB() const
- {
- return aabb_cache;
- }
-
- inline void computeCachedAABB()
- {
- Vec3f center = t.transform(aabb_center);
- Vec3f delta(aabb_radius, aabb_radius, aabb_radius);
- aabb_cache.min_ = center - delta;
- aabb_cache.max_ = center + delta;
- }
-
- inline const Vec3f& getTranslation() const
- {
- return t.getTranslation();
- }
-
- inline const Vec3f* getRotation() const
- {
- return t.getRotation();
- }
-
- inline const SimpleQuaternion& getQuatRotation() const
- {
- return t.getQuatRotation();
- }
-
- void setRotation(const Vec3f R[3])
- {
- t.setRotation(R);
- }
-
- void setTranslation(const Vec3f& T)
- {
- t.setTranslation(T);
- }
-
- void setQuatRotation(const SimpleQuaternion& q)
- {
- t.setQuatRotation(q);
- }
-
-protected:
-
- /** AABB in global coordinate */
- mutable AABB aabb_cache;
-
- /** AABB in local coordinate */
- AABB aabb;
-
- /** AABB center in local coordinate */
- Vec3f aabb_center;
-
- /** AABB radius */
- BVH_REAL aabb_radius;
-
- SimpleTransform t;
-};
-
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/geometric_shape_to_BVH_model.h b/branches/point_cloud/fcl/include/fcl/geometric_shape_to_BVH_model.h
deleted file mode 100644
index ef4f32170574aacd4bd5bbd26cf1c1a354dc1837..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/geometric_shape_to_BVH_model.h
+++ /dev/null
@@ -1,347 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#ifndef GEOMETRIC_SHAPE_TO_BVH_MODEL_H
-#define GEOMETRIC_SHAPE_TO_BVH_MODEL_H
-
-#include "fcl/geometric_shapes.h"
-#include "fcl/BVH_model.h"
-#include <boost/math/constants/constants.hpp>
-
-/** \brief Main namespace */
-namespace fcl
-{
-
-/** \brief Generate BVH model from box */
-template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Box& shape)
-{
- double a = shape.side[0];
- double b = shape.side[1];
- double c = shape.side[2];
- std::vector<Vec3f> points(8);
- std::vector<Triangle> tri_indices(12);
- points[0] = Vec3f(0.5 * a, -0.5 * b, 0.5 * c);
- points[1] = Vec3f(0.5 * a, 0.5 * b, 0.5 * c);
- points[2] = Vec3f(-0.5 * a, 0.5 * b, 0.5 * c);
- points[3] = Vec3f(-0.5 * a, -0.5 * b, 0.5 * c);
- points[4] = Vec3f(0.5 * a, -0.5 * b, -0.5 * c);
- points[5] = Vec3f(0.5 * a, 0.5 * b, -0.5 * c);
- points[6] = Vec3f(-0.5 * a, 0.5 * b, -0.5 * c);
- points[7] = Vec3f(-0.5 * a, -0.5 * b, -0.5 * c);
-
- tri_indices[0] = Triangle(0, 4, 1);
- tri_indices[1] = Triangle(1, 4, 5);
- tri_indices[2] = Triangle(2, 5, 3);
- tri_indices[3] = Triangle(3, 6, 7);
- tri_indices[4] = Triangle(3, 0, 2);
- tri_indices[5] = Triangle(2, 0, 1);
- tri_indices[6] = Triangle(6, 5, 7);
- tri_indices[7] = Triangle(7, 5, 4);
- tri_indices[8] = Triangle(1, 5, 2);
- tri_indices[9] = Triangle(2, 5, 6);
- tri_indices[10] = Triangle(3, 7, 0);
- tri_indices[11] = Triangle(0, 7, 4);
-
- for(unsigned int i = 0; i < points.size(); ++i)
- {
- Vec3f v = MxV(shape.getLocalRotation(), points[i]) + shape.getLocalPosition();
- v = MxV(shape.getRotation(), v) + shape.getTranslation();
- points[i] = v;
- }
-
- model.beginModel();
- model.addSubModel(points, tri_indices);
- model.endModel();
- model.computeAABB();
-}
-
-/** Generate BVH model from sphere */
-template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, unsigned int seg = 16, unsigned int ring = 16)
-{
- std::vector<Vec3f> points;
- std::vector<Triangle> tri_indices;
-
- double r = shape.radius;
- double phi, phid;
- const double pi = boost::math::constants::pi<double>();
- phid = pi * 2 / seg;
- phi = 0;
-
- double theta, thetad;
- thetad = pi / (ring + 1);
- theta = 0;
-
- for(unsigned int i = 0; i < ring; ++i)
- {
- double theta_ = theta + thetad * (i + 1);
- for(unsigned int j = 0; j < seg; ++j)
- {
- points.push_back(Vec3f(r * sin(theta_) * cos(phi + j * phid), r * sin(theta_) * sin(phi + j * phid), r * cos(theta_)));
- }
- }
- points.push_back(Vec3f(0, 0, r));
- points.push_back(Vec3f(0, 0, -r));
-
- for(unsigned int i = 0; i < ring - 1; ++i)
- {
- for(unsigned int j = 0; j < seg; ++j)
- {
- unsigned int a, b, c, d;
- a = i * seg + j;
- b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1);
- c = (i + 1) * seg + j;
- d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1);
- tri_indices.push_back(Triangle(a, c, b));
- tri_indices.push_back(Triangle(b, c, d));
- }
- }
-
- for(unsigned int j = 0; j < seg; ++j)
- {
- unsigned int a, b;
- a = j;
- b = (j == seg - 1) ? 0 : (j + 1);
- tri_indices.push_back(Triangle(ring * seg, a, b));
-
- a = (ring - 1) * seg + j;
- b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1);
- tri_indices.push_back(Triangle(a, ring * seg + 1, b));
- }
-
- for(unsigned int i = 0; i < points.size(); ++i)
- {
- Vec3f v = MxV(shape.getLocalRotation(), points[i]) + shape.getLocalPosition();
- v = MxV(shape.getRotation(), v) + shape.getTranslation();
- points[i] = v;
- }
-
- model.beginModel();
- model.addSubModel(points, tri_indices);
- model.endModel();
- model.computeAABB();
-}
-
-
-/** \brief Generate BVH model from cylinder */
-template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, unsigned int tot = 16)
-{
- std::vector<Vec3f> points;
- std::vector<Triangle> tri_indices;
-
- double r = shape.radius;
- double h = shape.lz;
- double phi, phid;
- const double pi = boost::math::constants::pi<double>();
- phid = pi * 2 / tot;
- phi = 0;
-
- double circle_edge = phid * r;
- unsigned int h_num = ceil(h / circle_edge);
- double hd = h / h_num;
-
- for(unsigned int i = 0; i < tot; ++i)
- points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), h / 2));
-
- for(unsigned int i = 0; i < h_num - 1; ++i)
- {
- for(unsigned int j = 0; j < tot; ++j)
- {
- points.push_back(Vec3f(r * cos(phi + phid * j), r * sin(phi + phid * j), h / 2 - (i + 1) * hd));
- }
- }
-
- for(unsigned int i = 0; i < tot; ++i)
- points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2));
-
- points.push_back(Vec3f(0, 0, h / 2));
- points.push_back(Vec3f(0, 0, -h / 2));
-
- for(unsigned int i = 0; i < tot; ++i)
- {
- Triangle tmp((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1)));
- tri_indices.push_back(tmp);
- }
-
- for(unsigned int i = 0; i < tot; ++i)
- {
- Triangle tmp((h_num + 1) * tot + 1, h_num * tot + i, h_num * tot + ((i == tot - 1) ? 0 : (i + 1)));
- tri_indices.push_back(tmp);
- }
-
- for(unsigned int i = 0; i < h_num; ++i)
- {
- for(unsigned int j = 0; j < tot; ++j)
- {
- int a, b, c, d;
- a = j;
- b = (j == tot - 1) ? 0 : (j + 1);
- c = j + tot;
- d = (j == tot - 1) ? tot : (j + 1 + tot);
-
- int start = i * tot;
- tri_indices.push_back(Triangle(start + b, start + a, start + c));
- tri_indices.push_back(Triangle(start + b, start + c, start + d));
- }
- }
-
- model.beginModel();
- model.addSubModel(points, tri_indices);
- model.endModel();
- model.computeAABB();
-}
-
-
-
-/** \brief Generate BVH model from box */
-template<typename BV>
-void generateBVHModelPointCloud(BVHModel<BV>& model, const Box& shape)
-{
- double a = shape.side[0];
- double b = shape.side[1];
- double c = shape.side[2];
- std::vector<Vec3f> points(8);
- points[0] = Vec3f(0.5 * a, -0.5 * b, 0.5 * c);
- points[1] = Vec3f(0.5 * a, 0.5 * b, 0.5 * c);
- points[2] = Vec3f(-0.5 * a, 0.5 * b, 0.5 * c);
- points[3] = Vec3f(-0.5 * a, -0.5 * b, 0.5 * c);
- points[4] = Vec3f(0.5 * a, -0.5 * b, -0.5 * c);
- points[5] = Vec3f(0.5 * a, 0.5 * b, -0.5 * c);
- points[6] = Vec3f(-0.5 * a, 0.5 * b, -0.5 * c);
- points[7] = Vec3f(-0.5 * a, -0.5 * b, -0.5 * c);
-
- for(unsigned int i = 0; i < points.size(); ++i)
- {
- Vec3f v = MxV(shape.getLocalRotation(), points[i]) + shape.getLocalPosition();
- v = MxV(shape.getRotation(), v) + shape.getTranslation();
- points[i] = v;
- }
-
- model.beginModel();
- model.addSubModel(points);
- model.endModel();
- model.computeAABB();
-}
-
-/** Generate BVH model from sphere */
-template<typename BV>
-void generateBVHModelPointCloud(BVHModel<BV>& model, const Sphere& shape, unsigned int seg = 16, unsigned int ring = 16)
-{
- std::vector<Vec3f> points;
-
- double r = shape.radius;
- double phi, phid;
- const double pi = boost::math::constants::pi<double>();
- phid = pi * 2 / seg;
- phi = 0;
-
- double theta, thetad;
- thetad = pi / (ring + 1);
- theta = 0;
-
- for(unsigned int i = 0; i < ring; ++i)
- {
- double theta_ = theta + thetad * (i + 1);
- for(unsigned int j = 0; j < seg; ++j)
- {
- points.push_back(Vec3f(r * sin(theta_) * cos(phi + j * phid), r * sin(theta_) * sin(phi + j * phid), r * cos(theta_)));
- }
- }
- points.push_back(Vec3f(0, 0, r));
- points.push_back(Vec3f(0, 0, -r));
-
- for(unsigned int i = 0; i < points.size(); ++i)
- {
- Vec3f v = MxV(shape.getLocalRotation(), points[i]) + shape.getLocalPosition();
- v = MxV(shape.getRotation(), v) + shape.getTranslation();
- points[i] = v;
- }
-
- model.beginModel();
- model.addSubModel(points);
- model.endModel();
- model.computeAABB();
-}
-
-
-/** \brief Generate BVH model from cylinder */
-template<typename BV>
-void generateBVHModelPointCloud(BVHModel<BV>& model, const Cylinder& shape, unsigned int tot = 16)
-{
- std::vector<Vec3f> points;
-
- double r = shape.radius;
- double h = shape.lz;
- double phi, phid;
- const double pi = boost::math::constants::pi<double>();
- phid = pi * 2 / tot;
- phi = 0;
-
- double circle_edge = phid * r;
- unsigned int h_num = ceil(h / circle_edge);
- double hd = h / h_num;
-
- for(unsigned int i = 0; i < tot; ++i)
- points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), h / 2));
-
- for(unsigned int i = 0; i < h_num - 1; ++i)
- {
- for(unsigned int j = 0; j < tot; ++j)
- {
- points.push_back(Vec3f(r * cos(phi + phid * j), r * sin(phi + phid * j), h / 2 - (i + 1) * hd));
- }
- }
-
- for(unsigned int i = 0; i < tot; ++i)
- points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2));
-
- points.push_back(Vec3f(0, 0, h / 2));
- points.push_back(Vec3f(0, 0, -h / 2));
-
- model.beginModel();
- model.addSubModel(points);
- model.endModel();
- model.computeAABB();
-}
-
-
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/geometric_shapes.h b/branches/point_cloud/fcl/include/fcl/geometric_shapes.h
deleted file mode 100644
index 93417027f6536742786e6ee18818277c12b1ca5f..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/geometric_shapes.h
+++ /dev/null
@@ -1,332 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#ifndef FCL_GEOMETRIC_SHAPES_H
-#define FCL_GEOMETRIC_SHAPES_H
-
-#include "fcl/collision_object.h"
-#include "fcl/vec_3f.h"
-#include <string.h>
-
-/** \brief Main namespace */
-namespace fcl
-{
-
-/** \brief Base class for all basic geometric shapes */
-class ShapeBase : public CollisionObject
-{
-public:
- /** \brief Default Constructor */
- ShapeBase()
- {
- Rloc[0][0] = 1;
- Rloc[1][1] = 1;
- Rloc[2][2] = 1;
- }
-
- /** \brief Set the local frame of the shape */
- void setLocalTransform(const Vec3f R_[3], const Vec3f& T_)
- {
- Rloc[0] = R_[0];
- Rloc[1] = R_[1];
- Rloc[2] = R_[2];
- Tloc = T_;
- }
-
- /** \brief Set the local orientation of the shape */
- void setLocalRotation(const Vec3f R[3])
- {
- Rloc[0] = R[0];
- Rloc[1] = R[1];
- Rloc[2] = R[2];
- }
-
- /** \brief Set the local position of the shape */
- void setLocalTranslation(const Vec3f& T)
- {
- Tloc = T;
- }
-
- /** \brief Append additional transform to the local transform */
- void appendLocalTransform(const Vec3f R[3], const Vec3f& T)
- {
- Vec3f R0[3];
- for(int i = 0; i < 3; ++i)
- R0[i] = Rloc[i];
- MxM(R, R0, Rloc);
- Tloc = MxV(R, Tloc) + T;
- }
-
- /** \brief Get local transform */
- void getLocalTransfrom(Vec3f R[3], Vec3f& T) const
- {
- T = Tloc;
- R[0] = Rloc[0];
- R[1] = Rloc[1];
- R[2] = Rloc[2];
- }
-
- /** \brief Get local position */
- inline const Vec3f& getLocalPosition() const
- {
- return Tloc;
- }
-
- /** \brief Get local orientation */
- inline const Vec3f* getLocalRotation() const
- {
- return Rloc;
- }
-
- /** \brief Get object type: a geometric shape */
- OBJECT_TYPE getObjectType() const { return OT_GEOM; }
-
-protected:
-
- Vec3f Rloc[3];
- Vec3f Tloc;
-};
-
-
-/** Center at zero point, axis aligned box */
-class Box : public ShapeBase
-{
-public:
- Box(BVH_REAL x, BVH_REAL y, BVH_REAL z) : ShapeBase(), side(x, y, z) {}
-
- /** box side length */
- Vec3f side;
-
- /** \brief Compute AABB */
- void computeAABB();
-
- /** \brief Get node type: a box */
- NODE_TYPE getNodeType() const { return GEOM_BOX; }
-};
-
-/** Center at zero point sphere */
-class Sphere : public ShapeBase
-{
-public:
- Sphere(BVH_REAL radius_) : ShapeBase(), radius(radius_) {}
-
- /** \brief Radius of the sphere */
- BVH_REAL radius;
-
- /** \brief Compute AABB */
- void computeAABB();
-
- /** \brief Get node type: a sphere */
- NODE_TYPE getNodeType() const { return GEOM_SPHERE; }
-};
-
-/** Center at zero point capsule */
-class Capsule : public ShapeBase
-{
-public:
- Capsule(BVH_REAL radius_, BVH_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_) {}
-
- /** \brief Radius of capsule */
- BVH_REAL radius;
-
- /** \brief Length along z axis */
- BVH_REAL lz;
-
- /** \brief Compute AABB */
- void computeAABB();
-
- /** \brief Get node type: a capsule */
- NODE_TYPE getNodeType() const { return GEOM_CAPSULE; }
-};
-
-/** Center at zero cone */
-class Cone : public ShapeBase
-{
-public:
- Cone(BVH_REAL radius_, BVH_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_) {}
-
- /** \brief Radius of the cone */
- BVH_REAL radius;
-
- /** \brief Length along z axis */
- BVH_REAL lz;
-
- /** \brief Compute AABB */
- void computeAABB();
-
- /** \brief Get node type: a cone */
- NODE_TYPE getNodeType() const { return GEOM_CONE; }
-};
-
-/** Center at zero cylinder */
-class Cylinder : public ShapeBase
-{
-public:
- Cylinder(BVH_REAL radius_, BVH_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_) {}
-
- /** \brief Radius of the cylinder */
- BVH_REAL radius;
-
- /** \brief Length along z axis */
- BVH_REAL lz;
-
- /** \brief Compute AABB */
- void computeAABB();
-
- /** \brief Get node type: a cylinder */
- NODE_TYPE getNodeType() const { return GEOM_CYLINDER; }
-};
-
-/** Convex polytope */
-class Convex : public ShapeBase
-{
-public:
- /** Constructing a convex, providing normal and offset of each polytype surface, and the points and shape topology information */
- Convex(Vec3f* plane_normals_,
- BVH_REAL* plane_dis_,
- int num_planes_,
- Vec3f* points_,
- int num_points_,
- int* polygons_) : ShapeBase()
- {
- plane_normals = plane_normals_;
- plane_dis = plane_dis_;
- num_planes = num_planes_;
- points = points_;
- polygons = polygons_;
- edges = NULL;
-
- Vec3f sum;
- for(int i = 0; i < num_points; ++i)
- {
- sum += points[i];
- }
-
- center = sum * (BVH_REAL)(1.0 / num_points);
-
- fillEdges();
- }
-
- /** Copy constructor */
- Convex(const Convex& other) : ShapeBase(other)
- {
- plane_normals = other.plane_normals;
- plane_dis = other.plane_dis;
- num_planes = other.num_planes;
- points = other.points;
- polygons = other.polygons;
- edges = new Edge[other.num_edges];
- memcpy(edges, other.edges, sizeof(Edge) * num_edges);
- }
-
- ~Convex()
- {
- delete [] edges;
- }
-
- /** Compute AABB */
- void computeAABB();
-
- /** Get node type: a conex polytope */
- NODE_TYPE getNodeType() const { return GEOM_CONVEX; }
-
-
- Vec3f* plane_normals;
- BVH_REAL* plane_dis;
-
- /** An array of indices to the points of each polygon, it should be the number of vertices
- * followed by that amount of indices to "points" in counter clockwise order
- */
- int* polygons;
-
- Vec3f* points;
- int num_points;
- int num_edges;
- int num_planes;
-
- struct Edge
- {
- int first, second;
- };
-
- Edge* edges;
-
- /** \brief center of the convex polytope, this is used for collision: center is guaranteed in the internal of the polytope (as it is convex) */
- Vec3f center;
-
-protected:
- /** \brief Get edge information */
- void fillEdges();
-};
-
-/** Infinite plane */
-class Plane : public ShapeBase
-{
-public:
- /** \brief Construct a plane with normal direction and offset */
- Plane(const Vec3f& n_, BVH_REAL d_) : ShapeBase(), n(n_), d(d_) { unitNormalTest(); }
-
- /** \brief Construct a plane with normal direction and offset */
- Plane(BVH_REAL a, BVH_REAL b, BVH_REAL c, BVH_REAL d_)
- {
- n = Vec3f(a, b, c);
- d = d_;
- unitNormalTest();
- }
-
- /** \brief Compute AABB */
- void computeAABB();
-
- /** \brief Get node type: a plane */
- NODE_TYPE getNodeType() const { return GEOM_PLANE; }
-
- /** \brief Plane normal */
- Vec3f n;
-
- /** \brief Plane offset */
- BVH_REAL d;
-
-protected:
-
- /** \brief Turn non-unit normal into unit */
- void unitNormalTest();
-};
-
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/geometric_shapes_intersect.h b/branches/point_cloud/fcl/include/fcl/geometric_shapes_intersect.h
deleted file mode 100644
index 5edc8d9082e74267965bf5b3255b148bfce79106..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/geometric_shapes_intersect.h
+++ /dev/null
@@ -1,278 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#ifndef FCL_GEOMETRIC_SHAPES_INTERSECT_H
-#define FCL_GEOMETRIC_SHAPES_INTERSECT_H
-
-#include "fcl/geometric_shapes.h"
-#include "fcl/transform.h"
-
-#include <ccd/ccd.h>
-#include <ccd/quat.h>
-
-namespace fcl
-{
-
-/** recall function used by GJK algorithm */
-typedef void (*GJKSupportFunction)(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v);
-typedef void (*GJKCenterFunction)(const void* obj, ccd_vec3_t* c);
-
-/** initialize GJK stuffs */
-template<typename T>
-class GJKInitializer
-{
-public:
- /** \brief Get GJK support function */
- static GJKSupportFunction getSupportFunction() { return NULL; }
-
- /** \brief Get GJK center function */
- static GJKCenterFunction getCenterFunction() { return NULL; }
-
- /** \brief Get GJK object from a shape
- * Notice that only local transformation is applied.
- * Gloal transformation are considered later
- */
- static void* createGJKObject(const T& s) { return NULL; }
-
- /** \brief Delete GJK object */
- static void deleteGJKObject(void* o) {}
-};
-
-template<>
-class GJKInitializer<Cylinder>
-{
-public:
- static GJKSupportFunction getSupportFunction();
- static GJKCenterFunction getCenterFunction();
- static void* createGJKObject(const Cylinder& s);
- static void deleteGJKObject(void* o);
-};
-
-
-template<>
-class GJKInitializer<Sphere>
-{
-public:
- static GJKSupportFunction getSupportFunction();
- static GJKCenterFunction getCenterFunction();
- static void* createGJKObject(const Sphere& s);
- static void deleteGJKObject(void* o);
-};
-
-
-template<>
-class GJKInitializer<Box>
-{
-public:
- static GJKSupportFunction getSupportFunction();
- static GJKCenterFunction getCenterFunction();
- static void* createGJKObject(const Box& s);
- static void deleteGJKObject(void* o);
-};
-
-
-template<>
-class GJKInitializer<Capsule>
-{
-public:
- static GJKSupportFunction getSupportFunction();
- static GJKCenterFunction getCenterFunction();
- static void* createGJKObject(const Capsule& s);
- static void deleteGJKObject(void* o);
-};
-
-
-template<>
-class GJKInitializer<Cone>
-{
-public:
- static GJKSupportFunction getSupportFunction();
- static GJKCenterFunction getCenterFunction();
- static void* createGJKObject(const Cone& s);
- static void deleteGJKObject(void* o);
-};
-
-template<>
-class GJKInitializer<Convex>
-{
-public:
- static GJKSupportFunction getSupportFunction();
- static GJKCenterFunction getCenterFunction();
- static void* createGJKObject(const Convex& s);
- static void deleteGJKObject(void* o);
-};
-
-
-GJKSupportFunction triGetSupportFunction();
-
-GJKCenterFunction triGetCenterFunction();
-
-void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3);
-
-void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Vec3f R[3], const Vec3f& T);
-
-void triDeleteGJKObject(void* o);
-
-/** GJK collision algorithm */
-bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
- void* obj2, ccd_support_fn supp2, ccd_center_fn cen2,
- Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal);
-
-void transformGJKObject(void* obj, const Vec3f R[3], const Vec3f& T);
-
-
-/** collision algorithm between two shapes */
-template<typename S1, typename S2>
-bool shapeIntersect(const S1& s1, const S2& s2, Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
-{
- void* o1 = GJKInitializer<S1>::createGJKObject(s1);
- void* o2 = GJKInitializer<S2>::createGJKObject(s2);
-
- return GJKCollide(o1, GJKInitializer<S1>::getSupportFunction(), GJKInitializer<S1>::getCenterFunction(),
- o2, GJKInitializer<S2>::getSupportFunction(), GJKInitializer<S2>::getCenterFunction(),
- contact_points, penetration_depth, normal);
-
- GJKInitializer<S1>::deleteGJKObject(o1);
- GJKInitializer<S2>::deleteGJKObject(o2);
-}
-
-template<typename S1, typename S2>
-bool shapeIntersect(const S1& s1, const S2& s2, const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
- Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
-{
- void* o1 = GJKInitializer<S1>::createGJKObject(s1);
- void* o2 = GJKInitializer<S2>::createGJKObject(s2);
-
- transformGJKObject(o1, R1, T1);
- transformGJKObject(o2, R2, T2);
-
- return GJKCollide(o1, GJKInitializer<S1>::getSupportFunction(), GJKInitializer<S1>::getCenterFunction(),
- o2, GJKInitializer<S2>::getSupportFunction(), GJKInitializer<S2>::getCenterFunction(),
- contact_points, penetration_depth, normal);
-
- GJKInitializer<S1>::deleteGJKObject(o1);
- GJKInitializer<S2>::deleteGJKObject(o2);
-}
-
-template<typename S1, typename S2>
-bool shapeIntersect(const S1& s1, const S2& s2, const Vec3f R[3], const Vec3f& T,
- Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
-{
- void* o1 = GJKInitializer<S1>::createGJKObject(s1);
- void* o2 = GJKInitializer<S2>::createGJKObject(s2);
-
- transformGJKObject(o2, R, T);
-
- return GJKCollide(o1, GJKInitializer<S1>::getSupportFunction(), GJKInitializer<S1>::getCenterFunction(),
- o2, GJKInitializer<S2>::getSupportFunction(), GJKInitializer<S2>::getCenterFunction(),
- contact_points, penetration_depth, normal);
-
- GJKInitializer<S1>::deleteGJKObject(o1);
- GJKInitializer<S2>::deleteGJKObject(o2);
-}
-
-
-template<typename S>
-bool shapeTriangleIntersect(const S& s, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
-{
- void* o1 = GJKInitializer<S>::createGJKObject(s);
- void* o2 = triCreateGJKObject(P1, P2, P3);
-
- return GJKCollide(o1, GJKInitializer<S>::getSupportFunction(), GJKInitializer<S>::getCenterFunction(),
- o2, triGetSupportFunction(), triGetCenterFunction(),
- contact_points, penetration_depth, normal);
-
- GJKInitializer<S>::deleteGJKObject(o1);
- triDeleteGJKObject(o2);
-}
-
-
-template<typename S>
-bool shapeTriangleIntersect(const S& s, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
- Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
-{
- void* o1 = GJKInitializer<S>::createGJKObject(s);
- void* o2 = triCreateGJKObject(P1, P2, P3, R2, T2);
-
- transformGJKObject(o1, R1, T1);
-
- return GJKCollide(o1, GJKInitializer<S>::getSupportFunction(), GJKInitializer<S>::getCenterFunction(),
- o2, triGetSupportFunction(), triGetCenterFunction(),
- contact_points, penetration_depth, normal);
-
- GJKInitializer<S>::deleteGJKObject(o1);
- triDeleteGJKObject(o2);
-}
-
-
-template<typename S>
-bool shapeTriangleIntersect(const S& s, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Vec3f R[3], const Vec3f& T,
- Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
-{
- void* o1 = GJKInitializer<S>::createGJKObject(s);
- void* o2 = triCreateGJKObject(P1, P2, P3, R, T);
-
- return GJKCollide(o1, GJKInitializer<S>::getSupportFunction(), GJKInitializer<S>::getCenterFunction(),
- o2, triGetSupportFunction(), triGetCenterFunction(),
- contact_points, penetration_depth, normal);
-
- GJKInitializer<S>::deleteGJKObject(o1);
- triDeleteGJKObject(o2);
-}
-
-template<typename S>
-bool shapeTriangleIntersect(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const S& s, const Vec3f R[3], const Vec3f& T,
- Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
-{
- void* o1 = triCreateGJKObject(P1, P2, P3);
- void* o2 = GJKInitializer<S>::createGJKObject(s);
- transformGJKObject(o2, R, T);
-
-
- return GJKCollide(o1, triGetSupportFunction(), triGetCenterFunction(),
- o2, GJKInitializer<S>::getSupportFunction(), GJKInitializer<S>::getCenterFunction(),
- contact_points, penetration_depth, normal);
-
- triDeleteGJKObject(o1);
- GJKInitializer<S>::deleteGJKObject(o2);
-}
-
-
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/geometric_shapes_utility.h b/branches/point_cloud/fcl/include/fcl/geometric_shapes_utility.h
deleted file mode 100644
index 166e5fc4f9f37d81af1b2a95822fc2ab6714a156..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/geometric_shapes_utility.h
+++ /dev/null
@@ -1,114 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#ifndef FCL_GEOMETRIC_SHAPES_UTILITY_H
-#define FCL_GEOMETRIC_SHAPES_UTILITY_H
-
-#include "fcl/geometric_shapes.h"
-#include "fcl/BV.h"
-
-namespace fcl
-{
- template<typename BV>
- void computeBV(const Box& s, BV& bv) {}
-
- template<typename BV>
- void computeBV(const Sphere& s, BV& bv) {}
-
- template<typename BV>
- void computeBV(const Capsule& s, BV& bv) {}
-
- template<typename BV>
- void computeBV(const Cone& s, BV& bv) {}
-
- template<typename BV>
- void computeBV(const Cylinder& s, BV& bv) {}
-
- template<typename BV>
- void computeBV(const Convex& s, BV& bv) {}
-
- /** the bounding volume for half space back of plane
- * for OBB, it is the plane itself
- */
- template<typename BV>
- void computeBV(const Plane& s, BV& bv) {}
-
- /** For AABB */
- template<>
- void computeBV<AABB>(const Box& s, AABB& bv);
-
- template<>
- void computeBV<AABB>(const Sphere& s, AABB& bv);
-
- template<>
- void computeBV<AABB>(const Capsule& s, AABB& bv);
-
- template<>
- void computeBV<AABB>(const Cone& s, AABB& bv);
-
- template<>
- void computeBV<AABB>(const Cylinder& s, AABB& bv);
-
- template<>
- void computeBV<AABB>(const Convex& s, AABB& bv);
-
- template<>
- void computeBV<AABB>(const Plane& s, AABB& bv);
-
- template<>
- void computeBV<OBB>(const Box& s, OBB& bv);
-
- template<>
- void computeBV<OBB>(const Sphere& s, OBB& bv);
-
- template<>
- void computeBV<OBB>(const Capsule& s, OBB& bv);
-
- template<>
- void computeBV<OBB>(const Cone& s, OBB& bv);
-
- template<>
- void computeBV<OBB>(const Cylinder& s, OBB& bv);
-
- template<>
- void computeBV<OBB>(const Convex& s, OBB& bv);
-
- template<>
- void computeBV<OBB>(const Plane& s, OBB& bv);
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/intersect.h b/branches/point_cloud/fcl/include/fcl/intersect.h
deleted file mode 100644
index 5b1944d400d323490691f4d2a23b19260d8632aa..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/intersect.h
+++ /dev/null
@@ -1,336 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-#ifndef FCL_INTERSECT_H
-#define FCL_INTERSECT_H
-
-#include "fcl/vec_3f.h"
-#include "fcl/BVH_internal.h"
-#include "fcl/primitive.h"
-
-#if USE_SVMLIGHT
-extern "C"
-{
-# include <svm_light/svm_common.h>
-# include <svm_light/svm_learn.h>
-}
-#endif
-
-/** \brief Main namespace */
-namespace fcl
-{
-
-/** \brief A class solves polynomial degree (1,2,3) equations */
-class PolySolver
-{
-public:
- /** \brief Solve a linear equation with coefficients c, return roots s and number of roots */
- static int solveLinear(BVH_REAL c[2], BVH_REAL s[1]);
-
- /** \brief Solve a quadratic function with coefficients c, return roots s and number of roots */
- static int solveQuadric(BVH_REAL c[3], BVH_REAL s[2]);
-
- /** \brief Solve a cubic function with coefficients c, return roots s and number of roots */
- static int solveCubic(BVH_REAL c[4], BVH_REAL s[3]);
-
-private:
- /** \brief Check whether v is zero */
- static inline bool isZero(BVH_REAL v);
-
- /** \brief Compute v^{1/3} */
- static inline bool cbrt(BVH_REAL v);
-
- static const BVH_REAL NEAR_ZERO_THRESHOLD;
-};
-
-#if USE_SVMLIGHT
-class CloudClassifierParam
-{
-public:
- LEARN_PARM learn_parm;
- KERNEL_PARM kernel_parm;
-
- CloudClassifierParam();
-};
-#endif
-
-
-/** \brief CCD intersect kernel among primitives */
-class Intersect
-{
-
-public:
-
- /** \brief CCD intersect between one vertex and one face
- * [a0, b0, c0] and [a1, b1, c1] are points for the triangle face in time t0 and t1
- * p0 and p1 are points for vertex in time t0 and t1
- * p_i returns the coordinate of the collision point
- */
- static bool intersect_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
- BVH_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
-
- /** \brief CCD intersect between two edges
- * [a0, b0] and [a1, b1] are points for one edge in time t0 and t1
- * [c0, d0] and [c1, d1] are points for the other edge in time t0 and t1
- * p_i returns the coordinate of the collision point
- */
- static bool intersect_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
- BVH_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
-
- /** \brief CCD intersect between one vertex and one face, using additional filter */
- static bool intersect_VF_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
- BVH_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
-
- /** \brief CCD intersect between two edges, using additional filter */
- static bool intersect_EE_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
- BVH_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
-
- /** \brief CCD intersect between one vertex and and one edge */
- static bool intersect_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& p1,
- const Vec3f& L);
-
- /** \brief CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3] */
- static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
- Vec3f* contact_points = NULL,
- unsigned int* num_contact_points = NULL,
- BVH_REAL* penetration_depth = NULL,
- Vec3f* normal = NULL);
-
- static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
- const Vec3f R[3], const Vec3f& T,
- Vec3f* contact_points = NULL,
- unsigned int* num_contact_points = NULL,
- BVH_REAL* penetration_depth = NULL,
- Vec3f* normal = NULL);
-
-#if USE_SVMLIGHT
-
- static BVH_REAL intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
- Vec3f* cloud2, Uncertainty* uc2, int size_cloud2,
- const CloudClassifierParam& solver, bool scaling = true);
-
- static BVH_REAL intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
- Vec3f* cloud2, Uncertainty* uc2, int size_cloud2,
- const Vec3f R[3], const Vec3f& T, const CloudClassifierParam& solver, bool scaling = true);
-
- static BVH_REAL intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
- const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3);
-
- static BVH_REAL intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
- const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
- const Vec3f R[3], const Vec3f& T);
-#endif
-
-private:
-
- /** \brief Project function used in intersect_Triangle() */
- static int project6(const Vec3f& ax,
- const Vec3f& p1, const Vec3f& p2, const Vec3f& p3,
- const Vec3f& q1, const Vec3f& q2, const Vec3f& q3);
-
- /** \brief Check whether one value is zero */
- static inline bool isZero(BVH_REAL v);
-
- /** \brief Solve the cubic function using Newton method, also satisfies the interval restriction */
- static bool solveCubicWithIntervalNewton(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- BVH_REAL& l, BVH_REAL& r, bool bVF, BVH_REAL coeffs[], Vec3f* data = NULL);
-
- /** \brief Check whether one point p is within triangle [a, b, c] */
- static bool insideTriangle(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f&p);
-
- /** \brief Check whether one point p is within a line segment [a, b] */
- static bool insideLineSegment(const Vec3f& a, const Vec3f& b, const Vec3f& p);
-
- /** \brief Calculate the line segment papb that is the shortest route between
- * two lines p1p2 and p3p4. Calculate also the values of mua and mub where
- * pa = p1 + mua (p2 - p1)
- * pb = p3 + mub (p4 - p3)
- * return FALSE if no solution exists.
- */
- static bool linelineIntersect(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const Vec3f& p4,
- Vec3f* pa, Vec3f* pb, BVH_REAL* mua, BVH_REAL* mub);
-
- /** \brief Check whether a root for VF intersection is valid (i.e. within the triangle at intersection t */
- static bool checkRootValidity_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
- BVH_REAL t);
-
- /** \brief Check whether a root for EE intersection is valid (i.e. within the two edges intersected at the given time */
- static bool checkRootValidity_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- BVH_REAL t, Vec3f* q_i = NULL);
-
- /** \brief Check whether a root for VE intersection is valid */
- static bool checkRootValidity_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
- BVH_REAL t);
-
- /** \brief Solve a square function for EE intersection (with interval restriction) */
- static bool solveSquare(BVH_REAL a, BVH_REAL b, BVH_REAL c,
- const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- bool bVF,
- BVH_REAL* ret);
-
- /** \brief Solve a square function for VE intersection (with interval restriction) */
- static bool solveSquare(BVH_REAL a, BVH_REAL b, BVH_REAL c,
- const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vp);
-
- /** \brief Compute the cubic coefficients for VF intersection
- * See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1.
- */
- static void computeCubicCoeff_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
- BVH_REAL* a, BVH_REAL* b, BVH_REAL* c, BVH_REAL* d);
-
- /** \brief Compute the cubic coefficients for EE intersection */
- static void computeCubicCoeff_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- BVH_REAL* a, BVH_REAL* b, BVH_REAL* c, BVH_REAL* d);
-
- /** \brief Compute the cubic coefficients for VE intersection */
- static void computeCubicCoeff_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
- const Vec3f& L,
- BVH_REAL* a, BVH_REAL* b, BVH_REAL* c);
-
- /** \brief filter for intersection, works for both VF and EE */
- static bool intersectPreFiltering(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1);
-
- /** \brief distance of point v to a plane n * x - t = 0 */
- static BVH_REAL distanceToPlane(const Vec3f& n, BVH_REAL t, const Vec3f& v);
-
- /** \brief check wether points v1, v2, v2 are on the same side of plane n * x - t = 0 */
- static bool sameSideOfPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, const Vec3f& n, BVH_REAL t);
-
- /** \brief clip triangle v1, v2, v3 by the prism made by t1, t2 and t3. The normal of the prism is tn and is cutted up by to */
- static void clipTriangleByTriangleAndEdgePlanes(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3,
- const Vec3f& t1, const Vec3f& t2, const Vec3f& t3,
- const Vec3f& tn, BVH_REAL to,
- Vec3f clipped_points[], unsigned int* num_clipped_points, bool clip_triangle = false);
-
- /** \brief build a plane passed through triangle v1 v2 v3 */
- static bool buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, BVH_REAL* t);
-
- /** \brief build a plane pass through edge v1 and v2, normal is tn */
- static bool buildEdgePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& tn, Vec3f* n, BVH_REAL* t);
-
- /** \brief compute the points which has deepest penetration depth */
- static void computeDeepestPoints(Vec3f* clipped_points, unsigned int num_clipped_points, const Vec3f& n, BVH_REAL t, BVH_REAL* penetration_depth, Vec3f* deepest_points, unsigned int* num_deepest_points);
-
- /** \brief clip polygon by plane */
- static void clipPolygonByPlane(Vec3f* polygon_points, unsigned int num_polygon_points, const Vec3f& n, BVH_REAL t, Vec3f clipped_points[], unsigned int* num_clipped_points);
-
- /** \brief clip a line segment by plane */
- static void clipSegmentByPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& n, BVH_REAL t, Vec3f* clipped_point);
-
- /** \brief compute the cdf(x) */
- static BVH_REAL gaussianCDF(BVH_REAL x)
- {
- return 0.5 * erfc(-x / sqrt(2.0));
- }
-
-#if USE_SVMLIGHT
- /** \brief compute the d K(x0, x) / dx, where x is one 3d point on or near the classification surface, and K is a composite kernel */
- static void kernelGradient(KERNEL_PARM *kernel_parm, DOC *a, DOC *b, Vec3f& g);
-
- /** \brief compute the d K(x0, x) / dx, where x is one 3d point on or near the classification surface, and K is a single kernel */
- static void singleKernelGradient(KERNEL_PARM *kernel_parm, SVECTOR *a, SVECTOR *b, Vec3f& g);
-#endif
-
- static const BVH_REAL EPSILON;
- static const BVH_REAL NEAR_ZERO_THRESHOLD;
- static const BVH_REAL CCD_RESOLUTION;
- static const unsigned int MAX_TRIANGLE_CLIPS = 8;
-};
-
-class TriangleDistance
-{
-public:
-
- /** \brief Returns closest points between an segment pair.
- * The first segment is P + t * A
- * The second segment is Q + t * B
- * X, Y are the closest points on the two segments
- * VEC is the vector between X and Y
- */
- static void segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, const Vec3f& B,
- Vec3f& VEC, Vec3f& X, Vec3f& Y);
-
- /** \brief Compute the closest points on two triangles given their absolute coordinate, and returns the distance between them
- * S and T are two triangles
- * If the triangles are disjoint, P and Q give the closet points of S and T respectively. However,
- * if the triangles overlap, P and Q are basically a random pair of points from the triangles, not
- * coincident points on the intersection of the triangles, as might be expected.
- */
- static BVH_REAL triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, Vec3f& Q);
-
- static BVH_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
- const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
- Vec3f& P, Vec3f& Q);
-
- /** \brief Compute the closest points on two triangles given the relative transform between them, and returns the distance between them
- * S and T are two triangles
- * If the triangles are disjoint, P and Q give the closet points of S and T respectively. However,
- * if the triangles overlap, P and Q are basically a random pair of points from the triangles, not
- * coincident points on the intersection of the triangles, as might be expected.
- * The returned P and Q are both in the coordinate of the first triangle's coordinate
- */
- static BVH_REAL triDistance(const Vec3f S[3], const Vec3f T[3],
- const Vec3f R[3], const Vec3f& Tl,
- Vec3f& P, Vec3f& Q);
-
- static BVH_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
- const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
- const Vec3f R[3], const Vec3f& Tl,
- Vec3f& P, Vec3f& Q);
-};
-
-
-}
-
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/interval_tree.h b/branches/point_cloud/fcl/include/fcl/interval_tree.h
deleted file mode 100644
index 90763b07727268d5571212c48a5bd6afc8eb411d..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/interval_tree.h
+++ /dev/null
@@ -1,171 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#ifndef FCL_INTERVAL_TREE_H
-#define FCL_INTERVAL_TREE_H
-
-#include <deque>
-#include <limits>
-
-/** \brief Main namespace */
-namespace fcl
-{
-
-/** \brief Interval trees implemented using red-black-trees as described in
- * the book Introduction_To_Algorithms_ by Cormen, Leisserson, and Rivest.
- * Can be replaced in part by boost::icl::interval_set, which is only supported after boost 1.46 and does not support delete node routine.
- */
-
-struct Interval
-{
-public:
- Interval() {}
- virtual ~Interval() {}
- virtual void print() {}
-
- double low, high;
-};
-
-class IntervalTreeNode
-{
- friend class IntervalTree;
-public:
- /** \brief Print the interval node information: set left = nil and right = root */
- void print(IntervalTreeNode* left, IntervalTreeNode* right) const;
-
- IntervalTreeNode();
-
- IntervalTreeNode(Interval* new_interval);
-
- ~IntervalTreeNode();
-
-protected:
-
- Interval* stored_interval;
-
- double key;
-
- double high;
-
- double max_high;
-
- bool red; /* if red = false then the node is black */
-
- IntervalTreeNode* left;
-
- IntervalTreeNode* right;
-
- IntervalTreeNode* parent;
-};
-
-/** \brief Class describes the information needed when we take the
- * right branch in searching for intervals but possibly come back
- * and check the left branch as well.
- */
-struct it_recursion_node
-{
-public:
- IntervalTreeNode* start_node;
-
- unsigned int parent_index;
-
- bool try_right_branch;
-};
-
-
-/** \brief Interval tree */
-class IntervalTree
-{
-public:
-
- IntervalTree();
-
- ~IntervalTree();
-
- /** \brief Print the whole interval tree */
- void print() const;
-
- /** \brief Delete one node of the interval tree */
- Interval* deleteNode(IntervalTreeNode* node);
-
- /** \brief Insert one node of the interval tree */
- IntervalTreeNode* insert(Interval* new_interval);
-
- /** \brief get the predecessor of a given node */
- IntervalTreeNode* getPredecessor(IntervalTreeNode* node) const;
-
- /** \brief Get the successor of a given node */
- IntervalTreeNode* getSuccessor(IntervalTreeNode* node) const;
-
- /** \brief Return result for a given query */
- std::deque<Interval*> query(double low, double high);
-
-protected:
-
- IntervalTreeNode* root;
-
- IntervalTreeNode* nil;
-
- /** \brief left rotation of tree node */
- void leftRotate(IntervalTreeNode* node);
-
- /** \brief right rotation of tree node */
- void rightRotate(IntervalTreeNode* node);
-
- /** \brief recursively insert a node */
- void recursiveInsert(IntervalTreeNode* node);
-
- /** \brief recursively print a subtree */
- void recursivePrint(IntervalTreeNode* node) const;
-
- /** \brief Travels up to the root fixing the max_high fields after an insertion or deletion */
- void fixupMaxHigh(IntervalTreeNode* node);
-
- void deleteFixup(IntervalTreeNode* node);
-
-private:
- unsigned int recursion_node_stack_size;
- it_recursion_node* recursion_node_stack;
- unsigned int current_parent;
- unsigned int recursion_node_stack_top;
-};
-
-}
-
-#endif
-
-
diff --git a/branches/point_cloud/fcl/include/fcl/kDOP.h b/branches/point_cloud/fcl/include/fcl/kDOP.h
deleted file mode 100644
index 6c59b40fc2698519e0a3ea0deb9876e82e619383..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/kDOP.h
+++ /dev/null
@@ -1,294 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-#ifndef FCL_KDOP_H
-#define FCL_KDOP_H
-
-#include "fcl/BVH_internal.h"
-#include "fcl/vec_3f.h"
-
-#include <cstdlib>
-#include <limits>
-#include <iostream>
-
-/** \brief Main namespace */
-namespace fcl
-{
-
-/** \brief Find the smaller and larger one of two values */
-inline void minmax(BVH_REAL a, BVH_REAL b, BVH_REAL& minv, BVH_REAL& maxv)
-{
- if(a > b)
- {
- minv = b;
- maxv = a;
- }
- else
- {
- minv = a;
- maxv = b;
- }
-}
-/** \brief Merge the interval [minv, maxv] and value p */
-inline void minmax(BVH_REAL p, BVH_REAL& minv, BVH_REAL& maxv)
-{
- if(p > maxv) maxv = p;
- if(p < minv) minv = p;
-}
-
-
-/** \brief Compute the distances to planes with normals from KDOP vectors except those of AABB face planes */
-template<size_t N>
-void getDistances(const Vec3f& p, BVH_REAL d[]) {}
-
-/** \brief Specification of getDistances */
-template<>
-inline void getDistances<5>(const Vec3f& p, BVH_REAL d[])
-{
- d[0] = p[0] + p[1];
- d[1] = p[0] + p[2];
- d[2] = p[1] + p[2];
- d[3] = p[0] - p[1];
- d[4] = p[0] - p[2];
-}
-
-template<>
-inline void getDistances<6>(const Vec3f& p, BVH_REAL d[])
-{
- d[0] = p[0] + p[1];
- d[1] = p[0] + p[2];
- d[2] = p[1] + p[2];
- d[3] = p[0] - p[1];
- d[4] = p[0] - p[2];
- d[5] = p[1] - p[2];
-}
-
-template<>
-inline void getDistances<9>(const Vec3f& p, BVH_REAL d[])
-{
- d[0] = p[0] + p[1];
- d[1] = p[0] + p[2];
- d[2] = p[1] + p[2];
- d[3] = p[0] - p[1];
- d[4] = p[0] - p[2];
- d[5] = p[1] - p[2];
- d[6] = p[0] + p[1] - p[2];
- d[7] = p[0] + p[2] - p[1];
- d[8] = p[1] + p[2] - p[0];
-}
-
-
-/** \brief KDOP class describes the KDOP collision structures. K is set as the template parameter, which should be 16, 18, or 24
- The KDOP structure is defined by some pairs of parallel planes defined by some axes.
- For K = 18, the planes are 6 AABB planes and 12 diagonal planes that cut off some space of the edges:
- (-1,0,0) and (1,0,0) -> indices 0 and 9
- (0,-1,0) and (0,1,0) -> indices 1 and 10
- (0,0,-1) and (0,0,1) -> indices 2 and 11
- (-1,-1,0) and (1,1,0) -> indices 3 and 12
- (-1,0,-1) and (1,0,1) -> indices 4 and 13
- (0,-1,-1) and (0,1,1) -> indices 5 and 14
- (-1,1,0) and (1,-1,0) -> indices 6 and 15
- (-1,0,1) and (1,0,-1) -> indices 7 and 16
- (0,-1,1) and (0,1,-1) -> indices 8 and 17
- */
-template<size_t N>
-class KDOP
-{
-public:
- KDOP()
- {
- BVH_REAL real_max = std::numeric_limits<BVH_REAL>::max();
- for(size_t i = 0; i < N / 2; ++i)
- {
- dist_[i] = real_max;
- dist_[i + N / 2] = -real_max;
- }
- }
-
-
- KDOP(const Vec3f& v)
- {
- for(size_t i = 0; i < 3; ++i)
- {
- dist_[i] = dist_[N / 2 + i] = v[i];
- }
-
- BVH_REAL d[(N - 6) / 2];
- getDistances<(N - 6) / 2>(v, d);
- for(size_t i = 0; i < (N - 6) / 2; ++i)
- {
- dist_[3 + i] = dist_[3 + i + N / 2] = d[i];
- }
- }
-
- KDOP(const Vec3f& a, const Vec3f& b)
- {
- for(size_t i = 0; i < 3; ++i)
- {
- minmax(a[i], b[i], dist_[i], dist_[i + N / 2]);
- }
-
- BVH_REAL ad[(N - 6) / 2], bd[(N - 6) / 2];
- getDistances<(N - 6) / 2>(a, ad);
- getDistances<(N - 6) / 2>(b, bd);
- for(size_t i = 0; i < (N - 6) / 2; ++i)
- {
- minmax(ad[i], bd[i], dist_[3 + i], dist_[3 + i + N / 2]);
- }
- }
-
- /** \brief Check whether two KDOPs are overlapped */
- inline bool overlap(const KDOP<N>& other) const
- {
- for(size_t i = 0; i < N / 2; ++i)
- {
- if(dist_[i] > other.dist_[i + N / 2]) return false;
- if(dist_[i + N / 2] < other.dist_[i]) return false;
- }
-
- return true;
- }
-
- /** \brief Check whether one point is inside the KDOP */
- inline bool inside(const Vec3f& p) const
- {
- for(size_t i = 0; i < 3; ++i)
- {
- if(p[i] < dist_[i] || p[i] > dist_[i + N / 2])
- return false;
- }
-
- BVH_REAL d[(N - 6) / 2];
- getDistances(p, d);
- for(size_t i = 0; i < (N - 6) / 2; ++i)
- {
- if(d[i] < dist_[3 + i] || d[i] > dist_[i + 3 + N / 2])
- return false;
- }
-
- return true;
- }
-
- /** \brief Merge the point and the KDOP */
- inline KDOP<N>& operator += (const Vec3f& p)
- {
- for(size_t i = 0; i < 3; ++i)
- {
- minmax(p[i], dist_[i], dist_[N / 2 + i]);
- }
-
- BVH_REAL pd[(N - 6) / 2];
- getDistances<(N - 6) / 2>(p, pd);
- for(size_t i = 0; i < (N - 6) / 2; ++i)
- {
- minmax(pd[i], dist_[3 + i], dist_[3 + N / 2 + i]);
- }
-
- return *this;
- }
-
- /** \brief Merge two KDOPs */
- inline KDOP<N>& operator += (const KDOP<N>& other)
- {
- for(size_t i = 0; i < N / 2; ++i)
- {
- dist_[i] = std::min(other.dist_[i], dist_[i]);
- dist_[i + N / 2] = std::max(other.dist_[i + N / 2], dist_[i + N / 2]);
- }
- return *this;
- }
-
- /** \brief Create a KDOP by mergin two KDOPs */
- inline KDOP<N> operator + (const KDOP<N>& other) const
- {
- KDOP<N> res(*this);
- return res += other;
- }
-
- /** \brief The (AABB) width */
- inline BVH_REAL width() const
- {
- return dist_[N / 2] - dist_[0];
- }
-
- /** \brief The (AABB) height */
- inline BVH_REAL height() const
- {
- return dist_[N / 2 + 1] - dist_[1];
- }
-
- /** \brief The (AABB) depth */
- inline BVH_REAL depth() const
- {
- return dist_[N / 2 + 2] - dist_[2];
- }
-
- /** \brief The (AABB) volume */
- inline BVH_REAL volume() const
- {
- return width() * height() * depth();
- }
-
- inline BVH_REAL size() const
- {
- return width() * width() + height() * height() + depth() * depth();
- }
-
- /** \brief The (AABB) center */
- inline Vec3f center() const
- {
- return Vec3f(dist_[0] + dist_[N / 2], dist_[1] + dist_[N / 2 + 1], dist_[2] + dist_[N / 2 + 2]) * 0.5;
- }
-
- /** \brief The distance between two KDOP<N>
- * Not implemented.
- */
- BVH_REAL distance(const KDOP<N>& other, Vec3f* P = NULL, Vec3f* Q = NULL) const
- {
- std::cerr << "KDOP distance not implemented!" << std::endl;
- return 0.0;
- }
-
-private:
- /** \brief distances to N KDOP planes */
- BVH_REAL dist_[N];
-
-
-};
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/motion.h b/branches/point_cloud/fcl/include/fcl/motion.h
deleted file mode 100644
index 857f6cdfcfa287ed8f38f584f6aa8efcce84d66d..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/motion.h
+++ /dev/null
@@ -1,210 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#ifndef FCL_MOTION_H
-#define FCL_MOTION_H
-
-#include "fcl/vec_3f.h"
-#include "fcl/RSS.h"
-#include "fcl/transform.h"
-#include "fcl/motion_base.h"
-
-namespace fcl
-{
-
-/** \brief Linear interpolation motion
- * Each Motion is assumed to have constant linear velocity and angular velocity
- */
-template<typename BV>
-class InterpMotion : public MotionBase<BV>
-{
-public:
- /** Default transformations are all identities */
- InterpMotion()
- {
- /** Default angular velocity is zero */
- angular_axis = Vec3f(1, 0, 0);
- angular_vel = 0;
-
- /** Default linear velocity is zero */
- }
-
- /** \brief Construct motion from the initial rotation/translation and goal rotation/translation */
- InterpMotion(const Vec3f R1[3], const Vec3f& T1,
- const Vec3f R2[3], const Vec3f& T2)
- {
- t1 = SimpleTransform(R1, T1);
- t2 = SimpleTransform(R2, T2);
-
- /** Current time is zero, so the transformation is t1 */
- t = t1;
-
- /** Compute the velocities for the motion */
- computeVelocity();
- }
-
- /** \brief Construct motion from the initial rotation/translation and goal rotation/translation related to some rotation center
- */
- InterpMotion(const Vec3f R1[3], const Vec3f& T1,
- const Vec3f R2[3], const Vec3f& T2,
- const Vec3f& O)
- {
- t1 = SimpleTransform(R1, T1 - MxV(R1, O));
- t2 = SimpleTransform(R2, T2 - MxV(R2, O));
- t = t1;
-
- /** Compute the velocities for the motion */
- computeVelocity();
- }
-
-
- /** \brief Integrate the motion from 0 to dt
- * We compute the current transformation from zero point instead of from last integrate time, for precision.
- */
- bool integrate(double dt)
- {
- if(dt > 1) dt = 1;
-
- t.T = t1.T + linear_vel * dt;
-
- t.q = absoluteRotation(dt);
- t.q.toRotation(t.R);
-
- return true;
- }
-
- /** \brief Compute the motion bound for a bounding volume, given the closest direction n between two query objects
- * according to mu < |v * n| + ||w x n||(r + max(||ci*||)) where ||ci*|| = ||ci x w||. w is the angular axis (normalized)
- * and ci are the endpoints of the generator primitives of RSS.
- * Notice that all bv parameters are in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame)
- */
- BVH_REAL computeMotionBound(const BV& bv, const Vec3f& n) const { return 0.0; }
-
- /** \brief Compute the motion bound for a triangle, given the closest direction n between two query objects
- * according to mu < |v * | + ||w x n||(max||ci*||) where ||ci*|| = ||ci x w||. w is the angular axis (normalized)
- * and ci are the triangle vertex coordinates.
- * Notice that the triangle is in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame)
- */
- BVH_REAL computeMotionBound(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) const
- {
- BVH_REAL c_proj_max = (a.cross(angular_axis)).sqrLength();
- BVH_REAL tmp;
- tmp = (b.cross(angular_axis)).sqrLength();
- if(tmp > c_proj_max) c_proj_max = tmp;
- tmp = (c.cross(angular_axis)).sqrLength();
- if(tmp > c_proj_max) c_proj_max = tmp;
-
- c_proj_max = sqrt(c_proj_max);
-
- BVH_REAL v_dot_n = linear_vel.dot(n);
- BVH_REAL w_cross_n = (angular_axis.cross(n)).length() * angular_vel;
- BVH_REAL mu = v_dot_n + w_cross_n * c_proj_max;
-
- return mu;
- }
-
- /** \brief Get the rotation and translation in current step */
- void getCurrentTransformation(Vec3f R[3], Vec3f& T) const
- {
- for(int i = 0; i < 3; ++i)
- {
- R[i] = t.R[i];
- }
-
- T = t.T;
- }
-
- void getCurrentRotation(Vec3f R[3]) const
- {
- for(int i = 0; i < 3; ++i)
- R[i] = t.R[i];
- }
-
- void getCurrentTranslation(Vec3f& T) const
- {
- T = t.T;
- }
-
-protected:
-
- void computeVelocity()
- {
- linear_vel = t2.T - t1.T;
- SimpleQuaternion deltaq = t2.q * t1.q.inverse();
- deltaq.toAxisAngle(angular_axis, angular_vel);
- }
-
-
- SimpleQuaternion deltaRotation(BVH_REAL t) const
- {
- SimpleQuaternion res;
- res.fromAxisAngle(angular_axis, (BVH_REAL)(t * angular_vel));
- return res;
- }
-
- SimpleQuaternion absoluteRotation(BVH_REAL t) const
- {
- SimpleQuaternion delta_t = deltaRotation(t);
- return delta_t * t1.q;
- }
-
- /** \brief The transformation at time 0 */
- SimpleTransform t1;
-
- /** \brief The transformation at time 1 */
- SimpleTransform t2;
-
- /** \brief The transformation at current time t */
- SimpleTransform t;
-
- /** \brief Linear velocity */
- Vec3f linear_vel;
-
- /** \brief Angular speed */
- BVH_REAL angular_vel;
-
- /** \brief Angular velocity axis */
- Vec3f angular_axis;
-};
-
-template<>
-BVH_REAL InterpMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const;
-
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/motion_base.h b/branches/point_cloud/fcl/include/fcl/motion_base.h
deleted file mode 100644
index 0771dbd5b4c990429b2dfbc807a29b64e38db24a..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/motion_base.h
+++ /dev/null
@@ -1,72 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#ifndef FCL_MOTION_BASE_H
-#define FCL_MOTION_BASE_H
-
-#include "fcl/vec_3f.h"
-#include "fcl/RSS.h"
-namespace fcl
-{
-
-template<typename BV>
-class MotionBase
-{
-public:
- virtual ~MotionBase() {}
-
- /** \brief Integrate the motion from 0 to dt */
- virtual bool integrate(double dt) = 0;
-
- /** \brief Compute the motion bound for a bounding volume, given the closest direction n between two query objects */
- virtual BVH_REAL computeMotionBound(const BV& bv, const Vec3f& n) const = 0;
-
- /** \brief Compute the motion bound for a triangle, given the closest direction n between two query objects */
- virtual BVH_REAL computeMotionBound(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) const = 0;
-
- /** \brief Get the rotation and translation in current step */
- virtual void getCurrentTransformation(Vec3f R[3], Vec3f& T) const = 0;
-
- virtual void getCurrentRotation(Vec3f R[3]) const = 0;
-
- virtual void getCurrentTranslation(Vec3f& T) const = 0;
-};
-
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/primitive.h b/branches/point_cloud/fcl/include/fcl/primitive.h
deleted file mode 100644
index 85ab7f72af96027e7e4d50ae92da3685b63a4746..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/primitive.h
+++ /dev/null
@@ -1,167 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-#ifndef COLLISION_CHECKING_PRIMITIVE_H
-#define COLLISION_CHECKING_PRIMITIVE_H
-
-#include "fcl/BVH_internal.h"
-#include "fcl/vec_3f.h"
-
-/** \brief Main namespace */
-namespace fcl
-{
-
-/** \brief Uncertainty information */
-struct Uncertainty
-{
- Uncertainty() {}
-
- Uncertainty(Vec3f Sigma_[3])
- {
- for(int i = 0; i < 3; ++i)
- Sigma[i] = Sigma_[i];
- preprocess();
- }
-
- /** preprocess performs the eigen decomposition on the Sigma matrix */
- void preprocess()
- {
- Meigen(Sigma, sigma, axis);
- }
-
- /** sqrt performs the sqrt of Sigma matrix based on the eigen decomposition result, this is useful when the uncertainty matrix is initialized
- * as a square variation matrix
- */
- void sqrt()
- {
- for(int i = 0; i < 3; ++i)
- {
- if(sigma[i] < 0) sigma[i] = 0;
- sigma[i] = std::sqrt(sigma[i]);
- }
-
-
- for(int i = 0; i < 3; ++i)
- {
- for(int j = 0; j < 3; ++j)
- {
- Sigma[i][j] = 0;
- }
- }
-
- for(int i = 0; i < 3; ++i)
- {
- for(int j = 0; j < 3; ++j)
- {
- Sigma[i][j] += sigma[0] * axis[0][i] * axis[0][j];
- Sigma[i][j] += sigma[1] * axis[1][i] * axis[1][j];
- Sigma[i][j] += sigma[2] * axis[2][i] * axis[2][j];
- }
- }
- }
-
- /** \brief Variation matrix for uncertainty */
- Vec3f Sigma[3];
-
- /** \brief Variations along the eigen axes */
- BVH_REAL sigma[3];
-
- /** \brief eigen axes of uncertainty matrix */
- Vec3f axis[3];
-};
-
-/** \brief Simple triangle with 3 indices for points */
-struct Triangle
-{
- unsigned int vids[3];
-
- Triangle() {}
-
- Triangle(unsigned int p1, unsigned int p2, unsigned int p3)
- {
- set(p1, p2, p3);
- }
-
- inline void set(unsigned int p1, unsigned int p2, unsigned int p3)
- {
- vids[0] = p1; vids[1] = p2; vids[2] = p3;
- }
-
- inline unsigned int operator[](int i) const { return vids[i]; }
-
- inline unsigned int& operator[](int i) { return vids[i]; }
-};
-
-
-/** \brief Simple edge with two indices for its endpoints */
-struct Edge
-{
- unsigned int vids[2];
- unsigned int fids[2];
-
- Edge()
- {
- vids[0] = -1; vids[1] = -1;
- fids[0] = -1; fids[1] = -1;
- }
-
- Edge(unsigned int vid0, unsigned int vid1, unsigned int fid)
- {
- vids[0] = vid0;
- vids[1] = vid1;
- fids[0] = fid;
- }
-
- /** \brief Whether two edges are the same, assuming belongs to the same object */
- bool operator == (const Edge& other) const
- {
- return (vids[0] == other.vids[0]) && (vids[1] == other.vids[1]);
- }
-
- bool operator < (const Edge& other) const
- {
- if(vids[0] == other.vids[0])
- return vids[1] < other.vids[1];
-
- return vids[0] < other.vids[0];
- }
-
-};
-
-}
-
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/simple_setup.h b/branches/point_cloud/fcl/include/fcl/simple_setup.h
deleted file mode 100644
index 4339a589ec0d947a9100876f4f98a0d1840cbca9..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/simple_setup.h
+++ /dev/null
@@ -1,727 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#ifndef FCL_SIMPLE_SETUP_H
-#define FCL_SIMPLE_SETUP_H
-
-#include "fcl/traversal_node_bvhs.h"
-#include "fcl/traversal_node_shapes.h"
-#include "fcl/traversal_node_bvh_shape.h"
-#include "fcl/BVH_utility.h"
-
-/** \brief Main namespace */
-namespace fcl
-{
-
-/** \brief Initialize traversal node for collision between two geometric shapes */
-template<typename S1, typename S2>
-bool initialize(ShapeCollisionTraversalNode<S1, S2>& node, const S1& shape1, const S2& shape2, bool enable_contact = false)
-{
- node.enable_contact = enable_contact;
- return true;
-}
-
-/** \brief Initialize traversal node for collision between two geometric shapes, given the current transforms of the two shapes */
-template<typename S1, typename S2>
-bool initialize(ShapeCollisionTraversalNode<S1, S2>& node, const S1& shape1, const S2& shape2,
- const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
- bool enable_contact = false)
-{
- for(int i = 0; i < 3; ++i)
- {
- node.R1[i] = R1[i];
- node.R2[i] = R2[i];
- }
- node.T1 = T1;
- node.T2 = T2;
- node.enable_contact = enable_contact;
-
- return true;
-}
-
-/** \brief Initialize traversal node for collision between one mesh and one shape */
-template<typename BV, typename S>
-bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node, const BVHModel<BV>& model1, const S& model2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
-{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.vertices = model1.vertices;
- node.tri_indices = model1.tri_indices;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
-
- return true;
-}
-
-/** \brief Initialize traversal node for collision between one mesh and one shape, given current object transform */
-template<typename BV, typename S>
-bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node, BVHModel<BV>& model1, S& model2,
- const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
- int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false,
- bool use_refit = false, bool refit_bottomup = false)
-{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
- for(int i = 0; i < model1.num_vertices; ++i)
- {
- Vec3f& p = model1.vertices[i];
- Vec3f new_v = MxV(R1, p) + T1;
- vertices_transformed1[i] = new_v;
- }
-
- model1.beginReplaceModel();
- model1.replaceSubModel(vertices_transformed1);
- model1.endReplaceModel(use_refit, refit_bottomup);
-
- model2.appendLocalTransform(R2, T2);
-
- node.vertices = model1.vertices;
- node.tri_indices = model1.tri_indices;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
-
- return true;
-}
-
-
-/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type */
-template<typename S>
-bool initialize(MeshShapeCollisionTraversalNodeOBB<S>& node, const BVHModel<OBB>& model1, const S& model2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
-{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.vertices = model1.vertices;
- node.tri_indices = model1.tri_indices;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
-
- return true;
-}
-
-/** \brief Initialize traversal node for collision between one mesh and one shape, given the current transform, specialized for OBB type */
-template<typename S>
-bool initialize(MeshShapeCollisionTraversalNodeOBB<S>& node, const BVHModel<OBB>& model1, S& model2,
- const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
- int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
-{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- model2.appendLocalTransform(R2, T2);
-
- node.vertices = model1.vertices;
- node.tri_indices = model1.tri_indices;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
- node.R[0] = R1[0]; node.R[1] = R1[1]; node.R[2] = R1[2];
- node.T = T1;
-
- return true;
-}
-
-/** \brief Initialize traversal node for collision between two meshes */
-template<typename BV>
-bool initialize(MeshCollisionTraversalNode<BV>& node, const BVHModel<BV>& model1, const BVHModel<BV>& model2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
-{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices1 = model1.tri_indices;
- node.tri_indices2 = model2.tri_indices;
-
- node.enable_contact = enable_contact;
- node.exhaustive = exhaustive;
- node.num_max_contacts = num_max_contacts;
-
- return true;
-}
-
-/** \brief Initialize traversal node for collision between two meshes, given the current transforms */
-template<typename BV>
-bool initialize(MeshCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHModel<BV>& model2,
- const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false,
- bool use_refit = false, bool refit_bottomup = false)
-{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
- for(int i = 0; i < model1.num_vertices; ++i)
- {
- Vec3f& p = model1.vertices[i];
- Vec3f new_v = MxV(R1, p) + T1;
- vertices_transformed1[i] = new_v;
- }
-
-
- std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
- for(int i = 0; i < model2.num_vertices; ++i)
- {
- Vec3f& p = model2.vertices[i];
- Vec3f new_v = MxV(R2, p) + T2;
- vertices_transformed2[i] = new_v;
- }
-
- model1.beginReplaceModel();
- model1.replaceSubModel(vertices_transformed1);
- model1.endReplaceModel(use_refit, refit_bottomup);
- model2.beginReplaceModel();
- model2.replaceSubModel(vertices_transformed2);
- model2.endReplaceModel(use_refit, refit_bottomup);
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices1 = model1.tri_indices;
- node.tri_indices2 = model2.tri_indices;
-
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
-
- return true;
-}
-
-
-/** \brief Initialize traversal node for collision between two meshes, specialized for OBB type */
-bool initialize(MeshCollisionTraversalNodeOBB& node, const BVHModel<OBB>& model1, const BVHModel<OBB>& model2,
- const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false);
-
-bool initialize(MeshCollisionTraversalNodeRSS& node, const BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
- const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false);
-
-
-#if USE_SVMLIGHT
-
-/** \brief Initialize traversal node for collision between two point clouds */
-template<typename BV>
-bool initialize(PointCloudCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHModel<BV>& model2,
- BVH_REAL collision_prob_threshold = 0.5,
- BVH_REAL max_collision_prob = 0,
- int leaf_size_threshold = 1,
- int num_max_contacts = 1,
- bool exhaustive = false,
- bool enable_contact = false,
- BVH_REAL expand_r = 1)
-{
- if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD)
- || !(model2.getModelType() == BVH_MODEL_TRIANGLES || model2.getModelType() == BVH_MODEL_POINTCLOUD))
- return false;
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.uc1.reset(new Uncertainty[model1.num_vertices]);
- node.uc2.reset(new Uncertainty[model2.num_vertices]);
-
- estimateSamplingUncertainty(model1.vertices, model1.num_vertices, node.uc1);
- estimateSamplingUncertainty(model2.vertices, model2.num_vertices, node.uc2);
-
- BVHExpand(model1, node.uc1.get(), expand_r);
- BVHExpand(model2, node.uc2.get(), expand_r);
-
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
- node.collision_prob_threshold = collision_prob_threshold;
- node.max_collision_prob = max_collision_prob;
- node.leaf_size_threshold = leaf_size_threshold;
-
- return true;
-}
-
-/** \brief Initialize traversal node for collision between two point clouds, given current transforms */
-template<typename BV, bool use_refit, bool refit_bottomup>
-bool initialize(PointCloudCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHModel<BV>& model2,
- const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
- BVH_REAL collision_prob_threshold = 0.5,
- int leaf_size_threshold = 1,
- int num_max_contacts = 1,
- bool exhaustive = false,
- bool enable_contact = false,
- BVH_REAL expand_r = 1)
-{
- if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD)
- || !(model2.getModelType() == BVH_MODEL_TRIANGLES || model2.getModelType() == BVH_MODEL_POINTCLOUD))
- return false;
-
- std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
- for(int i = 0; i < model1.num_vertices; ++i)
- {
- Vec3f& p = model1.vertices[i];
- Vec3f new_v = MxV(R1, p) + T1;
- vertices_transformed1[i] = new_v;
- }
-
-
- std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
- for(int i = 0; i < model2.num_vertices; ++i)
- {
- Vec3f& p = model2.vertices[i];
- Vec3f new_v = MxV(R2, p) + T2;
- vertices_transformed2[i] = new_v;
- }
-
- model1.beginReplaceModel();
- model1.replaceSubModel(vertices_transformed1);
- model1.endReplaceModel(use_refit, refit_bottomup);
-
- model2.beginReplaceModel();
- model2.replaceSubModel(vertices_transformed2);
- model2.endReplaceModel(use_refit, refit_bottomup);
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.uc1.reset(new Uncertainty[model1.num_vertices]);
- node.uc2.reset(new Uncertainty[model2.num_vertices]);
-
- estimateSamplingUncertainty(model1.vertices, model1.num_vertices, node.uc1.get());
- estimateSamplingUncertainty(model2.vertices, model2.num_vertices, node.uc2.get());
-
- BVHExpand(model1, node.uc1.get(), expand_r);
- BVHExpand(model2, node.uc2.get(), expand_r);
-
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
- node.collision_prob_threshold = collision_prob_threshold;
- node.leaf_size_threshold = leaf_size_threshold;
-
- return true;
-}
-
-/** \brief Initialize traversal node for collision between two point clouds, given current transforms, specialized for OBB type */
-bool initialize(PointCloudCollisionTraversalNodeOBB& node, BVHModel<OBB>& model1, BVHModel<OBB>& model2,
- const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
- BVH_REAL collision_prob_threshold = 0.5,
- int leaf_size_threshold = 1,
- int num_max_contacts = 1,
- bool exhaustive = false,
- bool enable_contact = false,
- BVH_REAL expand_r = 1);
-
-/** \brief Initialize traversal node for collision between two point clouds, given current transforms, specialized for RSS type */
-bool initialize(PointCloudCollisionTraversalNodeRSS& node, BVHModel<RSS>& model1, BVHModel<RSS>& model2,
- const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
- BVH_REAL collision_prob_threshold = 0.5,
- int leaf_size_threshold = 1,
- int num_max_contacts = 1,
- bool exhaustive = false,
- bool enable_contact = false,
- BVH_REAL expand_r = 1);
-
-/** \brief Initialize traversal node for collision between one point cloud and one mesh */
-template<typename BV>
-bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, const BVHModel<BV>& model2,
- BVH_REAL collision_prob_threshold = 0.5,
- int leaf_size_threshold = 1,
- int num_max_contacts = 1,
- bool exhaustive = false,
- bool enable_contact = false,
- BVH_REAL expand_r = 1)
-{
- if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD) || model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices2 = model2.tri_indices;
- node.uc1.reset(new Uncertainty[model1.num_vertices]);
-
- estimateSamplingUncertainty(model1.vertices, model1.num_vertices, node.uc1.get());
-
- BVHExpand(model1, node.uc1.get(), expand_r);
-
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
- node.collision_prob_threshold = collision_prob_threshold;
- node.leaf_size_threshold = leaf_size_threshold;
-
- return true;
-}
-
-/** \brief Initialize traversal node for collision between one point cloud and one mesh, given current transforms */
-template<typename BV, bool use_refit, bool refit_bottomup>
-bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHModel<BV>& model2,
- const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
- BVH_REAL collision_prob_threshold = 0.5,
- int leaf_size_threshold = 1,
- int num_max_contacts = 1,
- bool exhaustive = false,
- bool enable_contact = false,
- BVH_REAL expand_r = 1)
-{
- if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD) || model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
- for(int i = 0; i < model1.num_vertices; ++i)
- {
- Vec3f& p = model1.vertices[i];
- Vec3f new_v = MxV(R1, p) + T1;
- vertices_transformed1[i] = new_v;
- }
-
-
- std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
- for(int i = 0; i < model2.num_vertices; ++i)
- {
- Vec3f& p = model2.vertices[i];
- Vec3f new_v = MxV(R2, p) + T2;
- vertices_transformed2[i] = new_v;
- }
-
- model1.beginReplaceModel();
- model1.replaceSubModel(vertices_transformed1);
- model1.endReplaceModel(use_refit, refit_bottomup);
-
- model2.beginReplaceModel();
- model2.replaceSubModel(vertices_transformed2);
- model2.endReplaceModel(use_refit, refit_bottomup);
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices2 = model2.tri_indices;
- node.uc1.reset(new Uncertainty[model1.num_vertices]);
-
- estimateSamplingUncertainty(model1.vertices, model1.num_vertices, node.uc1.get());
-
- BVHExpand(model1, node.uc1.get(), expand_r);
-
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
- node.collision_prob_threshold = collision_prob_threshold;
- node.leaf_size_threshold = leaf_size_threshold;
-
- return true;
-}
-
-
-/** \brief Initialize traversal node for collision between one point cloud and one mesh, given current transforms, specialized for OBB type */
-bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node, BVHModel<OBB>& model1, const BVHModel<OBB>& model2,
- const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
- BVH_REAL collision_prob_threshold = 0.5,
- int leaf_size_threshold = 1,
- int num_max_contacts = 1,
- bool exhaustive = false,
- bool enable_contact = false,
- BVH_REAL expand_r = 1);
-
-/** \brief Initialize traversal node for collision between one point cloud and one mesh, given current transforms, specialized for RSS type */
-bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node, BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
- const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
- BVH_REAL collision_prob_threshold = 0.5,
- int leaf_size_threshold = 1,
- int num_max_contacts = 1,
- bool exhaustive = false,
- bool enable_contact = false,
- BVH_REAL expand_r = 1);
-
-#endif
-
-/** \brief Initialize traversal node for continuous collision detection between two meshes */
-template<typename BV>
-bool initialize(MeshContinuousCollisionTraversalNode<BV>& node, const BVHModel<BV>& model1, const BVHModel<BV>& model2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
-{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices1 = model1.tri_indices;
- node.tri_indices2 = model2.tri_indices;
-
- node.prev_vertices1 = model1.prev_vertices;
- node.prev_vertices2 = model2.prev_vertices;
-
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
-
- return true;
-}
-
-/** \brief Initialize traversal node for continuous collision detection between one mesh and one point cloud */
-template<typename BV>
-bool initialize(MeshPointCloudContinuousCollisionTraversalNode<BV>& node, const BVHModel<BV>& model1, const BVHModel<BV>& model2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
-{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES || !(model2.getModelType() == BVH_MODEL_TRIANGLES || model2.getModelType() == BVH_MODEL_POINTCLOUD))
- return false;
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices1 = model1.tri_indices;
- node.prev_vertices1 = model1.prev_vertices;
- node.prev_vertices2 = model2.prev_vertices;
-
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
-
- return true;
-}
-
-/** \brief Initialize traversal node for continuous collision detection between one point cloud and one mesh */
-template<typename BV>
-bool initialize(PointCloudMeshContinuousCollisionTraversalNode<BV>& node, const BVHModel<BV>& model1, const BVHModel<BV>& model2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
-{
- if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD) || model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices2 = model2.tri_indices;
- node.prev_vertices1 = model1.prev_vertices;
- node.prev_vertices2 = model2.prev_vertices;
-
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
-
- return true;
-}
-
-/** \brief Initialize traversal node for distance computation between two meshes */
-template<typename BV>
-bool initialize(MeshDistanceTraversalNode<BV>& node, const BVHModel<BV>& model1, const BVHModel<BV>& model2)
-{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices1 = model1.tri_indices;
- node.tri_indices2 = model2.tri_indices;
-
- return true;
-}
-
-
-/** \brief Initialize traversal node for distance computation between two meshes, given the current transforms */
-template<typename BV>
-bool initialize(MeshDistanceTraversalNode<BV>& node, BVHModel<BV>& model1, BVHModel<BV>& model2,
- const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
- bool use_refit = false, bool refit_bottomup = false)
-{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
- for(int i = 0; i < model1.num_vertices; ++i)
- {
- Vec3f& p = model1.vertices[i];
- Vec3f new_v = MxV(R1, p) + T1;
- vertices_transformed1[i] = new_v;
- }
-
-
- std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
- for(int i = 0; i < model2.num_vertices; ++i)
- {
- Vec3f& p = model2.vertices[i];
- Vec3f new_v = MxV(R2, p) + T2;
- vertices_transformed2[i] = new_v;
- }
-
- model1.beginReplaceModel();
- model1.replaceSubModel(vertices_transformed1);
- model1.endReplaceModel(use_refit, refit_bottomup);
-
- model2.beginReplaceModel();
- model2.replaceSubModel(vertices_transformed2);
- model2.endReplaceModel(use_refit, refit_bottomup);
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices1 = model1.tri_indices;
- node.tri_indices2 = model2.tri_indices;
-
- return true;
-}
-
-
-/** \brief Initialize traversal node for distance computation between two meshes, given the current transforms */
-bool initialize(MeshDistanceTraversalNodeRSS& node, const BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
- const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2);
-
-/** \brief Initialize traversal node for conservative advancement computation between two meshes, given the current transforms */
-template<typename BV>
-bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node, BVHModel<BV>& model1, BVHModel<BV>& model2,
- const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2, BVH_REAL w = 1,
- bool use_refit = false, bool refit_bottomup = false)
-{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
- for(int i = 0; i < model1.num_vertices; ++i)
- {
- Vec3f& p = model1.vertices[i];
- Vec3f new_v = MxV(R1, p) + T1;
- vertices_transformed1[i] = new_v;
- }
-
-
- std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
- for(int i = 0; i < model2.num_vertices; ++i)
- {
- Vec3f& p = model2.vertices[i];
- Vec3f new_v = MxV(R2, p) + T2;
- vertices_transformed2[i] = new_v;
- }
-
- model1.beginReplaceModel();
- model1.replaceSubModel(vertices_transformed1);
- model1.endReplaceModel(use_refit, refit_bottomup);
-
- model2.beginReplaceModel();
- model2.replaceSubModel(vertices_transformed2);
- model2.endReplaceModel(use_refit, refit_bottomup);
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices1 = model1.tri_indices;
- node.tri_indices2 = model2.tri_indices;
-
- node.w = w;
-
- //HOW?
- //node.motion1 = new InterpMotion<BV>
- //node.motion2 = new InterpMotion<BV>
-
- return true;
-}
-
-
-/** \brief Initialize traversal node for conservative advancement computation between two meshes, given the current transforms, specialized for RSS */
-inline bool initialize(MeshConservativeAdvancementTraversalNodeRSS& node, const BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
- const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2, BVH_REAL w = 1)
-{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices1 = model1.tri_indices;
- node.tri_indices2 = model2.tri_indices;
-
- node.w = w;
-
- relativeTransform(R1, T1, R2, T2, node.R, node.T);
-
-
-
- return true;
-}
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/transform.h b/branches/point_cloud/fcl/include/fcl/transform.h
deleted file mode 100644
index 6e406c8d3caa719e3f255949cb0ef0d401f146af..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/transform.h
+++ /dev/null
@@ -1,206 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#ifndef FCL_TRANSFORM_H
-#define FCL_TRANSFORM_H
-
-#include "fcl/vec_3f.h"
-
-namespace fcl
-{
-
-/** \brief Quaternion used locally by InterpMotion */
-class SimpleQuaternion
-{
-public:
- /** \brief Default quaternion is identity rotation */
- SimpleQuaternion()
- {
- data[0] = 1;
- data[1] = 0;
- data[2] = 0;
- data[3] = 0;
- }
-
- SimpleQuaternion(BVH_REAL a, BVH_REAL b, BVH_REAL c, BVH_REAL d)
- {
- data[0] = a; // w
- data[1] = b; // x
- data[2] = c; // y
- data[3] = d; // z
- }
-
- /** \brief Matrix to quaternion */
- void fromRotation(const Vec3f R[3]);
-
- /** \brief Quaternion to matrix */
- void toRotation(Vec3f R[3]) const;
-
- /** \brief Axes to quaternion */
- void fromAxes(const Vec3f axis[3]);
-
- /** \brief Axes to matrix */
- void toAxes(Vec3f axis[3]) const;
-
- /** \brief Axis and angle to quaternion */
- void fromAxisAngle(const Vec3f& axis, BVH_REAL angle);
-
- /** \brief Quaternion to axis and angle */
- void toAxisAngle(Vec3f& axis, BVH_REAL& angle) const;
-
- /** \brief Dot product between quaternions */
- BVH_REAL dot(const SimpleQuaternion& other) const;
-
- /** \brief addition */
- SimpleQuaternion operator + (const SimpleQuaternion& other) const;
-
- /** \brief minus */
- SimpleQuaternion operator - (const SimpleQuaternion& other) const;
-
- /** \brief multiplication */
- SimpleQuaternion operator * (const SimpleQuaternion& other) const;
-
- /** \brief division */
- SimpleQuaternion operator - () const;
-
- /** \brief scalar multiplication */
- SimpleQuaternion operator * (BVH_REAL t) const;
-
- /** \brief conjugate */
- SimpleQuaternion conj() const;
-
- /** \brief inverse */
- SimpleQuaternion inverse() const;
-
- /** \brief rotate a vector */
- Vec3f transform(const Vec3f& v) const;
-
- inline const BVH_REAL& getW() const { return data[0]; }
- inline const BVH_REAL& getX() const { return data[1]; }
- inline const BVH_REAL& getY() const { return data[2]; }
- inline const BVH_REAL& getZ() const { return data[3]; }
-
- inline BVH_REAL& getW() { return data[0]; }
- inline BVH_REAL& getX() { return data[1]; }
- inline BVH_REAL& getY() { return data[2]; }
- inline BVH_REAL& getZ() { return data[3]; }
-
-private:
-
- BVH_REAL data[4];
-};
-
-/** \brief Simple transform class used locally by InterpMotion */
-class SimpleTransform
-{
- /** \brief Rotation matrix and translation vector */
- Vec3f R[3];
- Vec3f T;
-
- /** \brief Quaternion representation for R */
- SimpleQuaternion q;
-
-public:
-
- /** \brief Default transform is no movement */
- SimpleTransform()
- {
- R[0][0] = 1; R[1][1] = 1; R[2][2] = 1;
- }
-
- SimpleTransform(const Vec3f R_[3], const Vec3f& T_)
- {
- for(int i = 0; i < 3; ++i)
- R[i] = R_[i];
- T = T_;
-
- q.fromRotation(R_);
- }
-
- inline const Vec3f& getTranslation() const
- {
- return T;
- }
-
- inline const Vec3f* getRotation() const
- {
- return R;
- }
-
- inline const SimpleQuaternion& getQuatRotation() const
- {
- return q;
- }
-
- inline void setTransform(const Vec3f R_[3], const Vec3f& T_)
- {
- for(int i = 0; i < 3; ++i)
- R[i] = R_[i];
- T = T_;
-
- q.fromRotation(R_);
- }
-
- inline void setRotation(const Vec3f R_[3])
- {
- for(int i = 0; i < 3; ++i)
- R[i] = R_[i];
- q.fromRotation(R_);
- }
-
- inline void setTranslation(const Vec3f& T_)
- {
- T = T_;
- }
-
- inline void setQuatRotation(const SimpleQuaternion& q_)
- {
- q = q_;
- q.toRotation(R);
- }
-
- Vec3f transform(const Vec3f& v) const
- {
- return q.transform(v) + T;
- }
-
-};
-
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/traversal_node_base.h b/branches/point_cloud/fcl/include/fcl/traversal_node_base.h
deleted file mode 100644
index 4e07274f767066fcaecd8120287bfa31e35149de..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/traversal_node_base.h
+++ /dev/null
@@ -1,117 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-#ifndef FCL_TRAVERSAL_NODE_BASE_H
-#define FCL_TRAVERSAL_NODE_BASE_H
-
-#include "fcl/vec_3f.h"
-#include "fcl/primitive.h"
-#include "fcl/BVH_front.h"
-#include "fcl/BVH_model.h"
-#include <vector>
-
-
-/** \brief Main namespace */
-namespace fcl
-{
-
-class TraversalNodeBase
-{
-public:
- TraversalNodeBase() : enable_statistics(false) {}
-
- virtual ~TraversalNodeBase();
-
- /** \brief Whether b is a leaf node in the first BVH tree */
- virtual bool isFirstNodeLeaf(int b) const;
-
- /** \brief Whether b is a leaf node in the second BVH tree */
- virtual bool isSecondNodeLeaf(int b) const;
-
- /** \brief Traverse the subtree of the node in the first tree first */
- virtual bool firstOverSecond(int b1, int b2) const;
-
- /** \brief Get the left child of the node b in the first tree */
- virtual int getFirstLeftChild(int b) const;
-
- /** \brief Get the right child of the node b in the first tree */
- virtual int getFirstRightChild(int b) const;
-
- /** \brief Get the left child of the node b in the second tree */
- virtual int getSecondLeftChild(int b) const;
-
- /** \brief Get the right child of the node b in the second tree */
- virtual int getSecondRightChild(int b) const;
-
- void enableStatistics(bool enable) { enable_statistics = enable; }
-
- bool enable_statistics;
-};
-
-class CollisionTraversalNodeBase : public TraversalNodeBase
-{
-public:
- CollisionTraversalNodeBase() : TraversalNodeBase() {}
-
- virtual ~CollisionTraversalNodeBase();
-
- /** \brief BV test between b1 and b2 */
- virtual bool BVTesting(int b1, int b2) const;
-
- /** \brief Leaf test between node b1 and b2, if they are both leafs */
- virtual void leafTesting(int b1, int b2) const;
-
- /** \brief Check whether the traversal can stop */
- virtual bool canStop() const;
-};
-
-class DistanceTraversalNodeBase : public TraversalNodeBase
-{
-public:
- DistanceTraversalNodeBase() : TraversalNodeBase() {}
-
- virtual ~DistanceTraversalNodeBase();
-
- virtual BVH_REAL BVTesting(int b1, int b2) const;
-
- virtual void leafTesting(int b1, int b2) const;
-
- virtual bool canStop(BVH_REAL c) const;
-};
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/traversal_node_bvh_shape.h b/branches/point_cloud/fcl/include/fcl/traversal_node_bvh_shape.h
deleted file mode 100644
index 802206127ac0f9b827bb3755b3b22ebd6107f086..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/traversal_node_bvh_shape.h
+++ /dev/null
@@ -1,302 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#ifndef FCL_TRAVERSAL_NODE_MESH_SHAPE_H
-#define FCL_TRAVERSAL_NODE_MESH_SHAPE_H
-
-#include "fcl/geometric_shapes.h"
-#include "fcl/traversal_node_base.h"
-#include "fcl/BVH_model.h"
-#include "fcl/geometric_shapes_utility.h"
-
-namespace fcl
-{
-
-template<typename BV, typename S>
-class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase
-{
-public:
- BVHShapeCollisionTraversalNode()
- {
- model1 = NULL;
- model2 = NULL;
-
- num_bv_tests = 0;
- num_leaf_tests = 0;
- query_time_seconds = 0.0;
- }
-
- bool isFirstNodeLeaf(int b) const
- {
- return model1->getBV(b).isLeaf();
- }
-
- int getFirstLeftChild(int b) const
- {
- return model1->getBV(b).leftChild();
- }
-
- int getFirstRightChild(int b) const
- {
- return model1->getBV(b).rightChild();
- }
-
- bool BVTesting(int b1, int b2) const
- {
- if(this->enable_statistics) num_bv_tests++;
- BV bv_shape;
- computeBV(*model2, bv_shape);
- return !model1->getBV(b1).bv.overlap(bv_shape);
- }
-
- const BVHModel<BV>* model1;
- const S* model2;
-
- mutable int num_bv_tests;
- mutable int num_leaf_tests;
- mutable BVH_REAL query_time_seconds;
-};
-
-
-template<typename S, typename BV>
-class ShapeBVHCollisionTraversalNode : public CollisionTraversalNodeBase
-{
-public:
- ShapeBVHCollisionTraversalNode()
- {
- model1 = NULL;
- model2 = NULL;
-
- num_bv_tests = 0;
- num_leaf_tests = 0;
- query_time_seconds = 0.0;
- }
-
- bool firstOverSecond(int, int) const
- {
- return false;
- }
-
- bool isSecondNodeLeaf(int b) const
- {
- return model2->getBV(b).isLeaf();
- }
-
- int getSecondLeftChild(int b) const
- {
- return model2->getBV(b).leftChild();
- }
-
- int getSecondRightChild(int b) const
- {
- return model2->getBV(b).rightChild();
- }
-
- bool BVTesting(int b1, int b2) const
- {
- if(this->enable_statistics) num_bv_tests++;
- BV bv_shape;
- computeBV(*model1, bv_shape);
- return !model2->getBV(b2).overlap(bv_shape);
- }
-
- const S* model1;
- const BVHModel<BV>* model2;
-
- mutable int num_bv_tests;
- mutable int num_leaf_tests;
- mutable BVH_REAL query_time_seconds;
-};
-
-
-
-/** \brief The indices of in-collision primitives of objects */
-struct BVHShapeCollisionPair
-{
- BVHShapeCollisionPair() {}
-
- BVHShapeCollisionPair(int id_) : id(id_) {}
-
- BVHShapeCollisionPair(int id_, const Vec3f& n, const Vec3f& contactp, BVH_REAL depth) : id(id_),
- normal(n), contact_point(contactp), penetration_depth(depth) {}
-
- /** \brief The index of BVH's in-collision primitive */
- int id;
-
- /** \brief Contact normal */
- Vec3f normal;
-
- /** \brief Contact points */
- Vec3f contact_point;
-
- /** \brief Penetration depth for two triangles */
- BVH_REAL penetration_depth;
-};
-
-struct BVHShapeCollisionPairComp
-{
- bool operator()(const BVHShapeCollisionPair& a, const BVHShapeCollisionPair& b)
- {
- return a.id < b.id;
- }
-};
-
-
-template<typename BV, typename S>
-class MeshShapeCollisionTraversalNode : public BVHShapeCollisionTraversalNode<BV, S>
-{
-public:
- MeshShapeCollisionTraversalNode() : BVHShapeCollisionTraversalNode<BV, S>()
- {
- vertices = NULL;
- tri_indices = NULL;
-
- num_max_contacts = 1;
- exhaustive = false;
- enable_contact = false;
- }
-
- void leafTesting(int b1, int b2) const
- {
- if(this->enable_statistics) this->num_leaf_tests++;
- const BVNode<BV>& node = this->model1->getBV(b1);
-
- int primitive_id = node.primitiveId();
-
- const Triangle& tri_id = tri_indices[primitive_id];
-
- const Vec3f& p1 = vertices[tri_id[0]];
- const Vec3f& p2 = vertices[tri_id[1]];
- const Vec3f& p3 = vertices[tri_id[2]];
-
- BVH_REAL penetration;
- Vec3f normal;
- Vec3f contactp;
-
- if(enable_contact) // only interested in collision or not
- {
- if(shapeTriangleIntersect(this->model2, p1, p2, p3))
- {
- pairs.push_back(BVHShapeCollisionPair(primitive_id));
- }
- else
- {
- if(shapeTriangleIntersect(this->model2, p1, p2, p3, &contactp, &penetration, &normal))
- {
- pairs.push_back(BVHShapeCollisionPair(primitive_id, normal, contactp, penetration));
- }
- }
- }
- }
-
- bool canStop() const
- {
- return (pairs.size() > 0) && (!exhaustive) && (num_max_contacts <= (int)pairs.size());
- }
-
- Vec3f* vertices;
- Triangle* tri_indices;
-
- int num_max_contacts;
- bool exhaustive;
- bool enable_contact;
-
- mutable std::vector<BVHShapeCollisionPair> pairs;
-};
-
-template<typename S>
-class MeshShapeCollisionTraversalNodeOBB : public MeshShapeCollisionTraversalNode<OBB, S>
-{
-public:
- MeshShapeCollisionTraversalNodeOBB() : MeshShapeCollisionTraversalNode<OBB, S>()
- {
- R[0] = Vec3f(1, 0, 0);
- R[1] = Vec3f(0, 1, 0);
- R[2] = Vec3f(0, 0, 1);
- }
-
- bool BVTesting(int b1, int b2) const
- {
- if(this->enable_statistics) this->num_bv_tests++;
- OBB bv_shape;
- computeBV(*this->model2, bv_shape);
- return !overlap(R, T, bv_shape, this->model1->getBV(b1).bv);
- }
-
- void leafTesting(int b1, int b2) const
- {
- if(this->enable_statistics) this->num_leaf_tests++;
- const BVNode<OBB>& node = this->model1->getBV(b1);
-
- int primitive_id = node.primitiveId();
-
- const Triangle& tri_id = this->tri_indices[primitive_id];
-
- const Vec3f& p1 = this->vertices[tri_id[0]];
- const Vec3f& p2 = this->vertices[tri_id[1]];
- const Vec3f& p3 = this->vertices[tri_id[2]];
-
- BVH_REAL penetration;
- Vec3f normal;
- Vec3f contactp;
-
- if(this->enable_contact == 0) // only interested in collision or not
- {
- if(shapeTriangleIntersect(this->model2, p1, p2, p3, R, T))
- {
- this->pairs.push_back(BVHShapeCollisionPair(primitive_id));
- }
- else
- {
- if(shapeTriangleIntersect(this->model2, p1, p2, p3, R, T, &contactp, &penetration, &normal))
- {
- this->pairs.push_back(BVHShapeCollisionPair(primitive_id, normal, contactp, penetration));
- }
- }
- }
- }
-
- // R, T is the transformation of bvh model
- Vec3f R[3];
- Vec3f T;
-};
-
-
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/traversal_node_bvhs.h b/branches/point_cloud/fcl/include/fcl/traversal_node_bvhs.h
deleted file mode 100644
index 9ab9265ca2b20ff199875282a6f0f75f22d55c61..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/traversal_node_bvhs.h
+++ /dev/null
@@ -1,1234 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#ifndef FCL_TRAVERSAL_NODE_MESHES_H
-#define FCL_TRAVERSAL_NODE_MESHES_H
-
-#include "fcl/traversal_node_base.h"
-#include "fcl/BV_node.h"
-#include "fcl/BV.h"
-#include "fcl/BVH_model.h"
-#include "fcl/intersect.h"
-#include "fcl/motion.h"
-
-#include <boost/shared_array.hpp>
-#include <boost/shared_ptr.hpp>
-#include <limits>
-#include <vector>
-
-/** \brief Main namespace */
-namespace fcl
-{
-
-/** \brief Traversal node for collision between BVH models */
-template<typename BV>
-class BVHCollisionTraversalNode : public CollisionTraversalNodeBase
-{
-public:
- BVHCollisionTraversalNode() : CollisionTraversalNodeBase()
- {
- model1 = NULL;
- model2 = NULL;
-
- num_bv_tests = 0;
- num_leaf_tests = 0;
- query_time_seconds = 0.0;
- }
-
- /** \brief Whether the BV node in the first BVH tree is leaf */
- bool isFirstNodeLeaf(int b) const
- {
- return model1->getBV(b).isLeaf();
- }
-
- /** \brief Whether the BV node in the second BVH tree is leaf */
- bool isSecondNodeLeaf(int b) const
- {
- return model2->getBV(b).isLeaf();
- }
-
- /** \brief Determine the traversal order, is the first BVTT subtree better */
- bool firstOverSecond(int b1, int b2) const
- {
- BVH_REAL sz1 = model1->getBV(b1).bv.size();
- BVH_REAL sz2 = model2->getBV(b2).bv.size();
-
- bool l1 = model1->getBV(b1).isLeaf();
- bool l2 = model2->getBV(b2).isLeaf();
-
- if(l2 || (!l1 && (sz1 > sz2)))
- return true;
- return false;
- }
-
- /** \brief Obtain the left child of BV node in the first BVH */
- int getFirstLeftChild(int b) const
- {
- return model1->getBV(b).leftChild();
- }
-
- /** \brief Obtain the right child of BV node in the first BVH */
- int getFirstRightChild(int b) const
- {
- return model1->getBV(b).rightChild();
- }
-
- /** \brief Obtain the left child of BV node in the second BVH */
- int getSecondLeftChild(int b) const
- {
- return model2->getBV(b).leftChild();
- }
-
- /** \brief Obtain the right child of BV node in the second BVH */
- int getSecondRightChild(int b) const
- {
- return model2->getBV(b).rightChild();
- }
-
- /** \brief BV culling test in one BVTT node */
- bool BVTesting(int b1, int b2) const
- {
- if(enable_statistics) num_bv_tests++;
- return !model1->getBV(b1).overlap(model2->getBV(b2));
- }
-
- /** \brief The first BVH model */
- const BVHModel<BV>* model1;
- /** \brief The second BVH model */
- const BVHModel<BV>* model2;
-
- /** \brief statistical information */
- mutable int num_bv_tests;
- mutable int num_leaf_tests;
- mutable BVH_REAL query_time_seconds;
-
-};
-
-
-/** \brief The collision/contact information between two primitives */
-struct BVHCollisionPair
-{
- BVHCollisionPair() {}
-
- BVHCollisionPair(int id1_, int id2_) : id1(id1_), id2(id2_) {}
-
- BVHCollisionPair(int id1_, int id2_, const Vec3f& n, const Vec3f& contactp, BVH_REAL depth) : id1(id1_),
- id2(id2_), normal(n), contact_point(contactp), penetration_depth(depth) {}
-
- /** \brief The index of one in-collision primitive */
- int id1;
-
- /** \brief The index of the other in-collision primitive */
- int id2;
-
- /** \brief Contact normal */
- Vec3f normal;
-
- /** \brief Contact points */
- Vec3f contact_point;
-
- /** \brief Penetration depth for two triangles */
- BVH_REAL penetration_depth;
-};
-
-/** \brief Sorting rule between two BVHCollisionPair, for testing */
-struct BVHCollisionPairComp
-{
- bool operator()(const BVHCollisionPair& a, const BVHCollisionPair& b)
- {
- if(a.id1 == b.id1)
- return a.id2 < b.id2;
- return a.id1 < b.id1;
- }
-};
-
-/** \brief Traversal node for collision between two meshes */
-template<typename BV>
-class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
-{
-public:
- MeshCollisionTraversalNode() : BVHCollisionTraversalNode<BV>()
- {
- vertices1 = NULL;
- vertices2 = NULL;
- tri_indices1 = NULL;
- tri_indices2 = NULL;
-
- num_max_contacts = 1;
- exhaustive = false;
- enable_contact = false;
- }
-
- /** \brief Intersection testing between leaves (two triangles) */
- void leafTesting(int b1, int b2) const
- {
- if(this->enable_statistics) this->num_leaf_tests++;
-
- const BVNode<BV>& node1 = this->model1->getBV(b1);
- const BVNode<BV>& node2 = this->model2->getBV(b2);
-
- int primitive_id1 = node1.primitiveId();
- int primitive_id2 = node2.primitiveId();
-
- const Triangle& tri_id1 = tri_indices1[primitive_id1];
- const Triangle& tri_id2 = tri_indices2[primitive_id2];
-
- const Vec3f& p1 = vertices1[tri_id1[0]];
- const Vec3f& p2 = vertices1[tri_id1[1]];
- const Vec3f& p3 = vertices1[tri_id1[2]];
- const Vec3f& q1 = vertices2[tri_id2[0]];
- const Vec3f& q2 = vertices2[tri_id2[1]];
- const Vec3f& q3 = vertices2[tri_id2[2]];
-
- BVH_REAL penetration;
- Vec3f normal;
- int n_contacts;
- Vec3f contacts[2];
-
-
- if(!enable_contact) // only interested in collision or not
- {
- if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3))
- {
- pairs.push_back(BVHCollisionPair(primitive_id1, primitive_id2));
- }
- }
- else // need compute the contact information
- {
- if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3,
- contacts,
- (unsigned int*)&n_contacts,
- &penetration,
- &normal))
- {
- for(int i = 0; i < n_contacts; ++i)
- {
- if((!exhaustive) && (num_max_contacts <= (int)pairs.size())) break;
- pairs.push_back(BVHCollisionPair(primitive_id1, primitive_id2, contacts[i], normal, penetration));
- }
- }
- }
- }
-
- /** \brief Whether the traversal process can stop early */
- bool canStop() const
- {
- return (pairs.size() > 0) && (!exhaustive) && (num_max_contacts <= (int)pairs.size());
- }
-
- Vec3f* vertices1;
- Vec3f* vertices2;
-
- Triangle* tri_indices1;
- Triangle* tri_indices2;
-
- int num_max_contacts;
- bool exhaustive;
- bool enable_contact;
-
- mutable std::vector<BVHCollisionPair> pairs;
-};
-
-
-
-class MeshCollisionTraversalNodeOBB : public MeshCollisionTraversalNode<OBB>
-{
-public:
- MeshCollisionTraversalNodeOBB();
-
- bool BVTesting(int b1, int b2) const;
-
- void leafTesting(int b1, int b2) const;
-
- Vec3f R[3];
- Vec3f T;
-};
-
-
-class MeshCollisionTraversalNodeRSS : public MeshCollisionTraversalNode<RSS>
-{
-public:
- MeshCollisionTraversalNodeRSS();
-
- bool BVTesting(int b1, int b2) const;
-
- void leafTesting(int b1, int b2) const;
-
- Vec3f R[3];
- Vec3f T;
-};
-
-
-
-#if USE_SVMLIGHT
-
-struct BVHPointCollisionPair
-{
- BVHPointCollisionPair() {}
-
- BVHPointCollisionPair(int id1_start_, int id1_num_, int id2_start_, int id2_num_, BVH_REAL collision_prob_)
- : id1_start(id1_start_), id1_num(id1_num_), id2_start(id2_start_), id2_num(id2_num_), collision_prob(collision_prob_) {}
-
- int id1_start;
- int id1_num;
-
- int id2_start;
- int id2_num;
-
- BVH_REAL collision_prob;
-};
-
-struct BVHPointCollisionPairComp
-{
- bool operator()(const BVHPointCollisionPair& a, const BVHPointCollisionPair& b)
- {
- if(a.id1_start == b.id1_start)
- return a.id2_start < b.id2_start;
- return a.id1_start < b.id1_start;
- }
-};
-
-
-template<typename BV>
-class PointCloudCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
-{
-public:
- PointCloudCollisionTraversalNode() : BVHCollisionTraversalNode<BV>()
- {
- vertices1 = NULL;
- vertices2 = NULL;
-
- num_max_contacts = 1;
- exhaustive = false;
- enable_contact = false;
-
- collision_prob_threshold = 0.5;
- max_collision_prob = 0;
- leaf_size_threshold = 1;
- }
-
- bool isFirstNodeLeaf(int b) const
- {
- const BVNode<BV>& node = this->model1->getBV(b);
- return ((node.num_primitives < leaf_size_threshold) || node.isLeaf());
- }
-
- bool isSecondNodeLeaf(int b) const
- {
- const BVNode<BV>& node = this->model2->getBV(b);
- return ((node.num_primitives < leaf_size_threshold) || node.isLeaf());
- }
-
- void leafTesting(int b1, int b2) const
- {
- if(this->enable_statistics) this->num_leaf_tests++;
-
- const BVNode<BV>& node1 = this->model1->getBV(b1);
- const BVNode<BV>& node2 = this->model2->getBV(b2);
-
- BVH_REAL collision_prob = Intersect::intersect_PointClouds(vertices1 + node1.first_primitive, uc1.get() + node1.first_primitive,
- node1.num_primitives,
- vertices2 + node2.first_primitive, uc2.get() + node2.first_primitive,
- node2.num_primitives,
- classifier_param);
-
- if(collision_prob > collision_prob_threshold)
- pairs.push_back(BVHPointCollisionPair(node1.first_primitive, node1.num_primitives, node2.first_primitive, node2.num_primitives, collision_prob));
-
- if(collision_prob > max_collision_prob)
- max_collision_prob = collision_prob;
-
- }
-
- bool canStop() const
- {
- return (pairs.size() > 0) && (!exhaustive) && (num_max_contacts <= (int)pairs.size());
- }
-
- Vec3f* vertices1;
- Vec3f* vertices2;
-
- boost::shared_array<Uncertainty> uc1;
- boost::shared_array<Uncertainty> uc2;
-
- int num_max_contacts;
- bool exhaustive;
- bool enable_contact;
-
- mutable std::vector<BVHPointCollisionPair> pairs;
-
- int leaf_size_threshold;
-
- BVH_REAL collision_prob_threshold;
-
- mutable BVH_REAL max_collision_prob;
-
- CloudClassifierParam classifier_param;
-};
-
-
-
-class PointCloudCollisionTraversalNodeOBB : public PointCloudCollisionTraversalNode<OBB>
-{
-public:
- PointCloudCollisionTraversalNodeOBB();
-
- bool BVTesting(int b1, int b2) const;
-
- void leafTesting(int b1, int b2) const;
-
- Vec3f R[3];
- Vec3f T;
-};
-
-
-class PointCloudCollisionTraversalNodeRSS : public PointCloudCollisionTraversalNode<RSS>
-{
-public:
- PointCloudCollisionTraversalNodeRSS();
-
- bool BVTesting(int b1, int b2) const;
-
- void leafTesting(int b1, int b2) const;
-
- Vec3f R[3];
- Vec3f T;
-};
-
-
-
-template<typename BV>
-class PointCloudMeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
-{
-public:
- PointCloudMeshCollisionTraversalNode() : BVHCollisionTraversalNode<BV>()
- {
- vertices1 = NULL;
- vertices2 = NULL;
- tri_indices2 = NULL;
-
- num_max_contacts = 1;
- exhaustive = false;
- enable_contact = false;
-
- collision_prob_threshold = 0.5;
- max_collision_prob = 0;
- leaf_size_threshold = 1;
- }
-
- bool isFirstNodeLeaf(int b) const
- {
- const BVNode<BV>& node = this->model1->getBV(b);
- return ((node.num_primitives < leaf_size_threshold) || node.isLeaf());
- }
-
- void leafTesting(int b1, int b2) const
- {
- if(this->enable_statistics) this->num_leaf_tests++;
-
- const BVNode<BV>& node1 = this->model1->getBV(b1);
- const BVNode<BV>& node2 = this->model2->getBV(b2);
-
- const Triangle& tri_id2 = tri_indices2[node2.primitiveId()];
-
- const Vec3f& q1 = vertices2[tri_id2[0]];
- const Vec3f& q2 = vertices2[tri_id2[1]];
- const Vec3f& q3 = vertices2[tri_id2[2]];
-
- BVH_REAL collision_prob = Intersect::intersect_PointCloudsTriangle(vertices1 + node1.first_primitive, uc1.get() + node1.first_primitive,
- node1.num_primitives,
- q1, q2, q3);
-
- if(collision_prob > collision_prob_threshold)
- pairs.push_back(BVHPointCollisionPair(node1.first_primitive, node1.num_primitives, node2.first_primitive, node2.num_primitives, collision_prob));
-
- if(collision_prob > max_collision_prob)
- max_collision_prob = collision_prob;
- }
-
- bool canStop() const
- {
- return (pairs.size() > 0) && (!exhaustive) && (num_max_contacts <= (int)pairs.size());
- }
-
- Vec3f* vertices1;
- Vec3f* vertices2;
-
- boost::shared_array<Uncertainty> uc1;
- Triangle* tri_indices2;
-
- int num_max_contacts;
- bool exhaustive;
- bool enable_contact;
-
- mutable std::vector<BVHPointCollisionPair> pairs;
-
- int leaf_size_threshold;
-
- BVH_REAL collision_prob_threshold;
-
- mutable BVH_REAL max_collision_prob;
-};
-
-
-class PointCloudMeshCollisionTraversalNodeOBB : public PointCloudMeshCollisionTraversalNode<OBB>
-{
-public:
- PointCloudMeshCollisionTraversalNodeOBB();
-
- bool BVTesting(int b1, int b2) const;
-
- void leafTesting(int b1, int b2) const;
-
- Vec3f R[3];
- Vec3f T;
-};
-
-class PointCloudMeshCollisionTraversalNodeRSS : public PointCloudMeshCollisionTraversalNode<RSS>
-{
-public:
- PointCloudMeshCollisionTraversalNodeRSS();
-
- bool BVTesting(int b1, int b2) const;
-
- void leafTesting(int b1, int b2) const;
-
- Vec3f R[3];
- Vec3f T;
-};
-
-#endif
-
-struct BVHContinuousCollisionPair
-{
- BVHContinuousCollisionPair() {}
-
- BVHContinuousCollisionPair(int id1_, int id2_, BVH_REAL time) : id1(id1_), id2(id2_), collision_time(time) {}
-
- /** \brief The index of one in-collision primitive */
- int id1;
-
- /** \brief The index of the other in-collision primitive */
- int id2;
-
- /** \brief Collision time normalized in [0, 1]. The collision time out of [0, 1] means collision-free */
- BVH_REAL collision_time;
-};
-
-
-template<typename BV>
-class MeshContinuousCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
-{
-public:
- MeshContinuousCollisionTraversalNode() : BVHCollisionTraversalNode<BV>()
- {
- vertices1 = NULL;
- vertices2 = NULL;
- tri_indices1 = NULL;
- tri_indices2 = NULL;
- prev_vertices1 = NULL;
- prev_vertices2 = NULL;
-
- num_max_contacts = 1;
- exhaustive = false;
- enable_contact = false;
-
- num_vf_tests = 0;
- num_ee_tests = 0;
- }
-
- void leafTesting(int b1, int b2) const
- {
- if(this->enable_statistics) this->num_leaf_tests++;
-
- const BVNode<BV>& node1 = this->model1->getBV(b1);
- const BVNode<BV>& node2 = this->model2->getBV(b2);
-
- BVH_REAL collision_time = 2;
- Vec3f collision_pos;
-
- int primitive_id1 = node1.primitiveId();
- int primitive_id2 = node2.primitiveId();
-
- const Triangle& tri_id1 = tri_indices1[primitive_id1];
- const Triangle& tri_id2 = tri_indices2[primitive_id2];
-
- Vec3f* S0[3];
- Vec3f* S1[3];
- Vec3f* T0[3];
- Vec3f* T1[3];
-
-
- for(int i = 0; i < 3; ++i)
- {
- S0[i] = prev_vertices1 + tri_id1[i];
- S1[i] = vertices1 + tri_id1[i];
- T0[i] = prev_vertices2 + tri_id2[i];
- T1[i] = vertices2 + tri_id2[i];
- }
-
- BVH_REAL tmp;
- Vec3f tmpv;
-
- // 6 VF checks
- for(int i = 0; i < 3; ++i)
- {
- if(this->enable_statistics) num_vf_tests++;
- if(Intersect::intersect_VF(*(S0[0]), *(S0[1]), *(S0[2]), *(T0[i]), *(S1[0]), *(S1[1]), *(S1[2]), *(T1[i]), &tmp, &tmpv))
- {
- if(collision_time > tmp)
- {
- collision_time = tmp; collision_pos = tmpv;
- }
- }
-
- if(this->enable_statistics) num_vf_tests++;
- if(Intersect::intersect_VF(*(T0[0]), *(T0[1]), *(T0[2]), *(S0[i]), *(T1[0]), *(T1[1]), *(T1[2]), *(S1[i]), &tmp, &tmpv))
- {
- if(collision_time > tmp)
- {
- collision_time = tmp; collision_pos = tmpv;
- }
- }
- }
-
- // 9 EE checks
- for(int i = 0; i < 3; ++i)
- {
- int S_id1 = i;
- int S_id2 = i + 1;
- if(S_id2 == 3) S_id2 = 0;
- for(int j = 0; j < 3; ++j)
- {
- int T_id1 = j;
- int T_id2 = j + 1;
- if(T_id2 == 3) T_id2 = 0;
-
- num_ee_tests++;
- if(Intersect::intersect_EE(*(S0[S_id1]), *(S0[S_id2]), *(T0[T_id1]), *(T0[T_id2]), *(S1[S_id1]), *(S1[S_id2]), *(T1[T_id1]), *(T1[T_id2]), &tmp, &tmpv))
- {
- if(collision_time > tmp)
- {
- collision_time = tmp; collision_pos = tmpv;
- }
- }
- }
- }
-
- if(!(collision_time > 1)) // collision happens
- pairs.push_back(BVHContinuousCollisionPair(primitive_id1, primitive_id2, collision_time));
- }
-
- bool canStop() const
- {
- return (pairs.size() > 0) && (!exhaustive) && (num_max_contacts <= (int)pairs.size());
- }
-
- Vec3f* vertices1;
- Vec3f* vertices2;
-
- Triangle* tri_indices1;
- Triangle* tri_indices2;
-
- Vec3f* prev_vertices1;
- Vec3f* prev_vertices2;
-
- int num_max_contacts;
- bool exhaustive;
- bool enable_contact;
-
- mutable int num_vf_tests;
- mutable int num_ee_tests;
-
- mutable std::vector<BVHContinuousCollisionPair> pairs;
-};
-
-
-template<typename BV>
-class MeshPointCloudContinuousCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
-{
-public:
- MeshPointCloudContinuousCollisionTraversalNode() : BVHCollisionTraversalNode<BV>()
- {
- vertices1 = NULL;
- vertices2 = NULL;
- tri_indices1 = NULL;
- prev_vertices1 = NULL;
- prev_vertices2 = NULL;
-
- num_max_contacts = 1;
- exhaustive = false;
- enable_contact = false;
-
- num_vf_tests = 0;
- }
-
- void leafTesting(int b1, int b2) const
- {
- if(this->enable_statistics) this->num_leaf_tests++;
-
- const BVNode<BV>& node1 = this->model1->getBV(b1);
- const BVNode<BV>& node2 = this->model2->getBV(b2);
-
- BVH_REAL collision_time = 2;
- Vec3f collision_pos;
-
- int primitive_id1 = node1.primitiveId();
- int primitive_id2 = node2.primitiveId();
-
- const Triangle& tri_id1 = tri_indices1[primitive_id1];
- int vertex_id2 = primitive_id2;
-
- Vec3f* S0[3];
- Vec3f* S1[3];
-
- for(int i = 0; i < 3; ++i)
- {
- S0[i] = prev_vertices1 + tri_id1[i];
- S1[i] = vertices1 + tri_id1[i];
- }
- Vec3f* T0 = prev_vertices2 + vertex_id2;
- Vec3f* T1 = vertices2 + vertex_id2;
-
- BVH_REAL tmp;
- Vec3f tmpv;
-
- // 3 VF checks
- for(int i = 0; i < 3; ++i)
- {
- num_vf_tests++;
- if(Intersect::intersect_VF(*(S0[0]), *(S0[1]), *(S0[2]), *T0, *(S1[0]), *(S1[1]), *(S1[2]), *T1, &tmp, &tmpv))
- {
- if(collision_time > tmp)
- {
- collision_time = tmp; collision_pos = tmpv;
- }
- }
- }
-
- if(!(collision_time > 1)) // collision happens
- {
- pairs.push_back(BVHContinuousCollisionPair(primitive_id1, primitive_id2, collision_time));
- }
- }
-
- bool canStop() const
- {
- return (pairs.size() > 0) && (!exhaustive) && (num_max_contacts <= (int)pairs.size());
- }
-
- Vec3f* vertices1;
- Vec3f* vertices2;
-
- Triangle* tri_indices1;
-
- Vec3f* prev_vertices1;
- Vec3f* prev_vertices2;
-
- int num_max_contacts;
- bool exhaustive;
- bool enable_contact;
-
- mutable int num_vf_tests;
-
- mutable std::vector<BVHContinuousCollisionPair> pairs;
-};
-
-
-template<typename BV>
-class PointCloudMeshContinuousCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
-{
-public:
- PointCloudMeshContinuousCollisionTraversalNode() : BVHCollisionTraversalNode<BV>()
- {
- vertices1 = NULL;
- vertices2 = NULL;
- tri_indices2 = NULL;
- prev_vertices1 = NULL;
- prev_vertices2 = NULL;
-
- num_max_contacts = 1;
- exhaustive = false;
- enable_contact = false;
-
- num_vf_tests = 0;
- }
-
- void leafTesting(int b1, int b2) const
- {
- if(this->enable_statistics) this->num_leaf_tests++;
-
- const BVNode<BV>& node1 = this->model1->getBV(b1);
- const BVNode<BV>& node2 = this->model2->getBV(b2);
-
- BVH_REAL collision_time = 2;
- Vec3f collision_pos;
-
- int primitive_id1 = node1.primitiveId();
- int primitive_id2 = node2.primitiveId();
-
- int vertex_id1 = primitive_id1;
- const Triangle& tri_id2 = tri_indices2[primitive_id2];
-
- Vec3f* S0 = prev_vertices1 + vertex_id1;
- Vec3f* S1 = vertices1 + vertex_id1;
-
- Vec3f* T0[3];
- Vec3f* T1[3];
- for(int i = 0; i < 3; ++i)
- {
- T0[i] = prev_vertices2 + tri_id2[i];
- T1[i] = vertices2 + tri_id2[i];
- }
-
- BVH_REAL tmp;
- Vec3f tmpv;
-
- // 3 VF checks
- for(int i = 0; i < 3; ++i)
- {
- num_vf_tests++;
- if(Intersect::intersect_VF(*(T0[0]), *(T0[1]), *(T0[2]), *S0, *(T1[0]), *(T1[1]), *(T1[2]), *S1, &tmp, &tmpv))
- {
- if(collision_time > tmp)
- {
- collision_time = tmp; collision_pos = tmpv;
- }
- }
- }
-
- if(!(collision_time > 1)) // collision happens
- {
- pairs.push_back(BVHContinuousCollisionPair(primitive_id1, primitive_id2, collision_time));
- }
- }
-
- bool canStop() const
- {
- return (pairs.size() > 0) && (!exhaustive) && (num_max_contacts <= (int)pairs.size());
- }
-
- Vec3f* vertices1;
- Vec3f* vertices2;
-
- Triangle* tri_indices2;
-
- Vec3f* prev_vertices1;
- Vec3f* prev_vertices2;
-
- int num_max_contacts;
- bool exhaustive;
- bool enable_contact;
-
- mutable int num_vf_tests;
-
- mutable std::vector<BVHContinuousCollisionPair> pairs;
-};
-
-
-
-
-template<typename BV>
-class BVHDistanceTraversalNode : public DistanceTraversalNodeBase
-{
-public:
- BVHDistanceTraversalNode() : DistanceTraversalNodeBase()
- {
- model1 = NULL;
- model2 = NULL;
-
- num_bv_tests = 0;
- num_leaf_tests = 0;
- query_time_seconds = 0.0;
- }
-
- bool isFirstNodeLeaf(int b) const
- {
- return model1->getBV(b).isLeaf();
- }
-
- bool isSecondNodeLeaf(int b) const
- {
- return model2->getBV(b).isLeaf();
- }
-
- bool firstOverSecond(int b1, int b2) const
- {
- BVH_REAL sz1 = model1->getBV(b1).bv.size();
- BVH_REAL sz2 = model2->getBV(b2).bv.size();
-
- bool l1 = model1->getBV(b1).isLeaf();
- bool l2 = model2->getBV(b2).isLeaf();
-
- if(l2 || (!l1 && (sz1 > sz2)))
- return true;
- return false;
- }
-
- int getFirstLeftChild(int b) const
- {
- return model1->getBV(b).leftChild();
- }
-
- int getFirstRightChild(int b) const
- {
- return model1->getBV(b).rightChild();
- }
-
- int getSecondLeftChild(int b) const
- {
- return model2->getBV(b).leftChild();
- }
-
- int getSecondRightChild(int b) const
- {
- return model2->getBV(b).rightChild();
- }
-
- BVH_REAL BVTesting(int b1, int b2) const
- {
- if(enable_statistics) num_bv_tests++;
- return model1->getBV(b1).distance(model2->getBV(b2));
- }
-
- const BVHModel<BV>* model1;
- const BVHModel<BV>* model2;
-
- mutable int num_bv_tests;
- mutable int num_leaf_tests;
- mutable BVH_REAL query_time_seconds;
-
-};
-
-
-
-template<typename BV>
-class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV>
-{
-public:
- MeshDistanceTraversalNode() : BVHDistanceTraversalNode<BV>()
- {
- vertices1 = NULL;
- vertices2 = NULL;
- tri_indices1 = NULL;
- tri_indices2 = NULL;
-
- last_tri_id1 = 0;
- last_tri_id2 = 0;
-
- rel_err = 0.01;
- abs_err = 0.01;
-
- min_distance = std::numeric_limits<BVH_REAL>::max();
- }
-
- void leafTesting(int b1, int b2) const
- {
- if(this->enable_statistics) this->num_leaf_tests++;
-
- const BVNode<BV>& node1 = this->model1->getBV(b1);
- const BVNode<BV>& node2 = this->model2->getBV(b2);
-
- int primitive_id1 = node1.primitiveId();
- int primitive_id2 = node2.primitiveId();
-
- const Triangle& tri_id1 = tri_indices1[primitive_id1];
- const Triangle& tri_id2 = tri_indices2[primitive_id2];
-
- const Vec3f& t11 = vertices1[tri_id1[0]];
- const Vec3f& t12 = vertices1[tri_id1[1]];
- const Vec3f& t13 = vertices1[tri_id1[2]];
-
- const Vec3f& t21 = vertices2[tri_id2[0]];
- const Vec3f& t22 = vertices2[tri_id2[1]];
- const Vec3f& t23 = vertices2[tri_id2[2]];
-
- // nearest point pair
- Vec3f P1, P2;
-
- BVH_REAL d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23,
- P1, P2);
-
- if(d < min_distance)
- {
- min_distance = d;
-
- p1 = P1;
- p2 = P2;
-
- last_tri_id1 = primitive_id1;
- last_tri_id2 = primitive_id2;
- }
- }
-
- bool canStop(BVH_REAL c) const
- {
- if((c >= min_distance - abs_err) && (c * (1 + rel_err) >= min_distance))
- return true;
- return false;
- }
-
- Vec3f* vertices1;
- Vec3f* vertices2;
-
- Triangle* tri_indices1;
- Triangle* tri_indices2;
-
- /** \brief relative and absolute error, default value is 0.01 for both terms */
- BVH_REAL rel_err;
- BVH_REAL abs_err;
-
- /** \brief distance and points establishing the minimum distance for the models, within the relative and absolute error bounds specified.
- * p1 is in model1's local coordinate system while p2 is in model2's local coordinate system
- */
- mutable BVH_REAL min_distance;
- mutable Vec3f p1;
- mutable Vec3f p2;
-
- /** \brief Remember the nearest neighbor points */
- mutable int last_tri_id1;
- mutable int last_tri_id2;
-};
-
-
-class MeshDistanceTraversalNodeRSS : public MeshDistanceTraversalNode<RSS>
-{
-public:
- MeshDistanceTraversalNodeRSS();
-
- BVH_REAL BVTesting(int b1, int b2) const;
-
- void leafTesting(int b1, int b2) const;
-
- Vec3f R[3];
- Vec3f T;
-};
-
-// when using this default version, must refit the BVH in current configuration (R_t, T_t) into default configuration
-template<typename BV>
-class MeshConservativeAdvancementTraversalNode : public MeshDistanceTraversalNode<BV>
-{
-public:
- MeshConservativeAdvancementTraversalNode(BVH_REAL w_ = 1) : MeshDistanceTraversalNode<BV>()
- {
- delta_t = 1;
- toc = 0;
- t_err = (BVH_REAL)0.001;
-
- w = w_;
-
- motion1 = NULL;
- motion2 = NULL;
- }
-
- BVH_REAL BVTesting(int b1, int b2) const
- {
- if(this->enable_statistics) this->num_bv_tests++;
- Vec3f P1, P2;
- BVH_REAL d = this->model1->getBV(b1).distance(this->model2->getBV(b2), &P1, &P2);
-
- stack.push_back(StackData(P1, P2, b1, b2, d));
-
- return d;
- }
-
- void leafTesting(int b1, int b2) const
- {
- if(this->enable_statistics) this->num_leaf_tests++;
-
- const BVNode<BV>& node1 = this->model1->getBV(b1);
- const BVNode<BV>& node2 = this->model2->getBV(b2);
-
- int primitive_id1 = node1.primitiveId();
- int primitive_id2 = node2.primitiveId();
-
- const Triangle& tri_id1 = this->tri_indices1[primitive_id1];
- const Triangle& tri_id2 = this->tri_indices2[primitive_id2];
-
- const Vec3f& p1 = this->vertices1[tri_id1[0]];
- const Vec3f& p2 = this->vertices1[tri_id1[1]];
- const Vec3f& p3 = this->vertices1[tri_id1[2]];
-
- const Vec3f& q1 = this->vertices2[tri_id2[0]];
- const Vec3f& q2 = this->vertices2[tri_id2[1]];
- const Vec3f& q3 = this->vertices2[tri_id2[2]];
-
- // nearest point pair
- Vec3f P1, P2;
-
- BVH_REAL d = TriangleDistance::triDistance(p1, p2, p3, q1, q2, q3,
- P1, P2);
-
- if(d < this->min_distance)
- {
- this->min_distance = d;
-
- this->p1 = P1;
- this->p2 = P2;
-
- this->last_tri_id1 = primitive_id1;
- this->last_tri_id2 = primitive_id2;
- }
-
-
- /** n is the local frame of object 1 */
- Vec3f n = P2 - P1;
- /** turn n into the global frame
- * nothing is done here because in this case we assume the body is in original configuration (I, 0)
- */
- BVH_REAL bound1 = motion1->computeMotionBound(p1, p2, p3, n);
- BVH_REAL bound2 = motion2->computeMotionBound(q1, q2, q3, n);
-
- BVH_REAL cur_delta_t = d / (bound1 + bound2);
-
- if(cur_delta_t < delta_t)
- delta_t = cur_delta_t;
- }
-
-
- bool canStop(BVH_REAL c) const
- {
- if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
- {
- const StackData& data = stack.back();
- BVH_REAL d = data.d;
- Vec3f n;
- int c1, c2;
-
- if(d > c)
- {
- const StackData& data2 = stack[stack.size() - 2];
- d = data2.d;
- n = data2.P2 - data2.P1;
- c1 = data2.c1;
- c2 = data2.c2;
- stack[stack.size() - 2] = stack[stack.size() - 1];
- }
- else
- {
- n = data.P2 - data.P1;
- c1 = data.c1;
- c2 = data.c2;
- }
-
- if(c != d) std::cout << "there is some problem here" << std::endl;
-
-
- // (this->tree1 + c1)->bv.axis[0] * n[0] + (this->tree1 + c1)->bv.axis[1] * n[1] + (this->tree1 + c1)->bv.axis[2] * n[2];
-
- BVH_REAL bound1 = motion1->computeMotionBound((this->tree1 + c1)->bv, n);
- BVH_REAL bound2 = motion2->computeMotionBound((this->tree2 + c2)->bv, n);
-
- BVH_REAL cur_delta_t = c / (bound1 + bound2);
- if(cur_delta_t < delta_t)
- delta_t = cur_delta_t;
-
- stack.pop_back();
-
- return true;
- }
- else
- {
- const StackData& data = stack.back();
- BVH_REAL d = data.d;
-
- if(d > c)
- stack[stack.size() - 2] = stack[stack.size() - 1];
-
- stack.pop_back();
-
- return false;
- }
- }
-
-
- /** \brief CA controlling variable: early stop for the early iterations of CA */
- BVH_REAL w;
-
- /** \brief The time from beginning point */
- BVH_REAL toc;
- BVH_REAL t_err;
-
- /** \brief The delta_t each step */
- mutable BVH_REAL delta_t;
-
- /** \brief Motions for the two objects in query */
- MotionBase<BV>* motion1;
- MotionBase<BV>* motion2;
-
- struct StackData
- {
- StackData(const Vec3f& P1_, const Vec3f& P2_, int c1_, int c2_, BVH_REAL d_) : P1(P1_), P2(P2_), c1(c1_), c2(c2_), d(d_) {}
-
- Vec3f P1;
- Vec3f P2;
- int c1;
- int c2;
- BVH_REAL d;
- };
-
- mutable std::vector<StackData> stack;
-};
-
-
-/** for OBB and RSS, there is local coordinate of BV, so normal need to be transformed */
-template<>
-bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(BVH_REAL c) const;
-
-template<>
-bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(BVH_REAL c) const;
-
-
-class MeshConservativeAdvancementTraversalNodeRSS : public MeshConservativeAdvancementTraversalNode<RSS>
-{
-public:
- MeshConservativeAdvancementTraversalNodeRSS(BVH_REAL w_ = 1);
-
- BVH_REAL BVTesting(int b1, int b2) const;
-
- void leafTesting(int b1, int b2) const;
-
- bool canStop(BVH_REAL c) const;
-
- Vec3f R[3];
- Vec3f T;
-};
-
-}
-
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/traversal_node_shapes.h b/branches/point_cloud/fcl/include/fcl/traversal_node_shapes.h
deleted file mode 100644
index 3b43f24f112f826e2901ec55df96ff1f6f101804..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/traversal_node_shapes.h
+++ /dev/null
@@ -1,98 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#ifndef FCL_TRAVERSAL_NODE_SHAPES_H
-#define FCL_TRAVERSAL_NODE_SHAPES_H
-
-#include "fcl/traversal_node_base.h"
-#include "fcl/geometric_shapes_intersect.h"
-
-namespace fcl
-{
-
-template<typename S1, typename S2>
-class ShapeCollisionTraversalNode : public CollisionTraversalNodeBase
-{
-public:
- ShapeCollisionTraversalNode() : CollisionTraversalNodeBase()
- {
- model1 = NULL;
- model2 = NULL;
-
- R1[0] = Vec3f(1, 0, 0);
- R1[1] = Vec3f(0, 1, 0);
- R1[2] = Vec3f(0, 0, 1);
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
-
- enable_contact = false;
- }
-
- bool BVTesting(int, int) const
- {
- return false;
- }
-
- void leafTesting(int, int) const
- {
- if(enable_contact)
- is_collision = shapeIntersect(*model1, *model2, R1, T1, R2, T2, &contact_point, &penetration_depth, &normal);
- else
- is_collision = shapeIntersect(*model1, *model2, R1, T1, R2, T2);
- }
-
- const S1* model1;
- const S2* model2;
-
- mutable Vec3f normal;
-
- mutable Vec3f contact_point;
-
- mutable BVH_REAL penetration_depth;
-
- bool enable_contact;
-
- mutable bool is_collision;
-
- Vec3f R1[3];
- Vec3f R2[3];
- Vec3f T1, T2;
- };
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/traversal_recurse.h b/branches/point_cloud/fcl/include/fcl/traversal_recurse.h
deleted file mode 100644
index ca5838835619ee1db43e400cb328f9b298ee27e0..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/traversal_recurse.h
+++ /dev/null
@@ -1,69 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#ifndef FCL_TRAVERSAL_RECURSE_H
-#define FCL_TRAVERSAL_RECURSE_H
-
-#include "fcl/traversal_node_base.h"
-#include "fcl/BVH_front.h"
-#include <queue>
-
-namespace fcl
-{
-
-inline void updateFrontList(BVHFrontList* front_list, int b1, int b2)
-{
- if(front_list) front_list->push_back(BVHFrontNode(b1, b2));
-}
-
-void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list);
-
-/** Recurse function for self collision
- * Make sure node is set correctly so that the first and second tree are the same
- */
-void selfCollisionRecurse(CollisionTraversalNodeBase* node, int b, BVHFrontList* front_list);
-
-void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list);
-
-void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list, int qsize);
-
-void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, BVHFrontList* front_list);
-
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/include/fcl/vec_3f.h b/branches/point_cloud/fcl/include/fcl/vec_3f.h
deleted file mode 100644
index 6ba98c5b36cadfa368480aaeb0fd1fdd2a4d540f..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/include/fcl/vec_3f.h
+++ /dev/null
@@ -1,479 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-#ifndef COLLISION_CHECKING_VEC_3F_H
-#define COLLISION_CHECKING_VEC_3F_H
-
-#include "fcl/BVH_internal.h"
-#include <cmath>
-#include <cstdlib>
-#include <algorithm>
-
-/** \brief Main namespace */
-namespace fcl
-{
-
-
-#if COLLISION_USE_SSE
-#include <xmmintrin.h>
-#include <pmmintrin.h>
- inline __m128 _mm_cross_ps(__m128 a , __m128 b)
- {
- // set to a[1][2][0][3] , b[2][0][1][3]
- // multiply
- __m128 xa = _mm_mul_ps(
- _mm_shuffle_ps(a, a, _MM_SHUFFLE(3, 0, 2, 1)),
- _mm_shuffle_ps(b, b, _MM_SHUFFLE(3, 1, 0, 2)));
-
- // set to a[2][0][1][3] , b[1][2][0][3]
- // multiply
- __m128 xb = _mm_mul_ps(
- _mm_shuffle_ps(a, a, _MM_SHUFFLE(3, 1, 0, 2)),
- _mm_shuffle_ps(b, b, _MM_SHUFFLE(3, 0, 2, 1 )));
-
- // subtract
- return _mm_sub_ps(xa, xb);
- }
-
- const __m128 xmms_0 = {0.f, 0.f, 0.f, 0.f};
-
- union ieee754_QNAN
- {
- const float f;
- struct
- {
- const unsigned int mantissa:23, exp:8, sign:1;
- };
-
- ieee754_QNAN() : f(0.0f), mantissa(0x7FFFFF), exp(0xFF), sign(0x0) {}
- } __attribute__ ((aligned (16)));
-
-
- class Vec3f
- {
- public:
- /** \brief vector data */
- union {float v_[4]; __m128 v4; } __attribute__ ((aligned (16)));
-
- Vec3f() { v4 = _mm_set1_ps(0); }
-
- Vec3f(const Vec3f& other)
- {
- v4 = other.v4;
- }
-
- Vec3f(const float* v)
- {
- v_[0] = v[0];
- v_[1] = v[1];
- v_[2] = v[2];
- v_[3] = 0.0f;
- }
-
- Vec3f(float x, float y, float z)
- {
- v_[0] = x;
- v_[1] = y;
- v_[2] = z;
- v_[3] = 0.0f;
- }
-
-
- Vec3f(__m128 v)
- {
- v4 = v;
- }
-
- virtual ~Vec3f() {}
-
- /** \brief Get the ith element */
- inline float operator [] (size_t i) const
- {
- return v_[i];
- }
-
- inline float& operator[] (size_t i)
- {
- return v_[i];
- }
-
- /** \brief Add the other vector */
- inline Vec3f& operator += (const Vec3f& other)
- {
- v4 = _mm_add_ps(v4, other.v4);
- return *this;
- }
-
-
- /** \brief Minus the other vector */
- inline Vec3f& operator -= (const Vec3f& other)
- {
- v4 = _mm_sub_ps(v4, other.v4);
- return *this;
- }
-
- /** \brief Negate the vector */
- inline void negate()
- {
- v4 = _mm_sub_ps(xmms_0, v4);
- }
-
- /** \brief Return a negated vector */
- inline Vec3f operator - () const
- {
- return Vec3f(_mm_sub_ps(xmms_0, v4));
- }
-
-
- /** \brief Return a summation vector */
- inline Vec3f operator + (const Vec3f& other) const
- {
- return Vec3f(_mm_add_ps(v4, other.v4));
- }
-
- /** \brief Return a substraction vector */
- inline Vec3f operator - (const Vec3f& other) const
- {
- return Vec3f(_mm_sub_ps(v4, other.v4));
- }
-
- /** \brief Scale the vector by t */
- inline Vec3f operator * (float t) const
- {
- return Vec3f(_mm_mul_ps(_mm_set1_ps(t), v4));
- }
-
- /** \brief Return the cross product with another vector */
- inline Vec3f cross(const Vec3f& other) const
- {
- return Vec3f(_mm_cross_ps(v4, other.v4));
- }
-
- /** \brief Return the dot product with another vector */
- inline float dot(const Vec3f& other) const
- {
- float d;
- register __m128 t = _mm_mul_ps(other.v4, v4);
- t = _mm_hadd_ps(t, t);
- t = _mm_hadd_ps(t, t);
- _mm_store_ss(&d, t);
- return d;
- }
-
- /** \brief Normalization */
- inline bool normalize()
- {
- float sqr_length = sqrLength();
- if(sqr_length > EPSILON)
- {
- float inv_length = 1.0 / sqrt(sqr_length);
- v4 = _mm_mul_ps(_mm_set1_ps(inv_length), v4);
- return true;
- }
- return false;
- }
-
- /** \brief Return vector length */
- inline float length() const
- {
- return sqrt(sqrLength());
- }
-
- /** \brief Return vector square length */
- inline float sqrLength() const
- {
- float d;
- register __m128 t = _mm_mul_ps(v4, v4);
- t = _mm_hadd_ps(t, t);
- t = _mm_hadd_ps(t, t);
- _mm_store_ss(&d, t);
- return d;
- }
-
- /** \brief Set the vector using new values */
- inline Vec3f& setValue(float x, float y, float z)
- {
- v_[0] = x; v_[1] = y; v_[2] = z; v_[3] = 0.0f;
- return *this;
- }
-
- /** \brief Check whether two vectors are the same in abstracted value */
- inline bool equal(const Vec3f& other) const
- {
- return ((v_[0] - other.v_[0] < EPSILON) &&
- (v_[0] - other.v_[0] > -EPSILON) &&
- (v_[1] - other.v_[1] < EPSILON) &&
- (v_[1] - other.v_[1] > -EPSILON) &&
- (v_[2] - other.v_[2] < EPSILON) &&
- (v_[2] - other.v_[2] > -EPSILON));
- }
-
- private:
- /** \brief Tolerance for comparision */
- static const float EPSILON;
-
- };
-
-
- /** \brief The minimum of two vectors */
- inline Vec3f min(const Vec3f& a, const Vec3f& b)
- {
- Vec3f ret(_mm_min_ps(a.v4, b.v4));
- return ret;
- }
-
- /** \brief the maximum of two vectors */
- inline Vec3f max(const Vec3f& a, const Vec3f& b)
- {
- Vec3f ret(_mm_max_ps(a.v4, b.v4));
- return ret;
- }
-
- inline Vec3f abs(const Vec3f& v)
- {
- const ieee754_QNAN mask;
- __m128 abs4mask = _mm_load1_ps(&mask.f);
- return Vec3f(_mm_and_ps(abs4mask, v.v4));
- }
-#else
-/** \brief A class describing a three-dimensional vector */
- class Vec3f
- {
- public:
- /** \brief vector data */
- BVH_REAL v_[3];
-
- Vec3f() { v_[0] = 0; v_[1] = 0; v_[2] = 0; }
-
- Vec3f(const Vec3f& other)
- {
- v_[0] = other.v_[0];
- v_[1] = other.v_[1];
- v_[2] = other.v_[2];
- }
-
- Vec3f(const BVH_REAL* v)
- {
- v_[0] = v[0];
- v_[1] = v[1];
- v_[2] = v[2];
- }
-
- Vec3f(BVH_REAL x, BVH_REAL y, BVH_REAL z)
- {
- v_[0] = x;
- v_[1] = y;
- v_[2] = z;
- }
-
- virtual ~Vec3f() {}
-
- /** \brief Get the ith element */
- inline BVH_REAL operator [] (size_t i) const
- {
- return v_[i];
- }
-
- inline BVH_REAL& operator[] (size_t i)
- {
- return v_[i];
- }
-
- /** \brief Add the other vector */
- inline Vec3f& operator += (const Vec3f& other)
- {
- v_[0] += other.v_[0];
- v_[1] += other.v_[1];
- v_[2] += other.v_[2];
- return *this;
- }
-
-
- /** \brief Minus the other vector */
- inline Vec3f& operator -= (const Vec3f& other)
- {
- v_[0] -= other.v_[0];
- v_[1] -= other.v_[1];
- v_[2] -= other.v_[2];
- return *this;
- }
-
- /** \brief Negate the vector */
- inline void negate()
- {
- v_[0] = - v_[0];
- v_[1] = - v_[1];
- v_[2] = - v_[2];
- }
-
- /** \brief Return a negated vector */
- inline Vec3f operator - () const
- {
- return Vec3f(-v_[0], -v_[1], -v_[2]);
- }
-
- /** \brief Return a summation vector */
- inline Vec3f operator + (const Vec3f& other) const
- {
- return Vec3f(v_[0] + other.v_[0], v_[1] + other.v_[1], v_[2] + other.v_[2]);
- }
-
- /** \brief Return a substraction vector */
- inline Vec3f operator - (const Vec3f& other) const
- {
- return Vec3f(v_[0] - other.v_[0], v_[1] - other.v_[1], v_[2] - other.v_[2]);
- }
-
- /** \brief Scale the vector by t */
- inline Vec3f operator * (BVH_REAL t) const
- {
- return Vec3f(v_[0] * t, v_[1] * t, v_[2] * t);
- }
-
- /** \brief Return the cross product with another vector */
- inline Vec3f cross(const Vec3f& other) const
- {
- return Vec3f(v_[1] * other.v_[2] - v_[2] * other.v_[1],
- v_[2] * other.v_[0] - v_[0] * other.v_[2],
- v_[0] * other.v_[1] - v_[1] * other.v_[0]);
- }
-
- /** \brief Return the dot product with another vector */
- inline BVH_REAL dot(const Vec3f& other) const
- {
- return v_[0] * other.v_[0] + v_[1] * other.v_[1] + v_[2] * other.v_[2];
- }
-
- /** \brief Normalization */
- inline bool normalize()
- {
- BVH_REAL sqr_length = v_[0] * v_[0] + v_[1] * v_[1] + v_[2] * v_[2];
- if(sqr_length > EPSILON * EPSILON)
- {
- BVH_REAL inv_length = (BVH_REAL)1.0 / (BVH_REAL)sqrt(sqr_length);
- v_[0] *= inv_length;
- v_[1] *= inv_length;
- v_[2] *= inv_length;
- return true;
- }
- return false;
- }
-
- /** \brief Return vector length */
- inline BVH_REAL length() const
- {
- return sqrt(v_[0] * v_[0] + v_[1] * v_[1] + v_[2] * v_[2]);
- }
-
- /** \brief Return vector square length */
- inline BVH_REAL sqrLength() const
- {
- return v_[0] * v_[0] + v_[1] * v_[1] + v_[2] * v_[2];
- }
-
- /** \brief Set the vector using new values */
- inline Vec3f& setValue(BVH_REAL x, BVH_REAL y, BVH_REAL z)
- {
- v_[0] = x; v_[1] = y; v_[2] = z;
- return *this;
- }
-
- /** \brief Check whether two vectors are the same in value */
- inline bool equal(const Vec3f& other) const
- {
- return ((v_[0] - other.v_[0] < EPSILON) &&
- (v_[0] - other.v_[0] > -EPSILON) &&
- (v_[1] - other.v_[1] < EPSILON) &&
- (v_[1] - other.v_[1] > -EPSILON) &&
- (v_[2] - other.v_[2] < EPSILON) &&
- (v_[2] - other.v_[2] > -EPSILON));
- }
-
- private:
- /** \brief Tolerance for comparision */
- static const BVH_REAL EPSILON;
- };
-
-
- /** \brief The minimum of two vectors */
- inline Vec3f min(const Vec3f& a, const Vec3f& b)
- {
- Vec3f ret(std::min(a[0], b[0]), std::min(a[1], b[1]), std::min(a[2], b[2]));
- return ret;
- }
-
- /** \brief the maximum of two vectors */
- inline Vec3f max(const Vec3f& a, const Vec3f& b)
- {
- Vec3f ret(std::max(a[0], b[0]), std::max(a[1], b[1]), std::max(a[2], b[2]));
- return ret;
- }
-
- inline Vec3f abs(const Vec3f& v)
- {
- BVH_REAL x = v[0] < 0 ? -v[0] : v[0];
- BVH_REAL y = v[1] < 0 ? -v[1] : v[1];
- BVH_REAL z = v[2] < 0 ? -v[2] : v[2];
-
- return Vec3f(x, y, z);
- }
-#endif
-
- /** \brief M * v */
- Vec3f MxV(const Vec3f M[3], const Vec3f& v);
-
- /** \brief M' * v */
- Vec3f MTxV(const Vec3f M[3], const Vec3f& v);
-
- /** \brief v' * M * v */
- BVH_REAL vTMv(const Vec3f M[3], const Vec3f& v);
-
- /** \brief S * M * S' */
- void SMST(const Vec3f M[3], const Vec3f S[3], Vec3f newM[3]);
-
- /** \brief A * B */
- void MxM(const Vec3f M1[3], const Vec3f M2[3], Vec3f newM[3]);
-
- /** \brief The relative transform from (R1, T1) to (R2, T2) */
- void relativeTransform(const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2, Vec3f R[3], Vec3f& T);
-
- /** \brief compute eigen values and vectors */
- void Meigen(Vec3f a[3], BVH_REAL dout[3], Vec3f vout[3]);
-
-
-}
-
-#endif
diff --git a/branches/point_cloud/fcl/mainpage.dox b/branches/point_cloud/fcl/mainpage.dox
deleted file mode 100644
index 877dbd152bca5e2449449cb81e7182cb62fc5583..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/mainpage.dox
+++ /dev/null
@@ -1,26 +0,0 @@
-/**
-\mainpage
-\htmlinclude manifest.html
-
-\b fcl 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.
--->
-
-
-*/
diff --git a/branches/point_cloud/fcl/manifest.xml b/branches/point_cloud/fcl/manifest.xml
deleted file mode 100644
index ac01d1469e0742eac956c069e0ceaba73221909e..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/manifest.xml
+++ /dev/null
@@ -1,21 +0,0 @@
-<package>
- <description brief="fcl">
-
- fcl
-
- </description>
- <author>Jia Pan</author>
- <license>BSD</license>
- <review status="unreviewed" notes=""/>
- <url>http://ros.org/wiki/fcl</url>
- <depend package="libccd"/>
- <depend package="ann"/>
- <depend package="PQP"/>
- <depend package="svm_light"/>
- <export>
- <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lfcl"/>
- </export>
-
-</package>
-
-
diff --git a/branches/point_cloud/fcl/src/AABB.cpp b/branches/point_cloud/fcl/src/AABB.cpp
deleted file mode 100644
index 62ae193b5621d8e02e75ec89eaf7e4e0271f1faa..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/src/AABB.cpp
+++ /dev/null
@@ -1,58 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-#include "fcl/AABB.h"
-
-#include <limits>
-#include <iostream>
-
-namespace fcl
-{
-
-AABB::AABB()
-{
- BVH_REAL real_max = std::numeric_limits<BVH_REAL>::max();
- min_ = Vec3f(real_max, real_max, real_max);
- max_ = Vec3f(-real_max, -real_max, -real_max);
-}
-
-BVH_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const
-{
- std::cerr << "AABB distance not implemented!" << std::endl;
- return 0.0;
-}
-
-}
diff --git a/branches/point_cloud/fcl/src/BVH_model.cpp b/branches/point_cloud/fcl/src/BVH_model.cpp
deleted file mode 100644
index 5e77b275cbbe0a1d1dff26abb2267655a58a9a03..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/src/BVH_model.cpp
+++ /dev/null
@@ -1,925 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-#include "fcl/BVH_model.h"
-#include "fcl/BV.h"
-#include <iostream>
-#include <string.h>
-
-namespace fcl
-{
-
-template<typename BV>
-BVHModel<BV>::BVHModel(const BVHModel<BV>& other) : CollisionObject(other)
-{
- num_tris = num_tris_allocated = other.num_tris;
- num_vertices = num_vertices_allocated = other.num_vertices;
- if(other.vertices)
- {
- vertices = new Vec3f[num_vertices];
- memcpy(vertices, other.vertices, sizeof(Vec3f) * num_vertices);
- }
- else
- vertices = NULL;
-
- if(other.tri_indices)
- {
- tri_indices = new Triangle[num_tris];
- memcpy(tri_indices, other.tri_indices, sizeof(Triangle) * num_tris);
- }
- else
- tri_indices = NULL;
-
- if(other.prev_vertices)
- {
- prev_vertices = new Vec3f[num_vertices];
- memcpy(prev_vertices, other.prev_vertices, sizeof(Vec3f) * num_vertices);
- }
- else
- prev_vertices = NULL;
-
- if(other.primitive_indices)
- {
- int num_primitives = 0;
- switch(other.getModelType())
- {
- case BVH_MODEL_TRIANGLES:
- num_primitives = num_tris;
- break;
- case BVH_MODEL_POINTCLOUD:
- num_primitives = num_vertices;
- break;
- default:
- ;
- }
-
- primitive_indices = new unsigned int[num_primitives];
- memcpy(primitive_indices, other.primitive_indices, sizeof(unsigned int) * num_primitives);
- }
- else
- primitive_indices = NULL;
-
- num_bvs = num_bvs_allocated = other.num_bvs;
- if(other.bvs)
- {
- bvs = new BVNode<BV>[num_bvs];
- memcpy(bvs, other.bvs, sizeof(BVNode<BV>) * num_bvs);
- }
- else
- bvs = NULL;
-
- build_state = other.build_state;
-
- bv_splitter = other.bv_splitter;
-
- bv_fitter = other.bv_fitter;
-}
-
-
-template<typename BV>
-int BVHModel<BV>::beginModel(int num_tris_, int num_vertices_)
-{
- if(build_state != BVH_BUILD_STATE_EMPTY)
- {
- delete [] vertices; vertices = NULL;
- delete [] tri_indices; tri_indices = NULL;
- delete [] bvs; bvs = NULL;
- delete [] prev_vertices; prev_vertices = NULL;
- delete [] primitive_indices; primitive_indices = NULL;
-
- num_vertices_allocated = num_vertices = num_tris_allocated = num_tris = num_bvs_allocated = num_bvs = 0;
- }
-
- if(num_tris_ <= 0) num_tris_ = 8;
- if(num_vertices_ <= 0) num_vertices_ = 8;
-
- num_vertices_allocated = num_vertices_;
- num_tris_allocated = num_tris_;
-
- tri_indices = new Triangle[num_tris_allocated];
- vertices = new Vec3f[num_vertices_allocated];
-
- if(!tri_indices)
- {
- std::cerr << "BVH Error! Out of memory for tri_indices array on BeginModel() call!" << std::endl;
- return BVH_ERR_MODEL_OUT_OF_MEMORY;
- }
- if(!vertices)
- {
- std::cerr << "BVH Error! Out of memory for vertices array on BeginModel() call!" << std::endl;
- return BVH_ERR_MODEL_OUT_OF_MEMORY;
- }
-
- if(build_state != BVH_BUILD_STATE_EMPTY)
- {
- std::cerr << "BVH Warning! Call beginModel() on a BVHModel that is not empty. This model was cleared and previous triangles/vertices were lost." << std::endl;
- build_state = BVH_BUILD_STATE_EMPTY;
- return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
- }
-
- build_state = BVH_BUILD_STATE_BEGUN;
-
- return BVH_OK;
-}
-
-
-template<typename BV>
-int BVHModel<BV>::addVertex(const Vec3f& p)
-{
- if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN)
- {
- std::cerr << "BVH Warning! Call addVertex() in a wrong order. addVertex() was ignored. Must do a beginModel() to clear the model for addition of new vertices." << std::endl;
- return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
- }
-
- if(num_vertices >= num_vertices_allocated)
- {
- Vec3f* temp = new Vec3f[num_vertices_allocated * 2];
- if(!temp)
- {
- std::cerr << "BVH Error! Out of memory for vertices array on addVertex() call!" << std::endl;
- return BVH_ERR_MODEL_OUT_OF_MEMORY;
- }
-
- memcpy(temp, vertices, sizeof(Vec3f) * num_vertices);
- delete [] vertices;
- vertices = temp;
- num_vertices_allocated *= 2;
- }
-
- vertices[num_vertices] = p;
- num_vertices += 1;
-
- return BVH_OK;
-}
-
-template<typename BV>
-int BVHModel<BV>::addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3)
-{
- if(build_state == BVH_BUILD_STATE_PROCESSED)
- {
- std::cerr << "BVH Warning! Call addTriangle() in a wrong order. addTriangle() was ignored. Must do a beginModel() to clear the model for addition of new triangles." << std::endl;
- return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
- }
-
- if(num_vertices + 2 >= num_vertices_allocated)
- {
- Vec3f* temp = new Vec3f[num_vertices_allocated * 2 + 2];
- if(!temp)
- {
- std::cerr << "BVH Error! Out of memory for vertices array on addTriangle() call!" << std::endl;
- return BVH_ERR_MODEL_OUT_OF_MEMORY;
- }
-
- memcpy(temp, vertices, sizeof(Vec3f) * num_vertices);
- delete [] vertices;
- vertices = temp;
- num_vertices_allocated = num_vertices_allocated * 2 + 2;
- }
-
- int offset = num_vertices;
-
- vertices[num_vertices] = p1;
- num_vertices++;
- vertices[num_vertices] = p2;
- num_vertices++;
- vertices[num_vertices] = p3;
- num_vertices++;
-
- if(num_tris >= num_tris_allocated)
- {
- Triangle* temp = new Triangle[num_tris_allocated * 2];
- if(!temp)
- {
- std::cerr << "BVH Error! Out of memory for tri_indices array on addTriangle() call!" << std::endl;
- return BVH_ERR_MODEL_OUT_OF_MEMORY;
- }
-
- memcpy(temp, tri_indices, sizeof(Triangle) * num_tris);
- delete [] tri_indices;
- tri_indices = temp;
- num_tris_allocated *= 2;
- }
-
- tri_indices[num_tris] = Triangle(offset, offset + 1, offset + 2);
-
- return BVH_OK;
-}
-
-template<typename BV>
-int BVHModel<BV>::addSubModel(const std::vector<Vec3f>& ps)
-{
- if(build_state == BVH_BUILD_STATE_PROCESSED)
- {
- std::cerr << "BVH Warning! Call addSubModel() in a wrong order. addSubModel() was ignored. Must do a beginModel() to clear the model for addition of new vertices." << std::endl;
- return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
- }
-
- int num_vertices_to_add = ps.size();
-
- if(num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated)
- {
- Vec3f* temp = new Vec3f[num_vertices_allocated * 2 + num_vertices_to_add - 1];
- if(!temp)
- {
- std::cerr << "BVH Error! Out of memory for vertices array on addSubModel() call!" << std::endl;
- return BVH_ERR_MODEL_OUT_OF_MEMORY;
- }
-
- memcpy(temp, vertices, sizeof(Vec3f) * num_vertices);
- delete [] vertices;
- vertices = temp;
- num_vertices_allocated = num_vertices_allocated * 2 + num_vertices_to_add - 1;
- }
-
- for(int i = 0; i < num_vertices_to_add; ++i)
- {
- vertices[num_vertices] = ps[i];
- num_vertices++;
- }
-
- return BVH_OK;
-}
-
-template<typename BV>
-int BVHModel<BV>::addSubModel(const std::vector<Vec3f>& ps, const std::vector<Triangle>& ts)
-{
- if(build_state == BVH_BUILD_STATE_PROCESSED)
- {
- std::cerr << "BVH Warning! Call addSubModel() in a wrong order. addSubModel() was ignored. Must do a beginModel() to clear the model for addition of new vertices." << std::endl;
- return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
- }
-
- int num_vertices_to_add = ps.size();
-
- if(num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated)
- {
- Vec3f* temp = new Vec3f[num_vertices_allocated * 2 + num_vertices_to_add - 1];
- if(!temp)
- {
- std::cerr << "BVH Error! Out of memory for vertices array on addSubModel() call!" << std::endl;
- return BVH_ERR_MODEL_OUT_OF_MEMORY;
- }
-
- memcpy(temp, vertices, sizeof(Vec3f) * num_vertices);
- delete [] vertices;
- vertices = temp;
- num_vertices_allocated = num_vertices_allocated * 2 + num_vertices_to_add - 1;
- }
-
- int offset = num_vertices;
-
- for(int i = 0; i < num_vertices_to_add; ++i)
- {
- vertices[num_vertices] = ps[i];
- num_vertices++;
- }
-
-
- int num_tris_to_add = ts.size();
-
- if(num_tris + num_tris_to_add - 1 >= num_tris_allocated)
- {
- Triangle* temp = new Triangle[num_tris_allocated * 2 + num_tris_to_add - 1];
- if(!temp)
- {
- std::cerr << "BVH Error! Out of memory for tri_indices array on addSubModel() call!" << std::endl;
- return BVH_ERR_MODEL_OUT_OF_MEMORY;
- }
-
- memcpy(temp, tri_indices, sizeof(Triangle) * num_tris);
- delete [] tri_indices;
- tri_indices = temp;
- num_tris_allocated = num_tris_allocated * 2 + num_tris_to_add - 1;
- }
-
- for(int i = 0; i < num_tris_to_add; ++i)
- {
- const Triangle& t = ts[i];
- tri_indices[num_tris] = Triangle(t[0] + offset, t[1] + offset, t[2] + offset);
- num_tris++;
- }
-
- return BVH_OK;
-}
-
-template<typename BV>
-int BVHModel<BV>::endModel()
-{
- if(build_state != BVH_BUILD_STATE_BEGUN)
- {
- std::cerr << "BVH Warning! Call endModel() in wrong order. endModel() was ignored." << std::endl;
- return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
- }
-
- if(num_tris == 0 && num_vertices == 0)
- {
- std::cerr << "BVH Error! endModel() called on model with no triangles and vertices." << std::endl;
- return BVH_ERR_BUILD_EMPTY_MODEL;
- }
-
- if(num_tris_allocated > num_tris)
- {
- Triangle* new_tris = new Triangle[num_tris];
- if(!new_tris)
- {
- std::cerr << "BVH Error! Out of memory for tri_indices array in endModel() call!" << std::endl;
- return BVH_ERR_MODEL_OUT_OF_MEMORY;
- }
- memcpy(new_tris, tri_indices, sizeof(Triangle) * num_tris);
- delete [] tri_indices;
- tri_indices = new_tris;
- num_tris_allocated = num_tris;
- }
-
- if(num_vertices_allocated > num_vertices)
- {
- Vec3f* new_vertices = new Vec3f[num_vertices];
- if(!new_vertices)
- {
- std::cerr << "BVH Error! Out of memory for vertices array in endModel() call!" << std::endl;
- return BVH_ERR_MODEL_OUT_OF_MEMORY;
- }
- memcpy(new_vertices, vertices, sizeof(Vec3f) * num_vertices);
- delete [] vertices;
- vertices = new_vertices;
- num_vertices_allocated = num_vertices;
- }
-
-
- // construct BVH tree
- int num_bvs_to_be_allocated = 0;
- if(num_tris == 0)
- num_bvs_to_be_allocated = 2 * num_vertices - 1;
- else
- num_bvs_to_be_allocated = 2 * num_tris - 1;
-
-
- bvs = new BVNode<BV> [num_bvs_to_be_allocated];
- primitive_indices = new unsigned int [num_bvs_to_be_allocated];
- if(!bvs || !primitive_indices)
- {
- std::cerr << "BVH Error! Out of memory for BV array in endModel()!" << std::endl;
- return BVH_ERR_MODEL_OUT_OF_MEMORY;
- }
- num_bvs_allocated = num_bvs_to_be_allocated;
- num_bvs = 0;
-
- buildTree();
-
- // finish constructing
- build_state = BVH_BUILD_STATE_PROCESSED;
-
- return BVH_OK;
-}
-
-
-
-template<typename BV>
-int BVHModel<BV>::beginReplaceModel()
-{
- if(build_state != BVH_BUILD_STATE_PROCESSED)
- {
- std::cerr << "BVH Error! Call beginReplaceModel() on a BVHModel that has no previous frame." << std::endl;
- return BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME;
- }
-
- if(prev_vertices) delete [] prev_vertices; prev_vertices = NULL;
-
- num_vertex_updated = 0;
-
- build_state = BVH_BUILD_STATE_REPLACE_BEGUN;
-
- return BVH_OK;
-}
-
-template<typename BV>
-int BVHModel<BV>::replaceVertex(const Vec3f& p)
-{
- if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN)
- {
- std::cerr << "BVH Warning! Call replaceVertex() in a wrong order. replaceVertex() was ignored. Must do a beginReplaceModel() for initialization." << std::endl;
- return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
- }
-
- vertices[num_vertex_updated] = p;
- num_vertex_updated++;
-
- return BVH_OK;
-}
-
-template<typename BV>
-int BVHModel<BV>::replaceTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3)
-{
- if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN)
- {
- std::cerr << "BVH Warning! Call replaceTriangle() in a wrong order. replaceTriangle() was ignored. Must do a beginReplaceModel() for initialization." << std::endl;
- return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
- }
-
- vertices[num_vertex_updated] = p1; num_vertex_updated++;
- vertices[num_vertex_updated] = p2; num_vertex_updated++;
- vertices[num_vertex_updated] = p3; num_vertex_updated++;
- return BVH_OK;
-}
-
-template<typename BV>
-int BVHModel<BV>::replaceSubModel(const std::vector<Vec3f>& ps)
-{
- if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN)
- {
- std::cerr << "BVH Warning! Call replaceSubModel() in a wrong order. replaceSubModel() was ignored. Must do a beginReplaceModel() for initialization." << std::endl;
- return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
- }
-
- for(unsigned int i = 0; i < ps.size(); ++i)
- {
- vertices[num_vertex_updated] = ps[i];
- num_vertex_updated++;
- }
- return BVH_OK;
-}
-
-template<typename BV>
-int BVHModel<BV>::endReplaceModel(bool refit, bool bottomup)
-{
- if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN)
- {
- std::cerr << "BVH Warning! Call endReplaceModel() in a wrong order. endReplaceModel() was ignored. " << std::endl;
- return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
- }
-
- if(num_vertex_updated != num_vertices)
- {
- std::cerr << "BVH Error! The replaced model should have the same number of vertices as the old model." << std::endl;
- return BVH_ERR_INCORRECT_DATA;
- }
-
- if(refit) // refit, do not change BVH structure
- {
- refitTree(bottomup);
- }
- else // reconstruct bvh tree based on current frame data
- {
- buildTree();
- }
-
- build_state = BVH_BUILD_STATE_PROCESSED;
-
- return BVH_OK;
-}
-
-
-
-
-
-template<typename BV>
-int BVHModel<BV>::beginUpdateModel()
-{
- if(build_state != BVH_BUILD_STATE_PROCESSED && build_state != BVH_BUILD_STATE_UPDATED)
- {
- std::cerr << "BVH Error! Call beginUpdatemodel() on a BVHModel that has no previous frame." << std::endl;
- return BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME;
- }
-
- if(prev_vertices)
- {
- Vec3f* temp = prev_vertices;
- prev_vertices = vertices;
- vertices = temp;
- }
- else
- {
- prev_vertices = vertices;
- vertices = new Vec3f[num_vertices];
- }
-
- num_vertex_updated = 0;
-
- build_state = BVH_BUILD_STATE_UPDATE_BEGUN;
-
- return BVH_OK;
-}
-
-template<typename BV>
-int BVHModel<BV>::updateVertex(const Vec3f& p)
-{
- if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN)
- {
- std::cerr << "BVH Warning! Call updateVertex() in a wrong order. updateVertex() was ignored. Must do a beginUpdateModel() for initialization." << std::endl;
- return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
- }
-
- vertices[num_vertex_updated] = p;
- num_vertex_updated++;
-
- return BVH_OK;
-}
-
-template<typename BV>
-int BVHModel<BV>::updateTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3)
-{
- if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN)
- {
- std::cerr << "BVH Warning! Call updateTriangle() in a wrong order. updateTriangle() was ignored. Must do a beginUpdateModel() for initialization." << std::endl;
- return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
- }
-
- vertices[num_vertex_updated] = p1; num_vertex_updated++;
- vertices[num_vertex_updated] = p2; num_vertex_updated++;
- vertices[num_vertex_updated] = p3; num_vertex_updated++;
- return BVH_OK;
-}
-
-template<typename BV>
-int BVHModel<BV>::updateSubModel(const std::vector<Vec3f>& ps)
-{
- if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN)
- {
- std::cerr << "BVH Warning! Call updateSubModel() in a wrong order. updateSubModel() was ignored. Must do a beginUpdateModel() for initialization." << std::endl;
- return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
- }
-
- for(unsigned int i = 0; i < ps.size(); ++i)
- {
- vertices[num_vertex_updated] = ps[i];
- num_vertex_updated++;
- }
- return BVH_OK;
-}
-
-template<typename BV>
-int BVHModel<BV>::endUpdateModel(bool refit, bool bottomup)
-{
- if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN)
- {
- std::cerr << "BVH Warning! Call endUpdateModel() in a wrong order. endUpdateModel() was ignored. " << std::endl;
- return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
- }
-
- if(num_vertex_updated != num_vertices)
- {
- std::cerr << "BVH Error! The updated model should have the same number of vertices as the old model." << std::endl;
- return BVH_ERR_INCORRECT_DATA;
- }
-
- if(refit) // refit, do not change BVH structure
- {
- refitTree(bottomup);
- }
- else // reconstruct bvh tree based on current frame data
- {
- buildTree();
-
- // then refit
-
- refitTree(bottomup);
- }
-
-
- build_state = BVH_BUILD_STATE_UPDATED;
-
- return BVH_OK;
-}
-
-
-
-
-template<typename BV>
-int BVHModel<BV>::memUsage(int msg) const
-{
- int mem_bv_list = sizeof(BV) * num_bvs;
- int mem_tri_list = sizeof(Triangle) * num_tris;
- int mem_vertex_list = sizeof(Vec3f) * num_vertices;
-
- int total_mem = mem_bv_list + mem_tri_list + mem_vertex_list + sizeof(BVHModel<BV>);
- if(msg)
- {
- std::cerr << "Total for model " << total_mem << " bytes." << std::endl;
- std::cerr << "BVs: " << num_bvs << " allocated." << std::endl;
- std::cerr << "Tris: " << num_tris << " allocated." << std::endl;
- std::cerr << "Vertices: " << num_vertices << " allocated." << std::endl;
- }
-
- return BVH_OK;
-}
-
-
-template<typename BV>
-int BVHModel<BV>::buildTree()
-{
- // set BVFitter
- bv_fitter->set(vertices, tri_indices, getModelType());
- // set SplitRule
- bv_splitter->set(vertices, tri_indices, getModelType());
-
- num_bvs = 1;
-
- int num_primitives = 0;
- switch(getModelType())
- {
- case BVH_MODEL_TRIANGLES:
- num_primitives = num_tris;
- break;
- case BVH_MODEL_POINTCLOUD:
- num_primitives = num_vertices;
- break;
- default:
- std::cerr << "BVH Error: Model type not supported!" << std::endl;
- return BVH_ERR_UNSUPPORTED_FUNCTION;
- }
-
- for(int i = 0; i < num_primitives; ++i)
- primitive_indices[i] = i;
- recursiveBuildTree(0, 0, num_primitives);
-
- bv_fitter->clear();
- bv_splitter->clear();
-
- return BVH_OK;
-}
-
-template<typename BV>
-int BVHModel<BV>::recursiveBuildTree(int bv_id, int first_primitive, int num_primitives)
-{
- BVHModelType type = getModelType();
- BVNode<BV>* bvnode = bvs + bv_id;
- unsigned int* cur_primitive_indices = primitive_indices + first_primitive;
-
- // constructing BV
- BV bv = bv_fitter->fit(cur_primitive_indices, num_primitives);
- bv_splitter->computeRule(bv, cur_primitive_indices, num_primitives);
-
- bvnode->bv = bv;
- bvnode->first_primitive = first_primitive;
- bvnode->num_primitives = num_primitives;
-
- if(num_primitives == 1)
- {
- bvnode->first_child = -((*cur_primitive_indices) + 1);
- }
- else
- {
- bvnode->first_child = num_bvs;
- num_bvs += 2;
-
- int c1 = 0;
- for(int i = 0; i < num_primitives; ++i)
- {
- Vec3f p;
- if(type == BVH_MODEL_POINTCLOUD) p = vertices[cur_primitive_indices[i]];
- else if(type == BVH_MODEL_TRIANGLES)
- {
- const Triangle& t = tri_indices[cur_primitive_indices[i]];
- const Vec3f& p1 = vertices[t[0]];
- const Vec3f& p2 = vertices[t[1]];
- const Vec3f& p3 = vertices[t[2]];
- BVH_REAL x = (p1[0] + p2[0] + p3[0]) / 3;
- BVH_REAL y = (p1[1] + p2[1] + p3[1]) / 3;
- BVH_REAL z = (p1[2] + p2[2] + p3[2]) / 3;
- p = Vec3f(x, y, z);
- }
- else
- {
- std::cerr << "BVH Error: Model type not supported!" << std::endl;
- return BVH_ERR_UNSUPPORTED_FUNCTION;
- }
-
-
- // loop invariant: up to (but not including) index c1 in group 1,
- // then up to (but not including) index i in group 2
- //
- // [1] [1] [1] [1] [2] [2] [2] [x] [x] ... [x]
- // c1 i
- //
- if(bv_splitter->apply(p)) // in the right side
- {
- // do nothing
- }
- else
- {
- unsigned int temp = cur_primitive_indices[i];
- cur_primitive_indices[i] = cur_primitive_indices[c1];
- cur_primitive_indices[c1] = temp;
- c1++;
- }
- }
-
-
- if((c1 == 0) || (c1 == num_primitives)) c1 = num_primitives / 2;
-
- int num_first_half = c1;
-
- recursiveBuildTree(bvnode->leftChild(), first_primitive, num_first_half);
- recursiveBuildTree(bvnode->rightChild(), first_primitive + num_first_half, num_primitives - num_first_half);
- }
-
- return BVH_OK;
-}
-
-template<typename BV>
-int BVHModel<BV>::refitTree(bool bottomup)
-{
- if(bottomup)
- return refitTree_bottomup();
- else
- return refitTree_topdown();
-}
-
-template<typename BV>
-int BVHModel<BV>::refitTree_bottomup()
-{
- int res = recursiveRefitTree_bottomup(0);
-
- return res;
-}
-
-
-template<typename BV>
-int BVHModel<BV>::recursiveRefitTree_bottomup(int bv_id)
-{
- BVNode<BV>* bvnode = bvs + bv_id;
- if(bvnode->isLeaf())
- {
- BVHModelType type = getModelType();
- int primitive_id = -(bvnode->first_child + 1);
- if(type == BVH_MODEL_POINTCLOUD)
- {
- BV bv;
-
- if(prev_vertices)
- {
- Vec3f v[2];
- v[0] = prev_vertices[primitive_id];
- v[1] = vertices[primitive_id];
- fit(v, 2, bv);
- }
- else
- fit(vertices + primitive_id, 1, bv);
-
- bvnode->bv = bv;
- }
- else if(type == BVH_MODEL_TRIANGLES)
- {
- BV bv;
- const Triangle& triangle = tri_indices[primitive_id];
-
- if(prev_vertices)
- {
- Vec3f v[6];
- for(int i = 0; i < 3; ++i)
- {
- v[i] = prev_vertices[triangle[i]];
- v[i + 3] = vertices[triangle[i]];
- }
-
- fit(v, 6, bv);
- }
- else
- {
- Vec3f v[3];
- for(int i = 0; i < 3; ++i)
- {
- v[i] = vertices[triangle[i]];
- }
-
- fit(v, 3, bv);
- }
-
- bvnode->bv = bv;
- }
- else
- {
- std::cerr << "BVH Error: Model type not supported!" << std::endl;
- return BVH_ERR_UNSUPPORTED_FUNCTION;
- }
- }
- else
- {
- recursiveRefitTree_bottomup(bvnode->leftChild());
- recursiveRefitTree_bottomup(bvnode->rightChild());
- bvnode->bv = bvs[bvnode->leftChild()].bv + bvs[bvnode->rightChild()].bv;
- }
-
- return BVH_OK;
-}
-
-template<typename BV>
-int BVHModel<BV>::refitTree_topdown()
-{
- bv_fitter->set(vertices, prev_vertices, tri_indices, getModelType());
- for(int i = 0; i < num_bvs; ++i)
- {
- BV bv = bv_fitter->fit(primitive_indices + bvs[i].first_primitive, bvs[i].num_primitives);
- bvs[i].bv = bv;
- }
-
- bv_fitter->clear();
-
- return BVH_OK;
-}
-
-template<typename BV>
-void BVHModel<BV>::computeAABB()
-{
- AABB aabb_;
- for(int i = 0; i < num_vertices; ++i)
- {
- aabb_ += vertices[i];
- }
-
- aabb = aabb_;
-
- aabb_cache = aabb;
-
- aabb_center = aabb.center();
-
- aabb_radius = 0;
- for(int i = 0; i < num_vertices; ++i)
- {
- BVH_REAL r = (aabb_center - vertices[i]).sqrLength();
- if(r > aabb_radius) aabb_radius = r;
- }
-
- aabb_radius = sqrt(aabb_radius);
-}
-
-template<>
-NODE_TYPE BVHModel<AABB>::getNodeType() const
-{
- return BV_AABB;
-}
-
-template<>
-NODE_TYPE BVHModel<OBB>::getNodeType() const
-{
- return BV_OBB;
-}
-
-template<>
-NODE_TYPE BVHModel<RSS>::getNodeType() const
-{
- return BV_RSS;
-}
-
-template<>
-NODE_TYPE BVHModel<KDOP<16> >::getNodeType() const
-{
- return BV_KDOP16;
-}
-
-template<>
-NODE_TYPE BVHModel<KDOP<18> >::getNodeType() const
-{
- return BV_KDOP18;
-}
-
-template<>
-NODE_TYPE BVHModel<KDOP<24> >::getNodeType() const
-{
- return BV_KDOP24;
-}
-
-
-
-template class BVHModel<KDOP<16> >;
-template class BVHModel<KDOP<18> >;
-template class BVHModel<KDOP<24> >;
-template class BVHModel<OBB>;
-template class BVHModel<AABB>;
-template class BVHModel<RSS>;
-
-}
diff --git a/branches/point_cloud/fcl/src/BVH_utility.cpp b/branches/point_cloud/fcl/src/BVH_utility.cpp
deleted file mode 100644
index e76d0b038e613a98ce07ec7147c949618c0f0e2e..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/src/BVH_utility.cpp
+++ /dev/null
@@ -1,963 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#include "fcl/BVH_utility.h"
-#include <ann/ANN.h>
-
-namespace fcl
-{
-
-void BVHExpand(BVHModel<OBB>& model, const Uncertainty* ucs, BVH_REAL r = 1.0)
-{
- for(int i = 0; i < model.getNumBVs(); ++i)
- {
- BVNode<OBB>& bvnode = model.getBV(i);
-
- Vec3f* vs = new Vec3f[bvnode.num_primitives * 6];
-
- for(int j = 0; j < bvnode.num_primitives; ++j)
- {
- int v_id = bvnode.first_primitive + j;
- const Uncertainty& uc = ucs[v_id];
-
- Vec3f&v = model.vertices[bvnode.first_primitive + j];
-
- for(int k = 0; k < 3; ++k)
- {
- vs[6 * j + 2 * k] = v + uc.axis[k] * (r * uc.sigma[k]);
- vs[6 * j + 2 * k + 1] = v - uc.axis[k] * (r * uc.sigma[k]);
- }
- }
-
- OBB bv;
- fit(vs, bvnode.num_primitives * 6, bv);
- delete [] vs;
-
- bvnode.bv = bv;
- }
-}
-
-void BVHExpand(BVHModel<RSS>& model, const Uncertainty* ucs, BVH_REAL r = 1.0)
-{
- for(int i = 0; i < model.getNumBVs(); ++i)
- {
- BVNode<RSS>& bvnode = model.getBV(i);
-
- Vec3f* vs = new Vec3f[bvnode.num_primitives * 6];
-
- for(int j = 0; j < bvnode.num_primitives; ++j)
- {
- int v_id = bvnode.first_primitive + j;
- const Uncertainty& uc = ucs[v_id];
-
- Vec3f&v = model.vertices[bvnode.first_primitive + j];
-
- for(int k = 0; k < 3; ++k)
- {
- vs[6 * j + 2 * k] = v + uc.axis[k] * (r * uc.sigma[k]);
- vs[6 * j + 2 * k + 1] = v - uc.axis[k] * (r * uc.sigma[k]);
- }
- }
-
- RSS bv;
- fit(vs, bvnode.num_primitives * 6, bv);
-
- delete [] vs;
-
- bvnode.bv = bv;
- }
-}
-
-
-void estimateSamplingUncertainty(Vec3f* vertices, int num_vertices, Uncertainty* ucs)
-{
- int nPts = num_vertices;
- ANNpointArray dataPts;
- ANNpoint queryPt;
-
- ANNidxArray nnIdx;
- ANNdistArray dists;
- ANNkd_tree* kdTree;
-
- int knn_k = 10;
- if(knn_k > nPts) knn_k = nPts;
- int dim = 3;
-
- queryPt = annAllocPt(dim);
- dataPts = annAllocPts(nPts, dim);
- nnIdx = new ANNidx[knn_k];
- dists = new ANNdist[knn_k];
-
- for(int i = 0; i < nPts; ++i)
- {
- for(int j = 0; j < 3; ++j)
- dataPts[i][j] = vertices[i][j];
- }
-
- kdTree = new ANNkd_tree(dataPts, nPts, dim);
-
- double eps = 0;
- double scale = 2;
-
- for(int i = 0; i < nPts; ++i)
- {
- float C[3][3];
- for(int j = 0; j < 3; ++j)
- {
- for(int k = 0; k < 3; ++k)
- C[j][k] = 0;
- }
-
- for(int j = 0; j < 3; ++j)
- queryPt[j] = vertices[i][j];
-
- kdTree->annkSearch(queryPt, knn_k, nnIdx, dists, eps);
-
- for(int j = 0; j < knn_k; ++j)
- dists[j] = sqrt(dists[j]);
-
- double r = dists[knn_k - 1];
- double sigma = scale * r;
-
-
- double weight_sum = 0;
- for(int j = 1; j < knn_k; ++j)
- {
- int id = nnIdx[j];
- Vec3f p = vertices[i] - vertices[id];
- double norm2p = p.sqrLength();
- double weight = exp(-norm2p / (sigma * sigma));
-
- weight_sum += weight;
-
- for(int k = 0; k < 3; ++k)
- {
- for(int l = 0; l < 3; ++l)
- C[k][l] += p[k] * p[l] * weight;
- }
- }
-
- for(int j = 0; j < 3; ++j)
- {
- for(int k = 0; k < 3; ++k)
- {
- C[j][k] /= weight_sum;
- ucs[i].Sigma[j][k] = C[j][k];
- }
- }
-
- ucs[i].preprocess();
- ucs[i].sqrt();
-
- /*
- sigma = dists[0] * 2 * r;
- ucs[i].axis[0] = Vec3f(1, 0, 0);
- ucs[i].axis[1] = Vec3f(0, 1, 0);
- ucs[i].axis[2] = Vec3f(0, 0, 1);
- ucs[i].sigma[0] = sigma;
- ucs[i].sigma[1] = sigma;
- ucs[i].sigma[2] = sigma;
-
- ucs[i].Sigma[0] = Vec3f(sigma, 0, 0);
- ucs[i].Sigma[1] = Vec3f(0, sigma, 0);
- ucs[i].Sigma[2] = Vec3f(0, 0, sigma);
- */
-
- }
-
- delete [] nnIdx;
- delete [] dists;
- delete kdTree;
- annClose();
-}
-
-/** \brief Compute the covariance matrix for a set or subset of points. */
-void getCovariance(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, Vec3f M[3])
-{
- bool indirect_index = true;
- if(!indices) indirect_index = false;
-
- Vec3f S1;
- Vec3f S2[3];
-
- for(int i = 0; i < n; ++i)
- {
- const Vec3f& p = indirect_index ? ps[indices[i]] : ps[i];
- S1 += p;
- S2[0][0] += (p[0] * p[0]);
- S2[1][1] += (p[1] * p[1]);
- S2[2][2] += (p[2] * p[2]);
- S2[0][1] += (p[0] * p[1]);
- S2[0][2] += (p[0] * p[2]);
- S2[1][2] += (p[1] * p[2]);
-
- if(ps2) // another frame
- {
- const Vec3f& p = indirect_index ? ps2[indices[i]] : ps2[i];
- S1 += p;
- S2[0][0] += (p[0] * p[0]);
- S2[1][1] += (p[1] * p[1]);
- S2[2][2] += (p[2] * p[2]);
- S2[0][1] += (p[0] * p[1]);
- S2[0][2] += (p[0] * p[2]);
- S2[1][2] += (p[1] * p[2]);
- }
- }
-
- int n_points = ((ps2 == NULL) ? n : 2*n);
-
- M[0][0] = S2[0][0] - S1[0]*S1[0] / n_points;
- M[1][1] = S2[1][1] - S1[1]*S1[1] / n_points;
- M[2][2] = S2[2][2] - S1[2]*S1[2] / n_points;
- M[0][1] = S2[0][1] - S1[0]*S1[1] / n_points;
- M[1][2] = S2[1][2] - S1[1]*S1[2] / n_points;
- M[0][2] = S2[0][2] - S1[0]*S1[2] / n_points;
- M[1][0] = M[0][1];
- M[2][0] = M[0][2];
- M[2][1] = M[1][2];
-}
-
-/** \brief Compute the covariance matrix for a set or subset of triangles. */
-void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f M[3])
-{
- bool indirect_index = true;
- if(!indices) indirect_index = false;
-
- Vec3f S1;
- Vec3f S2[3];
-
- for(int i = 0; i < n; ++i)
- {
- const Triangle& t = indirect_index ? ts[indices[i]] : ts[i];
-
- const Vec3f& p1 = ps[t[0]];
- const Vec3f& p2 = ps[t[1]];
- const Vec3f& p3 = ps[t[2]];
-
- S1[0] += (p1[0] + p2[0] + p3[0]);
- S1[1] += (p1[1] + p2[1] + p3[1]);
- S1[2] += (p1[2] + p2[2] + p3[2]);
-
- S2[0][0] += (p1[0] * p1[0] +
- p2[0] * p2[0] +
- p3[0] * p3[0]);
- S2[1][1] += (p1[1] * p1[1] +
- p2[1] * p2[1] +
- p3[1] * p3[1]);
- S2[2][2] += (p1[2] * p1[2] +
- p2[2] * p2[2] +
- p3[2] * p3[2]);
- S2[0][1] += (p1[0] * p1[1] +
- p2[0] * p2[1] +
- p3[0] * p3[1]);
- S2[0][2] += (p1[0] * p1[2] +
- p2[0] * p2[2] +
- p3[0] * p3[2]);
- S2[1][2] += (p1[1] * p1[2] +
- p2[1] * p2[2] +
- p3[1] * p3[2]);
-
- if(ps2)
- {
- const Vec3f& p1 = ps2[t[0]];
- const Vec3f& p2 = ps2[t[1]];
- const Vec3f& p3 = ps2[t[2]];
-
- S1[0] += (p1[0] + p2[0] + p3[0]);
- S1[1] += (p1[1] + p2[1] + p3[1]);
- S1[2] += (p1[2] + p2[2] + p3[2]);
-
- S2[0][0] += (p1[0] * p1[0] +
- p2[0] * p2[0] +
- p3[0] * p3[0]);
- S2[1][1] += (p1[1] * p1[1] +
- p2[1] * p2[1] +
- p3[1] * p3[1]);
- S2[2][2] += (p1[2] * p1[2] +
- p2[2] * p2[2] +
- p3[2] * p3[2]);
- S2[0][1] += (p1[0] * p1[1] +
- p2[0] * p2[1] +
- p3[0] * p3[1]);
- S2[0][2] += (p1[0] * p1[2] +
- p2[0] * p2[2] +
- p3[0] * p3[2]);
- S2[1][2] += (p1[1] * p1[2] +
- p2[1] * p2[2] +
- p3[1] * p3[2]);
- }
- }
-
- int n_points = ((ps2 == NULL) ? 3*n : 6*n);
-
- M[0][0] = S2[0][0] - S1[0]*S1[0] / n_points;
- M[1][1] = S2[1][1] - S1[1]*S1[1] / n_points;
- M[2][2] = S2[2][2] - S1[2]*S1[2] / n_points;
- M[0][1] = S2[0][1] - S1[0]*S1[1] / n_points;
- M[1][2] = S2[1][2] - S1[1]*S1[2] / n_points;
- M[0][2] = S2[0][2] - S1[0]*S1[2] / n_points;
- M[1][0] = M[0][1];
- M[2][0] = M[0][2];
- M[2][1] = M[1][2];
-}
-
-
-/** \brief Compute the RSS bounding volume parameters: radius, rectangle size and the origin.
- * The bounding volume axes are known.
- */
-void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, Vec3f axis[3], Vec3f& origin, BVH_REAL l[2], BVH_REAL& r)
-{
- bool indirect_index = true;
- if(!indices) indirect_index = false;
-
- int size_P = (ps2 == NULL) ? n : (2 * n);
-
- BVH_REAL (*P)[3] = new BVH_REAL[size_P][3];
-
-
- int P_id = 0;
- for(int i = 0; i < n; ++i)
- {
- int index = indirect_index ? indices[i] : i;
-
- const Vec3f& p = ps[index];
- Vec3f v(p[0], p[1], p[2]);
- P[P_id][0] = axis[0].dot(v);
- P[P_id][1] = axis[1].dot(v);
- P[P_id][2] = axis[2].dot(v);
- P_id++;
-
- if(ps2)
- {
- const Vec3f& v = ps2[index];
- P[P_id][0] = axis[0].dot(v);
- P[P_id][1] = axis[1].dot(v);
- P[P_id][2] = axis[2].dot(v);
- P_id++;
- }
- }
-
- BVH_REAL minx, maxx, miny, maxy, minz, maxz;
-
- BVH_REAL cz, radsqr;
-
- minz = maxz = P[0][2];
-
- for(int i = 1; i < size_P; ++i)
- {
- BVH_REAL z_value = P[i][2];
- if(z_value < minz) minz = z_value;
- else if(z_value > maxz) maxz = z_value;
- }
-
- r = (BVH_REAL)0.5 * (maxz - minz);
- radsqr = r * r;
- cz = (BVH_REAL)0.5 * (maxz + minz);
-
- // compute an initial length of rectangle along x direction
-
- // find minx and maxx as starting points
-
- int minindex, maxindex;
- minindex = maxindex = 0;
- BVH_REAL mintmp, maxtmp;
- mintmp = maxtmp = P[0][0];
-
- for(int i = 1; i < size_P; ++i)
- {
- BVH_REAL x_value = P[i][0];
- if(x_value < mintmp)
- {
- minindex = i;
- mintmp = x_value;
- }
- else if(x_value > maxtmp)
- {
- maxindex = i;
- maxtmp = x_value;
- }
- }
-
- BVH_REAL x, dz;
- dz = P[minindex][2] - cz;
- minx = P[minindex][0] + sqrt(std::max(radsqr - dz * dz, 0.0));
- dz = P[maxindex][2] - cz;
- maxx = P[maxindex][0] - sqrt(std::max(radsqr - dz * dz, 0.0));
-
-
- // grow minx
-
- for(int i = 0; i < size_P; ++i)
- {
- if(P[i][0] < minx)
- {
- dz = P[i][2] - cz;
- x = P[i][0] + sqrt(std::max(radsqr - dz * dz, 0.0));
- if(x < minx) minx = x;
- }
- }
-
- // grow maxx
-
- for(int i = 0; i < size_P; ++i)
- {
- if(P[i][0] > maxx)
- {
- dz = P[i][2] - cz;
- x = P[i][0] - sqrt(std::max(radsqr - dz * dz, 0.0));
- if(x > maxx) maxx = x;
- }
- }
-
- // compute an initial length of rectangle along y direction
-
- // find miny and maxy as starting points
-
- minindex = maxindex = 0;
- mintmp = maxtmp = P[0][1];
- for(int i = 1; i < size_P; ++i)
- {
- BVH_REAL y_value = P[i][1];
- if(y_value < mintmp)
- {
- minindex = i;
- mintmp = y_value;
- }
- else if(y_value > maxtmp)
- {
- maxindex = i;
- maxtmp = y_value;
- }
- }
-
- BVH_REAL y;
- dz = P[minindex][2] - cz;
- miny = P[minindex][1] + sqrt(std::max(radsqr - dz * dz, 0.0));
- dz = P[maxindex][2] - cz;
- maxy = P[maxindex][1] - sqrt(std::max(radsqr - dz * dz, 0.0));
-
- // grow miny
-
- for(int i = 0; i < size_P; ++i)
- {
- if(P[i][1] < miny)
- {
- dz = P[i][2] - cz;
- y = P[i][1] + sqrt(std::max(radsqr - dz * dz, 0.0));
- if(y < miny) miny = y;
- }
- }
-
- // grow maxy
-
- for(int i = 0; i < size_P; ++i)
- {
- if(P[i][1] > maxy)
- {
- dz = P[i][2] - cz;
- y = P[i][1] - sqrt(std::max(radsqr - dz * dz, 0.0));
- if(y > maxy) maxy = y;
- }
- }
-
- // corners may have some points which are not covered - grow lengths if necessary
- // quite conservative (can be improved)
-
- BVH_REAL dx, dy, u, t;
- BVH_REAL a = sqrt((BVH_REAL)0.5);
- for(int i = 0; i < size_P; ++i)
- {
- if(P[i][0] > maxx)
- {
- if(P[i][1] > maxy)
- {
- dx = P[i][0] - maxx;
- dy = P[i][1] - maxy;
- u = dx * a + dy * a;
- t = (a*u - dx)*(a*u - dx) +
- (a*u - dy)*(a*u - dy) +
- (cz - P[i][2])*(cz - P[i][2]);
- u = u - sqrt(std::max(radsqr - t, 0.0));
- if(u > 0)
- {
- maxx += u*a;
- maxy += u*a;
- }
- }
- else if(P[i][1] < miny)
- {
- dx = P[i][0] - maxx;
- dy = P[i][1] - miny;
- u = dx * a - dy * a;
- t = (a*u - dx)*(a*u - dx) +
- (-a*u - dy)*(-a*u - dy) +
- (cz - P[i][2])*(cz - P[i][2]);
- u = u - sqrt(std::max(radsqr - t, 0.0));
- if(u > 0)
- {
- maxx += u*a;
- miny -= u*a;
- }
- }
- }
- else if(P[i][0] < minx)
- {
- if(P[i][1] > maxy)
- {
- dx = P[i][0] - minx;
- dy = P[i][1] - maxy;
- u = dy * a - dx * a;
- t = (-a*u - dx)*(-a*u - dx) +
- (a*u - dy)*(a*u - dy) +
- (cz - P[i][2])*(cz - P[i][2]);
- u = u - sqrt(std::max(radsqr - t, 0.0));
- if(u > 0)
- {
- minx -= u*a;
- maxy += u*a;
- }
- }
- else if(P[i][1] < miny)
- {
- dx = P[i][0] - minx;
- dy = P[i][1] - miny;
- u = -dx * a - dy * a;
- t = (-a*u - dx)*(-a*u - dx) +
- (-a*u - dy)*(-a*u - dy) +
- (cz - P[i][2])*(cz - P[i][2]);
- u = u - sqrt(std::max(radsqr - t, 0.0));
- if (u > 0)
- {
- minx -= u*a;
- miny -= u*a;
- }
- }
- }
- }
-
- origin = axis[0] * minx + axis[1] * miny + axis[2] * cz;
-
- l[0] = maxx - minx;
- if(l[0] < 0) l[0] = 0;
- l[1] = maxy - miny;
- if(l[1] < 0) l[1] = 0;
-
- delete [] P;
-}
-
-
-/** \brief Compute the RSS bounding volume parameters: radius, rectangle size and the origin.
- * The bounding volume axes are known.
- */
-void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& origin, BVH_REAL l[2], BVH_REAL& r)
-{
- bool indirect_index = true;
- if(!indices) indirect_index = false;
-
- int size_P = (ps2 == NULL) ? n * 3 : (2 * n * 3);
-
- BVH_REAL (*P)[3] = new BVH_REAL[size_P][3];
-
-
- int P_id = 0;
- for(int i = 0; i < n; ++i)
- {
- int index = indirect_index ? indices[i] : i;
- const Triangle& t = ts[index];
-
- for(int j = 0; j < 3; ++j)
- {
- int point_id = t[j];
- const Vec3f& p = ps[point_id];
- Vec3f v(p[0], p[1], p[2]);
- P[P_id][0] = axis[0].dot(v);
- P[P_id][1] = axis[1].dot(v);
- P[P_id][2] = axis[2].dot(v);
- P_id++;
- }
-
- if(ps2)
- {
- for(int j = 0; j < 3; ++j)
- {
- int point_id = t[j];
- const Vec3f& p = ps2[point_id];
- Vec3f v(p[0], p[1], p[2]);
- P[P_id][0] = axis[0].dot(v);
- P[P_id][1] = axis[0].dot(v);
- P[P_id][2] = axis[1].dot(v);
- P_id++;
- }
- }
- }
-
- BVH_REAL minx, maxx, miny, maxy, minz, maxz;
-
- BVH_REAL cz, radsqr;
-
- minz = maxz = P[0][2];
-
- for(int i = 1; i < size_P; ++i)
- {
- BVH_REAL z_value = P[i][2];
- if(z_value < minz) minz = z_value;
- else if(z_value > maxz) maxz = z_value;
- }
-
- r = (BVH_REAL)0.5 * (maxz - minz);
- radsqr = r * r;
- cz = (BVH_REAL)0.5 * (maxz + minz);
-
- // compute an initial length of rectangle along x direction
-
- // find minx and maxx as starting points
-
- int minindex, maxindex;
- minindex = maxindex = 0;
- BVH_REAL mintmp, maxtmp;
- mintmp = maxtmp = P[0][0];
-
- for(int i = 1; i < size_P; ++i)
- {
- BVH_REAL x_value = P[i][0];
- if(x_value < mintmp)
- {
- minindex = i;
- mintmp = x_value;
- }
- else if(x_value > maxtmp)
- {
- maxindex = i;
- maxtmp = x_value;
- }
- }
-
- BVH_REAL x, dz;
- dz = P[minindex][2] - cz;
- minx = P[minindex][0] + sqrt(std::max(radsqr - dz * dz, 0.0));
- dz = P[maxindex][2] - cz;
- maxx = P[maxindex][0] - sqrt(std::max(radsqr - dz * dz, 0.0));
-
-
- // grow minx
-
- for(int i = 0; i < size_P; ++i)
- {
- if(P[i][0] < minx)
- {
- dz = P[i][2] - cz;
- x = P[i][0] + sqrt(std::max(radsqr - dz * dz, 0.0));
- if(x < minx) minx = x;
- }
- }
-
- // grow maxx
-
- for(int i = 0; i < size_P; ++i)
- {
- if(P[i][0] > maxx)
- {
- dz = P[i][2] - cz;
- x = P[i][0] - sqrt(std::max(radsqr - dz * dz, 0.0));
- if(x > maxx) maxx = x;
- }
- }
-
- // compute an initial length of rectangle along y direction
-
- // find miny and maxy as starting points
-
- minindex = maxindex = 0;
- mintmp = maxtmp = P[0][1];
- for(int i = 1; i < size_P; ++i)
- {
- BVH_REAL y_value = P[i][1];
- if(y_value < mintmp)
- {
- minindex = i;
- mintmp = y_value;
- }
- else if(y_value > maxtmp)
- {
- maxindex = i;
- maxtmp = y_value;
- }
- }
-
- BVH_REAL y;
- dz = P[minindex][2] - cz;
- miny = P[minindex][1] + sqrt(std::max(radsqr - dz * dz, 0.0));
- dz = P[maxindex][2] - cz;
- maxy = P[maxindex][1] - sqrt(std::max(radsqr - dz * dz, 0.0));
-
- // grow miny
-
- for(int i = 0; i < size_P; ++i)
- {
- if(P[i][1] < miny)
- {
- dz = P[i][2] - cz;
- y = P[i][1] + sqrt(std::max(radsqr - dz * dz, 0.0));
- if(y < miny) miny = y;
- }
- }
-
- // grow maxy
-
- for(int i = 0; i < size_P; ++i)
- {
- if(P[i][1] > maxy)
- {
- dz = P[i][2] - cz;
- y = P[i][1] - sqrt(std::max(radsqr - dz * dz, 0.0));
- if(y > maxy) maxy = y;
- }
- }
-
- // corners may have some points which are not covered - grow lengths if necessary
- // quite conservative (can be improved)
- BVH_REAL dx, dy, u, t;
- BVH_REAL a = sqrt((BVH_REAL)0.5);
- for(int i = 0; i < size_P; ++i)
- {
- if(P[i][0] > maxx)
- {
- if(P[i][1] > maxy)
- {
- dx = P[i][0] - maxx;
- dy = P[i][1] - maxy;
- u = dx * a + dy * a;
- t = (a*u - dx)*(a*u - dx) +
- (a*u - dy)*(a*u - dy) +
- (cz - P[i][2])*(cz - P[i][2]);
- u = u - sqrt(std::max(radsqr - t, 0.0));
- if(u > 0)
- {
- maxx += u*a;
- maxy += u*a;
- }
- }
- else if(P[i][1] < miny)
- {
- dx = P[i][0] - maxx;
- dy = P[i][1] - miny;
- u = dx * a - dy * a;
- t = (a*u - dx)*(a*u - dx) +
- (-a*u - dy)*(-a*u - dy) +
- (cz - P[i][2])*(cz - P[i][2]);
- u = u - sqrt(std::max(radsqr - t, 0.0));
- if(u > 0)
- {
- maxx += u*a;
- miny -= u*a;
- }
- }
- }
- else if(P[i][0] < minx)
- {
- if(P[i][1] > maxy)
- {
- dx = P[i][0] - minx;
- dy = P[i][1] - maxy;
- u = dy * a - dx * a;
- t = (-a*u - dx)*(-a*u - dx) +
- (a*u - dy)*(a*u - dy) +
- (cz - P[i][2])*(cz - P[i][2]);
- u = u - sqrt(std::max(radsqr - t, 0.0));
- if(u > 0)
- {
- minx -= u*a;
- maxy += u*a;
- }
- }
- else if(P[i][1] < miny)
- {
- dx = P[i][0] - minx;
- dy = P[i][1] - miny;
- u = -dx * a - dy * a;
- t = (-a*u - dx)*(-a*u - dx) +
- (-a*u - dy)*(-a*u - dy) +
- (cz - P[i][2])*(cz - P[i][2]);
- u = u - sqrt(std::max(radsqr - t, 0.0));
- if (u > 0)
- {
- minx -= u*a;
- miny -= u*a;
- }
- }
- }
- }
-
- origin = axis[0] * minx + axis[1] * miny + axis[2] * cz;
-
- l[0] = maxx - minx;
- if(l[0] < 0) l[0] = 0;
- l[1] = maxy - miny;
- if(l[1] < 0) l[1] = 0;
-
- delete [] P;
-}
-
-/** \brief Compute the bounding volume extent and center for a set or subset of points.
- * The bounding volume axes are known.
- */
-void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, Vec3f axis[3], Vec3f& center, Vec3f& extent)
-{
- bool indirect_index = true;
- if(!indices) indirect_index = false;
-
- BVH_REAL real_max = std::numeric_limits<BVH_REAL>::max();
-
- BVH_REAL min_coord[3] = {real_max, real_max, real_max};
- BVH_REAL max_coord[3] = {-real_max, -real_max, -real_max};
-
- for(int i = 0; i < n; ++i)
- {
- int index = indirect_index ? indices[i] : i;
-
- const Vec3f& p = ps[index];
- Vec3f v(p[0], p[1], p[2]);
- BVH_REAL proj[3];
- proj[0] = axis[0].dot(v);
- proj[1] = axis[1].dot(v);
- proj[2] = axis[2].dot(v);
-
- for(int j = 0; j < 3; ++j)
- {
- if(proj[j] > max_coord[j]) max_coord[j] = proj[j];
- if(proj[j] < min_coord[j]) min_coord[j] = proj[j];
- }
-
- if(ps2)
- {
- const Vec3f& v = ps2[index];
- proj[0] = axis[0].dot(v);
- proj[1] = axis[1].dot(v);
- proj[2] = axis[2].dot(v);
-
- for(int j = 0; j < 3; ++j)
- {
- if(proj[j] > max_coord[j]) max_coord[j] = proj[j];
- if(proj[j] < min_coord[j]) min_coord[j] = proj[j];
- }
- }
- }
-
- Vec3f o = Vec3f((max_coord[0] + min_coord[0]) / 2,
- (max_coord[1] + min_coord[1]) / 2,
- (max_coord[2] + min_coord[2]) / 2);
-
- center = axis[0] * o[0] + axis[1] * o[1] + axis[2] * o[2];
-
- extent = Vec3f((max_coord[0] - min_coord[0]) / 2,
- (max_coord[1] - min_coord[1]) / 2,
- (max_coord[2] - min_coord[2]) / 2);
-
-}
-
-
-/** \brief Compute the bounding volume extent and center for a set or subset of points.
- * The bounding volume axes are known.
- */
-void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& center, Vec3f& extent)
-{
- bool indirect_index = true;
- if(!indices) indirect_index = false;
-
- BVH_REAL real_max = std::numeric_limits<BVH_REAL>::max();
-
- BVH_REAL min_coord[3] = {real_max, real_max, real_max};
- BVH_REAL max_coord[3] = {-real_max, -real_max, -real_max};
-
- for(int i = 0; i < n; ++i)
- {
- unsigned int index = indirect_index? indices[i] : i;
- const Triangle& t = ts[index];
-
- for(int j = 0; j < 3; ++j)
- {
- int point_id = t[j];
- const Vec3f& p = ps[point_id];
- Vec3f v(p[0], p[1], p[2]);
- BVH_REAL proj[3];
- proj[0] = axis[0].dot(v);
- proj[1] = axis[1].dot(v);
- proj[2] = axis[2].dot(v);
-
- for(int k = 0; k < 3; ++k)
- {
- if(proj[k] > max_coord[k]) max_coord[k] = proj[k];
- if(proj[k] < min_coord[k]) min_coord[k] = proj[k];
- }
- }
-
- if(ps2)
- {
- for(int j = 0; j < 3; ++j)
- {
- int point_id = t[j];
- const Vec3f& p = ps2[point_id];
- Vec3f v(p[0], p[1], p[2]);
- BVH_REAL proj[3];
- proj[0] = axis[0].dot(v);
- proj[1] = axis[1].dot(v);
- proj[2] = axis[2].dot(v);
-
- for(int k = 0; k < 3; ++k)
- {
- if(proj[k] > max_coord[k]) max_coord[k] = proj[k];
- if(proj[k] < min_coord[k]) min_coord[k] = proj[k];
- }
- }
- }
- }
-
- Vec3f o = Vec3f((max_coord[0] + min_coord[0]) / 2,
- (max_coord[1] + min_coord[1]) / 2,
- (max_coord[2] + min_coord[2]) / 2);
-
- center = axis[0] * o[0] + axis[1] * o[1] + axis[2] * o[2];
-
- extent = Vec3f((max_coord[0] - min_coord[0]) / 2,
- (max_coord[1] - min_coord[1]) / 2,
- (max_coord[2] - min_coord[2]) / 2);
-
-}
-
-
-
-}
diff --git a/branches/point_cloud/fcl/src/BV_fitter.cpp b/branches/point_cloud/fcl/src/BV_fitter.cpp
deleted file mode 100644
index e72bba551b99c645314c7a1aa0e7739790575701..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/src/BV_fitter.cpp
+++ /dev/null
@@ -1,474 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-#include "fcl/BV_fitter.h"
-#include "fcl/BVH_utility.h"
-#include <limits>
-
-namespace fcl
-{
-
-
-namespace OBB_fit_functions
-{
-
-void fit1(Vec3f* ps, OBB& bv)
-{
- bv.To = ps[0];
- bv.axis[0] = Vec3f(1, 0, 0);
- bv.axis[1] = Vec3f(0, 1, 0);
- bv.axis[2] = Vec3f(0, 0, 1);
- bv.extent = Vec3f(0, 0, 0);
-}
-
-void fit2(Vec3f* ps, OBB& bv)
-{
- Vec3f p1(ps[0][0], ps[0][1], ps[0][2]);
- Vec3f p2(ps[1][0], ps[1][1], ps[1][2]);
- Vec3f p1p2 = p1 - p2;
- float len_p1p2 = p1p2.length();
- Vec3f w = p1p2;
- w.normalize();
-
- // then generate other two axes orthonormal to w
- Vec3f u, v;
- float inv_length;
- if(fabs(w[0]) >= fabs(w[1]))
- {
- inv_length = 1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
- u[0] = -w[2] * inv_length;
- u[1] = 0;
- u[2] = w[0] * inv_length;
- v[0] = w[1] * u[2];
- v[1] = w[2] * u[0] - w[0] * u[2];
- v[2] = -w[1] * u[0];
- }
- else
- {
- inv_length = 1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
- u[0] = 0;
- u[1] = w[2] * inv_length;
- u[2] = -w[1] * inv_length;
- v[0] = w[1] * u[2] - w[2] * u[1];
- v[1] = -w[0] * u[2];
- v[2] = w[0] * u[1];
- }
-
- bv.axis[0] = w;
- bv.axis[1] = u;
- bv.axis[2] = v;
-
- bv.extent = Vec3f(len_p1p2 * 0.5, 0, 0);
- bv.To = Vec3f(0.5 * (p1[0] + p2[0]),
- 0.5 * (p1[1] + p2[1]),
- 0.5 * (p1[2] + p2[2]));
-
-}
-
-void fit3(Vec3f* ps, OBB& bv)
-{
- Vec3f p1(ps[0][0], ps[0][1], ps[0][2]);
- Vec3f p2(ps[1][0], ps[1][1], ps[1][2]);
- Vec3f p3(ps[2][0], ps[2][1], ps[2][2]);
- Vec3f e[3];
- e[0] = p1 - p2;
- e[1] = p2 - p3;
- e[2] = p3 - p1;
- float len[3];
- len[0] = e[0].sqrLength();
- len[1] = e[1].sqrLength();
- len[2] = e[2].sqrLength();
-
- int imax = 0;
- if(len[1] > len[0]) imax = 1;
- if(len[2] > len[imax]) imax = 2;
-
- Vec3f w = e[0].cross(e[1]);
- w.normalize();
- Vec3f u = e[imax];
- u.normalize();
- Vec3f v = w.cross(u);
- bv.axis[0] = u;
- bv.axis[1] = v;
- bv.axis[2] = w;
-
- getExtentAndCenter(ps, NULL, NULL, 3, bv.axis, bv.To, bv.extent);
-}
-
-void fit6(Vec3f* ps, OBB& bv)
-{
- OBB bv1, bv2;
- fit3(ps, bv1);
- fit3(ps + 3, bv2);
- bv = bv1 + bv2;
-}
-
-
-void fitn(Vec3f* ps, int n, OBB& bv)
-{
- Vec3f M[3]; // row first matrix
- Vec3f E[3]; // row first eigen-vectors
- BVH_REAL s[3] = {0, 0, 0}; // three eigen values
-
- getCovariance(ps, NULL, NULL, n, M);
- Meigen(M, s, E);
-
- int min, mid, max;
- if(s[0] > s[1]) { max = 0; min = 1; }
- else { min = 0; max = 1; }
- if(s[2] < s[min]) { mid = min; min = 2; }
- else if(s[2] > s[max]) { mid = max; max = 2; }
- else { mid = 2; }
-
- Vec3f R[3]; // column first matrix, as the axis in OBB
- R[0] = Vec3f(E[0][max], E[1][max], E[2][max]);
- R[1] = Vec3f(E[0][mid], E[1][mid], E[2][mid]);
- R[2] = Vec3f(E[1][max]*E[2][mid] - E[1][mid]*E[2][max],
- E[0][mid]*E[2][max] - E[0][max]*E[2][mid],
- E[0][max]*E[1][mid] - E[0][mid]*E[1][max]);
-
-
- // set obb axes
- bv.axis[0] = R[0];
- bv.axis[1] = R[1];
- bv.axis[2] = R[2];
-
- // set obb centers and extensions
- Vec3f center, extent;
- getExtentAndCenter(ps, NULL, NULL, n, R, center, extent);
-
- bv.To = center;
- bv.extent = extent;
-}
-
-}
-
-
-namespace RSS_fit_functions
-{
-void fit1(Vec3f* ps, RSS& bv)
-{
- bv.Tr = ps[0];
- bv.axis[0] = Vec3f(1, 0, 0);
- bv.axis[1] = Vec3f(0, 1, 0);
- bv.axis[2] = Vec3f(0, 0, 1);
- bv.l[0] = 0;
- bv.l[1] = 0;
- bv.r = 0;
-}
-
-void fit2(Vec3f* ps, RSS& bv)
-{
- Vec3f p1(ps[0][0], ps[0][1], ps[0][2]);
- Vec3f p2(ps[1][0], ps[1][1], ps[1][2]);
- Vec3f p1p2 = p1 - p2;
- float len_p1p2 = p1p2.length();
- Vec3f w = p1p2;
- w.normalize();
-
- // then generate other two axes orthonormal to w
- Vec3f u, v;
- float inv_length;
- if(fabs(w[0]) >= fabs(w[1]))
- {
- inv_length = 1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
- u[0] = -w[2] * inv_length;
- u[1] = 0;
- u[2] = w[0] * inv_length;
- v[0] = w[1] * u[2];
- v[1] = w[2] * u[0] - w[0] * u[2];
- v[2] = -w[1] * u[0];
- }
- else
- {
- inv_length = 1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
- u[0] = 0;
- u[1] = w[2] * inv_length;
- u[2] = -w[1] * inv_length;
- v[0] = w[1] * u[2] - w[2] * u[1];
- v[1] = -w[0] * u[2];
- v[2] = w[0] * u[1];
- }
-
- bv.axis[0] = w;
- bv.axis[1] = u;
- bv.axis[2] = v;
-
- bv.l[0] = len_p1p2;
- bv.l[1] = 0;
-
- bv.Tr = p2;
- bv.r = 0;
-}
-
-void fit3(Vec3f* ps, RSS& bv)
-{
- Vec3f p1(ps[0][0], ps[0][1], ps[0][2]);
- Vec3f p2(ps[1][0], ps[1][1], ps[1][2]);
- Vec3f p3(ps[2][0], ps[2][1], ps[2][2]);
- Vec3f e[3];
- e[0] = p1 - p2;
- e[1] = p2 - p3;
- e[2] = p3 - p1;
- float len[3];
- len[0] = e[0].sqrLength();
- len[1] = e[1].sqrLength();
- len[2] = e[2].sqrLength();
-
- int imax = 0;
- if(len[1] > len[0]) imax = 1;
- if(len[2] > len[imax]) imax = 2;
-
- Vec3f w = e[0].cross(e[1]);
- w.normalize();
- Vec3f u = e[imax];
- u.normalize();
- Vec3f v = w.cross(u);
- bv.axis[0] = u;
- bv.axis[1] = v;
- bv.axis[2] = w;
-
- getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, 3, bv.axis, bv.Tr, bv.l, bv.r);
-}
-
-void fit6(Vec3f* ps, RSS& bv)
-{
- RSS bv1, bv2;
- fit3(ps, bv1);
- fit3(ps + 3, bv2);
- bv = bv1 + bv2;
-}
-
-void fitn(Vec3f* ps, int n, RSS& bv)
-{
- Vec3f M[3]; // row first matrix
- Vec3f E[3]; // row first eigen-vectors
- BVH_REAL s[3] = {0, 0, 0};
-
- getCovariance(ps, NULL, NULL, n, M);
- Meigen(M, s, E);
-
- int min, mid, max;
- if(s[0] > s[1]) { max = 0; min = 1; }
- else { min = 0; max = 1; }
- if(s[2] < s[min]) { mid = min; min = 2; }
- else if(s[2] > s[max]) { mid = max; max = 2; }
- else { mid = 2; }
-
- Vec3f R[3]; // column first matrix, as the axis in RSS
- R[0] = Vec3f(E[0][max], E[1][max], E[2][max]);
- R[1] = Vec3f(E[0][mid], E[1][mid], E[2][mid]);
- R[2] = Vec3f(E[1][max]*E[2][mid] - E[1][mid]*E[2][max],
- E[0][mid]*E[2][max] - E[0][max]*E[2][mid],
- E[0][max]*E[1][mid] - E[0][mid]*E[1][max]);
-
- // set obb axes
- bv.axis[0] = R[0];
- bv.axis[1] = R[1];
- bv.axis[2] = R[2];
-
- // set rss origin, rectangle size and radius
- getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, n, R, bv.Tr, bv.l, bv.r);
-}
-}
-
-
-template<>
-void fit(Vec3f* ps, int n, OBB& bv)
-{
- switch(n)
- {
- case 1:
- OBB_fit_functions::fit1(ps, bv);
- break;
- case 2:
- OBB_fit_functions::fit2(ps, bv);
- break;
- case 3:
- OBB_fit_functions::fit3(ps, bv);
- break;
- case 6:
- OBB_fit_functions::fit6(ps, bv);
- break;
- default:
- OBB_fit_functions::fitn(ps, n, bv);
- }
-}
-
-
-template<>
-void fit(Vec3f* ps, int n, RSS& bv)
-{
- switch(n)
- {
- case 1:
- RSS_fit_functions::fit1(ps, bv);
- break;
- case 2:
- RSS_fit_functions::fit2(ps, bv);
- break;
- case 3:
- RSS_fit_functions::fit3(ps, bv);
- break;
- default:
- RSS_fit_functions::fitn(ps, n, bv);
- }
-}
-
-
-OBB BVFitter<OBB>::fit(unsigned int* primitive_indices, int num_primitives)
-{
- OBB bv;
-
- Vec3f M[3]; // row first matrix
- Vec3f E[3]; // row first eigen-vectors
- BVH_REAL s[3]; // three eigen values
-
- if(type == BVH_MODEL_TRIANGLES)
- {
- getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
- Meigen(M, s, E);
- }
- else if(type == BVH_MODEL_POINTCLOUD)
- {
- getCovariance(vertices, prev_vertices, primitive_indices, num_primitives, M);
- Meigen(M, s, E);
- }
-
- int min, mid, max;
- if(s[0] > s[1]) { max = 0; min = 1; }
- else { min = 0; max = 1; }
- if(s[2] < s[min]) { mid = min; min = 2; }
- else if(s[2] > s[max]) { mid = max; max = 2; }
- else { mid = 2; }
-
- Vec3f R[3]; // column first matrix, as the axis in OBB
- R[0] = Vec3f(E[0][max], E[1][max], E[2][max]);
- R[1] = Vec3f(E[0][mid], E[1][mid], E[2][mid]);
- R[2] = Vec3f(E[1][max]*E[2][mid] - E[1][mid]*E[2][max],
- E[0][mid]*E[2][max] - E[0][max]*E[2][mid],
- E[0][max]*E[1][mid] - E[0][mid]*E[1][max]);
-
-
- // set obb axes
- bv.axis[0] = R[0];
- bv.axis[1] = R[1];
- bv.axis[2] = R[2];
-
- // set obb centers and extensions
- Vec3f center, extent;
- if(type == BVH_MODEL_TRIANGLES)
- {
- getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, R, center, extent);
- }
- else if(type == BVH_MODEL_POINTCLOUD)
- {
- getExtentAndCenter(vertices, prev_vertices, primitive_indices, num_primitives, R, center, extent);
- }
-
- bv.To = center;
- bv.extent = extent;
-
- return bv;
-}
-
-
-RSS BVFitter<RSS>::fit(unsigned int* primitive_indices, int num_primitives)
-{
- RSS bv;
-
- Vec3f M[3]; // row first matrix
- Vec3f E[3]; // row first eigen-vectors
- BVH_REAL s[3]; // three eigen values
-
- if(type == BVH_MODEL_TRIANGLES)
- {
- getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
- Meigen(M, s, E);
- }
- else if(type == BVH_MODEL_POINTCLOUD)
- {
- getCovariance(vertices, prev_vertices, primitive_indices, num_primitives, M);
- Meigen(M, s, E);
- }
-
- int min, mid, max;
- if(s[0] > s[1]) { max = 0; min = 1; }
- else { min = 0; max = 1; }
- if(s[2] < s[min]) { mid = min; min = 2; }
- else if(s[2] > s[max]) { mid = max; max = 2; }
- else { mid = 2; }
-
- Vec3f R[3]; // column first matrix, as the axis in OBB
- R[0] = Vec3f(E[0][max], E[1][max], E[2][max]);
- R[1] = Vec3f(E[0][mid], E[1][mid], E[2][mid]);
- R[2] = Vec3f(E[1][max]*E[2][mid] - E[1][mid]*E[2][max],
- E[0][mid]*E[2][max] - E[0][max]*E[2][mid],
- E[0][max]*E[1][mid] - E[0][mid]*E[1][max]);
-
- // set rss axes
- bv.axis[0] = R[0];
- bv.axis[1] = R[1];
- bv.axis[2] = R[2];
-
- // set rss origin, rectangle size and radius
-
- Vec3f origin;
- BVH_REAL l[2];
- BVH_REAL r;
-
- if(type == BVH_MODEL_TRIANGLES)
- {
- getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, R, origin, l, r);
- }
- else if(type == BVH_MODEL_POINTCLOUD)
- {
- getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, primitive_indices, num_primitives, R, origin, l, r);
- }
-
- bv.Tr = origin;
- bv.l[0] = l[0];
- bv.l[1] = l[1];
- bv.r = r;
-
-
- return bv;
-}
-
-
-
-
-}
diff --git a/branches/point_cloud/fcl/src/BV_splitter.cpp b/branches/point_cloud/fcl/src/BV_splitter.cpp
deleted file mode 100644
index 46e406c42aae0012e6b3dc23fbea446eda65ca33..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/src/BV_splitter.cpp
+++ /dev/null
@@ -1,124 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-#include "fcl/BV_splitter.h"
-
-namespace fcl
-{
-
-void BVSplitter<OBB>::computeRule_bvcenter(const OBB& bv, unsigned int* primitive_indices, int num_primitives)
-{
- Vec3f center = bv.center();
- split_vector = bv.axis[0];
- split_value = center[0];
-}
-
-void BVSplitter<OBB>::computeRule_mean(const OBB& bv, unsigned int* primitive_indices, int num_primitives)
-{
- split_vector = bv.axis[0];
- BVH_REAL sum = 0;
- if(type == BVH_MODEL_TRIANGLES)
- {
- for(int i = 0; i < num_primitives; ++i)
- {
- const Triangle& t = tri_indices[primitive_indices[i]];
- const Vec3f& p1 = vertices[t[0]];
- const Vec3f& p2 = vertices[t[1]];
- const Vec3f& p3 = vertices[t[2]];
- Vec3f centroid((p1[0] + p2[0] + p3[0]) / 3,
- (p1[1] + p2[1] + p3[1]) / 3,
- (p1[2] + p2[2] + p3[2]) / 3);
-
- sum += centroid.dot(split_vector);
- }
- }
- else if(type == BVH_MODEL_POINTCLOUD)
- {
- for(int i = 0; i < num_primitives; ++i)
- {
- const Vec3f& p = vertices[primitive_indices[i]];
- Vec3f v(p[0], p[1], p[2]);
- sum += v.dot(split_vector);
- }
- }
-
- split_value = sum / num_primitives;
-}
-
-
-void BVSplitter<OBB>::computeRule_median(const OBB& bv, unsigned int* primitive_indices, int num_primitives)
-{
- split_vector = bv.axis[0];
- std::vector<BVH_REAL> proj(num_primitives);
-
- if(type == BVH_MODEL_TRIANGLES)
- {
- for(int i = 0; i < num_primitives; ++i)
- {
- const Triangle& t = tri_indices[primitive_indices[i]];
- const Vec3f& p1 = vertices[t[0]];
- const Vec3f& p2 = vertices[t[1]];
- const Vec3f& p3 = vertices[t[2]];
- Vec3f centroid((p1[0] + p2[0] + p3[0]) / 3,
- (p1[1] + p2[1] + p3[1]) / 3,
- (p1[2] + p2[2] + p3[2]) / 3);
-
- proj[i] = centroid.dot(split_vector);
- }
- }
- else if(type == BVH_MODEL_POINTCLOUD)
- {
- for(int i = 0; i < num_primitives; ++i)
- {
- const Vec3f& p = vertices[primitive_indices[i]];
- Vec3f v(p[0], p[1], p[2]);
- proj[i] = v.dot(split_vector);
- }
- }
-
- std::sort(proj.begin(), proj.end());
-
- if(num_primitives % 2 == 1)
- {
- split_value = proj[(num_primitives - 1) / 2];
- }
- else
- {
- split_value = (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2;
- }
-}
-
-}
diff --git a/branches/point_cloud/fcl/src/OBB.cpp b/branches/point_cloud/fcl/src/OBB.cpp
deleted file mode 100644
index 27d8b81c39f8e7a4e6afa4ca13bf2b19533853d5..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/src/OBB.cpp
+++ /dev/null
@@ -1,406 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-#include "fcl/OBB.h"
-#include "fcl/BVH_utility.h"
-#include "fcl/transform.h"
-
-#include <iostream>
-#include <limits>
-
-namespace fcl
-{
-
-bool OBB::overlap(const OBB& other) const
-{
- // compute what transform [R,T] that takes us from cs1 to cs2.
- // [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
- // First compute the rotation part, then translation part
- Vec3f t = other.To - To; // T2 - T1
- Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
- Vec3f R[3];
- R[0] = Vec3f(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]));
- R[1] = Vec3f(axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]));
- R[2] = Vec3f(axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]));
-
- // R is row first
- return (obbDisjoint(R, T, extent, other.extent) == 0);
-}
-
-
-bool OBB::contain(const Vec3f& p) const
-{
- Vec3f local_p = p - To;
- BVH_REAL proj = local_p.dot(axis[0]);
- if((proj > extent[0]) || (proj < -extent[0]))
- return false;
-
- proj = local_p.dot(axis[1]);
- if((proj > extent[1]) || (proj < -extent[1]))
- return false;
-
- proj = local_p.dot(axis[2]);
- if((proj > extent[2]) || (proj < -extent[2]))
- return false;
-
- return true;
-}
-
-/** \brief A simple way to add new point to the OBB, not compact. */
-OBB& OBB::operator += (const Vec3f& p)
-{
- OBB bvp;
- bvp.To = p;
- bvp.axis[0] = axis[0];
- bvp.axis[1] = axis[1];
- bvp.axis[2] = axis[2];
- bvp.extent = Vec3f(0, 0, 0);
-
- *this += bvp;
- return *this;
-}
-
-OBB OBB::operator + (const OBB& other) const
-{
- Vec3f center_diff = To - other.To;
- BVH_REAL max_extent = std::max(std::max(extent[0], extent[1]), extent[2]);
- BVH_REAL max_extent2 = std::max(std::max(other.extent[0], other.extent[1]), other.extent[2]);
- if(center_diff.length() > 2 * (max_extent + max_extent2))
- {
- return merge_largedist(*this, other);
- }
- else
- {
- return merge_smalldist(*this, other);
- }
-}
-
-
-bool OBB::obbDisjoint(const Vec3f B[3], const Vec3f& T, const Vec3f& a, const Vec3f& b)
-{
- register BVH_REAL t, s;
- Vec3f Bf[3];
- const BVH_REAL reps = 1e-6;
-
- Bf[0] = abs(B[0]);
- Bf[1] = abs(B[1]);
- Bf[2] = abs(B[2]);
-
- Vec3f reps_vec(reps, reps, reps);
-
- Bf[0] += reps_vec;
- Bf[1] += reps_vec;
- Bf[2] += reps_vec;
-
- Vec3f Bf_col[3] = {Vec3f(Bf[0][0], Bf[1][0], Bf[2][0]),
- Vec3f(Bf[0][1], Bf[1][1], Bf[2][1]),
- Vec3f(Bf[0][2], Bf[1][2], Bf[2][2])};
-
- Vec3f B_col[3] = {Vec3f(B[0][0], B[1][0], B[2][0]),
- Vec3f(B[0][1], B[1][1], B[2][1]),
- Vec3f(B[0][2], B[1][2], B[2][2])};
-
-
- // if any of these tests are one-sided, then the polyhedra are disjoint
-
- // A1 x A2 = A0
- t = ((T[0] < 0) ? -T[0] : T[0]);
-
- if(t > (a[0] + b.dot(Bf[0])))
- return true;
-
- // B1 x B2 = B0
- s = T.dot(B_col[0]);
- t = ((s < 0) ? -s : s);
-
- if(t > (b[0] + a.dot(Bf_col[0])))
- return true;
-
- // A2 x A0 = A1
- t = ((T[1] < 0) ? -T[1] : T[1]);
-
- if(t > (a[1] + b.dot(Bf[1])))
- return true;
-
- // A0 x A1 = A2
- t =((T[2] < 0) ? -T[2] : T[2]);
-
- if(t > (a[2] + b.dot(Bf[2])))
- return true;
-
- // B2 x B0 = B1
- s = T.dot(B_col[1]);
- t = ((s < 0) ? -s : s);
-
- if(t > (b[1] + a.dot(Bf_col[1])))
- return true;
-
- // B0 x B1 = B2
- s = T.dot(B_col[2]);
- t = ((s < 0) ? -s : s);
-
- if(t > (b[2] + a.dot(Bf_col[2])))
- return true;
-
- // A0 x B0
- s = T[2] * B[1][0] - T[1] * B[2][0];
- t = ((s < 0) ? -s : s);
-
- if(t > (a[1] * Bf[2][0] + a[2] * Bf[1][0] +
- b[1] * Bf[0][2] + b[2] * Bf[0][1]))
- return true;
-
- // A0 x B1
- s = T[2] * B[1][1] - T[1] * B[2][1];
- t = ((s < 0) ? -s : s);
-
- if(t > (a[1] * Bf[2][1] + a[2] * Bf[1][1] +
- b[0] * Bf[0][2] + b[2] * Bf[0][0]))
- return true;
-
- // A0 x B2
- s = T[2] * B[1][2] - T[1] * B[2][2];
- t = ((s < 0) ? -s : s);
-
- if(t > (a[1] * Bf[2][2] + a[2] * Bf[1][2] +
- b[0] * Bf[0][1] + b[1] * Bf[0][0]))
- return true;
-
- // A1 x B0
- s = T[0] * B[2][0] - T[2] * B[0][0];
- t = ((s < 0) ? -s : s);
-
- if(t > (a[0] * Bf[2][0] + a[2] * Bf[0][0] +
- b[1] * Bf[1][2] + b[2] * Bf[1][1]))
- return true;
-
- // A1 x B1
- s = T[0] * B[2][1] - T[2] * B[0][1];
- t = ((s < 0) ? -s : s);
-
- if(t > (a[0] * Bf[2][1] + a[2] * Bf[0][1] +
- b[0] * Bf[1][2] + b[2] * Bf[1][0]))
- return true;
-
- // A1 x B2
- s = T[0] * B[2][2] - T[2] * B[0][2];
- t = ((s < 0) ? -s : s);
-
- if(t > (a[0] * Bf[2][2] + a[2] * Bf[0][2] +
- b[0] * Bf[1][1] + b[1] * Bf[1][0]))
- return true;
-
- // A2 x B0
- s = T[1] * B[0][0] - T[0] * B[1][0];
- t = ((s < 0) ? -s : s);
-
- if(t > (a[0] * Bf[1][0] + a[1] * Bf[0][0] +
- b[1] * Bf[2][2] + b[2] * Bf[2][1]))
- return true;
-
- // A2 x B1
- s = T[1] * B[0][1] - T[0] * B[1][1];
- t = ((s < 0) ? -s : s);
-
- if(t > (a[0] * Bf[1][1] + a[1] * Bf[0][1] +
- b[0] * Bf[2][2] + b[2] * Bf[2][0]))
- return true;
-
- // A2 x B2
- s = T[1] * B[0][2] - T[0] * B[1][2];
- t = ((s < 0) ? -s : s);
-
- if(t > (a[0] * Bf[1][2] + a[1] * Bf[0][2] +
- b[0] * Bf[2][1] + b[1] * Bf[2][0]))
- return true;
-
- return false;
-
-}
-
-
-void OBB::computeVertices(Vec3f vertex[8]) const
-{
- Vec3f extAxis0 = axis[0] * extent[0];
- Vec3f extAxis1 = axis[1] * extent[1];
- Vec3f extAxis2 = axis[2] * extent[2];
-
- vertex[0] = To - extAxis0 - extAxis1 - extAxis2;
- vertex[1] = To + extAxis0 - extAxis1 - extAxis2;
- vertex[2] = To + extAxis0 + extAxis1 - extAxis2;
- vertex[3] = To - extAxis0 + extAxis1 - extAxis2;
- vertex[4] = To - extAxis0 - extAxis1 + extAxis2;
- vertex[5] = To + extAxis0 - extAxis1 + extAxis2;
- vertex[6] = To + extAxis0 + extAxis1 + extAxis2;
- vertex[7] = To - extAxis0 + extAxis1 + extAxis2;
-}
-
-
-OBB OBB::merge_largedist(const OBB& b1, const OBB& b2)
-{
- OBB b;
- Vec3f vertex[16];
- b1.computeVertices(vertex);
- b2.computeVertices(vertex + 8);
- Vec3f M[3];
- Vec3f E[3];
- BVH_REAL s[3] = {0, 0, 0};
- Vec3f R[3];
-
- R[0] = b1.To - b2.To;
- R[0].normalize();
-
- Vec3f vertex_proj[16];
- for(int i = 0; i < 16; ++i)
- vertex_proj[i] = vertex[i] - R[0] * vertex[i].dot(R[0]);
-
- getCovariance(vertex_proj, NULL, NULL, 16, M);
- Meigen(M, s, E);
-
- int min, mid, max;
- if (s[0] > s[1]) { max = 0; min = 1; }
- else { min = 0; max = 1; }
- if (s[2] < s[min]) { mid = min; min = 2; }
- else if (s[2] > s[max]) { mid = max; max = 2; }
- else { mid = 2; }
-
-
- R[1] = Vec3f(E[0][max], E[1][max], E[2][max]);
- R[2] = Vec3f(E[0][mid], E[1][mid], E[2][mid]);
-
- // set obb axes
- b.axis[0] = R[0];
- b.axis[1] = R[1];
- b.axis[2] = R[2];
-
- // set obb centers and extensions
- Vec3f center, extent;
- getExtentAndCenter(vertex, NULL, NULL, 16, R, center, extent);
-
- b.To = center;
- b.extent = extent;
-
- return b;
-}
-
-OBB OBB::merge_smalldist(const OBB& b1, const OBB& b2)
-{
- OBB b;
- b.To = (b1.To + b2.To) * 0.5;
- SimpleQuaternion q0, q1;
- q0.fromAxes(b1.axis);
- q1.fromAxes(b2.axis);
- if(q0.dot(q1) < 0)
- q1 = -q1;
-
- SimpleQuaternion q = q0 + q1;
- BVH_REAL inv_length = 1.0 / sqrt(q.dot(q));
- q = q * inv_length;
- q.toAxes(b.axis);
-
-
- Vec3f vertex[8], diff;
- BVH_REAL real_max = std::numeric_limits<BVH_REAL>::max();
- Vec3f pmin(real_max, real_max, real_max);
- Vec3f pmax(-real_max, -real_max, -real_max);
-
- b1.computeVertices(vertex);
- for(int i = 0; i < 8; ++i)
- {
- diff = vertex[i] - b.To;
- for(int j = 0; j < 3; ++j)
- {
- BVH_REAL dot = diff.dot(b.axis[j]);
- if(dot > pmax[j])
- pmax[j] = dot;
- else if(dot < pmin[j])
- pmin[j] = dot;
- }
- }
-
- b2.computeVertices(vertex);
- for(int i = 0; i < 8; ++i)
- {
- diff = vertex[i] - b.To;
- for(int j = 0; j < 3; ++j)
- {
- BVH_REAL dot = diff.dot(b.axis[j]);
- if(dot > pmax[j])
- pmax[j] = dot;
- else if(dot < pmin[j])
- pmin[j] = dot;
- }
- }
-
- for(int j = 0; j < 3; ++j)
- {
- b.To += (b.axis[j] * (0.5 * (pmax[j] + pmin[j])));
- b.extent[j] = 0.5 * (pmax[j] - pmin[j]);
- }
-
- return b;
-}
-
-
-BVH_REAL OBB::distance(const OBB& other, Vec3f* P, Vec3f* Q) const
-{
- std::cerr << "OBB distance not implemented!" << std::endl;
- return 0.0;
-}
-
-// R is row first
-bool overlap(const Vec3f R0[3], const Vec3f& T0, const OBB& b1, const OBB& b2)
-{
- // R0 R2
- Vec3f Rtemp_col[3];
- Rtemp_col[0] = Vec3f(R0[0].dot(b2.axis[0]), R0[1].dot(b2.axis[0]), R0[2].dot(b2.axis[0]));
- Rtemp_col[1] = Vec3f(R0[0].dot(b2.axis[1]), R0[1].dot(b2.axis[1]), R0[2].dot(b2.axis[1]));
- Rtemp_col[2] = Vec3f(R0[0].dot(b2.axis[2]), R0[1].dot(b2.axis[2]), R0[2].dot(b2.axis[2]));
-
- // R1'Rtemp
- Vec3f R[3];
- R[0] = Vec3f(b1.axis[0].dot(Rtemp_col[0]), b1.axis[0].dot(Rtemp_col[1]), b1.axis[0].dot(Rtemp_col[2]));
- R[1] = Vec3f(b1.axis[1].dot(Rtemp_col[0]), b1.axis[1].dot(Rtemp_col[1]), b1.axis[1].dot(Rtemp_col[2]));
- R[2] = Vec3f(b1.axis[2].dot(Rtemp_col[0]), b1.axis[2].dot(Rtemp_col[1]), b1.axis[2].dot(Rtemp_col[2]));
-
- Vec3f Ttemp = Vec3f(R0[0].dot(b2.To), R0[1].dot(b2.To), R0[2].dot(b2.To)) + T0 - b1.To;
-
- Vec3f T = Vec3f(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2]));
-
- return (OBB::obbDisjoint(R, T, b1.extent, b2.extent) == 0);
-}
-
-}
diff --git a/branches/point_cloud/fcl/src/RSS.cpp b/branches/point_cloud/fcl/src/RSS.cpp
deleted file mode 100644
index cdc4da99ee4db65676038b29216379cc58eb75fa..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/src/RSS.cpp
+++ /dev/null
@@ -1,1181 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-#include "fcl/RSS.h"
-#include "fcl/BVH_utility.h"
-#include <iostream>
-namespace fcl
-{
-
-bool RSS::overlap(const RSS& other) const
-{
- // compute what transform [R,T] that takes us from cs1 to cs2.
- // [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
- // First compute the rotation part, then translation part
- Vec3f t = other.Tr - Tr; // T2 - T1
- Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
- Vec3f R[3];
- R[0] = Vec3f(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]));
- R[1] = Vec3f(axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]));
- R[2] = Vec3f(axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]));
-
- BVH_REAL dist = rectDistance(R, T, l, other.l);
- if(dist <= (r + other.r)) return true;
- return false;
-}
-
-bool overlap(const Vec3f R0[3], const Vec3f& T0, const RSS& b1, const RSS& b2)
-{
- // R0 R2
- Vec3f Rtemp_col[3];
- Rtemp_col[0] = Vec3f(R0[0].dot(b2.axis[0]), R0[1].dot(b2.axis[0]), R0[2].dot(b2.axis[0]));
- Rtemp_col[1] = Vec3f(R0[0].dot(b2.axis[1]), R0[1].dot(b2.axis[1]), R0[2].dot(b2.axis[1]));
- Rtemp_col[2] = Vec3f(R0[0].dot(b2.axis[2]), R0[1].dot(b2.axis[2]), R0[2].dot(b2.axis[2]));
-
- // R1'Rtemp
- Vec3f R[3];
- R[0] = Vec3f(b1.axis[0].dot(Rtemp_col[0]), b1.axis[0].dot(Rtemp_col[1]), b1.axis[0].dot(Rtemp_col[2]));
- R[1] = Vec3f(b1.axis[1].dot(Rtemp_col[0]), b1.axis[1].dot(Rtemp_col[1]), b1.axis[1].dot(Rtemp_col[2]));
- R[2] = Vec3f(b1.axis[2].dot(Rtemp_col[0]), b1.axis[2].dot(Rtemp_col[1]), b1.axis[2].dot(Rtemp_col[2]));
-
- Vec3f Ttemp = Vec3f(R0[0].dot(b2.Tr), R0[1].dot(b2.Tr), R0[2].dot(b2.Tr)) + T0 - b1.Tr;
-
- Vec3f T = Vec3f(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2]));
-
- BVH_REAL dist = RSS::rectDistance(R, T, b1.l, b2.l);
- if(dist <= (b1.r + b2.r)) return true;
- return false;
-}
-
-bool RSS::contain(const Vec3f& p) const
-{
- Vec3f local_p = p - Tr;
- BVH_REAL proj0 = local_p.dot(axis[0]);
- BVH_REAL proj1 = local_p.dot(axis[1]);
- BVH_REAL proj2 = local_p.dot(axis[2]);
- BVH_REAL abs_proj2 = fabs(proj2);
- Vec3f proj(proj0, proj1, proj2);
-
- // projection is within the rectangle
- if((proj0 < l[0]) && (proj0 > 0) && (proj1 < l[1]) && (proj1 > 0))
- {
- if(abs_proj2 < r)
- return true;
- else
- return false;
- }
- else if((proj0 < l[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > l[1])))
- {
- BVH_REAL y = (proj1 > 0) ? l[1] : 0;
- Vec3f v(proj0, y, 0);
- if((proj - v).sqrLength() < r * r)
- return true;
- else
- return false;
- }
- else if((proj1 < l[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > l[0])))
- {
- BVH_REAL x = (proj0 > 0) ? l[0] : 0;
- Vec3f v(x, proj1, 0);
- if((proj - v).sqrLength() < r * r)
- return true;
- else
- return false;
- }
- else
- {
- BVH_REAL x = (proj0 > 0) ? l[0] : 0;
- BVH_REAL y = (proj1 > 0) ? l[1] : 0;
- Vec3f v(x, y, 0);
- if((proj - v).sqrLength() < r * r)
- return true;
- else
- return false;
- }
-}
-
-RSS& RSS::operator += (const Vec3f& p)
-{
- Vec3f local_p = p - Tr;
- BVH_REAL proj0 = local_p.dot(axis[0]);
- BVH_REAL proj1 = local_p.dot(axis[1]);
- BVH_REAL proj2 = local_p.dot(axis[2]);
- BVH_REAL abs_proj2 = fabs(proj2);
- Vec3f proj(proj0, proj1, proj2);
-
- // projection is within the rectangle
- if((proj0 < l[0]) && (proj0 > 0) && (proj1 < l[1]) && (proj1 > 0))
- {
- if(abs_proj2 < r)
- ; // do nothing
- else
- {
- r = 0.5 * (r + abs_proj2); // enlarge the r
- // change RSS origin position
- if(proj2 > 0)
- Tr[2] += 0.5 * (abs_proj2 - r);
- else
- Tr[2] -= 0.5 * (abs_proj2 - r);
- }
- }
- else if((proj0 < l[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > l[1])))
- {
- BVH_REAL y = (proj1 > 0) ? l[1] : 0;
- Vec3f v(proj0, y, 0);
- BVH_REAL new_r_sqr = (proj - v).sqrLength();
- if(new_r_sqr < r * r)
- ; // do nothing
- else
- {
- if(abs_proj2 < r)
- {
- BVH_REAL delta_y = - sqrt(r * r - proj2 * proj2) + fabs(proj1 - y);
- l[1] += delta_y;
- if(proj1 < 0)
- Tr[1] -= delta_y;
- }
- else
- {
- BVH_REAL delta_y = fabs(proj1 - y);
- l[1] += delta_y;
- if(proj1 < 0)
- Tr[1] -= delta_y;
-
- if(proj2 > 0)
- Tr[2] += 0.5 * (abs_proj2 - r);
- else
- Tr[2] -= 0.5 * (abs_proj2 - r);
- }
- }
- }
- else if((proj1 < l[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > l[0])))
- {
- BVH_REAL x = (proj0 > 0) ? l[0] : 0;
- Vec3f v(x, proj1, 0);
- BVH_REAL new_r_sqr = (proj - v).sqrLength();
- if(new_r_sqr < r * r)
- ; // do nothing
- else
- {
- if(abs_proj2 < r)
- {
- BVH_REAL delta_x = - sqrt(r * r - proj2 * proj2) + fabs(proj0 - x);
- l[0] += delta_x;
- if(proj0 < 0)
- Tr[0] -= delta_x;
- }
- else
- {
- BVH_REAL delta_x = fabs(proj0 - x);
- l[0] += delta_x;
- if(proj0 < 0)
- Tr[0] -= delta_x;
-
- if(proj2 > 0)
- Tr[2] += 0.5 * (abs_proj2 - r);
- else
- Tr[2] -= 0.5 * (abs_proj2 - r);
- }
- }
- }
- else
- {
- BVH_REAL x = (proj0 > 0) ? l[0] : 0;
- BVH_REAL y = (proj1 > 0) ? l[1] : 0;
- Vec3f v(x, y, 0);
- BVH_REAL new_r_sqr = (proj - v).sqrLength();
- if(new_r_sqr < r * r)
- ; // do nothing
- else
- {
- if(abs_proj2 < r)
- {
- BVH_REAL diag = sqrt(new_r_sqr - proj2 * proj2);
- BVH_REAL delta_diag = - sqrt(r * r - proj2 * proj2) + diag;
-
- BVH_REAL delta_x = delta_diag / diag * fabs(proj0 - x);
- BVH_REAL delta_y = delta_diag / diag * fabs(proj1 - y);
- l[0] += delta_x;
- l[1] += delta_y;
-
- if(proj0 < 0 && proj1 < 0)
- {
- Tr[0] -= delta_x;
- Tr[1] -= delta_y;
- }
- }
- else
- {
- BVH_REAL delta_x = fabs(proj0 - x);
- BVH_REAL delta_y = fabs(proj1 - y);
-
- l[0] += delta_x;
- l[1] += delta_y;
-
- if(proj0 < 0 && proj1 < 0)
- {
- Tr[0] -= delta_x;
- Tr[1] -= delta_y;
- }
-
- if(proj2 > 0)
- Tr[2] += 0.5 * (abs_proj2 - r);
- else
- Tr[2] -= 0.5 * (abs_proj2 - r);
- }
- }
- }
-
- return *this;
-}
-/*
-RSS RSS::operator + (const RSS& other) const
-{
- RSS res = *this;
-
- Vec3f d0_pos = other.axis[0] * (other.l[0] + other.r);
- Vec3f d1_pos = other.axis[1] * (other.l[1] + other.r);
- Vec3f d0_neg = other.axis[0] * (-other.r);
- Vec3f d1_neg = other.axis[1] * (-other.r);
- Vec3f d2_pos = other.axis[2] * other.r;
- Vec3f d2_neg = other.axis[2] * (-other.r);
-
- Vec3f v[8];
- v[0] = other.Tr + d0_pos + d1_pos + d2_pos;
- v[1] = other.Tr + d0_pos + d1_pos + d2_neg;
- v[2] = other.Tr + d0_pos + d1_neg + d2_pos;
- v[3] = other.Tr + d0_pos + d1_neg + d2_neg;
- v[4] = other.Tr + d0_neg + d1_pos + d2_pos;
- v[5] = other.Tr + d0_neg + d1_pos + d2_neg;
- v[6] = other.Tr + d0_neg + d1_neg + d2_pos;
- v[7] = other.Tr + d0_neg + d1_neg + d2_neg;
-
- for(int i = 0; i < 8; ++i)
- {
- res += v[i];
- }
- return res;
-}
-*/
-
-RSS RSS::operator + (const RSS& other) const
-{
- RSS bv;
-
- Vec3f v[16];
- Vec3f d0_pos = other.axis[0] * (other.l[0] + other.r);
- Vec3f d1_pos = other.axis[1] * (other.l[1] + other.r);
- Vec3f d0_neg = other.axis[0] * (-other.r);
- Vec3f d1_neg = other.axis[1] * (-other.r);
- Vec3f d2_pos = other.axis[2] * other.r;
- Vec3f d2_neg = other.axis[2] * (-other.r);
-
- v[0] = other.Tr + d0_pos + d1_pos + d2_pos;
- v[1] = other.Tr + d0_pos + d1_pos + d2_neg;
- v[2] = other.Tr + d0_pos + d1_neg + d2_pos;
- v[3] = other.Tr + d0_pos + d1_neg + d2_neg;
- v[4] = other.Tr + d0_neg + d1_pos + d2_pos;
- v[5] = other.Tr + d0_neg + d1_pos + d2_neg;
- v[6] = other.Tr + d0_neg + d1_neg + d2_pos;
- v[7] = other.Tr + d0_neg + d1_neg + d2_neg;
-
- d0_pos = axis[0] * (l[0] + r);
- d1_pos = axis[1] * (l[1] + r);
- d0_neg = axis[0] * (-r);
- d1_neg = axis[1] * (-r);
- d2_pos = axis[2] * r;
- d2_neg = axis[2] * (-r);
-
- v[8] = Tr + d0_pos + d1_pos + d2_pos;
- v[9] = Tr + d0_pos + d1_pos + d2_neg;
- v[10] = Tr + d0_pos + d1_neg + d2_pos;
- v[11] = Tr + d0_pos + d1_neg + d2_neg;
- v[12] = Tr + d0_neg + d1_pos + d2_pos;
- v[13] = Tr + d0_neg + d1_pos + d2_neg;
- v[14] = Tr + d0_neg + d1_neg + d2_pos;
- v[15] = Tr + d0_neg + d1_neg + d2_neg;
-
-
- Vec3f M[3]; // row first matrix
- Vec3f E[3]; // row first eigen-vectors
- BVH_REAL s[3] = {0, 0, 0};
-
- getCovariance(v, NULL, NULL, 16, M);
- Meigen(M, s, E);
-
- int min, mid, max;
- if(s[0] > s[1]) { max = 0; min = 1; }
- else { min = 0; max = 1; }
- if(s[2] < s[min]) { mid = min; min = 2; }
- else if(s[2] > s[max]) { mid = max; max = 2; }
- else { mid = 2; }
-
- Vec3f R[3]; // column first matrix, as the axis in RSS
- R[0] = Vec3f(E[0][max], E[1][max], E[2][max]);
- R[1] = Vec3f(E[0][mid], E[1][mid], E[2][mid]);
- R[2] = Vec3f(E[1][max]*E[2][mid] - E[1][mid]*E[2][max],
- E[0][mid]*E[2][max] - E[0][max]*E[2][mid],
- E[0][max]*E[1][mid] - E[0][mid]*E[1][max]);
-
- // set obb axes
- bv.axis[0] = R[0];
- bv.axis[1] = R[1];
- bv.axis[2] = R[2];
-
- // set rss origin, rectangle size and radius
- getRadiusAndOriginAndRectangleSize(v, NULL, NULL, 16, R, bv.Tr, bv.l, bv.r);
-
- return bv;
-}
-
-BVH_REAL RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) const
-{
- // compute what transform [R,T] that takes us from cs1 to cs2.
- // [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
- // First compute the rotation part, then translation part
- Vec3f t = other.Tr - Tr; // T2 - T1
- Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
- Vec3f R[3];
- R[0] = Vec3f(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]));
- R[1] = Vec3f(axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]));
- R[2] = Vec3f(axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]));
-
- BVH_REAL dist = rectDistance(R, T, l, other.l, P, Q);
- dist -= (r + other.r);
- return (dist < (BVH_REAL)0.0) ? (BVH_REAL)0.0 : dist;
-}
-
-BVH_REAL distance(const Vec3f R0[3], const Vec3f& T0, const RSS& b1, const RSS& b2, Vec3f* P, Vec3f* Q)
-{
- // R0 R2
- Vec3f Rtemp_col[3];
- Rtemp_col[0] = Vec3f(R0[0].dot(b2.axis[0]), R0[1].dot(b2.axis[0]), R0[2].dot(b2.axis[0]));
- Rtemp_col[1] = Vec3f(R0[0].dot(b2.axis[1]), R0[1].dot(b2.axis[1]), R0[2].dot(b2.axis[1]));
- Rtemp_col[2] = Vec3f(R0[0].dot(b2.axis[2]), R0[1].dot(b2.axis[2]), R0[2].dot(b2.axis[2]));
-
- // R1'Rtemp
- Vec3f R[3];
- R[0] = Vec3f(b1.axis[0].dot(Rtemp_col[0]), b1.axis[0].dot(Rtemp_col[1]), b1.axis[0].dot(Rtemp_col[2]));
- R[1] = Vec3f(b1.axis[1].dot(Rtemp_col[0]), b1.axis[1].dot(Rtemp_col[1]), b1.axis[1].dot(Rtemp_col[2]));
- R[2] = Vec3f(b1.axis[2].dot(Rtemp_col[0]), b1.axis[2].dot(Rtemp_col[1]), b1.axis[2].dot(Rtemp_col[2]));
-
- Vec3f Ttemp = Vec3f(R0[0].dot(b2.Tr), R0[1].dot(b2.Tr), R0[2].dot(b2.Tr)) + T0 - b1.Tr;
-
- Vec3f T = Vec3f(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2]));
-
- BVH_REAL dist = RSS::rectDistance(R, T, b1.l, b2.l, P, Q);
- dist -= (b1.r + b2.r);
- return (dist < (BVH_REAL)0.0) ? (BVH_REAL)0.0 : dist;
-}
-
-
-BVH_REAL RSS::rectDistance(const Vec3f Rab[3], Vec3f const& Tab, const BVH_REAL a[2], const BVH_REAL b[2], Vec3f* P, Vec3f* Q)
-{
- BVH_REAL A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1;
-
- A0_dot_B0 = Rab[0][0];
- A0_dot_B1 = Rab[0][1];
- A1_dot_B0 = Rab[1][0];
- A1_dot_B1 = Rab[1][1];
-
- BVH_REAL aA0_dot_B0, aA0_dot_B1, aA1_dot_B0, aA1_dot_B1;
- BVH_REAL bA0_dot_B0, bA0_dot_B1, bA1_dot_B0, bA1_dot_B1;
-
- aA0_dot_B0 = a[0] * A0_dot_B0;
- aA0_dot_B1 = a[0] * A0_dot_B1;
- aA1_dot_B0 = a[1] * A1_dot_B0;
- aA1_dot_B1 = a[1] * A1_dot_B1;
- bA0_dot_B0 = b[0] * A0_dot_B0;
- bA1_dot_B0 = b[0] * A1_dot_B0;
- bA0_dot_B1 = b[1] * A0_dot_B1;
- bA1_dot_B1 = b[1] * A1_dot_B1;
-
- Vec3f Tba = MTxV(Rab, Tab);
-
- Vec3f S;
- BVH_REAL t, u;
-
- // determine if any edge pair contains the closest points
-
- BVH_REAL ALL_x, ALU_x, AUL_x, AUU_x;
- BVH_REAL BLL_x, BLU_x, BUL_x, BUU_x;
- BVH_REAL LA1_lx, LA1_ux, UA1_lx, UA1_ux, LB1_lx, LB1_ux, UB1_lx, UB1_ux;
-
- ALL_x = -Tba[0];
- ALU_x = ALL_x + aA1_dot_B0;
- AUL_x = ALL_x + aA0_dot_B0;
- AUU_x = ALU_x + aA0_dot_B0;
-
- if(ALL_x < ALU_x)
- {
- LA1_lx = ALL_x;
- LA1_ux = ALU_x;
- UA1_lx = AUL_x;
- UA1_ux = AUU_x;
- }
- else
- {
- LA1_lx = ALU_x;
- LA1_ux = ALL_x;
- UA1_lx = AUU_x;
- UA1_ux = AUL_x;
- }
-
- BLL_x = Tab[0];
- BLU_x = BLL_x + bA0_dot_B1;
- BUL_x = BLL_x + bA0_dot_B0;
- BUU_x = BLU_x + bA0_dot_B0;
-
- if(BLL_x < BLU_x)
- {
- LB1_lx = BLL_x;
- LB1_ux = BLU_x;
- UB1_lx = BUL_x;
- UB1_ux = BUU_x;
- }
- else
- {
- LB1_lx = BLU_x;
- LB1_ux = BLL_x;
- UB1_lx = BUU_x;
- UB1_ux = BUL_x;
- }
-
- // UA1, UB1
-
- if((UA1_ux > b[0]) && (UB1_ux > a[0]))
- {
- if(((UA1_lx > b[0]) ||
- inVoronoi(b[1], a[1], A1_dot_B0, aA0_dot_B0 - b[0] - Tba[0],
- A1_dot_B1, aA0_dot_B1 - Tba[1],
- -Tab[1] - bA1_dot_B0))
- &&
- ((UB1_lx > a[0]) ||
- inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0 - a[0],
- A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1] - aA0_dot_B1)))
- {
- segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0,
- Tba[1] - aA0_dot_B1);
-
- S[0] = Tab[0] + Rab[0][0] * b[0] + Rab[0][1] * u - a[0] ;
- S[1] = Tab[1] + Rab[1][0] * b[0] + Rab[1][1] * u - t;
- S[2] = Tab[2] + Rab[2][0] * b[0] + Rab[2][1] * u;
-
- if(P && Q)
- {
- *P = Vec3f(a[0], t, 0);
- *Q = S + (*P);
- }
-
- return S.length();
- }
- }
-
-
- // UA1, LB1
-
- if((UA1_lx < 0) && (LB1_ux > a[0]))
- {
- if(((UA1_ux < 0) ||
- inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0] - aA0_dot_B0,
- A1_dot_B1, aA0_dot_B1 - Tba[1], -Tab[1]))
- &&
- ((LB1_lx > a[0]) ||
- inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] - a[0],
- A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1)))
- {
- segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1);
-
- S[0] = Tab[0] + Rab[0][1] * u - a[0];
- S[1] = Tab[1] + Rab[1][1] * u - t;
- S[2] = Tab[2] + Rab[2][1] * u;
-
- if(P && Q)
- {
- *P = Vec3f(a[0], t, 0);
- *Q = S + (*P);
- }
-
- return S.length();
- }
- }
-
- // LA1, UB1
-
- if((LA1_ux > b[0]) && (UB1_lx < 0))
- {
- if(((LA1_lx > b[0]) ||
- inVoronoi(b[1], a[1], A1_dot_B0, -Tba[0] - b[0],
- A1_dot_B1, -Tba[1], -Tab[1] - bA1_dot_B0))
- &&
- ((UB1_ux < 0) ||
- inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0] - bA0_dot_B0,
- A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1])))
- {
- segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]);
-
- S[0] = Tab[0] + Rab[0][0] * b[0] + Rab[0][1] * u;
- S[1] = Tab[1] + Rab[1][0] * b[0] + Rab[1][1] * u - t;
- S[2] = Tab[2] + Rab[2][0] * b[0] + Rab[2][1] * u;
-
- if(P && Q)
- {
- *P = Vec3f(0, t, 0);
- *Q = S + (*P);
- }
-
- return S.length();
- }
- }
-
- // LA1, LB1
-
- if((LA1_lx < 0) && (LB1_lx < 0))
- {
- if (((LA1_ux < 0) ||
- inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0], A1_dot_B1,
- -Tba[1], -Tab[1]))
- &&
- ((LB1_ux < 0) ||
- inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0], A1_dot_B1,
- Tab[1], Tba[1])))
- {
- segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1]);
-
- S[0] = Tab[0] + Rab[0][1] * u;
- S[1] = Tab[1] + Rab[1][1] * u - t;
- S[2] = Tab[2] + Rab[2][1] * u;
-
- if(P && Q)
- {
- *P = Vec3f(0, t, 0);
- *Q = S + (*P);
- }
-
- return S.length();
- }
- }
-
- BVH_REAL ALL_y, ALU_y, AUL_y, AUU_y;
-
- ALL_y = -Tba[1];
- ALU_y = ALL_y + aA1_dot_B1;
- AUL_y = ALL_y + aA0_dot_B1;
- AUU_y = ALU_y + aA0_dot_B1;
-
- BVH_REAL LA1_ly, LA1_uy, UA1_ly, UA1_uy, LB0_lx, LB0_ux, UB0_lx, UB0_ux;
-
- if(ALL_y < ALU_y)
- {
- LA1_ly = ALL_y;
- LA1_uy = ALU_y;
- UA1_ly = AUL_y;
- UA1_uy = AUU_y;
- }
- else
- {
- LA1_ly = ALU_y;
- LA1_uy = ALL_y;
- UA1_ly = AUU_y;
- UA1_uy = AUL_y;
- }
-
- if(BLL_x < BUL_x)
- {
- LB0_lx = BLL_x;
- LB0_ux = BUL_x;
- UB0_lx = BLU_x;
- UB0_ux = BUU_x;
- }
- else
- {
- LB0_lx = BUL_x;
- LB0_ux = BLL_x;
- UB0_lx = BUU_x;
- UB0_ux = BLU_x;
- }
-
- // UA1, UB0
-
- if((UA1_uy > b[1]) && (UB0_ux > a[0]))
- {
- if(((UA1_ly > b[1]) ||
- inVoronoi(b[0], a[1], A1_dot_B1, aA0_dot_B1 - Tba[1] - b[1],
- A1_dot_B0, aA0_dot_B0 - Tba[0], -Tab[1] - bA1_dot_B1))
- &&
- ((UB0_lx > a[0]) ||
- inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0] + bA0_dot_B1,
- A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0] - aA0_dot_B0)))
- {
- segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1,
- Tba[0] - aA0_dot_B0);
-
- S[0] = Tab[0] + Rab[0][1] * b[1] + Rab[0][0] * u - a[0] ;
- S[1] = Tab[1] + Rab[1][1] * b[1] + Rab[1][0] * u - t;
- S[2] = Tab[2] + Rab[2][1] * b[1] + Rab[2][0] * u;
-
- if(P && Q)
- {
- *P = Vec3f(a[0], t, 0);
- *Q = S + (*P);
- }
-
- return S.length();
- }
- }
-
- // UA1, LB0
-
- if((UA1_ly < 0) && (LB0_ux > a[0]))
- {
- if(((UA1_uy < 0) ||
- inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1] - aA0_dot_B1, A1_dot_B0,
- aA0_dot_B0 - Tba[0], -Tab[1]))
- &&
- ((LB0_lx > a[0]) ||
- inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0],
- A1_dot_B0, Tab[1], Tba[0] - aA0_dot_B0)))
- {
- segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0] - aA0_dot_B0);
-
- S[0] = Tab[0] + Rab[0][0] * u - a[0];
- S[1] = Tab[1] + Rab[1][0] * u - t;
- S[2] = Tab[2] + Rab[2][0] * u;
-
- if(P && Q)
- {
- *P = Vec3f(a[0], t, 0);
- *Q = S + (*P);
- }
-
- return S.length();
- }
- }
-
- // LA1, UB0
-
- if((LA1_uy > b[1]) && (UB0_lx < 0))
- {
- if(((LA1_ly > b[1]) ||
- inVoronoi(b[0], a[1], A1_dot_B1, -Tba[1] - b[1],
- A1_dot_B0, -Tba[0], -Tab[1] - bA1_dot_B1))
- &&
-
- ((UB0_ux < 0) ||
- inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0] - bA0_dot_B1, A1_dot_B0,
- Tab[1] + bA1_dot_B1, Tba[0])))
- {
- segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0]);
-
- S[0] = Tab[0] + Rab[0][1] * b[1] + Rab[0][0] * u;
- S[1] = Tab[1] + Rab[1][1] * b[1] + Rab[1][0] * u - t;
- S[2] = Tab[2] + Rab[2][1] * b[1] + Rab[2][0] * u;
-
- if(P && Q)
- {
- *P = Vec3f(0, t, 0);
- *Q = S + (*P);
- }
-
-
- return S.length();
- }
- }
-
- // LA1, LB0
-
- if((LA1_ly < 0) && (LB0_lx < 0))
- {
- if(((LA1_uy < 0) ||
- inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1], A1_dot_B0,
- -Tba[0], -Tab[1]))
- &&
- ((LB0_ux < 0) ||
- inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0], A1_dot_B0,
- Tab[1], Tba[0])))
- {
- segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0]);
-
- S[0] = Tab[0] + Rab[0][0] * u;
- S[1] = Tab[1] + Rab[1][0] * u - t;
- S[2] = Tab[2] + Rab[2][0] * u;
-
- if(P&& Q)
- {
- *P = Vec3f(0, t, 0);
- *Q = S + (*P);
- }
-
- return S.length();
- }
- }
-
- BVH_REAL BLL_y, BLU_y, BUL_y, BUU_y;
-
- BLL_y = Tab[1];
- BLU_y = BLL_y + bA1_dot_B1;
- BUL_y = BLL_y + bA1_dot_B0;
- BUU_y = BLU_y + bA1_dot_B0;
-
- BVH_REAL LA0_lx, LA0_ux, UA0_lx, UA0_ux, LB1_ly, LB1_uy, UB1_ly, UB1_uy;
-
- if(ALL_x < AUL_x)
- {
- LA0_lx = ALL_x;
- LA0_ux = AUL_x;
- UA0_lx = ALU_x;
- UA0_ux = AUU_x;
- }
- else
- {
- LA0_lx = AUL_x;
- LA0_ux = ALL_x;
- UA0_lx = AUU_x;
- UA0_ux = ALU_x;
- }
-
- if(BLL_y < BLU_y)
- {
- LB1_ly = BLL_y;
- LB1_uy = BLU_y;
- UB1_ly = BUL_y;
- UB1_uy = BUU_y;
- }
- else
- {
- LB1_ly = BLU_y;
- LB1_uy = BLL_y;
- UB1_ly = BUU_y;
- UB1_uy = BUL_y;
- }
-
- // UA0, UB1
-
- if((UA0_ux > b[0]) && (UB1_uy > a[1]))
- {
- if(((UA0_lx > b[0]) ||
- inVoronoi(b[1], a[0], A0_dot_B0, aA1_dot_B0 - Tba[0] - b[0],
- A0_dot_B1, aA1_dot_B1 - Tba[1], -Tab[0] - bA0_dot_B0))
- &&
- ((UB1_ly > a[1]) ||
- inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1] + bA1_dot_B0,
- A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1] - aA1_dot_B1)))
- {
- segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0,
- Tba[1] - aA1_dot_B1);
-
- S[0] = Tab[0] + Rab[0][0] * b[0] + Rab[0][1] * u - t;
- S[1] = Tab[1] + Rab[1][0] * b[0] + Rab[1][1] * u - a[1];
- S[2] = Tab[2] + Rab[2][0] * b[0] + Rab[2][1] * u;
-
- if(P && Q)
- {
- *P = Vec3f(t, a[1], 0);
- *Q = S + (*P);
- }
-
- return S.length();
- }
- }
-
- // UA0, LB1
-
- if((UA0_lx < 0) && (LB1_uy > a[1]))
- {
- if(((UA0_ux < 0) ||
- inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0] - aA1_dot_B0, A0_dot_B1,
- aA1_dot_B1 - Tba[1], -Tab[0]))
- &&
- ((LB1_ly > a[1]) ||
- inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1], A0_dot_B1, Tab[0],
- Tba[1] - aA1_dot_B1)))
- {
- segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1] - aA1_dot_B1);
-
- S[0] = Tab[0] + Rab[0][1] * u - t;
- S[1] = Tab[1] + Rab[1][1] * u - a[1];
- S[2] = Tab[2] + Rab[2][1] * u;
-
- if(P && Q)
- {
- *P = Vec3f(t, a[1], 0);
- *Q = S + (*P);
- }
-
- return S.length();
- }
- }
-
- // LA0, UB1
-
- if((LA0_ux > b[0]) && (UB1_ly < 0))
- {
- if(((LA0_lx > b[0]) ||
- inVoronoi(b[1], a[0], A0_dot_B0, -b[0] - Tba[0], A0_dot_B1, -Tba[1],
- -bA0_dot_B0 - Tab[0]))
- &&
- ((UB1_uy < 0) ||
- inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1] - bA1_dot_B0, A0_dot_B1,
- Tab[0] + bA0_dot_B0, Tba[1])))
- {
- segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1]);
-
- S[0] = Tab[0] + Rab[0][0] * b[0] + Rab[0][1] * u - t;
- S[1] = Tab[1] + Rab[1][0] * b[0] + Rab[1][1] * u;
- S[2] = Tab[2] + Rab[2][0] * b[0] + Rab[2][1] * u;
-
- if(P && Q)
- {
- *P = Vec3f(t, 0, 0);
- *Q = S + (*P);
- }
-
- return S.length();
- }
- }
-
- // LA0, LB1
-
- if((LA0_lx < 0) && (LB1_ly < 0))
- {
- if(((LA0_ux < 0) ||
- inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0], A0_dot_B1, -Tba[1],
- -Tab[0]))
- &&
- ((LB1_uy < 0) ||
- inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1], A0_dot_B1,
- Tab[0], Tba[1])))
- {
- segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1]);
-
- S[0] = Tab[0] + Rab[0][1] * u - t;
- S[1] = Tab[1] + Rab[1][1] * u;
- S[2] = Tab[2] + Rab[2][1] * u;
-
- if(P && Q)
- {
- *P = Vec3f(t, 0, 0);
- *Q = S + (*P);
- }
-
- return S.length();
- }
- }
-
- BVH_REAL LA0_ly, LA0_uy, UA0_ly, UA0_uy, LB0_ly, LB0_uy, UB0_ly, UB0_uy;
-
- if(ALL_y < AUL_y)
- {
- LA0_ly = ALL_y;
- LA0_uy = AUL_y;
- UA0_ly = ALU_y;
- UA0_uy = AUU_y;
- }
- else
- {
- LA0_ly = AUL_y;
- LA0_uy = ALL_y;
- UA0_ly = AUU_y;
- UA0_uy = ALU_y;
- }
-
- if(BLL_y < BUL_y)
- {
- LB0_ly = BLL_y;
- LB0_uy = BUL_y;
- UB0_ly = BLU_y;
- UB0_uy = BUU_y;
- }
- else
- {
- LB0_ly = BUL_y;
- LB0_uy = BLL_y;
- UB0_ly = BUU_y;
- UB0_uy = BLU_y;
- }
-
- // UA0, UB0
-
- if((UA0_uy > b[1]) && (UB0_uy > a[1]))
- {
- if(((UA0_ly > b[1]) ||
- inVoronoi(b[0], a[0], A0_dot_B1, aA1_dot_B1 - Tba[1] - b[1],
- A0_dot_B0, aA1_dot_B0 - Tba[0], -Tab[0] - bA0_dot_B1))
- &&
- ((UB0_ly > a[1]) ||
- inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1] + bA1_dot_B1, A0_dot_B0,
- Tab[0] + bA0_dot_B1, Tba[0] - aA1_dot_B0)))
- {
- segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1,
- Tba[0] - aA1_dot_B0);
-
- S[0] = Tab[0] + Rab[0][1] * b[1] + Rab[0][0] * u - t;
- S[1] = Tab[1] + Rab[1][1] * b[1] + Rab[1][0] * u - a[1];
- S[2] = Tab[2] + Rab[2][1] * b[1] + Rab[2][0] * u;
-
- if(P && Q)
- {
- *P = Vec3f(t, a[1], 0);
- *Q = S + (*P);
- }
-
- return S.length();
- }
- }
-
- // UA0, LB0
-
- if((UA0_ly < 0) && (LB0_uy > a[1]))
- {
- if(((UA0_uy < 0) ||
- inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1] - aA1_dot_B1, A0_dot_B0,
- aA1_dot_B0 - Tba[0], -Tab[0]))
- &&
- ((LB0_ly > a[1]) ||
- inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1],
- A0_dot_B0, Tab[0], Tba[0] - aA1_dot_B0)))
- {
- segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0] - aA1_dot_B0);
-
- S[0] = Tab[0] + Rab[0][0] * u - t;
- S[1] = Tab[1] + Rab[1][0] * u - a[1];
- S[2] = Tab[2] + Rab[2][0] * u;
-
- if(P && Q)
- {
- *P = Vec3f(t, a[1], 0);
- *Q = S + (*P);
- }
-
- return S.length();
- }
- }
-
- // LA0, UB0
-
- if((LA0_uy > b[1]) && (UB0_ly < 0))
- {
- if(((LA0_ly > b[1]) ||
- inVoronoi(b[0], a[0], A0_dot_B1, -Tba[1] - b[1], A0_dot_B0, -Tba[0],
- -Tab[0] - bA0_dot_B1))
- &&
-
- ((UB0_uy < 0) ||
- inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1] - bA1_dot_B1, A0_dot_B0,
- Tab[0] + bA0_dot_B1, Tba[0])))
- {
- segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0]);
-
- S[0] = Tab[0] + Rab[0][1] * b[1] + Rab[0][0] * u - t;
- S[1] = Tab[1] + Rab[1][1] * b[1] + Rab[1][0] * u;
- S[2] = Tab[2] + Rab[2][1] * b[1] + Rab[2][0] * u;
-
- if(P && Q)
- {
- *P = Vec3f(t, 0, 0);
- *Q = S + (*P);
- }
-
- return S.length();
- }
- }
-
- // LA0, LB0
-
- if((LA0_ly < 0) && (LB0_ly < 0))
- {
- if(((LA0_uy < 0) ||
- inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1], A0_dot_B0,
- -Tba[0], -Tab[0]))
- &&
- ((LB0_uy < 0) ||
- inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1], A0_dot_B0,
- Tab[0], Tba[0])))
- {
- segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0]);
-
- S[0] = Tab[0] + Rab[0][0] * u - t;
- S[1] = Tab[1] + Rab[1][0] * u;
- S[2] = Tab[2] + Rab[2][0] * u;
-
- if(P && Q)
- {
- *P = Vec3f(t, 0, 0);
- *Q = S + (*P);
- }
-
- return S.length();
- }
- }
-
- // no edges passed, take max separation along face normals
-
- BVH_REAL sep1, sep2;
-
- if(Tab[2] > 0.0)
- {
- sep1 = Tab[2];
- if (Rab[2][0] < 0.0) sep1 += b[0] * Rab[2][0];
- if (Rab[2][1] < 0.0) sep1 += b[1] * Rab[2][1];
- }
- else
- {
- sep1 = -Tab[2];
- if (Rab[2][0] > 0.0) sep1 -= b[0] * Rab[2][0];
- if (Rab[2][1] > 0.0) sep1 -= b[1] * Rab[2][1];
- }
-
- if(Tba[2] < 0)
- {
- sep2 = -Tba[2];
- if (Rab[0][2] < 0.0) sep2 += a[0] * Rab[0][2];
- if (Rab[1][2] < 0.0) sep2 += a[1] * Rab[1][2];
- }
- else
- {
- sep2 = Tba[2];
- if (Rab[0][2] > 0.0) sep2 -= a[0] * Rab[0][2];
- if (Rab[1][2] > 0.0) sep2 -= a[1] * Rab[1][2];
- }
-
- if(sep1 >= sep2 && sep1 >= 0)
- {
- if(Tab[2] > 0)
- S = Vec3f(0, 0, sep1);
- else
- S = Vec3f(0, 0, -sep1);
-
- if(P && Q)
- {
- *Q = S;
- *P = Vec3f(0, 0, 0);
- }
- }
-
- if(sep2 >= sep1 && sep2 >= 0)
- {
- Vec3f Q_(Tab[0], Tab[1], Tab[2]);
- Vec3f P_;
- if(Tba[2] < 0)
- {
- P_[0] = Rab[0][2] * sep2 + Tab[0];
- P_[1] = Rab[1][2] * sep2 + Tab[1];
- P_[2] = Rab[2][2] * sep2 + Tab[2];
- }
- else
- {
- P_[0] = -Rab[0][2] * sep2 + Tab[0];
- P_[1] = -Rab[1][2] * sep2 + Tab[1];
- P_[2] = -Rab[2][2] * sep2 + Tab[2];
- }
-
- S = Q_ - P_;
-
- if(P && Q)
- {
- *P = P_;
- *Q = Q_;
- }
- }
-
- BVH_REAL sep = (sep1 > sep2 ? sep1 : sep2);
- return (sep > 0 ? sep : 0);
-}
-
-
-void RSS::clipToRange(BVH_REAL& val, BVH_REAL a, BVH_REAL b)
-{
- if(val < a) val = a;
- else if(val > b) val = b;
-}
-
-
-void RSS::segCoords(BVH_REAL& t, BVH_REAL& u, BVH_REAL a, BVH_REAL b, BVH_REAL A_dot_B, BVH_REAL A_dot_T, BVH_REAL B_dot_T)
-{
- BVH_REAL denom = 1 - A_dot_B * A_dot_B;
-
- if(denom == 0) t = 0;
- else
- {
- t = (A_dot_T - B_dot_T * A_dot_B) / denom;
- clipToRange(t, 0, a);
- }
-
- u = t * A_dot_B - B_dot_T;
- if(u < 0)
- {
- u = 0;
- t = A_dot_T;
- clipToRange(t, 0, a);
- }
- else if(u > b)
- {
- u = b;
- t = u * A_dot_B + A_dot_T;
- clipToRange(t, 0, a);
- }
-}
-
-bool RSS::inVoronoi(BVH_REAL a, BVH_REAL b, BVH_REAL Anorm_dot_B, BVH_REAL Anorm_dot_T, BVH_REAL A_dot_B, BVH_REAL A_dot_T, BVH_REAL B_dot_T)
-{
- if(fabs(Anorm_dot_B) < 1e-7) return false;
-
- BVH_REAL t, u, v;
-
- u = -Anorm_dot_T / Anorm_dot_B;
- clipToRange(u, 0, b);
-
- t = u * A_dot_B + A_dot_T;
- clipToRange(t, 0, a);
-
- v = t * A_dot_B - B_dot_T;
-
- if(Anorm_dot_B > 0)
- {
- if(v > (u + 1e-7)) return true;
- }
- else
- {
- if(v < (u - 1e-7)) return true;
- }
- return false;
-}
-
-
-
-
-
-}
diff --git a/branches/point_cloud/fcl/src/broad_phase_collision.cpp b/branches/point_cloud/fcl/src/broad_phase_collision.cpp
deleted file mode 100644
index f600d4ee362605ad7de3d52dde57bb2dd159874e..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/src/broad_phase_collision.cpp
+++ /dev/null
@@ -1,1057 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#include "fcl/broad_phase_collision.h"
-#include "fcl/collision.h"
-#include <algorithm>
-#include <set>
-#include <deque>
-
-namespace fcl
-{
-
-
-bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_)
-{
- CollisionData* cdata = (CollisionData*)cdata_;
-
- if(cdata->done) return true;
-
- std::vector<Contact> contacts;
- int num_contacts = collide(o1, o2, 1, false, false, contacts);
-
- cdata->is_collision = (num_contacts > 0);
- for(int i = 0; i < num_contacts; ++i)
- {
- cdata->contacts.push_back(contacts[i]);
- }
-
- // set done flag
- if( (!cdata->exhaustive) && (cdata->is_collision) && (cdata->contacts.size() >= cdata->num_max_contacts))
- cdata->done = true;
-
- return cdata->done;
-}
-
-void NaiveCollisionManager::unregisterObject(CollisionObject* obj)
-{
- objs.remove(obj);
-}
-
-void NaiveCollisionManager::registerObject(CollisionObject* obj)
-{
- objs.push_back(obj);
-}
-
-void NaiveCollisionManager::setup()
-{
-
-}
-
-void NaiveCollisionManager::update()
-{
-
-}
-
-void NaiveCollisionManager::clear()
-{
- objs.clear();
-}
-
-void NaiveCollisionManager::getObjects(std::vector<CollisionObject*>& objs_) const
-{
- objs_.resize(objs.size());
- std::list<CollisionObject*>::const_iterator it;
- int i = 0;
- for(it = objs.begin(); it != objs.end(); ++it, ++i)
- objs_[i] = *it;
-}
-
-void NaiveCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
-{
- std::list<CollisionObject*>::const_iterator it;
- for(it = objs.begin(); it != objs.end(); ++it)
- {
- if(callback(obj, *it, cdata))
- return;
- }
-}
-
-void NaiveCollisionManager::collide(void* cdata, CollisionCallBack callback) const
-{
- std::list<CollisionObject*>::const_iterator it1;
- std::list<CollisionObject*>::const_iterator it2;
- for(it1 = objs.begin(); it1 != objs.end(); ++it1)
- {
- it2 = it1; it2++;
- for(; it2 != objs.end(); ++it2)
- {
- if(callback(*it1, *it2, cdata))
- return;
- }
- }
-}
-
-bool NaiveCollisionManager::empty() const
-{
- return objs.empty();
-}
-
-
-
-void SSaPCollisionManager::unregisterObject(CollisionObject* obj)
-{
- setup();
-
- DummyCollisionObject dummyHigh(AABB(obj->getCachedAABB().max_));
-
- CollisionObject* found = NULL;
- std::vector<CollisionObject*>::iterator pos_start1 = objs_x.begin();
- std::vector<CollisionObject*>::iterator pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
-
- while(pos_start1 < pos_end1)
- {
- if(*pos_start1 == obj)
- {
- found = *pos_start1;
- objs_x.erase(pos_start1);
- break;
- }
- ++pos_start1;
- }
-
- std::vector<CollisionObject*>::iterator pos_start2 = objs_y.begin();
- std::vector<CollisionObject*>::iterator pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
- while(pos_start2 < pos_end2)
- {
- if(*pos_start2 == obj)
- {
- objs_y.erase(pos_start2);
- break;
- }
- ++pos_start2;
- }
-
- std::vector<CollisionObject*>::iterator pos_start3 = objs_z.begin();
- std::vector<CollisionObject*>::iterator pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow());
- while(pos_start3 < pos_end3)
- {
- if(*pos_start3 == obj)
- {
- objs_z.erase(pos_start3);
- break;
- }
- ++pos_start3;
- }
-}
-
-
-void SSaPCollisionManager::registerObject(CollisionObject* obj)
-{
- objs_x.push_back(obj);
- objs_y.push_back(obj);
- objs_z.push_back(obj);
- setup_ = false;
-}
-
-void SSaPCollisionManager::setup()
-{
- if(!setup_)
- {
- std::sort(objs_x.begin(), objs_x.end(), SortByXLow());
- std::sort(objs_y.begin(), objs_y.end(), SortByYLow());
- std::sort(objs_z.begin(), objs_z.end(), SortByZLow());
- setup_ = true;
- }
-}
-
-void SSaPCollisionManager::update()
-{
- setup();
-}
-
-void SSaPCollisionManager::clear()
-{
- objs_x.clear();
- objs_y.clear();
- objs_z.clear();
- setup_ = false;
-}
-
-void SSaPCollisionManager::getObjects(std::vector<CollisionObject*>& objs) const
-{
- objs.resize(objs_x.size());
- for(unsigned int i = 0; i < objs.size(); ++i)
- objs[i] = objs_x[i];
-}
-
-void SSaPCollisionManager::checkColl(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end,
- CollisionObject* obj, void* cdata, CollisionCallBack callback) const
-{
- while(pos_start < pos_end)
- {
- if((*pos_start)->getCachedAABB().overlap(obj->getCachedAABB()))
- {
- if(callback(*pos_start, obj, cdata))
- return;
- }
- pos_start++;
- }
-}
-
-
-void SSaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
-{
- static const unsigned int CUTOFF = 100;
-
- DummyCollisionObject dummyHigh(AABB(obj->getCachedAABB().max_));
-
- std::vector<CollisionObject*>::const_iterator pos_start1 = objs_x.begin();
- std::vector<CollisionObject*>::const_iterator pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
- unsigned int d1 = pos_end1 - pos_start1;
- if(d1 > CUTOFF)
- {
- std::vector<CollisionObject*>::const_iterator pos_start2 = objs_y.begin();
-
- std::vector<CollisionObject*>::const_iterator pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
- unsigned int d2 = pos_end2 - pos_start2;
-
- if(d2 > CUTOFF)
- {
- std::vector<CollisionObject*>::const_iterator pos_start3 = objs_z.begin();
-
- std::vector<CollisionObject*>::const_iterator pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow());
- unsigned int d3 = pos_end3 - pos_start3;
-
- if(d3 > CUTOFF)
- {
- if(d3 <= d2 && d3 <= d1)
- checkColl(pos_start3, pos_end3, obj, cdata, callback);
- else
- {
- if(d2 <= d3 && d2 <= d1)
- checkColl(pos_start2, pos_end2, obj, cdata, callback);
- else
- checkColl(pos_start1, pos_end1, obj, cdata, callback);
- }
- }
- else
- checkColl(pos_start3, pos_end3, obj, cdata, callback);
- }
- else
- checkColl(pos_start2, pos_end2, obj, cdata, callback);
- }
- else
- checkColl(pos_start1, pos_end1, obj, cdata, callback);
-}
-
-
-void SSaPCollisionManager::collide(void* cdata, CollisionCallBack callback) const
-{
- // simple sweep and prune method
- double delta_x = (objs_x[objs_x.size() - 1])->getCachedAABB().min_[0] - (objs_x[0])->getCachedAABB().min_[0];
- double delta_y = (objs_x[objs_y.size() - 1])->getCachedAABB().min_[1] - (objs_y[0])->getCachedAABB().min_[1];
- double delta_z = (objs_z[objs_z.size() - 1])->getCachedAABB().min_[2] - (objs_z[0])->getCachedAABB().min_[2];
-
- int axis = 0;
- if(delta_y > delta_x && delta_y > delta_z)
- axis = 1;
- else if(delta_z > delta_y && delta_z > delta_x)
- axis = 2;
-
- int axis2 = (axis + 1 > 2) ? 0 : (axis + 1);
- int axis3 = (axis2 + 1 > 2) ? 0 : (axis2 + 1);
-
- std::vector<CollisionObject*>::const_iterator pos, run_pos, pos_end;
-
- switch(axis)
- {
- case 0:
- pos = objs_x.begin();
- pos_end = objs_x.end();
- break;
- case 1:
- pos = objs_y.begin();
- pos_end = objs_y.end();
- break;
- case 2:
- pos = objs_z.begin();
- pos_end = objs_z.end();
- break;
- }
- run_pos = pos;
-
- while((run_pos != pos_end) && (pos != pos_end))
- {
- CollisionObject* obj = *(pos++);
-
- while(1)
- {
- if((*run_pos)->getCachedAABB().min_[axis] < obj->getCachedAABB().min_[axis])
- {
- run_pos++;
- if(run_pos == pos_end) break;
- continue;
- }
- else
- {
- run_pos++;
- break;
- }
- }
-
- if(run_pos != pos_end)
- {
- std::vector<CollisionObject*>::const_iterator run_pos2 = run_pos;
-
- while((*run_pos2)->getCachedAABB().min_[axis] <= obj->getCachedAABB().max_[axis])
- {
- CollisionObject* obj2 = *run_pos2;
- run_pos2++;
-
- if((obj->getCachedAABB().max_[axis2] >= obj2->getCachedAABB().min_[axis2]) && (obj2->getCachedAABB().max_[axis2] >= obj->getCachedAABB().min_[axis2]))
- {
- if((obj->getCachedAABB().max_[axis3] >= obj2->getCachedAABB().min_[axis3]) && (obj2->getCachedAABB().max_[axis3] >= obj->getCachedAABB().min_[axis3]))
- {
- if(callback(obj, obj2, cdata))
- return;
- }
- }
-
- if(run_pos2 == pos_end) break;
- }
- }
- }
-}
-
-bool SSaPCollisionManager::empty() const
-{
- return objs_x.empty();
-}
-
-
-void SaPCollisionManager::unregisterObject(CollisionObject* obj)
-{
- std::list<SaPAABB*>::iterator it = AABB_arr.begin();
- for(; it != AABB_arr.end(); ++it)
- {
- if((*it)->obj == obj)
- break;
- }
-
- AABB_arr.erase(it);
-
- if(it == AABB_arr.end())
- return;
-
- SaPAABB* curr = *it;
- *it = NULL;
-
- for(int coord = 0; coord < 3; ++coord)
- {
- //first delete the lo endpoint of the interval.
- if(curr->lo->prev[coord] == NULL)
- elist[coord] = curr->lo->next[coord];
- else
- curr->lo->prev[coord]->next[coord] = curr->lo->next[coord];
-
- curr->lo->next[coord]->prev[coord] = curr->lo->prev[coord];
-
- //then, delete the "hi" endpoint.
- if(curr->hi->prev[coord] == NULL)
- elist[coord] = curr->hi->next[coord];
- else
- curr->hi->prev[coord]->next[coord] = curr->hi->next[coord];
-
- if(curr->hi->next[coord] != NULL)
- curr->hi->next[coord]->prev[coord] = curr->hi->prev[coord];
- }
-
- delete curr->lo;
- delete curr->hi;
- delete curr;
-
- overlap_pairs.remove_if(isUnregistered(obj));
-}
-
-void SaPCollisionManager::registerObject(CollisionObject* obj)
-{
- SaPAABB* curr = new SaPAABB;
- curr->cached = obj->getCachedAABB();
- curr->obj = obj;
- curr->lo = new EndPoint;
- curr->lo->minmax = 0;
- curr->lo->aabb = curr;
-
- curr->hi = new EndPoint;
- curr->hi->minmax = 1;
- curr->hi->aabb = curr;
-
- for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(); it != AABB_arr.end(); ++it)
- {
- if((*it)->cached.overlap(curr->cached))
- overlap_pairs.push_back(SaPPair(obj, (*it)->obj));
- }
-
- AABB_arr.push_back(curr);
-
- for(int coord = 0; coord < 3; ++coord)
- {
- EndPoint* current = elist[coord];
-
- // first insert the hi end point
- if(current == NULL) // empty list
- {
- elist[coord] = curr->hi;
- curr->hi->prev[coord] = curr->hi->next[coord] = NULL;
- }
- else // otherwise, find the correct location in the list and insert
- {
- while((current->next[coord] != NULL) &&
- (current->getVal()[coord] < curr->hi->getVal()[coord]))
- current = current->next[coord];
-
- if(current->getVal()[coord] >= curr->hi->getVal()[coord])
- {
- curr->hi->prev[coord] = current->prev[coord];
- curr->hi->next[coord] = current;
- if(current->prev[coord] == NULL)
- elist[coord] = curr->hi;
- else
- current->prev[coord]->next[coord] = curr->hi;
- }
- else
- {
- curr->hi->prev[coord] = current;
- curr->hi->next[coord] = NULL;
- current->next[coord] = curr->hi;
- }
- }
-
- // now insert lo end point
- current = elist[coord];
-
- while ((current->next[coord] != NULL) && (current->getVal()[coord] < curr->lo->getVal()[coord]))
- current = current->next[coord];
-
- if (current->getVal()[coord] >= curr->lo->getVal()[coord])
- {
- curr->lo->prev[coord] = current->prev[coord];
- curr->lo->next[coord] = current;
- if(current->prev[coord] == NULL)
- elist[coord] = curr->lo;
- else
- current->prev[coord]->next[coord] = curr->lo;
-
- current->prev[coord] = curr->lo;
- }
- else
- {
- curr->lo->prev[coord] = current;
- curr->lo->next[coord] = NULL;
- current->next[coord] = curr->lo;
- }
- }
-}
-
-void SaPCollisionManager::setup()
-{
-
-}
-
-void SaPCollisionManager::update()
-{
- for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(); it != AABB_arr.end(); ++it)
- {
- SaPAABB* current = *it;
-
- Vec3f new_min = current->obj->getCachedAABB().min_;
- Vec3f new_max = current->obj->getCachedAABB().max_;
-
- SaPAABB dummy;
- dummy.cached = current->obj->getCachedAABB();
-
- EndPoint lo, hi;
- dummy.lo = &lo;
- dummy.hi = &hi;
-
- lo.minmax = 0;
- lo.aabb = &dummy;
- hi.minmax = 1;
- hi.aabb = &dummy;
-
-
- for(int coord = 0; coord < 3; ++coord)
- {
- int direction; // -1 reverse, 0 nochange, 1 forward
- EndPoint* temp;
-
- if(current->lo->getVal()[coord] > new_min[coord])
- direction = -1;
- else if(current->lo->getVal()[coord] < new_min[coord])
- direction = 1;
- else direction = 0;
-
- if(direction == -1)
- {
- //first update the "lo" endpoint of the interval
- if(current->lo->prev[coord] != NULL)
- {
- temp = current->lo;
- while((temp != NULL) && (temp->getVal()[coord] > new_min[coord]))
- {
- if(temp->minmax == 1)
- if(temp->aabb->cached.overlap(dummy.cached))
- overlap_pairs.push_back(SaPPair(temp->aabb->obj, current->obj));
- temp = temp->prev[coord];
- }
-
- if(temp == NULL)
- {
- current->lo->prev[coord]->next[coord] = current->lo->next[coord];
- current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
- current->lo->prev[coord] = NULL;
- current->lo->next[coord] = elist[coord];
- elist[coord]->prev[coord] = current->lo;
- elist[coord] = current->lo;
- }
- else
- {
- current->lo->prev[coord]->next[coord] = current->lo->next[coord];
- current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
- current->lo->prev[coord] = temp;
- current->lo->next[coord] = temp->next[coord];
- temp->next[coord]->prev[coord] = current->lo;
- temp->next[coord] = current->lo;
- }
- }
-
- current->lo->getVal()[coord] = new_min[coord];
-
- // update hi end point
- temp = current->hi;
- while(temp->getVal()[coord] > new_max[coord])
- {
- if((temp->minmax == 0) && (temp->aabb->cached.overlap(current->cached)))
- overlap_pairs.remove_if(isNotValidPair(temp->aabb->obj, current->obj));
- temp = temp->prev[coord];
- }
-
- current->hi->prev[coord]->next[coord] = current->hi->next[coord];
- if(current->hi->next[coord] != NULL)
- current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
- current->hi->prev[coord] = temp;
- current->hi->next[coord] = temp->next[coord];
- if(temp->next[coord] != NULL)
- temp->next[coord]->prev[coord] = current->hi;
- temp->next[coord] = current->hi;
-
- current->hi->getVal()[coord] = new_max[coord];
- }
- else if (direction == 1)
- {
- //here, we first update the "hi" endpoint.
- if(current->hi->next[coord] != NULL)
- {
- temp = current->hi;
- while((temp->next[coord] != NULL) && (temp->getVal()[coord] < new_max[coord]))
- {
- if(temp->minmax == 0)
- if(temp->aabb->cached.overlap(dummy.cached))
- overlap_pairs.push_back(SaPPair(temp->aabb->obj, current->obj));
- temp = temp->next[coord];
- }
-
- if(temp->getVal()[coord] < new_max[coord])
- {
- current->hi->prev[coord]->next[coord] = current->hi->next[coord];
- current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
- current->hi->prev[coord] = temp;
- current->hi->next[coord] = NULL;
- temp->next[coord] = current->hi;
- }
- else
- {
- current->hi->prev[coord]->next[coord] = current->hi->next[coord];
- current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
- current->hi->prev[coord] = temp->prev[coord];
- current->hi->next[coord] = temp;
- temp->prev[coord]->next[coord] = current->hi;
- temp->prev[coord] = current->hi;
- }
- }
-
- current->hi->getVal()[coord] = new_max[coord];
-
- //then, update the "lo" endpoint of the interval.
- temp = current->lo;
-
- while(temp->getVal()[coord] < new_min[coord])
- {
- if((temp->minmax == 1) && (temp->aabb->cached.overlap(current->cached)))
- overlap_pairs.remove_if(isNotValidPair(temp->aabb->obj, current->obj));
- temp = temp->next[coord];
- }
-
- if(current->lo->prev[coord] != NULL)
- current->lo->prev[coord]->next[coord] = current->lo->next[coord];
- else
- elist[coord] = current->lo->next[coord];
- current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
- current->lo->prev[coord] = temp->prev[coord];
- current->lo->next[coord] = temp;
- if(temp->prev[coord] != NULL)
- temp->prev[coord]->next[coord] = current->lo;
- else
- elist[coord] = current->lo;
- temp->prev[coord] = current->lo;
- current->lo->getVal()[coord] = new_min[coord];
- }
- }
- }
-}
-
-
-void SaPCollisionManager::clear()
-{
- for(std::list<SaPAABB*>::iterator it = AABB_arr.begin();
- it != AABB_arr.end();
- ++it)
- {
- delete (*it)->hi;
- delete (*it)->lo;
- delete *it;
- *it = NULL;
- }
-
- AABB_arr.clear();
- overlap_pairs.clear();
-
- elist[0] = NULL;
- elist[1] = NULL;
- elist[2] = NULL;
-}
-
-void SaPCollisionManager::getObjects(std::vector<CollisionObject*>& objs) const
-{
- objs.resize(AABB_arr.size());
- int i = 0;
- for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(); it != AABB_arr.end(); ++it, ++i)
- {
- objs[i] = (*it)->obj;
- }
-}
-
-void SaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
-{
- for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(); it != AABB_arr.end(); ++it)
- {
- if((*it)->obj->getCachedAABB().overlap(obj->getCachedAABB()))
- {
- if(callback(obj, (*it)->obj, cdata))
- return;
- }
- }
-}
-
-void SaPCollisionManager::collide(void* cdata, CollisionCallBack callback) const
-{
- for(std::list<SaPPair>::const_iterator it = overlap_pairs.begin(); it != overlap_pairs.end(); ++it)
- {
- CollisionObject* obj1 = it->obj1;
- CollisionObject* obj2 = it->obj2;
-
- if(callback(obj1, obj2, cdata))
- return;
- }
-}
-
-bool SaPCollisionManager::empty() const
-{
- return AABB_arr.size();
-}
-
-
-
-void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj)
-{
- // must sorted before
- setup();
-
- EndPoint p;
- p.value = obj->getCachedAABB().min_[0];
- std::vector<EndPoint>::iterator start1 = std::lower_bound(endpoints[0].begin(), endpoints[0].end(), p, SortByValue());
- p.value = obj->getCachedAABB().max_[0];
- std::vector<EndPoint>::iterator end1 = std::upper_bound(start1, endpoints[0].end(), p, SortByValue());
-
- if(start1 < end1)
- {
- unsigned int start_id = start1 - endpoints[0].begin();
- unsigned int end_id = end1 - endpoints[0].begin();
- unsigned int cur_id = start_id;
- for(unsigned int i = start_id; i < end_id; ++i)
- {
- if(endpoints[0][i].obj != obj)
- {
- if(i == cur_id) cur_id++;
- else
- {
- endpoints[0][cur_id] = endpoints[0][i];
- cur_id++;
- }
- }
- }
- if(cur_id < end_id)
- endpoints[0].resize(endpoints[0].size() - 2);
- }
-
- p.value = obj->getCachedAABB().min_[1];
- std::vector<EndPoint>::iterator start2 = std::lower_bound(endpoints[1].begin(), endpoints[1].end(), p, SortByValue());
- p.value = obj->getCachedAABB().max_[1];
- std::vector<EndPoint>::iterator end2 = std::upper_bound(start2, endpoints[1].end(), p, SortByValue());
-
- if(start2 < end2)
- {
- unsigned int start_id = start2 - endpoints[1].begin();
- unsigned int end_id = end2 - endpoints[1].begin();
- unsigned int cur_id = start_id;
- for(unsigned int i = start_id; i < end_id; ++i)
- {
- if(endpoints[1][i].obj != obj)
- {
- if(i == cur_id) cur_id++;
- else
- {
- endpoints[1][cur_id] = endpoints[1][i];
- cur_id++;
- }
- }
- }
- if(cur_id < end_id)
- endpoints[1].resize(endpoints[1].size() - 2);
- }
-
-
- p.value = obj->getCachedAABB().min_[2];
- std::vector<EndPoint>::iterator start3 = std::lower_bound(endpoints[2].begin(), endpoints[2].end(), p, SortByValue());
- p.value = obj->getCachedAABB().max_[2];
- std::vector<EndPoint>::iterator end3 = std::upper_bound(start3, endpoints[2].end(), p, SortByValue());
-
- if(start3 < end3)
- {
- unsigned int start_id = start3 - endpoints[2].begin();
- unsigned int end_id = end3 - endpoints[2].begin();
- unsigned int cur_id = start_id;
- for(unsigned int i = start_id; i < end_id; ++i)
- {
- if(endpoints[2][i].obj != obj)
- {
- if(i == cur_id) cur_id++;
- else
- {
- endpoints[2][cur_id] = endpoints[2][i];
- cur_id++;
- }
- }
- }
- if(cur_id < end_id)
- endpoints[2].resize(endpoints[2].size() - 2);
- }
-}
-
-void IntervalTreeCollisionManager::registerObject(CollisionObject* obj)
-{
- EndPoint p, q;
-
- p.obj = obj;
- q.obj = obj;
- p.minmax = 0;
- q.minmax = 1;
- p.value = obj->getCachedAABB().min_[0];
- q.value = obj->getCachedAABB().max_[0];
- endpoints[0].push_back(p);
- endpoints[0].push_back(q);
-
- p.value = obj->getCachedAABB().min_[1];
- q.value = obj->getCachedAABB().max_[1];
- endpoints[1].push_back(p);
- endpoints[1].push_back(q);
-
- p.value = obj->getCachedAABB().min_[2];
- q.value = obj->getCachedAABB().max_[2];
- endpoints[2].push_back(p);
- endpoints[2].push_back(q);
- setup_ = false;
-}
-
-void IntervalTreeCollisionManager::setup()
-{
- if(!setup_)
- {
- std::sort(endpoints[0].begin(), endpoints[0].end(), SortByValue());
- std::sort(endpoints[1].begin(), endpoints[1].end(), SortByValue());
- std::sort(endpoints[2].begin(), endpoints[2].end(), SortByValue());
-
- for(int i = 0; i < 3; ++i)
- delete interval_trees[i];
-
- for(int i = 0; i < 3; ++i)
- interval_trees[i] = new IntervalTree;
-
- for(unsigned int i = 0; i < endpoints[0].size(); ++i)
- {
- EndPoint p = endpoints[0][i];
- CollisionObject* obj = p.obj;
- if(p.minmax == 0)
- {
- SAPInterval* ivl1 = new SAPInterval(obj->getCachedAABB().min_[0], obj->getCachedAABB().max_[0], obj);
- SAPInterval* ivl2 = new SAPInterval(obj->getCachedAABB().min_[1], obj->getCachedAABB().max_[1], obj);
- SAPInterval* ivl3 = new SAPInterval(obj->getCachedAABB().min_[2], obj->getCachedAABB().max_[2], obj);
- interval_trees[0]->insert(ivl1);
- interval_trees[1]->insert(ivl2);
- interval_trees[2]->insert(ivl3);
- }
- }
-
- setup_ = true;
- }
-}
-
-void IntervalTreeCollisionManager::update()
-{
- setup_ = false;
-
- for(unsigned int i = 0; i < endpoints[0].size(); ++i)
- {
- if(endpoints[0][i].minmax == 0)
- endpoints[0][i].value = endpoints[0][i].obj->getCachedAABB().min_[0];
- else
- endpoints[0][i].value = endpoints[0][i].obj->getCachedAABB().max_[0];
- }
-
- for(unsigned int i = 0; i < endpoints[1].size(); ++i)
- {
- if(endpoints[1][i].minmax == 0)
- endpoints[1][i].value = endpoints[1][i].obj->getCachedAABB().min_[1];
- else
- endpoints[1][i].value = endpoints[1][i].obj->getCachedAABB().max_[1];
- }
-
- for(unsigned int i = 0; i < endpoints[2].size(); ++i)
- {
- if(endpoints[2][i].minmax == 0)
- endpoints[2][i].value = endpoints[2][i].obj->getCachedAABB().min_[2];
- else
- endpoints[2][i].value = endpoints[2][i].obj->getCachedAABB().max_[2];
- }
-
- setup();
-
-}
-
-void IntervalTreeCollisionManager::clear()
-{
- endpoints[0].clear();
- endpoints[1].clear();
- endpoints[2].clear();
- setup_ = false;
-}
-
-void IntervalTreeCollisionManager::getObjects(std::vector<CollisionObject*>& objs) const
-{
- objs.resize(endpoints[0].size() / 2);
- unsigned int j = 0;
- for(unsigned int i = 0; i < endpoints[0].size(); ++i)
- {
- if(endpoints[0][i].minmax == 0)
- {
- objs[j] = endpoints[0][i].obj; j++;
- }
- }
-}
-
-void IntervalTreeCollisionManager::checkColl(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end,
- CollisionObject* obj, void* cdata, CollisionCallBack callback) const
-{
-
-}
-
-void IntervalTreeCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
-{
- static const unsigned int CUTOFF = 100;
-
- std::deque<Interval*> results0, results1, results2;
-
- results0 = interval_trees[0]->query(obj->getCachedAABB().min_[0], obj->getCachedAABB().max_[0]);
- if(results0.size() > CUTOFF)
- {
- results1 = interval_trees[1]->query(obj->getCachedAABB().min_[1], obj->getCachedAABB().max_[1]);
- if(results1.size() > CUTOFF)
- {
- results2 = interval_trees[2]->query(obj->getCachedAABB().min_[2], obj->getCachedAABB().max_[2]);
- if(results2.size() > CUTOFF)
- {
- int d1 = results0.size();
- int d2 = results1.size();
- int d3 = results2.size();
-
- if(d1 >= d2 && d1 >= d3)
- {
- for(unsigned int i = 0; i < results0.size(); ++i)
- {
- SAPInterval* ivl = (SAPInterval*)results0[i];
- if(callback(ivl->obj, obj, cdata))
- break;
- }
- }
- else if(d2 >= d1 && d2 >= d3)
- {
- for(unsigned int i = 0; i < results1.size(); ++i)
- {
- SAPInterval* ivl = (SAPInterval*)results1[i];
- if(callback(ivl->obj, obj, cdata))
- break;
- }
- }
- else
- {
- for(unsigned int i = 0; i < results2.size(); ++i)
- {
- SAPInterval* ivl = (SAPInterval*)results2[i];
- if(callback(ivl->obj, obj, cdata))
- break;
- }
- }
- }
- else
- {
- for(unsigned int i = 0; i < results2.size(); ++i)
- {
- SAPInterval* ivl = (SAPInterval*)results2[i];
- if(callback(ivl->obj, obj, cdata))
- break;
- }
- }
- }
- else
- {
- for(unsigned int i = 0; i < results1.size(); ++i)
- {
- SAPInterval* ivl = (SAPInterval*)results1[i];
- if(callback(ivl->obj, obj, cdata))
- break;
- }
- }
- }
- else
- {
- for(unsigned int i = 0; i < results0.size(); ++i)
- {
- SAPInterval* ivl = (SAPInterval*)results0[i];
- if(callback(ivl->obj, obj, cdata))
- break;
- }
- }
-
- results0.clear();
- results1.clear();
- results2.clear();
-}
-
-void IntervalTreeCollisionManager::collide(void* cdata, CollisionCallBack callback) const
-{
- std::set<CollisionObject*> active;
- std::set<std::pair<CollisionObject*, CollisionObject*> > overlap;
- unsigned int n = endpoints[0].size();
- double diff_x = endpoints[0][0].value - endpoints[0][n-1].value;
- double diff_y = endpoints[1][0].value - endpoints[1][n-1].value;
- double diff_z = endpoints[2][0].value - endpoints[2][n-1].value;
-
- int axis = 0;
- if(diff_y > diff_x && diff_y > diff_z)
- axis = 1;
- else if(diff_z > diff_y && diff_z > diff_x)
- axis = 2;
-
- for(unsigned int i = 0; i < n; ++i)
- {
- const EndPoint& endpoint = endpoints[axis][i];
- CollisionObject* index = endpoint.obj;
- if(endpoint.minmax == 0)
- {
- std::set<CollisionObject*>::iterator iter = active.begin();
- std::set<CollisionObject*>::iterator end = active.end();
- for(; iter != end; ++iter)
- {
- CollisionObject* active_index = *iter;
- const AABB& b0 = active_index->getCachedAABB();
- const AABB& b1 = index->getCachedAABB();
-
- int axis2 = (axis + 1) % 3;
- int axis3 = (axis + 2) % 3;
-
- if(b0.axisOverlap(b1, axis2) && b0.axisOverlap(b1, axis3))
- {
- std::pair<std::set<std::pair<CollisionObject*, CollisionObject*> >::iterator, bool> insert_res;
- if(active_index < index)
- insert_res = overlap.insert(std::make_pair(active_index, index));
- else
- insert_res = overlap.insert(std::make_pair(index, active_index));
-
- if(insert_res.second)
- {
- if(callback(active_index, index, cdata))
- return;
- }
- }
- }
- active.insert(index);
- }
- else
- active.erase(index);
- }
-
-}
-
-bool IntervalTreeCollisionManager::empty() const
-{
- return endpoints[0].empty();
-}
-
-
-
-}
diff --git a/branches/point_cloud/fcl/src/collision.cpp b/branches/point_cloud/fcl/src/collision.cpp
deleted file mode 100644
index 2ce4ee0bc3f86e3884f8e07c3383c831b01c3d12..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/src/collision.cpp
+++ /dev/null
@@ -1,148 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-/*
-#include "fcl/collision.h"
-#include "fcl/collision_func_matrix.h"
-
-#include <iostream>
-
-namespace fcl
-{
-
-static CollisionFunctionMatrix CollisionFunctionLookTable;
-
-int collide(const CollisionObject* o1, const CollisionObject* o2,
- int num_max_contacts, bool exhaustive, bool enable_contact,
- std::vector<Contact>& contacts)
-{
- if(num_max_contacts <= 0 && !exhaustive)
- {
- std::cerr << "Warning: should stop early as num_max_contact is " << num_max_contacts << " !" << std::endl;
- return 0;
- }
-
- const OBJECT_TYPE object_type1 = o1->getObjectType();
- const NODE_TYPE node_type1 = o1->getNodeType();
-
- const OBJECT_TYPE object_type2 = o2->getObjectType();
- const NODE_TYPE node_type2 = o2->getNodeType();
-
-
- if(object_type1 == OT_GEOM && object_type2 == OT_GEOM)
- {
- return CollisionFunctionLookTable.collision_matrix[node_type1][node_type2](o1, o2, num_max_contacts, exhaustive, enable_contact, contacts);
- }
- else if(object_type1 == OT_GEOM && object_type2 == OT_BVH)
- {
- return CollisionFunctionLookTable.collision_matrix[node_type2][node_type1](o2, o1, num_max_contacts, exhaustive, enable_contact, contacts);
- }
- else if(object_type1 == OT_BVH && object_type2 == OT_GEOM)
- {
- return CollisionFunctionLookTable.collision_matrix[node_type1][node_type2](o1, o2, num_max_contacts, exhaustive, enable_contact, contacts);
- }
- else if(object_type1 == OT_BVH && object_type2 == OT_BVH)
- {
- if(node_type1 == node_type2)
- {
- return CollisionFunctionLookTable.collision_matrix[node_type1][node_type2](o1, o2, num_max_contacts, exhaustive, enable_contact, contacts);
- }
- }
-
- return 0;
-}
-
-}
-
-*/
-
-#include "fcl/collision.h"
-#include "fcl/simple_setup.h"
-#include "fcl/geometric_shapes.h"
-#include "fcl/BVH_model.h"
-#include "fcl/collision_node.h"
-#include "fcl/geometric_shapes_intersect.h"
-
-namespace fcl
-{
-
-int collide(const CollisionObject* o1, const CollisionObject* o2,
- int num_max_contacts, bool exhaustive, bool enable_contact,
- std::vector<Contact>& contacts)
-{
- const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
- const BVHModel<OBB>* obj2 = (BVHModel<OBB>*)o2;
-
- PointCloudCollisionTraversalNodeOBB node;
-
- node.model1 = obj1;
- node.model2 = obj2;
- node.vertices1 = obj1->vertices;
- node.vertices2 = obj2->vertices;
-
-
-
- Uncertainty* uc1 = new Uncertainty[obj1->num_vertices];
- Uncertainty* uc2 = new Uncertainty[obj2->num_vertices];
- memcpy(uc1, obj1->uc, sizeof(Uncertainty) * obj1->num_vertices);
- memcpy(uc2, obj2->uc, sizeof(Uncertainty) * obj2->num_vertices);
-
- node.uc1.reset(uc1);
- node.uc2.reset(uc2);
-
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
- node.collision_prob_threshold = 0.6;
- node.leaf_size_threshold = 20;
-
- relativeTransform(obj1->getRotation(), obj1->getTranslation(), obj2->getRotation(), obj2->getTranslation(), node.R, node.T);
-
- collide(&node);
- int num_contacts = node.pairs.size();
- if(num_contacts > 0)
- {
- if((!exhaustive) && (num_contacts > num_max_contacts)) num_contacts = num_max_contacts;
- contacts.resize(num_contacts);
- for(int i = 0; i < num_contacts; ++i)
- {
- contacts[i] = Contact(obj1, obj2, node.pairs[i].id1_start, node.pairs[i].id2_start);
- }
- }
- return num_contacts;
-}
-
-}
diff --git a/branches/point_cloud/fcl/src/collision_func_matrix.cpp b/branches/point_cloud/fcl/src/collision_func_matrix.cpp
deleted file mode 100644
index 5c64b2e28c69f96e71b2fca009363051813fb9e7..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/src/collision_func_matrix.cpp
+++ /dev/null
@@ -1,1153 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#include "fcl/collision_func_matrix.h"
-
-#include "fcl/collision.h"
-#include "fcl/simple_setup.h"
-#include "fcl/geometric_shapes.h"
-#include "fcl/BVH_model.h"
-#include "fcl/collision_node.h"
-#include "fcl/geometric_shapes_intersect.h"
-
-
-namespace fcl
-{
-
-/** \brief Hey, I know it is ugly... but it is the best way I can find now... */
-
-#define SHAPESHAPE_COMMON_CODE() do{ \
- initialize(node, *obj1, *obj2, obj1->getRotation(), obj1->getTranslation(), obj2->getRotation(), obj2->getTranslation(), enable_contact); \
- collide(&node); \
- if(!node.is_collision) return 0; \
- contacts.resize(1); \
- if(!enable_contact) contacts[0] = Contact(o1, o2, 0, 0); \
- else contacts[0] = Contact(o1, o2, 0, 0, node.contact_point, node.normal, node.penetration_depth); \
- return 1; \
- } while(0)
-
-
-#define MESHSHAPE_COMMON_CODE() do{ \
- initialize(node, *obj1_tmp, *obj2_tmp, obj1_tmp->getRotation(), obj1_tmp->getTranslation(), obj2_tmp->getRotation(), obj2_tmp->getTranslation(), num_max_contacts, exhaustive, enable_contact); \
- collide(&node); \
- int num_contacts = node.pairs.size(); \
- if(num_contacts > 0) \
- { \
- if((!exhaustive) && (num_contacts > num_max_contacts)) num_contacts = num_max_contacts; \
- contacts.resize(num_contacts); \
- if(!enable_contact) \
- { \
- for(int i = 0; i < num_contacts; ++i) \
- contacts[i] = Contact(obj1, obj2, node.pairs[i].id, 0); \
- } \
- else \
- { \
- for(int i = 0; i < num_contacts; ++i) \
- contacts[i] = Contact(obj1, obj2, node.pairs[i].id, 0, node.pairs[i].contact_point, node.pairs[i].normal, node.pairs[i].penetration_depth); \
- } \
- } \
- delete obj1_tmp; \
- obj1_tmp = NULL; \
- delete obj2_tmp; \
- obj2_tmp = NULL; \
- return num_contacts; \
- } while(0)
-
-#define MESHSHAPEOBBRSS_COMMON_CODE() do{ \
- initialize(node, *obj1, *obj2_tmp, obj1->getRotation(), obj1->getTranslation(), obj2_tmp->getRotation(), obj2_tmp->getTranslation(), num_max_contacts, exhaustive, enable_contact); \
- collide(&node); \
- int num_contacts = node.pairs.size(); \
- if(num_contacts > 0) \
- { \
- if((!exhaustive) && (num_contacts > num_max_contacts)) num_contacts = num_max_contacts; \
- contacts.resize(num_contacts); \
- if(!enable_contact) \
- { \
- for(int i = 0; i < num_contacts; ++i) \
- contacts[i] = Contact(obj1, obj2, node.pairs[i].id, 0); \
- } \
- else \
- { \
- for(int i = 0; i < num_contacts; ++i) \
- contacts[i] = Contact(obj1, obj2, node.pairs[i].id, 0, node.pairs[i].contact_point, node.pairs[i].normal, node.pairs[i].penetration_depth); \
- } \
- } \
- delete obj2_tmp; \
- obj2_tmp = NULL; \
- return num_contacts; \
- } while(0)
-
-
-
-#define MESHMESH_COMMON_CODE() do{ \
- initialize(node, *obj1_tmp, *obj2_tmp, obj1_tmp->getRotation(), obj1_tmp->getTranslation(), obj2_tmp->getRotation(), obj2_tmp->getTranslation(), num_max_contacts, exhaustive, enable_contact); \
- collide(&node); \
- int num_contacts = node.pairs.size(); \
- if(num_contacts > 0) \
- { \
- if((!exhaustive) && (num_contacts > num_max_contacts)) num_contacts = num_max_contacts; \
- contacts.resize(num_contacts); \
- if(!enable_contact) \
- { \
- for(int i = 0; i < num_contacts; ++i) \
- contacts[i] = Contact(obj1, obj2, node.pairs[i].id1, node.pairs[i].id2); \
- } \
- else \
- { \
- for(int i = 0; i < num_contacts; ++i) \
- contacts[i] = Contact(obj1, obj2, node.pairs[i].id1, node.pairs[i].id2, node.pairs[i].contact_point, node.pairs[i].normal, node.pairs[i].penetration_depth); \
- } \
- } \
- delete obj1_tmp; \
- obj1_tmp = NULL; \
- delete obj2_tmp; \
- obj2_tmp = NULL; \
- return num_contacts; \
- } while(0)
-
-
-#define MESHMESHOBBRSS_COMMON_CODE() do{ \
- initialize(node, *obj1, *obj2, obj1->getRotation(), obj1->getTranslation(), obj2->getRotation(), obj2->getTranslation(), num_max_contacts, exhaustive, enable_contact); \
- collide(&node); \
- int num_contacts = node.pairs.size(); \
- if(num_contacts > 0) \
- { \
- if((!exhaustive) && (num_contacts > num_max_contacts)) num_contacts = num_max_contacts; \
- contacts.resize(num_contacts); \
- if(!enable_contact) \
- { \
- for(int i = 0; i < num_contacts; ++i) \
- contacts[i] = Contact(obj1, obj2, node.pairs[i].id1, node.pairs[i].id2); \
- } \
- else \
- { \
- for(int i = 0; i < num_contacts; ++i) \
- { \
- Vec3f normal = MxV(obj1->getRotation(), node.pairs[i].normal); \
- Vec3f contact_point = MxV(obj1->getRotation(), node.pairs[i].contact_point) + obj1->getTranslation(); \
- contacts[i] = Contact(obj1, obj2, node.pairs[i].id1, node.pairs[i].id2, contact_point, normal, node.pairs[i].penetration_depth); \
- } \
- } \
- } \
- return num_contacts; \
- } while(0)
-
-
-
-int BoxBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Box, Box> node;
- const Box* obj1 = (Box*)o1;
- const Box* obj2 = (Box*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int BoxSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Box, Sphere> node;
- const Box* obj1 = (Box*)o1;
- const Sphere* obj2 = (Sphere*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int BoxCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Box, Capsule> node;
- const Box* obj1 = (Box*)o1;
- const Capsule* obj2 = (Capsule*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int BoxConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Box, Cone> node;
- const Box* obj1 = (Box*)o1;
- const Cone* obj2 = (Cone*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int BoxCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Box, Cylinder> node;
- const Box* obj1 = (Box*)o1;
- const Cylinder* obj2 = (Cylinder*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int BoxConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Box, Convex> node;
- const Box* obj1 = (Box*)o1;
- const Convex* obj2 = (Convex*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int BoxPlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Box, Plane> node;
- const Box* obj1 = (Box*)o1;
- const Plane* obj2 = (Plane*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int SphereBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Sphere, Box> node;
- const Sphere* obj1 = (Sphere*)o1;
- const Box* obj2 = (Box*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int SphereSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Sphere, Sphere> node;
- const Sphere* obj1 = (Sphere*)o1;
- const Sphere* obj2 = (Sphere*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int SphereCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Sphere, Capsule> node;
- const Sphere* obj1 = (Sphere*)o1;
- const Capsule* obj2 = (Capsule*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int SphereConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Sphere, Cone> node;
- const Sphere* obj1 = (Sphere*)o1;
- const Cone* obj2 = (Cone*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int SphereCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Sphere, Cylinder> node;
- const Sphere* obj1 = (Sphere*)o1;
- const Cylinder* obj2 = (Cylinder*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int SphereConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Sphere, Convex> node;
- const Sphere* obj1 = (Sphere*)o1;
- const Convex* obj2 = (Convex*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int SpherePlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Sphere, Plane> node;
- const Sphere* obj1 = (Sphere*)o1;
- const Plane* obj2 = (Plane*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CapBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Capsule, Box> node;
- const Capsule* obj1 = (Capsule*)o1;
- const Box* obj2 = (Box*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CapSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Capsule, Sphere> node;
- const Capsule* obj1 = (Capsule*)o1;
- const Sphere* obj2 = (Sphere*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CapCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Capsule, Capsule> node;
- const Capsule* obj1 = (Capsule*)o1;
- const Capsule* obj2 = (Capsule*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CapConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Capsule, Cone> node;
- const Capsule* obj1 = (Capsule*)o1;
- const Cone* obj2 = (Cone*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CapCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Capsule, Cylinder> node;
- const Capsule* obj1 = (Capsule*)o1;
- const Cylinder* obj2 = (Cylinder*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CapConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Capsule, Convex> node;
- const Capsule* obj1 = (Capsule*)o1;
- const Convex* obj2 = (Convex*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CapPlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Capsule, Plane> node;
- const Capsule* obj1 = (Capsule*)o1;
- const Plane* obj2 = (Plane*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConeBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cone, Box> node;
- const Cone* obj1 = (Cone*)o1;
- const Box* obj2 = (Box*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConeSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cone, Sphere> node;
- const Cone* obj1 = (Cone*)o1;
- const Sphere* obj2 = (Sphere*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConeCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cone, Capsule> node;
- const Cone* obj1 = (Cone*)o1;
- const Capsule* obj2 = (Capsule*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConeConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cone, Cone> node;
- const Cone* obj1 = (Cone*)o1;
- const Cone* obj2 = (Cone*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConeCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cone, Cylinder> node;
- const Cone* obj1 = (Cone*)o1;
- const Cylinder* obj2 = (Cylinder*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConeConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cone, Convex> node;
- const Cone* obj1 = (Cone*)o1;
- const Convex* obj2 = (Convex*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConePlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cone, Plane> node;
- const Cone* obj1 = (Cone*)o1;
- const Plane* obj2 = (Plane*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CylinderBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cylinder, Box> node;
- const Cylinder* obj1 = (Cylinder*)o1;
- const Box* obj2 = (Box*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CylinderSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cylinder, Sphere> node;
- const Cylinder* obj1 = (Cylinder*)o1;
- const Sphere* obj2 = (Sphere*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CylinderCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cylinder, Capsule> node;
- const Cylinder* obj1 = (Cylinder*)o1;
- const Capsule* obj2 = (Capsule*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CylinderConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cylinder, Cone> node;
- const Cylinder* obj1 = (Cylinder*)o1;
- const Cone* obj2 = (Cone*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CylinderCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cylinder, Cylinder> node;
- const Cylinder* obj1 = (Cylinder*)o1;
- const Cylinder* obj2 = (Cylinder*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CylinderConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cylinder, Convex> node;
- const Cylinder* obj1 = (Cylinder*)o1;
- const Convex* obj2 = (Convex*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CylinderPlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cylinder, Plane> node;
- const Cylinder* obj1 = (Cylinder*)o1;
- const Plane* obj2 = (Plane*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConvexBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Convex, Box> node;
- const Convex* obj1 = (Convex*)o1;
- const Box* obj2 = (Box*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConvexSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Convex, Sphere> node;
- const Convex* obj1 = (Convex*)o1;
- const Sphere* obj2 = (Sphere*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConvexCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Convex, Capsule> node;
- const Convex* obj1 = (Convex*)o1;
- const Capsule* obj2 = (Capsule*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConvexConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Convex, Cone> node;
- const Convex* obj1 = (Convex*)o1;
- const Cone* obj2 = (Cone*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConvexCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Convex, Cylinder> node;
- const Convex* obj1 = (Convex*)o1;
- const Cylinder* obj2 = (Cylinder*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConvexConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Convex, Convex> node;
- const Convex* obj1 = (Convex*)o1;
- const Convex* obj2 = (Convex*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConvexPlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Convex, Plane> node;
- const Convex* obj1 = (Convex*)o1;
- const Plane* obj2 = (Plane*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int PlaneBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Plane, Box> node;
- const Plane* obj1 = (Plane*)o1;
- const Box* obj2 = (Box*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int PlaneSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Plane, Sphere> node;
- const Plane* obj1 = (Plane*)o1;
- const Sphere* obj2 = (Sphere*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int PlaneCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Plane, Capsule> node;
- const Plane* obj1 = (Plane*)o1;
- const Capsule* obj2 = (Capsule*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int PlaneConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Plane, Cone> node;
- const Plane* obj1 = (Plane*)o1;
- const Cone* obj2 = (Cone*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int PlaneCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Plane, Cylinder> node;
- const Plane* obj1 = (Plane*)o1;
- const Cylinder* obj2 = (Cylinder*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int PlaneConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Plane, Convex> node;
- const Plane* obj1 = (Plane*)o1;
- const Convex* obj2 = (Convex*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int PlanePlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Plane, Plane> node;
- const Plane* obj1 = (Plane*)o1;
- const Plane* obj2 = (Plane*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int AABBBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<AABB, Box> node;
- const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
- BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
- const Box* obj2 = (Box*)o2;
- Box* obj2_tmp = new Box(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int AABBSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<AABB, Sphere> node;
- const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
- BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
- const Sphere* obj2 = (Sphere*)o2;
- Sphere* obj2_tmp = new Sphere(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int AABBCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<AABB, Capsule> node;
- const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
- BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
- const Capsule* obj2 = (Capsule*)o2;
- Capsule* obj2_tmp = new Capsule(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int AABBConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<AABB, Cone> node;
- const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
- BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
- const Cone* obj2 = (Cone*)o2;
- Cone* obj2_tmp = new Cone(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int AABBCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<AABB, Cylinder> node;
- const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
- BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
- const Cylinder* obj2 = (Cylinder*)o2;
- Cylinder* obj2_tmp = new Cylinder(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int AABBConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<AABB, Convex> node;
- const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
- BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
- const Convex* obj2 = (Convex*)o2;
- Convex* obj2_tmp = new Convex(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int AABBPlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<AABB, Plane> node;
- const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
- BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
- const Plane* obj2 = (Plane*)o2;
- Plane* obj2_tmp = new Plane(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int OBBBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNodeOBB<Box> node;
- const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
- const Box* obj2 = (Box*)o2;
- Box* obj2_tmp = new Box(*obj2);
- MESHSHAPEOBBRSS_COMMON_CODE();
-}
-
-int OBBSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNodeOBB<Sphere> node;
- const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
- const Sphere* obj2 = (Sphere*)o2;
- Sphere* obj2_tmp = new Sphere(*obj2);
- MESHSHAPEOBBRSS_COMMON_CODE();
-}
-
-int OBBCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNodeOBB<Capsule> node;
- const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
- const Capsule* obj2 = (Capsule*)o2;
- Capsule* obj2_tmp = new Capsule(*obj2);
- MESHSHAPEOBBRSS_COMMON_CODE();
-}
-
-int OBBConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNodeOBB<Cone> node;
- const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
- const Cone* obj2 = (Cone*)o2;
- Cone* obj2_tmp = new Cone(*obj2);
- MESHSHAPEOBBRSS_COMMON_CODE();
-}
-
-int OBBCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNodeOBB<Cylinder> node;
- const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
- const Cylinder* obj2 = (Cylinder*)o2;
- Cylinder* obj2_tmp = new Cylinder(*obj2);
- MESHSHAPEOBBRSS_COMMON_CODE();
-}
-
-int OBBConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNodeOBB<Convex> node;
- const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
- const Convex* obj2 = (Convex*)o2;
- Convex* obj2_tmp = new Convex(*obj2);
- MESHSHAPEOBBRSS_COMMON_CODE();
-}
-
-int OBBPlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNodeOBB<Plane> node;
- const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
- const Plane* obj2 = (Plane*)o2;
- Plane* obj2_tmp = new Plane(*obj2);
- MESHSHAPEOBBRSS_COMMON_CODE();
-}
-
-int RSSBoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<RSS, Box> node;
- const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
- BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
- const Box* obj2 = (Box*)o2;
- Box* obj2_tmp = new Box(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int RSSSphereCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<RSS, Sphere> node;
- const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
- BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
- const Sphere* obj2 = (Sphere*)o2;
- Sphere* obj2_tmp = new Sphere(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int RSSCapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<RSS, Capsule> node;
- const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
- BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
- const Capsule* obj2 = (Capsule*)o2;
- Capsule* obj2_tmp = new Capsule(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int RSSConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<RSS, Cone> node;
- const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
- BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
- const Cone* obj2 = (Cone*)o2;
- Cone* obj2_tmp = new Cone(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int RSSCylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<RSS, Cylinder> node;
- const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
- BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
- const Cylinder* obj2 = (Cylinder*)o2;
- Cylinder* obj2_tmp = new Cylinder(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int RSSConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<RSS, Convex> node;
- const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
- BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
- const Convex* obj2 = (Convex*)o2;
- Convex* obj2_tmp = new Convex(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int RSSPlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<RSS, Plane> node;
- const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
- BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
- const Plane* obj2 = (Plane*)o2;
- Plane* obj2_tmp = new Plane(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int KDOP16BoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<16> , Box> node;
- const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
- BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
- const Box* obj2 = (Box*)o2;
- Box* obj2_tmp = new Box(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int KDOP16SphereCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<16> , Sphere> node;
- const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
- BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
- const Sphere* obj2 = (Sphere*)o2;
- Sphere* obj2_tmp = new Sphere(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int KDOP16CapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<16> , Capsule> node;
- const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
- BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
- const Capsule* obj2 = (Capsule*)o2;
- Capsule* obj2_tmp = new Capsule(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int KDOP16ConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<16> , Cone> node;
- const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
- BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
- const Cone* obj2 = (Cone*)o2;
- Cone* obj2_tmp = new Cone(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int KDOP16CylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<16> , Cylinder> node;
- const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
- BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
- const Cylinder* obj2 = (Cylinder*)o2;
- Cylinder* obj2_tmp = new Cylinder(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int KDOP16ConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<16> , Convex> node;
- const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
- BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
- const Convex* obj2 = (Convex*)o2;
- Convex* obj2_tmp = new Convex(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int KDOP16PlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<16> , Plane> node;
- const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
- BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
- const Plane* obj2 = (Plane*)o2;
- Plane* obj2_tmp = new Plane(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int KDOP18BoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<18> , Box> node;
- const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
- BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
- const Box* obj2 = (Box*)o2;
- Box* obj2_tmp = new Box(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int KDOP18SphereCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<18> , Sphere> node;
- const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
- BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
- const Sphere* obj2 = (Sphere*)o2;
- Sphere* obj2_tmp = new Sphere(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int KDOP18CapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<18> , Capsule> node;
- const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
- BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
- const Capsule* obj2 = (Capsule*)o2;
- Capsule* obj2_tmp = new Capsule(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int KDOP18ConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<18> , Cone> node;
- const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
- BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
- const Cone* obj2 = (Cone*)o2;
- Cone* obj2_tmp = new Cone(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int KDOP18CylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<18> , Cylinder> node;
- const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
- BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
- const Cylinder* obj2 = (Cylinder*)o2;
- Cylinder* obj2_tmp = new Cylinder(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int KDOP18ConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<18> , Convex> node;
- const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
- BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
- const Convex* obj2 = (Convex*)o2;
- Convex* obj2_tmp = new Convex(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int KDOP18PlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<18> , Plane> node;
- const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
- BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
- const Plane* obj2 = (Plane*)o2;
- Plane* obj2_tmp = new Plane(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int KDOP24BoxCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<24> , Box> node;
- const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
- BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
- const Box* obj2 = (Box*)o2;
- Box* obj2_tmp = new Box(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int KDOP24SphereCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<24> , Sphere> node;
- const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
- BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
- const Sphere* obj2 = (Sphere*)o2;
- Sphere* obj2_tmp = new Sphere(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int KDOP24CapCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<24> , Capsule> node;
- const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
- BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
- const Capsule* obj2 = (Capsule*)o2;
- Capsule* obj2_tmp = new Capsule(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int KDOP24ConeCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<24> , Cone> node;
- const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
- BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
- const Cone* obj2 = (Cone*)o2;
- Cone* obj2_tmp = new Cone(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int KDOP24CylinderCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<24> , Cylinder> node;
- const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
- BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
- const Cylinder* obj2 = (Cylinder*)o2;
- Cylinder* obj2_tmp = new Cylinder(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int KDOP24ConvexCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<24> , Convex> node;
- const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
- BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
- const Convex* obj2 = (Convex*)o2;
- Convex* obj2_tmp = new Convex(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int KDOP24PlaneCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<24> , Plane> node;
- const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
- BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
- const Plane* obj2 = (Plane*)o2;
- Plane* obj2_tmp = new Plane(*obj2);
- MESHSHAPE_COMMON_CODE();
-}
-
-int AABBAABBCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshCollisionTraversalNode<AABB> node;
- const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
- const BVHModel<AABB>* obj2 = (BVHModel<AABB>*)o2;
- BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
- BVHModel<AABB>* obj2_tmp = new BVHModel<AABB>(*obj2);
- MESHMESH_COMMON_CODE();
-}
-
-int OBBOBBCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshCollisionTraversalNodeOBB node;
- const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
- const BVHModel<OBB>* obj2 = (BVHModel<OBB>*)o2;
- MESHMESHOBBRSS_COMMON_CODE();
-}
-
-int RSSRSSCollide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshCollisionTraversalNodeRSS node;
- const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
- const BVHModel<RSS>* obj2 = (BVHModel<RSS>*)o2;
- MESHMESHOBBRSS_COMMON_CODE();
-}
-
-int KDOP16KDOP16Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshCollisionTraversalNode<KDOP<16> > node;
- const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
- const BVHModel<KDOP<16> >* obj2 = (BVHModel<KDOP<16> >*)o2;
- BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
- BVHModel<KDOP<16> >* obj2_tmp = new BVHModel<KDOP<16> >(*obj2);
- MESHMESH_COMMON_CODE();
-}
-
-int KDOP18KDOP18Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshCollisionTraversalNode<KDOP<18> > node;
- const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
- const BVHModel<KDOP<18> >* obj2 = (BVHModel<KDOP<18> >*)o2;
- BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
- BVHModel<KDOP<18> >* obj2_tmp = new BVHModel<KDOP<18> >(*obj2);
- MESHMESH_COMMON_CODE();
-}
-
-int KDOP24KDOP24Collide(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshCollisionTraversalNode<KDOP<24> > node;
- const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
- const BVHModel<KDOP<24> >* obj2 = (BVHModel<KDOP<24> >*)o2;
- BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
- BVHModel<KDOP<24> >* obj2_tmp = new BVHModel<KDOP<24> >(*obj2);
- MESHMESH_COMMON_CODE();
-}
-
-
-CollisionFunctionMatrix::CollisionFunctionMatrix()
-{
- for(int i = 0; i < 14; ++i)
- {
- for(int j = 0; j < 14; ++j)
- collision_matrix[i][j] = NULL;
- }
-
- collision_matrix[GEOM_BOX][GEOM_BOX] = BoxBoxCollide;
- collision_matrix[GEOM_BOX][GEOM_SPHERE] = BoxSphereCollide;
- collision_matrix[GEOM_BOX][GEOM_CAPSULE] = BoxCapCollide;
- collision_matrix[GEOM_BOX][GEOM_CONE] = BoxConeCollide;
- collision_matrix[GEOM_BOX][GEOM_CYLINDER] = BoxCylinderCollide;
- collision_matrix[GEOM_BOX][GEOM_CONVEX] = BoxConvexCollide;
- collision_matrix[GEOM_BOX][GEOM_PLANE] = BoxPlaneCollide;
-
- collision_matrix[GEOM_SPHERE][GEOM_BOX] = SphereBoxCollide;
- collision_matrix[GEOM_SPHERE][GEOM_SPHERE] = SphereSphereCollide;
- collision_matrix[GEOM_SPHERE][GEOM_CAPSULE] = SphereCapCollide;
- collision_matrix[GEOM_SPHERE][GEOM_CONE] = SphereConeCollide;
- collision_matrix[GEOM_SPHERE][GEOM_CYLINDER] = SphereCylinderCollide;
- collision_matrix[GEOM_SPHERE][GEOM_CONVEX] = SphereConvexCollide;
- collision_matrix[GEOM_SPHERE][GEOM_PLANE] = SpherePlaneCollide;
-
- collision_matrix[GEOM_CAPSULE][GEOM_BOX] = CapBoxCollide;
- collision_matrix[GEOM_CAPSULE][GEOM_SPHERE] = CapSphereCollide;
- collision_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = CapCapCollide;
- collision_matrix[GEOM_CAPSULE][GEOM_CONE] = CapConeCollide;
- collision_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = CapCylinderCollide;
- collision_matrix[GEOM_CAPSULE][GEOM_CONVEX] = CapConvexCollide;
- collision_matrix[GEOM_CAPSULE][GEOM_PLANE] = CapPlaneCollide;
-
- collision_matrix[GEOM_CONE][GEOM_BOX] = ConeBoxCollide;
- collision_matrix[GEOM_CONE][GEOM_SPHERE] = ConeSphereCollide;
- collision_matrix[GEOM_CONE][GEOM_CAPSULE] = ConeCapCollide;
- collision_matrix[GEOM_CONE][GEOM_CONE] = ConeConeCollide;
- collision_matrix[GEOM_CONE][GEOM_CYLINDER] = ConeCylinderCollide;
- collision_matrix[GEOM_CONE][GEOM_CONVEX] = ConeConvexCollide;
- collision_matrix[GEOM_CONE][GEOM_PLANE] = ConePlaneCollide;
-
- collision_matrix[GEOM_CYLINDER][GEOM_BOX] = CylinderBoxCollide;
- collision_matrix[GEOM_CYLINDER][GEOM_SPHERE] = CylinderSphereCollide;
- collision_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = CylinderCapCollide;
- collision_matrix[GEOM_CYLINDER][GEOM_CONE] = CylinderConeCollide;
- collision_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = CylinderCylinderCollide;
- collision_matrix[GEOM_CYLINDER][GEOM_CONVEX] = CylinderConvexCollide;
- collision_matrix[GEOM_CYLINDER][GEOM_PLANE] = CylinderPlaneCollide;
-
- collision_matrix[GEOM_CONVEX][GEOM_BOX] = ConvexBoxCollide;
- collision_matrix[GEOM_CONVEX][GEOM_SPHERE] = ConvexSphereCollide;
- collision_matrix[GEOM_CONVEX][GEOM_CAPSULE] = ConvexCapCollide;
- collision_matrix[GEOM_CONVEX][GEOM_CONE] = ConvexConeCollide;
- collision_matrix[GEOM_CONVEX][GEOM_CYLINDER] = ConvexCylinderCollide;
- collision_matrix[GEOM_CONVEX][GEOM_CONVEX] = ConvexConvexCollide;
- collision_matrix[GEOM_CONVEX][GEOM_PLANE] = ConvexPlaneCollide;
-
- collision_matrix[GEOM_PLANE][GEOM_BOX] = PlaneBoxCollide;
- collision_matrix[GEOM_PLANE][GEOM_SPHERE] = PlaneSphereCollide;
- collision_matrix[GEOM_PLANE][GEOM_CAPSULE] = PlaneCapCollide;
- collision_matrix[GEOM_PLANE][GEOM_CONE] = PlaneConeCollide;
- collision_matrix[GEOM_PLANE][GEOM_CYLINDER] = PlaneCylinderCollide;
- collision_matrix[GEOM_PLANE][GEOM_CONVEX] = PlaneConvexCollide;
- collision_matrix[GEOM_PLANE][GEOM_PLANE] = PlanePlaneCollide;
-
- collision_matrix[BV_AABB][GEOM_BOX] = AABBBoxCollide;
- collision_matrix[BV_AABB][GEOM_SPHERE] = AABBSphereCollide;
- collision_matrix[BV_AABB][GEOM_CAPSULE] = AABBCapCollide;
- collision_matrix[BV_AABB][GEOM_CONE] = AABBConeCollide;
- collision_matrix[BV_AABB][GEOM_CYLINDER] = AABBCylinderCollide;
- collision_matrix[BV_AABB][GEOM_CONVEX] = AABBConvexCollide;
- collision_matrix[BV_AABB][GEOM_PLANE] = AABBPlaneCollide;
-
- collision_matrix[BV_OBB][GEOM_BOX] = OBBBoxCollide;
- collision_matrix[BV_OBB][GEOM_SPHERE] = OBBSphereCollide;
- collision_matrix[BV_OBB][GEOM_CAPSULE] = OBBCapCollide;
- collision_matrix[BV_OBB][GEOM_CONE] = OBBConeCollide;
- collision_matrix[BV_OBB][GEOM_CYLINDER] = OBBCylinderCollide;
- collision_matrix[BV_OBB][GEOM_CONVEX] = OBBConvexCollide;
- collision_matrix[BV_OBB][GEOM_PLANE] = OBBPlaneCollide;
-
- collision_matrix[BV_RSS][GEOM_BOX] = RSSBoxCollide;
- collision_matrix[BV_RSS][GEOM_SPHERE] = RSSSphereCollide;
- collision_matrix[BV_RSS][GEOM_CAPSULE] = RSSCapCollide;
- collision_matrix[BV_RSS][GEOM_CONE] = RSSConeCollide;
- collision_matrix[BV_RSS][GEOM_CYLINDER] = RSSCylinderCollide;
- collision_matrix[BV_RSS][GEOM_CONVEX] = RSSConvexCollide;
- collision_matrix[BV_RSS][GEOM_PLANE] = RSSPlaneCollide;
-
- collision_matrix[BV_KDOP16][GEOM_BOX] = KDOP16BoxCollide;
- collision_matrix[BV_KDOP16][GEOM_SPHERE] = KDOP16SphereCollide;
- collision_matrix[BV_KDOP16][GEOM_CAPSULE] = KDOP16CapCollide;
- collision_matrix[BV_KDOP16][GEOM_CONE] = KDOP16ConeCollide;
- collision_matrix[BV_KDOP16][GEOM_CYLINDER] = KDOP16CylinderCollide;
- collision_matrix[BV_KDOP16][GEOM_CONVEX] = KDOP16ConvexCollide;
- collision_matrix[BV_KDOP16][GEOM_PLANE] = KDOP16PlaneCollide;
-
- collision_matrix[BV_KDOP18][GEOM_BOX] = KDOP18BoxCollide;
- collision_matrix[BV_KDOP18][GEOM_SPHERE] = KDOP18SphereCollide;
- collision_matrix[BV_KDOP18][GEOM_CAPSULE] = KDOP18CapCollide;
- collision_matrix[BV_KDOP18][GEOM_CONE] = KDOP18ConeCollide;
- collision_matrix[BV_KDOP18][GEOM_CYLINDER] = KDOP18CylinderCollide;
- collision_matrix[BV_KDOP18][GEOM_CONVEX] = KDOP18ConvexCollide;
- collision_matrix[BV_KDOP18][GEOM_PLANE] = KDOP18PlaneCollide;
-
- collision_matrix[BV_KDOP24][GEOM_BOX] = KDOP24BoxCollide;
- collision_matrix[BV_KDOP24][GEOM_SPHERE] = KDOP24SphereCollide;
- collision_matrix[BV_KDOP24][GEOM_CAPSULE] = KDOP24CapCollide;
- collision_matrix[BV_KDOP24][GEOM_CONE] = KDOP24ConeCollide;
- collision_matrix[BV_KDOP24][GEOM_CYLINDER] = KDOP24CylinderCollide;
- collision_matrix[BV_KDOP24][GEOM_CONVEX] = KDOP24ConvexCollide;
- collision_matrix[BV_KDOP24][GEOM_PLANE] = KDOP24PlaneCollide;
-
- collision_matrix[BV_AABB][BV_AABB] = AABBAABBCollide;
- collision_matrix[BV_OBB][BV_OBB] = OBBOBBCollide;
- collision_matrix[BV_RSS][BV_RSS] = RSSRSSCollide;
- collision_matrix[BV_KDOP16][BV_KDOP16] = KDOP16KDOP16Collide;
- collision_matrix[BV_KDOP18][BV_KDOP18] = KDOP18KDOP18Collide;
- collision_matrix[BV_KDOP24][BV_KDOP24] = KDOP24KDOP24Collide;
-}
-
-}
diff --git a/branches/point_cloud/fcl/src/collision_node.cpp b/branches/point_cloud/fcl/src/collision_node.cpp
deleted file mode 100644
index c3dbe0901020cd1b5848e6a003dfc404e4d48aa1..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/src/collision_node.cpp
+++ /dev/null
@@ -1,113 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#include "fcl/collision_node.h"
-#include "fcl/traversal_recurse.h"
-
-namespace fcl
-{
-
-void collide(CollisionTraversalNodeBase* node, BVHFrontList* front_list)
-{
- if(front_list && front_list->size() > 0)
- {
- propagateBVHFrontListCollisionRecurse(node, front_list);
- }
- else
- {
- collisionRecurse(node, 0, 0, front_list);
- }
-}
-
-
-void selfCollide(CollisionTraversalNodeBase* node, BVHFrontList* front_list)
-{
-
- if(front_list && front_list->size() > 0)
- {
- propagateBVHFrontListCollisionRecurse(node, front_list);
- }
- else
- {
- selfCollisionRecurse(node, 0, front_list);
- }
-}
-
-void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list, int qsize)
-{
- if(qsize <= 2)
- distanceRecurse(node, 0, 0, front_list);
- else
- distanceQueueRecurse(node, 0, 0, front_list, qsize);
-}
-
-void distance(MeshDistanceTraversalNodeRSS* node, BVHFrontList* front_list, int qsize)
-{
- Triangle last_tri1 = node->tri_indices1[node->last_tri_id1];
- Triangle last_tri2 = node->tri_indices2[node->last_tri_id2];
-
- Vec3f last_tri1_points[3];
- Vec3f last_tri2_points[3];
-
- last_tri1_points[0] = node->vertices1[last_tri1[0]];
- last_tri1_points[1] = node->vertices1[last_tri1[1]];
- last_tri1_points[2] = node->vertices1[last_tri1[2]];
-
- last_tri2_points[0] = node->vertices2[last_tri2[0]];
- last_tri2_points[1] = node->vertices2[last_tri2[1]];
- last_tri2_points[2] = node->vertices2[last_tri2[2]];
-
- Vec3f last_tri_P, last_tri_Q;
-
- node->min_distance = TriangleDistance::triDistance(last_tri1_points[0], last_tri1_points[1], last_tri1_points[2],
- last_tri2_points[0], last_tri2_points[1], last_tri2_points[2],
- node->R, node->T, last_tri_P, last_tri_Q);
- node->p1 = last_tri_P;
- node->p2 = MTxV(node->R, last_tri_Q - node->T);
-
-
- if(qsize <= 2)
- distanceRecurse(node, 0, 0, front_list);
- else
- distanceQueueRecurse(node, 0, 0, front_list, qsize);
-
- Vec3f u = node->p2 - node->T;
- node->p2 = MTxV(node->R, u);
-}
-
-
-}
diff --git a/branches/point_cloud/fcl/src/geometric_shapes.cpp b/branches/point_cloud/fcl/src/geometric_shapes.cpp
deleted file mode 100644
index eaae35d734beb694b47edbf795c8d1e067325161..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/src/geometric_shapes.cpp
+++ /dev/null
@@ -1,118 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#include "fcl/geometric_shapes.h"
-#include "fcl/geometric_shapes_utility.h"
-
-namespace fcl
-{
-
-void Convex::fillEdges()
-{
- int* points_in_poly = polygons;
- if(edges) delete [] edges;
-
- int num_edges_alloc = 0;
- for(int i = 0; i < num_planes; ++i)
- {
- num_edges_alloc += *points_in_poly;
- points_in_poly += (*points_in_poly + 1);
- }
-
- edges = new Edge[num_edges_alloc];
-
- points_in_poly = polygons;
- int* index = polygons + 1;
- num_edges = 0;
- Edge e;
- bool isinset;
- for(int i = 0; i < num_planes; ++i)
- {
- for(int j = 0; j < *points_in_poly; ++j)
- {
- e.first = std::min(index[j], index[(j+1)%*points_in_poly]);
- e.second = std::max(index[j], index[(j+1)%*points_in_poly]);
- isinset = false;
- for(int k = 0; k < num_edges; ++k)
- {
- if((edges[k].first == e.first) && (edges[k].second == e.second))
- {
- isinset = true;
- break;
- }
- }
-
- if(!isinset)
- {
- edges[num_edges].first = e.first;
- edges[num_edges].second = e.second;
- ++num_edges;
- }
- }
-
- points_in_poly += (*points_in_poly + 1);
- index = points_in_poly + 1;
- }
-
- if(num_edges < num_edges_alloc)
- {
- Edge* tmp = new Edge[num_edges];
- memcpy(tmp, edges, num_edges * sizeof(Edge));
- delete [] edges;
- edges = tmp;
- }
-}
-
-void Plane::unitNormalTest()
-{
- BVH_REAL l = n.length();
- if(l > 0)
- {
- BVH_REAL inv_l = 1.0 / l;
- n[0] *= inv_l;
- n[1] *= inv_l;
- n[2] *= inv_l;
- d *= inv_l;
- }
- else
- {
- n = Vec3f(1, 0, 0);
- d = 0;
- }
-}
-
-}
diff --git a/branches/point_cloud/fcl/src/geometric_shapes_intersect.cpp b/branches/point_cloud/fcl/src/geometric_shapes_intersect.cpp
deleted file mode 100644
index af96816a89babbe96144d5649120f7ad65631e7f..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/src/geometric_shapes_intersect.cpp
+++ /dev/null
@@ -1,602 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#include "fcl/geometric_shapes_intersect.h"
-
-namespace fcl
-{
-
-struct ccd_obj_t
-{
- ccd_vec3_t pos;
- ccd_quat_t rot, rot_inv;
-};
-
-struct ccd_box_t : public ccd_obj_t
-{
- ccd_real_t dim[3];
-};
-
-struct ccd_cap_t : public ccd_obj_t
-{
- ccd_real_t radius, height;
-};
-
-struct ccd_cyl_t : public ccd_obj_t
-{
- ccd_real_t radius, height;
-};
-
-struct ccd_cone_t : public ccd_obj_t
-{
- ccd_real_t radius, height;
-};
-
-struct ccd_sphere_t : public ccd_obj_t
-{
- ccd_real_t radius;
-};
-
-struct ccd_convex_t : public ccd_obj_t
-{
- const Convex* convex;
-};
-
-struct ccd_triangle_t : public ccd_obj_t
-{
- ccd_vec3_t p[3];
- ccd_vec3_t c;
-};
-
-
-/** Basic shape to ccd shape */
-static void shapeToGJK(const ShapeBase& s, ccd_obj_t* o)
-{
- SimpleQuaternion q;
- q.fromRotation(s.getLocalRotation());
- ccdVec3Set(&o->pos, s.getLocalPosition()[0], s.getLocalPosition()[1], s.getLocalPosition()[2]);
- ccdQuatSet(&o->rot, q.getX(), q.getY(), q.getZ(), q.getW());
- ccdQuatInvert2(&o->rot_inv, &o->rot);
-}
-
-static void boxToGJK(const Box& s, ccd_box_t* box)
-{
- shapeToGJK(s, box);
- box->dim[0] = s.side[0] / 2.0;
- box->dim[1] = s.side[1] / 2.0;
- box->dim[2] = s.side[2] / 2.0;
-}
-
-static void capToGJK(const Capsule& s, ccd_cap_t* cap)
-{
- shapeToGJK(s, cap);
- cap->radius = s.radius;
- cap->height = s.lz / 2;
-}
-
-static void cylToGJK(const Cylinder& s, ccd_cyl_t* cyl)
-{
- shapeToGJK(s, cyl);
- cyl->radius = s.radius;
- cyl->height = s.lz / 2;
-}
-
-static void coneToGJK(const Cone& s, ccd_cone_t* cone)
-{
- shapeToGJK(s, cone);
- cone->radius = s.radius;
- cone->height = s.lz / 2;
-}
-
-static void sphereToGJK(const Sphere& s, ccd_sphere_t* sph)
-{
- shapeToGJK(s, sph);
- sph->radius = s.radius;
-}
-
-static void convexToGJK(const Convex& s, ccd_convex_t* conv)
-{
- shapeToGJK(s, conv);
- conv->convex = &s;
-}
-
-/** Support functions */
-static void supportBox(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
-{
- const ccd_box_t* o = (const ccd_box_t*)obj;
- ccd_vec3_t dir;
- ccdVec3Copy(&dir, dir_);
- ccdQuatRotVec(&dir, &o->rot_inv);
- ccdVec3Set(v, ccdSign(ccdVec3X(&dir)) * o->dim[0],
- ccdSign(ccdVec3Y(&dir)) * o->dim[1],
- ccdSign(ccdVec3Z(&dir)) * o->dim[2]);
- ccdQuatRotVec(v, &o->rot);
- ccdVec3Add(v, &o->pos);
-}
-
-static void supportCap(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
-{
- const ccd_cap_t* o = (const ccd_cap_t*)obj;
- ccd_vec3_t dir, pos1, pos2;
-
- ccdVec3Copy(&dir, dir_);
- ccdQuatRotVec(&dir, &o->rot_inv);
-
- ccdVec3Set(&pos1, CCD_ZERO, CCD_ZERO, o->height);
- ccdVec3Set(&pos2, CCD_ZERO, CCD_ZERO, -o->height);
-
- ccdVec3Copy(v, &dir);
- ccdVec3Scale(v, o->radius);
- ccdVec3Add(&pos1, v);
- ccdVec3Add(&pos2, v);
-
- if(ccdVec3Dot(&dir, &pos1) > ccdVec3Dot(&dir, &pos2))
- ccdVec3Copy(v, &pos1);
- else
- ccdVec3Copy(v, &pos2);
-
- // transform support vertex
- ccdQuatRotVec(v, &o->rot);
- ccdVec3Add(v, &o->pos);
-}
-
-static void supportCyl(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
-{
- const ccd_cyl_t* cyl = (const ccd_cyl_t*)obj;
- ccd_vec3_t dir;
- double zdist, rad;
-
- ccdVec3Copy(&dir, dir_);
- ccdQuatRotVec(&dir, &cyl->rot_inv);
-
- zdist = dir.v[0] * dir.v[0] + dir.v[1] * dir.v[1];
- zdist = sqrt(zdist);
- if(ccdIsZero(zdist))
- ccdVec3Set(v, 0., 0., ccdSign(ccdVec3Z(&dir)) * cyl->height);
- else
- {
- rad = cyl->radius / zdist;
-
- ccdVec3Set(v, rad * ccdVec3X(&dir),
- rad * ccdVec3Y(&dir),
- ccdSign(ccdVec3Z(&dir)) * cyl->height);
- }
-
- // transform support vertex
- ccdQuatRotVec(v, &cyl->rot);
- ccdVec3Add(v, &cyl->pos);
-}
-
-static void supportCone(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
-{
- const ccd_cone_t* cone = (const ccd_cone_t*)obj;
- ccd_vec3_t dir;
-
- ccdVec3Copy(&dir, dir_);
- ccdQuatRotVec(&dir, &cone->rot_inv);
-
- double zdist, len, rad;
- zdist = dir.v[0] * dir.v[0] + dir.v[1] * dir.v[1];
- len = zdist + dir.v[2] * dir.v[2];
- zdist = sqrt(zdist);
- len = sqrt(len);
-
- double sin_a = cone->radius / sqrt(cone->radius * cone->radius + 4 * cone->height * cone->height);
-
- if(dir.v[2] > len * sin_a)
- ccdVec3Set(v, 0., 0., cone->height);
- else if(zdist > 0)
- {
- rad = cone->radius / zdist;
- ccdVec3Set(v, rad * ccdVec3X(&dir), rad * ccdVec3Y(&dir), -cone->height);
- }
- else
- ccdVec3Set(v, 0, 0, -cone->height);
-
- // transform support vertex
- ccdQuatRotVec(v, &cone->rot);
- ccdVec3Add(v, &cone->pos);
-}
-
-static void supportSphere(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
-{
- const ccd_sphere_t* s = (const ccd_sphere_t*)obj;
- ccd_vec3_t dir;
-
- ccdVec3Copy(&dir, dir_);
- ccdQuatRotVec(&dir, &s->rot_inv);
-
- ccdVec3Copy(v, &dir);
- ccdVec3Scale(v, s->radius);
- ccdVec3Scale(v, CCD_ONE / CCD_SQRT(ccdVec3Len2(&dir)));
-
- // transform support vertex
- ccdQuatRotVec(v, &s->rot);
- ccdVec3Add(v, &s->pos);
-}
-
-static void supportConvex(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
-{
- const ccd_convex_t* c = (const ccd_convex_t*)obj;
- ccd_vec3_t dir, p;
- ccd_real_t maxdot, dot;
- int i;
- Vec3f* curp;
- const Vec3f& center = c->convex->center;
-
- ccdVec3Copy(&dir, dir_);
- ccdQuatRotVec(&dir, &c->rot_inv);
-
- maxdot = -CCD_REAL_MAX;
- curp = c->convex->points;
-
- for(i = 0; i < c->convex->num_points; ++i, curp += 1)
- {
- ccdVec3Set(&p, (*curp)[0] - center[0], (*curp)[1] - center[1], (*curp)[2] - center[2]);
- dot = ccdVec3Dot(&dir, &p);
- if(dot > maxdot)
- {
- ccdVec3Set(v, (*curp)[0], (*curp)[1], (*curp)[2]);
- maxdot = dot;
- }
- }
-
- // transform support vertex
- ccdQuatRotVec(v, &c->rot);
- ccdVec3Add(v, &c->pos);
-}
-
-static void supportTriangle(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
-{
- const ccd_triangle_t* tri = (const ccd_triangle_t*)obj;
- ccd_vec3_t dir, p;
- ccd_real_t maxdot, dot;
- int i;
-
- ccdVec3Copy(&dir, dir_);
- ccdQuatRotVec(&dir, &tri->rot_inv);
-
- maxdot = -CCD_REAL_MAX;
-
- for(i = 0; i < 3; ++i)
- {
- ccdVec3Set(&p, tri->p[i].v[0] - tri->c.v[0], tri->p[i].v[1] - tri->c.v[1], tri->p[i].v[2] - tri->c.v[2]);
- dot = ccdVec3Dot(&dir, &p);
- if(dot > maxdot)
- {
- ccdVec3Copy(v, &tri->p[i]);
- maxdot = dot;
- }
- }
-
- // transform support vertex
- ccdQuatRotVec(v, &tri->rot);
- ccdVec3Add(v, &tri->pos);
-}
-
-static void centerShape(const void* obj, ccd_vec3_t* c)
-{
- const ccd_obj_t *o = (const ccd_obj_t*)obj;
- ccdVec3Copy(c, &o->pos);
-}
-
-static void centerConvex(const void* obj, ccd_vec3_t* c)
-{
- const ccd_convex_t *o = (const ccd_convex_t*)obj;
- ccdVec3Set(c, o->convex->center[0], o->convex->center[1], o->convex->center[2]);
- ccdQuatRotVec(c, &o->rot);
- ccdVec3Add(c, &o->pos);
-}
-
-static void centerTriangle(const void* obj, ccd_vec3_t* c)
-{
- const ccd_triangle_t *o = (const ccd_triangle_t*)obj;
- ccdVec3Copy(c, &o->c);
- ccdQuatRotVec(c, &o->rot);
- ccdVec3Add(c, &o->pos);
-}
-
-bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
- void* obj2, ccd_support_fn supp2, ccd_center_fn cen2,
- Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal)
-{
- ccd_t ccd;
- int res;
- ccd_real_t depth;
- ccd_vec3_t dir, pos;
-
-
- CCD_INIT(&ccd);
- ccd.support1 = supp1;
- ccd.support2 = supp2;
- ccd.center1 = cen1;
- ccd.center2 = cen2;
- ccd.max_iterations = 500;
- ccd.mpr_tolerance = 1e-6;
-
- if(!contact_points)
- {
- return ccdMPRIntersect(obj1, obj2, &ccd);
- }
-
-
- res = ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos);
- if(res == 0)
- {
- *contact_points = Vec3f(ccdVec3X(&pos), ccdVec3Y(&pos), ccdVec3Z(&pos));
- *penetration_depth = depth;
- *normal = Vec3f(-ccdVec3X(&dir), -ccdVec3Y(&dir), -ccdVec3Z(&dir));
-
- return true;
- }
-
- return false;
-}
-
-
-GJKSupportFunction GJKInitializer<Cylinder>::getSupportFunction()
-{
- return &supportCyl;
-}
-
-
-GJKCenterFunction GJKInitializer<Cylinder>::getCenterFunction()
-{
- return ¢erShape;
-}
-
-
-void* GJKInitializer<Cylinder>::createGJKObject(const Cylinder& s)
-{
- ccd_cyl_t* o = new ccd_cyl_t;
- cylToGJK(s, o);
- return o;
-}
-
-
-void GJKInitializer<Cylinder>::deleteGJKObject(void* o_)
-{
- ccd_cyl_t* o = (ccd_cyl_t*)o_;
- delete o;
-}
-
-
-GJKSupportFunction GJKInitializer<Sphere>::getSupportFunction()
-{
- return &supportSphere;
-}
-
-
-GJKCenterFunction GJKInitializer<Sphere>::getCenterFunction()
-{
- return ¢erShape;
-}
-
-
-void* GJKInitializer<Sphere>::createGJKObject(const Sphere& s)
-{
- ccd_sphere_t* o = new ccd_sphere_t;
- sphereToGJK(s, o);
- return o;
-}
-
-void GJKInitializer<Sphere>::deleteGJKObject(void* o_)
-{
- ccd_sphere_t* o = (ccd_sphere_t*)o_;
- delete o;
-}
-
-GJKSupportFunction GJKInitializer<Box>::getSupportFunction()
-{
- return &supportBox;
-}
-
-
-GJKCenterFunction GJKInitializer<Box>::getCenterFunction()
-{
- return ¢erShape;
-}
-
-
-void* GJKInitializer<Box>::createGJKObject(const Box& s)
-{
- ccd_box_t* o = new ccd_box_t;
- boxToGJK(s, o);
- return o;
-}
-
-
-void GJKInitializer<Box>::deleteGJKObject(void* o_)
-{
- ccd_box_t* o = (ccd_box_t*)o_;
- delete o;
-}
-
-
-GJKSupportFunction GJKInitializer<Capsule>::getSupportFunction()
-{
- return &supportCap;
-}
-
-
-GJKCenterFunction GJKInitializer<Capsule>::getCenterFunction()
-{
- return ¢erShape;
-}
-
-
-void* GJKInitializer<Capsule>::createGJKObject(const Capsule& s)
-{
- ccd_cap_t* o = new ccd_cap_t;
- capToGJK(s, o);
- return o;
-}
-
-
-void GJKInitializer<Capsule>::deleteGJKObject(void* o_)
-{
- ccd_cap_t* o = (ccd_cap_t*)o_;
- delete o;
-}
-
-
-GJKSupportFunction GJKInitializer<Cone>::getSupportFunction()
-{
- return &supportCone;
-}
-
-
-GJKCenterFunction GJKInitializer<Cone>::getCenterFunction()
-{
- return ¢erShape;
-}
-
-
-void* GJKInitializer<Cone>::createGJKObject(const Cone& s)
-{
- ccd_cone_t* o = new ccd_cone_t;
- coneToGJK(s, o);
- return o;
-}
-
-
-void GJKInitializer<Cone>::deleteGJKObject(void* o_)
-{
- ccd_cone_t* o = (ccd_cone_t*)o_;
- delete o;
-}
-
-
-GJKSupportFunction GJKInitializer<Convex>::getSupportFunction()
-{
- return &supportConvex;
-}
-
-
-GJKCenterFunction GJKInitializer<Convex>::getCenterFunction()
-{
- return ¢erConvex;
-}
-
-
-void* GJKInitializer<Convex>::createGJKObject(const Convex& s)
-{
- ccd_convex_t* o = new ccd_convex_t;
- convexToGJK(s, o);
- return o;
-}
-
-
-void GJKInitializer<Convex>::deleteGJKObject(void* o_)
-{
- ccd_convex_t* o = (ccd_convex_t*)o_;
- delete o;
-}
-
-
-GJKSupportFunction triGetSupportFunction()
-{
- return &supportTriangle;
-}
-
-
-GJKCenterFunction triGetCenterFunction()
-{
- return ¢erTriangle;
-}
-
-
-void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3)
-{
- ccd_triangle_t* o = new ccd_triangle_t;
- Vec3f center((P1[0] + P2[0] + P3[0]) / 3, (P1[1] + P2[1] + P3[1]) / 3, (P1[2] + P2[2] + P3[2]) / 3);
-
- ccdVec3Set(&o->p[0], P1[0], P1[1], P1[2]);
- ccdVec3Set(&o->p[1], P2[0], P2[1], P2[2]);
- ccdVec3Set(&o->p[2], P3[0], P3[1], P3[2]);
- ccdVec3Set(&o->c, center[0], center[1], center[2]);
- ccdVec3Set(&o->pos, 0., 0., 0.);
- ccdQuatSet(&o->rot, 0., 0., 0., 1.);
- ccdQuatInvert2(&o->rot_inv, &o->rot);
-
- return o;
-}
-
-void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Vec3f R[3], Vec3f const& T)
-{
- ccd_triangle_t* o = new ccd_triangle_t;
- Vec3f center((P1[0] + P2[0] + P3[0]) / 3, (P1[1] + P2[1] + P3[1]) / 3, (P1[2] + P2[2] + P3[2]) / 3);
-
- ccdVec3Set(&o->p[0], P1[0], P1[1], P1[2]);
- ccdVec3Set(&o->p[1], P2[0], P2[1], P2[2]);
- ccdVec3Set(&o->p[2], P3[0], P3[1], P3[2]);
- ccdVec3Set(&o->c, center[0], center[1], center[2]);
- SimpleQuaternion q;
- q.fromRotation(R);
- ccdVec3Set(&o->pos, T[0], T[1], T[2]);
- ccdQuatSet(&o->rot, q.getX(), q.getY(), q.getZ(), q.getW());
- ccdQuatInvert2(&o->rot_inv, &o->rot);
-
- return o;
-}
-
-void triDeleteGJKObject(void* o_)
-{
- ccd_triangle_t* o = (ccd_triangle_t*)o_;
- delete o;
-}
-
-void transformGJKObject(void* obj, const Vec3f R[3], const Vec3f& T)
-{
- ccd_obj_t* o = (ccd_obj_t*)obj;
- SimpleQuaternion q_;
- q_.fromRotation(R);
-
- ccd_vec3_t t;
- ccd_quat_t q;
- ccdVec3Set(&t, T[0], T[1], T[2]);
- ccdQuatSet(&q, q_.getX(), q_.getY(), q_.getZ(), q_.getW());
-
- ccd_quat_t tmp;
- ccdQuatMul2(&tmp, &q, &o->rot); // make sure it is correct here!!
- ccdQuatCopy(&o->rot, &tmp);
- ccdQuatRotVec(&o->pos, &q);
- ccdVec3Add(&o->pos, &t);
- ccdQuatInvert2(&o->rot_inv, &o->rot);
-}
-
-}
diff --git a/branches/point_cloud/fcl/src/geometric_shapes_utility.cpp b/branches/point_cloud/fcl/src/geometric_shapes_utility.cpp
deleted file mode 100644
index dd78440bc4cc05ac513459fe550e8bc62b920839..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/src/geometric_shapes_utility.cpp
+++ /dev/null
@@ -1,300 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#include "fcl/geometric_shapes_utility.h"
-#include "fcl/BV_fitter.h"
-
-namespace fcl
-{
-
-template<>
-void computeBV<AABB>(const Box& s, AABB& bv)
-{
- BVH_REAL x_range = 0.5 * (fabs(s.getLocalRotation()[0][0] * s.side[0]) + fabs(s.getLocalRotation()[0][1] * s.side[1]) + fabs(s.getLocalRotation()[0][2] * s.side[2]));
- BVH_REAL y_range = 0.5 * (fabs(s.getLocalRotation()[1][0] * s.side[0]) + fabs(s.getLocalRotation()[1][1] * s.side[1]) + fabs(s.getLocalRotation()[1][2] * s.side[2]));
- BVH_REAL z_range = 0.5 * (fabs(s.getLocalRotation()[2][0] * s.side[0]) + fabs(s.getLocalRotation()[2][1] * s.side[1]) + fabs(s.getLocalRotation()[2][2] * s.side[2]));
-
- bv.max_ = s.getLocalPosition() + Vec3f(x_range, y_range, z_range);
- bv.min_ = s.getLocalPosition() + Vec3f(-x_range, -y_range, -z_range);
-}
-
-template<>
-void computeBV<AABB>(const Sphere& s, AABB& bv)
-{
- bv.max_ = s.getLocalPosition() + Vec3f(s.radius, s.radius, s.radius);
- bv.min_ = s.getLocalPosition() + Vec3f(-s.radius, -s.radius, -s.radius);
-}
-
-template<>
-void computeBV<AABB>(const Capsule& s, AABB& bv)
-{
- BVH_REAL x_range = 0.5 * fabs(s.getLocalRotation()[0][2] * s.lz) + s.radius;
- BVH_REAL y_range = 0.5 * fabs(s.getLocalRotation()[1][2] * s.lz) + s.radius;
- BVH_REAL z_range = 0.5 * fabs(s.getLocalRotation()[2][2] * s.lz) + s.radius;
-
- bv.max_ = s.getLocalPosition() + Vec3f(x_range, y_range, z_range);
- bv.min_ = s.getLocalPosition() + Vec3f(-x_range, -y_range, -z_range);
-}
-
-template<>
-void computeBV<AABB>(const Cone& s, AABB& bv)
-{
- BVH_REAL x_range = fabs(s.getLocalRotation()[0][0] * s.radius) + fabs(s.getLocalRotation()[0][1] * s.radius) + 0.5 * fabs(s.getLocalRotation()[0][2] * s.lz);
- BVH_REAL y_range = fabs(s.getLocalRotation()[1][0] * s.radius) + fabs(s.getLocalRotation()[1][1] * s.radius) + 0.5 * fabs(s.getLocalRotation()[1][2] * s.lz);
- BVH_REAL z_range = fabs(s.getLocalRotation()[2][0] * s.radius) + fabs(s.getLocalRotation()[2][1] * s.radius) + 0.5 * fabs(s.getLocalRotation()[2][2] * s.lz);
-
- bv.max_ = s.getLocalPosition() + Vec3f(x_range, y_range, z_range);
- bv.min_ = s.getLocalPosition() + Vec3f(-x_range, -y_range, -z_range);
-}
-
-template<>
-void computeBV<AABB>(const Cylinder& s, AABB& bv)
-{
- BVH_REAL x_range = fabs(s.getLocalRotation()[0][0] * s.radius) + fabs(s.getLocalRotation()[0][1] * s.radius) + 0.5 * fabs(s.getLocalRotation()[0][2] * s.lz);
- BVH_REAL y_range = fabs(s.getLocalRotation()[1][0] * s.radius) + fabs(s.getLocalRotation()[1][1] * s.radius) + 0.5 * fabs(s.getLocalRotation()[1][2] * s.lz);
- BVH_REAL z_range = fabs(s.getLocalRotation()[2][0] * s.radius) + fabs(s.getLocalRotation()[2][1] * s.radius) + 0.5 * fabs(s.getLocalRotation()[2][2] * s.lz);
-
- bv.max_ = s.getLocalPosition() + Vec3f(x_range, y_range, z_range);
- bv.min_ = s.getLocalPosition() + Vec3f(-x_range, -y_range, -z_range);
-}
-
-template<>
-void computeBV<AABB>(const Convex& s, AABB& bv)
-{
- AABB bv_;
- for(int i = 0; i < s.num_points; ++i)
- {
- Vec3f new_p = MxV(s.getLocalRotation(), s.points[i]) + s.getLocalPosition();
- bv_ += new_p;
- }
-
- bv = bv_;
-}
-
-template<>
-void computeBV<AABB>(const Plane& s, AABB& bv)
-{
- AABB bv_;
- if(s.n[1] == (BVH_REAL)0.0 && s.n[2] == (BVH_REAL)0.0)
- {
- // normal aligned with x axis
- if(s.n[0] < 0) bv_.min_[0] = -s.d;
- else if(s.n[0] > 0) bv_.max_[0] = s.d;
- }
- else if(s.n[0] == (BVH_REAL)0.0 && s.n[2] == (BVH_REAL)0.0)
- {
- // normal aligned with y axis
- if(s.n[1] < 0) bv_.min_[1] = -s.d;
- else if(s.n[1] > 0) bv_.max_[1] = s.d;
- }
- else if(s.n[0] == (BVH_REAL)0.0 && s.n[1] == (BVH_REAL)0.0)
- {
- // normal aligned with z axis
- if(s.n[2] < 0) bv_.min_[2] = -s.d;
- else if(s.n[2] > 0) bv_.max_[2] = s.d;
- }
-
- bv = bv_;
-}
-
-
-template<>
-void computeBV<OBB>(const Box& s, OBB& bv)
-{
- bv.To = s.getLocalPosition();
- bv.axis[0] = Vec3f(s.getLocalRotation()[0][0], s.getLocalRotation()[1][0], s.getLocalRotation()[2][0]);
- bv.axis[1] = Vec3f(s.getLocalRotation()[0][1], s.getLocalRotation()[1][1], s.getLocalRotation()[2][1]);
- bv.axis[2] = Vec3f(s.getLocalRotation()[0][2], s.getLocalRotation()[1][2], s.getLocalRotation()[2][2]);
- bv.extent = s.side * (BVH_REAL)0.5;
-}
-
-template<>
-void computeBV<OBB>(const Sphere& s, OBB& bv)
-{
- bv.To = s.getLocalPosition();
- bv.axis[0] = Vec3f(1, 0, 0);
- bv.axis[1] = Vec3f(0, 1, 0);
- bv.axis[2] = Vec3f(0, 0, 1);
- bv.extent = Vec3f(s.radius, s.radius, s.radius);
-}
-
-template<>
-void computeBV<OBB>(const Capsule& s, OBB& bv)
-{
- bv.To = s.getLocalPosition();
- bv.axis[0] = Vec3f(s.getLocalRotation()[0][0], s.getLocalRotation()[1][0], s.getLocalRotation()[2][0]);
- bv.axis[1] = Vec3f(s.getLocalRotation()[0][1], s.getLocalRotation()[1][1], s.getLocalRotation()[2][1]);
- bv.axis[2] = Vec3f(s.getLocalRotation()[0][2], s.getLocalRotation()[1][2], s.getLocalRotation()[2][2]);
- bv.extent = Vec3f(s.radius, s.radius, s.lz / 2 + s.radius);
-}
-
-template<>
-void computeBV<OBB>(const Cone& s, OBB& bv)
-{
- bv.To = s.getLocalPosition();
- bv.axis[0] = Vec3f(s.getLocalRotation()[0][0], s.getLocalRotation()[1][0], s.getLocalRotation()[2][0]);
- bv.axis[1] = Vec3f(s.getLocalRotation()[0][1], s.getLocalRotation()[1][1], s.getLocalRotation()[2][1]);
- bv.axis[2] = Vec3f(s.getLocalRotation()[0][2], s.getLocalRotation()[1][2], s.getLocalRotation()[2][2]);
- bv.extent = Vec3f(s.radius, s.radius, s.lz / 2);
-}
-
-template<>
-void computeBV<OBB>(const Cylinder& s, OBB& bv)
-{
- bv.To = s.getLocalPosition();
- bv.axis[0] = Vec3f(s.getLocalRotation()[0][0], s.getLocalRotation()[1][0], s.getLocalRotation()[2][0]);
- bv.axis[1] = Vec3f(s.getLocalRotation()[0][1], s.getLocalRotation()[1][1], s.getLocalRotation()[2][1]);
- bv.axis[2] = Vec3f(s.getLocalRotation()[0][2], s.getLocalRotation()[1][2], s.getLocalRotation()[2][2]);
- bv.extent = Vec3f(s.radius, s.radius, s.lz / 2);
-}
-
-template<>
-void computeBV<OBB>(const Convex& s, OBB& bv)
-{
- fit(s.points, s.num_points, bv);
-
- Vec3f axis[3];
- axis[0] = MxV(s.getLocalRotation(), bv.axis[0]);
- axis[1] = MxV(s.getLocalRotation(), bv.axis[1]);
- axis[2] = MxV(s.getLocalRotation(), bv.axis[2]);
-
- bv.axis[0] = axis[0];
- bv.axis[1] = axis[1];
- bv.axis[2] = axis[2];
-
- bv.To = MxV(s.getLocalRotation(), bv.To) + s.getLocalPosition();
-
-}
-
-template<>
-void computeBV<OBB>(const Plane& s, OBB& bv)
-{
- // generate other two axes orthonormal to plane normal
- const Vec3f& w = s.n;
- Vec3f u, v;
- float inv_length;
- if(fabs(w[0]) >= fabs(w[1]))
- {
- inv_length = 1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
- u[0] = -w[2] * inv_length;
- u[1] = 0;
- u[2] = w[0] * inv_length;
- v[0] = w[1] * u[2];
- v[1] = w[2] * u[0] - w[0] * u[2];
- v[2] = -w[1] * u[0];
- }
- else
- {
- inv_length = 1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
- u[0] = 0;
- u[1] = w[2] * inv_length;
- u[2] = -w[1] * inv_length;
- v[0] = w[1] * u[2] - w[2] * u[1];
- v[1] = -w[0] * u[2];
- v[2] = w[0] * u[1];
- }
-
- bv.axis[0] = w;
- bv.axis[1] = u;
- bv.axis[2] = v;
-
- bv.extent = Vec3f(0, std::numeric_limits<BVH_REAL>::max(), std::numeric_limits<BVH_REAL>::max());
-
- Vec3f p = s.n * s.d;
- bv.To = MxV(s.getLocalRotation(), p) + s.getLocalPosition();
-}
-
-void Box::computeAABB()
-{
- computeBV<AABB>(*this, aabb);
- aabb_cache = aabb;
- aabb_center = aabb.center();
- aabb_radius = (aabb.min_ - aabb_center).length();
-}
-
-void Sphere::computeAABB()
-{
- computeBV<AABB>(*this, aabb);
- aabb_cache = aabb;
- aabb_center = aabb.center();
- aabb_radius = radius;
-}
-
-void Capsule::computeAABB()
-{
- computeBV<AABB>(*this, aabb);
- aabb_cache = aabb;
- aabb_center = aabb.center();
- aabb_radius = (aabb.min_ - aabb_center).length();
-}
-
-void Cone::computeAABB()
-{
- computeBV<AABB>(*this, aabb);
- aabb_cache = aabb;
- aabb_center = aabb.center();
- aabb_radius = (aabb.min_ - aabb_center).length();
-}
-
-void Cylinder::computeAABB()
-{
- computeBV<AABB>(*this, aabb);
- aabb_cache = aabb;
- aabb_center = aabb.center();
- aabb_radius = (aabb.min_ - aabb_center).length();
-}
-
-void Convex::computeAABB()
-{
- computeBV<AABB>(*this, aabb);
- aabb_cache = aabb;
- aabb_center = aabb.center();
- aabb_radius = (aabb.min_ - aabb_center).length();
-}
-
-void Plane::computeAABB()
-{
- computeBV<AABB>(*this, aabb);
- aabb_cache = aabb;
- aabb_center = aabb.center();
- aabb_radius = (aabb.min_ - aabb_center).length();
-}
-
-
-
-
-}
diff --git a/branches/point_cloud/fcl/src/intersect.cpp b/branches/point_cloud/fcl/src/intersect.cpp
deleted file mode 100644
index 2dcde43d3de95f365fdd2a40ad85fdac641752e8..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/src/intersect.cpp
+++ /dev/null
@@ -1,2198 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-#include "fcl/intersect.h"
-#include <iostream>
-#include <limits>
-#include <vector>
-
-namespace fcl
-{
-const BVH_REAL PolySolver::NEAR_ZERO_THRESHOLD = 1e-9;
-
-
-bool PolySolver::isZero(BVH_REAL v)
-{
- return (v < NEAR_ZERO_THRESHOLD) && (v > -NEAR_ZERO_THRESHOLD);
-}
-
-bool PolySolver::cbrt(BVH_REAL v)
-{
- return powf(v, 1.0 / 3.0);
-}
-
-int PolySolver::solveLinear(BVH_REAL c[2], BVH_REAL s[1])
-{
- if(isZero(c[1]))
- return 0;
- s[0] = - c[0] / c[1];
- return 1;
-}
-
-int PolySolver::solveQuadric(BVH_REAL c[3], BVH_REAL s[2])
-{
- BVH_REAL p, q, D;
-
- // make sure we have a d2 equation
-
- if(isZero(c[2]))
- return solveLinear(c, s);
-
- // normal for: x^2 + px + q
- p = c[1] / (2.0 * c[2]);
- q = c[0] / c[2];
- D = p * p - q;
-
- if(isZero(D))
- {
- // one BVH_REAL root
- s[0] = s[1] = -p;
- return 1;
- }
-
- if(D < 0.0)
- // no real root
- return 0;
- else
- {
- // two real roots
- BVH_REAL sqrt_D = sqrt(D);
- s[0] = sqrt_D - p;
- s[1] = -sqrt_D - p;
- return 2;
- }
-}
-
-int PolySolver::solveCubic(BVH_REAL c[4], BVH_REAL s[3])
-{
- int i, num;
- BVH_REAL sub, A, B, C, sq_A, p, q, cb_p, D;
- const BVH_REAL ONE_OVER_THREE = 1 / 3.0;
- const BVH_REAL PI = 3.14159265358979323846;
-
- // make sure we have a d2 equation
- if(isZero(c[3]))
- return solveQuadric(c, s);
-
- // normalize the equation:x ^ 3 + Ax ^ 2 + Bx + C = 0
- A = c[2] / c[3];
- B = c[1] / c[3];
- C = c[0] / c[3];
-
- // substitute x = y - A / 3 to eliminate the quadratic term: x^3 + px + q = 0
- sq_A = A * A;
- p = (-ONE_OVER_THREE * sq_A + B) * ONE_OVER_THREE;
- q = 0.5 * (2.0 / 27.0 * A * sq_A - ONE_OVER_THREE * A * B + C);
-
- // use Cardano's formula
- cb_p = p * p * p;
- D = q * q + cb_p;
-
- if(isZero(D))
- {
- if(isZero(q))
- {
- // one triple solution
- s[0] = 0.0;
- num = 1;
- }
- else
- {
- // one single and one BVH_REAL solution
- BVH_REAL u = cbrt(-q);
- s[0] = 2.0 * u;
- s[1] = -u;
- num = 2;
- }
- }
- else
- {
- if(D < 0.0)
- {
- // three real solutions
- BVH_REAL phi = ONE_OVER_THREE * acos(-q / sqrt(-cb_p));
- BVH_REAL t = 2.0 * sqrt(-p);
- s[0] = t * cos(phi);
- s[1] = -t * cos(phi + PI / 3.0);
- s[2] = -t * cos(phi - PI / 3.0);
- num = 3;
- }
- else
- {
- // one real solution
- BVH_REAL sqrt_D = sqrt(D);
- BVH_REAL u = cbrt(sqrt_D + fabs(q));
- if(q > 0.0)
- s[0] = - u + p / u ;
- else
- s[0] = u - p / u;
- num = 1;
- }
- }
-
- // re-substitute
- sub = ONE_OVER_THREE * A;
- for(i = 0; i < num; i++)
- s[i] -= sub;
- return num;
-}
-
-
-#if USE_SVMLIGHT
-CloudClassifierParam::CloudClassifierParam()
-{
- strcpy(learn_parm.predfile, "");
- strcpy(learn_parm.alphafile, "");
- learn_parm.biased_hyperplane = 1;
- learn_parm.sharedslack = 0;
- learn_parm.remove_inconsistent = 0;
- learn_parm.skip_final_opt_check = 0;
- learn_parm.svm_maxqpsize = 10;
- learn_parm.svm_newvarsinqp = 0;
- learn_parm.svm_iter_to_shrink = -9999;
- learn_parm.maxiter = 100000;
- learn_parm.kernel_cache_size = 40;
- learn_parm.svm_c = 10;
- learn_parm.eps = 0.1;
- learn_parm.transduction_posratio = -1.0;
- learn_parm.svm_costratio = 1.0;
- learn_parm.svm_costratio_unlab = 1.0;
- learn_parm.svm_unlabbound = 1E-5;
- learn_parm.epsilon_crit = 0.001;
- learn_parm.epsilon_a = 1E-15;
- learn_parm.compute_loo = 0;
- learn_parm.rho = 1.0;
- learn_parm.xa_depth = 0;
- learn_parm.type = CLASSIFICATION;
- kernel_parm.kernel_type = 2;
- kernel_parm.poly_degree = 3;
- kernel_parm.rbf_gamma = 1.0;
- kernel_parm.coef_lin = 1;
- kernel_parm.coef_const = 1;
- strcpy(kernel_parm.custom, "empty");
-
- bool res = true;
-
- if(learn_parm.svm_iter_to_shrink == -9999)
- {
- if(kernel_parm.kernel_type == LINEAR)
- learn_parm.svm_iter_to_shrink = 2;
- else
- learn_parm.svm_iter_to_shrink = 100;
- }
-
- if((learn_parm.skip_final_opt_check)
- && (kernel_parm.kernel_type == LINEAR))
- {
- std::cout << "It does not make sense to skip the final optimality check for linear kernels." << std::endl;
- learn_parm.skip_final_opt_check = 0;
- }
- if((learn_parm.skip_final_opt_check)
- && (learn_parm.remove_inconsistent))
- {
- std::cout << "It is necessary to do the final optimality check when removing inconsistent examples." << std::endl;
- res = false;
- }
- if((learn_parm.svm_maxqpsize < 2))
- {
- std::cout << "Maximum size of QP-subproblems not in valid range: " << learn_parm.svm_maxqpsize << "[2..]" << std::endl;
- res = false;
- }
- if((learn_parm.svm_maxqpsize < learn_parm.svm_newvarsinqp))
- {
- std::cout << "Maximum size of QP-subproblems " << learn_parm.svm_maxqpsize << " must be larger than the number of new variables [" << learn_parm.svm_newvarsinqp << "] entering the working set in each iteration." << std::endl;
- res = false;
- }
- if(learn_parm.svm_iter_to_shrink < 1)
- {
- std::cout << "Maximum number of iterations for shrinking not in valid range: " << learn_parm.svm_iter_to_shrink << " [1,..]." << std::endl;
- res = false;
- }
- if(learn_parm.svm_c < 0)
- {
- std::cout << "The C parameter must be greater than zero!" << std::endl;
- res = false;
- }
- if(learn_parm.transduction_posratio > 1)
- {
- std::cout << "The fraction of unlabeled examples to classify as positives must be less than 1.0." << std::endl;
- res = false;
- }
- if(learn_parm.svm_costratio <= 0)
- {
- std::cout << "The COSTRATIO parameter must be greater than zero!" << std::endl;
- res = false;
- }
- if(learn_parm.epsilon_crit <= 0)
- {
- std::cout << "The epsilon parameter must be greater than zero!" << std::endl;
- res = false;
- }
- if(learn_parm.rho < 0)
- {
- std::cout << "The parameter rho for xi/alpha-estimates and leave-one-out pruning must be greater than zero (typically 1.0 or 2.0, see T. Joachims, Estimating the Generalization Performance of an SVM Efficiently, ICML, 2000.)" << std::endl;
- res = false;
- }
- if((learn_parm.xa_depth < 0) || (learn_parm.xa_depth > 100))
- {
- std::cout << "The parameter depth for ext. xi/alpha-estimates must be in [0..100] (zero for switching to the conventional xa/estimates described in T. Joachims, Estimating the Generalization Performance of an SVM Efficiently, ICML, 2000.)" << std::endl;
- res = false;
- }
-
- if(!res)
- {
- std::cout << "Solver initialization fails" << std::endl;
- }
-}
-
-#endif
-
-const BVH_REAL Intersect::EPSILON = 1e-5;
-const BVH_REAL Intersect::NEAR_ZERO_THRESHOLD = 1e-7;
-const BVH_REAL Intersect::CCD_RESOLUTION = 1e-7;
-
-
-bool Intersect::isZero(BVH_REAL v)
-{
- return (v < NEAR_ZERO_THRESHOLD) && (v > -NEAR_ZERO_THRESHOLD);
-}
-
-/** \brief
- * data: only used for EE, return the intersect point
- */
-bool Intersect::solveCubicWithIntervalNewton(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- BVH_REAL& l, BVH_REAL& r, bool bVF, BVH_REAL coeffs[], Vec3f* data)
-{
- BVH_REAL v2[2]= {l*l,r*r};
- BVH_REAL v[2]= {l,r};
- BVH_REAL r_backup;
-
- unsigned char min3, min2, min1, max3, max2, max1;
-
- min3= *((unsigned char*)&coeffs[3]+7)>>7; max3=min3^1;
- min2= *((unsigned char*)&coeffs[2]+7)>>7; max2=min2^1;
- min1= *((unsigned char*)&coeffs[1]+7)>>7; max1=min1^1;
-
- // bound the cubic
-
- BVH_REAL minor = coeffs[3]*v2[min3]*v[min3]+coeffs[2]*v2[min2]+coeffs[1]*v[min1]+coeffs[0];
- BVH_REAL major = coeffs[3]*v2[max3]*v[max3]+coeffs[2]*v2[max2]+coeffs[1]*v[max1]+coeffs[0];
-
- if(major<0) return false;
- if(minor>0) return false;
-
- // starting here, the bounds have opposite values
- BVH_REAL m = 0.5 * (r + l);
-
- // bound the derivative
- BVH_REAL dminor = 3.0*coeffs[3]*v2[min3]+2.0*coeffs[2]*v[min2]+coeffs[1];
- BVH_REAL dmajor = 3.0*coeffs[3]*v2[max3]+2.0*coeffs[2]*v[max2]+coeffs[1];
-
- if((dminor > 0)||(dmajor < 0)) // we can use Newton
- {
- BVH_REAL m2 = m*m;
- BVH_REAL fm = coeffs[3]*m2*m+coeffs[2]*m2+coeffs[1]*m+coeffs[0];
- BVH_REAL nl = m;
- BVH_REAL nu = m;
- if(fm>0)
- {
- nl-=(fm/dminor);
- nu-=(fm/dmajor);
- }
- else
- {
- nu-=(fm/dminor);
- nl-=(fm/dmajor);
- }
-
- //intersect with [l,r]
-
- if(nl>r) return false;
- if(nu<l) return false;
- if(nl>l)
- {
- if(nu<r) { l=nl; r=nu; m=0.5*(l+r); }
- else { l=nl; m=0.5*(l+r); }
- }
- else
- {
- if(nu<r) { r=nu; m=0.5*(l+r); }
- }
- }
-
- // sufficient temporal resolution, check root validity
- if((r-l)< CCD_RESOLUTION)
- {
- if(bVF)
- return checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r);
- else
- return checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r, data);
- }
-
- r_backup = r, r = m;
- if(solveCubicWithIntervalNewton(a0, b0, c0, d0, va, vb, vc, vd, l, r, bVF, coeffs, data))
- return true;
-
- l = m, r = r_backup;
- return solveCubicWithIntervalNewton(a0, b0, c0, d0, va, vb, vc, vd, l, r, bVF, coeffs, data);
-}
-
-
-
-bool Intersect::insideTriangle(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f&p)
-{
- Vec3f ab = b - a;
- Vec3f ac = c - a;
- Vec3f n = ab.cross(ac);
-
- Vec3f pa = a - p;
- Vec3f pb = b - p;
- Vec3f pc = c - p;
-
- if((pb.cross(pc)).dot(n) < -EPSILON) return false;
- if((pc.cross(pa)).dot(n) < -EPSILON) return false;
- if((pa.cross(pb)).dot(n) < -EPSILON) return false;
-
- return true;
-}
-
-bool Intersect::insideLineSegment(const Vec3f& a, const Vec3f& b, const Vec3f& p)
-{
- return (p - a).dot(p - b) <= 0;
-}
-
-/* \brief Calculate the line segment papb that is the shortest route between
- two lines p1p2 and p3p4. Calculate also the values of mua and mub where
- pa = p1 + mua (p2 - p1)
- pb = p3 + mub (p4 - p3)
- Return FALSE if no solution exists.
-*/
-bool Intersect::linelineIntersect(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const Vec3f& p4,
- Vec3f* pa, Vec3f* pb, BVH_REAL* mua, BVH_REAL* mub)
-{
- Vec3f p31 = p1 - p3;
- Vec3f p34 = p4 - p3;
- if(fabs(p34[0]) < EPSILON && fabs(p34[1]) < EPSILON && fabs(p34[2]) < EPSILON)
- return false;
-
- Vec3f p12 = p2 - p1;
- if(fabs(p12[0]) < EPSILON && fabs(p12[1]) < EPSILON && fabs(p12[2]) < EPSILON)
- return false;
-
- BVH_REAL d3134 = p31.dot(p34);
- BVH_REAL d3412 = p34.dot(p12);
- BVH_REAL d3112 = p31.dot(p12);
- BVH_REAL d3434 = p34.dot(p34);
- BVH_REAL d1212 = p12.dot(p12);
-
- BVH_REAL denom = d1212 * d3434 - d3412 * d3412;
- if(fabs(denom) < EPSILON)
- return false;
- BVH_REAL numer = d3134 * d3412 - d3112 * d3434;
-
- *mua = numer / denom;
- if(*mua < 0 || *mua > 1)
- return false;
-
- *mub = (d3134 + d3412 * (*mua)) / d3434;
- if(*mub < 0 || *mub > 1)
- return false;
-
- *pa = p1 + p12 * (*mua);
- *pb = p3 + p34 * (*mub);
- return true;
-}
-
-bool Intersect::checkRootValidity_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
- BVH_REAL t)
-{
- return insideTriangle(a0 + va * t, b0 + vb * t, c0 + vc * t, p0 + vp * t);
-}
-
-bool Intersect::checkRootValidity_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- BVH_REAL t, Vec3f* q_i)
-{
- Vec3f a = a0 + va * t;
- Vec3f b = b0 + vb * t;
- Vec3f c = c0 + vc * t;
- Vec3f d = d0 + vd * t;
- Vec3f p1, p2;
- BVH_REAL t_ab, t_cd;
- if(linelineIntersect(a, b, c, d, &p1, &p2, &t_ab, &t_cd))
- {
- if(q_i) *q_i = p1;
- return true;
- }
-
- return false;
-}
-
-bool Intersect::checkRootValidity_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
- BVH_REAL t)
-{
- return insideLineSegment(a0 + va * t, b0 + vb * t, p0 + vp * t);
-}
-
-bool Intersect::solveSquare(BVH_REAL a, BVH_REAL b, BVH_REAL c,
- const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- bool bVF,
- BVH_REAL* ret)
-{
- BVH_REAL discriminant = b * b - 4 * a * c;
- if(discriminant < 0)
- return false;
-
- BVH_REAL sqrt_dis = sqrt(discriminant);
- BVH_REAL r1 = (-b + sqrt_dis) / (2 * a);
- bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r1) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r1)) : false;
-
- BVH_REAL r2 = (-b - sqrt_dis) / (2 * a);
- bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r2) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r2)) : false;
-
- if(v1 && v2)
- {
- *ret = (r1 > r2) ? r2 : r1;
- return true;
- }
- if(v1)
- {
- *ret = r1;
- return true;
- }
- if(v2)
- {
- *ret = r2;
- return true;
- }
-
- return false;
-}
-
-bool Intersect::solveSquare(BVH_REAL a, BVH_REAL b, BVH_REAL c,
- const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vp)
-{
- if(isZero(a))
- {
- BVH_REAL t = -c/b;
- return (t >= 0 && t <= 1) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, t) : false;
- }
-
- BVH_REAL discriminant = b*b-4*a*c;
- if(discriminant < 0)
- return false;
-
- BVH_REAL sqrt_dis = sqrt(discriminant);
-
- BVH_REAL r1 = (-b+sqrt_dis) / (2 * a);
- bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r1) : false;
- if(v1) return true;
-
- BVH_REAL r2 = (-b-sqrt_dis) / (2 * a);
- bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r2) : false;
- return v2;
-}
-
-
-
-/** \brief Compute the cubic coefficients for VF case
- * See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1.
- */
-void Intersect::computeCubicCoeff_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
- BVH_REAL* a, BVH_REAL* b, BVH_REAL* c, BVH_REAL* d)
-{
- Vec3f vavb = vb - va;
- Vec3f vavc = vc - va;
- Vec3f vavp = vp - va;
- Vec3f a0b0 = b0 - a0;
- Vec3f a0c0 = c0 - a0;
- Vec3f a0p0 = p0 - a0;
-
- Vec3f vavb_cross_vavc = vavb.cross(vavc);
- Vec3f vavb_cross_a0c0 = vavb.cross(a0c0);
- Vec3f a0b0_cross_vavc = a0b0.cross(vavc);
- Vec3f a0b0_cross_a0c0 = a0b0.cross(a0c0);
-
- *a = vavp.dot(vavb_cross_vavc);
- *b = a0p0.dot(vavb_cross_vavc) + vavp.dot(vavb_cross_a0c0 + a0b0_cross_vavc);
- *c = vavp.dot(a0b0_cross_a0c0) + a0p0.dot(vavb_cross_a0c0 + a0b0_cross_vavc);
- *d = a0p0.dot(a0b0_cross_a0c0);
-}
-
-void Intersect::computeCubicCoeff_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- BVH_REAL* a, BVH_REAL* b, BVH_REAL* c, BVH_REAL* d)
-{
- Vec3f vavb = vb - va;
- Vec3f vcvd = vd - vc;
- Vec3f vavc = vc - va;
- Vec3f c0d0 = d0 - c0;
- Vec3f a0b0 = b0 - a0;
- Vec3f a0c0 = c0 - a0;
- Vec3f vavb_cross_vcvd = vavb.cross(vcvd);
- Vec3f vavb_cross_c0d0 = vavb.cross(c0d0);
- Vec3f a0b0_cross_vcvd = a0b0.cross(vcvd);
- Vec3f a0b0_cross_c0d0 = a0b0.cross(c0d0);
-
- *a = vavc.dot(vavb_cross_vcvd);
- *b = a0c0.dot(vavb_cross_vcvd) + vavc.dot(vavb_cross_c0d0 + a0b0_cross_vcvd);
- *c = vavc.dot(a0b0_cross_c0d0) + a0c0.dot(vavb_cross_c0d0 + a0b0_cross_vcvd);
- *d = a0c0.dot(a0b0_cross_c0d0);
-}
-
-void Intersect::computeCubicCoeff_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
- const Vec3f& L,
- BVH_REAL* a, BVH_REAL* b, BVH_REAL* c)
-{
- Vec3f vbva = va - vb;
- Vec3f vbvp = vp - vb;
- Vec3f b0a0 = a0 - b0;
- Vec3f b0p0 = p0 - b0;
-
- Vec3f L_cross_vbvp = L.cross(vbvp);
- Vec3f L_cross_b0p0 = L.cross(b0p0);
-
- *a = L_cross_vbvp.dot(vbva);
- *b = L_cross_vbvp.dot(b0a0) + L_cross_b0p0.dot(vbva);
- *c = L_cross_b0p0.dot(b0a0);
-}
-
-
-bool Intersect::intersect_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
- BVH_REAL* collision_time, Vec3f* p_i, bool useNewton)
-{
- *collision_time = 2.0;
-
- Vec3f vp, va, vb, vc;
- vp = p1 - p0;
- va = a1 - a0;
- vb = b1 - b0;
- vc = c1 - c0;
-
- BVH_REAL a, b, c, d;
- computeCubicCoeff_VF(a0, b0, c0, p0, va, vb, vc, vp, &a, &b, &c, &d);
-
- if(isZero(a) && isZero(b) && isZero(c) && isZero(d))
- {
- return false;
- }
-
- /*
- if(isZero(a))
- {
- return solveSquare(b, c, d, a0, b0, c0, p0, va, vb, vc, vp, true, collision_time);
- }
- */
-
-
- BVH_REAL coeffs[4];
- coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d;
-
- if(useNewton)
- {
- BVH_REAL l = 0;
- BVH_REAL r = 1;
-
- if(solveCubicWithIntervalNewton(a0, b0, c0, p0, va, vb, vc, vp, l, r, true, coeffs))
- {
- *collision_time = 0.5 * (l + r);
- }
- }
- else
- {
- BVH_REAL roots[3];
- int num = PolySolver::solveCubic(coeffs, roots);
- for(int i = 0; i < num; ++i)
- {
- BVH_REAL r = roots[i];
- if(r < 0 || r > 1) continue;
- if(checkRootValidity_VF(a0, b0, c0, p0, va, vb, vc, vp, r))
- {
- *collision_time = r;
- break;
- }
- }
- }
-
- if(*collision_time > 1)
- {
- return false;
- }
-
- *p_i = vp * (*collision_time) + p0;
- return true;
-}
-
-bool Intersect::intersect_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
- BVH_REAL* collision_time, Vec3f* p_i, bool useNewton)
-{
- *collision_time = 2.0;
-
- Vec3f va, vb, vc, vd;
- va = a1 - a0;
- vb = b1 - b0;
- vc = c1 - c0;
- vd = d1 - d0;
-
- BVH_REAL a, b, c, d;
- computeCubicCoeff_EE(a0, b0, c0, d0, va, vb, vc, vd, &a, &b, &c, &d);
-
- if(isZero(a) && isZero(b) && isZero(c) && isZero(d))
- {
- return false;
- }
- /*
- if(isZero(a))
- {
- return solveSquare(b, c, d, a0, b0, c0, d0, va, vb, vc, vd, collision_time, false);
- }
- */
-
- BVH_REAL coeffs[4];
- coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d;
-
- if(useNewton)
- {
- BVH_REAL l = 0;
- BVH_REAL r = 1;
-
- if(solveCubicWithIntervalNewton(a0, b0, c0, d0, va, vb, vc, vd, l, r, false, coeffs, p_i))
- {
- *collision_time = (l + r) * 0.5;
- }
- }
- else
- {
- BVH_REAL roots[3];
- int num = PolySolver::solveCubic(coeffs, roots);
- for(int i = 0; i < num; ++i)
- {
- BVH_REAL r = roots[i];
- if(r < 0 || r > 1) continue;
-
- if(checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r, p_i))
- {
- *collision_time = r;
- break;
- }
- }
- }
-
- if(*collision_time > 1)
- {
- return false;
- }
-
- return true;
-}
-
-
-bool Intersect::intersect_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& p1,
- const Vec3f& L)
-{
- Vec3f va, vb, vp;
- va = a1 - a0;
- vb = b1 - b0;
- vp = p1 - p0;
-
- BVH_REAL a, b, c;
- computeCubicCoeff_VE(a0, b0, p0, va, vb, vp, L, &a, &b, &c);
-
- if(isZero(a) && isZero(b) && isZero(c))
- return true;
-
- return solveSquare(a, b, c, a0, b0, p0, va, vb, vp);
-
-}
-
-
-/** \brief Prefilter for intersection, works for both VF and EE */
-bool Intersect::intersectPreFiltering(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1)
-{
- Vec3f n0 = (b0 - a0).cross(c0 - a0);
- Vec3f n1 = (b1 - a1).cross(c1 - a1);
- Vec3f a0a1 = a1 - a0;
- Vec3f b0b1 = b1 - b0;
- Vec3f c0c1 = c1 - c0;
- Vec3f delta = (b0b1 - a0a1).cross(c0c1 - a0a1);
- Vec3f nx = (n0 + n1 - delta) * 0.5;
-
- Vec3f a0d0 = d0 - a0;
- Vec3f a1d1 = d1 - a1;
-
- BVH_REAL A = n0.dot(a0d0);
- BVH_REAL B = n1.dot(a1d1);
- BVH_REAL C = nx.dot(a0d0);
- BVH_REAL D = nx.dot(a1d1);
- BVH_REAL E = n1.dot(a0d0);
- BVH_REAL F = n0.dot(a1d1);
-
- if(A > 0 && B > 0 && (2*C +F) > 0 && (2*D+E) > 0)
- return false;
- if(A < 0 && B < 0 && (2*C +F) < 0 && (2*D+E) < 0)
- return false;
-
- return true;
-}
-
-bool Intersect::intersect_VF_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
- BVH_REAL* collision_time, Vec3f* p_i, bool useNewton)
-{
- if(intersectPreFiltering(a0, b0, c0, p0, a1, b1, c1, p1))
- {
- return intersect_VF(a0, b0, c0, p0, a1, b1, c1, p1, collision_time, p_i, useNewton);
- }
- else
- return false;
-}
-
-bool Intersect::intersect_EE_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
- BVH_REAL* collision_time, Vec3f* p_i, bool useNewton)
-{
- if(intersectPreFiltering(a0, b0, c0, d0, a1, b1, c1, d1))
- {
- return intersect_EE(a0, b0, c0, d0, a1, b1, c1, d1, collision_time, p_i, useNewton);
- }
- else
- return false;
-}
-
-bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
- const Vec3f R[3], const Vec3f& T,
- Vec3f* contact_points,
- unsigned int* num_contact_points,
- BVH_REAL* penetration_depth,
- Vec3f* normal)
-{
- Vec3f Q1_ = Vec3f(R[0].dot(Q1), R[1].dot(Q1), R[2].dot(Q1)) + T;
- Vec3f Q2_ = Vec3f(R[0].dot(Q2), R[1].dot(Q2), R[2].dot(Q2)) + T;
- Vec3f Q3_ = Vec3f(R[0].dot(Q3), R[1].dot(Q3), R[2].dot(Q3)) + T;
-
- return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal);
-}
-
-#if ODE_STYLE
-bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
- Vec3f* contact_points,
- unsigned int* num_contact_points,
- BVH_REAL* penetration_depth,
- Vec3f* normal)
-{
-
-
- Vec3f n1;
- BVH_REAL t1;
- bool b1 = buildTrianglePlane(P1, P2, P3, &n1, &t1);
- if(!b1) return false;
-
- Vec3f n2;
- BVH_REAL t2;
- bool b2 = buildTrianglePlane(Q1, Q2, Q3, &n2, &t2);
- if(!b2) return false;
-
- if(sameSideOfPlane(P1, P2, P3, n2, t2))
- return false;
-
- if(sameSideOfPlane(Q1, Q2, Q3, n1, t1))
- return false;
-
- Vec3f clipped_points1[MAX_TRIANGLE_CLIPS];
- unsigned int num_clipped_points1 = 0;
- Vec3f clipped_points2[MAX_TRIANGLE_CLIPS];
- unsigned int num_clipped_points2 = 0;
-
- Vec3f deepest_points1[MAX_TRIANGLE_CLIPS];
- unsigned int num_deepest_points1 = 0;
- Vec3f deepest_points2[MAX_TRIANGLE_CLIPS];
- unsigned int num_deepest_points2 = 0;
- BVH_REAL penetration_depth1 = -1, penetration_depth2 = -1;
-
- clipTriangleByTriangleAndEdgePlanes(Q1, Q2, Q3, P1, P2, P3, n1, t1, clipped_points2, &num_clipped_points2);
-
- if(num_clipped_points2 == 0)
- return false;
-
- computeDeepestPoints(clipped_points2, num_clipped_points2, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2);
- if(num_deepest_points2 == 0)
- return false;
-
- clipTriangleByTriangleAndEdgePlanes(P1, P2, P3, Q1, Q2, Q3, n2, t2, clipped_points1, &num_clipped_points1);
- if(num_clipped_points1 == 0)
- return false;
-
- computeDeepestPoints(clipped_points1, num_clipped_points1, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1);
- if(num_deepest_points1 == 0)
- return false;
-
-
- /* Return contact information */
- if(contact_points && num_contact_points && penetration_depth && normal)
- {
- if(penetration_depth1 > penetration_depth2)
- {
- *num_contact_points = num_deepest_points2;
- for(unsigned int i = 0; i < num_deepest_points2; ++i)
- {
- contact_points[i] = deepest_points2[i];
- }
-
- *normal = -n1;
- *penetration_depth = penetration_depth2;
- }
- else
- {
- *num_contact_points = num_deepest_points1;
- for(unsigned int i = 0; i < num_deepest_points1; ++i)
- {
- contact_points[i] = deepest_points1[i];
- }
-
- *normal = n2;
- *penetration_depth = penetration_depth1;
- }
- }
-
- return true;
-}
-#else
-bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
- Vec3f* contact_points,
- unsigned int* num_contact_points,
- BVH_REAL* penetration_depth,
- Vec3f* normal)
-{
- Vec3f p1, p2, p3, q1, q2, q3, e1, e2, e3, f1, f2, f3, g1, g2, g3, h1, h2, h3, n1, m1;
- Vec3f ef11, ef12, ef13, ef21, ef22, ef23, ef31, ef32, ef33;
-
- p1 = P1 - P1;
- p2 = P2 - P1;
- p3 = P3 - P1;
- q1 = Q1 - P1;
- q2 = Q2 - P1;
- q3 = Q3 - P1;
- e1 = p2 - p1;
- e2 = p3 - p2;
- e3 = p1 - p3;
- f1 = q2 - q1;
- f2 = q3 - q2;
- f3 = q1 - q3;
-
- n1 = e1.cross(e2);
- m1 = f1.cross(f2);
- g1 = e1.cross(n1);
- g2 = e2.cross(n1);
- g3 = e3.cross(n1);
- h1 = f1.cross(m1);
- h2 = f2.cross(m1);
- h3 = f3.cross(m1);
-
- ef11 = e1.cross(f1);
- ef12 = e1.cross(f2);
- ef13 = e1.cross(f3);
- ef21 = e2.cross(f1);
- ef22 = e2.cross(f2);
- ef23 = e2.cross(f3);
- ef31 = e3.cross(f1);
- ef32 = e3.cross(f2);
- ef33 = e3.cross(f3);
-
- if (!project6(n1, p1, p2, p3, q1, q2, q3)) return 0;
- if (!project6(m1, p1, p2, p3, q1, q2, q3)) return 0;
-
- if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return 0;
- if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return 0;
- if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return 0;
- if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return 0;
- if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return 0;
- if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return 0;
- if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return 0;
- if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return 0;
- if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return 0;
-
- if (!project6(g1, p1, p2, p3, q1, q2, q3)) return 0;
- if (!project6(g2, p1, p2, p3, q1, q2, q3)) return 0;
- if (!project6(g3, p1, p2, p3, q1, q2, q3)) return 0;
- if (!project6(h1, p1, p2, p3, q1, q2, q3)) return 0;
- if (!project6(h2, p1, p2, p3, q1, q2, q3)) return 0;
- if (!project6(h3, p1, p2, p3, q1, q2, q3)) return 0;
-
- if(contact_points && num_contact_points && penetration_depth && normal)
- {
- Vec3f n1, n2;
- BVH_REAL t1, t2;
- buildTrianglePlane(P1, P2, P3, &n1, &t1);
- buildTrianglePlane(Q1, Q2, Q3, &n2, &t2);
-
- Vec3f deepest_points1[3];
- unsigned int num_deepest_points1 = 0;
- Vec3f deepest_points2[3];
- unsigned int num_deepest_points2 = 0;
- BVH_REAL penetration_depth1, penetration_depth2;
-
- Vec3f P[3] = {P1, P2, P3};
- Vec3f Q[3] = {Q1, Q2, Q3};
-
- computeDeepestPoints(Q, 3, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2);
- computeDeepestPoints(P, 3, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1);
-
-
- if(penetration_depth1 > penetration_depth2)
- {
- *num_contact_points = std::min(num_deepest_points2, (unsigned int)2);
- for(unsigned int i = 0; i < num_deepest_points2; ++i)
- {
- contact_points[i] = deepest_points2[i];
- }
-
- *normal = -n1;
- *penetration_depth = penetration_depth2;
- }
- else
- {
- *num_contact_points = std::min(num_deepest_points1, (unsigned int)2);
- for(unsigned int i = 0; i < num_deepest_points1; ++i)
- {
- contact_points[i] = deepest_points1[i];
- }
-
- *normal = n2;
- *penetration_depth = penetration_depth1;
- }
- }
-
- return 1;
-}
-#endif
-
-
-void Intersect::computeDeepestPoints(Vec3f* clipped_points, unsigned int num_clipped_points, const Vec3f& n, BVH_REAL t, BVH_REAL* penetration_depth, Vec3f* deepest_points, unsigned int* num_deepest_points)
-{
- *num_deepest_points = 0;
- BVH_REAL max_depth = -std::numeric_limits<BVH_REAL>::max();
- unsigned int num_deepest_points_ = 0;
- unsigned int num_neg = 0;
- unsigned int num_pos = 0;
- unsigned int num_zero = 0;
-
- for(unsigned int i = 0; i < num_clipped_points; ++i)
- {
- BVH_REAL dist = -distanceToPlane(n, t, clipped_points[i]);
- if(dist > EPSILON) num_pos++;
- else if(dist < -EPSILON) num_neg++;
- else num_zero++;
- if(dist > max_depth)
- {
- max_depth = dist;
- num_deepest_points_ = 1;
- deepest_points[num_deepest_points_ - 1] = clipped_points[i];
- }
- else if(dist + 1e-6 >= max_depth)
- {
- num_deepest_points_++;
- deepest_points[num_deepest_points_ - 1] = clipped_points[i];
- }
- }
-
- if(max_depth < -EPSILON)
- num_deepest_points_ = 0;
-
- if(num_zero == 0 && ((num_neg == 0) || (num_pos == 0)))
- num_deepest_points_ = 0;
-
- *penetration_depth = max_depth;
- *num_deepest_points = num_deepest_points_;
-}
-
-void Intersect::clipTriangleByTriangleAndEdgePlanes(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3,
- const Vec3f& t1, const Vec3f& t2, const Vec3f& t3,
- const Vec3f& tn, BVH_REAL to,
- Vec3f clipped_points[], unsigned int* num_clipped_points,
- bool clip_triangle)
-{
- *num_clipped_points = 0;
- Vec3f temp_clip[MAX_TRIANGLE_CLIPS];
- Vec3f temp_clip2[MAX_TRIANGLE_CLIPS];
- unsigned int num_temp_clip = 0;
- unsigned int num_temp_clip2 = 0;
- Vec3f v[3] = {v1, v2, v3};
-
- Vec3f plane_n;
- BVH_REAL plane_dist;
-
- if(buildEdgePlane(t1, t2, tn, &plane_n, &plane_dist))
- {
- clipPolygonByPlane(v, 3, plane_n, plane_dist, temp_clip, &num_temp_clip);
- if(num_temp_clip > 0)
- {
- if(buildEdgePlane(t2, t3, tn, &plane_n, &plane_dist))
- {
- clipPolygonByPlane(temp_clip, num_temp_clip, plane_n, plane_dist, temp_clip2, &num_temp_clip2);
- if(num_temp_clip2 > 0)
- {
- if(buildEdgePlane(t3, t1, tn, &plane_n, &plane_dist))
- {
- if(clip_triangle)
- {
- num_temp_clip = 0;
- clipPolygonByPlane(temp_clip2, num_temp_clip2, plane_n, plane_dist, temp_clip, &num_temp_clip);
- if(num_temp_clip > 0)
- {
- clipPolygonByPlane(temp_clip, num_temp_clip, tn, to, clipped_points, num_clipped_points);
- }
- }
- else
- {
- clipPolygonByPlane(temp_clip2, num_temp_clip2, plane_n, plane_dist, clipped_points, num_clipped_points);
- }
- }
- }
- }
- }
- }
-}
-
-void Intersect::clipPolygonByPlane(Vec3f* polygon_points, unsigned int num_polygon_points, const Vec3f& n, BVH_REAL t, Vec3f clipped_points[], unsigned int* num_clipped_points)
-{
- *num_clipped_points = 0;
-
- unsigned int num_clipped_points_ = 0;
- unsigned int vi;
- unsigned int prev_classify = 2;
- unsigned int classify;
- for(unsigned int i = 0; i <= num_polygon_points; ++i)
- {
- vi = (i % num_polygon_points);
- BVH_REAL d = distanceToPlane(n, t, polygon_points[i]);
- classify = ((d > EPSILON) ? 1 : 0);
- if(classify == 0)
- {
- if(prev_classify == 1)
- {
- if(num_clipped_points_ < MAX_TRIANGLE_CLIPS)
- {
- Vec3f tmp;
- clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp);
- if(num_clipped_points_ > 0)
- {
- if((tmp - clipped_points[num_clipped_points_ - 1]).sqrLength() > EPSILON)
- {
- clipped_points[num_clipped_points_] = tmp;
- num_clipped_points_++;
- }
- }
- else
- {
- clipped_points[num_clipped_points_] = tmp;
- num_clipped_points_++;
- }
- }
- }
-
- if(num_clipped_points_ < MAX_TRIANGLE_CLIPS && i < num_polygon_points)
- {
- clipped_points[num_clipped_points_] = polygon_points[vi];
- num_clipped_points_++;
- }
- }
- else
- {
- if(prev_classify == 0)
- {
- if(num_clipped_points_ < MAX_TRIANGLE_CLIPS)
- {
- Vec3f tmp;
- clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp);
- if(num_clipped_points_ > 0)
- {
- if((tmp - clipped_points[num_clipped_points_ - 1]).sqrLength() > EPSILON)
- {
- clipped_points[num_clipped_points_] = tmp;
- num_clipped_points_++;
- }
- }
- else
- {
- clipped_points[num_clipped_points_] = tmp;
- num_clipped_points_++;
- }
- }
- }
- }
-
- prev_classify = classify;
- }
-
- if(num_clipped_points_ > 2)
- {
- if((clipped_points[0] - clipped_points[num_clipped_points_ - 1]).sqrLength() < EPSILON)
- {
- num_clipped_points_--;
- }
- }
-
- *num_clipped_points = num_clipped_points_;
-}
-
-void Intersect::clipSegmentByPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& n, BVH_REAL t, Vec3f* clipped_point)
-{
- BVH_REAL dist1 = distanceToPlane(n, t, v1);
- Vec3f tmp = v2 - v1;
- BVH_REAL dist2 = tmp.dot(n);
- *clipped_point = tmp * (-dist1 / dist2) + v1;
-}
-
-BVH_REAL Intersect::distanceToPlane(const Vec3f& n, BVH_REAL t, const Vec3f& v)
-{
- return n.dot(v) - t;
-}
-
-bool Intersect::buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, BVH_REAL* t)
-{
- Vec3f n_ = (v2 - v1).cross(v3 - v1);
- if(n_.normalize())
- {
- *n = n_;
- *t = n_.dot(v1);
- return true;
- }
-
- return false;
-}
-
-bool Intersect::buildEdgePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& tn, Vec3f* n, BVH_REAL* t)
-{
- Vec3f n_ = (v2 - v1).cross(tn);
- if(n_.normalize())
- {
- *n = n_;
- *t = n_.dot(v1);
- return true;
- }
-
- return false;
-}
-
-bool Intersect::sameSideOfPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, const Vec3f& n, BVH_REAL t)
-{
- BVH_REAL dist1 = distanceToPlane(n, t, v1);
- BVH_REAL dist2 = dist1 * distanceToPlane(n, t, v2);
- BVH_REAL dist3 = dist1 * distanceToPlane(n, t, v3);
- if((dist2 > 0) && (dist3 > 0))
- return true;
- return false;
-}
-
-int Intersect::project6(const Vec3f& ax,
- const Vec3f& p1, const Vec3f& p2, const Vec3f& p3,
- const Vec3f& q1, const Vec3f& q2, const Vec3f& q3)
-{
- BVH_REAL P1 = ax.dot(p1);
- BVH_REAL P2 = ax.dot(p2);
- BVH_REAL P3 = ax.dot(p3);
- BVH_REAL Q1 = ax.dot(q1);
- BVH_REAL Q2 = ax.dot(q2);
- BVH_REAL Q3 = ax.dot(q3);
-
- BVH_REAL mx1 = std::max(P1, std::max(P2, P3));
- BVH_REAL mn1 = std::min(P1, std::min(P2, P3));
- BVH_REAL mx2 = std::max(Q1, std::max(Q2, Q3));
- BVH_REAL mn2 = std::min(Q1, std::min(Q2, Q3));
-
- if(mn1 > mx2) return 0;
- if(mn2 > mx1) return 0;
- return 1;
-}
-
-
-#if USE_SVMLIGHT
-
-void Intersect::singleKernelGradient(KERNEL_PARM *kernel_parm, SVECTOR *a, SVECTOR *b, Vec3f& g)
-{
- double tmp;
- switch(kernel_parm->kernel_type)
- {
- case 0: /* linear */
- g[0] = a->words[0].weight;
- g[1] = a->words[1].weight;
- g[2] = a->words[2].weight;
- break;
- case 1: /* polynomial */
- tmp = pow(kernel_parm->coef_lin * sprod_ss(a, b) + kernel_parm->coef_const, (double)kernel_parm->poly_degree - 1) * kernel_parm->poly_degree * kernel_parm->coef_lin;
- g[0] = tmp * a->words[0].weight;
- g[1] = tmp * a->words[1].weight;
- g[2] = tmp * a->words[2].weight;
- break;
- case 2: /* radial basis function */
- tmp = (exp(-kernel_parm->rbf_gamma * (a->twonorm_sq - 2 * sprod_ss(a, b) + b->twonorm_sq))) * 2 * (-kernel_parm->rbf_gamma);
- g[0] = tmp * (b->words[0].weight - a->words[0].weight);
- g[1] = tmp * (b->words[1].weight - a->words[1].weight);
- g[2] = tmp * (b->words[2].weight - a->words[2].weight);
- break;
- case 3: /* sigmoid neural net */
- tmp = 1 - (tanh(kernel_parm->coef_lin * sprod_ss(a, b) + kernel_parm->coef_const)) * (tanh(kernel_parm->coef_lin * sprod_ss(a, b) + kernel_parm->coef_const));
- tmp *= kernel_parm->coef_lin;
- g[0] = tmp * a->words[0].weight;
- g[1] = tmp * a->words[1].weight;
- g[2] = tmp * a->words[2].weight;
- break;
- default:
- std::cout << "Error: Unknown kernel function" << std::endl;
- return;
- }
-}
-
-void Intersect::kernelGradient(KERNEL_PARM *kernel_parm, DOC *a, DOC *b, Vec3f& g)
-{
- g = Vec3f(0, 0, 0);
- SVECTOR *fa, *fb;
- Vec3f tmp;
-
- for(fa = a->fvec; fa; fa = fa->next)
- {
- for(fb = b->fvec; fb; fb = fb->next)
- {
- if (fa->kernel_id == fb->kernel_id)
- {
- singleKernelGradient(kernel_parm, fa, fb, tmp);
- g += (tmp * (fa->factor * fb->factor));
- }
- }
- }
-}
-
-BVH_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
- Vec3f* cloud2, Uncertainty* uc2, int size_cloud2,
- const CloudClassifierParam& solver, bool scaling)
-{
- KERNEL_CACHE *kernel_cache;
- LEARN_PARM learn_parm = solver.learn_parm;
- KERNEL_PARM kernel_parm = solver.kernel_parm;
- MODEL *model = (MODEL *)my_malloc(sizeof(MODEL));
-
- int nPositiveExamples = size_cloud1;
- int nNegativeExamples = size_cloud2;
- int totdoc = nPositiveExamples + nNegativeExamples;
- int totwords = 3;
-
- int queryid = 0;
- int slackid = 0;
- double costfactor = 1;
- WORD *words = (WORD *)my_malloc(sizeof(WORD) * 4);
-
- DOC** docs = (DOC **)my_malloc(sizeof(DOC *) * (nPositiveExamples + nNegativeExamples));
- double* label = (double *)my_malloc(sizeof(double) * (nPositiveExamples + nNegativeExamples));
-
- float bbmin[3] = {0, 0, 0};
- float bbmax[3] = {0, 0, 0};
-
- for(int i = 0; i < nPositiveExamples; ++i)
- {
- int example_id = i;
- label[example_id] = 1;
- float coord1[3];
- coord1[0] = cloud1[i][0];
- coord1[1] = cloud1[i][1];
- coord1[2] = cloud1[i][2];
-
-
- words[0].wnum = 1;
- words[0].weight = coord1[0];
- words[1].wnum = 2;
- words[1].weight = coord1[1];
- words[2].wnum = 3;
- words[2].weight = coord1[2];
- words[3].wnum = 0;
- words[3].weight = 3;
-
- docs[example_id] = create_example(example_id, queryid, slackid, costfactor,
- create_svector(words, (char*)"", 1.0));
-
- if(scaling)
- {
- if(i == 0)
- {
- bbmin[0] = coord1[0];
- bbmin[1] = coord1[1];
- bbmin[2] = coord1[2];
- bbmax[0] = coord1[0];
- bbmax[1] = coord1[1];
- bbmax[2] = coord1[2];
- }
- else
- {
- if(bbmin[0] > coord1[0]) bbmin[0] = coord1[0];
- if(bbmax[0] < coord1[0]) bbmax[0] = coord1[0];
- if(bbmin[1] > coord1[1]) bbmin[1] = coord1[1];
- if(bbmax[1] < coord1[1]) bbmax[1] = coord1[1];
- if(bbmin[2] > coord1[2]) bbmin[2] = coord1[2];
- if(bbmax[2] < coord1[2]) bbmax[2] = coord1[2];
- }
- }
- }
-
-
- for(int i = 0; i < nNegativeExamples; ++i)
- {
- int example_id = i + nPositiveExamples;
- label[example_id] = -1;
- float coord1[3];
-
- coord1[0] = cloud2[i][0];
- coord1[1] = cloud2[i][1];
- coord1[2] = cloud2[i][2];
-
- words[0].wnum = 1;
- words[0].weight = coord1[0];
- words[1].wnum = 2;
- words[1].weight = coord1[1];
- words[2].wnum = 3;
- words[2].weight = coord1[2];
- words[3].wnum = 0;
- words[3].weight = 3;
- docs[example_id] = create_example(example_id, queryid, slackid, costfactor,
- create_svector(words, (char*)"", 1.0));
-
- if(scaling)
- {
- if(bbmin[0] > coord1[0]) bbmin[0] = coord1[0];
- if(bbmax[0] < coord1[0]) bbmax[0] = coord1[0];
- if(bbmin[1] > coord1[1]) bbmin[1] = coord1[1];
- if(bbmax[1] < coord1[1]) bbmax[1] = coord1[1];
- if(bbmin[2] > coord1[2]) bbmin[2] = coord1[2];
- if(bbmax[2] < coord1[2]) bbmax[2] = coord1[2];
- }
- }
-
- BVH_REAL S[3];
- S[0] = 1 / (bbmax[0] - bbmin[0]);
- S[1] = 1 / (bbmax[1] - bbmin[1]);
- S[2] = 1 / (bbmax[2] - bbmin[2]);
-
- if(scaling)
- {
- for(int i = 0; i < totdoc; ++i)
- {
- BVH_REAL f = docs[i]->fvec->words[0].weight;
- docs[i]->fvec->words[0].weight = (f - bbmin[0]) * S[0];
- f = docs[i]->fvec->words[1].weight;
- docs[i]->fvec->words[1].weight = (f - bbmin[1]) * S[1];
- f = docs[i]->fvec->words[2].weight;
- docs[i]->fvec->words[2].weight = (f - bbmin[2]) * S[2];
- docs[i]->fvec->twonorm_sq = sprod_ss(docs[i]->fvec, docs[i]->fvec);
- }
- }
-
- if(kernel_parm.kernel_type == LINEAR) /** don't need the cache */
- kernel_cache = NULL;
- else
- {
- /** Always get a new kernel cache. It is not possible to use the
- * same cache for two different training runs
- */
- kernel_cache = kernel_cache_init(totdoc, learn_parm.kernel_cache_size);
- }
-
-
- double *alpha_in = NULL;
- int nerrors;
- double maxerror;
- svm_learn_classification_extend(docs, label, totdoc, totwords, &learn_parm,
- &kernel_parm, kernel_cache, model, alpha_in, &nerrors, &maxerror);
-
- /** compute collision probability */
- double max_collision_prob = 0;
- for(int i = 0; i < totdoc; ++i)
- {
- Vec3f fgrad;
- Vec3f g;
- double f = - label[i] * classify_example(model, docs[i]);
-
- for(int j = 1; j < model->sv_num; j++)
- {
- kernelGradient(&model->kernel_parm, model->supvec[j], docs[i], g);
- fgrad += (g * (model->alpha[j] * label[j]));
-
- if (i < nPositiveExamples)
- {
- double sigma = MxV(uc1[i].Sigma, fgrad).dot(fgrad);
- BVH_REAL col_prob = gaussianCDF(f / sqrt(sigma));
- if(max_collision_prob < col_prob)
- max_collision_prob = col_prob;
- }
- else
- {
- double sigma = MxV(uc2[i - nPositiveExamples].Sigma, fgrad).dot(fgrad);
- BVH_REAL col_prob = gaussianCDF(f / sqrt(sigma));
- if(max_collision_prob < col_prob)
- max_collision_prob = col_prob;
- }
- }
- }
-
- if(kernel_cache)
- {
- kernel_cache_cleanup(kernel_cache);
- kernel_cache = NULL;
- }
-
-
- free(alpha_in);
- free_model(model, 0);
- for (int i = 0; i < totdoc; i++)
- free_example(docs[i], 1);
- free(docs);
- free(label);
- free(words);
-
- return max_collision_prob;
-}
-
-BVH_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
- Vec3f* cloud2, Uncertainty* uc2, int size_cloud2,
- const Vec3f R[3], const Vec3f& T, const CloudClassifierParam& solver, bool scaling)
-{
- KERNEL_CACHE *kernel_cache;
- LEARN_PARM learn_parm = solver.learn_parm;
- KERNEL_PARM kernel_parm = solver.kernel_parm;
- MODEL *model = (MODEL *)my_malloc(sizeof(MODEL));
-
- int nPositiveExamples = size_cloud1;
- int nNegativeExamples = size_cloud2;
- int totdoc = nPositiveExamples + nNegativeExamples;
- int totwords = 3;
-
- int queryid = 0;
- int slackid = 0;
- double costfactor = 1;
- WORD *words = (WORD *)my_malloc(sizeof(WORD) * 4);
-
- DOC** docs = (DOC **)my_malloc(sizeof(DOC *) * (nPositiveExamples + nNegativeExamples));
- double* label = (double *)my_malloc(sizeof(double) * (nPositiveExamples + nNegativeExamples));
-
- float bbmin[3] = {0, 0, 0};
- float bbmax[3] = {0, 0, 0};
-
- for(int i = 0; i < nPositiveExamples; ++i)
- {
- int example_id = i;
- label[example_id] = 1;
- float coord1[3];
- coord1[0] = cloud1[i][0];
- coord1[1] = cloud1[i][1];
- coord1[2] = cloud1[i][2];
-
-
- words[0].wnum = 1;
- words[0].weight = coord1[0];
- words[1].wnum = 2;
- words[1].weight = coord1[1];
- words[2].wnum = 3;
- words[2].weight = coord1[2];
- words[3].wnum = 0;
- words[3].weight = 3;
-
- docs[example_id] = create_example(example_id, queryid, slackid, costfactor,
- create_svector(words, (char*)"", 1.0));
-
- if(scaling)
- {
- if(i == 0)
- {
- bbmin[0] = coord1[0];
- bbmin[1] = coord1[1];
- bbmin[2] = coord1[2];
- bbmax[0] = coord1[0];
- bbmax[1] = coord1[1];
- bbmax[2] = coord1[2];
- }
- else
- {
- if(bbmin[0] > coord1[0]) bbmin[0] = coord1[0];
- if(bbmax[0] < coord1[0]) bbmax[0] = coord1[0];
- if(bbmin[1] > coord1[1]) bbmin[1] = coord1[1];
- if(bbmax[1] < coord1[1]) bbmax[1] = coord1[1];
- if(bbmin[2] > coord1[2]) bbmin[2] = coord1[2];
- if(bbmax[2] < coord1[2]) bbmax[2] = coord1[2];
- }
- }
- }
-
-
- for(int i = 0; i < nNegativeExamples; ++i)
- {
- int example_id = i + nPositiveExamples;
- label[example_id] = -1;
- Vec3f coord0, coord1;
-
- coord0[0] = cloud2[i][0];
- coord0[1] = cloud2[i][1];
- coord0[2] = cloud2[i][2];
- coord1 = MxV(R, coord0) + T; // rotate the coordinate
-
- words[0].wnum = 1;
- words[0].weight = coord1[0];
- words[1].wnum = 2;
- words[1].weight = coord1[1];
- words[2].wnum = 3;
- words[2].weight = coord1[2];
- words[3].wnum = 0;
- words[3].weight = 3;
- docs[example_id] = create_example(example_id, queryid, slackid, costfactor,
- create_svector(words, (char*)"", 1.0));
-
- if(scaling)
- {
- if(bbmin[0] > coord1[0]) bbmin[0] = coord1[0];
- if(bbmax[0] < coord1[0]) bbmax[0] = coord1[0];
- if(bbmin[1] > coord1[1]) bbmin[1] = coord1[1];
- if(bbmax[1] < coord1[1]) bbmax[1] = coord1[1];
- if(bbmin[2] > coord1[2]) bbmin[2] = coord1[2];
- if(bbmax[2] < coord1[2]) bbmax[2] = coord1[2];
- }
- }
-
- BVH_REAL S[3];
- S[0] = 1 / (bbmax[0] - bbmin[0]);
- S[1] = 1 / (bbmax[1] - bbmin[1]);
- S[2] = 1 / (bbmax[2] - bbmin[2]);
-
- if(scaling)
- {
- for(int i = 0; i < totdoc; ++i)
- {
- BVH_REAL f = docs[i]->fvec->words[0].weight;
- docs[i]->fvec->words[0].weight = (f - bbmin[0]) * S[0];
- f = docs[i]->fvec->words[1].weight;
- docs[i]->fvec->words[1].weight = (f - bbmin[1]) * S[1];
- f = docs[i]->fvec->words[2].weight;
- docs[i]->fvec->words[2].weight = (f - bbmin[2]) * S[2];
- docs[i]->fvec->twonorm_sq = sprod_ss(docs[i]->fvec, docs[i]->fvec);
- }
- }
-
- if(kernel_parm.kernel_type == LINEAR) /** don't need the cache */
- kernel_cache = NULL;
- else
- {
- /** Always get a new kernel cache. It is not possible to use the
- * same cache for two different training runs
- */
- kernel_cache = kernel_cache_init(totdoc, learn_parm.kernel_cache_size);
- }
-
-
- double *alpha_in = NULL;
- int nerrors;
- double maxerror;
- svm_learn_classification_extend(docs, label, totdoc, totwords, &learn_parm,
- &kernel_parm, kernel_cache, model, alpha_in, &nerrors, &maxerror);
-
- /** compute collision probability */
- double max_collision_prob = 0;
- for(int i = 0; i < totdoc; ++i)
- {
- Vec3f fgrad;
- Vec3f g;
- double f = - label[i] * classify_example(model, docs[i]);
-
- for(int j = 1; j < model->sv_num; j++)
- {
- kernelGradient(&model->kernel_parm, model->supvec[j], docs[i], g);
- fgrad += (g * (model->alpha[j] * label[j]));
-
- if (i < nPositiveExamples)
- {
- double sigma = MxV(uc1[i].Sigma, fgrad).dot(fgrad);
- BVH_REAL col_prob = gaussianCDF(f / sqrt(sigma));
- if(max_collision_prob < col_prob)
- max_collision_prob = col_prob;
- }
- else
- {
- Vec3f rotatedSigma[3];
- SMST(uc2[i - nPositiveExamples].Sigma, R, rotatedSigma);
- double sigma = MxV(rotatedSigma, fgrad).dot(fgrad);
- BVH_REAL col_prob = gaussianCDF(f / sqrt(sigma));
- if(max_collision_prob < col_prob)
- max_collision_prob = col_prob;
- }
- }
- }
-
- if(kernel_cache)
- {
- kernel_cache_cleanup(kernel_cache);
- kernel_cache = NULL;
- }
-
-
- free(alpha_in);
- free_model(model, 0);
- for (int i = 0; i < totdoc; i++)
- free_example(docs[i], 1);
- free(docs);
- free(label);
- free(words);
-
- return max_collision_prob;
-}
-
-
-BVH_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
- const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3)
-{
- // get the plane x * n - t = 0 and the compute the projection matrix according to (I - nn^t) y + t * n = y'
- BVH_REAL t;
- Vec3f n;
- bool b_plane = buildTrianglePlane(Q1, Q2, Q3, &n, &t);
- if(!b_plane)
- {
- std::cerr << "build triangle plane failed!" << std::endl;
- return 0.0;
- }
-
- BVH_REAL edge_t[3];
- Vec3f edge_n[3];
-
- bool b_edge_plane1 = buildEdgePlane(Q1, Q2, n, edge_n + 0, edge_t + 0);
- if(!b_edge_plane1)
- {
- std::cerr << "build edge plane1 failed!" << std::endl;
- return 0.0;
- }
-
- bool b_edge_plane2 = buildEdgePlane(Q2, Q3, n, edge_n + 1, edge_t + 1);
- if(!b_edge_plane2)
- {
- std::cerr << "build edge plane2 failed!" << std::endl;
- return 0.0;
- }
-
- bool b_edge_plane3 = buildEdgePlane(Q3, Q1, n, edge_n + 2, edge_t + 2);
- if(!b_edge_plane3)
- {
- std::cerr << "build edge plane3 failed!" << std::endl;
- return 0.0;
- }
-
- Vec3f P[3];
- for(int i = 0; i < 3; ++i)
- {
- for(int j = 0; j < 3; ++j)
- {
- P[i][j] = ((i == j) ? 1 : 0) - n[i] * n[j];
- }
- }
-
- Vec3f delta = n * t;
-
- BVH_REAL max_prob = 0;
- for(int i = 0; i < size_cloud1; ++i)
- {
- Vec3f projected_p = MxV(P, cloud1[i]) + delta;
-
- // compute the projected uncertainty by P * S * P'
- const Vec3f* S = uc1[i].Sigma;
- Vec3f newS[3];
- SMST(S, P, newS);
-
- // check whether the point is inside or outside the triangle
-
- bool b_inside = insideTriangle(Q1, Q2, Q3, projected_p);
-
- if(b_inside)
- {
- BVH_REAL prob1 = gaussianCDF((projected_p.dot(edge_n[0]) - edge_t[0]) / sqrt(vTMv(newS, edge_n[0])));
- BVH_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[1]) - edge_t[1]) / sqrt(vTMv(newS, edge_n[1])));
- BVH_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[2]) - edge_t[2]) / sqrt(vTMv(newS, edge_n[2])));
- BVH_REAL prob = 1.0 - prob1 - prob2 - prob3;
- if(prob > max_prob) max_prob = prob;
- }
- else
- {
- BVH_REAL d1 = projected_p.dot(edge_n[0]) - edge_t[0];
- BVH_REAL d2 = projected_p.dot(edge_n[1]) - edge_t[1];
- BVH_REAL d3 = projected_p.dot(edge_n[2]) - edge_t[2];
-
- std::vector<int> pos_plane;
- std::vector<int> neg_plane;
- if(d1 > 0) pos_plane.push_back(0); else neg_plane.push_back(0);
- if(d2 > 0) pos_plane.push_back(1); else neg_plane.push_back(1);
- if(d3 > 0) pos_plane.push_back(2); else neg_plane.push_back(2);
-
- if(pos_plane.size() == 1)
- {
- int pos_id = pos_plane[0];
- BVH_REAL prob1 = gaussianCDF(-(projected_p.dot(edge_n[pos_id]) - edge_t[pos_id]) / sqrt(vTMv(newS, edge_n[pos_id])));
-
- int neg_id1 = neg_plane[0];
- int neg_id2 = neg_plane[1];
- BVH_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[neg_id1]) - edge_t[neg_id1]) / sqrt(vTMv(newS, edge_n[neg_id2])));
- BVH_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[neg_id2]) - edge_t[neg_id2]) / sqrt(vTMv(newS, edge_n[neg_id2])));
-
- BVH_REAL prob = prob1 - prob2 - prob3;
- if(prob > max_prob) max_prob = prob;
-
- }
- else if(pos_plane.size() == 2)
- {
- int neg_id = neg_plane[0];
- BVH_REAL prob1 = gaussianCDF(-(projected_p.dot(edge_n[neg_id]) - edge_t[neg_id]) / sqrt(vTMv(newS, edge_n[neg_id])));
-
- int pos_id1 = pos_plane[0];
- int pos_id2 = pos_plane[1];
-
- BVH_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[pos_id1])) / sqrt(vTMv(newS, edge_n[pos_id1])));
- BVH_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[pos_id2])) / sqrt(vTMv(newS, edge_n[pos_id2])));
-
- BVH_REAL prob = prob1 - prob2 - prob3;
- if(prob > max_prob) max_prob = prob;
- }
- else
- {
- std::cerr << "Ooops, seems something is wrong: " << pos_plane.size() << std::endl;
- }
- }
- }
-
- return max_prob;
-}
-
-
-BVH_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
- const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
- const Vec3f R[3], const Vec3f& T)
-{
- Vec3f Q1_ = Vec3f(R[0].dot(Q1), R[1].dot(Q1), R[2].dot(Q1)) + T;
- Vec3f Q2_ = Vec3f(R[0].dot(Q2), R[1].dot(Q2), R[2].dot(Q2)) + T;
- Vec3f Q3_ = Vec3f(R[0].dot(Q3), R[1].dot(Q3), R[2].dot(Q3)) + T;
-
- return intersect_PointCloudsTriangle(cloud1, uc1, size_cloud1, Q1_, Q2_, Q3_);
-}
-
-#endif
-
-
-void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, const Vec3f& B,
- Vec3f& VEC, Vec3f& X, Vec3f& Y)
-{
- Vec3f T;
- BVH_REAL A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T;
- Vec3f TMP;
-
- T = Q - P;
- A_dot_A = A.dot(A);
- B_dot_B = B.dot(B);
- A_dot_B = A.dot(B);
- A_dot_T = A.dot(T);
- B_dot_T = B.dot(T);
-
- // t parameterizes ray P,A
- // u parameterizes ray Q,B
-
- BVH_REAL t, u;
-
- // compute t for the closest point on ray P,A to
- // ray Q,B
-
- BVH_REAL denom = A_dot_A*B_dot_B - A_dot_B*A_dot_B;
-
- t = (A_dot_T*B_dot_B - B_dot_T*A_dot_B) / denom;
-
- // clamp result so t is on the segment P,A
-
- if((t < 0) || isnan(t)) t = 0; else if(t > 1) t = 1;
-
- // find u for point on ray Q,B closest to point at t
-
- u = (t*A_dot_B - B_dot_T) / B_dot_B;
-
- // if u is on segment Q,B, t and u correspond to
- // closest points, otherwise, clamp u, recompute and
- // clamp t
-
- if((u <= 0) || isnan(u))
- {
- Y = Q;
-
- t = A_dot_T / A_dot_A;
-
- if((t <= 0) || isnan(t))
- {
- X = P;
- VEC = Q - P;
- }
- else if(t >= 1)
- {
- X = P + A;
- VEC = Q - X;
- }
- else
- {
- X = P + A * t;
- TMP = T.cross(A);
- VEC = A.cross(TMP);
- }
- }
- else if (u >= 1)
- {
- Y = Q + B;
-
- t = (A_dot_B + A_dot_T) / A_dot_A;
-
- if((t <= 0) || isnan(t))
- {
- X = P;
- VEC = Y - P;
- }
- else if(t >= 1)
- {
- X = P + A;
- VEC = Y - X;
- }
- else
- {
- X = P + A * t;
- T = Y - P;
- TMP = T.cross(A);
- VEC= A.cross(TMP);
- }
- }
- else
- {
- Y = Q + B * u;
-
- if((t <= 0) || isnan(t))
- {
- X = P;
- TMP = T.cross(B);
- VEC = B.cross(TMP);
- }
- else if(t >= 1)
- {
- X = P + A;
- T = Q - X;
- TMP = T.cross(B);
- VEC = B.cross(TMP);
- }
- else
- {
- X = P + A * t;
- VEC = A.cross(B);
- if(VEC.dot(T) < 0)
- {
- VEC = VEC * (-1);
- }
- }
- }
-}
-
-
-BVH_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, Vec3f& Q)
-{
- // Compute vectors along the 6 sides
-
- Vec3f Sv[3];
- Vec3f Tv[3];
- Vec3f VEC;
-
- Sv[0] = S[1] - S[0];
- Sv[1] = S[2] - S[1];
- Sv[2] = S[0] - S[2];
-
- Tv[0] = T[1] - T[0];
- Tv[1] = T[2] - T[1];
- Tv[2] = T[0] - T[2];
-
- // For each edge pair, the vector connecting the closest points
- // of the edges defines a slab (parallel planes at head and tail
- // enclose the slab). If we can show that the off-edge vertex of
- // each triangle is outside of the slab, then the closest points
- // of the edges are the closest points for the triangles.
- // Even if these tests fail, it may be helpful to know the closest
- // points found, and whether the triangles were shown disjoint
-
- Vec3f V, Z, minP, minQ;
- BVH_REAL mindd;
- int shown_disjoint = 0;
-
- mindd = (S[0] - T[0]).sqrLength() + 1; // Set first minimum safely high
-
- for(int i = 0; i < 3; ++i)
- {
- for(int j = 0; j < 3; ++j)
- {
- // Find closest points on edges i & j, plus the
- // vector (and distance squared) between these points
- segPoints(S[i], Sv[i], T[j], Tv[j], VEC, P, Q);
-
- V = Q - P;
- BVH_REAL dd = V.dot(V);
-
- // Verify this closest point pair only if the distance
- // squared is less than the minimum found thus far.
-
- if(dd <= mindd)
- {
- minP = P;
- minQ = Q;
- mindd = dd;
-
- Z = S[(i+2)%3] - P;
- BVH_REAL a = Z.dot(VEC);
- Z = T[(j+2)%3] - Q;
- BVH_REAL b = Z.dot(VEC);
-
- if((a <= 0) && (b >= 0)) return sqrt(dd);
-
- BVH_REAL p = V.dot(VEC);
-
- if(a < 0) a = 0;
- if(b > 0) b = 0;
- if((p - a + b) > 0) shown_disjoint = 1;
- }
- }
- }
-
- // No edge pairs contained the closest points.
- // either:
- // 1. one of the closest points is a vertex, and the
- // other point is interior to a face.
- // 2. the triangles are overlapping.
- // 3. an edge of one triangle is parallel to the other's face. If
- // cases 1 and 2 are not true, then the closest points from the 9
- // edge pairs checks above can be taken as closest points for the
- // triangles.
- // 4. possibly, the triangles were degenerate. When the
- // triangle points are nearly colinear or coincident, one
- // of above tests might fail even though the edges tested
- // contain the closest points.
-
- // First check for case 1
-
- Vec3f Sn;
- BVH_REAL Snl;
-
- Sn = Sv[0].cross(Sv[1]); // Compute normal to S triangle
- Snl = Sn.dot(Sn); // Compute square of length of normal
-
- // If cross product is long enough,
-
- if(Snl > 1e-15)
- {
- // Get projection lengths of T points
-
- Vec3f Tp;
-
- V = S[0] - T[0];
- Tp[0] = V.dot(Sn);
-
- V = S[0] - T[1];
- Tp[1] = V.dot(Sn);
-
- V = S[0] - T[2];
- Tp[2] = V.dot(Sn);
-
- // If Sn is a separating direction,
- // find point with smallest projection
-
- int point = -1;
- if((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0))
- {
- if(Tp[0] < Tp[1]) point = 0; else point = 1;
- if(Tp[2] < Tp[point]) point = 2;
- }
- else if((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0))
- {
- if(Tp[0] > Tp[1]) point = 0; else point = 1;
- if(Tp[2] > Tp[point]) point = 2;
- }
-
- // If Sn is a separating direction,
-
- if(point >= 0)
- {
- shown_disjoint = 1;
-
- // Test whether the point found, when projected onto the
- // other triangle, lies within the face.
-
- V = T[point] - S[0];
- Z = Sn.cross(Sv[0]);
- if(V.dot(Z) > 0)
- {
- V = T[point] - S[1];
- Z = Sn.cross(Sv[1]);
- if(V.dot(Z) > 0)
- {
- V = T[point] - S[2];
- Z = Sn.cross(Sv[2]);
- if(V.dot(Z) > 0)
- {
- // T[point] passed the test - it's a closest point for
- // the T triangle; the other point is on the face of S
- P = T[point] + Sn * (Tp[point] / Snl);
- Q = T[point];
- return (P - Q).length();
- }
- }
- }
- }
- }
-
- Vec3f Tn;
- BVH_REAL Tnl;
-
- Tn = Tv[0].cross(Tv[1]);
- Tnl = Tn.dot(Tn);
-
- if(Tnl > 1e-15)
- {
- Vec3f Sp;
-
- V = T[0] - S[0];
- Sp[0] = V.dot(Tn);
-
- V = T[0] - S[1];
- Sp[1] = V.dot(Tn);
-
- V = T[0] - S[2];
- Sp[2] = V.dot(Tn);
-
- int point = -1;
- if((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0))
- {
- if(Sp[0] < Sp[1]) point = 0; else point = 1;
- if(Sp[2] < Sp[point]) point = 2;
- }
- else if((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0))
- {
- if(Sp[0] > Sp[1]) point = 0; else point = 1;
- if(Sp[2] > Sp[point]) point = 2;
- }
-
- if(point >= 0)
- {
- shown_disjoint = 1;
-
- V = S[point] - T[0];
- Z = Tn.cross(Tv[0]);
- if(V.dot(Z) > 0)
- {
- V = S[point] - T[1];
- Z = Tn.cross(Tv[1]);
- if(V.dot(Z) > 0)
- {
- V = S[point] - T[2];
- Z = Tn.cross(Tv[2]);
- if(V.dot(Z) > 0)
- {
- P = S[point];
- Q = S[point] + Tn * (Sp[point] / Tnl);
- return (P - Q).length();
- }
- }
- }
- }
- }
-
- // Case 1 can't be shown.
- // If one of these tests showed the triangles disjoint,
- // we assume case 3 or 4, otherwise we conclude case 2,
- // that the triangles overlap.
-
- if(shown_disjoint)
- {
- P = minP;
- Q = minQ;
- return sqrt(mindd);
- }
- else return 0;
-}
-
-
-BVH_REAL TriangleDistance::triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
- const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
- Vec3f& P, Vec3f& Q)
-{
- Vec3f S[3];
- Vec3f T[3];
- S[0] = S1; S[1] = S2; S[2] = S3;
- T[0] = T1; T[1] = T2; T[2] = T3;
-
- return triDistance(S, T, P, Q);
-}
-
-BVH_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3],
- const Vec3f R[3], const Vec3f& Tl,
- Vec3f& P, Vec3f& Q)
-{
- Vec3f T_transformed[3];
- T_transformed[0] = MxV(R, T[0]) + Tl;
- T_transformed[1] = MxV(R, T[1]) + Tl;
- T_transformed[2] = MxV(R, T[2]) + Tl;
-
- return triDistance(S, T_transformed, P, Q);
-}
-
-BVH_REAL TriangleDistance::triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
- const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
- const Vec3f R[3], const Vec3f& Tl,
- Vec3f& P, Vec3f& Q)
-{
- Vec3f T1_transformed = MxV(R, T1) + Tl;
- Vec3f T2_transformed = MxV(R, T2) + Tl;
- Vec3f T3_transformed = MxV(R, T3) + Tl;
- return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q);
-}
-
-}
diff --git a/branches/point_cloud/fcl/src/interval_tree.cpp b/branches/point_cloud/fcl/src/interval_tree.cpp
deleted file mode 100644
index 5a106b50f646c301912056f2a59313859c2888d6..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/src/interval_tree.cpp
+++ /dev/null
@@ -1,528 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-#include "fcl/interval_tree.h"
-#include <iostream>
-#include <cstdlib>
-
-
-namespace fcl
-{
-
-IntervalTreeNode::IntervalTreeNode(){}
-
-IntervalTreeNode::IntervalTreeNode(Interval* new_interval) :
- stored_interval (new_interval),
- key(new_interval->low),
- high(new_interval->high),
- max_high(high) {}
-
-IntervalTreeNode::~IntervalTreeNode(){}
-
-
-IntervalTree::IntervalTree()
-{
- nil = new IntervalTreeNode;
- nil->left = nil->right = nil->parent = nil;
- nil->red = false;
- nil->key = nil->high = nil->max_high = -std::numeric_limits<double>::max();
- nil->stored_interval = NULL;
-
- root = new IntervalTreeNode;
- root->parent = root->left = root->right = nil;
- root->key = root->high = root->max_high = std::numeric_limits<double>::max();
- root->red = false;
- root->stored_interval = NULL;
-
- /* the following are used for the query function */
- recursion_node_stack_size = 128;
- recursion_node_stack = (it_recursion_node*)malloc(recursion_node_stack_size*sizeof(it_recursion_node));
- recursion_node_stack_top = 1;
- recursion_node_stack[0].start_node = NULL;
-}
-
-
-void IntervalTree::leftRotate(IntervalTreeNode* x)
-{
- IntervalTreeNode* y;
-
- y = x->right;
- x->right = y->left;
-
- if(y->left != nil) y->left->parent = x;
-
- y->parent = x->parent;
-
- if(x == x->parent->left)
- x->parent->left = y;
- else
- x->parent->right = y;
-
- y->left = x;
- x->parent = y;
-
- x->max_high = std::max(x->left->max_high, std::max(x->right->max_high, x->high));
- y->max_high = std::max(x->max_high, std::max(y->right->max_high, y->high));
-}
-
-
-void IntervalTree::rightRotate(IntervalTreeNode* y)
-{
- IntervalTreeNode* x;
-
- x = y->left;
- y->left = x->right;
-
- if(nil != x->right) x->right->parent = y;
-
- x->parent = y->parent;
- if(y == y->parent->left)
- y->parent->left = x;
- else
- y->parent->right = x;
-
- x->right = y;
- y->parent = x;
-
- y->max_high = std::max(y->left->max_high, std::max(y->right->max_high, y->high));
- x->max_high = std::max(x->left->max_high, std::max(y->max_high, x->high));
-}
-
-
-
-/** \brief Inserts z into the tree as if it were a regular binary tree */
-void IntervalTree::recursiveInsert(IntervalTreeNode* z)
-{
- IntervalTreeNode* x;
- IntervalTreeNode* y;
-
- z->left = z->right = nil;
- y = root;
- x = root->left;
- while(x != nil)
- {
- y = x;
- if(x->key > z->key)
- x = x->left;
- else
- x = x->right;
- }
- z->parent = y;
- if((y == root) || (y->key > z->key))
- y->left = z;
- else
- y->right = z;
-}
-
-
-void IntervalTree::fixupMaxHigh(IntervalTreeNode* x)
-{
- while(x != root)
- {
- x->max_high = std::max(x->high, std::max(x->left->max_high, x->right->max_high));
- x = x->parent;
- }
-}
-
-IntervalTreeNode* IntervalTree::insert(Interval* new_interval)
-{
- IntervalTreeNode* y;
- IntervalTreeNode* x;
- IntervalTreeNode* new_node;
-
- x = new IntervalTreeNode(new_interval);
- recursiveInsert(x);
- fixupMaxHigh(x->parent);
- new_node = x;
- x->red = true;
- while(x->parent->red)
- {
- /* use sentinel instead of checking for root */
- if(x->parent == x->parent->parent->left)
- {
- y = x->parent->parent->right;
- if(y->red)
- {
- x->parent->red = true;
- y->red = true;
- x->parent->parent->red = true;
- x = x->parent->parent;
- }
- else
- {
- if(x == x->parent->right)
- {
- x = x->parent;
- leftRotate(x);
- }
- x->parent->red = false;
- x->parent->parent->red = true;
- rightRotate(x->parent->parent);
- }
- }
- else
- {
- y = x->parent->parent->left;
- if(y->red)
- {
- x->parent->red = false;
- y->red = false;
- x->parent->parent->red = true;
- x = x->parent->parent;
- }
- else
- {
- if(x == x->parent->left)
- {
- x = x->parent;
- rightRotate(x);
- }
- x->parent->red = false;
- x->parent->parent->red = true;
- leftRotate(x->parent->parent);
- }
- }
- }
- root->left->red = false;
- return new_node;
-}
-
-IntervalTreeNode* IntervalTree::getSuccessor(IntervalTreeNode* x) const
-{
- IntervalTreeNode* y;
-
- if(nil != (y = x->right))
- {
- while(y->left != nil)
- y = y->left;
- return y;
- }
- else
- {
- y = x->parent;
- while(x == y->right)
- {
- x = y;
- y = y->parent;
- }
- if(y == root) return nil;
- return y;
- }
-}
-
-
-IntervalTreeNode* IntervalTree::getPredecessor(IntervalTreeNode* x) const
-{
- IntervalTreeNode* y;
-
- if(nil != (y = x->left))
- {
- while(y->right != nil)
- y = y->right;
- return y;
- }
- else
- {
- y = x->parent;
- while(x == y->left)
- {
- if(y == root) return nil;
- x = y;
- y = y->parent;
- }
- return y;
- }
-}
-
-void IntervalTreeNode::print(IntervalTreeNode* nil, IntervalTreeNode* root) const
-{
- stored_interval->print();
- std::cout << ", k = " << key << ", h = " << high << ", mH = " << max_high;
- std::cout << " l->key = ";
- if(left == nil) std::cout << "NULL"; else std::cout << left->key;
- std::cout << " r->key = ";
- if(right == nil) std::cout << "NULL"; else std::cout << right->key;
- std::cout << " p->key = ";
- if(parent == root) std::cout << "NULL"; else std::cout << parent->key;
- std::cout << " red = " << (int)red << std::endl;
-}
-
-void IntervalTree::recursivePrint(IntervalTreeNode* x) const
-{
- if(x != nil)
- {
- recursivePrint(x->left);
- x->print(nil,root);
- recursivePrint(x->right);
- }
-}
-
-IntervalTree::~IntervalTree()
-{
- IntervalTreeNode* x = root->left;
- std::deque<IntervalTreeNode*> nodes_to_free;
-
- if(x != nil)
- {
- if(x->left != nil)
- {
- nodes_to_free.push_back(x->left);
- }
- if(x->right != nil)
- {
- nodes_to_free.push_back(x->right);
- }
-
- delete x;
- while( nodes_to_free.size() > 0)
- {
- x = nodes_to_free.back();
- nodes_to_free.pop_back();
- if(x->left != nil)
- {
- nodes_to_free.push_back(x->left);
- }
- if(x->right != nil)
- {
- nodes_to_free.push_back(x->right);
- }
- delete x;
- }
- }
- delete nil;
- delete root;
- free(recursion_node_stack);
-}
-
-
-void IntervalTree::print() const
-{
- recursivePrint(root->left);
-}
-
-void IntervalTree::deleteFixup(IntervalTreeNode* x)
-{
- IntervalTreeNode* w;
- IntervalTreeNode* root_left_node = root->left;
-
- while((!x->red) && (root_left_node != x))
- {
- if(x == x->parent->left)
- {
- w = x->parent->right;
- if(w->red)
- {
- w->red = false;
- x->parent->red = true;
- leftRotate(x->parent);
- w = x->parent->right;
- }
- if((!w->right->red) && (!w->left->red))
- {
- w->red = true;
- x = x->parent;
- }
- else
- {
- if(!w->right->red)
- {
- w->left->red = false;
- w->red = true;
- rightRotate(w);
- w = x->parent->right;
- }
- w->red = x->parent->red;
- x->parent->red = false;
- w->right->red = false;
- leftRotate(x->parent);
- x = root_left_node;
- }
- }
- else
- {
- w = x->parent->left;
- if(w->red)
- {
- w->red = false;
- x->parent->red = true;
- rightRotate(x->parent);
- w = x->parent->left;
- }
- if((!w->right->red) && (!w->left->red))
- {
- w->red = true;
- x = x->parent;
- }
- else
- {
- if(!w->left->red)
- {
- w->right->red = false;
- w->red = true;
- leftRotate(w);
- w = x->parent->left;
- }
- w->red = x->parent->red;
- x->parent->red = false;
- w->left->red = false;
- rightRotate(x->parent);
- x = root_left_node;
- }
- }
- }
- x->red = false;
-}
-
-Interval* IntervalTree::deleteNode(IntervalTreeNode* z)
-{
- IntervalTreeNode* y;
- IntervalTreeNode* x;
- Interval* node_to_delete = z->stored_interval;
-
- y= ((z->left == nil) || (z->right == nil)) ? z : getSuccessor(z);
- x= (y->left == nil) ? y->right : y->left;
- if(root == (x->parent = y->parent))
- {
- root->left = x;
- }
- else
- {
- if(y == y->parent->left)
- {
- y->parent->left = x;
- }
- else
- {
- y->parent->right = x;
- }
- }
-
- /* y should not be nil in this case */
- /* y is the node to splice out and x is its child */
- if(y != z)
- {
- y->max_high = -std::numeric_limits<double>::max();
- y->left = z->left;
- y->right = z->right;
- y->parent = z->parent;
- z->left->parent = z->right->parent = y;
- if(z == z->parent->left)
- z->parent->left = y;
- else
- z->parent->right = y;
-
- fixupMaxHigh(x->parent);
- if(!(y->red))
- {
- y->red = z->red;
- deleteFixup(x);
- }
- else
- y->red = z->red;
- delete z;
- }
- else
- {
- fixupMaxHigh(x->parent);
- if(!(y->red)) deleteFixup(x);
- delete y;
- }
-
- return node_to_delete;
-}
-
-/** \brief returns 1 if the intervals overlap, and 0 otherwise */
-bool overlap(double a1, double a2, double b1, double b2)
-{
- if(a1 <= b1)
- {
- return (b1 <= a2);
- }
- else
- {
- return (a1 <= b2);
- }
-}
-
-std::deque<Interval*> IntervalTree::query(double low, double high)
-{
- std::deque<Interval*> result_stack;
- IntervalTreeNode* x = root->left;
- bool run = (x != nil);
-
- current_parent = 0;
-
- while(run)
- {
- if(overlap(low,high,x->key,x->high))
- {
- result_stack.push_back(x->stored_interval);
- recursion_node_stack[current_parent].try_right_branch = true;
- }
- if(x->left->max_high >= low)
- {
- if(recursion_node_stack_top == recursion_node_stack_size)
- {
- recursion_node_stack_size *= 2;
- recursion_node_stack = (it_recursion_node *)realloc(recursion_node_stack, recursion_node_stack_size * sizeof(it_recursion_node));
- if(recursion_node_stack == NULL)
- exit(1);
- }
- recursion_node_stack[recursion_node_stack_top].start_node = x;
- recursion_node_stack[recursion_node_stack_top].try_right_branch = false;
- recursion_node_stack[recursion_node_stack_top].parent_index = current_parent;
- current_parent = recursion_node_stack_top++;
- x = x->left;
- }
- else
- x = x->right;
-
- run = (x != nil);
- while((!run) && (recursion_node_stack_top > 1))
- {
- if(recursion_node_stack[--recursion_node_stack_top].try_right_branch)
- {
- x=recursion_node_stack[recursion_node_stack_top].start_node->right;
- current_parent=recursion_node_stack[recursion_node_stack_top].parent_index;
- recursion_node_stack[current_parent].try_right_branch = true;
- run = (x != nil);
- }
- }
- }
- return result_stack;
-}
-
-}
diff --git a/branches/point_cloud/fcl/src/motion.cpp b/branches/point_cloud/fcl/src/motion.cpp
deleted file mode 100644
index 3f2c95680fdedfd89d047d49afdc595f6ae0eb2a..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/src/motion.cpp
+++ /dev/null
@@ -1,64 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#include "fcl/motion.h"
-
-namespace fcl
-{
-
-template<>
-BVH_REAL InterpMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const
-{
- BVH_REAL c_proj_max = (bv.Tr.cross(angular_axis)).sqrLength();
- BVH_REAL tmp;
- tmp = ((bv.Tr + bv.axis[0] * bv.l[0]).cross(angular_axis)).sqrLength();
- if(tmp > c_proj_max) c_proj_max = tmp;
- tmp = ((bv.Tr + bv.axis[1] * bv.l[1]).cross(angular_axis)).sqrLength();
- if(tmp > c_proj_max) c_proj_max = tmp;
- tmp = ((bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1]).cross(angular_axis)).sqrLength();
- if(tmp > c_proj_max) c_proj_max = tmp;
-
- c_proj_max = sqrt(c_proj_max);
-
- BVH_REAL v_dot_n = linear_vel.dot(n);
- BVH_REAL w_cross_n = (angular_axis.cross(n)).length() * angular_vel;
- BVH_REAL mu = v_dot_n + w_cross_n * (bv.r + c_proj_max);
-
- return mu;
-}
-
-}
diff --git a/branches/point_cloud/fcl/src/simple_setup.cpp b/branches/point_cloud/fcl/src/simple_setup.cpp
deleted file mode 100644
index 83d5d8800ff7547afdf6998bb206251ce97904cd..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/src/simple_setup.cpp
+++ /dev/null
@@ -1,269 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#include "fcl/simple_setup.h"
-
-namespace fcl
-{
-
-bool initialize(MeshCollisionTraversalNodeOBB& node, const BVHModel<OBB>& model1, const BVHModel<OBB>& model2,
- const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2, int num_max_contacts, bool exhaustive, bool enable_contact)
-{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices1 = model1.tri_indices;
- node.tri_indices2 = model2.tri_indices;
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
-
- relativeTransform(R1, T1, R2, T2, node.R, node.T);
-
- return true;
-}
-
-
-bool initialize(MeshCollisionTraversalNodeRSS& node, const BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
- const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2, int num_max_contacts, bool exhaustive, bool enable_contact)
-{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices1 = model1.tri_indices;
- node.tri_indices2 = model2.tri_indices;
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
-
- relativeTransform(R1, T1, R2, T2, node.R, node.T);
-
- return true;
-}
-
-#if USE_SVMLIGHT
-
-bool initialize(PointCloudCollisionTraversalNodeOBB& node, BVHModel<OBB>& model1, BVHModel<OBB>& model2,
- const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
- BVH_REAL collision_prob_threshold,
- int leaf_size_threshold,
- int num_max_contacts,
- bool exhaustive,
- bool enable_contact,
- BVH_REAL expand_r)
-{
- if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD)
- || !(model2.getModelType() == BVH_MODEL_TRIANGLES || model2.getModelType() == BVH_MODEL_POINTCLOUD))
- return false;
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.uc1.reset(new Uncertainty[model1.num_vertices]);
- node.uc2.reset(new Uncertainty[model2.num_vertices]);
-
- estimateSamplingUncertainty(model1.vertices, model1.num_vertices, node.uc1.get());
- estimateSamplingUncertainty(model2.vertices, model2.num_vertices, node.uc2.get());
-
- BVHExpand(model1, node.uc1.get(), expand_r);
- BVHExpand(model2, node.uc2.get(), expand_r);
-
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
- node.collision_prob_threshold = collision_prob_threshold;
- node.leaf_size_threshold = leaf_size_threshold;
-
- relativeTransform(R1, T1, R2, T2, node.R, node.T);
-
- return true;
-}
-
-
-bool initialize(PointCloudCollisionTraversalNodeRSS& node, BVHModel<RSS>& model1, BVHModel<RSS>& model2,
- const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
- BVH_REAL collision_prob_threshold,
- int leaf_size_threshold,
- int num_max_contacts,
- bool exhaustive,
- bool enable_contact,
- BVH_REAL expand_r)
-{
- if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD)
- || !(model2.getModelType() == BVH_MODEL_TRIANGLES || model2.getModelType() == BVH_MODEL_POINTCLOUD))
- return false;
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.uc1.reset(new Uncertainty[model1.num_vertices]);
- node.uc2.reset(new Uncertainty[model2.num_vertices]);
-
- estimateSamplingUncertainty(model1.vertices, model1.num_vertices, node.uc1.get());
- estimateSamplingUncertainty(model2.vertices, model2.num_vertices, node.uc2.get());
-
- BVHExpand(model1, node.uc1.get(), expand_r);
- BVHExpand(model2, node.uc2.get(), expand_r);
-
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
- node.collision_prob_threshold = collision_prob_threshold;
- node.leaf_size_threshold = leaf_size_threshold;
-
- relativeTransform(R1, T1, R2, T2, node.R, node.T);
-
- return true;
-}
-
-bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node, BVHModel<OBB>& model1, const BVHModel<OBB>& model2,
- const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
- BVH_REAL collision_prob_threshold,
- int leaf_size_threshold,
- int num_max_contacts,
- bool exhaustive,
- bool enable_contact,
- BVH_REAL expand_r)
-{
- if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD) || model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices2 = model2.tri_indices;
- node.uc1.reset(new Uncertainty[model1.num_vertices]);
-
- estimateSamplingUncertainty(model1.vertices, model1.num_vertices, node.uc1.get());
-
- BVHExpand(model1, node.uc1.get(), expand_r);
-
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
- node.collision_prob_threshold = collision_prob_threshold;
- node.leaf_size_threshold = leaf_size_threshold;
-
- relativeTransform(R1, T1, R2, T2, node.R, node.T);
-
- return true;
-}
-
-
-bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node, BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
- const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
- BVH_REAL collision_prob_threshold,
- int leaf_size_threshold,
- int num_max_contacts,
- bool exhaustive,
- bool enable_contact,
- BVH_REAL expand_r)
-{
- if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD) || model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices2 = model2.tri_indices;
- node.uc1.reset(new Uncertainty[model1.num_vertices]);
-
- estimateSamplingUncertainty(model1.vertices, model1.num_vertices, node.uc1.get());
-
- BVHExpand(model1, node.uc1.get(), expand_r);
-
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
- node.collision_prob_threshold = collision_prob_threshold;
- node.leaf_size_threshold = leaf_size_threshold;
-
- relativeTransform(R1, T1, R2, T2, node.R, node.T);
-
- return true;
-}
-
-#endif
-
-bool initialize(MeshDistanceTraversalNodeRSS& node, const BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
- const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2)
-{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices1 = model1.tri_indices;
- node.tri_indices2 = model2.tri_indices;
-
- relativeTransform(R1, T1, R2, T2, node.R, node.T);
-
- return true;
-}
-
-
-}
diff --git a/branches/point_cloud/fcl/src/transform.cpp b/branches/point_cloud/fcl/src/transform.cpp
deleted file mode 100644
index e43e513504493feaa5384726e66dba6fcc5dc664..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/src/transform.cpp
+++ /dev/null
@@ -1,263 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#include "fcl/transform.h"
-
-namespace fcl
-{
-
-void SimpleQuaternion::fromRotation(const Vec3f R[3])
-{
- const int next[3] = {1, 2, 0};
-
- BVH_REAL trace = R[0][0] + R[1][1] + R[2][2];
- BVH_REAL root;
-
- if(trace > 0.0)
- {
- // |w| > 1/2, may as well choose w > 1/2
- root = sqrt(trace + 1.0); // 2w
- data[0] = 0.5 * root;
- root = 0.5 / root; // 1/(4w)
- data[1] = (R[2][1] - R[1][2])*root;
- data[2] = (R[0][2] - R[2][0])*root;
- data[3] = (R[1][0] - R[0][1])*root;
- }
- else
- {
- // |w| <= 1/2
- int i = 0;
- if(R[1][1] > R[0][0])
- {
- i = 1;
- }
- if(R[2][2] > R[i][i])
- {
- i = 2;
- }
- int j = next[i];
- int k = next[j];
-
- root = sqrt(R[i][i] - R[j][j] - R[k][k] + 1.0);
- BVH_REAL* quat[3] = { &data[1], &data[2], &data[3] };
- *quat[i] = 0.5 * root;
- root = 0.5 / root;
- data[0] = (R[k][j] - R[j][k]) * root;
- *quat[j] = (R[j][i] + R[i][j]) * root;
- *quat[k] = (R[k][i] + R[i][k]) * root;
- }
-}
-
-void SimpleQuaternion::toRotation(Vec3f R[3]) const
-{
- BVH_REAL twoX = 2.0*data[1];
- BVH_REAL twoY = 2.0*data[2];
- BVH_REAL twoZ = 2.0*data[3];
- BVH_REAL twoWX = twoX*data[0];
- BVH_REAL twoWY = twoY*data[0];
- BVH_REAL twoWZ = twoZ*data[0];
- BVH_REAL twoXX = twoX*data[1];
- BVH_REAL twoXY = twoY*data[1];
- BVH_REAL twoXZ = twoZ*data[1];
- BVH_REAL twoYY = twoY*data[2];
- BVH_REAL twoYZ = twoZ*data[2];
- BVH_REAL twoZZ = twoZ*data[3];
-
- R[0] = Vec3f(1.0 - (twoYY + twoZZ), twoXY - twoWZ, twoXZ + twoWY);
- R[1] = Vec3f(twoXY + twoWZ, 1.0 - (twoXX + twoZZ), twoYZ - twoWX);
- R[2] = Vec3f(twoXZ - twoWY, twoYZ + twoWX, 1.0 - (twoXX + twoYY));
-}
-
-
-void SimpleQuaternion::fromAxes(const Vec3f axis[3])
-{
- // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
- // article "Quaternion Calculus and Fast Animation".
-
- const int next[3] = {1, 2, 0};
-
- BVH_REAL trace = axis[0][0] + axis[1][1] + axis[2][2];
- BVH_REAL root;
-
- if(trace > 0.0)
- {
- // |w| > 1/2, may as well choose w > 1/2
- root = sqrt(trace + 1.0); // 2w
- data[0] = 0.5 * root;
- root = 0.5 / root; // 1/(4w)
- data[1] = (axis[1][2] - axis[2][1])*root;
- data[2] = (axis[2][0] - axis[0][2])*root;
- data[3] = (axis[0][1] - axis[1][0])*root;
- }
- else
- {
- // |w| <= 1/2
- int i = 0;
- if(axis[1][1] > axis[0][0])
- {
- i = 1;
- }
- if(axis[2][2] > axis[i][i])
- {
- i = 2;
- }
- int j = next[i];
- int k = next[j];
-
- root = sqrt(axis[i][i] - axis[j][j] - axis[k][k] + 1.0);
- BVH_REAL* quat[3] = { &data[1], &data[2], &data[3] };
- *quat[i] = 0.5 * root;
- root = 0.5 / root;
- data[0] = (axis[j][k] - axis[k][j]) * root;
- *quat[j] = (axis[i][j] + axis[j][i]) * root;
- *quat[k] = (axis[i][k] + axis[k][i]) * root;
- }
-}
-
-void SimpleQuaternion::toAxes(Vec3f axis[3]) const
-{
- BVH_REAL twoX = 2.0*data[1];
- BVH_REAL twoY = 2.0*data[2];
- BVH_REAL twoZ = 2.0*data[3];
- BVH_REAL twoWX = twoX*data[0];
- BVH_REAL twoWY = twoY*data[0];
- BVH_REAL twoWZ = twoZ*data[0];
- BVH_REAL twoXX = twoX*data[1];
- BVH_REAL twoXY = twoY*data[1];
- BVH_REAL twoXZ = twoZ*data[1];
- BVH_REAL twoYY = twoY*data[2];
- BVH_REAL twoYZ = twoZ*data[2];
- BVH_REAL twoZZ = twoZ*data[3];
-
- axis[0] = Vec3f(1.0 - (twoYY + twoZZ), twoXY + twoWZ, twoXZ - twoWY);
- axis[1] = Vec3f(twoXY - twoWZ, 1.0 - (twoXX + twoZZ), twoYZ + twoWX);
- axis[2] = Vec3f(twoXZ + twoWY, twoYZ - twoWX, 1.0 - (twoXX + twoYY));
-}
-
-
-void SimpleQuaternion::fromAxisAngle(const Vec3f& axis, BVH_REAL angle)
-{
- BVH_REAL half_angle = 0.5 * angle;
- BVH_REAL sn = sin((double)half_angle);
- data[0] = cos((double)half_angle);
- data[1] = sn * axis[0];
- data[2] = sn * axis[1];
- data[3] = sn * axis[2];
-}
-
-void SimpleQuaternion::toAxisAngle(Vec3f& axis, BVH_REAL& angle) const
-{
- double sqr_length = data[1] * data[1] + data[2] * data[2] + data[3] * data[3];
- if(sqr_length > 0)
- {
- angle = 2.0 * acos((double)data[0]);
- double inv_length = 1.0 / sqrt(sqr_length);
- axis[0] = inv_length * data[1];
- axis[1] = inv_length * data[2];
- axis[2] = inv_length * data[3];
- }
- else
- {
- angle = 0;
- axis[0] = 1;
- axis[1] = 0;
- axis[2] = 0;
- }
-}
-
-BVH_REAL SimpleQuaternion::dot(const SimpleQuaternion& other) const
-{
- return data[0] * other.data[0] + data[1] * other.data[1] + data[2] * other.data[2] + data[3] * other.data[3];
-}
-
-SimpleQuaternion SimpleQuaternion::operator + (const SimpleQuaternion& other) const
-{
- return SimpleQuaternion(data[0] + other.data[0], data[1] + other.data[1],
- data[2] + other.data[2], data[3] + other.data[3]);
-}
-
-SimpleQuaternion SimpleQuaternion::operator - (const SimpleQuaternion& other) const
-{
- return SimpleQuaternion(data[0] - other.data[0], data[1] - other.data[1],
- data[2] - other.data[2], data[3] - other.data[3]);
-}
-
-SimpleQuaternion SimpleQuaternion::operator * (const SimpleQuaternion& other) const
-{
- return SimpleQuaternion(data[0] * other.data[0] - data[1] * other.data[1] - data[2] * other.data[2] - data[3] * other.data[3],
- data[0] * other.data[1] + data[1] * other.data[0] + data[2] * other.data[3] - data[3] * other.data[2],
- data[0] * other.data[2] - data[1] * other.data[3] + data[2] * other.data[0] + data[3] * other.data[1],
- data[0] * other.data[3] + data[1] * other.data[2] - data[2] * other.data[1] + data[3] * other.data[0]);
-}
-
-SimpleQuaternion SimpleQuaternion::operator - () const
-{
- return SimpleQuaternion(-data[0], -data[1], -data[2], -data[3]);
-}
-
-SimpleQuaternion SimpleQuaternion::operator * (BVH_REAL t) const
-{
- return SimpleQuaternion(data[0] * t, data[1] * t, data[2] * t, data[3] * t);
-}
-
-SimpleQuaternion SimpleQuaternion::conj() const
-{
- return SimpleQuaternion(data[0], -data[1], -data[2], -data[3]);
-}
-
-SimpleQuaternion SimpleQuaternion::inverse() const
-{
- double sqr_length = data[0] * data[0] + data[1] * data[1] + data[2] * data[2] + data[3] * data[3];
- if(sqr_length > 0)
- {
- double inv_length = 1.0 / sqrt(sqr_length);
- return SimpleQuaternion(data[0] * inv_length, -data[1] * inv_length, -data[2] * inv_length, -data[3] * inv_length);
- }
- else
- {
- return SimpleQuaternion(data[0], -data[1], -data[2], -data[3]);
- }
-}
-
-Vec3f SimpleQuaternion::transform(const Vec3f& v) const
-{
- SimpleQuaternion r = (*this) * SimpleQuaternion(0, v[0], v[1], v[2]) * (this->conj());
- return Vec3f(r.data[1], r.data[2], r.data[3]);
-}
-
-
-}
diff --git a/branches/point_cloud/fcl/src/traversal_node_base.cpp b/branches/point_cloud/fcl/src/traversal_node_base.cpp
deleted file mode 100644
index 1f8a39391538176725a6ec96cf093b28d5f1b2d5..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/src/traversal_node_base.cpp
+++ /dev/null
@@ -1,120 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#include "fcl/traversal_node_base.h"
-#include <limits>
-
-namespace fcl
-{
-
-TraversalNodeBase::~TraversalNodeBase()
-{
-}
-
-bool TraversalNodeBase::isFirstNodeLeaf(int b) const
-{
- return true;
-}
-
-bool TraversalNodeBase::isSecondNodeLeaf(int b) const
-{
- return true;
-}
-
-bool TraversalNodeBase::firstOverSecond(int b1, int b2) const
-{
- return true;
-}
-
-int TraversalNodeBase::getFirstLeftChild(int b) const
-{
- return b;
-}
-
-int TraversalNodeBase::getFirstRightChild(int b) const
-{
- return b;
-}
-
-int TraversalNodeBase::getSecondLeftChild(int b) const
-{
- return b;
-}
-
-int TraversalNodeBase::getSecondRightChild(int b) const
-{
- return b;
-}
-
-CollisionTraversalNodeBase::~CollisionTraversalNodeBase()
-{
-}
-
-bool CollisionTraversalNodeBase::BVTesting(int b1, int b2) const
-{
- return true;
-}
-
-void CollisionTraversalNodeBase::leafTesting(int b1, int b2) const
-{
-}
-
-bool CollisionTraversalNodeBase::canStop() const
-{
- return false;
-}
-
-
-DistanceTraversalNodeBase::~DistanceTraversalNodeBase()
-{
-}
-
-BVH_REAL DistanceTraversalNodeBase::BVTesting(int b1, int b2) const
-{
- return std::numeric_limits<BVH_REAL>::max();
-}
-
-void DistanceTraversalNodeBase::leafTesting(int b1, int b2) const
-{
-}
-
-bool DistanceTraversalNodeBase::canStop(BVH_REAL c) const
-{
- return false;
-}
-
-}
diff --git a/branches/point_cloud/fcl/src/traversal_node_bvhs.cpp b/branches/point_cloud/fcl/src/traversal_node_bvhs.cpp
deleted file mode 100644
index bdfb6caa8a5729eb2b2db6d473c6e2d9aff3c877..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/src/traversal_node_bvhs.cpp
+++ /dev/null
@@ -1,643 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#include "fcl/traversal_node_bvhs.h"
-
-namespace fcl
-{
-
-MeshCollisionTraversalNodeOBB::MeshCollisionTraversalNodeOBB() : MeshCollisionTraversalNode<OBB>()
-{
- R[0] = Vec3f(1, 0, 0);
- R[1] = Vec3f(0, 1, 0);
- R[2] = Vec3f(0, 0, 1);
-
- // default T is 0
-}
-
-bool MeshCollisionTraversalNodeOBB::BVTesting(int b1, int b2) const
-{
- if(enable_statistics) num_bv_tests++;
- return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
-}
-
-void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const
-{
- if(enable_statistics) num_leaf_tests++;
-
- const BVNode<OBB>& node1 = model1->getBV(b1);
- const BVNode<OBB>& node2 = model2->getBV(b2);
-
- int primitive_id1 = node1.primitiveId();
- int primitive_id2 = node2.primitiveId();
-
- const Triangle& tri_id1 = tri_indices1[primitive_id1];
- const Triangle& tri_id2 = tri_indices2[primitive_id2];
-
- const Vec3f& p1 = vertices1[tri_id1[0]];
- const Vec3f& p2 = vertices1[tri_id1[1]];
- const Vec3f& p3 = vertices1[tri_id1[2]];
- const Vec3f& q1 = vertices2[tri_id2[0]];
- const Vec3f& q2 = vertices2[tri_id2[1]];
- const Vec3f& q3 = vertices2[tri_id2[2]];
-
- BVH_REAL penetration;
- Vec3f normal;
- int n_contacts;
- Vec3f contacts[2];
-
-
- if(!enable_contact) // only interested in collision or not
- {
- if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, R, T))
- pairs.push_back(BVHCollisionPair(primitive_id1, primitive_id2));
- }
- else // need compute the contact information
- {
- if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3,
- R, T,
- contacts,
- (unsigned int*)&n_contacts,
- &penetration,
- &normal))
- {
- for(int i = 0; i < n_contacts; ++i)
- {
- if((!exhaustive) && (num_max_contacts <= (int)pairs.size())) break;
- pairs.push_back(BVHCollisionPair(primitive_id1, primitive_id2, contacts[i], normal, penetration));
- }
- }
- }
-}
-
-
-MeshCollisionTraversalNodeRSS::MeshCollisionTraversalNodeRSS() : MeshCollisionTraversalNode<RSS>()
-{
- R[0] = Vec3f(1, 0, 0);
- R[1] = Vec3f(0, 1, 0);
- R[2] = Vec3f(0, 0, 1);
-
- // default T is 0
-}
-
-bool MeshCollisionTraversalNodeRSS::BVTesting(int b1, int b2) const
-{
- if(enable_statistics) num_bv_tests++;
- return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
-}
-
-void MeshCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const
-{
- if(enable_statistics) num_leaf_tests++;
-
- const BVNode<RSS>& node1 = model1->getBV(b1);
- const BVNode<RSS>& node2 = model2->getBV(b2);
-
- int primitive_id1 = node1.primitiveId();
- int primitive_id2 = node2.primitiveId();
-
- const Triangle& tri_id1 = tri_indices1[primitive_id1];
- const Triangle& tri_id2 = tri_indices2[primitive_id2];
-
- const Vec3f& p1 = vertices1[tri_id1[0]];
- const Vec3f& p2 = vertices1[tri_id1[1]];
- const Vec3f& p3 = vertices1[tri_id1[2]];
- const Vec3f& q1 = vertices2[tri_id2[0]];
- const Vec3f& q2 = vertices2[tri_id2[1]];
- const Vec3f& q3 = vertices2[tri_id2[2]];
-
- BVH_REAL penetration;
- Vec3f normal;
- int n_contacts;
- Vec3f contacts[2];
-
-
- if(!enable_contact) // only interested in collision or not
- {
- if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, R, T))
- pairs.push_back(BVHCollisionPair(primitive_id1, primitive_id2));
- }
- else // need compute the contact information
- {
- if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3,
- R, T,
- contacts,
- (unsigned int*)&n_contacts,
- &penetration,
- &normal))
- {
- for(int i = 0; i < n_contacts; ++i)
- {
- if((!exhaustive) && (num_max_contacts <= (int)pairs.size())) break;
- pairs.push_back(BVHCollisionPair(primitive_id1, primitive_id2, contacts[i], normal, penetration));
- }
- }
- }
-}
-
-#if USE_SVMLIGHT
-
-PointCloudCollisionTraversalNodeOBB::PointCloudCollisionTraversalNodeOBB() : PointCloudCollisionTraversalNode<OBB>()
-{
- R[0] = Vec3f(1, 0, 0);
- R[1] = Vec3f(0, 1, 0);
- R[2] = Vec3f(0, 0, 1);
-
- // default T is 0
-}
-
-bool PointCloudCollisionTraversalNodeOBB::BVTesting(int b1, int b2) const
-{
- if(enable_statistics) num_bv_tests++;
- return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
-}
-
-void PointCloudCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const
-{
- if(enable_statistics) num_leaf_tests++;
-
- const BVNode<OBB>& node1 = model1->getBV(b1);
- const BVNode<OBB>& node2 = model2->getBV(b2);
-
- BVH_REAL collision_prob = Intersect::intersect_PointClouds(vertices1 + node1.first_primitive, uc1.get() + node1.first_primitive,
- node1.num_primitives,
- vertices2 + node2.first_primitive, uc2.get() + node2.first_primitive,
- node2.num_primitives,
- R, T,
- classifier_param);
-
-
- if(collision_prob > collision_prob_threshold)
- pairs.push_back(BVHPointCollisionPair(node1.first_primitive, node1.num_primitives, node2.first_primitive, node2.num_primitives, collision_prob));
-
-
- if(collision_prob > max_collision_prob)
- max_collision_prob = collision_prob;
-}
-
-PointCloudCollisionTraversalNodeRSS::PointCloudCollisionTraversalNodeRSS() : PointCloudCollisionTraversalNode<RSS>()
-{
- R[0] = Vec3f(1, 0, 0);
- R[1] = Vec3f(0, 1, 0);
- R[2] = Vec3f(0, 0, 1);
-
- // default T is 0
-}
-
-bool PointCloudCollisionTraversalNodeRSS::BVTesting(int b1, int b2) const
-{
- if(enable_statistics) num_bv_tests++;
- return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
-}
-
-void PointCloudCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const
-{
- if(enable_statistics) num_leaf_tests++;
-
- const BVNode<RSS>& node1 = model1->getBV(b1);
- const BVNode<RSS>& node2 = model2->getBV(b2);
-
- BVH_REAL collision_prob = Intersect::intersect_PointClouds(vertices1 + node1.first_primitive, uc1.get() + node1.first_primitive,
- node1.num_primitives,
- vertices2 + node2.first_primitive, uc2.get() + node2.first_primitive,
- node2.num_primitives,
- R, T,
- classifier_param);
-
-
- if(collision_prob > collision_prob_threshold)
- pairs.push_back(BVHPointCollisionPair(node1.first_primitive, node1.num_primitives, node2.first_primitive, node2.num_primitives, collision_prob));
-
- if(collision_prob > max_collision_prob)
- max_collision_prob = collision_prob;
-}
-
-
-PointCloudMeshCollisionTraversalNodeOBB::PointCloudMeshCollisionTraversalNodeOBB() : PointCloudMeshCollisionTraversalNode<OBB>()
-{
- R[0] = Vec3f(1, 0, 0);
- R[1] = Vec3f(0, 1, 0);
- R[2] = Vec3f(0, 0, 1);
-
- // default T is 0
-}
-
-bool PointCloudMeshCollisionTraversalNodeOBB::BVTesting(int b1, int b2) const
-{
- if(enable_statistics) num_bv_tests++;
- return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
-}
-
-void PointCloudMeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const
-{
- if(enable_statistics) num_leaf_tests++;
-
- const BVNode<OBB>& node1 = model1->getBV(b1);
- const BVNode<OBB>& node2 = model2->getBV(b2);
-
-
- const Triangle& tri_id2 = tri_indices2[node2.primitiveId()];
-
- const Vec3f& q1 = vertices2[tri_id2[0]];
- const Vec3f& q2 = vertices2[tri_id2[1]];
- const Vec3f& q3 = vertices2[tri_id2[2]];
-
- BVH_REAL collision_prob = Intersect::intersect_PointCloudsTriangle(vertices1 + node1.first_primitive, uc1.get() + node1.first_primitive,
- node1.num_primitives,
- q1, q2, q3,
- R, T);
-
- if(collision_prob > collision_prob_threshold)
- pairs.push_back(BVHPointCollisionPair(node1.first_primitive, node1.num_primitives, node2.first_primitive, node2.num_primitives, collision_prob));
-
- if(collision_prob > max_collision_prob)
- max_collision_prob = collision_prob;
-}
-
-PointCloudMeshCollisionTraversalNodeRSS::PointCloudMeshCollisionTraversalNodeRSS() : PointCloudMeshCollisionTraversalNode<RSS>()
-{
- R[0] = Vec3f(1, 0, 0);
- R[1] = Vec3f(0, 1, 0);
- R[2] = Vec3f(0, 0, 1);
-
- // default T is 0
-}
-
-bool PointCloudMeshCollisionTraversalNodeRSS::BVTesting(int b1, int b2) const
-{
- if(enable_statistics) num_bv_tests++;
- return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
-}
-
-void PointCloudMeshCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const
-{
- if(enable_statistics) num_leaf_tests++;
-
- const BVNode<RSS>& node1 = model1->getBV(b1);
- const BVNode<RSS>& node2 = model2->getBV(b2);
-
- const Triangle& tri_id2 = tri_indices2[node2.primitiveId()];
-
- const Vec3f& q1 = vertices2[tri_id2[0]];
- const Vec3f& q2 = vertices2[tri_id2[1]];
- const Vec3f& q3 = vertices2[tri_id2[2]];
-
- BVH_REAL collision_prob = Intersect::intersect_PointCloudsTriangle(vertices1 + node1.first_primitive, uc1.get() + node1.first_primitive,
- node1.num_primitives,
- q1, q2, q3,
- R, T);
-
- if(collision_prob > collision_prob_threshold)
- pairs.push_back(BVHPointCollisionPair(node1.first_primitive, node1.num_primitives, node2.first_primitive, node2.num_primitives, collision_prob));
-
- if(collision_prob > max_collision_prob)
- max_collision_prob = collision_prob;
-}
-
-#endif
-
-MeshDistanceTraversalNodeRSS::MeshDistanceTraversalNodeRSS() : MeshDistanceTraversalNode<RSS>()
-{
- R[0] = Vec3f(1, 0, 0);
- R[1] = Vec3f(0, 1, 0);
- R[2] = Vec3f(0, 0, 1);
-
- // default T is 0
-}
-
-BVH_REAL MeshDistanceTraversalNodeRSS::BVTesting(int b1, int b2) const
-{
- if(enable_statistics) num_bv_tests++;
- return distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
-}
-
-void MeshDistanceTraversalNodeRSS::leafTesting(int b1, int b2) const
-{
- if(enable_statistics) num_leaf_tests++;
-
- const BVNode<RSS>& node1 = model1->getBV(b1);
- const BVNode<RSS>& node2 = model2->getBV(b2);
-
- int primitive_id1 = node1.primitiveId();
- int primitive_id2 = node2.primitiveId();
-
- const Triangle& tri_id1 = tri_indices1[primitive_id1];
- const Triangle& tri_id2 = tri_indices2[primitive_id2];
-
- const Vec3f& t11 = vertices1[tri_id1[0]];
- const Vec3f& t12 = vertices1[tri_id1[1]];
- const Vec3f& t13 = vertices1[tri_id1[2]];
-
- const Vec3f& t21 = vertices2[tri_id2[0]];
- const Vec3f& t22 = vertices2[tri_id2[1]];
- const Vec3f& t23 = vertices2[tri_id2[2]];
-
- // nearest point pair
- Vec3f P1, P2;
-
- BVH_REAL d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23,
- R, T,
- P1, P2);
-
- if(d < min_distance)
- {
- min_distance = d;
-
- p1 = P1;
- p2 = P2;
-
- last_tri_id1 = primitive_id1;
- last_tri_id2 = primitive_id2;
- }
-}
-
-
-
-/** for OBB and RSS, there is local coordinate of BV, so normal need to be transformed */
-template<>
-bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(BVH_REAL c) const
-{
- if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
- {
- const StackData& data = stack.back();
- BVH_REAL d = data.d;
- Vec3f n;
- int c1, c2;
-
- if(d > c)
- {
- const StackData& data2 = stack[stack.size() - 2];
- d = data2.d;
- n = data2.P2 - data2.P1;
- c1 = data2.c1;
- c2 = data2.c2;
- stack[stack.size() - 2] = stack[stack.size() - 1];
- }
- else
- {
- n = data.P2 - data.P1;
- c1 = data.c1;
- c2 = data.c2;
- }
-
- if(c != d) std::cout << "there is some problem here" << std::endl;
-
- Vec3f n_transformed = model1->getBV(c1).bv.axis[0] * n[0] + model1->getBV(c1).bv.axis[1] * n[1] + model1->getBV(c1).bv.axis[2] * n[2];
-
- BVH_REAL bound1 = motion1->computeMotionBound(this->model1->getBV(c1).bv, n_transformed);
- BVH_REAL bound2 = motion2->computeMotionBound(this->model2->getBV(c2).bv, n_transformed);
-
- BVH_REAL cur_delta_t = c / (bound1 + bound2);
- if(cur_delta_t < delta_t)
- delta_t = cur_delta_t;
-
- stack.pop_back();
-
- return true;
- }
- else
- {
- const StackData& data = stack.back();
- BVH_REAL d = data.d;
-
- if(d > c)
- stack[stack.size() - 2] = stack[stack.size() - 1];
-
- stack.pop_back();
-
- return false;
- }
-}
-
-template<>
-bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(BVH_REAL c) const
-{
- if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
- {
- const StackData& data = stack.back();
- BVH_REAL d = data.d;
- Vec3f n;
- int c1, c2;
-
- if(d > c)
- {
- const StackData& data2 = stack[stack.size() - 2];
- d = data2.d;
- n = data2.P2 - data2.P1;
- c1 = data2.c1;
- c2 = data2.c2;
- stack[stack.size() - 2] = stack[stack.size() - 1];
- }
- else
- {
- n = data.P2 - data.P1;
- c1 = data.c1;
- c2 = data.c2;
- }
-
- if(c != d) std::cout << "there is some problem here" << std::endl;
-
- Vec3f n_transformed = model1->getBV(c1).bv.axis[0] * n[0] + model1->getBV(c1).bv.axis[1] * n[1] + model1->getBV(c1).bv.axis[2] * n[2];
-
- BVH_REAL bound1 = motion1->computeMotionBound(this->model1->getBV(c1).bv, n_transformed);
- BVH_REAL bound2 = motion2->computeMotionBound(this->model2->getBV(c2).bv, n_transformed);
-
- BVH_REAL cur_delta_t = c / (bound1 + bound2);
- if(cur_delta_t < delta_t)
- delta_t = cur_delta_t;
-
- stack.pop_back();
-
- return true;
- }
- else
- {
- const StackData& data = stack.back();
- BVH_REAL d = data.d;
-
- if(d > c)
- stack[stack.size() - 2] = stack[stack.size() - 1];
-
- stack.pop_back();
-
- return false;
- }
-}
-
-
-MeshConservativeAdvancementTraversalNodeRSS::MeshConservativeAdvancementTraversalNodeRSS(BVH_REAL w_) : MeshConservativeAdvancementTraversalNode<RSS>(w_)
-{
- R[0] = Vec3f(1, 0, 0);
- R[1] = Vec3f(0, 1, 0);
- R[2] = Vec3f(0, 0, 1);
-
- // default T is 0
-}
-
-BVH_REAL MeshConservativeAdvancementTraversalNodeRSS::BVTesting(int b1, int b2) const
-{
- if(enable_statistics) num_bv_tests++;
- Vec3f P1, P2;
- BVH_REAL d = distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv, &P1, &P2);
-
- stack.push_back(StackData(P1, P2, b1, b2, d));
-
- return d;
-}
-
-
-void MeshConservativeAdvancementTraversalNodeRSS::leafTesting(int b1, int b2) const
-{
- if(enable_statistics) num_leaf_tests++;
-
- const BVNode<RSS>& node1 = model1->getBV(b1);
- const BVNode<RSS>& node2 = model2->getBV(b2);
-
- int primitive_id1 = node1.primitiveId();
- int primitive_id2 = node2.primitiveId();
-
- const Triangle& tri_id1 = tri_indices1[primitive_id1];
- const Triangle& tri_id2 = tri_indices2[primitive_id2];
-
- const Vec3f& t11 = vertices1[tri_id1[0]];
- const Vec3f& t12 = vertices1[tri_id1[1]];
- const Vec3f& t13 = vertices1[tri_id1[2]];
-
- const Vec3f& t21 = vertices2[tri_id2[0]];
- const Vec3f& t22 = vertices2[tri_id2[1]];
- const Vec3f& t23 = vertices2[tri_id2[2]];
-
- // nearest point pair
- Vec3f P1, P2;
-
- BVH_REAL d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23,
- R, T,
- P1, P2);
-
- if(d < min_distance)
- {
- min_distance = d;
-
- p1 = P1;
- p2 = P2;
-
- last_tri_id1 = primitive_id1;
- last_tri_id2 = primitive_id2;
- }
-
-
- /** n is the local frame of object 1 */
- Vec3f n = P2 - P1;
- /** turn n into the global frame */
- Vec3f R[3];
- motion1->getCurrentRotation(R);
- Vec3f n_transformed = MxV(R, n);
- n_transformed.normalize();
- BVH_REAL bound1 = motion1->computeMotionBound(t11, t12, t13, n_transformed);
- BVH_REAL bound2 = motion2->computeMotionBound(t21, t22, t23, n_transformed);
-
- BVH_REAL cur_delta_t = d / (bound1 + bound2);
-
- if(cur_delta_t < delta_t)
- delta_t = cur_delta_t;
-}
-
-bool MeshConservativeAdvancementTraversalNodeRSS::canStop(BVH_REAL c) const
-{
- if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance))
- {
- const StackData& data = stack.back();
- BVH_REAL d = data.d;
- Vec3f n;
- int c1, c2;
-
- if(d > c)
- {
- const StackData& data2 = stack[stack.size() - 2];
- d = data2.d;
- n = data2.P2 - data2.P1;
- c1 = data2.c1;
- c2 = data2.c2;
- stack[stack.size() - 2] = stack[stack.size() - 1];
- }
- else
- {
- n = data.P2 - data.P1;
- c1 = data.c1;
- c2 = data.c2;
- }
-
- if(c != d) std::cout << "there is some problem here" << std::endl;
-
- /** n is in local frame of RSS c1 */
- /** turn n into the global frame */
- Vec3f n_transformed = model1->getBV(c1).bv.axis[0] * n[0] + model1->getBV(c1).bv.axis[1] * n[2] + model1->getBV(c1).bv.axis[2] * n[2];
- Vec3f R[3];
- motion1->getCurrentRotation(R);
- n_transformed = MxV(R, n_transformed);
- n_transformed.normalize();
-
- BVH_REAL bound1 = motion1->computeMotionBound(model1->getBV(c1).bv, n_transformed);
- BVH_REAL bound2 = motion2->computeMotionBound(model2->getBV(c2).bv, n_transformed);
-
- BVH_REAL cur_delta_t = c / (bound1 + bound2);
- if(cur_delta_t < delta_t)
- delta_t = cur_delta_t;
-
- stack.pop_back();
-
- return true;
- }
- else
- {
- const StackData& data = stack.back();
- BVH_REAL d = data.d;
-
- if(d > c)
- stack[stack.size() - 2] = stack[stack.size() - 1];
-
- stack.pop_back();
-
- return false;
- }
-}
-
-
-
-
-
-}
diff --git a/branches/point_cloud/fcl/src/traversal_recurse.cpp b/branches/point_cloud/fcl/src/traversal_recurse.cpp
deleted file mode 100644
index 8c0f2f7a2ddc18a535917e5f63383523d57a8376..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/src/traversal_recurse.cpp
+++ /dev/null
@@ -1,363 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#include "fcl/traversal_recurse.h"
-
-namespace fcl
-{
-void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list)
-{
- bool l1 = node->isFirstNodeLeaf(b1);
- bool l2 = node->isSecondNodeLeaf(b2);
-
- if(l1 && l2)
- {
- updateFrontList(front_list, b1, b2);
-
- if(node->BVTesting(b1, b2)) return;
-
- node->leafTesting(b1, b2);
- return;
- }
-
- if(node->BVTesting(b1, b2))
- {
- updateFrontList(front_list, b1, b2);
- return;
- }
-
- if(node->firstOverSecond(b1, b2))
- {
- int c1 = node->getFirstLeftChild(b1);
- int c2 = node->getFirstRightChild(b1);
-
- collisionRecurse(node, c1, b2, front_list);
-
- // early stop is disabled is front_list is used
- if(node->canStop() && !front_list) return;
-
- collisionRecurse(node, c2, b2, front_list);
- }
- else
- {
- int c1 = node->getSecondLeftChild(b2);
- int c2 = node->getSecondRightChild(b2);
-
- collisionRecurse(node, b1, c1, front_list);
-
- // early stop is disabled is front_list is used
- if(node->canStop() && !front_list) return;
-
- collisionRecurse(node, b1, c2, front_list);
- }
-}
-
-/** Recurse function for self collision
- * Make sure node is set correctly so that the first and second tree are the same
- */
-void selfCollisionRecurse(CollisionTraversalNodeBase* node, int b, BVHFrontList* front_list)
-{
- bool l = node->isFirstNodeLeaf(b);
-
- if(l) return;
-
- int c1 = node->getFirstLeftChild(b);
- int c2 = node->getFirstRightChild(b);
-
- selfCollisionRecurse(node, c1, front_list);
-
- selfCollisionRecurse(node, c2, front_list);
-
- collisionRecurse(node, c1, c2, front_list);
-}
-
-void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list)
-{
- bool l1 = node->isFirstNodeLeaf(b1);
- bool l2 = node->isSecondNodeLeaf(b2);
-
- if(l1 && l2)
- {
- updateFrontList(front_list, b1, b2);
-
- node->leafTesting(b1, b2);
- return;
- }
-
- int a1, a2, c1, c2;
-
- if(node->firstOverSecond(b1, b2))
- {
- a1 = node->getFirstLeftChild(b1);
- a2 = b2;
- c1 = node->getFirstRightChild(b1);
- c2 = b2;
- }
- else
- {
- a1 = b1;
- a2 = node->getSecondLeftChild(b2);
- c1 = b1;
- c2 = node->getSecondRightChild(b2);
- }
-
- BVH_REAL d1 = node->BVTesting(a1, a2);
- BVH_REAL d2 = node->BVTesting(c1, c2);
-
- if(d2 < d1)
- {
- if(!node->canStop(d2))
- distanceRecurse(node, c1, c2, front_list);
- else
- updateFrontList(front_list, c1, c2);
-
- if(!node->canStop(d1))
- distanceRecurse(node, a1, a2, front_list);
- else
- updateFrontList(front_list, a1, a2);
- }
- else
- {
- if(!node->canStop(d1))
- distanceRecurse(node, a1, a2, front_list);
- else
- updateFrontList(front_list, a1, a2);
-
- if(!node->canStop(d2))
- distanceRecurse(node, c1, c2, front_list);
- else
- updateFrontList(front_list, c1, c2);
- }
-}
-
-
-/** \brief Bounding volume test structure */
-struct BVT
-{
- /** \brief distance between bvs */
- BVH_REAL d;
-
- /** \brief bv indices for a pair of bvs in two models */
- int b1, b2;
-};
-
-/** \brief Comparer between two BVT */
-struct BVT_Comparer
-{
- bool operator() (const BVT& lhs, const BVT& rhs) const
- {
- return lhs.d > rhs.d;
- }
-};
-
-struct BVTQ
-{
- BVTQ() : qsize(2) {}
-
- bool empty() const
- {
- return pq.empty();
- }
-
- size_t size() const
- {
- return pq.size();
- }
-
- const BVT& top() const
- {
- return pq.top();
- }
-
- void push(const BVT& x)
- {
- pq.push(x);
- }
-
- void pop()
- {
- pq.pop();
- }
-
- bool full() const
- {
- return ((int)pq.size() >= qsize - 1);
- }
-
- std::priority_queue<BVT, std::vector<BVT>, BVT_Comparer> pq;
-
- /** \brief Queue size */
- int qsize;
-};
-
-
-void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list, int qsize)
-{
- BVTQ bvtq;
- bvtq.qsize = qsize;
-
- BVT min_test;
- min_test.b1 = b1;
- min_test.b2 = b2;
-
- while(1)
- {
- bool l1 = node->isFirstNodeLeaf(min_test.b1);
- bool l2 = node->isSecondNodeLeaf(min_test.b2);
-
- if(l1 && l2)
- {
- updateFrontList(front_list, min_test.b1, min_test.b2);
-
- node->leafTesting(min_test.b1, min_test.b2);
- }
- else if(bvtq.full())
- {
- // queue should not get two more tests, recur
-
- distanceQueueRecurse(node, min_test.b1, min_test.b2, front_list, qsize);
- }
- else
- {
- // queue capacity is not full yet
- BVT bvt1, bvt2;
-
- if(node->firstOverSecond(min_test.b1, min_test.b2))
- {
- int c1 = node->getFirstLeftChild(min_test.b1);
- int c2 = node->getFirstRightChild(min_test.b1);
- bvt1.b1 = c1;
- bvt1.b2 = min_test.b2;
- bvt1.d = node->BVTesting(bvt1.b1, bvt1.b2);
-
- bvt2.b1 = c2;
- bvt2.b2 = min_test.b2;
- bvt2.d = node->BVTesting(bvt2.b1, bvt2.b2);
- }
- else
- {
- int c1 = node->getSecondLeftChild(min_test.b2);
- int c2 = node->getSecondRightChild(min_test.b2);
- bvt1.b1 = min_test.b1;
- bvt1.b2 = c1;
- bvt1.d = node->BVTesting(bvt1.b1, bvt1.b2);
-
- bvt2.b1 = min_test.b1;
- bvt2.b2 = c2;
- bvt2.d = node->BVTesting(bvt2.b1, bvt2.b2);
- }
-
- bvtq.push(bvt1);
- bvtq.push(bvt2);
- }
-
- if(bvtq.empty())
- break;
- else
- {
- min_test = bvtq.top();
- bvtq.pop();
-
- if(node->canStop(min_test.d))
- {
- updateFrontList(front_list, min_test.b1, min_test.b2);
- break;
- }
- }
- }
-}
-
-void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, BVHFrontList* front_list)
-{
- BVHFrontList::iterator front_iter;
- BVHFrontList append;
- for(front_iter = front_list->begin(); front_iter != front_list->end(); ++front_iter)
- {
- int b1 = front_iter->left;
- int b2 = front_iter->right;
- bool l1 = node->isFirstNodeLeaf(b1);
- bool l2 = node->isSecondNodeLeaf(b2);
-
- if(l1 & l2)
- {
- front_iter->valid = false; // the front node is no longer valid, in collideRecurse will add again.
- collisionRecurse(node, b1, b2, &append);
- }
- else
- {
- if(!node->BVTesting(b1, b2))
- {
- front_iter->valid = false;
-
- if(node->firstOverSecond(b1, b2))
- {
- int c1 = node->getFirstLeftChild(b1);
- int c2 = node->getFirstRightChild(b2);
-
- collisionRecurse(node, c1, b2, front_list);
- collisionRecurse(node, c2, b2, front_list);
- }
- else
- {
- int c1 = node->getSecondLeftChild(b2);
- int c2 = node->getSecondRightChild(b2);
-
- collisionRecurse(node, b1, c1, front_list);
- collisionRecurse(node, b1, c2, front_list);
- }
- }
- }
- }
-
-
- // clean the old front list (remove invalid node)
- for(front_iter = front_list->begin(); front_iter != front_list->end();)
- {
- if(!front_iter->valid)
- front_iter = front_list->erase(front_iter);
- else
- ++front_iter;
- }
-
- for(front_iter = append.begin(); front_iter != append.end(); ++front_iter)
- {
- front_list->push_back(*front_iter);
- }
-}
-
-
-}
diff --git a/branches/point_cloud/fcl/src/vec_3f.cpp b/branches/point_cloud/fcl/src/vec_3f.cpp
deleted file mode 100644
index cd0c15627ec4f75ebae88b67fb0d3aff226b775d..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/src/vec_3f.cpp
+++ /dev/null
@@ -1,194 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-#include "fcl/vec_3f.h"
-#include <iostream>
-
-namespace fcl
-{
-#if COLLISION_USE_SSE
-const float Vec3f::EPSILON = 1e-11;
-#else
-const BVH_REAL Vec3f::EPSILON = 1e-11;
-#endif
-
-Vec3f MxV(const Vec3f M[3], const Vec3f& v)
-{
- return Vec3f(M[0].dot(v), M[1].dot(v), M[2].dot(v));
-}
-
-Vec3f MTxV(const Vec3f M[3], const Vec3f& v)
-{
- return M[0] * v[0] + M[1] * v[1] + M[2] * v[2];
-}
-
-BVH_REAL vTMv(const Vec3f M[3], const Vec3f& v)
-{
- return v.dot(Vec3f(M[0].dot(v), M[1].dot(v), M[2].dot(v)));
-}
-
-
-void SMST(const Vec3f M[3], const Vec3f S[3], Vec3f newM[3])
-{
- Vec3f SMT_col[3] = {Vec3f(M[0].dot(S[0]), M[1].dot(S[0]), M[2].dot(S[0])),
- Vec3f(M[0].dot(S[1]), M[1].dot(S[1]), M[2].dot(S[1])),
- Vec3f(M[0].dot(S[2]), M[1].dot(S[2]), M[2].dot(S[2]))
- };
-
- newM[0] = Vec3f(S[0].dot(SMT_col[0]), S[1].dot(SMT_col[0]), S[2].dot(SMT_col[0]));
- newM[1] = Vec3f(S[0].dot(SMT_col[1]), S[1].dot(SMT_col[1]), S[2].dot(SMT_col[1]));
- newM[2] = Vec3f(S[0].dot(SMT_col[2]), S[1].dot(SMT_col[2]), S[2].dot(SMT_col[2]));
-}
-
-void relativeTransform(const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2, Vec3f R[3], Vec3f& T)
-{
- R[0] = Vec3f(R1[0][0] * R2[0][0] + R1[1][0] * R2[1][0] + R1[2][0] * R2[2][0],
- R1[0][0] * R2[0][1] + R1[1][0] * R2[1][1] + R1[2][0] * R2[2][1],
- R1[0][0] * R2[0][2] + R1[1][0] * R2[1][2] + R1[2][0] * R2[2][2]);
- R[1] = Vec3f(R1[0][1] * R2[0][0] + R1[1][1] * R2[1][0] + R1[2][1] * R2[2][0],
- R1[0][1] * R2[0][1] + R1[1][1] * R2[1][1] + R1[2][1] * R2[2][1],
- R1[0][1] * R2[0][2] + R1[1][1] * R2[1][2] + R1[2][1] * R2[2][2]);
- R[2] = Vec3f(R1[0][2] * R2[0][0] + R1[1][2] * R2[1][0] + R1[2][2] * R2[2][0],
- R1[0][2] * R2[0][1] + R1[1][2] * R2[1][1] + R1[2][2] * R2[2][1],
- R1[0][2] * R2[0][2] + R1[1][2] * R2[1][2] + R1[2][2] * R2[2][2]);
-
- Vec3f temp = T2 - T1;
- T = Vec3f(R1[0][0] * temp[0] + R1[1][0] * temp[1] + R1[2][0] * temp[2],
- R1[0][1] * temp[0] + R1[1][1] * temp[1] + R1[2][1] * temp[2],
- R1[0][2] * temp[0] + R1[1][2] * temp[1] + R1[2][2] * temp[2]);
-}
-
-void Meigen(Vec3f a[3], BVH_REAL dout[3], Vec3f vout[3])
-{
- int n = 3;
- int j, iq, ip, i;
- BVH_REAL tresh, theta, tau, t, sm, s, h, g, c;
- int nrot;
- BVH_REAL b[3];
- BVH_REAL z[3];
- BVH_REAL v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
- BVH_REAL d[3];
-
- for(ip = 0; ip < n; ++ip)
- {
- b[ip] = a[ip][ip];
- d[ip] = a[ip][ip];
- z[ip] = 0.0;
- }
-
- nrot = 0;
-
- for(i = 0; i < 50; ++i)
- {
- sm = 0.0;
- for(ip = 0; ip < n; ++ip)
- for(iq = ip + 1; iq < n; ++iq)
- sm += fabs(a[ip][iq]);
- if(sm == 0.0)
- {
- vout[0] = Vec3f(v[0][0], v[0][1], v[0][2]);
- vout[1] = Vec3f(v[1][0], v[1][1], v[1][2]);
- vout[2] = Vec3f(v[2][0], v[2][1], v[2][2]);
- dout[0] = d[0]; dout[1] = d[1]; dout[2] = d[2];
- return;
- }
-
- if(i < 3) tresh = 0.2 * sm / (n * n);
- else tresh = 0.0;
-
- for(ip = 0; ip < n; ++ip)
- {
- for(iq= ip + 1; iq < n; ++iq)
- {
- g = 100.0 * fabs(a[ip][iq]);
- if(i > 3 &&
- fabs(d[ip]) + g == fabs(d[ip]) &&
- fabs(d[iq]) + g == fabs(d[iq]))
- a[ip][iq] = 0.0;
- else if(fabs(a[ip][iq]) > tresh)
- {
- h = d[iq] - d[ip];
- if(fabs(h) + g == fabs(h)) t = (a[ip][iq]) / h;
- else
- {
- theta = 0.5 * h / (a[ip][iq]);
- t = 1.0 /(fabs(theta) + sqrt(1.0 + theta * theta));
- if(theta < 0.0) t = -t;
- }
- c = 1.0 / sqrt(1 + t * t);
- s = t * c;
- tau = s / (1.0 + c);
- h = t * a[ip][iq];
- z[ip] -= h;
- z[iq] += h;
- d[ip] -= h;
- d[iq] += h;
- a[ip][iq] = 0.0;
- for(j = 0; j < ip; ++j) { g = a[j][ip]; h = a[j][iq]; a[j][ip] = g - s * (h + g * tau); a[j][iq] = h + s * (g - h * tau); }
- for(j = ip + 1; j < iq; ++j) { g = a[ip][j]; h = a[j][iq]; a[ip][j] = g - s * (h + g * tau); a[j][iq] = h + s * (g - h * tau); }
- for(j = iq + 1; j < n; ++j) { g = a[ip][j]; h = a[iq][j]; a[ip][j] = g - s * (h + g * tau); a[iq][j] = h + s * (g - h * tau); }
- for(j = 0; j < n; ++j) { g = v[j][ip]; h = v[j][iq]; v[j][ip] = g - s * (h + g * tau); v[j][iq] = h + s * (g - h * tau); }
- nrot++;
- }
- }
- }
- for(ip = 0; ip < n; ++ip)
- {
- b[ip] += z[ip];
- d[ip] = b[ip];
- z[ip] = 0.0;
- }
- }
-
- //std::cerr << "eigen: too many iterations in Jacobi transform." << std::endl;
-
- return;
-}
-
-
-void MxM(const Vec3f M1[3], const Vec3f M2[3], Vec3f newM[3])
-{
- for(int i = 0; i < 3; ++i)
- {
- for(int j = 0; j < 3; ++j)
- {
- newM[i][j] = M1[i][0] * M2[0][j] + M1[i][1] * M2[1][j] + M1[i][2] * M2[2][j];
- }
- }
-}
-
-
-}
diff --git a/branches/point_cloud/fcl/test/test_core_broad_phase.cpp b/branches/point_cloud/fcl/test/test_core_broad_phase.cpp
deleted file mode 100644
index 64b585aa38fbb67a48fc8a09a0682da30f7c1e4f..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/test/test_core_broad_phase.cpp
+++ /dev/null
@@ -1,246 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#include "fcl/broad_phase_collision.h"
-#include "fcl/geometric_shape_to_BVH_model.h"
-#include "fcl/transform.h"
-#include "test_core_utility.h"
-#include <gtest/gtest.h>
-
-using namespace fcl;
-
-
-void generateEnvironments(std::vector<CollisionObject*>& env, int n);
-
-
-TEST(test_core, broad_phase_collision)
-{
- bool exhaustive = false;
-
- std::vector<CollisionObject*> env;
- generateEnvironments(env, 100);
-
- std::vector<CollisionObject*> query;
- generateEnvironments(query, 10);
-
- BroadPhaseCollisionManager* manager1 = new NaiveCollisionManager();
- BroadPhaseCollisionManager* manager2 = new SSaPCollisionManager();
- BroadPhaseCollisionManager* manager3 = new SaPCollisionManager();
- BroadPhaseCollisionManager* manager4 = new IntervalTreeCollisionManager();
-
-
- for(unsigned int i = 0; i < env.size(); ++i)
- {
- manager1->registerObject(env[i]);
- manager2->registerObject(env[i]);
- manager3->registerObject(env[i]);
- manager4->registerObject(env[i]);
- }
-
- manager1->setup();
- manager2->setup();
- manager3->setup();
- manager4->setup();
-
- CollisionData self_data1;
- self_data1.exhaustive = exhaustive;
- CollisionData self_data2;
- self_data2.exhaustive = exhaustive;
- CollisionData self_data3;
- self_data3.exhaustive = exhaustive;
- CollisionData self_data4;
- self_data4.exhaustive = exhaustive;
-
- manager1->collide(&self_data1, defaultCollisionFunction);
- manager2->collide(&self_data2, defaultCollisionFunction);
- manager3->collide(&self_data3, defaultCollisionFunction);
- manager3->collide(&self_data4, defaultCollisionFunction);
-
- bool self_res1 = (self_data1.contacts.size() > 0);
- bool self_res2 = (self_data2.contacts.size() > 0);
- bool self_res3 = (self_data3.contacts.size() > 0);
- bool self_res4 = (self_data4.contacts.size() > 0);
-
- ASSERT_TRUE(self_res1 == self_res2);
- ASSERT_TRUE(self_res1 == self_res3);
- ASSERT_TRUE(self_res1 == self_res4);
-
-
- for(unsigned int i = 0; i < query.size(); ++i)
- {
- CollisionData query_data1;
- query_data1.exhaustive = exhaustive;
- CollisionData query_data2;
- query_data2.exhaustive = exhaustive;
- CollisionData query_data3;
- query_data3.exhaustive = exhaustive;
- CollisionData query_data4;
- query_data4.exhaustive = exhaustive;
-
- manager1->collide(query[i], &query_data1, defaultCollisionFunction);
- manager2->collide(query[i], &query_data2, defaultCollisionFunction);
- manager3->collide(query[i], &query_data3, defaultCollisionFunction);
- manager4->collide(query[i], &query_data4, defaultCollisionFunction);
-
- bool query_res1 = (query_data1.contacts.size() > 0);
- bool query_res2 = (query_data2.contacts.size() > 0);
- bool query_res3 = (query_data3.contacts.size() > 0);
- bool query_res4 = (query_data4.contacts.size() > 0);
-
- ASSERT_TRUE(query_res1 == query_res2);
- ASSERT_TRUE(query_res1 == query_res3);
- ASSERT_TRUE(query_res1 == query_res4);
- }
-
- for(unsigned int i = 0; i < env.size(); ++i)
- delete env[i];
- for(unsigned int i = 0; i < query.size(); ++i)
- delete query[i];
-
- delete manager1;
- delete manager2;
- delete manager3;
- delete manager4;
-}
-
-
-
-TEST(test_core, broad_phase_self_collision_exhaustive)
-{
- bool exhaustive = true;
-
- std::vector<CollisionObject*> env;
- generateEnvironments(env, 100);
-
- BroadPhaseCollisionManager* manager1 = new NaiveCollisionManager();
- BroadPhaseCollisionManager* manager2 = new SSaPCollisionManager();
- BroadPhaseCollisionManager* manager3 = new SaPCollisionManager();
- BroadPhaseCollisionManager* manager4 = new IntervalTreeCollisionManager();
-
-
- for(unsigned int i = 0; i < env.size(); ++i)
- {
- manager1->registerObject(env[i]);
- manager2->registerObject(env[i]);
- manager3->registerObject(env[i]);
- manager4->registerObject(env[i]);
- }
-
- manager1->setup();
- manager2->setup();
- manager3->setup();
- manager4->setup();
-
- CollisionData self_data1;
- self_data1.exhaustive = exhaustive;
- CollisionData self_data2;
- self_data2.exhaustive = exhaustive;
- CollisionData self_data3;
- self_data3.exhaustive = exhaustive;
- CollisionData self_data4;
- self_data4.exhaustive = exhaustive;
-
- manager1->collide(&self_data1, defaultCollisionFunction);
- manager2->collide(&self_data2, defaultCollisionFunction);
- manager3->collide(&self_data3, defaultCollisionFunction);
- manager3->collide(&self_data4, defaultCollisionFunction);
-
- unsigned int self_res1 = self_data1.contacts.size();
- unsigned int self_res2 = self_data2.contacts.size();
- unsigned int self_res3 = self_data3.contacts.size();
- unsigned int self_res4 = self_data4.contacts.size();
-
- ASSERT_TRUE(self_res1 == self_res2);
- ASSERT_TRUE(self_res1 == self_res3);
- ASSERT_TRUE(self_res1 == self_res4);
-
-
- for(unsigned int i = 0; i < env.size(); ++i)
- delete env[i];
-
- delete manager1;
- delete manager2;
- delete manager3;
- delete manager4;
-}
-
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
-
-void generateEnvironments(std::vector<CollisionObject*>& env, int n)
-{
- BVH_REAL extents[] = {-200, 200, -200, 200, -200, 200};
- BVH_REAL delta_trans[] = {1, 1, 1};
- std::vector<Transform> transforms;
- std::vector<Transform> transforms2;
-
-
- generateRandomTransform(extents, transforms, transforms2, delta_trans, 0.005 * 2 * 3.1415, n);
- Box box(5, 10, 20);
- for(int i = 0; i < n; ++i)
- {
- BVHModel<OBB>* model = new BVHModel<OBB>();
- box.setLocalTransform(transforms[i].R, transforms[i].T);
- generateBVHModel(*model, box);
- env.push_back(model);
- }
-
- generateRandomTransform(extents, transforms, transforms2, delta_trans, 0.005 * 2 * 3.1415, n);
- Sphere sphere(30);
- for(int i = 0; i < n; ++i)
- {
- BVHModel<OBB>* model = new BVHModel<OBB>();
- sphere.setLocalTransform(transforms[i].R, transforms[i].T);
- generateBVHModel(*model, sphere);
- env.push_back(model);
- }
-
- generateRandomTransform(extents, transforms, transforms2, delta_trans, 0.005 * 2 * 3.1415, n);
- Cylinder cylinder(10, 40);
- for(int i = 0; i < n; ++i)
- {
- BVHModel<OBB>* model = new BVHModel<OBB>();
- cylinder.setLocalTransform(transforms[i].R, transforms[i].T);
- generateBVHModel(*model, cylinder);
- env.push_back(model);
- }
-}
diff --git a/branches/point_cloud/fcl/test/test_core_collision.cpp b/branches/point_cloud/fcl/test/test_core_collision.cpp
deleted file mode 100644
index d3d258cda6b5f95cc5f1d696fc5d3583b2a67c5e..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/test/test_core_collision.cpp
+++ /dev/null
@@ -1,869 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#include "fcl/traversal_node_bvhs.h"
-#include "fcl/collision_node.h"
-#include "fcl/simple_setup.h"
-#include "fcl/collision.h"
-#include "test_core_utility.h"
-#include <gtest/gtest.h>
-
-#if USE_PQP
-#include <PQP.h>
-#endif
-
-using namespace fcl;
-
-template<typename BV>
-bool collide_Test(const Transform& tf,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose = true);
-
-template<typename BV>
-bool collide_Test2(const Transform& tf,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose = true);
-
-bool collide_Test_OBB(const Transform& tf,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose = true);
-
-bool collide_Test_RSS(const Transform& tf,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose = true);
-
-template<typename BV>
-bool test_collide_func(const Transform& tf,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method);
-
-int num_max_contacts = -1;
-bool exhaustive = true;
-bool enable_contact = true;
-
-std::vector<BVHCollisionPair> global_pairs;
-std::vector<BVHCollisionPair> global_pairs_now;
-
-std::vector<std::pair<int, int> > PQP_pairs;
-
-TEST(collision_test, mesh_mesh)
-{
- std::vector<Vec3f> p1, p2;
- std::vector<Triangle> t1, t2;
- loadOBJFile("test/env.obj", p1, t1);
- loadOBJFile("test/rob.obj", p2, t2);
-
- std::vector<Transform> transforms; // t0
- std::vector<Transform> transforms2; // t1
- std::vector<Transform> transforms_ccd; // t0
- std::vector<Transform> transforms_ccd2; // t1
- BVH_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
- BVH_REAL delta_trans[] = {1, 1, 1};
- int n = 10;
- bool verbose = false;
-
- generateRandomTransform(extents, transforms, transforms2, delta_trans, 0.005 * 2 * 3.1415, n);
-
- // collision
- for(unsigned int i = 0; i < transforms.size(); ++i)
- {
- global_pairs.clear();
- global_pairs_now.clear();
-#if USE_PQP
- PQP_pairs.clear();
- collide_PQP(transforms[i], p1, t1, p2, t2, PQP_pairs, verbose);
- global_pairs.resize(PQP_pairs.size());
- for(unsigned int j = 0; j < PQP_pairs.size(); ++j)
- global_pairs[j] = BVHCollisionPair(PQP_pairs[j].first, PQP_pairs[j].second);
-
- PQP_pairs.clear();
- collide_PQP2(transforms[i], p1, t1, p2, t2, PQP_pairs, verbose);
- global_pairs_now.resize(PQP_pairs.size());
- for(unsigned int j = 0; j < PQP_pairs.size(); ++j)
- global_pairs_now[j] = BVHCollisionPair(PQP_pairs[j].first, PQP_pairs[j].second);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-#endif
-
- collide_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
-
-#if USE_PQP
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-#endif
-
- collide_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test<AABB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test<AABB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test<AABB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test<KDOP<24> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test<KDOP<24> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test<KDOP<24> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test<KDOP<18> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test<KDOP<18> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test<KDOP<18> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test<KDOP<16> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test<KDOP<16> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test<KDOP<16> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test2<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test2<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test2<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test2<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test2<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test2<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test2<AABB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test2<AABB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test2<AABB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test2<KDOP<24> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test2<KDOP<24> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test2<KDOP<24> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test2<KDOP<18> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test2<KDOP<18> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test2<KDOP<18> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test2<KDOP<16> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test2<KDOP<16> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test2<KDOP<16> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test_OBB(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test_OBB(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test_OBB(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test_RSS(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test_RSS(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- collide_Test_RSS(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- test_collide_func<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- test_collide_func<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
-
- test_collide_func<AABB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN);
-
- ASSERT_TRUE(global_pairs.size() == global_pairs_now.size());
- for(unsigned int j = 0; j < global_pairs.size(); ++j)
- {
- ASSERT_TRUE(global_pairs[j].id1 == global_pairs_now[j].id1);
- ASSERT_TRUE(global_pairs[j].id2 == global_pairs_now[j].id2);
- }
- }
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
-template<typename BV>
-bool collide_Test2(const Transform& tf,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose)
-{
- BVHModel<BV> m1;
- BVHModel<BV> m2;
- m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
- m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
-
- std::vector<Vec3f> vertices1_new(vertices1.size());
- for(unsigned int i = 0; i < vertices1_new.size(); ++i)
- {
- vertices1_new[i] = MxV(tf.R, vertices1[i]) + tf.T;
- }
-
-
- m1.beginModel();
- m1.addSubModel(vertices1_new, triangles1);
- m1.endModel();
-
- m2.beginModel();
- m2.addSubModel(vertices2, triangles2);
- m2.endModel();
-
- MeshCollisionTraversalNode<BV> node;
-
- if(!initialize<BV>(node, m1, m2))
- std::cout << "initialize error" << std::endl;
-
- node.enable_statistics = verbose;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
-
- collide(&node);
-
-
- if(node.pairs.size() > 0)
- {
- std::vector<BVHCollisionPair> pairs(node.pairs.size());
- for(unsigned i = 0; i < node.pairs.size(); ++i)
- pairs[i] = node.pairs[i];
-
- std::sort(pairs.begin(), pairs.end(), BVHCollisionPairComp());
-
- if(global_pairs.size() == 0)
- {
- global_pairs.resize(pairs.size());
- for(unsigned int i = 0 ; i < pairs.size(); ++i)
- global_pairs[i] = pairs[i];
- }
- else
- {
- global_pairs_now.resize(pairs.size());
- for(unsigned int i = 0 ; i < pairs.size(); ++i)
- global_pairs_now[i] = pairs[i];
- }
-
-
- if(verbose)
- std::cout << "in collision " << node.pairs.size() << ": " << std::endl;
- if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
- return true;
- }
- else
- {
- if(verbose) std::cout << "collision free " << std::endl;
- if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
- return false;
- }
-}
-
-
-template<typename BV>
-bool collide_Test(const Transform& tf,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose)
-{
- BVHModel<BV> m1;
- BVHModel<BV> m2;
- m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
- m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
-
- m1.beginModel();
- m1.addSubModel(vertices1, triangles1);
- m1.endModel();
-
- m2.beginModel();
- m2.addSubModel(vertices2, triangles2);
- m2.endModel();
-
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
- Vec3f T2;
-
- MeshCollisionTraversalNode<BV> node;
-
- if(!initialize<BV>(node, m1, m2, tf.R, tf.T, R2, T2))
- std::cout << "initialize error" << std::endl;
-
- node.enable_statistics = verbose;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
-
- collide(&node);
-
-
- if(node.pairs.size() > 0)
- {
- std::vector<BVHCollisionPair> pairs(node.pairs.size());
- for(unsigned i = 0; i < node.pairs.size(); ++i)
- pairs[i] = node.pairs[i];
-
- std::sort(pairs.begin(), pairs.end(), BVHCollisionPairComp());
-
- if(global_pairs.size() == 0)
- {
- global_pairs.resize(pairs.size());
- for(unsigned int i = 0 ; i < pairs.size(); ++i)
- global_pairs[i] = pairs[i];
- }
- else
- {
- global_pairs_now.resize(pairs.size());
- for(unsigned int i = 0 ; i < pairs.size(); ++i)
- global_pairs_now[i] = pairs[i];
- }
-
- if(verbose)
- std::cout << "in collision " << node.pairs.size() << ": " << std::endl;
- if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
- return true;
- }
- else
- {
- if(verbose) std::cout << "collision free " << std::endl;
- if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
- return false;
- }
-}
-
-bool collide_Test_OBB(const Transform& tf,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose)
-{
- BVHModel<OBB> m1;
- BVHModel<OBB> m2;
- m1.bv_splitter.reset(new BVSplitter<OBB>(split_method));
- m2.bv_splitter.reset(new BVSplitter<OBB>(split_method));
-
- m1.beginModel();
- m1.addSubModel(vertices1, triangles1);
- m1.endModel();
-
- m2.beginModel();
- m2.addSubModel(vertices2, triangles2);
- m2.endModel();
-
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
- Vec3f T2;
-
- MeshCollisionTraversalNodeOBB node;
- if(!initialize(node, (const BVHModel<OBB>&)m1, (const BVHModel<OBB>&)m2, tf.R, tf.T, R2, T2))
- std::cout << "initialize error" << std::endl;
-
- node.enable_statistics = verbose;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
-
- collide(&node);
-
- if(node.pairs.size() > 0)
- {
- std::vector<BVHCollisionPair> pairs(node.pairs.size());
- for(unsigned i = 0; i < node.pairs.size(); ++i)
- pairs[i] = node.pairs[i];
-
- std::sort(pairs.begin(), pairs.end(), BVHCollisionPairComp());
-
- if(global_pairs.size() == 0)
- {
- global_pairs.resize(pairs.size());
- for(unsigned int i = 0 ; i < pairs.size(); ++i)
- global_pairs[i] = pairs[i];
- }
- else
- {
- global_pairs_now.resize(pairs.size());
- for(unsigned int i = 0 ; i < pairs.size(); ++i)
- global_pairs_now[i] = pairs[i];
- }
-
-
- if(verbose)
- std::cout << "in collision " << node.pairs.size() << ": " << std::endl;
- if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
- return true;
- }
- else
- {
- if(verbose) std::cout << "collision free " << std::endl;
- if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
- return false;
- }
-}
-
-bool collide_Test_RSS(const Transform& tf,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose)
-{
- BVHModel<RSS> m1;
- BVHModel<RSS> m2;
- m1.bv_splitter.reset(new BVSplitter<RSS>(split_method));
- m2.bv_splitter.reset(new BVSplitter<RSS>(split_method));
-
- m1.beginModel();
- m1.addSubModel(vertices1, triangles1);
- m1.endModel();
-
- m2.beginModel();
- m2.addSubModel(vertices2, triangles2);
- m2.endModel();
-
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
- Vec3f T2;
-
- MeshCollisionTraversalNodeRSS node;
- if(!initialize(node, (const BVHModel<RSS>&)m1, (const BVHModel<RSS>&)m2, tf.R, tf.T, R2, T2))
- std::cout << "initialize error" << std::endl;
-
- node.enable_statistics = verbose;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
-
- collide(&node);
-
- if(node.pairs.size() > 0)
- {
- std::vector<BVHCollisionPair> pairs(node.pairs.size());
- for(unsigned i = 0; i < node.pairs.size(); ++i)
- pairs[i] = node.pairs[i];
-
- std::sort(pairs.begin(), pairs.end(), BVHCollisionPairComp());
-
- if(global_pairs.size() == 0)
- {
- global_pairs.resize(pairs.size());
- for(unsigned int i = 0 ; i < pairs.size(); ++i)
- global_pairs[i] = pairs[i];
- }
- else
- {
- global_pairs_now.resize(pairs.size());
- for(unsigned int i = 0 ; i < pairs.size(); ++i)
- global_pairs_now[i] = pairs[i];
- }
-
- if(verbose)
- std::cout << "in collision " << node.pairs.size() << ": " << std::endl;
- if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
- return true;
- }
- else
- {
- if(verbose) std::cout << "collision free " << std::endl;
- if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
- return false;
- }
-}
-
-
-template<typename BV>
-bool test_collide_func(const Transform& tf,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method)
-{
- BVHModel<BV> m1;
- BVHModel<BV> m2;
- m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
- m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
-
- m1.beginModel();
- m1.addSubModel(vertices1, triangles1);
- m1.endModel();
-
- m2.beginModel();
- m2.addSubModel(vertices2, triangles2);
- m2.endModel();
-
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
- Vec3f T2;
-
- m1.setRotation(tf.R);
- m1.setTranslation(tf.T);
- m2.setRotation(R2);
- m2.setTranslation(T2);
-
-
- std::vector<Contact> contacts;
- int num_contacts = collide(&m1, &m2, num_max_contacts, enable_contact, exhaustive, contacts);
- global_pairs_now.resize(num_contacts);
-
- for(int i = 0; i < num_contacts; ++i)
- {
- global_pairs_now[i].id1 = contacts[i].b1;
- global_pairs_now[i].id2 = contacts[i].b2;
- }
-
- std::sort(global_pairs_now.begin(), global_pairs_now.end(), BVHCollisionPairComp());
-
- if(num_contacts > 0) return true;
- else return false;
-}
diff --git a/branches/point_cloud/fcl/test/test_core_collision_point.cpp b/branches/point_cloud/fcl/test/test_core_collision_point.cpp
deleted file mode 100644
index fe0a988fe87d46164ea1fbed26265c6b01d2d9b1..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/test/test_core_collision_point.cpp
+++ /dev/null
@@ -1,540 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#if USE_SVMLIGHT
-
-#include "fcl/traversal_node_bvhs.h"
-#include "fcl/collision_node.h"
-#include "fcl/simple_setup.h"
-#include "test_core_utility.h"
-
-#if USE_PQP
-#include <PQP.h>
-#endif
-
-using namespace fcl;
-
-template<typename BV>
-bool collide_Test_PP(const Transform& tf,
- const std::vector<Vec3f>& vertices1, const std::vector<Vec3f>& vertices2, SplitMethodType split_method, bool verbose = true);
-
-template<typename BV>
-bool collide_Test_MP(const Transform& tf,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, SplitMethodType split_method, bool verbose = true);
-template<typename BV>
-bool collide_Test_PM(const Transform& tf,
- const std::vector<Vec3f>& vertices1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose = true);
-
-bool collide_Test_PP_OBB(const Transform& tf,
- const std::vector<Vec3f>& vertices1, const std::vector<Vec3f>& vertices2, SplitMethodType split_method, bool verbose = true);
-
-bool collide_Test_MP_OBB(const Transform& tf,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, SplitMethodType split_method, bool verbose = true);
-
-bool collide_Test_PM_OBB(const Transform& tf,
- const std::vector<Vec3f>& vertices1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose = true);
-
-int num_max_contacts = 1;
-
-int main()
-{
- std::vector<Vec3f> p1, p2;
- std::vector<Triangle> t1, t2;
- loadOBJFile("env.obj", p1, t1);
- loadOBJFile("rob.obj", p2, t2);
-
- std::vector<Transform> transforms; // t0
- std::vector<Transform> transforms2; // t1
- std::vector<Transform> transforms_ccd; // t0
- std::vector<Transform> transforms_ccd2; // t1
- BVH_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
- BVH_REAL delta_trans[] = {1, 1, 1};
- int n = 10;
-
- generateRandomTransform(extents, transforms, transforms2, delta_trans, 0.005 * 2 * 3.1415, n);
-
- for(unsigned int i = 0; i < transforms.size(); ++i)
- {
- std::cout << "test id " << i << std::endl;
-
- std::vector<std::pair<int, int> > PQP_pairs;
- DistanceRes PQP_dist;
-
-#if USE_PQP
- collide_PQP(transforms[i], p1, t1, p2, t2, PQP_pairs);
-
- distance_PQP(transforms[i], p1, t1, p2, t2, PQP_dist);
-#endif
-
- collide_Test_PP<OBB>(transforms[i], p1, p2, SPLIT_METHOD_MEAN);
-
- collide_Test_PP<OBB>(transforms[i], p1, p2, SPLIT_METHOD_BV_CENTER);
-
- collide_Test_PP<OBB>(transforms[i], p1, p2, SPLIT_METHOD_MEDIAN);
-
- collide_Test_PM<OBB>(transforms[i], p1, p2, t2, SPLIT_METHOD_MEAN);
-
- collide_Test_PM<OBB>(transforms[i], p1, p2, t2, SPLIT_METHOD_BV_CENTER);
-
- collide_Test_PM<OBB>(transforms[i], p1, p2, t2, SPLIT_METHOD_MEDIAN);
-
- collide_Test_MP<OBB>(transforms[i], p1, t1, p2, SPLIT_METHOD_MEAN);
-
- collide_Test_MP<OBB>(transforms[i], p1, t1, p2, SPLIT_METHOD_BV_CENTER);
-
- collide_Test_MP<OBB>(transforms[i], p1, t1, p2, SPLIT_METHOD_MEDIAN);
-
- collide_Test_PP_OBB(transforms[i], p1, p2, SPLIT_METHOD_MEAN);
-
- collide_Test_PP_OBB(transforms[i], p1, p2, SPLIT_METHOD_BV_CENTER);
-
- collide_Test_PP_OBB(transforms[i], p1, p2, SPLIT_METHOD_MEDIAN);
-
- collide_Test_PM_OBB(transforms[i], p1, p2, t2, SPLIT_METHOD_MEAN);
-
- collide_Test_PM_OBB(transforms[i], p1, p2, t2, SPLIT_METHOD_BV_CENTER);
-
- collide_Test_PM_OBB(transforms[i], p1, p2, t2, SPLIT_METHOD_MEDIAN);
-
- collide_Test_MP_OBB(transforms[i], p1, t1, p2, SPLIT_METHOD_MEAN);
-
- collide_Test_MP_OBB(transforms[i], p1, t1, p2, SPLIT_METHOD_BV_CENTER);
-
- collide_Test_MP_OBB(transforms[i], p1, t1, p2, SPLIT_METHOD_MEDIAN);
-
- std::cout << std::endl;
- }
-}
-
-template<typename BV>
-bool collide_Test_PP(const Transform& tf,
- const std::vector<Vec3f>& vertices1, const std::vector<Vec3f>& vertices2, SplitMethodType split_method, bool verbose)
-{
- BVHModel<BV> m1;
- BVHModel<BV> m2;
- m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
- m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
-
- m1.beginModel();
- m1.addSubModel(vertices1);
- m1.endModel();
-
- m2.beginModel();
- m2.addSubModel(vertices2);
- m2.endModel();
-
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
- Vec3f T2;
-
- PointCloudCollisionTraversalNode<BV> node;
-
- if(!initialize<BV, false, false>(node, m1, m2, tf.R, tf.T, R2, T2, 0.6, 20))
- std::cout << "initialize error" << std::endl;
-
- node.enable_statistics = verbose;
- node.num_max_contacts = num_max_contacts;
-
- collide(&node);
-
- if(node.pairs.size() > 0)
- {
- if(verbose)
- {
- std::cout << "in collision " << node.pairs.size() << ": " << std::endl;
-
- std::vector<BVHPointCollisionPair> pairs(node.pairs.size());
- for(unsigned i = 0; i < node.pairs.size(); ++i)
- pairs[i] = node.pairs[i];
-
- std::sort(pairs.begin(), pairs.end(), BVHPointCollisionPairComp());
-
- for(unsigned i = 0; i < pairs.size(); ++i)
- {
- std::cout << "(" << pairs[i].id1_start << "(" << pairs[i].id1_num << ")" << " " << pairs[i].id2_start << "(" << pairs[i].id2_num << ")" << " " << pairs[i].collision_prob << ") ";
- }
- std::cout << std::endl;
- }
- if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
- return true;
- }
- else
- {
- if(verbose) std::cout << "collision free " << std::endl;
- if(verbose) std::cout << node.max_collision_prob << std::endl;
- if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
- return false;
- }
-}
-
-bool collide_Test_PP_OBB(const Transform& tf,
- const std::vector<Vec3f>& vertices1, const std::vector<Vec3f>& vertices2, SplitMethodType split_method, bool verbose)
-{
- BVHModel<OBB> m1;
- BVHModel<OBB> m2;
- m1.bv_splitter.reset(new BVSplitter<OBB>(split_method));
- m2.bv_splitter.reset(new BVSplitter<OBB>(split_method));
-
- m1.beginModel();
- m1.addSubModel(vertices1);
- m1.endModel();
-
- m2.beginModel();
- m2.addSubModel(vertices2);
- m2.endModel();
-
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
- Vec3f T2;
-
- PointCloudCollisionTraversalNodeOBB node;
-
- if(!initialize(node, m1, m2, tf.R, tf.T, R2, T2, 0.6, 20))
- std::cout << "initialize error" << std::endl;
-
- node.enable_statistics = verbose;
- node.num_max_contacts = num_max_contacts;
-
- collide(&node);
-
- if(node.pairs.size() > 0)
- {
- if(verbose)
- {
- std::cout << "in collision " << node.pairs.size() << ": " << std::endl;
-
- std::vector<BVHPointCollisionPair> pairs(node.pairs.size());
- for(unsigned i = 0; i < node.pairs.size(); ++i)
- pairs[i] = node.pairs[i];
-
- std::sort(pairs.begin(), pairs.end(), BVHPointCollisionPairComp());
-
- for(unsigned i = 0; i < pairs.size(); ++i)
- {
- std::cout << "(" << pairs[i].id1_start << "(" << pairs[i].id1_num << ")" << " " << pairs[i].id2_start << "(" << pairs[i].id2_num << ")" << " " << pairs[i].collision_prob << ") ";
- }
- std::cout << std::endl;
- }
- if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
- return true;
- }
- else
- {
- if(verbose) std::cout << "collision free " << std::endl;
- if(verbose) std::cout << node.max_collision_prob << std::endl;
- if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
- return false;
- }
-}
-
-
-
-template<typename BV>
-bool collide_Test_MP(const Transform& tf,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, SplitMethodType split_method, bool verbose)
-{
- BVHModel<BV> m1;
- BVHModel<BV> m2;
- m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
- m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
-
- m1.beginModel();
- m1.addSubModel(vertices1, triangles1);
- m1.endModel();
-
- m2.beginModel();
- m2.addSubModel(vertices2);
- m2.endModel();
-
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
- Vec3f T2;
-
- PointCloudMeshCollisionTraversalNode<BV> node;
-
- if(!initialize<BV, false, false>(node, m2, m1, R2, T2, tf.R, tf.T, 0.6, 20))
- std::cout << "initialize error" << std::endl;
-
- node.enable_statistics = verbose;
- node.num_max_contacts = num_max_contacts;
-
- collide(&node);
-
- if(node.pairs.size() > 0)
- {
- if(verbose)
- {
- std::cout << "in collision " << node.pairs.size() << ": " << std::endl;
-
- std::vector<BVHPointCollisionPair> pairs(node.pairs.size());
- for(unsigned i = 0; i < node.pairs.size(); ++i)
- pairs[i] = node.pairs[i];
-
- std::sort(pairs.begin(), pairs.end(), BVHPointCollisionPairComp());
-
- for(unsigned i = 0; i < pairs.size(); ++i)
- {
- std::cout << "(" << pairs[i].id1_start << "(" << pairs[i].id1_num << ")" << " " << pairs[i].id2_start << "(" << pairs[i].id2_num << ")" << " " << pairs[i].collision_prob << ") ";
- }
- std::cout << std::endl;
- }
- if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
- return true;
- }
- else
- {
- if(verbose) std::cout << "collision free " << std::endl;
- if(verbose) std::cout << node.max_collision_prob << std::endl;
- if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
- return false;
- }
-}
-
-
-
-bool collide_Test_MP_OBB(const Transform& tf,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, SplitMethodType split_method, bool verbose)
-{
- BVHModel<OBB> m1;
- BVHModel<OBB> m2;
- m1.bv_splitter.reset(new BVSplitter<OBB>(split_method));
- m2.bv_splitter.reset(new BVSplitter<OBB>(split_method));
-
- m1.beginModel();
- m1.addSubModel(vertices1, triangles1);
- m1.endModel();
-
- m2.beginModel();
- m2.addSubModel(vertices2);
- m2.endModel();
-
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
- Vec3f T2;
-
- PointCloudMeshCollisionTraversalNodeOBB node;
-
- if(!initialize(node, m2, m1, R2, T2, tf.R, tf.T, 0.6, 20))
- std::cout << "initialize error" << std::endl;
-
- node.enable_statistics = verbose;
- node.num_max_contacts = num_max_contacts;
-
- collide(&node);
-
- if(node.pairs.size() > 0)
- {
- if(verbose)
- {
- std::cout << "in collision " << node.pairs.size() << ": " << std::endl;
-
- std::vector<BVHPointCollisionPair> pairs(node.pairs.size());
- for(unsigned i = 0; i < node.pairs.size(); ++i)
- pairs[i] = node.pairs[i];
-
- std::sort(pairs.begin(), pairs.end(), BVHPointCollisionPairComp());
-
- for(unsigned i = 0; i < pairs.size(); ++i)
- {
- std::cout << "(" << pairs[i].id1_start << "(" << pairs[i].id1_num << ")" << " " << pairs[i].id2_start << "(" << pairs[i].id2_num << ")" << " " << pairs[i].collision_prob << ") ";
- }
- std::cout << std::endl;
- }
- if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
- return true;
- }
- else
- {
- if(verbose) std::cout << "collision free " << std::endl;
- if(verbose) std::cout << node.max_collision_prob << std::endl;
- if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
- return false;
- }
-}
-
-
-
-template<typename BV>
-bool collide_Test_PM(const Transform& tf,
- const std::vector<Vec3f>& vertices1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose)
-{
- BVHModel<BV> m1;
- BVHModel<BV> m2;
- m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
- m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
-
- m1.beginModel();
- m1.addSubModel(vertices1);
- m1.endModel();
-
- m2.beginModel();
- m2.addSubModel(vertices2, triangles2);
- m2.endModel();
-
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
- Vec3f T2;
-
- PointCloudMeshCollisionTraversalNode<BV> node;
-
- if(!initialize<BV, false, false>(node, m1, m2, tf.R, tf.T, R2, T2, 0.6, 20))
- std::cout << "initialize error" << std::endl;
-
- node.enable_statistics = verbose;
- node.num_max_contacts = num_max_contacts;
-
- collide(&node);
-
- if(node.pairs.size() > 0)
- {
- if(verbose)
- {
- std::cout << "in collision " << node.pairs.size() << ": " << std::endl;
-
- std::vector<BVHPointCollisionPair> pairs(node.pairs.size());
- for(unsigned i = 0; i < node.pairs.size(); ++i)
- pairs[i] = node.pairs[i];
-
- std::sort(pairs.begin(), pairs.end(), BVHPointCollisionPairComp());
-
- for(unsigned i = 0; i < pairs.size(); ++i)
- {
- std::cout << "(" << pairs[i].id1_start << "(" << pairs[i].id1_num << ")" << " " << pairs[i].id2_start << "(" << pairs[i].id2_num << ")" << " " << pairs[i].collision_prob << ") ";
- }
- std::cout << std::endl;
- }
- if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
- return true;
- }
- else
- {
- if(verbose) std::cout << "collision free " << std::endl;
- if(verbose) std::cout << node.max_collision_prob << std::endl;
- if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
- return false;
- }
-}
-
-
-bool collide_Test_PM_OBB(const Transform& tf,
- const std::vector<Vec3f>& vertices1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose)
-{
- BVHModel<OBB> m1;
- BVHModel<OBB> m2;
- m1.bv_splitter.reset(new BVSplitter<OBB>(split_method));
- m2.bv_splitter.reset(new BVSplitter<OBB>(split_method));
-
- m1.beginModel();
- m1.addSubModel(vertices1);
- m1.endModel();
-
- m2.beginModel();
- m2.addSubModel(vertices2, triangles2);
- m2.endModel();
-
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
- Vec3f T2;
-
- PointCloudMeshCollisionTraversalNodeOBB node;
-
- if(!initialize(node, m1, m2, tf.R, tf.T, R2, T2, 0.6, 20))
- std::cout << "initialize error" << std::endl;
-
- node.enable_statistics = verbose;
- node.num_max_contacts = num_max_contacts;
-
- collide(&node);
-
- if(node.pairs.size() > 0)
- {
- if(verbose)
- {
- std::cout << "in collision " << node.pairs.size() << ": " << std::endl;
-
- std::vector<BVHPointCollisionPair> pairs(node.pairs.size());
- for(unsigned i = 0; i < node.pairs.size(); ++i)
- pairs[i] = node.pairs[i];
-
- std::sort(pairs.begin(), pairs.end(), BVHPointCollisionPairComp());
-
- for(unsigned i = 0; i < pairs.size(); ++i)
- {
- std::cout << "(" << pairs[i].id1_start << "(" << pairs[i].id1_num << ")" << " " << pairs[i].id2_start << "(" << pairs[i].id2_num << ")" << " " << pairs[i].collision_prob << ") ";
- }
- std::cout << std::endl;
- }
- if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
- return true;
- }
- else
- {
- if(verbose) std::cout << "collision free " << std::endl;
- if(verbose) std::cout << node.max_collision_prob << std::endl;
- if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
- return false;
- }
-}
-
-#else
-int main()
-{
- return 1;
-}
-#endif
-
-
diff --git a/branches/point_cloud/fcl/test/test_core_distance.cpp b/branches/point_cloud/fcl/test/test_core_distance.cpp
deleted file mode 100644
index e97b6be6aaed5390d742ed2e88474a2c6f825d61..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/test/test_core_distance.cpp
+++ /dev/null
@@ -1,284 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#include "fcl/traversal_node_bvhs.h"
-#include "fcl/collision_node.h"
-#include "fcl/simple_setup.h"
-#include "test_core_utility.h"
-#include <gtest/gtest.h>
-
-
-#if USE_PQP
-#include <PQP.h>
-#endif
-
-using namespace fcl;
-
-
-void distance_Test(const Transform& tf,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method,
- int qsize,
- DistanceRes& distance_result,
- bool verbose = true);
-
-template<typename BV>
-void distance_Test2(const Transform& tf,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method,
- int qsize,
- DistanceRes& distance_result,
- bool verbose = true);
-
-bool verbose = false;
-BVH_REAL DELTA = 0.001;
-
-inline bool nearlyEqual(const Vec3f& a, const Vec3f& b)
-{
- if(fabs(a[0] - b[0]) > DELTA) return false;
- if(fabs(a[1] - b[1]) > DELTA) return false;
- if(fabs(a[2] - b[2]) > DELTA) return false;
- return true;
-}
-
-TEST(collision_core, mesh_distance)
-{
- std::vector<Vec3f> p1, p2;
- std::vector<Triangle> t1, t2;
- loadOBJFile("test/env.obj", p1, t1);
- loadOBJFile("test/rob.obj", p2, t2);
-
- std::vector<Transform> transforms; // t0
- std::vector<Transform> transforms2; // t1
- std::vector<Transform> transforms_ccd; // t0
- std::vector<Transform> transforms_ccd2; // t1
- BVH_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
- BVH_REAL delta_trans[] = {1, 1, 1};
- int n = 10;
-
- generateRandomTransform(extents, transforms, transforms2, delta_trans, 0.005 * 2 * 3.1415, n);
-
- DistanceRes res, res_now;
- for(unsigned int i = 0; i < transforms.size(); ++i)
- {
-#if USE_PQP
- distance_PQP(transforms[i], p1, t1, p2, t2, res, verbose);
-
- distance_PQP2(transforms[i], p1, t1, p2, t2, res_now, verbose);
-
- ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
- ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
-#endif
-
-#if USE_PQP
- distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);
- ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
- ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
-#else
- distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res, verbose);
-#endif
-
- distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
-
- ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
- ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
-
- distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);
-
- ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
- ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
-
- distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);
-
- ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
- ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
-
- distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);
-
- ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
- ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
-
- distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);
-
- ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
- ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
-
- distance_Test2<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);
-
- ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
- ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
-
- distance_Test2<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
-
- ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
- ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
-
- distance_Test2<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);
-
- ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
- ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
-
- distance_Test2<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);
-
- ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
- ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
-
- distance_Test2<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);
-
- ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
- ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
-
- distance_Test2<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);
-
- ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
- ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
- }
-}
-
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
-void distance_Test(const Transform& tf,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method,
- int qsize,
- DistanceRes& distance_result,
- bool verbose)
-{
- BVHModel<RSS> m1;
- BVHModel<RSS> m2;
- m1.bv_splitter.reset(new BVSplitter<RSS>(split_method));
- m2.bv_splitter.reset(new BVSplitter<RSS>(split_method));
-
-
- m1.beginModel();
- m1.addSubModel(vertices1, triangles1);
- m1.endModel();
-
- m2.beginModel();
- m2.addSubModel(vertices2, triangles2);
- m2.endModel();
-
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
- Vec3f T2;
-
-
- MeshDistanceTraversalNodeRSS node;
-
- if(!initialize(node, (const BVHModel<RSS>&)m1, (const BVHModel<RSS>&)m2, tf.R, tf.T, R2, T2))
- std::cout << "initialize error" << std::endl;
-
- node.enable_statistics = verbose;
-
- distance(&node, NULL, qsize);
-
- // points are in local coordinate, to global coordinate
- Vec3f p1 = MxV(tf.R, node.p1) + tf.T;
- Vec3f p2 = MxV(R2, node.p2) + T2;
-
-
- distance_result.distance = node.min_distance;
- distance_result.p1 = p1;
- distance_result.p2 = p2;
-
- if(verbose)
- {
- std::cout << "distance " << node.min_distance << std::endl;
-
- std::cout << p1[0] << " " << p1[1] << " " << p1[2] << std::endl;
- std::cout << p2[0] << " " << p2[1] << " " << p2[2] << std::endl;
- std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
- }
-}
-
-template<typename BV>
-void distance_Test2(const Transform& tf,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method,
- int qsize,
- DistanceRes& distance_result,
- bool verbose)
-{
- BVHModel<BV> m1;
- BVHModel<BV> m2;
- m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
- m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
-
-
- m1.beginModel();
- m1.addSubModel(vertices1, triangles1);
- m1.endModel();
-
- m2.beginModel();
- m2.addSubModel(vertices2, triangles2);
- m2.endModel();
-
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
- Vec3f T2;
-
- MeshDistanceTraversalNode<BV> node;
-
- if(!initialize<BV>(node, m1, m2, tf.R, tf.T, R2, T2))
- std::cout << "initialize error" << std::endl;
-
- node.enable_statistics = verbose;
-
- distance(&node, NULL, qsize);
-
- distance_result.distance = node.min_distance;
- distance_result.p1 = node.p1;
- distance_result.p2 = node.p2;
-
- if(verbose)
- {
- std::cout << "distance " << node.min_distance << std::endl;
-
- std::cout << node.p1[0] << " " << node.p1[1] << " " << node.p1[2] << std::endl;
- std::cout << node.p2[0] << " " << node.p2[1] << " " << node.p2[2] << std::endl;
- std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
- }
-}
diff --git a/branches/point_cloud/fcl/test/test_core_geometric_shapes.cpp b/branches/point_cloud/fcl/test/test_core_geometric_shapes.cpp
deleted file mode 100644
index 9af5e8d84d65cc09b138362dfc3159366d8185a2..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/test/test_core_geometric_shapes.cpp
+++ /dev/null
@@ -1,222 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#include "fcl/geometric_shapes_intersect.h"
-#include <gtest/gtest.h>
-
-using namespace fcl;
-
-TEST(shapeIntersection, spheresphere)
-{
- Sphere s1(20);
- Sphere s2(10);
-
- bool res;
-
- s2.setLocalTranslation(Vec3f(40, 0, 0));
- res = shapeIntersect(s1, s2);
- ASSERT_FALSE(res);
-
- s2.setLocalTranslation(Vec3f(30, 0, 0));
- res = shapeIntersect(s1, s2);
- ASSERT_FALSE(res);
-
- s2.setLocalTranslation(Vec3f(29.9, 0, 0));
- res = shapeIntersect(s1, s2);
- ASSERT_TRUE(res);
-
- s2.setLocalTranslation(Vec3f(0, 0, 0));
- res = shapeIntersect(s1, s2);
- ASSERT_TRUE(res);
-
- s2.setLocalTranslation(Vec3f(-29.9, 0, 0));
- res = shapeIntersect(s1, s2);
- ASSERT_TRUE(res);
-
- s2.setLocalTranslation(Vec3f(-30, 0, 0));
- res = shapeIntersect(s1, s2);
- ASSERT_FALSE(res);
-}
-
-TEST(shapeIntersection, boxbox)
-{
- Box s1(20, 40, 50);
- Box s2(10, 10, 10);
-
- bool res;
-
- res = shapeIntersect(s1, s2);
- ASSERT_TRUE(res);
-
- s2.setLocalTranslation(Vec3f(15, 0, 0));
- res = shapeIntersect(s1, s2);
- ASSERT_FALSE(res);
-
- SimpleQuaternion q;
- q.fromAxisAngle(Vec3f(0, 0, 1), (BVH_REAL)3.140 / 6);
- Vec3f R[3];
- q.toRotation(R);
- s2.setLocalRotation(R);
- res = shapeIntersect(s1, s2);
- ASSERT_TRUE(res);
-}
-
-TEST(shapeIntersection, spherebox)
-{
- Sphere s1(20);
- Box s2(5, 5, 5);
-
- bool res;
-
- res = shapeIntersect(s1, s2);
- ASSERT_TRUE(res);
-
- s2.setLocalTranslation(Vec3f(22.5, 0, 0));
- res = shapeIntersect(s1, s2);
- ASSERT_FALSE(res);
-
- s2.setLocalTranslation(Vec3f(22.4, 0, 0));
- res = shapeIntersect(s1, s2);
- ASSERT_TRUE(res);
-}
-
-TEST(shapeIntersection, cylindercylinder)
-{
- Cylinder s1(5, 10);
- Cylinder s2(5, 10);
-
- bool res;
-
- res = shapeIntersect(s1, s2);
- ASSERT_TRUE(res);
-
- s2.setLocalTranslation(Vec3f(9.9, 0, 0));
- res = shapeIntersect(s1, s2);
- ASSERT_TRUE(res);
-
- s2.setLocalTranslation(Vec3f(10, 0, 0));
- res = shapeIntersect(s1, s2);
- ASSERT_FALSE(res);
-}
-
-TEST(shapeIntersection, conecone)
-{
- Cone s1(5, 10);
- Cone s2(5, 10);
-
- bool res;
-
- res = shapeIntersect(s1, s2);
- ASSERT_TRUE(res);
-
- s2.setLocalTranslation(Vec3f(9.9, 0, 0));
- res = shapeIntersect(s1, s2);
- ASSERT_TRUE(res);
-
- s2.setLocalTranslation(Vec3f(10, 0, 0));
- res = shapeIntersect(s1, s2);
- ASSERT_FALSE(res);
-
- s2.setLocalTranslation(Vec3f(0, 0, 9.9));
- res = shapeIntersect(s1, s2);
- ASSERT_TRUE(res);
-
- s2.setLocalTranslation(Vec3f(0, 0, 10));
- res = shapeIntersect(s1, s2);
- ASSERT_FALSE(res);
-}
-
-TEST(shapeIntersection, conecylinder)
-{
- Cylinder s1(5, 10);
- Cone s2(5, 10);
-
- bool res;
-
- res = shapeIntersect(s1, s2);
- ASSERT_TRUE(res);
-
- s2.setLocalTranslation(Vec3f(9.9, 0, 0));
- res = shapeIntersect(s1, s2);
- ASSERT_TRUE(res);
-
- s2.setLocalTranslation(Vec3f(10, 0, 0));
- res = shapeIntersect(s1, s2);
- ASSERT_FALSE(res);
-
- s2.setLocalTranslation(Vec3f(0, 0, 9.9));
- res = shapeIntersect(s1, s2);
- ASSERT_TRUE(res);
-
- s2.setLocalTranslation(Vec3f(0, 0, 10));
- res = shapeIntersect(s1, s2);
- ASSERT_FALSE(res);
-}
-
-TEST(shapeIntersection, spheretriangle)
-{
- Sphere s(10);
- Vec3f t[3];
- t[0] = Vec3f(20, 0, 0);
- t[1] = Vec3f(-20, 0, 0);
- t[2] = Vec3f(0, 20, 0);
-
- bool res;
-
- res = shapeTriangleIntersect(s, t[0], t[1], t[2]);
- ASSERT_TRUE(res);
-
- t[0] = Vec3f(30, 0, 0);
- t[1] = Vec3f(10, -20, 0);
- t[2] = Vec3f(10, 20, 0);
- res = shapeTriangleIntersect(s, t[0], t[1], t[2]);
- ASSERT_FALSE(res);
-
- t[0] = Vec3f(30, 0, 0);
- t[1] = Vec3f(9.9, -20, 0);
- t[2] = Vec3f(9.9, 20, 0);
- res = shapeTriangleIntersect(s, t[0], t[1], t[2]);
- ASSERT_TRUE(res);
-
-}
-
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/branches/point_cloud/fcl/test/test_core_utility.h b/branches/point_cloud/fcl/test/test_core_utility.h
deleted file mode 100644
index d2b410a41f219bec8001a92d9e2b05f4097d4dc7..0000000000000000000000000000000000000000
--- a/branches/point_cloud/fcl/test/test_core_utility.h
+++ /dev/null
@@ -1,640 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, 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 Jia Pan */
-
-
-#ifndef FCL_TEST_CORE_UTILITY_H
-#define FCL_TEST_CORE_UTILITY_H
-
-#include "fcl/vec_3f.h"
-#include <cstdio>
-using namespace fcl;
-
-#if USE_PQP
-#include <PQP.h>
-#endif
-
-struct Transform
-{
- Vec3f R[3];
- Vec3f T;
-};
-
-
-#if USE_PQP
-
-struct CollisionPairComp_PQP
-{
- bool operator()(const CollisionPair& a, const CollisionPair& b)
- {
- if(a.id1 == b.id1)
- return a.id2 < b.id2;
- return a.id1 < b.id1;
- }
-};
-
-inline void sortCollisionPair_PQP(CollisionPair* pairs, int n)
-{
- std::vector<CollisionPair> pair_array(n);
- for(int i = 0; i < n; ++i)
- pair_array[i] = pairs[i];
-
- std::sort(pair_array.begin(), pair_array.end(), CollisionPairComp_PQP());
-
- for(int i = 0; i < n; ++i)
- pairs[i] = pair_array[i];
-}
-
-inline bool collide_PQP(const Transform& tf,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
- std::vector<std::pair<int, int> >& results,
- bool verbose = true)
-{
- PQP_Model m1, m2;
-
- m1.BeginModel();
- for(unsigned int i = 0; i < triangles1.size(); ++i)
- {
- const Triangle& t = triangles1[i];
- const Vec3f& p1_ = vertices1[t[0]];
- const Vec3f& p2_ = vertices1[t[1]];
- const Vec3f& p3_ = vertices1[t[2]];
-
- Vec3f p1 = MxV(tf.R, p1_) + tf.T;
- Vec3f p2 = MxV(tf.R, p2_) + tf.T;
- Vec3f p3 = MxV(tf.R, p3_) + tf.T;
-
- PQP_REAL q1[3];
- PQP_REAL q2[3];
- PQP_REAL q3[3];
-
- q1[0] = p1[0]; q1[1] = p1[1]; q1[2] = p1[2];
- q2[0] = p2[0]; q2[1] = p2[1]; q2[2] = p2[2];
- q3[0] = p3[0]; q3[1] = p3[1]; q3[2] = p3[2];
-
- m1.AddTri(q1, q2, q3, i);
- }
-
- m1.EndModel();
-
-
- m2.BeginModel();
- for(unsigned int i = 0; i < triangles2.size(); ++i)
- {
- const Triangle& t = triangles2[i];
- const Vec3f& p1 = vertices2[t[0]];
- const Vec3f& p2 = vertices2[t[1]];
- const Vec3f& p3 = vertices2[t[2]];
-
- PQP_REAL q1[3];
- PQP_REAL q2[3];
- PQP_REAL q3[3];
-
- q1[0] = p1[0]; q1[1] = p1[1]; q1[2] = p1[2];
- q2[0] = p2[0]; q2[1] = p2[1]; q2[2] = p2[2];
- q3[0] = p3[0]; q3[1] = p3[1]; q3[2] = p3[2];
-
- m2.AddTri(q1, q2, q3, i);
- }
-
- m2.EndModel();
-
-
- PQP_CollideResult res;
- PQP_REAL R1[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
- PQP_REAL R2[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
- PQP_REAL T1[3] = {0, 0, 0};
- PQP_REAL T2[3] = {0, 0, 0};
- PQP_Collide(&res, R1, T1, &m1, R2, T2, &m2);
-
- if(res.NumPairs() > 0)
- {
- sortCollisionPair_PQP(res.pairs, res.NumPairs());
- results.resize(res.NumPairs());
-
- for(int i = 0; i < res.NumPairs(); ++i)
- {
- results[i] = std::make_pair(res.Id1(i), res.Id2(i));
- }
-
- if(verbose)
- std::cout << "in collision " << res.NumPairs() << ": " << std::endl;
- if(verbose) std::cout << res.num_bv_tests << " " << res.num_tri_tests << std::endl;
- return true;
- }
- else
- {
- if(verbose) std::cout << "collision free " << std::endl;
- if(verbose) std::cout << res.num_bv_tests << " " << res.num_tri_tests << std::endl;
- return false;
- }
-}
-
-
-inline bool collide_PQP2(const Transform& tf,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
- std::vector<std::pair<int, int> >& results, bool verbose = true)
-{
- PQP_Model m1, m2;
-
- m1.BeginModel();
- for(unsigned int i = 0; i < triangles1.size(); ++i)
- {
- const Triangle& t = triangles1[i];
- const Vec3f& p1 = vertices1[t[0]];
- const Vec3f& p2 = vertices1[t[1]];
- const Vec3f& p3 = vertices1[t[2]];
-
- PQP_REAL q1[3];
- PQP_REAL q2[3];
- PQP_REAL q3[3];
-
- q1[0] = p1[0]; q1[1] = p1[1]; q1[2] = p1[2];
- q2[0] = p2[0]; q2[1] = p2[1]; q2[2] = p2[2];
- q3[0] = p3[0]; q3[1] = p3[1]; q3[2] = p3[2];
-
- m1.AddTri(q1, q2, q3, i);
- }
-
- m1.EndModel();
-
-
- m2.BeginModel();
- for(unsigned int i = 0; i < triangles2.size(); ++i)
- {
- const Triangle& t = triangles2[i];
- const Vec3f& p1 = vertices2[t[0]];
- const Vec3f& p2 = vertices2[t[1]];
- const Vec3f& p3 = vertices2[t[2]];
-
- PQP_REAL q1[3];
- PQP_REAL q2[3];
- PQP_REAL q3[3];
-
- q1[0] = p1[0]; q1[1] = p1[1]; q1[2] = p1[2];
- q2[0] = p2[0]; q2[1] = p2[1]; q2[2] = p2[2];
- q3[0] = p3[0]; q3[1] = p3[1]; q3[2] = p3[2];
-
- m2.AddTri(q1, q2, q3, i);
- }
-
- m2.EndModel();
-
-
- PQP_CollideResult res;
- PQP_REAL R1[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
- PQP_REAL R2[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
- PQP_REAL T1[3] = {0, 0, 0};
- PQP_REAL T2[3] = {0, 0, 0};
- T1[0] = tf.T[0];
- T1[1] = tf.T[1];
- T1[2] = tf.T[2];
- for(int i = 0; i < 3; ++i)
- {
- R1[i][0] = tf.R[i][0];
- R1[i][1] = tf.R[i][1];
- R1[i][2] = tf.R[i][2];
- }
-
- PQP_Collide(&res, R1, T1, &m1, R2, T2, &m2);
-
- if(res.NumPairs() > 0)
- {
- sortCollisionPair_PQP(res.pairs, res.NumPairs());
- results.resize(res.NumPairs());
- for(int i = 0; i < res.NumPairs(); ++i)
- {
- results[i] = std::make_pair(res.Id1(i), res.Id2(i));
- }
-
- if(verbose)
- std::cout << "in collision " << res.NumPairs() << ": " << std::endl;
- if(verbose) std::cout << res.num_bv_tests << " " << res.num_tri_tests << std::endl;
- return true;
- }
- else
- {
- if(verbose) std::cout << "collision free " << std::endl;
- if(verbose) std::cout << res.num_bv_tests << " " << res.num_tri_tests << std::endl;
- return false;
- }
-}
-
-#endif
-
-
-struct DistanceRes
-{
- double distance;
- Vec3f p1;
- Vec3f p2;
-};
-
-#if USE_PQP
-inline bool distance_PQP(const Transform& tf,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
- DistanceRes& distance_result,
- bool verbose = true)
-{
- PQP_Model m1, m2;
-
- m1.BeginModel();
- for(unsigned int i = 0; i < triangles1.size(); ++i)
- {
- const Triangle& t = triangles1[i];
- const Vec3f& p1_ = vertices1[t[0]];
- const Vec3f& p2_ = vertices1[t[1]];
- const Vec3f& p3_ = vertices1[t[2]];
-
- Vec3f p1 = MxV(tf.R, p1_) + tf.T;
- Vec3f p2 = MxV(tf.R, p2_) + tf.T;
- Vec3f p3 = MxV(tf.R, p3_) + tf.T;
-
- PQP_REAL q1[3];
- PQP_REAL q2[3];
- PQP_REAL q3[3];
-
- q1[0] = p1[0]; q1[1] = p1[1]; q1[2] = p1[2];
- q2[0] = p2[0]; q2[1] = p2[1]; q2[2] = p2[2];
- q3[0] = p3[0]; q3[1] = p3[1]; q3[2] = p3[2];
-
- m1.AddTri(q1, q2, q3, i);
- }
-
- m1.EndModel();
-
-
- m2.BeginModel();
- for(unsigned int i = 0; i < triangles2.size(); ++i)
- {
- const Triangle& t = triangles2[i];
- const Vec3f& p1 = vertices2[t[0]];
- const Vec3f& p2 = vertices2[t[1]];
- const Vec3f& p3 = vertices2[t[2]];
-
- PQP_REAL q1[3];
- PQP_REAL q2[3];
- PQP_REAL q3[3];
-
- q1[0] = p1[0]; q1[1] = p1[1]; q1[2] = p1[2];
- q2[0] = p2[0]; q2[1] = p2[1]; q2[2] = p2[2];
- q3[0] = p3[0]; q3[1] = p3[1]; q3[2] = p3[2];
-
- m2.AddTri(q1, q2, q3, i);
- }
-
- m2.EndModel();
-
-
- PQP_DistanceResult res;
- PQP_REAL R1[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
- PQP_REAL R2[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
- PQP_REAL T1[3] = {0, 0, 0};
- PQP_REAL T2[3] = {0, 0, 0};
-
- PQP_Distance(&res, R1, T1, &m1, R2, T2, &m2, 0.01, 0.01);
-
-
- distance_result.distance = res.distance;
- distance_result.p1 = Vec3f(res.p1[0], res.p1[1], res.p1[2]);
- distance_result.p2 = Vec3f(res.p2[0], res.p2[1], res.p2[2]);
-
-
- if(verbose)
- {
- std::cout << "distance " << res.distance << std::endl;
- std::cout << res.p1[0] << " " << res.p1[1] << " " << res.p1[2] << std::endl;
- std::cout << res.p2[0] << " " << res.p2[1] << " " << res.p2[2] << std::endl;
- std::cout << res.num_bv_tests << " " << res.num_tri_tests << std::endl;
- }
-
- return true;
-}
-
-
-inline bool distance_PQP2(const Transform& tf,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
- DistanceRes& distance_result, bool verbose = true)
-{
- PQP_Model m1, m2;
-
- m1.BeginModel();
- for(unsigned int i = 0; i < triangles1.size(); ++i)
- {
- const Triangle& t = triangles1[i];
- const Vec3f& p1 = vertices1[t[0]];
- const Vec3f& p2 = vertices1[t[1]];
- const Vec3f& p3 = vertices1[t[2]];
-
- PQP_REAL q1[3];
- PQP_REAL q2[3];
- PQP_REAL q3[3];
-
- q1[0] = p1[0]; q1[1] = p1[1]; q1[2] = p1[2];
- q2[0] = p2[0]; q2[1] = p2[1]; q2[2] = p2[2];
- q3[0] = p3[0]; q3[1] = p3[1]; q3[2] = p3[2];
-
- m1.AddTri(q1, q2, q3, i);
- }
-
- m1.EndModel();
-
-
- m2.BeginModel();
- for(unsigned int i = 0; i < triangles2.size(); ++i)
- {
- const Triangle& t = triangles2[i];
- const Vec3f& p1 = vertices2[t[0]];
- const Vec3f& p2 = vertices2[t[1]];
- const Vec3f& p3 = vertices2[t[2]];
-
- PQP_REAL q1[3];
- PQP_REAL q2[3];
- PQP_REAL q3[3];
-
- q1[0] = p1[0]; q1[1] = p1[1]; q1[2] = p1[2];
- q2[0] = p2[0]; q2[1] = p2[1]; q2[2] = p2[2];
- q3[0] = p3[0]; q3[1] = p3[1]; q3[2] = p3[2];
-
- m2.AddTri(q1, q2, q3, i);
- }
-
- m2.EndModel();
-
-
- PQP_DistanceResult res;
- PQP_REAL R1[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
- PQP_REAL R2[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
- PQP_REAL T1[3] = {0, 0, 0};
- PQP_REAL T2[3] = {0, 0, 0};
-
- T1[0] = tf.T[0];
- T1[1] = tf.T[1];
- T1[2] = tf.T[2];
- for(int i = 0; i < 3; ++i)
- {
- R1[i][0] = tf.R[i][0];
- R1[i][1] = tf.R[i][1];
- R1[i][2] = tf.R[i][2];
- }
-
- PQP_Distance(&res, R1, T1, &m1, R2, T2, &m2, 0.01, 0.01);
-
-
- Vec3f p1_temp(res.p1[0], res.p1[1], res.p1[2]);
- Vec3f p2_temp(res.p2[0], res.p2[1], res.p2[2]);
-
- Vec3f p1 = MxV(tf.R, p1_temp) + tf.T;
- Vec3f p2 = p2_temp;
-
-
- distance_result.distance = res.distance;
- distance_result.p1 = p1;
- distance_result.p2 = p2;
-
-
- if(verbose)
- {
- std::cout << "distance " << res.distance << std::endl;
- std::cout << p1[0] << " " << p1[1] << " " << p1[2] << std::endl;
- std::cout << p2[0] << " " << p2[1] << " " << p2[2] << std::endl;
- std::cout << res.num_bv_tests << " " << res.num_tri_tests << std::endl;
- }
-
- return true;
-}
-
-#endif
-
-inline BVH_REAL rand_interval(BVH_REAL rmin, BVH_REAL rmax)
-{
- BVH_REAL t = rand() / ((BVH_REAL)RAND_MAX + 1);
- return (t * (rmax - rmin) + rmin);
-}
-
-inline void loadOBJFile(const char* filename, std::vector<Vec3f>& points, std::vector<Triangle>& triangles)
-{
-
- FILE* file = fopen(filename, "rb");
- if(!file)
- {
- std::cerr << "file not exist" << std::endl;
- return;
- }
-
- bool has_normal = false;
- bool has_texture = false;
- char line_buffer[2000];
- while(fgets(line_buffer, 2000, file))
- {
- char* first_token = strtok(line_buffer, "\r\n\t ");
- if(!first_token || first_token[0] == '#' || first_token[0] == 0)
- continue;
-
- switch(first_token[0])
- {
- case 'v':
- {
- if(first_token[1] == 'n')
- {
- strtok(NULL, "\t ");
- strtok(NULL, "\t ");
- strtok(NULL, "\t ");
- has_normal = true;
- }
- else if(first_token[1] == 't')
- {
- strtok(NULL, "\t ");
- strtok(NULL, "\t ");
- has_texture = true;
- }
- else
- {
- BVH_REAL x = (BVH_REAL)atof(strtok(NULL, "\t "));
- BVH_REAL y = (BVH_REAL)atof(strtok(NULL, "\t "));
- BVH_REAL z = (BVH_REAL)atof(strtok(NULL, "\t "));
- Vec3f p(x, y, z);
- points.push_back(p);
- }
- }
- break;
- case 'f':
- {
- Triangle tri;
- char* data[30];
- int n = 0;
- while((data[n] = strtok(NULL, "\t \r\n")) != NULL)
- {
- if(strlen(data[n]))
- n++;
- }
-
- for(int t = 0; t < (n - 2); ++t)
- {
- if((!has_texture) && (!has_normal))
- {
- tri[0] = atoi(data[0]) - 1;
- tri[1] = atoi(data[1]) - 1;
- tri[2] = atoi(data[2]) - 1;
- }
- else
- {
- const char *v1;
- for(int i = 0; i < 3; i++)
- {
- // vertex ID
- if(i == 0)
- v1 = data[0];
- else
- v1 = data[t + i];
-
- tri[i] = atoi(v1) - 1;
-
- // texture coordinate
- const char *v2 = strchr(v1 + 1, '/');
-
- if(v2)
- {
- strchr(v2 + 1, '/');
- }
- }
- }
- triangles.push_back(tri);
- }
- }
- }
- }
-}
-
-
-inline void eulerToMatrix(BVH_REAL a, BVH_REAL b, BVH_REAL c, Vec3f R[3])
-{
- BVH_REAL c1 = cos(a);
- BVH_REAL c2 = cos(b);
- BVH_REAL c3 = cos(c);
- BVH_REAL s1 = sin(a);
- BVH_REAL s2 = sin(b);
- BVH_REAL s3 = sin(c);
-
- R[0] = Vec3f(c1 * c2, - c2 * s1, s2);
- R[1] = Vec3f(c3 * s1 + c1 * s2 * s3, c1 * c3 - s1 * s2 * s3, - c2 * s3);
- R[2] = Vec3f(s1 * s3 - c1 * c3 * s2, c3 * s1 * s2 + c1 * s3, c2 * c3);
-}
-
-inline void generateRandomTransform(BVH_REAL extents[6], std::vector<Transform>& transforms, std::vector<Transform>& transforms2, BVH_REAL delta_trans[3], BVH_REAL delta_rot, int n)
-{
- transforms.resize(n);
- transforms2.resize(n);
- for(int i = 0; i < n; ++i)
- {
- BVH_REAL x = rand_interval(extents[0], extents[3]);
- BVH_REAL y = rand_interval(extents[1], extents[4]);
- BVH_REAL z = rand_interval(extents[2], extents[5]);
-
- const BVH_REAL pi = 3.1415926;
- BVH_REAL a = rand_interval(0, 2 * pi);
- BVH_REAL b = rand_interval(0, 2 * pi);
- BVH_REAL c = rand_interval(0, 2 * pi);
-
- eulerToMatrix(a, b, c, transforms[i].R);
- transforms[i].T = Vec3f(x, y, z);
-
- BVH_REAL deltax = rand_interval(-delta_trans[0], delta_trans[0]);
- BVH_REAL deltay = rand_interval(-delta_trans[1], delta_trans[1]);
- BVH_REAL deltaz = rand_interval(-delta_trans[2], delta_trans[2]);
-
- BVH_REAL deltaa = rand_interval(-delta_rot, delta_rot);
- BVH_REAL deltab = rand_interval(-delta_rot, delta_rot);
- BVH_REAL deltac = rand_interval(-delta_rot, delta_rot);
-
- eulerToMatrix(a + deltaa, b + deltab, c + deltac, transforms2[i].R);
- transforms2[i].T = Vec3f(x + deltax, y + deltay, z + deltaz);
- }
-}
-
-
-inline void generateRandomTransform_ccd(BVH_REAL extents[6], std::vector<Transform>& transforms, std::vector<Transform>& transforms2, BVH_REAL delta_trans[3], BVH_REAL delta_rot, int n,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2)
-{
- transforms.resize(n);
- transforms2.resize(n);
-
- for(int i = 0; i < n;)
- {
- BVH_REAL x = rand_interval(extents[0], extents[3]);
- BVH_REAL y = rand_interval(extents[1], extents[4]);
- BVH_REAL z = rand_interval(extents[2], extents[5]);
-
- const BVH_REAL pi = 3.1415926;
- BVH_REAL a = rand_interval(0, 2 * pi);
- BVH_REAL b = rand_interval(0, 2 * pi);
- BVH_REAL c = rand_interval(0, 2 * pi);
-
- Transform tf;
-
- eulerToMatrix(a, b, c, tf.R);
- tf.T = Vec3f(x, y, z);
-
- std::vector<std::pair<int, int> > results;
-#if USE_PQP
- if(!collide_PQP(tf, vertices1, triangles1, vertices2, triangles2, results, false))
-#endif
- {
- transforms[i] = tf;
-
- BVH_REAL deltax = rand_interval(-delta_trans[0], delta_trans[0]);
- BVH_REAL deltay = rand_interval(-delta_trans[1], delta_trans[1]);
- BVH_REAL deltaz = rand_interval(-delta_trans[2], delta_trans[2]);
-
- BVH_REAL deltaa = rand_interval(-delta_rot, delta_rot);
- BVH_REAL deltab = rand_interval(-delta_rot, delta_rot);
- BVH_REAL deltac = rand_interval(-delta_rot, delta_rot);
-
- eulerToMatrix(a + deltaa, b + deltab, c + deltac, transforms2[i].R);
- transforms2[i].T = Vec3f(x + deltax, y + deltay, z + deltaz);
- ++i;
- }
- }
-}
-
-
-
-
-#endif
diff --git a/branches/point_cloud/libccd/Makefile b/branches/point_cloud/libccd/Makefile
deleted file mode 100644
index 707e57b882b1a2e642b1246811c828123ae98dc1..0000000000000000000000000000000000000000
--- a/branches/point_cloud/libccd/Makefile
+++ /dev/null
@@ -1,36 +0,0 @@
-all: installed
-
-#
-# Download, extract and compile from a released tarball:
-#
-TARBALL = build/libccd_1.0.tar.gz
-TARBALL_URL = http://libccd.danfis.cz/files/libccd-1.0.tar.gz
-TARBALL_PATCH = fPIC.diff
-INITIAL_DIR = build/libccd-1.0
-SOURCE_DIR = build/libccd-tar
-include $(shell rospack find mk)/download_unpack_build.mk
-
-INSTALL_DIR = libccd
-CMAKE = cmake
-CMAKE_ARGS = -D CMAKE_BUILD_TYPE="Release" -D CMAKE_INSTALL_PREFIX=`rospack find libccd`/$(INSTALL_DIR)
-MAKE = make
-
-installed: wiped $(SOURCE_DIR)/unpacked
- cd $(SOURCE_DIR)/src && make $(ROS_PARALLEL_JOBS)
- mkdir -p $(INSTALL_DIR)/lib
- mkdir -p $(INSTALL_DIR)/include
- mkdir -p $(INSTALL_DIR)/include/ccd
- cp -r $(SOURCE_DIR)/src/ccd/*.h $(INSTALL_DIR)/include/ccd
- cp -r $(SOURCE_DIR)/src/*.a $(INSTALL_DIR)/lib
- touch installed
-
-clean:
- rm -rf build
- rm -rf $(INSTALL_DIR) installed
-
-wiped: Makefile
- make wipe
- touch wiped
-
-wipe: clean
- rm -rf build patched
diff --git a/branches/point_cloud/libccd/fPIC.diff b/branches/point_cloud/libccd/fPIC.diff
deleted file mode 100644
index 042b73b7ae741e7eff746bcf8c26f3f4bc7340c6..0000000000000000000000000000000000000000
--- a/branches/point_cloud/libccd/fPIC.diff
+++ /dev/null
@@ -1,11 +0,0 @@
---- src/Makefile 2010-11-04 09:46:32.000000000 -0700
-+++ src/Makefile 2011-08-25 14:49:52.000000000 -0700
-@@ -17,7 +17,7 @@
-
- -include Makefile.include
-
--CFLAGS += -I.
-+CFLAGS += -I. -fPIC
-
- TARGETS = libccd.a
- OBJS = ccd.o mpr.o support.o vec3.o alloc.o polytope.o
diff --git a/branches/point_cloud/libccd/mainpage.dox b/branches/point_cloud/libccd/mainpage.dox
deleted file mode 100644
index 3231eae26c1ff9cf702431f9803235d6cdcceb5c..0000000000000000000000000000000000000000
--- a/branches/point_cloud/libccd/mainpage.dox
+++ /dev/null
@@ -1,26 +0,0 @@
-/**
-\mainpage
-\htmlinclude manifest.html
-
-\b libccd 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.
--->
-
-
-*/
diff --git a/branches/point_cloud/libccd/manifest.xml b/branches/point_cloud/libccd/manifest.xml
deleted file mode 100644
index 4b74422553d6155683c1eba7be217e6094cfd4bc..0000000000000000000000000000000000000000
--- a/branches/point_cloud/libccd/manifest.xml
+++ /dev/null
@@ -1,15 +0,0 @@
-<package>
- <description brief="libccd">
- This package is a wrapper on the libccd library available from <a href="http://libccd.danfis.cz/">here</a>. This package does not modify the contents of the original library in any manner and only wraps it for easy distribution with the ROS packaging system.
- </description>
- <author>Maintained by Jia Pan and Sachin Chitta</author>
- <license>BSD</license>
- <review status="unreviewed" notes=""/>
- <url>http://ros.org/wiki/libccd</url>
- <export>
- <cpp cflags="-I${prefix}/libccd/include" lflags="-L${prefix}/libccd/lib -Wl,-rpath,${prefix}/libccd/lib -lccd"/>
- </export>
-
-</package>
-
-
diff --git a/branches/point_cloud/stack.xml b/branches/point_cloud/stack.xml
deleted file mode 100644
index 693e310b050d210e7c377245e912dbb675a2c668..0000000000000000000000000000000000000000
--- a/branches/point_cloud/stack.xml
+++ /dev/null
@@ -1,12 +0,0 @@
-<stack>
- <description brief="fcl_ros">fcl_ros</description>
- <author>Maintained by Jia Pan, Dinesh Manocha, Sachin Chitta</author>
- <license>BSD</license>
- <review status="unreviewed" notes=""/>
- <url>http://ros.org/wiki/fcl_ros</url>
- <depend stack="arm_navigation" /> <!-- planning_models, geometric_shapes -->
- <depend stack="bullet" /> <!-- bullet -->
- <depend stack="ros" />
- <depend stack="ros_comm" /> <!-- rosconsole -->
-
-</stack>
diff --git a/trunk/fcl/fcl.pc.in b/fcl.pc.in
similarity index 100%
rename from trunk/fcl/fcl.pc.in
rename to fcl.pc.in
diff --git a/trunk/fcl/include/fcl/BV/AABB.h b/include/fcl/BV/AABB.h
similarity index 100%
rename from trunk/fcl/include/fcl/BV/AABB.h
rename to include/fcl/BV/AABB.h
diff --git a/trunk/fcl/include/fcl/BV/BV.h b/include/fcl/BV/BV.h
similarity index 100%
rename from trunk/fcl/include/fcl/BV/BV.h
rename to include/fcl/BV/BV.h
diff --git a/trunk/fcl/include/fcl/BV/BV_node.h b/include/fcl/BV/BV_node.h
similarity index 100%
rename from trunk/fcl/include/fcl/BV/BV_node.h
rename to include/fcl/BV/BV_node.h
diff --git a/trunk/fcl/include/fcl/BV/OBB.h b/include/fcl/BV/OBB.h
similarity index 100%
rename from trunk/fcl/include/fcl/BV/OBB.h
rename to include/fcl/BV/OBB.h
diff --git a/trunk/fcl/include/fcl/BV/OBBRSS.h b/include/fcl/BV/OBBRSS.h
similarity index 100%
rename from trunk/fcl/include/fcl/BV/OBBRSS.h
rename to include/fcl/BV/OBBRSS.h
diff --git a/trunk/fcl/include/fcl/BV/RSS.h b/include/fcl/BV/RSS.h
similarity index 100%
rename from trunk/fcl/include/fcl/BV/RSS.h
rename to include/fcl/BV/RSS.h
diff --git a/trunk/fcl/include/fcl/BV/kDOP.h b/include/fcl/BV/kDOP.h
similarity index 100%
rename from trunk/fcl/include/fcl/BV/kDOP.h
rename to include/fcl/BV/kDOP.h
diff --git a/trunk/fcl/include/fcl/BV/kIOS.h b/include/fcl/BV/kIOS.h
similarity index 100%
rename from trunk/fcl/include/fcl/BV/kIOS.h
rename to include/fcl/BV/kIOS.h
diff --git a/trunk/fcl/include/fcl/BVH/BVH_front.h b/include/fcl/BVH/BVH_front.h
similarity index 100%
rename from trunk/fcl/include/fcl/BVH/BVH_front.h
rename to include/fcl/BVH/BVH_front.h
diff --git a/trunk/fcl/include/fcl/BVH/BVH_internal.h b/include/fcl/BVH/BVH_internal.h
similarity index 100%
rename from trunk/fcl/include/fcl/BVH/BVH_internal.h
rename to include/fcl/BVH/BVH_internal.h
diff --git a/trunk/fcl/include/fcl/BVH/BVH_model.h b/include/fcl/BVH/BVH_model.h
similarity index 100%
rename from trunk/fcl/include/fcl/BVH/BVH_model.h
rename to include/fcl/BVH/BVH_model.h
diff --git a/trunk/fcl/include/fcl/BVH/BVH_utility.h b/include/fcl/BVH/BVH_utility.h
similarity index 100%
rename from trunk/fcl/include/fcl/BVH/BVH_utility.h
rename to include/fcl/BVH/BVH_utility.h
diff --git a/trunk/fcl/include/fcl/BVH/BV_fitter.h b/include/fcl/BVH/BV_fitter.h
similarity index 100%
rename from trunk/fcl/include/fcl/BVH/BV_fitter.h
rename to include/fcl/BVH/BV_fitter.h
diff --git a/trunk/fcl/include/fcl/BVH/BV_splitter.h b/include/fcl/BVH/BV_splitter.h
similarity index 100%
rename from trunk/fcl/include/fcl/BVH/BV_splitter.h
rename to include/fcl/BVH/BV_splitter.h
diff --git a/trunk/fcl/include/fcl/CMakeLists.txt b/include/fcl/CMakeLists.txt
similarity index 100%
rename from trunk/fcl/include/fcl/CMakeLists.txt
rename to include/fcl/CMakeLists.txt
diff --git a/trunk/fcl/include/fcl/articulated_model/joint.h b/include/fcl/articulated_model/joint.h
similarity index 100%
rename from trunk/fcl/include/fcl/articulated_model/joint.h
rename to include/fcl/articulated_model/joint.h
diff --git a/trunk/fcl/include/fcl/articulated_model/joint_config.h b/include/fcl/articulated_model/joint_config.h
similarity index 100%
rename from trunk/fcl/include/fcl/articulated_model/joint_config.h
rename to include/fcl/articulated_model/joint_config.h
diff --git a/trunk/fcl/include/fcl/articulated_model/link.h b/include/fcl/articulated_model/link.h
similarity index 100%
rename from trunk/fcl/include/fcl/articulated_model/link.h
rename to include/fcl/articulated_model/link.h
diff --git a/trunk/fcl/include/fcl/articulated_model/model.h b/include/fcl/articulated_model/model.h
similarity index 100%
rename from trunk/fcl/include/fcl/articulated_model/model.h
rename to include/fcl/articulated_model/model.h
diff --git a/trunk/fcl/include/fcl/articulated_model/model_config.h b/include/fcl/articulated_model/model_config.h
similarity index 100%
rename from trunk/fcl/include/fcl/articulated_model/model_config.h
rename to include/fcl/articulated_model/model_config.h
diff --git a/trunk/fcl/include/fcl/broadphase/broadphase.h b/include/fcl/broadphase/broadphase.h
similarity index 100%
rename from trunk/fcl/include/fcl/broadphase/broadphase.h
rename to include/fcl/broadphase/broadphase.h
diff --git a/trunk/fcl/include/fcl/broadphase/broadphase_SSaP.h b/include/fcl/broadphase/broadphase_SSaP.h
similarity index 100%
rename from trunk/fcl/include/fcl/broadphase/broadphase_SSaP.h
rename to include/fcl/broadphase/broadphase_SSaP.h
diff --git a/trunk/fcl/include/fcl/broadphase/broadphase_SaP.h b/include/fcl/broadphase/broadphase_SaP.h
similarity index 100%
rename from trunk/fcl/include/fcl/broadphase/broadphase_SaP.h
rename to include/fcl/broadphase/broadphase_SaP.h
diff --git a/trunk/fcl/include/fcl/broadphase/broadphase_bruteforce.h b/include/fcl/broadphase/broadphase_bruteforce.h
similarity index 100%
rename from trunk/fcl/include/fcl/broadphase/broadphase_bruteforce.h
rename to include/fcl/broadphase/broadphase_bruteforce.h
diff --git a/trunk/fcl/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h b/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h
similarity index 100%
rename from trunk/fcl/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h
rename to include/fcl/broadphase/broadphase_dynamic_AABB_tree.h
diff --git a/trunk/fcl/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h b/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h
similarity index 100%
rename from trunk/fcl/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h
rename to include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h
diff --git a/trunk/fcl/include/fcl/broadphase/broadphase_interval_tree.h b/include/fcl/broadphase/broadphase_interval_tree.h
similarity index 100%
rename from trunk/fcl/include/fcl/broadphase/broadphase_interval_tree.h
rename to include/fcl/broadphase/broadphase_interval_tree.h
diff --git a/trunk/fcl/include/fcl/broadphase/broadphase_spatialhash.h b/include/fcl/broadphase/broadphase_spatialhash.h
similarity index 100%
rename from trunk/fcl/include/fcl/broadphase/broadphase_spatialhash.h
rename to include/fcl/broadphase/broadphase_spatialhash.h
diff --git a/trunk/fcl/include/fcl/broadphase/broadphase_spatialhash.hxx b/include/fcl/broadphase/broadphase_spatialhash.hxx
similarity index 100%
rename from trunk/fcl/include/fcl/broadphase/broadphase_spatialhash.hxx
rename to include/fcl/broadphase/broadphase_spatialhash.hxx
diff --git a/trunk/fcl/include/fcl/broadphase/hash.h b/include/fcl/broadphase/hash.h
similarity index 100%
rename from trunk/fcl/include/fcl/broadphase/hash.h
rename to include/fcl/broadphase/hash.h
diff --git a/trunk/fcl/include/fcl/broadphase/hierarchy_tree.h b/include/fcl/broadphase/hierarchy_tree.h
similarity index 100%
rename from trunk/fcl/include/fcl/broadphase/hierarchy_tree.h
rename to include/fcl/broadphase/hierarchy_tree.h
diff --git a/trunk/fcl/include/fcl/broadphase/hierarchy_tree.hxx b/include/fcl/broadphase/hierarchy_tree.hxx
similarity index 100%
rename from trunk/fcl/include/fcl/broadphase/hierarchy_tree.hxx
rename to include/fcl/broadphase/hierarchy_tree.hxx
diff --git a/trunk/fcl/include/fcl/broadphase/interval_tree.h b/include/fcl/broadphase/interval_tree.h
similarity index 100%
rename from trunk/fcl/include/fcl/broadphase/interval_tree.h
rename to include/fcl/broadphase/interval_tree.h
diff --git a/trunk/fcl/include/fcl/broadphase/morton.h b/include/fcl/broadphase/morton.h
similarity index 100%
rename from trunk/fcl/include/fcl/broadphase/morton.h
rename to include/fcl/broadphase/morton.h
diff --git a/trunk/fcl/include/fcl/ccd/conservative_advancement.h b/include/fcl/ccd/conservative_advancement.h
similarity index 100%
rename from trunk/fcl/include/fcl/ccd/conservative_advancement.h
rename to include/fcl/ccd/conservative_advancement.h
diff --git a/trunk/fcl/include/fcl/ccd/interpolation/interpolation.h b/include/fcl/ccd/interpolation/interpolation.h
similarity index 100%
rename from trunk/fcl/include/fcl/ccd/interpolation/interpolation.h
rename to include/fcl/ccd/interpolation/interpolation.h
diff --git a/trunk/fcl/include/fcl/ccd/interpolation/interpolation_factory.h b/include/fcl/ccd/interpolation/interpolation_factory.h
similarity index 100%
rename from trunk/fcl/include/fcl/ccd/interpolation/interpolation_factory.h
rename to include/fcl/ccd/interpolation/interpolation_factory.h
diff --git a/trunk/fcl/include/fcl/ccd/interpolation/interpolation_linear.h b/include/fcl/ccd/interpolation/interpolation_linear.h
similarity index 100%
rename from trunk/fcl/include/fcl/ccd/interpolation/interpolation_linear.h
rename to include/fcl/ccd/interpolation/interpolation_linear.h
diff --git a/trunk/fcl/include/fcl/ccd/interval.h b/include/fcl/ccd/interval.h
similarity index 100%
rename from trunk/fcl/include/fcl/ccd/interval.h
rename to include/fcl/ccd/interval.h
diff --git a/trunk/fcl/include/fcl/ccd/interval_matrix.h b/include/fcl/ccd/interval_matrix.h
similarity index 100%
rename from trunk/fcl/include/fcl/ccd/interval_matrix.h
rename to include/fcl/ccd/interval_matrix.h
diff --git a/trunk/fcl/include/fcl/ccd/interval_vector.h b/include/fcl/ccd/interval_vector.h
similarity index 100%
rename from trunk/fcl/include/fcl/ccd/interval_vector.h
rename to include/fcl/ccd/interval_vector.h
diff --git a/trunk/fcl/include/fcl/ccd/motion.h b/include/fcl/ccd/motion.h
similarity index 100%
rename from trunk/fcl/include/fcl/ccd/motion.h
rename to include/fcl/ccd/motion.h
diff --git a/trunk/fcl/include/fcl/ccd/motion_base.h b/include/fcl/ccd/motion_base.h
similarity index 100%
rename from trunk/fcl/include/fcl/ccd/motion_base.h
rename to include/fcl/ccd/motion_base.h
diff --git a/trunk/fcl/include/fcl/ccd/taylor_matrix.h b/include/fcl/ccd/taylor_matrix.h
similarity index 100%
rename from trunk/fcl/include/fcl/ccd/taylor_matrix.h
rename to include/fcl/ccd/taylor_matrix.h
diff --git a/trunk/fcl/include/fcl/ccd/taylor_model.h b/include/fcl/ccd/taylor_model.h
similarity index 100%
rename from trunk/fcl/include/fcl/ccd/taylor_model.h
rename to include/fcl/ccd/taylor_model.h
diff --git a/trunk/fcl/include/fcl/ccd/taylor_vector.h b/include/fcl/ccd/taylor_vector.h
similarity index 100%
rename from trunk/fcl/include/fcl/ccd/taylor_vector.h
rename to include/fcl/ccd/taylor_vector.h
diff --git a/trunk/fcl/include/fcl/collision.h b/include/fcl/collision.h
similarity index 100%
rename from trunk/fcl/include/fcl/collision.h
rename to include/fcl/collision.h
diff --git a/trunk/fcl/include/fcl/collision_data.h b/include/fcl/collision_data.h
similarity index 100%
rename from trunk/fcl/include/fcl/collision_data.h
rename to include/fcl/collision_data.h
diff --git a/trunk/fcl/include/fcl/collision_func_matrix.h b/include/fcl/collision_func_matrix.h
similarity index 100%
rename from trunk/fcl/include/fcl/collision_func_matrix.h
rename to include/fcl/collision_func_matrix.h
diff --git a/trunk/fcl/include/fcl/collision_node.h b/include/fcl/collision_node.h
similarity index 100%
rename from trunk/fcl/include/fcl/collision_node.h
rename to include/fcl/collision_node.h
diff --git a/trunk/fcl/include/fcl/collision_object.h b/include/fcl/collision_object.h
similarity index 100%
rename from trunk/fcl/include/fcl/collision_object.h
rename to include/fcl/collision_object.h
diff --git a/trunk/fcl/include/fcl/config.h.in b/include/fcl/config.h.in
similarity index 100%
rename from trunk/fcl/include/fcl/config.h.in
rename to include/fcl/config.h.in
diff --git a/trunk/fcl/include/fcl/data_types.h b/include/fcl/data_types.h
similarity index 100%
rename from trunk/fcl/include/fcl/data_types.h
rename to include/fcl/data_types.h
diff --git a/trunk/fcl/include/fcl/distance.h b/include/fcl/distance.h
similarity index 100%
rename from trunk/fcl/include/fcl/distance.h
rename to include/fcl/distance.h
diff --git a/trunk/fcl/include/fcl/distance_func_matrix.h b/include/fcl/distance_func_matrix.h
similarity index 100%
rename from trunk/fcl/include/fcl/distance_func_matrix.h
rename to include/fcl/distance_func_matrix.h
diff --git a/trunk/fcl/include/fcl/intersect.h b/include/fcl/intersect.h
similarity index 100%
rename from trunk/fcl/include/fcl/intersect.h
rename to include/fcl/intersect.h
diff --git a/trunk/fcl/include/fcl/math/math_details.h b/include/fcl/math/math_details.h
similarity index 100%
rename from trunk/fcl/include/fcl/math/math_details.h
rename to include/fcl/math/math_details.h
diff --git a/trunk/fcl/include/fcl/math/matrix_3f.h b/include/fcl/math/matrix_3f.h
similarity index 100%
rename from trunk/fcl/include/fcl/math/matrix_3f.h
rename to include/fcl/math/matrix_3f.h
diff --git a/trunk/fcl/include/fcl/math/transform.h b/include/fcl/math/transform.h
similarity index 100%
rename from trunk/fcl/include/fcl/math/transform.h
rename to include/fcl/math/transform.h
diff --git a/trunk/fcl/include/fcl/math/vec_3f.h b/include/fcl/math/vec_3f.h
similarity index 100%
rename from trunk/fcl/include/fcl/math/vec_3f.h
rename to include/fcl/math/vec_3f.h
diff --git a/trunk/fcl/include/fcl/narrowphase/gjk.h b/include/fcl/narrowphase/gjk.h
similarity index 100%
rename from trunk/fcl/include/fcl/narrowphase/gjk.h
rename to include/fcl/narrowphase/gjk.h
diff --git a/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h b/include/fcl/narrowphase/gjk_libccd.h
similarity index 100%
rename from trunk/fcl/include/fcl/narrowphase/gjk_libccd.h
rename to include/fcl/narrowphase/gjk_libccd.h
diff --git a/trunk/fcl/include/fcl/narrowphase/narrowphase.h b/include/fcl/narrowphase/narrowphase.h
similarity index 100%
rename from trunk/fcl/include/fcl/narrowphase/narrowphase.h
rename to include/fcl/narrowphase/narrowphase.h
diff --git a/trunk/fcl/include/fcl/octree.h b/include/fcl/octree.h
similarity index 100%
rename from trunk/fcl/include/fcl/octree.h
rename to include/fcl/octree.h
diff --git a/trunk/fcl/include/fcl/profile.h b/include/fcl/profile.h
similarity index 100%
rename from trunk/fcl/include/fcl/profile.h
rename to include/fcl/profile.h
diff --git a/trunk/fcl/include/fcl/shape/geometric_shape_to_BVH_model.h b/include/fcl/shape/geometric_shape_to_BVH_model.h
similarity index 100%
rename from trunk/fcl/include/fcl/shape/geometric_shape_to_BVH_model.h
rename to include/fcl/shape/geometric_shape_to_BVH_model.h
diff --git a/trunk/fcl/include/fcl/shape/geometric_shapes.h b/include/fcl/shape/geometric_shapes.h
similarity index 100%
rename from trunk/fcl/include/fcl/shape/geometric_shapes.h
rename to include/fcl/shape/geometric_shapes.h
diff --git a/trunk/fcl/include/fcl/shape/geometric_shapes_utility.h b/include/fcl/shape/geometric_shapes_utility.h
similarity index 100%
rename from trunk/fcl/include/fcl/shape/geometric_shapes_utility.h
rename to include/fcl/shape/geometric_shapes_utility.h
diff --git a/trunk/fcl/include/fcl/simd/math_simd_details.h b/include/fcl/simd/math_simd_details.h
similarity index 100%
rename from trunk/fcl/include/fcl/simd/math_simd_details.h
rename to include/fcl/simd/math_simd_details.h
diff --git a/trunk/fcl/include/fcl/simd/simd_intersect.h b/include/fcl/simd/simd_intersect.h
similarity index 100%
rename from trunk/fcl/include/fcl/simd/simd_intersect.h
rename to include/fcl/simd/simd_intersect.h
diff --git a/trunk/fcl/include/fcl/traversal/traversal_node_base.h b/include/fcl/traversal/traversal_node_base.h
similarity index 100%
rename from trunk/fcl/include/fcl/traversal/traversal_node_base.h
rename to include/fcl/traversal/traversal_node_base.h
diff --git a/trunk/fcl/include/fcl/traversal/traversal_node_bvh_shape.h b/include/fcl/traversal/traversal_node_bvh_shape.h
similarity index 100%
rename from trunk/fcl/include/fcl/traversal/traversal_node_bvh_shape.h
rename to include/fcl/traversal/traversal_node_bvh_shape.h
diff --git a/trunk/fcl/include/fcl/traversal/traversal_node_bvhs.h b/include/fcl/traversal/traversal_node_bvhs.h
similarity index 100%
rename from trunk/fcl/include/fcl/traversal/traversal_node_bvhs.h
rename to include/fcl/traversal/traversal_node_bvhs.h
diff --git a/trunk/fcl/include/fcl/traversal/traversal_node_octree.h b/include/fcl/traversal/traversal_node_octree.h
similarity index 100%
rename from trunk/fcl/include/fcl/traversal/traversal_node_octree.h
rename to include/fcl/traversal/traversal_node_octree.h
diff --git a/trunk/fcl/include/fcl/traversal/traversal_node_setup.h b/include/fcl/traversal/traversal_node_setup.h
similarity index 100%
rename from trunk/fcl/include/fcl/traversal/traversal_node_setup.h
rename to include/fcl/traversal/traversal_node_setup.h
diff --git a/trunk/fcl/include/fcl/traversal/traversal_node_shapes.h b/include/fcl/traversal/traversal_node_shapes.h
similarity index 100%
rename from trunk/fcl/include/fcl/traversal/traversal_node_shapes.h
rename to include/fcl/traversal/traversal_node_shapes.h
diff --git a/trunk/fcl/include/fcl/traversal/traversal_recurse.h b/include/fcl/traversal/traversal_recurse.h
similarity index 100%
rename from trunk/fcl/include/fcl/traversal/traversal_recurse.h
rename to include/fcl/traversal/traversal_recurse.h
diff --git a/trunk/fcl/manifest.xml b/manifest.xml
similarity index 100%
rename from trunk/fcl/manifest.xml
rename to manifest.xml
diff --git a/trunk/fcl/src/BV/AABB.cpp b/src/BV/AABB.cpp
similarity index 100%
rename from trunk/fcl/src/BV/AABB.cpp
rename to src/BV/AABB.cpp
diff --git a/trunk/fcl/src/BV/OBB.cpp b/src/BV/OBB.cpp
similarity index 100%
rename from trunk/fcl/src/BV/OBB.cpp
rename to src/BV/OBB.cpp
diff --git a/trunk/fcl/src/BV/OBBRSS.cpp b/src/BV/OBBRSS.cpp
similarity index 100%
rename from trunk/fcl/src/BV/OBBRSS.cpp
rename to src/BV/OBBRSS.cpp
diff --git a/trunk/fcl/src/BV/RSS.cpp b/src/BV/RSS.cpp
similarity index 100%
rename from trunk/fcl/src/BV/RSS.cpp
rename to src/BV/RSS.cpp
diff --git a/trunk/fcl/src/BV/kDOP.cpp b/src/BV/kDOP.cpp
similarity index 100%
rename from trunk/fcl/src/BV/kDOP.cpp
rename to src/BV/kDOP.cpp
diff --git a/trunk/fcl/src/BV/kIOS.cpp b/src/BV/kIOS.cpp
similarity index 100%
rename from trunk/fcl/src/BV/kIOS.cpp
rename to src/BV/kIOS.cpp
diff --git a/trunk/fcl/src/BVH/BVH_model.cpp b/src/BVH/BVH_model.cpp
similarity index 100%
rename from trunk/fcl/src/BVH/BVH_model.cpp
rename to src/BVH/BVH_model.cpp
diff --git a/trunk/fcl/src/BVH/BVH_utility.cpp b/src/BVH/BVH_utility.cpp
similarity index 100%
rename from trunk/fcl/src/BVH/BVH_utility.cpp
rename to src/BVH/BVH_utility.cpp
diff --git a/trunk/fcl/src/BVH/BV_fitter.cpp b/src/BVH/BV_fitter.cpp
similarity index 100%
rename from trunk/fcl/src/BVH/BV_fitter.cpp
rename to src/BVH/BV_fitter.cpp
diff --git a/trunk/fcl/src/BVH/BV_splitter.cpp b/src/BVH/BV_splitter.cpp
similarity index 100%
rename from trunk/fcl/src/BVH/BV_splitter.cpp
rename to src/BVH/BV_splitter.cpp
diff --git a/trunk/fcl/src/CMakeLists.txt b/src/CMakeLists.txt
similarity index 100%
rename from trunk/fcl/src/CMakeLists.txt
rename to src/CMakeLists.txt
diff --git a/trunk/fcl/src/articulated_model/joint.cpp b/src/articulated_model/joint.cpp
similarity index 100%
rename from trunk/fcl/src/articulated_model/joint.cpp
rename to src/articulated_model/joint.cpp
diff --git a/trunk/fcl/src/articulated_model/joint_config.cpp b/src/articulated_model/joint_config.cpp
similarity index 100%
rename from trunk/fcl/src/articulated_model/joint_config.cpp
rename to src/articulated_model/joint_config.cpp
diff --git a/trunk/fcl/src/articulated_model/link.cpp b/src/articulated_model/link.cpp
similarity index 100%
rename from trunk/fcl/src/articulated_model/link.cpp
rename to src/articulated_model/link.cpp
diff --git a/trunk/fcl/src/articulated_model/model.cpp b/src/articulated_model/model.cpp
similarity index 100%
rename from trunk/fcl/src/articulated_model/model.cpp
rename to src/articulated_model/model.cpp
diff --git a/trunk/fcl/src/articulated_model/model_config.cpp b/src/articulated_model/model_config.cpp
similarity index 100%
rename from trunk/fcl/src/articulated_model/model_config.cpp
rename to src/articulated_model/model_config.cpp
diff --git a/trunk/fcl/src/broadphase/broadphase_SSaP.cpp b/src/broadphase/broadphase_SSaP.cpp
similarity index 100%
rename from trunk/fcl/src/broadphase/broadphase_SSaP.cpp
rename to src/broadphase/broadphase_SSaP.cpp
diff --git a/trunk/fcl/src/broadphase/broadphase_SaP.cpp b/src/broadphase/broadphase_SaP.cpp
similarity index 100%
rename from trunk/fcl/src/broadphase/broadphase_SaP.cpp
rename to src/broadphase/broadphase_SaP.cpp
diff --git a/trunk/fcl/src/broadphase/broadphase_bruteforce.cpp b/src/broadphase/broadphase_bruteforce.cpp
similarity index 100%
rename from trunk/fcl/src/broadphase/broadphase_bruteforce.cpp
rename to src/broadphase/broadphase_bruteforce.cpp
diff --git a/trunk/fcl/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp
similarity index 100%
rename from trunk/fcl/src/broadphase/broadphase_dynamic_AABB_tree.cpp
rename to src/broadphase/broadphase_dynamic_AABB_tree.cpp
diff --git a/trunk/fcl/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp
similarity index 100%
rename from trunk/fcl/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp
rename to src/broadphase/broadphase_dynamic_AABB_tree_array.cpp
diff --git a/trunk/fcl/src/broadphase/broadphase_interval_tree.cpp b/src/broadphase/broadphase_interval_tree.cpp
similarity index 100%
rename from trunk/fcl/src/broadphase/broadphase_interval_tree.cpp
rename to src/broadphase/broadphase_interval_tree.cpp
diff --git a/trunk/fcl/src/broadphase/broadphase_spatialhash.cpp b/src/broadphase/broadphase_spatialhash.cpp
similarity index 100%
rename from trunk/fcl/src/broadphase/broadphase_spatialhash.cpp
rename to src/broadphase/broadphase_spatialhash.cpp
diff --git a/trunk/fcl/src/broadphase/hierarchy_tree.cpp b/src/broadphase/hierarchy_tree.cpp
similarity index 100%
rename from trunk/fcl/src/broadphase/hierarchy_tree.cpp
rename to src/broadphase/hierarchy_tree.cpp
diff --git a/trunk/fcl/src/broadphase/interval_tree.cpp b/src/broadphase/interval_tree.cpp
similarity index 100%
rename from trunk/fcl/src/broadphase/interval_tree.cpp
rename to src/broadphase/interval_tree.cpp
diff --git a/trunk/fcl/src/ccd/conservative_advancement.cpp b/src/ccd/conservative_advancement.cpp
similarity index 100%
rename from trunk/fcl/src/ccd/conservative_advancement.cpp
rename to src/ccd/conservative_advancement.cpp
diff --git a/trunk/fcl/src/ccd/interpolation/interpolation.cpp b/src/ccd/interpolation/interpolation.cpp
similarity index 100%
rename from trunk/fcl/src/ccd/interpolation/interpolation.cpp
rename to src/ccd/interpolation/interpolation.cpp
diff --git a/trunk/fcl/src/ccd/interpolation/interpolation_factory.cpp b/src/ccd/interpolation/interpolation_factory.cpp
similarity index 100%
rename from trunk/fcl/src/ccd/interpolation/interpolation_factory.cpp
rename to src/ccd/interpolation/interpolation_factory.cpp
diff --git a/trunk/fcl/src/ccd/interpolation/interpolation_linear.cpp b/src/ccd/interpolation/interpolation_linear.cpp
similarity index 100%
rename from trunk/fcl/src/ccd/interpolation/interpolation_linear.cpp
rename to src/ccd/interpolation/interpolation_linear.cpp
diff --git a/trunk/fcl/src/ccd/interval.cpp b/src/ccd/interval.cpp
similarity index 100%
rename from trunk/fcl/src/ccd/interval.cpp
rename to src/ccd/interval.cpp
diff --git a/trunk/fcl/src/ccd/interval_matrix.cpp b/src/ccd/interval_matrix.cpp
similarity index 100%
rename from trunk/fcl/src/ccd/interval_matrix.cpp
rename to src/ccd/interval_matrix.cpp
diff --git a/trunk/fcl/src/ccd/interval_vector.cpp b/src/ccd/interval_vector.cpp
similarity index 100%
rename from trunk/fcl/src/ccd/interval_vector.cpp
rename to src/ccd/interval_vector.cpp
diff --git a/trunk/fcl/src/ccd/motion.cpp b/src/ccd/motion.cpp
similarity index 100%
rename from trunk/fcl/src/ccd/motion.cpp
rename to src/ccd/motion.cpp
diff --git a/trunk/fcl/src/ccd/taylor_matrix.cpp b/src/ccd/taylor_matrix.cpp
similarity index 100%
rename from trunk/fcl/src/ccd/taylor_matrix.cpp
rename to src/ccd/taylor_matrix.cpp
diff --git a/trunk/fcl/src/ccd/taylor_model.cpp b/src/ccd/taylor_model.cpp
similarity index 100%
rename from trunk/fcl/src/ccd/taylor_model.cpp
rename to src/ccd/taylor_model.cpp
diff --git a/trunk/fcl/src/ccd/taylor_vector.cpp b/src/ccd/taylor_vector.cpp
similarity index 100%
rename from trunk/fcl/src/ccd/taylor_vector.cpp
rename to src/ccd/taylor_vector.cpp
diff --git a/trunk/fcl/src/collision.cpp b/src/collision.cpp
similarity index 100%
rename from trunk/fcl/src/collision.cpp
rename to src/collision.cpp
diff --git a/trunk/fcl/src/collision_data.cpp b/src/collision_data.cpp
similarity index 100%
rename from trunk/fcl/src/collision_data.cpp
rename to src/collision_data.cpp
diff --git a/trunk/fcl/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp
similarity index 100%
rename from trunk/fcl/src/collision_func_matrix.cpp
rename to src/collision_func_matrix.cpp
diff --git a/trunk/fcl/src/collision_node.cpp b/src/collision_node.cpp
similarity index 100%
rename from trunk/fcl/src/collision_node.cpp
rename to src/collision_node.cpp
diff --git a/trunk/fcl/src/distance.cpp b/src/distance.cpp
similarity index 100%
rename from trunk/fcl/src/distance.cpp
rename to src/distance.cpp
diff --git a/trunk/fcl/src/distance_func_matrix.cpp b/src/distance_func_matrix.cpp
similarity index 100%
rename from trunk/fcl/src/distance_func_matrix.cpp
rename to src/distance_func_matrix.cpp
diff --git a/trunk/fcl/src/intersect.cpp b/src/intersect.cpp
similarity index 100%
rename from trunk/fcl/src/intersect.cpp
rename to src/intersect.cpp
diff --git a/trunk/fcl/src/math/transform.cpp b/src/math/transform.cpp
similarity index 100%
rename from trunk/fcl/src/math/transform.cpp
rename to src/math/transform.cpp
diff --git a/trunk/fcl/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
similarity index 100%
rename from trunk/fcl/src/narrowphase/gjk.cpp
rename to src/narrowphase/gjk.cpp
diff --git a/trunk/fcl/src/narrowphase/gjk_libccd.cpp b/src/narrowphase/gjk_libccd.cpp
similarity index 100%
rename from trunk/fcl/src/narrowphase/gjk_libccd.cpp
rename to src/narrowphase/gjk_libccd.cpp
diff --git a/trunk/fcl/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
similarity index 100%
rename from trunk/fcl/src/narrowphase/narrowphase.cpp
rename to src/narrowphase/narrowphase.cpp
diff --git a/trunk/fcl/src/profile.cpp b/src/profile.cpp
similarity index 100%
rename from trunk/fcl/src/profile.cpp
rename to src/profile.cpp
diff --git a/trunk/fcl/src/shape/geometric_shapes.cpp b/src/shape/geometric_shapes.cpp
similarity index 100%
rename from trunk/fcl/src/shape/geometric_shapes.cpp
rename to src/shape/geometric_shapes.cpp
diff --git a/trunk/fcl/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp
similarity index 100%
rename from trunk/fcl/src/shape/geometric_shapes_utility.cpp
rename to src/shape/geometric_shapes_utility.cpp
diff --git a/trunk/fcl/src/traversal/traversal_node_base.cpp b/src/traversal/traversal_node_base.cpp
similarity index 100%
rename from trunk/fcl/src/traversal/traversal_node_base.cpp
rename to src/traversal/traversal_node_base.cpp
diff --git a/trunk/fcl/src/traversal/traversal_node_bvhs.cpp b/src/traversal/traversal_node_bvhs.cpp
similarity index 100%
rename from trunk/fcl/src/traversal/traversal_node_bvhs.cpp
rename to src/traversal/traversal_node_bvhs.cpp
diff --git a/trunk/fcl/src/traversal/traversal_node_setup.cpp b/src/traversal/traversal_node_setup.cpp
similarity index 100%
rename from trunk/fcl/src/traversal/traversal_node_setup.cpp
rename to src/traversal/traversal_node_setup.cpp
diff --git a/trunk/fcl/src/traversal/traversal_recurse.cpp b/src/traversal/traversal_recurse.cpp
similarity index 100%
rename from trunk/fcl/src/traversal/traversal_recurse.cpp
rename to src/traversal/traversal_recurse.cpp
diff --git a/trunk/fcl/test/CMakeLists.txt b/test/CMakeLists.txt
similarity index 100%
rename from trunk/fcl/test/CMakeLists.txt
rename to test/CMakeLists.txt
diff --git a/trunk/fcl/test/fcl_resources/config.h.in b/test/fcl_resources/config.h.in
similarity index 100%
rename from trunk/fcl/test/fcl_resources/config.h.in
rename to test/fcl_resources/config.h.in
diff --git a/branches/point_cloud/fcl/test/env.obj b/test/fcl_resources/env.obj
similarity index 100%
rename from branches/point_cloud/fcl/test/env.obj
rename to test/fcl_resources/env.obj
diff --git a/branches/point_cloud/fcl/test/rob.obj b/test/fcl_resources/rob.obj
similarity index 100%
rename from branches/point_cloud/fcl/test/rob.obj
rename to test/fcl_resources/rob.obj
diff --git a/trunk/fcl/test/test_fcl_broadphase.cpp b/test/test_fcl_broadphase.cpp
similarity index 100%
rename from trunk/fcl/test/test_fcl_broadphase.cpp
rename to test/test_fcl_broadphase.cpp
diff --git a/trunk/fcl/test/test_fcl_collision.cpp b/test/test_fcl_collision.cpp
similarity index 100%
rename from trunk/fcl/test/test_fcl_collision.cpp
rename to test/test_fcl_collision.cpp
diff --git a/trunk/fcl/test/test_fcl_distance.cpp b/test/test_fcl_distance.cpp
similarity index 100%
rename from trunk/fcl/test/test_fcl_distance.cpp
rename to test/test_fcl_distance.cpp
diff --git a/trunk/fcl/test/test_fcl_frontlist.cpp b/test/test_fcl_frontlist.cpp
similarity index 100%
rename from trunk/fcl/test/test_fcl_frontlist.cpp
rename to test/test_fcl_frontlist.cpp
diff --git a/trunk/fcl/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp
similarity index 100%
rename from trunk/fcl/test/test_fcl_geometric_shapes.cpp
rename to test/test_fcl_geometric_shapes.cpp
diff --git a/trunk/fcl/test/test_fcl_math.cpp b/test/test_fcl_math.cpp
similarity index 100%
rename from trunk/fcl/test/test_fcl_math.cpp
rename to test/test_fcl_math.cpp
diff --git a/trunk/fcl/test/test_fcl_octomap.cpp b/test/test_fcl_octomap.cpp
similarity index 100%
rename from trunk/fcl/test/test_fcl_octomap.cpp
rename to test/test_fcl_octomap.cpp
diff --git a/trunk/fcl/test/test_fcl_shape_mesh_consistency.cpp b/test/test_fcl_shape_mesh_consistency.cpp
similarity index 100%
rename from trunk/fcl/test/test_fcl_shape_mesh_consistency.cpp
rename to test/test_fcl_shape_mesh_consistency.cpp
diff --git a/trunk/fcl/test/test_fcl_utility.cpp b/test/test_fcl_utility.cpp
similarity index 100%
rename from trunk/fcl/test/test_fcl_utility.cpp
rename to test/test_fcl_utility.cpp
diff --git a/trunk/fcl/test/test_fcl_utility.h b/test/test_fcl_utility.h
similarity index 100%
rename from trunk/fcl/test/test_fcl_utility.h
rename to test/test_fcl_utility.h
diff --git a/trunk/fcl/test/fcl_resources/env.obj b/trunk/fcl/test/fcl_resources/env.obj
deleted file mode 100644
index 1aeb971dd0ef443b111e3e53aeb11ed989428260..0000000000000000000000000000000000000000
--- a/trunk/fcl/test/fcl_resources/env.obj
+++ /dev/null
@@ -1,8721 +0,0 @@
-6540 2180
-v -2987.5 -3004.5 2987.5
-v 987.5 -3004.5 2987.5
-v -2987.5 -2995.5 2987.5
-v -2987.5 -2995.5 2987.5
-v 987.5 -3004.5 2987.5
-v 987.5 -2995.5 2987.5
-v 987.5 -3004.5 12.5
-v -2987.5 -3004.5 12.5
-v 987.5 -2995.5 12.5
-v 987.5 -2995.5 12.5
-v -2987.5 -3004.5 12.5
-v -2987.5 -2995.5 12.5
-v 987.5 -3004.5 2987.5
-v 987.5 -3004.5 12.5
-v 987.5 -2995.5 2987.5
-v 987.5 -2995.5 2987.5
-v 987.5 -3004.5 12.5
-v 987.5 -2995.5 12.5
-v -2987.5 -3004.5 12.5
-v -2987.5 -3004.5 2987.5
-v -2987.5 -2995.5 12.5
-v -2987.5 -2995.5 12.5
-v -2987.5 -3004.5 2987.5
-v -2987.5 -2995.5 2987.5
-v -2987.5 -2995.5 12.5
-v -2987.5 -2995.5 2987.5
-v 987.5 -2995.5 12.5
-v 987.5 -2995.5 12.5
-v -2987.5 -2995.5 2987.5
-v 987.5 -2995.5 2987.5
-v -2987.5 -3004.5 12.5
-v 987.5 -3004.5 12.5
-v -2987.5 -3004.5 2987.5
-v -2987.5 -3004.5 2987.5
-v 987.5 -3004.5 12.5
-v 987.5 -3004.5 2987.5
-v -2987.5 -2987.5 -5.5
-v 2987.5 -2987.5 -5.5
-v -2987.5 2987.5 -5.5
-v -2987.5 2987.5 -5.5
-v 2987.5 -2987.5 -5.5
-v 2987.5 2987.5 -5.5
-v 2987.5 -2987.5 -14.5
-v -2987.5 -2987.5 -14.5
-v 2987.5 2987.5 -14.5
-v 2987.5 2987.5 -14.5
-v -2987.5 -2987.5 -14.5
-v -2987.5 2987.5 -14.5
-v 2987.5 -2987.5 -5.5
-v 2987.5 -2987.5 -14.5
-v 2987.5 2987.5 -5.5
-v 2987.5 2987.5 -5.5
-v 2987.5 -2987.5 -14.5
-v 2987.5 2987.5 -14.5
-v -2987.5 -2987.5 -14.5
-v -2987.5 -2987.5 -5.5
-v -2987.5 2987.5 -14.5
-v -2987.5 2987.5 -14.5
-v -2987.5 -2987.5 -5.5
-v -2987.5 2987.5 -5.5
-v -2987.5 2987.5 -14.5
-v -2987.5 2987.5 -5.5
-v 2987.5 2987.5 -14.5
-v 2987.5 2987.5 -14.5
-v -2987.5 2987.5 -5.5
-v 2987.5 2987.5 -5.5
-v -2987.5 -2987.5 -14.5
-v 2987.5 -2987.5 -14.5
-v -2987.5 -2987.5 -5.5
-v -2987.5 -2987.5 -5.5
-v 2987.5 -2987.5 -14.5
-v 2987.5 -2987.5 -5.5
-v 995.5 -2987.5 987.5
-v 1004.5 -2987.5 987.5
-v 995.5 2987.5 987.5
-v 995.5 2987.5 987.5
-v 1004.5 -2987.5 987.5
-v 1004.5 2987.5 987.5
-v 1004.5 -2987.5 12.5
-v 995.5 -2987.5 12.5
-v 1004.5 2987.5 12.5
-v 1004.5 2987.5 12.5
-v 995.5 -2987.5 12.5
-v 995.5 2987.5 12.5
-v 1004.5 -2987.5 987.5
-v 1004.5 -2987.5 12.5
-v 1004.5 2987.5 987.5
-v 1004.5 2987.5 987.5
-v 1004.5 -2987.5 12.5
-v 1004.5 2987.5 12.5
-v 995.5 -2987.5 12.5
-v 995.5 -2987.5 987.5
-v 995.5 2987.5 12.5
-v 995.5 2987.5 12.5
-v 995.5 -2987.5 987.5
-v 995.5 2987.5 987.5
-v 995.5 2987.5 12.5
-v 995.5 2987.5 987.5
-v 1004.5 2987.5 12.5
-v 1004.5 2987.5 12.5
-v 995.5 2987.5 987.5
-v 1004.5 2987.5 987.5
-v 995.5 -2987.5 12.5
-v 1004.5 -2987.5 12.5
-v 995.5 -2987.5 987.5
-v 995.5 -2987.5 987.5
-v 1004.5 -2987.5 12.5
-v 1004.5 -2987.5 987.5
-v 995.5 -2987.5 2987.5
-v 1004.5 -2987.5 2987.5
-v 995.5 2987.5 2987.5
-v 995.5 2987.5 2987.5
-v 1004.5 -2987.5 2987.5
-v 1004.5 2987.5 2987.5
-v 1004.5 -2987.5 2012.5
-v 995.5 -2987.5 2012.5
-v 1004.5 2987.5 2012.5
-v 1004.5 2987.5 2012.5
-v 995.5 -2987.5 2012.5
-v 995.5 2987.5 2012.5
-v 1004.5 -2987.5 2987.5
-v 1004.5 -2987.5 2012.5
-v 1004.5 2987.5 2987.5
-v 1004.5 2987.5 2987.5
-v 1004.5 -2987.5 2012.5
-v 1004.5 2987.5 2012.5
-v 995.5 -2987.5 2012.5
-v 995.5 -2987.5 2987.5
-v 995.5 2987.5 2012.5
-v 995.5 2987.5 2012.5
-v 995.5 -2987.5 2987.5
-v 995.5 2987.5 2987.5
-v 995.5 2987.5 2012.5
-v 995.5 2987.5 2987.5
-v 1004.5 2987.5 2012.5
-v 1004.5 2987.5 2012.5
-v 995.5 2987.5 2987.5
-v 1004.5 2987.5 2987.5
-v 995.5 -2987.5 2012.5
-v 1004.5 -2987.5 2012.5
-v 995.5 -2987.5 2987.5
-v 995.5 -2987.5 2987.5
-v 1004.5 -2987.5 2012.5
-v 1004.5 -2987.5 2987.5
-v 995.5 -2987.5 1987.5
-v 1004.5 -2987.5 1987.5
-v 995.5 -1012.5 1987.5
-v 995.5 -1012.5 1987.5
-v 1004.5 -2987.5 1987.5
-v 1004.5 -1012.5 1987.5
-v 1004.5 -2987.5 1012.5
-v 995.5 -2987.5 1012.5
-v 1004.5 -1012.5 1012.5
-v 1004.5 -1012.5 1012.5
-v 995.5 -2987.5 1012.5
-v 995.5 -1012.5 1012.5
-v 1004.5 -2987.5 1987.5
-v 1004.5 -2987.5 1012.5
-v 1004.5 -1012.5 1987.5
-v 1004.5 -1012.5 1987.5
-v 1004.5 -2987.5 1012.5
-v 1004.5 -1012.5 1012.5
-v 995.5 -2987.5 1012.5
-v 995.5 -2987.5 1987.5
-v 995.5 -1012.5 1012.5
-v 995.5 -1012.5 1012.5
-v 995.5 -2987.5 1987.5
-v 995.5 -1012.5 1987.5
-v 995.5 -1012.5 1012.5
-v 995.5 -1012.5 1987.5
-v 1004.5 -1012.5 1012.5
-v 1004.5 -1012.5 1012.5
-v 995.5 -1012.5 1987.5
-v 1004.5 -1012.5 1987.5
-v 995.5 -2987.5 1012.5
-v 1004.5 -2987.5 1012.5
-v 995.5 -2987.5 1987.5
-v 995.5 -2987.5 1987.5
-v 1004.5 -2987.5 1012.5
-v 1004.5 -2987.5 1987.5
-v 995.5 1012.5 1987.5
-v 1004.5 1012.5 1987.5
-v 995.5 2987.5 1987.5
-v 995.5 2987.5 1987.5
-v 1004.5 1012.5 1987.5
-v 1004.5 2987.5 1987.5
-v 1004.5 1012.5 1012.5
-v 995.5 1012.5 1012.5
-v 1004.5 2987.5 1012.5
-v 1004.5 2987.5 1012.5
-v 995.5 1012.5 1012.5
-v 995.5 2987.5 1012.5
-v 1004.5 1012.5 1987.5
-v 1004.5 1012.5 1012.5
-v 1004.5 2987.5 1987.5
-v 1004.5 2987.5 1987.5
-v 1004.5 1012.5 1012.5
-v 1004.5 2987.5 1012.5
-v 995.5 1012.5 1012.5
-v 995.5 1012.5 1987.5
-v 995.5 2987.5 1012.5
-v 995.5 2987.5 1012.5
-v 995.5 1012.5 1987.5
-v 995.5 2987.5 1987.5
-v 995.5 2987.5 1012.5
-v 995.5 2987.5 1987.5
-v 1004.5 2987.5 1012.5
-v 1004.5 2987.5 1012.5
-v 995.5 2987.5 1987.5
-v 1004.5 2987.5 1987.5
-v 995.5 1012.5 1012.5
-v 1004.5 1012.5 1012.5
-v 995.5 1012.5 1987.5
-v 995.5 1012.5 1987.5
-v 1004.5 1012.5 1012.5
-v 1004.5 1012.5 1987.5
-v -2987.5 2995.5 2987.5
-v 987.5 2995.5 2987.5
-v -2987.5 3004.5 2987.5
-v -2987.5 3004.5 2987.5
-v 987.5 2995.5 2987.5
-v 987.5 3004.5 2987.5
-v 987.5 2995.5 12.5
-v -2987.5 2995.5 12.5
-v 987.5 3004.5 12.5
-v 987.5 3004.5 12.5
-v -2987.5 2995.5 12.5
-v -2987.5 3004.5 12.5
-v 987.5 2995.5 2987.5
-v 987.5 2995.5 12.5
-v 987.5 3004.5 2987.5
-v 987.5 3004.5 2987.5
-v 987.5 2995.5 12.5
-v 987.5 3004.5 12.5
-v -2987.5 2995.5 12.5
-v -2987.5 2995.5 2987.5
-v -2987.5 3004.5 12.5
-v -2987.5 3004.5 12.5
-v -2987.5 2995.5 2987.5
-v -2987.5 3004.5 2987.5
-v -2987.5 3004.5 12.5
-v -2987.5 3004.5 2987.5
-v 987.5 3004.5 12.5
-v 987.5 3004.5 12.5
-v -2987.5 3004.5 2987.5
-v 987.5 3004.5 2987.5
-v -2987.5 2995.5 12.5
-v 987.5 2995.5 12.5
-v -2987.5 2995.5 2987.5
-v -2987.5 2995.5 2987.5
-v 987.5 2995.5 12.5
-v 987.5 2995.5 2987.5
-v -3004.5 -2987.5 2987.5
-v -2995.5 -2987.5 2987.5
-v -3004.5 2987.5 2987.5
-v -3004.5 2987.5 2987.5
-v -2995.5 -2987.5 2987.5
-v -2995.5 2987.5 2987.5
-v -2995.5 -2987.5 12.5
-v -3004.5 -2987.5 12.5
-v -2995.5 2987.5 12.5
-v -2995.5 2987.5 12.5
-v -3004.5 -2987.5 12.5
-v -3004.5 2987.5 12.5
-v -2995.5 -2987.5 2987.5
-v -2995.5 -2987.5 12.5
-v -2995.5 2987.5 2987.5
-v -2995.5 2987.5 2987.5
-v -2995.5 -2987.5 12.5
-v -2995.5 2987.5 12.5
-v -3004.5 -2987.5 12.5
-v -3004.5 -2987.5 2987.5
-v -3004.5 2987.5 12.5
-v -3004.5 2987.5 12.5
-v -3004.5 -2987.5 2987.5
-v -3004.5 2987.5 2987.5
-v -3004.5 2987.5 12.5
-v -3004.5 2987.5 2987.5
-v -2995.5 2987.5 12.5
-v -2995.5 2987.5 12.5
-v -3004.5 2987.5 2987.5
-v -2995.5 2987.5 2987.5
-v -3004.5 -2987.5 12.5
-v -2995.5 -2987.5 12.5
-v -3004.5 -2987.5 2987.5
-v -3004.5 -2987.5 2987.5
-v -2995.5 -2987.5 12.5
-v -2995.5 -2987.5 2987.5
-v -1660 -2650 495
-v -1659.24 -2653.83 495
-v -1660 -2650 5
-v -1660 -2650 5
-v -1659.24 -2653.83 495
-v -1659.24 -2653.83 5
-v -1659.24 -2653.83 495
-v -1657.07 -2657.07 495
-v -1659.24 -2653.83 5
-v -1659.24 -2653.83 5
-v -1657.07 -2657.07 495
-v -1657.07 -2657.07 5
-v -1657.07 -2657.07 495
-v -1653.83 -2659.24 495
-v -1657.07 -2657.07 5
-v -1657.07 -2657.07 5
-v -1653.83 -2659.24 495
-v -1653.83 -2659.24 5
-v -1653.83 -2659.24 495
-v -1650 -2660 495
-v -1653.83 -2659.24 5
-v -1653.83 -2659.24 5
-v -1650 -2660 495
-v -1650 -2660 5
-v -1650 -2660 495
-v -1646.17 -2659.24 495
-v -1650 -2660 5
-v -1650 -2660 5
-v -1646.17 -2659.24 495
-v -1646.17 -2659.24 5
-v -1646.17 -2659.24 495
-v -1642.93 -2657.07 495
-v -1646.17 -2659.24 5
-v -1646.17 -2659.24 5
-v -1642.93 -2657.07 495
-v -1642.93 -2657.07 5
-v -1642.93 -2657.07 495
-v -1640.76 -2653.83 495
-v -1642.93 -2657.07 5
-v -1642.93 -2657.07 5
-v -1640.76 -2653.83 495
-v -1640.76 -2653.83 5
-v -1640.76 -2653.83 495
-v -1640 -2650 495
-v -1640.76 -2653.83 5
-v -1640.76 -2653.83 5
-v -1640 -2650 495
-v -1640 -2650 5
-v -1640 -2650 495
-v -1640.76 -2646.17 495
-v -1640 -2650 5
-v -1640 -2650 5
-v -1640.76 -2646.17 495
-v -1640.76 -2646.17 5
-v -1640.76 -2646.17 495
-v -1642.93 -2642.93 495
-v -1640.76 -2646.17 5
-v -1640.76 -2646.17 5
-v -1642.93 -2642.93 495
-v -1642.93 -2642.93 5
-v -1642.93 -2642.93 495
-v -1646.17 -2640.76 495
-v -1642.93 -2642.93 5
-v -1642.93 -2642.93 5
-v -1646.17 -2640.76 495
-v -1646.17 -2640.76 5
-v -1646.17 -2640.76 495
-v -1650 -2640 495
-v -1646.17 -2640.76 5
-v -1646.17 -2640.76 5
-v -1650 -2640 495
-v -1650 -2640 5
-v -1650 -2640 495
-v -1653.83 -2640.76 495
-v -1650 -2640 5
-v -1650 -2640 5
-v -1653.83 -2640.76 495
-v -1653.83 -2640.76 5
-v -1653.83 -2640.76 495
-v -1657.07 -2642.93 495
-v -1653.83 -2640.76 5
-v -1653.83 -2640.76 5
-v -1657.07 -2642.93 495
-v -1657.07 -2642.93 5
-v -1657.07 -2642.93 495
-v -1659.24 -2646.17 495
-v -1657.07 -2642.93 5
-v -1657.07 -2642.93 5
-v -1659.24 -2646.17 495
-v -1659.24 -2646.17 5
-v -1659.24 -2646.17 495
-v -1660 -2650 495
-v -1659.24 -2646.17 5
-v -1659.24 -2646.17 5
-v -1660 -2650 495
-v -1660 -2650 5
-v -1360 -2650 495
-v -1359.24 -2653.83 495
-v -1360 -2650 5
-v -1360 -2650 5
-v -1359.24 -2653.83 495
-v -1359.24 -2653.83 5
-v -1359.24 -2653.83 495
-v -1357.07 -2657.07 495
-v -1359.24 -2653.83 5
-v -1359.24 -2653.83 5
-v -1357.07 -2657.07 495
-v -1357.07 -2657.07 5
-v -1357.07 -2657.07 495
-v -1353.83 -2659.24 495
-v -1357.07 -2657.07 5
-v -1357.07 -2657.07 5
-v -1353.83 -2659.24 495
-v -1353.83 -2659.24 5
-v -1353.83 -2659.24 495
-v -1350 -2660 495
-v -1353.83 -2659.24 5
-v -1353.83 -2659.24 5
-v -1350 -2660 495
-v -1350 -2660 5
-v -1350 -2660 495
-v -1346.17 -2659.24 495
-v -1350 -2660 5
-v -1350 -2660 5
-v -1346.17 -2659.24 495
-v -1346.17 -2659.24 5
-v -1346.17 -2659.24 495
-v -1342.93 -2657.07 495
-v -1346.17 -2659.24 5
-v -1346.17 -2659.24 5
-v -1342.93 -2657.07 495
-v -1342.93 -2657.07 5
-v -1342.93 -2657.07 495
-v -1340.76 -2653.83 495
-v -1342.93 -2657.07 5
-v -1342.93 -2657.07 5
-v -1340.76 -2653.83 495
-v -1340.76 -2653.83 5
-v -1340.76 -2653.83 495
-v -1340 -2650 495
-v -1340.76 -2653.83 5
-v -1340.76 -2653.83 5
-v -1340 -2650 495
-v -1340 -2650 5
-v -1340 -2650 495
-v -1340.76 -2646.17 495
-v -1340 -2650 5
-v -1340 -2650 5
-v -1340.76 -2646.17 495
-v -1340.76 -2646.17 5
-v -1340.76 -2646.17 495
-v -1342.93 -2642.93 495
-v -1340.76 -2646.17 5
-v -1340.76 -2646.17 5
-v -1342.93 -2642.93 495
-v -1342.93 -2642.93 5
-v -1342.93 -2642.93 495
-v -1346.17 -2640.76 495
-v -1342.93 -2642.93 5
-v -1342.93 -2642.93 5
-v -1346.17 -2640.76 495
-v -1346.17 -2640.76 5
-v -1346.17 -2640.76 495
-v -1350 -2640 495
-v -1346.17 -2640.76 5
-v -1346.17 -2640.76 5
-v -1350 -2640 495
-v -1350 -2640 5
-v -1350 -2640 495
-v -1353.83 -2640.76 495
-v -1350 -2640 5
-v -1350 -2640 5
-v -1353.83 -2640.76 495
-v -1353.83 -2640.76 5
-v -1353.83 -2640.76 495
-v -1357.07 -2642.93 495
-v -1353.83 -2640.76 5
-v -1353.83 -2640.76 5
-v -1357.07 -2642.93 495
-v -1357.07 -2642.93 5
-v -1357.07 -2642.93 495
-v -1359.24 -2646.17 495
-v -1357.07 -2642.93 5
-v -1357.07 -2642.93 5
-v -1359.24 -2646.17 495
-v -1359.24 -2646.17 5
-v -1359.24 -2646.17 495
-v -1360 -2650 495
-v -1359.24 -2646.17 5
-v -1359.24 -2646.17 5
-v -1360 -2650 495
-v -1360 -2650 5
-v -1360 -2350 495
-v -1359.24 -2353.83 495
-v -1360 -2350 5
-v -1360 -2350 5
-v -1359.24 -2353.83 495
-v -1359.24 -2353.83 5
-v -1359.24 -2353.83 495
-v -1357.07 -2357.07 495
-v -1359.24 -2353.83 5
-v -1359.24 -2353.83 5
-v -1357.07 -2357.07 495
-v -1357.07 -2357.07 5
-v -1357.07 -2357.07 495
-v -1353.83 -2359.24 495
-v -1357.07 -2357.07 5
-v -1357.07 -2357.07 5
-v -1353.83 -2359.24 495
-v -1353.83 -2359.24 5
-v -1353.83 -2359.24 495
-v -1350 -2360 495
-v -1353.83 -2359.24 5
-v -1353.83 -2359.24 5
-v -1350 -2360 495
-v -1350 -2360 5
-v -1350 -2360 495
-v -1346.17 -2359.24 495
-v -1350 -2360 5
-v -1350 -2360 5
-v -1346.17 -2359.24 495
-v -1346.17 -2359.24 5
-v -1346.17 -2359.24 495
-v -1342.93 -2357.07 495
-v -1346.17 -2359.24 5
-v -1346.17 -2359.24 5
-v -1342.93 -2357.07 495
-v -1342.93 -2357.07 5
-v -1342.93 -2357.07 495
-v -1340.76 -2353.83 495
-v -1342.93 -2357.07 5
-v -1342.93 -2357.07 5
-v -1340.76 -2353.83 495
-v -1340.76 -2353.83 5
-v -1340.76 -2353.83 495
-v -1340 -2350 495
-v -1340.76 -2353.83 5
-v -1340.76 -2353.83 5
-v -1340 -2350 495
-v -1340 -2350 5
-v -1340 -2350 495
-v -1340.76 -2346.17 495
-v -1340 -2350 5
-v -1340 -2350 5
-v -1340.76 -2346.17 495
-v -1340.76 -2346.17 5
-v -1340.76 -2346.17 495
-v -1342.93 -2342.93 495
-v -1340.76 -2346.17 5
-v -1340.76 -2346.17 5
-v -1342.93 -2342.93 495
-v -1342.93 -2342.93 5
-v -1342.93 -2342.93 495
-v -1346.17 -2340.76 495
-v -1342.93 -2342.93 5
-v -1342.93 -2342.93 5
-v -1346.17 -2340.76 495
-v -1346.17 -2340.76 5
-v -1346.17 -2340.76 495
-v -1350 -2340 495
-v -1346.17 -2340.76 5
-v -1346.17 -2340.76 5
-v -1350 -2340 495
-v -1350 -2340 5
-v -1350 -2340 495
-v -1353.83 -2340.76 495
-v -1350 -2340 5
-v -1350 -2340 5
-v -1353.83 -2340.76 495
-v -1353.83 -2340.76 5
-v -1353.83 -2340.76 495
-v -1357.07 -2342.93 495
-v -1353.83 -2340.76 5
-v -1353.83 -2340.76 5
-v -1357.07 -2342.93 495
-v -1357.07 -2342.93 5
-v -1357.07 -2342.93 495
-v -1359.24 -2346.17 495
-v -1357.07 -2342.93 5
-v -1357.07 -2342.93 5
-v -1359.24 -2346.17 495
-v -1359.24 -2346.17 5
-v -1359.24 -2346.17 495
-v -1360 -2350 495
-v -1359.24 -2346.17 5
-v -1359.24 -2346.17 5
-v -1360 -2350 495
-v -1360 -2350 5
-v -1660 -2350 495
-v -1659.24 -2353.83 495
-v -1660 -2350 5
-v -1660 -2350 5
-v -1659.24 -2353.83 495
-v -1659.24 -2353.83 5
-v -1659.24 -2353.83 495
-v -1657.07 -2357.07 495
-v -1659.24 -2353.83 5
-v -1659.24 -2353.83 5
-v -1657.07 -2357.07 495
-v -1657.07 -2357.07 5
-v -1657.07 -2357.07 495
-v -1653.83 -2359.24 495
-v -1657.07 -2357.07 5
-v -1657.07 -2357.07 5
-v -1653.83 -2359.24 495
-v -1653.83 -2359.24 5
-v -1653.83 -2359.24 495
-v -1650 -2360 495
-v -1653.83 -2359.24 5
-v -1653.83 -2359.24 5
-v -1650 -2360 495
-v -1650 -2360 5
-v -1650 -2360 495
-v -1646.17 -2359.24 495
-v -1650 -2360 5
-v -1650 -2360 5
-v -1646.17 -2359.24 495
-v -1646.17 -2359.24 5
-v -1646.17 -2359.24 495
-v -1642.93 -2357.07 495
-v -1646.17 -2359.24 5
-v -1646.17 -2359.24 5
-v -1642.93 -2357.07 495
-v -1642.93 -2357.07 5
-v -1642.93 -2357.07 495
-v -1640.76 -2353.83 495
-v -1642.93 -2357.07 5
-v -1642.93 -2357.07 5
-v -1640.76 -2353.83 495
-v -1640.76 -2353.83 5
-v -1640.76 -2353.83 495
-v -1640 -2350 495
-v -1640.76 -2353.83 5
-v -1640.76 -2353.83 5
-v -1640 -2350 495
-v -1640 -2350 5
-v -1640 -2350 495
-v -1640.76 -2346.17 495
-v -1640 -2350 5
-v -1640 -2350 5
-v -1640.76 -2346.17 495
-v -1640.76 -2346.17 5
-v -1640.76 -2346.17 495
-v -1642.93 -2342.93 495
-v -1640.76 -2346.17 5
-v -1640.76 -2346.17 5
-v -1642.93 -2342.93 495
-v -1642.93 -2342.93 5
-v -1642.93 -2342.93 495
-v -1646.17 -2340.76 495
-v -1642.93 -2342.93 5
-v -1642.93 -2342.93 5
-v -1646.17 -2340.76 495
-v -1646.17 -2340.76 5
-v -1646.17 -2340.76 495
-v -1650 -2340 495
-v -1646.17 -2340.76 5
-v -1646.17 -2340.76 5
-v -1650 -2340 495
-v -1650 -2340 5
-v -1650 -2340 495
-v -1653.83 -2340.76 495
-v -1650 -2340 5
-v -1650 -2340 5
-v -1653.83 -2340.76 495
-v -1653.83 -2340.76 5
-v -1653.83 -2340.76 495
-v -1657.07 -2342.93 495
-v -1653.83 -2340.76 5
-v -1653.83 -2340.76 5
-v -1657.07 -2342.93 495
-v -1657.07 -2342.93 5
-v -1657.07 -2342.93 495
-v -1659.24 -2346.17 495
-v -1657.07 -2342.93 5
-v -1657.07 -2342.93 5
-v -1659.24 -2346.17 495
-v -1659.24 -2346.17 5
-v -1659.24 -2346.17 495
-v -1660 -2350 495
-v -1659.24 -2346.17 5
-v -1659.24 -2346.17 5
-v -1660 -2350 495
-v -1660 -2350 5
-v -1300 -2300 510
-v -1700 -2300 510
-v -1300 -2700 510
-v -1300 -2700 510
-v -1700 -2300 510
-v -1700 -2700 510
-v -1700 -2300 500
-v -1300 -2300 500
-v -1700 -2700 500
-v -1700 -2700 500
-v -1300 -2300 500
-v -1300 -2700 500
-v -1700 -2300 510
-v -1700 -2300 500
-v -1700 -2700 510
-v -1700 -2700 510
-v -1700 -2300 500
-v -1700 -2700 500
-v -1300 -2300 500
-v -1300 -2300 510
-v -1300 -2700 500
-v -1300 -2700 500
-v -1300 -2300 510
-v -1300 -2700 510
-v -1300 -2700 500
-v -1300 -2700 510
-v -1700 -2700 500
-v -1700 -2700 500
-v -1300 -2700 510
-v -1700 -2700 510
-v -1300 -2300 500
-v -1700 -2300 500
-v -1300 -2300 510
-v -1300 -2300 510
-v -1700 -2300 500
-v -1700 -2300 510
-v -1560 -2705 810
-v -1559.24 -2708.83 810
-v -1560 -2705 510
-v -1560 -2705 510
-v -1559.24 -2708.83 810
-v -1559.24 -2708.83 510
-v -1559.24 -2708.83 810
-v -1557.07 -2712.07 810
-v -1559.24 -2708.83 510
-v -1559.24 -2708.83 510
-v -1557.07 -2712.07 810
-v -1557.07 -2712.07 510
-v -1557.07 -2712.07 810
-v -1553.83 -2714.24 810
-v -1557.07 -2712.07 510
-v -1557.07 -2712.07 510
-v -1553.83 -2714.24 810
-v -1553.83 -2714.24 510
-v -1553.83 -2714.24 810
-v -1550 -2715 810
-v -1553.83 -2714.24 510
-v -1553.83 -2714.24 510
-v -1550 -2715 810
-v -1550 -2715 510
-v -1550 -2715 810
-v -1546.17 -2714.24 810
-v -1550 -2715 510
-v -1550 -2715 510
-v -1546.17 -2714.24 810
-v -1546.17 -2714.24 510
-v -1546.17 -2714.24 810
-v -1542.93 -2712.07 810
-v -1546.17 -2714.24 510
-v -1546.17 -2714.24 510
-v -1542.93 -2712.07 810
-v -1542.93 -2712.07 510
-v -1542.93 -2712.07 810
-v -1540.76 -2708.83 810
-v -1542.93 -2712.07 510
-v -1542.93 -2712.07 510
-v -1540.76 -2708.83 810
-v -1540.76 -2708.83 510
-v -1540.76 -2708.83 810
-v -1540 -2705 810
-v -1540.76 -2708.83 510
-v -1540.76 -2708.83 510
-v -1540 -2705 810
-v -1540 -2705 510
-v -1540 -2705 810
-v -1540.76 -2701.17 810
-v -1540 -2705 510
-v -1540 -2705 510
-v -1540.76 -2701.17 810
-v -1540.76 -2701.17 510
-v -1540.76 -2701.17 810
-v -1542.93 -2697.93 810
-v -1540.76 -2701.17 510
-v -1540.76 -2701.17 510
-v -1542.93 -2697.93 810
-v -1542.93 -2697.93 510
-v -1542.93 -2697.93 810
-v -1546.17 -2695.76 810
-v -1542.93 -2697.93 510
-v -1542.93 -2697.93 510
-v -1546.17 -2695.76 810
-v -1546.17 -2695.76 510
-v -1546.17 -2695.76 810
-v -1550 -2695 810
-v -1546.17 -2695.76 510
-v -1546.17 -2695.76 510
-v -1550 -2695 810
-v -1550 -2695 510
-v -1550 -2695 810
-v -1553.83 -2695.76 810
-v -1550 -2695 510
-v -1550 -2695 510
-v -1553.83 -2695.76 810
-v -1553.83 -2695.76 510
-v -1553.83 -2695.76 810
-v -1557.07 -2697.93 810
-v -1553.83 -2695.76 510
-v -1553.83 -2695.76 510
-v -1557.07 -2697.93 810
-v -1557.07 -2697.93 510
-v -1557.07 -2697.93 810
-v -1559.24 -2701.17 810
-v -1557.07 -2697.93 510
-v -1557.07 -2697.93 510
-v -1559.24 -2701.17 810
-v -1559.24 -2701.17 510
-v -1559.24 -2701.17 810
-v -1560 -2705 810
-v -1559.24 -2701.17 510
-v -1559.24 -2701.17 510
-v -1560 -2705 810
-v -1560 -2705 510
-v -1460 -2705 810
-v -1459.24 -2708.83 810
-v -1460 -2705 510
-v -1460 -2705 510
-v -1459.24 -2708.83 810
-v -1459.24 -2708.83 510
-v -1459.24 -2708.83 810
-v -1457.07 -2712.07 810
-v -1459.24 -2708.83 510
-v -1459.24 -2708.83 510
-v -1457.07 -2712.07 810
-v -1457.07 -2712.07 510
-v -1457.07 -2712.07 810
-v -1453.83 -2714.24 810
-v -1457.07 -2712.07 510
-v -1457.07 -2712.07 510
-v -1453.83 -2714.24 810
-v -1453.83 -2714.24 510
-v -1453.83 -2714.24 810
-v -1450 -2715 810
-v -1453.83 -2714.24 510
-v -1453.83 -2714.24 510
-v -1450 -2715 810
-v -1450 -2715 510
-v -1450 -2715 810
-v -1446.17 -2714.24 810
-v -1450 -2715 510
-v -1450 -2715 510
-v -1446.17 -2714.24 810
-v -1446.17 -2714.24 510
-v -1446.17 -2714.24 810
-v -1442.93 -2712.07 810
-v -1446.17 -2714.24 510
-v -1446.17 -2714.24 510
-v -1442.93 -2712.07 810
-v -1442.93 -2712.07 510
-v -1442.93 -2712.07 810
-v -1440.76 -2708.83 810
-v -1442.93 -2712.07 510
-v -1442.93 -2712.07 510
-v -1440.76 -2708.83 810
-v -1440.76 -2708.83 510
-v -1440.76 -2708.83 810
-v -1440 -2705 810
-v -1440.76 -2708.83 510
-v -1440.76 -2708.83 510
-v -1440 -2705 810
-v -1440 -2705 510
-v -1440 -2705 810
-v -1440.76 -2701.17 810
-v -1440 -2705 510
-v -1440 -2705 510
-v -1440.76 -2701.17 810
-v -1440.76 -2701.17 510
-v -1440.76 -2701.17 810
-v -1442.93 -2697.93 810
-v -1440.76 -2701.17 510
-v -1440.76 -2701.17 510
-v -1442.93 -2697.93 810
-v -1442.93 -2697.93 510
-v -1442.93 -2697.93 810
-v -1446.17 -2695.76 810
-v -1442.93 -2697.93 510
-v -1442.93 -2697.93 510
-v -1446.17 -2695.76 810
-v -1446.17 -2695.76 510
-v -1446.17 -2695.76 810
-v -1450 -2695 810
-v -1446.17 -2695.76 510
-v -1446.17 -2695.76 510
-v -1450 -2695 810
-v -1450 -2695 510
-v -1450 -2695 810
-v -1453.83 -2695.76 810
-v -1450 -2695 510
-v -1450 -2695 510
-v -1453.83 -2695.76 810
-v -1453.83 -2695.76 510
-v -1453.83 -2695.76 810
-v -1457.07 -2697.93 810
-v -1453.83 -2695.76 510
-v -1453.83 -2695.76 510
-v -1457.07 -2697.93 810
-v -1457.07 -2697.93 510
-v -1457.07 -2697.93 810
-v -1459.24 -2701.17 810
-v -1457.07 -2697.93 510
-v -1457.07 -2697.93 510
-v -1459.24 -2701.17 810
-v -1459.24 -2701.17 510
-v -1459.24 -2701.17 810
-v -1460 -2705 810
-v -1459.24 -2701.17 510
-v -1459.24 -2701.17 510
-v -1460 -2705 810
-v -1460 -2705 510
-v -1300 -2690 910
-v -1700 -2690 910
-v -1300 -2700 910
-v -1300 -2700 910
-v -1700 -2690 910
-v -1700 -2700 910
-v -1700 -2690 710
-v -1300 -2690 710
-v -1700 -2700 710
-v -1700 -2700 710
-v -1300 -2690 710
-v -1300 -2700 710
-v -1700 -2690 910
-v -1700 -2690 710
-v -1700 -2700 910
-v -1700 -2700 910
-v -1700 -2690 710
-v -1700 -2700 710
-v -1300 -2690 710
-v -1300 -2690 910
-v -1300 -2700 710
-v -1300 -2700 710
-v -1300 -2690 910
-v -1300 -2700 910
-v -1300 -2700 710
-v -1300 -2700 910
-v -1700 -2700 710
-v -1700 -2700 710
-v -1300 -2700 910
-v -1700 -2700 910
-v -1300 -2690 710
-v -1700 -2690 710
-v -1300 -2690 910
-v -1300 -2690 910
-v -1700 -2690 710
-v -1700 -2690 910
-v -798.346 -313.769 495
-v -797.753 -309.913 495
-v -798.346 -313.769 5
-v -798.346 -313.769 5
-v -797.753 -309.913 495
-v -797.753 -309.913 5
-v -797.753 -309.913 495
-v -798.68 -306.123 495
-v -797.753 -309.913 5
-v -797.753 -309.913 5
-v -798.68 -306.123 495
-v -798.68 -306.123 5
-v -798.68 -306.123 495
-v -800.987 -302.976 495
-v -798.68 -306.123 5
-v -798.68 -306.123 5
-v -800.987 -302.976 495
-v -800.987 -302.976 5
-v -800.987 -302.976 495
-v -804.323 -300.952 495
-v -800.987 -302.976 5
-v -800.987 -302.976 5
-v -804.323 -300.952 495
-v -804.323 -300.952 5
-v -804.323 -300.952 495
-v -808.179 -300.359 495
-v -804.323 -300.952 5
-v -804.323 -300.952 5
-v -808.179 -300.359 495
-v -808.179 -300.359 5
-v -808.179 -300.359 495
-v -811.969 -301.286 495
-v -808.179 -300.359 5
-v -808.179 -300.359 5
-v -811.969 -301.286 495
-v -811.969 -301.286 5
-v -811.969 -301.286 495
-v -815.116 -303.593 495
-v -811.969 -301.286 5
-v -811.969 -301.286 5
-v -815.116 -303.593 495
-v -815.116 -303.593 5
-v -815.116 -303.593 495
-v -817.14 -306.929 495
-v -815.116 -303.593 5
-v -815.116 -303.593 5
-v -817.14 -306.929 495
-v -817.14 -306.929 5
-v -817.14 -306.929 495
-v -817.734 -310.785 495
-v -817.14 -306.929 5
-v -817.14 -306.929 5
-v -817.734 -310.785 495
-v -817.734 -310.785 5
-v -817.734 -310.785 495
-v -816.806 -314.575 495
-v -817.734 -310.785 5
-v -817.734 -310.785 5
-v -816.806 -314.575 495
-v -816.806 -314.575 5
-v -816.806 -314.575 495
-v -814.499 -317.722 495
-v -816.806 -314.575 5
-v -816.806 -314.575 5
-v -814.499 -317.722 495
-v -814.499 -317.722 5
-v -814.499 -317.722 495
-v -811.163 -319.746 495
-v -814.499 -317.722 5
-v -814.499 -317.722 5
-v -811.163 -319.746 495
-v -811.163 -319.746 5
-v -811.163 -319.746 495
-v -807.307 -320.34 495
-v -811.163 -319.746 5
-v -811.163 -319.746 5
-v -807.307 -320.34 495
-v -807.307 -320.34 5
-v -807.307 -320.34 495
-v -803.517 -319.412 495
-v -807.307 -320.34 5
-v -807.307 -320.34 5
-v -803.517 -319.412 495
-v -803.517 -319.412 5
-v -803.517 -319.412 495
-v -800.37 -317.105 495
-v -803.517 -319.412 5
-v -803.517 -319.412 5
-v -800.37 -317.105 495
-v -800.37 -317.105 5
-v -800.37 -317.105 495
-v -798.346 -313.769 495
-v -800.37 -317.105 5
-v -800.37 -317.105 5
-v -798.346 -313.769 495
-v -798.346 -313.769 5
-v -1080.25 -211.163 495
-v -1079.66 -207.307 495
-v -1080.25 -211.163 5
-v -1080.25 -211.163 5
-v -1079.66 -207.307 495
-v -1079.66 -207.307 5
-v -1079.66 -207.307 495
-v -1080.59 -203.517 495
-v -1079.66 -207.307 5
-v -1079.66 -207.307 5
-v -1080.59 -203.517 495
-v -1080.59 -203.517 5
-v -1080.59 -203.517 495
-v -1082.9 -200.37 495
-v -1080.59 -203.517 5
-v -1080.59 -203.517 5
-v -1082.9 -200.37 495
-v -1082.9 -200.37 5
-v -1082.9 -200.37 495
-v -1086.23 -198.346 495
-v -1082.9 -200.37 5
-v -1082.9 -200.37 5
-v -1086.23 -198.346 495
-v -1086.23 -198.346 5
-v -1086.23 -198.346 495
-v -1090.09 -197.753 495
-v -1086.23 -198.346 5
-v -1086.23 -198.346 5
-v -1090.09 -197.753 495
-v -1090.09 -197.753 5
-v -1090.09 -197.753 495
-v -1093.88 -198.68 495
-v -1090.09 -197.753 5
-v -1090.09 -197.753 5
-v -1093.88 -198.68 495
-v -1093.88 -198.68 5
-v -1093.88 -198.68 495
-v -1097.02 -200.987 495
-v -1093.88 -198.68 5
-v -1093.88 -198.68 5
-v -1097.02 -200.987 495
-v -1097.02 -200.987 5
-v -1097.02 -200.987 495
-v -1099.05 -204.323 495
-v -1097.02 -200.987 5
-v -1097.02 -200.987 5
-v -1099.05 -204.323 495
-v -1099.05 -204.323 5
-v -1099.05 -204.323 495
-v -1099.64 -208.179 495
-v -1099.05 -204.323 5
-v -1099.05 -204.323 5
-v -1099.64 -208.179 495
-v -1099.64 -208.179 5
-v -1099.64 -208.179 495
-v -1098.71 -211.969 495
-v -1099.64 -208.179 5
-v -1099.64 -208.179 5
-v -1098.71 -211.969 495
-v -1098.71 -211.969 5
-v -1098.71 -211.969 495
-v -1096.41 -215.116 495
-v -1098.71 -211.969 5
-v -1098.71 -211.969 5
-v -1096.41 -215.116 495
-v -1096.41 -215.116 5
-v -1096.41 -215.116 495
-v -1093.07 -217.14 495
-v -1096.41 -215.116 5
-v -1096.41 -215.116 5
-v -1093.07 -217.14 495
-v -1093.07 -217.14 5
-v -1093.07 -217.14 495
-v -1089.21 -217.734 495
-v -1093.07 -217.14 5
-v -1093.07 -217.14 5
-v -1089.21 -217.734 495
-v -1089.21 -217.734 5
-v -1089.21 -217.734 495
-v -1085.42 -216.806 495
-v -1089.21 -217.734 5
-v -1089.21 -217.734 5
-v -1085.42 -216.806 495
-v -1085.42 -216.806 5
-v -1085.42 -216.806 495
-v -1082.28 -214.499 495
-v -1085.42 -216.806 5
-v -1085.42 -216.806 5
-v -1082.28 -214.499 495
-v -1082.28 -214.499 5
-v -1082.28 -214.499 495
-v -1080.25 -211.163 495
-v -1082.28 -214.499 5
-v -1082.28 -214.499 5
-v -1080.25 -211.163 495
-v -1080.25 -211.163 5
-v -1182.86 -493.071 495
-v -1182.27 -489.215 495
-v -1182.86 -493.071 5
-v -1182.86 -493.071 5
-v -1182.27 -489.215 495
-v -1182.27 -489.215 5
-v -1182.27 -489.215 495
-v -1183.19 -485.425 495
-v -1182.27 -489.215 5
-v -1182.27 -489.215 5
-v -1183.19 -485.425 495
-v -1183.19 -485.425 5
-v -1183.19 -485.425 495
-v -1185.5 -482.278 495
-v -1183.19 -485.425 5
-v -1183.19 -485.425 5
-v -1185.5 -482.278 495
-v -1185.5 -482.278 5
-v -1185.5 -482.278 495
-v -1188.84 -480.254 495
-v -1185.5 -482.278 5
-v -1185.5 -482.278 5
-v -1188.84 -480.254 495
-v -1188.84 -480.254 5
-v -1188.84 -480.254 495
-v -1192.69 -479.66 495
-v -1188.84 -480.254 5
-v -1188.84 -480.254 5
-v -1192.69 -479.66 495
-v -1192.69 -479.66 5
-v -1192.69 -479.66 495
-v -1196.48 -480.588 495
-v -1192.69 -479.66 5
-v -1192.69 -479.66 5
-v -1196.48 -480.588 495
-v -1196.48 -480.588 5
-v -1196.48 -480.588 495
-v -1199.63 -482.895 495
-v -1196.48 -480.588 5
-v -1196.48 -480.588 5
-v -1199.63 -482.895 495
-v -1199.63 -482.895 5
-v -1199.63 -482.895 495
-v -1201.65 -486.231 495
-v -1199.63 -482.895 5
-v -1199.63 -482.895 5
-v -1201.65 -486.231 495
-v -1201.65 -486.231 5
-v -1201.65 -486.231 495
-v -1202.25 -490.087 495
-v -1201.65 -486.231 5
-v -1201.65 -486.231 5
-v -1202.25 -490.087 495
-v -1202.25 -490.087 5
-v -1202.25 -490.087 495
-v -1201.32 -493.877 495
-v -1202.25 -490.087 5
-v -1202.25 -490.087 5
-v -1201.32 -493.877 495
-v -1201.32 -493.877 5
-v -1201.32 -493.877 495
-v -1199.01 -497.024 495
-v -1201.32 -493.877 5
-v -1201.32 -493.877 5
-v -1199.01 -497.024 495
-v -1199.01 -497.024 5
-v -1199.01 -497.024 495
-v -1195.68 -499.048 495
-v -1199.01 -497.024 5
-v -1199.01 -497.024 5
-v -1195.68 -499.048 495
-v -1195.68 -499.048 5
-v -1195.68 -499.048 495
-v -1191.82 -499.641 495
-v -1195.68 -499.048 5
-v -1195.68 -499.048 5
-v -1191.82 -499.641 495
-v -1191.82 -499.641 5
-v -1191.82 -499.641 495
-v -1188.03 -498.714 495
-v -1191.82 -499.641 5
-v -1191.82 -499.641 5
-v -1188.03 -498.714 495
-v -1188.03 -498.714 5
-v -1188.03 -498.714 495
-v -1184.88 -496.407 495
-v -1188.03 -498.714 5
-v -1188.03 -498.714 5
-v -1184.88 -496.407 495
-v -1184.88 -496.407 5
-v -1184.88 -496.407 495
-v -1182.86 -493.071 495
-v -1184.88 -496.407 5
-v -1184.88 -496.407 5
-v -1182.86 -493.071 495
-v -1182.86 -493.071 5
-v -900.952 -595.677 495
-v -900.359 -591.821 495
-v -900.952 -595.677 5
-v -900.952 -595.677 5
-v -900.359 -591.821 495
-v -900.359 -591.821 5
-v -900.359 -591.821 495
-v -901.286 -588.031 495
-v -900.359 -591.821 5
-v -900.359 -591.821 5
-v -901.286 -588.031 495
-v -901.286 -588.031 5
-v -901.286 -588.031 495
-v -903.593 -584.884 495
-v -901.286 -588.031 5
-v -901.286 -588.031 5
-v -903.593 -584.884 495
-v -903.593 -584.884 5
-v -903.593 -584.884 495
-v -906.929 -582.86 495
-v -903.593 -584.884 5
-v -903.593 -584.884 5
-v -906.929 -582.86 495
-v -906.929 -582.86 5
-v -906.929 -582.86 495
-v -910.785 -582.266 495
-v -906.929 -582.86 5
-v -906.929 -582.86 5
-v -910.785 -582.266 495
-v -910.785 -582.266 5
-v -910.785 -582.266 495
-v -914.575 -583.194 495
-v -910.785 -582.266 5
-v -910.785 -582.266 5
-v -914.575 -583.194 495
-v -914.575 -583.194 5
-v -914.575 -583.194 495
-v -917.722 -585.501 495
-v -914.575 -583.194 5
-v -914.575 -583.194 5
-v -917.722 -585.501 495
-v -917.722 -585.501 5
-v -917.722 -585.501 495
-v -919.746 -588.837 495
-v -917.722 -585.501 5
-v -917.722 -585.501 5
-v -919.746 -588.837 495
-v -919.746 -588.837 5
-v -919.746 -588.837 495
-v -920.34 -592.693 495
-v -919.746 -588.837 5
-v -919.746 -588.837 5
-v -920.34 -592.693 495
-v -920.34 -592.693 5
-v -920.34 -592.693 495
-v -919.412 -596.483 495
-v -920.34 -592.693 5
-v -920.34 -592.693 5
-v -919.412 -596.483 495
-v -919.412 -596.483 5
-v -919.412 -596.483 495
-v -917.105 -599.63 495
-v -919.412 -596.483 5
-v -919.412 -596.483 5
-v -917.105 -599.63 495
-v -917.105 -599.63 5
-v -917.105 -599.63 495
-v -913.769 -601.654 495
-v -917.105 -599.63 5
-v -917.105 -599.63 5
-v -913.769 -601.654 495
-v -913.769 -601.654 5
-v -913.769 -601.654 495
-v -909.913 -602.247 495
-v -913.769 -601.654 5
-v -913.769 -601.654 5
-v -909.913 -602.247 495
-v -909.913 -602.247 5
-v -909.913 -602.247 495
-v -906.123 -601.32 495
-v -909.913 -602.247 5
-v -909.913 -602.247 5
-v -906.123 -601.32 495
-v -906.123 -601.32 5
-v -906.123 -601.32 495
-v -902.976 -599.013 495
-v -906.123 -601.32 5
-v -906.123 -601.32 5
-v -902.976 -599.013 495
-v -902.976 -599.013 5
-v -902.976 -599.013 495
-v -900.952 -595.677 495
-v -902.976 -599.013 5
-v -902.976 -599.013 5
-v -900.952 -595.677 495
-v -900.952 -595.677 5
-v -1256.34 -519.534 510
-v -880.466 -656.343 510
-v -1119.53 -143.657 510
-v -1119.53 -143.657 510
-v -880.466 -656.343 510
-v -743.657 -280.466 510
-v -880.466 -656.343 500
-v -1256.34 -519.534 500
-v -743.657 -280.466 500
-v -743.657 -280.466 500
-v -1256.34 -519.534 500
-v -1119.53 -143.657 500
-v -880.466 -656.343 510
-v -880.466 -656.343 500
-v -743.657 -280.466 510
-v -743.657 -280.466 510
-v -880.466 -656.343 500
-v -743.657 -280.466 500
-v -1256.34 -519.534 500
-v -1256.34 -519.534 510
-v -1119.53 -143.657 500
-v -1119.53 -143.657 500
-v -1256.34 -519.534 510
-v -1119.53 -143.657 510
-v -1119.53 -143.657 500
-v -1119.53 -143.657 510
-v -743.657 -280.466 500
-v -743.657 -280.466 500
-v -1119.53 -143.657 510
-v -743.657 -280.466 510
-v -1256.34 -519.534 500
-v -880.466 -656.343 500
-v -1256.34 -519.534 510
-v -1256.34 -519.534 510
-v -880.466 -656.343 500
-v -880.466 -656.343 510
-v -873.504 -227.884 810
-v -872.911 -224.028 810
-v -873.504 -227.884 510
-v -873.504 -227.884 510
-v -872.911 -224.028 810
-v -872.911 -224.028 510
-v -872.911 -224.028 810
-v -873.838 -220.238 810
-v -872.911 -224.028 510
-v -872.911 -224.028 510
-v -873.838 -220.238 810
-v -873.838 -220.238 510
-v -873.838 -220.238 810
-v -876.145 -217.091 810
-v -873.838 -220.238 510
-v -873.838 -220.238 510
-v -876.145 -217.091 810
-v -876.145 -217.091 510
-v -876.145 -217.091 810
-v -879.481 -215.067 810
-v -876.145 -217.091 510
-v -876.145 -217.091 510
-v -879.481 -215.067 810
-v -879.481 -215.067 510
-v -879.481 -215.067 810
-v -883.337 -214.474 810
-v -879.481 -215.067 510
-v -879.481 -215.067 510
-v -883.337 -214.474 810
-v -883.337 -214.474 510
-v -883.337 -214.474 810
-v -887.127 -215.401 810
-v -883.337 -214.474 510
-v -883.337 -214.474 510
-v -887.127 -215.401 810
-v -887.127 -215.401 510
-v -887.127 -215.401 810
-v -890.274 -217.708 810
-v -887.127 -215.401 510
-v -887.127 -215.401 510
-v -890.274 -217.708 810
-v -890.274 -217.708 510
-v -890.274 -217.708 810
-v -892.298 -221.044 810
-v -890.274 -217.708 510
-v -890.274 -217.708 510
-v -892.298 -221.044 810
-v -892.298 -221.044 510
-v -892.298 -221.044 810
-v -892.892 -224.9 810
-v -892.298 -221.044 510
-v -892.298 -221.044 510
-v -892.892 -224.9 810
-v -892.892 -224.9 510
-v -892.892 -224.9 810
-v -891.964 -228.69 810
-v -892.892 -224.9 510
-v -892.892 -224.9 510
-v -891.964 -228.69 810
-v -891.964 -228.69 510
-v -891.964 -228.69 810
-v -889.657 -231.837 810
-v -891.964 -228.69 510
-v -891.964 -228.69 510
-v -889.657 -231.837 810
-v -889.657 -231.837 510
-v -889.657 -231.837 810
-v -886.321 -233.861 810
-v -889.657 -231.837 510
-v -889.657 -231.837 510
-v -886.321 -233.861 810
-v -886.321 -233.861 510
-v -886.321 -233.861 810
-v -882.465 -234.454 810
-v -886.321 -233.861 510
-v -886.321 -233.861 510
-v -882.465 -234.454 810
-v -882.465 -234.454 510
-v -882.465 -234.454 810
-v -878.675 -233.527 810
-v -882.465 -234.454 510
-v -882.465 -234.454 510
-v -878.675 -233.527 810
-v -878.675 -233.527 510
-v -878.675 -233.527 810
-v -875.528 -231.22 810
-v -878.675 -233.527 510
-v -878.675 -233.527 510
-v -875.528 -231.22 810
-v -875.528 -231.22 510
-v -875.528 -231.22 810
-v -873.504 -227.884 810
-v -875.528 -231.22 510
-v -875.528 -231.22 510
-v -873.504 -227.884 810
-v -873.504 -227.884 510
-v -967.474 -193.682 810
-v -966.88 -189.826 810
-v -967.474 -193.682 510
-v -967.474 -193.682 510
-v -966.88 -189.826 810
-v -966.88 -189.826 510
-v -966.88 -189.826 810
-v -967.807 -186.036 810
-v -966.88 -189.826 510
-v -966.88 -189.826 510
-v -967.807 -186.036 810
-v -967.807 -186.036 510
-v -967.807 -186.036 810
-v -970.115 -182.889 810
-v -967.807 -186.036 510
-v -967.807 -186.036 510
-v -970.115 -182.889 810
-v -970.115 -182.889 510
-v -970.115 -182.889 810
-v -973.45 -180.865 810
-v -970.115 -182.889 510
-v -970.115 -182.889 510
-v -973.45 -180.865 810
-v -973.45 -180.865 510
-v -973.45 -180.865 810
-v -977.307 -180.272 810
-v -973.45 -180.865 510
-v -973.45 -180.865 510
-v -977.307 -180.272 810
-v -977.307 -180.272 510
-v -977.307 -180.272 810
-v -981.097 -181.199 810
-v -977.307 -180.272 510
-v -977.307 -180.272 510
-v -981.097 -181.199 810
-v -981.097 -181.199 510
-v -981.097 -181.199 810
-v -984.243 -183.506 810
-v -981.097 -181.199 510
-v -981.097 -181.199 510
-v -984.243 -183.506 810
-v -984.243 -183.506 510
-v -984.243 -183.506 810
-v -986.267 -186.842 810
-v -984.243 -183.506 510
-v -984.243 -183.506 510
-v -986.267 -186.842 810
-v -986.267 -186.842 510
-v -986.267 -186.842 810
-v -986.861 -190.698 810
-v -986.267 -186.842 510
-v -986.267 -186.842 510
-v -986.861 -190.698 810
-v -986.861 -190.698 510
-v -986.861 -190.698 810
-v -985.934 -194.488 810
-v -986.861 -190.698 510
-v -986.861 -190.698 510
-v -985.934 -194.488 810
-v -985.934 -194.488 510
-v -985.934 -194.488 810
-v -983.626 -197.635 810
-v -985.934 -194.488 510
-v -985.934 -194.488 510
-v -983.626 -197.635 810
-v -983.626 -197.635 510
-v -983.626 -197.635 810
-v -980.291 -199.659 810
-v -983.626 -197.635 510
-v -983.626 -197.635 510
-v -980.291 -199.659 810
-v -980.291 -199.659 510
-v -980.291 -199.659 810
-v -976.434 -200.252 810
-v -980.291 -199.659 510
-v -980.291 -199.659 510
-v -976.434 -200.252 810
-v -976.434 -200.252 510
-v -976.434 -200.252 810
-v -972.644 -199.325 810
-v -976.434 -200.252 510
-v -976.434 -200.252 510
-v -972.644 -199.325 810
-v -972.644 -199.325 510
-v -972.644 -199.325 810
-v -969.498 -197.018 810
-v -972.644 -199.325 510
-v -972.644 -199.325 510
-v -969.498 -197.018 810
-v -969.498 -197.018 510
-v -969.498 -197.018 810
-v -967.474 -193.682 810
-v -969.498 -197.018 510
-v -969.498 -197.018 510
-v -967.474 -193.682 810
-v -967.474 -193.682 510
-v -1122.95 -153.054 910
-v -747.078 -289.862 910
-v -1119.53 -143.657 910
-v -1119.53 -143.657 910
-v -747.078 -289.862 910
-v -743.657 -280.465 910
-v -747.078 -289.862 710
-v -1122.95 -153.054 710
-v -743.657 -280.465 710
-v -743.657 -280.465 710
-v -1122.95 -153.054 710
-v -1119.53 -143.657 710
-v -747.078 -289.862 910
-v -747.078 -289.862 710
-v -743.657 -280.465 910
-v -743.657 -280.465 910
-v -747.078 -289.862 710
-v -743.657 -280.465 710
-v -1122.95 -153.054 710
-v -1122.95 -153.054 910
-v -1119.53 -143.657 710
-v -1119.53 -143.657 710
-v -1122.95 -153.054 910
-v -1119.53 -143.657 910
-v -1119.53 -143.657 710
-v -1119.53 -143.657 910
-v -743.657 -280.465 710
-v -743.657 -280.465 710
-v -1119.53 -143.657 910
-v -743.657 -280.465 910
-v -1122.95 -153.054 710
-v -747.078 -289.862 710
-v -1122.95 -153.054 910
-v -1122.95 -153.054 910
-v -747.078 -289.862 710
-v -747.078 -289.862 910
-v -1900.95 -1604.32 495
-v -1902.98 -1600.99 495
-v -1900.95 -1604.32 5
-v -1900.95 -1604.32 5
-v -1902.98 -1600.99 495
-v -1902.98 -1600.99 5
-v -1902.98 -1600.99 495
-v -1906.12 -1598.68 495
-v -1902.98 -1600.99 5
-v -1902.98 -1600.99 5
-v -1906.12 -1598.68 495
-v -1906.12 -1598.68 5
-v -1906.12 -1598.68 495
-v -1909.91 -1597.75 495
-v -1906.12 -1598.68 5
-v -1906.12 -1598.68 5
-v -1909.91 -1597.75 495
-v -1909.91 -1597.75 5
-v -1909.91 -1597.75 495
-v -1913.77 -1598.35 495
-v -1909.91 -1597.75 5
-v -1909.91 -1597.75 5
-v -1913.77 -1598.35 495
-v -1913.77 -1598.35 5
-v -1913.77 -1598.35 495
-v -1917.1 -1600.37 495
-v -1913.77 -1598.35 5
-v -1913.77 -1598.35 5
-v -1917.1 -1600.37 495
-v -1917.1 -1600.37 5
-v -1917.1 -1600.37 495
-v -1919.41 -1603.52 495
-v -1917.1 -1600.37 5
-v -1917.1 -1600.37 5
-v -1919.41 -1603.52 495
-v -1919.41 -1603.52 5
-v -1919.41 -1603.52 495
-v -1920.34 -1607.31 495
-v -1919.41 -1603.52 5
-v -1919.41 -1603.52 5
-v -1920.34 -1607.31 495
-v -1920.34 -1607.31 5
-v -1920.34 -1607.31 495
-v -1919.75 -1611.16 495
-v -1920.34 -1607.31 5
-v -1920.34 -1607.31 5
-v -1919.75 -1611.16 495
-v -1919.75 -1611.16 5
-v -1919.75 -1611.16 495
-v -1917.72 -1614.5 495
-v -1919.75 -1611.16 5
-v -1919.75 -1611.16 5
-v -1917.72 -1614.5 495
-v -1917.72 -1614.5 5
-v -1917.72 -1614.5 495
-v -1914.58 -1616.81 495
-v -1917.72 -1614.5 5
-v -1917.72 -1614.5 5
-v -1914.58 -1616.81 495
-v -1914.58 -1616.81 5
-v -1914.58 -1616.81 495
-v -1910.79 -1617.73 495
-v -1914.58 -1616.81 5
-v -1914.58 -1616.81 5
-v -1910.79 -1617.73 495
-v -1910.79 -1617.73 5
-v -1910.79 -1617.73 495
-v -1906.93 -1617.14 495
-v -1910.79 -1617.73 5
-v -1910.79 -1617.73 5
-v -1906.93 -1617.14 495
-v -1906.93 -1617.14 5
-v -1906.93 -1617.14 495
-v -1903.59 -1615.12 495
-v -1906.93 -1617.14 5
-v -1906.93 -1617.14 5
-v -1903.59 -1615.12 495
-v -1903.59 -1615.12 5
-v -1903.59 -1615.12 495
-v -1901.29 -1611.97 495
-v -1903.59 -1615.12 5
-v -1903.59 -1615.12 5
-v -1901.29 -1611.97 495
-v -1901.29 -1611.97 5
-v -1901.29 -1611.97 495
-v -1900.36 -1608.18 495
-v -1901.29 -1611.97 5
-v -1901.29 -1611.97 5
-v -1900.36 -1608.18 495
-v -1900.36 -1608.18 5
-v -1900.36 -1608.18 495
-v -1900.95 -1604.32 495
-v -1900.36 -1608.18 5
-v -1900.36 -1608.18 5
-v -1900.95 -1604.32 495
-v -1900.95 -1604.32 5
-v -2182.86 -1706.93 495
-v -2184.88 -1703.59 495
-v -2182.86 -1706.93 5
-v -2182.86 -1706.93 5
-v -2184.88 -1703.59 495
-v -2184.88 -1703.59 5
-v -2184.88 -1703.59 495
-v -2188.03 -1701.29 495
-v -2184.88 -1703.59 5
-v -2184.88 -1703.59 5
-v -2188.03 -1701.29 495
-v -2188.03 -1701.29 5
-v -2188.03 -1701.29 495
-v -2191.82 -1700.36 495
-v -2188.03 -1701.29 5
-v -2188.03 -1701.29 5
-v -2191.82 -1700.36 495
-v -2191.82 -1700.36 5
-v -2191.82 -1700.36 495
-v -2195.68 -1700.95 495
-v -2191.82 -1700.36 5
-v -2191.82 -1700.36 5
-v -2195.68 -1700.95 495
-v -2195.68 -1700.95 5
-v -2195.68 -1700.95 495
-v -2199.01 -1702.98 495
-v -2195.68 -1700.95 5
-v -2195.68 -1700.95 5
-v -2199.01 -1702.98 495
-v -2199.01 -1702.98 5
-v -2199.01 -1702.98 495
-v -2201.32 -1706.12 495
-v -2199.01 -1702.98 5
-v -2199.01 -1702.98 5
-v -2201.32 -1706.12 495
-v -2201.32 -1706.12 5
-v -2201.32 -1706.12 495
-v -2202.25 -1709.91 495
-v -2201.32 -1706.12 5
-v -2201.32 -1706.12 5
-v -2202.25 -1709.91 495
-v -2202.25 -1709.91 5
-v -2202.25 -1709.91 495
-v -2201.65 -1713.77 495
-v -2202.25 -1709.91 5
-v -2202.25 -1709.91 5
-v -2201.65 -1713.77 495
-v -2201.65 -1713.77 5
-v -2201.65 -1713.77 495
-v -2199.63 -1717.1 495
-v -2201.65 -1713.77 5
-v -2201.65 -1713.77 5
-v -2199.63 -1717.1 495
-v -2199.63 -1717.1 5
-v -2199.63 -1717.1 495
-v -2196.48 -1719.41 495
-v -2199.63 -1717.1 5
-v -2199.63 -1717.1 5
-v -2196.48 -1719.41 495
-v -2196.48 -1719.41 5
-v -2196.48 -1719.41 495
-v -2192.69 -1720.34 495
-v -2196.48 -1719.41 5
-v -2196.48 -1719.41 5
-v -2192.69 -1720.34 495
-v -2192.69 -1720.34 5
-v -2192.69 -1720.34 495
-v -2188.84 -1719.75 495
-v -2192.69 -1720.34 5
-v -2192.69 -1720.34 5
-v -2188.84 -1719.75 495
-v -2188.84 -1719.75 5
-v -2188.84 -1719.75 495
-v -2185.5 -1717.72 495
-v -2188.84 -1719.75 5
-v -2188.84 -1719.75 5
-v -2185.5 -1717.72 495
-v -2185.5 -1717.72 5
-v -2185.5 -1717.72 495
-v -2183.19 -1714.58 495
-v -2185.5 -1717.72 5
-v -2185.5 -1717.72 5
-v -2183.19 -1714.58 495
-v -2183.19 -1714.58 5
-v -2183.19 -1714.58 495
-v -2182.27 -1710.79 495
-v -2183.19 -1714.58 5
-v -2183.19 -1714.58 5
-v -2182.27 -1710.79 495
-v -2182.27 -1710.79 5
-v -2182.27 -1710.79 495
-v -2182.86 -1706.93 495
-v -2182.27 -1710.79 5
-v -2182.27 -1710.79 5
-v -2182.86 -1706.93 495
-v -2182.86 -1706.93 5
-v -2080.25 -1988.84 495
-v -2082.28 -1985.5 495
-v -2080.25 -1988.84 5
-v -2080.25 -1988.84 5
-v -2082.28 -1985.5 495
-v -2082.28 -1985.5 5
-v -2082.28 -1985.5 495
-v -2085.42 -1983.19 495
-v -2082.28 -1985.5 5
-v -2082.28 -1985.5 5
-v -2085.42 -1983.19 495
-v -2085.42 -1983.19 5
-v -2085.42 -1983.19 495
-v -2089.21 -1982.27 495
-v -2085.42 -1983.19 5
-v -2085.42 -1983.19 5
-v -2089.21 -1982.27 495
-v -2089.21 -1982.27 5
-v -2089.21 -1982.27 495
-v -2093.07 -1982.86 495
-v -2089.21 -1982.27 5
-v -2089.21 -1982.27 5
-v -2093.07 -1982.86 495
-v -2093.07 -1982.86 5
-v -2093.07 -1982.86 495
-v -2096.41 -1984.88 495
-v -2093.07 -1982.86 5
-v -2093.07 -1982.86 5
-v -2096.41 -1984.88 495
-v -2096.41 -1984.88 5
-v -2096.41 -1984.88 495
-v -2098.71 -1988.03 495
-v -2096.41 -1984.88 5
-v -2096.41 -1984.88 5
-v -2098.71 -1988.03 495
-v -2098.71 -1988.03 5
-v -2098.71 -1988.03 495
-v -2099.64 -1991.82 495
-v -2098.71 -1988.03 5
-v -2098.71 -1988.03 5
-v -2099.64 -1991.82 495
-v -2099.64 -1991.82 5
-v -2099.64 -1991.82 495
-v -2099.05 -1995.68 495
-v -2099.64 -1991.82 5
-v -2099.64 -1991.82 5
-v -2099.05 -1995.68 495
-v -2099.05 -1995.68 5
-v -2099.05 -1995.68 495
-v -2097.02 -1999.01 495
-v -2099.05 -1995.68 5
-v -2099.05 -1995.68 5
-v -2097.02 -1999.01 495
-v -2097.02 -1999.01 5
-v -2097.02 -1999.01 495
-v -2093.88 -2001.32 495
-v -2097.02 -1999.01 5
-v -2097.02 -1999.01 5
-v -2093.88 -2001.32 495
-v -2093.88 -2001.32 5
-v -2093.88 -2001.32 495
-v -2090.09 -2002.25 495
-v -2093.88 -2001.32 5
-v -2093.88 -2001.32 5
-v -2090.09 -2002.25 495
-v -2090.09 -2002.25 5
-v -2090.09 -2002.25 495
-v -2086.23 -2001.65 495
-v -2090.09 -2002.25 5
-v -2090.09 -2002.25 5
-v -2086.23 -2001.65 495
-v -2086.23 -2001.65 5
-v -2086.23 -2001.65 495
-v -2082.9 -1999.63 495
-v -2086.23 -2001.65 5
-v -2086.23 -2001.65 5
-v -2082.9 -1999.63 495
-v -2082.9 -1999.63 5
-v -2082.9 -1999.63 495
-v -2080.59 -1996.48 495
-v -2082.9 -1999.63 5
-v -2082.9 -1999.63 5
-v -2080.59 -1996.48 495
-v -2080.59 -1996.48 5
-v -2080.59 -1996.48 495
-v -2079.66 -1992.69 495
-v -2080.59 -1996.48 5
-v -2080.59 -1996.48 5
-v -2079.66 -1992.69 495
-v -2079.66 -1992.69 5
-v -2079.66 -1992.69 495
-v -2080.25 -1988.84 495
-v -2079.66 -1992.69 5
-v -2079.66 -1992.69 5
-v -2080.25 -1988.84 495
-v -2080.25 -1988.84 5
-v -1798.35 -1886.23 495
-v -1800.37 -1882.9 495
-v -1798.35 -1886.23 5
-v -1798.35 -1886.23 5
-v -1800.37 -1882.9 495
-v -1800.37 -1882.9 5
-v -1800.37 -1882.9 495
-v -1803.52 -1880.59 495
-v -1800.37 -1882.9 5
-v -1800.37 -1882.9 5
-v -1803.52 -1880.59 495
-v -1803.52 -1880.59 5
-v -1803.52 -1880.59 495
-v -1807.31 -1879.66 495
-v -1803.52 -1880.59 5
-v -1803.52 -1880.59 5
-v -1807.31 -1879.66 495
-v -1807.31 -1879.66 5
-v -1807.31 -1879.66 495
-v -1811.16 -1880.25 495
-v -1807.31 -1879.66 5
-v -1807.31 -1879.66 5
-v -1811.16 -1880.25 495
-v -1811.16 -1880.25 5
-v -1811.16 -1880.25 495
-v -1814.5 -1882.28 495
-v -1811.16 -1880.25 5
-v -1811.16 -1880.25 5
-v -1814.5 -1882.28 495
-v -1814.5 -1882.28 5
-v -1814.5 -1882.28 495
-v -1816.81 -1885.42 495
-v -1814.5 -1882.28 5
-v -1814.5 -1882.28 5
-v -1816.81 -1885.42 495
-v -1816.81 -1885.42 5
-v -1816.81 -1885.42 495
-v -1817.73 -1889.21 495
-v -1816.81 -1885.42 5
-v -1816.81 -1885.42 5
-v -1817.73 -1889.21 495
-v -1817.73 -1889.21 5
-v -1817.73 -1889.21 495
-v -1817.14 -1893.07 495
-v -1817.73 -1889.21 5
-v -1817.73 -1889.21 5
-v -1817.14 -1893.07 495
-v -1817.14 -1893.07 5
-v -1817.14 -1893.07 495
-v -1815.12 -1896.41 495
-v -1817.14 -1893.07 5
-v -1817.14 -1893.07 5
-v -1815.12 -1896.41 495
-v -1815.12 -1896.41 5
-v -1815.12 -1896.41 495
-v -1811.97 -1898.71 495
-v -1815.12 -1896.41 5
-v -1815.12 -1896.41 5
-v -1811.97 -1898.71 495
-v -1811.97 -1898.71 5
-v -1811.97 -1898.71 495
-v -1808.18 -1899.64 495
-v -1811.97 -1898.71 5
-v -1811.97 -1898.71 5
-v -1808.18 -1899.64 495
-v -1808.18 -1899.64 5
-v -1808.18 -1899.64 495
-v -1804.32 -1899.05 495
-v -1808.18 -1899.64 5
-v -1808.18 -1899.64 5
-v -1804.32 -1899.05 495
-v -1804.32 -1899.05 5
-v -1804.32 -1899.05 495
-v -1800.99 -1897.02 495
-v -1804.32 -1899.05 5
-v -1804.32 -1899.05 5
-v -1800.99 -1897.02 495
-v -1800.99 -1897.02 5
-v -1800.99 -1897.02 495
-v -1798.68 -1893.88 495
-v -1800.99 -1897.02 5
-v -1800.99 -1897.02 5
-v -1798.68 -1893.88 495
-v -1798.68 -1893.88 5
-v -1798.68 -1893.88 495
-v -1797.75 -1890.09 495
-v -1798.68 -1893.88 5
-v -1798.68 -1893.88 5
-v -1797.75 -1890.09 495
-v -1797.75 -1890.09 5
-v -1797.75 -1890.09 495
-v -1798.35 -1886.23 495
-v -1797.75 -1890.09 5
-v -1797.75 -1890.09 5
-v -1798.35 -1886.23 495
-v -1798.35 -1886.23 5
-v -2119.53 -2056.34 510
-v -1743.66 -1919.53 510
-v -2256.34 -1680.47 510
-v -2256.34 -1680.47 510
-v -1743.66 -1919.53 510
-v -1880.47 -1543.66 510
-v -1743.66 -1919.53 500
-v -2119.53 -2056.34 500
-v -1880.47 -1543.66 500
-v -1880.47 -1543.66 500
-v -2119.53 -2056.34 500
-v -2256.34 -1680.47 500
-v -1743.66 -1919.53 510
-v -1743.66 -1919.53 500
-v -1880.47 -1543.66 510
-v -1880.47 -1543.66 510
-v -1743.66 -1919.53 500
-v -1880.47 -1543.66 500
-v -2119.53 -2056.34 500
-v -2119.53 -2056.34 510
-v -2256.34 -1680.47 500
-v -2256.34 -1680.47 500
-v -2119.53 -2056.34 510
-v -2256.34 -1680.47 510
-v -2256.34 -1680.47 500
-v -2256.34 -1680.47 510
-v -1880.47 -1543.66 500
-v -1880.47 -1543.66 500
-v -2256.34 -1680.47 510
-v -1880.47 -1543.66 510
-v -2119.53 -2056.34 500
-v -1743.66 -1919.53 500
-v -2119.53 -2056.34 510
-v -2119.53 -2056.34 510
-v -1743.66 -1919.53 500
-v -1743.66 -1919.53 510
-v -2013.73 -1586.84 810
-v -2015.76 -1583.51 810
-v -2013.73 -1586.84 510
-v -2013.73 -1586.84 510
-v -2015.76 -1583.51 810
-v -2015.76 -1583.51 510
-v -2015.76 -1583.51 810
-v -2018.9 -1581.2 810
-v -2015.76 -1583.51 510
-v -2015.76 -1583.51 510
-v -2018.9 -1581.2 810
-v -2018.9 -1581.2 510
-v -2018.9 -1581.2 810
-v -2022.69 -1580.27 810
-v -2018.9 -1581.2 510
-v -2018.9 -1581.2 510
-v -2022.69 -1580.27 810
-v -2022.69 -1580.27 510
-v -2022.69 -1580.27 810
-v -2026.55 -1580.86 810
-v -2022.69 -1580.27 510
-v -2022.69 -1580.27 510
-v -2026.55 -1580.86 810
-v -2026.55 -1580.86 510
-v -2026.55 -1580.86 810
-v -2029.89 -1582.89 810
-v -2026.55 -1580.86 510
-v -2026.55 -1580.86 510
-v -2029.89 -1582.89 810
-v -2029.89 -1582.89 510
-v -2029.89 -1582.89 810
-v -2032.19 -1586.04 810
-v -2029.89 -1582.89 510
-v -2029.89 -1582.89 510
-v -2032.19 -1586.04 810
-v -2032.19 -1586.04 510
-v -2032.19 -1586.04 810
-v -2033.12 -1589.83 810
-v -2032.19 -1586.04 510
-v -2032.19 -1586.04 510
-v -2033.12 -1589.83 810
-v -2033.12 -1589.83 510
-v -2033.12 -1589.83 810
-v -2032.53 -1593.68 810
-v -2033.12 -1589.83 510
-v -2033.12 -1589.83 510
-v -2032.53 -1593.68 810
-v -2032.53 -1593.68 510
-v -2032.53 -1593.68 810
-v -2030.5 -1597.02 810
-v -2032.53 -1593.68 510
-v -2032.53 -1593.68 510
-v -2030.5 -1597.02 810
-v -2030.5 -1597.02 510
-v -2030.5 -1597.02 810
-v -2027.36 -1599.33 810
-v -2030.5 -1597.02 510
-v -2030.5 -1597.02 510
-v -2027.36 -1599.33 810
-v -2027.36 -1599.33 510
-v -2027.36 -1599.33 810
-v -2023.57 -1600.25 810
-v -2027.36 -1599.33 510
-v -2027.36 -1599.33 510
-v -2023.57 -1600.25 810
-v -2023.57 -1600.25 510
-v -2023.57 -1600.25 810
-v -2019.71 -1599.66 810
-v -2023.57 -1600.25 510
-v -2023.57 -1600.25 510
-v -2019.71 -1599.66 810
-v -2019.71 -1599.66 510
-v -2019.71 -1599.66 810
-v -2016.37 -1597.63 810
-v -2019.71 -1599.66 510
-v -2019.71 -1599.66 510
-v -2016.37 -1597.63 810
-v -2016.37 -1597.63 510
-v -2016.37 -1597.63 810
-v -2014.07 -1594.49 810
-v -2016.37 -1597.63 510
-v -2016.37 -1597.63 510
-v -2014.07 -1594.49 810
-v -2014.07 -1594.49 510
-v -2014.07 -1594.49 810
-v -2013.14 -1590.7 810
-v -2014.07 -1594.49 510
-v -2014.07 -1594.49 510
-v -2013.14 -1590.7 810
-v -2013.14 -1590.7 510
-v -2013.14 -1590.7 810
-v -2013.73 -1586.84 810
-v -2013.14 -1590.7 510
-v -2013.14 -1590.7 510
-v -2013.73 -1586.84 810
-v -2013.73 -1586.84 510
-v -2107.7 -1621.04 810
-v -2109.73 -1617.71 810
-v -2107.7 -1621.04 510
-v -2107.7 -1621.04 510
-v -2109.73 -1617.71 810
-v -2109.73 -1617.71 510
-v -2109.73 -1617.71 810
-v -2112.87 -1615.4 810
-v -2109.73 -1617.71 510
-v -2109.73 -1617.71 510
-v -2112.87 -1615.4 810
-v -2112.87 -1615.4 510
-v -2112.87 -1615.4 810
-v -2116.66 -1614.47 810
-v -2112.87 -1615.4 510
-v -2112.87 -1615.4 510
-v -2116.66 -1614.47 810
-v -2116.66 -1614.47 510
-v -2116.66 -1614.47 810
-v -2120.52 -1615.07 810
-v -2116.66 -1614.47 510
-v -2116.66 -1614.47 510
-v -2120.52 -1615.07 810
-v -2120.52 -1615.07 510
-v -2120.52 -1615.07 810
-v -2123.85 -1617.09 810
-v -2120.52 -1615.07 510
-v -2120.52 -1615.07 510
-v -2123.85 -1617.09 810
-v -2123.85 -1617.09 510
-v -2123.85 -1617.09 810
-v -2126.16 -1620.24 810
-v -2123.85 -1617.09 510
-v -2123.85 -1617.09 510
-v -2126.16 -1620.24 810
-v -2126.16 -1620.24 510
-v -2126.16 -1620.24 810
-v -2127.09 -1624.03 810
-v -2126.16 -1620.24 510
-v -2126.16 -1620.24 510
-v -2127.09 -1624.03 810
-v -2127.09 -1624.03 510
-v -2127.09 -1624.03 810
-v -2126.5 -1627.88 810
-v -2127.09 -1624.03 510
-v -2127.09 -1624.03 510
-v -2126.5 -1627.88 810
-v -2126.5 -1627.88 510
-v -2126.5 -1627.88 810
-v -2124.47 -1631.22 810
-v -2126.5 -1627.88 510
-v -2126.5 -1627.88 510
-v -2124.47 -1631.22 810
-v -2124.47 -1631.22 510
-v -2124.47 -1631.22 810
-v -2121.32 -1633.53 810
-v -2124.47 -1631.22 510
-v -2124.47 -1631.22 510
-v -2121.32 -1633.53 810
-v -2121.32 -1633.53 510
-v -2121.32 -1633.53 810
-v -2117.53 -1634.45 810
-v -2121.32 -1633.53 510
-v -2121.32 -1633.53 510
-v -2117.53 -1634.45 810
-v -2117.53 -1634.45 510
-v -2117.53 -1634.45 810
-v -2113.68 -1633.86 810
-v -2117.53 -1634.45 510
-v -2117.53 -1634.45 510
-v -2113.68 -1633.86 810
-v -2113.68 -1633.86 510
-v -2113.68 -1633.86 810
-v -2110.34 -1631.84 810
-v -2113.68 -1633.86 510
-v -2113.68 -1633.86 510
-v -2110.34 -1631.84 810
-v -2110.34 -1631.84 510
-v -2110.34 -1631.84 810
-v -2108.04 -1628.69 810
-v -2110.34 -1631.84 510
-v -2110.34 -1631.84 510
-v -2108.04 -1628.69 810
-v -2108.04 -1628.69 510
-v -2108.04 -1628.69 810
-v -2107.11 -1624.9 810
-v -2108.04 -1628.69 510
-v -2108.04 -1628.69 510
-v -2107.11 -1624.9 810
-v -2107.11 -1624.9 510
-v -2107.11 -1624.9 810
-v -2107.7 -1621.04 810
-v -2107.11 -1624.9 510
-v -2107.11 -1624.9 510
-v -2107.7 -1621.04 810
-v -2107.7 -1621.04 510
-v -2252.92 -1689.86 910
-v -1877.05 -1553.05 910
-v -2256.34 -1680.47 910
-v -2256.34 -1680.47 910
-v -1877.05 -1553.05 910
-v -1880.47 -1543.66 910
-v -1877.05 -1553.05 710
-v -2252.92 -1689.86 710
-v -1880.47 -1543.66 710
-v -1880.47 -1543.66 710
-v -2252.92 -1689.86 710
-v -2256.34 -1680.47 710
-v -1877.05 -1553.05 910
-v -1877.05 -1553.05 710
-v -1880.47 -1543.66 910
-v -1880.47 -1543.66 910
-v -1877.05 -1553.05 710
-v -1880.47 -1543.66 710
-v -2252.92 -1689.86 710
-v -2252.92 -1689.86 910
-v -2256.34 -1680.47 710
-v -2256.34 -1680.47 710
-v -2252.92 -1689.86 910
-v -2256.34 -1680.47 910
-v -2256.34 -1680.47 710
-v -2256.34 -1680.47 910
-v -1880.47 -1543.66 710
-v -1880.47 -1543.66 710
-v -2256.34 -1680.47 910
-v -1880.47 -1543.66 910
-v -2252.92 -1689.86 710
-v -1877.05 -1553.05 710
-v -2252.92 -1689.86 910
-v -2252.92 -1689.86 910
-v -1877.05 -1553.05 710
-v -1877.05 -1553.05 910
-v -2880 1350 695
-v -2881.52 1357.65 695
-v -2880 1350 5
-v -2880 1350 5
-v -2881.52 1357.65 695
-v -2881.52 1357.65 5
-v -2881.52 1357.65 695
-v -2885.86 1364.14 695
-v -2881.52 1357.65 5
-v -2881.52 1357.65 5
-v -2885.86 1364.14 695
-v -2885.86 1364.14 5
-v -2885.86 1364.14 695
-v -2892.35 1368.48 695
-v -2885.86 1364.14 5
-v -2885.86 1364.14 5
-v -2892.35 1368.48 695
-v -2892.35 1368.48 5
-v -2892.35 1368.48 695
-v -2900 1370 695
-v -2892.35 1368.48 5
-v -2892.35 1368.48 5
-v -2900 1370 695
-v -2900 1370 5
-v -2900 1370 695
-v -2907.65 1368.48 695
-v -2900 1370 5
-v -2900 1370 5
-v -2907.65 1368.48 695
-v -2907.65 1368.48 5
-v -2907.65 1368.48 695
-v -2914.14 1364.14 695
-v -2907.65 1368.48 5
-v -2907.65 1368.48 5
-v -2914.14 1364.14 695
-v -2914.14 1364.14 5
-v -2914.14 1364.14 695
-v -2918.48 1357.65 695
-v -2914.14 1364.14 5
-v -2914.14 1364.14 5
-v -2918.48 1357.65 695
-v -2918.48 1357.65 5
-v -2918.48 1357.65 695
-v -2920 1350 695
-v -2918.48 1357.65 5
-v -2918.48 1357.65 5
-v -2920 1350 695
-v -2920 1350 5
-v -2920 1350 695
-v -2918.48 1342.35 695
-v -2920 1350 5
-v -2920 1350 5
-v -2918.48 1342.35 695
-v -2918.48 1342.35 5
-v -2918.48 1342.35 695
-v -2914.14 1335.86 695
-v -2918.48 1342.35 5
-v -2918.48 1342.35 5
-v -2914.14 1335.86 695
-v -2914.14 1335.86 5
-v -2914.14 1335.86 695
-v -2907.65 1331.52 695
-v -2914.14 1335.86 5
-v -2914.14 1335.86 5
-v -2907.65 1331.52 695
-v -2907.65 1331.52 5
-v -2907.65 1331.52 695
-v -2900 1330 695
-v -2907.65 1331.52 5
-v -2907.65 1331.52 5
-v -2900 1330 695
-v -2900 1330 5
-v -2900 1330 695
-v -2892.35 1331.52 695
-v -2900 1330 5
-v -2900 1330 5
-v -2892.35 1331.52 695
-v -2892.35 1331.52 5
-v -2892.35 1331.52 695
-v -2885.86 1335.86 695
-v -2892.35 1331.52 5
-v -2892.35 1331.52 5
-v -2885.86 1335.86 695
-v -2885.86 1335.86 5
-v -2885.86 1335.86 695
-v -2881.52 1342.35 695
-v -2885.86 1335.86 5
-v -2885.86 1335.86 5
-v -2881.52 1342.35 695
-v -2881.52 1342.35 5
-v -2881.52 1342.35 695
-v -2880 1350 695
-v -2881.52 1342.35 5
-v -2881.52 1342.35 5
-v -2880 1350 695
-v -2880 1350 5
-v 20 1350 695
-v 18.4776 1357.65 695
-v 20 1350 5
-v 20 1350 5
-v 18.4776 1357.65 695
-v 18.4776 1357.65 5
-v 18.4776 1357.65 695
-v 14.1421 1364.14 695
-v 18.4776 1357.65 5
-v 18.4776 1357.65 5
-v 14.1421 1364.14 695
-v 14.1421 1364.14 5
-v 14.1421 1364.14 695
-v 7.65367 1368.48 695
-v 14.1421 1364.14 5
-v 14.1421 1364.14 5
-v 7.65367 1368.48 695
-v 7.65367 1368.48 5
-v 7.65367 1368.48 695
-v -1e-006 1370 695
-v 7.65367 1368.48 5
-v 7.65367 1368.48 5
-v -1e-006 1370 695
-v -1e-006 1370 5
-v -1e-006 1370 695
-v -7.65367 1368.48 695
-v -1e-006 1370 5
-v -1e-006 1370 5
-v -7.65367 1368.48 695
-v -7.65367 1368.48 5
-v -7.65367 1368.48 695
-v -14.1421 1364.14 695
-v -7.65367 1368.48 5
-v -7.65367 1368.48 5
-v -14.1421 1364.14 695
-v -14.1421 1364.14 5
-v -14.1421 1364.14 695
-v -18.4776 1357.65 695
-v -14.1421 1364.14 5
-v -14.1421 1364.14 5
-v -18.4776 1357.65 695
-v -18.4776 1357.65 5
-v -18.4776 1357.65 695
-v -20 1350 695
-v -18.4776 1357.65 5
-v -18.4776 1357.65 5
-v -20 1350 695
-v -20 1350 5
-v -20 1350 695
-v -18.4776 1342.35 695
-v -20 1350 5
-v -20 1350 5
-v -18.4776 1342.35 695
-v -18.4776 1342.35 5
-v -18.4776 1342.35 695
-v -14.1421 1335.86 695
-v -18.4776 1342.35 5
-v -18.4776 1342.35 5
-v -14.1421 1335.86 695
-v -14.1421 1335.86 5
-v -14.1421 1335.86 695
-v -7.65367 1331.52 695
-v -14.1421 1335.86 5
-v -14.1421 1335.86 5
-v -7.65367 1331.52 695
-v -7.65367 1331.52 5
-v -7.65367 1331.52 695
-v 0 1330 695
-v -7.65367 1331.52 5
-v -7.65367 1331.52 5
-v 0 1330 695
-v 0 1330 5
-v 0 1330 695
-v 7.65367 1331.52 695
-v 0 1330 5
-v 0 1330 5
-v 7.65367 1331.52 695
-v 7.65367 1331.52 5
-v 7.65367 1331.52 695
-v 14.1421 1335.86 695
-v 7.65367 1331.52 5
-v 7.65367 1331.52 5
-v 14.1421 1335.86 695
-v 14.1421 1335.86 5
-v 14.1421 1335.86 695
-v 18.4776 1342.35 695
-v 14.1421 1335.86 5
-v 14.1421 1335.86 5
-v 18.4776 1342.35 695
-v 18.4776 1342.35 5
-v 18.4776 1342.35 695
-v 20 1350 695
-v 18.4776 1342.35 5
-v 18.4776 1342.35 5
-v 20 1350 695
-v 20 1350 5
-v 20 2650 695
-v 18.4776 2657.65 695
-v 20 2650 5
-v 20 2650 5
-v 18.4776 2657.65 695
-v 18.4776 2657.65 5
-v 18.4776 2657.65 695
-v 14.1421 2664.14 695
-v 18.4776 2657.65 5
-v 18.4776 2657.65 5
-v 14.1421 2664.14 695
-v 14.1421 2664.14 5
-v 14.1421 2664.14 695
-v 7.65367 2668.48 695
-v 14.1421 2664.14 5
-v 14.1421 2664.14 5
-v 7.65367 2668.48 695
-v 7.65367 2668.48 5
-v 7.65367 2668.48 695
-v -1e-006 2670 695
-v 7.65367 2668.48 5
-v 7.65367 2668.48 5
-v -1e-006 2670 695
-v -1e-006 2670 5
-v -1e-006 2670 695
-v -7.65367 2668.48 695
-v -1e-006 2670 5
-v -1e-006 2670 5
-v -7.65367 2668.48 695
-v -7.65367 2668.48 5
-v -7.65367 2668.48 695
-v -14.1421 2664.14 695
-v -7.65367 2668.48 5
-v -7.65367 2668.48 5
-v -14.1421 2664.14 695
-v -14.1421 2664.14 5
-v -14.1421 2664.14 695
-v -18.4776 2657.65 695
-v -14.1421 2664.14 5
-v -14.1421 2664.14 5
-v -18.4776 2657.65 695
-v -18.4776 2657.65 5
-v -18.4776 2657.65 695
-v -20 2650 695
-v -18.4776 2657.65 5
-v -18.4776 2657.65 5
-v -20 2650 695
-v -20 2650 5
-v -20 2650 695
-v -18.4776 2642.35 695
-v -20 2650 5
-v -20 2650 5
-v -18.4776 2642.35 695
-v -18.4776 2642.35 5
-v -18.4776 2642.35 695
-v -14.1421 2635.86 695
-v -18.4776 2642.35 5
-v -18.4776 2642.35 5
-v -14.1421 2635.86 695
-v -14.1421 2635.86 5
-v -14.1421 2635.86 695
-v -7.65367 2631.52 695
-v -14.1421 2635.86 5
-v -14.1421 2635.86 5
-v -7.65367 2631.52 695
-v -7.65367 2631.52 5
-v -7.65367 2631.52 695
-v 0 2630 695
-v -7.65367 2631.52 5
-v -7.65367 2631.52 5
-v 0 2630 695
-v 0 2630 5
-v 0 2630 695
-v 7.65367 2631.52 695
-v 0 2630 5
-v 0 2630 5
-v 7.65367 2631.52 695
-v 7.65367 2631.52 5
-v 7.65367 2631.52 695
-v 14.1421 2635.86 695
-v 7.65367 2631.52 5
-v 7.65367 2631.52 5
-v 14.1421 2635.86 695
-v 14.1421 2635.86 5
-v 14.1421 2635.86 695
-v 18.4776 2642.35 695
-v 14.1421 2635.86 5
-v 14.1421 2635.86 5
-v 18.4776 2642.35 695
-v 18.4776 2642.35 5
-v 18.4776 2642.35 695
-v 20 2650 695
-v 18.4776 2642.35 5
-v 18.4776 2642.35 5
-v 20 2650 695
-v 20 2650 5
-v -2880 2650 695
-v -2881.52 2657.65 695
-v -2880 2650 5
-v -2880 2650 5
-v -2881.52 2657.65 695
-v -2881.52 2657.65 5
-v -2881.52 2657.65 695
-v -2885.86 2664.14 695
-v -2881.52 2657.65 5
-v -2881.52 2657.65 5
-v -2885.86 2664.14 695
-v -2885.86 2664.14 5
-v -2885.86 2664.14 695
-v -2892.35 2668.48 695
-v -2885.86 2664.14 5
-v -2885.86 2664.14 5
-v -2892.35 2668.48 695
-v -2892.35 2668.48 5
-v -2892.35 2668.48 695
-v -2900 2670 695
-v -2892.35 2668.48 5
-v -2892.35 2668.48 5
-v -2900 2670 695
-v -2900 2670 5
-v -2900 2670 695
-v -2907.65 2668.48 695
-v -2900 2670 5
-v -2900 2670 5
-v -2907.65 2668.48 695
-v -2907.65 2668.48 5
-v -2907.65 2668.48 695
-v -2914.14 2664.14 695
-v -2907.65 2668.48 5
-v -2907.65 2668.48 5
-v -2914.14 2664.14 695
-v -2914.14 2664.14 5
-v -2914.14 2664.14 695
-v -2918.48 2657.65 695
-v -2914.14 2664.14 5
-v -2914.14 2664.14 5
-v -2918.48 2657.65 695
-v -2918.48 2657.65 5
-v -2918.48 2657.65 695
-v -2920 2650 695
-v -2918.48 2657.65 5
-v -2918.48 2657.65 5
-v -2920 2650 695
-v -2920 2650 5
-v -2920 2650 695
-v -2918.48 2642.35 695
-v -2920 2650 5
-v -2920 2650 5
-v -2918.48 2642.35 695
-v -2918.48 2642.35 5
-v -2918.48 2642.35 695
-v -2914.14 2635.86 695
-v -2918.48 2642.35 5
-v -2918.48 2642.35 5
-v -2914.14 2635.86 695
-v -2914.14 2635.86 5
-v -2914.14 2635.86 695
-v -2907.65 2631.52 695
-v -2914.14 2635.86 5
-v -2914.14 2635.86 5
-v -2907.65 2631.52 695
-v -2907.65 2631.52 5
-v -2907.65 2631.52 695
-v -2900 2630 695
-v -2907.65 2631.52 5
-v -2907.65 2631.52 5
-v -2900 2630 695
-v -2900 2630 5
-v -2900 2630 695
-v -2892.35 2631.52 695
-v -2900 2630 5
-v -2900 2630 5
-v -2892.35 2631.52 695
-v -2892.35 2631.52 5
-v -2892.35 2631.52 695
-v -2885.86 2635.86 695
-v -2892.35 2631.52 5
-v -2892.35 2631.52 5
-v -2885.86 2635.86 695
-v -2885.86 2635.86 5
-v -2885.86 2635.86 695
-v -2881.52 2642.35 695
-v -2885.86 2635.86 5
-v -2885.86 2635.86 5
-v -2881.52 2642.35 695
-v -2881.52 2642.35 5
-v -2881.52 2642.35 695
-v -2880 2650 695
-v -2881.52 2642.35 5
-v -2881.52 2642.35 5
-v -2880 2650 695
-v -2880 2650 5
-v -2900 1250 730
-v -100 1250 730
-v -2900 2750 730
-v -2900 2750 730
-v -100 1250 730
-v -100 2750 730
-v -100 1250 680
-v -2900 1250 680
-v -100 2750 680
-v -100 2750 680
-v -2900 1250 680
-v -2900 2750 680
-v -100 1250 730
-v -100 1250 680
-v -100 2750 730
-v -100 2750 730
-v -100 1250 680
-v -100 2750 680
-v -2900 1250 680
-v -2900 1250 730
-v -2900 2750 680
-v -2900 2750 680
-v -2900 1250 730
-v -2900 2750 730
-v -2900 2750 680
-v -2900 2750 730
-v -100 2750 680
-v -100 2750 680
-v -2900 2750 730
-v -100 2750 730
-v -2900 1250 680
-v -100 1250 680
-v -2900 1250 730
-v -2900 1250 730
-v -100 1250 680
-v -100 1250 730
-v -281.015 2712.06 495
-v -279.138 2715.48 495
-v -281.015 2712.06 5
-v -281.015 2712.06 5
-v -279.138 2715.48 495
-v -279.138 2715.48 5
-v -279.138 2715.48 495
-v -278.713 2719.36 495
-v -279.138 2715.48 5
-v -279.138 2715.48 5
-v -278.713 2719.36 495
-v -278.713 2719.36 5
-v -278.713 2719.36 495
-v -279.805 2723.11 495
-v -278.713 2719.36 5
-v -278.713 2719.36 5
-v -279.805 2723.11 495
-v -279.805 2723.11 5
-v -279.805 2723.11 495
-v -282.247 2726.15 495
-v -279.805 2723.11 5
-v -279.805 2723.11 5
-v -282.247 2726.15 495
-v -282.247 2726.15 5
-v -282.247 2726.15 495
-v -285.668 2728.03 495
-v -282.247 2726.15 5
-v -282.247 2726.15 5
-v -285.668 2728.03 495
-v -285.668 2728.03 5
-v -285.668 2728.03 495
-v -289.547 2728.45 495
-v -285.668 2728.03 5
-v -285.668 2728.03 5
-v -289.547 2728.45 495
-v -289.547 2728.45 5
-v -289.547 2728.45 495
-v -293.293 2727.36 495
-v -289.547 2728.45 5
-v -289.547 2728.45 5
-v -293.293 2727.36 495
-v -293.293 2727.36 5
-v -293.293 2727.36 495
-v -296.336 2724.92 495
-v -293.293 2727.36 5
-v -293.293 2727.36 5
-v -296.336 2724.92 495
-v -296.336 2724.92 5
-v -296.336 2724.92 495
-v -298.212 2721.5 495
-v -296.336 2724.92 5
-v -296.336 2724.92 5
-v -298.212 2721.5 495
-v -298.212 2721.5 5
-v -298.212 2721.5 495
-v -298.637 2717.62 495
-v -298.212 2721.5 5
-v -298.212 2721.5 5
-v -298.637 2717.62 495
-v -298.637 2717.62 5
-v -298.637 2717.62 495
-v -297.545 2713.87 495
-v -298.637 2717.62 5
-v -298.637 2717.62 5
-v -297.545 2713.87 495
-v -297.545 2713.87 5
-v -297.545 2713.87 495
-v -295.103 2710.83 495
-v -297.545 2713.87 5
-v -297.545 2713.87 5
-v -295.103 2710.83 495
-v -295.103 2710.83 5
-v -295.103 2710.83 495
-v -291.682 2708.95 495
-v -295.103 2710.83 5
-v -295.103 2710.83 5
-v -291.682 2708.95 495
-v -291.682 2708.95 5
-v -291.682 2708.95 495
-v -287.804 2708.53 495
-v -291.682 2708.95 5
-v -291.682 2708.95 5
-v -287.804 2708.53 495
-v -287.804 2708.53 5
-v -287.804 2708.53 495
-v -284.058 2709.62 495
-v -287.804 2708.53 5
-v -287.804 2708.53 5
-v -284.058 2709.62 495
-v -284.058 2709.62 5
-v -284.058 2709.62 495
-v -281.015 2712.06 495
-v -284.058 2709.62 5
-v -284.058 2709.62 5
-v -281.015 2712.06 495
-v -281.015 2712.06 5
-v -510.828 2904.9 495
-v -508.951 2908.32 495
-v -510.828 2904.9 5
-v -510.828 2904.9 5
-v -508.951 2908.32 495
-v -508.951 2908.32 5
-v -508.951 2908.32 495
-v -508.527 2912.2 495
-v -508.951 2908.32 5
-v -508.951 2908.32 5
-v -508.527 2912.2 495
-v -508.527 2912.2 5
-v -508.527 2912.2 495
-v -509.618 2915.94 495
-v -508.527 2912.2 5
-v -508.527 2912.2 5
-v -509.618 2915.94 495
-v -509.618 2915.94 5
-v -509.618 2915.94 495
-v -512.061 2918.99 495
-v -509.618 2915.94 5
-v -509.618 2915.94 5
-v -512.061 2918.99 495
-v -512.061 2918.99 5
-v -512.061 2918.99 495
-v -515.481 2920.86 495
-v -512.061 2918.99 5
-v -512.061 2918.99 5
-v -515.481 2920.86 495
-v -515.481 2920.86 5
-v -515.481 2920.86 495
-v -519.36 2921.29 495
-v -515.481 2920.86 5
-v -515.481 2920.86 5
-v -519.36 2921.29 495
-v -519.36 2921.29 5
-v -519.36 2921.29 495
-v -523.106 2920.19 495
-v -519.36 2921.29 5
-v -519.36 2921.29 5
-v -523.106 2920.19 495
-v -523.106 2920.19 5
-v -523.106 2920.19 495
-v -526.149 2917.75 495
-v -523.106 2920.19 5
-v -523.106 2920.19 5
-v -526.149 2917.75 495
-v -526.149 2917.75 5
-v -526.149 2917.75 495
-v -528.026 2914.33 495
-v -526.149 2917.75 5
-v -526.149 2917.75 5
-v -528.026 2914.33 495
-v -528.026 2914.33 5
-v -528.026 2914.33 495
-v -528.451 2910.45 495
-v -528.026 2914.33 5
-v -528.026 2914.33 5
-v -528.451 2910.45 495
-v -528.451 2910.45 5
-v -528.451 2910.45 495
-v -527.359 2906.71 495
-v -528.451 2910.45 5
-v -528.451 2910.45 5
-v -527.359 2906.71 495
-v -527.359 2906.71 5
-v -527.359 2906.71 495
-v -524.916 2903.66 495
-v -527.359 2906.71 5
-v -527.359 2906.71 5
-v -524.916 2903.66 495
-v -524.916 2903.66 5
-v -524.916 2903.66 495
-v -521.496 2901.79 495
-v -524.916 2903.66 5
-v -524.916 2903.66 5
-v -521.496 2901.79 495
-v -521.496 2901.79 5
-v -521.496 2901.79 495
-v -517.617 2901.36 495
-v -521.496 2901.79 5
-v -521.496 2901.79 5
-v -517.617 2901.36 495
-v -517.617 2901.36 5
-v -517.617 2901.36 495
-v -513.871 2902.45 495
-v -517.617 2901.36 5
-v -517.617 2901.36 5
-v -513.871 2902.45 495
-v -513.871 2902.45 5
-v -513.871 2902.45 495
-v -510.828 2904.9 495
-v -513.871 2902.45 5
-v -513.871 2902.45 5
-v -510.828 2904.9 495
-v -510.828 2904.9 5
-v -703.664 2675.08 495
-v -701.788 2678.5 495
-v -703.664 2675.08 5
-v -703.664 2675.08 5
-v -701.788 2678.5 495
-v -701.788 2678.5 5
-v -701.788 2678.5 495
-v -701.363 2682.38 495
-v -701.788 2678.5 5
-v -701.788 2678.5 5
-v -701.363 2682.38 495
-v -701.363 2682.38 5
-v -701.363 2682.38 495
-v -702.455 2686.13 495
-v -701.363 2682.38 5
-v -701.363 2682.38 5
-v -702.455 2686.13 495
-v -702.455 2686.13 5
-v -702.455 2686.13 495
-v -704.897 2689.17 495
-v -702.455 2686.13 5
-v -702.455 2686.13 5
-v -704.897 2689.17 495
-v -704.897 2689.17 5
-v -704.897 2689.17 495
-v -708.318 2691.05 495
-v -704.897 2689.17 5
-v -704.897 2689.17 5
-v -708.318 2691.05 495
-v -708.318 2691.05 5
-v -708.318 2691.05 495
-v -712.196 2691.47 495
-v -708.318 2691.05 5
-v -708.318 2691.05 5
-v -712.196 2691.47 495
-v -712.196 2691.47 5
-v -712.196 2691.47 495
-v -715.942 2690.38 495
-v -712.196 2691.47 5
-v -712.196 2691.47 5
-v -715.942 2690.38 495
-v -715.942 2690.38 5
-v -715.942 2690.38 495
-v -718.985 2687.94 495
-v -715.942 2690.38 5
-v -715.942 2690.38 5
-v -718.985 2687.94 495
-v -718.985 2687.94 5
-v -718.985 2687.94 495
-v -720.862 2684.52 495
-v -718.985 2687.94 5
-v -718.985 2687.94 5
-v -720.862 2684.52 495
-v -720.862 2684.52 5
-v -720.862 2684.52 495
-v -721.287 2680.64 495
-v -720.862 2684.52 5
-v -720.862 2684.52 5
-v -721.287 2680.64 495
-v -721.287 2680.64 5
-v -721.287 2680.64 495
-v -720.195 2676.89 495
-v -721.287 2680.64 5
-v -721.287 2680.64 5
-v -720.195 2676.89 495
-v -720.195 2676.89 5
-v -720.195 2676.89 495
-v -717.753 2673.85 495
-v -720.195 2676.89 5
-v -720.195 2676.89 5
-v -717.753 2673.85 495
-v -717.753 2673.85 5
-v -717.753 2673.85 495
-v -714.332 2671.97 495
-v -717.753 2673.85 5
-v -717.753 2673.85 5
-v -714.332 2671.97 495
-v -714.332 2671.97 5
-v -714.332 2671.97 495
-v -710.453 2671.55 495
-v -714.332 2671.97 5
-v -714.332 2671.97 5
-v -710.453 2671.55 495
-v -710.453 2671.55 5
-v -710.453 2671.55 495
-v -706.707 2672.64 495
-v -710.453 2671.55 5
-v -710.453 2671.55 5
-v -706.707 2672.64 495
-v -706.707 2672.64 5
-v -706.707 2672.64 495
-v -703.664 2675.08 495
-v -706.707 2672.64 5
-v -706.707 2672.64 5
-v -703.664 2675.08 495
-v -703.664 2675.08 5
-v -473.851 2482.25 495
-v -471.974 2485.67 495
-v -473.851 2482.25 5
-v -473.851 2482.25 5
-v -471.974 2485.67 495
-v -471.974 2485.67 5
-v -471.974 2485.67 495
-v -471.55 2489.55 495
-v -471.974 2485.67 5
-v -471.974 2485.67 5
-v -471.55 2489.55 495
-v -471.55 2489.55 5
-v -471.55 2489.55 495
-v -472.641 2493.29 495
-v -471.55 2489.55 5
-v -471.55 2489.55 5
-v -472.641 2493.29 495
-v -472.641 2493.29 5
-v -472.641 2493.29 495
-v -475.084 2496.34 495
-v -472.641 2493.29 5
-v -472.641 2493.29 5
-v -475.084 2496.34 495
-v -475.084 2496.34 5
-v -475.084 2496.34 495
-v -478.504 2498.21 495
-v -475.084 2496.34 5
-v -475.084 2496.34 5
-v -478.504 2498.21 495
-v -478.504 2498.21 5
-v -478.504 2498.21 495
-v -482.383 2498.64 495
-v -478.504 2498.21 5
-v -478.504 2498.21 5
-v -482.383 2498.64 495
-v -482.383 2498.64 5
-v -482.383 2498.64 495
-v -486.129 2497.55 495
-v -482.383 2498.64 5
-v -482.383 2498.64 5
-v -486.129 2497.55 495
-v -486.129 2497.55 5
-v -486.129 2497.55 495
-v -489.172 2495.1 495
-v -486.129 2497.55 5
-v -486.129 2497.55 5
-v -489.172 2495.1 495
-v -489.172 2495.1 5
-v -489.172 2495.1 495
-v -491.049 2491.68 495
-v -489.172 2495.1 5
-v -489.172 2495.1 5
-v -491.049 2491.68 495
-v -491.049 2491.68 5
-v -491.049 2491.68 495
-v -491.473 2487.8 495
-v -491.049 2491.68 5
-v -491.049 2491.68 5
-v -491.473 2487.8 495
-v -491.473 2487.8 5
-v -491.473 2487.8 495
-v -490.382 2484.06 495
-v -491.473 2487.8 5
-v -491.473 2487.8 5
-v -490.382 2484.06 495
-v -490.382 2484.06 5
-v -490.382 2484.06 495
-v -487.939 2481.01 495
-v -490.382 2484.06 5
-v -490.382 2484.06 5
-v -487.939 2481.01 495
-v -487.939 2481.01 5
-v -487.939 2481.01 495
-v -484.519 2479.14 495
-v -487.939 2481.01 5
-v -487.939 2481.01 5
-v -484.519 2479.14 495
-v -484.519 2479.14 5
-v -484.519 2479.14 495
-v -480.64 2478.71 495
-v -484.519 2479.14 5
-v -484.519 2479.14 5
-v -480.64 2478.71 495
-v -480.64 2478.71 5
-v -480.64 2478.71 495
-v -476.894 2479.81 495
-v -480.64 2478.71 5
-v -480.64 2478.71 5
-v -476.894 2479.81 495
-v -476.894 2479.81 5
-v -476.894 2479.81 495
-v -473.851 2482.25 495
-v -476.894 2479.81 5
-v -476.894 2479.81 5
-v -473.851 2482.25 495
-v -473.851 2482.25 5
-v -781.766 2675.35 510
-v -475.349 2418.23 510
-v -524.651 2981.77 510
-v -524.651 2981.77 510
-v -475.349 2418.23 510
-v -218.234 2724.65 510
-v -475.349 2418.23 500
-v -781.766 2675.35 500
-v -218.234 2724.65 500
-v -218.234 2724.65 500
-v -781.766 2675.35 500
-v -524.651 2981.77 500
-v -475.349 2418.23 510
-v -475.349 2418.23 500
-v -218.234 2724.65 510
-v -218.234 2724.65 510
-v -475.349 2418.23 500
-v -218.234 2724.65 500
-v -781.766 2675.35 500
-v -781.766 2675.35 510
-v -524.651 2981.77 500
-v -524.651 2981.77 500
-v -781.766 2675.35 510
-v -524.651 2981.77 510
-v -524.651 2981.77 500
-v -524.651 2981.77 510
-v -218.234 2724.65 500
-v -218.234 2724.65 500
-v -524.651 2981.77 510
-v -218.234 2724.65 510
-v -781.766 2675.35 500
-v -475.349 2418.23 500
-v -781.766 2675.35 510
-v -781.766 2675.35 510
-v -475.349 2418.23 500
-v -475.349 2418.23 510
-v -322.266 2818.47 810
-v -320.389 2821.89 810
-v -322.266 2818.47 510
-v -322.266 2818.47 510
-v -320.389 2821.89 810
-v -320.389 2821.89 510
-v -320.389 2821.89 810
-v -319.964 2825.77 810
-v -320.389 2821.89 510
-v -320.389 2821.89 510
-v -319.964 2825.77 810
-v -319.964 2825.77 510
-v -319.964 2825.77 810
-v -321.056 2829.52 810
-v -319.964 2825.77 510
-v -319.964 2825.77 510
-v -321.056 2829.52 810
-v -321.056 2829.52 510
-v -321.056 2829.52 810
-v -323.498 2832.56 810
-v -321.056 2829.52 510
-v -321.056 2829.52 510
-v -323.498 2832.56 810
-v -323.498 2832.56 510
-v -323.498 2832.56 810
-v -326.919 2834.44 810
-v -323.498 2832.56 510
-v -323.498 2832.56 510
-v -326.919 2834.44 810
-v -326.919 2834.44 510
-v -326.919 2834.44 810
-v -330.798 2834.86 810
-v -326.919 2834.44 510
-v -326.919 2834.44 510
-v -330.798 2834.86 810
-v -330.798 2834.86 510
-v -330.798 2834.86 810
-v -334.544 2833.77 810
-v -330.798 2834.86 510
-v -330.798 2834.86 510
-v -334.544 2833.77 810
-v -334.544 2833.77 510
-v -334.544 2833.77 810
-v -337.587 2831.33 810
-v -334.544 2833.77 510
-v -334.544 2833.77 510
-v -337.587 2831.33 810
-v -337.587 2831.33 510
-v -337.587 2831.33 810
-v -339.464 2827.91 810
-v -337.587 2831.33 510
-v -337.587 2831.33 510
-v -339.464 2827.91 810
-v -339.464 2827.91 510
-v -339.464 2827.91 810
-v -339.888 2824.03 810
-v -339.464 2827.91 510
-v -339.464 2827.91 510
-v -339.888 2824.03 810
-v -339.888 2824.03 510
-v -339.888 2824.03 810
-v -338.796 2820.28 810
-v -339.888 2824.03 510
-v -339.888 2824.03 510
-v -338.796 2820.28 810
-v -338.796 2820.28 510
-v -338.796 2820.28 810
-v -336.354 2817.24 810
-v -338.796 2820.28 510
-v -338.796 2820.28 510
-v -336.354 2817.24 810
-v -336.354 2817.24 510
-v -336.354 2817.24 810
-v -332.933 2815.36 810
-v -336.354 2817.24 510
-v -336.354 2817.24 510
-v -332.933 2815.36 810
-v -332.933 2815.36 510
-v -332.933 2815.36 810
-v -329.055 2814.94 810
-v -332.933 2815.36 510
-v -332.933 2815.36 510
-v -329.055 2814.94 810
-v -329.055 2814.94 510
-v -329.055 2814.94 810
-v -325.309 2816.03 810
-v -329.055 2814.94 510
-v -329.055 2814.94 510
-v -325.309 2816.03 810
-v -325.309 2816.03 510
-v -325.309 2816.03 810
-v -322.266 2818.47 810
-v -325.309 2816.03 510
-v -325.309 2816.03 510
-v -322.266 2818.47 810
-v -322.266 2818.47 510
-v -398.87 2882.75 810
-v -396.994 2886.17 810
-v -398.87 2882.75 510
-v -398.87 2882.75 510
-v -396.994 2886.17 810
-v -396.994 2886.17 510
-v -396.994 2886.17 810
-v -396.569 2890.05 810
-v -396.994 2886.17 510
-v -396.994 2886.17 510
-v -396.569 2890.05 810
-v -396.569 2890.05 510
-v -396.569 2890.05 810
-v -397.661 2893.8 810
-v -396.569 2890.05 510
-v -396.569 2890.05 510
-v -397.661 2893.8 810
-v -397.661 2893.8 510
-v -397.661 2893.8 810
-v -400.103 2896.84 810
-v -397.661 2893.8 510
-v -397.661 2893.8 510
-v -400.103 2896.84 810
-v -400.103 2896.84 510
-v -400.103 2896.84 810
-v -403.524 2898.72 810
-v -400.103 2896.84 510
-v -400.103 2896.84 510
-v -403.524 2898.72 810
-v -403.524 2898.72 510
-v -403.524 2898.72 810
-v -407.402 2899.14 810
-v -403.524 2898.72 510
-v -403.524 2898.72 510
-v -407.402 2899.14 810
-v -407.402 2899.14 510
-v -407.402 2899.14 810
-v -411.148 2898.05 810
-v -407.402 2899.14 510
-v -407.402 2899.14 510
-v -411.148 2898.05 810
-v -411.148 2898.05 510
-v -411.148 2898.05 810
-v -414.191 2895.61 810
-v -411.148 2898.05 510
-v -411.148 2898.05 510
-v -414.191 2895.61 810
-v -414.191 2895.61 510
-v -414.191 2895.61 810
-v -416.068 2892.19 810
-v -414.191 2895.61 510
-v -414.191 2895.61 510
-v -416.068 2892.19 810
-v -416.068 2892.19 510
-v -416.068 2892.19 810
-v -416.493 2888.31 810
-v -416.068 2892.19 510
-v -416.068 2892.19 510
-v -416.493 2888.31 810
-v -416.493 2888.31 510
-v -416.493 2888.31 810
-v -415.401 2884.56 810
-v -416.493 2888.31 510
-v -416.493 2888.31 510
-v -415.401 2884.56 810
-v -415.401 2884.56 510
-v -415.401 2884.56 810
-v -412.959 2881.52 810
-v -415.401 2884.56 510
-v -415.401 2884.56 510
-v -412.959 2881.52 810
-v -412.959 2881.52 510
-v -412.959 2881.52 810
-v -409.538 2879.64 810
-v -412.959 2881.52 510
-v -412.959 2881.52 510
-v -409.538 2879.64 810
-v -409.538 2879.64 510
-v -409.538 2879.64 810
-v -405.659 2879.22 810
-v -409.538 2879.64 510
-v -409.538 2879.64 510
-v -405.659 2879.22 810
-v -405.659 2879.22 510
-v -405.659 2879.22 810
-v -401.913 2880.31 810
-v -405.659 2879.22 510
-v -405.659 2879.22 510
-v -401.913 2880.31 810
-v -401.913 2880.31 510
-v -401.913 2880.31 810
-v -398.87 2882.75 810
-v -401.913 2880.31 510
-v -401.913 2880.31 510
-v -398.87 2882.75 810
-v -398.87 2882.75 510
-v -531.079 2974.11 910
-v -224.661 2716.99 910
-v -524.651 2981.77 910
-v -524.651 2981.77 910
-v -224.661 2716.99 910
-v -218.234 2724.65 910
-v -224.661 2716.99 710
-v -531.079 2974.11 710
-v -218.234 2724.65 710
-v -218.234 2724.65 710
-v -531.079 2974.11 710
-v -524.651 2981.77 710
-v -224.661 2716.99 910
-v -224.661 2716.99 710
-v -218.234 2724.65 910
-v -218.234 2724.65 910
-v -224.661 2716.99 710
-v -218.234 2724.65 710
-v -531.079 2974.11 710
-v -531.079 2974.11 910
-v -524.651 2981.77 710
-v -524.651 2981.77 710
-v -531.079 2974.11 910
-v -524.651 2981.77 910
-v -524.651 2981.77 710
-v -524.651 2981.77 910
-v -218.234 2724.65 710
-v -218.234 2724.65 710
-v -524.651 2981.77 910
-v -218.234 2724.65 910
-v -531.079 2974.11 710
-v -224.661 2716.99 710
-v -531.079 2974.11 910
-v -531.079 2974.11 910
-v -224.661 2716.99 710
-v -224.661 2716.99 910
-v -1281.01 2712.06 495
-v -1279.14 2715.48 495
-v -1281.01 2712.06 5
-v -1281.01 2712.06 5
-v -1279.14 2715.48 495
-v -1279.14 2715.48 5
-v -1279.14 2715.48 495
-v -1278.71 2719.36 495
-v -1279.14 2715.48 5
-v -1279.14 2715.48 5
-v -1278.71 2719.36 495
-v -1278.71 2719.36 5
-v -1278.71 2719.36 495
-v -1279.81 2723.11 495
-v -1278.71 2719.36 5
-v -1278.71 2719.36 5
-v -1279.81 2723.11 495
-v -1279.81 2723.11 5
-v -1279.81 2723.11 495
-v -1282.25 2726.15 495
-v -1279.81 2723.11 5
-v -1279.81 2723.11 5
-v -1282.25 2726.15 495
-v -1282.25 2726.15 5
-v -1282.25 2726.15 495
-v -1285.67 2728.03 495
-v -1282.25 2726.15 5
-v -1282.25 2726.15 5
-v -1285.67 2728.03 495
-v -1285.67 2728.03 5
-v -1285.67 2728.03 495
-v -1289.55 2728.45 495
-v -1285.67 2728.03 5
-v -1285.67 2728.03 5
-v -1289.55 2728.45 495
-v -1289.55 2728.45 5
-v -1289.55 2728.45 495
-v -1293.29 2727.36 495
-v -1289.55 2728.45 5
-v -1289.55 2728.45 5
-v -1293.29 2727.36 495
-v -1293.29 2727.36 5
-v -1293.29 2727.36 495
-v -1296.34 2724.92 495
-v -1293.29 2727.36 5
-v -1293.29 2727.36 5
-v -1296.34 2724.92 495
-v -1296.34 2724.92 5
-v -1296.34 2724.92 495
-v -1298.21 2721.5 495
-v -1296.34 2724.92 5
-v -1296.34 2724.92 5
-v -1298.21 2721.5 495
-v -1298.21 2721.5 5
-v -1298.21 2721.5 495
-v -1298.64 2717.62 495
-v -1298.21 2721.5 5
-v -1298.21 2721.5 5
-v -1298.64 2717.62 495
-v -1298.64 2717.62 5
-v -1298.64 2717.62 495
-v -1297.55 2713.87 495
-v -1298.64 2717.62 5
-v -1298.64 2717.62 5
-v -1297.55 2713.87 495
-v -1297.55 2713.87 5
-v -1297.55 2713.87 495
-v -1295.1 2710.83 495
-v -1297.55 2713.87 5
-v -1297.55 2713.87 5
-v -1295.1 2710.83 495
-v -1295.1 2710.83 5
-v -1295.1 2710.83 495
-v -1291.68 2708.95 495
-v -1295.1 2710.83 5
-v -1295.1 2710.83 5
-v -1291.68 2708.95 495
-v -1291.68 2708.95 5
-v -1291.68 2708.95 495
-v -1287.8 2708.53 495
-v -1291.68 2708.95 5
-v -1291.68 2708.95 5
-v -1287.8 2708.53 495
-v -1287.8 2708.53 5
-v -1287.8 2708.53 495
-v -1284.06 2709.62 495
-v -1287.8 2708.53 5
-v -1287.8 2708.53 5
-v -1284.06 2709.62 495
-v -1284.06 2709.62 5
-v -1284.06 2709.62 495
-v -1281.01 2712.06 495
-v -1284.06 2709.62 5
-v -1284.06 2709.62 5
-v -1281.01 2712.06 495
-v -1281.01 2712.06 5
-v -1510.83 2904.9 495
-v -1508.95 2908.32 495
-v -1510.83 2904.9 5
-v -1510.83 2904.9 5
-v -1508.95 2908.32 495
-v -1508.95 2908.32 5
-v -1508.95 2908.32 495
-v -1508.53 2912.2 495
-v -1508.95 2908.32 5
-v -1508.95 2908.32 5
-v -1508.53 2912.2 495
-v -1508.53 2912.2 5
-v -1508.53 2912.2 495
-v -1509.62 2915.94 495
-v -1508.53 2912.2 5
-v -1508.53 2912.2 5
-v -1509.62 2915.94 495
-v -1509.62 2915.94 5
-v -1509.62 2915.94 495
-v -1512.06 2918.99 495
-v -1509.62 2915.94 5
-v -1509.62 2915.94 5
-v -1512.06 2918.99 495
-v -1512.06 2918.99 5
-v -1512.06 2918.99 495
-v -1515.48 2920.86 495
-v -1512.06 2918.99 5
-v -1512.06 2918.99 5
-v -1515.48 2920.86 495
-v -1515.48 2920.86 5
-v -1515.48 2920.86 495
-v -1519.36 2921.29 495
-v -1515.48 2920.86 5
-v -1515.48 2920.86 5
-v -1519.36 2921.29 495
-v -1519.36 2921.29 5
-v -1519.36 2921.29 495
-v -1523.11 2920.19 495
-v -1519.36 2921.29 5
-v -1519.36 2921.29 5
-v -1523.11 2920.19 495
-v -1523.11 2920.19 5
-v -1523.11 2920.19 495
-v -1526.15 2917.75 495
-v -1523.11 2920.19 5
-v -1523.11 2920.19 5
-v -1526.15 2917.75 495
-v -1526.15 2917.75 5
-v -1526.15 2917.75 495
-v -1528.03 2914.33 495
-v -1526.15 2917.75 5
-v -1526.15 2917.75 5
-v -1528.03 2914.33 495
-v -1528.03 2914.33 5
-v -1528.03 2914.33 495
-v -1528.45 2910.45 495
-v -1528.03 2914.33 5
-v -1528.03 2914.33 5
-v -1528.45 2910.45 495
-v -1528.45 2910.45 5
-v -1528.45 2910.45 495
-v -1527.36 2906.71 495
-v -1528.45 2910.45 5
-v -1528.45 2910.45 5
-v -1527.36 2906.71 495
-v -1527.36 2906.71 5
-v -1527.36 2906.71 495
-v -1524.92 2903.66 495
-v -1527.36 2906.71 5
-v -1527.36 2906.71 5
-v -1524.92 2903.66 495
-v -1524.92 2903.66 5
-v -1524.92 2903.66 495
-v -1521.5 2901.79 495
-v -1524.92 2903.66 5
-v -1524.92 2903.66 5
-v -1521.5 2901.79 495
-v -1521.5 2901.79 5
-v -1521.5 2901.79 495
-v -1517.62 2901.36 495
-v -1521.5 2901.79 5
-v -1521.5 2901.79 5
-v -1517.62 2901.36 495
-v -1517.62 2901.36 5
-v -1517.62 2901.36 495
-v -1513.87 2902.45 495
-v -1517.62 2901.36 5
-v -1517.62 2901.36 5
-v -1513.87 2902.45 495
-v -1513.87 2902.45 5
-v -1513.87 2902.45 495
-v -1510.83 2904.9 495
-v -1513.87 2902.45 5
-v -1513.87 2902.45 5
-v -1510.83 2904.9 495
-v -1510.83 2904.9 5
-v -1703.66 2675.08 495
-v -1701.79 2678.5 495
-v -1703.66 2675.08 5
-v -1703.66 2675.08 5
-v -1701.79 2678.5 495
-v -1701.79 2678.5 5
-v -1701.79 2678.5 495
-v -1701.36 2682.38 495
-v -1701.79 2678.5 5
-v -1701.79 2678.5 5
-v -1701.36 2682.38 495
-v -1701.36 2682.38 5
-v -1701.36 2682.38 495
-v -1702.45 2686.13 495
-v -1701.36 2682.38 5
-v -1701.36 2682.38 5
-v -1702.45 2686.13 495
-v -1702.45 2686.13 5
-v -1702.45 2686.13 495
-v -1704.9 2689.17 495
-v -1702.45 2686.13 5
-v -1702.45 2686.13 5
-v -1704.9 2689.17 495
-v -1704.9 2689.17 5
-v -1704.9 2689.17 495
-v -1708.32 2691.05 495
-v -1704.9 2689.17 5
-v -1704.9 2689.17 5
-v -1708.32 2691.05 495
-v -1708.32 2691.05 5
-v -1708.32 2691.05 495
-v -1712.2 2691.47 495
-v -1708.32 2691.05 5
-v -1708.32 2691.05 5
-v -1712.2 2691.47 495
-v -1712.2 2691.47 5
-v -1712.2 2691.47 495
-v -1715.94 2690.38 495
-v -1712.2 2691.47 5
-v -1712.2 2691.47 5
-v -1715.94 2690.38 495
-v -1715.94 2690.38 5
-v -1715.94 2690.38 495
-v -1718.99 2687.94 495
-v -1715.94 2690.38 5
-v -1715.94 2690.38 5
-v -1718.99 2687.94 495
-v -1718.99 2687.94 5
-v -1718.99 2687.94 495
-v -1720.86 2684.52 495
-v -1718.99 2687.94 5
-v -1718.99 2687.94 5
-v -1720.86 2684.52 495
-v -1720.86 2684.52 5
-v -1720.86 2684.52 495
-v -1721.29 2680.64 495
-v -1720.86 2684.52 5
-v -1720.86 2684.52 5
-v -1721.29 2680.64 495
-v -1721.29 2680.64 5
-v -1721.29 2680.64 495
-v -1720.19 2676.89 495
-v -1721.29 2680.64 5
-v -1721.29 2680.64 5
-v -1720.19 2676.89 495
-v -1720.19 2676.89 5
-v -1720.19 2676.89 495
-v -1717.75 2673.85 495
-v -1720.19 2676.89 5
-v -1720.19 2676.89 5
-v -1717.75 2673.85 495
-v -1717.75 2673.85 5
-v -1717.75 2673.85 495
-v -1714.33 2671.97 495
-v -1717.75 2673.85 5
-v -1717.75 2673.85 5
-v -1714.33 2671.97 495
-v -1714.33 2671.97 5
-v -1714.33 2671.97 495
-v -1710.45 2671.55 495
-v -1714.33 2671.97 5
-v -1714.33 2671.97 5
-v -1710.45 2671.55 495
-v -1710.45 2671.55 5
-v -1710.45 2671.55 495
-v -1706.71 2672.64 495
-v -1710.45 2671.55 5
-v -1710.45 2671.55 5
-v -1706.71 2672.64 495
-v -1706.71 2672.64 5
-v -1706.71 2672.64 495
-v -1703.66 2675.08 495
-v -1706.71 2672.64 5
-v -1706.71 2672.64 5
-v -1703.66 2675.08 495
-v -1703.66 2675.08 5
-v -1473.85 2482.25 495
-v -1471.97 2485.67 495
-v -1473.85 2482.25 5
-v -1473.85 2482.25 5
-v -1471.97 2485.67 495
-v -1471.97 2485.67 5
-v -1471.97 2485.67 495
-v -1471.55 2489.55 495
-v -1471.97 2485.67 5
-v -1471.97 2485.67 5
-v -1471.55 2489.55 495
-v -1471.55 2489.55 5
-v -1471.55 2489.55 495
-v -1472.64 2493.29 495
-v -1471.55 2489.55 5
-v -1471.55 2489.55 5
-v -1472.64 2493.29 495
-v -1472.64 2493.29 5
-v -1472.64 2493.29 495
-v -1475.08 2496.34 495
-v -1472.64 2493.29 5
-v -1472.64 2493.29 5
-v -1475.08 2496.34 495
-v -1475.08 2496.34 5
-v -1475.08 2496.34 495
-v -1478.5 2498.21 495
-v -1475.08 2496.34 5
-v -1475.08 2496.34 5
-v -1478.5 2498.21 495
-v -1478.5 2498.21 5
-v -1478.5 2498.21 495
-v -1482.38 2498.64 495
-v -1478.5 2498.21 5
-v -1478.5 2498.21 5
-v -1482.38 2498.64 495
-v -1482.38 2498.64 5
-v -1482.38 2498.64 495
-v -1486.13 2497.55 495
-v -1482.38 2498.64 5
-v -1482.38 2498.64 5
-v -1486.13 2497.55 495
-v -1486.13 2497.55 5
-v -1486.13 2497.55 495
-v -1489.17 2495.1 495
-v -1486.13 2497.55 5
-v -1486.13 2497.55 5
-v -1489.17 2495.1 495
-v -1489.17 2495.1 5
-v -1489.17 2495.1 495
-v -1491.05 2491.68 495
-v -1489.17 2495.1 5
-v -1489.17 2495.1 5
-v -1491.05 2491.68 495
-v -1491.05 2491.68 5
-v -1491.05 2491.68 495
-v -1491.47 2487.8 495
-v -1491.05 2491.68 5
-v -1491.05 2491.68 5
-v -1491.47 2487.8 495
-v -1491.47 2487.8 5
-v -1491.47 2487.8 495
-v -1490.38 2484.06 495
-v -1491.47 2487.8 5
-v -1491.47 2487.8 5
-v -1490.38 2484.06 495
-v -1490.38 2484.06 5
-v -1490.38 2484.06 495
-v -1487.94 2481.01 495
-v -1490.38 2484.06 5
-v -1490.38 2484.06 5
-v -1487.94 2481.01 495
-v -1487.94 2481.01 5
-v -1487.94 2481.01 495
-v -1484.52 2479.14 495
-v -1487.94 2481.01 5
-v -1487.94 2481.01 5
-v -1484.52 2479.14 495
-v -1484.52 2479.14 5
-v -1484.52 2479.14 495
-v -1480.64 2478.71 495
-v -1484.52 2479.14 5
-v -1484.52 2479.14 5
-v -1480.64 2478.71 495
-v -1480.64 2478.71 5
-v -1480.64 2478.71 495
-v -1476.89 2479.81 495
-v -1480.64 2478.71 5
-v -1480.64 2478.71 5
-v -1476.89 2479.81 495
-v -1476.89 2479.81 5
-v -1476.89 2479.81 495
-v -1473.85 2482.25 495
-v -1476.89 2479.81 5
-v -1476.89 2479.81 5
-v -1473.85 2482.25 495
-v -1473.85 2482.25 5
-v -1781.77 2675.35 510
-v -1475.35 2418.23 510
-v -1524.65 2981.77 510
-v -1524.65 2981.77 510
-v -1475.35 2418.23 510
-v -1218.23 2724.65 510
-v -1475.35 2418.23 500
-v -1781.77 2675.35 500
-v -1218.23 2724.65 500
-v -1218.23 2724.65 500
-v -1781.77 2675.35 500
-v -1524.65 2981.77 500
-v -1475.35 2418.23 510
-v -1475.35 2418.23 500
-v -1218.23 2724.65 510
-v -1218.23 2724.65 510
-v -1475.35 2418.23 500
-v -1218.23 2724.65 500
-v -1781.77 2675.35 500
-v -1781.77 2675.35 510
-v -1524.65 2981.77 500
-v -1524.65 2981.77 500
-v -1781.77 2675.35 510
-v -1524.65 2981.77 510
-v -1524.65 2981.77 500
-v -1524.65 2981.77 510
-v -1218.23 2724.65 500
-v -1218.23 2724.65 500
-v -1524.65 2981.77 510
-v -1218.23 2724.65 510
-v -1781.77 2675.35 500
-v -1475.35 2418.23 500
-v -1781.77 2675.35 510
-v -1781.77 2675.35 510
-v -1475.35 2418.23 500
-v -1475.35 2418.23 510
-v -1322.27 2818.47 810
-v -1320.39 2821.89 810
-v -1322.27 2818.47 510
-v -1322.27 2818.47 510
-v -1320.39 2821.89 810
-v -1320.39 2821.89 510
-v -1320.39 2821.89 810
-v -1319.96 2825.77 810
-v -1320.39 2821.89 510
-v -1320.39 2821.89 510
-v -1319.96 2825.77 810
-v -1319.96 2825.77 510
-v -1319.96 2825.77 810
-v -1321.06 2829.52 810
-v -1319.96 2825.77 510
-v -1319.96 2825.77 510
-v -1321.06 2829.52 810
-v -1321.06 2829.52 510
-v -1321.06 2829.52 810
-v -1323.5 2832.56 810
-v -1321.06 2829.52 510
-v -1321.06 2829.52 510
-v -1323.5 2832.56 810
-v -1323.5 2832.56 510
-v -1323.5 2832.56 810
-v -1326.92 2834.44 810
-v -1323.5 2832.56 510
-v -1323.5 2832.56 510
-v -1326.92 2834.44 810
-v -1326.92 2834.44 510
-v -1326.92 2834.44 810
-v -1330.8 2834.86 810
-v -1326.92 2834.44 510
-v -1326.92 2834.44 510
-v -1330.8 2834.86 810
-v -1330.8 2834.86 510
-v -1330.8 2834.86 810
-v -1334.54 2833.77 810
-v -1330.8 2834.86 510
-v -1330.8 2834.86 510
-v -1334.54 2833.77 810
-v -1334.54 2833.77 510
-v -1334.54 2833.77 810
-v -1337.59 2831.33 810
-v -1334.54 2833.77 510
-v -1334.54 2833.77 510
-v -1337.59 2831.33 810
-v -1337.59 2831.33 510
-v -1337.59 2831.33 810
-v -1339.46 2827.91 810
-v -1337.59 2831.33 510
-v -1337.59 2831.33 510
-v -1339.46 2827.91 810
-v -1339.46 2827.91 510
-v -1339.46 2827.91 810
-v -1339.89 2824.03 810
-v -1339.46 2827.91 510
-v -1339.46 2827.91 510
-v -1339.89 2824.03 810
-v -1339.89 2824.03 510
-v -1339.89 2824.03 810
-v -1338.8 2820.28 810
-v -1339.89 2824.03 510
-v -1339.89 2824.03 510
-v -1338.8 2820.28 810
-v -1338.8 2820.28 510
-v -1338.8 2820.28 810
-v -1336.35 2817.24 810
-v -1338.8 2820.28 510
-v -1338.8 2820.28 510
-v -1336.35 2817.24 810
-v -1336.35 2817.24 510
-v -1336.35 2817.24 810
-v -1332.93 2815.36 810
-v -1336.35 2817.24 510
-v -1336.35 2817.24 510
-v -1332.93 2815.36 810
-v -1332.93 2815.36 510
-v -1332.93 2815.36 810
-v -1329.05 2814.94 810
-v -1332.93 2815.36 510
-v -1332.93 2815.36 510
-v -1329.05 2814.94 810
-v -1329.05 2814.94 510
-v -1329.05 2814.94 810
-v -1325.31 2816.03 810
-v -1329.05 2814.94 510
-v -1329.05 2814.94 510
-v -1325.31 2816.03 810
-v -1325.31 2816.03 510
-v -1325.31 2816.03 810
-v -1322.27 2818.47 810
-v -1325.31 2816.03 510
-v -1325.31 2816.03 510
-v -1322.27 2818.47 810
-v -1322.27 2818.47 510
-v -1398.87 2882.75 810
-v -1396.99 2886.17 810
-v -1398.87 2882.75 510
-v -1398.87 2882.75 510
-v -1396.99 2886.17 810
-v -1396.99 2886.17 510
-v -1396.99 2886.17 810
-v -1396.57 2890.05 810
-v -1396.99 2886.17 510
-v -1396.99 2886.17 510
-v -1396.57 2890.05 810
-v -1396.57 2890.05 510
-v -1396.57 2890.05 810
-v -1397.66 2893.8 810
-v -1396.57 2890.05 510
-v -1396.57 2890.05 510
-v -1397.66 2893.8 810
-v -1397.66 2893.8 510
-v -1397.66 2893.8 810
-v -1400.1 2896.84 810
-v -1397.66 2893.8 510
-v -1397.66 2893.8 510
-v -1400.1 2896.84 810
-v -1400.1 2896.84 510
-v -1400.1 2896.84 810
-v -1403.52 2898.72 810
-v -1400.1 2896.84 510
-v -1400.1 2896.84 510
-v -1403.52 2898.72 810
-v -1403.52 2898.72 510
-v -1403.52 2898.72 810
-v -1407.4 2899.14 810
-v -1403.52 2898.72 510
-v -1403.52 2898.72 510
-v -1407.4 2899.14 810
-v -1407.4 2899.14 510
-v -1407.4 2899.14 810
-v -1411.15 2898.05 810
-v -1407.4 2899.14 510
-v -1407.4 2899.14 510
-v -1411.15 2898.05 810
-v -1411.15 2898.05 510
-v -1411.15 2898.05 810
-v -1414.19 2895.61 810
-v -1411.15 2898.05 510
-v -1411.15 2898.05 510
-v -1414.19 2895.61 810
-v -1414.19 2895.61 510
-v -1414.19 2895.61 810
-v -1416.07 2892.19 810
-v -1414.19 2895.61 510
-v -1414.19 2895.61 510
-v -1416.07 2892.19 810
-v -1416.07 2892.19 510
-v -1416.07 2892.19 810
-v -1416.49 2888.31 810
-v -1416.07 2892.19 510
-v -1416.07 2892.19 510
-v -1416.49 2888.31 810
-v -1416.49 2888.31 510
-v -1416.49 2888.31 810
-v -1415.4 2884.56 810
-v -1416.49 2888.31 510
-v -1416.49 2888.31 510
-v -1415.4 2884.56 810
-v -1415.4 2884.56 510
-v -1415.4 2884.56 810
-v -1412.96 2881.52 810
-v -1415.4 2884.56 510
-v -1415.4 2884.56 510
-v -1412.96 2881.52 810
-v -1412.96 2881.52 510
-v -1412.96 2881.52 810
-v -1409.54 2879.64 810
-v -1412.96 2881.52 510
-v -1412.96 2881.52 510
-v -1409.54 2879.64 810
-v -1409.54 2879.64 510
-v -1409.54 2879.64 810
-v -1405.66 2879.22 810
-v -1409.54 2879.64 510
-v -1409.54 2879.64 510
-v -1405.66 2879.22 810
-v -1405.66 2879.22 510
-v -1405.66 2879.22 810
-v -1401.91 2880.31 810
-v -1405.66 2879.22 510
-v -1405.66 2879.22 510
-v -1401.91 2880.31 810
-v -1401.91 2880.31 510
-v -1401.91 2880.31 810
-v -1398.87 2882.75 810
-v -1401.91 2880.31 510
-v -1401.91 2880.31 510
-v -1398.87 2882.75 810
-v -1398.87 2882.75 510
-v -1531.08 2974.11 910
-v -1224.66 2716.99 910
-v -1524.65 2981.77 910
-v -1524.65 2981.77 910
-v -1224.66 2716.99 910
-v -1218.23 2724.65 910
-v -1224.66 2716.99 710
-v -1531.08 2974.11 710
-v -1218.23 2724.65 710
-v -1218.23 2724.65 710
-v -1531.08 2974.11 710
-v -1524.65 2981.77 710
-v -1224.66 2716.99 910
-v -1224.66 2716.99 710
-v -1218.23 2724.65 910
-v -1218.23 2724.65 910
-v -1224.66 2716.99 710
-v -1218.23 2724.65 710
-v -1531.08 2974.11 710
-v -1531.08 2974.11 910
-v -1524.65 2981.77 710
-v -1524.65 2981.77 710
-v -1531.08 2974.11 910
-v -1524.65 2981.77 910
-v -1524.65 2981.77 710
-v -1524.65 2981.77 910
-v -1218.23 2724.65 710
-v -1218.23 2724.65 710
-v -1524.65 2981.77 910
-v -1218.23 2724.65 910
-v -1531.08 2974.11 710
-v -1224.66 2716.99 710
-v -1531.08 2974.11 910
-v -1531.08 2974.11 910
-v -1224.66 2716.99 710
-v -1224.66 2716.99 910
-v -2281.01 2712.06 495
-v -2279.14 2715.48 495
-v -2281.01 2712.06 5
-v -2281.01 2712.06 5
-v -2279.14 2715.48 495
-v -2279.14 2715.48 5
-v -2279.14 2715.48 495
-v -2278.71 2719.36 495
-v -2279.14 2715.48 5
-v -2279.14 2715.48 5
-v -2278.71 2719.36 495
-v -2278.71 2719.36 5
-v -2278.71 2719.36 495
-v -2279.81 2723.11 495
-v -2278.71 2719.36 5
-v -2278.71 2719.36 5
-v -2279.81 2723.11 495
-v -2279.81 2723.11 5
-v -2279.81 2723.11 495
-v -2282.25 2726.15 495
-v -2279.81 2723.11 5
-v -2279.81 2723.11 5
-v -2282.25 2726.15 495
-v -2282.25 2726.15 5
-v -2282.25 2726.15 495
-v -2285.67 2728.03 495
-v -2282.25 2726.15 5
-v -2282.25 2726.15 5
-v -2285.67 2728.03 495
-v -2285.67 2728.03 5
-v -2285.67 2728.03 495
-v -2289.55 2728.45 495
-v -2285.67 2728.03 5
-v -2285.67 2728.03 5
-v -2289.55 2728.45 495
-v -2289.55 2728.45 5
-v -2289.55 2728.45 495
-v -2293.29 2727.36 495
-v -2289.55 2728.45 5
-v -2289.55 2728.45 5
-v -2293.29 2727.36 495
-v -2293.29 2727.36 5
-v -2293.29 2727.36 495
-v -2296.34 2724.92 495
-v -2293.29 2727.36 5
-v -2293.29 2727.36 5
-v -2296.34 2724.92 495
-v -2296.34 2724.92 5
-v -2296.34 2724.92 495
-v -2298.21 2721.5 495
-v -2296.34 2724.92 5
-v -2296.34 2724.92 5
-v -2298.21 2721.5 495
-v -2298.21 2721.5 5
-v -2298.21 2721.5 495
-v -2298.64 2717.62 495
-v -2298.21 2721.5 5
-v -2298.21 2721.5 5
-v -2298.64 2717.62 495
-v -2298.64 2717.62 5
-v -2298.64 2717.62 495
-v -2297.55 2713.87 495
-v -2298.64 2717.62 5
-v -2298.64 2717.62 5
-v -2297.55 2713.87 495
-v -2297.55 2713.87 5
-v -2297.55 2713.87 495
-v -2295.1 2710.83 495
-v -2297.55 2713.87 5
-v -2297.55 2713.87 5
-v -2295.1 2710.83 495
-v -2295.1 2710.83 5
-v -2295.1 2710.83 495
-v -2291.68 2708.95 495
-v -2295.1 2710.83 5
-v -2295.1 2710.83 5
-v -2291.68 2708.95 495
-v -2291.68 2708.95 5
-v -2291.68 2708.95 495
-v -2287.8 2708.53 495
-v -2291.68 2708.95 5
-v -2291.68 2708.95 5
-v -2287.8 2708.53 495
-v -2287.8 2708.53 5
-v -2287.8 2708.53 495
-v -2284.06 2709.62 495
-v -2287.8 2708.53 5
-v -2287.8 2708.53 5
-v -2284.06 2709.62 495
-v -2284.06 2709.62 5
-v -2284.06 2709.62 495
-v -2281.01 2712.06 495
-v -2284.06 2709.62 5
-v -2284.06 2709.62 5
-v -2281.01 2712.06 495
-v -2281.01 2712.06 5
-v -2510.83 2904.9 495
-v -2508.95 2908.32 495
-v -2510.83 2904.9 5
-v -2510.83 2904.9 5
-v -2508.95 2908.32 495
-v -2508.95 2908.32 5
-v -2508.95 2908.32 495
-v -2508.53 2912.2 495
-v -2508.95 2908.32 5
-v -2508.95 2908.32 5
-v -2508.53 2912.2 495
-v -2508.53 2912.2 5
-v -2508.53 2912.2 495
-v -2509.62 2915.94 495
-v -2508.53 2912.2 5
-v -2508.53 2912.2 5
-v -2509.62 2915.94 495
-v -2509.62 2915.94 5
-v -2509.62 2915.94 495
-v -2512.06 2918.99 495
-v -2509.62 2915.94 5
-v -2509.62 2915.94 5
-v -2512.06 2918.99 495
-v -2512.06 2918.99 5
-v -2512.06 2918.99 495
-v -2515.48 2920.86 495
-v -2512.06 2918.99 5
-v -2512.06 2918.99 5
-v -2515.48 2920.86 495
-v -2515.48 2920.86 5
-v -2515.48 2920.86 495
-v -2519.36 2921.29 495
-v -2515.48 2920.86 5
-v -2515.48 2920.86 5
-v -2519.36 2921.29 495
-v -2519.36 2921.29 5
-v -2519.36 2921.29 495
-v -2523.11 2920.19 495
-v -2519.36 2921.29 5
-v -2519.36 2921.29 5
-v -2523.11 2920.19 495
-v -2523.11 2920.19 5
-v -2523.11 2920.19 495
-v -2526.15 2917.75 495
-v -2523.11 2920.19 5
-v -2523.11 2920.19 5
-v -2526.15 2917.75 495
-v -2526.15 2917.75 5
-v -2526.15 2917.75 495
-v -2528.03 2914.33 495
-v -2526.15 2917.75 5
-v -2526.15 2917.75 5
-v -2528.03 2914.33 495
-v -2528.03 2914.33 5
-v -2528.03 2914.33 495
-v -2528.45 2910.45 495
-v -2528.03 2914.33 5
-v -2528.03 2914.33 5
-v -2528.45 2910.45 495
-v -2528.45 2910.45 5
-v -2528.45 2910.45 495
-v -2527.36 2906.71 495
-v -2528.45 2910.45 5
-v -2528.45 2910.45 5
-v -2527.36 2906.71 495
-v -2527.36 2906.71 5
-v -2527.36 2906.71 495
-v -2524.92 2903.66 495
-v -2527.36 2906.71 5
-v -2527.36 2906.71 5
-v -2524.92 2903.66 495
-v -2524.92 2903.66 5
-v -2524.92 2903.66 495
-v -2521.5 2901.79 495
-v -2524.92 2903.66 5
-v -2524.92 2903.66 5
-v -2521.5 2901.79 495
-v -2521.5 2901.79 5
-v -2521.5 2901.79 495
-v -2517.62 2901.36 495
-v -2521.5 2901.79 5
-v -2521.5 2901.79 5
-v -2517.62 2901.36 495
-v -2517.62 2901.36 5
-v -2517.62 2901.36 495
-v -2513.87 2902.45 495
-v -2517.62 2901.36 5
-v -2517.62 2901.36 5
-v -2513.87 2902.45 495
-v -2513.87 2902.45 5
-v -2513.87 2902.45 495
-v -2510.83 2904.9 495
-v -2513.87 2902.45 5
-v -2513.87 2902.45 5
-v -2510.83 2904.9 495
-v -2510.83 2904.9 5
-v -2703.66 2675.08 495
-v -2701.79 2678.5 495
-v -2703.66 2675.08 5
-v -2703.66 2675.08 5
-v -2701.79 2678.5 495
-v -2701.79 2678.5 5
-v -2701.79 2678.5 495
-v -2701.36 2682.38 495
-v -2701.79 2678.5 5
-v -2701.79 2678.5 5
-v -2701.36 2682.38 495
-v -2701.36 2682.38 5
-v -2701.36 2682.38 495
-v -2702.45 2686.13 495
-v -2701.36 2682.38 5
-v -2701.36 2682.38 5
-v -2702.45 2686.13 495
-v -2702.45 2686.13 5
-v -2702.45 2686.13 495
-v -2704.9 2689.17 495
-v -2702.45 2686.13 5
-v -2702.45 2686.13 5
-v -2704.9 2689.17 495
-v -2704.9 2689.17 5
-v -2704.9 2689.17 495
-v -2708.32 2691.05 495
-v -2704.9 2689.17 5
-v -2704.9 2689.17 5
-v -2708.32 2691.05 495
-v -2708.32 2691.05 5
-v -2708.32 2691.05 495
-v -2712.2 2691.47 495
-v -2708.32 2691.05 5
-v -2708.32 2691.05 5
-v -2712.2 2691.47 495
-v -2712.2 2691.47 5
-v -2712.2 2691.47 495
-v -2715.94 2690.38 495
-v -2712.2 2691.47 5
-v -2712.2 2691.47 5
-v -2715.94 2690.38 495
-v -2715.94 2690.38 5
-v -2715.94 2690.38 495
-v -2718.99 2687.94 495
-v -2715.94 2690.38 5
-v -2715.94 2690.38 5
-v -2718.99 2687.94 495
-v -2718.99 2687.94 5
-v -2718.99 2687.94 495
-v -2720.86 2684.52 495
-v -2718.99 2687.94 5
-v -2718.99 2687.94 5
-v -2720.86 2684.52 495
-v -2720.86 2684.52 5
-v -2720.86 2684.52 495
-v -2721.29 2680.64 495
-v -2720.86 2684.52 5
-v -2720.86 2684.52 5
-v -2721.29 2680.64 495
-v -2721.29 2680.64 5
-v -2721.29 2680.64 495
-v -2720.19 2676.89 495
-v -2721.29 2680.64 5
-v -2721.29 2680.64 5
-v -2720.19 2676.89 495
-v -2720.19 2676.89 5
-v -2720.19 2676.89 495
-v -2717.75 2673.85 495
-v -2720.19 2676.89 5
-v -2720.19 2676.89 5
-v -2717.75 2673.85 495
-v -2717.75 2673.85 5
-v -2717.75 2673.85 495
-v -2714.33 2671.97 495
-v -2717.75 2673.85 5
-v -2717.75 2673.85 5
-v -2714.33 2671.97 495
-v -2714.33 2671.97 5
-v -2714.33 2671.97 495
-v -2710.45 2671.55 495
-v -2714.33 2671.97 5
-v -2714.33 2671.97 5
-v -2710.45 2671.55 495
-v -2710.45 2671.55 5
-v -2710.45 2671.55 495
-v -2706.71 2672.64 495
-v -2710.45 2671.55 5
-v -2710.45 2671.55 5
-v -2706.71 2672.64 495
-v -2706.71 2672.64 5
-v -2706.71 2672.64 495
-v -2703.66 2675.08 495
-v -2706.71 2672.64 5
-v -2706.71 2672.64 5
-v -2703.66 2675.08 495
-v -2703.66 2675.08 5
-v -2473.85 2482.25 495
-v -2471.97 2485.67 495
-v -2473.85 2482.25 5
-v -2473.85 2482.25 5
-v -2471.97 2485.67 495
-v -2471.97 2485.67 5
-v -2471.97 2485.67 495
-v -2471.55 2489.55 495
-v -2471.97 2485.67 5
-v -2471.97 2485.67 5
-v -2471.55 2489.55 495
-v -2471.55 2489.55 5
-v -2471.55 2489.55 495
-v -2472.64 2493.29 495
-v -2471.55 2489.55 5
-v -2471.55 2489.55 5
-v -2472.64 2493.29 495
-v -2472.64 2493.29 5
-v -2472.64 2493.29 495
-v -2475.08 2496.34 495
-v -2472.64 2493.29 5
-v -2472.64 2493.29 5
-v -2475.08 2496.34 495
-v -2475.08 2496.34 5
-v -2475.08 2496.34 495
-v -2478.5 2498.21 495
-v -2475.08 2496.34 5
-v -2475.08 2496.34 5
-v -2478.5 2498.21 495
-v -2478.5 2498.21 5
-v -2478.5 2498.21 495
-v -2482.38 2498.64 495
-v -2478.5 2498.21 5
-v -2478.5 2498.21 5
-v -2482.38 2498.64 495
-v -2482.38 2498.64 5
-v -2482.38 2498.64 495
-v -2486.13 2497.55 495
-v -2482.38 2498.64 5
-v -2482.38 2498.64 5
-v -2486.13 2497.55 495
-v -2486.13 2497.55 5
-v -2486.13 2497.55 495
-v -2489.17 2495.1 495
-v -2486.13 2497.55 5
-v -2486.13 2497.55 5
-v -2489.17 2495.1 495
-v -2489.17 2495.1 5
-v -2489.17 2495.1 495
-v -2491.05 2491.68 495
-v -2489.17 2495.1 5
-v -2489.17 2495.1 5
-v -2491.05 2491.68 495
-v -2491.05 2491.68 5
-v -2491.05 2491.68 495
-v -2491.47 2487.8 495
-v -2491.05 2491.68 5
-v -2491.05 2491.68 5
-v -2491.47 2487.8 495
-v -2491.47 2487.8 5
-v -2491.47 2487.8 495
-v -2490.38 2484.06 495
-v -2491.47 2487.8 5
-v -2491.47 2487.8 5
-v -2490.38 2484.06 495
-v -2490.38 2484.06 5
-v -2490.38 2484.06 495
-v -2487.94 2481.01 495
-v -2490.38 2484.06 5
-v -2490.38 2484.06 5
-v -2487.94 2481.01 495
-v -2487.94 2481.01 5
-v -2487.94 2481.01 495
-v -2484.52 2479.14 495
-v -2487.94 2481.01 5
-v -2487.94 2481.01 5
-v -2484.52 2479.14 495
-v -2484.52 2479.14 5
-v -2484.52 2479.14 495
-v -2480.64 2478.71 495
-v -2484.52 2479.14 5
-v -2484.52 2479.14 5
-v -2480.64 2478.71 495
-v -2480.64 2478.71 5
-v -2480.64 2478.71 495
-v -2476.89 2479.81 495
-v -2480.64 2478.71 5
-v -2480.64 2478.71 5
-v -2476.89 2479.81 495
-v -2476.89 2479.81 5
-v -2476.89 2479.81 495
-v -2473.85 2482.25 495
-v -2476.89 2479.81 5
-v -2476.89 2479.81 5
-v -2473.85 2482.25 495
-v -2473.85 2482.25 5
-v -2781.77 2675.35 510
-v -2475.35 2418.23 510
-v -2524.65 2981.77 510
-v -2524.65 2981.77 510
-v -2475.35 2418.23 510
-v -2218.23 2724.65 510
-v -2475.35 2418.23 500
-v -2781.77 2675.35 500
-v -2218.23 2724.65 500
-v -2218.23 2724.65 500
-v -2781.77 2675.35 500
-v -2524.65 2981.77 500
-v -2475.35 2418.23 510
-v -2475.35 2418.23 500
-v -2218.23 2724.65 510
-v -2218.23 2724.65 510
-v -2475.35 2418.23 500
-v -2218.23 2724.65 500
-v -2781.77 2675.35 500
-v -2781.77 2675.35 510
-v -2524.65 2981.77 500
-v -2524.65 2981.77 500
-v -2781.77 2675.35 510
-v -2524.65 2981.77 510
-v -2524.65 2981.77 500
-v -2524.65 2981.77 510
-v -2218.23 2724.65 500
-v -2218.23 2724.65 500
-v -2524.65 2981.77 510
-v -2218.23 2724.65 510
-v -2781.77 2675.35 500
-v -2475.35 2418.23 500
-v -2781.77 2675.35 510
-v -2781.77 2675.35 510
-v -2475.35 2418.23 500
-v -2475.35 2418.23 510
-v -2322.27 2818.47 810
-v -2320.39 2821.89 810
-v -2322.27 2818.47 510
-v -2322.27 2818.47 510
-v -2320.39 2821.89 810
-v -2320.39 2821.89 510
-v -2320.39 2821.89 810
-v -2319.96 2825.77 810
-v -2320.39 2821.89 510
-v -2320.39 2821.89 510
-v -2319.96 2825.77 810
-v -2319.96 2825.77 510
-v -2319.96 2825.77 810
-v -2321.06 2829.52 810
-v -2319.96 2825.77 510
-v -2319.96 2825.77 510
-v -2321.06 2829.52 810
-v -2321.06 2829.52 510
-v -2321.06 2829.52 810
-v -2323.5 2832.56 810
-v -2321.06 2829.52 510
-v -2321.06 2829.52 510
-v -2323.5 2832.56 810
-v -2323.5 2832.56 510
-v -2323.5 2832.56 810
-v -2326.92 2834.44 810
-v -2323.5 2832.56 510
-v -2323.5 2832.56 510
-v -2326.92 2834.44 810
-v -2326.92 2834.44 510
-v -2326.92 2834.44 810
-v -2330.8 2834.86 810
-v -2326.92 2834.44 510
-v -2326.92 2834.44 510
-v -2330.8 2834.86 810
-v -2330.8 2834.86 510
-v -2330.8 2834.86 810
-v -2334.54 2833.77 810
-v -2330.8 2834.86 510
-v -2330.8 2834.86 510
-v -2334.54 2833.77 810
-v -2334.54 2833.77 510
-v -2334.54 2833.77 810
-v -2337.59 2831.33 810
-v -2334.54 2833.77 510
-v -2334.54 2833.77 510
-v -2337.59 2831.33 810
-v -2337.59 2831.33 510
-v -2337.59 2831.33 810
-v -2339.46 2827.91 810
-v -2337.59 2831.33 510
-v -2337.59 2831.33 510
-v -2339.46 2827.91 810
-v -2339.46 2827.91 510
-v -2339.46 2827.91 810
-v -2339.89 2824.03 810
-v -2339.46 2827.91 510
-v -2339.46 2827.91 510
-v -2339.89 2824.03 810
-v -2339.89 2824.03 510
-v -2339.89 2824.03 810
-v -2338.8 2820.28 810
-v -2339.89 2824.03 510
-v -2339.89 2824.03 510
-v -2338.8 2820.28 810
-v -2338.8 2820.28 510
-v -2338.8 2820.28 810
-v -2336.35 2817.24 810
-v -2338.8 2820.28 510
-v -2338.8 2820.28 510
-v -2336.35 2817.24 810
-v -2336.35 2817.24 510
-v -2336.35 2817.24 810
-v -2332.93 2815.36 810
-v -2336.35 2817.24 510
-v -2336.35 2817.24 510
-v -2332.93 2815.36 810
-v -2332.93 2815.36 510
-v -2332.93 2815.36 810
-v -2329.05 2814.94 810
-v -2332.93 2815.36 510
-v -2332.93 2815.36 510
-v -2329.05 2814.94 810
-v -2329.05 2814.94 510
-v -2329.05 2814.94 810
-v -2325.31 2816.03 810
-v -2329.05 2814.94 510
-v -2329.05 2814.94 510
-v -2325.31 2816.03 810
-v -2325.31 2816.03 510
-v -2325.31 2816.03 810
-v -2322.27 2818.47 810
-v -2325.31 2816.03 510
-v -2325.31 2816.03 510
-v -2322.27 2818.47 810
-v -2322.27 2818.47 510
-v -2398.87 2882.75 810
-v -2396.99 2886.17 810
-v -2398.87 2882.75 510
-v -2398.87 2882.75 510
-v -2396.99 2886.17 810
-v -2396.99 2886.17 510
-v -2396.99 2886.17 810
-v -2396.57 2890.05 810
-v -2396.99 2886.17 510
-v -2396.99 2886.17 510
-v -2396.57 2890.05 810
-v -2396.57 2890.05 510
-v -2396.57 2890.05 810
-v -2397.66 2893.8 810
-v -2396.57 2890.05 510
-v -2396.57 2890.05 510
-v -2397.66 2893.8 810
-v -2397.66 2893.8 510
-v -2397.66 2893.8 810
-v -2400.1 2896.84 810
-v -2397.66 2893.8 510
-v -2397.66 2893.8 510
-v -2400.1 2896.84 810
-v -2400.1 2896.84 510
-v -2400.1 2896.84 810
-v -2403.52 2898.72 810
-v -2400.1 2896.84 510
-v -2400.1 2896.84 510
-v -2403.52 2898.72 810
-v -2403.52 2898.72 510
-v -2403.52 2898.72 810
-v -2407.4 2899.14 810
-v -2403.52 2898.72 510
-v -2403.52 2898.72 510
-v -2407.4 2899.14 810
-v -2407.4 2899.14 510
-v -2407.4 2899.14 810
-v -2411.15 2898.05 810
-v -2407.4 2899.14 510
-v -2407.4 2899.14 510
-v -2411.15 2898.05 810
-v -2411.15 2898.05 510
-v -2411.15 2898.05 810
-v -2414.19 2895.61 810
-v -2411.15 2898.05 510
-v -2411.15 2898.05 510
-v -2414.19 2895.61 810
-v -2414.19 2895.61 510
-v -2414.19 2895.61 810
-v -2416.07 2892.19 810
-v -2414.19 2895.61 510
-v -2414.19 2895.61 510
-v -2416.07 2892.19 810
-v -2416.07 2892.19 510
-v -2416.07 2892.19 810
-v -2416.49 2888.31 810
-v -2416.07 2892.19 510
-v -2416.07 2892.19 510
-v -2416.49 2888.31 810
-v -2416.49 2888.31 510
-v -2416.49 2888.31 810
-v -2415.4 2884.56 810
-v -2416.49 2888.31 510
-v -2416.49 2888.31 510
-v -2415.4 2884.56 810
-v -2415.4 2884.56 510
-v -2415.4 2884.56 810
-v -2412.96 2881.52 810
-v -2415.4 2884.56 510
-v -2415.4 2884.56 510
-v -2412.96 2881.52 810
-v -2412.96 2881.52 510
-v -2412.96 2881.52 810
-v -2409.54 2879.64 810
-v -2412.96 2881.52 510
-v -2412.96 2881.52 510
-v -2409.54 2879.64 810
-v -2409.54 2879.64 510
-v -2409.54 2879.64 810
-v -2405.66 2879.22 810
-v -2409.54 2879.64 510
-v -2409.54 2879.64 510
-v -2405.66 2879.22 810
-v -2405.66 2879.22 510
-v -2405.66 2879.22 810
-v -2401.91 2880.31 810
-v -2405.66 2879.22 510
-v -2405.66 2879.22 510
-v -2401.91 2880.31 810
-v -2401.91 2880.31 510
-v -2401.91 2880.31 810
-v -2398.87 2882.75 810
-v -2401.91 2880.31 510
-v -2401.91 2880.31 510
-v -2398.87 2882.75 810
-v -2398.87 2882.75 510
-v -2531.08 2974.11 910
-v -2224.66 2716.99 910
-v -2524.65 2981.77 910
-v -2524.65 2981.77 910
-v -2224.66 2716.99 910
-v -2218.23 2724.65 910
-v -2224.66 2716.99 710
-v -2531.08 2974.11 710
-v -2218.23 2724.65 710
-v -2218.23 2724.65 710
-v -2531.08 2974.11 710
-v -2524.65 2981.77 710
-v -2224.66 2716.99 910
-v -2224.66 2716.99 710
-v -2218.23 2724.65 910
-v -2218.23 2724.65 910
-v -2224.66 2716.99 710
-v -2218.23 2724.65 710
-v -2531.08 2974.11 710
-v -2531.08 2974.11 910
-v -2524.65 2981.77 710
-v -2524.65 2981.77 710
-v -2531.08 2974.11 910
-v -2524.65 2981.77 910
-v -2524.65 2981.77 710
-v -2524.65 2981.77 910
-v -2218.23 2724.65 710
-v -2218.23 2724.65 710
-v -2524.65 2981.77 910
-v -2218.23 2724.65 910
-v -2531.08 2974.11 710
-v -2224.66 2716.99 710
-v -2531.08 2974.11 910
-v -2531.08 2974.11 910
-v -2224.66 2716.99 710
-v -2224.66 2716.99 910
-v -660 1150 495
-v -659.239 1146.17 495
-v -660 1150 5
-v -660 1150 5
-v -659.239 1146.17 495
-v -659.239 1146.17 5
-v -659.239 1146.17 495
-v -657.071 1142.93 495
-v -659.239 1146.17 5
-v -659.239 1146.17 5
-v -657.071 1142.93 495
-v -657.071 1142.93 5
-v -657.071 1142.93 495
-v -653.827 1140.76 495
-v -657.071 1142.93 5
-v -657.071 1142.93 5
-v -653.827 1140.76 495
-v -653.827 1140.76 5
-v -653.827 1140.76 495
-v -650 1140 495
-v -653.827 1140.76 5
-v -653.827 1140.76 5
-v -650 1140 495
-v -650 1140 5
-v -650 1140 495
-v -646.173 1140.76 495
-v -650 1140 5
-v -650 1140 5
-v -646.173 1140.76 495
-v -646.173 1140.76 5
-v -646.173 1140.76 495
-v -642.929 1142.93 495
-v -646.173 1140.76 5
-v -646.173 1140.76 5
-v -642.929 1142.93 495
-v -642.929 1142.93 5
-v -642.929 1142.93 495
-v -640.761 1146.17 495
-v -642.929 1142.93 5
-v -642.929 1142.93 5
-v -640.761 1146.17 495
-v -640.761 1146.17 5
-v -640.761 1146.17 495
-v -640 1150 495
-v -640.761 1146.17 5
-v -640.761 1146.17 5
-v -640 1150 495
-v -640 1150 5
-v -640 1150 495
-v -640.761 1153.83 495
-v -640 1150 5
-v -640 1150 5
-v -640.761 1153.83 495
-v -640.761 1153.83 5
-v -640.761 1153.83 495
-v -642.929 1157.07 495
-v -640.761 1153.83 5
-v -640.761 1153.83 5
-v -642.929 1157.07 495
-v -642.929 1157.07 5
-v -642.929 1157.07 495
-v -646.173 1159.24 495
-v -642.929 1157.07 5
-v -642.929 1157.07 5
-v -646.173 1159.24 495
-v -646.173 1159.24 5
-v -646.173 1159.24 495
-v -650 1160 495
-v -646.173 1159.24 5
-v -646.173 1159.24 5
-v -650 1160 495
-v -650 1160 5
-v -650 1160 495
-v -653.827 1159.24 495
-v -650 1160 5
-v -650 1160 5
-v -653.827 1159.24 495
-v -653.827 1159.24 5
-v -653.827 1159.24 495
-v -657.071 1157.07 495
-v -653.827 1159.24 5
-v -653.827 1159.24 5
-v -657.071 1157.07 495
-v -657.071 1157.07 5
-v -657.071 1157.07 495
-v -659.239 1153.83 495
-v -657.071 1157.07 5
-v -657.071 1157.07 5
-v -659.239 1153.83 495
-v -659.239 1153.83 5
-v -659.239 1153.83 495
-v -660 1150 495
-v -659.239 1153.83 5
-v -659.239 1153.83 5
-v -660 1150 495
-v -660 1150 5
-v -360 1150 495
-v -359.239 1146.17 495
-v -360 1150 5
-v -360 1150 5
-v -359.239 1146.17 495
-v -359.239 1146.17 5
-v -359.239 1146.17 495
-v -357.071 1142.93 495
-v -359.239 1146.17 5
-v -359.239 1146.17 5
-v -357.071 1142.93 495
-v -357.071 1142.93 5
-v -357.071 1142.93 495
-v -353.827 1140.76 495
-v -357.071 1142.93 5
-v -357.071 1142.93 5
-v -353.827 1140.76 495
-v -353.827 1140.76 5
-v -353.827 1140.76 495
-v -350 1140 495
-v -353.827 1140.76 5
-v -353.827 1140.76 5
-v -350 1140 495
-v -350 1140 5
-v -350 1140 495
-v -346.173 1140.76 495
-v -350 1140 5
-v -350 1140 5
-v -346.173 1140.76 495
-v -346.173 1140.76 5
-v -346.173 1140.76 495
-v -342.929 1142.93 495
-v -346.173 1140.76 5
-v -346.173 1140.76 5
-v -342.929 1142.93 495
-v -342.929 1142.93 5
-v -342.929 1142.93 495
-v -340.761 1146.17 495
-v -342.929 1142.93 5
-v -342.929 1142.93 5
-v -340.761 1146.17 495
-v -340.761 1146.17 5
-v -340.761 1146.17 495
-v -340 1150 495
-v -340.761 1146.17 5
-v -340.761 1146.17 5
-v -340 1150 495
-v -340 1150 5
-v -340 1150 495
-v -340.761 1153.83 495
-v -340 1150 5
-v -340 1150 5
-v -340.761 1153.83 495
-v -340.761 1153.83 5
-v -340.761 1153.83 495
-v -342.929 1157.07 495
-v -340.761 1153.83 5
-v -340.761 1153.83 5
-v -342.929 1157.07 495
-v -342.929 1157.07 5
-v -342.929 1157.07 495
-v -346.173 1159.24 495
-v -342.929 1157.07 5
-v -342.929 1157.07 5
-v -346.173 1159.24 495
-v -346.173 1159.24 5
-v -346.173 1159.24 495
-v -350 1160 495
-v -346.173 1159.24 5
-v -346.173 1159.24 5
-v -350 1160 495
-v -350 1160 5
-v -350 1160 495
-v -353.827 1159.24 495
-v -350 1160 5
-v -350 1160 5
-v -353.827 1159.24 495
-v -353.827 1159.24 5
-v -353.827 1159.24 495
-v -357.071 1157.07 495
-v -353.827 1159.24 5
-v -353.827 1159.24 5
-v -357.071 1157.07 495
-v -357.071 1157.07 5
-v -357.071 1157.07 495
-v -359.239 1153.83 495
-v -357.071 1157.07 5
-v -357.071 1157.07 5
-v -359.239 1153.83 495
-v -359.239 1153.83 5
-v -359.239 1153.83 495
-v -360 1150 495
-v -359.239 1153.83 5
-v -359.239 1153.83 5
-v -360 1150 495
-v -360 1150 5
-v -360 1450 495
-v -359.239 1446.17 495
-v -360 1450 5
-v -360 1450 5
-v -359.239 1446.17 495
-v -359.239 1446.17 5
-v -359.239 1446.17 495
-v -357.071 1442.93 495
-v -359.239 1446.17 5
-v -359.239 1446.17 5
-v -357.071 1442.93 495
-v -357.071 1442.93 5
-v -357.071 1442.93 495
-v -353.827 1440.76 495
-v -357.071 1442.93 5
-v -357.071 1442.93 5
-v -353.827 1440.76 495
-v -353.827 1440.76 5
-v -353.827 1440.76 495
-v -350 1440 495
-v -353.827 1440.76 5
-v -353.827 1440.76 5
-v -350 1440 495
-v -350 1440 5
-v -350 1440 495
-v -346.173 1440.76 495
-v -350 1440 5
-v -350 1440 5
-v -346.173 1440.76 495
-v -346.173 1440.76 5
-v -346.173 1440.76 495
-v -342.929 1442.93 495
-v -346.173 1440.76 5
-v -346.173 1440.76 5
-v -342.929 1442.93 495
-v -342.929 1442.93 5
-v -342.929 1442.93 495
-v -340.761 1446.17 495
-v -342.929 1442.93 5
-v -342.929 1442.93 5
-v -340.761 1446.17 495
-v -340.761 1446.17 5
-v -340.761 1446.17 495
-v -340 1450 495
-v -340.761 1446.17 5
-v -340.761 1446.17 5
-v -340 1450 495
-v -340 1450 5
-v -340 1450 495
-v -340.761 1453.83 495
-v -340 1450 5
-v -340 1450 5
-v -340.761 1453.83 495
-v -340.761 1453.83 5
-v -340.761 1453.83 495
-v -342.929 1457.07 495
-v -340.761 1453.83 5
-v -340.761 1453.83 5
-v -342.929 1457.07 495
-v -342.929 1457.07 5
-v -342.929 1457.07 495
-v -346.173 1459.24 495
-v -342.929 1457.07 5
-v -342.929 1457.07 5
-v -346.173 1459.24 495
-v -346.173 1459.24 5
-v -346.173 1459.24 495
-v -350 1460 495
-v -346.173 1459.24 5
-v -346.173 1459.24 5
-v -350 1460 495
-v -350 1460 5
-v -350 1460 495
-v -353.827 1459.24 495
-v -350 1460 5
-v -350 1460 5
-v -353.827 1459.24 495
-v -353.827 1459.24 5
-v -353.827 1459.24 495
-v -357.071 1457.07 495
-v -353.827 1459.24 5
-v -353.827 1459.24 5
-v -357.071 1457.07 495
-v -357.071 1457.07 5
-v -357.071 1457.07 495
-v -359.239 1453.83 495
-v -357.071 1457.07 5
-v -357.071 1457.07 5
-v -359.239 1453.83 495
-v -359.239 1453.83 5
-v -359.239 1453.83 495
-v -360 1450 495
-v -359.239 1453.83 5
-v -359.239 1453.83 5
-v -360 1450 495
-v -360 1450 5
-v -660 1450 495
-v -659.239 1446.17 495
-v -660 1450 5
-v -660 1450 5
-v -659.239 1446.17 495
-v -659.239 1446.17 5
-v -659.239 1446.17 495
-v -657.071 1442.93 495
-v -659.239 1446.17 5
-v -659.239 1446.17 5
-v -657.071 1442.93 495
-v -657.071 1442.93 5
-v -657.071 1442.93 495
-v -653.827 1440.76 495
-v -657.071 1442.93 5
-v -657.071 1442.93 5
-v -653.827 1440.76 495
-v -653.827 1440.76 5
-v -653.827 1440.76 495
-v -650 1440 495
-v -653.827 1440.76 5
-v -653.827 1440.76 5
-v -650 1440 495
-v -650 1440 5
-v -650 1440 495
-v -646.173 1440.76 495
-v -650 1440 5
-v -650 1440 5
-v -646.173 1440.76 495
-v -646.173 1440.76 5
-v -646.173 1440.76 495
-v -642.929 1442.93 495
-v -646.173 1440.76 5
-v -646.173 1440.76 5
-v -642.929 1442.93 495
-v -642.929 1442.93 5
-v -642.929 1442.93 495
-v -640.761 1446.17 495
-v -642.929 1442.93 5
-v -642.929 1442.93 5
-v -640.761 1446.17 495
-v -640.761 1446.17 5
-v -640.761 1446.17 495
-v -640 1450 495
-v -640.761 1446.17 5
-v -640.761 1446.17 5
-v -640 1450 495
-v -640 1450 5
-v -640 1450 495
-v -640.761 1453.83 495
-v -640 1450 5
-v -640 1450 5
-v -640.761 1453.83 495
-v -640.761 1453.83 5
-v -640.761 1453.83 495
-v -642.929 1457.07 495
-v -640.761 1453.83 5
-v -640.761 1453.83 5
-v -642.929 1457.07 495
-v -642.929 1457.07 5
-v -642.929 1457.07 495
-v -646.173 1459.24 495
-v -642.929 1457.07 5
-v -642.929 1457.07 5
-v -646.173 1459.24 495
-v -646.173 1459.24 5
-v -646.173 1459.24 495
-v -650 1460 495
-v -646.173 1459.24 5
-v -646.173 1459.24 5
-v -650 1460 495
-v -650 1460 5
-v -650 1460 495
-v -653.827 1459.24 495
-v -650 1460 5
-v -650 1460 5
-v -653.827 1459.24 495
-v -653.827 1459.24 5
-v -653.827 1459.24 495
-v -657.071 1457.07 495
-v -653.827 1459.24 5
-v -653.827 1459.24 5
-v -657.071 1457.07 495
-v -657.071 1457.07 5
-v -657.071 1457.07 495
-v -659.239 1453.83 495
-v -657.071 1457.07 5
-v -657.071 1457.07 5
-v -659.239 1453.83 495
-v -659.239 1453.83 5
-v -659.239 1453.83 495
-v -660 1450 495
-v -659.239 1453.83 5
-v -659.239 1453.83 5
-v -660 1450 495
-v -660 1450 5
-v -300 1500 510
-v -700 1500 510
-v -300 1100 510
-v -300 1100 510
-v -700 1500 510
-v -700 1100 510
-v -700 1500 500
-v -300 1500 500
-v -700 1100 500
-v -700 1100 500
-v -300 1500 500
-v -300 1100 500
-v -700 1500 510
-v -700 1500 500
-v -700 1100 510
-v -700 1100 510
-v -700 1500 500
-v -700 1100 500
-v -300 1500 500
-v -300 1500 510
-v -300 1100 500
-v -300 1100 500
-v -300 1500 510
-v -300 1100 510
-v -300 1100 500
-v -300 1100 510
-v -700 1100 500
-v -700 1100 500
-v -300 1100 510
-v -700 1100 510
-v -300 1500 500
-v -700 1500 500
-v -300 1500 510
-v -300 1500 510
-v -700 1500 500
-v -700 1500 510
-v -560 1095 810
-v -559.239 1091.17 810
-v -560 1095 510
-v -560 1095 510
-v -559.239 1091.17 810
-v -559.239 1091.17 510
-v -559.239 1091.17 810
-v -557.071 1087.93 810
-v -559.239 1091.17 510
-v -559.239 1091.17 510
-v -557.071 1087.93 810
-v -557.071 1087.93 510
-v -557.071 1087.93 810
-v -553.827 1085.76 810
-v -557.071 1087.93 510
-v -557.071 1087.93 510
-v -553.827 1085.76 810
-v -553.827 1085.76 510
-v -553.827 1085.76 810
-v -550 1085 810
-v -553.827 1085.76 510
-v -553.827 1085.76 510
-v -550 1085 810
-v -550 1085 510
-v -550 1085 810
-v -546.173 1085.76 810
-v -550 1085 510
-v -550 1085 510
-v -546.173 1085.76 810
-v -546.173 1085.76 510
-v -546.173 1085.76 810
-v -542.929 1087.93 810
-v -546.173 1085.76 510
-v -546.173 1085.76 510
-v -542.929 1087.93 810
-v -542.929 1087.93 510
-v -542.929 1087.93 810
-v -540.761 1091.17 810
-v -542.929 1087.93 510
-v -542.929 1087.93 510
-v -540.761 1091.17 810
-v -540.761 1091.17 510
-v -540.761 1091.17 810
-v -540 1095 810
-v -540.761 1091.17 510
-v -540.761 1091.17 510
-v -540 1095 810
-v -540 1095 510
-v -540 1095 810
-v -540.761 1098.83 810
-v -540 1095 510
-v -540 1095 510
-v -540.761 1098.83 810
-v -540.761 1098.83 510
-v -540.761 1098.83 810
-v -542.929 1102.07 810
-v -540.761 1098.83 510
-v -540.761 1098.83 510
-v -542.929 1102.07 810
-v -542.929 1102.07 510
-v -542.929 1102.07 810
-v -546.173 1104.24 810
-v -542.929 1102.07 510
-v -542.929 1102.07 510
-v -546.173 1104.24 810
-v -546.173 1104.24 510
-v -546.173 1104.24 810
-v -550 1105 810
-v -546.173 1104.24 510
-v -546.173 1104.24 510
-v -550 1105 810
-v -550 1105 510
-v -550 1105 810
-v -553.827 1104.24 810
-v -550 1105 510
-v -550 1105 510
-v -553.827 1104.24 810
-v -553.827 1104.24 510
-v -553.827 1104.24 810
-v -557.071 1102.07 810
-v -553.827 1104.24 510
-v -553.827 1104.24 510
-v -557.071 1102.07 810
-v -557.071 1102.07 510
-v -557.071 1102.07 810
-v -559.239 1098.83 810
-v -557.071 1102.07 510
-v -557.071 1102.07 510
-v -559.239 1098.83 810
-v -559.239 1098.83 510
-v -559.239 1098.83 810
-v -560 1095 810
-v -559.239 1098.83 510
-v -559.239 1098.83 510
-v -560 1095 810
-v -560 1095 510
-v -460 1095 810
-v -459.239 1091.17 810
-v -460 1095 510
-v -460 1095 510
-v -459.239 1091.17 810
-v -459.239 1091.17 510
-v -459.239 1091.17 810
-v -457.071 1087.93 810
-v -459.239 1091.17 510
-v -459.239 1091.17 510
-v -457.071 1087.93 810
-v -457.071 1087.93 510
-v -457.071 1087.93 810
-v -453.827 1085.76 810
-v -457.071 1087.93 510
-v -457.071 1087.93 510
-v -453.827 1085.76 810
-v -453.827 1085.76 510
-v -453.827 1085.76 810
-v -450 1085 810
-v -453.827 1085.76 510
-v -453.827 1085.76 510
-v -450 1085 810
-v -450 1085 510
-v -450 1085 810
-v -446.173 1085.76 810
-v -450 1085 510
-v -450 1085 510
-v -446.173 1085.76 810
-v -446.173 1085.76 510
-v -446.173 1085.76 810
-v -442.929 1087.93 810
-v -446.173 1085.76 510
-v -446.173 1085.76 510
-v -442.929 1087.93 810
-v -442.929 1087.93 510
-v -442.929 1087.93 810
-v -440.761 1091.17 810
-v -442.929 1087.93 510
-v -442.929 1087.93 510
-v -440.761 1091.17 810
-v -440.761 1091.17 510
-v -440.761 1091.17 810
-v -440 1095 810
-v -440.761 1091.17 510
-v -440.761 1091.17 510
-v -440 1095 810
-v -440 1095 510
-v -440 1095 810
-v -440.761 1098.83 810
-v -440 1095 510
-v -440 1095 510
-v -440.761 1098.83 810
-v -440.761 1098.83 510
-v -440.761 1098.83 810
-v -442.929 1102.07 810
-v -440.761 1098.83 510
-v -440.761 1098.83 510
-v -442.929 1102.07 810
-v -442.929 1102.07 510
-v -442.929 1102.07 810
-v -446.173 1104.24 810
-v -442.929 1102.07 510
-v -442.929 1102.07 510
-v -446.173 1104.24 810
-v -446.173 1104.24 510
-v -446.173 1104.24 810
-v -450 1105 810
-v -446.173 1104.24 510
-v -446.173 1104.24 510
-v -450 1105 810
-v -450 1105 510
-v -450 1105 810
-v -453.827 1104.24 810
-v -450 1105 510
-v -450 1105 510
-v -453.827 1104.24 810
-v -453.827 1104.24 510
-v -453.827 1104.24 810
-v -457.071 1102.07 810
-v -453.827 1104.24 510
-v -453.827 1104.24 510
-v -457.071 1102.07 810
-v -457.071 1102.07 510
-v -457.071 1102.07 810
-v -459.239 1098.83 810
-v -457.071 1102.07 510
-v -457.071 1102.07 510
-v -459.239 1098.83 810
-v -459.239 1098.83 510
-v -459.239 1098.83 810
-v -460 1095 810
-v -459.239 1098.83 510
-v -459.239 1098.83 510
-v -460 1095 810
-v -460 1095 510
-v -300 1110 910
-v -700 1110 910
-v -300 1100 910
-v -300 1100 910
-v -700 1110 910
-v -700 1100 910
-v -700 1110 710
-v -300 1110 710
-v -700 1100 710
-v -700 1100 710
-v -300 1110 710
-v -300 1100 710
-v -700 1110 910
-v -700 1110 710
-v -700 1100 910
-v -700 1100 910
-v -700 1110 710
-v -700 1100 710
-v -300 1110 710
-v -300 1110 910
-v -300 1100 710
-v -300 1100 710
-v -300 1110 910
-v -300 1100 910
-v -300 1100 710
-v -300 1100 910
-v -700 1100 710
-v -700 1100 710
-v -300 1100 910
-v -700 1100 910
-v -300 1110 710
-v -700 1110 710
-v -300 1110 910
-v -300 1110 910
-v -700 1110 710
-v -700 1110 910
-v -1660 1150 495
-v -1659.24 1146.17 495
-v -1660 1150 5
-v -1660 1150 5
-v -1659.24 1146.17 495
-v -1659.24 1146.17 5
-v -1659.24 1146.17 495
-v -1657.07 1142.93 495
-v -1659.24 1146.17 5
-v -1659.24 1146.17 5
-v -1657.07 1142.93 495
-v -1657.07 1142.93 5
-v -1657.07 1142.93 495
-v -1653.83 1140.76 495
-v -1657.07 1142.93 5
-v -1657.07 1142.93 5
-v -1653.83 1140.76 495
-v -1653.83 1140.76 5
-v -1653.83 1140.76 495
-v -1650 1140 495
-v -1653.83 1140.76 5
-v -1653.83 1140.76 5
-v -1650 1140 495
-v -1650 1140 5
-v -1650 1140 495
-v -1646.17 1140.76 495
-v -1650 1140 5
-v -1650 1140 5
-v -1646.17 1140.76 495
-v -1646.17 1140.76 5
-v -1646.17 1140.76 495
-v -1642.93 1142.93 495
-v -1646.17 1140.76 5
-v -1646.17 1140.76 5
-v -1642.93 1142.93 495
-v -1642.93 1142.93 5
-v -1642.93 1142.93 495
-v -1640.76 1146.17 495
-v -1642.93 1142.93 5
-v -1642.93 1142.93 5
-v -1640.76 1146.17 495
-v -1640.76 1146.17 5
-v -1640.76 1146.17 495
-v -1640 1150 495
-v -1640.76 1146.17 5
-v -1640.76 1146.17 5
-v -1640 1150 495
-v -1640 1150 5
-v -1640 1150 495
-v -1640.76 1153.83 495
-v -1640 1150 5
-v -1640 1150 5
-v -1640.76 1153.83 495
-v -1640.76 1153.83 5
-v -1640.76 1153.83 495
-v -1642.93 1157.07 495
-v -1640.76 1153.83 5
-v -1640.76 1153.83 5
-v -1642.93 1157.07 495
-v -1642.93 1157.07 5
-v -1642.93 1157.07 495
-v -1646.17 1159.24 495
-v -1642.93 1157.07 5
-v -1642.93 1157.07 5
-v -1646.17 1159.24 495
-v -1646.17 1159.24 5
-v -1646.17 1159.24 495
-v -1650 1160 495
-v -1646.17 1159.24 5
-v -1646.17 1159.24 5
-v -1650 1160 495
-v -1650 1160 5
-v -1650 1160 495
-v -1653.83 1159.24 495
-v -1650 1160 5
-v -1650 1160 5
-v -1653.83 1159.24 495
-v -1653.83 1159.24 5
-v -1653.83 1159.24 495
-v -1657.07 1157.07 495
-v -1653.83 1159.24 5
-v -1653.83 1159.24 5
-v -1657.07 1157.07 495
-v -1657.07 1157.07 5
-v -1657.07 1157.07 495
-v -1659.24 1153.83 495
-v -1657.07 1157.07 5
-v -1657.07 1157.07 5
-v -1659.24 1153.83 495
-v -1659.24 1153.83 5
-v -1659.24 1153.83 495
-v -1660 1150 495
-v -1659.24 1153.83 5
-v -1659.24 1153.83 5
-v -1660 1150 495
-v -1660 1150 5
-v -1360 1150 495
-v -1359.24 1146.17 495
-v -1360 1150 5
-v -1360 1150 5
-v -1359.24 1146.17 495
-v -1359.24 1146.17 5
-v -1359.24 1146.17 495
-v -1357.07 1142.93 495
-v -1359.24 1146.17 5
-v -1359.24 1146.17 5
-v -1357.07 1142.93 495
-v -1357.07 1142.93 5
-v -1357.07 1142.93 495
-v -1353.83 1140.76 495
-v -1357.07 1142.93 5
-v -1357.07 1142.93 5
-v -1353.83 1140.76 495
-v -1353.83 1140.76 5
-v -1353.83 1140.76 495
-v -1350 1140 495
-v -1353.83 1140.76 5
-v -1353.83 1140.76 5
-v -1350 1140 495
-v -1350 1140 5
-v -1350 1140 495
-v -1346.17 1140.76 495
-v -1350 1140 5
-v -1350 1140 5
-v -1346.17 1140.76 495
-v -1346.17 1140.76 5
-v -1346.17 1140.76 495
-v -1342.93 1142.93 495
-v -1346.17 1140.76 5
-v -1346.17 1140.76 5
-v -1342.93 1142.93 495
-v -1342.93 1142.93 5
-v -1342.93 1142.93 495
-v -1340.76 1146.17 495
-v -1342.93 1142.93 5
-v -1342.93 1142.93 5
-v -1340.76 1146.17 495
-v -1340.76 1146.17 5
-v -1340.76 1146.17 495
-v -1340 1150 495
-v -1340.76 1146.17 5
-v -1340.76 1146.17 5
-v -1340 1150 495
-v -1340 1150 5
-v -1340 1150 495
-v -1340.76 1153.83 495
-v -1340 1150 5
-v -1340 1150 5
-v -1340.76 1153.83 495
-v -1340.76 1153.83 5
-v -1340.76 1153.83 495
-v -1342.93 1157.07 495
-v -1340.76 1153.83 5
-v -1340.76 1153.83 5
-v -1342.93 1157.07 495
-v -1342.93 1157.07 5
-v -1342.93 1157.07 495
-v -1346.17 1159.24 495
-v -1342.93 1157.07 5
-v -1342.93 1157.07 5
-v -1346.17 1159.24 495
-v -1346.17 1159.24 5
-v -1346.17 1159.24 495
-v -1350 1160 495
-v -1346.17 1159.24 5
-v -1346.17 1159.24 5
-v -1350 1160 495
-v -1350 1160 5
-v -1350 1160 495
-v -1353.83 1159.24 495
-v -1350 1160 5
-v -1350 1160 5
-v -1353.83 1159.24 495
-v -1353.83 1159.24 5
-v -1353.83 1159.24 495
-v -1357.07 1157.07 495
-v -1353.83 1159.24 5
-v -1353.83 1159.24 5
-v -1357.07 1157.07 495
-v -1357.07 1157.07 5
-v -1357.07 1157.07 495
-v -1359.24 1153.83 495
-v -1357.07 1157.07 5
-v -1357.07 1157.07 5
-v -1359.24 1153.83 495
-v -1359.24 1153.83 5
-v -1359.24 1153.83 495
-v -1360 1150 495
-v -1359.24 1153.83 5
-v -1359.24 1153.83 5
-v -1360 1150 495
-v -1360 1150 5
-v -1360 1450 495
-v -1359.24 1446.17 495
-v -1360 1450 5
-v -1360 1450 5
-v -1359.24 1446.17 495
-v -1359.24 1446.17 5
-v -1359.24 1446.17 495
-v -1357.07 1442.93 495
-v -1359.24 1446.17 5
-v -1359.24 1446.17 5
-v -1357.07 1442.93 495
-v -1357.07 1442.93 5
-v -1357.07 1442.93 495
-v -1353.83 1440.76 495
-v -1357.07 1442.93 5
-v -1357.07 1442.93 5
-v -1353.83 1440.76 495
-v -1353.83 1440.76 5
-v -1353.83 1440.76 495
-v -1350 1440 495
-v -1353.83 1440.76 5
-v -1353.83 1440.76 5
-v -1350 1440 495
-v -1350 1440 5
-v -1350 1440 495
-v -1346.17 1440.76 495
-v -1350 1440 5
-v -1350 1440 5
-v -1346.17 1440.76 495
-v -1346.17 1440.76 5
-v -1346.17 1440.76 495
-v -1342.93 1442.93 495
-v -1346.17 1440.76 5
-v -1346.17 1440.76 5
-v -1342.93 1442.93 495
-v -1342.93 1442.93 5
-v -1342.93 1442.93 495
-v -1340.76 1446.17 495
-v -1342.93 1442.93 5
-v -1342.93 1442.93 5
-v -1340.76 1446.17 495
-v -1340.76 1446.17 5
-v -1340.76 1446.17 495
-v -1340 1450 495
-v -1340.76 1446.17 5
-v -1340.76 1446.17 5
-v -1340 1450 495
-v -1340 1450 5
-v -1340 1450 495
-v -1340.76 1453.83 495
-v -1340 1450 5
-v -1340 1450 5
-v -1340.76 1453.83 495
-v -1340.76 1453.83 5
-v -1340.76 1453.83 495
-v -1342.93 1457.07 495
-v -1340.76 1453.83 5
-v -1340.76 1453.83 5
-v -1342.93 1457.07 495
-v -1342.93 1457.07 5
-v -1342.93 1457.07 495
-v -1346.17 1459.24 495
-v -1342.93 1457.07 5
-v -1342.93 1457.07 5
-v -1346.17 1459.24 495
-v -1346.17 1459.24 5
-v -1346.17 1459.24 495
-v -1350 1460 495
-v -1346.17 1459.24 5
-v -1346.17 1459.24 5
-v -1350 1460 495
-v -1350 1460 5
-v -1350 1460 495
-v -1353.83 1459.24 495
-v -1350 1460 5
-v -1350 1460 5
-v -1353.83 1459.24 495
-v -1353.83 1459.24 5
-v -1353.83 1459.24 495
-v -1357.07 1457.07 495
-v -1353.83 1459.24 5
-v -1353.83 1459.24 5
-v -1357.07 1457.07 495
-v -1357.07 1457.07 5
-v -1357.07 1457.07 495
-v -1359.24 1453.83 495
-v -1357.07 1457.07 5
-v -1357.07 1457.07 5
-v -1359.24 1453.83 495
-v -1359.24 1453.83 5
-v -1359.24 1453.83 495
-v -1360 1450 495
-v -1359.24 1453.83 5
-v -1359.24 1453.83 5
-v -1360 1450 495
-v -1360 1450 5
-v -1660 1450 495
-v -1659.24 1446.17 495
-v -1660 1450 5
-v -1660 1450 5
-v -1659.24 1446.17 495
-v -1659.24 1446.17 5
-v -1659.24 1446.17 495
-v -1657.07 1442.93 495
-v -1659.24 1446.17 5
-v -1659.24 1446.17 5
-v -1657.07 1442.93 495
-v -1657.07 1442.93 5
-v -1657.07 1442.93 495
-v -1653.83 1440.76 495
-v -1657.07 1442.93 5
-v -1657.07 1442.93 5
-v -1653.83 1440.76 495
-v -1653.83 1440.76 5
-v -1653.83 1440.76 495
-v -1650 1440 495
-v -1653.83 1440.76 5
-v -1653.83 1440.76 5
-v -1650 1440 495
-v -1650 1440 5
-v -1650 1440 495
-v -1646.17 1440.76 495
-v -1650 1440 5
-v -1650 1440 5
-v -1646.17 1440.76 495
-v -1646.17 1440.76 5
-v -1646.17 1440.76 495
-v -1642.93 1442.93 495
-v -1646.17 1440.76 5
-v -1646.17 1440.76 5
-v -1642.93 1442.93 495
-v -1642.93 1442.93 5
-v -1642.93 1442.93 495
-v -1640.76 1446.17 495
-v -1642.93 1442.93 5
-v -1642.93 1442.93 5
-v -1640.76 1446.17 495
-v -1640.76 1446.17 5
-v -1640.76 1446.17 495
-v -1640 1450 495
-v -1640.76 1446.17 5
-v -1640.76 1446.17 5
-v -1640 1450 495
-v -1640 1450 5
-v -1640 1450 495
-v -1640.76 1453.83 495
-v -1640 1450 5
-v -1640 1450 5
-v -1640.76 1453.83 495
-v -1640.76 1453.83 5
-v -1640.76 1453.83 495
-v -1642.93 1457.07 495
-v -1640.76 1453.83 5
-v -1640.76 1453.83 5
-v -1642.93 1457.07 495
-v -1642.93 1457.07 5
-v -1642.93 1457.07 495
-v -1646.17 1459.24 495
-v -1642.93 1457.07 5
-v -1642.93 1457.07 5
-v -1646.17 1459.24 495
-v -1646.17 1459.24 5
-v -1646.17 1459.24 495
-v -1650 1460 495
-v -1646.17 1459.24 5
-v -1646.17 1459.24 5
-v -1650 1460 495
-v -1650 1460 5
-v -1650 1460 495
-v -1653.83 1459.24 495
-v -1650 1460 5
-v -1650 1460 5
-v -1653.83 1459.24 495
-v -1653.83 1459.24 5
-v -1653.83 1459.24 495
-v -1657.07 1457.07 495
-v -1653.83 1459.24 5
-v -1653.83 1459.24 5
-v -1657.07 1457.07 495
-v -1657.07 1457.07 5
-v -1657.07 1457.07 495
-v -1659.24 1453.83 495
-v -1657.07 1457.07 5
-v -1657.07 1457.07 5
-v -1659.24 1453.83 495
-v -1659.24 1453.83 5
-v -1659.24 1453.83 495
-v -1660 1450 495
-v -1659.24 1453.83 5
-v -1659.24 1453.83 5
-v -1660 1450 495
-v -1660 1450 5
-v -1300 1500 510
-v -1700 1500 510
-v -1300 1100 510
-v -1300 1100 510
-v -1700 1500 510
-v -1700 1100 510
-v -1700 1500 500
-v -1300 1500 500
-v -1700 1100 500
-v -1700 1100 500
-v -1300 1500 500
-v -1300 1100 500
-v -1700 1500 510
-v -1700 1500 500
-v -1700 1100 510
-v -1700 1100 510
-v -1700 1500 500
-v -1700 1100 500
-v -1300 1500 500
-v -1300 1500 510
-v -1300 1100 500
-v -1300 1100 500
-v -1300 1500 510
-v -1300 1100 510
-v -1300 1100 500
-v -1300 1100 510
-v -1700 1100 500
-v -1700 1100 500
-v -1300 1100 510
-v -1700 1100 510
-v -1300 1500 500
-v -1700 1500 500
-v -1300 1500 510
-v -1300 1500 510
-v -1700 1500 500
-v -1700 1500 510
-v -1560 1095 810
-v -1559.24 1091.17 810
-v -1560 1095 510
-v -1560 1095 510
-v -1559.24 1091.17 810
-v -1559.24 1091.17 510
-v -1559.24 1091.17 810
-v -1557.07 1087.93 810
-v -1559.24 1091.17 510
-v -1559.24 1091.17 510
-v -1557.07 1087.93 810
-v -1557.07 1087.93 510
-v -1557.07 1087.93 810
-v -1553.83 1085.76 810
-v -1557.07 1087.93 510
-v -1557.07 1087.93 510
-v -1553.83 1085.76 810
-v -1553.83 1085.76 510
-v -1553.83 1085.76 810
-v -1550 1085 810
-v -1553.83 1085.76 510
-v -1553.83 1085.76 510
-v -1550 1085 810
-v -1550 1085 510
-v -1550 1085 810
-v -1546.17 1085.76 810
-v -1550 1085 510
-v -1550 1085 510
-v -1546.17 1085.76 810
-v -1546.17 1085.76 510
-v -1546.17 1085.76 810
-v -1542.93 1087.93 810
-v -1546.17 1085.76 510
-v -1546.17 1085.76 510
-v -1542.93 1087.93 810
-v -1542.93 1087.93 510
-v -1542.93 1087.93 810
-v -1540.76 1091.17 810
-v -1542.93 1087.93 510
-v -1542.93 1087.93 510
-v -1540.76 1091.17 810
-v -1540.76 1091.17 510
-v -1540.76 1091.17 810
-v -1540 1095 810
-v -1540.76 1091.17 510
-v -1540.76 1091.17 510
-v -1540 1095 810
-v -1540 1095 510
-v -1540 1095 810
-v -1540.76 1098.83 810
-v -1540 1095 510
-v -1540 1095 510
-v -1540.76 1098.83 810
-v -1540.76 1098.83 510
-v -1540.76 1098.83 810
-v -1542.93 1102.07 810
-v -1540.76 1098.83 510
-v -1540.76 1098.83 510
-v -1542.93 1102.07 810
-v -1542.93 1102.07 510
-v -1542.93 1102.07 810
-v -1546.17 1104.24 810
-v -1542.93 1102.07 510
-v -1542.93 1102.07 510
-v -1546.17 1104.24 810
-v -1546.17 1104.24 510
-v -1546.17 1104.24 810
-v -1550 1105 810
-v -1546.17 1104.24 510
-v -1546.17 1104.24 510
-v -1550 1105 810
-v -1550 1105 510
-v -1550 1105 810
-v -1553.83 1104.24 810
-v -1550 1105 510
-v -1550 1105 510
-v -1553.83 1104.24 810
-v -1553.83 1104.24 510
-v -1553.83 1104.24 810
-v -1557.07 1102.07 810
-v -1553.83 1104.24 510
-v -1553.83 1104.24 510
-v -1557.07 1102.07 810
-v -1557.07 1102.07 510
-v -1557.07 1102.07 810
-v -1559.24 1098.83 810
-v -1557.07 1102.07 510
-v -1557.07 1102.07 510
-v -1559.24 1098.83 810
-v -1559.24 1098.83 510
-v -1559.24 1098.83 810
-v -1560 1095 810
-v -1559.24 1098.83 510
-v -1559.24 1098.83 510
-v -1560 1095 810
-v -1560 1095 510
-v -1460 1095 810
-v -1459.24 1091.17 810
-v -1460 1095 510
-v -1460 1095 510
-v -1459.24 1091.17 810
-v -1459.24 1091.17 510
-v -1459.24 1091.17 810
-v -1457.07 1087.93 810
-v -1459.24 1091.17 510
-v -1459.24 1091.17 510
-v -1457.07 1087.93 810
-v -1457.07 1087.93 510
-v -1457.07 1087.93 810
-v -1453.83 1085.76 810
-v -1457.07 1087.93 510
-v -1457.07 1087.93 510
-v -1453.83 1085.76 810
-v -1453.83 1085.76 510
-v -1453.83 1085.76 810
-v -1450 1085 810
-v -1453.83 1085.76 510
-v -1453.83 1085.76 510
-v -1450 1085 810
-v -1450 1085 510
-v -1450 1085 810
-v -1446.17 1085.76 810
-v -1450 1085 510
-v -1450 1085 510
-v -1446.17 1085.76 810
-v -1446.17 1085.76 510
-v -1446.17 1085.76 810
-v -1442.93 1087.93 810
-v -1446.17 1085.76 510
-v -1446.17 1085.76 510
-v -1442.93 1087.93 810
-v -1442.93 1087.93 510
-v -1442.93 1087.93 810
-v -1440.76 1091.17 810
-v -1442.93 1087.93 510
-v -1442.93 1087.93 510
-v -1440.76 1091.17 810
-v -1440.76 1091.17 510
-v -1440.76 1091.17 810
-v -1440 1095 810
-v -1440.76 1091.17 510
-v -1440.76 1091.17 510
-v -1440 1095 810
-v -1440 1095 510
-v -1440 1095 810
-v -1440.76 1098.83 810
-v -1440 1095 510
-v -1440 1095 510
-v -1440.76 1098.83 810
-v -1440.76 1098.83 510
-v -1440.76 1098.83 810
-v -1442.93 1102.07 810
-v -1440.76 1098.83 510
-v -1440.76 1098.83 510
-v -1442.93 1102.07 810
-v -1442.93 1102.07 510
-v -1442.93 1102.07 810
-v -1446.17 1104.24 810
-v -1442.93 1102.07 510
-v -1442.93 1102.07 510
-v -1446.17 1104.24 810
-v -1446.17 1104.24 510
-v -1446.17 1104.24 810
-v -1450 1105 810
-v -1446.17 1104.24 510
-v -1446.17 1104.24 510
-v -1450 1105 810
-v -1450 1105 510
-v -1450 1105 810
-v -1453.83 1104.24 810
-v -1450 1105 510
-v -1450 1105 510
-v -1453.83 1104.24 810
-v -1453.83 1104.24 510
-v -1453.83 1104.24 810
-v -1457.07 1102.07 810
-v -1453.83 1104.24 510
-v -1453.83 1104.24 510
-v -1457.07 1102.07 810
-v -1457.07 1102.07 510
-v -1457.07 1102.07 810
-v -1459.24 1098.83 810
-v -1457.07 1102.07 510
-v -1457.07 1102.07 510
-v -1459.24 1098.83 810
-v -1459.24 1098.83 510
-v -1459.24 1098.83 810
-v -1460 1095 810
-v -1459.24 1098.83 510
-v -1459.24 1098.83 510
-v -1460 1095 810
-v -1460 1095 510
-v -1300 1110 910
-v -1700 1110 910
-v -1300 1100 910
-v -1300 1100 910
-v -1700 1110 910
-v -1700 1100 910
-v -1700 1110 710
-v -1300 1110 710
-v -1700 1100 710
-v -1700 1100 710
-v -1300 1110 710
-v -1300 1100 710
-v -1700 1110 910
-v -1700 1110 710
-v -1700 1100 910
-v -1700 1100 910
-v -1700 1110 710
-v -1700 1100 710
-v -1300 1110 710
-v -1300 1110 910
-v -1300 1100 710
-v -1300 1100 710
-v -1300 1110 910
-v -1300 1100 910
-v -1300 1100 710
-v -1300 1100 910
-v -1700 1100 710
-v -1700 1100 710
-v -1300 1100 910
-v -1700 1100 910
-v -1300 1110 710
-v -1700 1110 710
-v -1300 1110 910
-v -1300 1110 910
-v -1700 1110 710
-v -1700 1110 910
-v -2660 1150 495
-v -2659.24 1146.17 495
-v -2660 1150 5
-v -2660 1150 5
-v -2659.24 1146.17 495
-v -2659.24 1146.17 5
-v -2659.24 1146.17 495
-v -2657.07 1142.93 495
-v -2659.24 1146.17 5
-v -2659.24 1146.17 5
-v -2657.07 1142.93 495
-v -2657.07 1142.93 5
-v -2657.07 1142.93 495
-v -2653.83 1140.76 495
-v -2657.07 1142.93 5
-v -2657.07 1142.93 5
-v -2653.83 1140.76 495
-v -2653.83 1140.76 5
-v -2653.83 1140.76 495
-v -2650 1140 495
-v -2653.83 1140.76 5
-v -2653.83 1140.76 5
-v -2650 1140 495
-v -2650 1140 5
-v -2650 1140 495
-v -2646.17 1140.76 495
-v -2650 1140 5
-v -2650 1140 5
-v -2646.17 1140.76 495
-v -2646.17 1140.76 5
-v -2646.17 1140.76 495
-v -2642.93 1142.93 495
-v -2646.17 1140.76 5
-v -2646.17 1140.76 5
-v -2642.93 1142.93 495
-v -2642.93 1142.93 5
-v -2642.93 1142.93 495
-v -2640.76 1146.17 495
-v -2642.93 1142.93 5
-v -2642.93 1142.93 5
-v -2640.76 1146.17 495
-v -2640.76 1146.17 5
-v -2640.76 1146.17 495
-v -2640 1150 495
-v -2640.76 1146.17 5
-v -2640.76 1146.17 5
-v -2640 1150 495
-v -2640 1150 5
-v -2640 1150 495
-v -2640.76 1153.83 495
-v -2640 1150 5
-v -2640 1150 5
-v -2640.76 1153.83 495
-v -2640.76 1153.83 5
-v -2640.76 1153.83 495
-v -2642.93 1157.07 495
-v -2640.76 1153.83 5
-v -2640.76 1153.83 5
-v -2642.93 1157.07 495
-v -2642.93 1157.07 5
-v -2642.93 1157.07 495
-v -2646.17 1159.24 495
-v -2642.93 1157.07 5
-v -2642.93 1157.07 5
-v -2646.17 1159.24 495
-v -2646.17 1159.24 5
-v -2646.17 1159.24 495
-v -2650 1160 495
-v -2646.17 1159.24 5
-v -2646.17 1159.24 5
-v -2650 1160 495
-v -2650 1160 5
-v -2650 1160 495
-v -2653.83 1159.24 495
-v -2650 1160 5
-v -2650 1160 5
-v -2653.83 1159.24 495
-v -2653.83 1159.24 5
-v -2653.83 1159.24 495
-v -2657.07 1157.07 495
-v -2653.83 1159.24 5
-v -2653.83 1159.24 5
-v -2657.07 1157.07 495
-v -2657.07 1157.07 5
-v -2657.07 1157.07 495
-v -2659.24 1153.83 495
-v -2657.07 1157.07 5
-v -2657.07 1157.07 5
-v -2659.24 1153.83 495
-v -2659.24 1153.83 5
-v -2659.24 1153.83 495
-v -2660 1150 495
-v -2659.24 1153.83 5
-v -2659.24 1153.83 5
-v -2660 1150 495
-v -2660 1150 5
-v -2360 1150 495
-v -2359.24 1146.17 495
-v -2360 1150 5
-v -2360 1150 5
-v -2359.24 1146.17 495
-v -2359.24 1146.17 5
-v -2359.24 1146.17 495
-v -2357.07 1142.93 495
-v -2359.24 1146.17 5
-v -2359.24 1146.17 5
-v -2357.07 1142.93 495
-v -2357.07 1142.93 5
-v -2357.07 1142.93 495
-v -2353.83 1140.76 495
-v -2357.07 1142.93 5
-v -2357.07 1142.93 5
-v -2353.83 1140.76 495
-v -2353.83 1140.76 5
-v -2353.83 1140.76 495
-v -2350 1140 495
-v -2353.83 1140.76 5
-v -2353.83 1140.76 5
-v -2350 1140 495
-v -2350 1140 5
-v -2350 1140 495
-v -2346.17 1140.76 495
-v -2350 1140 5
-v -2350 1140 5
-v -2346.17 1140.76 495
-v -2346.17 1140.76 5
-v -2346.17 1140.76 495
-v -2342.93 1142.93 495
-v -2346.17 1140.76 5
-v -2346.17 1140.76 5
-v -2342.93 1142.93 495
-v -2342.93 1142.93 5
-v -2342.93 1142.93 495
-v -2340.76 1146.17 495
-v -2342.93 1142.93 5
-v -2342.93 1142.93 5
-v -2340.76 1146.17 495
-v -2340.76 1146.17 5
-v -2340.76 1146.17 495
-v -2340 1150 495
-v -2340.76 1146.17 5
-v -2340.76 1146.17 5
-v -2340 1150 495
-v -2340 1150 5
-v -2340 1150 495
-v -2340.76 1153.83 495
-v -2340 1150 5
-v -2340 1150 5
-v -2340.76 1153.83 495
-v -2340.76 1153.83 5
-v -2340.76 1153.83 495
-v -2342.93 1157.07 495
-v -2340.76 1153.83 5
-v -2340.76 1153.83 5
-v -2342.93 1157.07 495
-v -2342.93 1157.07 5
-v -2342.93 1157.07 495
-v -2346.17 1159.24 495
-v -2342.93 1157.07 5
-v -2342.93 1157.07 5
-v -2346.17 1159.24 495
-v -2346.17 1159.24 5
-v -2346.17 1159.24 495
-v -2350 1160 495
-v -2346.17 1159.24 5
-v -2346.17 1159.24 5
-v -2350 1160 495
-v -2350 1160 5
-v -2350 1160 495
-v -2353.83 1159.24 495
-v -2350 1160 5
-v -2350 1160 5
-v -2353.83 1159.24 495
-v -2353.83 1159.24 5
-v -2353.83 1159.24 495
-v -2357.07 1157.07 495
-v -2353.83 1159.24 5
-v -2353.83 1159.24 5
-v -2357.07 1157.07 495
-v -2357.07 1157.07 5
-v -2357.07 1157.07 495
-v -2359.24 1153.83 495
-v -2357.07 1157.07 5
-v -2357.07 1157.07 5
-v -2359.24 1153.83 495
-v -2359.24 1153.83 5
-v -2359.24 1153.83 495
-v -2360 1150 495
-v -2359.24 1153.83 5
-v -2359.24 1153.83 5
-v -2360 1150 495
-v -2360 1150 5
-v -2360 1450 495
-v -2359.24 1446.17 495
-v -2360 1450 5
-v -2360 1450 5
-v -2359.24 1446.17 495
-v -2359.24 1446.17 5
-v -2359.24 1446.17 495
-v -2357.07 1442.93 495
-v -2359.24 1446.17 5
-v -2359.24 1446.17 5
-v -2357.07 1442.93 495
-v -2357.07 1442.93 5
-v -2357.07 1442.93 495
-v -2353.83 1440.76 495
-v -2357.07 1442.93 5
-v -2357.07 1442.93 5
-v -2353.83 1440.76 495
-v -2353.83 1440.76 5
-v -2353.83 1440.76 495
-v -2350 1440 495
-v -2353.83 1440.76 5
-v -2353.83 1440.76 5
-v -2350 1440 495
-v -2350 1440 5
-v -2350 1440 495
-v -2346.17 1440.76 495
-v -2350 1440 5
-v -2350 1440 5
-v -2346.17 1440.76 495
-v -2346.17 1440.76 5
-v -2346.17 1440.76 495
-v -2342.93 1442.93 495
-v -2346.17 1440.76 5
-v -2346.17 1440.76 5
-v -2342.93 1442.93 495
-v -2342.93 1442.93 5
-v -2342.93 1442.93 495
-v -2340.76 1446.17 495
-v -2342.93 1442.93 5
-v -2342.93 1442.93 5
-v -2340.76 1446.17 495
-v -2340.76 1446.17 5
-v -2340.76 1446.17 495
-v -2340 1450 495
-v -2340.76 1446.17 5
-v -2340.76 1446.17 5
-v -2340 1450 495
-v -2340 1450 5
-v -2340 1450 495
-v -2340.76 1453.83 495
-v -2340 1450 5
-v -2340 1450 5
-v -2340.76 1453.83 495
-v -2340.76 1453.83 5
-v -2340.76 1453.83 495
-v -2342.93 1457.07 495
-v -2340.76 1453.83 5
-v -2340.76 1453.83 5
-v -2342.93 1457.07 495
-v -2342.93 1457.07 5
-v -2342.93 1457.07 495
-v -2346.17 1459.24 495
-v -2342.93 1457.07 5
-v -2342.93 1457.07 5
-v -2346.17 1459.24 495
-v -2346.17 1459.24 5
-v -2346.17 1459.24 495
-v -2350 1460 495
-v -2346.17 1459.24 5
-v -2346.17 1459.24 5
-v -2350 1460 495
-v -2350 1460 5
-v -2350 1460 495
-v -2353.83 1459.24 495
-v -2350 1460 5
-v -2350 1460 5
-v -2353.83 1459.24 495
-v -2353.83 1459.24 5
-v -2353.83 1459.24 495
-v -2357.07 1457.07 495
-v -2353.83 1459.24 5
-v -2353.83 1459.24 5
-v -2357.07 1457.07 495
-v -2357.07 1457.07 5
-v -2357.07 1457.07 495
-v -2359.24 1453.83 495
-v -2357.07 1457.07 5
-v -2357.07 1457.07 5
-v -2359.24 1453.83 495
-v -2359.24 1453.83 5
-v -2359.24 1453.83 495
-v -2360 1450 495
-v -2359.24 1453.83 5
-v -2359.24 1453.83 5
-v -2360 1450 495
-v -2360 1450 5
-v -2660 1450 495
-v -2659.24 1446.17 495
-v -2660 1450 5
-v -2660 1450 5
-v -2659.24 1446.17 495
-v -2659.24 1446.17 5
-v -2659.24 1446.17 495
-v -2657.07 1442.93 495
-v -2659.24 1446.17 5
-v -2659.24 1446.17 5
-v -2657.07 1442.93 495
-v -2657.07 1442.93 5
-v -2657.07 1442.93 495
-v -2653.83 1440.76 495
-v -2657.07 1442.93 5
-v -2657.07 1442.93 5
-v -2653.83 1440.76 495
-v -2653.83 1440.76 5
-v -2653.83 1440.76 495
-v -2650 1440 495
-v -2653.83 1440.76 5
-v -2653.83 1440.76 5
-v -2650 1440 495
-v -2650 1440 5
-v -2650 1440 495
-v -2646.17 1440.76 495
-v -2650 1440 5
-v -2650 1440 5
-v -2646.17 1440.76 495
-v -2646.17 1440.76 5
-v -2646.17 1440.76 495
-v -2642.93 1442.93 495
-v -2646.17 1440.76 5
-v -2646.17 1440.76 5
-v -2642.93 1442.93 495
-v -2642.93 1442.93 5
-v -2642.93 1442.93 495
-v -2640.76 1446.17 495
-v -2642.93 1442.93 5
-v -2642.93 1442.93 5
-v -2640.76 1446.17 495
-v -2640.76 1446.17 5
-v -2640.76 1446.17 495
-v -2640 1450 495
-v -2640.76 1446.17 5
-v -2640.76 1446.17 5
-v -2640 1450 495
-v -2640 1450 5
-v -2640 1450 495
-v -2640.76 1453.83 495
-v -2640 1450 5
-v -2640 1450 5
-v -2640.76 1453.83 495
-v -2640.76 1453.83 5
-v -2640.76 1453.83 495
-v -2642.93 1457.07 495
-v -2640.76 1453.83 5
-v -2640.76 1453.83 5
-v -2642.93 1457.07 495
-v -2642.93 1457.07 5
-v -2642.93 1457.07 495
-v -2646.17 1459.24 495
-v -2642.93 1457.07 5
-v -2642.93 1457.07 5
-v -2646.17 1459.24 495
-v -2646.17 1459.24 5
-v -2646.17 1459.24 495
-v -2650 1460 495
-v -2646.17 1459.24 5
-v -2646.17 1459.24 5
-v -2650 1460 495
-v -2650 1460 5
-v -2650 1460 495
-v -2653.83 1459.24 495
-v -2650 1460 5
-v -2650 1460 5
-v -2653.83 1459.24 495
-v -2653.83 1459.24 5
-v -2653.83 1459.24 495
-v -2657.07 1457.07 495
-v -2653.83 1459.24 5
-v -2653.83 1459.24 5
-v -2657.07 1457.07 495
-v -2657.07 1457.07 5
-v -2657.07 1457.07 495
-v -2659.24 1453.83 495
-v -2657.07 1457.07 5
-v -2657.07 1457.07 5
-v -2659.24 1453.83 495
-v -2659.24 1453.83 5
-v -2659.24 1453.83 495
-v -2660 1450 495
-v -2659.24 1453.83 5
-v -2659.24 1453.83 5
-v -2660 1450 495
-v -2660 1450 5
-v -2300 1500 510
-v -2700 1500 510
-v -2300 1100 510
-v -2300 1100 510
-v -2700 1500 510
-v -2700 1100 510
-v -2700 1500 500
-v -2300 1500 500
-v -2700 1100 500
-v -2700 1100 500
-v -2300 1500 500
-v -2300 1100 500
-v -2700 1500 510
-v -2700 1500 500
-v -2700 1100 510
-v -2700 1100 510
-v -2700 1500 500
-v -2700 1100 500
-v -2300 1500 500
-v -2300 1500 510
-v -2300 1100 500
-v -2300 1100 500
-v -2300 1500 510
-v -2300 1100 510
-v -2300 1100 500
-v -2300 1100 510
-v -2700 1100 500
-v -2700 1100 500
-v -2300 1100 510
-v -2700 1100 510
-v -2300 1500 500
-v -2700 1500 500
-v -2300 1500 510
-v -2300 1500 510
-v -2700 1500 500
-v -2700 1500 510
-v -2560 1095 810
-v -2559.24 1091.17 810
-v -2560 1095 510
-v -2560 1095 510
-v -2559.24 1091.17 810
-v -2559.24 1091.17 510
-v -2559.24 1091.17 810
-v -2557.07 1087.93 810
-v -2559.24 1091.17 510
-v -2559.24 1091.17 510
-v -2557.07 1087.93 810
-v -2557.07 1087.93 510
-v -2557.07 1087.93 810
-v -2553.83 1085.76 810
-v -2557.07 1087.93 510
-v -2557.07 1087.93 510
-v -2553.83 1085.76 810
-v -2553.83 1085.76 510
-v -2553.83 1085.76 810
-v -2550 1085 810
-v -2553.83 1085.76 510
-v -2553.83 1085.76 510
-v -2550 1085 810
-v -2550 1085 510
-v -2550 1085 810
-v -2546.17 1085.76 810
-v -2550 1085 510
-v -2550 1085 510
-v -2546.17 1085.76 810
-v -2546.17 1085.76 510
-v -2546.17 1085.76 810
-v -2542.93 1087.93 810
-v -2546.17 1085.76 510
-v -2546.17 1085.76 510
-v -2542.93 1087.93 810
-v -2542.93 1087.93 510
-v -2542.93 1087.93 810
-v -2540.76 1091.17 810
-v -2542.93 1087.93 510
-v -2542.93 1087.93 510
-v -2540.76 1091.17 810
-v -2540.76 1091.17 510
-v -2540.76 1091.17 810
-v -2540 1095 810
-v -2540.76 1091.17 510
-v -2540.76 1091.17 510
-v -2540 1095 810
-v -2540 1095 510
-v -2540 1095 810
-v -2540.76 1098.83 810
-v -2540 1095 510
-v -2540 1095 510
-v -2540.76 1098.83 810
-v -2540.76 1098.83 510
-v -2540.76 1098.83 810
-v -2542.93 1102.07 810
-v -2540.76 1098.83 510
-v -2540.76 1098.83 510
-v -2542.93 1102.07 810
-v -2542.93 1102.07 510
-v -2542.93 1102.07 810
-v -2546.17 1104.24 810
-v -2542.93 1102.07 510
-v -2542.93 1102.07 510
-v -2546.17 1104.24 810
-v -2546.17 1104.24 510
-v -2546.17 1104.24 810
-v -2550 1105 810
-v -2546.17 1104.24 510
-v -2546.17 1104.24 510
-v -2550 1105 810
-v -2550 1105 510
-v -2550 1105 810
-v -2553.83 1104.24 810
-v -2550 1105 510
-v -2550 1105 510
-v -2553.83 1104.24 810
-v -2553.83 1104.24 510
-v -2553.83 1104.24 810
-v -2557.07 1102.07 810
-v -2553.83 1104.24 510
-v -2553.83 1104.24 510
-v -2557.07 1102.07 810
-v -2557.07 1102.07 510
-v -2557.07 1102.07 810
-v -2559.24 1098.83 810
-v -2557.07 1102.07 510
-v -2557.07 1102.07 510
-v -2559.24 1098.83 810
-v -2559.24 1098.83 510
-v -2559.24 1098.83 810
-v -2560 1095 810
-v -2559.24 1098.83 510
-v -2559.24 1098.83 510
-v -2560 1095 810
-v -2560 1095 510
-v -2460 1095 810
-v -2459.24 1091.17 810
-v -2460 1095 510
-v -2460 1095 510
-v -2459.24 1091.17 810
-v -2459.24 1091.17 510
-v -2459.24 1091.17 810
-v -2457.07 1087.93 810
-v -2459.24 1091.17 510
-v -2459.24 1091.17 510
-v -2457.07 1087.93 810
-v -2457.07 1087.93 510
-v -2457.07 1087.93 810
-v -2453.83 1085.76 810
-v -2457.07 1087.93 510
-v -2457.07 1087.93 510
-v -2453.83 1085.76 810
-v -2453.83 1085.76 510
-v -2453.83 1085.76 810
-v -2450 1085 810
-v -2453.83 1085.76 510
-v -2453.83 1085.76 510
-v -2450 1085 810
-v -2450 1085 510
-v -2450 1085 810
-v -2446.17 1085.76 810
-v -2450 1085 510
-v -2450 1085 510
-v -2446.17 1085.76 810
-v -2446.17 1085.76 510
-v -2446.17 1085.76 810
-v -2442.93 1087.93 810
-v -2446.17 1085.76 510
-v -2446.17 1085.76 510
-v -2442.93 1087.93 810
-v -2442.93 1087.93 510
-v -2442.93 1087.93 810
-v -2440.76 1091.17 810
-v -2442.93 1087.93 510
-v -2442.93 1087.93 510
-v -2440.76 1091.17 810
-v -2440.76 1091.17 510
-v -2440.76 1091.17 810
-v -2440 1095 810
-v -2440.76 1091.17 510
-v -2440.76 1091.17 510
-v -2440 1095 810
-v -2440 1095 510
-v -2440 1095 810
-v -2440.76 1098.83 810
-v -2440 1095 510
-v -2440 1095 510
-v -2440.76 1098.83 810
-v -2440.76 1098.83 510
-v -2440.76 1098.83 810
-v -2442.93 1102.07 810
-v -2440.76 1098.83 510
-v -2440.76 1098.83 510
-v -2442.93 1102.07 810
-v -2442.93 1102.07 510
-v -2442.93 1102.07 810
-v -2446.17 1104.24 810
-v -2442.93 1102.07 510
-v -2442.93 1102.07 510
-v -2446.17 1104.24 810
-v -2446.17 1104.24 510
-v -2446.17 1104.24 810
-v -2450 1105 810
-v -2446.17 1104.24 510
-v -2446.17 1104.24 510
-v -2450 1105 810
-v -2450 1105 510
-v -2450 1105 810
-v -2453.83 1104.24 810
-v -2450 1105 510
-v -2450 1105 510
-v -2453.83 1104.24 810
-v -2453.83 1104.24 510
-v -2453.83 1104.24 810
-v -2457.07 1102.07 810
-v -2453.83 1104.24 510
-v -2453.83 1104.24 510
-v -2457.07 1102.07 810
-v -2457.07 1102.07 510
-v -2457.07 1102.07 810
-v -2459.24 1098.83 810
-v -2457.07 1102.07 510
-v -2457.07 1102.07 510
-v -2459.24 1098.83 810
-v -2459.24 1098.83 510
-v -2459.24 1098.83 810
-v -2460 1095 810
-v -2459.24 1098.83 510
-v -2459.24 1098.83 510
-v -2460 1095 810
-v -2460 1095 510
-v -2300 1110 910
-v -2700 1110 910
-v -2300 1100 910
-v -2300 1100 910
-v -2700 1110 910
-v -2700 1100 910
-v -2700 1110 710
-v -2300 1110 710
-v -2700 1100 710
-v -2700 1100 710
-v -2300 1110 710
-v -2300 1100 710
-v -2700 1110 910
-v -2700 1110 710
-v -2700 1100 910
-v -2700 1100 910
-v -2700 1110 710
-v -2700 1100 710
-v -2300 1110 710
-v -2300 1110 910
-v -2300 1100 710
-v -2300 1100 710
-v -2300 1110 910
-v -2300 1100 910
-v -2300 1100 710
-v -2300 1100 910
-v -2700 1100 710
-v -2700 1100 710
-v -2300 1100 910
-v -2700 1100 910
-v -2300 1110 710
-v -2700 1110 710
-v -2300 1110 910
-v -2300 1110 910
-v -2700 1110 710
-v -2700 1110 910
-f 1 2 3
-f 4 5 6
-f 7 8 9
-f 10 11 12
-f 13 14 15
-f 16 17 18
-f 19 20 21
-f 22 23 24
-f 25 26 27
-f 28 29 30
-f 31 32 33
-f 34 35 36
-f 37 38 39
-f 40 41 42
-f 43 44 45
-f 46 47 48
-f 49 50 51
-f 52 53 54
-f 55 56 57
-f 58 59 60
-f 61 62 63
-f 64 65 66
-f 67 68 69
-f 70 71 72
-f 73 74 75
-f 76 77 78
-f 79 80 81
-f 82 83 84
-f 85 86 87
-f 88 89 90
-f 91 92 93
-f 94 95 96
-f 97 98 99
-f 100 101 102
-f 103 104 105
-f 106 107 108
-f 109 110 111
-f 112 113 114
-f 115 116 117
-f 118 119 120
-f 121 122 123
-f 124 125 126
-f 127 128 129
-f 130 131 132
-f 133 134 135
-f 136 137 138
-f 139 140 141
-f 142 143 144
-f 145 146 147
-f 148 149 150
-f 151 152 153
-f 154 155 156
-f 157 158 159
-f 160 161 162
-f 163 164 165
-f 166 167 168
-f 169 170 171
-f 172 173 174
-f 175 176 177
-f 178 179 180
-f 181 182 183
-f 184 185 186
-f 187 188 189
-f 190 191 192
-f 193 194 195
-f 196 197 198
-f 199 200 201
-f 202 203 204
-f 205 206 207
-f 208 209 210
-f 211 212 213
-f 214 215 216
-f 217 218 219
-f 220 221 222
-f 223 224 225
-f 226 227 228
-f 229 230 231
-f 232 233 234
-f 235 236 237
-f 238 239 240
-f 241 242 243
-f 244 245 246
-f 247 248 249
-f 250 251 252
-f 253 254 255
-f 256 257 258
-f 259 260 261
-f 262 263 264
-f 265 266 267
-f 268 269 270
-f 271 272 273
-f 274 275 276
-f 277 278 279
-f 280 281 282
-f 283 284 285
-f 286 287 288
-f 289 290 291
-f 292 293 294
-f 295 296 297
-f 298 299 300
-f 301 302 303
-f 304 305 306
-f 307 308 309
-f 310 311 312
-f 313 314 315
-f 316 317 318
-f 319 320 321
-f 322 323 324
-f 325 326 327
-f 328 329 330
-f 331 332 333
-f 334 335 336
-f 337 338 339
-f 340 341 342
-f 343 344 345
-f 346 347 348
-f 349 350 351
-f 352 353 354
-f 355 356 357
-f 358 359 360
-f 361 362 363
-f 364 365 366
-f 367 368 369
-f 370 371 372
-f 373 374 375
-f 376 377 378
-f 379 380 381
-f 382 383 384
-f 385 386 387
-f 388 389 390
-f 391 392 393
-f 394 395 396
-f 397 398 399
-f 400 401 402
-f 403 404 405
-f 406 407 408
-f 409 410 411
-f 412 413 414
-f 415 416 417
-f 418 419 420
-f 421 422 423
-f 424 425 426
-f 427 428 429
-f 430 431 432
-f 433 434 435
-f 436 437 438
-f 439 440 441
-f 442 443 444
-f 445 446 447
-f 448 449 450
-f 451 452 453
-f 454 455 456
-f 457 458 459
-f 460 461 462
-f 463 464 465
-f 466 467 468
-f 469 470 471
-f 472 473 474
-f 475 476 477
-f 478 479 480
-f 481 482 483
-f 484 485 486
-f 487 488 489
-f 490 491 492
-f 493 494 495
-f 496 497 498
-f 499 500 501
-f 502 503 504
-f 505 506 507
-f 508 509 510
-f 511 512 513
-f 514 515 516
-f 517 518 519
-f 520 521 522
-f 523 524 525
-f 526 527 528
-f 529 530 531
-f 532 533 534
-f 535 536 537
-f 538 539 540
-f 541 542 543
-f 544 545 546
-f 547 548 549
-f 550 551 552
-f 553 554 555
-f 556 557 558
-f 559 560 561
-f 562 563 564
-f 565 566 567
-f 568 569 570
-f 571 572 573
-f 574 575 576
-f 577 578 579
-f 580 581 582
-f 583 584 585
-f 586 587 588
-f 589 590 591
-f 592 593 594
-f 595 596 597
-f 598 599 600
-f 601 602 603
-f 604 605 606
-f 607 608 609
-f 610 611 612
-f 613 614 615
-f 616 617 618
-f 619 620 621
-f 622 623 624
-f 625 626 627
-f 628 629 630
-f 631 632 633
-f 634 635 636
-f 637 638 639
-f 640 641 642
-f 643 644 645
-f 646 647 648
-f 649 650 651
-f 652 653 654
-f 655 656 657
-f 658 659 660
-f 661 662 663
-f 664 665 666
-f 667 668 669
-f 670 671 672
-f 673 674 675
-f 676 677 678
-f 679 680 681
-f 682 683 684
-f 685 686 687
-f 688 689 690
-f 691 692 693
-f 694 695 696
-f 697 698 699
-f 700 701 702
-f 703 704 705
-f 706 707 708
-f 709 710 711
-f 712 713 714
-f 715 716 717
-f 718 719 720
-f 721 722 723
-f 724 725 726
-f 727 728 729
-f 730 731 732
-f 733 734 735
-f 736 737 738
-f 739 740 741
-f 742 743 744
-f 745 746 747
-f 748 749 750
-f 751 752 753
-f 754 755 756
-f 757 758 759
-f 760 761 762
-f 763 764 765
-f 766 767 768
-f 769 770 771
-f 772 773 774
-f 775 776 777
-f 778 779 780
-f 781 782 783
-f 784 785 786
-f 787 788 789
-f 790 791 792
-f 793 794 795
-f 796 797 798
-f 799 800 801
-f 802 803 804
-f 805 806 807
-f 808 809 810
-f 811 812 813
-f 814 815 816
-f 817 818 819
-f 820 821 822
-f 823 824 825
-f 826 827 828
-f 829 830 831
-f 832 833 834
-f 835 836 837
-f 838 839 840
-f 841 842 843
-f 844 845 846
-f 847 848 849
-f 850 851 852
-f 853 854 855
-f 856 857 858
-f 859 860 861
-f 862 863 864
-f 865 866 867
-f 868 869 870
-f 871 872 873
-f 874 875 876
-f 877 878 879
-f 880 881 882
-f 883 884 885
-f 886 887 888
-f 889 890 891
-f 892 893 894
-f 895 896 897
-f 898 899 900
-f 901 902 903
-f 904 905 906
-f 907 908 909
-f 910 911 912
-f 913 914 915
-f 916 917 918
-f 919 920 921
-f 922 923 924
-f 925 926 927
-f 928 929 930
-f 931 932 933
-f 934 935 936
-f 937 938 939
-f 940 941 942
-f 943 944 945
-f 946 947 948
-f 949 950 951
-f 952 953 954
-f 955 956 957
-f 958 959 960
-f 961 962 963
-f 964 965 966
-f 967 968 969
-f 970 971 972
-f 973 974 975
-f 976 977 978
-f 979 980 981
-f 982 983 984
-f 985 986 987
-f 988 989 990
-f 991 992 993
-f 994 995 996
-f 997 998 999
-f 1000 1001 1002
-f 1003 1004 1005
-f 1006 1007 1008
-f 1009 1010 1011
-f 1012 1013 1014
-f 1015 1016 1017
-f 1018 1019 1020
-f 1021 1022 1023
-f 1024 1025 1026
-f 1027 1028 1029
-f 1030 1031 1032
-f 1033 1034 1035
-f 1036 1037 1038
-f 1039 1040 1041
-f 1042 1043 1044
-f 1045 1046 1047
-f 1048 1049 1050
-f 1051 1052 1053
-f 1054 1055 1056
-f 1057 1058 1059
-f 1060 1061 1062
-f 1063 1064 1065
-f 1066 1067 1068
-f 1069 1070 1071
-f 1072 1073 1074
-f 1075 1076 1077
-f 1078 1079 1080
-f 1081 1082 1083
-f 1084 1085 1086
-f 1087 1088 1089
-f 1090 1091 1092
-f 1093 1094 1095
-f 1096 1097 1098
-f 1099 1100 1101
-f 1102 1103 1104
-f 1105 1106 1107
-f 1108 1109 1110
-f 1111 1112 1113
-f 1114 1115 1116
-f 1117 1118 1119
-f 1120 1121 1122
-f 1123 1124 1125
-f 1126 1127 1128
-f 1129 1130 1131
-f 1132 1133 1134
-f 1135 1136 1137
-f 1138 1139 1140
-f 1141 1142 1143
-f 1144 1145 1146
-f 1147 1148 1149
-f 1150 1151 1152
-f 1153 1154 1155
-f 1156 1157 1158
-f 1159 1160 1161
-f 1162 1163 1164
-f 1165 1166 1167
-f 1168 1169 1170
-f 1171 1172 1173
-f 1174 1175 1176
-f 1177 1178 1179
-f 1180 1181 1182
-f 1183 1184 1185
-f 1186 1187 1188
-f 1189 1190 1191
-f 1192 1193 1194
-f 1195 1196 1197
-f 1198 1199 1200
-f 1201 1202 1203
-f 1204 1205 1206
-f 1207 1208 1209
-f 1210 1211 1212
-f 1213 1214 1215
-f 1216 1217 1218
-f 1219 1220 1221
-f 1222 1223 1224
-f 1225 1226 1227
-f 1228 1229 1230
-f 1231 1232 1233
-f 1234 1235 1236
-f 1237 1238 1239
-f 1240 1241 1242
-f 1243 1244 1245
-f 1246 1247 1248
-f 1249 1250 1251
-f 1252 1253 1254
-f 1255 1256 1257
-f 1258 1259 1260
-f 1261 1262 1263
-f 1264 1265 1266
-f 1267 1268 1269
-f 1270 1271 1272
-f 1273 1274 1275
-f 1276 1277 1278
-f 1279 1280 1281
-f 1282 1283 1284
-f 1285 1286 1287
-f 1288 1289 1290
-f 1291 1292 1293
-f 1294 1295 1296
-f 1297 1298 1299
-f 1300 1301 1302
-f 1303 1304 1305
-f 1306 1307 1308
-f 1309 1310 1311
-f 1312 1313 1314
-f 1315 1316 1317
-f 1318 1319 1320
-f 1321 1322 1323
-f 1324 1325 1326
-f 1327 1328 1329
-f 1330 1331 1332
-f 1333 1334 1335
-f 1336 1337 1338
-f 1339 1340 1341
-f 1342 1343 1344
-f 1345 1346 1347
-f 1348 1349 1350
-f 1351 1352 1353
-f 1354 1355 1356
-f 1357 1358 1359
-f 1360 1361 1362
-f 1363 1364 1365
-f 1366 1367 1368
-f 1369 1370 1371
-f 1372 1373 1374
-f 1375 1376 1377
-f 1378 1379 1380
-f 1381 1382 1383
-f 1384 1385 1386
-f 1387 1388 1389
-f 1390 1391 1392
-f 1393 1394 1395
-f 1396 1397 1398
-f 1399 1400 1401
-f 1402 1403 1404
-f 1405 1406 1407
-f 1408 1409 1410
-f 1411 1412 1413
-f 1414 1415 1416
-f 1417 1418 1419
-f 1420 1421 1422
-f 1423 1424 1425
-f 1426 1427 1428
-f 1429 1430 1431
-f 1432 1433 1434
-f 1435 1436 1437
-f 1438 1439 1440
-f 1441 1442 1443
-f 1444 1445 1446
-f 1447 1448 1449
-f 1450 1451 1452
-f 1453 1454 1455
-f 1456 1457 1458
-f 1459 1460 1461
-f 1462 1463 1464
-f 1465 1466 1467
-f 1468 1469 1470
-f 1471 1472 1473
-f 1474 1475 1476
-f 1477 1478 1479
-f 1480 1481 1482
-f 1483 1484 1485
-f 1486 1487 1488
-f 1489 1490 1491
-f 1492 1493 1494
-f 1495 1496 1497
-f 1498 1499 1500
-f 1501 1502 1503
-f 1504 1505 1506
-f 1507 1508 1509
-f 1510 1511 1512
-f 1513 1514 1515
-f 1516 1517 1518
-f 1519 1520 1521
-f 1522 1523 1524
-f 1525 1526 1527
-f 1528 1529 1530
-f 1531 1532 1533
-f 1534 1535 1536
-f 1537 1538 1539
-f 1540 1541 1542
-f 1543 1544 1545
-f 1546 1547 1548
-f 1549 1550 1551
-f 1552 1553 1554
-f 1555 1556 1557
-f 1558 1559 1560
-f 1561 1562 1563
-f 1564 1565 1566
-f 1567 1568 1569
-f 1570 1571 1572
-f 1573 1574 1575
-f 1576 1577 1578
-f 1579 1580 1581
-f 1582 1583 1584
-f 1585 1586 1587
-f 1588 1589 1590
-f 1591 1592 1593
-f 1594 1595 1596
-f 1597 1598 1599
-f 1600 1601 1602
-f 1603 1604 1605
-f 1606 1607 1608
-f 1609 1610 1611
-f 1612 1613 1614
-f 1615 1616 1617
-f 1618 1619 1620
-f 1621 1622 1623
-f 1624 1625 1626
-f 1627 1628 1629
-f 1630 1631 1632
-f 1633 1634 1635
-f 1636 1637 1638
-f 1639 1640 1641
-f 1642 1643 1644
-f 1645 1646 1647
-f 1648 1649 1650
-f 1651 1652 1653
-f 1654 1655 1656
-f 1657 1658 1659
-f 1660 1661 1662
-f 1663 1664 1665
-f 1666 1667 1668
-f 1669 1670 1671
-f 1672 1673 1674
-f 1675 1676 1677
-f 1678 1679 1680
-f 1681 1682 1683
-f 1684 1685 1686
-f 1687 1688 1689
-f 1690 1691 1692
-f 1693 1694 1695
-f 1696 1697 1698
-f 1699 1700 1701
-f 1702 1703 1704
-f 1705 1706 1707
-f 1708 1709 1710
-f 1711 1712 1713
-f 1714 1715 1716
-f 1717 1718 1719
-f 1720 1721 1722
-f 1723 1724 1725
-f 1726 1727 1728
-f 1729 1730 1731
-f 1732 1733 1734
-f 1735 1736 1737
-f 1738 1739 1740
-f 1741 1742 1743
-f 1744 1745 1746
-f 1747 1748 1749
-f 1750 1751 1752
-f 1753 1754 1755
-f 1756 1757 1758
-f 1759 1760 1761
-f 1762 1763 1764
-f 1765 1766 1767
-f 1768 1769 1770
-f 1771 1772 1773
-f 1774 1775 1776
-f 1777 1778 1779
-f 1780 1781 1782
-f 1783 1784 1785
-f 1786 1787 1788
-f 1789 1790 1791
-f 1792 1793 1794
-f 1795 1796 1797
-f 1798 1799 1800
-f 1801 1802 1803
-f 1804 1805 1806
-f 1807 1808 1809
-f 1810 1811 1812
-f 1813 1814 1815
-f 1816 1817 1818
-f 1819 1820 1821
-f 1822 1823 1824
-f 1825 1826 1827
-f 1828 1829 1830
-f 1831 1832 1833
-f 1834 1835 1836
-f 1837 1838 1839
-f 1840 1841 1842
-f 1843 1844 1845
-f 1846 1847 1848
-f 1849 1850 1851
-f 1852 1853 1854
-f 1855 1856 1857
-f 1858 1859 1860
-f 1861 1862 1863
-f 1864 1865 1866
-f 1867 1868 1869
-f 1870 1871 1872
-f 1873 1874 1875
-f 1876 1877 1878
-f 1879 1880 1881
-f 1882 1883 1884
-f 1885 1886 1887
-f 1888 1889 1890
-f 1891 1892 1893
-f 1894 1895 1896
-f 1897 1898 1899
-f 1900 1901 1902
-f 1903 1904 1905
-f 1906 1907 1908
-f 1909 1910 1911
-f 1912 1913 1914
-f 1915 1916 1917
-f 1918 1919 1920
-f 1921 1922 1923
-f 1924 1925 1926
-f 1927 1928 1929
-f 1930 1931 1932
-f 1933 1934 1935
-f 1936 1937 1938
-f 1939 1940 1941
-f 1942 1943 1944
-f 1945 1946 1947
-f 1948 1949 1950
-f 1951 1952 1953
-f 1954 1955 1956
-f 1957 1958 1959
-f 1960 1961 1962
-f 1963 1964 1965
-f 1966 1967 1968
-f 1969 1970 1971
-f 1972 1973 1974
-f 1975 1976 1977
-f 1978 1979 1980
-f 1981 1982 1983
-f 1984 1985 1986
-f 1987 1988 1989
-f 1990 1991 1992
-f 1993 1994 1995
-f 1996 1997 1998
-f 1999 2000 2001
-f 2002 2003 2004
-f 2005 2006 2007
-f 2008 2009 2010
-f 2011 2012 2013
-f 2014 2015 2016
-f 2017 2018 2019
-f 2020 2021 2022
-f 2023 2024 2025
-f 2026 2027 2028
-f 2029 2030 2031
-f 2032 2033 2034
-f 2035 2036 2037
-f 2038 2039 2040
-f 2041 2042 2043
-f 2044 2045 2046
-f 2047 2048 2049
-f 2050 2051 2052
-f 2053 2054 2055
-f 2056 2057 2058
-f 2059 2060 2061
-f 2062 2063 2064
-f 2065 2066 2067
-f 2068 2069 2070
-f 2071 2072 2073
-f 2074 2075 2076
-f 2077 2078 2079
-f 2080 2081 2082
-f 2083 2084 2085
-f 2086 2087 2088
-f 2089 2090 2091
-f 2092 2093 2094
-f 2095 2096 2097
-f 2098 2099 2100
-f 2101 2102 2103
-f 2104 2105 2106
-f 2107 2108 2109
-f 2110 2111 2112
-f 2113 2114 2115
-f 2116 2117 2118
-f 2119 2120 2121
-f 2122 2123 2124
-f 2125 2126 2127
-f 2128 2129 2130
-f 2131 2132 2133
-f 2134 2135 2136
-f 2137 2138 2139
-f 2140 2141 2142
-f 2143 2144 2145
-f 2146 2147 2148
-f 2149 2150 2151
-f 2152 2153 2154
-f 2155 2156 2157
-f 2158 2159 2160
-f 2161 2162 2163
-f 2164 2165 2166
-f 2167 2168 2169
-f 2170 2171 2172
-f 2173 2174 2175
-f 2176 2177 2178
-f 2179 2180 2181
-f 2182 2183 2184
-f 2185 2186 2187
-f 2188 2189 2190
-f 2191 2192 2193
-f 2194 2195 2196
-f 2197 2198 2199
-f 2200 2201 2202
-f 2203 2204 2205
-f 2206 2207 2208
-f 2209 2210 2211
-f 2212 2213 2214
-f 2215 2216 2217
-f 2218 2219 2220
-f 2221 2222 2223
-f 2224 2225 2226
-f 2227 2228 2229
-f 2230 2231 2232
-f 2233 2234 2235
-f 2236 2237 2238
-f 2239 2240 2241
-f 2242 2243 2244
-f 2245 2246 2247
-f 2248 2249 2250
-f 2251 2252 2253
-f 2254 2255 2256
-f 2257 2258 2259
-f 2260 2261 2262
-f 2263 2264 2265
-f 2266 2267 2268
-f 2269 2270 2271
-f 2272 2273 2274
-f 2275 2276 2277
-f 2278 2279 2280
-f 2281 2282 2283
-f 2284 2285 2286
-f 2287 2288 2289
-f 2290 2291 2292
-f 2293 2294 2295
-f 2296 2297 2298
-f 2299 2300 2301
-f 2302 2303 2304
-f 2305 2306 2307
-f 2308 2309 2310
-f 2311 2312 2313
-f 2314 2315 2316
-f 2317 2318 2319
-f 2320 2321 2322
-f 2323 2324 2325
-f 2326 2327 2328
-f 2329 2330 2331
-f 2332 2333 2334
-f 2335 2336 2337
-f 2338 2339 2340
-f 2341 2342 2343
-f 2344 2345 2346
-f 2347 2348 2349
-f 2350 2351 2352
-f 2353 2354 2355
-f 2356 2357 2358
-f 2359 2360 2361
-f 2362 2363 2364
-f 2365 2366 2367
-f 2368 2369 2370
-f 2371 2372 2373
-f 2374 2375 2376
-f 2377 2378 2379
-f 2380 2381 2382
-f 2383 2384 2385
-f 2386 2387 2388
-f 2389 2390 2391
-f 2392 2393 2394
-f 2395 2396 2397
-f 2398 2399 2400
-f 2401 2402 2403
-f 2404 2405 2406
-f 2407 2408 2409
-f 2410 2411 2412
-f 2413 2414 2415
-f 2416 2417 2418
-f 2419 2420 2421
-f 2422 2423 2424
-f 2425 2426 2427
-f 2428 2429 2430
-f 2431 2432 2433
-f 2434 2435 2436
-f 2437 2438 2439
-f 2440 2441 2442
-f 2443 2444 2445
-f 2446 2447 2448
-f 2449 2450 2451
-f 2452 2453 2454
-f 2455 2456 2457
-f 2458 2459 2460
-f 2461 2462 2463
-f 2464 2465 2466
-f 2467 2468 2469
-f 2470 2471 2472
-f 2473 2474 2475
-f 2476 2477 2478
-f 2479 2480 2481
-f 2482 2483 2484
-f 2485 2486 2487
-f 2488 2489 2490
-f 2491 2492 2493
-f 2494 2495 2496
-f 2497 2498 2499
-f 2500 2501 2502
-f 2503 2504 2505
-f 2506 2507 2508
-f 2509 2510 2511
-f 2512 2513 2514
-f 2515 2516 2517
-f 2518 2519 2520
-f 2521 2522 2523
-f 2524 2525 2526
-f 2527 2528 2529
-f 2530 2531 2532
-f 2533 2534 2535
-f 2536 2537 2538
-f 2539 2540 2541
-f 2542 2543 2544
-f 2545 2546 2547
-f 2548 2549 2550
-f 2551 2552 2553
-f 2554 2555 2556
-f 2557 2558 2559
-f 2560 2561 2562
-f 2563 2564 2565
-f 2566 2567 2568
-f 2569 2570 2571
-f 2572 2573 2574
-f 2575 2576 2577
-f 2578 2579 2580
-f 2581 2582 2583
-f 2584 2585 2586
-f 2587 2588 2589
-f 2590 2591 2592
-f 2593 2594 2595
-f 2596 2597 2598
-f 2599 2600 2601
-f 2602 2603 2604
-f 2605 2606 2607
-f 2608 2609 2610
-f 2611 2612 2613
-f 2614 2615 2616
-f 2617 2618 2619
-f 2620 2621 2622
-f 2623 2624 2625
-f 2626 2627 2628
-f 2629 2630 2631
-f 2632 2633 2634
-f 2635 2636 2637
-f 2638 2639 2640
-f 2641 2642 2643
-f 2644 2645 2646
-f 2647 2648 2649
-f 2650 2651 2652
-f 2653 2654 2655
-f 2656 2657 2658
-f 2659 2660 2661
-f 2662 2663 2664
-f 2665 2666 2667
-f 2668 2669 2670
-f 2671 2672 2673
-f 2674 2675 2676
-f 2677 2678 2679
-f 2680 2681 2682
-f 2683 2684 2685
-f 2686 2687 2688
-f 2689 2690 2691
-f 2692 2693 2694
-f 2695 2696 2697
-f 2698 2699 2700
-f 2701 2702 2703
-f 2704 2705 2706
-f 2707 2708 2709
-f 2710 2711 2712
-f 2713 2714 2715
-f 2716 2717 2718
-f 2719 2720 2721
-f 2722 2723 2724
-f 2725 2726 2727
-f 2728 2729 2730
-f 2731 2732 2733
-f 2734 2735 2736
-f 2737 2738 2739
-f 2740 2741 2742
-f 2743 2744 2745
-f 2746 2747 2748
-f 2749 2750 2751
-f 2752 2753 2754
-f 2755 2756 2757
-f 2758 2759 2760
-f 2761 2762 2763
-f 2764 2765 2766
-f 2767 2768 2769
-f 2770 2771 2772
-f 2773 2774 2775
-f 2776 2777 2778
-f 2779 2780 2781
-f 2782 2783 2784
-f 2785 2786 2787
-f 2788 2789 2790
-f 2791 2792 2793
-f 2794 2795 2796
-f 2797 2798 2799
-f 2800 2801 2802
-f 2803 2804 2805
-f 2806 2807 2808
-f 2809 2810 2811
-f 2812 2813 2814
-f 2815 2816 2817
-f 2818 2819 2820
-f 2821 2822 2823
-f 2824 2825 2826
-f 2827 2828 2829
-f 2830 2831 2832
-f 2833 2834 2835
-f 2836 2837 2838
-f 2839 2840 2841
-f 2842 2843 2844
-f 2845 2846 2847
-f 2848 2849 2850
-f 2851 2852 2853
-f 2854 2855 2856
-f 2857 2858 2859
-f 2860 2861 2862
-f 2863 2864 2865
-f 2866 2867 2868
-f 2869 2870 2871
-f 2872 2873 2874
-f 2875 2876 2877
-f 2878 2879 2880
-f 2881 2882 2883
-f 2884 2885 2886
-f 2887 2888 2889
-f 2890 2891 2892
-f 2893 2894 2895
-f 2896 2897 2898
-f 2899 2900 2901
-f 2902 2903 2904
-f 2905 2906 2907
-f 2908 2909 2910
-f 2911 2912 2913
-f 2914 2915 2916
-f 2917 2918 2919
-f 2920 2921 2922
-f 2923 2924 2925
-f 2926 2927 2928
-f 2929 2930 2931
-f 2932 2933 2934
-f 2935 2936 2937
-f 2938 2939 2940
-f 2941 2942 2943
-f 2944 2945 2946
-f 2947 2948 2949
-f 2950 2951 2952
-f 2953 2954 2955
-f 2956 2957 2958
-f 2959 2960 2961
-f 2962 2963 2964
-f 2965 2966 2967
-f 2968 2969 2970
-f 2971 2972 2973
-f 2974 2975 2976
-f 2977 2978 2979
-f 2980 2981 2982
-f 2983 2984 2985
-f 2986 2987 2988
-f 2989 2990 2991
-f 2992 2993 2994
-f 2995 2996 2997
-f 2998 2999 3000
-f 3001 3002 3003
-f 3004 3005 3006
-f 3007 3008 3009
-f 3010 3011 3012
-f 3013 3014 3015
-f 3016 3017 3018
-f 3019 3020 3021
-f 3022 3023 3024
-f 3025 3026 3027
-f 3028 3029 3030
-f 3031 3032 3033
-f 3034 3035 3036
-f 3037 3038 3039
-f 3040 3041 3042
-f 3043 3044 3045
-f 3046 3047 3048
-f 3049 3050 3051
-f 3052 3053 3054
-f 3055 3056 3057
-f 3058 3059 3060
-f 3061 3062 3063
-f 3064 3065 3066
-f 3067 3068 3069
-f 3070 3071 3072
-f 3073 3074 3075
-f 3076 3077 3078
-f 3079 3080 3081
-f 3082 3083 3084
-f 3085 3086 3087
-f 3088 3089 3090
-f 3091 3092 3093
-f 3094 3095 3096
-f 3097 3098 3099
-f 3100 3101 3102
-f 3103 3104 3105
-f 3106 3107 3108
-f 3109 3110 3111
-f 3112 3113 3114
-f 3115 3116 3117
-f 3118 3119 3120
-f 3121 3122 3123
-f 3124 3125 3126
-f 3127 3128 3129
-f 3130 3131 3132
-f 3133 3134 3135
-f 3136 3137 3138
-f 3139 3140 3141
-f 3142 3143 3144
-f 3145 3146 3147
-f 3148 3149 3150
-f 3151 3152 3153
-f 3154 3155 3156
-f 3157 3158 3159
-f 3160 3161 3162
-f 3163 3164 3165
-f 3166 3167 3168
-f 3169 3170 3171
-f 3172 3173 3174
-f 3175 3176 3177
-f 3178 3179 3180
-f 3181 3182 3183
-f 3184 3185 3186
-f 3187 3188 3189
-f 3190 3191 3192
-f 3193 3194 3195
-f 3196 3197 3198
-f 3199 3200 3201
-f 3202 3203 3204
-f 3205 3206 3207
-f 3208 3209 3210
-f 3211 3212 3213
-f 3214 3215 3216
-f 3217 3218 3219
-f 3220 3221 3222
-f 3223 3224 3225
-f 3226 3227 3228
-f 3229 3230 3231
-f 3232 3233 3234
-f 3235 3236 3237
-f 3238 3239 3240
-f 3241 3242 3243
-f 3244 3245 3246
-f 3247 3248 3249
-f 3250 3251 3252
-f 3253 3254 3255
-f 3256 3257 3258
-f 3259 3260 3261
-f 3262 3263 3264
-f 3265 3266 3267
-f 3268 3269 3270
-f 3271 3272 3273
-f 3274 3275 3276
-f 3277 3278 3279
-f 3280 3281 3282
-f 3283 3284 3285
-f 3286 3287 3288
-f 3289 3290 3291
-f 3292 3293 3294
-f 3295 3296 3297
-f 3298 3299 3300
-f 3301 3302 3303
-f 3304 3305 3306
-f 3307 3308 3309
-f 3310 3311 3312
-f 3313 3314 3315
-f 3316 3317 3318
-f 3319 3320 3321
-f 3322 3323 3324
-f 3325 3326 3327
-f 3328 3329 3330
-f 3331 3332 3333
-f 3334 3335 3336
-f 3337 3338 3339
-f 3340 3341 3342
-f 3343 3344 3345
-f 3346 3347 3348
-f 3349 3350 3351
-f 3352 3353 3354
-f 3355 3356 3357
-f 3358 3359 3360
-f 3361 3362 3363
-f 3364 3365 3366
-f 3367 3368 3369
-f 3370 3371 3372
-f 3373 3374 3375
-f 3376 3377 3378
-f 3379 3380 3381
-f 3382 3383 3384
-f 3385 3386 3387
-f 3388 3389 3390
-f 3391 3392 3393
-f 3394 3395 3396
-f 3397 3398 3399
-f 3400 3401 3402
-f 3403 3404 3405
-f 3406 3407 3408
-f 3409 3410 3411
-f 3412 3413 3414
-f 3415 3416 3417
-f 3418 3419 3420
-f 3421 3422 3423
-f 3424 3425 3426
-f 3427 3428 3429
-f 3430 3431 3432
-f 3433 3434 3435
-f 3436 3437 3438
-f 3439 3440 3441
-f 3442 3443 3444
-f 3445 3446 3447
-f 3448 3449 3450
-f 3451 3452 3453
-f 3454 3455 3456
-f 3457 3458 3459
-f 3460 3461 3462
-f 3463 3464 3465
-f 3466 3467 3468
-f 3469 3470 3471
-f 3472 3473 3474
-f 3475 3476 3477
-f 3478 3479 3480
-f 3481 3482 3483
-f 3484 3485 3486
-f 3487 3488 3489
-f 3490 3491 3492
-f 3493 3494 3495
-f 3496 3497 3498
-f 3499 3500 3501
-f 3502 3503 3504
-f 3505 3506 3507
-f 3508 3509 3510
-f 3511 3512 3513
-f 3514 3515 3516
-f 3517 3518 3519
-f 3520 3521 3522
-f 3523 3524 3525
-f 3526 3527 3528
-f 3529 3530 3531
-f 3532 3533 3534
-f 3535 3536 3537
-f 3538 3539 3540
-f 3541 3542 3543
-f 3544 3545 3546
-f 3547 3548 3549
-f 3550 3551 3552
-f 3553 3554 3555
-f 3556 3557 3558
-f 3559 3560 3561
-f 3562 3563 3564
-f 3565 3566 3567
-f 3568 3569 3570
-f 3571 3572 3573
-f 3574 3575 3576
-f 3577 3578 3579
-f 3580 3581 3582
-f 3583 3584 3585
-f 3586 3587 3588
-f 3589 3590 3591
-f 3592 3593 3594
-f 3595 3596 3597
-f 3598 3599 3600
-f 3601 3602 3603
-f 3604 3605 3606
-f 3607 3608 3609
-f 3610 3611 3612
-f 3613 3614 3615
-f 3616 3617 3618
-f 3619 3620 3621
-f 3622 3623 3624
-f 3625 3626 3627
-f 3628 3629 3630
-f 3631 3632 3633
-f 3634 3635 3636
-f 3637 3638 3639
-f 3640 3641 3642
-f 3643 3644 3645
-f 3646 3647 3648
-f 3649 3650 3651
-f 3652 3653 3654
-f 3655 3656 3657
-f 3658 3659 3660
-f 3661 3662 3663
-f 3664 3665 3666
-f 3667 3668 3669
-f 3670 3671 3672
-f 3673 3674 3675
-f 3676 3677 3678
-f 3679 3680 3681
-f 3682 3683 3684
-f 3685 3686 3687
-f 3688 3689 3690
-f 3691 3692 3693
-f 3694 3695 3696
-f 3697 3698 3699
-f 3700 3701 3702
-f 3703 3704 3705
-f 3706 3707 3708
-f 3709 3710 3711
-f 3712 3713 3714
-f 3715 3716 3717
-f 3718 3719 3720
-f 3721 3722 3723
-f 3724 3725 3726
-f 3727 3728 3729
-f 3730 3731 3732
-f 3733 3734 3735
-f 3736 3737 3738
-f 3739 3740 3741
-f 3742 3743 3744
-f 3745 3746 3747
-f 3748 3749 3750
-f 3751 3752 3753
-f 3754 3755 3756
-f 3757 3758 3759
-f 3760 3761 3762
-f 3763 3764 3765
-f 3766 3767 3768
-f 3769 3770 3771
-f 3772 3773 3774
-f 3775 3776 3777
-f 3778 3779 3780
-f 3781 3782 3783
-f 3784 3785 3786
-f 3787 3788 3789
-f 3790 3791 3792
-f 3793 3794 3795
-f 3796 3797 3798
-f 3799 3800 3801
-f 3802 3803 3804
-f 3805 3806 3807
-f 3808 3809 3810
-f 3811 3812 3813
-f 3814 3815 3816
-f 3817 3818 3819
-f 3820 3821 3822
-f 3823 3824 3825
-f 3826 3827 3828
-f 3829 3830 3831
-f 3832 3833 3834
-f 3835 3836 3837
-f 3838 3839 3840
-f 3841 3842 3843
-f 3844 3845 3846
-f 3847 3848 3849
-f 3850 3851 3852
-f 3853 3854 3855
-f 3856 3857 3858
-f 3859 3860 3861
-f 3862 3863 3864
-f 3865 3866 3867
-f 3868 3869 3870
-f 3871 3872 3873
-f 3874 3875 3876
-f 3877 3878 3879
-f 3880 3881 3882
-f 3883 3884 3885
-f 3886 3887 3888
-f 3889 3890 3891
-f 3892 3893 3894
-f 3895 3896 3897
-f 3898 3899 3900
-f 3901 3902 3903
-f 3904 3905 3906
-f 3907 3908 3909
-f 3910 3911 3912
-f 3913 3914 3915
-f 3916 3917 3918
-f 3919 3920 3921
-f 3922 3923 3924
-f 3925 3926 3927
-f 3928 3929 3930
-f 3931 3932 3933
-f 3934 3935 3936
-f 3937 3938 3939
-f 3940 3941 3942
-f 3943 3944 3945
-f 3946 3947 3948
-f 3949 3950 3951
-f 3952 3953 3954
-f 3955 3956 3957
-f 3958 3959 3960
-f 3961 3962 3963
-f 3964 3965 3966
-f 3967 3968 3969
-f 3970 3971 3972
-f 3973 3974 3975
-f 3976 3977 3978
-f 3979 3980 3981
-f 3982 3983 3984
-f 3985 3986 3987
-f 3988 3989 3990
-f 3991 3992 3993
-f 3994 3995 3996
-f 3997 3998 3999
-f 4000 4001 4002
-f 4003 4004 4005
-f 4006 4007 4008
-f 4009 4010 4011
-f 4012 4013 4014
-f 4015 4016 4017
-f 4018 4019 4020
-f 4021 4022 4023
-f 4024 4025 4026
-f 4027 4028 4029
-f 4030 4031 4032
-f 4033 4034 4035
-f 4036 4037 4038
-f 4039 4040 4041
-f 4042 4043 4044
-f 4045 4046 4047
-f 4048 4049 4050
-f 4051 4052 4053
-f 4054 4055 4056
-f 4057 4058 4059
-f 4060 4061 4062
-f 4063 4064 4065
-f 4066 4067 4068
-f 4069 4070 4071
-f 4072 4073 4074
-f 4075 4076 4077
-f 4078 4079 4080
-f 4081 4082 4083
-f 4084 4085 4086
-f 4087 4088 4089
-f 4090 4091 4092
-f 4093 4094 4095
-f 4096 4097 4098
-f 4099 4100 4101
-f 4102 4103 4104
-f 4105 4106 4107
-f 4108 4109 4110
-f 4111 4112 4113
-f 4114 4115 4116
-f 4117 4118 4119
-f 4120 4121 4122
-f 4123 4124 4125
-f 4126 4127 4128
-f 4129 4130 4131
-f 4132 4133 4134
-f 4135 4136 4137
-f 4138 4139 4140
-f 4141 4142 4143
-f 4144 4145 4146
-f 4147 4148 4149
-f 4150 4151 4152
-f 4153 4154 4155
-f 4156 4157 4158
-f 4159 4160 4161
-f 4162 4163 4164
-f 4165 4166 4167
-f 4168 4169 4170
-f 4171 4172 4173
-f 4174 4175 4176
-f 4177 4178 4179
-f 4180 4181 4182
-f 4183 4184 4185
-f 4186 4187 4188
-f 4189 4190 4191
-f 4192 4193 4194
-f 4195 4196 4197
-f 4198 4199 4200
-f 4201 4202 4203
-f 4204 4205 4206
-f 4207 4208 4209
-f 4210 4211 4212
-f 4213 4214 4215
-f 4216 4217 4218
-f 4219 4220 4221
-f 4222 4223 4224
-f 4225 4226 4227
-f 4228 4229 4230
-f 4231 4232 4233
-f 4234 4235 4236
-f 4237 4238 4239
-f 4240 4241 4242
-f 4243 4244 4245
-f 4246 4247 4248
-f 4249 4250 4251
-f 4252 4253 4254
-f 4255 4256 4257
-f 4258 4259 4260
-f 4261 4262 4263
-f 4264 4265 4266
-f 4267 4268 4269
-f 4270 4271 4272
-f 4273 4274 4275
-f 4276 4277 4278
-f 4279 4280 4281
-f 4282 4283 4284
-f 4285 4286 4287
-f 4288 4289 4290
-f 4291 4292 4293
-f 4294 4295 4296
-f 4297 4298 4299
-f 4300 4301 4302
-f 4303 4304 4305
-f 4306 4307 4308
-f 4309 4310 4311
-f 4312 4313 4314
-f 4315 4316 4317
-f 4318 4319 4320
-f 4321 4322 4323
-f 4324 4325 4326
-f 4327 4328 4329
-f 4330 4331 4332
-f 4333 4334 4335
-f 4336 4337 4338
-f 4339 4340 4341
-f 4342 4343 4344
-f 4345 4346 4347
-f 4348 4349 4350
-f 4351 4352 4353
-f 4354 4355 4356
-f 4357 4358 4359
-f 4360 4361 4362
-f 4363 4364 4365
-f 4366 4367 4368
-f 4369 4370 4371
-f 4372 4373 4374
-f 4375 4376 4377
-f 4378 4379 4380
-f 4381 4382 4383
-f 4384 4385 4386
-f 4387 4388 4389
-f 4390 4391 4392
-f 4393 4394 4395
-f 4396 4397 4398
-f 4399 4400 4401
-f 4402 4403 4404
-f 4405 4406 4407
-f 4408 4409 4410
-f 4411 4412 4413
-f 4414 4415 4416
-f 4417 4418 4419
-f 4420 4421 4422
-f 4423 4424 4425
-f 4426 4427 4428
-f 4429 4430 4431
-f 4432 4433 4434
-f 4435 4436 4437
-f 4438 4439 4440
-f 4441 4442 4443
-f 4444 4445 4446
-f 4447 4448 4449
-f 4450 4451 4452
-f 4453 4454 4455
-f 4456 4457 4458
-f 4459 4460 4461
-f 4462 4463 4464
-f 4465 4466 4467
-f 4468 4469 4470
-f 4471 4472 4473
-f 4474 4475 4476
-f 4477 4478 4479
-f 4480 4481 4482
-f 4483 4484 4485
-f 4486 4487 4488
-f 4489 4490 4491
-f 4492 4493 4494
-f 4495 4496 4497
-f 4498 4499 4500
-f 4501 4502 4503
-f 4504 4505 4506
-f 4507 4508 4509
-f 4510 4511 4512
-f 4513 4514 4515
-f 4516 4517 4518
-f 4519 4520 4521
-f 4522 4523 4524
-f 4525 4526 4527
-f 4528 4529 4530
-f 4531 4532 4533
-f 4534 4535 4536
-f 4537 4538 4539
-f 4540 4541 4542
-f 4543 4544 4545
-f 4546 4547 4548
-f 4549 4550 4551
-f 4552 4553 4554
-f 4555 4556 4557
-f 4558 4559 4560
-f 4561 4562 4563
-f 4564 4565 4566
-f 4567 4568 4569
-f 4570 4571 4572
-f 4573 4574 4575
-f 4576 4577 4578
-f 4579 4580 4581
-f 4582 4583 4584
-f 4585 4586 4587
-f 4588 4589 4590
-f 4591 4592 4593
-f 4594 4595 4596
-f 4597 4598 4599
-f 4600 4601 4602
-f 4603 4604 4605
-f 4606 4607 4608
-f 4609 4610 4611
-f 4612 4613 4614
-f 4615 4616 4617
-f 4618 4619 4620
-f 4621 4622 4623
-f 4624 4625 4626
-f 4627 4628 4629
-f 4630 4631 4632
-f 4633 4634 4635
-f 4636 4637 4638
-f 4639 4640 4641
-f 4642 4643 4644
-f 4645 4646 4647
-f 4648 4649 4650
-f 4651 4652 4653
-f 4654 4655 4656
-f 4657 4658 4659
-f 4660 4661 4662
-f 4663 4664 4665
-f 4666 4667 4668
-f 4669 4670 4671
-f 4672 4673 4674
-f 4675 4676 4677
-f 4678 4679 4680
-f 4681 4682 4683
-f 4684 4685 4686
-f 4687 4688 4689
-f 4690 4691 4692
-f 4693 4694 4695
-f 4696 4697 4698
-f 4699 4700 4701
-f 4702 4703 4704
-f 4705 4706 4707
-f 4708 4709 4710
-f 4711 4712 4713
-f 4714 4715 4716
-f 4717 4718 4719
-f 4720 4721 4722
-f 4723 4724 4725
-f 4726 4727 4728
-f 4729 4730 4731
-f 4732 4733 4734
-f 4735 4736 4737
-f 4738 4739 4740
-f 4741 4742 4743
-f 4744 4745 4746
-f 4747 4748 4749
-f 4750 4751 4752
-f 4753 4754 4755
-f 4756 4757 4758
-f 4759 4760 4761
-f 4762 4763 4764
-f 4765 4766 4767
-f 4768 4769 4770
-f 4771 4772 4773
-f 4774 4775 4776
-f 4777 4778 4779
-f 4780 4781 4782
-f 4783 4784 4785
-f 4786 4787 4788
-f 4789 4790 4791
-f 4792 4793 4794
-f 4795 4796 4797
-f 4798 4799 4800
-f 4801 4802 4803
-f 4804 4805 4806
-f 4807 4808 4809
-f 4810 4811 4812
-f 4813 4814 4815
-f 4816 4817 4818
-f 4819 4820 4821
-f 4822 4823 4824
-f 4825 4826 4827
-f 4828 4829 4830
-f 4831 4832 4833
-f 4834 4835 4836
-f 4837 4838 4839
-f 4840 4841 4842
-f 4843 4844 4845
-f 4846 4847 4848
-f 4849 4850 4851
-f 4852 4853 4854
-f 4855 4856 4857
-f 4858 4859 4860
-f 4861 4862 4863
-f 4864 4865 4866
-f 4867 4868 4869
-f 4870 4871 4872
-f 4873 4874 4875
-f 4876 4877 4878
-f 4879 4880 4881
-f 4882 4883 4884
-f 4885 4886 4887
-f 4888 4889 4890
-f 4891 4892 4893
-f 4894 4895 4896
-f 4897 4898 4899
-f 4900 4901 4902
-f 4903 4904 4905
-f 4906 4907 4908
-f 4909 4910 4911
-f 4912 4913 4914
-f 4915 4916 4917
-f 4918 4919 4920
-f 4921 4922 4923
-f 4924 4925 4926
-f 4927 4928 4929
-f 4930 4931 4932
-f 4933 4934 4935
-f 4936 4937 4938
-f 4939 4940 4941
-f 4942 4943 4944
-f 4945 4946 4947
-f 4948 4949 4950
-f 4951 4952 4953
-f 4954 4955 4956
-f 4957 4958 4959
-f 4960 4961 4962
-f 4963 4964 4965
-f 4966 4967 4968
-f 4969 4970 4971
-f 4972 4973 4974
-f 4975 4976 4977
-f 4978 4979 4980
-f 4981 4982 4983
-f 4984 4985 4986
-f 4987 4988 4989
-f 4990 4991 4992
-f 4993 4994 4995
-f 4996 4997 4998
-f 4999 5000 5001
-f 5002 5003 5004
-f 5005 5006 5007
-f 5008 5009 5010
-f 5011 5012 5013
-f 5014 5015 5016
-f 5017 5018 5019
-f 5020 5021 5022
-f 5023 5024 5025
-f 5026 5027 5028
-f 5029 5030 5031
-f 5032 5033 5034
-f 5035 5036 5037
-f 5038 5039 5040
-f 5041 5042 5043
-f 5044 5045 5046
-f 5047 5048 5049
-f 5050 5051 5052
-f 5053 5054 5055
-f 5056 5057 5058
-f 5059 5060 5061
-f 5062 5063 5064
-f 5065 5066 5067
-f 5068 5069 5070
-f 5071 5072 5073
-f 5074 5075 5076
-f 5077 5078 5079
-f 5080 5081 5082
-f 5083 5084 5085
-f 5086 5087 5088
-f 5089 5090 5091
-f 5092 5093 5094
-f 5095 5096 5097
-f 5098 5099 5100
-f 5101 5102 5103
-f 5104 5105 5106
-f 5107 5108 5109
-f 5110 5111 5112
-f 5113 5114 5115
-f 5116 5117 5118
-f 5119 5120 5121
-f 5122 5123 5124
-f 5125 5126 5127
-f 5128 5129 5130
-f 5131 5132 5133
-f 5134 5135 5136
-f 5137 5138 5139
-f 5140 5141 5142
-f 5143 5144 5145
-f 5146 5147 5148
-f 5149 5150 5151
-f 5152 5153 5154
-f 5155 5156 5157
-f 5158 5159 5160
-f 5161 5162 5163
-f 5164 5165 5166
-f 5167 5168 5169
-f 5170 5171 5172
-f 5173 5174 5175
-f 5176 5177 5178
-f 5179 5180 5181
-f 5182 5183 5184
-f 5185 5186 5187
-f 5188 5189 5190
-f 5191 5192 5193
-f 5194 5195 5196
-f 5197 5198 5199
-f 5200 5201 5202
-f 5203 5204 5205
-f 5206 5207 5208
-f 5209 5210 5211
-f 5212 5213 5214
-f 5215 5216 5217
-f 5218 5219 5220
-f 5221 5222 5223
-f 5224 5225 5226
-f 5227 5228 5229
-f 5230 5231 5232
-f 5233 5234 5235
-f 5236 5237 5238
-f 5239 5240 5241
-f 5242 5243 5244
-f 5245 5246 5247
-f 5248 5249 5250
-f 5251 5252 5253
-f 5254 5255 5256
-f 5257 5258 5259
-f 5260 5261 5262
-f 5263 5264 5265
-f 5266 5267 5268
-f 5269 5270 5271
-f 5272 5273 5274
-f 5275 5276 5277
-f 5278 5279 5280
-f 5281 5282 5283
-f 5284 5285 5286
-f 5287 5288 5289
-f 5290 5291 5292
-f 5293 5294 5295
-f 5296 5297 5298
-f 5299 5300 5301
-f 5302 5303 5304
-f 5305 5306 5307
-f 5308 5309 5310
-f 5311 5312 5313
-f 5314 5315 5316
-f 5317 5318 5319
-f 5320 5321 5322
-f 5323 5324 5325
-f 5326 5327 5328
-f 5329 5330 5331
-f 5332 5333 5334
-f 5335 5336 5337
-f 5338 5339 5340
-f 5341 5342 5343
-f 5344 5345 5346
-f 5347 5348 5349
-f 5350 5351 5352
-f 5353 5354 5355
-f 5356 5357 5358
-f 5359 5360 5361
-f 5362 5363 5364
-f 5365 5366 5367
-f 5368 5369 5370
-f 5371 5372 5373
-f 5374 5375 5376
-f 5377 5378 5379
-f 5380 5381 5382
-f 5383 5384 5385
-f 5386 5387 5388
-f 5389 5390 5391
-f 5392 5393 5394
-f 5395 5396 5397
-f 5398 5399 5400
-f 5401 5402 5403
-f 5404 5405 5406
-f 5407 5408 5409
-f 5410 5411 5412
-f 5413 5414 5415
-f 5416 5417 5418
-f 5419 5420 5421
-f 5422 5423 5424
-f 5425 5426 5427
-f 5428 5429 5430
-f 5431 5432 5433
-f 5434 5435 5436
-f 5437 5438 5439
-f 5440 5441 5442
-f 5443 5444 5445
-f 5446 5447 5448
-f 5449 5450 5451
-f 5452 5453 5454
-f 5455 5456 5457
-f 5458 5459 5460
-f 5461 5462 5463
-f 5464 5465 5466
-f 5467 5468 5469
-f 5470 5471 5472
-f 5473 5474 5475
-f 5476 5477 5478
-f 5479 5480 5481
-f 5482 5483 5484
-f 5485 5486 5487
-f 5488 5489 5490
-f 5491 5492 5493
-f 5494 5495 5496
-f 5497 5498 5499
-f 5500 5501 5502
-f 5503 5504 5505
-f 5506 5507 5508
-f 5509 5510 5511
-f 5512 5513 5514
-f 5515 5516 5517
-f 5518 5519 5520
-f 5521 5522 5523
-f 5524 5525 5526
-f 5527 5528 5529
-f 5530 5531 5532
-f 5533 5534 5535
-f 5536 5537 5538
-f 5539 5540 5541
-f 5542 5543 5544
-f 5545 5546 5547
-f 5548 5549 5550
-f 5551 5552 5553
-f 5554 5555 5556
-f 5557 5558 5559
-f 5560 5561 5562
-f 5563 5564 5565
-f 5566 5567 5568
-f 5569 5570 5571
-f 5572 5573 5574
-f 5575 5576 5577
-f 5578 5579 5580
-f 5581 5582 5583
-f 5584 5585 5586
-f 5587 5588 5589
-f 5590 5591 5592
-f 5593 5594 5595
-f 5596 5597 5598
-f 5599 5600 5601
-f 5602 5603 5604
-f 5605 5606 5607
-f 5608 5609 5610
-f 5611 5612 5613
-f 5614 5615 5616
-f 5617 5618 5619
-f 5620 5621 5622
-f 5623 5624 5625
-f 5626 5627 5628
-f 5629 5630 5631
-f 5632 5633 5634
-f 5635 5636 5637
-f 5638 5639 5640
-f 5641 5642 5643
-f 5644 5645 5646
-f 5647 5648 5649
-f 5650 5651 5652
-f 5653 5654 5655
-f 5656 5657 5658
-f 5659 5660 5661
-f 5662 5663 5664
-f 5665 5666 5667
-f 5668 5669 5670
-f 5671 5672 5673
-f 5674 5675 5676
-f 5677 5678 5679
-f 5680 5681 5682
-f 5683 5684 5685
-f 5686 5687 5688
-f 5689 5690 5691
-f 5692 5693 5694
-f 5695 5696 5697
-f 5698 5699 5700
-f 5701 5702 5703
-f 5704 5705 5706
-f 5707 5708 5709
-f 5710 5711 5712
-f 5713 5714 5715
-f 5716 5717 5718
-f 5719 5720 5721
-f 5722 5723 5724
-f 5725 5726 5727
-f 5728 5729 5730
-f 5731 5732 5733
-f 5734 5735 5736
-f 5737 5738 5739
-f 5740 5741 5742
-f 5743 5744 5745
-f 5746 5747 5748
-f 5749 5750 5751
-f 5752 5753 5754
-f 5755 5756 5757
-f 5758 5759 5760
-f 5761 5762 5763
-f 5764 5765 5766
-f 5767 5768 5769
-f 5770 5771 5772
-f 5773 5774 5775
-f 5776 5777 5778
-f 5779 5780 5781
-f 5782 5783 5784
-f 5785 5786 5787
-f 5788 5789 5790
-f 5791 5792 5793
-f 5794 5795 5796
-f 5797 5798 5799
-f 5800 5801 5802
-f 5803 5804 5805
-f 5806 5807 5808
-f 5809 5810 5811
-f 5812 5813 5814
-f 5815 5816 5817
-f 5818 5819 5820
-f 5821 5822 5823
-f 5824 5825 5826
-f 5827 5828 5829
-f 5830 5831 5832
-f 5833 5834 5835
-f 5836 5837 5838
-f 5839 5840 5841
-f 5842 5843 5844
-f 5845 5846 5847
-f 5848 5849 5850
-f 5851 5852 5853
-f 5854 5855 5856
-f 5857 5858 5859
-f 5860 5861 5862
-f 5863 5864 5865
-f 5866 5867 5868
-f 5869 5870 5871
-f 5872 5873 5874
-f 5875 5876 5877
-f 5878 5879 5880
-f 5881 5882 5883
-f 5884 5885 5886
-f 5887 5888 5889
-f 5890 5891 5892
-f 5893 5894 5895
-f 5896 5897 5898
-f 5899 5900 5901
-f 5902 5903 5904
-f 5905 5906 5907
-f 5908 5909 5910
-f 5911 5912 5913
-f 5914 5915 5916
-f 5917 5918 5919
-f 5920 5921 5922
-f 5923 5924 5925
-f 5926 5927 5928
-f 5929 5930 5931
-f 5932 5933 5934
-f 5935 5936 5937
-f 5938 5939 5940
-f 5941 5942 5943
-f 5944 5945 5946
-f 5947 5948 5949
-f 5950 5951 5952
-f 5953 5954 5955
-f 5956 5957 5958
-f 5959 5960 5961
-f 5962 5963 5964
-f 5965 5966 5967
-f 5968 5969 5970
-f 5971 5972 5973
-f 5974 5975 5976
-f 5977 5978 5979
-f 5980 5981 5982
-f 5983 5984 5985
-f 5986 5987 5988
-f 5989 5990 5991
-f 5992 5993 5994
-f 5995 5996 5997
-f 5998 5999 6000
-f 6001 6002 6003
-f 6004 6005 6006
-f 6007 6008 6009
-f 6010 6011 6012
-f 6013 6014 6015
-f 6016 6017 6018
-f 6019 6020 6021
-f 6022 6023 6024
-f 6025 6026 6027
-f 6028 6029 6030
-f 6031 6032 6033
-f 6034 6035 6036
-f 6037 6038 6039
-f 6040 6041 6042
-f 6043 6044 6045
-f 6046 6047 6048
-f 6049 6050 6051
-f 6052 6053 6054
-f 6055 6056 6057
-f 6058 6059 6060
-f 6061 6062 6063
-f 6064 6065 6066
-f 6067 6068 6069
-f 6070 6071 6072
-f 6073 6074 6075
-f 6076 6077 6078
-f 6079 6080 6081
-f 6082 6083 6084
-f 6085 6086 6087
-f 6088 6089 6090
-f 6091 6092 6093
-f 6094 6095 6096
-f 6097 6098 6099
-f 6100 6101 6102
-f 6103 6104 6105
-f 6106 6107 6108
-f 6109 6110 6111
-f 6112 6113 6114
-f 6115 6116 6117
-f 6118 6119 6120
-f 6121 6122 6123
-f 6124 6125 6126
-f 6127 6128 6129
-f 6130 6131 6132
-f 6133 6134 6135
-f 6136 6137 6138
-f 6139 6140 6141
-f 6142 6143 6144
-f 6145 6146 6147
-f 6148 6149 6150
-f 6151 6152 6153
-f 6154 6155 6156
-f 6157 6158 6159
-f 6160 6161 6162
-f 6163 6164 6165
-f 6166 6167 6168
-f 6169 6170 6171
-f 6172 6173 6174
-f 6175 6176 6177
-f 6178 6179 6180
-f 6181 6182 6183
-f 6184 6185 6186
-f 6187 6188 6189
-f 6190 6191 6192
-f 6193 6194 6195
-f 6196 6197 6198
-f 6199 6200 6201
-f 6202 6203 6204
-f 6205 6206 6207
-f 6208 6209 6210
-f 6211 6212 6213
-f 6214 6215 6216
-f 6217 6218 6219
-f 6220 6221 6222
-f 6223 6224 6225
-f 6226 6227 6228
-f 6229 6230 6231
-f 6232 6233 6234
-f 6235 6236 6237
-f 6238 6239 6240
-f 6241 6242 6243
-f 6244 6245 6246
-f 6247 6248 6249
-f 6250 6251 6252
-f 6253 6254 6255
-f 6256 6257 6258
-f 6259 6260 6261
-f 6262 6263 6264
-f 6265 6266 6267
-f 6268 6269 6270
-f 6271 6272 6273
-f 6274 6275 6276
-f 6277 6278 6279
-f 6280 6281 6282
-f 6283 6284 6285
-f 6286 6287 6288
-f 6289 6290 6291
-f 6292 6293 6294
-f 6295 6296 6297
-f 6298 6299 6300
-f 6301 6302 6303
-f 6304 6305 6306
-f 6307 6308 6309
-f 6310 6311 6312
-f 6313 6314 6315
-f 6316 6317 6318
-f 6319 6320 6321
-f 6322 6323 6324
-f 6325 6326 6327
-f 6328 6329 6330
-f 6331 6332 6333
-f 6334 6335 6336
-f 6337 6338 6339
-f 6340 6341 6342
-f 6343 6344 6345
-f 6346 6347 6348
-f 6349 6350 6351
-f 6352 6353 6354
-f 6355 6356 6357
-f 6358 6359 6360
-f 6361 6362 6363
-f 6364 6365 6366
-f 6367 6368 6369
-f 6370 6371 6372
-f 6373 6374 6375
-f 6376 6377 6378
-f 6379 6380 6381
-f 6382 6383 6384
-f 6385 6386 6387
-f 6388 6389 6390
-f 6391 6392 6393
-f 6394 6395 6396
-f 6397 6398 6399
-f 6400 6401 6402
-f 6403 6404 6405
-f 6406 6407 6408
-f 6409 6410 6411
-f 6412 6413 6414
-f 6415 6416 6417
-f 6418 6419 6420
-f 6421 6422 6423
-f 6424 6425 6426
-f 6427 6428 6429
-f 6430 6431 6432
-f 6433 6434 6435
-f 6436 6437 6438
-f 6439 6440 6441
-f 6442 6443 6444
-f 6445 6446 6447
-f 6448 6449 6450
-f 6451 6452 6453
-f 6454 6455 6456
-f 6457 6458 6459
-f 6460 6461 6462
-f 6463 6464 6465
-f 6466 6467 6468
-f 6469 6470 6471
-f 6472 6473 6474
-f 6475 6476 6477
-f 6478 6479 6480
-f 6481 6482 6483
-f 6484 6485 6486
-f 6487 6488 6489
-f 6490 6491 6492
-f 6493 6494 6495
-f 6496 6497 6498
-f 6499 6500 6501
-f 6502 6503 6504
-f 6505 6506 6507
-f 6508 6509 6510
-f 6511 6512 6513
-f 6514 6515 6516
-f 6517 6518 6519
-f 6520 6521 6522
-f 6523 6524 6525
-f 6526 6527 6528
-f 6529 6530 6531
-f 6532 6533 6534
-f 6535 6536 6537
-f 6538 6539 6540
diff --git a/trunk/fcl/test/fcl_resources/rob.obj b/trunk/fcl/test/fcl_resources/rob.obj
deleted file mode 100644
index fbc5c75f41b300bfd47ac4a10728a755bda68e7c..0000000000000000000000000000000000000000
--- a/trunk/fcl/test/fcl_resources/rob.obj
+++ /dev/null
@@ -1,865 +0,0 @@
-648 216
-v -500 -395 375
-v 500 -395 375
-v 462 -203.6 375
-v -500 -395 375
-v 462 -203.6 375
-v 353.5 -41.5 375
-v -500 -395 375
-v 353.5 -41.5 375
-v 191.4 66.4 375
-v -500 -395 375
-v 191.4 66.4 375
-v -500 66.4 375
-v -500 66.4 75
-v 191.4 66.4 75
-v 353.5 -41.5 75
-v -500 66.4 75
-v 353.5 -41.5 75
-v 462 -203.6 75
-v -500 66.4 75
-v 462 -203.6 75
-v 500 -395 75
-v -500 66.4 75
-v 500 -395 75
-v -500 -395 75
-v -500 -395 375
-v -500 -395 75
-v 500 -395 75
-v -500 -395 375
-v 500 -395 75
-v 500 -395 375
-v 500 -395 375
-v 500 -395 75
-v 462 -203.6 75
-v 500 -395 375
-v 462 -203.6 75
-v 462 -203.6 375
-v 462 -203.6 375
-v 462 -203.6 75
-v 353.5 -41.5 75
-v 462 -203.6 375
-v 353.5 -41.5 75
-v 353.5 -41.5 375
-v 353.5 -41.5 375
-v 353.5 -41.5 75
-v 191.4 66.4 75
-v 353.5 -41.5 375
-v 191.4 66.4 75
-v 191.4 66.4 375
-v 191.4 66.4 375
-v 191.4 66.4 75
-v -500 66.4 75
-v 191.4 66.4 375
-v -500 66.4 75
-v -500 66.4 375
-v -500 66.4 375
-v -500 66.4 75
-v -500 -395 75
-v -500 66.4 375
-v -500 -395 75
-v -500 -395 375
-v -500 66.4 375
-v 191.4 66.4 375
-v -38 296.4 375
-v -500 66.4 375
-v -38 296.4 375
-v -500 296.4 375
-v -500 296.4 75
-v -38 296.4 75
-v 191.4 66.4 75
-v -500 296.4 75
-v 191.4 66.4 75
-v -500 66.4 75
-v -500 66.4 375
-v -500 66.4 75
-v 191.4 66.4 75
-v -500 66.4 375
-v 191.4 66.4 75
-v 191.4 66.4 375
-v 191.4 66.4 375
-v 191.4 66.4 75
-v -38 296.4 75
-v 191.4 66.4 375
-v -38 296.4 75
-v -38 296.4 375
-v -38 296.4 375
-v -38 296.4 75
-v -500 296.4 75
-v -38 296.4 375
-v -500 296.4 75
-v -500 296.4 375
-v -500 296.4 375
-v -500 296.4 75
-v -500 66.4 75
-v -500 296.4 375
-v -500 66.4 75
-v -500 66.4 375
-v -500 296.4 375
-v -38 296.4 375
-v -146.5 458.6 375
-v -500 296.4 375
-v -146.5 458.6 375
-v -308.7 566.4 375
-v -500 296.4 375
-v -308.7 566.4 375
-v -500 605 375
-v -500 605 75
-v -308.7 566.4 75
-v -146.5 458.6 75
-v -500 605 75
-v -146.5 458.6 75
-v -38 296.4 75
-v -500 605 75
-v -38 296.4 75
-v -500 296.4 75
-v -500 296.4 375
-v -500 296.4 75
-v -38 296.4 75
-v -500 296.4 375
-v -38 296.4 75
-v -38 296.4 375
-v -38 296.4 375
-v -38 296.4 75
-v -146.5 458.6 75
-v -38 296.4 375
-v -146.5 458.6 75
-v -146.5 458.6 375
-v -146.5 458.6 375
-v -146.5 458.6 75
-v -308.7 566.4 75
-v -146.5 458.6 375
-v -308.7 566.4 75
-v -308.7 566.4 375
-v -308.7 566.4 375
-v -308.7 566.4 75
-v -500 605 75
-v -308.7 566.4 375
-v -500 605 75
-v -500 605 375
-v -500 605 375
-v -500 605 75
-v -500 296.4 75
-v -500 605 375
-v -500 296.4 75
-v -500 296.4 375
-v -500 -595 175
-v 500 -595 175
-v -500 -395 175
-v -500 -395 175
-v 500 -595 175
-v 500 -395 175
-v 500 -595 75
-v -500 -595 75
-v 500 -395 75
-v 500 -395 75
-v -500 -595 75
-v -500 -395 75
-v 500 -595 175
-v 500 -595 75
-v 500 -395 175
-v 500 -395 175
-v 500 -595 75
-v 500 -395 75
-v -500 -595 75
-v -500 -595 175
-v -500 -395 75
-v -500 -395 75
-v -500 -595 175
-v -500 -395 175
-v -500 -395 75
-v -500 -395 175
-v 500 -395 75
-v 500 -395 75
-v -500 -395 175
-v 500 -395 175
-v -500 -595 75
-v 500 -595 75
-v -500 -595 175
-v -500 -595 175
-v 500 -595 75
-v 500 -595 175
-v -500 -605 195
-v 500 -605 195
-v -500 -395 195
-v -500 -395 195
-v 500 -605 195
-v 500 -395 195
-v 500 -605 175
-v -500 -605 175
-v 500 -395 175
-v 500 -395 175
-v -500 -605 175
-v -500 -395 175
-v 500 -605 195
-v 500 -605 175
-v 500 -395 195
-v 500 -395 195
-v 500 -605 175
-v 500 -395 175
-v -500 -605 175
-v -500 -605 195
-v -500 -395 175
-v -500 -395 175
-v -500 -605 195
-v -500 -395 195
-v -500 -395 175
-v -500 -395 195
-v 500 -395 175
-v 500 -395 175
-v -500 -395 195
-v 500 -395 195
-v -500 -605 175
-v 500 -605 175
-v -500 -605 195
-v -500 -605 195
-v 500 -605 175
-v 500 -605 195
-v -430 -495 75
-v -431.522 -487.346 75
-v -430 -495 -425
-v -430 -495 -425
-v -431.522 -487.346 75
-v -431.522 -487.346 -425
-v -431.522 -487.346 75
-v -435.858 -480.858 75
-v -431.522 -487.346 -425
-v -431.522 -487.346 -425
-v -435.858 -480.858 75
-v -435.858 -480.858 -425
-v -435.858 -480.858 75
-v -442.346 -476.522 75
-v -435.858 -480.858 -425
-v -435.858 -480.858 -425
-v -442.346 -476.522 75
-v -442.346 -476.522 -425
-v -442.346 -476.522 75
-v -450 -475 75
-v -442.346 -476.522 -425
-v -442.346 -476.522 -425
-v -450 -475 75
-v -450 -475 -425
-v -450 -475 75
-v -457.654 -476.522 75
-v -450 -475 -425
-v -450 -475 -425
-v -457.654 -476.522 75
-v -457.654 -476.522 -425
-v -457.654 -476.522 75
-v -464.142 -480.858 75
-v -457.654 -476.522 -425
-v -457.654 -476.522 -425
-v -464.142 -480.858 75
-v -464.142 -480.858 -425
-v -464.142 -480.858 75
-v -468.478 -487.346 75
-v -464.142 -480.858 -425
-v -464.142 -480.858 -425
-v -468.478 -487.346 75
-v -468.478 -487.346 -425
-v -468.478 -487.346 75
-v -470 -495 75
-v -468.478 -487.346 -425
-v -468.478 -487.346 -425
-v -470 -495 75
-v -470 -495 -425
-v -470 -495 75
-v -468.478 -502.654 75
-v -470 -495 -425
-v -470 -495 -425
-v -468.478 -502.654 75
-v -468.478 -502.654 -425
-v -468.478 -502.654 75
-v -464.142 -509.142 75
-v -468.478 -502.654 -425
-v -468.478 -502.654 -425
-v -464.142 -509.142 75
-v -464.142 -509.142 -425
-v -464.142 -509.142 75
-v -457.654 -513.478 75
-v -464.142 -509.142 -425
-v -464.142 -509.142 -425
-v -457.654 -513.478 75
-v -457.654 -513.478 -425
-v -457.654 -513.478 75
-v -450 -515 75
-v -457.654 -513.478 -425
-v -457.654 -513.478 -425
-v -450 -515 75
-v -450 -515 -425
-v -450 -515 75
-v -442.346 -513.478 75
-v -450 -515 -425
-v -450 -515 -425
-v -442.346 -513.478 75
-v -442.346 -513.478 -425
-v -442.346 -513.478 75
-v -435.858 -509.142 75
-v -442.346 -513.478 -425
-v -442.346 -513.478 -425
-v -435.858 -509.142 75
-v -435.858 -509.142 -425
-v -435.858 -509.142 75
-v -431.522 -502.654 75
-v -435.858 -509.142 -425
-v -435.858 -509.142 -425
-v -431.522 -502.654 75
-v -431.522 -502.654 -425
-v -431.522 -502.654 75
-v -430 -495 75
-v -431.522 -502.654 -425
-v -431.522 -502.654 -425
-v -430 -495 75
-v -430 -495 -425
-v 470 -495 75
-v 468.478 -487.346 75
-v 470 -495 -425
-v 470 -495 -425
-v 468.478 -487.346 75
-v 468.478 -487.346 -425
-v 468.478 -487.346 75
-v 464.142 -480.858 75
-v 468.478 -487.346 -425
-v 468.478 -487.346 -425
-v 464.142 -480.858 75
-v 464.142 -480.858 -425
-v 464.142 -480.858 75
-v 457.654 -476.522 75
-v 464.142 -480.858 -425
-v 464.142 -480.858 -425
-v 457.654 -476.522 75
-v 457.654 -476.522 -425
-v 457.654 -476.522 75
-v 450 -475 75
-v 457.654 -476.522 -425
-v 457.654 -476.522 -425
-v 450 -475 75
-v 450 -475 -425
-v 450 -475 75
-v 442.346 -476.522 75
-v 450 -475 -425
-v 450 -475 -425
-v 442.346 -476.522 75
-v 442.346 -476.522 -425
-v 442.346 -476.522 75
-v 435.858 -480.858 75
-v 442.346 -476.522 -425
-v 442.346 -476.522 -425
-v 435.858 -480.858 75
-v 435.858 -480.858 -425
-v 435.858 -480.858 75
-v 431.522 -487.346 75
-v 435.858 -480.858 -425
-v 435.858 -480.858 -425
-v 431.522 -487.346 75
-v 431.522 -487.346 -425
-v 431.522 -487.346 75
-v 430 -495 75
-v 431.522 -487.346 -425
-v 431.522 -487.346 -425
-v 430 -495 75
-v 430 -495 -425
-v 430 -495 75
-v 431.522 -502.654 75
-v 430 -495 -425
-v 430 -495 -425
-v 431.522 -502.654 75
-v 431.522 -502.654 -425
-v 431.522 -502.654 75
-v 435.858 -509.142 75
-v 431.522 -502.654 -425
-v 431.522 -502.654 -425
-v 435.858 -509.142 75
-v 435.858 -509.142 -425
-v 435.858 -509.142 75
-v 442.346 -513.478 75
-v 435.858 -509.142 -425
-v 435.858 -509.142 -425
-v 442.346 -513.478 75
-v 442.346 -513.478 -425
-v 442.346 -513.478 75
-v 450 -515 75
-v 442.346 -513.478 -425
-v 442.346 -513.478 -425
-v 450 -515 75
-v 450 -515 -425
-v 450 -515 75
-v 457.654 -513.478 75
-v 450 -515 -425
-v 450 -515 -425
-v 457.654 -513.478 75
-v 457.654 -513.478 -425
-v 457.654 -513.478 75
-v 464.142 -509.142 75
-v 457.654 -513.478 -425
-v 457.654 -513.478 -425
-v 464.142 -509.142 75
-v 464.142 -509.142 -425
-v 464.142 -509.142 75
-v 468.478 -502.654 75
-v 464.142 -509.142 -425
-v 464.142 -509.142 -425
-v 468.478 -502.654 75
-v 468.478 -502.654 -425
-v 468.478 -502.654 75
-v 470 -495 75
-v 468.478 -502.654 -425
-v 468.478 -502.654 -425
-v 470 -495 75
-v 470 -495 -425
-v -380 505 75
-v -381.522 512.654 75
-v -380 505 -425
-v -380 505 -425
-v -381.522 512.654 75
-v -381.522 512.654 -425
-v -381.522 512.654 75
-v -385.858 519.142 75
-v -381.522 512.654 -425
-v -381.522 512.654 -425
-v -385.858 519.142 75
-v -385.858 519.142 -425
-v -385.858 519.142 75
-v -392.346 523.478 75
-v -385.858 519.142 -425
-v -385.858 519.142 -425
-v -392.346 523.478 75
-v -392.346 523.478 -425
-v -392.346 523.478 75
-v -400 525 75
-v -392.346 523.478 -425
-v -392.346 523.478 -425
-v -400 525 75
-v -400 525 -425
-v -400 525 75
-v -407.654 523.478 75
-v -400 525 -425
-v -400 525 -425
-v -407.654 523.478 75
-v -407.654 523.478 -425
-v -407.654 523.478 75
-v -414.142 519.142 75
-v -407.654 523.478 -425
-v -407.654 523.478 -425
-v -414.142 519.142 75
-v -414.142 519.142 -425
-v -414.142 519.142 75
-v -418.478 512.654 75
-v -414.142 519.142 -425
-v -414.142 519.142 -425
-v -418.478 512.654 75
-v -418.478 512.654 -425
-v -418.478 512.654 75
-v -420 505 75
-v -418.478 512.654 -425
-v -418.478 512.654 -425
-v -420 505 75
-v -420 505 -425
-v -420 505 75
-v -418.478 497.346 75
-v -420 505 -425
-v -420 505 -425
-v -418.478 497.346 75
-v -418.478 497.346 -425
-v -418.478 497.346 75
-v -414.142 490.858 75
-v -418.478 497.346 -425
-v -418.478 497.346 -425
-v -414.142 490.858 75
-v -414.142 490.858 -425
-v -414.142 490.858 75
-v -407.654 486.522 75
-v -414.142 490.858 -425
-v -414.142 490.858 -425
-v -407.654 486.522 75
-v -407.654 486.522 -425
-v -407.654 486.522 75
-v -400 485 75
-v -407.654 486.522 -425
-v -407.654 486.522 -425
-v -400 485 75
-v -400 485 -425
-v -400 485 75
-v -392.346 486.522 75
-v -400 485 -425
-v -400 485 -425
-v -392.346 486.522 75
-v -392.346 486.522 -425
-v -392.346 486.522 75
-v -385.858 490.858 75
-v -392.346 486.522 -425
-v -392.346 486.522 -425
-v -385.858 490.858 75
-v -385.858 490.858 -425
-v -385.858 490.858 75
-v -381.522 497.346 75
-v -385.858 490.858 -425
-v -385.858 490.858 -425
-v -381.522 497.346 75
-v -381.522 497.346 -425
-v -381.522 497.346 75
-v -380 505 75
-v -381.522 497.346 -425
-v -381.522 497.346 -425
-v -380 505 75
-v -380 505 -425
-v -500 -395 425
-v 500 -395 425
-v 462 -203.6 425
-v -500 -395 425
-v 462 -203.6 425
-v 353.5 -41.5 425
-v -500 -395 425
-v 353.5 -41.5 425
-v 191.4 66.4 425
-v -500 -395 425
-v 191.4 66.4 425
-v -500 66.4 425
-v -500 66.4 375
-v 191.4 66.4 375
-v 353.5 -41.5 375
-v -500 66.4 375
-v 353.5 -41.5 375
-v 462 -203.6 375
-v -500 66.4 375
-v 462 -203.6 375
-v 500 -395 375
-v -500 66.4 375
-v 500 -395 375
-v -500 -395 375
-v -500 -395 425
-v -500 -395 375
-v 500 -395 375
-v -500 -395 425
-v 500 -395 375
-v 500 -395 425
-v 500 -395 425
-v 500 -395 375
-v 462 -203.6 375
-v 500 -395 425
-v 462 -203.6 375
-v 462 -203.6 425
-v 462 -203.6 425
-v 462 -203.6 375
-v 353.5 -41.5 375
-v 462 -203.6 425
-v 353.5 -41.5 375
-v 353.5 -41.5 425
-v 353.5 -41.5 425
-v 353.5 -41.5 375
-v 191.4 66.4 375
-v 353.5 -41.5 425
-v 191.4 66.4 375
-v 191.4 66.4 425
-v 191.4 66.4 425
-v 191.4 66.4 375
-v -500 66.4 375
-v 191.4 66.4 425
-v -500 66.4 375
-v -500 66.4 425
-v -500 66.4 425
-v -500 66.4 375
-v -500 -395 375
-v -500 66.4 425
-v -500 -395 375
-v -500 -395 425
-v -500 66.4 425
-v 191.4 66.4 425
-v -38 296.4 425
-v -500 66.4 425
-v -38 296.4 425
-v -500 296.4 425
-v -500 296.4 375
-v -38 296.4 375
-v 191.4 66.4 375
-v -500 296.4 375
-v 191.4 66.4 375
-v -500 66.4 375
-v -500 66.4 425
-v -500 66.4 375
-v 191.4 66.4 375
-v -500 66.4 425
-v 191.4 66.4 375
-v 191.4 66.4 425
-v 191.4 66.4 425
-v 191.4 66.4 375
-v -38 296.4 375
-v 191.4 66.4 425
-v -38 296.4 375
-v -38 296.4 425
-v -38 296.4 425
-v -38 296.4 375
-v -500 296.4 375
-v -38 296.4 425
-v -500 296.4 375
-v -500 296.4 425
-v -500 296.4 425
-v -500 296.4 375
-v -500 66.4 375
-v -500 296.4 425
-v -500 66.4 375
-v -500 66.4 425
-v -500 296.4 425
-v -38 296.4 425
-v -146.5 458.6 425
-v -500 296.4 425
-v -146.5 458.6 425
-v -308.7 566.4 425
-v -500 296.4 425
-v -308.7 566.4 425
-v -500 605 425
-v -500 605 375
-v -308.7 566.4 375
-v -146.5 458.6 375
-v -500 605 375
-v -146.5 458.6 375
-v -38 296.4 375
-v -500 605 375
-v -38 296.4 375
-v -500 296.4 375
-v -500 296.4 425
-v -500 296.4 375
-v -38 296.4 375
-v -500 296.4 425
-v -38 296.4 375
-v -38 296.4 425
-v -38 296.4 425
-v -38 296.4 375
-v -146.5 458.6 375
-v -38 296.4 425
-v -146.5 458.6 375
-v -146.5 458.6 425
-v -146.5 458.6 425
-v -146.5 458.6 375
-v -308.7 566.4 375
-v -146.5 458.6 425
-v -308.7 566.4 375
-v -308.7 566.4 425
-v -308.7 566.4 425
-v -308.7 566.4 375
-v -500 605 375
-v -308.7 566.4 425
-v -500 605 375
-v -500 605 425
-v -500 605 425
-v -500 605 375
-v -500 296.4 375
-v -500 605 425
-v -500 296.4 375
-v -500 296.4 425
-f 1 2 3
-f 4 5 6
-f 7 8 9
-f 10 11 12
-f 13 14 15
-f 16 17 18
-f 19 20 21
-f 22 23 24
-f 25 26 27
-f 28 29 30
-f 31 32 33
-f 34 35 36
-f 37 38 39
-f 40 41 42
-f 43 44 45
-f 46 47 48
-f 49 50 51
-f 52 53 54
-f 55 56 57
-f 58 59 60
-f 61 62 63
-f 64 65 66
-f 67 68 69
-f 70 71 72
-f 73 74 75
-f 76 77 78
-f 79 80 81
-f 82 83 84
-f 85 86 87
-f 88 89 90
-f 91 92 93
-f 94 95 96
-f 97 98 99
-f 100 101 102
-f 103 104 105
-f 106 107 108
-f 109 110 111
-f 112 113 114
-f 115 116 117
-f 118 119 120
-f 121 122 123
-f 124 125 126
-f 127 128 129
-f 130 131 132
-f 133 134 135
-f 136 137 138
-f 139 140 141
-f 142 143 144
-f 145 146 147
-f 148 149 150
-f 151 152 153
-f 154 155 156
-f 157 158 159
-f 160 161 162
-f 163 164 165
-f 166 167 168
-f 169 170 171
-f 172 173 174
-f 175 176 177
-f 178 179 180
-f 181 182 183
-f 184 185 186
-f 187 188 189
-f 190 191 192
-f 193 194 195
-f 196 197 198
-f 199 200 201
-f 202 203 204
-f 205 206 207
-f 208 209 210
-f 211 212 213
-f 214 215 216
-f 217 218 219
-f 220 221 222
-f 223 224 225
-f 226 227 228
-f 229 230 231
-f 232 233 234
-f 235 236 237
-f 238 239 240
-f 241 242 243
-f 244 245 246
-f 247 248 249
-f 250 251 252
-f 253 254 255
-f 256 257 258
-f 259 260 261
-f 262 263 264
-f 265 266 267
-f 268 269 270
-f 271 272 273
-f 274 275 276
-f 277 278 279
-f 280 281 282
-f 283 284 285
-f 286 287 288
-f 289 290 291
-f 292 293 294
-f 295 296 297
-f 298 299 300
-f 301 302 303
-f 304 305 306
-f 307 308 309
-f 310 311 312
-f 313 314 315
-f 316 317 318
-f 319 320 321
-f 322 323 324
-f 325 326 327
-f 328 329 330
-f 331 332 333
-f 334 335 336
-f 337 338 339
-f 340 341 342
-f 343 344 345
-f 346 347 348
-f 349 350 351
-f 352 353 354
-f 355 356 357
-f 358 359 360
-f 361 362 363
-f 364 365 366
-f 367 368 369
-f 370 371 372
-f 373 374 375
-f 376 377 378
-f 379 380 381
-f 382 383 384
-f 385 386 387
-f 388 389 390
-f 391 392 393
-f 394 395 396
-f 397 398 399
-f 400 401 402
-f 403 404 405
-f 406 407 408
-f 409 410 411
-f 412 413 414
-f 415 416 417
-f 418 419 420
-f 421 422 423
-f 424 425 426
-f 427 428 429
-f 430 431 432
-f 433 434 435
-f 436 437 438
-f 439 440 441
-f 442 443 444
-f 445 446 447
-f 448 449 450
-f 451 452 453
-f 454 455 456
-f 457 458 459
-f 460 461 462
-f 463 464 465
-f 466 467 468
-f 469 470 471
-f 472 473 474
-f 475 476 477
-f 478 479 480
-f 481 482 483
-f 484 485 486
-f 487 488 489
-f 490 491 492
-f 493 494 495
-f 496 497 498
-f 499 500 501
-f 502 503 504
-f 505 506 507
-f 508 509 510
-f 511 512 513
-f 514 515 516
-f 517 518 519
-f 520 521 522
-f 523 524 525
-f 526 527 528
-f 529 530 531
-f 532 533 534
-f 535 536 537
-f 538 539 540
-f 541 542 543
-f 544 545 546
-f 547 548 549
-f 550 551 552
-f 553 554 555
-f 556 557 558
-f 559 560 561
-f 562 563 564
-f 565 566 567
-f 568 569 570
-f 571 572 573
-f 574 575 576
-f 577 578 579
-f 580 581 582
-f 583 584 585
-f 586 587 588
-f 589 590 591
-f 592 593 594
-f 595 596 597
-f 598 599 600
-f 601 602 603
-f 604 605 606
-f 607 608 609
-f 610 611 612
-f 613 614 615
-f 616 617 618
-f 619 620 621
-f 622 623 624
-f 625 626 627
-f 628 629 630
-f 631 632 633
-f 634 635 636
-f 637 638 639
-f 640 641 642
-f 643 644 645
-f 646 647 648